v3: Log lr when a fault handler is executed

src/board/v3/Makefile.inc

@@ -82,6 +82,7 @@ BOARDSRC := \
 BOARDASM := \
         $(BOARDDIR)/cpu_init.s \
         $(BOARDDIR)/mmu_table.s \
+        $(BOARDDIR)/error_asm.s \
 
 # Define linker script file here
 LDSCRIPT= $(BOARDDIR)/ZYNQ7000.ld

src/board/v3/error.c

@@ -71,7 +71,7 @@ void _screaming_death(const char *pos, const char *msg)
   sbp_state_init(&sbp_state);
 
   /* Continuously send error message */
-  #define APPROX_ONE_SEC 33000000
+  #define APPROX_ONE_SEC 200000000
   while (1) {
     led_toggle(LED_RED);
     for (u32 d = 0; d < APPROX_ONE_SEC; d++) {
@@ -118,34 +118,12 @@ void _exit(int status)
 void fault_handling_setup(void) {
 }
 
-__attribute__((interrupt("UNDEF")))
-void Und_Handler(void)
+/* Called by fault handlers in error_asm.s */
+void fault_handler_screaming_death(const char *msg_str, u32 lr)
 {
-  screaming_death("Undefined instruction!");
-}
-
-__attribute__((interrupt("ABORT")))
-void Prefetch_Handler(void)
-{
-  screaming_death("Prefetch Abort!");
-}
-
-__attribute__((interrupt("ABORT")))
-void Abort_Handler(void)
-{
-  screaming_death("Data Abort!");
-}
-
-__attribute__((interrupt("SWI")))
-void Swi_Handler(void)
-{
-  screaming_death("Software Interrupt!");
-}
-
-__attribute__((interrupt("FIQ")))
-void Fiq_Handler(void)
-{
-  screaming_death("Unused FIQ!");
+  char msg[128];
+  sprintf(msg, "%s lr=%08X", msg_str, (unsigned int)lr);
+  screaming_death(msg);
 }
 
 /** \} */

src/board/v3/error_asm.s

@@ -0,0 +1,61 @@
+/*
+ * Copyright (C) 2016 Swift Navigation Inc.
+ * Contact: Jacob McNamee <[email protected]>
+ *
+ * This source is subject to the license found in the file 'LICENSE' which must
+ * be be distributed together with this source. All other rights reserved.
+ *
+ * THIS CODE AND INFORMATION IS PROVIDED "AS IS" WITHOUT WARRANTY OF ANY KIND,
+ * EITHER EXPRESSED OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND/OR FITNESS FOR A PARTICULAR PURPOSE.
+ */
+
+.macro handler str
+    /* R1 = LR */
+    mov r1, lr
+    /* Switch to SYS mode with IRQs masked */
+    msr CPSR_c, #0x9F
+    /* R0 = str */
+    ldr r0, =\str
+    /* fault_handler_screaming_death(str, lr); */
+    blx fault_handler_screaming_death
+.endm
+
+    .data
+
+und_handler_str:
+    .asciz "Undefined instruction!"
+prefetch_handler_str:
+    .asciz "Prefetch Handler!"
+abort_handler_str:
+    .asciz "Abort Handler!"
+swi_handler_str:
+    .asciz "Software Interrupt!"
+fiq_handler_str:
+    .asciz "Unused FIQ!"
+
+    .text
+    .code   32
+    .balign 4
+
+.global Und_Handler
+Und_Handler:
+    handler und_handler_str
+
+.global Prefetch_Handler
+Prefetch_Handler:
+    handler prefetch_handler_str
+
+.global Abort_Handler
+Abort_Handler:
+    handler abort_handler_str
+
+.global Swi_Handler
+Swi_Handler:
+    handler swi_handler_str
+
+.global Fiq_Handler
+Fiq_Handler:
+    handler fiq_handler_str
+
+.end