1 files changed, 44 insertions, 1 deletions
diff --git a/src/fsm/common.c b/src/fsm/common.c
index 517eb80..33020e0 100644
--- a/src/fsm/common.c
+++ b/src/fsm/common.c
@@ -23,6 +23,7 @@
 */
 
 #include "common.h"
+#include "usart.h"
 #include "FreeRTOS.h"
 #include "timers.h"
 #include <stm32f4xx.h>
@@ -93,6 +94,8 @@ int random_bool(void)
 static int faultsource = 0;
 void trigger_fault(int source)
 {
+    usart_debug("Fatal: %d", source);
+
     __disable_irq();
 
     faultsource = source;
@@ -100,4 +103,44 @@ void trigger_fault(int source)
     while (1) {}
 }
 
-
+void hard_fault_handler_c(uint32_t *hardfault_args)
+{
+    uint32_t stacked_r0;
+    uint32_t stacked_r1;
+    uint32_t stacked_r2;
+    uint32_t stacked_r3;
+    uint32_t stacked_r12;
+    uint32_t stacked_lr;
+    uint32_t stacked_pc;
+    uint32_t stacked_psr;
+
+    stacked_r0 = hardfault_args[0];
+    stacked_r1 = hardfault_args[1];
+    stacked_r2 = hardfault_args[2];
+    stacked_r3 = hardfault_args[3];
+
+    stacked_r12 = hardfault_args[4];
+    stacked_lr = hardfault_args[5];
+    stacked_pc = hardfault_args[6];
+    stacked_psr = hardfault_args[7];
+
+    while (1);
+
+    usart_debug_puts("\n\n[Hard fault handler - all numbers in hex]\n");
+    usart_debug("R0 = %x\n", stacked_r0);
+    usart_debug("R1 = %x\n", stacked_r1);
+    usart_debug("R2 = %x\n", stacked_r2);
+    usart_debug("R3 = %x\n", stacked_r3);
+    usart_debug("R12 = %x\n", stacked_r12);
+    usart_debug("LR [R14] = %x  subroutine call return address\n", stacked_lr);
+    usart_debug("PC [R15] = %x  program counter\n", stacked_pc);
+    usart_debug("PSR = %x\n", stacked_psr);
+    usart_debug("BFAR = %x\n", (*((volatile unsigned long *)(0xE000ED38))));
+    usart_debug("CFSR = %x\n", (*((volatile unsigned long *)(0xE000ED28))));
+    usart_debug("HFSR = %x\n", (*((volatile unsigned long *)(0xE000ED2C))));
+    usart_debug("DFSR = %x\n", (*((volatile unsigned long *)(0xE000ED30))));
+    usart_debug("AFSR = %x\n", (*((volatile unsigned long *)(0xE000ED3C))));
+    usart_debug("SCB_SHCSR = %x\n", SCB->SHCSR);
+
+    while (1);
+}