From 98e561b79a1249b87727c1a88b4fd5028372479c Mon Sep 17 00:00:00 2001
From: Felipe Neves <felipe.neves@espressif.com>
Date: Fri, 28 Feb 2020 13:17:34 -0300
Subject: [PATCH] heap: added aligned alloc implementation on multi_heap layer

---
 components/heap/multi_heap.c        | 58 +++++++++++++++++++++++++++++
 components/heap/test/test_realloc.c |  4 +-
 2 files changed, 60 insertions(+), 2 deletions(-)

diff --git a/components/heap/multi_heap.c b/components/heap/multi_heap.c
index d131ff269..9a860d4ec 100644
--- a/components/heap/multi_heap.c
+++ b/components/heap/multi_heap.c
@@ -33,9 +33,15 @@
 void *multi_heap_malloc(multi_heap_handle_t heap, size_t size)
     __attribute__((alias("multi_heap_malloc_impl")));
 
+void *multi_heap_aligned_alloc(multi_heap_handle_t heap, size_t size, size_t alignment)
+    __attribute__((alias("multi_heap_aligned_alloc_impl")));
+
 void multi_heap_free(multi_heap_handle_t heap, void *p)
     __attribute__((alias("multi_heap_free_impl")));
 
+void multi_heap_aligned_free(multi_heap_handle_t heap, void *p)
+    __attribute__((alias("multi_heap_aligned_free_impl")));
+
 void *multi_heap_realloc(multi_heap_handle_t heap, void *p, size_t size)
     __attribute__((alias("multi_heap_realloc_impl")));
 
@@ -66,6 +72,7 @@ void *multi_heap_get_block_owner(multi_heap_block_handle_t block)
 
 #define ALIGN(X) ((X) & ~(sizeof(void *)-1))
 #define ALIGN_UP(X) ALIGN((X)+sizeof(void *)-1)
+#define ALIGN_UP_BY(num, align) (((num) + ((align) - 1)) & ~((align) - 1))
 
 struct heap_block;
 
@@ -463,6 +470,57 @@ void *multi_heap_malloc_impl(multi_heap_handle_t heap, size_t size)
     return best_block->data;
 }
 
+void *multi_heap_aligned_alloc_impl(multi_heap_handle_t heap, size_t size, size_t alignment)
+{
+    if(heap == NULL) {
+        return NULL;
+    }
+
+    if(!size) {
+        return NULL;
+    }
+
+    if(!alignment) {
+        return NULL;
+    }
+
+    //Alignment must be a power of two...
+    if((alignment & (alignment - 1)) != 0) {
+        return NULL;
+    }
+
+    uint32_t overhead = (sizeof(uint32_t) + (alignment - 1));
+
+    multi_heap_internal_lock(heap);
+    void *head = multi_heap_malloc_impl(heap, size + overhead);
+    if(head == NULL) {
+        multi_heap_internal_unlock(heap);
+        return NULL;
+    }
+
+    //Lets align our new obtained block address:
+    //and save information to recover original block pointer
+    //to allow us to deallocate the memory when needed
+    void *ptr = (void *)ALIGN_UP_BY((uintptr_t)head + sizeof(uint32_t), alignment);
+    *((uint32_t *)ptr - 1) = (uint32_t)((uintptr_t)ptr - (uintptr_t)head);
+
+    multi_heap_internal_unlock(heap);
+    return ptr;
+}
+
+void multi_heap_aligned_free_impl(multi_heap_handle_t heap, void *p)
+{
+    if(p == NULL) {
+        return;
+    }
+
+    multi_heap_internal_lock(heap);
+    uint32_t offset = *((uint32_t *)p - 1);
+    void *block_head = (void *)((uint8_t *)p - offset);
+    multi_heap_free_impl(heap, block_head);
+    multi_heap_internal_unlock(heap);
+}
+
 void multi_heap_free_impl(multi_heap_handle_t heap, void *p)
 {
     heap_block_t *pb = get_block(p);
diff --git a/components/heap/test/test_realloc.c b/components/heap/test/test_realloc.c
index 655666fe2..c4c2f5e10 100644
--- a/components/heap/test/test_realloc.c
+++ b/components/heap/test/test_realloc.c
@@ -14,10 +14,10 @@
 /* (can't realloc in place if comprehensive is enabled) */
 
 TEST_CASE("realloc shrink buffer in place", "[heap]")
-{
+{    
     void *x = malloc(64);
     TEST_ASSERT(x);
-    void *y = realloc(p, 48);
+    void *y = realloc(x, 48);
     TEST_ASSERT_EQUAL_PTR(x, y);
 }