From 4f73d75ea8d86600f3bf2bb9428ae1df86f26218 Mon Sep 17 00:00:00 2001
From: lazymio <mio@lazym.io>
Date: Thu, 23 Dec 2021 21:46:27 +0100
Subject: [PATCH] Fix #1500

---
 qemu/target/arm/helper.c |  4 +++-
 tests/unit/test_arm.c    | 38 ++++++++++++++++++++++++++++++++++++++
 2 files changed, 41 insertions(+), 1 deletion(-)

diff --git a/qemu/target/arm/helper.c b/qemu/target/arm/helper.c
index 629165b1..48c9b013 100644
--- a/qemu/target/arm/helper.c
+++ b/qemu/target/arm/helper.c
@@ -7983,7 +7983,9 @@ void cpsr_write(CPUARMState *env, uint32_t val, uint32_t mask,
              * to switch mode. (Those are caught by translate.c for writes
              * triggered by guest instructions.)
              */
-            mask &= ~CPSR_M;
+            // mask &= ~CPSR_M;
+            // Unicorn: No, it can also be uc_reg_write
+            switch_mode(env, val & CPSR_M);
         } else if (bad_mode_switch(env, val & CPSR_M, write_type)) {
             /* Attempt to switch to an invalid mode: this is UNPREDICTABLE in
              * v7, and has defined behaviour in v8:
diff --git a/tests/unit/test_arm.c b/tests/unit/test_arm.c
index b44d5372..c40b3c65 100644
--- a/tests/unit/test_arm.c
+++ b/tests/unit/test_arm.c
@@ -308,6 +308,43 @@ static void test_arm_und32_to_svc32()
     OK(uc_close(uc));
 }
 
+static void test_arm_usr32_to_svc32()
+{
+    uc_engine *uc;
+    int r_cpsr, r_sp, r_spsr, r_lr;
+
+    OK(uc_open(UC_ARCH_ARM, UC_MODE_ARM, &uc));
+    OK(uc_ctl_set_cpu_model(uc, UC_CPU_ARM_CORTEX_A9));
+
+    // https://www.keil.com/pack/doc/CMSIS/Core_A/html/group__CMSIS__CPSR__M.html
+    r_cpsr = 0x40000093; // SVC32
+    OK(uc_reg_write(uc, UC_ARM_REG_CPSR, &r_cpsr));
+    r_sp = 0x12345678;
+    OK(uc_reg_write(uc, UC_ARM_REG_SP, &r_sp));
+
+    r_cpsr = 0x4000009b; // UND32
+    OK(uc_reg_write(uc, UC_ARM_REG_CPSR, &r_cpsr));
+    r_spsr = 0x40000093; // Save previous CPSR
+    OK(uc_reg_write(uc, UC_ARM_REG_SPSR, &r_spsr));
+    r_sp = 0xDEAD0000;
+    OK(uc_reg_write(uc, UC_ARM_REG_SP, &r_sp));
+    r_lr = code_start + 8;
+    OK(uc_reg_write(uc, UC_ARM_REG_LR, &r_lr));
+
+    r_cpsr = 0x40000090; // USR32
+    OK(uc_reg_write(uc, UC_ARM_REG_CPSR, &r_cpsr));
+    r_sp = 0x0010000;
+    OK(uc_reg_write(uc, UC_ARM_REG_R13, &r_sp));
+
+    r_cpsr = 0x40000093; // SVC32
+    OK(uc_reg_write(uc, UC_ARM_REG_CPSR, &r_cpsr));
+
+    OK(uc_reg_read(uc, UC_ARM_REG_SP, &r_sp));
+    TEST_CHECK(r_sp == 0x12345678);
+
+    OK(uc_close(uc));
+}
+
 TEST_LIST = {{"test_arm_nop", test_arm_nop},
              {"test_arm_thumb_sub", test_arm_thumb_sub},
              {"test_armeb_sub", test_armeb_sub},
@@ -317,4 +354,5 @@ TEST_LIST = {{"test_arm_nop", test_arm_nop},
              {"test_arm_m_control", test_arm_m_control},
              {"test_arm_m_exc_return", test_arm_m_exc_return},
              {"test_arm_und32_to_svc32", test_arm_und32_to_svc32},
+             {"test_arm_usr32_to_svc32", test_arm_usr32_to_svc32},
              {NULL, NULL}};
\ No newline at end of file