7 files changed, 28 insertions, 33 deletions

diff --git a/src/core/arm/dyncom/arm_dyncom.cpp b/src/core/arm/dyncom/arm_dyncom.cpp
index c4af85242..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,29 +16,29 @@ 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());
+    ARMul_SelectProcessor(state.get(), ARM_v6_Prop | ARM_v5_Prop | ARM_v5e_Prop);
 
     state->abort_model = ABORT_BASE_RESTORED;
     state->cpu = (cpu_config_t*)&s_arm11_cpu_info;
-    state->bigendSig = LOW;
 
-    ARMul_SelectProcessor(state.get(), ARM_v6_Prop | ARM_v5_Prop | ARM_v5e_Prop);
+    state->bigendSig = LOW;
     state->lateabtSig = LOW;
+    state->NirqSig = HIGH;
 
     // Reset the core to initial state
-    ARMul_CoProInit(state.get());
     ARMul_Reset(state.get());
     state->NextInstr = RESUME; // NOTE: This will be overwritten by LoadContext
     state->Emulate = RUN;
 
-    state->Reg[15] = 0x00000000;
-    state->Reg[13] = 0x10000000; // Set stack pointer to the top of the stack
-    state->NirqSig = HIGH;
+    // Switch to the desired privilege mode.
+    switch_mode(state.get(), initial_mode);
 
-    VFPInit(state.get()); // Initialize the VFP
+    state->Reg[13] = 0x10000000; // Set stack pointer to the top of the stack
+    state->Reg[15] = 0x00000000;
 }
 
 ARM_DynCom::~ARM_DynCom() {
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/interpreter/arminit.cpp b/src/core/arm/interpreter/arminit.cpp
index 0c0ce6c91..d3174c9a0 100644
--- a/src/core/arm/interpreter/arminit.cpp
+++ b/src/core/arm/interpreter/arminit.cpp
@@ -64,10 +64,9 @@ void ARMul_SelectProcessor(ARMul_State* state, unsigned properties)
     state->is_pxa27x = (properties & ARM_PXA27X_Prop) != 0;
     state->is_v7     = (properties & ARM_v7_Prop) != 0;
 
-    /* Only initialse the coprocessor support once we
-       know what kind of chip we are dealing with.  */
-    //ARMul_CoProInit (state);
-
+    // Only initialse the coprocessor support once we
+    // know what kind of chip we are dealing with.
+    ARMul_CoProInit(state);
 }
 
 /***************************************************************************\
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/arm/skyeye_common/vfp/vfp.cpp b/src/core/arm/skyeye_common/vfp/vfp.cpp
index 1cf146c53..6f22923bb 100644
--- a/src/core/arm/skyeye_common/vfp/vfp.cpp
+++ b/src/core/arm/skyeye_common/vfp/vfp.cpp
@@ -26,8 +26,6 @@
 #include "core/arm/skyeye_common/vfp/asm_vfp.h"
 #include "core/arm/skyeye_common/vfp/vfp.h"
 
-//ARMul_State* persistent_state; /* function calls from SoftFloat lib don't have an access to ARMul_state. */
-
 unsigned VFPInit(ARMul_State* state)
 {
     state->VFP[VFP_OFFSET(VFP_FPSID)] = VFP_FPSID_IMPLMEN<<24 | VFP_FPSID_SW<<23 | VFP_FPSID_SUBARCH<<16 |
@@ -35,9 +33,6 @@ unsigned VFPInit(ARMul_State* state)
     state->VFP[VFP_OFFSET(VFP_FPEXC)] = 0;
     state->VFP[VFP_OFFSET(VFP_FPSCR)] = 0;
 
-    //persistent_state = state;
-    /* Reset only specify VFP_FPEXC_EN = '0' */
-
     return 0;
 }
 
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