11716 loader: add memalign()

Reviewed by: Jason King <jason.king@joyent.com>
Approved by: Robert Mustacchi <rm@fingolfin.org>

4 files changed, 57 insertions, 12 deletions

diff --git a/usr/src/boot/lib/libstand/stand.h b/usr/src/boot/lib/libstand/stand.h
index 1fa3055844..9406d4f296 100644
--- a/usr/src/boot/lib/libstand/stand.h
+++ b/usr/src/boot/lib/libstand/stand.h
@@ -252,12 +252,6 @@ static __inline int tolower(int c)
 extern void	setheap(void *base, void *top);
 extern char	*sbrk(int incr);
 
-/* Matt Dillon's zalloc/zmalloc */
-extern void	*malloc(size_t bytes);
-extern void	free(void *ptr);
-extern void	*calloc(size_t n1, size_t n2);
-extern void	*realloc(void *ptr, size_t size);
-extern void	*reallocf(void *ptr, size_t size);
 extern void	mallocstats(void);
 
 extern int	printf(const char *fmt, ...) __printflike(1, 2);
@@ -416,20 +410,26 @@ 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);
 
-#if 1
+#if 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)
+#define	reallocf(x, y)	Reallocf(x, y, NULL, 0)
 #endif
 
 #endif	/* STAND_H */
diff --git a/usr/src/boot/lib/libstand/zalloc.c b/usr/src/boot/lib/libstand/zalloc.c
index 546723617d..8ecdc2fdf1 100644
--- a/usr/src/boot/lib/libstand/zalloc.c
+++ b/usr/src/boot/lib/libstand/zalloc.c
@@ -28,6 +28,7 @@
  */
 
 #include <sys/cdefs.h>
+#include <sys/param.h>
 
 /*
  * LIB/MEMORY/ZALLOC.C	- self contained low-overhead memory pool/allocation
@@ -85,7 +86,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;
@@ -110,14 +111,41 @@ 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;
 
-		if (bytes > mn->mr_Bytes)
+		dptr = (uintptr_t)(ptr + MALLOCALIGN);	/* pointer to data */
+		aligned = (char *)(roundup2(dptr, align) - MALLOCALIGN);
+		extra = aligned - ptr;
+
+		if (bytes + extra > mn->mr_Bytes)
+			continue;
+
+		/*
+		 * Cut extra from head and create new memory node from
+		 * remainder.
+		 */
+
+		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/usr/src/boot/lib/libstand/zalloc_malloc.c b/usr/src/boot/lib/libstand/zalloc_malloc.c
index 2117804c5a..0551b8c945 100644
--- a/usr/src/boot/lib/libstand/zalloc_malloc.c
+++ b/usr/src/boot/lib/libstand/zalloc_malloc.c
@@ -49,9 +49,27 @@ void mallocstats(void);
 #undef free
 #endif
 
+static void *Malloc_align(size_t, size_t);
+
 void *
 Malloc(size_t bytes, const char *file, int line)
 {
+	return (Malloc_align(bytes, 1));
+}
+
+void *
+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;
 
 #ifdef USEENDGUARD
@@ -60,7 +78,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;
 
@@ -125,7 +143,6 @@ Free(void *ptr, const char *file, int line)
 	}
 }
 
-
 void *
 Calloc(size_t n1, size_t n2, const char *file, int line)
 {
diff --git a/usr/src/boot/lib/libstand/zalloc_protos.h b/usr/src/boot/lib/libstand/zalloc_protos.h
index 0df9c91795..71f29c5fd3 100644
--- a/usr/src/boot/lib/libstand/zalloc_protos.h
+++ b/usr/src/boot/lib/libstand/zalloc_protos.h
@@ -30,7 +30,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);