Fix potential bugs in the exception handlers

exceptions/arm11/source/handlers.h

@@ -11,7 +11,7 @@
 #define NULL 0
 
 void __attribute__((noreturn)) mcuReboot(void);
-void clearDCacheAndDMB(void);
+void cleanInvalidateDCacheAndDMB(void);
 
 void FIQHandler(void);
 void undefinedInstructionHandler(void);

exceptions/arm11/source/handlers.s

@@ -30,6 +30,33 @@ _commonHandler:
     mov r6, sp
     mrs r3, cpsr
     
+    cmp r1, #1
+    bne noFPUInit
+    tst r2, #0x20
+    bne noFPUInit
+    
+    ldr r4, [lr, #-4]
+    lsl r4, #4
+    sub r4, #0xc0000000
+    cmp r4, #0x30000000
+    bcs noFPUInit
+    fmrx r3, fpexc
+    tst r3, #0x40000000
+    bne noFPUInit
+    
+    sub lr, #4
+    srsfd sp!, #0x13
+    ldmfd sp!, {r0-r7}      @ restore context
+    cps #0x13               @ FPU init
+    stmfd sp, {r0-r3, r11-lr}^
+    sub sp, #0x20
+    bl .                    @ will be replaced
+    ldmfd sp, {r0-r3, r11-lr}^
+    add sp, #0x20
+    rfefd sp!
+
+    noFPUInit:
+
     ands r4, r2, #0xf       @ get the mode that triggered the exception
     moveq r4, #0xf          @ usr => sys
     bic r5, r3, #0xf
@@ -38,40 +65,13 @@ _commonHandler:
     stmfd r6!, {r8-lr}
     msr cpsr_c, r3          @ restore processor mode
     mov sp, r6
-    fmrx r3, fpexc
-
-    cmp r1, #1
-    bne noFPUInit
-    tst r5, #0x20
-    bne noFPUInit
     
-    ldr r4, [lr, #-4]
-    lsl r4, #4
-    sub r4, #0xc0000000
-    cmp r4, #0x30000000
-    bcs noFPUInit
-    tst r3, #0x40000000
-    bne noFPUInit
-    
-    sub lr, #4
-    srsfd sp!, #0x13
-    add sp, #28             @ restore context
-    ldmfd sp!, {r0-r7}
-    cps #0x13               @ FPU init
-    stmfd sp, {r0-r3, r11-lr}^
-    sub sp, #0x20
-    bl .                    @ will be replaced
-    ldmfd sp, {r0-r3, r11-lr}^
-    add sp, #0x20
-    rfefd sp!    
-    
-    noFPUInit:
     stmfd sp!, {r2,lr}
 
     mrc p15,0,r4,c5,c0,0    @ dfsr
-    mrc p15,0,r5,c5,c0,1    @ ifsr 
+    mrc p15,0,r5,c5,c0,1    @ ifsr
     mrc p15,0,r6,c6,c0,0    @ far
-    mov r7, r3
+    fmrx r7, fpexc
     fmrx r8, fpinst
     fmrx r9, fpinst2
     
@@ -96,9 +96,9 @@ GEN_HANDLER dataAbortHandler
 mcuReboot:
     b .                     @ will be replaced
     
-.global clearDCacheAndDMB
+.global cleanInvalidateDCacheAndDMB
-.type   clearDCacheAndDMB, %function
+.type   cleanInvalidateDCacheAndDMB, %function
-clearDCacheAndDMB:
+cleanInvalidateDCacheAndDMB:
     mov r0, #0
     mcr p15,0,r0,c7,c14,0   @ Clean and Invalidate Entire Data Cache
     mcr p15,0,r0,c7,c10,4   @ Drain Memory Barrier

exceptions/arm11/source/mainHandler.c

@@ -92,6 +92,6 @@ void __attribute__((noreturn)) mainHandler(u32 regs[REG_DUMP_SIZE / 4], u32 type
     *(ExceptionDumpHeader *)final = dumpHeader;
 
     
-    clearDCacheAndDMB();
+    cleanInvalidateDCacheAndDMB();
     mcuReboot(); //Also contains DCache-cleaning code
 }

exceptions/arm9/source/handlers.s

@@ -36,7 +36,7 @@ _commonHandler:
     orr r5, r4
     msr cpsr_c, r5          @ change processor mode
     stmfd r6!, {r8-lr}
-    msr cpsr_c, r3          @ restore processor mode
+    msr cpsr_cx, r3         @ restore processor mode
     mov sp, r6
     
     stmfd sp!, {r2,lr}      @ it's a bit of a mess, but we will fix that later