From 43d1816ac2b6c2beb978f442ce1291235c13f14a Mon Sep 17 00:00:00 2001 From: TuxSH Date: Mon, 21 May 2018 02:24:41 +0200 Subject: [PATCH] [stage1] Add exception handlers... which somehow don't work? --- fusee/fusee-primary/src/exception_handlers.c | 60 ++++++++++ fusee/fusee-primary/src/exception_handlers.h | 12 ++ .../src/exception_handlers_asm.s | 110 ++++++++++++++++++ fusee/fusee-primary/src/main.c | 4 + fusee/fusee-primary/src/utils.c | 36 ++++++ fusee/fusee-primary/src/utils.h | 2 + 6 files changed, 224 insertions(+) create mode 100644 fusee/fusee-primary/src/exception_handlers.c create mode 100644 fusee/fusee-primary/src/exception_handlers.h create mode 100644 fusee/fusee-primary/src/exception_handlers_asm.s diff --git a/fusee/fusee-primary/src/exception_handlers.c b/fusee/fusee-primary/src/exception_handlers.c new file mode 100644 index 000000000..fb6dcda29 --- /dev/null +++ b/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"); +} diff --git a/fusee/fusee-primary/src/exception_handlers.h b/fusee/fusee-primary/src/exception_handlers.h new file mode 100644 index 000000000..8e79b4136 --- /dev/null +++ b/fusee/fusee-primary/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-primary/src/exception_handlers_asm.s b/fusee/fusee-primary/src/exception_handlers_asm.s new file mode 100644 index 000000000..cde577de0 --- /dev/null +++ b/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) diff --git a/fusee/fusee-primary/src/main.c b/fusee/fusee-primary/src/main.c index 95ef2bf01..5adbe630e 100644 --- a/fusee/fusee-primary/src/main.c +++ b/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) { diff --git a/fusee/fusee-primary/src/utils.c b/fusee/fusee-primary/src/utils.c index 2966c48b2..1ffe4b249 100644 --- a/fusee/fusee-primary/src/utils.c +++ b/fusee/fusee-primary/src/utils.c @@ -11,6 +11,8 @@ #include "lib/printk.h" #include "hwinit/btn.h" +#include + __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); + } + } + } +} diff --git a/fusee/fusee-primary/src/utils.h b/fusee/fusee-primary/src/utils.h index 819529f21..bb647f47d 100644 --- a/fusee/fusee-primary/src/utils.h +++ b/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);