Index: head/lib/libc/gen/tls.c
===================================================================
--- head/lib/libc/gen/tls.c
+++ head/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 @@
 
 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 @@
 
 #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 @@
  * 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 @@
 	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 @@
 
 	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 @@
 			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