From bffda451542854fb06021e2f7fac57534e9d2768 Mon Sep 17 00:00:00 2001
From: Damien George <damien.p.george@gmail.com>
Date: Mon, 6 Feb 2017 12:04:43 +1100
Subject: [PATCH] stmhal: On HardFault, print stack pointer and do a stack
 dump.

---
 stmhal/stm32_it.c | 31 +++++++++++++++++++++++++++++++
 1 file changed, 31 insertions(+)

diff --git a/stmhal/stm32_it.c b/stmhal/stm32_it.c
index 4dddc5e0ba..245b2ade4b 100644
--- a/stmhal/stm32_it.c
+++ b/stmhal/stm32_it.c
@@ -74,6 +74,7 @@
 #include "pendsv.h"
 #include "irq.h"
 #include "pybthread.h"
+#include "gccollect.h"
 #include "extint.h"
 #include "timer.h"
 #include "uart.h"
@@ -81,6 +82,7 @@
 #include "can.h"
 #include "dma.h"
 #include "i2c.h"
+#include "usb.h"
 
 extern void __fatal_error(const char*);
 extern PCD_HandleTypeDef pcd_fs_handle;
@@ -123,6 +125,15 @@ STATIC void print_reg(const char *label, uint32_t val) {
     mp_hal_stdout_tx_str("\r\n");
 }
 
+STATIC void print_hex_hex(const char *label, uint32_t val1, uint32_t val2) {
+    char hex_str[9];
+    mp_hal_stdout_tx_str(label);
+    mp_hal_stdout_tx_str(fmt_hex(val1, hex_str));
+    mp_hal_stdout_tx_str("  ");
+    mp_hal_stdout_tx_str(fmt_hex(val2, hex_str));
+    mp_hal_stdout_tx_str("\r\n");
+}
+
 // The ARMv7M Architecture manual (section B.1.5.6) says that upon entry
 // to an exception, that the registers will be in the following order on the
 // // stack: R0, R1, R2, R3, R12, LR, PC, XPSR
@@ -132,11 +143,18 @@ typedef struct {
 } ExceptionRegisters_t;
 
 void HardFault_C_Handler(ExceptionRegisters_t *regs) {
+    // We need to disable the USB so it doesn't try to write data out on
+    // the VCP and then block indefinitely waiting for the buffer to drain.
+    pyb_usb_flags = 0;
+
+    mp_hal_stdout_tx_str("HardFault\r\n");
+
     print_reg("R0    ", regs->r0);
     print_reg("R1    ", regs->r1);
     print_reg("R2    ", regs->r2);
     print_reg("R3    ", regs->r3);
     print_reg("R12   ", regs->r12);
+    print_reg("SP    ", (uint32_t)regs);
     print_reg("LR    ", regs->lr);
     print_reg("PC    ", regs->pc);
     print_reg("XPSR  ", regs->xpsr);
@@ -151,6 +169,19 @@ void HardFault_C_Handler(ExceptionRegisters_t *regs) {
     if (cfsr & 0x8000) {
         print_reg("BFAR  ", SCB->BFAR);
     }
+
+    if ((void*)&_ram_start <= (void*)regs && (void*)regs < (void*)&_ram_end) {
+        mp_hal_stdout_tx_str("Stack:\r\n");
+        uint32_t *stack_top = &_estack;
+        if ((void*)regs < (void*)&_heap_end) {
+            // stack not in static stack area so limit the amount we print
+            stack_top = (uint32_t*)regs + 32;
+        }
+        for (uint32_t *sp = (uint32_t*)regs; sp < stack_top; ++sp) {
+            print_hex_hex("  ", (uint32_t)sp, *sp);
+        }
+    }
+
     /* Go to infinite loop when Hard Fault exception occurs */
     while (1) {
         __fatal_error("HardFault");