From 463dc8c727e91cde3027c58b238bb286506cef89 Mon Sep 17 00:00:00 2001
From: SG <who.just.the.doctor@gmail.com>
Date: Mon, 8 Apr 2024 20:27:58 +0300
Subject: [PATCH] memmgr: alloc aligned, realloc

---
 furi/core/memmgr.c      |  39 ++++----------
 furi/core/memmgr_heap.c | 116 ++++++++++++++++++++++++++++++++++++++--
 2 files changed, 123 insertions(+), 32 deletions(-)

diff --git a/furi/core/memmgr.c b/furi/core/memmgr.c
index 768adc05d..0fe79dff0 100644
--- a/furi/core/memmgr.c
+++ b/furi/core/memmgr.c
@@ -4,6 +4,8 @@
 #include <furi_hal_memory.h>
 
 extern void* pvPortMalloc(size_t xSize);
+extern void* pvPortAllocAligned(size_t xSize, size_t xAlignment);
+extern void* pvPortRealloc(void* pv, size_t xSize);
 extern void vPortFree(void* pv);
 extern size_t xPortGetFreeHeapSize(void);
 extern size_t xPortGetTotalHeapSize(void);
@@ -18,18 +20,7 @@ void free(void* ptr) {
 }
 
 void* realloc(void* ptr, size_t size) {
-    if(size == 0) {
-        vPortFree(ptr);
-        return NULL;
-    }
-
-    void* p = pvPortMalloc(size);
-    if(ptr != NULL) {
-        memcpy(p, ptr, size);
-        vPortFree(ptr);
-    }
-
-    return p;
+    return pvPortRealloc(ptr, size);
 }
 
 void* calloc(size_t count, size_t size) {
@@ -47,6 +38,14 @@ char* strdup(const char* s) {
     return y;
 }
 
+void* aligned_malloc(size_t size, size_t alignment) {
+    return pvPortAllocAligned(size, alignment);
+}
+
+void aligned_free(void* p) {
+    vPortFree(p);
+}
+
 size_t memmgr_get_free_heap(void) {
     return xPortGetFreeHeapSize();
 }
@@ -92,20 +91,4 @@ size_t memmgr_pool_get_free(void) {
 
 size_t memmgr_pool_get_max_block(void) {
     return furi_hal_memory_max_pool_block();
-}
-
-void* aligned_malloc(size_t size, size_t alignment) {
-    void* p1; // original block
-    void** p2; // aligned block
-    int offset = alignment - 1 + sizeof(void*);
-    if((p1 = (void*)malloc(size + offset)) == NULL) {
-        return NULL;
-    }
-    p2 = (void**)(((size_t)(p1) + offset) & ~(alignment - 1));
-    p2[-1] = p1;
-    return p2;
-}
-
-void aligned_free(void* p) {
-    free(((void**)p)[-1]);
 }
\ No newline at end of file
diff --git a/furi/core/memmgr_heap.c b/furi/core/memmgr_heap.c
index 630e26bd9..d6ca3f255 100644
--- a/furi/core/memmgr_heap.c
+++ b/furi/core/memmgr_heap.c
@@ -204,11 +204,11 @@ void* pvPortMalloc(size_t xSize) {
     // clear block content
     memset(data, 0, xSize);
 
+    memmgr_unlock();
+
     // trace allocation
     memmgr_heap_trace_malloc(data, xSize);
 
-    memmgr_unlock();
-
     return data;
 }
 
@@ -233,13 +233,121 @@ void vPortFree(void* pv) {
         // free
         tlsf_free(tlsf, pv);
 
+        memmgr_unlock();
+
         // trace free
         memmgr_heap_trace_free(pv);
-
-        memmgr_unlock();
     }
 }
 
+extern void* pvPortAllocAligned(size_t xSize, size_t xAlignment) {
+    // memory management in ISR is not allowed
+    if(FURI_IS_IRQ_MODE()) {
+        furi_crash("memmgt in ISR");
+    }
+
+    memmgr_lock();
+
+    // initialize tlsf, if not initialized
+    if(tlsf == NULL) {
+        size_t pool_size = (size_t)&__heap_end__ - (size_t)&__heap_start__;
+        tlsf = tlsf_create_with_pool((void*)&__heap_start__, pool_size, pool_size);
+        memmgr_heap_init();
+    }
+
+    // allocate block
+    void* data = tlsf_memalign(tlsf, xAlignment, xSize);
+    if(data == NULL) {
+        if(xSize == 0) {
+            furi_crash("malloc_aligned(0)");
+        } else {
+            furi_crash("out of memory");
+        }
+    }
+
+    // update heap usage
+    heap_used += tlsf_block_size(data);
+    heap_used += tlsf_alloc_overhead();
+    if(heap_used > heap_max_used) {
+        heap_max_used = heap_used;
+    }
+
+    memmgr_unlock();
+
+    // clear block content
+    memset(data, 0, xSize);
+
+    // trace allocation
+    memmgr_heap_trace_malloc(data, xSize);
+
+    return data;
+}
+
+extern void* pvPortRealloc(void* pv, size_t xSize) {
+    // size 0 is considered as free
+    if(xSize == 0) {
+        vPortFree(pv);
+        return NULL;
+    }
+
+    // realloc(NULL, size) is equivalent to malloc(size)
+    if(pv == NULL) {
+        return pvPortMalloc(xSize);
+    }
+
+    // realloc things //
+
+    // memory management in ISR is not allowed
+    if(FURI_IS_IRQ_MODE()) {
+        furi_crash("memmgt in ISR");
+    }
+
+    memmgr_lock();
+
+    // initialize tlsf, if not initialized
+    if(tlsf == NULL) {
+        size_t pool_size = (size_t)&__heap_end__ - (size_t)&__heap_start__;
+        tlsf = tlsf_create_with_pool((void*)&__heap_start__, pool_size, pool_size);
+        memmgr_heap_init();
+    }
+
+    // trace old block as free
+    size_t old_size = 0;
+    if(pv != NULL) {
+        old_size = tlsf_block_size(pv);
+        memmgr_heap_trace_free(pv);
+    }
+
+    // reallocate block
+    void* data = tlsf_realloc(tlsf, pv, xSize);
+    if(data == NULL) {
+        if(xSize == 0) {
+            furi_crash("realloc(0)");
+        } else {
+            furi_crash("out of memory");
+        }
+    }
+
+    // update heap usage
+    heap_used -= old_size;
+    heap_used += tlsf_block_size(data);
+    if(heap_used > heap_max_used) {
+        heap_max_used = heap_used;
+    }
+
+    // clear remain block content, if the new size is bigger
+    if(xSize > old_size) {
+        memset((uint8_t*)data + old_size, 0, xSize - old_size);
+    }
+
+    memmgr_unlock();
+
+    // trace allocation
+    memmgr_heap_trace_malloc(data, xSize);
+
+    return data;
+}
+
 size_t xPortGetFreeHeapSize(void) {
     return memmgr_get_heap_size() - heap_used - tlsf_size(tlsf);
 }