1 files changed, 20 insertions, 16 deletions

diff --git a/firmware/target/arm/crt0.S b/firmware/target/arm/crt0.S
index 3765df9ffc..f75f37006d 100644
--- a/firmware/target/arm/crt0.S
+++ b/firmware/target/arm/crt0.S
@@ -100,15 +100,6 @@ newstart:
     strhi   r4, [r2], #4
     bhi     1b
 
-    /* Set up some stack and munge it with 0xdeadbeef */
-    ldr     sp, =stackend
-    ldr     r2, =stackbegin
-    ldr     r3, =0xdeadbeef
-1:
-    cmp     sp, r2
-    strhi   r3, [r2], #4
-    bhi     1b
-    
     /* Set up stack for IRQ mode */ 
     msr     cpsr_c, #0xd2
     ldr     sp, =irq_stack
@@ -117,14 +108,26 @@ newstart:
     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 back 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
+    ldr     r2, =stackbegin
+    ldr     r3, =0xdeadbeef
+1:
+    cmp     sp, r2
+    strhi   r3, [r2], #4
+    bhi     1b
+
     ldr     ip, =main       @ make sure we are using the virtual address
     bx      ip
 
@@ -142,11 +145,13 @@ 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
@@ -165,5 +170,4 @@ irq_stack:
 /* 256 words of FIQ stack */
     .space 256*4
 fiq_stack:
-
 end: