diff --git a/fusee/fusee-primary/src/exception_handlers.c b/fusee/fusee-primary/src/exception_handlers.c index 0ae4835cf..0d71e9ecb 100644 --- a/fusee/fusee-primary/src/exception_handlers.c +++ b/fusee/fusee-primary/src/exception_handlers.c @@ -1,3 +1,5 @@ +#include + #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); diff --git a/fusee/fusee-secondary/src/exception_handlers.c b/fusee/fusee-secondary/src/exception_handlers.c new file mode 100644 index 000000000..0d71e9ecb --- /dev/null +++ b/fusee/fusee-secondary/src/exception_handlers.c @@ -0,0 +1,64 @@ +#include + +#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"); +} diff --git a/fusee/fusee-secondary/src/exception_handlers.h b/fusee/fusee-secondary/src/exception_handlers.h new file mode 100644 index 000000000..8e79b4136 --- /dev/null +++ b/fusee/fusee-secondary/src/exception_handlers.h @@ -0,0 +1,12 @@ +#ifndef FUSEE_EXCEPTION_HANDLERS_H +#define FUSEE_EXCEPTION_HANDLERS_H + +#include +#include + +/* 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 diff --git a/fusee/fusee-secondary/src/exception_handlers_asm.s b/fusee/fusee-secondary/src/exception_handlers_asm.s new file mode 100644 index 000000000..291b663ff --- /dev/null +++ b/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) diff --git a/fusee/fusee-secondary/src/main.c b/fusee/fusee-secondary/src/main.c index d495a41f5..10811e614 100644 --- a/fusee/fusee-secondary/src/main.c +++ b/fusee/fusee-secondary/src/main.c @@ -5,6 +5,7 @@ #include #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"); - generic_panic(); + fatal_error("Failed to mount at least one parition: %s\n", strerror(errno)); } /* TODO: What other hardware init should we do here? */