1 files changed, 10 insertions, 8 deletions

diff --git a/target-m68k/op_helper.c b/target-m68k/op_helper.c
index 8086238db4..4c423ca984 100644
--- a/target-m68k/op_helper.c
+++ b/target-m68k/op_helper.c
@@ -87,6 +87,7 @@ static void do_rte(void)
     env->pc = ldl_kernel(sp + 4);
     sp |= (fmt >> 28) & 3;
     env->sr = fmt & 0xffff;
+    m68k_switch_sp(env);
     env->aregs[7] = sp + 8;
 }
 
@@ -128,9 +129,6 @@ void do_interrupt(int is_hw)
         }
     }
 
-    /* TODO: Implement USP.  */
-    sp = env->aregs[7];
-
     vector = env->exception_index << 2;
 
     fmt |= 0x40000000;
@@ -138,6 +136,15 @@ void do_interrupt(int is_hw)
     fmt |= vector << 16;
     fmt |= env->sr;
 
+    env->sr |= SR_S;
+    if (is_hw) {
+        env->sr = (env->sr & ~SR_I) | (env->pending_level << SR_I_SHIFT);
+        env->sr &= ~SR_M;
+    }
+    m68k_switch_sp(env);
+
+    sp = env->aregs[7];
+
     /* ??? This could cause MMU faults.  */
     sp &= ~3;
     sp -= 4;
@@ -145,11 +152,6 @@ void do_interrupt(int is_hw)
     sp -= 4;
     stl_kernel(sp, fmt);
     env->aregs[7] = sp;
-    env->sr |= SR_S;
-    if (is_hw) {
-        env->sr = (env->sr & ~SR_I) | (env->pending_level << SR_I_SHIFT);
-        env->sr &= ~SR_M;
-    }
     /* Jump to vector.  */
     env->pc = ldl_kernel(env->vbr + vector);
 }