pbio/platform/ev3: Hook up exception handlers to invoke C code

Author: ArcaneNibble

lib/pbio/platform/ev3/exceptionhandler.S

@@ -16,6 +16,11 @@
 
         .equ I_F_BIT, 0xC0 
         .equ MASK_SWI_NUM, 0xFF000000
+        .equ MASK_SR_MODE, 0x1F
+        .equ EXCEPT_TYPE_UNKNOWN, 0
+        .equ EXCEPT_TYPE_UNDEF, 1
+        .equ EXCEPT_TYPE_PREFETCH_ABORT, 2
+        .equ EXCEPT_TYPE_DATA_ABORT, 3
 
         .text
 
@@ -29,16 +34,66 @@
 @ Since I can only say that the IRQ Handler works, we leave the remaining ones as they are
 ExceptionHandlerStart:
         ldr pc, =Entry
-        B ExceptionHandlerStart
+        B UndefHandlder
         B SWIHandler
-        B ExceptionHandlerStart
-        B ExceptionHandlerStart
-        B ExceptionHandlerStart
+        B PrefetchAbortHandler
+        B DataAbortHandler
+        B UnknownExceptionHandler
         B IRQHandler
         B FIQHandler
 
 .pool
-        
+
+UndefHandlder:
+        @ Save r0-r12, lr
+        stmfd   sp!, {r0-r12, lr}
+        mov     r7, #EXCEPT_TYPE_UNDEF
+        b       CommonPanicHandler
+
+PrefetchAbortHandler:
+        @ Save r0-r12, lr
+        stmfd   sp!, {r0-r12, lr}
+        mov     r7, #EXCEPT_TYPE_PREFETCH_ABORT
+        b       CommonPanicHandler
+
+DataAbortHandler:
+        @ Save r0-r12, lr
+        stmfd   sp!, {r0-r12, lr}
+        mov     r7, #EXCEPT_TYPE_DATA_ABORT
+        b       CommonPanicHandler
+
+UnknownExceptionHandler:
+        @ Save r0-r12, lr
+        stmfd   sp!, {r0-r12, lr}
+        mov     r7, #EXCEPT_TYPE_UNKNOWN
+        @ b       CommonPanicHandler
+        @ fall through
+
+CommonPanicHandler:
+        @ Save SPSR
+        mrs     r0, spsr
+        stmfd   sp!, {r0}
+        @ Switch to previous mode to read its sp/lr
+        @ NOTE: Assumes we never use User mode
+        and     r0, #MASK_SR_MODE
+        mrs     r1, cpsr
+        and     r2, r1, #MASK_SR_MODE
+        orr     r2, r0
+        msr     cpsr, r2
+        mov     r3, r13
+        mov     r4, r14
+        msr     cpsr, r1
+        stmfd   sp!, {r3, r4}
+        @ Call panic handler
+        mov     r0, r7
+        mov     r1, sp
+        bl      ev3_panic_handler_
+        @ Reset
+        b       ExceptionHandlerStart
+
+ev3_panic_handler_:
+        ldr pc, =ev3_panic_handler
+
 FIQHandler:
         STMFD    r13!, {r0-r7, lr}
         LDR      r0, =0xFFFEF600

lib/pbio/platform/ev3/platform.c

@@ -49,6 +49,7 @@
 #include <tiam1808/hw/soc_AM1808.h>
 #include <tiam1808/i2c.h>
 #include <tiam1808/psc.h>
+#include <tiam1808/uart.h>
 
 #include <pbdrv/ioport.h>
 #include <pbio/port_interface.h>
@@ -409,6 +410,17 @@ unsigned int EDMAVersionGet(void) {
     return 1;
 }
 
+void ev3_panic_handler(int except_type, void *except_data) {
+    // Regardless of what's going on, configure the UART1 for a debug console
+    PSCModuleControl(SOC_PSC_1_REGS, HW_PSC_UART1, PSC_POWERDOMAIN_ALWAYS_ON, PSC_MDCTL_NEXT_ENABLE);
+    UARTConfigSetExpClk(SOC_UART_1_REGS, SOC_UART_1_MODULE_FREQ, 115200, UART_WORDL_8BITS, UART_OVER_SAMP_RATE_13);
+    UARTFIFOEnable(SOC_UART_1_REGS);
+
+    // TODO: Implement panic handler
+    UARTCharPut(SOC_UART_1_REGS, 'H');
+    UARTCharPut(SOC_UART_1_REGS, 'i');
+}
+
 /**
  * Callback for completion of all EDMA3 transfers on this platform.
  *