Add alignment support to __libc_allocate_tls().

For statically linked binaries, where all relocation are solved by static
linker, the linker expect that offset to TLS section is aligned. Additionaly,
to maintain absolute alignment, TLS TCB should by also aligned.

Obtained from:	CheriBSD (initial version)
MFC after:	1 month
Reviewed by:	brooks (previous version), kib
Differential Revision:	https://reviews.freebsd.org/D12907

lib/libc/gen/tls.c

@@ -37,9 +37,15 @@
 #include <stdlib.h>
 #include <string.h>
 #include <elf.h>
+#include <unistd.h>
 
 #include "libc_private.h"
 
+#define	tls_assert(cond)	((cond) ? (void) 0 :			\
+    (tls_msg(#cond ": assert failed: " __FILE__ ":"			\
+      __XSTRING(__LINE__) "\n"), abort()))
+#define	tls_msg(s)		write(STDOUT_FILENO, s, strlen(s))
+
 /* Provided by jemalloc to avoid bootstrapping issues. */
 void	*__je_bootstrap_malloc(size_t size);
 void	*__je_bootstrap_calloc(size_t num, size_t size);
@@ -85,6 +91,7 @@ void __libc_free_tls(void *tls, size_t tcbsize, size_t tcbalign);
 
 static size_t tls_static_space;
 static size_t tls_init_size;
+static size_t tls_init_align;
 static void *tls_init;
 #endif
 
@@ -109,6 +116,35 @@ __libc_tls_get_addr(void *ti __unused)
 
 #ifndef PIC
 
+static void *
+malloc_aligned(size_t size, size_t align)
+{
+	void *mem, *res;
+
+	if (align < sizeof(void *))
+		align = sizeof(void *);
+
+	mem = __je_bootstrap_malloc(size + sizeof(void *) + align - 1);
+	res = (void *)roundup2((uintptr_t)mem + sizeof(void *), align);
+	*(void **)((uintptr_t)res - sizeof(void *)) = mem;
+	return (res);
+}
+
+static void
+free_aligned(void *ptr)
+{
+	void *mem;
+	uintptr_t x;
+
+	if (ptr == NULL)
+		return;
+
+	x = (uintptr_t)ptr;
+	x -= sizeof(void *);
+	mem = *(void **)x;
+	__je_bootstrap_free(mem);
+}
+
 #ifdef TLS_VARIANT_I
 
 #define	TLS_TCB_SIZE	(2 * sizeof(void *))
@@ -117,52 +153,63 @@ __libc_tls_get_addr(void *ti __unused)
  * Free Static TLS using the Variant I method.
  */
 void
-__libc_free_tls(void *tcb, size_t tcbsize, size_t tcbalign __unused)
+__libc_free_tls(void *tcb, size_t tcbsize __unused, size_t tcbalign __unused)
 {
 	Elf_Addr *dtv;
 	Elf_Addr **tls;
 
-	tls = (Elf_Addr **)((Elf_Addr)tcb + tcbsize - TLS_TCB_SIZE);
+	tls = (Elf_Addr **)tcb;
 	dtv = tls[0];
 	__je_bootstrap_free(dtv);
-	__je_bootstrap_free(tcb);
+	free_aligned(tls);
 }
 
 /*
  * Allocate Static TLS using the Variant I method.
  */
 void *
-__libc_allocate_tls(void *oldtcb, size_t tcbsize, size_t tcbalign __unused)
+__libc_allocate_tls(void *oldtcb, size_t tcbsize, size_t tcbalign)
 {
 	Elf_Addr *dtv;
 	Elf_Addr **tls;
-	char *tcb;
 
 	if (oldtcb != NULL && tcbsize == TLS_TCB_SIZE)
 		return (oldtcb);
 
-	tcb = __je_bootstrap_calloc(1, tls_static_space + tcbsize - TLS_TCB_SIZE);
-	tls = (Elf_Addr **)(tcb + tcbsize - TLS_TCB_SIZE);
+	tls_assert(tcbalign >= TLS_TCB_ALIGN);
+	tls_assert(tcbsize == TLS_TCB_SIZE);
+
+	tcbsize = roundup2(tcbsize, tcbalign);
+	tls = malloc_aligned(tcbsize + tls_static_space, tcbalign);
+	if (tls == NULL) {
+		tls_msg("__libc_allocate_tls: Out of memory.\n");
+		abort();
+	}
+	memset(tls, 0, tcbsize + tls_static_space);
 
 	if (oldtcb != NULL) {
-		memcpy(tls, oldtcb, tls_static_space);
+		memcpy(tls, oldtcb, tcbsize + tls_static_space);
 		__je_bootstrap_free(oldtcb);
 
 		/* Adjust the DTV. */
 		dtv = tls[0];
-		dtv[2] = (Elf_Addr)tls + TLS_TCB_SIZE;
+		dtv[2] = (Elf_Addr)tls + tcbsize;
 	} else {
 		dtv = __je_bootstrap_malloc(3 * sizeof(Elf_Addr));
+		if (dtv == NULL) {
+			tls_msg("__libc_allocate_tls: Out of memory.\n");
+			abort();
+		}
 		tls[0] = dtv;
-		dtv[0] = 1;
-		dtv[1] = 1;
-		dtv[2] = (Elf_Addr)tls + TLS_TCB_SIZE;
+		dtv[0] = 1;		/* Generation. */
+		dtv[1] = 1;		/* Segments count. */
+		dtv[2] = (Elf_Addr)tls + tcbsize;
 
 		if (tls_init_size > 0)
 			memcpy((void*)dtv[2], tls_init, tls_init_size);
 	}
 
-	return(tcb); 
+	return (tls);
 }
 
 #endif
@@ -190,7 +237,7 @@ __libc_free_tls(void *tcb, size_t tcbsize __unused, size_t tcbalign)
 	dtv = ((Elf_Addr**)tcb)[1];
 	tlsend = (Elf_Addr) tcb;
 	tlsstart = tlsend - size;
-	__je_bootstrap_free((void*) tlsstart);
+	free_aligned((void*)tlsstart);
 	__je_bootstrap_free(dtv);
 }
 
@@ -209,8 +256,17 @@ __libc_allocate_tls(void *oldtls, size_t tcbsize, size_t tcbalign)
 
 	if (tcbsize < 2 * sizeof(Elf_Addr))
 		tcbsize = 2 * sizeof(Elf_Addr);
-	tls = __je_bootstrap_calloc(1, size + tcbsize);
+	tls = malloc_aligned(size + tcbsize, tcbalign);
+	if (tls == NULL) {
+		tls_msg("__libc_allocate_tls: Out of memory.\n");
+		abort();
+	}
+	memset(tls, 0, size + tcbsize);
 	dtv = __je_bootstrap_malloc(3 * sizeof(Elf_Addr));
+	if (dtv == NULL) {
+		tls_msg("__libc_allocate_tls: Out of memory.\n");
+		abort();
+	}
 
 	segbase = (Elf_Addr)(tls + size);
 	((Elf_Addr*)segbase)[0] = segbase;
@@ -305,18 +361,14 @@ _init_tls(void)
 			tls_static_space = roundup2(phdr[i].p_memsz,
 			    phdr[i].p_align);
 			tls_init_size = phdr[i].p_filesz;
+			tls_init_align = phdr[i].p_align;
 			tls_init = (void*) phdr[i].p_vaddr;
+			break;
 		}
 	}
 
-#ifdef TLS_VARIANT_I
-	/*
-	 * tls_static_space should include space for TLS structure
-	 */
-	tls_static_space += TLS_TCB_SIZE;
-#endif
-
-	tls = _rtld_allocate_tls(NULL, TLS_TCB_SIZE, TLS_TCB_ALIGN);
+	tls = _rtld_allocate_tls(NULL, TLS_TCB_SIZE,
+	    MAX(TLS_TCB_ALIGN, tls_init_align));
 
 	_set_tp(tls);
 #endif