From 32188a03dae09c8a02bc72f26ece4a98183cc787 Mon Sep 17 00:00:00 2001
From: Nathan Froyd <froydnj@codesourcery.com>
Date: Tue, 8 Dec 2009 08:06:23 -0800
Subject: [PATCH] target-mips: change interrupt bits to be mips16-aware

We need to stash the operating mode into the low bit of the error PC and
restore it on return from interrupts.

Signed-off-by: Nathan Froyd <froydnj@codesourcery.com>
Signed-off-by: Aurelien Jarno <aurelien@aurel32.net>
---
 target-mips/helper.c    | 51 ++++++++++++++++++++++++-----------------
 target-mips/op_helper.c | 17 +++++++++++---
 2 files changed, 44 insertions(+), 24 deletions(-)

diff --git a/target-mips/helper.c b/target-mips/helper.c
index 1e7e016084..903987bb83 100644
--- a/target-mips/helper.c
+++ b/target-mips/helper.c
@@ -369,6 +369,24 @@ static const char * const excp_names[EXCP_LAST + 1] = {
     [EXCP_CACHE] = "cache error",
 };
 
+#if !defined(CONFIG_USER_ONLY)
+static target_ulong exception_resume_pc (CPUState *env)
+{
+    target_ulong bad_pc;
+    target_ulong isa_mode;
+
+    isa_mode = !!(env->hflags & MIPS_HFLAG_M16);
+    bad_pc = env->active_tc.PC | isa_mode;
+    if (env->hflags & MIPS_HFLAG_BMASK) {
+        /* If the exception was raised from a delay slot, come back to
+           the jump.  */
+        bad_pc -= (env->hflags & MIPS_HFLAG_B16 ? 2 : 4);
+    }
+
+    return bad_pc;
+}
+#endif
+
 void do_interrupt (CPUState *env)
 {
 #if !defined(CONFIG_USER_ONLY)
@@ -396,7 +414,7 @@ void do_interrupt (CPUState *env)
            resume will always occur on the next instruction
            (but we assume the pc has always been updated during
            code translation). */
-        env->CP0_DEPC = env->active_tc.PC;
+        env->CP0_DEPC = env->active_tc.PC | !!(env->hflags & MIPS_HFLAG_M16);
         goto enter_debug_mode;
     case EXCP_DINT:
         env->CP0_Debug |= 1 << CP0DB_DINT;
@@ -413,14 +431,8 @@ void do_interrupt (CPUState *env)
     case EXCP_DDBL:
         env->CP0_Debug |= 1 << CP0DB_DDBL;
     set_DEPC:
-        if (env->hflags & MIPS_HFLAG_BMASK) {
-            /* If the exception was raised from a delay slot,
-               come back to the jump.  */
-            env->CP0_DEPC = env->active_tc.PC - 4;
-            env->hflags &= ~MIPS_HFLAG_BMASK;
-        } else {
-            env->CP0_DEPC = env->active_tc.PC;
-        }
+        env->CP0_DEPC = exception_resume_pc(env);
+        env->hflags &= ~MIPS_HFLAG_BMASK;
  enter_debug_mode:
         env->hflags |= MIPS_HFLAG_DM | MIPS_HFLAG_64 | MIPS_HFLAG_CP0;
         env->hflags &= ~(MIPS_HFLAG_KSU);
@@ -428,6 +440,8 @@ void do_interrupt (CPUState *env)
         if (!(env->CP0_Status & (1 << CP0St_EXL)))
             env->CP0_Cause &= ~(1 << CP0Ca_BD);
         env->active_tc.PC = (int32_t)0xBFC00480;
+        /* Exception handlers are entered in 32-bit mode.  */
+        env->hflags &= ~(MIPS_HFLAG_M16);
         break;
     case EXCP_RESET:
         cpu_reset(env);
@@ -439,20 +453,16 @@ void do_interrupt (CPUState *env)
     case EXCP_NMI:
         env->CP0_Status |= (1 << CP0St_NMI);
  set_error_EPC:
-        if (env->hflags & MIPS_HFLAG_BMASK) {
-            /* If the exception was raised from a delay slot,
-               come back to the jump.  */
-            env->CP0_ErrorEPC = env->active_tc.PC - 4;
-            env->hflags &= ~MIPS_HFLAG_BMASK;
-        } else {
-            env->CP0_ErrorEPC = env->active_tc.PC;
-        }
+        env->CP0_ErrorEPC = exception_resume_pc(env);
+        env->hflags &= ~MIPS_HFLAG_BMASK;
         env->CP0_Status |= (1 << CP0St_ERL) | (1 << CP0St_BEV);
         env->hflags |= MIPS_HFLAG_64 | MIPS_HFLAG_CP0;
         env->hflags &= ~(MIPS_HFLAG_KSU);
         if (!(env->CP0_Status & (1 << CP0St_EXL)))
             env->CP0_Cause &= ~(1 << CP0Ca_BD);
         env->active_tc.PC = (int32_t)0xBFC00000;
+        /* Exception handlers are entered in 32-bit mode.  */
+        env->hflags &= ~(MIPS_HFLAG_M16);
         break;
     case EXCP_EXT_INTERRUPT:
         cause = 0;
@@ -554,13 +564,10 @@ void do_interrupt (CPUState *env)
         }
  set_EPC:
         if (!(env->CP0_Status & (1 << CP0St_EXL))) {
+            env->CP0_EPC = exception_resume_pc(env);
             if (env->hflags & MIPS_HFLAG_BMASK) {
-                /* If the exception was raised from a delay slot,
-                   come back to the jump.  */
-                env->CP0_EPC = env->active_tc.PC - 4;
                 env->CP0_Cause |= (1 << CP0Ca_BD);
             } else {
-                env->CP0_EPC = env->active_tc.PC;
                 env->CP0_Cause &= ~(1 << CP0Ca_BD);
             }
             env->CP0_Status |= (1 << CP0St_EXL);
@@ -574,6 +581,8 @@ void do_interrupt (CPUState *env)
             env->active_tc.PC = (int32_t)(env->CP0_EBase & ~0x3ff);
         }
         env->active_tc.PC += offset;
+        /* Exception handlers are entered in 32-bit mode.  */
+        env->hflags &= ~(MIPS_HFLAG_M16);
         env->CP0_Cause = (env->CP0_Cause & ~(0x1f << CP0Ca_EC)) | (cause << CP0Ca_EC);
         break;
     default:
diff --git a/target-mips/op_helper.c b/target-mips/op_helper.c
index be75af5e6e..cccfd8e07e 100644
--- a/target-mips/op_helper.c
+++ b/target-mips/op_helper.c
@@ -1684,14 +1684,24 @@ static void debug_post_eret (void)
     }
 }
 
+static void set_pc (target_ulong error_pc)
+{
+    env->active_tc.PC = error_pc & ~(target_ulong)1;
+    if (error_pc & 1) {
+        env->hflags |= MIPS_HFLAG_M16;
+    } else {
+        env->hflags &= ~(MIPS_HFLAG_M16);
+    }
+}
+
 void helper_eret (void)
 {
     debug_pre_eret();
     if (env->CP0_Status & (1 << CP0St_ERL)) {
-        env->active_tc.PC = env->CP0_ErrorEPC;
+        set_pc(env->CP0_ErrorEPC);
         env->CP0_Status &= ~(1 << CP0St_ERL);
     } else {
-        env->active_tc.PC = env->CP0_EPC;
+        set_pc(env->CP0_EPC);
         env->CP0_Status &= ~(1 << CP0St_EXL);
     }
     compute_hflags(env);
@@ -1702,7 +1712,8 @@ void helper_eret (void)
 void helper_deret (void)
 {
     debug_pre_eret();
-    env->active_tc.PC = env->CP0_DEPC;
+    set_pc(env->CP0_DEPC);
+
     env->hflags &= MIPS_HFLAG_DM;
     compute_hflags(env);
     debug_post_eret();