[stage1] Add exception handlers... which somehow don't work?

fusee/fusee-primary/src/exception_handlers.c

@@ -0,0 +1,60 @@
+#include "exception_handlers.h"
+#include "lib/driver_utils.h"
+#include "utils.h"
+#include "display/video_fb.h"
+
+#define CODE_DUMP_SIZE      0x30
+#define STACK_DUMP_SIZE     0x60
+
+extern const uint32_t exception_handler_table[];
+
+static const char *exception_names[] = {
+    "Reset", "Undefined instruction", "SWI", "Prefetch abort", "Data abort", "Reserved", "IRQ", "FIQ",
+};
+
+static const char *register_names[] = {
+    "R0", "R1", "R2", "R3", "R4", "R5", "R6", "R7", "R8", "R9", "R10", "R11", "R12",
+    "SP", "LR", "PC", "CPSR",
+};
+
+void setup_exception_handlers(void) {
+    volatile uint32_t *bpmp_exception_handler_table = (volatile uint32_t *)0x6000F200;
+    for (int i = 0; i < 8; i++) {
+        if (exception_handler_table[i] != 0) {
+            bpmp_exception_handler_table[i] = exception_handler_table[i];
+        }
+    } 
+}
+
+void exception_handler_main(uint32_t *registers, unsigned int exception_type) {
+    uint8_t code_dump[CODE_DUMP_SIZE];
+    uint8_t stack_dump[STACK_DUMP_SIZE];
+    size_t  code_dump_size;
+    size_t  stack_dump_size;
+
+    uint32_t pc = registers[15];
+    uint32_t cpsr = registers[16];
+
+    uint32_t instr_addr = pc + ((cpsr & 0x20) ? 2 : 4) - CODE_DUMP_SIZE;
+
+    code_dump_size =0;// safecpy(code_dump, (const void *)instr_addr, CODE_DUMP_SIZE);
+    stack_dump_size = safecpy(stack_dump, (const void *)registers[14], STACK_DUMP_SIZE);
+
+    printk("Something went wrong...\n");
+    printk("\nException type: %s\n", exception_names[exception_type]);
+    printk("\nRegisters:\n\n");
+
+    /* Print r0 to pc. */
+    for (int i = 0; i < 16; i += 2) {
+        printk("%-7s%08X       %-7s%08X\n", register_names[i], registers[i], register_names[i+1], registers[i+1]);
+    }
+
+    /* Print cpsr. */
+    printk("%-7s%08X\n", register_names[16], registers[16]);
+
+    printk("\nCode dump:\n");
+    hexdump(code_dump, code_dump_size, instr_addr);
+    printk("\nStack dump:\n");
+    hexdump(stack_dump, stack_dump_size, registers[14]);
+    fatal_error("An exception occured!\n");
+}

fusee/fusee-primary/src/exception_handlers.h

@@ -0,0 +1,12 @@
+#ifndef FUSEE_EXCEPTION_HANDLERS_H
+#define FUSEE_EXCEPTION_HANDLERS_H
+
+#include <stdint.h>
+#include <stddef.h>
+
+/* Copies up to len bytes, stops and returns the read length on data fault. */
+size_t safecpy(void *dst, const void *src, size_t len);
+
+void setup_exception_handlers(void);
+
+#endif

fusee/fusee-primary/src/exception_handlers_asm.s

@@ -0,0 +1,110 @@
+.macro GEN_USUAL_HANDLER name, index, lr_arm_displ, lr_thumb_displ
+    _exception_handler_\name:
+        ldr     sp, =_regs
+        stmia   sp!, {r0-r7}
+
+        /* Adjust lr to make it point to the location where the exception occured. */
+        mrs     r1, spsr
+        tst     r1, #0x20
+        subeq   lr, lr, #\lr_arm_displ
+        subne   lr, lr, #\lr_thumb_displ
+
+        mov     r0, sp
+        mov     r1, #\index
+        b       _exception_handler_common
+.endm
+
+.section    .text.exception_handlers_asm, "ax", %progbits
+.arm
+.align      4
+
+_exception_handler_common:
+    mrs     r2, spsr
+    mrs     r3, cpsr
+
+    /* Mask interrupts. */
+    orr     r3, #0xC0
+    msr     cpsr_cx, r3
+
+    /* Switch to the mode that triggered the interrupt, get the remaining regs, switch back. */
+    ands    r4, r2, #0xF
+    moveq   r4, #0xF             /* usr => sys */
+    bic     r5, r3, #0xF
+    orr     r5, r4
+    msr     cpsr_c, r5
+    stmia   r0!, {r8-lr}
+    msr     cpsr_c, r3
+
+    str     lr, [r0], #4
+    str     r2, [r0]
+
+    /* Finally, switch to system mode, setting interrupts and clearing the flags; set sp as well. */
+    msr     cpsr_cxsf, #0xDF
+    ldr     sp, =(_exception_handler_stack + 0x1000)
+    ldr     r0, =_regs
+    bl      exception_handler_main
+    b       .
+
+GEN_USUAL_HANDLER undefined_instruction, 1, 4, 2
+GEN_USUAL_HANDLER swi, 2, 4, 2
+GEN_USUAL_HANDLER prefetch_abort, 3, 4, 4
+GEN_USUAL_HANDLER data_abort_normal, 4, 8, 8
+GEN_USUAL_HANDLER fiq, 7, 4, 4
+
+_exception_handler_data_abort:
+    /* Mask interrupts (abort mode). */
+    msr     cpsr_cx, #0xD7
+
+    adr     sp, safecpy
+    add     sp, sp, #8
+    cmp     lr, sp
+    blo     _exception_handler_data_abort_normal
+    adr     sp, _safecpy_end
+    add     sp, sp, #8
+    cmp     lr, sp
+    bhs     _exception_handler_data_abort_normal
+
+    /* Set the flags, set r12 to 0 for safecpy, return from exception. */
+    msr     spsr_f, #(1 << 30)
+    mov     r12, #0
+    subs    pc, lr, #4
+
+.global safecpy
+.type   safecpy, %function
+safecpy:
+    push    {r4, lr}
+    mov     r3, #0
+    movs    r12, #1
+
+    _safecpy_loop:
+        ldrb    r4, [r1, r3]
+        cmp     r12, #0
+        beq     _safecpy_loop_end
+        strb    r4, [r0, r3]
+        add     r3, #1
+        cmp     r3, r2
+        blo     _safecpy_loop
+
+    _safecpy_loop_end:
+    mov     r0, r3
+    pop     {r4, pc}
+
+_safecpy_end:
+
+.section    .rodata.exception_handlers_asm, "a", %progbits
+.align      2
+.global     exception_handler_table
+exception_handler_table:
+    .word   0   /* Reset */
+    .word   _exception_handler_undefined_instruction
+    .word   _exception_handler_swi
+    .word   _exception_handler_prefetch_abort
+    .word   _exception_handler_data_abort
+    .word   0   /* Reserved */
+    .word   0   /* IRQ */
+    .word   _exception_handler_fiq
+
+.section    .bss.exception_handlers_asm, "w", %nobits
+.align      4
+_exception_handler_stack: .skip 0x1000
+_regs: .skip (4 * 17)

fusee/fusee-primary/src/main.c

@@ -1,4 +1,5 @@
 #include "utils.h"
+#include "exception_handlers.h"
 #include "panic.h"
 #include "hwinit.h"
 #include "fuse.h"
@@ -85,6 +86,9 @@ static void setup_env(void) {
     /* Turn on the backlight after initializing the lfb */
     /* to avoid flickering. */
     display_enable_backlight(true);
+
+    /* Set up the exception handlers. */
+    setup_exception_handlers();
 }
 
 static void cleanup_env(void) {

fusee/fusee-primary/src/utils.c

@@ -11,6 +11,8 @@
 #include "lib/printk.h"
 #include "hwinit/btn.h"
 
+#include <inttypes.h>
+
 __attribute__((noreturn)) void watchdog_reboot(void) {
     volatile watchdog_timers_t *wdt = GET_WDT(4);
     wdt->PATTERN = WDT_REBOOT_PATTERN;
@@ -75,3 +77,37 @@ __attribute__((noinline)) bool overlaps(uint64_t as, uint64_t ae, uint64_t bs, u
         return true;
     return false;
 }
+
+/* Adapted from https://gist.github.com/ccbrown/9722406 */
+void hexdump(const void* data, size_t size, uintptr_t addrbase) {
+    const uint8_t *d = (const uint8_t *)data;
+    char ascii[17];
+    ascii[16] = '\0';
+
+    for (size_t i = 0; i < size; i++) {
+        if (i % 16 == 0) {
+            printk("%0*" PRIXPTR ": | ", 2 * sizeof(addrbase), addrbase + i);
+        }
+        printk("%02X ", d[i]);
+        if (d[i] >= ' ' && d[i] <= '~') {
+            ascii[i % 16] = d[i];
+        } else {
+            ascii[i % 16] = '.';
+        }
+        if ((i+1) % 8 == 0 || i+1 == size) {
+            printk(" ");
+            if ((i+1) % 16 == 0) {
+                printk("|  %s \n", ascii);
+            } else if (i+1 == size) {
+                ascii[(i+1) % 16] = '\0';
+                if ((i+1) % 16 <= 8) {
+                    printk(" ");
+                }
+                for (size_t j = (i+1) % 16; j < 16; j++) {
+                    printk("   ");
+                }
+                printk("|  %s \n", ascii);
+            }
+        }
+    }
+}

fusee/fusee-primary/src/utils.h

@@ -101,6 +101,8 @@ static inline bool check_32bit_address_range_in_program(uintptr_t addr, size_t s
     overlaps_a(start, end, __start__, __end__);
 }
 
+void hexdump(const void* data, size_t size, uintptr_t addrbase);
+
 __attribute__((noreturn)) void watchdog_reboot(void);
 __attribute__((noreturn)) void pmc_reboot(uint32_t scratch0);
 __attribute__((noreturn)) void car_reboot(void);