arm64: Make exceptions work

BUG=chrome-os-partner:31515
BRANCH=None
TEST=test_exception generates a page fault which is handled by the exception
handler and execution continues after eret from the exception

Change-Id: Ie550492d2ed21b2c3009b5627f1e1a37429e6af0
Signed-off-by: Patrick Georgi <pgeorgi@chromium.org>
Original-Commit-Id: e29fe77745d10e840c02498e54a0c53835530e5e
Original-Change-Id: I29b7dabaece9b11a04ee3628d83513d30eb07b1d
Original-Signed-off-by: Furquan Shaikh <furquan@google.com>
Original-Reviewed-on: https://chromium-review.googlesource.com/213661
Original-Tested-by: Furquan Shaikh <furquan@chromium.org>
Original-Reviewed-by: Aaron Durbin <adurbin@chromium.org>
Original-Commit-Queue: Furquan Shaikh <furquan@chromium.org>
Reviewed-on: http://review.coreboot.org/9000
Tested-by: build bot (Jenkins)
Reviewed-by: Stefan Reinauer <stefan.reinauer@coreboot.org>

3 files changed, 175 insertions, 132 deletions

diff --git a/src/arch/arm64/armv8/exception.c b/src/arch/arm64/armv8/exception.c
index 31e31311a7..4b4d7b7185 100644
--- a/src/arch/arm64/armv8/exception.c
+++ b/src/arch/arm64/armv8/exception.c
@@ -32,98 +32,110 @@
 #include <arch/cache.h>
 #include <arch/exception.h>
 #include <console/console.h>
+#include <arch/lib_helpers.h>
 
-void exception_sync_el0(uint64_t *regs, uint64_t esr);
-void exception_irq_el0(uint64_t *regs, uint64_t esr);
-void exception_fiq_el0(uint64_t *regs, uint64_t esr);
-void exception_serror_el0(uint64_t *regs, uint64_t esr);
-void exception_sync(uint64_t *regs, uint64_t esr);
-void exception_irq(uint64_t *regs, uint64_t esr);
-void exception_fiq(uint64_t *regs, uint64_t esr);
-void exception_serror(uint64_t *regs, uint64_t esr);
+static unsigned int test_exc;
 
-static void print_regs(uint64_t *regs)
+struct exception_handler_info
+{
+	const char *name;
+};
+
+enum {
+	EXC_SYNC_SP0 = 0,
+	EXC_IRQ_SP0,
+	EXC_FIQ_SP0,
+	EXC_SERROR_SP0,
+	EXC_SYNC_SP3,
+	EXC_IRQ_SP3,
+	EXC_FIQ_SP3,
+	EXC_SERROR_SP3,
+	EXC_SYNC_ELX_64,
+	EXC_IRQ_ELX_64,
+	EXC_FIQ_ELX_64,
+	EXC_SERROR_ELX_64,
+	EXC_SYNC_ELX_32,
+	EXC_IRQ_ELX_32,
+	EXC_FIQ_ELX_32,
+	EXC_SERROR_ELX_32,
+	EXC_COUNT
+};
+
+static struct exception_handler_info exceptions[EXC_COUNT] = {
+	[EXC_SYNC_SP0] = { "_sync_sp_el0" },
+	[EXC_IRQ_SP0]  = { "_irq_sp_el0" },
+	[EXC_FIQ_SP0]  = { "_fiq_sp_el0" },
+	[EXC_SERROR_SP0] = {"_serror_sp_el0"},
+	[EXC_SYNC_SP3] = { "_sync_sp_el3" },
+	[EXC_IRQ_SP3]  = { "_irq_sp_el3" },
+	[EXC_FIQ_SP3]  = { "_fiq_sp_el3" },
+	[EXC_SERROR_SP3] = {"_serror_sp_el3"},
+	[EXC_SYNC_ELX_64] = { "_sync_elx_64" },
+	[EXC_IRQ_ELX_64]  = { "_irq_elx_64" },
+	[EXC_FIQ_ELX_64]  = { "_fiq_elx_64" },
+	[EXC_SERROR_ELX_64] = {"_serror_elx_64"},
+	[EXC_SYNC_ELX_32] = { "_sync_elx_32" },
+	[EXC_IRQ_ELX_32]  = { "_irq_elx_32" },
+	[EXC_FIQ_ELX_32]  = { "_fiq_elx_32" },
+	[EXC_SERROR_ELX_32] = {"_serror_elx_32"},
+};
+
+static void print_regs(struct exception_state *state)
 {
 	int i;
+	uint64_t far_el3;
 
-	/* ELR contains the restart PC at target exception level */
-	printk(BIOS_ERR, "ELR = 0x%016llx        ", regs[0]);
-	printk(BIOS_ERR, "X00 = 0x%016llx\n", regs[1]);
+	far_el3 = raw_read_far_el3();
 
-	for (i = 2; i < 31; i+=2) {
-		printk(BIOS_ERR, "X%02d = 0x%016llx        ", i - 1, regs[i]);
-		printk(BIOS_ERR, "X%02d = 0x%016llx\n", i, regs[i + 1]);
-	}
+	printk(BIOS_DEBUG, "ELR = 0x%016llx\n", state->elr);
+	printk(BIOS_DEBUG, "ESR = 0x%08llx\n", state->esr);
+	printk(BIOS_DEBUG, "FAR_EL3 = 0x%016llx\n", far_el3);
+	for (i = 0; i < 31; i++)
+		printk(BIOS_DEBUG, "X%02d = 0x%016llx\n", i, state->regs[i]);
 }
 
-void exception_sync_el0(uint64_t *regs, uint64_t esr)
+void exception_dispatch(struct exception_state *state, int idx)
 {
-	printk(BIOS_ERR, "exception _sync_el0 (ESR = 0x%08llx)\n", esr);
-	print_regs(regs);
-	die("exception");
+	if (idx >= EXC_COUNT) {
+		printk(BIOS_DEBUG, "Bad exception index %d.\n", idx);
+	} else {
+		struct exception_handler_info *info = &exceptions[idx];
+
+		if (info->name)
+			printk(BIOS_DEBUG, "exception %s\n", info->name);
+		else
+			printk(BIOS_DEBUG, "exception _not_used.\n");
+	}
+	print_regs(state);
+
+	if (test_exc) {
+		state->elr += 4;
+		test_exc = 0;
+		printk(BIOS_DEBUG, "new ELR = 0x%016llx\n", state->elr);
+	} else
+		die("exception");
 }
 
-void exception_irq_el0(uint64_t *regs, uint64_t esr)
+static uint64_t test_exception(void)
 {
-	printk(BIOS_ERR, "exception _irq_el0 (ESR = 0x%08llx)\n", esr);
-	print_regs(regs);
-	die("exception");
-}
+	uint64_t *a = (uint64_t *)0xfffffffff0000000ULL;
 
-void exception_fiq_el0(uint64_t *regs, uint64_t esr)
-{
-	printk(BIOS_ERR, "exception _fiq_el0 (ESR = 0x%08llx)\n", esr);
-	print_regs(regs);
-	die("exception");
-}
+	test_exc = 1;
 
-void exception_serror_el0(uint64_t *regs, uint64_t esr)
-{
-	printk(BIOS_ERR, "exception _serror_el0 (ESR = 0x%08llx)\n", esr);
-	print_regs(regs);
-	die("exception");
-}
+	printk(BIOS_DEBUG, "%llx\n", *a);
 
-void exception_sync(uint64_t *regs, uint64_t esr)
-{
-	printk(BIOS_ERR, "exception _sync (ESR = 0x%08llx)\n", esr);
-	print_regs(regs);
-	die("exception");
+	return 0;
 }
 
-void exception_irq(uint64_t *regs, uint64_t esr)
+void exception_init(void)
 {
-	printk(BIOS_ERR, "exception _irq (ESR = 0x%08llx)\n", esr);
-	print_regs(regs);
-	die("exception");
-}
+	extern void *exception_table;
 
-void exception_fiq(uint64_t *regs, uint64_t esr)
-{
-	printk(BIOS_ERR, "exception _fiq (ESR = 0x%08llx)\n", esr);
-	print_regs(regs);
-	die("exception");
-}
+	set_vbar(&exception_table);
 
-void exception_serror(uint64_t *regs, uint64_t esr)
-{
-	printk(BIOS_ERR, "exception _serror (ESR = 0x%08llx)\n", esr);
-	print_regs(regs);
-	die("exception");
-}
+	printk(BIOS_DEBUG, "ARM64: Exception handlers installed.\n");
 
-void exception_init(void)
-{
-	//uint32_t sctlr = read_sctlr();
-	/* Handle exceptions in ARM mode. */
-	//sctlr &= ~SCTLR_TE;
-	/* Set V=0 in SCTLR so VBAR points to the exception vector table. */
-	//sctlr &= ~SCTLR_V;
-	/* Enforce alignment temporarily. */
-	//write_sctlr(sctlr);
-
-	extern uint32_t exception_table[];
-	set_vbar((uintptr_t)exception_table);
-
-	printk(BIOS_DEBUG, "Exception handlers installed.\n");
+	printk(BIOS_DEBUG, "ARM64: Testing exception\n");
+	test_exception();
+	printk(BIOS_DEBUG, "ARM64: Done test exception\n");
 }
diff --git a/src/arch/arm64/armv8/exception_asm.S b/src/arch/arm64/armv8/exception_asm.S
index b1f1a94174..66377235cd 100644
--- a/src/arch/arm64/armv8/exception_asm.S
+++ b/src/arch/arm64/armv8/exception_asm.S
@@ -29,75 +29,98 @@
 
 #include <arch/asm.h>
 
-	.text
+.macro eentry lbl id
+	.align 7
+\lbl:
+	stp	x30, xzr, [sp, #-16]!
+	bl	exception_prologue
+	mov	x1, \id
+	bl	exception_handler
+.endm
 
-	.align 11
+	.align	11
 	.global exception_table
+
 exception_table:
-	.align 7
-	bl	exception_prologue
-	bl	exception_sync_el0
-	.align 7
-	bl	exception_prologue
-	bl	exception_irq_el0
-	.align 7
-	bl	exception_prologue
-	bl	exception_fiq_el0
-	.align 7
-	bl	exception_prologue
-	bl	exception_serror_el0
-	.align 7
-	bl	exception_prologue
-	bl	exception_sync
-	.align 7
-	bl	exception_prologue
-	bl	exception_irq
-	.align 7
-	bl	exception_prologue
-	bl	exception_fiq
-	.align 7
-	bl	exception_prologue
-	bl	exception_serror
 
-/*
- * Save state (register file + ELR) to stack
- * and set arguments x0 and x1 for exception call
- */
-ENTRY(exception_prologue)
-	stp	x29, x30, [sp, #-16]!
-	stp	x27, x28, [sp, #-16]!
-	stp	x25, x26, [sp, #-16]!
-	stp	x23, x24, [sp, #-16]!
-	stp	x21, x22, [sp, #-16]!
-	stp	x19, x20, [sp, #-16]!
-	stp	x17, x18, [sp, #-16]!
-	stp	x15, x16, [sp, #-16]!
-	stp	x13, x14, [sp, #-16]!
-	stp	x11, x12, [sp, #-16]!
-	stp	x9, x10, [sp, #-16]!
-	stp	x7, x8, [sp, #-16]!
-	stp	x5, x6, [sp, #-16]!
-	stp	x3, x4, [sp, #-16]!
-	stp	x1, x2, [sp, #-16]!
+eentry	sync_el3_sp0,#0
+eentry	irq_el3_sp0,#1
+eentry	fiq_el3_sp0,#2
+eentry	serror_el3_sp0,#3
+eentry	sync_el3_sp3,#4
+eentry	irq_el3_sp3,#5
+eentry	fiq_el3_sp3,#6
+eentry	serror_el3_sp3,#7
+eentry	sync_elx_64,#8
+eentry	irq_elx_64,#9
+eentry	fiq_elx_64,#10
+eentry	serror_elx_64,#11
+eentry	sync_elx_32,#12
+eentry	irq_elx_32,#13
+eentry	fiq_elx_32,#14
+eentry	serror_elx_32,#15
 
-	/* FIXME: Don't assume always running in EL2 */
-	mrs	x1, elr_el2
-	stp	x1, x0, [sp, #-16]!
+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]!
 
-	mrs	x1, esr_el2
-	mov	x0, sp
+	/* Save the exception reason on stack */
+	mrs	x1,  esr_el3
 
-	ret
-ENDPROC(exception_prologue)
+	/* Save the return address on stack */
+	mrs	x0, elr_el3
+	stp	x0, x1, [sp, #-16]!
 
-	.global exception_stack_end
-exception_stack_end:
-	.quad 0
+	ret
 
 exception_handler:
-	.word 0
+	/* Save address of saved registers into x0
+	 * This acts as first argument to exception_dispatch
+	 */
+	mov	x0, sp
+	bl	exception_dispatch
+
+	/* Pop return address and exception reason saved on stack */
+	ldp	x0, x1, [sp], #16
+	msr	elr_el3, x0
+	msr	esr_el3, x1
+	/* Pop registers 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
+	eret
 
 	.global set_vbar
 set_vbar:
-	msr	vbar_el2, x0
+	/* Initialize the exception table address in vbar for EL3 */
+	msr	vbar_el3, x0
+	dsb	sy
+	isb
 	ret
diff --git a/src/arch/arm64/include/armv8/arch/exception.h b/src/arch/arm64/include/armv8/arch/exception.h
index 5987d85af4..b33e20fa1b 100644
--- a/src/arch/arm64/include/armv8/arch/exception.h
+++ b/src/arch/arm64/include/armv8/arch/exception.h
@@ -32,7 +32,15 @@
 
 #include <stdint.h>
 
+struct exception_state
+{
+	uint64_t elr;
+	uint64_t esr;
+	uint64_t regs[31];
+} __attribute__((packed));
+
 void exception_init(void);
-void set_vbar(uint64_t vbar);
+void set_vbar(void *vbar);
+void exception_dispatch(struct exception_state *state, int idx);
 
 #endif