Merge pull request #528 from kevinhartman/scheduling-logic

Scheduler refactor Pt. 1

7 files changed, 287 insertions, 284 deletions

diff --git a/src/core/arm/arm_interface.h b/src/core/arm/arm_interface.h
index e612f7439..ef37ee055 100644
--- a/src/core/arm/arm_interface.h
+++ b/src/core/arm/arm_interface.h
@@ -86,6 +86,15 @@ public:
     virtual void AddTicks(u64 ticks) = 0;
 
     /**
+     * Initializes a CPU context for use on this CPU
+     * @param context Thread context to reset
+     * @param stack_top Pointer to the top of the stack
+     * @param entry_point Entry point for execution
+     * @param arg User argument for thread
+     */
+    virtual void ResetContext(Core::ThreadContext& context, u32 stack_top, u32 entry_point, u32 arg) = 0;
+
+    /**
      * Saves the current CPU context
      * @param ctx Thread context to save
      */
diff --git a/src/core/arm/dyncom/arm_dyncom.cpp b/src/core/arm/dyncom/arm_dyncom.cpp
index f6628ca33..68fddc94f 100644
--- a/src/core/arm/dyncom/arm_dyncom.cpp
+++ b/src/core/arm/dyncom/arm_dyncom.cpp
@@ -93,6 +93,16 @@ 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) {
+    memset(&context, 0, sizeof(Core::ThreadContext));
+
+    context.cpu_registers[0] = arg;
+    context.pc = entry_point;
+    context.sp = stack_top;
+    context.cpsr = 0x1F; // Usermode
+    context.mode = 8;    // Instructs dyncom CPU core to start execution as if it's "resuming" a thread.
+}
+
 void ARM_DynCom::SaveContext(Core::ThreadContext& ctx) {
     memcpy(ctx.cpu_registers, state->Reg, sizeof(ctx.cpu_registers));
     memcpy(ctx.fpu_registers, state->ExtReg, sizeof(ctx.fpu_registers));
diff --git a/src/core/arm/dyncom/arm_dyncom.h b/src/core/arm/dyncom/arm_dyncom.h
index f16fb070c..9e2dda843 100644
--- a/src/core/arm/dyncom/arm_dyncom.h
+++ b/src/core/arm/dyncom/arm_dyncom.h
@@ -13,79 +13,24 @@
 
 class ARM_DynCom final : virtual public ARM_Interface {
 public:
-
     ARM_DynCom();
     ~ARM_DynCom();
 
-    /**
-     * Set the Program Counter to an address
-     * @param pc Address to set PC to
-     */
     void SetPC(u32 pc) override;
-
-    /*
-     * Get the current Program Counter
-     * @return Returns current PC
-     */
     u32 GetPC() const override;
-
-    /**
-     * Get an ARM register
-     * @param index Register index (0-15)
-     * @return Returns the value in the register
-     */
     u32 GetReg(int index) const override;
-
-    /**
-     * Set an ARM register
-     * @param index Register index (0-15)
-     * @param value Value to set register to
-     */
     void SetReg(int index, u32 value) override;
-
-    /**
-     * Get the current CPSR register
-     * @return Returns the value of the CPSR register
-     */
     u32 GetCPSR() const override;
-
-    /**
-     * Set the current CPSR register
-     * @param cpsr Value to set CPSR to
-     */
     void SetCPSR(u32 cpsr) override;
 
-    /**
-     * Returns the number of clock ticks since the last reset
-     * @return Returns number of clock ticks
-     */
     u64 GetTicks() const override;
-
-    /**
-    * Advance the CPU core by the specified number of ticks (e.g. to simulate CPU execution time)
-    * @param ticks Number of ticks to advance the CPU core
-    */
     void AddTicks(u64 ticks) override;
 
-    /**
-     * Saves the current CPU context
-     * @param ctx Thread context to save
-     */
+    void ResetContext(Core::ThreadContext& context, u32 stack_top, u32 entry_point, u32 arg);
     void SaveContext(Core::ThreadContext& ctx) override;
-
-    /**
-     * Loads a CPU context
-     * @param ctx Thread context to load
-     */
     void LoadContext(const Core::ThreadContext& ctx) override;
 
-    /// Prepare core for thread reschedule (if needed to correctly handle state)
     void PrepareReschedule() override;
-
-    /**
-     * Executes the given number of instructions
-     * @param num_instructions Number of instructions to executes
-     */
     void ExecuteInstructions(int num_instructions) override;
 
 private:
diff --git a/src/core/hle/kernel/kernel.cpp b/src/core/hle/kernel/kernel.cpp
index 52dca4dd8..a2ffbcdb7 100644
--- a/src/core/hle/kernel/kernel.cpp
+++ b/src/core/hle/kernel/kernel.cpp
@@ -153,12 +153,8 @@ void Shutdown() {
  * @return True on success, otherwise false
  */
 bool LoadExec(u32 entry_point) {
-    Core::g_app_core->SetPC(entry_point);
-
     // 0x30 is the typical main thread priority I've seen used so far
-    g_main_thread = Kernel::SetupMainThread(0x30, Kernel::DEFAULT_STACK_SIZE);
-    // Setup the idle thread
-    Kernel::SetupIdleThread();
+    g_main_thread = Kernel::SetupMainThread(Kernel::DEFAULT_STACK_SIZE, entry_point, 0x30);
 
     return true;
 }
diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index 3987f9608..7f629c20e 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -21,8 +21,11 @@
 
 namespace Kernel {
 
+/// Event type for the thread wake up event
+static int ThreadWakeupEventType = -1;
+
 bool Thread::ShouldWait() {
-    return status != THREADSTATUS_DORMANT;
+    return status != THREADSTATUS_DEAD;
 }
 
 void Thread::Acquire() {
@@ -33,12 +36,20 @@ void Thread::Acquire() {
 static std::vector<SharedPtr<Thread>> thread_list;
 
 // Lists only ready thread ids.
-static Common::ThreadQueueList<Thread*, THREADPRIO_LOWEST+1> thread_ready_queue;
+static Common::ThreadQueueList<Thread*, THREADPRIO_LOWEST+1> ready_queue;
 
 static Thread* current_thread;
 
-static const u32 INITIAL_THREAD_ID = 1; ///< The first available thread id at startup
-static u32 next_thread_id; ///< The next available thread id
+// The first available thread id at startup
+static u32 next_thread_id = 1;
+
+/**
+ * Creates a new thread ID
+ * @return The new thread ID
+ */
+inline static u32 const NewThreadId() {
+    return next_thread_id++;
+}
 
 Thread::Thread() {}
 Thread::~Thread() {}
@@ -47,86 +58,53 @@ Thread* GetCurrentThread() {
     return current_thread;
 }
 
-/// Resets a thread
-static void ResetThread(Thread* t, u32 arg, s32 lowest_priority) {
-    memset(&t->context, 0, sizeof(Core::ThreadContext));
-
-    t->context.cpu_registers[0] = arg;
-    t->context.pc = t->entry_point;
-    t->context.sp = t->stack_top;
-    t->context.cpsr = 0x1F; // Usermode
-
-    // TODO(bunnei): This instructs the CPU core to start the execution as if it is "resuming" a
-    // thread. This is somewhat Sky-Eye specific, and should be re-architected in the future to be
-    // agnostic of the CPU core.
-    t->context.mode = 8;
-
-    if (t->current_priority < lowest_priority) {
-        t->current_priority = t->initial_priority;
-    }
-
-    t->wait_objects.clear();
-    t->wait_address = 0;
-}
-
-/// Change a thread to "ready" state
-static void ChangeReadyState(Thread* t, bool ready) {
-    if (t->IsReady()) {
-        if (!ready) {
-            thread_ready_queue.remove(t->current_priority, t);
-        }
-    }  else if (ready) {
-        if (t->IsRunning()) {
-            thread_ready_queue.push_front(t->current_priority, t);
-        } else {
-            thread_ready_queue.push_back(t->current_priority, t);
-        }
-        t->status = THREADSTATUS_READY;
-    }
-}
-
-/// Check if a thread is waiting on a the specified wait object
+/**
+ * Check if a thread is waiting on the specified wait object
+ * @param thread The thread to test
+ * @param wait_object The object to test against
+ * @return True if the thread is waiting, false otherwise
+ */
 static bool CheckWait_WaitObject(const Thread* thread, WaitObject* wait_object) {
-    auto itr = std::find(thread->wait_objects.begin(), thread->wait_objects.end(), wait_object);
+    if (thread->status != THREADSTATUS_WAIT_SYNCH)
+        return false;
 
-    if (itr != thread->wait_objects.end())
-        return thread->IsWaiting();
-
-    return false;
+    auto itr = std::find(thread->wait_objects.begin(), thread->wait_objects.end(), wait_object);
+    return itr != thread->wait_objects.end();
 }
 
-/// Check if the specified thread is waiting on the specified address to be arbitrated
+/**
+ * Check if the specified thread is waiting on the specified address to be arbitrated
+ * @param thread The thread to test
+ * @param wait_address The address to test against
+ * @return True if the thread is waiting, false otherwise
+ */
 static bool CheckWait_AddressArbiter(const Thread* thread, VAddr wait_address) {
-    return thread->IsWaiting() && thread->wait_objects.empty() && wait_address == thread->wait_address;
+    return thread->status == THREADSTATUS_WAIT_ARB && wait_address == thread->wait_address;
 }
 
-/// Stops the current thread
-void Thread::Stop(const char* reason) {
+void Thread::Stop() {
     // Release all the mutexes that this thread holds
     ReleaseThreadMutexes(this);
 
-    ChangeReadyState(this, false);
-    status = THREADSTATUS_DORMANT;
+    // Cancel any outstanding wakeup events for this thread
+    CoreTiming::UnscheduleEvent(ThreadWakeupEventType, callback_handle);
+
+    // Clean up thread from ready queue
+    // This is only needed when the thread is termintated forcefully (SVC TerminateProcess)
+    if (status == THREADSTATUS_READY){
+        ready_queue.remove(current_priority, this);
+    }
+
+    status = THREADSTATUS_DEAD;
+    
     WakeupAllWaitingThreads();
 
-    // Stopped threads are never waiting.
+    // Clean up any dangling references in objects that this thread was waiting for
     for (auto& wait_object : wait_objects) {
         wait_object->RemoveWaitingThread(this);
     }
-    wait_objects.clear();
-    wait_address = 0;
-}
-
-/// Changes a threads state
-static void ChangeThreadState(Thread* t, ThreadStatus new_status) {
-    if (!t || t->status == new_status) {
-        return;
-    }
-    ChangeReadyState(t, (new_status & THREADSTATUS_READY) != 0);
-    t->status = new_status;
 }
 
-/// Arbitrate the highest priority thread that is waiting
 Thread* ArbitrateHighestPriorityThread(u32 address) {
     Thread* highest_priority_thread = nullptr;
     s32 priority = THREADPRIO_LOWEST;
@@ -153,108 +131,113 @@ Thread* ArbitrateHighestPriorityThread(u32 address) {
     return highest_priority_thread;
 }
 
-/// Arbitrate all threads currently waiting
 void ArbitrateAllThreads(u32 address) {
-
-    // Iterate through threads, find highest priority thread that is waiting to be arbitrated...
+    // Resume all threads found to be waiting on the address
     for (auto& thread : thread_list) {
         if (CheckWait_AddressArbiter(thread.get(), address))
             thread->ResumeFromWait();
     }
 }
 
-/// Calls a thread by marking it as "ready" (note: will not actually execute until current thread yields)
-static void CallThread(Thread* t) {
-    // Stop waiting
-    ChangeThreadState(t, THREADSTATUS_READY);
-}
+/** 
+ * Switches the CPU's active thread context to that of the specified thread
+ * @param new_thread The thread to switch to
+ */
+static void SwitchContext(Thread* new_thread) {
+    _dbg_assert_msg_(Kernel, new_thread->status == THREADSTATUS_READY, "Thread must be ready to become running.");
 
-/// Switches CPU context to that of the specified thread
-static void SwitchContext(Thread* t) {
-    Thread* cur = GetCurrentThread();
+    Thread* previous_thread = GetCurrentThread();
 
-    // Save context for current thread
-    if (cur) {
-        Core::g_app_core->SaveContext(cur->context);
+    // Save context for previous thread
+    if (previous_thread) {
+        Core::g_app_core->SaveContext(previous_thread->context);
 
-        if (cur->IsRunning()) {
-            ChangeReadyState(cur, true);
+        if (previous_thread->status == THREADSTATUS_RUNNING) {
+            // This is only the case when a reschedule is triggered without the current thread
+            // yielding execution (i.e. an event triggered, system core time-sliced, etc)
+            ready_queue.push_front(previous_thread->current_priority, previous_thread);
+            previous_thread->status = THREADSTATUS_READY;
         }
     }
+
     // Load context of new thread
-    if (t) {
-        current_thread = t;
-        ChangeReadyState(t, false);
-        t->status = (t->status | THREADSTATUS_RUNNING) & ~THREADSTATUS_READY;
-        Core::g_app_core->LoadContext(t->context);
+    if (new_thread) {
+        current_thread = new_thread;
+
+        ready_queue.remove(new_thread->current_priority, new_thread);
+        new_thread->status = THREADSTATUS_RUNNING;
+
+        Core::g_app_core->LoadContext(new_thread->context);
     } else {
         current_thread = nullptr;
     }
 }
 
-/// Gets the next thread that is ready to be run by priority
-static Thread* NextThread() {
+/**
+ * Pops and returns the next thread from the thread queue
+ * @return A pointer to the next ready thread
+ */
+static Thread* PopNextReadyThread() {
     Thread* next;
-    Thread* cur = GetCurrentThread();
+    Thread* thread = GetCurrentThread();
 
-    if (cur && cur->IsRunning()) {
-        next = thread_ready_queue.pop_first_better(cur->current_priority);
+    if (thread && thread->status == THREADSTATUS_RUNNING) {
+        // 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);
     } else  {
-        next = thread_ready_queue.pop_first();
-    }
-    if (next == 0) {
-        return nullptr;
+        next = ready_queue.pop_first();
     }
+
     return next;
 }
 
 void WaitCurrentThread_Sleep() {
     Thread* thread = GetCurrentThread();
-    ChangeThreadState(thread, ThreadStatus(THREADSTATUS_WAIT | (thread->status & THREADSTATUS_SUSPEND)));
+    thread->status = THREADSTATUS_WAIT_SLEEP;
 }
 
-void WaitCurrentThread_WaitSynchronization(SharedPtr<WaitObject> wait_object, bool wait_set_output, bool wait_all) {
+void WaitCurrentThread_WaitSynchronization(std::vector<SharedPtr<WaitObject>> wait_objects, bool wait_set_output, bool wait_all) {
     Thread* thread = GetCurrentThread();
     thread->wait_set_output = wait_set_output;
     thread->wait_all = wait_all;
-
-    // It's possible to call WaitSynchronizationN without any objects passed in...
-    if (wait_object != nullptr)
-        thread->wait_objects.push_back(wait_object);
-
-    ChangeThreadState(thread, ThreadStatus(THREADSTATUS_WAIT | (thread->status & THREADSTATUS_SUSPEND)));
+    thread->wait_objects = std::move(wait_objects);
+    thread->status = THREADSTATUS_WAIT_SYNCH;
 }
 
 void WaitCurrentThread_ArbitrateAddress(VAddr wait_address) {
     Thread* thread = GetCurrentThread();
     thread->wait_address = wait_address;
-    ChangeThreadState(thread, ThreadStatus(THREADSTATUS_WAIT | (thread->status & THREADSTATUS_SUSPEND)));
+    thread->status = THREADSTATUS_WAIT_ARB;
 }
 
-/// Event type for the thread wake up event
-static int ThreadWakeupEventType = -1;
 // TODO(yuriks): This can be removed if Thread objects are explicitly pooled in the future, allowing
 //               us to simply use a pool index or similar.
 static Kernel::HandleTable wakeup_callback_handle_table;
 
-/// Callback that will wake up the thread it was scheduled for
+/**
+ * Callback that will wake up the thread it was scheduled for
+ * @param thread_handle The handle of the thread that's been awoken
+ * @param cycles_late The number of CPU cycles that have passed since the desired wakeup time
+ */
 static void ThreadWakeupCallback(u64 thread_handle, int cycles_late) {
     SharedPtr<Thread> thread = wakeup_callback_handle_table.Get<Thread>((Handle)thread_handle);
     if (thread == nullptr) {
-        LOG_CRITICAL(Kernel, "Callback fired for invalid thread %08X", thread_handle);
+        LOG_CRITICAL(Kernel, "Callback fired for invalid thread %08X", (Handle)thread_handle);
         return;
     }
 
-    thread->SetWaitSynchronizationResult(ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
-        ErrorSummary::StatusChanged, ErrorLevel::Info));
+    if (thread->status == THREADSTATUS_WAIT_SYNCH) {
+        thread->SetWaitSynchronizationResult(ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
+                                                        ErrorSummary::StatusChanged, ErrorLevel::Info));
 
-    if (thread->wait_set_output)
-        thread->SetWaitSynchronizationOutput(-1);
+        if (thread->wait_set_output)
+            thread->SetWaitSynchronizationOutput(-1);
+    }
 
     thread->ResumeFromWait();
 }
 
-
 void Thread::WakeAfterDelay(s64 nanoseconds) {
     // Don't schedule a wakeup if the thread wants to wait forever
     if (nanoseconds == -1)
@@ -265,7 +248,7 @@ void Thread::WakeAfterDelay(s64 nanoseconds) {
 }
 
 void Thread::ReleaseWaitObject(WaitObject* wait_object) {
-    if (wait_objects.empty()) {
+    if (status != THREADSTATUS_WAIT_SYNCH || wait_objects.empty()) {
         LOG_CRITICAL(Kernel, "thread is not waiting on any objects!");
         return;
     }
@@ -307,34 +290,48 @@ void Thread::ReleaseWaitObject(WaitObject* wait_object) {
 }
 
 void Thread::ResumeFromWait() {
-    // Cancel any outstanding wakeup events
+    // Cancel any outstanding wakeup events for this thread
     CoreTiming::UnscheduleEvent(ThreadWakeupEventType, callback_handle);
 
-    status &= ~THREADSTATUS_WAIT;
-
-    // Remove this thread from all other WaitObjects
-    for (auto wait_object : wait_objects)
-        wait_object->RemoveWaitingThread(this);
-
-    wait_objects.clear();
-    wait_set_output = false;
-    wait_all = false;
-    wait_address = 0;
-
-    if (!(status & (THREADSTATUS_WAITSUSPEND | THREADSTATUS_DORMANT | THREADSTATUS_DEAD))) {
-        ChangeReadyState(this, true);
+    switch (status) {
+        case THREADSTATUS_WAIT_SYNCH:
+            // Remove this thread from all other WaitObjects
+            for (auto wait_object : wait_objects)
+                wait_object->RemoveWaitingThread(this);
+            break;
+        case THREADSTATUS_WAIT_ARB:
+        case THREADSTATUS_WAIT_SLEEP:
+            break;
+        case THREADSTATUS_RUNNING:
+        case THREADSTATUS_READY:
+            LOG_ERROR(Kernel, "Thread with object id %u has already resumed.", GetObjectId());
+            _dbg_assert_(Kernel, false);
+            return;
+        case THREADSTATUS_DEAD:
+            // This should never happen, as threads must complete before being stopped.
+            LOG_CRITICAL(Kernel, "Thread with object id %u cannot be resumed because it's DEAD.",
+                GetObjectId());
+            _dbg_assert_(Kernel, false);
+            return;
     }
+    
+    ready_queue.push_back(current_priority, this);
+    status = THREADSTATUS_READY;
 }
 
-/// Prints the thread queue for debugging purposes
+/**
+ * Prints the thread queue for debugging purposes
+ */
 static void DebugThreadQueue() {
     Thread* thread = GetCurrentThread();
     if (!thread) {
-        return;
+        LOG_DEBUG(Kernel, "Current: NO CURRENT THREAD");
+    } else {
+        LOG_DEBUG(Kernel, "0x%02X %u (current)", thread->current_priority, GetCurrentThread()->GetObjectId());
     }
-    LOG_DEBUG(Kernel, "0x%02X %u (current)", thread->current_priority, GetCurrentThread()->GetObjectId());
+
     for (auto& t : thread_list) {
-        s32 priority = thread_ready_queue.contains(t.get());
+        s32 priority = ready_queue.contains(t.get());
         if (priority != -1) {
             LOG_DEBUG(Kernel, "0x%02X %u", priority, t->GetObjectId());
         }
@@ -342,14 +339,7 @@ static void DebugThreadQueue() {
 }
 
 ResultVal<SharedPtr<Thread>> Thread::Create(std::string name, VAddr entry_point, s32 priority,
-        u32 arg, s32 processor_id, VAddr stack_top, u32 stack_size) {
-    if (stack_size < 0x200) {
-        LOG_ERROR(Kernel, "(name=%s): invalid stack_size=0x%08X", name.c_str(), stack_size);
-        // TODO: Verify error
-        return ResultCode(ErrorDescription::InvalidSize, ErrorModule::Kernel,
-                ErrorSummary::InvalidArgument, ErrorLevel::Permanent);
-    }
-
+        u32 arg, s32 processor_id, VAddr stack_top) {
     if (priority < THREADPRIO_HIGHEST || priority > THREADPRIO_LOWEST) {
         s32 new_priority = CLAMP(priority, THREADPRIO_HIGHEST, THREADPRIO_LOWEST);
         LOG_WARNING(Kernel_SVC, "(name=%s): invalid priority=%d, clamping to %d",
@@ -369,13 +359,12 @@ ResultVal<SharedPtr<Thread>> Thread::Create(std::string name, VAddr entry_point,
     SharedPtr<Thread> thread(new Thread);
 
     thread_list.push_back(thread);
-    thread_ready_queue.prepare(priority);
+    ready_queue.prepare(priority);
 
-    thread->thread_id = next_thread_id++;
+    thread->thread_id = NewThreadId();
     thread->status = THREADSTATUS_DORMANT;
     thread->entry_point = entry_point;
     thread->stack_top = stack_top;
-    thread->stack_size = stack_size;
     thread->initial_priority = thread->current_priority = priority;
     thread->processor_id = processor_id;
     thread->wait_set_output = false;
@@ -385,75 +374,74 @@ 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();
 
-    ResetThread(thread.get(), arg, 0);
-    CallThread(thread.get());
+    // 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);
+
+    ready_queue.push_back(thread->current_priority, thread.get());
+    thread->status = THREADSTATUS_READY;
 
     return MakeResult<SharedPtr<Thread>>(std::move(thread));
 }
 
-/// Set the priority of the thread specified by handle
-void Thread::SetPriority(s32 priority) {
-    // If priority is invalid, clamp to valid range
-    if (priority < THREADPRIO_HIGHEST || priority > THREADPRIO_LOWEST) {
-        s32 new_priority = CLAMP(priority, THREADPRIO_HIGHEST, THREADPRIO_LOWEST);
-        LOG_WARNING(Kernel_SVC, "invalid priority=%d, clamping to %d", priority, new_priority);
+// TODO(peachum): Remove this. Range checking should be done, and an appropriate error should be returned.
+static void ClampPriority(const Thread* thread, s32* priority) {
+    if (*priority < THREADPRIO_HIGHEST || *priority > THREADPRIO_LOWEST) {
+        _dbg_assert_msg_(Kernel, false, "Application passed an out of range priority. An error should be returned.");
+
+        s32 new_priority = CLAMP(*priority, THREADPRIO_HIGHEST, THREADPRIO_LOWEST);
+        LOG_WARNING(Kernel_SVC, "(name=%s): invalid priority=%d, clamping to %d",
+                    thread->name.c_str(), *priority, new_priority);
         // TODO(bunnei): Clamping to a valid priority is not necessarily correct behavior... Confirm
         // validity of this
-        priority = new_priority;
+        *priority = new_priority;
     }
+}
 
-    // Change thread priority
-    s32 old = current_priority;
-    thread_ready_queue.remove(old, this);
-    current_priority = priority;
-    thread_ready_queue.prepare(current_priority);
+void Thread::SetPriority(s32 priority) {
+    ClampPriority(this, &priority);
 
-    // Change thread status to "ready" and push to ready queue
-    if (IsRunning()) {
-        status = (status & ~THREADSTATUS_RUNNING) | THREADSTATUS_READY;
+    if (current_priority == priority) {
+        return;
     }
-    if (IsReady()) {
-        thread_ready_queue.push_back(current_priority, this);
+
+    if (status == THREADSTATUS_READY) {
+        // If thread was ready, adjust queues
+        ready_queue.remove(current_priority, this);
+        ready_queue.prepare(priority);
+        ready_queue.push_back(priority, this);
     }
+    
+    current_priority = priority;
 }
 
 SharedPtr<Thread> SetupIdleThread() {
     // We need to pass a few valid values to get around parameter checking in Thread::Create.
     auto thread = Thread::Create("idle", Memory::KERNEL_MEMORY_VADDR, THREADPRIO_LOWEST, 0,
-            THREADPROCESSORID_0, 0, Kernel::DEFAULT_STACK_SIZE).MoveFrom();
+            THREADPROCESSORID_0, 0).MoveFrom();
 
     thread->idle = true;
-    CallThread(thread.get());
     return thread;
 }
 
-SharedPtr<Thread> SetupMainThread(s32 priority, u32 stack_size) {
+SharedPtr<Thread> SetupMainThread(u32 stack_size, u32 entry_point, s32 priority) {
+    _dbg_assert_(Kernel, !GetCurrentThread());
+
     // Initialize new "main" thread
-    auto thread_res = Thread::Create("main", Core::g_app_core->GetPC(), priority, 0,
-            THREADPROCESSORID_0, Memory::SCRATCHPAD_VADDR_END, stack_size);
-    // TODO(yuriks): Propagate error
-    _dbg_assert_(Kernel, thread_res.Succeeded());
-    SharedPtr<Thread> thread = std::move(*thread_res);
-
-    // If running another thread already, set it to "ready" state
-    Thread* cur = GetCurrentThread();
-    if (cur && cur->IsRunning()) {
-        ChangeReadyState(cur, true);
-    }
+    auto thread_res = Thread::Create("main", entry_point, priority, 0,
+            THREADPROCESSORID_0, Memory::SCRATCHPAD_VADDR_END);
+
+    SharedPtr<Thread> thread = thread_res.MoveFrom();
 
     // Run new "main" thread
-    current_thread = thread.get();
-    thread->status = THREADSTATUS_RUNNING;
-    Core::g_app_core->LoadContext(thread->context);
+    SwitchContext(thread.get());
 
     return thread;
 }
 
-
-/// Reschedules to the next available thread (call after current thread is suspended)
 void Reschedule() {
     Thread* prev = GetCurrentThread();
-    Thread* next = NextThread();
+    Thread* next = PopNextReadyThread();
     HLE::g_reschedule = false;
 
     if (next != nullptr) {
@@ -480,8 +468,10 @@ void Thread::SetWaitSynchronizationOutput(s32 output) {
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 
 void ThreadingInit() {
-    next_thread_id = INITIAL_THREAD_ID;
     ThreadWakeupEventType = CoreTiming::RegisterEvent("ThreadWakeupCallback", ThreadWakeupCallback);
+
+    // Setup the idle thread
+    SetupIdleThread();
 }
 
 void ThreadingShutdown() {
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index 633bb7c98..cfd073a70 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -31,13 +31,13 @@ enum ThreadProcessorId {
 };
 
 enum ThreadStatus {
-    THREADSTATUS_RUNNING        = 1,
-    THREADSTATUS_READY          = 2,
-    THREADSTATUS_WAIT           = 4,
-    THREADSTATUS_SUSPEND        = 8,
-    THREADSTATUS_DORMANT        = 16,
-    THREADSTATUS_DEAD           = 32,
-    THREADSTATUS_WAITSUSPEND    = THREADSTATUS_WAIT | THREADSTATUS_SUSPEND
+    THREADSTATUS_RUNNING,       ///< Currently running
+    THREADSTATUS_READY,         ///< Ready to run
+    THREADSTATUS_WAIT_ARB,      ///< Waiting on an address arbiter
+    THREADSTATUS_WAIT_SLEEP,    ///< Waiting due to a SleepThread SVC
+    THREADSTATUS_WAIT_SYNCH,    ///< Waiting due to a WaitSynchronization SVC
+    THREADSTATUS_DORMANT,       ///< Created but not yet made ready
+    THREADSTATUS_DEAD           ///< Run to completion, or forcefully terminated
 };
 
 namespace Kernel {
@@ -46,8 +46,19 @@ class Mutex;
 
 class Thread final : public WaitObject {
 public:
+    /**
+     * Creates and returns a new thread. The new thread is immediately scheduled
+     * @param name The friendly name desired for the thread
+     * @param entry_point The address at which the thread should start execution
+     * @param priority The thread's priority
+     * @param arg User data to pass to the thread
+     * @param processor_id The ID(s) of the processors on which the thread is desired to be run
+     * @param stack_top The address of the thread's stack top
+     * @param stack_size The size of the thread's stack
+     * @return A shared pointer to the newly created thread
+     */
     static ResultVal<SharedPtr<Thread>> Create(std::string name, VAddr entry_point, s32 priority,
-        u32 arg, s32 processor_id, VAddr stack_top, u32 stack_size);
+        u32 arg, s32 processor_id, VAddr stack_top);
 
     std::string GetName() const override { return name; }
     std::string GetTypeName() const override { return "Thread"; }
@@ -55,22 +66,32 @@ public:
     static const HandleType HANDLE_TYPE = HandleType::Thread;
     HandleType GetHandleType() const override { return HANDLE_TYPE; }
 
-    inline bool IsRunning() const { return (status & THREADSTATUS_RUNNING) != 0; }
-    inline bool IsStopped() const { return (status & THREADSTATUS_DORMANT) != 0; }
-    inline bool IsReady() const { return (status & THREADSTATUS_READY) != 0; }
-    inline bool IsWaiting() const { return (status & THREADSTATUS_WAIT) != 0; }
-    inline bool IsSuspended() const { return (status & THREADSTATUS_SUSPEND) != 0; }
-    inline bool IsIdle() const { return idle; }
-
     bool ShouldWait() override;
     void Acquire() override;
 
+    /**
+     * Checks if the thread is an idle (stub) thread
+     * @return True if the thread is an idle (stub) thread, false otherwise
+     */
+    inline bool IsIdle() const { return idle; }
+
+    /**
+     * Gets the thread's current priority
+     * @return The current thread's priority
+     */
     s32 GetPriority() const { return current_priority; }
+
+    /**
+     * Sets the thread's current priority
+     * @param priority The new priority
+     */
     void SetPriority(s32 priority);
 
+    /**
+     * Gets the thread's thread ID
+     * @return The thread's ID
+     */
     u32 GetThreadId() const { return thread_id; }
-
-    void Stop(const char* reason);
     
     /**
      * Release an acquired wait object
@@ -78,12 +99,14 @@ public:
      */
     void ReleaseWaitObject(WaitObject* wait_object);
 
-    /// Resumes a thread from waiting by marking it as "ready"
+    /**
+     * Resumes a thread from waiting
+     */
     void ResumeFromWait();
 
     /**
-    * Schedules an event to wake up the specified thread after the specified delay.
-    * @param nanoseconds The time this thread will be allowed to sleep for.
+    * Schedules an event to wake up the specified thread after the specified delay
+    * @param nanoseconds The time this thread will be allowed to sleep for
     */
     void WakeAfterDelay(s64 nanoseconds);
 
@@ -99,6 +122,11 @@ public:
      */
     void SetWaitSynchronizationOutput(s32 output);
 
+    /**
+     * Stops a thread, invalidating it from further use
+     */
+    void Stop();
+
     Core::ThreadContext context;
 
     u32 thread_id;
@@ -106,7 +134,6 @@ public:
     u32 status;
     u32 entry_point;
     u32 stack_top;
-    u32 stack_size;
 
     s32 initial_priority;
     s32 current_priority;
@@ -136,31 +163,49 @@ private:
 
 extern SharedPtr<Thread> g_main_thread;
 
-/// Sets up the primary application thread
-SharedPtr<Thread> SetupMainThread(s32 priority, u32 stack_size);
+/**
+ * Sets up the primary application thread
+ * @param stack_size The size of the thread's stack
+ * @param entry_point The address at which the thread should start execution
+ * @param priority The priority to give the main thread
+ * @return A shared pointer to the main thread
+ */
+SharedPtr<Thread> SetupMainThread(u32 stack_size, u32 entry_point, s32 priority);
 
-/// Reschedules to the next available thread (call after current thread is suspended)
+/**
+ * Reschedules to the next available thread (call after current thread is suspended)
+ */
 void Reschedule();
 
-/// Arbitrate the highest priority thread that is waiting
+/**
+ * Arbitrate the highest priority thread that is waiting
+ * @param address The address for which waiting threads should be arbitrated
+ */
 Thread* ArbitrateHighestPriorityThread(u32 address);
 
-/// Arbitrate all threads currently waiting...
+/**
+ * Arbitrate all threads currently waiting.
+ * @param address The address for which waiting threads should be arbitrated
+ */
 void ArbitrateAllThreads(u32 address);
 
-/// Gets the current thread
+/**
+ * Gets the current thread
+ */
 Thread* GetCurrentThread();
 
-/// Waits the current thread on a sleep
+/**
+ * Waits the current thread on a sleep
+ */
 void WaitCurrentThread_Sleep();
 
 /**
  * Waits the current thread from a WaitSynchronization call
- * @param wait_object Kernel object that we are waiting on
+ * @param wait_objects Kernel objects that we are waiting on
  * @param wait_set_output If true, set the output parameter on thread wakeup (for WaitSynchronizationN only)
  * @param wait_all If true, wait on all objects before resuming (for WaitSynchronizationN only)
  */
-void WaitCurrentThread_WaitSynchronization(SharedPtr<WaitObject> wait_object, bool wait_set_output, bool wait_all);
+void WaitCurrentThread_WaitSynchronization(std::vector<SharedPtr<WaitObject>> wait_objects, bool wait_set_output, bool wait_all);
 
 /**
  * Waits the current thread from an ArbitrateAddress call
@@ -172,14 +217,18 @@ void WaitCurrentThread_ArbitrateAddress(VAddr wait_address);
  * Sets up the idle thread, this is a thread that is intended to never execute instructions,
  * only to advance the timing. It is scheduled when there are no other ready threads in the thread queue
  * and will try to yield on every call.
- * @returns The handle of the idle thread
+ * @return The handle of the idle thread
  */
 SharedPtr<Thread> SetupIdleThread();
 
-/// Initialize threading
+/**
+ * Initialize threading
+ */
 void ThreadingInit();
 
-/// Shutdown threading
+/**
+ * Shutdown threading
+ */
 void ThreadingShutdown();
 
 } // namespace
diff --git a/src/core/hle/svc.cpp b/src/core/hle/svc.cpp
index c4866fcce..96da29923 100644
--- a/src/core/hle/svc.cpp
+++ b/src/core/hle/svc.cpp
@@ -150,7 +150,7 @@ static ResultCode WaitSynchronization1(Handle handle, s64 nano_seconds) {
     if (object->ShouldWait()) {
 
         object->AddWaitingThread(Kernel::GetCurrentThread());
-        Kernel::WaitCurrentThread_WaitSynchronization(object, false, false);
+        Kernel::WaitCurrentThread_WaitSynchronization({ object }, false, false);
 
         // Create an event to wake the thread up after the specified nanosecond delay has passed
         Kernel::GetCurrentThread()->WakeAfterDelay(nano_seconds);
@@ -212,7 +212,6 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
         // NOTE: This should deadlock the current thread if no timeout was specified
         if (!wait_all) {
             wait_thread = true;
-            Kernel::WaitCurrentThread_WaitSynchronization(nullptr, true, wait_all);
         }
     }
 
@@ -222,12 +221,17 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
     if (wait_thread) {
 
         // Actually wait the current thread on each object if we decided to wait...
+        std::vector<SharedPtr<Kernel::WaitObject>> wait_objects;
+        wait_objects.reserve(handle_count);
+        
         for (int i = 0; i < handle_count; ++i) {
             auto object = Kernel::g_handle_table.GetWaitObject(handles[i]);
             object->AddWaitingThread(Kernel::GetCurrentThread());
-            Kernel::WaitCurrentThread_WaitSynchronization(object, true, wait_all);
+            wait_objects.push_back(object);
         }
 
+        Kernel::WaitCurrentThread_WaitSynchronization(std::move(wait_objects), true, wait_all);
+
         // Create an event to wake the thread up after the specified nanosecond delay has passed
         Kernel::GetCurrentThread()->WakeAfterDelay(nano_seconds);
 
@@ -319,7 +323,7 @@ static ResultCode CreateThread(u32* out_handle, u32 priority, u32 entry_point, u
     }
 
     CASCADE_RESULT(SharedPtr<Thread> thread, Kernel::Thread::Create(
-            name, entry_point, priority, arg, processor_id, stack_top, Kernel::DEFAULT_STACK_SIZE));
+            name, entry_point, priority, arg, processor_id, stack_top));
     CASCADE_RESULT(*out_handle, Kernel::g_handle_table.Create(std::move(thread)));
 
     LOG_TRACE(Kernel_SVC, "called entrypoint=0x%08X (%s), arg=0x%08X, stacktop=0x%08X, "
@@ -338,7 +342,7 @@ static ResultCode CreateThread(u32* out_handle, u32 priority, u32 entry_point, u
 static void ExitThread() {
     LOG_TRACE(Kernel_SVC, "called, pc=0x%08X", Core::g_app_core->GetPC());
 
-    Kernel::GetCurrentThread()->Stop(__func__);
+    Kernel::GetCurrentThread()->Stop();
     HLE::Reschedule(__func__);
 }