Kernel/LibC/DynamicLoader: Implement thread local storage

For some reason this does not work on 32 bit version, so it is disabled
on that platform. I'll have to look into it later to find the bug :)

userspace/libraries/LibC/CMakeLists.txt

@@ -58,6 +58,10 @@ target_compile_definitions(objlibc PRIVATE __arch=${BANAN_ARCH})
 target_compile_options(objlibc PRIVATE -O2 -g -Wstack-usage=512 -fno-exceptions -fpic -nolibc)
 target_compile_options(objlibc PUBLIC -Wall -Wextra -Werror -Wno-error=stack-usage=)
 
+if("${BANAN_ARCH}" STREQUAL "i686")
+	target_compile_definitions(objlibc PRIVATE __disable_thread_local_storage)
+endif()
+
 function(add_crtx crtx)
 	add_custom_target(${crtx}
 		COMMAND ${CMAKE_CXX_COMPILER} -c -o ${CMAKE_INSTALL_LIBDIR}/${crtx}.o ${CMAKE_CURRENT_SOURCE_DIR}/arch/${BANAN_ARCH}/${crtx}.S
@@ -71,6 +75,7 @@ add_crtx(crtn)
 
 banan_include_headers(objlibc ban)
 banan_include_headers(objlibc kernel)
+banan_include_headers(objlibc libelf)
 
 banan_install_headers(objlibc)
 

userspace/libraries/LibC/errno.cpp

@@ -1,6 +1,10 @@
 #include <errno.h>
 
+#if __disable_thread_local_storage
 static int s_errno = 0;
+#else
+static thread_local int s_errno = 0;
+#endif
 
 int* __errno_location()
 {

userspace/libraries/LibC/include/pthread.h

@@ -8,6 +8,7 @@
 __BEGIN_DECLS
 
 #include <sched.h>
+#include <stdint.h>
 #include <time.h>
 
 #define __need_size_t
@@ -27,6 +28,14 @@ __BEGIN_DECLS
 #define __need_pthread_t
 #include <sys/types.h>
 
+struct uthread
+{
+	struct uthread* self;
+	void* master_tls_addr;
+	size_t master_tls_size;
+	uintptr_t dtv[];
+};
+
 #define PTHREAD_BARRIER_SERIAL_THREAD	1
 #define PTHREAD_CANCEL_ASYNCHRONOUS		2
 #define PTHREAD_CANCEL_ENABLE			3

userspace/libraries/LibC/pthread.cpp

@@ -8,11 +8,13 @@
 #include <pthread.h>
 #include <stdlib.h>
 #include <string.h>
+#include <sys/mman.h>
 #include <sys/syscall.h>
 #include <unistd.h>
 
 struct pthread_trampoline_info_t
 {
+	struct uthread* uthread;
 	void* (*start_routine)(void*);
 	void* arg;
 };
@@ -41,11 +43,36 @@ asm(
 extern "C" void _pthread_trampoline_cpp(void* arg)
 {
 	auto info = *reinterpret_cast<pthread_trampoline_info_t*>(arg);
+	syscall(SYS_SET_TLS, info.uthread);
 	free(arg);
 	pthread_exit(info.start_routine(info.arg));
 	ASSERT_NOT_REACHED();
 }
 
+static uthread* get_uthread()
+{
+	uthread* result;
+#if ARCH(x86_64)
+	asm volatile("movq %%fs:0, %0" : "=r"(result));
+#elif ARCH(i686)
+	asm volatile("movl %%gs:0, %0" : "=r"(result));
+#endif
+	return result;
+}
+
+static void free_uthread(uthread* uthread)
+{
+	if (uthread->dtv[0] == 0)
+		return free(uthread);
+
+	uint8_t* tls_addr = reinterpret_cast<uint8_t*>(uthread) - uthread->master_tls_size;
+	const size_t tls_size = uthread->master_tls_size
+		+ sizeof(struct uthread)
+		+ (uthread->dtv[0] + 1) * sizeof(uintptr_t);
+	munmap(tls_addr, tls_size);
+}
+
+#if not __disable_thread_local_storage
 struct pthread_cleanup_t
 {
 	void (*routine)(void*);
@@ -79,6 +106,7 @@ void pthread_cleanup_push(void (*routine)(void*), void* arg)
 
 	s_cleanup_stack = cleanup;
 }
+#endif
 
 int pthread_attr_init(pthread_attr_t* attr)
 {
@@ -93,29 +121,76 @@ int pthread_create(pthread_t* __restrict thread_id, const pthread_attr_t* __rest
 		return errno;
 
 	*info = {
+		.uthread = nullptr,
 		.start_routine = start_routine,
 		.arg = arg,
 	};
 
-	const auto ret = syscall(SYS_PTHREAD_CREATE, attr, pthread_trampoline, info);
-	if (ret == -1)
+	long syscall_ret = 0;
+
+	if (uthread* self = get_uthread(); self->master_tls_addr == nullptr)
+	{
+		uthread* uthread = static_cast<struct uthread*>(malloc(sizeof(struct uthread) + sizeof(uintptr_t)));
+		if (uthread == nullptr)
+			goto pthread_create_error;
+		uthread->self = uthread;
+		uthread->master_tls_addr = nullptr;
+		uthread->master_tls_size = 0;
+		uthread->dtv[0] = 0;
+
+		info->uthread = uthread;
+	}
+	else
+	{
+		const size_t module_count = self->dtv[0];
+
+		const size_t tls_size = self->master_tls_size
+			+ sizeof(uthread)
+			+ (module_count + 1) * sizeof(uintptr_t);
+
+		uint8_t* tls_addr = static_cast<uint8_t*>(mmap(nullptr, tls_size, PROT_READ | PROT_WRITE, MAP_ANONYMOUS | MAP_PRIVATE, -1, 0));
+		if (tls_addr == MAP_FAILED)
+			goto pthread_create_error;
+		memcpy(tls_addr, self->master_tls_addr, self->master_tls_size);
+
+		uthread* uthread = reinterpret_cast<struct uthread*>(tls_addr + self->master_tls_size);
+		uthread->self = uthread;
+		uthread->master_tls_addr = self->master_tls_addr;
+		uthread->master_tls_size = self->master_tls_size;
+
+		const uintptr_t self_addr = reinterpret_cast<uintptr_t>(self);
+		const uintptr_t uthread_addr = reinterpret_cast<uintptr_t>(uthread);
+
+		uthread->dtv[0] = module_count;
+		for (size_t i = 1; i <= module_count; i++)
+			uthread->dtv[i] = self->dtv[i] - self_addr + uthread_addr;
+
+		info->uthread = uthread;
+	}
+
+	syscall_ret = syscall(SYS_PTHREAD_CREATE, attr, _pthread_trampoline, info);
+	if (syscall_ret == -1)
 		goto pthread_create_error;
 
-
 	if (thread_id)
-		*thread_id = ret;
+		*thread_id = syscall_ret;
 	return 0;
 
 pthread_create_error:
 	const int return_code = errno;
+	if (info->uthread)
+		free_uthread(info->uthread);
 	free(info);
 	return return_code;
 }
 
 void pthread_exit(void* value_ptr)
 {
+#if not __disable_thread_local_storage
 	while (s_cleanup_stack)
 		pthread_cleanup_pop(1);
+#endif
+	free_uthread(get_uthread());
 	syscall(SYS_PTHREAD_EXIT, value_ptr);
 	ASSERT_NOT_REACHED();
 }
@@ -127,7 +202,14 @@ int pthread_join(pthread_t thread, void** value_ptr)
 
 pthread_t pthread_self(void)
 {
+#if __disable_thread_local_storage
 	return syscall(SYS_PTHREAD_SELF);
+#else
+	static thread_local pthread_t s_pthread_self { -1 };
+	if (s_pthread_self == -1) [[unlikely]]
+		s_pthread_self = syscall(SYS_PTHREAD_SELF);
+	return s_pthread_self;
+#endif
 }
 
 static inline BAN::Atomic<pthread_t>& pthread_spin_get_atomic(pthread_spinlock_t* lock)
@@ -188,3 +270,21 @@ int pthread_spin_unlock(pthread_spinlock_t* lock)
 	atomic.store(0, BAN::MemoryOrder::memory_order_release);
 	return 0;
 }
+
+struct tls_index
+{
+	unsigned long int ti_module;
+	unsigned long int ti_offset;
+};
+
+extern "C" void* __tls_get_addr(tls_index* ti)
+{
+	return reinterpret_cast<void*>(get_uthread()->dtv[ti->ti_module] + ti->ti_offset);
+}
+
+#if ARCH(i686)
+extern "C" void* __attribute__((__regparm__(1))) ___tls_get_addr(tls_index* ti)
+{
+	return reinterpret_cast<void*>(get_uthread()->dtv[ti->ti_module] + ti->ti_offset);
+}
+#endif

userspace/libraries/LibC/unistd.cpp

@@ -1,10 +1,12 @@
 #include <BAN/Assert.h>
 #include <BAN/Debug.h>
+
 #include <kernel/Memory/Types.h>
 #include <kernel/Syscall.h>
 
 #include <errno.h>
 #include <fcntl.h>
+#include <pthread.h>
 #include <pwd.h>
 #include <stdarg.h>
 #include <stdio.h>
@@ -30,6 +32,21 @@ extern "C" void _init_libc(char** environ, init_funcs_t init_funcs, init_funcs_t
 	if (::environ == nullptr)
 		::environ = environ;
 
+	if (syscall(SYS_GET_TLS) == 0)
+	{
+		alignas(uthread) static uint8_t storage[sizeof(uthread) + sizeof(uintptr_t)];
+
+		uthread& uthread = *reinterpret_cast<struct uthread*>(storage);
+		uthread = {
+			.self = &uthread,
+			.master_tls_addr = nullptr,
+			.master_tls_size = 0,
+		};
+		uthread.dtv[0] = 0;
+
+		syscall(SYS_SET_TLS, &uthread);
+	}
+
 	// call global constructors
 	if (init_funcs.func)
 		init_funcs.func();