heap: added aligned alloc implementation on multi_heap layer

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);

components/heap/test/test_realloc.c

@@ -17,7 +17,7 @@ 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);
 }