IA64: fix fp fault/trap handler.

This patch is a part of fixes to bug reported as
http://bugzilla.xensource.com/bugzilla/show_bug.cgi?id=1392

When fpswa handler fails to get a bundle in guest,
fp fault/trap should be injected into the guest and let a guest
to handle it.
When the fpswa library return a error, there is no way to
pass the value to the guest. In that case, revert the change
and inject the fp fault/trap.

Signed-off-by: Isaku Yamahata <yamahata@xxxxxxxxxxxxx>

diff --git a/xen/arch/ia64/vmx/vmx_fault.c b/xen/arch/ia64/vmx/vmx_fault.c
--- a/xen/arch/ia64/vmx/vmx_fault.c
+++ b/xen/arch/ia64/vmx/vmx_fault.c
@@ -122,8 +122,7 @@ void vmx_reflect_interruption(u64 ifa, u
         if (!status) {
             vcpu_increment_iip(vcpu);
             return;
-        } else if (IA64_RETRY == status)
-            return;
+        }
         break;
 
     case 33:	// IA64_FP_TRAP_VECTOR
@@ -133,10 +132,6 @@ void vmx_reflect_interruption(u64 ifa, u
         status = handle_fpu_swa(0, regs, isr);
         if (!status)
             return;
-        else if (IA64_RETRY == status) {
-            vcpu_decrement_iip(vcpu);
-            return;
-        }
         break;
 
     case 29: // IA64_DEBUG_VECTOR
diff --git a/xen/arch/ia64/xen/faults.c b/xen/arch/ia64/xen/faults.c
--- a/xen/arch/ia64/xen/faults.c
+++ b/xen/arch/ia64/xen/faults.c
@@ -314,11 +314,15 @@ unsigned long
 unsigned long
 handle_fpu_swa(int fp_fault, struct pt_regs *regs, unsigned long isr)
 {
-	struct vcpu *v = current;
 	IA64_BUNDLE bundle;
 	unsigned long fault_ip;
 	fpswa_ret_t ret;
 	unsigned long rc;
+
+	unsigned long ipsr_save;
+	unsigned long fpsr_save;
+	unsigned long pr_save;
+	struct ia64_fpreg fp_save[6];
 
 	fault_ip = regs->cr_iip;
 	/*
@@ -344,11 +348,22 @@ handle_fpu_swa(int fp_fault, struct pt_r
 		return IA64_RETRY;
 	}
 
+	/* If fpswa returns error, fp falut/trap is reflected and
+	   a guest will call fpswa itself. So we have to revert the effect
+	   to avoid calling fpswa twice. */
+	ipsr_save = regs->cr_ipsr;
+	fpsr_save = regs->ar_fpsr;
+	pr_save = regs->pr;
+	memcpy(fp_save, &regs->f6, sizeof(fp_save));
+
 	ret = fp_emulate(fp_fault, &bundle, &regs->cr_ipsr, &regs->ar_fpsr,
 	                 &isr, &regs->pr, &regs->cr_ifs, regs);
 
 	if (ret.status) {
-		PSCBX(v, fpswa_ret) = ret;
+		regs->cr_ipsr = ipsr_save;
+		regs->ar_fpsr = fpsr_save;
+		regs->pr = pr_save;
+		memcpy(&regs->f6, fp_save, sizeof(fp_save));
 		printk("%s(%s): fp_emulate() returned %ld\n",
 		       __FUNCTION__, fp_fault ? "fault" : "trap", ret.status);
 	}
@@ -688,9 +703,6 @@ ia64_handle_reflection(unsigned long ifa
 			vcpu_increment_iip(v);
 			return;
 		}
-		// fetch code fail
-		if (IA64_RETRY == status)
-			return;
 		printk("ia64_handle_reflection: handling FP fault\n");
 		vector = IA64_FP_FAULT_VECTOR;
 		break;
@@ -698,11 +710,6 @@ ia64_handle_reflection(unsigned long ifa
 		status = handle_fpu_swa(0, regs, isr);
 		if (!status)
 			return;
-		// fetch code fail
-		if (IA64_RETRY == status) {
-			vcpu_decrement_iip(v);
-			return;
-		}
 		printk("ia64_handle_reflection: handling FP trap\n");
 		vector = IA64_FP_TRAP_VECTOR;
 		break;
diff --git a/xen/include/asm-ia64/domain.h b/xen/include/asm-ia64/domain.h
--- a/xen/include/asm-ia64/domain.h
+++ b/xen/include/asm-ia64/domain.h
@@ -288,7 +288,6 @@ struct arch_vcpu {
     char irq_new_condition;    // vpsr.i/vtpr change, check for pending VHPI
     char hypercall_continuation;
 
-    fpswa_ret_t fpswa_ret;	/* save return values of FPSWA emulation */
     struct timer hlt_timer;
     struct arch_vmx_struct arch_vmx; /* Virtual Machine Extensions */