5 files changed, 138 insertions, 161 deletions

diff --git a/payloads/libpayload/arch/arm/exception.c b/payloads/libpayload/arch/arm/exception.c
index 099d2a46fa..f0ce0ea8c3 100644
--- a/payloads/libpayload/arch/arm/exception.c
+++ b/payloads/libpayload/arch/arm/exception.c
@@ -29,19 +29,27 @@
 
 #include <arch/cache.h>
 #include <arch/exception.h>
+#include <exception.h>
 #include <libpayload.h>
 #include <stdint.h>
 
-uint8_t exception_stack[0x100] __attribute__((aligned(8)));
+uint8_t exception_stack[0x1000] __attribute__((aligned(8)));
 extern void *exception_stack_end;
 
-void exception_undefined_instruction(uint32_t *);
-void exception_software_interrupt(uint32_t *);
-void exception_prefetch_abort(uint32_t *);
-void exception_data_abort(uint32_t *);
-void exception_not_used(uint32_t *);
-void exception_irq(uint32_t *);
-void exception_fiq(uint32_t *);
+struct exception_handler_info
+{
+	const char *name;
+	exception_hook hook;
+};
+
+static struct exception_handler_info exceptions[EXC_COUNT] = {
+	[EXC_UNDEF]  = { "_undefined_instruction" },
+	[EXC_SWI]    = { "_software_interrupt" },
+	[EXC_PABORT] = { "_prefetch_abort" },
+	[EXC_DABORT] = { "_data_abort" },
+	[EXC_IRQ]    = { "_irq" },
+	[EXC_FIQ]    = { "_fiq" },
+};
 
 static void dump_stack(uintptr_t addr, size_t bytes)
 {
@@ -77,59 +85,25 @@ static void print_regs(uint32_t *regs)
 	}
 }
 
-void exception_undefined_instruction(uint32_t *regs)
-{
-	printf("exception _undefined_instruction\n");
-	print_regs(regs);
-	dump_stack(regs[13], 512);
-	halt();
-}
-
-void exception_software_interrupt(uint32_t *regs)
-{
-	printf("exception _software_interrupt\n");
-	print_regs(regs);
-	dump_stack(regs[13], 512);
-	halt();
-}
-
-void exception_prefetch_abort(uint32_t *regs)
-{
-	printf("exception _prefetch_abort\n");
-	print_regs(regs);
-	dump_stack(regs[13], 512);
-	halt();
-}
-
-void exception_data_abort(uint32_t *regs)
-{
-	printf("exception _data_abort\n");
-	print_regs(regs);
-	dump_stack(regs[13], 512);
-	halt();
-}
-
-void exception_not_used(uint32_t *regs)
-{
-	printf("exception _not_used\n");
-	print_regs(regs);
-	dump_stack(regs[13], 512);
-	halt();
-}
-
-void exception_irq(uint32_t *regs)
+void exception_dispatch(struct exception_state *state, int idx);
+void exception_dispatch(struct exception_state *state, int idx)
 {
-	printf("exception _irq\n");
-	print_regs(regs);
-	dump_stack(regs[13], 512);
-	halt();
-}
-
-void exception_fiq(uint32_t *regs)
-{
-	printf("exception _fiq\n");
-	print_regs(regs);
-	dump_stack(regs[13], 512);
+	if (idx >= EXC_COUNT) {
+		printf("Bad exception index %d.\n", idx);
+	} else {
+		struct exception_handler_info *info = &exceptions[idx];
+		if (info->hook) {
+			info->hook(idx, state);
+			return;
+		}
+
+		if (info->name)
+			printf("exception %s\n", info->name);
+		else
+			printf("exception _not_used.\n");
+	}
+	print_regs(state->regs);
+	dump_stack(state->regs[13], 512);
 	halt();
 }
 
@@ -146,3 +120,9 @@ void exception_init(void)
 	set_vbar((uintptr_t)exception_table);
 	exception_stack_end = exception_stack + sizeof(exception_stack);
 }
+
+void exception_install_hook(int type, exception_hook hook)
+{
+	die_if(type >= EXC_COUNT, "Out of bounds exception index %d.\n", type);
+	exceptions[type].hook = hook;
+}
diff --git a/payloads/libpayload/arch/arm/exception_asm.S b/payloads/libpayload/arch/arm/exception_asm.S
index 1f369bcc05..6a28c5cdef 100644
--- a/payloads/libpayload/arch/arm/exception_asm.S
+++ b/payloads/libpayload/arch/arm/exception_asm.S
@@ -43,59 +43,51 @@ exception_table:
 	b	8f
 
 1:
-	ldr	sp, _not_used
+	mov	sp, $0
 	b	exception_common
 2:
-	ldr	sp, _undefined_instruction
+	mov	sp, $1
 	b	exception_common
 3:
-	ldr	sp, _software_interrupt
+	mov	sp, $2
 	b	exception_common
 4:
-	ldr	sp, _prefetch_abort
+	mov	sp, $3
 	b	exception_common
 5:
-	ldr	sp, _data_abort
+	mov	sp, $4
 	b	exception_common
 6:
-	ldr	sp, _not_used
+	mov	sp, $5
 	b	exception_common
 7:
-	ldr	sp, _irq
+	mov	sp, $6
 	b	exception_common
 8:
-	ldr	sp, _fiq
+	mov	sp, $7
 	b	exception_common
 
 exception_common:
-	str	sp, exception_handler
+	str	sp, exception_idx
 	ldr	sp, exception_stack_end
 	push	{ lr }
 	stmfd	sp, { sp, lr }^
 	sub	sp, sp, $8
 	push	{ r0 - r12 }
 	mov	r0, sp
-	mov	lr, pc
-	ldr	pc, exception_handler
+	ldr	r1, exception_idx
+	blx	exception_dispatch
 	pop	{ r0 - r12 }
 	add	sp, sp, $8
 	ldmfd	sp!, { pc }^
 
 
 	.align 2
-_undefined_instruction:	.word exception_undefined_instruction
-_software_interrupt:	.word exception_software_interrupt
-_prefetch_abort:	.word exception_prefetch_abort
-_data_abort:		.word exception_data_abort
-_not_used:		.word exception_not_used
-_irq:			.word exception_irq
-_fiq:			.word exception_fiq
-
 	.global exception_stack_end
 exception_stack_end:
 	.word 0
 
-exception_handler:
+exception_idx:
 	.word 0
 
 	.thumb
diff --git a/payloads/libpayload/arch/arm/main.c b/payloads/libpayload/arch/arm/main.c
index 4e7c05dfe1..cff992fce6 100644
--- a/payloads/libpayload/arch/arm/main.c
+++ b/payloads/libpayload/arch/arm/main.c
@@ -27,7 +27,7 @@
  * SUCH DAMAGE.
  */
 
-#include <arch/exception.h>
+#include <exception.h>
 #include <libpayload.h>
 
 unsigned int main_argc;    /**< The argc value to pass to main() */
diff --git a/payloads/libpayload/arch/x86/exception.c b/payloads/libpayload/arch/x86/exception.c
index a9a65ca5a2..33bef71e66 100644
--- a/payloads/libpayload/arch/x86/exception.c
+++ b/payloads/libpayload/arch/x86/exception.c
@@ -27,39 +27,27 @@
  * SUCH DAMAGE.
  */
 
+#include <arch/exception.h>
 #include <exception.h>
 #include <libpayload.h>
 #include <stdint.h>
 
 uint8_t exception_stack[0x1000] __attribute__((aligned(8)));
 extern void *exception_stack_end;
+extern struct exception_handler_state *exception_handler_state_handoff;
 
-struct exception_state
+struct exception_handler_state
 {
-	u32 eax;
-	u32 ecx;
-	u32 edx;
-	u32 ebx;
-	u32 esp;
-	u32 ebp;
-	u32 esi;
-	u32 edi;
-	u32 eip;
-	u32 eflags;
-	u32 cs;
-	u32 ss;
-	u32 ds;
-	u32 es;
-	u32 fs;
-	u32 gs;
+	struct exception_state regs;
 	u32 error_code;
 	u32 vector;
 } __attribute__((packed));
 
-struct exception_info
+struct exception_handler_info
 {
 	const char *name;
 	void (*error_code_printer)(u32 code);
+	exception_hook hook;
 };
 
 static void print_segment_error_code(u32 code)
@@ -105,34 +93,34 @@ static void print_raw_error_code(u32 code)
 	printf("%#x", code);
 }
 
-static struct exception_info exceptions[] = {
-	[0] =  { .name = "divide by zero" },
-	[1] =  { .name = "debug" },
-	[2] =  { .name = "non-maskable-interrupt" },
-	[3] =  { .name = "breakpoint" },
-	[4] =  { .name = "overflow" },
-	[5] =  { .name = "bound range" },
-	[6] =  { .name = "invalid opcode" },
-	[7] =  { .name = "device not available" },
-	[8] =  { .name = "double fault",
-		 .error_code_printer = &print_raw_error_code },
-	[10] = { .name = "invalid tss",
-		 .error_code_printer = &print_segment_error_code },
-	[11] = { .name = "segment not present",
-		 .error_code_printer = &print_segment_error_code },
-	[12] = { .name = "stack",
-		 .error_code_printer = &print_segment_error_code },
-	[13] = { .name = "general protection",
-		 .error_code_printer = &print_segment_error_code },
-	[14] = { .name = "page fault",
-		 .error_code_printer = &print_page_fault_error_code },
-	[16] = { .name = "x87 floating point" },
-	[17] = { .name = "alignment check",
-		 .error_code_printer = &print_raw_error_code },
-	[18] = { .name = "machine check" },
-	[19] = { .name = "SIMD floating point" },
-	[30] = { .name = "security",
-		 .error_code_printer = &print_raw_error_code },
+static struct exception_handler_info exceptions[EXC_COUNT] = {
+	[EXC_DE] =  { .name = "divide by zero" },
+	[EXC_DB] =  { .name = "debug" },
+	[EXC_NMI] =  { .name = "non-maskable-interrupt" },
+	[EXC_BP] =  { .name = "breakpoint" },
+	[EXC_OF] =  { .name = "overflow" },
+	[EXC_BR] =  { .name = "bound range" },
+	[EXC_UD] =  { .name = "invalid opcode" },
+	[EXC_NM] =  { .name = "device not available" },
+	[EXC_DF] =  { .name = "double fault",
+		      .error_code_printer = &print_raw_error_code },
+	[EXC_TS] = { .name = "invalid tss",
+		     .error_code_printer = &print_segment_error_code },
+	[EXC_NP] = { .name = "segment not present",
+		     .error_code_printer = &print_segment_error_code },
+	[EXC_SS] = { .name = "stack",
+		     .error_code_printer = &print_segment_error_code },
+	[EXC_GP] = { .name = "general protection",
+		     .error_code_printer = &print_segment_error_code },
+	[EXC_PF] = { .name = "page fault",
+		     .error_code_printer = &print_page_fault_error_code },
+	[EXC_MF] = { .name = "x87 floating point" },
+	[EXC_AC] = { .name = "alignment check",
+		     .error_code_printer = &print_raw_error_code },
+	[EXC_MC] = { .name = "machine check" },
+	[EXC_XF] = { .name = "SIMD floating point" },
+	[EXC_SX] = { .name = "security",
+		     .error_code_printer = &print_raw_error_code },
 };
 
 static void dump_stack(uintptr_t addr, size_t bytes)
@@ -150,16 +138,9 @@ static void dump_stack(uintptr_t addr, size_t bytes)
 	}
 }
 
-void exception_handler(void);
-void exception_handler(void)
+static void dump_exception_state(struct exception_handler_state *state,
+				 struct exception_handler_info *info)
 {
-	struct exception_state *state =
-		(void *)((u8 *)exception_stack_end - sizeof(*state));
-
-	struct exception_info *info = NULL;
-	if (state->vector < ARRAY_SIZE(exceptions))
-		info = &exceptions[state->vector];
-
 	if (info)
 		printf("Exception %d (%s)\n", state->vector, info->name);
 	else
@@ -169,31 +150,50 @@ void exception_handler(void)
 		info->error_code_printer(state->error_code);
 		printf("\n");
 	}
-	printf("EIP:    0x%08x\n", state->eip);
-	printf("CS:     0x%04x\n", state->cs);
-	printf("EFLAGS: 0x%08x\n", state->eflags);
-	printf("EAX:    0x%08x\n", state->eax);
-	printf("ECX:    0x%08x\n", state->ecx);
-	printf("EDX:    0x%08x\n", state->edx);
-	printf("EBX:    0x%08x\n", state->ebx);
-	printf("ESP:    0x%08x\n", state->esp);
-	printf("EBP:    0x%08x\n", state->ebp);
-	printf("ESI:    0x%08x\n", state->esi);
-	printf("EDI:    0x%08x\n", state->edi);
-	printf("DS:     0x%04x\n", state->ds);
-	printf("ES:     0x%04x\n", state->es);
-	printf("SS:     0x%04x\n", state->ss);
-	printf("FS:     0x%04x\n", state->fs);
-	printf("GS:     0x%04x\n", state->gs);
-
-	dump_stack(state->esp, 512);
-
-	halt();
+	printf("EIP:    0x%08x\n", state->regs.eip);
+	printf("CS:     0x%04x\n", state->regs.cs);
+	printf("EFLAGS: 0x%08x\n", state->regs.eflags);
+	printf("EAX:    0x%08x\n", state->regs.eax);
+	printf("ECX:    0x%08x\n", state->regs.ecx);
+	printf("EDX:    0x%08x\n", state->regs.edx);
+	printf("EBX:    0x%08x\n", state->regs.ebx);
+	printf("ESP:    0x%08x\n", state->regs.esp);
+	printf("EBP:    0x%08x\n", state->regs.ebp);
+	printf("ESI:    0x%08x\n", state->regs.esi);
+	printf("EDI:    0x%08x\n", state->regs.edi);
+	printf("DS:     0x%04x\n", state->regs.ds);
+	printf("ES:     0x%04x\n", state->regs.es);
+	printf("SS:     0x%04x\n", state->regs.ss);
+	printf("FS:     0x%04x\n", state->regs.fs);
+	printf("GS:     0x%04x\n", state->regs.gs);
+}
+
+void exception_dispatch(void)
+{
+	struct exception_handler_state *state =
+		exception_handler_state_handoff;
+
+	struct exception_handler_info *info = NULL;
+	if (state->vector < EXC_COUNT)
+		info = &exceptions[state->vector];
+
+	if (info && info->hook) {
+		info->hook(state->vector, &state->regs);
+	} else {
+		dump_exception_state(state, info);
+		dump_stack(state->regs.esp, 512);
+		halt();
+	}
 }
 
-void exception_init_asm(void);
 void exception_init(void)
 {
 	exception_stack_end = exception_stack + sizeof(exception_stack);
 	exception_init_asm();
 }
+
+void exception_install_hook(int type, exception_hook hook)
+{
+	die_if(type >= EXC_COUNT, "Out of bound exception type %d.\n", type);
+	exceptions[type].hook = hook;
+}
diff --git a/payloads/libpayload/arch/x86/exception_asm.S b/payloads/libpayload/arch/x86/exception_asm.S
index c56a7a0bab..de612ba057 100644
--- a/payloads/libpayload/arch/x86/exception_asm.S
+++ b/payloads/libpayload/arch/x86/exception_asm.S
@@ -31,6 +31,9 @@
 	.global exception_stack_end
 exception_stack_end:
 	.long 0
+	.global exception_handler_state_handoff
+exception_handler_state_handoff:
+	.long 0
 
 /* Some temporary variables which are used while saving exception state. */
 vector:
@@ -134,11 +137,13 @@ exception_common:
 	pushl	old_eax
 
 	/*
-	 * Call the C exception handler. It will find the exception state on
-	 * the exception stack. Not passing parameters means we don't have to
-	 * worry about what ABI is being used.
+	 * Call the C exception handler. It will find the exception state
+	 * using the exception_handler_state_handoff global pointer. Not
+	 * passing parameters means we don't have to worry about what ABI
+	 * is being used.
 	 */
-	call	exception_handler
+	mov	%esp, exception_handler_state_handoff
+	call	exception_dispatch
 
 	/*
 	 * Restore state from the exception state structure, including any