5 files changed, 21 insertions, 17 deletions

diff --git a/src/core/arm/dyncom/arm_dyncom.cpp b/src/core/arm/dyncom/arm_dyncom.cpp
index 6a95ede24..bbcbbdd2b 100644
--- a/src/core/arm/dyncom/arm_dyncom.cpp
+++ b/src/core/arm/dyncom/arm_dyncom.cpp
@@ -7,6 +7,7 @@
 
 #include "core/arm/dyncom/arm_dyncom.h"
 #include "core/arm/dyncom/arm_dyncom_interpreter.h"
+#include "core/arm/dyncom/arm_dyncom_run.h"
 
 #include "core/core.h"
 #include "core/core_timing.h"
@@ -15,7 +16,7 @@ const static cpu_config_t s_arm11_cpu_info = {
     "armv6", "arm11", 0x0007b000, 0x0007f000, NONCACHE
 };
 
-ARM_DynCom::ARM_DynCom() {
+ARM_DynCom::ARM_DynCom(PrivilegeMode initial_mode) {
     state = std::unique_ptr<ARMul_State>(new ARMul_State);
 
     ARMul_NewState(state.get());
@@ -33,6 +34,9 @@ ARM_DynCom::ARM_DynCom() {
     state->NextInstr = RESUME; // NOTE: This will be overwritten by LoadContext
     state->Emulate = RUN;
 
+    // Switch to the desired privilege mode.
+    switch_mode(state.get(), initial_mode);
+
     state->Reg[13] = 0x10000000; // Set stack pointer to the top of the stack
     state->Reg[15] = 0x00000000;
 }
diff --git a/src/core/arm/dyncom/arm_dyncom.h b/src/core/arm/dyncom/arm_dyncom.h
index 9e2dda843..213cac1ad 100644
--- a/src/core/arm/dyncom/arm_dyncom.h
+++ b/src/core/arm/dyncom/arm_dyncom.h
@@ -13,7 +13,7 @@
 
 class ARM_DynCom final : virtual public ARM_Interface {
 public:
-    ARM_DynCom();
+    ARM_DynCom(PrivilegeMode initial_mode);
     ~ARM_DynCom();
 
     void SetPC(u32 pc) override;
diff --git a/src/core/arm/dyncom/arm_dyncom_run.cpp b/src/core/arm/dyncom/arm_dyncom_run.cpp
index 15677da27..d79e3e4b2 100644
--- a/src/core/arm/dyncom/arm_dyncom_run.cpp
+++ b/src/core/arm/dyncom/arm_dyncom_run.cpp
@@ -2,9 +2,6 @@
 // Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
-#include <assert.h>
-
-#include "common/logging/log.h"
 #include "core/arm/skyeye_common/armdefs.h"
 
 void switch_mode(arm_core_t *core, uint32_t mode) {
@@ -13,6 +10,7 @@ void switch_mode(arm_core_t *core, uint32_t mode) {
 
     if (mode != USERBANK) {
         switch (core->Mode) {
+        case SYSTEM32MODE: // Shares registers with user mode
         case USER32MODE:
             core->Reg_usr[0] = core->Reg[13];
             core->Reg_usr[1] = core->Reg[14];
@@ -42,7 +40,6 @@ void switch_mode(arm_core_t *core, uint32_t mode) {
             core->Reg_firq[1] = core->Reg[14];
             core->Spsr[FIQBANK] = core->Spsr_copy;
             break;
-
         }
 
         switch (mode) {
@@ -81,11 +78,15 @@ void switch_mode(arm_core_t *core, uint32_t mode) {
             core->Spsr_copy = core->Spsr[FIQBANK];
             core->Bank = FIQBANK;
             break;
-
+        case SYSTEM32MODE: // Shares registers with user mode.
+            core->Reg[13] = core->Reg_usr[0];
+            core->Reg[14] = core->Reg_usr[1];
+            core->Bank = SYSTEMBANK;
+            break;
         }
+
+        // Set the mode bits in the APSR
+        core->Cpsr = (core->Cpsr & ~core->Mode) | mode;
         core->Mode = mode;
-    } else {
-        LOG_CRITICAL(Core_ARM11, "user mode");
-        exit(-2);
     }
 }
diff --git a/src/core/arm/skyeye_common/armdefs.h b/src/core/arm/skyeye_common/armdefs.h
index 02f54f385..dc2256a35 100644
--- a/src/core/arm/skyeye_common/armdefs.h
+++ b/src/core/arm/skyeye_common/armdefs.h
@@ -270,7 +270,7 @@ enum {
 *                          Mode and Bank Constants                          *
 \***************************************************************************/
 
-enum {
+enum PrivilegeMode {
     USER32MODE   = 16,
     FIQ32MODE    = 17,
     IRQ32MODE    = 18,
@@ -288,7 +288,7 @@ enum {
     ABORTBANK  = 4,
     UNDEFBANK  = 5,
     DUMMYBANK  = 6,
-    SYSTEMBANK = USERBANK
+    SYSTEMBANK = 7
 };
 
 /***************************************************************************\
diff --git a/src/core/core.cpp b/src/core/core.cpp
index 63be27be2..15787bc17 100644
--- a/src/core/core.cpp
+++ b/src/core/core.cpp
@@ -56,11 +56,10 @@ void Stop() {
 
 /// Initialize the core
 int Init() {
-    LOG_DEBUG(Core, "initialized OK");
-
-    g_sys_core = new ARM_DynCom();
-    g_app_core = new ARM_DynCom();
+    g_sys_core = new ARM_DynCom(USER32MODE);
+    g_app_core = new ARM_DynCom(USER32MODE);
 
+    LOG_DEBUG(Core, "Initialized OK");
     return 0;
 }
 
@@ -68,7 +67,7 @@ void Shutdown() {
     delete g_app_core;
     delete g_sys_core;
 
-    LOG_DEBUG(Core, "shutdown OK");
+    LOG_DEBUG(Core, "Shutdown OK");
 }
 
 } // namespace