1 files changed, 76 insertions, 0 deletions
diff --git a/ARM.cpp b/ARM.cpp
index 0d274b9..f5e7ad7 100644
--- a/ARM.cpp
+++ b/ARM.cpp
@@ -74,6 +74,82 @@ void ARM::RestoreCPSR()
     printf("TODO: restore CPSR\n");
 }
 
+void ARM::UpdateMode(u32 oldmode, u32 newmode)
+{
+    u32 temp;
+    #define SWAP(a, b)  temp = a; a = b; b = temp;
+
+    if ((oldmode & 0x1F) == (newmode & 0x1F)) return;
+
+    switch (oldmode & 0x1F)
+    {
+    case 0x11:
+        SWAP(R[8], R_FIQ[0]);
+        SWAP(R[9], R_FIQ[1]);
+        SWAP(R[10], R_FIQ[2]);
+        SWAP(R[11], R_FIQ[3]);
+        SWAP(R[12], R_FIQ[4]);
+        SWAP(R[13], R_FIQ[5]);
+        SWAP(R[14], R_FIQ[6]);
+        break;
+
+    case 0x12:
+        SWAP(R[13], R_IRQ[0]);
+        SWAP(R[14], R_IRQ[1]);
+        break;
+
+    case 0x13:
+        SWAP(R[13], R_SVC[0]);
+        SWAP(R[14], R_SVC[1]);
+        break;
+
+    case 0x17:
+        SWAP(R[13], R_ABT[0]);
+        SWAP(R[14], R_ABT[1]);
+        break;
+
+    case 0x1B:
+        SWAP(R[13], R_UND[0]);
+        SWAP(R[14], R_UND[1]);
+        break;
+    }
+
+    switch (newmode & 0x1F)
+    {
+    case 0x11:
+        SWAP(R[8], R_FIQ[0]);
+        SWAP(R[9], R_FIQ[1]);
+        SWAP(R[10], R_FIQ[2]);
+        SWAP(R[11], R_FIQ[3]);
+        SWAP(R[12], R_FIQ[4]);
+        SWAP(R[13], R_FIQ[5]);
+        SWAP(R[14], R_FIQ[6]);
+        break;
+
+    case 0x12:
+        SWAP(R[13], R_IRQ[0]);
+        SWAP(R[14], R_IRQ[1]);
+        break;
+
+    case 0x13:
+        SWAP(R[13], R_SVC[0]);
+        SWAP(R[14], R_SVC[1]);
+        break;
+
+    case 0x17:
+        SWAP(R[13], R_ABT[0]);
+        SWAP(R[14], R_ABT[1]);
+        break;
+
+    case 0x1B:
+        SWAP(R[13], R_UND[0]);
+        SWAP(R[14], R_UND[1]);
+        break;
+    }
+
+    #undef SWAP
+}
+
 s32 ARM::Execute(s32 cycles)
 {
     while (cycles > 0)