diff --git a/libraries/AP_HAL_ChibiOS/Util.cpp b/libraries/AP_HAL_ChibiOS/Util.cpp index 251ce44bcd..ddabaf482f 100644 --- a/libraries/AP_HAL_ChibiOS/Util.cpp +++ b/libraries/AP_HAL_ChibiOS/Util.cpp @@ -34,7 +34,6 @@ extern AP_IOMCU iomcu; extern const AP_HAL::HAL& hal; using namespace ChibiOS; - #if CH_CFG_USE_HEAP == TRUE /** @@ -79,6 +78,46 @@ void* Util::try_alloc_from_ccm_ram(size_t size) return ret; } +#ifdef ENABLE_HEAP + +void *Util::allocate_heap_memory(size_t size) +{ + void *buf = malloc(size); + if (buf == nullptr) { + return nullptr; + } + + memory_heap_t *heap = (memory_heap_t *)malloc(sizeof(memory_heap_t)); + if (heap != nullptr) { + chHeapObjectInit(heap, buf, size); + } + + return heap; +} + +void *Util::heap_realloc(void *heap, void *ptr, size_t new_size) +{ + if (heap == nullptr) { + return nullptr; + } + if (new_size == 0) { + if (ptr != nullptr) { + chHeapFree(ptr); + } + return nullptr; + } + if (ptr == nullptr) { + return chHeapAlloc((memory_heap_t *)heap, new_size); + } + void *new_mem = chHeapAlloc((memory_heap_t *)heap, new_size); + if (new_mem != nullptr) { + memcpy(new_mem, ptr, chHeapGetSize(ptr) > new_size ? new_size : chHeapGetSize(ptr)); + chHeapFree(ptr); + } + return new_mem; +} +#endif // ENABLE_HEAP + #endif // CH_CFG_USE_HEAP /* diff --git a/libraries/AP_HAL_ChibiOS/Util.h b/libraries/AP_HAL_ChibiOS/Util.h index 49219d0641..d26fa5a4b1 100644 --- a/libraries/AP_HAL_ChibiOS/Util.h +++ b/libraries/AP_HAL_ChibiOS/Util.h @@ -19,6 +19,7 @@ #include #include "AP_HAL_ChibiOS_Namespace.h" #include "AP_HAL_ChibiOS.h" +#include class ChibiOS::Util : public AP_HAL::Util { public: @@ -33,6 +34,12 @@ public: void *malloc_type(size_t size, AP_HAL::Util::Memory_Type mem_type) override; void free_type(void *ptr, size_t size, AP_HAL::Util::Memory_Type mem_type) override; +#ifdef ENABLE_HEAP + // heap functions, note that a heap once alloc'd cannot be dealloc'd + virtual void *allocate_heap_memory(size_t size); + virtual void *heap_realloc(void *heap, void *ptr, size_t new_size); +#endif // ENABLE_HEAP + /* return state of safety switch, if applicable */ @@ -93,4 +100,9 @@ private: #ifndef HAL_NO_FLASH_SUPPORT bool flash_bootloader() override; #endif + +#ifdef ENABLE_HEAP + static memory_heap_t scripting_heap; +#endif // ENABLE_HEAP + }; diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c index 4fc46aeb59..2e2330d800 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/malloc.c @@ -216,21 +216,4 @@ size_t mem_available(void) return totalp; } -void *realloc(void *addr, size_t size) -{ - if (size == 0) { - free(addr); - return NULL; - } - if (addr == NULL) { - return malloc(size); - } - void *new_mem = malloc(size); - if (new_mem != NULL) { - memcpy(new_mem, addr, chHeapGetSize(addr) > size ? size : chHeapGetSize(addr)); - free(addr); - } - return new_mem; -} - #endif // CH_CFG_USE_HEAP diff --git a/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.h b/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.h index 6435a22f91..b57111f8f2 100644 --- a/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.h +++ b/libraries/AP_HAL_ChibiOS/hwdef/common/stdio.h @@ -42,7 +42,6 @@ int vsscanf (const char *buf, const char *s, va_list ap); void *malloc(size_t size); void *calloc(size_t nmemb, size_t size); void free(void *ptr); -void *realloc(void *ptr, size_t size); extern int (*vprintf_console_hook)(const char *fmt, va_list arg);