1 files changed, 225 insertions, 2 deletions

diff --git a/lib/libc/gen/tls.c b/lib/libc/gen/tls.c
index 1c4f8ce..59e44b7 100644
--- a/lib/libc/gen/tls.c
+++ b/lib/libc/gen/tls.c
@@ -32,7 +32,35 @@
  * runtime from ld-elf.so.1.
  */
 
-#include <stddef.h>
+#include <stdlib.h>
+#include <string.h>
+#include <elf.h>
+#include <assert.h>
+#include "libc_private.h"
+
+/* XXX not sure what variants to use for arm. */
+
+#if defined(__ia64__) || defined(__alpha__) || defined(__powerpc__)
+#define TLS_VARIANT_I
+#endif
+#if defined(__i386__) || defined(__amd64__) || defined(__sparc64__)
+#define TLS_VARIANT_II
+#endif
+
+#ifndef PIC
+
+#define round(size, align) \
+	(((size) + (align) - 1) & ~((align) - 1))
+
+static size_t tls_static_space;
+static size_t tls_init_size;
+#ifdef TLS_VARIANT_I
+static size_t tls_init_offset;
+#endif
+static void *tls_init;
+
+void _rtld_free_tls(void *tcb, size_t tcbsize, size_t tcbalign);
+#endif
 
 #ifdef __i386__
 
@@ -55,15 +83,210 @@ __tls_get_addr()
 	return (0);
 }
 
-#pragma weak _rtld_allocate_tls
+#ifdef TLS_VARIANT_I
+
+void
+_rtld_free_tls(void *tls, size_t tcbsize, size_t tcbalign)
+{
+#ifndef PIC
+	Elf_Addr* dtv;
+
+	dtv = ((Elf_Addr**)tls)[0];
+	free(tls);
+	free(dtv);
+#endif
+}
+
+/*
+ * Allocate Static TLS using the Variant I method.
+ */
 void *
 _rtld_allocate_tls(void *oldtls, size_t tcbsize, size_t tcbalign)
 {
+#ifndef PIC
+	size_t size;
+	char *tls;
+	Elf_Addr *dtv;
+
+	size = tls_static_space;
+
+	tls = malloc(size);
+	dtv = malloc(3 * sizeof(Elf_Addr));
+
+	*(Elf_Addr**) tls = dtv;
+
+	dtv[0] = 1;
+	dtv[1] = 1;
+	dtv[2] = (Elf_Addr)(tls + tls_init_offset);
+	if (oldtls) {
+		/*
+		 * Copy the static TLS block over whole.
+		 */
+		memcpy(tls + tls_init_offset,
+		    (char*) oldtls + tls_init_offset,
+		    tls_static_space - tls_init_offset);
+
+		/*
+		 * We assume that this block was the one we created with
+		 * allocate_initial_tls().
+		 */
+		_rtld_free_tls(oldtls, 2*sizeof(Elf_Addr), sizeof(Elf_Addr));
+	} else {
+		memcpy(tls + tls_init_offset, tls_init, tls_init_size);
+		memset(tls + tls_init_offset + tls_init_size,
+		    0, tls_static_space - tls_init_size);
+	}
+
+	return tls;
+#else
 	return (0);
+#endif
 }
 
+#endif
+
+#ifdef TLS_VARIANT_II
+
+/*
+ * Free Static TLS using the Variant II method.
+ */
 #pragma weak _rtld_free_tls
 void
 _rtld_free_tls(void *tcb, size_t tcbsize, size_t tcbalign)
 {
+#ifndef PIC
+	size_t size;
+	Elf_Addr* dtv;
+	Elf_Addr tlsstart, tlsend;
+
+	/*
+	 * Figure out the size of the initial TLS block so that we can
+	 * find stuff which ___tls_get_addr() allocated dynamically.
+	 */
+	size = round(tls_static_space, tcbalign);
+
+	dtv = ((Elf_Addr**)tcb)[1];
+	tlsend = (Elf_Addr) tcb;
+	tlsstart = tlsend - size;
+	free((void*) tlsstart);
+	free(dtv);
+#endif
+}
+
+#pragma weak _rtld_allocate_tls
+/*
+ * Allocate Static TLS using the Variant II method.
+ */
+void *
+_rtld_allocate_tls(void *oldtls, size_t tcbsize, size_t tcbalign)
+{
+#ifndef PIC
+	size_t size;
+	char *tls;
+	Elf_Addr *dtv;
+	Elf_Addr segbase, oldsegbase;
+
+	size = round(tls_static_space, tcbalign);
+
+	assert(tcbsize >= 2*sizeof(Elf_Addr));
+	tls = malloc(size + tcbsize);
+	dtv = malloc(3 * sizeof(Elf_Addr));
+
+	segbase = (Elf_Addr)(tls + size);
+	((Elf_Addr*)segbase)[0] = segbase;
+	((Elf_Addr*)segbase)[1] = (Elf_Addr) dtv;
+
+	dtv[0] = 1;
+	dtv[1] = 1;
+	dtv[2] = segbase - tls_static_space;
+
+	if (oldtls) {
+		/*
+		 * Copy the static TLS block over whole.
+		 */
+		oldsegbase = (Elf_Addr) oldtls;
+		memcpy((void *)(segbase - tls_static_space),
+		    (const void *)(oldsegbase - tls_static_space),
+		    tls_static_space);
+
+		/*
+		 * We assume that this block was the one we created with
+		 * allocate_initial_tls().
+		 */
+		_rtld_free_tls(oldtls, 2*sizeof(Elf_Addr), sizeof(Elf_Addr));
+	} else {
+		memcpy((void *)(segbase - tls_static_space),
+		    tls_init, tls_init_size);
+		memset((void *)(segbase - tls_static_space + tls_init_size),
+		    0, tls_static_space - tls_init_size);
+	}
+
+	return (void*) segbase;
+#else
+	return (0);
+#endif
+}
+
+#endif
+
+void
+_init_tls()
+{
+#ifndef PIC
+	extern char **environ;
+	Elf_Addr *sp;
+	Elf_Auxinfo *aux, *auxp;
+	Elf_Phdr *phdr;
+	size_t phent, phnum;
+	int i;
+
+	sp = (Elf_Addr *) environ;
+	while (*sp++ != 0)
+		;
+	aux = (Elf_Auxinfo *) sp;
+	phdr = 0;
+	phent = phnum = 0;
+	for (auxp = aux; auxp->a_type != AT_NULL; auxp++) {
+		switch (auxp->a_type) {
+		case AT_PHDR:
+			phdr = auxp->a_un.a_ptr;
+			break;
+
+		case AT_PHENT:
+			phent = auxp->a_un.a_val;
+			break;
+
+		case AT_PHNUM:
+			phnum = auxp->a_un.a_val;
+			break;
+		}
+	}
+	if (phdr == 0 || phent != sizeof(Elf_Phdr) || phnum == 0)
+		return;
+
+	for (i = 0; i < phnum; i++) {
+		if (phdr[i].p_type == PT_TLS) {
+#ifdef TLS_VARIANT_I
+			tls_static_space = round(2*sizeof(Elf_Addr),
+			    phdr[i].p_align) + phdr[i].p_memsz;
+			tls_init_offset = round(2*sizeof(Elf_Addr),
+			    phdr[i].p_align);
+#else			    
+			tls_static_space = round(phdr[i].p_memsz,
+			    phdr[i].p_align);
+#endif
+			tls_init_size = phdr[i].p_filesz;
+			tls_init = (void*) phdr[i].p_vaddr;
+		}
+	}
+
+	if (tls_static_space > 0) {
+		void* tls;
+
+		tls = _rtld_allocate_tls(NULL, 2*sizeof(Elf_Addr),
+		    sizeof(Elf_Addr));
+
+		_set_tp(tls);
+	}
+#endif
 }