Initial cut at calling the EFI-provided FPSWA (Floating Point Software

Assist) driver to handle the "messy" floating point cases which
cause traps to the kernel for handling.

3 files changed, 163 insertions, 7 deletions

diff --git a/sys/ia64/ia64/machdep.c b/sys/ia64/ia64/machdep.c
index a67f7a9..0a053d9 100644
--- a/sys/ia64/ia64/machdep.c
+++ b/sys/ia64/ia64/machdep.c
@@ -104,6 +104,8 @@ extern u_int64_t kernel_text[], _end[];
 extern u_int64_t _ia64_unwind_start[];
 extern u_int64_t _ia64_unwind_end[];
 
+FPSWA_INTERFACE *fpswa_interface;
+
 u_int64_t ia64_port_base;
 
 char machine[] = "ia64";
@@ -224,6 +226,12 @@ cpu_startup(dummy)
 
 	printf("avail memory = %ld (%ldK bytes)\n", ptoa(cnt.v_free_count),
 	    ptoa(cnt.v_free_count) / 1024);
+ 
+	if (fpswa_interface == NULL)
+		printf("Warning: no FPSWA package supplied\n");
+	else
+		printf("FPSWA Revision = 0x%lx, Entry = %p\n",
+		    fpswa_interface->Revision, (void *)fpswa_interface->Fpswa);
 
 	/*
 	 * Set up buffers, so they can be used to read disk labels.
@@ -511,6 +519,9 @@ ia64_init(u_int64_t arg1, u_int64_t arg2)
 	else
 		kern_envp = (caddr_t)bootinfo.bi_envp;
 
+	/* get fpswa interface */
+	fpswa_interface = (FPSWA_INTERFACE*)IA64_PHYS_TO_RR7(bootinfo.bi_fpswa);
+
 	/* Init basic tunables, including hz */
 	init_param();
 
diff --git a/sys/ia64/ia64/trap.c b/sys/ia64/ia64/trap.c
index 2e4c92a..6558925 100644
--- a/sys/ia64/ia64/trap.c
+++ b/sys/ia64/ia64/trap.c
@@ -59,6 +59,7 @@
 #include <machine/reg.h>
 #include <machine/pal.h>
 #include <machine/fpu.h>
+#include <machine/efi.h>
 
 #ifdef KTRACE
 #include <sys/uio.h>
@@ -71,6 +72,33 @@
 
 extern int unaligned_fixup(struct trapframe *framep, struct thread *td);
 
+/*
+ * EFI-Provided FPSWA interface (Floating Point SoftWare Assist
+ */
+
+/* The function entry address */
+extern FPSWA_INTERFACE *fpswa_interface;
+
+/* Copy of the faulting instruction bundle */
+typedef struct {
+	u_int64_t	bundle_low64;
+	u_int64_t	bundle_high64;
+} FPSWA_BUNDLE;
+
+/*
+ * The fp state descriptor... tell FPSWA where the "true" copy is.
+ * We save some registers in the trapframe, so we have to point some of
+ * these there.  The rest of the registers are "live"
+ */
+typedef struct {
+	u_int64_t	bitmask_low64;		/* f63 - f2 */
+	u_int64_t	bitmask_high64;		/* f127 - f64 */
+	struct ia64_fpreg *fp_low_preserved;	/* f2 - f5 */
+	struct ia64_fpreg *fp_low_volatile;	/* f6 - f15 */
+	struct ia64_fpreg *fp_high_preserved;	/* f16 - f31 */
+	struct ia64_fpreg *fp_high_volatile;	/* f32 - f127 */
+} FP_STATE;
+
 #ifdef WITNESS
 extern char *syscallnames[];
 #endif
@@ -336,19 +364,135 @@ trap(int vector, int imm, struct trapframe *framep)
 		goto dopanic;
 
 	case IA64_VEC_FLOATING_POINT_FAULT:
-	case IA64_VEC_FLOATING_POINT_TRAP:
-		/* 
-		 * If user-land, give a SIGFPE if software completion
-		 * is not requested or if the completion fails.
-		 */
-		if (user) {
+	{
+		FP_STATE fp_state;
+		FPSWA_RET fpswa_ret;
+		FPSWA_BUNDLE bundle;
+
+		/* Always fatal in kernel.  Should never happen. */
+		if (!user)
+			goto dopanic;
+		if (fpswa_interface == NULL) {
+			i = SIGFPE;
+			code = 0;
+			break;
+		}
+		mtx_lock(&Giant);
+	        i = copyin((const void *)(framep->tf_cr_iip), &bundle, 16);
+		mtx_unlock(&Giant);
+		if (i) {
+			i = SIGBUS;		/* EFAULT, basically */
+			ucode = /*a0*/ 0;	/* exception summary */
+			break;
+		}
+		/* f6-f15 are saved in exception_save */
+		fp_state.bitmask_low64 = 0xffc0;	/* bits 6 - 15 */
+		fp_state.bitmask_high64 = 0x0;
+		fp_state.fp_low_preserved = NULL;
+		fp_state.fp_low_volatile = framep->tf_f;
+		fp_state.fp_high_preserved = NULL;
+		fp_state.fp_high_volatile = NULL;
+		/* The docs are unclear.  Is Fpswa reentrant? */
+		fpswa_ret = fpswa_interface->Fpswa(1, &bundle,
+		    &framep->tf_cr_ipsr, &framep->tf_ar_fpsr,
+		    &framep->tf_cr_isr, &framep->tf_pr,
+		    &framep->tf_cr_ifs, &fp_state);
+		if (fpswa_ret.status == 0) {
+			/* fixed.  update ipsr and iip to next insn */
+			int ei;
+
+			ei = (framep->tf_cr_isr >> 41) & 0x03;
+			if (ei == 0) {		/* no template for this case */
+				framep->tf_cr_ipsr &= ~IA64_ISR_EI;
+				framep->tf_cr_ipsr |= IA64_ISR_EI_1;
+			} else if (ei == 1) {	/* MFI or MFB */
+				framep->tf_cr_ipsr &= ~IA64_ISR_EI;
+				framep->tf_cr_ipsr |= IA64_ISR_EI_2;
+			} else if (ei == 2) {	/* MMF */
+				framep->tf_cr_ipsr &= ~IA64_ISR_EI;
+				framep->tf_cr_iip += 0x10;
+			}
+			goto out;
+		} else if (fpswa_ret.status == -1) {
+			printf("FATAL: FPSWA err1 %lx, err2 %lx, err3 %lx\n",
+			    fpswa_ret.err1, fpswa_ret.err2, fpswa_ret.err3);
+			panic("fpswa fatal error on fp fault");
+		} else if (fpswa_ret.status > 0) {
+#if 0
+			if (fpswa_ret.status & 1) {
+				/*
+				 * New exception needs to be raised.
+				 * If set then the following bits also apply:
+				 * & 2 -> fault was converted to a trap
+				 * & 4 -> SIMD caused the exception
+				 */
+				i = SIGFPE;
+				ucode = /*a0*/ 0;	/* exception summary */
+				break;
+			}
+#endif
 			i = SIGFPE;
 			ucode = /*a0*/ 0;		/* exception summary */
 			break;
+		} else {
+			panic("bad fpswa return code %lx", fpswa_ret.status);
 		}
+	}
+
+	case IA64_VEC_FLOATING_POINT_TRAP:
+	{
+		FP_STATE fp_state;
+		FPSWA_RET fpswa_ret;
+		FPSWA_BUNDLE bundle;
 
 		/* Always fatal in kernel.  Should never happen. */
-		goto dopanic;
+		if (!user)
+			goto dopanic;
+		if (fpswa_interface == NULL) {
+			i = SIGFPE;
+			code = 0;
+			break;
+		}
+		mtx_lock(&Giant);
+	        i = copyin((const void *)(framep->tf_cr_iip), &bundle, 16);
+		mtx_unlock(&Giant);
+		if (i) {
+			i = SIGBUS;			/* EFAULT, basically */
+			ucode = /*a0*/ 0;		/* exception summary */
+			break;
+		}
+		/* f6-f15 are saved in exception_save */
+		fp_state.bitmask_low64 = 0xffc0;	/* bits 6 - 15 */
+		fp_state.bitmask_high64 = 0x0;
+		fp_state.fp_low_preserved = NULL;
+		fp_state.fp_low_volatile = framep->tf_f;
+		fp_state.fp_high_preserved = NULL;
+		fp_state.fp_high_volatile = NULL;
+		/* The docs are unclear.  Is Fpswa reentrant? */
+		fpswa_ret = fpswa_interface->Fpswa(0, &bundle,
+		    &framep->tf_cr_ipsr, &framep->tf_ar_fpsr,
+		    &framep->tf_cr_isr, &framep->tf_pr,
+		    &framep->tf_cr_ifs, &fp_state);
+		if (fpswa_ret.status == 0) {
+			/* fixed */
+			/*
+			 * should we increment iip like the fault case?
+			 * or has fpswa done something like normalizing a
+			 * register so that we should just rerun it?
+			 */
+			goto out;
+		} else if (fpswa_ret.status == -1) {
+			printf("FATAL: FPSWA err1 %lx, err2 %lx, err3 %lx\n",
+			    fpswa_ret.err1, fpswa_ret.err2, fpswa_ret.err3);
+			panic("fpswa fatal error on fp trap");
+		} else if (fpswa_ret.status > 0) {
+			i = SIGFPE;
+			ucode = /*a0*/ 0;		/* exception summary */
+			break;
+		} else {
+			panic("bad fpswa return code %lx", fpswa_ret.status);
+		}
+	}
 
 	case IA64_VEC_DISABLED_FP:
 		/*
diff --git a/sys/ia64/include/efi.h b/sys/ia64/include/efi.h
index 2363812..1ac7154 100644
--- a/sys/ia64/include/efi.h
+++ b/sys/ia64/include/efi.h
@@ -35,6 +35,7 @@
 #include <boot/efi/include/efidevp.h>
 #include <boot/efi/include/eficon.h>
 #include <boot/efi/include/efiapi.h>
+#include <boot/efi/include/efifpswa.h>
 
 extern EFI_SYSTEM_TABLE		*ia64_efi_systab;
 extern EFI_RUNTIME_SERVICES	*ia64_efi_runtime;