lib/heap: Implement posix_memalign

Bug: 230134581
Change-Id: If0d543f1372fa4423a88ea6ded9d7a1373347762

2 files changed, 32 insertions, 0 deletions

diff --git a/lib/heap/heap_wrapper.c b/lib/heap/heap_wrapper.c
index 181dd6f5..ea012191 100644
--- a/lib/heap/heap_wrapper.c
+++ b/lib/heap/heap_wrapper.c
@@ -131,6 +131,25 @@ static inline void HEAP_TRIM(void) { dlmalloc_trim(0); }
 #error need to select valid heap implementation or provide wrapper
 #endif
 
+static inline int HEAP_POSIX_MEMALIGN(void **res, size_t align, size_t size)
+{
+    if (align < sizeof(void *)) {
+        LTRACEF("Invalid alignment requested\n");
+        return ERR_INVALID_ARGS;
+    }
+    if (res == NULL) {
+        LTRACEF("Invalid result pointer\n");
+        return ERR_INVALID_ARGS;
+    }
+    void *mem = HEAP_MEMALIGN(align, size);
+    if (mem == NULL) {
+        LTRACEF("Insufficient memory\n");
+        return ERR_NO_MEMORY;
+    }
+    *res = mem;
+    return NO_ERROR;
+}
+
 void heap_init(void)
 {
     HEAP_INIT();
@@ -161,6 +180,18 @@ void *memalign(size_t boundary, size_t size)
     return ptr;
 }
 
+int posix_memalign(void **res, size_t align, size_t size)
+{
+    LTRACEF("res %p, align %zu, size %zd\n", res, align, size);
+
+    int rc = HEAP_POSIX_MEMALIGN(res, align, size);
+    if (heap_trace) {
+        printf("caller %p posix_memalign %p, %zu, %zu -> %d\n", __GET_CALLER(),
+               res, align, size, rc);
+    }
+    return rc;
+}
+
 void *calloc(size_t count, size_t size)
 {
     LTRACEF("count %zu, size %zd\n", count, size);
diff --git a/lib/heap/include/lib/heap.h b/lib/heap/include/lib/heap.h
index 7f5cde7f..d899f2b8 100644
--- a/lib/heap/include/lib/heap.h
+++ b/lib/heap/include/lib/heap.h
@@ -31,6 +31,7 @@ __BEGIN_CDECLS;
 /* standard heap definitions */
 void *malloc(size_t size) __MALLOC __WARN_UNUSED_RESULT;
 void *memalign(size_t boundary, size_t size) __MALLOC __WARN_UNUSED_RESULT;
+int posix_memalign(void** res, size_t align, size_t size);
 void *calloc(size_t count, size_t size) __WARN_UNUSED_RESULT;
 void *realloc(void *ptr, size_t size) __WARN_UNUSED_RESULT;
 void free(void *ptr);