From f8d8ea0debbdbcebbddcb31b560ef266dd68c2b0 Mon Sep 17 00:00:00 2001
From: MerryMage <git@mary.rs>
Date: Tue, 4 May 2021 12:43:51 +0100
Subject: [PATCH] thumb32: Implement MRS (register)

---
 src/frontend/A32/decoder/thumb32.inc          |  5 +--
 .../A32/translate/impl/thumb32_control.cpp    | 40 +++++++++++++++++++
 src/frontend/A32/translate/impl/translate.h   |  1 +
 3 files changed, 42 insertions(+), 4 deletions(-)

diff --git a/src/frontend/A32/decoder/thumb32.inc b/src/frontend/A32/decoder/thumb32.inc
index eb545434..0ac6d1b0 100644
--- a/src/frontend/A32/decoder/thumb32.inc
+++ b/src/frontend/A32/decoder/thumb32.inc
@@ -84,10 +84,7 @@ INST(thumb32_UBFX,           "UBFX",                     "111100111100nnnn0iiidd
 
 // Branches and Miscellaneous Control
 //INST(thumb32_MSR_banked,     "MSR (banked)",             "11110011100-----10-0------1-----")
-//INST(thumb32_MSR_reg_1,      "MSR (reg)",                "111100111001----10-0------0-----")
-//INST(thumb32_MSR_reg_2,      "MSR (reg)",                "111100111000----10-0--01--0-----")
-//INST(thumb32_MSR_reg_3,      "MSR (reg)",                "111100111000----10-0--1---0-----")
-//INST(thumb32_MSR_reg_4,      "MSR (reg)",                "111100111000----10-0--00--0-----")
+INST(thumb32_MSR_reg,        "MSR (reg)",                "11110011100Rnnnn1000mmmm00000000")
 
 INST(thumb32_NOP,            "NOP",                      "11110011101011111000000000000000")
 INST(thumb32_YIELD,          "YIELD",                    "11110011101011111000000000000001")
diff --git a/src/frontend/A32/translate/impl/thumb32_control.cpp b/src/frontend/A32/translate/impl/thumb32_control.cpp
index 253e6e62..d13111c7 100644
--- a/src/frontend/A32/translate/impl/thumb32_control.cpp
+++ b/src/frontend/A32/translate/impl/thumb32_control.cpp
@@ -66,6 +66,46 @@ bool TranslatorVisitor::thumb32_YIELD() {
     return thumb16_YIELD();
 }
 
+bool TranslatorVisitor::thumb32_MSR_reg(bool write_spsr, Reg n, Imm<4> mask) {
+    if (mask == 0) {
+        return UnpredictableInstruction();
+    }
+
+    if (n == Reg::PC) {
+        return UnpredictableInstruction();
+    }
+
+    if (write_spsr) {
+        return UndefinedInstruction();
+    }
+
+    const bool write_nzcvq = mask.Bit<3>();
+    const bool write_g = mask.Bit<2>();
+    const bool write_e = mask.Bit<1>();
+    const auto value = ir.GetRegister(n);
+
+    if (!write_e) {
+        if (write_nzcvq) {
+            ir.SetCpsrNZCVQ(ir.And(value, ir.Imm32(0xF8000000)));
+        }
+
+        if (write_g) {
+            ir.SetGEFlagsCompressed(ir.And(value, ir.Imm32(0x000F0000)));
+        }
+    } else {
+        const u32 cpsr_mask = (write_nzcvq ? 0xF8000000 : 0) | (write_g ? 0x000F0000 : 0) | 0x00000200;
+        const auto old_cpsr = ir.And(ir.GetCpsr(), ir.Imm32(~cpsr_mask));
+        const auto new_cpsr = ir.And(value, ir.Imm32(cpsr_mask));
+        ir.SetCpsr(ir.Or(old_cpsr, new_cpsr));
+        ir.PushRSB(ir.current_location.AdvancePC(4));
+        ir.BranchWritePC(ir.Imm32(ir.current_location.PC() + 4));
+        ir.SetTerm(IR::Term::CheckHalt{IR::Term::PopRSBHint{}});
+        return false;
+    }
+
+    return true;
+}
+
 bool TranslatorVisitor::thumb32_MRS_reg(bool read_spsr, Reg d) {
     if (d == Reg::R15) {
         return UnpredictableInstruction();
diff --git a/src/frontend/A32/translate/impl/translate.h b/src/frontend/A32/translate/impl/translate.h
index c6f82a4d..c8218de3 100644
--- a/src/frontend/A32/translate/impl/translate.h
+++ b/src/frontend/A32/translate/impl/translate.h
@@ -576,6 +576,7 @@ struct TranslatorVisitor final {
     bool thumb32_WFI();
     bool thumb32_YIELD();
     bool thumb32_MSR_reg(bool R, Reg n, Imm<4> mask);
+    bool thumb32_MRS_reg(bool R, Reg d);
 
     // thumb32 branch instructions
     bool thumb32_BL_imm(Imm<1> S, Imm<10> hi, Imm<1> j1, Imm<1> j2, Imm<11> lo);