* Add a model of the ETRAX interrupt controller.

* Clean up the interrupt handling a bit. * Connect some NOR flash to the test board. git-svn-id: svn://svn.savannah.nongnu.org/qemu/trunk@4055 c046a42c-6fe2-441c-8c8c-71466251a162

* Add a model of the ETRAX interrupt controller.
* Clean up the interrupt handling a bit. * Connect some NOR flash to the test board. git-svn-id: svn://svn.savannah.nongnu.org/qemu/trunk@4055 c046a42c-6fe2-441c-8c8c-71466251a162
edgar_igl
1 parent 1ec6d2ea
Showing 8 changed files with 323 additions and 179 deletions
Makefile.target
cpu-exec.c
hw/etraxfs.c
hw/etraxfs_pic.c
hw/etraxfs_ser.c
hw/etraxfs_timer.c
target-cris/helper.c
translate-all.c
@@ -565,9 +565,12 @@ CPPFLAGS += -DHAS_AUDIO
 endif
 ifeq ($(TARGET_BASE_ARCH), cris)
 OBJS+= etraxfs.o
-OBJS+= ptimer.o
+OBJS+= etraxfs_pic.o
 OBJS+= etraxfs_timer.o
 OBJS+= etraxfs_ser.o
+
+OBJS+= ptimer.o
+OBJS+= pflash_cfi01.o
 endif
 ifeq ($(TARGET_BASE_ARCH), sparc)
 ifeq ($(TARGET_ARCH), sparc64)
@@ -556,7 +556,6 @@ int cpu_exec(CPUState *env1)
 #elif defined(TARGET_CRIS)
                     if (interrupt_request & CPU_INTERRUPT_HARD) {
                         do_interrupt(env);
-			env->interrupt_request &= ~CPU_INTERRUPT_HARD;
                         BREAK_CHAIN;
                     }
 #elif defined(TARGET_M68K)
@@ -1181,10 +1180,6 @@ static inline int handle_cpu_signal(unsigned long pc, unsigned long address,
            a virtual CPU fault */
         cpu_restore_state(tb, env, pc, puc);
     }
-#if 0
-        printf("PF exception: NIP=0x%08x error=0x%x %p\n",
-               env->nip, env->error_code, tb);
-#endif
     /* we restore the process signal mask as the sigreturn should
        do it (XXX: use sigsetjmp) */
     sigprocmask(SIG_SETMASK, old_set, NULL);
@@ -25,89 +25,21 @@
 #include <sys/time.h>
 #include "hw.h"
 #include "sysemu.h"
+#include "flash.h"
 #include "boards.h"
-extern FILE *logfile;
-
 static void main_cpu_reset(void *opaque)
 {
     CPUState *env = opaque;
     cpu_reset(env);
 }
-static uint32_t fs_mmio_readb (void *opaque, target_phys_addr_t addr)
-{
-	CPUState *env = opaque;
-	uint32_t r = 0;
-	printf ("%s %x pc=%x\n", __func__, addr, env->pc);
-	return r;
-}
-static uint32_t fs_mmio_readw (void *opaque, target_phys_addr_t addr)
-{
-	CPUState *env = opaque;
-	uint32_t r = 0;
-	printf ("%s %x pc=%x\n", __func__, addr, env->pc);
-	return r;
-}
-
-static uint32_t fs_mmio_readl (void *opaque, target_phys_addr_t addr)
-{
-	CPUState *env = opaque;
-	uint32_t r = 0;
-	printf ("%s %x p=%x\n", __func__, addr, env->pc);
-	return r;
-}
-
-static void
-fs_mmio_writeb (void *opaque, target_phys_addr_t addr, uint32_t value)
-{
-	CPUState *env = opaque;
-	printf ("%s %x %x pc=%x\n", __func__, addr, value, env->pc);
-}
-static void
-fs_mmio_writew (void *opaque, target_phys_addr_t addr, uint32_t value)
-{
-	CPUState *env = opaque;
-	printf ("%s %x %x pc=%x\n", __func__, addr, value, env->pc);
-}
-static void
-fs_mmio_writel (void *opaque, target_phys_addr_t addr, uint32_t value)
-{
-	CPUState *env = opaque;
-	printf ("%s %x %x pc=%x\n", __func__, addr, value, env->pc);
-}
-
-static CPUReadMemoryFunc *fs_mmio_read[] = {
-    &fs_mmio_readb,
-    &fs_mmio_readw,
-    &fs_mmio_readl,
-};
-
-static CPUWriteMemoryFunc *fs_mmio_write[] = {
-    &fs_mmio_writeb,
-    &fs_mmio_writew,
-    &fs_mmio_writel,
-};
-
-
 /* Init functions for different blocks.  */
+extern qemu_irq *etraxfs_pic_init(CPUState *env, target_ulong base);
+/* TODO: Make these blocks relocate:able.  */
 extern void etraxfs_timer_init(CPUState *env, qemu_irq *irqs);
 extern void etraxfs_ser_init(CPUState *env, qemu_irq *irqs);
-void etrax_ack_irq(CPUState *env, uint32_t mask)
-{
-	env->pending_interrupts &= ~mask;
-}
-
-static void dummy_cpu_set_irq(void *opaque, int irq, int level)
-{
-	CPUState *env = opaque;
-
-	/* Hmm, should this really be done here?  */
-	env->pending_interrupts |= 1 << irq;
-	cpu_interrupt(env, CPU_INTERRUPT_HARD);
-}
-
 static
 void bareetraxfs_init (int ram_size, int vga_ram_size,
                        const char *boot_device, DisplayState *ds,
@@ -115,9 +47,12 @@ void bareetraxfs_init (int ram_size, int vga_ram_size,
                        const char *initrd_filename, const char *cpu_model)
 {
     CPUState *env;
-    qemu_irq *irqs;
+    qemu_irq *pic;
     int kernel_size;
-    int internal_regs;
+    int flash_size = 0x800000;
+    int index;
+    ram_addr_t phys_flash;
+    ram_addr_t phys_ram;
     /* init CPUs */
     if (cpu_model == NULL) {
@@ -126,17 +61,31 @@ void bareetraxfs_init (int ram_size, int vga_ram_size,
     env = cpu_init(cpu_model);
 /*    register_savevm("cpu", 0, 3, cpu_save, cpu_load, env); */
     qemu_register_reset(main_cpu_reset, env);
-    irqs = qemu_allocate_irqs(dummy_cpu_set_irq, env, 32);
-    internal_regs = cpu_register_io_memory(0,
-					   fs_mmio_read, fs_mmio_write, env);
-    /* 0xb0050000 is the last reg.  */
-    cpu_register_physical_memory (0xac000000, 0x4010000, internal_regs);
     /* allocate RAM */
-    cpu_register_physical_memory(0x40000000, ram_size, IO_MEM_RAM);
-
-    etraxfs_timer_init(env, irqs);
-    etraxfs_ser_init(env, irqs);
+    phys_ram = qemu_ram_alloc(ram_size);
+    cpu_register_physical_memory(0x40000000, ram_size, phys_ram | IO_MEM_RAM);
+    /* Unached mapping.  */
+    cpu_register_physical_memory(0xc0000000, ram_size, phys_ram | IO_MEM_RAM);
+
+    phys_flash = qemu_ram_alloc(flash_size);
+    cpu_register_physical_memory(0,flash_size, IO_MEM_ROM);
+    cpu_register_physical_memory(0x80000000, flash_size, IO_MEM_ROM);
+    cpu_register_physical_memory(0x04000000, flash_size, IO_MEM_ROM);
+    cpu_register_physical_memory(0x84000000, flash_size, 
+				 0x04000000 | IO_MEM_ROM);
+    index = drive_get_index(IF_PFLASH, 0, 0);
+    pflash_cfi01_register(0x80000000, flash_size,
+			  drives_table[index].bdrv, 65536, flash_size >> 16,
+			  4, 0x0000, 0x0000, 0x0000, 0x0000);
+    index = drive_get_index(IF_PFLASH, 0, 1);
+    pflash_cfi01_register(0x84000000, flash_size,
+			  drives_table[index].bdrv, 65536, flash_size >> 16,
+			  4, 0x0000, 0x0000, 0x0000, 0x0000);
+
+    pic = etraxfs_pic_init(env, 0xb001c000);
+    etraxfs_timer_init(env, pic);
+    etraxfs_ser_init(env, pic);
     kernel_size = load_image(kernel_filename, phys_ram_base + 0x4000);
     /* magic for boot.  */
@@ -165,14 +114,6 @@ void DMA_run(void)
 {
 }
-void pic_info()
-{
-}
-
-void irq_info()
-{
-}
-
 QEMUMachine bareetraxfs_machine = {
     "bareetraxfs",
     "Bare ETRAX FS board",
+/*
+ * QEMU ETRAX Interrupt Controller.
+ *
+ * Copyright (c) 2008 Edgar E. Iglesias, Axis Communications AB.
+ *
+ * Permission is hereby granted, free of charge, to any person obtaining a copy
+ * of this software and associated documentation files (the "Software"), to deal
+ * in the Software without restriction, including without limitation the rights
+ * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
+ * copies of the Software, and to permit persons to whom the Software is
+ * furnished to do so, subject to the following conditions:
+ *
+ * The above copyright notice and this permission notice shall be included in
+ * all copies or substantial portions of the Software.
+ *
+ * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
+ * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
+ * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL
+ * THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
+ * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+ * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
+ * THE SOFTWARE.
+ */
+
+#include <stdio.h>
+#include "hw.h"
+
+#define D(x)
+
+struct fs_pic_state_t
+{
+	CPUState *env;
+	target_ulong base;
+
+	uint32_t rw_mask;
+	/* Active interrupt lines.  */
+	uint32_t r_vect;
+	/* Active lines, gated through the mask.  */
+	uint32_t r_masked_vect;
+	uint32_t r_nmi;
+	uint32_t r_guru;
+};
+
+static uint32_t pic_readb (void *opaque, target_phys_addr_t addr)
+{
+	return 0;
+}
+static uint32_t pic_readw (void *opaque, target_phys_addr_t addr)
+{
+	return 0;
+}
+
+static uint32_t pic_readl (void *opaque, target_phys_addr_t addr)
+{
+	struct fs_pic_state_t *fs = opaque;
+	uint32_t rval;
+
+	/* Transform this to a relative addr.  */
+	addr -= fs->base;
+	switch (addr)
+	{
+		case 0x0: 
+			rval = fs->rw_mask;
+			break;
+		case 0x4: 
+			rval = fs->r_vect;
+			break;
+		case 0x8: 
+			rval = fs->r_masked_vect;
+			break;
+		case 0xc: 
+			rval = fs->r_nmi;
+			break;
+		case 0x10: 
+			rval = fs->r_guru;
+			break;
+		default:
+			cpu_abort(fs->env, "invalid PIC register.\n");
+			break;
+
+	}
+	D(printf("%s %x=%x\n", __func__, addr, rval));
+	return rval;
+}
+
+static void
+pic_writeb (void *opaque, target_phys_addr_t addr, uint32_t value)
+{
+}
+
+static void
+pic_writew (void *opaque, target_phys_addr_t addr, uint32_t value)
+{
+}
+
+static void
+pic_writel (void *opaque, target_phys_addr_t addr, uint32_t value)
+{
+	struct fs_pic_state_t *fs = opaque;
+	D(printf("%s addr=%x val=%x\n", __func__, addr, value));
+	/* Transform this to a relative addr.  */
+	addr -= fs->base;
+	switch (addr) 
+	{
+		case 0x0: 
+			fs->rw_mask = value;
+			break;
+		case 0x4: 
+			fs->r_vect = value;
+			break;
+		case 0x8: 
+			fs->r_masked_vect = value;
+			break;
+		case 0xc: 
+			fs->r_nmi = value;
+			break;
+		case 0x10: 
+			fs->r_guru = value;
+			break;
+		default:
+			cpu_abort(fs->env, "invalid PIC register.\n");
+			break;
+	}
+}
+
+static CPUReadMemoryFunc *pic_read[] = {
+	&pic_readb,
+	&pic_readw,
+	&pic_readl,
+};
+
+static CPUWriteMemoryFunc *pic_write[] = {
+	&pic_writeb,
+	&pic_writew,
+	&pic_writel,
+};
+
+void pic_info(void)
+{
+}
+
+void irq_info(void)
+{
+}
+
+static void etraxfs_pic_handler(void *opaque, int irq, int level)
+{	
+	struct fs_pic_state_t *fs = (void *)opaque;
+	CPUState *env = fs->env;
+	int i;
+	uint32_t vector = 0;
+
+	D(printf("%s irq=%d level=%d mask=%x v=%x mv=%x\n", 
+		 __func__, irq, level,
+		 fs->rw_mask, fs->r_vect, fs->r_masked_vect));
+
+	fs->r_vect &= ~(1 << irq);
+	fs->r_vect |= (!!level << irq);
+	fs->r_masked_vect = fs->r_vect & fs->rw_mask;
+
+	/* The ETRAX interrupt controller signals interrupts to teh core
+	   through an interrupt request wire and an irq vector bus. If 
+	   multiple interrupts are simultaneously active it chooses vector 
+	   0x30 and lets the sw choose the priorities.  */
+	if (fs->r_masked_vect) {
+		uint32_t mv = fs->r_masked_vect;
+		for (i = 0; i < 31; i++) {
+			if (mv & 1) {
+				vector = 0x31 + i;
+				/* Check for multiple interrupts.  */
+				if (mv > 1)
+					vector = 0x30;
+				break;
+			}
+			mv >>= 1;
+		}
+		if (vector) {
+			env->interrupt_vector = vector;
+			D(printf("%s vector=%x\n", __func__, vector));
+			cpu_interrupt(env, CPU_INTERRUPT_HARD);
+		}
+	} else {
+		env->interrupt_vector = 0;
+		cpu_reset_interrupt(env, CPU_INTERRUPT_HARD);
+		D(printf("%s reset irqs\n", __func__));
+	}
+}
+
+qemu_irq *etraxfs_pic_init(CPUState *env, target_ulong base)
+{
+	struct fs_pic_state_t *fs;
+	qemu_irq *pic;
+	int intr_vect_regs;
+
+	fs = qemu_mallocz(sizeof *fs);
+	if (!fs)
+		return NULL;
+	fs->env = env;
+
+	pic = qemu_allocate_irqs(etraxfs_pic_handler, fs, 30);
+
+	intr_vect_regs = cpu_register_io_memory(0, pic_read, pic_write, fs);
+	cpu_register_physical_memory(base, 0x14, intr_vect_regs);
+	fs->base = base;
+
+	return pic;
+}
@@ -66,7 +66,7 @@ static uint32_t ser_readl (void *opaque, target_phys_addr_t addr)
 			break;
 		default:
-			printf ("%s %x p=%x\n", __func__, addr, env->pc);
+			D(printf ("%s %x p=%x\n", __func__, addr, env->pc));
 			break;
 	}
 	return r;
@@ -100,10 +100,11 @@ ser_writel (void *opaque, target_phys_addr_t addr, uint32_t value)
 				putchar(value);
 			else
 				putchar('.');
+			fflush(stdout);
 			break;
 		default:
-			printf ("%s %x %x pc=%x\n",
-				__func__, addr, value, env->pc);
+			D(printf ("%s %x %x pc=%x\n",
+				  __func__, addr, value, env->pc));
 			break;
 	}
 }
 /*
- * QEMU ETRAX System Emulator
+ * QEMU ETRAX Timers
  *
  * Copyright (c) 2007 Edgar E. Iglesias, Axis Communications AB.
  *
@@ -28,8 +28,6 @@
 #define D(x)
-void etrax_ack_irq(CPUState *env, uint32_t mask);
-
 #define R_TIME 0xb001e038
 #define RW_TMR0_DIV 0xb001e000
 #define R_TMR0_DATA 0xb001e004
@@ -38,16 +36,12 @@ void etrax_ack_irq(CPUState *env, uint32_t mask);
 #define R_TMR1_DATA 0xb001e014
 #define RW_TMR1_CTRL 0xb001e018
+#define RW_WD_CTRL 0xb001e040
 #define RW_INTR_MASK 0xb001e048
 #define RW_ACK_INTR 0xb001e04c
 #define R_INTR 0xb001e050
 #define R_MASKED_INTR 0xb001e054
-
-uint32_t rw_intr_mask;
-uint32_t rw_ack_intr;
-uint32_t r_intr;
-
 struct fs_timer_t {
 	QEMUBH *bh;
 	unsigned int limit;
@@ -57,6 +51,10 @@ struct fs_timer_t {
 	qemu_irq *irq;
 	uint32_t mask;
 	struct timeval last;
+
+	uint32_t rw_intr_mask;
+	uint32_t rw_ack_intr;
+	uint32_t r_intr;
 };
 static struct fs_timer_t timer[2];
@@ -126,13 +124,13 @@ static uint32_t timer_readl (void *opaque, target_phys_addr_t addr)
 	}
 	case RW_INTR_MASK:
-		r = rw_intr_mask;
+		r = timer[t].rw_intr_mask;
 		break;
 	case R_MASKED_INTR:
-		r = r_intr & rw_intr_mask;
+		r = timer[t].r_intr & timer[t].rw_intr_mask;
 		break;
 	default:
-		printf ("%s %x p=%x\n", __func__, addr, env->pc);
+		D(printf ("%s %x p=%x\n", __func__, addr, env->pc));
 		break;
 	}
 	return r;
@@ -167,7 +165,7 @@ static void write_ctrl(struct fs_timer_t *t, uint32_t v)
 	{
 	case 0:
 	case 1:
-		printf ("extern or disabled timer clock?\n");
+		D(printf ("extern or disabled timer clock?\n"));
 		break;
 	case 4: freq_hz =  29493000; break;
 	case 5: freq_hz =  32000000; break;
@@ -178,7 +176,7 @@ static void write_ctrl(struct fs_timer_t *t, uint32_t v)
 		break;
 	}
-	printf ("freq_hz=%d limit=%d\n", freq_hz, t->limit);
+	D(printf ("freq_hz=%d limit=%d\n", freq_hz, t->limit));
 	t->scale = 0;
 	if (t->limit > 2048)
 	{
@@ -186,11 +184,11 @@ static void write_ctrl(struct fs_timer_t *t, uint32_t v)
 		ptimer_set_period(t->ptimer, freq_hz / t->scale);
 	}
-	printf ("op=%d\n", op);
 	switch (op)
 	{
 		case 0:
-			printf ("limit=%d %d\n", t->limit, t->limit/t->scale);
+			D(printf ("limit=%d %d\n", 
+				  t->limit, t->limit/t->scale));
 			ptimer_set_limit(t->ptimer, t->limit / t->scale, 1);
 			break;
 		case 1:
@@ -207,10 +205,8 @@ static void write_ctrl(struct fs_timer_t *t, uint32_t v)
 static void timer_ack_irq(struct fs_timer_t *t)
 {
-	if (!(r_intr & t->mask & rw_intr_mask)) {
+	if (!(t->r_intr & t->mask & t->rw_intr_mask))
 		qemu_irq_lower(t->irq[0]);
-		etrax_ack_irq(t->env, 1 << 0x1b);
-	}
 }
 static void
@@ -239,10 +235,13 @@ timer_writel (void *opaque, target_phys_addr_t addr, uint32_t value)
 			break;
 		case RW_INTR_MASK:
 			D(printf ("RW_INTR_MASK=%x\n", value));
-			rw_intr_mask = value;
+			timer[t].rw_intr_mask = value;
+			break;
+		case RW_WD_CTRL:
+			D(printf ("RW_WD_CTRL=%x\n", value));
 			break;
 		case RW_ACK_INTR:
-			r_intr &= ~value;
+			timer[t].r_intr &= ~value;
 			timer_ack_irq(&timer[t]);
 			break;
 		default:
@@ -267,8 +266,9 @@ static CPUWriteMemoryFunc *timer_write[] = {
 static void timer_irq(void *opaque)
 {
 	struct fs_timer_t *t = opaque;
-	r_intr |= t->mask;
-	if (t->mask & rw_intr_mask) {
+	t->r_intr |= t->mask;
+	if (t->mask & t->rw_intr_mask) {
+		D(printf("%s raise\n", __func__));
 		qemu_irq_raise(t->irq[0]);
 	}
 }
@@ -279,13 +279,13 @@ void etraxfs_timer_init(CPUState *env, qemu_irq *irqs)
 	timer[0].bh = qemu_bh_new(timer_irq, &timer[0]);
 	timer[0].ptimer = ptimer_init(timer[0].bh);
-	timer[0].irq = irqs + 0x1b;
+	timer[0].irq = irqs + 26;
 	timer[0].mask = 1;
 	timer[0].env = env;
 	timer[1].bh = qemu_bh_new(timer_irq, &timer[1]);
 	timer[1].ptimer = ptimer_init(timer[1].bh);
-	timer[1].irq = irqs + 0x1b;
+	timer[1].irq = irqs + 26;
 	timer[1].mask = 1;
 	timer[1].env = env;
@@ -28,6 +28,8 @@
 #include "exec-all.h"
 #include "host-utils.h"
+#define D(x)
+
 #if defined(CONFIG_USER_ONLY)
 void do_interrupt (CPUState *env)
@@ -53,71 +55,68 @@ target_phys_addr_t cpu_get_phys_page_debug(CPUState * env, target_ulong addr)
 #else /* !CONFIG_USER_ONLY */
+
+static void cris_shift_ccs(CPUState *env)
+{
+	uint32_t ccs;
+	/* Apply the ccs shift.  */
+	ccs = env->pregs[PR_CCS];
+	ccs = (ccs & 0xc0000000) | ((ccs << 12) >> 2);
+	env->pregs[PR_CCS] = ccs;
+}
+
 int cpu_cris_handle_mmu_fault (CPUState *env, target_ulong address, int rw,
                                int mmu_idx, int is_softmmu)
 {
 	struct cris_mmu_result_t res;
 	int prot, miss;
+	int r = -1;
 	target_ulong phy;
+	D(printf ("%s addr=%x pc=%x\n", __func__, address, env->pc));
 	address &= TARGET_PAGE_MASK;
 	prot = PAGE_READ | PAGE_WRITE | PAGE_EXEC;
 	miss = cris_mmu_translate(&res, env, address, rw, mmu_idx);
 	if (miss)
 	{
-		/* handle the miss.  */
-		phy = 0;
-		env->exception_index = EXCP_MMU_MISS;
+		env->exception_index = EXCP_MMU_FAULT;
+		env->fault_vector = res.bf_vec;
+		r = 1;
 	}
 	else
 	{
 		phy = res.phy;
+		prot = PAGE_READ | PAGE_WRITE | PAGE_EXEC;
+		r = tlb_set_page(env, address, phy, prot, mmu_idx, is_softmmu);
 	}
-	return tlb_set_page(env, address, phy, prot, mmu_idx, is_softmmu);
-}
-
-
-static void cris_shift_ccs(CPUState *env)
-{
-	uint32_t ccs;
-	/* Apply the ccs shift.  */
-	ccs = env->pregs[PR_CCS];
-	ccs = (ccs & 0xc0000000) | ((ccs << 12) >> 2);
-	env->pregs[PR_CCS] = ccs;
+	D(printf("%s returns %d irqreq=%x addr=%x ismmu=%d\n", 
+			__func__, r, env->interrupt_request, 
+			address, is_softmmu));
+	return r;
 }
 void do_interrupt(CPUState *env)
 {
-	uint32_t ebp, isr;
-	int irqnum;
-
-	fflush(NULL);
+	int ex_vec = -1;
-#if 0
-	printf ("exception index=%d interrupt_req=%d\n",
-		env->exception_index,
-		env->interrupt_request);
-#endif
+	D(fprintf (stderr, "exception index=%d interrupt_req=%d\n",
+		 env->exception_index,
+		 env->interrupt_request));
 	switch (env->exception_index)
 	{
 		case EXCP_BREAK:
-			irqnum = env->trapnr;
-			ebp = env->pregs[PR_EBP];
-			isr = ldl_code(ebp + irqnum * 4);
+			/* These exceptions are genereated by the core itself.
+			   ERP should point to the insn following the brk.  */
+			ex_vec = env->trap_vector;
 			env->pregs[PR_ERP] = env->pc + 2;
-			env->pc = isr;
-
-			cris_shift_ccs(env);
-
 			break;
-		case EXCP_MMU_MISS:
-			irqnum = 4;
-			ebp = env->pregs[PR_EBP];
-			isr = ldl_code(ebp + irqnum * 4);
-			env->pregs[PR_ERP] = env->pc;
-			env->pc = isr;
-			cris_shift_ccs(env);
+
+		case EXCP_MMU_FAULT:
+			/* ERP is already setup by translate-all.c through
+			   re-translation of the aborted TB combined with 
+			   pc searching.  */
+			ex_vec = env->fault_vector;
 			break;
 		default:
@@ -125,34 +124,29 @@ void do_interrupt(CPUState *env)
 			/* Maybe the irq was acked by sw before we got a
 			   change to take it.  */
 			if (env->interrupt_request & CPU_INTERRUPT_HARD) {
-				if (!env->pending_interrupts)
+				/* Vectors below 0x30 are internal
+				   exceptions, i.e not interrupt requests
+				   from the interrupt controller.  */
+				if (env->interrupt_vector < 0x30)
 					return;
+				/* Is the core accepting interrupts?  */
 				if (!(env->pregs[PR_CCS] & I_FLAG)) {
 					return;
 				}
-
-				irqnum = 31 - clz32(env->pending_interrupts);
-				irqnum += 0x30;
-				ebp = env->pregs[PR_EBP];
-				isr = ldl_code(ebp + irqnum * 4);
+				/* The interrupt controller gives us the
+				   vector.  */
+				ex_vec = env->interrupt_vector;
+				/* Normal interrupts are taken between
+				   TB's.  env->pc is valid here.  */
 				env->pregs[PR_ERP] = env->pc;
-				env->pc = isr;
-
-				cris_shift_ccs(env);
-#if 0
-				printf ("%s ebp=%x %x isr=%x %d"
-					" ir=%x pending=%x\n",
-					__func__,
-					ebp, ebp + irqnum * 4,
-					isr, env->exception_index,
-					env->interrupt_request,
-					env->pending_interrupts);
-#endif
 			}
-
 		}
 		break;
 	}
+	env->pc = ldl_code(env->pregs[PR_EBP] + ex_vec * 4);
+	/* Apply the CRIS CCS shift.  */
+	cris_shift_ccs(env);
+	D(printf ("%s ebp=%x isr=%x vec=%x\n", __func__, ebp, isr, ex_vec));
 }
 target_phys_addr_t cpu_get_phys_page_debug(CPUState * env, target_ulong addr)
@@ -163,6 +157,7 @@ target_phys_addr_t cpu_get_phys_page_debug(CPUState * env, target_ulong addr)
 	miss = cris_mmu_translate(&res, env, addr, 0, 0);
 	if (!miss)
 		phy = res.phy;
+	D(fprintf(stderr, "%s %x -> %x\n", __func__, addr, phy));
 	return phy;
 }
 #endif
@@ -286,6 +286,8 @@ int cpu_restore_state(TranslationBlock *tb,
 #elif defined(TARGET_SH4)
     env->pc = gen_opc_pc[j];
     env->flags = gen_opc_hflags[j];
+#elif defined(TARGET_CRIS)
+    env->pregs[PR_ERP] = gen_opc_pc[j];
 #endif
 #ifdef CONFIG_PROFILER