From 18e88e0ea3b6e8afbc359d8c9e523b2001c50cdf Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 28 Apr 2020 16:34:52 +1000
Subject: [PATCH] HAL_ChibiOS: implement stack overflow hook

this needs C bindings to allow call from low level RTOS thread
switching code
---
 libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h   |  9 +++++++++
 .../AP_HAL_ChibiOS/hwdef/common/stm32_util.c     | 10 ++++++++++
 .../AP_HAL_ChibiOS/hwdef/common/stm32_util.h     |  3 +++
 libraries/AP_HAL_ChibiOS/system.cpp              | 16 +++++++++++++---
 4 files changed, 35 insertions(+), 3 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h b/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h
index 65ff4f0618..b3cb0f3c14 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/chconf.h
@@ -730,6 +730,15 @@
 } while(0)
 #endif
 
+/**
+ * @brief   stack overflow event hook.
+ * @details This hook is invoked when we have a stack overflow on task switch
+ */
+#define CH_CFG_STACK_OVERFLOW_HOOK(tp) {                                         \
+  extern void stack_overflow_hook(thread_t *tp); \
+  stack_overflow(tp); \
+}
+
 /**
  * @brief   Trace hook.
  * @details This hook is invoked each time a new record is written in the
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
index 35e01fd945..442ed9c944 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c
@@ -435,3 +435,13 @@ void system_halt_hook(void)
 #endif
 }
 
+// hook for stack overflow
+void stack_overflow(thread_t *tp)
+{
+    extern void AP_stack_overflow(const char *thread_name);
+    AP_stack_overflow(tp->name);
+    // if we get here then we are armed and got a stack overflow. We
+    // will report an internal error and keep trying to fly. We are
+    // quite likely to crash anyway due to memory corruption. The
+    // watchdog data should record the thread name and fault type
+}
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
index ecd5c4a77b..6fa15ea235 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
@@ -107,6 +107,9 @@ void fault_printf(const char *fmt, ...);
 // halt hook for printing panic message
 void system_halt_hook(void);
 
+// hook for stack overflow
+void stack_overflow(thread_t *tp);
+    
 #ifdef __cplusplus
 }
 #endif
diff --git a/libraries/AP_HAL_ChibiOS/system.cpp b/libraries/AP_HAL_ChibiOS/system.cpp
index a632093065..0e780c2fab 100644
--- a/libraries/AP_HAL_ChibiOS/system.cpp
+++ b/libraries/AP_HAL_ChibiOS/system.cpp
@@ -56,16 +56,26 @@ void NMI_Handler(void) { while (1); }
 /*
   save watchdog data for a hard fault
  */
-    static void save_fault_watchdog(uint16_t line, FaultType fault_type, uint32_t fault_addr, uint32_t lr)
+static void save_fault_watchdog(uint16_t line, FaultType fault_type, uint32_t fault_addr, uint32_t lr)
 {
 #ifndef HAL_BOOTLOADER_BUILD
     bool using_watchdog = AP_BoardConfig::watchdog_enabled();
     if (using_watchdog) {
         AP_HAL::Util::PersistentData &pd = hal.util->persistent_data;
         pd.fault_line = line;
-        pd.fault_type = fault_type;
+        if (pd.fault_type == 0) {
+            // don't overwrite earlier fault
+            pd.fault_type = fault_type;
+        }
         pd.fault_addr = fault_addr;
-        pd.fault_thd_prio = chThdGetPriorityX();
+        thread_t *tp = chThdGetSelfX();
+        if (tp) {
+            pd.fault_thd_prio = tp->prio;
+            // get first 4 bytes of the name, but only of first fault
+            if (tp->name && pd.thread_name4[0] == 0) {
+                strncpy(pd.thread_name4, tp->name, 4);
+            }
+        }
         pd.fault_icsr = SCB->ICSR;
         pd.fault_lr = lr;
         stm32_watchdog_save((uint32_t *)&hal.util->persistent_data, (sizeof(hal.util->persistent_data)+3)/4);