From f04efe7985c7b38ffa9ce3b184032712d6c5010e Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Wed, 27 Feb 2019 11:59:27 +1100
Subject: [PATCH] HAL_ChibiOS: allocate threads from any heap

this fixes a failure on MatekF405-Wing where it fails to allocate the
SPI thread for the IMU
---
 libraries/AP_HAL_ChibiOS/Device.cpp           | 14 +++++----
 libraries/AP_HAL_ChibiOS/Scheduler.cpp        |  3 +-
 libraries/AP_HAL_ChibiOS/UARTDriver.cpp       |  3 +-
 .../AP_HAL_ChibiOS/hwdef/common/malloc.c      | 31 +++++++++++++++++++
 .../AP_HAL_ChibiOS/hwdef/common/stm32_util.h  |  1 +
 5 files changed, 42 insertions(+), 10 deletions(-)

diff --git a/libraries/AP_HAL_ChibiOS/Device.cpp b/libraries/AP_HAL_ChibiOS/Device.cpp
index f0b6779626..3fe5dbcc4b 100644
--- a/libraries/AP_HAL_ChibiOS/Device.cpp
+++ b/libraries/AP_HAL_ChibiOS/Device.cpp
@@ -113,12 +113,14 @@ AP_HAL::Device::PeriodicHandle DeviceBus::register_periodic_callback(uint32_t pe
             break;
         }
 
-        thread_ctx = chThdCreateFromHeap(NULL,
-                         THD_WORKING_AREA_SIZE(1024),
-                         name,
-                         thread_priority,           /* Initial priority.    */
-                         DeviceBus::bus_thread,    /* Thread function.     */
-                         this);                     /* Thread parameter.    */
+        thread_ctx = thread_create_alloc(THD_WORKING_AREA_SIZE(1024),
+                                         name,
+                                         thread_priority,           /* Initial priority.    */
+                                         DeviceBus::bus_thread,    /* Thread function.     */
+                                         this);                     /* Thread parameter.    */
+        if (thread_ctx == nullptr) {
+            AP_HAL::panic("Failed to create bus thread %s", name);
+        }
     }
     DeviceBus::callback_info *callback = new DeviceBus::callback_info;
     if (callback == nullptr) {
diff --git a/libraries/AP_HAL_ChibiOS/Scheduler.cpp b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
index 638503fdb6..ca260bcc8b 100644
--- a/libraries/AP_HAL_ChibiOS/Scheduler.cpp
+++ b/libraries/AP_HAL_ChibiOS/Scheduler.cpp
@@ -461,8 +461,7 @@ bool Scheduler::thread_create(AP_HAL::MemberProc proc, const char *name, uint32_
             break;
         }
     }
-    thread_t *thread_ctx = chThdCreateFromHeap(NULL,
-                                               THD_WORKING_AREA_SIZE(stack_size),
+    thread_t *thread_ctx = thread_create_alloc(THD_WORKING_AREA_SIZE(stack_size),
                                                name,
                                                thread_priority,
                                                thread_create_trampoline,
diff --git a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
index 8f3290a44c..dba2674d52 100644
--- a/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
+++ b/libraries/AP_HAL_ChibiOS/UARTDriver.cpp
@@ -101,8 +101,7 @@ void UARTDriver::thread_init(void)
         return;
     }
 #if CH_CFG_USE_HEAP == TRUE
-    uart_thread_ctx = chThdCreateFromHeap(NULL,
-                                          THD_WORKING_AREA_SIZE(2048),
+    uart_thread_ctx = thread_create_alloc(THD_WORKING_AREA_SIZE(2048),
                                           "apm_uart",
                                           APM_UART_PRIORITY,
                                           uart_thread,
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c
index 3ceac1cb30..3f609f6598 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c
@@ -207,4 +207,35 @@ size_t mem_available(void)
     return totalp;
 }
 
+/*
+  allocate a thread on any available heap
+ */
+thread_t *thread_create_alloc(size_t size,
+                              const char *name, tprio_t prio,
+                              tfunc_t pf, void *arg)
+{
+    thread_t *ret;
+    // first try default heap
+    ret = chThdCreateFromHeap(NULL, size, name, prio, pf, arg);
+    if (ret != NULL) {
+        return ret;
+    }
+
+#if defined(CCM_RAM_SIZE_KB)
+    ret = chThdCreateFromHeap(&ccm_heap, size, name, prio, pf, arg);
+    if (ret != NULL) {
+        return ret;
+    }
+#endif
+
+#if defined(DTCM_RAM_SIZE_KB)
+    ret = chThdCreateFromHeap(&dtcm_heap, size, name, prio, pf, arg);
+    if (ret != NULL) {
+        return ret;
+    }
+#endif
+    
+    return NULL;
+}
+
 #endif // CH_CFG_USE_HEAP
diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
index 14754f773f..9aa6c961a8 100644
--- a/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
+++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stm32_util.h
@@ -32,6 +32,7 @@ void show_stack_usage(void);
 size_t mem_available(void);
 void *malloc_ccm(size_t size);
 void *malloc_dma(size_t size);
+thread_t *thread_create_alloc(size_t size, const char *name, tprio_t prio, tfunc_t pf, void *arg);
 
 // flush all dcache
 void memory_flush_all(void);