From d2e84f0a3034548cd9326e15127b045b135c7098 Mon Sep 17 00:00:00 2001
From: Jeffrey Pfau <jeffrey@endrift.com>
Date: Fri, 26 Apr 2013 02:00:59 -0700
Subject: [PATCH] Implement IntrWait

---
 src/arm/arm.c      | 18 ++++++++++++++++++
 src/arm/arm.h      |  1 +
 src/gba/gba-bios.c | 15 +++++++++++++++
 3 files changed, 34 insertions(+)

diff --git a/src/arm/arm.c b/src/arm/arm.c
index 645e98505..a9fce845d 100644
--- a/src/arm/arm.c
+++ b/src/arm/arm.c
@@ -135,6 +135,24 @@ void ARMRaiseIRQ(struct ARMCore* cpu) {
 	cpu->cpsr.i = 1;
 }
 
+void ARMRaiseSWI(struct ARMCore* cpu) {
+	union PSR cpsr = cpu->cpsr;
+	int instructionWidth;
+	if (cpu->executionMode == MODE_THUMB) {
+		instructionWidth = WORD_SIZE_THUMB;
+	} else {
+		instructionWidth = WORD_SIZE_ARM;
+	}
+	ARMSetPrivilegeMode(cpu, MODE_SUPERVISOR);
+	cpu->cpsr.priv = MODE_IRQ;
+	cpu->gprs[ARM_LR] = cpu->gprs[ARM_PC] - instructionWidth + WORD_SIZE_ARM;
+	cpu->gprs[ARM_PC] = BASE_SWI + WORD_SIZE_ARM;
+	cpu->memory->setActiveRegion(cpu->memory, cpu->gprs[ARM_PC]);
+	_ARMSetMode(cpu, MODE_ARM);
+	cpu->spsr = cpsr;
+	cpu->cpsr.i = 1;
+}
+
 void ARMRun(struct ARMCore* cpu) {
 	if (cpu->executionMode == MODE_THUMB) {
 		ThumbStep(cpu);
diff --git a/src/arm/arm.h b/src/arm/arm.h
index e4eeeddaf..ae49015dc 100644
--- a/src/arm/arm.h
+++ b/src/arm/arm.h
@@ -122,6 +122,7 @@ void ARMAssociateBoard(struct ARMCore* cpu, struct ARMBoard* board);
 void ARMReset(struct ARMCore* cpu);
 void ARMSetPrivilegeMode(struct ARMCore*, enum PrivilegeMode);
 void ARMRaiseIRQ(struct ARMCore*);
+void ARMRaiseSWI(struct ARMCore*);
 
 void ARMRun(struct ARMCore* cpu);
 
diff --git a/src/gba/gba-bios.c b/src/gba/gba-bios.c
index 44e17eebf..24ac54443 100644
--- a/src/gba/gba-bios.c
+++ b/src/gba/gba-bios.c
@@ -1,6 +1,7 @@
 #include "gba-bios.h"
 
 #include "gba.h"
+#include "gba-io.h"
 #include "gba-memory.h"
 
 #include <math.h>
@@ -81,6 +82,20 @@ void GBASwi16(struct ARMBoard* board, int immediate) {
 	case 0x2:
 		GBAHalt(gba);
 		break;
+	case 0x05:
+		// VBlankIntrWait
+		gba->cpu.gprs[0] = 1;
+		gba->cpu.gprs[1] = 1;
+		// Fall through:
+	case 0x04:
+		// IntrWait
+		gba->memory.io[REG_IME >> 1] = 1;
+		if (!gba->cpu.gprs[0] && gba->memory.io[REG_IF >> 1] & gba->cpu.gprs[1]) {
+			break;
+		}
+		gba->memory.io[REG_IF >> 1] = 0;
+		ARMRaiseSWI(&gba->cpu);
+		break;
 	case 0xB:
 		_CpuSet(gba);
 		break;