From 7a066dd5a5245e7424cf15acb8924fa3478f5758 Mon Sep 17 00:00:00 2001
From: Daniel Beer <dlbeer@gmail.com>
Date: Thu, 10 Mar 2011 14:40:31 +1300
Subject: [PATCH] simio: implemented interrupt handling.

---
 sim.c          | 30 +++++++++++++++++++++++++++---
 simio_cpu.h    |  3 +++
 simio_tracer.c |  6 ++++--
 3 files changed, 34 insertions(+), 5 deletions(-)

diff --git a/sim.c b/sim.c
index bafb794..e1c52a7 100644
--- a/sim.c
+++ b/sim.c
@@ -394,6 +394,7 @@ static int step_single(struct sim_device *dev, uint16_t ins)
 		dev->regs[MSP430_REG_PC] =
 			MEM_GETW(dev, dev->regs[MSP430_REG_SP]);
 		dev->regs[MSP430_REG_SP] += 2;
+		cycles = 5;
 		break;
 
 	default:
@@ -492,14 +493,37 @@ static int step_cpu(struct sim_device *dev)
 static int step_system(struct sim_device *dev)
 {
 	int count = 1;
+	int irq;
+	uint16_t status = dev->regs[MSP430_REG_SR];
 
-	if (!(dev->regs[MSP430_REG_SR] & MSP430_SR_CPUOFF)) {
+	irq = simio_check_interrupt();
+	if ((status & MSP430_SR_GIE) && irq >= 0) {
+		if (irq >= 16) {
+			printc_err("sim: invalid interrupt number: %d\n", irq);
+			return -1;
+		}
+
+		dev->regs[MSP430_REG_SP] -= 2;
+		MEM_SETW(dev, dev->regs[MSP430_REG_SP],
+			 dev->regs[MSP430_REG_PC]);
+
+		dev->regs[MSP430_REG_SP] -= 2;
+		MEM_SETW(dev, dev->regs[MSP430_REG_SP],
+			 dev->regs[MSP430_REG_SR]);
+
+		dev->regs[MSP430_REG_SR] &=
+			~(MSP430_SR_GIE | MSP430_SR_CPUOFF);
+		dev->regs[MSP430_REG_PC] = MEM_GETW(dev, 0xffe0 + irq * 2);
+
+		simio_ack_interrupt(irq);
+		count = 6;
+	} else if (!(status & MSP430_SR_CPUOFF)) {
 		count = step_cpu(dev);
 		if (count < 0)
 			return -1;
 	}
 
-	simio_step(dev->regs[MSP430_REG_SR], count);
+	simio_step(status, count);
 	return 0;
 }
 
@@ -571,11 +595,11 @@ static int sim_ctl(device_t dev_base, device_ctl_t op)
 
 	switch (op) {
 	case DEVICE_CTL_RESET:
+		simio_step(dev->regs[MSP430_REG_SR], 4);
 		memset(dev->regs, 0, sizeof(dev->regs));
 		dev->regs[MSP430_REG_PC] = MEM_GETW(dev, 0xfffe);
 		dev->regs[MSP430_REG_SR] = 0;
 		simio_reset();
-		simio_step(dev->regs[MSP430_REG_SR], 4);
 		return 0;
 
 	case DEVICE_CTL_HALT:
diff --git a/simio_cpu.h b/simio_cpu.h
index 099516f..5696c26 100644
--- a/simio_cpu.h
+++ b/simio_cpu.h
@@ -54,6 +54,9 @@ void simio_ack_interrupt(int irq);
 
 /* This should be called after executing an instruction to advance the system
  * clocks.
+ *
+ * The status_register value should be the value of SR _before_ the
+ * instruction was executed.
  */
 void simio_step(uint16_t status_register, int cycles);
 
diff --git a/simio_tracer.c b/simio_tracer.c
index 2775f8b..20d3aa3 100644
--- a/simio_tracer.c
+++ b/simio_tracer.c
@@ -23,6 +23,7 @@
 #include "simio_tracer.h"
 #include "expr.h"
 #include "output.h"
+#include "dis.h"
 
 #define DEFAULT_HISTORY		16
 
@@ -321,7 +322,7 @@ static void tracer_ack_interrupt(struct simio_device *dev, int irq)
 }
 
 static void tracer_step(struct simio_device *dev,
-			uint16_t addr, const int *clocks)
+			uint16_t status, const int *clocks)
 {
 	struct tracer *tr = (struct tracer *)dev;
 	int i;
@@ -329,7 +330,8 @@ static void tracer_step(struct simio_device *dev,
 	for (i = 0; i < SIMIO_NUM_CLOCKS; i++)
 		tr->cycles[i] += clocks[i];
 
-	tr->inscount++;
+	if (!(status & MSP430_SR_CPUOFF))
+		tr->inscount++;
 }
 
 const struct simio_class simio_tracer = {