Reintroduce memalign() and friends

memalign() and friends where removed with the
commit 8cd8a6296974 ("Remove memalign()").

At the time memalign() was unused and a bit buggy. This new memalign()
is believed to work correctly due to extensive testing.  Recently
memalign() has been needed by certain drivers so it makes sense to add
it again.

Acked-by: Etienne Carriere <etienne.carriere@linaro.org>
Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>

3 files changed, 159 insertions, 11 deletions

diff --git a/core/pta/tests/misc.c b/core/pta/tests/misc.c
index a5065088a..cd9e5ee7b 100644
--- a/core/pta/tests/misc.c
+++ b/core/pta/tests/misc.c
@@ -312,7 +312,7 @@ static int self_test_malloc(void)
         bool r;
         int ret = 0;
 
-        LOG("malloc tests (malloc, free, calloc, realloc):");
+        LOG("malloc tests:");
         LOG("  p1=%p  p2=%p  p3=%p  p4=%p",
             (void *)p1, (void *)p2, (void *)p3, (void *)p4);
         /* test malloc */
@@ -373,6 +373,47 @@ static int self_test_malloc(void)
         p3 = NULL;
         p4 = NULL;
 
+        /* test memalign */
+        p3 = memalign(0x1000, 1024);
+        LOG("- p3 = memalign(%d, 1024)", 0x1000);
+        p1 = malloc(1024);
+        LOG("- p1 = malloc(1024)");
+        p4 = memalign(0x100, 512);
+        LOG("- p4 = memalign(%d, 512)", 0x100);
+        LOG("  p1=%p  p2=%p  p3=%p  p4=%p",
+            (void *)p1, (void *)p2, (void *)p3, (void *)p4);
+        r = (p1 && p3 && p4 &&
+            !((vaddr_t)p3 % 0x1000) && !((vaddr_t)p4 % 0x100));
+        if (!r)
+                ret = -1;
+        LOG("  => test %s", r ? "ok" : "FAILED");
+        LOG("");
+        LOG("- free p1, p3, p4");
+        free(p1);
+        free(p3);
+        free(p4);
+        p1 = NULL;
+        p3 = NULL;
+        p4 = NULL;
+
+        /* test memalign with invalid alignments */
+        p3 = memalign(100, 1024);
+        LOG("- p3 = memalign(%d, 1024)", 100);
+        p4 = memalign(0, 1024);
+        LOG("- p4 = memalign(%d, 1024)", 0);
+        LOG("  p1=%p  p2=%p  p3=%p  p4=%p",
+            (void *)p1, (void *)p2, (void *)p3, (void *)p4);
+        r = (!p3 && !p4);
+        if (!r)
+                ret = -1;
+        LOG("  => test %s", r ? "ok" : "FAILED");
+        LOG("");
+        LOG("- free p3, p4");
+        free(p3);
+        free(p4);
+        p3 = NULL;
+        p4 = NULL;
+
         /* test free(NULL) */
         LOG("- free NULL");
         free(NULL);
@@ -391,7 +432,7 @@ static int self_test_nex_malloc(void)
         bool r;
         int ret = 0;
 
-        LOG("nex_malloc tests (nex_malloc, nex_free, nex_calloc, nex_realloc):");
+        LOG("nex_malloc tests:");
         LOG("  p1=%p  p2=%p  p3=%p  p4=%p",
             (void *)p1, (void *)p2, (void *)p3, (void *)p4);
         /* test malloc */
@@ -452,6 +493,47 @@ static int self_test_nex_malloc(void)
         p3 = NULL;
         p4 = NULL;
 
+        /* test memalign */
+        p3 = nex_memalign(0x1000, 1024);
+        LOG("- p3 = nex_memalign(%d, 1024)", 0x1000);
+        p1 = nex_malloc(1024);
+        LOG("- p1 = nex_malloc(1024)");
+        p4 = nex_memalign(0x100, 512);
+        LOG("- p4 = nex_memalign(%d, 512)", 0x100);
+        LOG("  p1=%p  p2=%p  p3=%p  p4=%p",
+            (void *)p1, (void *)p2, (void *)p3, (void *)p4);
+        r = (p1 && p3 && p4 &&
+            !((vaddr_t)p3 % 0x1000) && !((vaddr_t)p4 % 0x100));
+        if (!r)
+                ret = -1;
+        LOG("  => test %s", r ? "ok" : "FAILED");
+        LOG("");
+        LOG("- nex_free p1, p3, p4");
+        nex_free(p1);
+        nex_free(p3);
+        nex_free(p4);
+        p1 = NULL;
+        p3 = NULL;
+        p4 = NULL;
+
+        /* test memalign with invalid alignments */
+        p3 = nex_memalign(100, 1024);
+        LOG("- p3 = nex_memalign(%d, 1024)", 100);
+        p4 = nex_memalign(0, 1024);
+        LOG("- p4 = nex_memalign(%d, 1024)", 0);
+        LOG("  p1=%p  p2=%p  p3=%p  p4=%p",
+            (void *)p1, (void *)p2, (void *)p3, (void *)p4);
+        r = (!p3 && !p4);
+        if (!r)
+                ret = -1;
+        LOG("  => test %s", r ? "ok" : "FAILED");
+        LOG("");
+        LOG("- nex_free p3, p4");
+        nex_free(p3);
+        nex_free(p4);
+        p3 = NULL;
+        p4 = NULL;
+
         /* test free(NULL) */
         LOG("- nex_free NULL");
         nex_free(NULL);
diff --git a/lib/libutils/isoc/bget_malloc.c b/lib/libutils/isoc/bget_malloc.c
index ced39ea2d..8aeffb7f2 100644
--- a/lib/libutils/isoc/bget_malloc.c
+++ b/lib/libutils/isoc/bget_malloc.c
@@ -346,18 +346,14 @@ static bool bpool_foreach(struct malloc_ctx *ctx,
         for (bpool_foreach_iterator_init((ctx),(iterator));   \
              bpool_foreach((ctx),(iterator), (bp));)
 
-static void *raw_malloc(size_t hdr_size, size_t ftr_size, size_t pl_size,
-                        struct malloc_ctx *ctx)
+static void *raw_memalign(size_t hdr_size, size_t ftr_size, size_t alignment,
+                          size_t pl_size, struct malloc_ctx *ctx)
 {
         void *ptr = NULL;
         bufsize s;
 
-        /*
-         * Make sure that malloc has correct alignment of returned buffers.
-         * The assumption is that uintptr_t will be as wide as the largest
-         * required alignment of any type.
-         */
-        COMPILE_TIME_ASSERT(SizeQuant >= sizeof(uintptr_t));
+        if (!alignment || !IS_POWER_OF_TWO(alignment))
+                return NULL;
 
         raw_malloc_validate_pools(ctx);
 
@@ -369,13 +365,23 @@ static void *raw_malloc(size_t hdr_size, size_t ftr_size, size_t pl_size,
         if (!s)
                 s++;
 
-        ptr = bget(0, hdr_size, s, &ctx->poolset);
+        ptr = bget(alignment, hdr_size, s, &ctx->poolset);
 out:
         raw_malloc_return_hook(ptr, pl_size, ctx);
 
         return ptr;
 }
 
+static void *raw_malloc(size_t hdr_size, size_t ftr_size, size_t pl_size,
+                        struct malloc_ctx *ctx)
+{
+        /*
+         * Note that we're feeding SizeQ as alignment, this is the smallest
+         * alignment that bget() can use.
+         */
+        return raw_memalign(hdr_size, ftr_size, SizeQ, pl_size, ctx);
+}
+
 static void raw_free(void *ptr, struct malloc_ctx *ctx, bool wipe)
 {
         raw_malloc_validate_pools(ctx);
@@ -600,6 +606,23 @@ static void *gen_mdbg_realloc(struct malloc_ctx *ctx, const char *fname,
 #define realloc_unlocked(ctx, ptr, size)                                        \
                 gen_mdbg_realloc_unlocked(ctx, __FILE__, __LINE__, (ptr), (size))
 
+static void *gen_mdbg_memalign(struct malloc_ctx *ctx, const char *fname,
+                               int lineno, size_t alignment, size_t size)
+{
+        struct mdbg_hdr *hdr;
+        uint32_t exceptions = malloc_lock(ctx);
+
+        hdr = raw_memalign(sizeof(struct mdbg_hdr), mdbg_get_ftr_size(size),
+                           alignment, size, ctx);
+        if (hdr) {
+                mdbg_update_hdr(hdr, fname, lineno, size);
+                hdr++;
+        }
+        malloc_unlock(ctx, exceptions);
+        return hdr;
+}
+
+
 static void *get_payload_start_size(void *raw_buf, size_t *size)
 {
         struct mdbg_hdr *hdr = raw_buf;
@@ -651,6 +674,12 @@ void *mdbg_realloc(const char *fname, int lineno, void *ptr, size_t size)
         return gen_mdbg_realloc(&malloc_ctx, fname, lineno, ptr, size);
 }
 
+void *mdbg_memalign(const char *fname, int lineno, size_t alignment,
+                    size_t size)
+{
+        return gen_mdbg_memalign(&malloc_ctx, fname, lineno, alignment, size);
+}
+
 void mdbg_check(int bufdump)
 {
         gen_mdbg_check(&malloc_ctx, bufdump);
@@ -701,6 +730,16 @@ void *realloc(void *ptr, size_t size)
         return p;
 }
 
+void *memalign(size_t alignment, size_t size)
+{
+        void *p;
+        uint32_t exceptions = malloc_lock(&malloc_ctx);
+
+        p = raw_memalign(0, 0, alignment, size, &malloc_ctx);
+        malloc_unlock(&malloc_ctx, exceptions);
+        return p;
+}
+
 static void *get_payload_start_size(void *ptr, size_t *size)
 {
         *size = bget_buf_size(ptr);
@@ -873,6 +912,16 @@ void *nex_realloc(void *ptr, size_t size)
         return p;
 }
 
+void *nex_memalign(size_t alignment, size_t size)
+{
+        void *p;
+        uint32_t exceptions = malloc_lock(&nex_malloc_ctx);
+
+        p = raw_memalign(0, 0, alignment, size, &nex_malloc_ctx);
+        malloc_unlock(&nex_malloc_ctx, exceptions);
+        return p;
+}
+
 void nex_free(void *ptr)
 {
         uint32_t exceptions = malloc_lock(&nex_malloc_ctx);
@@ -898,6 +947,12 @@ void *nex_mdbg_realloc(const char *fname, int lineno, void *ptr, size_t size)
         return gen_mdbg_realloc(&nex_malloc_ctx, fname, lineno, ptr, size);
 }
 
+void *nex_mdbg_memalign(const char *fname, int lineno, size_t alignment,
+                size_t size)
+{
+        return gen_mdbg_memalign(&nex_malloc_ctx, fname, lineno, alignment, size);
+}
+
 void nex_mdbg_check(int bufdump)
 {
         gen_mdbg_check(&nex_malloc_ctx, bufdump);
diff --git a/lib/libutils/isoc/include/malloc.h b/lib/libutils/isoc/include/malloc.h
index 992d2282c..36547ff7c 100644
--- a/lib/libutils/isoc/include/malloc.h
+++ b/lib/libutils/isoc/include/malloc.h
@@ -15,6 +15,8 @@ void free(void *ptr);
 void *mdbg_malloc(const char *fname, int lineno, size_t size);
 void *mdbg_calloc(const char *fname, int lineno, size_t nmemb, size_t size);
 void *mdbg_realloc(const char *fname, int lineno, void *ptr, size_t size);
+void *mdbg_memalign(const char *fname, int lineno, size_t alignment,
+                    size_t size);
 
 void mdbg_check(int bufdump);
 
@@ -23,12 +25,15 @@ void mdbg_check(int bufdump);
                 mdbg_calloc(__FILE__, __LINE__, (nmemb), (size))
 #define realloc(ptr, size) \
                 mdbg_realloc(__FILE__, __LINE__, (ptr), (size))
+#define memalign(alignment, size) \
+                mdbg_memalign(__FILE__, __LINE__, (alignment), (size))
 
 #else
 
 void *malloc(size_t size);
 void *calloc(size_t nmemb, size_t size);
 void *realloc(void *ptr, size_t size);
+void *memalign(size_t alignment, size_t size);
 
 #define mdbg_check(x)        do { } while (0)
 
@@ -86,6 +91,8 @@ void nex_free(void *ptr);
 void *nex_mdbg_malloc(const char *fname, int lineno, size_t size);
 void *nex_mdbg_calloc(const char *fname, int lineno, size_t nmemb, size_t size);
 void *nex_mdbg_realloc(const char *fname, int lineno, void *ptr, size_t size);
+void *nex_mdbg_memalign(const char *fname, int lineno, size_t alignment,
+                        size_t size);
 
 void nex_mdbg_check(int bufdump);
 
@@ -94,12 +101,15 @@ void nex_mdbg_check(int bufdump);
                 nex_mdbg_calloc(__FILE__, __LINE__, (nmemb), (size))
 #define nex_realloc(ptr, size) \
                 nex_mdbg_realloc(__FILE__, __LINE__, (ptr), (size))
+#define nex_memalign(alignment, size) \
+                nex_mdbg_memalign(__FILE__, __LINE__, (alignment), (size))
 
 #else /* ENABLE_MDBG */
 
 void *nex_malloc(size_t size);
 void *nex_calloc(size_t nmemb, size_t size);
 void *nex_realloc(void *ptr, size_t size);
+void *nex_memalign(size_t alignment, size_t size);
 
 #define nex_mdbg_check(x)        do { } while (0)
 
@@ -124,6 +134,7 @@ void nex_malloc_reset_stats(void);
 #define nex_malloc(size) malloc(size)
 #define nex_calloc(nmemb, size) calloc(nmemb, size)
 #define nex_realloc(ptr, size) realloc(ptr, size)
+#define nex_memalign(alignment, size) memalign(alignment, size)
 
 #endif  /* CFG_VIRTUALIZATION */