From 7c5a6b121c4d9d4287ce0d178ea2cede753d808c Mon Sep 17 00:00:00 2001
From: Jeffrey Pfau <jeffrey@endrift.com>
Date: Sun, 14 Apr 2013 11:57:39 -0700
Subject: [PATCH] Implement SWI

---
 src/arm.h       |  2 ++
 src/gba.c       | 16 ++++++++++++++++
 src/gba.h       |  2 ++
 src/isa-thumb.c |  2 +-
 4 files changed, 21 insertions(+), 1 deletion(-)

diff --git a/src/arm.h b/src/arm.h
index fa0046f8e..36bdf9ce0 100644
--- a/src/arm.h
+++ b/src/arm.h
@@ -85,6 +85,8 @@ struct ARMMemory {
 struct ARMBoard {
 	struct ARMCore* cpu;
 	void (*reset)(struct ARMBoard* board);
+	void (*swi16)(struct ARMBoard* board, int immediate);
+	void (*swi32)(struct ARMBoard* board, int immediate);
 
 	void (*hitStub)(struct ARMBoard* board, uint32_t opcode);
 };
diff --git a/src/gba.c b/src/gba.c
index 4b366d1e5..911ca0fbb 100644
--- a/src/gba.c
+++ b/src/gba.c
@@ -21,6 +21,9 @@ static void GBAStore32(struct ARMMemory* memory, uint32_t address, int32_t value
 static void GBAStore16(struct ARMMemory* memory, uint32_t address, int16_t value);
 static void GBAStore8(struct ARMMemory* memory, uint32_t address, int8_t value);
 
+static void GBASwi16(struct ARMBoard* board, int immediate);
+static void GBASwi32(struct ARMBoard* board, int immediate);
+
 static void GBASetActiveRegion(struct ARMMemory* memory, uint32_t region);
 static void GBAHitStub(struct ARMBoard* board, uint32_t opcode);
 
@@ -81,6 +84,8 @@ void GBAMemoryDeinit(struct GBAMemory* memory) {
 
 void GBABoardInit(struct GBABoard* board) {
 	board->d.reset = GBABoardReset;
+	board->d.swi16 = GBASwi16;
+	board->d.swi32 = GBASwi32;
 	board->d.hitStub = GBAHitStub;
 }
 
@@ -396,6 +401,17 @@ void GBAStore8(struct ARMMemory* memory, uint32_t address, int8_t value) {
 	}
 }
 
+static void GBASwi16(struct ARMBoard* board, int immediate) {
+	switch (immediate) {
+	default:
+		GBALog(GBA_LOG_STUB, "Stub software interrupt: %02x", immediate);
+	}
+}
+
+static void GBASwi32(struct ARMBoard* board, int immediate) {
+	GBASwi32(board, immediate >> 8);
+}
+
 void GBALog(int level, const char* format, ...) {
 	va_list args;
 	va_start(args, format);
diff --git a/src/gba.h b/src/gba.h
index 832fe58a0..8e8197e38 100644
--- a/src/gba.h
+++ b/src/gba.h
@@ -251,4 +251,6 @@ void GBAAttachDebugger(struct GBA* gba, struct ARMDebugger* debugger);
 
 void GBALoadROM(struct GBA* gba, int fd);
 
+void GBALog(int level, const char* format, ...);
+
 #endif
diff --git a/src/isa-thumb.c b/src/isa-thumb.c
index 13268d127..7477e8f3f 100644
--- a/src/isa-thumb.c
+++ b/src/isa-thumb.c
@@ -410,7 +410,7 @@ DEFINE_INSTRUCTION_THUMB(BX, \
 		ARM_WRITE_PC; \
 	})
 
-DEFINE_INSTRUCTION_THUMB(SWI, ARM_STUB)
+DEFINE_INSTRUCTION_THUMB(SWI, cpu->board->swi16(cpu->board, opcode & 0xFF))
 
 #define DECLARE_INSTRUCTION_THUMB(EMITTER, NAME) \
 	EMITTER ## NAME