Mode switching

src/arm.c

@@ -4,17 +4,78 @@
 #define ARM_ROR(I, ROTATE) (((I) >> ROTATE) | (I << (32 - ROTATE)))
 
 static inline void _ARMSetMode(struct ARMCore*, enum ExecutionMode);
+static void _ARMSetPrivilegeMode(struct ARMCore*, enum PrivilegeMode);
 static ARMInstruction _ARMLoadInstructionARM(struct ARMMemory*, uint32_t address, uint32_t* opcodeOut);
 static ARMInstruction _ARMLoadInstructionThumb(struct ARMMemory*, uint32_t address, uint32_t* opcodeOut);
+static inline enum RegisterBank _ARMSelectBank(enum PrivilegeMode);
 
 static inline void _ARMReadCPSR(struct ARMCore* cpu) {
 	_ARMSetMode(cpu, cpu->cpsr.t);
+	_ARMSetPrivilegeMode(cpu, cpu->cpsr.priv);
+}
+
+static void _ARMSetPrivilegeMode(struct ARMCore* cpu, enum PrivilegeMode mode) {
+	if (mode == cpu->privilegeMode) {
+		// Not switching modes after all
+		return;
+	}
+
+	enum RegisterBank newBank = _ARMSelectBank(mode);
+	enum RegisterBank oldBank = _ARMSelectBank(cpu->privilegeMode);
+	if (newBank != oldBank) {
+		// Switch banked registers
+		if (mode == MODE_FIQ || cpu->privilegeMode == MODE_FIQ) {
+			int oldFIQBank = oldBank == BANK_FIQ;
+			int newFIQBank = newBank == BANK_FIQ;
+			cpu->bankedRegisters[oldFIQBank][2] = cpu->gprs[8];
+			cpu->bankedRegisters[oldFIQBank][3] = cpu->gprs[9];
+			cpu->bankedRegisters[oldFIQBank][4] = cpu->gprs[10];
+			cpu->bankedRegisters[oldFIQBank][5] = cpu->gprs[11];
+			cpu->bankedRegisters[oldFIQBank][6] = cpu->gprs[12];
+			cpu->gprs[8] = cpu->bankedRegisters[newFIQBank][2];
+			cpu->gprs[9] = cpu->bankedRegisters[newFIQBank][3];
+			cpu->gprs[10] = cpu->bankedRegisters[newFIQBank][4];
+			cpu->gprs[11] = cpu->bankedRegisters[newFIQBank][5];
+			cpu->gprs[12] = cpu->bankedRegisters[newFIQBank][6];
+		}
+		cpu->bankedRegisters[oldBank][0] = cpu->gprs[ARM_SP];
+		cpu->bankedRegisters[oldBank][1] = cpu->gprs[ARM_LR];
+		cpu->gprs[ARM_SP] = cpu->bankedRegisters[newBank][0];
+		cpu->gprs[ARM_LR] = cpu->bankedRegisters[newBank][1];
+
+		cpu->bankedSPSRs[oldBank] = cpu->spsr.packed;
+		cpu->spsr.packed = cpu->bankedSPSRs[newBank];
+
+	}
+	cpu->privilegeMode = mode;
 }
 
 static inline int _ARMModeHasSPSR(enum PrivilegeMode mode) {
 	return mode != MODE_SYSTEM && mode != MODE_USER;
 }
 
+static inline enum RegisterBank _ARMSelectBank(enum PrivilegeMode mode) {
+	switch (mode) {
+	case MODE_USER:
+	case MODE_SYSTEM:
+		// No banked registers
+		return BANK_NONE;
+	case MODE_FIQ:
+		return BANK_FIQ;
+	case MODE_IRQ:
+		return BANK_IRQ;
+	case MODE_SUPERVISOR:
+		return BANK_SUPERVISOR;
+	case MODE_ABORT:
+		return BANK_ABORT;
+	case MODE_UNDEFINED:
+		return BANK_UNDEFINED;
+	default:
+		// This should be unreached
+		return BANK_NONE;
+	}
+}
+
 // Addressing mode 1
 static inline void _barrelShift(struct ARMCore* cpu, uint32_t opcode) {
 	// TODO
@@ -70,6 +131,16 @@ void ARMInit(struct ARMCore* cpu) {
 	for (i = 0; i < 16; ++i) {
 		cpu->gprs[i] = 0;
 	}
+	for (i = 0; i < 6; ++i) {
+		cpu->bankedRegisters[i][0] = 0;
+		cpu->bankedRegisters[i][1] = 0;
+		cpu->bankedRegisters[i][2] = 0;
+		cpu->bankedRegisters[i][3] = 0;
+		cpu->bankedRegisters[i][4] = 0;
+		cpu->bankedRegisters[i][5] = 0;
+		cpu->bankedRegisters[i][6] = 0;
+		cpu->bankedSPSRs[i] = 0;
+	}
 
 	cpu->cpsr.packed = MODE_SYSTEM;
 	cpu->spsr.packed = 0;

src/arm.h

@@ -39,6 +39,15 @@ enum ExecutionVector {
 	BASE_FIQ = 0x0000001C
 };
 
+enum RegisterBank {
+	BANK_NONE = 0,
+	BANK_FIQ = 1,
+	BANK_IRQ = 2,
+	BANK_SUPERVISOR = 3,
+	BANK_ABORT = 4,
+	BANK_UNDEFINED = 5
+};
+
 struct ARMCore;
 typedef void (*ARMInstruction)(struct ARMCore*, uint32_t opcode);
 
@@ -81,6 +90,9 @@ struct ARMCore {
 
 	int32_t cyclesToEvent;
 
+	int32_t bankedRegisters[6][7];
+	int32_t bankedSPSRs[6];
+
 	int32_t shifterOperand;
 	int32_t shifterCarryOut;
 
@@ -88,6 +100,7 @@ struct ARMCore {
 
 	ARMInstruction (*loadInstruction)(struct ARMMemory*, uint32_t address, uint32_t* opcodeOut);
 	enum ExecutionMode executionMode;
+	enum PrivilegeMode privilegeMode;
 
 	struct ARMMemory* memory;
 	struct ARMBoard* board;