loader: add memalign() to libsa

Implement memalign(size_t alignment, size_t size) to allocate aligned memory.

diff --git a/stand/libsa/stand.h b/stand/libsa/stand.h
index 82a28b34380c346cecda1bd3241d8f3668e82a6e..c9e691b551035ec417c7870a6ebd3aced0796928 100644 (file)
--- a/stand/libsa/stand.h
+++ b/stand/libsa/stand.h
@@ -427,19 +427,23 @@ extern uint16_t           ntohs(uint16_t);
 #endif
 
 void *Malloc(size_t, const char *, int);
+void *Memalign(size_t, size_t, const char *, int);
 void *Calloc(size_t, size_t, const char *, int);
 void *Realloc(void *, size_t, const char *, int);
+void *Reallocf(void *, size_t, const char *, int);
 void Free(void *, const char *, int);
 extern void    mallocstats(void);
 
 #ifdef DEBUG_MALLOC
 #define malloc(x)      Malloc(x, __FILE__, __LINE__)
+#define memalign(x, y) Memalign(x, y, __FILE__, __LINE__)
 #define calloc(x, y)   Calloc(x, y, __FILE__, __LINE__)
 #define free(x)                Free(x, __FILE__, __LINE__)
 #define realloc(x, y)  Realloc(x, y, __FILE__, __LINE__)
 #define reallocf(x, y) Reallocf(x, y, __FILE__, __LINE__)
 #else
 #define malloc(x)      Malloc(x, NULL, 0)
+#define memalign(x, y) Memalign(x, y, NULL, 0)
 #define calloc(x, y)   Calloc(x, y, NULL, 0)
 #define free(x)                Free(x, NULL, 0)
 #define realloc(x, y)  Realloc(x, y, NULL, 0)

diff --git a/stand/libsa/zalloc.c b/stand/libsa/zalloc.c
index f359e1830aa48bea872a7d562076b348002ea22b..371a1449409bb9f1b1401f191a1ebdfda86761a2 100644 (file)
--- a/stand/libsa/zalloc.c
+++ b/stand/libsa/zalloc.c
@@ -30,6 +30,8 @@
 #include <sys/cdefs.h>
 __FBSDID("$FreeBSD$");
 
+#include <sys/param.h>
+
 /*
  * LIB/MEMORY/ZALLOC.C - self contained low-overhead memory pool/allocation
  *                       subsystem
@@ -86,7 +88,7 @@ typedef char assert_align[(sizeof(struct MemNode) <= MALLOCALIGN) ? 1 : -1];
  */
 
 void *
-znalloc(MemPool *mp, uintptr_t bytes)
+znalloc(MemPool *mp, uintptr_t bytes, size_t align)
 {
        MemNode **pmn;
        MemNode *mn;
@@ -111,14 +113,40 @@ znalloc(MemPool *mp, uintptr_t bytes)
 
        for (pmn = &mp->mp_First; (mn = *pmn) != NULL; pmn = &mn->mr_Next) {
                char *ptr = (char *)mn;
+               uintptr_t dptr;
+               char *aligned;
+               size_t extra;
+
+               dptr = (uintptr_t)(ptr + MALLOCALIGN);  /* pointer to data */
+               aligned = (char *)(roundup2(dptr, align) - MALLOCALIGN);
+               extra = aligned - ptr;
 
-               if (bytes > mn->mr_Bytes)
+               if (bytes + extra > mn->mr_Bytes)
                        continue;
 
+               /*
+                * Cut extra from head and create new memory node from reminder.
+                */
+
+               if (extra != 0) {
+                       MemNode *new;
+
+                       new = (MemNode *)aligned;
+                       new->mr_Next = mn->mr_Next;
+                       new->mr_Bytes = mn->mr_Bytes - extra;
+
+                       /* And update current memory node */
+                       mn->mr_Bytes = extra;
+                       mn->mr_Next = new;
+                       /* In next iteration, we will get our aligned address */
+                       continue;
+               }
+
                /*
                 *  Cut a chunk of memory out of the beginning of this
                 *  block and fixup the link appropriately.
                 */
+
                if (mn->mr_Bytes == bytes) {
                        *pmn = mn->mr_Next;
                } else {

diff --git a/stand/libsa/zalloc_malloc.c b/stand/libsa/zalloc_malloc.c
index da68cf3319616a03aab302ccb431d2b5fffb7cd9..17c1648e05e36bd0a4f1f87362861b0f7df1fce2 100644 (file)
--- a/stand/libsa/zalloc_malloc.c
+++ b/stand/libsa/zalloc_malloc.c
@@ -50,8 +50,26 @@ void mallocstats(void);
 #undef free
 #endif
 
+static void *Malloc_align(size_t, size_t);
+
+void *
+Malloc(size_t bytes, const char *file __unused, int line __unused)
+{
+       return (Malloc_align(bytes, 1));
+}
+
 void *
-Malloc(size_t bytes, const char *file, int line)
+Memalign(size_t alignment, size_t bytes, const char *file __unused,
+    int line __unused)
+{
+       if (alignment == 0)
+               alignment = 1;
+
+       return (Malloc_align(bytes, alignment));
+}
+
+static void *
+Malloc_align(size_t bytes, size_t alignment)
 {
        Guard *res;
 
@@ -64,7 +82,7 @@ Malloc(size_t bytes, const char *file, int line)
        bytes += MALLOCALIGN;
 #endif
 
-       while ((res = znalloc(&MallocPool, bytes)) == NULL) {
+       while ((res = znalloc(&MallocPool, bytes, alignment)) == NULL) {
                int incr = (bytes + BLKEXTENDMASK) & ~BLKEXTENDMASK;
                char *base;
 

diff --git a/stand/libsa/zalloc_protos.h b/stand/libsa/zalloc_protos.h
index aba3cb32b9b3b1224e355a9aad7b0d2fadcf1f0b..d129a64f29934c658bf03b0bcb3445c39de5c47e 100644 (file)
--- a/stand/libsa/zalloc_protos.h
+++ b/stand/libsa/zalloc_protos.h
@@ -32,7 +32,7 @@
 #ifndef _ZALLOC_PROTOS_H
 #define        _ZALLOC_PROTOS_H
 
-Library void *znalloc(struct MemPool *mpool, uintptr_t bytes);
+Library void *znalloc(struct MemPool *mpool, uintptr_t bytes, size_t align);
 Library void zfree(struct MemPool *mpool, void *ptr, uintptr_t bytes);
 Library void zextendPool(MemPool *mp, void *base, uintptr_t bytes);
 Library void zallocstats(struct MemPool *mp);