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: 41623c91b0 ("arm: move exception handling out of start.S files")
Signed-off-by: Sean Anderson <sean.anderson@seco.com>

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