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

@@ -69,6 +69,7 @@ }
 }
 
 void ARMInit(struct ARMCore* cpu) {
+	memset(&cpu->cp15, 0, sizeof(cpu->cp15));
 	cpu->master->init(cpu, cpu->master);
 	size_t i;
 	for (i = 0; i < cpu->numComponents; ++i) {
@@ -115,6 +116,10 @@ int i;
 	for (i = 0; i < 16; ++i) {
 		cpu->gprs[i] = 0;
 	}
+	if (ARMControlRegIsVE(cpu->cp15.r1.c0)) {
+		cpu->gprs[ARM_PC] = 0xFFFF0000;
+	}
+
 	for (i = 0; i < 6; ++i) {
 		cpu->bankedRegisters[i][0] = 0;
 		cpu->bankedRegisters[i][1] = 0;
@@ -161,6 +166,9 @@ ARMSetPrivilegeMode(cpu, MODE_IRQ);
 	cpu->cpsr.priv = MODE_IRQ;
 	cpu->gprs[ARM_LR] = cpu->gprs[ARM_PC] - instructionWidth + WORD_SIZE_ARM;
 	cpu->gprs[ARM_PC] = BASE_IRQ;
+	if (ARMControlRegIsVE(cpu->cp15.r1.c0)) {
+		cpu->gprs[ARM_PC] |= 0xFFFF0000;
+	}
 	int currentCycles = 0;
 	ARM_WRITE_PC;
 	_ARMSetMode(cpu, MODE_ARM);
@@ -181,6 +189,9 @@ ARMSetPrivilegeMode(cpu, MODE_SUPERVISOR);
 	cpu->cpsr.priv = MODE_SUPERVISOR;
 	cpu->gprs[ARM_LR] = cpu->gprs[ARM_PC] - instructionWidth;
 	cpu->gprs[ARM_PC] = BASE_SWI;
+	if (ARMControlRegIsVE(cpu->cp15.r1.c0)) {
+		cpu->gprs[ARM_PC] |= 0xFFFF0000;
+	}
 	int currentCycles = 0;
 	ARM_WRITE_PC;
 	_ARMSetMode(cpu, MODE_ARM);
@@ -201,6 +212,9 @@ ARMSetPrivilegeMode(cpu, MODE_UNDEFINED);
 	cpu->cpsr.priv = MODE_UNDEFINED;
 	cpu->gprs[ARM_LR] = cpu->gprs[ARM_PC] - instructionWidth;
 	cpu->gprs[ARM_PC] = BASE_UNDEF;
+	if (ARMControlRegIsVE(cpu->cp15.r1.c0)) {
+		cpu->gprs[ARM_PC] |= 0xFFFF0000;
+	}
 	int currentCycles = 0;
 	ARM_WRITE_PC;
 	_ARMSetMode(cpu, MODE_ARM);

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

@@ -131,6 +131,55 @@
 	void (*hitStub)(struct ARMCore* cpu, uint32_t opcode);
 };
 
+DECL_BITFIELD(ARMCPUID, uint32_t);
+DECL_BITFIELD(ARMCacheType, uint32_t);
+DECL_BITFIELD(ARMTCMType, uint32_t);
+DECL_BITFIELD(ARMTLBType, uint32_t);
+DECL_BITFIELD(ARMMPUType, uint32_t);
+
+DECL_BITFIELD(ARMControlReg, uint32_t);
+DECL_BIT(ARMControlReg, M, 0);
+DECL_BIT(ARMControlReg, A, 1);
+DECL_BIT(ARMControlReg, C, 2);
+DECL_BIT(ARMControlReg, W, 3);
+DECL_BIT(ARMControlReg, P, 4);
+DECL_BIT(ARMControlReg, D, 5);
+DECL_BIT(ARMControlReg, L, 6);
+DECL_BIT(ARMControlReg, B, 7);
+DECL_BIT(ARMControlReg, S, 8);
+DECL_BIT(ARMControlReg, R, 9);
+DECL_BIT(ARMControlReg, F, 10);
+DECL_BIT(ARMControlReg, Z, 11);
+DECL_BIT(ARMControlReg, I, 12);
+DECL_BIT(ARMControlReg, V, 13);
+DECL_BIT(ARMControlReg, RR, 14);
+DECL_BIT(ARMControlReg, L4, 15);
+DECL_BIT(ARMControlReg, FI, 21);
+DECL_BIT(ARMControlReg, U, 22);
+DECL_BIT(ARMControlReg, XP, 23);
+DECL_BIT(ARMControlReg, VE, 24);
+DECL_BIT(ARMControlReg, EE, 25);
+DECL_BIT(ARMControlReg, L2, 26);
+
+DECL_BITFIELD(ARMCoprocessorAccess, uint32_t);
+
+struct ARMCP15 {
+	struct {
+		ARMCPUID cpuid;
+		ARMCacheType cachetype;
+		ARMTCMType tcmtype;
+		ARMTLBType tlbtype;
+		ARMMPUType mputype;
+	} r0;
+	struct {
+		ARMControlReg c0;
+		uint32_t c1;
+		ARMCoprocessorAccess cpAccess;
+	} r1;
+
+	uint32_t (*write)(struct ARMCore*, int crn, int crm, int opcode2, uint32_t value);
+};
+
 struct ARMCore {
 	int32_t gprs[16];
 	union PSR cpsr;
@@ -152,6 +201,7 @@ enum PrivilegeMode privilegeMode;
 
 	struct ARMMemory memory;
 	struct ARMInterruptHandler irqh;
+	struct ARMCP15 cp15;
 
 	struct mCPUComponent* master;