From 4a01cc1666f60e11d02c9187d71224544436a003 Mon Sep 17 00:00:00 2001
From: Michael du Breuil <wicked.shell.scripts@gmail.com>
Date: Sat, 23 Mar 2019 14:36:17 +0000
Subject: [PATCH] AH_HAL_Linux: add heap support to the HAL

---
 libraries/AP_HAL_Linux/Util.cpp | 57 +++++++++++++++++++++++++++++++++
 libraries/AP_HAL_Linux/Util.h   | 18 +++++++++++
 2 files changed, 75 insertions(+)

diff --git a/libraries/AP_HAL_Linux/Util.cpp b/libraries/AP_HAL_Linux/Util.cpp
index 25acd80913..155131a8f9 100644
--- a/libraries/AP_HAL_Linux/Util.cpp
+++ b/libraries/AP_HAL_Linux/Util.cpp
@@ -229,3 +229,60 @@ int Util::get_hw_arm32()
     fclose(f);
     return -ENOENT;
 }
+
+#ifdef ENABLE_HEAP
+void *Util::allocate_heap_memory(size_t size)
+{
+    struct heap *new_heap = (struct heap*)malloc(sizeof(struct heap));
+    if (new_heap != nullptr) {
+        new_heap->max_heap_size = size;
+        new_heap->current_heap_usage = 0;
+    }
+    return (void *)new_heap;
+}
+
+void *Util::heap_realloc(void *h, void *ptr, size_t new_size)
+{
+    if (h == nullptr) {
+        return nullptr;
+    }
+
+    struct heap *heapp = (struct heap*)h;
+
+    // extract appropriate headers
+    size_t old_size = 0;
+    heap_allocation_header *old_header = nullptr;
+    if (ptr != nullptr) {
+        old_header = ((heap_allocation_header *)ptr) - 1;
+        old_size = old_header->allocation_size;
+    }
+
+    heapp->current_heap_usage -= old_size;
+    if (new_size == 0) {
+       free(old_header);
+       return nullptr;
+    }
+
+    if ((heapp->current_heap_usage + new_size - old_size) > heapp->max_heap_size) {
+        // fail the allocation as we don't have the memory. Note that we don't simulate fragmentation
+        return nullptr;
+    }
+
+    heap_allocation_header *new_header = (heap_allocation_header *)malloc(new_size + sizeof(heap_allocation_header));
+    if (new_header == nullptr) {
+        // total failure to allocate, this is very surprising in SITL
+        return nullptr;
+    }
+    heapp->current_heap_usage += new_size;
+    new_header->allocation_size = new_size;
+    void *new_mem = new_header + 1;
+
+    if (ptr == nullptr) {
+        return new_mem;
+    }
+    memcpy(new_mem, ptr, old_size > new_size ? new_size : old_size);
+    free(old_header);
+    return new_mem;
+}
+
+#endif // ENABLE_HEAP
diff --git a/libraries/AP_HAL_Linux/Util.h b/libraries/AP_HAL_Linux/Util.h
index 6a679e5ddc..531292e889 100644
--- a/libraries/AP_HAL_Linux/Util.h
+++ b/libraries/AP_HAL_Linux/Util.h
@@ -58,6 +58,12 @@ public:
 
     bool get_system_id(char buf[40]) override;
     bool get_system_id_unformatted(uint8_t buf[], uint8_t &len) 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) override;
+    virtual void *heap_realloc(void *h, void *ptr, size_t new_size) override;
+#endif // ENABLE_HEAP
     
     /*
      * Write a string as specified by @fmt to the file in @path. Note this
@@ -113,6 +119,18 @@ private:
     const char *custom_terrain_directory = nullptr;
     const char *custom_storage_directory = nullptr;
     static const char *_hw_names[UTIL_NUM_HARDWARES];
+
+#ifdef ENABLE_HEAP
+    struct heap_allocation_header {
+        size_t allocation_size; // size of allocated block, not including this header
+    };
+
+    struct heap {
+      size_t max_heap_size;
+      size_t current_heap_usage;
+    };
+#endif // ENABLE_HEAP
+
 };
 
 }