From 290d5b77dd4f47e7f96981728eea5dc33bd29f1d Mon Sep 17 00:00:00 2001
From: Jeffrey Pfau <jeffrey@endrift.com>
Date: Wed, 1 Jun 2016 19:13:31 -0700
Subject: [PATCH] ARM: Add basic CP15 information

---
 src/arm/arm.c | 14 ++++++++++++++
 src/arm/arm.h | 50 ++++++++++++++++++++++++++++++++++++++++++++++++++
 2 files changed, 64 insertions(+)

diff --git a/src/arm/arm.c b/src/arm/arm.c
index 9fc35de71..5b1a1ac1c 100644
--- a/src/arm/arm.c
+++ b/src/arm/arm.c
@@ -69,6 +69,7 @@ static inline enum RegisterBank _ARMSelectBank(enum PrivilegeMode mode) {
 }
 
 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 @@ void ARMReset(struct ARMCore* cpu) {
 	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 @@ void ARMRaiseIRQ(struct ARMCore* cpu) {
 	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 @@ void ARMRaiseSWI(struct ARMCore* cpu) {
 	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 @@ void ARMRaiseUndefined(struct ARMCore* cpu) {
 	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);
diff --git a/src/arm/arm.h b/src/arm/arm.h
index 947cedf15..4c4c59318 100644
--- a/src/arm/arm.h
+++ b/src/arm/arm.h
@@ -131,6 +131,55 @@ struct ARMInterruptHandler {
 	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 @@ struct ARMCore {
 
 	struct ARMMemory memory;
 	struct ARMInterruptHandler irqh;
+	struct ARMCP15 cp15;
 
 	struct mCPUComponent* master;