3 files changed, 10 insertions, 7 deletions
diff --git a/pk/fp.c b/pk/fp.c
index 55e416d..587cc3f 100644
--- a/pk/fp.c
+++ b/pk/fp.c
@@ -27,7 +27,11 @@ validate_address(trapframe_t* tf, long addr, int size, int store)
 int emulate_fp(trapframe_t* tf)
 {
   if(have_fp)
+  {
+    if(!(mfpcr(PCR_SR) & SR_EF))
+      init_fp();
     fp_state.fsr = get_fp_state(fp_state.fpr);
+  }
 
   if(noisy)
     printk("FPU emulation at pc %lx, insn %x\n",tf->epc,(uint32_t)tf->insn);
@@ -257,10 +261,12 @@ get_fp_reg(unsigned int which, unsigned int dp)
 
 #endif
 
-void init_fp_regs()
+void init_fp(trapframe_t* tf)
 {
   long sr = mfpcr(PCR_SR);
   mtpcr(sr | SR_EF, PCR_SR);
+
   put_fp_state(fp_state.fpr,fp_state.fsr);
-  mtpcr(sr, PCR_SR);
+
+  tf->sr |= SR_EF;
 }
diff --git a/pk/handlers.c b/pk/handlers.c
index 2947903..cb580e6 100644
--- a/pk/handlers.c
+++ b/pk/handlers.c
@@ -13,10 +13,7 @@ void handle_breakpoint(trapframe_t* tf)
 void handle_fp_disabled(trapframe_t* tf)
 {
   if(have_fp)
-  {
-    init_fp_regs();
-    tf->sr |= SR_EF;
-  }
+    init_fp();
   else
   {
 #ifdef PK_ENABLE_FP_EMULATION
diff --git a/pk/pk.h b/pk/pk.h
index a1fd0da..cf3d7fb 100644
--- a/pk/pk.h
+++ b/pk/pk.h
@@ -26,7 +26,7 @@ extern "C" {
 
 extern int have_fp;
 int emulate_fp(trapframe_t*);
-void init_fp_regs();
+void init_fp();
 
 void printk(const char* s, ...);
 void init_tf(trapframe_t*, long pc, long sp);