16 files changed, 285 insertions, 220 deletions

diff --git a/firmware/target/arm/at91sam/lyre_proto1/crt0.S b/firmware/target/arm/at91sam/lyre_proto1/crt0.S
index 0146a8409c..029c1b7aec 100644
--- a/firmware/target/arm/at91sam/lyre_proto1/crt0.S
+++ b/firmware/target/arm/at91sam/lyre_proto1/crt0.S
@@ -33,6 +33,8 @@
 #define ARM_MODE_FIQ     0x11
 #define ARM_MODE_IRQ     0x12
 #define ARM_MODE_SVC     0x13
+#define ARM_MODE_UND     0x1b
+#define ARM_MODE_SYS     0x1f
 
 #define I_BIT            0x80
 #define F_BIT            0x40
@@ -163,6 +165,15 @@ _stack_init:
     * r2 = IRQ_STACK_SIZE
     */
 
+    /*- Set up Interrupt Mode and set IRQ Mode Stack */
+    msr      CPSR_c, #ARM_MODE_IRQ | I_BIT | F_BIT /* Enter in Mode Interrupt
+    * and disable IRQ (Interrupt) and FIQ (Fast Interrupt)
+    */
+
+    mov      sp, r0 /* sp (Interrupt Mode Stack Pointer) =
+    * TOP_OF_MEMORY (end of IRAM 1)
+    */
+
     /*- Set up Abort Mode and set ABT Mode Stack */
     msr      CPSR_c, #ARM_MODE_ABT | I_BIT | F_BIT /* Enter in Mode Abort
     * and disable IRQ (Interrupt) and FIQ (Fast Interrupt)
@@ -172,26 +183,27 @@ _stack_init:
     * (end of IRAM 1)
     */
 
-    /* put r0 with value of the new address for next Stack */
-    sub      r0, r0, r1 /* r0 = r0 - r1 --> r0 = (end of IRAM 1) -
-    * (ABT_STACK_SIZE)
-    */
-
-    /*- Set up Interrupt Mode and set IRQ Mode Stack */
-    msr      CPSR_c, #ARM_MODE_IRQ | I_BIT | F_BIT /* Enter in Mode Interrupt
+    /*- Set up Abort Mode and set ABT Mode Stack */
+    msr      CPSR_c, #ARM_MODE_UND | I_BIT | F_BIT /* Enter in Mode Undefined
     * and disable IRQ (Interrupt) and FIQ (Fast Interrupt)
     */
 
-    mov      sp, r0 /* sp (Interrupt Mode Stack Pointer) =
-    * TOP_OF_MEMORY (end of IRAM 1) - (ABT_STACK_SIZE)
+    mov      sp, r0 /* sp (ABT Mode Stack Pointer) = TOP_OF_MEMORY
+    * (end of IRAM 1)
     */
 
+    /*- Set up Supervisor Mode and set Supervisor Mode Stack */
+    msr      CPSR_c, #ARM_MODE_SVC | I_BIT | F_BIT
+    mov      sp, r0
+
     sub      r0, r0, r2 /* Put on r0 the new address for next
-    * Stack (Supervisor Mode)
+    * Stack (Sys Mode)
     */
 
-/*- Enable interrupt & Set up Supervisor Mode and set Supervisor Mode Stack */
-    msr      CPSR_c, #ARM_MODE_SVC | F_BIT
+    /*- Set up Sys Mode and set Sys Mode Stack */
+    msr      CPSR_c, #ARM_MODE_SYS | F_BIT /* Enter in Sys Mode
+    * and enable IRQ (Interrupt) and disable FIQ (Fast Interrupt)
+    */
     mov      sp, r0
 
 /*-----------------------------------------------------------------------------
diff --git a/firmware/target/arm/crt0-pp-bl.S b/firmware/target/arm/crt0-pp-bl.S
index d1d9182314..01681288f9 100644
--- a/firmware/target/arm/crt0-pp-bl.S
+++ b/firmware/target/arm/crt0-pp-bl.S
@@ -61,7 +61,7 @@ start:
     .equ    CACHE_ENAB,  0x1
 #endif
 
-    msr    cpsr_c, #0xd3 /* enter supervisor mode, disable IRQ */
+    msr    cpsr_c, #0xdf /* enter sys mode, disable IRQ */
 #ifndef E200R_INSTALLER
 /* 1 - Copy the bootloader to IRAM */
     /* get the high part of our execute address */
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
diff --git a/firmware/target/arm/crt0-pp502x-bl-usb.S b/firmware/target/arm/crt0-pp502x-bl-usb.S
index c8b7fb4ee8..7b0489b2a8 100644
--- a/firmware/target/arm/crt0-pp502x-bl-usb.S
+++ b/firmware/target/arm/crt0-pp502x-bl-usb.S
@@ -239,26 +239,28 @@ start_stub_end:
     strhi   r0, [r1], #4
     bhi     1b
 
-    /* Set up some stack and munge it with 0xdeadbeef */
-    ldr     r0, =0xdeadbeef
-    ldr     r1, =stackbegin
-    ldr     sp, =stackend
-1:
-    cmp     sp, r1
-    strhi   r0, [r1], #4
-    bhi     1b
-
     /* Set up stack for IRQ mode */ 
     msr     cpsr_c, #0xd2 /* IRQ/FIQ disabled */
     ldr     sp, =irq_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 /* 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 back to sys mode */
+    msr     cpsr_c, #0xdf
+
+    /* Set up some stack and munge it with 0xdeadbeef */
+    ldr     r0, =0xdeadbeef
+    ldr     r1, =stackbegin
+    ldr     sp, =stackend
+1:
+    cmp     sp, r1
+    strhi   r0, [r1], #4
+    bhi     1b
 
     /* execute the loader - this will load an image to 0x10000000 */
     ldr     r0, =main
@@ -335,12 +337,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
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:
diff --git a/firmware/target/arm/imx233/crt0.S b/firmware/target/arm/imx233/crt0.S
index abbff5816a..4ae083ccb2 100644
--- a/firmware/target/arm/imx233/crt0.S
+++ b/firmware/target/arm/imx233/crt0.S
@@ -126,15 +126,6 @@ remap:
     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
@@ -143,14 +134,25 @@ remap:
     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
 
     /* Jump to main */
     mov     r0, r6
@@ -173,11 +175,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
@@ -196,5 +200,4 @@ irq_stack:
 /* 256 words of FIQ stack */
     .space 256*4
 fiq_stack:
-
 end:
diff --git a/firmware/target/arm/imx31/crt0.S b/firmware/target/arm/imx31/crt0.S
index 776699da14..69577e4d3d 100644
--- a/firmware/target/arm/imx31/crt0.S
+++ b/firmware/target/arm/imx31/crt0.S
@@ -253,15 +253,6 @@ remap_end:
     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
@@ -270,14 +261,25 @@ remap_end:
     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
 
 #ifndef BOOTLOADER
     /* Enable access to VFP */
@@ -334,11 +336,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
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
diff --git a/firmware/target/arm/rk27xx/crt0.S b/firmware/target/arm/rk27xx/crt0.S
index 4ddae01c53..23f3fcf2c9 100644
--- a/firmware/target/arm/rk27xx/crt0.S
+++ b/firmware/target/arm/rk27xx/crt0.S
@@ -156,15 +156,6 @@ newstart2:
     bhi     1b
 #endif
 
-    /* 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, =_irqstackend
@@ -173,14 +164,26 @@ newstart2:
     msr     cpsr_c, #0xd1
     ldr     sp, =_fiqstackend
 
-    /* Let abort and undefined modes use IRQ stack */
+    /* Let svc, abort and undefined modes use irq stack */
+    msr     cpsr_c, #0xd3
+    ldr     sp, =_irqstackend
     msr     cpsr_c, #0xd7
     ldr     sp, =_irqstackend
     msr     cpsr_c, #0xdb
     ldr     sp, =_irqstackend
 
-    /* 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
+
 
     bl      main
 
@@ -196,12 +199,6 @@ 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? */
-software_int_handler:
-reserved_handler:
-    movs   pc, lr
-
 prefetch_abort_handler:
     sub    r0, lr, #4
     mov    r1, #1
@@ -211,3 +208,11 @@ data_abort_handler:
     sub    r0, lr, #8 
     mov    r1, #2
     b      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:
+    sub    r0, lr, #4
+    mov    r1, #5
+    b      UIE
diff --git a/firmware/target/arm/s3c2440/crt0.S b/firmware/target/arm/s3c2440/crt0.S
index faa54313e7..8cbd8ab91e 100644
--- a/firmware/target/arm/s3c2440/crt0.S
+++ b/firmware/target/arm/s3c2440/crt0.S
@@ -527,32 +527,34 @@ bsszero:
     cmp     r3, r2
     strhi   r4, [r2], #4
     bhi     bsszero
-    
-    /* Set up some stack and munge it with 0xdeadbeef */
-    ldr     sp, =stackend
-    mov     r3, sp
-    ldr     r2, =stackbegin
-    ldr     r4, =0xdeadbeef
-stackmunge:
-    cmp     r3, r2
-    strhi   r4, [r2], #4
-    bhi     stackmunge
 
-     /* Set up stack for IRQ mode */
+    /* Set up stack for IRQ mode */
     msr     cpsr_c, #0xd2
     ldr     sp, =irq_stack
     /* Set up stack for FIQ mode */ 
     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
+stackmunge:
+    cmp     r3, r2
+    strhi   r4, [r2], #4
+    bhi     stackmunge
 
     /* Start the main function */
     ldr     lr, =vectors
@@ -573,12 +575,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
@@ -749,4 +753,3 @@ irq_stack:
 /* 256 words of FIQ stack */
     .space 256*4
 fiq_stack:
-
diff --git a/firmware/target/arm/s5l8700/crt0.S b/firmware/target/arm/s5l8700/crt0.S
index 0d9793208c..0582ab0c8f 100644
--- a/firmware/target/arm/s5l8700/crt0.S
+++ b/firmware/target/arm/s5l8700/crt0.S
@@ -465,15 +465,6 @@ start_loc:
     bhi     1b
 #endif
 
-    /* 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, =_irqstackend
@@ -482,14 +473,25 @@ start_loc:
     msr     cpsr_c, #0xd1
     ldr     sp, =_fiqstackend
 
-    /* Let abort and undefined modes use IRQ stack */
+    /* Let svc, abort and undefined modes use irq stack */
+    msr     cpsr_c, #0xd3
+    ldr     sp, =_irqstackend
     msr     cpsr_c, #0xd7
     ldr     sp, =_irqstackend
     msr     cpsr_c, #0xdb
     ldr     sp, =_irqstackend
 
-    /* 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
 
 // if we did not switch remap on, device
 // would crash when MENU is pressed,
@@ -518,11 +520,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
diff --git a/firmware/target/arm/s5l8702/crt0.S b/firmware/target/arm/s5l8702/crt0.S
index da2f49c971..e894e8f157 100644
--- a/firmware/target/arm/s5l8702/crt0.S
+++ b/firmware/target/arm/s5l8702/crt0.S
@@ -150,15 +150,6 @@ start_loc:
     bhi     1b
 #endif
 
-    /* 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, =_irqstackend
@@ -167,14 +158,25 @@ start_loc:
     msr     cpsr_c, #0xd1
     ldr     sp, =_fiqstackend
 
-    /* Let abort and undefined modes use IRQ stack */
+    /* Let svc, abort and undefined modes use irq stack */
+    msr     cpsr_c, #0xd3
+    ldr     sp, =_irqstackend
     msr     cpsr_c, #0xd7
     ldr     sp, =_irqstackend
     msr     cpsr_c, #0xdb
     ldr     sp, =_irqstackend
 
-    /* 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
 
     bl      main
 
@@ -190,11 +192,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
diff --git a/firmware/target/arm/system-arm.c b/firmware/target/arm/system-arm.c
index 59eaa903b6..23ccfd1f14 100644
--- a/firmware/target/arm/system-arm.c
+++ b/firmware/target/arm/system-arm.c
@@ -29,7 +29,8 @@ static const char* const uiename[] = {
     "Undefined instruction",
     "Prefetch abort",
     "Data abort",
-    "Divide by zero"
+    "Divide by zero",
+    "SWI"
 };
 
 /* Unexpected Interrupt or Exception handler. Currently only deals with
diff --git a/firmware/target/arm/tcc77x/crt0.S b/firmware/target/arm/tcc77x/crt0.S
index 251fd0c903..d0873102d4 100644
--- a/firmware/target/arm/tcc77x/crt0.S
+++ b/firmware/target/arm/tcc77x/crt0.S
@@ -141,16 +141,16 @@ copied_start:
     ldr    r11, =dma_play_data
 #endif
 
-    /* Let abort and undefined modes use IRQ stack */
-    mov    r0,#0xd7
-    msr    cpsr, r0
+    /* Let svc, abort and undefined modes use irq stack */
+    msr    cpsr, #0xd3
     ldr    sp, =irq_stack
-    mov    r0,#0xdb
-    msr    cpsr, r0
+    msr    cpsr, #0xd7
+    ldr    sp, =irq_stack
+    msr    cpsr, #0xdb
     ldr    sp, =irq_stack
 
-    /* Switch to supervisor mode */
-    mov    r0,#0xd3
+    /* Switch to sys mode */
+    mov    r0,#0xdf
     msr    cpsr, r0
     ldr    sp, =stackend
     
@@ -222,12 +222,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
@@ -255,4 +257,3 @@ irq_stack:
 /* 256 words of FIQ stack */
     .space 256*4
 fiq_stack:
-
diff --git a/firmware/target/arm/tcc780x/crt0.S b/firmware/target/arm/tcc780x/crt0.S
index 1612973866..1dbccf9828 100644
--- a/firmware/target/arm/tcc780x/crt0.S
+++ b/firmware/target/arm/tcc780x/crt0.S
@@ -120,17 +120,16 @@ copied_start:
     ldr    r11, =dma_play_data
 #endif
 
-    /* Let abort and undefined modes use IRQ stack */
-    mov    r0,#0xd7
-    msr    cpsr, r0
+    /* Let svc, abort and undefined modes use irq stack */
+    msr    cpsr, #0xd3
     ldr    sp, =irq_stack
-    mov    r0,#0xdb
-    msr    cpsr, r0
+    msr    cpsr, #0xd7
+    ldr    sp, =irq_stack
+    msr    cpsr, #0xdb
     ldr    sp, =irq_stack
 
-    /* Switch to supervisor mode */
-    mov    r0,#0xd3
-    msr    cpsr, r0
+    /* Switch to sys mode */
+    msr    cpsr, #0xdf
     ldr    sp, =stackend
 
     /* Enable MMU & caches. At present this is just doing what the OF does.
@@ -293,12 +292,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
diff --git a/firmware/target/arm/tms320dm320/crt0.S b/firmware/target/arm/tms320dm320/crt0.S
index 0119ade3c7..d53cff2b63 100644
--- a/firmware/target/arm/tms320dm320/crt0.S
+++ b/firmware/target/arm/tms320dm320/crt0.S
@@ -169,23 +169,25 @@ _start:
     msr     cpsr_c, #0xd2           /* Go into irq state */
     ldr     sp, =_irq_stack_start   /* set the irq stack pointer */
 
-    /* This should not be needed, but set the stack location for abort and
-    * undefined to at least a known stack location (IRQ)
-    */
-    msr     cpsr_c, #0xd7           /* Go into abort state */
+    /* SVC, ABT, UNDEF share irq stack */
+    msr     cpsr_c, #0xd3           /* Go into svc state */
+    ldr     sp, =_irq_stack_start /* set svc stack pointer */
+
+    msr     cpsr_c, #0xd7                    /* Go into abort state */
     ldr     sp, =_irq_stack_start   /* set the stack pointer */
 
-    msr     cpsr_c, #0xdb           /* Go into undefined state */
+    msr     cpsr_c, #0xdb                    /* Go into undefined state */
     ldr     sp, =_irq_stack_start   /* set the stack pointer */
 
     /* Initialize program stack */
-    msr     cpsr_c, #0xd3           /* Go into supervisor state */
+    msr     cpsr_c, #0xdf           /* Go into sys state */
+
     ldr     r0, =0xDEADBEEF         /* Can be taken out; left for clarity */
     ldr     r1, =_pro_stack_end     /* Stack counts backwards, so end is first*/
     ldr     r2, =_pro_stack_start
     bl      _init_section
 
-    ldr     sp, =_pro_stack_start   /* set the supervisor stack pointer */
+    ldr     sp, =_pro_stack_start   /* set the sys stack pointer */
 
     /* MMU initialization */ 
     bl      ttb_init
@@ -298,6 +300,8 @@ _delay_cycles:
  *      0: Undefined Instruction                                              *
  *      1: Prefetch Abort                                                     *
  *      2: Data Abort                                                         *
+ *      3: DIV0                                                               *
+ *      4: SWI                                                                *
  *  The exceptions return operations are documented in section A2.6 of the    *
  *  ARM Architecture Reference Manual.                                        *
  ******************************************************************************/
@@ -309,11 +313,12 @@ _undefined_instruction:
     bl     UIE
 
 /* A2.6.4: Software Interrupt exception - These should not happen in Rockbox,
- * but for now leave this as a placeholder and continue with the program.
- * LR=PC of next instruction.
+ * make it illegal
  */
 _software_interrupt:
-    mov    pc, lr
+    sub    r0, lr, #4
+    mov    r1, #4
+    bl     UIE
 
 /* A2.6.5 Prefetch Abort - This is also the BKPT instruction since this is a
  * v5 target.  Pass it on to UIE since it is not currently used.