libpayload: arm64: Make exception handling closer to arm32

This patch reworks the arm64 exception handling to be more similar to
how it works on arm32. This includes a bunch of features like actually
saving and restoring more exception state in the exception_state
structure and supporting the same sort of partial reentrancy that is
useful for GDB. Since there's no instruction to directly load into or
store out of SP on arm64, we can't do quite the same thing where we use
that to read an exception_state_ptr variable right after exception entry
when no other register is available. But we can do something very
similar by (ab-)using the "high" stack pointer (SP_EL2) as a pointer to
the exception_state struct and providing a function to change it.

Change-Id: Ia16a1124be1824392a309ae1f4cb031547d184c1
Signed-off-by: Julius Werner <jwerner@chromium.org>
Reviewed-on: https://review.coreboot.org/29018
Tested-by: build bot (Jenkins) <no-reply@coreboot.org>
Reviewed-by: Aaron Durbin <adurbin@chromium.org>

5 files changed, 191 insertions, 87 deletions

diff --git a/payloads/libpayload/arch/arm64/exception.c b/payloads/libpayload/arch/arm64/exception.c
index a84daf1563cb..a5e55163a884 100644
--- a/payloads/libpayload/arch/arm64/exception.c
+++ b/payloads/libpayload/arch/arm64/exception.c
@@ -31,6 +31,8 @@
 #include <libpayload.h>
 #include <stdint.h>
 
+u64 exception_stack[0x200] __attribute__((aligned(16)));
+u64 *exception_stack_end = exception_stack + ARRAY_SIZE(exception_stack);
 extern unsigned int test_exc;
 
 struct exception_handler_info
@@ -39,7 +41,7 @@ struct exception_handler_info
 };
 
 static exception_hook hook;
-struct exception_state *exception_state;
+struct exception_state exception_state;
 
 static struct exception_handler_info exceptions[EXC_COUNT] = {
 	[EXC_SYNC_SP0] = { "_sync_sp_el0" },
@@ -88,14 +90,12 @@ static void print_regs(struct exception_state *state)
 		       i, state->regs[i], i + 1, state->regs[i + 1]);
 	}
 	printf("X30 = 0x%016llx          SP = 0x%016llx\n",
-	       state->regs[30], raw_read_sp_el0());
+	       state->regs[30], state->sp);
 }
 
 void exception_dispatch(struct exception_state *state, int idx);
 void exception_dispatch(struct exception_state *state, int idx)
 {
-	exception_state = state;
-
 	if (idx >= EXC_COUNT) {
 		printf("Bad exception index %d.\n", idx);
 	} else {
@@ -110,7 +110,7 @@ void exception_dispatch(struct exception_state *state, int idx)
 	}
 	print_regs(state);
 	/* Few words below SP in case we need state from a returned function. */
-	dump_stack(raw_read_sp_el0() - 32, 512);
+	dump_stack(state->sp - 32, 512);
 
 	if (test_exc) {
 		state->elr += 4;
@@ -123,8 +123,9 @@ void exception_dispatch(struct exception_state *state, int idx)
 
 void exception_init(void)
 {
-	extern void* exception_table;
-	set_vbar(&exception_table);
+	extern uint64_t exception_table[];
+	raw_write_vbar_el2((uintptr_t)exception_table);
+	exception_set_state_ptr(&exception_state);
 }
 
 void exception_install_hook(exception_hook h)
diff --git a/payloads/libpayload/arch/arm64/exception_asm.S b/payloads/libpayload/arch/arm64/exception_asm.S
index d428940730e7..efa3f9d7e2c4 100644
--- a/payloads/libpayload/arch/arm64/exception_asm.S
+++ b/payloads/libpayload/arch/arm64/exception_asm.S
@@ -27,19 +27,25 @@
  * SUCH DAMAGE.
  */
 
+#include <arch/asm.h>
+#include <arch/exception.h>
+
 	/* Macro for exception entry
 	 * Store x30 before any branch
-	 * Branch to exception_prologue to save rest of the registers
+	 * Branch to exception_prologue to save rest and switch stacks
 	 * Move exception id into x1
-	 * Branch to exception_handler
+	 * Branch to exception_dispatch (exception C entry point)
+	 * Branch to exception_return to return from exception
 	 */
 .macro eentry lbl id
 	.align 7
 \lbl:
-	stp	x30, xzr, [sp, #-16]!
+	/* Note: SP points to exception_state (see exception_set_state_ptr) */
+	str	x30, [sp, #EXCEPTION_STATE_REG(30)]
 	bl	exception_prologue
 	mov	x1, \id
-	bl	exception_handler
+	bl	exception_dispatch
+	b	exception_return
 .endm
 
 	/* Exception table has 16 entries and each of 128 bytes
@@ -68,64 +74,100 @@ eentry	irq_elx_32,#13
 eentry	fiq_elx_32,#14
 eentry	serror_elx_32,#15
 
-exception_prologue:
-	/* Save all registers x0-x29 */
-	stp     x28, x29, [sp, #-16]!
-	stp     x26, x27, [sp, #-16]!
-	stp     x24, x25, [sp, #-16]!
-	stp     x22, x23, [sp, #-16]!
-	stp     x20, x21, [sp, #-16]!
-	stp     x18, x19, [sp, #-16]!
-	stp     x16, x17, [sp, #-16]!
-	stp     x14, x15, [sp, #-16]!
-	stp     x12, x13, [sp, #-16]!
-	stp     x10, x11, [sp, #-16]!
-	stp     x8, x9, [sp, #-16]!
-	stp     x6, x7, [sp, #-16]!
-	stp     x4, x5, [sp, #-16]!
-	stp     x2, x3, [sp, #-16]!
-	stp     x0, x1, [sp, #-16]!
-
-	/* Save the exception reason on stack */
-	mrs	x1, esr_el2
+/* This code must match the layout of struct exception_state (minus x30) */
+ENTRY(exception_prologue)
+	/* Save registers x0-x29 */
+	stp	x28, x29, [sp, #EXCEPTION_STATE_REG(28)]
+	stp	x26, x27, [sp, #EXCEPTION_STATE_REG(26)]
+	stp	x24, x25, [sp, #EXCEPTION_STATE_REG(24)]
+	stp	x22, x23, [sp, #EXCEPTION_STATE_REG(22)]
+	stp	x20, x21, [sp, #EXCEPTION_STATE_REG(20)]
+	stp	x18, x19, [sp, #EXCEPTION_STATE_REG(18)]
+	stp	x16, x17, [sp, #EXCEPTION_STATE_REG(16)]
+	stp	x14, x15, [sp, #EXCEPTION_STATE_REG(14)]
+	stp	x12, x13, [sp, #EXCEPTION_STATE_REG(12)]
+	stp	x10, x11, [sp, #EXCEPTION_STATE_REG(10)]
+	stp	x8, x9, [sp, #EXCEPTION_STATE_REG(8)]
+	stp	x6, x7, [sp, #EXCEPTION_STATE_REG(6)]
+	stp	x4, x5, [sp, #EXCEPTION_STATE_REG(4)]
+	stp	x2, x3, [sp, #EXCEPTION_STATE_REG(2)]
+	stp	x0, x1, [sp, #EXCEPTION_STATE_REG(0)]
+
+	/* Save the stack pointer and SPSR */
+	mrs	x1, sp_el0
+	mrs	x0, spsr_el2
+	stp	x0, x1, [sp, #EXCEPTION_STATE_SPSR]
 
-	/* Save the return address on stack */
+	/* Save return address (ELR) and exception syndrome */
+	mrs	x1, esr_el2
 	mrs	x0, elr_el2
-	stp     x0, x1, [sp, #-16]!
+	stp	x0, x1, [sp, #EXCEPTION_STATE_ELR]
+
+	/* Now switch to the actual exception stack. Keep a pointer to the
+	   exception_state structure in x0 as an argument for dispatch(). */
+	mov	x0, sp
+	adrp	x1, exception_stack_end
+	ldr	x1, [x1, :lo12:exception_stack_end]
+	msr	SPSel, #0
+	mov	sp, x1
 
 	ret
+ENDPROC(exception_prologue)
 
-exception_handler:
-	/* Save address of saved registers into x0
-	 * This acts as first argument to exception_dispatch
-	 */
-	mov     x0, sp
-	bl      exception_dispatch
+ENTRY(exception_return)
+	/* Switch SP back to the exception_state structure */
+	msr	SPSel, #1
 
-	/* Pop return address saved on stack */
-	ldp     x0, x1, [sp], #16
+	/* Restore return address (ELR) -- skip ESR (unneeded for return) */
+	ldr	x0, [sp, #EXCEPTION_STATE_ELR]
 	msr	elr_el2, x0
-	msr	esr_el2, x1
-	/* Pop exception reason saved on stack, followed by regs x0-x30 */
-	ldp     x0, x1, [sp], #16
-	ldp     x2, x3, [sp], #16
-	ldp     x4, x5, [sp], #16
-	ldp     x6, x7, [sp], #16
-	ldp     x8, x9, [sp], #16
-	ldp     x10, x11, [sp], #16
-	ldp     x12, x13, [sp], #16
-	ldp     x14, x15, [sp], #16
-	ldp     x16, x17, [sp], #16
-	ldp     x18, x19, [sp], #16
-	ldp     x20, x21, [sp], #16
-	ldp     x22, x23, [sp], #16
-	ldp     x24, x25, [sp], #16
-	ldp     x26, x27, [sp], #16
-	ldp     x28, x29, [sp], #16
-	ldp     x30, xzr, [sp], #16
+
+	/* Restore stack pointer and SPSR */
+	ldp	x0, x1, [sp, #EXCEPTION_STATE_SPSR]
+	msr	spsr_el2, x0
+	msr	sp_el0, x1
+
+	/* Restore all registers (x0-x30) */
+	ldp	x0, x1, [sp, #EXCEPTION_STATE_REG(0)]
+	ldp	x2, x3, [sp, #EXCEPTION_STATE_REG(2)]
+	ldp	x4, x5, [sp, #EXCEPTION_STATE_REG(4)]
+	ldp	x6, x7, [sp, #EXCEPTION_STATE_REG(6)]
+	ldp	x8, x9, [sp, #EXCEPTION_STATE_REG(8)]
+	ldp	x10, x11, [sp, #EXCEPTION_STATE_REG(10)]
+	ldp	x12, x13, [sp, #EXCEPTION_STATE_REG(12)]
+	ldp	x14, x15, [sp, #EXCEPTION_STATE_REG(14)]
+	ldp	x16, x17, [sp, #EXCEPTION_STATE_REG(16)]
+	ldp	x18, x19, [sp, #EXCEPTION_STATE_REG(18)]
+	ldp	x20, x21, [sp, #EXCEPTION_STATE_REG(20)]
+	ldp	x22, x23, [sp, #EXCEPTION_STATE_REG(22)]
+	ldp	x24, x25, [sp, #EXCEPTION_STATE_REG(24)]
+	ldp	x26, x27, [sp, #EXCEPTION_STATE_REG(26)]
+	ldp	x28, x29, [sp, #EXCEPTION_STATE_REG(28)]
+	ldr	x30, [sp, #EXCEPTION_STATE_REG(30)]
+
+	/* Return from exception */
 	eret
+ENDPROC(exception_return)
 
-	.global set_vbar
-set_vbar:
-	msr	vbar_el2, x0
+	/*
+	 * We have two stack pointers on AArch64: SP_EL0 (which despite the
+	 * naming is used in all ELs) and SP_EL2. We can select which one to
+	 * use by writing to SPSel. Normally we're using SP_EL0, but on
+	 * exception entry it automatically switches to SP_EL2.
+	 *
+	 * It is important for exception reentrancy to switch back to SP_EL0
+	 * while handling the exception. We only need SP_EL2 for the assembly
+	 * exception entry and exit code that stores all register state
+	 * (including the old SP_EL0, before we switch to the real exception
+	 * stack). Rather than having yet another stack to push/pop those
+	 * register values on so that we can later sort them into the
+	 * exception_state structure, it's much easier to just make SP_EL2 point
+	 * directly to exception_state and just use it as a normal base register
+	 * rather than a real stack. This function sets that up.
+	 */
+ENTRY(exception_set_state_ptr)
+	msr	SPSel, #1
+	mov	sp, x0
+	msr	SPSel, #0
 	ret
+ENDPROC(exception_set_state_ptr)
diff --git a/payloads/libpayload/arch/arm64/head.S b/payloads/libpayload/arch/arm64/head.S
index 349dfd43eef7..8bac70fee51d 100644
--- a/payloads/libpayload/arch/arm64/head.S
+++ b/payloads/libpayload/arch/arm64/head.S
@@ -38,18 +38,8 @@ ENTRY(_entry)
 	ldr x1, 1f
 	str x0, [x1]
 
-	/* Setup exception stack */
-	ldr x1, 3f
-	msr SPSel, #1
-	isb
-
-	mov sp, x1
-
 	/* Setup new stack */
 	ldr x1, 2f
-	msr SPSel, #0
-	isb
-
 	mov sp, x1
 
 	/* Let's rock. */
@@ -63,5 +53,3 @@ ENDPROC(_entry)
 .quad	cb_header_ptr
 2:
 .quad	_stack
-3:
-.quad   _exc_stack
diff --git a/payloads/libpayload/arch/arm64/libpayload.ldscript b/payloads/libpayload/arch/arm64/libpayload.ldscript
index 5c807cef171b..ceb07116809b 100644
--- a/payloads/libpayload/arch/arm64/libpayload.ldscript
+++ b/payloads/libpayload/arch/arm64/libpayload.ldscript
@@ -77,15 +77,6 @@ SECTIONS
 		. += CONFIG_LP_STACK_SIZE;
 		. = ALIGN(16);
 		_stack = .;
-
-		/* Exception stack. Having a separate exception stack
-		 * allows us to have later stages running in non-EL3 levels.
-		 */
-
-		_exc_estack = .;
-		. += CONFIG_LP_STACK_SIZE;
-		. = ALIGN(16);
-		_exc_stack = .;
 	}
 
 	_end = .;
diff --git a/payloads/libpayload/include/arm64/arch/exception.h b/payloads/libpayload/include/arm64/arch/exception.h
index 8afe39103b79..f699d3d6ed28 100644
--- a/payloads/libpayload/include/arm64/arch/exception.h
+++ b/payloads/libpayload/include/arm64/arch/exception.h
@@ -30,18 +30,98 @@
 #ifndef _ARCH_EXCEPTION_H
 #define _ARCH_EXCEPTION_H
 
-#include <stdint.h>
+#define EXCEPTION_STATE_ELR	0x0
+#define EXCEPTION_STATE_ESR	0x8
+#define EXCEPTION_STATE_SPSR	0x10
+#define EXCEPTION_STATE_SP	0x18
+#define EXCEPTION_STATE_REG(r)	(0x20 + r * 0x8)
+
+#define ESR_EC_UNKNOWN		0b000000
+#define ESR_EC_SVC_64		0b010101
+#define ESR_EC_INSN_ABT_LOWER	0b100000
+#define ESR_EC_INSN_ABT_SAME	0b100001
+#define ESR_EC_DATA_ABT_LOWER	0b100100
+#define ESR_EC_DATA_ABT_SAME	0b100101
+#define ESR_EC_SERROR		0b101111
+#define ESR_EC_SS_SAME		0b110011
+#define ESR_EC_BKPT_64		0b111100
+
+#define MDCR_TDE		(1 << 8)
 
-void set_vbar(void* vbar);
+#define MDSCR_SS		(1 << 0)
+#define MDSCR_KDE		(1 << 13)
+#define MDSCR_MDE		(1 << 15)
+
+#ifndef __ASSEMBLER__
+
+#include <stddef.h>
+#include <stdint.h>
 
 struct exception_state
 {
 	uint64_t elr;
-	uint64_t esr;
+	union {
+		uint64_t esr;
+		union {
+			struct {
+				uint64_t iss	: 25;
+				uint64_t il	: 1;
+				uint64_t ec	: 6;
+				uint64_t _res0	: 32;
+			};
+			struct {
+				uint64_t isfc	: 6;
+				uint64_t _res0	: 1;
+				uint64_t s1ptw	: 1;
+				uint64_t _res1	: 1;
+				uint64_t ea	: 1;
+				uint64_t fnv	: 1;
+				uint64_t _res2	: 53;
+			} insn_abt;
+		};
+	};
+	union {
+		uint32_t spsr;
+		struct {
+			uint32_t sp	: 1;	/* M[0] */
+			uint32_t _res0	: 1;	/* M[1] */
+			uint32_t el	: 2;	/* M[3:2] */
+			uint32_t arch	: 1;	/* M[4] */
+			uint32_t _res1	: 1;
+			uint32_t f	: 1;
+			uint32_t i	: 1;
+			uint32_t a	: 1;
+			uint32_t d	: 1;
+			uint32_t _res2	: 10;
+			uint32_t il	: 1;
+			uint32_t ss	: 1;
+			uint32_t _res3	: 6;
+			uint32_t v	: 1;
+			uint32_t c	: 1;
+			uint32_t z	: 1;
+			uint32_t n	: 1;
+		} pstate;
+	};
+	uint32_t spsr_high_unused;
+	uint64_t sp;
 	uint64_t regs[31];
 } __packed;
 
-extern struct exception_state *exception_state;
+#define CHECK_ES(field, constant) \
+	_Static_assert(offsetof(struct exception_state, field) == constant, \
+	"(struct exception_state)." #field " doesn't match constant " #constant)
+CHECK_ES(elr, EXCEPTION_STATE_ELR);
+CHECK_ES(esr, EXCEPTION_STATE_ESR);
+CHECK_ES(spsr, EXCEPTION_STATE_SPSR);
+CHECK_ES(sp, EXCEPTION_STATE_SP);
+CHECK_ES(regs[0], EXCEPTION_STATE_REG(0));
+CHECK_ES(regs[30], EXCEPTION_STATE_REG(30));
+
+extern struct exception_state exception_state;
+extern u64 exception_stack[];
+extern u64 *exception_stack_end;
+
+void exception_set_state_ptr(struct exception_state *exception_state_ptr);
 
 enum {
 	EXC_SYNC_SP0 = 0,
@@ -63,4 +143,6 @@ enum {
 	EXC_COUNT
 };
 
+#endif	/* !__ASSEMBLER__ */
+
 #endif