aarch64: Implement basic exception handling

This installs a exception vector in bootblock, aborts are now
handled with a register file dump to console for debugging.

BUG=None
BRANCH=none
TEST=Ran image in foundation model, tested some exceptions
Signed-off-by: Marcelo Povoa <marcelogp@chromium.org>

Change-Id: I4cb4d95fc5eb905f8c3c623315a46a00fdbf2677
Reviewed-on: https://chromium-review.googlesource.com/185755
Reviewed-by: David Hendricks <dhendrix@chromium.org>
Tested-by: Marcelo Póvoa <marcelogp@chromium.org>
Commit-Queue: Marcelo Póvoa <marcelogp@chromium.org>

src/arch/aarch64/Makefile.inc

@@ -123,7 +123,7 @@ else
 	$(CC) $(CFLAGS) -nostdlib -r -o $@ -Wl,--start-group $(stages_o) $(ramstage-objs) $(LIBGCC_FILE_NAME) -Wl,--end-group
 endif
 
-CFLAGS += \
+CFLAGS += -mstrict-align \
 
 ldscripts =
 ldscripts += $(src)/arch/aarch64/romstage.ld
@@ -139,15 +139,19 @@ $(obj)/mainboard/$(MAINBOARDDIR)/romstage.pre.inc: $(src)/mainboard/$(MAINBOARDD
 
 # Things that appear in every board
 ramstage-y += exception.c
-#ramstage-y += exception_asm.S
+ramstage-y += exception_asm.S
 
 bootblock-$(CONFIG_BOOTBLOCK_CONSOLE) += early_console.c
 bootblock-y += cache.c
 bootblock-y += cpu.S
+bootblock-$(CONFIG_BOOTBLOCK_CONSOLE) += exception.c
+bootblock-$(CONFIG_BOOTBLOCK_CONSOLE) += exception_asm.S
 
 romstage-y += cache.c
 romstage-y += cpu.S
 #romstage-y += div0.c
+romstage-y += exception.c
+romstage-y += exception_asm.S
 romstage-$(CONFIG_EARLY_CONSOLE) += early_console.c
 
 bootblock-y += stages.c

src/arch/aarch64/bootblock_simple.c

@@ -23,6 +23,7 @@
 #include <arch/cache.h>
 #include <arch/hlt.h>
 #include <arch/stages.h>
+#include <arch/exception.h>
 #include <cbfs.h>
 #include <console/console.h>
 
@@ -44,6 +45,7 @@ void main(void)
 	/* Globally disable MMU, caches, and branch prediction (these should
 	 * be disabled by default on reset) */
 	dcache_mmu_disable();
+	write_sctlr(read_sctlr() & ~(SCTLR_A));
 
 	/*
 	 * Re-enable icache and branch prediction. MMU and dcache will be
@@ -60,7 +62,7 @@ void main(void)
 
 #ifdef CONFIG_BOOTBLOCK_CONSOLE
 	console_init();
-	//exception_init();
+	exception_init();
 #endif
 
 	entry = cbfs_load_stage(CBFS_DEFAULT_MEDIA, stage_name);

src/arch/aarch64/exception.c

@@ -33,98 +33,93 @@
 #include <arch/exception.h>
 #include <console/console.h>
 
-uint8_t exception_stack[0x100] __attribute__((aligned(8)));
+void exception_sync_el0(uint64_t *regs, uint64_t esr);
-extern void *exception_stack_end;
+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);
 
-void exception_undefined_instruction(uint32_t *);
+static void print_regs(uint64_t *regs)
-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 *);
-
-static void print_regs(uint32_t *regs)
 {
 	int i;
 
-	for (i = 0; i < 16; i++) {
+	for (i = 0; i < 31; i+=2) {
-		if (i == 15)
+		printk(BIOS_ERR, "X%02d = 0x%016llx        ", i, regs[i]);
-			printk(BIOS_ERR, "PC");
+		printk(BIOS_ERR, "X%02d = 0x%016llx\n", i + 1, regs[i + 1]);
-		else if (i == 14)
-			printk(BIOS_ERR, "LR");
-		else if (i == 13)
-			printk(BIOS_ERR, "SP");
-		else if (i == 12)
-			printk(BIOS_ERR, "IP");
-		else
-			printk(BIOS_ERR, "R%d", i);
-		printk(BIOS_ERR, " = 0x%08x\n", regs[i]);
 	}
 }
 
-void exception_undefined_instruction(uint32_t *regs)
+void exception_sync_el0(uint64_t *regs, uint64_t esr)
 {
-	printk(BIOS_ERR, "exception _undefined_instruction\n");
+	printk(BIOS_ERR, "exception _sync_el0 (ESR = 0x%08llx)\n", esr);
 	print_regs(regs);
 	die("exception");
 }
 
-void exception_software_interrupt(uint32_t *regs)
+void exception_irq_el0(uint64_t *regs, uint64_t esr)
 {
-	printk(BIOS_ERR, "exception _software_interrupt\n");
+	printk(BIOS_ERR, "exception _irq_el0 (ESR = 0x%08llx)\n", esr);
 	print_regs(regs);
 	die("exception");
 }
 
-void exception_prefetch_abort(uint32_t *regs)
+void exception_fiq_el0(uint64_t *regs, uint64_t esr)
 {
-	printk(BIOS_ERR, "exception _prefetch_abort\n");
+	printk(BIOS_ERR, "exception _fiq_el0 (ESR = 0x%08llx)\n", esr);
 	print_regs(regs);
 	die("exception");
 }
 
-void exception_data_abort(uint32_t *regs)
+void exception_serror_el0(uint64_t *regs, uint64_t esr)
 {
-	printk(BIOS_ERR, "exception _data_abort\n");
+	printk(BIOS_ERR, "exception _serror_el0 (ESR = 0x%08llx)\n", esr);
 	print_regs(regs);
 	die("exception");
 }
 
-void exception_not_used(uint32_t *regs)
+void exception_sync(uint64_t *regs, uint64_t esr)
 {
-	printk(BIOS_ERR, "exception _not_used\n");
+	printk(BIOS_ERR, "exception _sync (ESR = 0x%08llx)\n", esr);
 	print_regs(regs);
 	die("exception");
 }
 
-void exception_irq(uint32_t *regs)
+void exception_irq(uint64_t *regs, uint64_t esr)
 {
-	printk(BIOS_ERR, "exception _irq\n");
+	printk(BIOS_ERR, "exception _irq (ESR = 0x%08llx)\n", esr);
 	print_regs(regs);
 	die("exception");
 }
 
-void exception_fiq(uint32_t *regs)
+void exception_fiq(uint64_t *regs, uint64_t esr)
 {
-	printk(BIOS_ERR, "exception _fiq\n");
+	printk(BIOS_ERR, "exception _fiq (ESR = 0x%08llx)\n", esr);
+	print_regs(regs);
+	die("exception");
+}
+
+void exception_serror(uint64_t *regs, uint64_t esr)
+{
+	printk(BIOS_ERR, "exception _serror (ESR = 0x%08llx)\n", esr);
 	print_regs(regs);
 	die("exception");
 }
 
 void exception_init(void)
 {
-	uint32_t sctlr = read_sctlr();
+	//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);
+	//write_sctlr(sctlr);
 
-	//extern uint32_t exception_table[];
+	extern uint32_t exception_table[];
-	//set_vbar((uintptr_t)exception_table);
+	set_vbar((uintptr_t)exception_table);
-	//exception_stack_end = exception_stack + sizeof(exception_stack);
 
-	printk(BIOS_DEBUG, "Exception handlers installed.\n");
+	printk(0, "Exception handlers installed.\n");
 }

src/arch/aarch64/exception_asm.S

@@ -0,0 +1,103 @@
+/*
+ * This file is part of the libpayload project.
+ *
+ * Copyright 2014 Google Inc.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. The name of the author may not be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE AUTHOR AND CONTRIBUTORS ``AS IS'' AND
+ * ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED.  IN NO EVENT SHALL THE AUTHOR OR CONTRIBUTORS BE LIABLE
+ * FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
+ * DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS
+ * OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION)
+ * HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY
+ * OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF
+ * SUCH DAMAGE.
+ */
+
+#include <arch/asm.h>
+
+	.text
+
+	.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]!
+
+	/* FIXME: Don't assume always running in EL3 */
+	mrs	x1, elr_el3
+	stp	x1, x0, [sp, #-16]!
+
+	mrs	x1, esr_el3
+	mov	x0, sp
+
+	ret
+ENDPROC(exception_prologue)
+
+	.global exception_stack_end
+exception_stack_end:
+	.quad 0
+
+exception_handler:
+	.word 0
+
+	.global set_vbar
+set_vbar:
+	msr	vbar_el3, x0
+	ret

src/arch/aarch64/stages.c

@@ -35,6 +35,7 @@
 
 #include <arch/stages.h>
 #include <arch/cache.h>
+#include <console/console.h>
 
 void stage_entry(void)
 {

src/mainboard/emulation/foundation-armv8/romstage.c

@@ -19,11 +19,11 @@
 
 void main(void)
 {
-        void *entry;
+	void *entry;
 
 	console_init();
 
 	entry = cbfs_load_stage(CBFS_DEFAULT_MEDIA, "fallback/coreboot_ram");
 
-	stage_exit(entry);
+	//stage_exit(entry);
 }