From b41e11d4c11ffb8b86df626794c31bc0ba10f261 Mon Sep 17 00:00:00 2001
From: Jeffrey Pfau <jeffrey@endrift.com>
Date: Sat, 12 Jul 2014 00:29:00 -0700
Subject: [PATCH] Decode MSR and MRS

---
 src/arm/decoder-arm.c | 70 +++++++++++++++++++++++++++++++++++--------
 src/arm/decoder.c     |  6 ++++
 src/arm/decoder.h     |  5 ++++
 3 files changed, 68 insertions(+), 13 deletions(-)

diff --git a/src/arm/decoder-arm.c b/src/arm/decoder-arm.c
index e9c5801c6..164345cc5 100644
--- a/src/arm/decoder-arm.c
+++ b/src/arm/decoder-arm.c
@@ -362,23 +362,67 @@ DEFINE_DECODER_ARM(BX, BX,
 
 // Begin coprocessor definitions
 
-DEFINE_DECODER_ARM(CDP, ILL,)
-DEFINE_DECODER_ARM(LDC, ILL,)
-DEFINE_DECODER_ARM(STC, ILL,)
-DEFINE_DECODER_ARM(MCR, ILL,)
-DEFINE_DECODER_ARM(MRC, ILL,)
+DEFINE_DECODER_ARM(CDP, ILL, info->operandFormat = ARM_OPERAND_NONE;)
+DEFINE_DECODER_ARM(LDC, ILL, info->operandFormat = ARM_OPERAND_NONE;)
+DEFINE_DECODER_ARM(STC, ILL, info->operandFormat = ARM_OPERAND_NONE;)
+DEFINE_DECODER_ARM(MCR, ILL, info->operandFormat = ARM_OPERAND_NONE;)
+DEFINE_DECODER_ARM(MRC, ILL, info->operandFormat = ARM_OPERAND_NONE;)
 
 // Begin miscellaneous definitions
 
-DEFINE_DECODER_ARM(BKPT, BKPT,) // Not strictly in ARMv4T, but here for convenience
-DEFINE_DECODER_ARM(ILL, ILL,) // Illegal opcode
+DEFINE_DECODER_ARM(BKPT, BKPT, info->operandFormat = ARM_OPERAND_NONE;) // Not strictly in ARMv4T, but here for convenience
+DEFINE_DECODER_ARM(ILL, ILL, info->operandFormat = ARM_OPERAND_NONE;) // Illegal opcode
 
-DEFINE_DECODER_ARM(MSR, MSR, info->affectsCPSR = 1;)
-DEFINE_DECODER_ARM(MSRR, MSR, info->affectsCPSR = 1;)
-DEFINE_DECODER_ARM(MRS, MRS, info->affectsCPSR = 1;)
-DEFINE_DECODER_ARM(MRSR, MRS, info->affectsCPSR = 1;)
-DEFINE_DECODER_ARM(MSRI, MSR, info->affectsCPSR = 1;)
-DEFINE_DECODER_ARM(MSRRI, MSR, info->affectsCPSR = 1;)
+DEFINE_DECODER_ARM(MSR, MSR,
+	info->affectsCPSR = 1;
+	info->op1.reg = ARM_CPSR;
+	info->op2.reg = opcode & 0x0000000F;
+	info->operandFormat = ARM_OPERAND_REGISTER_1 |
+		ARM_OPERAND_AFFECTED_1 |
+		ARM_OPERAND_REGISTER_2;)
+
+DEFINE_DECODER_ARM(MSRR, MSR,
+	info->op1.reg = ARM_SPSR;
+	info->op2.reg = opcode & 0x0000000F;
+	info->operandFormat = ARM_OPERAND_REGISTER_1 |
+		ARM_OPERAND_AFFECTED_1 |
+		ARM_OPERAND_REGISTER_2;)
+
+DEFINE_DECODER_ARM(MRS, MRS, info->affectsCPSR = 1;
+	info->affectsCPSR = 1;
+	info->op1.reg = (opcode >> 12) & 0xF;
+	info->op2.reg = ARM_CPSR;
+	info->operandFormat = ARM_OPERAND_REGISTER_1 |
+		ARM_OPERAND_AFFECTED_1 |
+		ARM_OPERAND_REGISTER_2;)
+
+DEFINE_DECODER_ARM(MRSR, MRS, info->affectsCPSR = 1;
+	info->affectsCPSR = 1;
+	info->op1.reg = (opcode >> 12) & 0xF;
+	info->op2.reg = ARM_SPSR;
+	info->operandFormat = ARM_OPERAND_REGISTER_1 |
+		ARM_OPERAND_AFFECTED_1 |
+		ARM_OPERAND_REGISTER_2;)
+
+DEFINE_DECODER_ARM(MSRI, MSR, info->affectsCPSR = 1;
+	int rotate = (opcode & 0x00000F00) >> 7;
+	int32_t operand = ARM_ROR(opcode & 0x000000FF, rotate);
+	info->affectsCPSR = 1;
+	info->op1.reg = ARM_CPSR;
+	info->op2.immediate = operand;
+	info->operandFormat = ARM_OPERAND_REGISTER_1 |
+		ARM_OPERAND_AFFECTED_1 |
+		ARM_OPERAND_IMMEDIATE_2;)
+
+DEFINE_DECODER_ARM(MSRRI, MSR, info->affectsCPSR = 1;
+	int rotate = (opcode & 0x00000F00) >> 7;
+	int32_t operand = ARM_ROR(opcode & 0x000000FF, rotate);
+	info->affectsCPSR = 1;
+	info->op1.reg = ARM_SPSR;
+	info->op2.immediate = operand;
+	info->operandFormat = ARM_OPERAND_REGISTER_1 |
+		ARM_OPERAND_AFFECTED_1 |
+		ARM_OPERAND_IMMEDIATE_2;)
 
 DEFINE_DECODER_ARM(SWI, SWI,
 	info->op1.immediate = opcode & 0xFFFFFF;
diff --git a/src/arm/decoder.c b/src/arm/decoder.c
index 51929133e..42c58057f 100644
--- a/src/arm/decoder.c
+++ b/src/arm/decoder.c
@@ -46,6 +46,12 @@ static int _decodeRegister(int reg, char* buffer, int blen) {
 	case ARM_PC:
 		strncpy(buffer, "pc", blen - 1);
 		return 2;
+	case ARM_CPSR:
+		strncpy(buffer, "cpsr", blen - 1);
+		return 4;
+	case ARM_SPSR:
+		strncpy(buffer, "spsr", blen - 1);
+		return 4;
 	default:
 		return snprintf(buffer, blen - 1, "r%i", reg);
 	}
diff --git a/src/arm/decoder.h b/src/arm/decoder.h
index 50a447e95..0bfb3aff5 100644
--- a/src/arm/decoder.h
+++ b/src/arm/decoder.h
@@ -166,6 +166,11 @@ enum ARMMnemonic {
 	ARM_MN_MAX
 };
 
+enum {
+	ARM_CPSR = 16,
+	ARM_SPSR = 17
+};
+
 struct ARMInstructionInfo {
 	enum ExecutionMode execMode;
 	uint32_t opcode;