From f32e92e0f16db74668be2d10f56a6e0f10c7f214 Mon Sep 17 00:00:00 2001
From: Jeffrey Pfau <jeffrey@endrift.com>
Date: Wed, 1 Jun 2016 21:42:34 -0700
Subject: [PATCH] ARM: Implement MCR for coprocessor 15

---
 src/arm/arm.h     |  2 +-
 src/arm/isa-arm.c | 26 ++++++++++++++++++++++++--
 2 files changed, 25 insertions(+), 3 deletions(-)

diff --git a/src/arm/arm.h b/src/arm/arm.h
index 4c4c59318..f63002641 100644
--- a/src/arm/arm.h
+++ b/src/arm/arm.h
@@ -177,7 +177,7 @@ struct ARMCP15 {
 		ARMCoprocessorAccess cpAccess;
 	} r1;
 
-	uint32_t (*write)(struct ARMCore*, int crn, int crm, int opcode2, uint32_t value);
+	uint32_t (*write)(struct ARMCore*, int crn, int crm, int opcode1, int opcode2, uint32_t value);
 };
 
 struct ARMCore {
diff --git a/src/arm/isa-arm.c b/src/arm/isa-arm.c
index d1f7b7c29..f1abb3877 100644
--- a/src/arm/isa-arm.c
+++ b/src/arm/isa-arm.c
@@ -618,11 +618,33 @@ DEFINE_INSTRUCTION_ARM(BX,
 
 // Begin coprocessor definitions
 
+#define DEFINE_COPROCESSOR_INSTRUCTION(NAME, BODY) \
+	DEFINE_INSTRUCTION_ARM(NAME, \
+		int op1 = (opcode >> 21) & 7; \
+		int op2 = (opcode >> 5) & 7; \
+		int rd = (opcode >> 12) & 0xF; \
+		int cp = (opcode >> 8) & 0xF; \
+		int crn = (opcode >> 16) & 0xF; \
+		int crm = opcode & 0xF; \
+		UNUSED(op1); \
+		UNUSED(op2); \
+		UNUSED(rd); \
+		UNUSED(crn); \
+		UNUSED(crm); \
+		BODY;)
+
+DEFINE_COPROCESSOR_INSTRUCTION(MRC, ARM_STUB)
+
+DEFINE_COPROCESSOR_INSTRUCTION(MCR,
+	if (cp == 15) {
+		cpu->cp15.write(cpu, crn, crm, op1, op2, cpu->gprs[rd]);
+	} else {
+		ARM_STUB;
+	})
+
 DEFINE_INSTRUCTION_ARM(CDP, ARM_STUB)
 DEFINE_INSTRUCTION_ARM(LDC, ARM_STUB)
 DEFINE_INSTRUCTION_ARM(STC, ARM_STUB)
-DEFINE_INSTRUCTION_ARM(MCR, ARM_STUB)
-DEFINE_INSTRUCTION_ARM(MRC, ARM_STUB)
 
 // Begin miscellaneous definitions