arm64: add support for thread local storage

Adds the necessary bits to initialize TLS in the stack
area and sets up CPU registers during context switch.

Signed-off-by: Daniel Leung <daniel.leung@intel.com>

arch/Kconfig

@@ -32,6 +32,7 @@ config ARM
 	# FIXME: current state of the code for all ARM requires this, but
 	# is really only necessary for Cortex-M with ARM MPU!
 	select GEN_PRIV_STACKS
+	select ARCH_HAS_THREAD_LOCAL_STORAGE if ARM64
 	help
 	  ARM architecture
 

arch/arm/core/aarch64/CMakeLists.txt

@@ -30,3 +30,4 @@ endif ()
 zephyr_library_sources_ifdef(CONFIG_GEN_SW_ISR_TABLE isr_wrapper.S)
 zephyr_library_sources_ifdef(CONFIG_IRQ_OFFLOAD irq_offload.c)
 zephyr_library_sources_ifdef(CONFIG_ARM_MMU arm_mmu.c)
+zephyr_library_sources_ifdef(CONFIG_THREAD_LOCAL_STORAGE ../common/tls.c)

arch/arm/core/aarch64/switch.S

@@ -48,6 +48,19 @@ SECTION_FUNC(TEXT, z_arm64_context_switch)
 	mov	x1, sp
 	str	x1, [x2]
 
+#ifdef CONFIG_THREAD_LOCAL_STORAGE
+	/* Grab the TLS pointer */
+	ldr	x2, =_thread_offset_to_tls
+	add	x2, x2, x0
+	ldr	x2, [x2]
+
+	/* Store in the "Thread ID" register.
+	 * This register is used as a base pointer to all
+	 * thread variables with offsets added by toolchain.
+	 */
+	msr	tpidr_el0, x2
+#endif
+
 	/* addr of callee-saved regs in thread in x2 */
 	ldr	x2, =_thread_offset_to_callee_saved
 	add	x2, x2, x0

arch/arm/core/common/tls.c

@@ -0,0 +1,42 @@
+/*
+ * Copyright (c) 2020 Intel Corporation
+ *
+ * SPDX-License-Identifier: Apache-2.0
+ */
+
+#include <kernel.h>
+#include <kernel_structs.h>
+#include <kernel_internal.h>
+#include <kernel_tls.h>
+#include <sys/util.h>
+
+size_t arch_tls_stack_setup(struct k_thread *new_thread, char *stack_ptr)
+{
+	/*
+	 * TLS area for ARM has some data fields following by
+	 * thread data and bss. These fields are supposed to be
+	 * used by toolchain and OS TLS code to aid in locating
+	 * the TLS data/bss. Zephyr currently has no use for
+	 * this so we can simply skip these. However, since GCC
+	 * is generating code assuming these fields are there,
+	 * we simply skip them when setting the TLS pointer.
+	 */
+
+	/*
+	 * Since we are populating things backwards,
+	 * setup the TLS data/bss area first.
+	 */
+	stack_ptr -= z_tls_data_size();
+	z_tls_copy(stack_ptr);
+
+	/* Skip two pointers due to toolchain */
+	stack_ptr -= sizeof(uintptr_t) * 2;
+
+	/*
+	 * Set thread TLS pointer which is used in
+	 * context switch to point to TLS area.
+	 */
+	new_thread->tls = POINTER_TO_UINT(stack_ptr);
+
+	return (z_tls_data_size() + (sizeof(uintptr_t) * 2));
+}

include/arch/arm/aarch64/scripts/linker.ld

@@ -165,6 +165,7 @@ SECTIONS
         _image_rodata_start = .;
 
 #include <linker/common-rom.ld>
+#include <linker/thread-local-storage.ld>
 
     SECTION_PROLOGUE(_RODATA_SECTION_NAME,,)
     {