M src/arm/arm.c → src/arm/arm.c

@@ -135,6 +135,24 @@ cpu->spsr = cpsr;
 	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);

M src/arm/arm.h → src/arm/arm.h

@@ -122,6 +122,7 @@
 void ARMReset(struct ARMCore* cpu);
 void ARMSetPrivilegeMode(struct ARMCore*, enum PrivilegeMode);
 void ARMRaiseIRQ(struct ARMCore*);
+void ARMRaiseSWI(struct ARMCore*);
 
 void ARMRun(struct ARMCore* cpu);
 

M src/gba/gba-bios.c → 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>
@@ -80,6 +81,20 @@ struct GBA* gba = ((struct GBABoard*) board)->p;
 	switch (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);