diff --git a/firmware/target/arm/rk27xx/app.lds b/firmware/target/arm/rk27xx/app.lds
index 080c74f..a282a4d 100644
--- a/firmware/target/arm/rk27xx/app.lds
+++ b/firmware/target/arm/rk27xx/app.lds
@@ -89,6 +89,9 @@ SECTIONS
      _fiqstackbegin = .;
      . += 0x400;
      _fiqstackend = .;
+     _svcabtundfstackbegin = .;
+     . += 0x100;
+     _svcabtundfstackend = .;
   } > DRAM

   .bss (NOLOAD) : {
diff --git a/firmware/target/arm/rk27xx/crt0.S b/firmware/target/arm/rk27xx/crt0.S
index 4ddae01..c29657f 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,25 @@ newstart2:
     msr     cpsr_c, #0xd1
     ldr     sp, =_fiqstackend

-    /* Let abort and undefined modes use IRQ stack */
+    /* Let svc, abort and undefined modes use separate stack */
+    ldr     sp, =_svcabtundfstackend
     msr     cpsr_c, #0xd7
-    ldr     sp, =_irqstackend
+    ldr     sp, =_svcabtundfstackend
     msr     cpsr_c, #0xdb
-    ldr     sp, =_irqstackend
+    ldr     sp, =_svcabtundfstackend
+
+    /* 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

-    /* Switch back to supervisor mode */
-    msr     cpsr_c, #0xd3

     bl      main

@@ -196,12 +198,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 +207,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/system-arm.c b/firmware/target/arm/system-arm.c
index 59eaa90..23ccfd1 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