Index: head/sys/riscv/include/fpe.h
===================================================================
--- head/sys/riscv/include/fpe.h
+++ head/sys/riscv/include/fpe.h
@@ -34,5 +34,6 @@
 #define	_MACHINE_FPE_H_
 
 void fpe_state_save(struct thread *td);
+void fpe_state_clear(void);
 
 #endif /* !_MACHINE_FPE_H_ */
Index: head/sys/riscv/riscv/machdep.c
===================================================================
--- head/sys/riscv/riscv/machdep.c
+++ head/sys/riscv/riscv/machdep.c
@@ -204,13 +204,14 @@
 		 * If we have just been running FPE instructions we will
 		 * need to save the state to memcpy it below.
 		 */
-		fpe_state_save(td);
+		if (td == curthread)
+			fpe_state_save(td);
 
 		memcpy(regs->fp_x, pcb->pcb_x, sizeof(regs->fp_x));
 		regs->fp_fcsr = pcb->pcb_fcsr;
 	} else
 #endif
-		memset(regs->fp_x, 0, sizeof(regs->fp_x));
+		memset(regs, 0, sizeof(*regs));
 
 	return (0);
 }
@@ -219,12 +220,17 @@
 set_fpregs(struct thread *td, struct fpreg *regs)
 {
 #ifdef FPE
+	struct trapframe *frame;
 	struct pcb *pcb;
 
+	frame = td->td_frame;
 	pcb = td->td_pcb;
 
 	memcpy(pcb->pcb_x, regs->fp_x, sizeof(regs->fp_x));
 	pcb->pcb_fcsr = regs->fp_fcsr;
+	pcb->pcb_fpflags |= PCB_FP_STARTED;
+	frame->tf_sstatus &= ~SSTATUS_FS_MASK;
+	frame->tf_sstatus |= SSTATUS_FS_CLEAN;
 #endif
 
 	return (0);
Index: head/sys/riscv/riscv/swtch.S
===================================================================
--- head/sys/riscv/riscv/swtch.S
+++ head/sys/riscv/riscv/swtch.S
@@ -154,6 +154,59 @@
 #endif /* FPE */
 
 /*
+ * void
+ * fpe_state_clear(void)
+ */
+ENTRY(fpe_state_clear)
+	/*
+	 * Enable FPE usage in supervisor mode,
+	 * so we can access registers.
+	 */
+	li	t0, SSTATUS_FS_INITIAL
+	csrs	sstatus, t0
+
+	fscsr	zero
+	fcvt.d.l f0, zero
+	fcvt.d.l f1, zero
+	fcvt.d.l f2, zero
+	fcvt.d.l f3, zero
+	fcvt.d.l f4, zero
+	fcvt.d.l f5, zero
+	fcvt.d.l f6, zero
+	fcvt.d.l f7, zero
+	fcvt.d.l f8, zero
+	fcvt.d.l f9, zero
+	fcvt.d.l f10, zero
+	fcvt.d.l f11, zero
+	fcvt.d.l f12, zero
+	fcvt.d.l f13, zero
+	fcvt.d.l f14, zero
+	fcvt.d.l f15, zero
+	fcvt.d.l f16, zero
+	fcvt.d.l f17, zero
+	fcvt.d.l f18, zero
+	fcvt.d.l f19, zero
+	fcvt.d.l f20, zero
+	fcvt.d.l f21, zero
+	fcvt.d.l f22, zero
+	fcvt.d.l f23, zero
+	fcvt.d.l f24, zero
+	fcvt.d.l f25, zero
+	fcvt.d.l f26, zero
+	fcvt.d.l f27, zero
+	fcvt.d.l f28, zero
+	fcvt.d.l f29, zero
+	fcvt.d.l f30, zero
+	fcvt.d.l f31, zero
+
+	/* Disable FPE usage in supervisor mode. */
+	li	t0, SSTATUS_FS_MASK
+	csrc	sstatus, t0
+
+	ret
+END(fpe_state_clear)
+	
+/*
  * void cpu_throw(struct thread *old, struct thread *new)
  */
 ENTRY(cpu_throw)
Index: head/sys/riscv/riscv/trap.c
===================================================================
--- head/sys/riscv/riscv/trap.c
+++ head/sys/riscv/riscv/trap.c
@@ -57,6 +57,9 @@
 #include <vm/vm_param.h>
 #include <vm/vm_extern.h>
 
+#ifdef FPE
+#include <machine/fpe.h>
+#endif
 #include <machine/frame.h>
 #include <machine/pcb.h>
 #include <machine/pcpu.h>
@@ -363,7 +366,9 @@
 			 * May be a FPE trap. Enable FPE usage
 			 * for this thread and try again.
 			 */
-			frame->tf_sstatus |= SSTATUS_FS_INITIAL;
+			fpe_state_clear();
+			frame->tf_sstatus &= ~SSTATUS_FS_MASK;
+			frame->tf_sstatus |= SSTATUS_FS_CLEAN;
 			pcb->pcb_fpflags |= PCB_FP_STARTED;
 			break;
 		}