From 11db1a1654a7a8dedaee5330cb8d348bf6524787 Mon Sep 17 00:00:00 2001
From: Toomas Soome <tsoome@FreeBSD.org>
Date: Tue, 17 Sep 2019 13:15:27 +0000
Subject: [PATCH] loader: add memalign() to libsa

Implement memalign(size_t alignment, size_t size) to allocate aligned memory.
---
 stand/libsa/stand.h         |  4 ++++
 stand/libsa/zalloc.c        | 32 ++++++++++++++++++++++++++++++--
 stand/libsa/zalloc_malloc.c | 22 ++++++++++++++++++++--
 stand/libsa/zalloc_protos.h |  2 +-
 4 files changed, 55 insertions(+), 5 deletions(-)

diff --git a/stand/libsa/stand.h b/stand/libsa/stand.h
index 82a28b34380c..c9e691b55103 100644
--- 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 f359e1830aa4..371a1449409b 100644
--- 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;
 
-		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 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 da68cf331961..17c1648e05e3 100644
--- 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, int line)
+Malloc(size_t bytes, const char *file __unused, int line __unused)
+{
+	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;
 
@@ -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 aba3cb32b9b3..d129a64f2993 100644
--- 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);