1 files changed, 66 insertions, 25 deletions

diff --git a/target-sh4/helper.c b/target-sh4/helper.c
index a5af5ba15..52bef2fca 100644
--- a/target-sh4/helper.c
+++ b/target-sh4/helper.c
@@ -27,6 +27,7 @@
 
 #include "cpu.h"
 #include "exec-all.h"
+#include "hw/sh_intc.h"
 
 #if defined(CONFIG_USER_ONLY)
 
@@ -36,19 +37,19 @@ void do_interrupt (CPUState *env)
 }
 
 int cpu_sh4_handle_mmu_fault(CPUState * env, target_ulong address, int rw,
-			     int is_user, int is_softmmu)
+			     int mmu_idx, int is_softmmu)
 {
     env->tea = address;
+    env->exception_index = 0;
     switch (rw) {
     case 0:
+	env->tea = address;
         env->exception_index = 0x0a0;
         break;
     case 1:
+	env->tea = address;
         env->exception_index = 0x0c0;
         break;
-    case 2:
-        env->exception_index = 0x0a0;
-        break;
     }
     return 1;
 }
@@ -74,6 +75,31 @@ target_phys_addr_t cpu_get_phys_page_debug(CPUState * env, target_ulong addr)
 
 void do_interrupt(CPUState * env)
 {
+    int do_irq = env->interrupt_request & CPU_INTERRUPT_HARD;
+    int do_exp, irq_vector = env->exception_index;
+
+    /* prioritize exceptions over interrupts */
+
+    do_exp = env->exception_index != -1;
+    do_irq = do_irq && (env->exception_index == -1);
+
+    if (env->sr & SR_BL) {
+        if (do_exp && env->exception_index != 0x1e0) {
+            env->exception_index = 0x000; /* masked exception -> reset */
+        }
+        if (do_irq) {
+            return; /* masked */
+        }
+    }
+
+    if (do_irq) {
+        irq_vector = sh_intc_get_pending_vector(env->intc_handle,
+						(env->sr >> 4) & 0xf);
+        if (irq_vector == -1) {
+            return; /* masked */
+	}
+    }
+
     if (loglevel & CPU_LOG_INT) {
 	const char *expname;
 	switch (env->exception_index) {
@@ -117,32 +143,47 @@ void do_interrupt(CPUState * env)
 	    expname = "trapa";
 	    break;
 	default:
-	    expname = "???";
-	    break;
+            expname = do_irq ? "interrupt" : "???";
+            break;
 	}
 	fprintf(logfile, "exception 0x%03x [%s] raised\n",
-		env->exception_index, expname);
+		irq_vector, expname);
 	cpu_dump_state(env, logfile, fprintf, 0);
     }
 
     env->ssr = env->sr;
-    env->spc = env->spc;
+    env->spc = env->pc;
     env->sgr = env->gregs[15];
     env->sr |= SR_BL | SR_MD | SR_RB;
 
-    env->expevt = env->exception_index & 0x7ff;
-    switch (env->exception_index) {
-    case 0x040:
-    case 0x060:
-    case 0x080:
-	env->pc = env->vbr + 0x400;
-	break;
-    case 0x140:
-	env->pc = 0xa0000000;
-	break;
-    default:
-	env->pc = env->vbr + 0x100;
-	break;
+    if (do_exp) {
+        env->expevt = env->exception_index;
+        switch (env->exception_index) {
+        case 0x000:
+        case 0x020:
+        case 0x140:
+            env->sr &= ~SR_FD;
+            env->sr |= 0xf << 4; /* IMASK */
+            env->pc = 0xa0000000;
+            break;
+        case 0x040:
+        case 0x060:
+            env->pc = env->vbr + 0x400;
+            break;
+        case 0x160:
+            env->spc += 2; /* special case for TRAPA */
+            /* fall through */
+        default:
+            env->pc = env->vbr + 0x100;
+            break;
+        }
+        return;
+    }
+
+    if (do_irq) {
+        env->intevt = irq_vector;
+        env->pc = env->vbr + 0x600;
+        return;
     }
 }
 
@@ -372,15 +413,15 @@ int get_physical_address(CPUState * env, target_ulong * physical,
 }
 
 int cpu_sh4_handle_mmu_fault(CPUState * env, target_ulong address, int rw,
-			     int is_user, int is_softmmu)
+			     int mmu_idx, int is_softmmu)
 {
     target_ulong physical, page_offset, page_size;
     int prot, ret, access_type;
 
     /* XXXXX */
 #if 0
-    fprintf(stderr, "%s pc %08x ad %08x rw %d is_user %d smmu %d\n",
-	    __func__, env->pc, address, rw, is_user, is_softmmu);
+    fprintf(stderr, "%s pc %08x ad %08x rw %d mmu_idx %d smmu %d\n",
+	    __func__, env->pc, address, rw, mmu_idx, is_softmmu);
 #endif
 
     access_type = ACCESS_INT;
@@ -426,7 +467,7 @@ int cpu_sh4_handle_mmu_fault(CPUState * env, target_ulong address, int rw,
     address = (address & TARGET_PAGE_MASK) + page_offset;
     physical = (physical & TARGET_PAGE_MASK) + page_offset;
 
-    return tlb_set_page(env, address, physical, prot, is_user, is_softmmu);
+    return tlb_set_page(env, address, physical, prot, mmu_idx, is_softmmu);
 }
 
 target_phys_addr_t cpu_get_phys_page_debug(CPUState * env, target_ulong addr)