1 files changed, 21 insertions, 16 deletions

diff --git a/firmware/target/arm/pnx0101/crt0-pnx0101.S b/firmware/target/arm/pnx0101/crt0-pnx0101.S
index c8c4232a3a..f297c29d0c 100644
--- a/firmware/target/arm/pnx0101/crt0-pnx0101.S
+++ b/firmware/target/arm/pnx0101/crt0-pnx0101.S
@@ -115,16 +115,6 @@ start:
     strhi  r4, [r2], #4
     bhi    1b
     
-    /* Set up some stack and munge it with 0xdeadbeef */
-    ldr    sp, =stackend
-    mov    r3, sp
-    ldr    r2, =stackbegin
-    ldr    r4, =0xdeadbeef
-1:
-    cmp    r3, r2
-    strhi  r4, [r2], #4
-    bhi    1b
-
     /* Set up stack for IRQ mode */ 
     msr    cpsr_c, #0xd2
     ldr    sp, =irq_stack
@@ -132,14 +122,27 @@ start:
     msr    cpsr_c, #0xd1
     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
+    ldr    sp =irq_stack
     msr    cpsr_c, #0xd7
     ldr    sp, =irq_stack
     msr    cpsr_c, #0xdb
     ldr    sp, =irq_stack
-    /* Switch to supervisor mode */
-    msr    cpsr_c, #0xd3
+
+    /* Switch to sys mode */
+    msr    cpsr_c, #0xdf
+
+    /* Set up some stack and munge it with 0xdeadbeef */
     ldr    sp, =stackend
+    mov    r3, sp
+    ldr    r2, =stackbegin
+    ldr    r4, =0xdeadbeef
+1:
+    cmp    r3, r2
+    strhi  r4, [r2], #4
+    bhi    1b
+
     bl     main
     /* main() should never return */
    
@@ -178,12 +181,14 @@ 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