[POWERPC] Floating-Point Exception trap support

Add support for Floating-Point Exception traps on 32 and 64 bit platforms.
Also make sure to clean FPSCR on EXEC and thread exit

Author of initial version: Renato Riolino <renato.riolino@eldorad.org.br>

Reviewed by:	jhibbits
Sponsored by:	Eldorado Research Institute (eldorado.org.br)
Differential Revision:	https://reviews.freebsd.org/D23623

6 files changed, 119 insertions, 15 deletions

diff --git a/sys/powerpc/include/cpufunc.h b/sys/powerpc/include/cpufunc.h
index 7f332aa49118..1b94d565e67d 100644
--- a/sys/powerpc/include/cpufunc.h
+++ b/sys/powerpc/include/cpufunc.h
@@ -163,6 +163,25 @@ mttb(u_quad_t time)
 	mtspr(TBR_TBWL, (uint32_t)(time & 0xffffffff));
 }
 
+
+static __inline register_t
+mffs(void)
+{
+	register_t value;
+
+	__asm __volatile ("mffs 0; stfd 0,0(%0)"
+			:: "b"(&value));
+
+	return (value);
+}
+
+static __inline void
+mtfsf(register_t value)
+{
+	__asm __volatile ("lfd 0,0(%0); mtfsf 0xff,0"
+			:: "b"(&value));
+}
+
 static __inline void
 eieio(void)
 {
diff --git a/sys/powerpc/include/fpu.h b/sys/powerpc/include/fpu.h
index 1fb07c6cc599..19f0cb0c9a65 100644
--- a/sys/powerpc/include/fpu.h
+++ b/sys/powerpc/include/fpu.h
@@ -75,6 +75,8 @@
 void    enable_fpu(struct thread *);
 void    save_fpu(struct thread *);
 void    save_fpu_nodrop(struct thread *);
+void    cleanup_fpscr(void);
+u_int   get_fpu_exception(struct thread *);
 
 #endif /* _KERNEL */
 
diff --git a/sys/powerpc/include/psl.h b/sys/powerpc/include/psl.h
index 1179d7441865..9d1c746de741 100644
--- a/sys/powerpc/include/psl.h
+++ b/sys/powerpc/include/psl.h
@@ -88,7 +88,7 @@
 #define	PSL_FE_NONREC	PSL_FE1		/* imprecise non-recoverable */
 #define	PSL_FE_REC	PSL_FE0		/* imprecise recoverable */
 #define	PSL_FE_PREC	(PSL_FE0 | PSL_FE1) /* precise */
-#define	PSL_FE_DFLT	PSL_FE_DIS	/* default == none */
+#define	PSL_FE_DFLT	PSL_FE_PREC	/* default == precise */
 
 #ifndef LOCORE
 extern register_t psl_kernset;		/* Default MSR values for kernel */
diff --git a/sys/powerpc/powerpc/exec_machdep.c b/sys/powerpc/powerpc/exec_machdep.c
index 5df7f38aa3d0..2a2fffbdd6ef 100644
--- a/sys/powerpc/powerpc/exec_machdep.c
+++ b/sys/powerpc/powerpc/exec_machdep.c
@@ -239,10 +239,13 @@ sendsig(sig_t catcher, ksiginfo_t *ksi, sigset_t *mask)
 		usfp = (void *)((sp - rndfsize) & ~0xFul);
 	}
 
-	/*
-	 * Save the floating-point state, if necessary, then copy it.
+	/* 
+	 * Set Floating Point facility to "Ignore Exceptions Mode" so signal
+	 * handler can run. 
 	 */
-	/* XXX */
+	if (td->td_pcb->pcb_flags & PCB_FPU)
+		tf->srr1 = tf->srr1 & ~(PSL_FE0 | PSL_FE1);
+
 
 	/*
 	 * Set up the registers to return to sigcode.
@@ -333,6 +336,13 @@ sys_sigreturn(struct thread *td, struct sigreturn_args *uap)
 
 	kern_sigprocmask(td, SIG_SETMASK, &uc.uc_sigmask, NULL, 0);
 
+	/* 
+	 * Save FPU state if needed. User may have changed it on
+	 * signal handler 
+	 */ 
+	if (uc.uc_mcontext.mc_srr1 & PSL_FP)
+		save_fpu(td);
+
 	CTR3(KTR_SIG, "sigreturn: return td=%p pc=%#x sp=%#x",
 	     td, uc.uc_mcontext.mc_srr0, uc.uc_mcontext.mc_gpr[1]);
 
@@ -556,6 +566,8 @@ cleanup_power_extras(struct thread *td)
 		mtspr(SPR_FSCR, 0);
 	if (pcb_flags & PCB_CDSCR) 
 		mtspr(SPR_DSCRP, 0);
+
+	cleanup_fpscr();
 }
 
 /*
@@ -826,6 +838,14 @@ freebsd32_sigreturn(struct thread *td, struct freebsd32_sigreturn_args *uap)
 
 	kern_sigprocmask(td, SIG_SETMASK, &uc.uc_sigmask, NULL, 0);
 
+	/*
+	 * Save FPU state if needed. User may have changed it on
+	 * signal handler
+	 */
+	if (uc.uc_mcontext.mc_srr1 & PSL_FP)
+		save_fpu(td);
+
+
 	CTR3(KTR_SIG, "sigreturn: return td=%p pc=%#x sp=%#x",
 	     td, uc.uc_mcontext.mc_srr0, uc.uc_mcontext.mc_gpr[1]);
 
diff --git a/sys/powerpc/powerpc/fpu.c b/sys/powerpc/powerpc/fpu.c
index f0deca65bb06..1bb9f3de07b5 100644
--- a/sys/powerpc/powerpc/fpu.c
+++ b/sys/powerpc/powerpc/fpu.c
@@ -48,7 +48,7 @@ __FBSDID("$FreeBSD$");
 static void
 save_fpu_int(struct thread *td)
 {
-	int	msr;
+	register_t msr;
 	struct	pcb *pcb;
 
 	pcb = td->td_pcb;
@@ -102,7 +102,7 @@ save_fpu_int(struct thread *td)
 void
 enable_fpu(struct thread *td)
 {
-	int	msr;
+	register_t msr;
 	struct	pcb *pcb;
 	struct	trapframe *tf;
 
@@ -208,3 +208,58 @@ save_fpu_nodrop(struct thread *td)
 	if (td == PCPU_GET(fputhread))
 		save_fpu_int(td);
 }
+
+
+/*
+ * Clear Floating-Point Status and Control Register
+ */
+void
+cleanup_fpscr()
+{
+	register_t msr;
+	msr = mfmsr();
+	mtmsr(msr | PSL_FP | PSL_VSX);
+
+	mtfsf(0);
+
+	isync();
+	mtmsr(msr);
+}
+
+
+/*
+ *  * Returns the current fp exception
+ *   */
+u_int
+get_fpu_exception(struct thread *td)
+{
+	register_t msr;
+	u_int ucode;
+	register_t reg;
+
+	critical_enter();
+
+	msr = mfmsr();
+	mtmsr(msr | PSL_FP);
+
+	reg = mffs();
+
+	isync();
+	mtmsr(msr);
+
+	critical_exit();
+
+	if (reg & FPSCR_ZX)
+		ucode = FPE_FLTDIV;
+	else if (reg & FPSCR_OX)
+		ucode = FPE_FLTOVF;
+	else if (reg & FPSCR_UX)
+		ucode = FPE_FLTUND;
+	else if (reg & FPSCR_XX)
+		ucode = FPE_FLTRES;
+	else
+		ucode = FPE_FLTINV;
+
+	return ucode;
+}
+
diff --git a/sys/powerpc/powerpc/trap.c b/sys/powerpc/powerpc/trap.c
index 1501c051dc82..5de67dd4ef6c 100644
--- a/sys/powerpc/powerpc/trap.c
+++ b/sys/powerpc/powerpc/trap.c
@@ -405,16 +405,24 @@ trap(struct trapframe *frame)
 #endif
  				sig = SIGTRAP;
 				ucode = TRAP_BRKPT;
-			} else {
+				break;
+			}
+
+			if ((frame->srr1 & EXC_PGM_FPENABLED) &&
+			     (td->td_pcb->pcb_flags & PCB_FPU))
+				sig = SIGFPE;
+			else
 				sig = ppc_instr_emulate(frame, td);
-				if (sig == SIGILL) {
-					if (frame->srr1 & EXC_PGM_PRIV)
-						ucode = ILL_PRVOPC;
-					else if (frame->srr1 & EXC_PGM_ILLEGAL)
-						ucode = ILL_ILLOPC;
-				} else if (sig == SIGFPE)
-					ucode = FPE_FLTINV;	/* Punt for now, invalid operation. */
+
+			if (sig == SIGILL) {
+				if (frame->srr1 & EXC_PGM_PRIV)
+					ucode = ILL_PRVOPC;
+				else if (frame->srr1 & EXC_PGM_ILLEGAL)
+					ucode = ILL_ILLOPC;
+			} else if (sig == SIGFPE) {
+				ucode = get_fpu_exception(td);
 			}
+
 			break;
 
 		case EXC_MCHK:
@@ -964,7 +972,7 @@ fix_unaligned(struct thread *td, struct trapframe *frame)
 static void
 normalize_inputs(void)
 {
-	unsigned long msr;
+	register_t msr;
 
 	/* enable VSX */
 	msr = mfmsr();