8 files changed, 31 insertions, 11 deletions

diff --git a/src/core/arm/arm_interface.h b/src/core/arm/arm_interface.h
index 85ed2c698..976c339e8 100644
--- a/src/core/arm/arm_interface.h
+++ b/src/core/arm/arm_interface.h
@@ -99,8 +99,9 @@ public:
      * @param stack_top Pointer to the top of the stack
      * @param entry_point Entry point for execution
      * @param arg User argument for thread
+     * @param tls_address Address of the Thread Local Storage for the thread
      */
-    virtual void ResetContext(Core::ThreadContext& context, u32 stack_top, u32 entry_point, u32 arg) = 0;
+    virtual void ResetContext(Core::ThreadContext& context, u32 stack_top, u32 entry_point, u32 arg, u32 tls_address) = 0;
 
     /**
      * Saves the current CPU context
diff --git a/src/core/arm/dyncom/arm_dyncom.cpp b/src/core/arm/dyncom/arm_dyncom.cpp
index 0072ae533..1de1d2612 100644
--- a/src/core/arm/dyncom/arm_dyncom.cpp
+++ b/src/core/arm/dyncom/arm_dyncom.cpp
@@ -90,13 +90,14 @@ void ARM_DynCom::ExecuteInstructions(int num_instructions) {
     AddTicks(ticks_executed);
 }
 
-void ARM_DynCom::ResetContext(Core::ThreadContext& context, u32 stack_top, u32 entry_point, u32 arg) {
+void ARM_DynCom::ResetContext(Core::ThreadContext& context, u32 stack_top, u32 entry_point, u32 arg, u32 tls_address) {
     memset(&context, 0, sizeof(Core::ThreadContext));
 
     context.cpu_registers[0] = arg;
     context.pc = entry_point;
     context.sp = stack_top;
     context.cpsr = 0x1F; // Usermode
+    context.tls = tls_address;
 }
 
 void ARM_DynCom::SaveContext(Core::ThreadContext& ctx) {
@@ -123,6 +124,8 @@ void ARM_DynCom::LoadContext(const Core::ThreadContext& ctx) {
 
     state->VFP[1] = ctx.fpscr;
     state->VFP[2] = ctx.fpexc;
+
+    SetCP15Register(CP15_THREAD_URO, ctx.tls);
 }
 
 void ARM_DynCom::PrepareReschedule() {
diff --git a/src/core/arm/dyncom/arm_dyncom.h b/src/core/arm/dyncom/arm_dyncom.h
index 2488c879c..b3fd708f1 100644
--- a/src/core/arm/dyncom/arm_dyncom.h
+++ b/src/core/arm/dyncom/arm_dyncom.h
@@ -27,7 +27,7 @@ public:
 
     void AddTicks(u64 ticks) override;
 
-    void ResetContext(Core::ThreadContext& context, u32 stack_top, u32 entry_point, u32 arg) override;
+    void ResetContext(Core::ThreadContext& context, u32 stack_top, u32 entry_point, u32 arg, u32 tls_address) override;
     void SaveContext(Core::ThreadContext& ctx) override;
     void LoadContext(const Core::ThreadContext& ctx) override;
 
diff --git a/src/core/core.cpp b/src/core/core.cpp
index bb2ed7a92..b5c258230 100644
--- a/src/core/core.cpp
+++ b/src/core/core.cpp
@@ -61,10 +61,6 @@ int Init() {
     g_sys_core = new ARM_DynCom(USER32MODE);
     g_app_core = new ARM_DynCom(USER32MODE);
 
-    // TODO: Whenever TLS is implemented, this should contain
-    // the address of the 0x200-byte TLS
-    g_app_core->SetCP15Register(CP15_THREAD_URO, Memory::TLS_AREA_VADDR);
-
     LOG_DEBUG(Core, "Initialized OK");
     return 0;
 }
diff --git a/src/core/core.h b/src/core/core.h
index 278f0f1cc..215b5a49f 100644
--- a/src/core/core.h
+++ b/src/core/core.h
@@ -21,6 +21,8 @@ struct ThreadContext {
     u32 fpu_registers[32];
     u32 fpscr;
     u32 fpexc;
+
+    u32 tls;
 };
 
 extern ARM_Interface*   g_app_core;     ///< ARM11 application core
diff --git a/src/core/hle/kernel/session.h b/src/core/hle/kernel/session.h
index 0fd18148a..8c3886ffd 100644
--- a/src/core/hle/kernel/session.h
+++ b/src/core/hle/kernel/session.h
@@ -5,6 +5,7 @@
 #pragma once
 
 #include "core/hle/kernel/kernel.h"
+#include "core/hle/kernel/thread.h"
 #include "core/mem_map.h"
 
 namespace Kernel {
@@ -12,12 +13,15 @@ namespace Kernel {
 static const int kCommandHeaderOffset = 0x80; ///< Offset into command buffer of header
 
 /**
- * Returns a pointer to the command buffer in kernel memory
+ * Returns a pointer to the command buffer in the current thread's TLS
+ * TODO(Subv): This is not entirely correct, the command buffer should be copied from
+ * the thread's TLS to an intermediate buffer in kernel memory, and then copied again to
+ * the service handler process' memory.
  * @param offset Optional offset into command buffer
  * @return Pointer to command buffer
  */
-inline static u32* GetCommandBuffer(const int offset=0) {
-    return (u32*)Memory::GetPointer(Memory::TLS_AREA_VADDR + kCommandHeaderOffset + offset);
+inline static u32* GetCommandBuffer(const int offset = 0) {
+    return (u32*)Memory::GetPointer(GetCurrentThread()->GetTLSAddress() + kCommandHeaderOffset + offset);
 }
 
 /**
diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index 0a3fd7cb1..61199c12a 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -402,9 +402,13 @@ ResultVal<SharedPtr<Thread>> Thread::Create(std::string name, VAddr entry_point,
     thread->name = std::move(name);
     thread->callback_handle = wakeup_callback_handle_table.Create(thread).MoveFrom();
 
+    VAddr tls_address = Memory::TLS_AREA_VADDR + (thread->thread_id - 1) * 0x200;
+
+    ASSERT_MSG(tls_address < Memory::TLS_AREA_VADDR_END, "Too many threads");
+
     // TODO(peachum): move to ScheduleThread() when scheduler is added so selected core is used
     // to initialize the context
-    Core::g_app_core->ResetContext(thread->context, stack_top, entry_point, arg);
+    Core::g_app_core->ResetContext(thread->context, stack_top, entry_point, arg, tls_address);
 
     ready_queue.push_back(thread->current_priority, thread.get());
     thread->status = THREADSTATUS_READY;
@@ -495,6 +499,10 @@ void Thread::SetWaitSynchronizationOutput(s32 output) {
     context.cpu_registers[1] = output;
 }
 
+VAddr Thread::GetTLSAddress() const {
+    return context.tls;
+}
+
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 
 void ThreadingInit() {
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index 9958b16e6..17bb69f45 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -135,6 +135,12 @@ public:
      */
     void Stop();
 
+    /*
+     * Returns the Thread Local Storage address of the current thread
+     * @returns VAddr of the thread's TLS
+     */
+    VAddr GetTLSAddress() const;
+
     Core::ThreadContext context;
 
     u32 thread_id;