arm: Fix software interrupt handler

When we take a software interrupt, we are already in supervisor mode.
get_bad_stack assumes we are not in supervisor mode so it can clobber
the stack pointer. This causes us to have an invalid stack once that
macro finishes. Revert back to the get_bad_stack_swi macro which was
previously removed.

Fixes: 41623c91b09 ("arm: move exception handling out of start.S files")
Signed-off-by: Sean Anderson <sean.anderson@seco.com>

diff --git a/arch/arm/lib/vectors.S b/arch/arm/lib/vectors.S
index 7cf7d1636f54c68f8e023d4e3fc94f1b1965cd33..fe8ca403ac9ec043e61d7899d81224567649f5ee 100644 (file)
--- a/arch/arm/lib/vectors.S
+++ b/arch/arm/lib/vectors.S
@@ -240,6 +240,18 @@ IRQ_STACK_START_IN:
        movs    pc, lr          @ jump to next instruction & switch modes.
        .endm
 
+       .macro get_bad_stack_swi
+       sub     r13, r13, #4    @ space on current stack for scratch reg.
+       str     r0, [r13]       @ save R0's value.
+       ldr     r0, IRQ_STACK_START_IN          @ get data regions start
+       str     lr, [r0]        @ save caller lr in position 0 of saved stack
+       mrs     lr, spsr        @ get the spsr
+       str     lr, [r0, #4]    @ save spsr in position 1 of saved stack
+       ldr     lr, [r0]        @ restore lr
+       ldr     r0, [r13]       @ restore r0
+       add     r13, r13, #4    @ pop stack entry
+       .endm
+
        .macro get_irq_stack                    @ setup IRQ stack
        ldr     sp, IRQ_STACK_START
        .endm
@@ -260,7 +272,7 @@ undefined_instruction:
 
        .align  5
 software_interrupt:
-       get_bad_stack
+       get_bad_stack_swi
        bad_save_user_regs
        bl      do_software_interrupt