HAL_ChibiOS: add support for CCM memory

this uses DMA bounce buffers for bus transfers, and falls back to CCM
ram in allocations if the type is unspecified

libraries/AP_HAL_ChibiOS/Device.cpp

@@ -19,9 +19,10 @@
 #include <stdio.h>
 #include "Scheduler.h"
 #include "Semaphores.h"
+#include "Util.h"
 
 
-namespace ChibiOS {
+using namespace ChibiOS;
 
 static const AP_HAL::HAL &hal = AP_HAL::get_HAL();
 
@@ -141,4 +142,48 @@ bool DeviceBus::adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_u
     return true;
 }
 
+/*
+  setup to use DMA-safe bouncebuffers for device transfers
+ */
+void DeviceBus::bouncebuffer_setup(const uint8_t *&buf_tx, uint16_t tx_len,
+                                   uint8_t *&buf_rx, uint16_t rx_len)
+{
+    if (buf_tx && !IS_DMA_SAFE(buf_tx)) {
+        if (tx_len > bounce_buffer_tx_size) {
+            if (bounce_buffer_tx_size) {
+                hal.util->free_type(bounce_buffer_tx, bounce_buffer_tx_size, AP_HAL::Util::MEM_DMA_SAFE);
+                bounce_buffer_tx_size = 0;
+            }
+            bounce_buffer_tx = (uint8_t *)hal.util->malloc_type(tx_len, AP_HAL::Util::MEM_DMA_SAFE);
+            if (bounce_buffer_tx == nullptr) {
+                AP_HAL::panic("Out of memory for DMA TX");
+            }
+            bounce_buffer_tx_size = tx_len;
+        }
+        memcpy(bounce_buffer_tx, buf_tx, tx_len);
+        buf_tx = bounce_buffer_tx;
+    }
+
+    if (buf_rx && !IS_DMA_SAFE(buf_rx)) {
+        if (rx_len > bounce_buffer_rx_size) {
+            if (bounce_buffer_rx_size) {
+                hal.util->free_type(bounce_buffer_rx, bounce_buffer_rx_size, AP_HAL::Util::MEM_DMA_SAFE);
+                bounce_buffer_rx_size = 0;
+            }
+            bounce_buffer_rx = (uint8_t *)hal.util->malloc_type(rx_len, AP_HAL::Util::MEM_DMA_SAFE);
+            if (bounce_buffer_rx == nullptr) {
+                AP_HAL::panic("Out of memory for DMA RX");
+            }
+            bounce_buffer_rx_size = rx_len;
+        }
+        buf_rx = bounce_buffer_rx;
+    }
+}
+
+/*
+  complete a transfer using DMA bounce buffer
+ */
+void DeviceBus::bouncebuffer_rx_copy(uint8_t *buf_rx, uint16_t rx_len)
+{
+    memcpy(buf_rx, bounce_buffer_rx, rx_len);
 }

libraries/AP_HAL_ChibiOS/Device.h

@@ -35,6 +35,10 @@ public:
     bool adjust_timer(AP_HAL::Device::PeriodicHandle h, uint32_t period_usec);
     static void bus_thread(void *arg);
 
+    void bouncebuffer_setup(const uint8_t *&buf_tx, uint16_t tx_len,
+                            uint8_t *&buf_rx, uint16_t rx_len);
+    void bouncebuffer_rx_copy(uint8_t *buf_rx, uint16_t rx_len);
+    
 private:
     struct callback_info {
         struct callback_info *next;
@@ -46,6 +50,12 @@ private:
     thread_t* thread_ctx;
     bool thread_started;
     AP_HAL::Device *hal_device;
+
+    // support for bounce buffers for DMA-safe transfers
+    uint8_t *bounce_buffer_tx;
+    uint8_t *bounce_buffer_rx;
+    uint16_t bounce_buffer_tx_size;
+    uint16_t bounce_buffer_rx_size;
 };
 
 }

libraries/AP_HAL_ChibiOS/HAL_ChibiOS_Class.cpp

@@ -103,7 +103,6 @@ thread_t* get_main_thread()
 }
 
 static AP_HAL::HAL::Callbacks* g_callbacks;
-THD_WORKING_AREA(_main_thread_wa, APM_MAIN_THREAD_STACK_SIZE);
 static THD_FUNCTION(main_loop,arg)
 {
     daemon_task = chThdGetSelfX();
@@ -191,8 +190,9 @@ void HAL_ChibiOS::run(int argc, char * const argv[], Callbacks* callbacks) const
     assert(callbacks);
     g_callbacks = callbacks;
 
-    chThdCreateStatic(_main_thread_wa,
-                      sizeof(_main_thread_wa),
+    void *main_thread_wa = hal.util->malloc_type(THD_WORKING_AREA_SIZE(APM_MAIN_THREAD_STACK_SIZE), AP_HAL::Util::MEM_FAST);
+    chThdCreateStatic(main_thread_wa,
+                      APM_MAIN_THREAD_STACK_SIZE,
                       APM_MAIN_PRIORITY,     /* Initial priority.    */
                       main_loop,             /* Thread function.     */
                       nullptr);              /* Thread parameter.    */

libraries/AP_HAL_ChibiOS/I2CDevice.cpp

@@ -38,6 +38,7 @@ static uint8_t rx_dma_stream[] = {
 };
 
 using namespace ChibiOS;
+extern const AP_HAL::HAL& hal;
 
 I2CBus I2CDeviceManager::businfo[ARRAY_SIZE(I2CD)];
 
@@ -151,17 +152,22 @@ bool I2CDevice::transfer(const uint8_t *send, uint32_t send_len,
 bool I2CDevice::_transfer(const uint8_t *send, uint32_t send_len,
                          uint8_t *recv, uint32_t recv_len)
 {
-    int ret;
+    uint8_t *recv_buf = recv;
+    const uint8_t *send_buf = send;
+
+    bus.bouncebuffer_setup(send_buf, send_len, recv_buf, recv_len);
+
     for(uint8_t i=0 ; i <= _retries; i++) {
+        int ret;
         i2cAcquireBus(I2CD[bus.busnum]);
         // calculate a timeout as twice the expected transfer time, and set as min of 4ms
         uint32_t timeout_ms = 1+2*(((8*1000000UL/bus.i2ccfg.clock_speed)*MAX(send_len, recv_len))/1000);
         timeout_ms = MAX(timeout_ms, _timeout_ms);
         if(send_len == 0) {
-            ret = i2cMasterReceiveTimeout(I2CD[bus.busnum], _address,recv, recv_len, MS2ST(timeout_ms));
+            ret = i2cMasterReceiveTimeout(I2CD[bus.busnum], _address, recv_buf, recv_len, MS2ST(timeout_ms));
         } else {
-            ret = i2cMasterTransmitTimeout(I2CD[bus.busnum], _address, send, send_len,
-                                           recv, recv_len, MS2ST(timeout_ms));
+            ret = i2cMasterTransmitTimeout(I2CD[bus.busnum], _address, send_buf, send_len,
+                                           recv_buf, recv_len, MS2ST(timeout_ms));
         }
         i2cReleaseBus(I2CD[bus.busnum]);
         if (ret != MSG_OK){
@@ -169,6 +175,9 @@ bool I2CDevice::_transfer(const uint8_t *send, uint32_t send_len,
             i2cStop(I2CD[bus.busnum]);
             i2cStart(I2CD[bus.busnum], &bus.i2ccfg);
         } else {
+            if (recv_buf != recv) {
+                memcpy(recv, recv_buf, recv_len);
+            }
             return true;
         }
     }

libraries/AP_HAL_ChibiOS/SPIDevice.cpp

@@ -16,11 +16,13 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_HAL/utility/OwnPtr.h>
+#include "Util.h"
 #include "Scheduler.h"
 #include "Semaphores.h"
 #include <stdio.h>
 
 using namespace ChibiOS;
+extern const AP_HAL::HAL& hal;
 
 #if CONFIG_HAL_BOARD_SUBTYPE == HAL_BOARD_SUBTYPE_CHIBIOS_SKYVIPER_F412
 #define SPI_BUS_FLOW  1
@@ -198,7 +200,17 @@ void SPIDevice::do_transfer(const uint8_t *send, uint8_t *recv, uint32_t len)
     if (!set_chip_select(true)) {
         return;
     }
-    spiExchange(spi_devices[device_desc.bus].driver, len, send, recv); 
+    uint8_t *recv_buf = recv;
+    const uint8_t *send_buf = send;
+
+    bus.bouncebuffer_setup(send_buf, len, recv_buf, len);
+
+    spiExchange(spi_devices[device_desc.bus].driver, len, send_buf, recv_buf);
+
+    if (recv_buf != recv) {
+        memcpy(recv, recv_buf, len);
+    }
+    
     set_chip_select(old_cs_forced);
 }
 
@@ -259,15 +271,14 @@ bool SPIDevice::transfer(const uint8_t *send, uint32_t send_len,
         do_transfer(send, recv, recv_len);
         return true;
     }
-    uint32_t buf_aligned[1+((send_len+recv_len)/4)];
-    uint8_t *buf = (uint8_t *)&buf_aligned[0];
+    uint8_t buf[send_len+recv_len];
     if (send_len > 0) {
         memcpy(buf, send, send_len);
     }
     if (recv_len > 0) {
         memset(&buf[send_len], 0, recv_len);
     }
-    do_transfer((uint8_t *)buf, (uint8_t *)buf, send_len+recv_len);
+    do_transfer(buf, buf, send_len+recv_len);
     if (recv_len > 0) {
         memcpy(recv, &buf[send_len], recv_len);
     }

libraries/AP_HAL_ChibiOS/Util.cpp

@@ -28,19 +28,50 @@ extern AP_IOMCU iomcu;
 #endif
 
 using namespace ChibiOS;
+
+extern "C" {
+    size_t mem_available(void);
+    void *malloc_ccm(size_t size);
+};
+
 /**
    how much free memory do we have in bytes.
 */
 uint32_t ChibiUtil::available_memory(void)
 {
-    size_t totalp = 0;
-    // get memory available on heap
-    chHeapStatus(nullptr, &totalp, nullptr);
+    // from malloc.c in hwdef
+    return mem_available();
+}
 
-    // we also need to add in memory that is not yet allocated to the heap
-    totalp += chCoreGetStatusX();
+/*
+    Special Allocation Routines
+*/
 
-    return totalp;
+void* ChibiUtil::malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type)
+{
+    if (mem_type == AP_HAL::Util::MEM_FAST) {
+        return try_alloc_from_ccm_ram(size);
+    } else {
+        return malloc(size);
+    }
+}
+
+void ChibiUtil::free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type)
+{
+    if (ptr != NULL) {
+        chHeapFree(ptr);
+    }
+}
+
+
+void* ChibiUtil::try_alloc_from_ccm_ram(size_t size)
+{
+    void *ret = malloc_ccm(size);
+    if (ret == nullptr) {
+        //we failed to allocate from CCM so we are going to try common SRAM
+        ret = malloc(size);
+    }
+    return ret;
 }
 
 /*

libraries/AP_HAL_ChibiOS/Util.h

@@ -20,12 +20,19 @@
 #include "AP_HAL_ChibiOS_Namespace.h"
 #include "Semaphores.h"
 
+// this checks an address is in main memory and 16 bit aligned
+#define IS_DMA_SAFE(addr) ((uint32_t(addr) & 0xF0000001) == 0x20000000)
+
 class ChibiOS::ChibiUtil : public AP_HAL::Util {
 public:
     bool run_debug_shell(AP_HAL::BetterStream *stream) { return false; }
     AP_HAL::Semaphore *new_semaphore(void) override { return new ChibiOS::Semaphore; }
     uint32_t available_memory() override;
 
+    // Special Allocation Routines
+    void *malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type);
+    void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type);
+
     /*
       return state of safety switch, if applicable
      */
@@ -36,6 +43,8 @@ public:
     void set_imu_target_temp(int8_t *target);
 
 private:
+    void* try_alloc_from_ccm_ram(size_t size);
+    uint32_t available_memory_in_ccm_ram(void);
 
 #if HAL_WITH_IO_MCU && HAL_HAVE_IMU_HEATER
     struct {

libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c

@@ -32,6 +32,35 @@
 
 #define MIN_ALIGNMENT 8
 
+#ifdef STM32F4xx_MCUCONF
+// assume all F4xx MCUs have 64k CCM 
+#define CCM_RAM_ATTRIBUTE __attribute__((section(".ram4")))
+#endif
+
+#ifdef CCM_RAM_ATTRIBUTE
+//CCM RAM Heap
+#define CCM_REGION_SIZE 64*1024
+CH_HEAP_AREA(ccm_heap_region, CCM_REGION_SIZE) CCM_RAM_ATTRIBUTE;
+static memory_heap_t ccm_heap;
+static bool ccm_heap_initialised = false;
+#endif
+
+void *malloc_ccm(size_t size)
+{
+    void *p = NULL;
+#ifdef CCM_RAM_ATTRIBUTE
+    if (!ccm_heap_initialised) {
+        ccm_heap_initialised = true;
+        chHeapObjectInit(&ccm_heap, ccm_heap_region, CCM_REGION_SIZE);
+    }
+    p = chHeapAllocAligned(&ccm_heap, size, CH_HEAP_ALIGNMENT);
+    if (p != NULL) {
+        memset(p, 0, size);
+    }
+#endif
+    return p;
+}
+
 void *malloc(size_t size)
 {
     if (size == 0) {
@@ -40,20 +69,16 @@ void *malloc(size_t size)
     void *p = chHeapAllocAligned(NULL, size, MIN_ALIGNMENT);
     if (p) {
         memset(p, 0, size);
+    } else {
+        // fall back to CCM memory when main memory full
+        p = malloc_ccm(size);
     }
     return p;
 }
 
 void *calloc(size_t nmemb, size_t size)
 {
-    if (nmemb * size == 0) {
-        return NULL;
-    }
-    void *p = chHeapAllocAligned(NULL, nmemb*size, MIN_ALIGNMENT);
-    if (p != NULL) {
-        memset(p, 0, nmemb*size);
-    }
-    return p;
+    return malloc(nmemb * size);
 }
 
 void free(void *ptr)
@@ -62,3 +87,24 @@ void free(void *ptr)
         chHeapFree(ptr);
     }
 }
+
+/*
+  return total available memory in bytes
+ */
+size_t mem_available(void)
+{
+    size_t totalp = 0;
+    // get memory available on main heap
+    chHeapStatus(NULL, &totalp, NULL);
+
+    // we also need to add in memory that is not yet allocated to the heap
+    totalp += chCoreGetStatusX();
+
+#ifdef CCM_RAM_ATTRIBUTE
+    size_t ccm_available = 0;
+    chHeapStatus(&ccm_heap, &ccm_available, NULL);
+    totalp += ccm_available;
+#endif
+    
+    return totalp;
+}