Stage2: Add exception handlers

fusee/fusee-primary/src/exception_handlers.c

@@ -1,3 +1,5 @@
+#include <inttypes.h>
+
 #include "exception_handlers.h"
 #include "lib/driver_utils.h"
 #include "utils.h"
@@ -37,7 +39,7 @@ void exception_handler_main(uint32_t *registers, unsigned int exception_type) {
 
     uint32_t instr_addr = pc + ((cpsr & 0x20) ? 2 : 4) - CODE_DUMP_SIZE;
 
-    printk("Something went wrong...\n");
+    printk("\nSomething went wrong...\n");
 
     code_dump_size = safecpy(code_dump, (const void *)instr_addr, CODE_DUMP_SIZE);
     stack_dump_size = safecpy(stack_dump, (const void *)registers[13], STACK_DUMP_SIZE);
@@ -47,11 +49,11 @@ void exception_handler_main(uint32_t *registers, unsigned int exception_type) {
 
     /* 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]);
+        printk("%-7s%08"PRIX32"       %-7s%08"PRIX32"\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("%-7s%08"PRIX32"\n", register_names[16], registers[16]);
 
     printk("\nCode dump:\n");
     hexdump(code_dump, code_dump_size, instr_addr);

fusee/fusee-secondary/src/exception_handlers.c

@@ -0,0 +1,64 @@
+#include <inttypes.h>
+
+#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;
+
+    printk("\nSomething went wrong...\n");
+
+    code_dump_size = safecpy(code_dump, (const void *)instr_addr, CODE_DUMP_SIZE);
+    stack_dump_size = safecpy(stack_dump, (const void *)registers[13], STACK_DUMP_SIZE);
+
+    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%08"PRIX32"       %-7s%08"PRIX32"\n", register_names[i], registers[i], register_names[i+1], registers[i+1]);
+    }
+
+    /* Print cpsr. */
+    printk("%-7s%08"PRIX32"\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[13]);
+    printk("\n");
+    fatal_error("An exception occured!\n");
+}

fusee/fusee-secondary/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-secondary/src/exception_handlers_asm.s

@@ -0,0 +1,109 @@
+.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+8
+    cmp     lr, sp
+    blo     _exception_handler_data_abort_normal
+    adr     sp, _safecpy_end+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, lr}
+    bx      lr         /* Need to do that separately on ARMv4. */
+
+_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-secondary/src/main.c

@@ -5,6 +5,7 @@
 #include <malloc.h>
 #include "utils.h"
 #include "panic.h"
+#include "exception_handlers.h"
 #include "loader.h"
 #include "chainloader.h"
 #include "stage2.h"
@@ -26,9 +27,11 @@ static void setup_env(void) {
         generic_panic();
     }
 
+    /* Set up exception handlers. */
+    setup_exception_handlers();
+
     if(switchfs_mount_all() == -1) {
-        perror("Failed to mount at least one parition");
+        fatal_error("Failed to mount at least one parition: %s\n", strerror(errno));
-        generic_panic();
     }
 
     /* TODO: What other hardware init should we do here? */