2 files changed, 13 insertions, 53 deletions
diff --git a/payloads/libpayload/arch/arm/exception.c b/payloads/libpayload/arch/arm/exception.c
index 8d8b50b175..099d2a46fa 100644
--- a/payloads/libpayload/arch/arm/exception.c
+++ b/payloads/libpayload/arch/arm/exception.c
@@ -27,13 +27,13 @@
  * SUCH DAMAGE.
  */
 
+#include <arch/cache.h>
 #include <arch/exception.h>
 #include <libpayload.h>
 #include <stdint.h>
 
-void exception_test(void);
-
-static int test_abort;
+uint8_t exception_stack[0x100] __attribute__((aligned(8)));
+extern void *exception_stack_end;
 
 void exception_undefined_instruction(uint32_t *);
 void exception_software_interrupt(uint32_t *);
@@ -103,14 +103,9 @@ void exception_prefetch_abort(uint32_t *regs)
 
 void exception_data_abort(uint32_t *regs)
 {
-	if (test_abort) {
-		regs[15] = regs[0];
-		return;
-	} else {
-		printf("exception _data_abort\n");
-		print_regs(regs);
-		dump_stack(regs[13], 512);
-	}
+	printf("exception _data_abort\n");
+	print_regs(regs);
+	dump_stack(regs[13], 512);
 	halt();
 }
 
@@ -138,40 +133,16 @@ void exception_fiq(uint32_t *regs)
 	halt();
 }
 
-static inline uint32_t get_sctlr(void)
-{
-	uint32_t val;
-	asm("mrc p15, 0, %0, c1, c0, 0" : "=r" (val));
-	return val;
-}
-
-static inline void set_sctlr(uint32_t val)
-{
-	asm volatile("mcr p15, 0, %0, c1, c0, 0" :: "r" (val));
-	asm volatile("" ::: "memory");
-}
-
 void exception_init(void)
 {
-	static const uint32_t sctlr_te = (0x1 << 30);
-	static const uint32_t sctlr_v = (0x1 << 13);
-	static const uint32_t sctlr_a = (0x1 << 1);
-
-	uint32_t sctlr = get_sctlr();
+	uint32_t sctlr = read_sctlr();
 	/* Handle exceptions in ARM mode. */
-	sctlr &= ~sctlr_te;
+	sctlr &= ~SCTLR_TE;
 	/* Set V=0 in SCTLR so VBAR points to the exception vector table. */
-	sctlr &= ~sctlr_v;
-	/* Enforce alignment temporarily. */
-	set_sctlr(sctlr | sctlr_a);
+	sctlr &= ~SCTLR_V;
+	write_sctlr(sctlr);
 
 	extern uint32_t exception_table[];
 	set_vbar((uintptr_t)exception_table);
-
-	test_abort = 1;
-	exception_test();
-	test_abort = 0;
-
-	/* Restore alignment settings. */
-	set_sctlr(sctlr);
+	exception_stack_end = exception_stack + sizeof(exception_stack);
 }
diff --git a/payloads/libpayload/arch/arm/exception_asm.S b/payloads/libpayload/arch/arm/exception_asm.S
index 163fdbd52a..31d759335f 100644
--- a/payloads/libpayload/arch/arm/exception_asm.S
+++ b/payloads/libpayload/arch/arm/exception_asm.S
@@ -27,11 +27,9 @@
  * SUCH DAMAGE.
  */
 
-exception_stack:
-	.align 5
-	.skip 0x2000, 0xa5
+	.global exception_stack_end
 exception_stack_end:
-	.word	exception_stack_end
+	.word	0
 
 exception_handler:
 	.word 0
@@ -105,12 +103,3 @@ set_vbar:
 	mcr	p15, 0, r0, c12, c0, 0
 	bx	lr
 
-	.global exception_test
-	.thumb_func
-exception_test:
-	mov	r1, $1
-	mov	r0, pc
-	add	r0, $3
-	ldr	r1, [r1]
-	bx	lr
-