HAL_ChibiOS: improved stack checking

libraries/AP_HAL_ChibiOS/Scheduler.cpp

@@ -473,6 +473,9 @@ void Scheduler::_io_thread(void* arg)
     }
 #ifndef HAL_NO_LOGGING
     uint32_t last_sd_start_ms = AP_HAL::millis();
+#endif
+#if CH_DBG_ENABLE_STACK_CHECK == TRUE
+    uint32_t last_stack_check_ms = 0;
 #endif
     while (true) {
         sched->delay_microseconds(1000);
@@ -480,16 +483,25 @@ void Scheduler::_io_thread(void* arg)
         // run registered IO processes
         sched->_run_io();
 
+#if !defined(HAL_NO_LOGGING) || CH_DBG_ENABLE_STACK_CHECK == TRUE
+        uint32_t now = AP_HAL::millis();
+#endif
+
 #ifndef HAL_NO_LOGGING
         if (!hal.util->get_soft_armed()) {
             // if sdcard hasn't mounted then retry it every 3s in the IO
             // thread when disarmed
-            uint32_t now = AP_HAL::millis();
             if (now - last_sd_start_ms > 3000) {
                 last_sd_start_ms = now;
                 AP::FS().retry_mount();
             }
         }
+#endif
+#if CH_DBG_ENABLE_STACK_CHECK == TRUE
+        if (now - last_stack_check_ms > 1000) {
+            last_stack_check_ms = now;
+            check_stack_free();
+        }
 #endif
     }
 }
@@ -693,4 +705,28 @@ void Scheduler::watchdog_pat(void)
     last_watchdog_pat_ms = AP_HAL::millis();
 }
 
+#if CH_DBG_ENABLE_STACK_CHECK == TRUE
+/*
+  check we have enough stack free on all threads and the ISR stack
+ */
+void Scheduler::check_stack_free(void)
+{
+    const uint32_t min_stack = 32;
+    bool ok = false;
+
+    if (stack_free(__main_stack_base__) < min_stack) {
+        AP::internalerror().error(error_number, 0xFFFF);
+    }
+
+    thread_t *tp = chRegFirstThread();
+    do {
+        if (stack_free(tp->wabase) < min_stack) {
+            AP::internalerror().error(error_number, tp->prio);
+        }
+        tp = chRegNextThread(tp);
+    } while (tp != NULL);
+}
+#endif // CH_DBG_ENABLE_STACK_CHECK == TRUE
+
+
 #endif // CH_CFG_USE_DYNAMIC

libraries/AP_HAL_ChibiOS/Scheduler.h

@@ -176,5 +176,8 @@ private:
 #if defined STM32H7
     void check_low_memory_is_zero();
 #endif
+
+    // check for free stack space
+    void check_stack_free(void);
 };
 #endif

libraries/AP_HAL_ChibiOS/Util.cpp

@@ -304,7 +304,8 @@ size_t Util::thread_info(char *buf, size_t bufsize)
   size_t total = 0;
 
   // a header to allow for machine parsers to determine format
-  int n = snprintf(buf, bufsize, "ThreadsV1\n");
+  int n = snprintf(buf, bufsize, "ThreadsV2\nISR PRI=255 sp=%p STACK_FREE=%u/%u\n",
+                   __main_stack_base__, stack_free(__main_stack_base__), __main_stack_size__);
   if (n <= 0) {
       return 0;
   }
@@ -315,13 +316,10 @@ size_t Util::thread_info(char *buf, size_t bufsize)
   tp = chRegFirstThread();
 
   do {
-      uint32_t stklimit = (uint32_t)tp->wabase;
+      const uint32_t total_stack = uint32_t(tp) - uint32_t(tp->wabase);
-      uint8_t *p = (uint8_t *)tp->wabase;
+      n = snprintf(buf, bufsize, "%-13.13s PRI=%3u sp=%p STACK_LEFT=%u/%u\n",
-      while (*p == CH_DBG_STACK_FILL_VALUE) {
+                   tp->name, unsigned(tp->prio), tp->wabase,
-          p++;
+                   stack_free(tp->wabase), total_stack);
-      }
-      uint32_t stack_left = ((uint32_t)p) - stklimit;
-      n = snprintf(buf, bufsize, "%-13.13s PRI=%3u STACK_LEFT=%u\n", tp->name, unsigned(tp->prio), unsigned(stack_left));
       if (n <= 0) {
           break;
       }

libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.c

@@ -449,3 +449,19 @@ void stack_overflow(thread_t *tp)
     (void)tp;
 #endif
 }
+
+#if CH_DBG_ENABLE_STACK_CHECK == TRUE
+/*
+  check how much stack is free given a stack base. Assumes the fill
+  byte is 0x55
+ */
+uint32_t stack_free(void *stack_base)
+{
+    const uint32_t *p = (uint32_t *)stack_base;
+    const uint32_t canary_word = 0x55555555;
+    while (*p == canary_word) {
+        p++;
+    }
+    return uint32_t(p) - uint32_t(stack_base);
+}
+#endif

libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h

@@ -110,6 +110,16 @@ void system_halt_hook(void);
 // hook for stack overflow
 void stack_overflow(thread_t *tp);
 
+/*
+  check how much stack is free given a stack base. Assumes the fill
+  byte is 0x55
+ */
+uint32_t stack_free(void *stack_base);
+
+// allow stack view code to show free ISR stack
+extern void *__main_stack_base__;
+extern uint32_t __main_stack_size__;
+
 #ifdef __cplusplus
 }
 #endif