AP_HAL: rework heap allocation functions

make functions for lua heap allocation suitable for use in all
non-ChibiOS HALs

libraries/AP_HAL/Util.cpp

@@ -9,6 +9,8 @@
 #else
 #include <time.h>
 #endif
+#include <AP_InternalError/AP_InternalError.h>
+#include <AP_Math/AP_Math.h>
 
 /* Helper class implements AP_HAL::Print so we can use utility/vprintf */
 class BufferPrinter : public AP_HAL::BetterStream {
@@ -78,3 +80,111 @@ void AP_HAL::Util::set_soft_armed(const bool b)
         }
     }
 }
+
+#if ENABLE_HEAP
+/*
+  default functions for heap management for lua scripting for systems
+  that don't have the ability to create separable heaps
+ */
+
+struct heap_allocation_header {
+    struct heap *hp;
+    uint32_t allocation_size; // size of allocated block, not including this header
+};
+
+#define HEAP_MAGIC 0x5681ef9f
+
+struct heap {
+    uint32_t magic;
+    uint32_t max_heap_size;
+    uint32_t current_heap_usage;
+};
+
+void *AP_HAL::Util::heap_create(uint32_t size)
+{
+    struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap));
+    if (new_heap != nullptr) {
+        new_heap->max_heap_size = size;
+    }
+    new_heap->magic = HEAP_MAGIC;
+    return (void *)new_heap;
+}
+
+void AP_HAL::Util::heap_destroy(void *heap_ptr)
+{
+    struct heap *heapp = (struct heap*)heap_ptr;
+    if (heapp->magic != HEAP_MAGIC) {
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        return;
+    }
+
+#if CONFIG_HAL_BOARD == HAL_BOARD_SITL
+    if (heapp->current_heap_usage != 0) {
+        // lua should guarantee that there is no memory still
+        // allocated when we destroy the heap. Throw an error in SITL
+        // if this proves not to be true
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+    }
+#endif
+
+    // free the heap structure
+    free(heapp);
+}
+
+/*
+  allocate memory on a specific heap
+ */
+void *AP_HAL::Util::heap_allocate(void *heap_ptr, uint32_t size)
+{
+    if (heap_ptr == nullptr || size == 0) {
+        return nullptr;
+    }
+    struct heap *heapp = (struct heap*)heap_ptr;
+    if (heapp->magic != HEAP_MAGIC) {
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        return nullptr;
+    }
+
+    if (heapp->current_heap_usage + size > heapp->max_heap_size) {
+        // fail the allocation as we don't have the memory. Note that
+        // we don't simulate the fragmentation that we would get with
+        // HAL_ChibiOS
+        return nullptr;
+    }
+
+    auto *header = (heap_allocation_header *)malloc(size + sizeof(heap_allocation_header));
+    if (header == nullptr) {
+        // can't allocate the new memory
+        return nullptr;
+    }
+    header->hp = heapp;
+    header->allocation_size = size;
+
+    heapp->current_heap_usage += size;
+
+    return header + 1;
+}
+
+/*
+  free memory from a previous heap_allocate call
+ */
+void AP_HAL::Util::heap_free(void *ptr)
+{
+    if (ptr == nullptr) {
+        return;
+    }
+    // extract header
+    auto *header = ((struct heap_allocation_header *)ptr)-1;
+    auto *heapp = header->hp;
+    if (heapp->magic != HEAP_MAGIC) {
+        INTERNAL_ERROR(AP_InternalError::error_t::flow_of_control);
+        return;
+    }
+    const uint32_t size = header->allocation_size;
+    heapp->current_heap_usage -= size;
+
+    // free the memory
+    free(header);
+}
+
+#endif // ENABLE_HEAP

libraries/AP_HAL/Util.h

@@ -149,16 +149,33 @@ public:
     virtual void free_type(void *ptr, size_t size, Memory_Type mem_type) { return free(ptr); }
 
 #if ENABLE_HEAP
-    // heap functions, note that a heap once alloc'd cannot be dealloc'd
+    /*
-    virtual void *allocate_heap_memory(size_t size) = 0;
+      heap functions used by lua scripting
-    virtual void *heap_realloc(void *heap, void *ptr, size_t old_size, size_t new_size) = 0;
+    */
-#if USE_LIBC_REALLOC
+    // allocate a new heap
-    virtual void *std_realloc(void *ptr, size_t new_size) { return realloc(ptr, new_size); }
+    virtual void *heap_create(uint32_t size);
-#else
+
-    virtual void *std_realloc(void *ptr, size_t new_size) = 0;
+    // destroy a heap
-#endif // USE_LIBC_REALLOC
+    virtual void heap_destroy(void *ptr);
+
+    // allocate some memory on a specific heap
+    virtual void *heap_allocate(void *heap, uint32_t size);
+
+    // free some memory that was allocated by heap_allocate. The implementation must
+    // be able to determine which heap the allocation was from using the pointer
+    virtual void heap_free(void *ptr);
 #endif // ENABLE_HEAP
 
+#if ENABLE_HEAP
+    /*
+      heap functions used by non-scripting
+     */
+#if USE_LIBC_REALLOC
+    virtual void *std_realloc(void *ptr, uint32_t new_size) { return realloc(ptr, new_size); }
+#else
+    virtual void *std_realloc(void *ptr, uint32_t new_size) = 0;
+#endif // USE_LIBC_REALLOC
+#endif
 
     /**
        how much free memory do we have in bytes. If unknown return 4096