1 files changed, 38 insertions, 30 deletions

diff --git a/firmware/target/arm/crt0-pp.S b/firmware/target/arm/crt0-pp.S
index 12c885068d..4a9d4232b4 100644
--- a/firmware/target/arm/crt0-pp.S
+++ b/firmware/target/arm/crt0-pp.S
@@ -262,17 +262,6 @@ cpu_init:
     strhi  r4, [r2], #4
     bhi    1b
 
-    /* Load stack munge value */    
-    ldr    r4, =0xdeadbeef
-
-    /* Set up some stack and munge it with 0xdeadbeef */
-    ldr    r2, =stackbegin
-    ldr    sp, =stackend
-1:
-    cmp    sp, r2
-    strhi  r4, [r2], #4
-    bhi    1b
-
 #if NUM_CORES > 1
     /* Set up idle stack and munge it with 0xdeadbeef */
     ldr    r2, =cpu_idlestackbegin
@@ -289,14 +278,28 @@ cpu_init:
     /* Set up stack for FIQ mode */ 
     msr    cpsr_c, #0xd1 /* IRQ/FIQ disabled */
     ldr    sp, =fiq_stack
-    /* Let abort and undefined modes use IRQ stack */
+
+    /* Let svc, abort and undefined modes use irq stack */
+    msr    cpsr_c, #0xd3 /* IRQ/FIQ disabled */
+    ldr    sp, =irq_stack
     msr    cpsr_c, #0xd7 /* IRQ/FIQ disabled */
     ldr    sp, =irq_stack
     msr    cpsr_c, #0xdb /* IRQ/FIQ disabled */
     ldr    sp, =irq_stack
 
-    /* Switch back to supervisor mode */
-    msr    cpsr_c, #0xd3
+    /* Switch to sys mode */
+    msr    cpsr_c, #0xdf
+
+    /* Load stack munge value */    
+    ldr    r4, =0xdeadbeef
+
+    /* Set up some stack and munge it with 0xdeadbeef */
+    ldr    r2, =stackbegin
+    ldr    sp, =stackend
+1:
+    cmp    sp, r2
+    strhi  r4, [r2], #4
+    bhi    1b
 
     /* Delay waking the COP until thread initialization is complete unless dual-core
        support is not enabled in which case the cop_main function does not perform
@@ -320,15 +323,6 @@ cop_init:
     beq    1b
 #endif
 
-    /* Set up idle stack for COP and munge it with 0xdeadbeef */
-    ldr    sp, =cop_idlestackend
-    ldr    r2, =cop_idlestackbegin
-    ldr    r4, =0xdeadbeef
-2:
-    cmp    sp, r2
-    strhi  r4, [r2], #4
-    bhi    2b
-
     /* Set up stack for IRQ mode */
     msr    cpsr_c, #0x92 /* IRQ disabled, FIQ enabled */
     ldr    sp, =cop_irq_stack
@@ -336,15 +330,26 @@ cop_init:
     msr    cpsr_c, #0xd1 /* IRQ/FIQ disabled */
     ldr    sp, =cop_fiq_stack
 
-    /* Let abort and undefined modes use IRQ stack */
+    /* Let svc, abort and undefined modes use irq stack */
+    msr    cpsr_c, #0xd3 /* IRQ/FIQ disabled */
+    ldr    sp, =cop_irq_stack
     msr    cpsr_c, #0xd7 /* IRQ/FIQ disabled */
     ldr    sp, =cop_irq_stack
     msr    cpsr_c, #0xdb /* IRQ/FIQ disabled */
     ldr    sp, =cop_irq_stack
 
-    /* Switch back to supervisor mode */
-    msr    cpsr_c, #0xd3
-    
+    /* Switch to sys mode */
+    msr    cpsr_c, #0xdf
+
+    /* Set up idle stack for COP and munge it with 0xdeadbeef */
+    ldr    sp, =cop_idlestackend
+    ldr    r2, =cop_idlestackbegin
+    ldr    r4, =0xdeadbeef
+2:
+    cmp    sp, r2
+    strhi  r4, [r2], #4
+    bhi    2b
+   
     /* Run cop_main() in apps/main.c */
     ldr    pc, =cop_main
 
@@ -383,12 +388,15 @@ undef_instr_handler:
     mov    r1, #0
     b      UIE
 
-/* We run supervisor mode most of the time, and should never see a software
-   exception being thrown. Perhaps make it illegal and call UIE?
+/* We run sys mode most of the time, and should never see a software
+   exception being thrown. Make it illegal and call UIE.
  */
 software_int_handler:
 reserved_handler:
-    movs   pc, lr
+    sub    r0, lr, #4
+    mov    r1, #4
+    b      UIE
+
 prefetch_abort_handler:
     sub    r0, lr, #4
     mov    r1, #1