From 9efdc0dbef0f828cdd7ac987c76f14e8081e33c3 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Mon, 18 Nov 2024 09:52:15 +1100
Subject: [PATCH] AP_MultiHeap: added library

this is a standalone (no-HAL based) implementation of MultiHeap
---
 libraries/AP_MultiHeap/AP_MultiHeap.cpp      | 206 +++++++++++++++++++
 libraries/AP_MultiHeap/AP_MultiHeap.h        |  83 ++++++++
 libraries/AP_MultiHeap/MultiHeap_chibios.cpp |  47 +++++
 libraries/AP_MultiHeap/MultiHeap_malloc.cpp  | 119 +++++++++++
 4 files changed, 455 insertions(+)
 create mode 100644 libraries/AP_MultiHeap/AP_MultiHeap.cpp
 create mode 100644 libraries/AP_MultiHeap/AP_MultiHeap.h
 create mode 100644 libraries/AP_MultiHeap/MultiHeap_chibios.cpp
 create mode 100644 libraries/AP_MultiHeap/MultiHeap_malloc.cpp

diff --git a/libraries/AP_MultiHeap/AP_MultiHeap.cpp b/libraries/AP_MultiHeap/AP_MultiHeap.cpp
new file mode 100644
index 0000000000..ff81139cc8
--- /dev/null
+++ b/libraries/AP_MultiHeap/AP_MultiHeap.cpp
@@ -0,0 +1,206 @@
+/*
+  multiple heap interface, allowing for an allocator that uses
+  multiple underlying heaps to cope with multiple memory regions on
+  STM32 boards
+ */
+
+#include "AP_MultiHeap.h"
+
+#if ENABLE_HEAP
+#include <AP_Math/AP_Math.h>
+#include <stdio.h>
+
+/*
+  allow up to 10 heaps
+ */
+#ifndef MAX_HEAPS
+#define MAX_HEAPS 10
+#endif
+
+extern const AP_HAL::HAL &hal;
+
+/*
+  create heaps with a total memory size, splitting over at most
+  max_heaps
+ */
+bool MultiHeap::create(uint32_t total_size, uint8_t max_heaps, bool _allow_expansion, uint32_t _reserve_size)
+{
+    max_heaps = MIN(MAX_HEAPS, max_heaps);
+    if (heaps != nullptr) {
+        // don't allow double allocation
+        return false;
+    }
+    heaps = NEW_NOTHROW Heap[max_heaps];
+    if (heaps == nullptr) {
+        return false;
+    }
+    num_heaps = max_heaps;
+    for (uint8_t i=0; i<max_heaps; i++) {
+        uint32_t alloc_size = total_size;
+        while (alloc_size > 0) {
+            heaps[i].hp = heap_create(alloc_size);
+            if (heaps[i].hp != nullptr) {
+                total_size -= alloc_size;
+                sum_size += alloc_size;
+                break;
+            }
+            alloc_size *= 0.9;
+        }
+        if (total_size == 0) {
+            break;
+        }
+    }
+    if (total_size != 0) {
+        destroy();
+        return false;
+    }
+
+    allow_expansion = _allow_expansion;
+    reserve_size = _reserve_size;
+
+    return true;
+}
+
+// destroy heap
+void MultiHeap::destroy(void)
+{
+    if (!available()) {
+        return;
+    }
+    for (uint8_t i=0; i<num_heaps; i++) {
+        if (heaps[i].hp != nullptr) {
+            heap_destroy(heaps[i].hp);
+            heaps[i].hp = nullptr;
+        }
+    }
+    delete[] heaps;
+    heaps = nullptr;
+    num_heaps = 0;
+    sum_size = 0;
+    expanded_to = 0;
+}
+
+// return true if heap is available for operations
+bool MultiHeap::available(void) const
+{
+    return heaps != nullptr && heaps[0].hp != nullptr;
+}
+
+/*
+  allocate memory from a heap
+ */
+void *MultiHeap::allocate(uint32_t size)
+{
+    if (!available() || size == 0) {
+        return nullptr;
+    }
+    for (uint8_t i=0; i<num_heaps; i++) {
+        if (heaps[i].hp == nullptr) {
+            break;
+        }
+        void *newptr = heap_allocate(heaps[i].hp, size);
+        if (newptr != nullptr) {
+            last_failed = false;
+            return newptr;
+        }
+    }
+    if (!allow_expansion || !last_failed) {
+        /*
+          we only allow expansion when the last allocation
+          failed. This gives the lua engine a chance to use garbage
+          collection to recover memory
+         */
+        last_failed = true;
+        return nullptr;
+    }
+
+    if (!hal.util->get_soft_armed()) {
+        // only expand the available heaps when armed. When disarmed
+        // user should fix their SCR_HEAP_SIZE parameter
+        last_failed = true;
+        return nullptr;
+    }
+
+    /*
+      vehicle is armed and MultiHeap (for scripting) is out of
+      memory. We will see if we can add a new heap from available
+      memory if we have at least reserve_size bytes free
+     */
+    const uint32_t available = hal.util->available_memory();
+    const uint32_t heap_overhead = 128; // conservative value, varies with HAL
+    const uint32_t min_size = size + heap_overhead;
+    if (available < reserve_size+min_size) {
+        last_failed = true;
+        return nullptr;
+    }
+
+    // round up to a minimum of 30k to allocate, and allow for heap overhead
+    const uint32_t round_to = 30*1024U;
+    const uint32_t alloc_size = MIN(available - reserve_size, MAX(size+heap_overhead, round_to));
+    if (alloc_size < min_size) {
+        last_failed = true;
+        return nullptr;
+    }
+    for (uint8_t i=0; i<num_heaps; i++) {
+        if (heaps[i].hp == nullptr) {
+            heaps[i].hp = heap_create(alloc_size);
+            if (heaps[i].hp == nullptr) {
+                last_failed = true;
+                return nullptr;
+            }
+            sum_size += alloc_size;
+            expanded_to = sum_size;
+            void *p = heap_allocate(heaps[i].hp, size);
+            last_failed = p == nullptr;
+            return p;
+        }
+    }
+    last_failed = true;
+    return nullptr;
+}
+
+/*
+  free memory from a heap
+ */
+void MultiHeap::deallocate(void *ptr)
+{
+    if (!available() || ptr == nullptr) {
+        return;
+    }
+    heap_free(ptr);
+}
+
+/*
+  change size of an allocation, operates like realloc(), but requires
+  the old_size when ptr is not NULL
+ */
+void *MultiHeap::change_size(void *ptr, uint32_t old_size, uint32_t new_size)
+{
+    if (new_size == 0) {
+        deallocate(ptr);
+        return nullptr;
+    }
+    /*
+      we don't want to require the underlying allocation system to
+      support realloc() and we also want to be able to handle the case
+      of having to move the allocation to a new heap, so we do a
+      simple alloc/copy/deallocate for reallocation
+     */
+    void *newp = allocate(new_size);
+    if (ptr == nullptr) {
+        return newp;
+    }
+    if (newp == nullptr) {
+        if (old_size >= new_size) {
+            // Lua assumes that the allocator never fails when osize >= nsize
+            // the best we can do is return the old pointer
+            return ptr;
+        }
+        return nullptr;
+    }
+    memcpy(newp, ptr, MIN(old_size, new_size));
+    deallocate(ptr);
+    return newp;
+}
+
+#endif // ENABLE_HEAP
diff --git a/libraries/AP_MultiHeap/AP_MultiHeap.h b/libraries/AP_MultiHeap/AP_MultiHeap.h
new file mode 100644
index 0000000000..3b67f11739
--- /dev/null
+++ b/libraries/AP_MultiHeap/AP_MultiHeap.h
@@ -0,0 +1,83 @@
+/*
+  multiple heap interface, allowing for an allocator that uses
+  multiple underlying heaps to cope with multiple memory regions on
+  STM32 boards
+ */
+
+#pragma once
+
+#include <AP_HAL/AP_HAL.h>
+#include <AP_HAL/Util.h>
+
+#if ENABLE_HEAP
+
+#include <stdint.h>
+#include <stdbool.h>
+
+class MultiHeap {
+public:
+    /*
+      allocate/deallocate heaps
+     */
+    bool create(uint32_t total_size, uint8_t max_heaps, bool allow_expansion, uint32_t reserve_size);
+    void destroy(void);
+
+    // return true if the heap is available for operations
+    bool available(void) const;
+
+    // allocate memory within heaps
+    void *allocate(uint32_t size);
+    void deallocate(void *ptr);
+
+    // change allocated size of a pointer - this operates in a similar
+    // fashion to realloc, but requires an (accurate!) old_size value
+    // when ptr is not NULL. This is guaranteed by the lua scripting
+    // allocation API
+    void *change_size(void *ptr, uint32_t old_size, uint32_t new_size);
+
+    /*
+      get the size that we have expanded to. Used by error reporting in scripting
+     */
+    uint32_t get_expansion_size(void) const {
+        return expanded_to;
+    }
+
+private:
+    struct Heap {
+        void *hp;
+    };
+    struct Heap *heaps;
+
+    uint8_t num_heaps;
+    bool allow_expansion;
+    uint32_t reserve_size;
+    uint32_t sum_size;
+    uint32_t expanded_to;
+
+    // we only do heap expansion if the last allocation failed this
+    // encourages the lua scripting engine to garbage collect to
+    // re-use memory when possible
+    bool last_failed;
+
+
+    /*
+      low level allocation functions
+     */
+    /*
+      heap functions used by lua scripting
+    */
+    // allocate a new heap
+    void *heap_create(uint32_t size);
+
+    // destroy a heap
+    void heap_destroy(void *ptr);
+
+    // allocate some memory on a specific heap
+    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
+    void heap_free(void *ptr);
+};
+
+#endif // ENABLE_HEAP
diff --git a/libraries/AP_MultiHeap/MultiHeap_chibios.cpp b/libraries/AP_MultiHeap/MultiHeap_chibios.cpp
new file mode 100644
index 0000000000..d3225cd3b7
--- /dev/null
+++ b/libraries/AP_MultiHeap/MultiHeap_chibios.cpp
@@ -0,0 +1,47 @@
+/*
+  allocation backend functions using native ChibiOS chHeap API
+ */
+
+#include "AP_MultiHeap.h"
+#include <AP_HAL/AP_HAL_Boards.h>
+
+#if ENABLE_HEAP && CONFIG_HAL_BOARD == HAL_BOARD_CHIBIOS
+
+#include <ch.h>
+#include <hal.h>
+
+/*
+  heap functions used by lua scripting
+ */
+void *MultiHeap::heap_create(uint32_t size)
+{
+    memory_heap_t *heap = (memory_heap_t *)malloc(size + sizeof(memory_heap_t));
+    if (heap == nullptr) {
+        return nullptr;
+    }
+    chHeapObjectInit(heap, heap + 1U, size);
+    return heap;
+}
+
+void MultiHeap::heap_destroy(void *ptr)
+{
+    free(ptr);
+}
+
+void *MultiHeap::heap_allocate(void *heap, uint32_t size)
+{
+    if (heap == nullptr) {
+        return nullptr;
+    }
+    if (size == 0) {
+        return nullptr;
+    }
+    return chHeapAlloc((memory_heap_t *)heap, size);
+}
+
+void MultiHeap::heap_free(void *ptr)
+{
+    return chHeapFree(ptr);
+}
+
+#endif // ENABLE_HEAP && CONFIG_HAL_BOARD
diff --git a/libraries/AP_MultiHeap/MultiHeap_malloc.cpp b/libraries/AP_MultiHeap/MultiHeap_malloc.cpp
new file mode 100644
index 0000000000..857d01e76b
--- /dev/null
+++ b/libraries/AP_MultiHeap/MultiHeap_malloc.cpp
@@ -0,0 +1,119 @@
+#include "AP_MultiHeap.h"
+#include <AP_HAL/AP_HAL_Boards.h>
+
+#if ENABLE_HEAP && CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS
+
+/*
+  on systems other than chibios we map the allocation to the system
+  malloc/free functions
+ */
+
+#include <AP_InternalError/AP_InternalError.h>
+#include <stdlib.h>
+
+/*
+  low level allocator interface using system malloc
+ */
+/*
+  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 *MultiHeap::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 MultiHeap::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 *MultiHeap::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 MultiHeap::heap_free(void *ptr)
+{
+    // 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 && CONFIG_HAL_BOARD != HAL_BOARD_CHIBIOS