From 8634b8cb83755b6c6554faa11c0e488d2ad21f90 Mon Sep 17 00:00:00 2001
From: Subv <subv2112@gmail.com>
Date: Sat, 3 Dec 2016 22:38:14 -0500
Subject: Threading: Reworked the way our scheduler works.

Threads will now be awakened when the objects they're waiting on are signaled, instead of repeating the WaitSynchronization call every now and then.

The scheduler is now called once after every SVC call, and once after a thread is awakened from sleep by its timeout callback.

This new implementation is based off reverse-engineering of the real kernel.

See https://gist.github.com/Subv/02f29bd9f1e5deb7aceea1e8f019c8f4 for a more detailed description of how the real kernel handles rescheduling.
---
 src/citra_qt/debugger/wait_tree.cpp     |   2 +-
 src/core/hle/kernel/address_arbiter.cpp |   2 -
 src/core/hle/kernel/kernel.cpp          |  59 ++++++++++-
 src/core/hle/kernel/kernel.h            |   3 +
 src/core/hle/kernel/thread.cpp          |  97 +----------------
 src/core/hle/kernel/thread.h            |  22 ++--
 src/core/hle/kernel/timer.cpp           |   4 -
 src/core/hle/svc.cpp                    | 181 ++++++++++++++++++--------------
 8 files changed, 180 insertions(+), 190 deletions(-)

diff --git a/src/citra_qt/debugger/wait_tree.cpp b/src/citra_qt/debugger/wait_tree.cpp
index be5a51e52..8fc3e37e0 100644
--- a/src/citra_qt/debugger/wait_tree.cpp
+++ b/src/citra_qt/debugger/wait_tree.cpp
@@ -230,7 +230,7 @@ std::vector<std::unique_ptr<WaitTreeItem>> WaitTreeThread::GetChildren() const {
         list.push_back(std::make_unique<WaitTreeMutexList>(thread.held_mutexes));
     }
     if (thread.status == THREADSTATUS_WAIT_SYNCH) {
-        list.push_back(std::make_unique<WaitTreeObjectList>(thread.wait_objects, thread.wait_all));
+        list.push_back(std::make_unique<WaitTreeObjectList>(thread.wait_objects, !thread.wait_objects.empty()));
     }
 
     return list;
diff --git a/src/core/hle/kernel/address_arbiter.cpp b/src/core/hle/kernel/address_arbiter.cpp
index 37eec4c84..b5a0cc3a3 100644
--- a/src/core/hle/kernel/address_arbiter.cpp
+++ b/src/core/hle/kernel/address_arbiter.cpp
@@ -79,8 +79,6 @@ ResultCode AddressArbiter::ArbitrateAddress(ArbitrationType type, VAddr address,
                           ErrorSummary::WrongArgument, ErrorLevel::Usage);
     }
 
-    HLE::Reschedule(__func__);
-
     // The calls that use a timeout seem to always return a Timeout error even if they did not put
     // the thread to sleep
     if (type == ArbitrationType::WaitIfLessThanWithTimeout ||
diff --git a/src/core/hle/kernel/kernel.cpp b/src/core/hle/kernel/kernel.cpp
index 0c8752670..be7a5a6d8 100644
--- a/src/core/hle/kernel/kernel.cpp
+++ b/src/core/hle/kernel/kernel.cpp
@@ -31,13 +31,62 @@ void WaitObject::RemoveWaitingThread(Thread* thread) {
         waiting_threads.erase(itr);
 }
 
-void WaitObject::WakeupAllWaitingThreads() {
-    for (auto thread : waiting_threads)
-        thread->ResumeFromWait();
+SharedPtr<Thread> WaitObject::GetHighestPriorityReadyThread() {
+    // Remove the threads that are ready or already running from our waitlist
+    waiting_threads.erase(std::remove_if(waiting_threads.begin(), waiting_threads.end(), [](SharedPtr<Thread> thread) -> bool {
+        return thread->status == THREADSTATUS_RUNNING || thread->status == THREADSTATUS_READY;
+    }), waiting_threads.end());
+
+    if (waiting_threads.empty())
+        return nullptr;
 
-    waiting_threads.clear();
+    auto candidate_threads = waiting_threads;
 
-    HLE::Reschedule(__func__);
+    // Eliminate all threads that are waiting on more than one object, and not all of them are ready
+    candidate_threads.erase(std::remove_if(candidate_threads.begin(), candidate_threads.end(), [](SharedPtr<Thread> thread) -> bool {
+        for (auto object : thread->wait_objects)
+            if (object->ShouldWait())
+                return true;
+        return false;
+    }), candidate_threads.end());
+
+    // Return the thread with the lowest priority value (The one with the highest priority)
+    auto thread_itr = std::min_element(candidate_threads.begin(), candidate_threads.end(), [](const SharedPtr<Thread>& lhs, const SharedPtr<Thread>& rhs) {
+        return lhs->current_priority < rhs->current_priority;
+    });
+
+    if (thread_itr == candidate_threads.end())
+        return nullptr;
+
+    return *thread_itr;
+}
+
+void WaitObject::WakeupAllWaitingThreads() {
+    // Wake up all threads that can be awoken, in priority order
+    while (auto thread = GetHighestPriorityReadyThread()) {
+        if (thread->wait_objects.empty()) {
+            Acquire();
+            // Set the output index of the WaitSynchronizationN call to the index of this object.
+            if (thread->wait_set_output) {
+                thread->SetWaitSynchronizationOutput(thread->GetWaitObjectIndex(this));
+                thread->wait_set_output = false;
+            }
+        } else {
+            for (auto object : thread->wait_objects) {
+                object->Acquire();
+                // Remove the thread from the object's waitlist
+                object->RemoveWaitingThread(thread.get());
+            }
+            // Note: This case doesn't update the output index of WaitSynchronizationN.
+            // Clear the thread's waitlist
+            thread->wait_objects.clear();
+        }
+
+        // Set the result of the call to WaitSynchronization to RESULT_SUCCESS
+        thread->SetWaitSynchronizationResult(RESULT_SUCCESS);
+        thread->ResumeFromWait();
+        // Note: Removing the thread from the object's waitlist will be done by GetHighestPriorityReadyThread
+    }
 }
 
 const std::vector<SharedPtr<Thread>>& WaitObject::GetWaitingThreads() const {
diff --git a/src/core/hle/kernel/kernel.h b/src/core/hle/kernel/kernel.h
index 231cf7b75..eb5a3bf7e 100644
--- a/src/core/hle/kernel/kernel.h
+++ b/src/core/hle/kernel/kernel.h
@@ -155,6 +155,9 @@ public:
     /// Wake up all threads waiting on this object
     void WakeupAllWaitingThreads();
 
+    /// Obtains the highest priority thread that is ready to run from this object's waiting list.
+    SharedPtr<Thread> GetHighestPriorityReadyThread();
+
     /// Get a const reference to the waiting threads list for debug use
     const std::vector<SharedPtr<Thread>>& GetWaitingThreads() const;
 
diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index 84d6d24c6..49ed9d899 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -120,8 +120,6 @@ void Thread::Stop() {
     u32 tls_slot =
         ((tls_address - Memory::TLS_AREA_VADDR) % Memory::PAGE_SIZE) / Memory::TLS_ENTRY_SIZE;
     Kernel::g_current_process->tls_slots[tls_page].reset(tls_slot);
-
-    HLE::Reschedule(__func__);
 }
 
 Thread* ArbitrateHighestPriorityThread(u32 address) {
@@ -180,50 +178,6 @@ static void PriorityBoostStarvedThreads() {
     }
 }
 
-/**
- * Gets the registers for timeout parameter of the next WaitSynchronization call.
- * @param thread a pointer to the thread that is ready to call WaitSynchronization
- * @returns a tuple of two register pointers to low and high part of the timeout parameter
- */
-static std::tuple<u32*, u32*> GetWaitSynchTimeoutParameterRegister(Thread* thread) {
-    bool thumb_mode = (thread->context.cpsr & TBIT) != 0;
-    u16 thumb_inst = Memory::Read16(thread->context.pc & 0xFFFFFFFE);
-    u32 inst = Memory::Read32(thread->context.pc & 0xFFFFFFFC) & 0x0FFFFFFF;
-
-    if ((thumb_mode && thumb_inst == 0xDF24) || (!thumb_mode && inst == 0x0F000024)) {
-        // svc #0x24 (WaitSynchronization1)
-        return std::make_tuple(&thread->context.cpu_registers[2],
-                               &thread->context.cpu_registers[3]);
-    } else if ((thumb_mode && thumb_inst == 0xDF25) || (!thumb_mode && inst == 0x0F000025)) {
-        // svc #0x25 (WaitSynchronizationN)
-        return std::make_tuple(&thread->context.cpu_registers[0],
-                               &thread->context.cpu_registers[4]);
-    }
-
-    UNREACHABLE();
-}
-
-/**
- * Updates the WaitSynchronization timeout parameter according to the difference
- * between ticks of the last WaitSynchronization call and the incoming one.
- * @param timeout_low a pointer to the register for the low part of the timeout parameter
- * @param timeout_high a pointer to the register for the high part of the timeout parameter
- * @param last_tick tick of the last WaitSynchronization call
- */
-static void UpdateTimeoutParameter(u32* timeout_low, u32* timeout_high, u64 last_tick) {
-    s64 timeout = ((s64)*timeout_high << 32) | *timeout_low;
-
-    if (timeout != -1) {
-        timeout -= cyclesToUs(CoreTiming::GetTicks() - last_tick) * 1000; // in nanoseconds
-
-        if (timeout < 0)
-            timeout = 0;
-
-        *timeout_low = timeout & 0xFFFFFFFF;
-        *timeout_high = timeout >> 32;
-    }
-}
-
 /**
  * Switches the CPU's active thread context to that of the specified thread
  * @param new_thread The thread to switch to
@@ -254,32 +208,6 @@ static void SwitchContext(Thread* new_thread) {
 
         current_thread = new_thread;
 
-        // If the thread was waited by a svcWaitSynch call, step back PC by one instruction to rerun
-        // the SVC when the thread wakes up. This is necessary to ensure that the thread can acquire
-        // the requested wait object(s) before continuing.
-        if (new_thread->waitsynch_waited) {
-            // CPSR flag indicates CPU mode
-            bool thumb_mode = (new_thread->context.cpsr & TBIT) != 0;
-
-            // SVC instruction is 2 bytes for THUMB, 4 bytes for ARM
-            new_thread->context.pc -= thumb_mode ? 2 : 4;
-
-            // Get the register for timeout parameter
-            u32 *timeout_low, *timeout_high;
-            std::tie(timeout_low, timeout_high) = GetWaitSynchTimeoutParameterRegister(new_thread);
-
-            // Update the timeout parameter
-            UpdateTimeoutParameter(timeout_low, timeout_high, new_thread->last_running_ticks);
-        }
-
-        // Clean up the thread's wait_objects, they'll be restored if needed during
-        // the svcWaitSynchronization call
-        for (size_t i = 0; i < new_thread->wait_objects.size(); ++i) {
-            SharedPtr<WaitObject> object = new_thread->wait_objects[i];
-            object->RemoveWaitingThread(new_thread);
-        }
-        new_thread->wait_objects.clear();
-
         ready_queue.remove(new_thread->current_priority, new_thread);
         new_thread->status = THREADSTATUS_RUNNING;
 
@@ -319,17 +247,13 @@ static Thread* PopNextReadyThread() {
 void WaitCurrentThread_Sleep() {
     Thread* thread = GetCurrentThread();
     thread->status = THREADSTATUS_WAIT_SLEEP;
-
-    HLE::Reschedule(__func__);
 }
 
 void WaitCurrentThread_WaitSynchronization(std::vector<SharedPtr<WaitObject>> wait_objects,
-                                           bool wait_set_output, bool wait_all) {
+                                           bool wait_set_output) {
     Thread* thread = GetCurrentThread();
     thread->wait_set_output = wait_set_output;
-    thread->wait_all = wait_all;
     thread->wait_objects = std::move(wait_objects);
-    thread->waitsynch_waited = true;
     thread->status = THREADSTATUS_WAIT_SYNCH;
 }
 
@@ -351,15 +275,11 @@ static void ThreadWakeupCallback(u64 thread_handle, int cycles_late) {
         return;
     }
 
-    thread->waitsynch_waited = false;
-
     if (thread->status == THREADSTATUS_WAIT_SYNCH || thread->status == THREADSTATUS_WAIT_ARB) {
+        thread->wait_set_output = false;
         thread->SetWaitSynchronizationResult(ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
                                                         ErrorSummary::StatusChanged,
                                                         ErrorLevel::Info));
-
-        if (thread->wait_set_output)
-            thread->SetWaitSynchronizationOutput(-1);
     }
 
     thread->ResumeFromWait();
@@ -399,6 +319,7 @@ void Thread::ResumeFromWait() {
 
     ready_queue.push_back(current_priority, this);
     status = THREADSTATUS_READY;
+    HLE::Reschedule(__func__);
 }
 
 /**
@@ -494,13 +415,11 @@ ResultVal<SharedPtr<Thread>> Thread::Create(std::string name, VAddr entry_point,
     thread->last_running_ticks = CoreTiming::GetTicks();
     thread->processor_id = processor_id;
     thread->wait_set_output = false;
-    thread->wait_all = false;
     thread->wait_objects.clear();
     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->waitsynch_waited = false;
 
     // Find the next available TLS index, and mark it as used
     auto& tls_slots = Kernel::g_current_process->tls_slots;
@@ -555,8 +474,6 @@ ResultVal<SharedPtr<Thread>> Thread::Create(std::string name, VAddr entry_point,
     ready_queue.push_back(thread->current_priority, thread.get());
     thread->status = THREADSTATUS_READY;
 
-    HLE::Reschedule(__func__);
-
     return MakeResult<SharedPtr<Thread>>(std::move(thread));
 }
 
@@ -619,14 +536,6 @@ void Reschedule() {
 
     HLE::DoneRescheduling();
 
-    // Don't bother switching to the same thread.
-    // But if the thread was waiting on objects, we still need to switch it
-    // to perform PC modification, change state to RUNNING, etc.
-    // This occurs in the case when an object the thread is waiting on immediately wakes up
-    // the current thread before Reschedule() is called.
-    if (next == cur && (next == nullptr || next->waitsynch_waited == false))
-        return;
-
     if (cur && next) {
         LOG_TRACE(Kernel, "context switch %u -> %u", cur->GetObjectId(), next->GetObjectId());
     } else if (cur) {
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index e0ffcea8a..63b97b74f 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -5,6 +5,7 @@
 #pragma once
 
 #include <string>
+#include <unordered_map>
 #include <vector>
 #include <boost/container/flat_set.hpp>
 #include "common/common_types.h"
@@ -124,6 +125,16 @@ public:
      */
     void SetWaitSynchronizationOutput(s32 output);
 
+    /**
+     * Retrieves the index that this particular object occupies in the list of objects
+     * that the thread passed to WaitSynchronizationN.
+     * It is used to set the output value of WaitSynchronizationN when the thread is awakened.
+     * @param object Object to query the index of.
+     */
+    s32 GetWaitObjectIndex(WaitObject* object) {
+        return wait_objects_index[object->GetObjectId()];
+    }
+
     /**
      * Stops a thread, invalidating it from further use
      */
@@ -154,16 +165,16 @@ public:
 
     VAddr tls_address; ///< Virtual address of the Thread Local Storage of the thread
 
-    bool waitsynch_waited; ///< Set to true if the last svcWaitSynch call caused the thread to wait
-
     /// Mutexes currently held by this thread, which will be released when it exits.
     boost::container::flat_set<SharedPtr<Mutex>> held_mutexes;
 
     SharedPtr<Process> owner_process;                ///< Process that owns this thread
     std::vector<SharedPtr<WaitObject>> wait_objects; ///< Objects that the thread is waiting on
+    std::unordered_map<int, s32> wait_objects_index; ///< Mapping of Object ids to their position in the last waitlist that this object waited on.
+
     VAddr wait_address;   ///< If waiting on an AddressArbiter, this is the arbitration address
-    bool wait_all;        ///< True if the thread is waiting on all objects before resuming
-    bool wait_set_output; ///< True if the output parameter should be set on thread wakeup
+
+    bool wait_set_output; ///< True if the WaitSynchronizationN output parameter should be set on thread wakeup
 
     std::string name;
 
@@ -215,10 +226,9 @@ void WaitCurrentThread_Sleep();
  * @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(std::vector<SharedPtr<WaitObject>> wait_objects,
-                                           bool wait_set_output, bool wait_all);
+                                           bool wait_set_output);
 
 /**
  * Waits the current thread from an ArbitrateAddress call
diff --git a/src/core/hle/kernel/timer.cpp b/src/core/hle/kernel/timer.cpp
index eac181f4e..b50cf520d 100644
--- a/src/core/hle/kernel/timer.cpp
+++ b/src/core/hle/kernel/timer.cpp
@@ -60,14 +60,10 @@ void Timer::Set(s64 initial, s64 interval) {
     u64 initial_microseconds = initial / 1000;
     CoreTiming::ScheduleEvent(usToCycles(initial_microseconds), timer_callback_event_type,
                               callback_handle);
-
-    HLE::Reschedule(__func__);
 }
 
 void Timer::Cancel() {
     CoreTiming::UnscheduleEvent(timer_callback_event_type, callback_handle);
-
-    HLE::Reschedule(__func__);
 }
 
 void Timer::Clear() {
diff --git a/src/core/hle/svc.cpp b/src/core/hle/svc.cpp
index c6b80dc50..061692af8 100644
--- a/src/core/hle/svc.cpp
+++ b/src/core/hle/svc.cpp
@@ -249,27 +249,30 @@ static ResultCode WaitSynchronization1(Handle handle, s64 nano_seconds) {
     auto object = Kernel::g_handle_table.GetWaitObject(handle);
     Kernel::Thread* thread = Kernel::GetCurrentThread();
 
-    thread->waitsynch_waited = false;
-
     if (object == nullptr)
         return ERR_INVALID_HANDLE;
 
     LOG_TRACE(Kernel_SVC, "called handle=0x%08X(%s:%s), nanoseconds=%lld", handle,
               object->GetTypeName().c_str(), object->GetName().c_str(), nano_seconds);
 
-    HLE::Reschedule(__func__);
-
-    // Check for next thread to schedule
     if (object->ShouldWait()) {
 
+        if (nano_seconds == 0)
+            return ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
+                              ErrorSummary::StatusChanged,
+                              ErrorLevel::Info);
+
         object->AddWaitingThread(thread);
-        Kernel::WaitCurrentThread_WaitSynchronization({object}, false, false);
+        thread->status = THREADSTATUS_WAIT_SYNCH;
 
         // Create an event to wake the thread up after the specified nanosecond delay has passed
         thread->WakeAfterDelay(nano_seconds);
 
-        // NOTE: output of this SVC will be set later depending on how the thread resumes
-        return HLE::RESULT_INVALID;
+        // Note: The output of this SVC will be set to RESULT_SUCCESS if the thread resumes due to a signal in one of its wait objects.
+        // Otherwise we retain the default value of timeout.
+        return ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
+                               ErrorSummary::StatusChanged,
+                               ErrorLevel::Info);
     }
 
     object->Acquire();
@@ -283,8 +286,6 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
     bool wait_thread = !wait_all;
     int handle_index = 0;
     Kernel::Thread* thread = Kernel::GetCurrentThread();
-    bool was_waiting = thread->waitsynch_waited;
-    thread->waitsynch_waited = false;
 
     // Check if 'handles' is invalid
     if (handles == nullptr)
@@ -300,90 +301,113 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
         return ResultCode(ErrorDescription::OutOfRange, ErrorModule::OS,
                           ErrorSummary::InvalidArgument, ErrorLevel::Usage);
 
-    // If 'handle_count' is non-zero, iterate through each handle and wait the current thread if
-    // necessary
-    if (handle_count != 0) {
-        bool selected = false; // True once an object has been selected
-
-        Kernel::SharedPtr<Kernel::WaitObject> wait_object;
-
-        for (int i = 0; i < handle_count; ++i) {
-            auto object = Kernel::g_handle_table.GetWaitObject(handles[i]);
-            if (object == nullptr)
-                return ERR_INVALID_HANDLE;
-
-            // Check if the current thread should wait on this object...
-            if (object->ShouldWait()) {
-
-                // Check we are waiting on all objects...
-                if (wait_all)
-                    // Wait the thread
-                    wait_thread = true;
-            } else {
-                // Do not wait on this object, check if this object should be selected...
-                if (!wait_all && (!selected || (wait_object == object && was_waiting))) {
-                    // Do not wait the thread
-                    wait_thread = false;
-                    handle_index = i;
-                    wait_object = object;
-                    selected = true;
-                }
-            }
-        }
-    } else {
-        // If no handles were passed in, put the thread to sleep only when 'wait_all' is false
-        // NOTE: This should deadlock the current thread if no timeout was specified
-        if (!wait_all) {
-            wait_thread = true;
-        }
+    using ObjectPtr = Kernel::SharedPtr<Kernel::WaitObject>;
+
+    std::vector<ObjectPtr> objects(handle_count);
+
+    for (int i = 0; i < handle_count; ++i) {
+        auto object = Kernel::g_handle_table.GetWaitObject(handles[i]);
+        if (object == nullptr)
+            return ERR_INVALID_HANDLE;
+        objects[i] = object;
     }
 
-    SCOPE_EXIT({
-        HLE::Reschedule("WaitSynchronizationN");
-    }); // Reschedule after putting the threads to sleep.
+    // Clear the mapping of wait object indices
+    thread->wait_objects_index.clear();
+
+    if (!wait_all) {
+        // Find the first object that is acquireable in the provided list of objects
+        auto itr = std::find_if(objects.begin(), objects.end(), [](const ObjectPtr& object) {
+            return !object->ShouldWait();
+        });
+
+        if (itr != objects.end()) {
+            // We found a ready object, acquire it and set the result value
+            ObjectPtr object = *itr;
+            object->Acquire();
+            *out = std::distance(objects.begin(), itr);
+            return RESULT_SUCCESS;
+        }
+
+        // No objects were ready to be acquired, prepare to suspend the thread.
+
+        // If a timeout value of 0 was provided, just return the Timeout error code instead of suspending the thread.
+        if (nano_seconds == 0) {
+            return ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
+                              ErrorSummary::StatusChanged,
+                              ErrorLevel::Info);
+        }
 
-    // If thread should wait, then set its state to waiting
-    if (wait_thread) {
+        // Put the thread to sleep
+        thread->status = THREADSTATUS_WAIT_SYNCH;
 
-        // 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);
+        // Clear the thread's waitlist, we won't use it for wait_all = false
+        thread->wait_objects.clear();
 
-        for (int i = 0; i < handle_count; ++i) {
-            auto object = Kernel::g_handle_table.GetWaitObject(handles[i]);
-            object->AddWaitingThread(Kernel::GetCurrentThread());
-            wait_objects.push_back(object);
+        // Add the thread to each of the objects' waiting threads.
+        for (int i = 0; i < objects.size(); ++i) {
+            ObjectPtr object = objects[i];
+            // Set the index of this object in the mapping of Objects -> index for this thread.
+            thread->wait_objects_index[object->GetObjectId()] = i;
+            object->AddWaitingThread(thread);
+            // TODO(Subv): Perform things like update the mutex lock owner's priority to prevent priority inversion.
         }
 
-        Kernel::WaitCurrentThread_WaitSynchronization(std::move(wait_objects), true, wait_all);
+        // Note: If no handles and no timeout were given, then the thread will deadlock, this is consistent with hardware behavior.
 
         // Create an event to wake the thread up after the specified nanosecond delay has passed
-        Kernel::GetCurrentThread()->WakeAfterDelay(nano_seconds);
+        thread->WakeAfterDelay(nano_seconds);
 
-        // NOTE: output of this SVC will be set later depending on how the thread resumes
-        return HLE::RESULT_INVALID;
-    }
+        // Note: The output of this SVC will be set to RESULT_SUCCESS if the thread resumes due to a signal in one of its wait objects.
+        // Otherwise we retain the default value of timeout, and -1 in the out parameter
+        thread->wait_set_output = true;
+        *out = -1;
+        return ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
+                          ErrorSummary::StatusChanged,
+                          ErrorLevel::Info);
+    } else {
+        bool all_available = std::all_of(objects.begin(), objects.end(), [](const ObjectPtr& object) {
+            return !object->ShouldWait();
+        });
+        if (all_available) {
+            // We can acquire all objects right now, do so.
+            for (auto object : objects)
+                object->Acquire();
+            // Note: In this case, the `out` parameter is not set, and retains whatever value it had before.
+            return RESULT_SUCCESS;
+        }
 
-    // Acquire objects if we did not wait...
-    for (int i = 0; i < handle_count; ++i) {
-        auto object = Kernel::g_handle_table.GetWaitObject(handles[i]);
+        // Not all objects were available right now, prepare to suspend the thread.
 
-        // Acquire the object if it is not waiting...
-        if (!object->ShouldWait()) {
-            object->Acquire();
+        // If a timeout value of 0 was provided, just return the Timeout error code instead of suspending the thread.
+        if (nano_seconds == 0) {
+            return ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
+                              ErrorSummary::StatusChanged,
+                              ErrorLevel::Info);
+        }
+
+        // Put the thread to sleep
+        thread->status = THREADSTATUS_WAIT_SYNCH;
 
-            // If this was the first non-waiting object and 'wait_all' is false, don't acquire
-            // any other objects
-            if (!wait_all)
-                break;
+        // Set the thread's waitlist to the list of objects passed to WaitSynchronizationN
+        thread->wait_objects = objects;
+
+        // Add the thread to each of the objects' waiting threads.
+        for (auto object : objects) {
+            object->AddWaitingThread(thread);
+            // TODO(Subv): Perform things like update the mutex lock owner's priority to prevent priority inversion.
         }
-    }
 
-    // TODO(bunnei): If 'wait_all' is true, this is probably wrong. However, real hardware does
-    // not seem to set it to any meaningful value.
-    *out = handle_count != 0 ? (wait_all ? -1 : handle_index) : 0;
+        // Create an event to wake the thread up after the specified nanosecond delay has passed
+        thread->WakeAfterDelay(nano_seconds);
 
-    return RESULT_SUCCESS;
+        // This value gets set to -1 by default in this case, it is not modified after this.
+        *out = -1;
+        // Note: The output of this SVC will be set to RESULT_SUCCESS if the thread resumes due to a signal in one of its wait objects.
+        return ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
+                          ErrorSummary::StatusChanged,
+                          ErrorLevel::Info);
+    }
 }
 
 /// Create an address arbiter (to allocate access to shared resources)
@@ -1148,6 +1172,7 @@ void CallSVC(u32 immediate) {
     if (info) {
         if (info->func) {
             info->func();
+            HLE::Reschedule(__func__);
         } else {
             LOG_ERROR(Kernel_SVC, "unimplemented SVC function %s(..)", info->name);
         }
-- 
cgit v1.2.3


From bdad00c73f46106ba78995bdde1b50349e940b09 Mon Sep 17 00:00:00 2001
From: Subv <subv2112@gmail.com>
Date: Sun, 4 Dec 2016 09:58:36 -0500
Subject: Threading: Added some utility functions and const correctness.

---
 src/citra_qt/debugger/wait_tree.cpp |  2 +-
 src/core/hle/kernel/kernel.cpp      | 13 ++++++-------
 src/core/hle/kernel/thread.h        | 19 ++++++++++++++++---
 src/core/hle/svc.cpp                | 18 +++++++++++++-----
 4 files changed, 36 insertions(+), 16 deletions(-)

diff --git a/src/citra_qt/debugger/wait_tree.cpp b/src/citra_qt/debugger/wait_tree.cpp
index 8fc3e37e0..829ac7dd6 100644
--- a/src/citra_qt/debugger/wait_tree.cpp
+++ b/src/citra_qt/debugger/wait_tree.cpp
@@ -230,7 +230,7 @@ std::vector<std::unique_ptr<WaitTreeItem>> WaitTreeThread::GetChildren() const {
         list.push_back(std::make_unique<WaitTreeMutexList>(thread.held_mutexes));
     }
     if (thread.status == THREADSTATUS_WAIT_SYNCH) {
-        list.push_back(std::make_unique<WaitTreeObjectList>(thread.wait_objects, !thread.wait_objects.empty()));
+        list.push_back(std::make_unique<WaitTreeObjectList>(thread.wait_objects, thread.IsWaitingAll()));
     }
 
     return list;
diff --git a/src/core/hle/kernel/kernel.cpp b/src/core/hle/kernel/kernel.cpp
index be7a5a6d8..6d358def7 100644
--- a/src/core/hle/kernel/kernel.cpp
+++ b/src/core/hle/kernel/kernel.cpp
@@ -33,7 +33,7 @@ void WaitObject::RemoveWaitingThread(Thread* thread) {
 
 SharedPtr<Thread> WaitObject::GetHighestPriorityReadyThread() {
     // Remove the threads that are ready or already running from our waitlist
-    waiting_threads.erase(std::remove_if(waiting_threads.begin(), waiting_threads.end(), [](SharedPtr<Thread> thread) -> bool {
+    waiting_threads.erase(std::remove_if(waiting_threads.begin(), waiting_threads.end(), [](const SharedPtr<Thread>& thread) -> bool {
         return thread->status == THREADSTATUS_RUNNING || thread->status == THREADSTATUS_READY;
     }), waiting_threads.end());
 
@@ -42,12 +42,11 @@ SharedPtr<Thread> WaitObject::GetHighestPriorityReadyThread() {
 
     auto candidate_threads = waiting_threads;
 
-    // Eliminate all threads that are waiting on more than one object, and not all of them are ready
-    candidate_threads.erase(std::remove_if(candidate_threads.begin(), candidate_threads.end(), [](SharedPtr<Thread> thread) -> bool {
-        for (auto object : thread->wait_objects)
-            if (object->ShouldWait())
-                return true;
-        return false;
+    // Eliminate all threads that are waiting on more than one object, and not all of said objects are ready
+    candidate_threads.erase(std::remove_if(candidate_threads.begin(), candidate_threads.end(), [](const SharedPtr<Thread>& thread) -> bool {
+        return std::any_of(thread->wait_objects.begin(), thread->wait_objects.end(), [](const SharedPtr<WaitObject>& object) -> bool {
+            return object->ShouldWait();
+        });
     }), candidate_threads.end());
 
     // Return the thread with the lowest priority value (The one with the highest priority)
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index 63b97b74f..1b29fb3a3 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -131,8 +131,8 @@ public:
      * It is used to set the output value of WaitSynchronizationN when the thread is awakened.
      * @param object Object to query the index of.
      */
-    s32 GetWaitObjectIndex(WaitObject* object) {
-        return wait_objects_index[object->GetObjectId()];
+    s32 GetWaitObjectIndex(const WaitObject* object) const {
+        return wait_objects_index.at(object->GetObjectId());
     }
 
     /**
@@ -148,6 +148,15 @@ public:
         return tls_address;
     }
 
+    /**
+     * Returns whether this thread is waiting for all the objects in
+     * its wait list to become ready, as a result of a WaitSynchronizationN call
+     * with wait_all = true, or a ReplyAndReceive call.
+     */
+    bool IsWaitingAll() const {
+        return !wait_objects.empty();
+    }
+
     Core::ThreadContext context;
 
     u32 thread_id;
@@ -169,7 +178,11 @@ public:
     boost::container::flat_set<SharedPtr<Mutex>> held_mutexes;
 
     SharedPtr<Process> owner_process;                ///< Process that owns this thread
-    std::vector<SharedPtr<WaitObject>> wait_objects; ///< Objects that the thread is waiting on
+
+    /// Objects that the thread is waiting on.
+    /// This is only populated when the thread should wait for all the objects to become ready.
+    std::vector<SharedPtr<WaitObject>> wait_objects;
+
     std::unordered_map<int, s32> wait_objects_index; ///< Mapping of Object ids to their position in the last waitlist that this object waited on.
 
     VAddr wait_address;   ///< If waiting on an AddressArbiter, this is the arbitration address
diff --git a/src/core/hle/svc.cpp b/src/core/hle/svc.cpp
index 061692af8..c06df84b3 100644
--- a/src/core/hle/svc.cpp
+++ b/src/core/hle/svc.cpp
@@ -257,18 +257,21 @@ static ResultCode WaitSynchronization1(Handle handle, s64 nano_seconds) {
 
     if (object->ShouldWait()) {
 
-        if (nano_seconds == 0)
+        if (nano_seconds == 0) {
             return ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
                               ErrorSummary::StatusChanged,
                               ErrorLevel::Info);
+        }
 
         object->AddWaitingThread(thread);
+        // TODO(Subv): Perform things like update the mutex lock owner's priority to prevent priority inversion.
+        // Currently this is done in Mutex::ShouldWait, but it should be moved to a function that is called from here.
         thread->status = THREADSTATUS_WAIT_SYNCH;
 
         // Create an event to wake the thread up after the specified nanosecond delay has passed
         thread->WakeAfterDelay(nano_seconds);
 
-        // Note: The output of this SVC will be set to RESULT_SUCCESS if the thread resumes due to a signal in one of its wait objects.
+        // Note: The output of this SVC will be set to RESULT_SUCCESS if the thread resumes due to a signal in its wait objects.
         // Otherwise we retain the default value of timeout.
         return ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
                                ErrorSummary::StatusChanged,
@@ -312,7 +315,9 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
         objects[i] = object;
     }
 
-    // Clear the mapping of wait object indices
+    // Clear the mapping of wait object indices.
+    // We don't want any lingering state in this map.
+    // It will be repopulated later in the wait_all = false case.
     thread->wait_objects_index.clear();
 
     if (!wait_all) {
@@ -345,12 +350,13 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
         thread->wait_objects.clear();
 
         // Add the thread to each of the objects' waiting threads.
-        for (int i = 0; i < objects.size(); ++i) {
+        for (size_t i = 0; i < objects.size(); ++i) {
             ObjectPtr object = objects[i];
             // Set the index of this object in the mapping of Objects -> index for this thread.
-            thread->wait_objects_index[object->GetObjectId()] = i;
+            thread->wait_objects_index[object->GetObjectId()] = static_cast<int>(i);
             object->AddWaitingThread(thread);
             // TODO(Subv): Perform things like update the mutex lock owner's priority to prevent priority inversion.
+            // Currently this is done in Mutex::ShouldWait, but it should be moved to a function that is called from here.
         }
 
         // Note: If no handles and no timeout were given, then the thread will deadlock, this is consistent with hardware behavior.
@@ -396,6 +402,7 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
         for (auto object : objects) {
             object->AddWaitingThread(thread);
             // TODO(Subv): Perform things like update the mutex lock owner's priority to prevent priority inversion.
+            // Currently this is done in Mutex::ShouldWait, but it should be moved to a function that is called from here.
         }
 
         // Create an event to wake the thread up after the specified nanosecond delay has passed
@@ -1172,6 +1179,7 @@ void CallSVC(u32 immediate) {
     if (info) {
         if (info->func) {
             info->func();
+            //  TODO(Subv): Not all service functions should cause a reschedule in all cases.
             HLE::Reschedule(__func__);
         } else {
             LOG_ERROR(Kernel_SVC, "unimplemented SVC function %s(..)", info->name);
-- 
cgit v1.2.3


From 1f286b72a1b9e1fb98e37a01192f4e31b0ad4e1f Mon Sep 17 00:00:00 2001
From: Subv <subv2112@gmail.com>
Date: Tue, 6 Dec 2016 19:15:32 -0500
Subject: Improved the algorithm for GetHighestPriorityReadyThread.

---
 src/core/hle/kernel/kernel.cpp | 27 +++++++++++++--------------
 1 file changed, 13 insertions(+), 14 deletions(-)

diff --git a/src/core/hle/kernel/kernel.cpp b/src/core/hle/kernel/kernel.cpp
index 6d358def7..07f420099 100644
--- a/src/core/hle/kernel/kernel.cpp
+++ b/src/core/hle/kernel/kernel.cpp
@@ -40,24 +40,23 @@ SharedPtr<Thread> WaitObject::GetHighestPriorityReadyThread() {
     if (waiting_threads.empty())
         return nullptr;
 
-    auto candidate_threads = waiting_threads;
+    SharedPtr<Thread> candidate = nullptr;
+    s32 candidate_priority = THREADPRIO_LOWEST + 1;
 
-    // Eliminate all threads that are waiting on more than one object, and not all of said objects are ready
-    candidate_threads.erase(std::remove_if(candidate_threads.begin(), candidate_threads.end(), [](const SharedPtr<Thread>& thread) -> bool {
-        return std::any_of(thread->wait_objects.begin(), thread->wait_objects.end(), [](const SharedPtr<WaitObject>& object) -> bool {
+    for (const auto& thread : waiting_threads) {
+        if (thread->current_priority >= candidate_priority)
+            continue;
+
+        bool ready_to_run = std::none_of(thread->wait_objects.begin(), thread->wait_objects.end(), [](const SharedPtr<WaitObject>& object) {
             return object->ShouldWait();
         });
-    }), candidate_threads.end());
-
-    // Return the thread with the lowest priority value (The one with the highest priority)
-    auto thread_itr = std::min_element(candidate_threads.begin(), candidate_threads.end(), [](const SharedPtr<Thread>& lhs, const SharedPtr<Thread>& rhs) {
-        return lhs->current_priority < rhs->current_priority;
-    });
-
-    if (thread_itr == candidate_threads.end())
-        return nullptr;
+        if (ready_to_run) {
+            candidate = thread;
+            candidate_priority = thread->current_priority;
+        }
+    }
 
-    return *thread_itr;
+    return candidate;
 }
 
 void WaitObject::WakeupAllWaitingThreads() {
-- 
cgit v1.2.3


From 7cde5b83bc9c2dc178bca9fa16ebb28657456b81 Mon Sep 17 00:00:00 2001
From: Subv <subv2112@gmail.com>
Date: Tue, 6 Dec 2016 19:31:53 -0500
Subject: Use boost remove_erase_if instead of the erase-remove idiom

---
 src/core/hle/kernel/kernel.cpp | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

diff --git a/src/core/hle/kernel/kernel.cpp b/src/core/hle/kernel/kernel.cpp
index 07f420099..b8b69f9d0 100644
--- a/src/core/hle/kernel/kernel.cpp
+++ b/src/core/hle/kernel/kernel.cpp
@@ -3,6 +3,7 @@
 // Refer to the license.txt file included.
 
 #include <algorithm>
+#include <boost/range/algorithm_ext/erase.hpp>
 #include "common/assert.h"
 #include "common/logging/log.h"
 #include "core/hle/config_mem.h"
@@ -33,9 +34,9 @@ void WaitObject::RemoveWaitingThread(Thread* thread) {
 
 SharedPtr<Thread> WaitObject::GetHighestPriorityReadyThread() {
     // Remove the threads that are ready or already running from our waitlist
-    waiting_threads.erase(std::remove_if(waiting_threads.begin(), waiting_threads.end(), [](const SharedPtr<Thread>& thread) -> bool {
+    boost::range::remove_erase_if(waiting_threads, [](const SharedPtr<Thread>& thread) -> bool {
         return thread->status == THREADSTATUS_RUNNING || thread->status == THREADSTATUS_READY;
-    }), waiting_threads.end());
+    });
 
     if (waiting_threads.empty())
         return nullptr;
-- 
cgit v1.2.3


From 17b29d8865ea4d96c18f7e1671bd6d0f01eab95f Mon Sep 17 00:00:00 2001
From: Subv <subv2112@gmail.com>
Date: Thu, 8 Dec 2016 10:34:53 -0500
Subject: WaitSynch: Removed unused variables and reduced SharedPtr copies.

Define a variable with the value of the sync timeout error code.

Use a boost::flat_map instead of an unordered_map to hold the equivalence of objects and wait indices in a WaitSynchN call.
---
 src/citra_qt/debugger/wait_tree.cpp |   2 +-
 src/core/hle/kernel/kernel.cpp      |  14 ++---
 src/core/hle/kernel/kernel.h        |   5 +-
 src/core/hle/kernel/thread.h        |   5 +-
 src/core/hle/svc.cpp                | 105 ++++++++++++++++--------------------
 5 files changed, 57 insertions(+), 74 deletions(-)

diff --git a/src/citra_qt/debugger/wait_tree.cpp b/src/citra_qt/debugger/wait_tree.cpp
index 829ac7dd6..8ff094209 100644
--- a/src/citra_qt/debugger/wait_tree.cpp
+++ b/src/citra_qt/debugger/wait_tree.cpp
@@ -230,7 +230,7 @@ std::vector<std::unique_ptr<WaitTreeItem>> WaitTreeThread::GetChildren() const {
         list.push_back(std::make_unique<WaitTreeMutexList>(thread.held_mutexes));
     }
     if (thread.status == THREADSTATUS_WAIT_SYNCH) {
-        list.push_back(std::make_unique<WaitTreeObjectList>(thread.wait_objects, thread.IsWaitingAll()));
+        list.push_back(std::make_unique<WaitTreeObjectList>(thread.wait_objects, thread.IsSleepingOnWaitAll()));
     }
 
     return list;
diff --git a/src/core/hle/kernel/kernel.cpp b/src/core/hle/kernel/kernel.cpp
index b8b69f9d0..653697843 100644
--- a/src/core/hle/kernel/kernel.cpp
+++ b/src/core/hle/kernel/kernel.cpp
@@ -34,14 +34,11 @@ void WaitObject::RemoveWaitingThread(Thread* thread) {
 
 SharedPtr<Thread> WaitObject::GetHighestPriorityReadyThread() {
     // Remove the threads that are ready or already running from our waitlist
-    boost::range::remove_erase_if(waiting_threads, [](const SharedPtr<Thread>& thread) -> bool {
+    boost::range::remove_erase_if(waiting_threads, [](const SharedPtr<Thread>& thread) {
         return thread->status == THREADSTATUS_RUNNING || thread->status == THREADSTATUS_READY;
     });
 
-    if (waiting_threads.empty())
-        return nullptr;
-
-    SharedPtr<Thread> candidate = nullptr;
+    Thread* candidate = nullptr;
     s32 candidate_priority = THREADPRIO_LOWEST + 1;
 
     for (const auto& thread : waiting_threads) {
@@ -52,7 +49,7 @@ SharedPtr<Thread> WaitObject::GetHighestPriorityReadyThread() {
             return object->ShouldWait();
         });
         if (ready_to_run) {
-            candidate = thread;
+            candidate = thread.get();
             candidate_priority = thread->current_priority;
         }
     }
@@ -61,9 +58,8 @@ SharedPtr<Thread> WaitObject::GetHighestPriorityReadyThread() {
 }
 
 void WaitObject::WakeupAllWaitingThreads() {
-    // Wake up all threads that can be awoken, in priority order
     while (auto thread = GetHighestPriorityReadyThread()) {
-        if (thread->wait_objects.empty()) {
+        if (!thread->IsSleepingOnWaitAll()) {
             Acquire();
             // Set the output index of the WaitSynchronizationN call to the index of this object.
             if (thread->wait_set_output) {
@@ -73,7 +69,6 @@ void WaitObject::WakeupAllWaitingThreads() {
         } else {
             for (auto object : thread->wait_objects) {
                 object->Acquire();
-                // Remove the thread from the object's waitlist
                 object->RemoveWaitingThread(thread.get());
             }
             // Note: This case doesn't update the output index of WaitSynchronizationN.
@@ -81,7 +76,6 @@ void WaitObject::WakeupAllWaitingThreads() {
             thread->wait_objects.clear();
         }
 
-        // Set the result of the call to WaitSynchronization to RESULT_SUCCESS
         thread->SetWaitSynchronizationResult(RESULT_SUCCESS);
         thread->ResumeFromWait();
         // Note: Removing the thread from the object's waitlist will be done by GetHighestPriorityReadyThread
diff --git a/src/core/hle/kernel/kernel.h b/src/core/hle/kernel/kernel.h
index eb5a3bf7e..4227d2373 100644
--- a/src/core/hle/kernel/kernel.h
+++ b/src/core/hle/kernel/kernel.h
@@ -152,7 +152,10 @@ public:
      */
     void RemoveWaitingThread(Thread* thread);
 
-    /// Wake up all threads waiting on this object
+    /**
+     * Wake up all threads waiting on this object that can be awoken, in priority order,
+     * and set the synchronization result and output of the thread.
+     */
     void WakeupAllWaitingThreads();
 
     /// Obtains the highest priority thread that is ready to run from this object's waiting list.
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index 1b29fb3a3..4c254cb9d 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -7,6 +7,7 @@
 #include <string>
 #include <unordered_map>
 #include <vector>
+#include <boost/container/flat_map.hpp>
 #include <boost/container/flat_set.hpp>
 #include "common/common_types.h"
 #include "core/core.h"
@@ -153,7 +154,7 @@ public:
      * its wait list to become ready, as a result of a WaitSynchronizationN call
      * with wait_all = true, or a ReplyAndReceive call.
      */
-    bool IsWaitingAll() const {
+    bool IsSleepingOnWaitAll() const {
         return !wait_objects.empty();
     }
 
@@ -183,7 +184,7 @@ public:
     /// This is only populated when the thread should wait for all the objects to become ready.
     std::vector<SharedPtr<WaitObject>> wait_objects;
 
-    std::unordered_map<int, s32> wait_objects_index; ///< Mapping of Object ids to their position in the last waitlist that this object waited on.
+    boost::container::flat_map<int, s32> wait_objects_index; ///< Mapping of Object ids to their position in the last waitlist that this object waited on.
 
     VAddr wait_address;   ///< If waiting on an AddressArbiter, this is the arbitration address
 
diff --git a/src/core/hle/svc.cpp b/src/core/hle/svc.cpp
index c06df84b3..14da09883 100644
--- a/src/core/hle/svc.cpp
+++ b/src/core/hle/svc.cpp
@@ -41,6 +41,9 @@ const ResultCode ERR_PORT_NAME_TOO_LONG(ErrorDescription(30), ErrorModule::OS,
                                         ErrorSummary::InvalidArgument,
                                         ErrorLevel::Usage); // 0xE0E0181E
 
+const ResultCode ERR_SYNC_TIMEOUT(ErrorDescription::Timeout, ErrorModule::OS,
+                                  ErrorSummary::StatusChanged, ErrorLevel::Info);
+
 const ResultCode ERR_MISALIGNED_ADDRESS{// 0xE0E01BF1
                                         ErrorDescription::MisalignedAddress, ErrorModule::OS,
                                         ErrorSummary::InvalidArgument, ErrorLevel::Usage};
@@ -257,11 +260,8 @@ static ResultCode WaitSynchronization1(Handle handle, s64 nano_seconds) {
 
     if (object->ShouldWait()) {
 
-        if (nano_seconds == 0) {
-            return ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
-                              ErrorSummary::StatusChanged,
-                              ErrorLevel::Info);
-        }
+        if (nano_seconds == 0)
+            return ERR_SYNC_TIMEOUT;
 
         object->AddWaitingThread(thread);
         // TODO(Subv): Perform things like update the mutex lock owner's priority to prevent priority inversion.
@@ -273,9 +273,7 @@ static ResultCode WaitSynchronization1(Handle handle, s64 nano_seconds) {
 
         // Note: The output of this SVC will be set to RESULT_SUCCESS if the thread resumes due to a signal in its wait objects.
         // Otherwise we retain the default value of timeout.
-        return ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
-                               ErrorSummary::StatusChanged,
-                               ErrorLevel::Info);
+        return ERR_SYNC_TIMEOUT;
     }
 
     object->Acquire();
@@ -286,8 +284,6 @@ static ResultCode WaitSynchronization1(Handle handle, s64 nano_seconds) {
 /// Wait for the given handles to synchronize, timeout after the specified nanoseconds
 static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_count, bool wait_all,
                                        s64 nano_seconds) {
-    bool wait_thread = !wait_all;
-    int handle_index = 0;
     Kernel::Thread* thread = Kernel::GetCurrentThread();
 
     // Check if 'handles' is invalid
@@ -305,7 +301,6 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
                           ErrorSummary::InvalidArgument, ErrorLevel::Usage);
 
     using ObjectPtr = Kernel::SharedPtr<Kernel::WaitObject>;
-
     std::vector<ObjectPtr> objects(handle_count);
 
     for (int i = 0; i < handle_count; ++i) {
@@ -320,100 +315,90 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
     // It will be repopulated later in the wait_all = false case.
     thread->wait_objects_index.clear();
 
-    if (!wait_all) {
-        // Find the first object that is acquireable in the provided list of objects
-        auto itr = std::find_if(objects.begin(), objects.end(), [](const ObjectPtr& object) {
+    if (wait_all) {
+        bool all_available = std::all_of(objects.begin(), objects.end(), [](const ObjectPtr& object) {
             return !object->ShouldWait();
         });
-
-        if (itr != objects.end()) {
-            // We found a ready object, acquire it and set the result value
-            ObjectPtr object = *itr;
-            object->Acquire();
-            *out = std::distance(objects.begin(), itr);
+        if (all_available) {
+            // We can acquire all objects right now, do so.
+            for (auto object : objects)
+                object->Acquire();
+            // Note: In this case, the `out` parameter is not set, and retains whatever value it had before.
             return RESULT_SUCCESS;
         }
 
-        // No objects were ready to be acquired, prepare to suspend the thread.
+        // Not all objects were available right now, prepare to suspend the thread.
 
         // If a timeout value of 0 was provided, just return the Timeout error code instead of suspending the thread.
-        if (nano_seconds == 0) {
-            return ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
-                              ErrorSummary::StatusChanged,
-                              ErrorLevel::Info);
-        }
+        if (nano_seconds == 0)
+            return ERR_SYNC_TIMEOUT;
 
         // Put the thread to sleep
         thread->status = THREADSTATUS_WAIT_SYNCH;
 
-        // Clear the thread's waitlist, we won't use it for wait_all = false
-        thread->wait_objects.clear();
-
         // Add the thread to each of the objects' waiting threads.
-        for (size_t i = 0; i < objects.size(); ++i) {
-            ObjectPtr object = objects[i];
-            // Set the index of this object in the mapping of Objects -> index for this thread.
-            thread->wait_objects_index[object->GetObjectId()] = static_cast<int>(i);
+        for (auto& object : objects) {
             object->AddWaitingThread(thread);
             // TODO(Subv): Perform things like update the mutex lock owner's priority to prevent priority inversion.
             // Currently this is done in Mutex::ShouldWait, but it should be moved to a function that is called from here.
         }
 
-        // Note: If no handles and no timeout were given, then the thread will deadlock, this is consistent with hardware behavior.
+        // Set the thread's waitlist to the list of objects passed to WaitSynchronizationN
+        thread->wait_objects = std::move(objects);
 
         // Create an event to wake the thread up after the specified nanosecond delay has passed
         thread->WakeAfterDelay(nano_seconds);
 
-        // Note: The output of this SVC will be set to RESULT_SUCCESS if the thread resumes due to a signal in one of its wait objects.
-        // Otherwise we retain the default value of timeout, and -1 in the out parameter
-        thread->wait_set_output = true;
+        // This value gets set to -1 by default in this case, it is not modified after this.
         *out = -1;
-        return ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
-                          ErrorSummary::StatusChanged,
-                          ErrorLevel::Info);
+        // Note: The output of this SVC will be set to RESULT_SUCCESS if the thread resumes due to a signal in one of its wait objects.
+        return ERR_SYNC_TIMEOUT;
     } else {
-        bool all_available = std::all_of(objects.begin(), objects.end(), [](const ObjectPtr& object) {
+        // Find the first object that is acquirable in the provided list of objects
+        auto itr = std::find_if(objects.begin(), objects.end(), [](const ObjectPtr& object) {
             return !object->ShouldWait();
         });
-        if (all_available) {
-            // We can acquire all objects right now, do so.
-            for (auto object : objects)
-                object->Acquire();
-            // Note: In this case, the `out` parameter is not set, and retains whatever value it had before.
+
+        if (itr != objects.end()) {
+            // We found a ready object, acquire it and set the result value
+            Kernel::WaitObject* object = itr->get();
+            object->Acquire();
+            *out = std::distance(objects.begin(), itr);
             return RESULT_SUCCESS;
         }
 
-        // Not all objects were available right now, prepare to suspend the thread.
+        // No objects were ready to be acquired, prepare to suspend the thread.
 
         // If a timeout value of 0 was provided, just return the Timeout error code instead of suspending the thread.
-        if (nano_seconds == 0) {
-            return ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
-                              ErrorSummary::StatusChanged,
-                              ErrorLevel::Info);
-        }
+        if (nano_seconds == 0)
+            return ERR_SYNC_TIMEOUT;
 
         // Put the thread to sleep
         thread->status = THREADSTATUS_WAIT_SYNCH;
 
-        // Set the thread's waitlist to the list of objects passed to WaitSynchronizationN
-        thread->wait_objects = objects;
+        // Clear the thread's waitlist, we won't use it for wait_all = false
+        thread->wait_objects.clear();
 
         // Add the thread to each of the objects' waiting threads.
-        for (auto object : objects) {
+        for (size_t i = 0; i < objects.size(); ++i) {
+            Kernel::WaitObject* object = objects[i].get();
+            // Set the index of this object in the mapping of Objects -> index for this thread.
+            thread->wait_objects_index[object->GetObjectId()] = static_cast<int>(i);
             object->AddWaitingThread(thread);
             // TODO(Subv): Perform things like update the mutex lock owner's priority to prevent priority inversion.
             // Currently this is done in Mutex::ShouldWait, but it should be moved to a function that is called from here.
         }
 
+        // Note: If no handles and no timeout were given, then the thread will deadlock, this is consistent with hardware behavior.
+
         // Create an event to wake the thread up after the specified nanosecond delay has passed
         thread->WakeAfterDelay(nano_seconds);
 
-        // This value gets set to -1 by default in this case, it is not modified after this.
-        *out = -1;
         // Note: The output of this SVC will be set to RESULT_SUCCESS if the thread resumes due to a signal in one of its wait objects.
-        return ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
-                          ErrorSummary::StatusChanged,
-                          ErrorLevel::Info);
+        // Otherwise we retain the default value of timeout, and -1 in the out parameter
+        thread->wait_set_output = true;
+        *out = -1;
+        return ERR_SYNC_TIMEOUT;
     }
 }
 
-- 
cgit v1.2.3


From 406907d57055965780e04769482556995de8c50a Mon Sep 17 00:00:00 2001
From: Subv <subv2112@gmail.com>
Date: Sat, 10 Dec 2016 13:29:31 -0500
Subject: Properly remove a thread from its wait_objects' waitlist when it is
 awoken by a timeout.

---
 src/core/hle/kernel/kernel.cpp | 7 ++++++-
 src/core/hle/kernel/thread.cpp | 4 ++++
 src/core/hle/svc.cpp           | 2 +-
 3 files changed, 11 insertions(+), 2 deletions(-)

diff --git a/src/core/hle/kernel/kernel.cpp b/src/core/hle/kernel/kernel.cpp
index 653697843..2ddeffcdd 100644
--- a/src/core/hle/kernel/kernel.cpp
+++ b/src/core/hle/kernel/kernel.cpp
@@ -38,6 +38,11 @@ SharedPtr<Thread> WaitObject::GetHighestPriorityReadyThread() {
         return thread->status == THREADSTATUS_RUNNING || thread->status == THREADSTATUS_READY;
     });
 
+    // TODO(Subv): This call should be performed inside the loop below to check if an object can be
+    // acquired by a particular thread. This is useful for things like recursive locking of Mutexes.
+    if (ShouldWait())
+        return nullptr;
+
     Thread* candidate = nullptr;
     s32 candidate_priority = THREADPRIO_LOWEST + 1;
 
@@ -67,7 +72,7 @@ void WaitObject::WakeupAllWaitingThreads() {
                 thread->wait_set_output = false;
             }
         } else {
-            for (auto object : thread->wait_objects) {
+            for (auto& object : thread->wait_objects) {
                 object->Acquire();
                 object->RemoveWaitingThread(thread.get());
             }
diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index 49ed9d899..4bbc08516 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -277,6 +277,10 @@ static void ThreadWakeupCallback(u64 thread_handle, int cycles_late) {
 
     if (thread->status == THREADSTATUS_WAIT_SYNCH || thread->status == THREADSTATUS_WAIT_ARB) {
         thread->wait_set_output = false;
+        // Remove the thread from each of its waiting objects' waitlists
+        for (auto& object : thread->wait_objects)
+            object->RemoveWaitingThread(thread.get());
+        thread->wait_objects.clear();
         thread->SetWaitSynchronizationResult(ResultCode(ErrorDescription::Timeout, ErrorModule::OS,
                                                         ErrorSummary::StatusChanged,
                                                         ErrorLevel::Info));
diff --git a/src/core/hle/svc.cpp b/src/core/hle/svc.cpp
index 14da09883..c81c14443 100644
--- a/src/core/hle/svc.cpp
+++ b/src/core/hle/svc.cpp
@@ -321,7 +321,7 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
         });
         if (all_available) {
             // We can acquire all objects right now, do so.
-            for (auto object : objects)
+            for (auto& object : objects)
                 object->Acquire();
             // Note: In this case, the `out` parameter is not set, and retains whatever value it had before.
             return RESULT_SUCCESS;
-- 
cgit v1.2.3


From 5b1edc6ae70972d4a11eee1f1ff8fdff2122b5a2 Mon Sep 17 00:00:00 2001
From: Subv <subv2112@gmail.com>
Date: Wed, 14 Dec 2016 12:13:02 -0500
Subject: Fixed the codestyle to match our clang-format rules.

---
 src/citra_qt/debugger/wait_tree.cpp |  3 ++-
 src/core/hle/kernel/kernel.cpp      |  9 +++----
 src/core/hle/kernel/thread.h        | 10 ++++----
 src/core/hle/svc.cpp                | 47 ++++++++++++++++++++++---------------
 4 files changed, 41 insertions(+), 28 deletions(-)

diff --git a/src/citra_qt/debugger/wait_tree.cpp b/src/citra_qt/debugger/wait_tree.cpp
index 8ff094209..b2e7f4a97 100644
--- a/src/citra_qt/debugger/wait_tree.cpp
+++ b/src/citra_qt/debugger/wait_tree.cpp
@@ -230,7 +230,8 @@ std::vector<std::unique_ptr<WaitTreeItem>> WaitTreeThread::GetChildren() const {
         list.push_back(std::make_unique<WaitTreeMutexList>(thread.held_mutexes));
     }
     if (thread.status == THREADSTATUS_WAIT_SYNCH) {
-        list.push_back(std::make_unique<WaitTreeObjectList>(thread.wait_objects, thread.IsSleepingOnWaitAll()));
+        list.push_back(std::make_unique<WaitTreeObjectList>(thread.wait_objects,
+                                                            thread.IsSleepingOnWaitAll()));
     }
 
     return list;
diff --git a/src/core/hle/kernel/kernel.cpp b/src/core/hle/kernel/kernel.cpp
index 2ddeffcdd..209d35270 100644
--- a/src/core/hle/kernel/kernel.cpp
+++ b/src/core/hle/kernel/kernel.cpp
@@ -50,9 +50,9 @@ SharedPtr<Thread> WaitObject::GetHighestPriorityReadyThread() {
         if (thread->current_priority >= candidate_priority)
             continue;
 
-        bool ready_to_run = std::none_of(thread->wait_objects.begin(), thread->wait_objects.end(), [](const SharedPtr<WaitObject>& object) {
-            return object->ShouldWait();
-        });
+        bool ready_to_run =
+            std::none_of(thread->wait_objects.begin(), thread->wait_objects.end(),
+                         [](const SharedPtr<WaitObject>& object) { return object->ShouldWait(); });
         if (ready_to_run) {
             candidate = thread.get();
             candidate_priority = thread->current_priority;
@@ -83,7 +83,8 @@ void WaitObject::WakeupAllWaitingThreads() {
 
         thread->SetWaitSynchronizationResult(RESULT_SUCCESS);
         thread->ResumeFromWait();
-        // Note: Removing the thread from the object's waitlist will be done by GetHighestPriorityReadyThread
+        // Note: Removing the thread from the object's waitlist will be
+        // done by GetHighestPriorityReadyThread.
     }
 }
 
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index 4c254cb9d..238359fc5 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -178,17 +178,19 @@ public:
     /// Mutexes currently held by this thread, which will be released when it exits.
     boost::container::flat_set<SharedPtr<Mutex>> held_mutexes;
 
-    SharedPtr<Process> owner_process;                ///< Process that owns this thread
+    SharedPtr<Process> owner_process; ///< Process that owns this thread
 
     /// Objects that the thread is waiting on.
     /// This is only populated when the thread should wait for all the objects to become ready.
     std::vector<SharedPtr<WaitObject>> wait_objects;
 
-    boost::container::flat_map<int, s32> wait_objects_index; ///< Mapping of Object ids to their position in the last waitlist that this object waited on.
+    /// Mapping of Object ids to their position in the last waitlist that this object waited on.
+    boost::container::flat_map<int, s32> wait_objects_index;
 
-    VAddr wait_address;   ///< If waiting on an AddressArbiter, this is the arbitration address
+    VAddr wait_address; ///< If waiting on an AddressArbiter, this is the arbitration address
 
-    bool wait_set_output; ///< True if the WaitSynchronizationN output parameter should be set on thread wakeup
+    /// True if the WaitSynchronizationN output parameter should be set on thread wakeup.
+    bool wait_set_output;
 
     std::string name;
 
diff --git a/src/core/hle/svc.cpp b/src/core/hle/svc.cpp
index c81c14443..a4a00d9b2 100644
--- a/src/core/hle/svc.cpp
+++ b/src/core/hle/svc.cpp
@@ -264,14 +264,16 @@ static ResultCode WaitSynchronization1(Handle handle, s64 nano_seconds) {
             return ERR_SYNC_TIMEOUT;
 
         object->AddWaitingThread(thread);
-        // TODO(Subv): Perform things like update the mutex lock owner's priority to prevent priority inversion.
-        // Currently this is done in Mutex::ShouldWait, but it should be moved to a function that is called from here.
+        // TODO(Subv): Perform things like update the mutex lock owner's priority to
+        // prevent priority inversion. Currently this is done in Mutex::ShouldWait,
+        // but it should be moved to a function that is called from here.
         thread->status = THREADSTATUS_WAIT_SYNCH;
 
         // Create an event to wake the thread up after the specified nanosecond delay has passed
         thread->WakeAfterDelay(nano_seconds);
 
-        // Note: The output of this SVC will be set to RESULT_SUCCESS if the thread resumes due to a signal in its wait objects.
+        // Note: The output of this SVC will be set to RESULT_SUCCESS if the thread
+        // resumes due to a signal in its wait objects.
         // Otherwise we retain the default value of timeout.
         return ERR_SYNC_TIMEOUT;
     }
@@ -316,20 +318,22 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
     thread->wait_objects_index.clear();
 
     if (wait_all) {
-        bool all_available = std::all_of(objects.begin(), objects.end(), [](const ObjectPtr& object) {
-            return !object->ShouldWait();
-        });
+        bool all_available =
+            std::all_of(objects.begin(), objects.end(),
+                        [](const ObjectPtr& object) { return !object->ShouldWait(); });
         if (all_available) {
             // We can acquire all objects right now, do so.
             for (auto& object : objects)
                 object->Acquire();
-            // Note: In this case, the `out` parameter is not set, and retains whatever value it had before.
+            // Note: In this case, the `out` parameter is not set,
+            // and retains whatever value it had before.
             return RESULT_SUCCESS;
         }
 
         // Not all objects were available right now, prepare to suspend the thread.
 
-        // If a timeout value of 0 was provided, just return the Timeout error code instead of suspending the thread.
+        // If a timeout value of 0 was provided, just return the Timeout error code instead of
+        // suspending the thread.
         if (nano_seconds == 0)
             return ERR_SYNC_TIMEOUT;
 
@@ -339,8 +343,9 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
         // Add the thread to each of the objects' waiting threads.
         for (auto& object : objects) {
             object->AddWaitingThread(thread);
-            // TODO(Subv): Perform things like update the mutex lock owner's priority to prevent priority inversion.
-            // Currently this is done in Mutex::ShouldWait, but it should be moved to a function that is called from here.
+            // TODO(Subv): Perform things like update the mutex lock owner's priority to
+            // prevent priority inversion. Currently this is done in Mutex::ShouldWait,
+            // but it should be moved to a function that is called from here.
         }
 
         // Set the thread's waitlist to the list of objects passed to WaitSynchronizationN
@@ -351,13 +356,13 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
 
         // This value gets set to -1 by default in this case, it is not modified after this.
         *out = -1;
-        // Note: The output of this SVC will be set to RESULT_SUCCESS if the thread resumes due to a signal in one of its wait objects.
+        // Note: The output of this SVC will be set to RESULT_SUCCESS if the thread resumes due to
+        // a signal in one of its wait objects.
         return ERR_SYNC_TIMEOUT;
     } else {
         // Find the first object that is acquirable in the provided list of objects
-        auto itr = std::find_if(objects.begin(), objects.end(), [](const ObjectPtr& object) {
-            return !object->ShouldWait();
-        });
+        auto itr = std::find_if(objects.begin(), objects.end(),
+                                [](const ObjectPtr& object) { return !object->ShouldWait(); });
 
         if (itr != objects.end()) {
             // We found a ready object, acquire it and set the result value
@@ -369,7 +374,8 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
 
         // No objects were ready to be acquired, prepare to suspend the thread.
 
-        // If a timeout value of 0 was provided, just return the Timeout error code instead of suspending the thread.
+        // If a timeout value of 0 was provided, just return the Timeout error code instead of
+        // suspending the thread.
         if (nano_seconds == 0)
             return ERR_SYNC_TIMEOUT;
 
@@ -385,16 +391,19 @@ static ResultCode WaitSynchronizationN(s32* out, Handle* handles, s32 handle_cou
             // Set the index of this object in the mapping of Objects -> index for this thread.
             thread->wait_objects_index[object->GetObjectId()] = static_cast<int>(i);
             object->AddWaitingThread(thread);
-            // TODO(Subv): Perform things like update the mutex lock owner's priority to prevent priority inversion.
-            // Currently this is done in Mutex::ShouldWait, but it should be moved to a function that is called from here.
+            // TODO(Subv): Perform things like update the mutex lock owner's priority to
+            // prevent priority inversion. Currently this is done in Mutex::ShouldWait,
+            // but it should be moved to a function that is called from here.
         }
 
-        // Note: If no handles and no timeout were given, then the thread will deadlock, this is consistent with hardware behavior.
+        // Note: If no handles and no timeout were given, then the thread will deadlock, this is
+        // consistent with hardware behavior.
 
         // Create an event to wake the thread up after the specified nanosecond delay has passed
         thread->WakeAfterDelay(nano_seconds);
 
-        // Note: The output of this SVC will be set to RESULT_SUCCESS if the thread resumes due to a signal in one of its wait objects.
+        // Note: The output of this SVC will be set to RESULT_SUCCESS if the thread resumes due to a
+        // signal in one of its wait objects.
         // Otherwise we retain the default value of timeout, and -1 in the out parameter
         thread->wait_set_output = true;
         *out = -1;
-- 
cgit v1.2.3