1 files changed, 38 insertions, 33 deletions

diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index 957cbdfee..afaf0cd5d 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -17,6 +17,7 @@
 #include "core/core_timing.h"
 #include "core/hle/hle.h"
 #include "core/hle/kernel/kernel.h"
+#include "core/hle/kernel/process.h"
 #include "core/hle/kernel/thread.h"
 #include "core/hle/kernel/mutex.h"
 #include "core/hle/result.h"
@@ -106,6 +107,8 @@ void Thread::Stop() {
     for (auto& wait_object : wait_objects) {
         wait_object->RemoveWaitingThread(this);
     }
+
+    Kernel::g_current_process->used_tls_slots[tls_index] = false;
 }
 
 Thread* ArbitrateHighestPriorityThread(u32 address) {
@@ -157,7 +160,7 @@ static void PriorityBoostStarvedThreads() {
 
         u64 delta = current_ticks - thread->last_running_ticks;
 
-        if (thread->status == THREADSTATUS_READY && delta > boost_timeout && !thread->idle) {
+        if (thread->status == THREADSTATUS_READY && delta > boost_timeout) {
             const s32 priority = std::max(ready_queue.get_first()->current_priority - 1, 0);
             thread->BoostPriority(priority);
         }
@@ -169,8 +172,6 @@ static void PriorityBoostStarvedThreads() {
  * @param new_thread The thread to switch to
  */
 static void SwitchContext(Thread* new_thread) {
-    DEBUG_ASSERT_MSG(new_thread->status == THREADSTATUS_READY, "Thread must be ready to become running.");
-
     Thread* previous_thread = GetCurrentThread();
 
     // Save context for previous thread
@@ -188,6 +189,8 @@ static void SwitchContext(Thread* new_thread) {
 
     // Load context of new thread
     if (new_thread) {
+        DEBUG_ASSERT_MSG(new_thread->status == THREADSTATUS_READY, "Thread must be ready to become running.");
+
         current_thread = new_thread;
 
         ready_queue.remove(new_thread->current_priority, new_thread);
@@ -215,6 +218,10 @@ static Thread* PopNextReadyThread() {
         // We have to do better than the current thread.
         // This call returns null when that's not possible.
         next = ready_queue.pop_first_better(thread->current_priority);
+        if (!next) {
+            // Otherwise just keep going with the current thread
+            next = thread;
+        }
     } else  {
         next = ready_queue.pop_first();
     }
@@ -402,12 +409,20 @@ ResultVal<SharedPtr<Thread>> Thread::Create(std::string name, VAddr entry_point,
     thread->wait_address = 0;
     thread->name = std::move(name);
     thread->callback_handle = wakeup_callback_handle_table.Create(thread).MoveFrom();
+    thread->owner_process = g_current_process;
+    thread->tls_index = -1;
+
+    // Find the next available TLS index, and mark it as used
+    auto& used_tls_slots = Kernel::g_current_process->used_tls_slots;
+    for (unsigned int i = 0; i < used_tls_slots.size(); ++i) {
+        if (used_tls_slots[i] == false) {
+            thread->tls_index = i;
+            used_tls_slots[i] = true;
+            break;
+        }
+    }
 
-    VAddr tls_address = Memory::TLS_AREA_VADDR + (thread->thread_id - 1) * 0x200;
-
-    ASSERT_MSG(tls_address < Memory::TLS_AREA_VADDR_END, "Too many threads");
-
-    thread->tls_address = tls_address;
+    ASSERT_MSG(thread->tls_index != -1, "Out of TLS space");
 
     // TODO(peachum): move to ScheduleThread() when scheduler is added so selected core is used
     // to initialize the context
@@ -439,6 +454,8 @@ void Thread::SetPriority(s32 priority) {
     // If thread was ready, adjust queues
     if (status == THREADSTATUS_READY)
         ready_queue.move(this, current_priority, priority);
+    else
+        ready_queue.prepare(priority);
 
     nominal_priority = current_priority = priority;
 }
@@ -448,16 +465,6 @@ void Thread::BoostPriority(s32 priority) {
     current_priority = priority;
 }
 
-SharedPtr<Thread> SetupIdleThread() {
-    // We need to pass a few valid values to get around parameter checking in Thread::Create.
-    // TODO(yuriks): Figure out a way to avoid passing the bogus VAddr parameter
-    auto thread = Thread::Create("idle", Memory::TLS_AREA_VADDR, THREADPRIO_LOWEST, 0,
-            THREADPROCESSORID_0, 0).MoveFrom();
-
-    thread->idle = true;
-    return thread;
-}
-
 SharedPtr<Thread> SetupMainThread(u32 entry_point, s32 priority) {
     DEBUG_ASSERT(!GetCurrentThread());
 
@@ -474,24 +481,25 @@ SharedPtr<Thread> SetupMainThread(u32 entry_point, s32 priority) {
 }
 
 void Reschedule() {
-    Thread* prev = GetCurrentThread();
-
     PriorityBoostStarvedThreads();
 
+    Thread* cur = GetCurrentThread();
     Thread* next = PopNextReadyThread();
     HLE::g_reschedule = false;
 
-    if (next != nullptr) {
-        LOG_TRACE(Kernel, "context switch %u -> %u", prev->GetObjectId(), next->GetObjectId());
-        SwitchContext(next);
-    } else {
-        LOG_TRACE(Kernel, "cannot context switch from %u, no higher priority thread!", prev->GetObjectId());
+    // Don't bother switching to the same thread
+    if (next == cur)
+        return;
 
-        for (auto& thread : thread_list) {
-            LOG_TRACE(Kernel, "\tid=%u prio=0x%02X, status=0x%08X", thread->GetObjectId(), 
-                      thread->current_priority, thread->status);
-        }
+    if (cur && next) {
+        LOG_TRACE(Kernel, "context switch %u -> %u", cur->GetObjectId(), next->GetObjectId());
+    } else if (cur) {
+        LOG_TRACE(Kernel, "context switch %u -> idle", cur->GetObjectId());
+    } else {
+        LOG_TRACE(Kernel, "context switch idle -> %u", next->GetObjectId());
     }
+    
+    SwitchContext(next);
 }
 
 void Thread::SetWaitSynchronizationResult(ResultCode result) {
@@ -503,7 +511,7 @@ void Thread::SetWaitSynchronizationOutput(s32 output) {
 }
 
 VAddr Thread::GetTLSAddress() const {
-    return tls_address;
+    return Memory::TLS_AREA_VADDR + tls_index * 0x200;
 }
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
@@ -516,9 +524,6 @@ void ThreadingInit() {
 
     thread_list.clear();
     ready_queue.clear();
-
-    // Setup the idle thread
-    SetupIdleThread();
 }
 
 void ThreadingShutdown() {