From dfe296c43a26175f98ded41168e5c106edd52599 Mon Sep 17 00:00:00 2001
From: Konstantin Belousov <kib@FreeBSD.org>
Date: Fri, 6 Dec 2013 21:30:31 +0000
Subject: [PATCH] Build an allocator for the aligned memory on top of the
 rtld-private malloc.

Reviewed by:	kan
Sponsored by:	The FreeBSD Foundation
MFC after:	1 week
---
 libexec/rtld-elf/rtld.h    |  2 ++
 libexec/rtld-elf/xmalloc.c | 30 ++++++++++++++++++++++++++++++
 2 files changed, 32 insertions(+)

diff --git a/libexec/rtld-elf/rtld.h b/libexec/rtld-elf/rtld.h
index 083f5a4f673e..d186dcc18dcd 100644
--- a/libexec/rtld-elf/rtld.h
+++ b/libexec/rtld-elf/rtld.h
@@ -352,6 +352,8 @@ Obj_Entry *map_object(int, const char *, const struct stat *);
 void *xcalloc(size_t, size_t);
 void *xmalloc(size_t);
 char *xstrdup(const char *);
+void *malloc_aligned(size_t size, size_t align);
+void free_aligned(void *ptr);
 extern Elf_Addr _GLOBAL_OFFSET_TABLE_[];
 extern Elf_Sym sym_zero;	/* For resolving undefined weak refs. */
 
diff --git a/libexec/rtld-elf/xmalloc.c b/libexec/rtld-elf/xmalloc.c
index 178f49bda0df..3b3408c4e628 100644
--- a/libexec/rtld-elf/xmalloc.c
+++ b/libexec/rtld-elf/xmalloc.c
@@ -67,3 +67,33 @@ xstrdup(const char *str)
 	memcpy(copy, str, len);
 	return (copy);
 }
+
+void *
+malloc_aligned(size_t size, size_t align)
+{
+	void *mem, *res;
+	uintptr_t x;
+	size_t asize, r;
+
+	r = round(sizeof(void *), align);
+	asize = round(size, align) + r;
+	mem = xmalloc(asize);
+	x = (uintptr_t)mem;
+	res = (void *)round(x, align);
+	*(void **)((uintptr_t)res - sizeof(void *)) = mem;
+	return (res);
+}
+
+void
+free_aligned(void *ptr)
+{
+	void *mem;
+	uintptr_t x;
+
+	if (ptr == NULL)
+		return;
+	x = (uintptr_t)ptr;
+	x -= sizeof(void *);
+	mem = *(void **)x;
+	free(mem);
+}