HAL_SITL: added stack checking to SITL

libraries/AP_HAL_SITL/Scheduler.cpp

@@ -5,6 +5,8 @@
 #include "UARTDriver.h"
 #include <sys/time.h>
 #include <fenv.h>
+#include <malloc.h>
+#include <AP_Common/Semaphore.h>
 
 using namespace HALSITL;
 
@@ -22,6 +24,9 @@ uint8_t Scheduler::_num_io_procs = 0;
 bool Scheduler::_in_io_proc = false;
 bool Scheduler::_should_reboot = false;
 
+Scheduler::thread_attr *Scheduler::threads;
+HAL_Semaphore Scheduler::_thread_sem;
+
 Scheduler::Scheduler(SITL_State *sitlState) :
     _sitlState(sitlState),
     _stopped_clock_usec(0)
@@ -190,6 +195,8 @@ void Scheduler::_run_io_procs()
     hal.uartE->_timer_tick();
     hal.uartF->_timer_tick();
     hal.uartG->_timer_tick();
+
+    check_thread_stacks();
 }
 
 /*
@@ -209,29 +216,98 @@ void Scheduler::stop_clock(uint64_t time_usec)
 */
 void *Scheduler::thread_create_trampoline(void *ctx)
 {
-    AP_HAL::MemberProc *t = (AP_HAL::MemberProc *)ctx;
-    (*t)();
-    free(t);
+    struct thread_attr *a = (struct thread_attr *)ctx;
+    a->f[0]();
+    
+    WITH_SEMAPHORE(_thread_sem);
+    if (threads == a) {
+        threads = a->next;
+    } else {
+        for (struct thread_attr *p=threads; p->next; p=p->next) {
+            if (p->next == a) {
+                p->next = p->next->next;
+                break;
+            }
+        }
+    }
+    free(a->stack);
+    free(a->f);
+    delete a;
     return nullptr;
 }
 
+#ifndef PTHREAD_STACK_MIN
+#define PTHREAD_STACK_MIN 16384U
+#endif
 
 /*
   create a new thread
 */
 bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_t stack_size, priority_base base, int8_t priority)
 {
-    // take a copy of the MemberProc, it is freed after thread exits
-    AP_HAL::MemberProc *tproc = (AP_HAL::MemberProc *)malloc(sizeof(proc));
-    if (!tproc) {
-        return false;
-    }
-    *tproc = proc;
+    WITH_SEMAPHORE(_thread_sem);
+
+    // even an empty thread takes 2500 bytes on Linux, so always add 2300, giving us 200 bytes
+    // safety margin
+    stack_size += 2300;
+    
     pthread_t thread {};
-    if (pthread_create(&thread, NULL, thread_create_trampoline, tproc) != 0) {
-        free(tproc);
+    uint32_t alloc_stack = MAX(PTHREAD_STACK_MIN,stack_size);
+    
+    struct thread_attr *a = new struct thread_attr;
+    if (!a) {
         return false;
     }
+    // take a copy of the MemberProc, it is freed after thread exits
+    a->f = (AP_HAL::MemberProc *)malloc(sizeof(proc));
+    if (!a->f) {
+        goto failed;
+    }
+    a->stack = memalign(4096, alloc_stack);
+    if (!a->stack) {
+        goto failed;
+    }
+    memset(a->stack, stackfill, alloc_stack);
+    a->stack_min = (const uint8_t *)((((uint8_t *)a->stack) + alloc_stack) - stack_size);
+
+    a->stack_size = stack_size;
+    a->f[0] = proc;
+    a->name = name;
+    
+    pthread_attr_init(&a->attr);
+    if (pthread_attr_setstack(&a->attr, a->stack, alloc_stack) != 0) {
+        AP_HAL::panic("Failed to set stack of size %u for thread %s", alloc_stack, name);
+    }
+    if (pthread_create(&thread, &a->attr, thread_create_trampoline, a) != 0) {
+        goto failed;
+    }
+    a->next = threads;
+    threads = a;
     return true;
+
+failed:
+    if (a->stack) {
+        free(a->stack);
+    }
+    if (a->f) {
+        free(a->f);
+    }
+    delete a;
+    return false;
 }
 
+/*
+  check for stack overflow
+ */
+void Scheduler::check_thread_stacks(void)
+{
+    WITH_SEMAPHORE(_thread_sem);
+    for (struct thread_attr *p=threads; p; p=p->next) {
+        const uint8_t ncheck = 8;
+        for (uint8_t i=0; i<ncheck; i++) {
+            if (p->stack_min[i] != stackfill) {
+                AP_HAL::panic("stack overflow in thread %s\n", p->name);
+            }
+        }
+    }
+}

libraries/AP_HAL_SITL/Scheduler.h

@@ -75,10 +75,24 @@ private:
     void stop_clock(uint64_t time_usec);
 
     static void *thread_create_trampoline(void *ctx);
+    static void check_thread_stacks(void);
     
     bool _initialized;
     uint64_t _stopped_clock_usec;
     uint64_t _last_io_run;
     pthread_t _main_ctx;
+
+    static HAL_Semaphore _thread_sem;
+    struct thread_attr {
+        struct thread_attr *next;
+        AP_HAL::MemberProc *f;
+        pthread_attr_t attr;
+        uint32_t stack_size;
+        void *stack;
+        const uint8_t *stack_min;
+        const char *name;
+    };
+    static struct thread_attr *threads;
+    static const uint8_t stackfill = 0xEB;
 };
 #endif  // CONFIG_HAL_BOARD