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/core/hle/kernel/kernel.cpp

@@ -31,13 +31,62 @@ void WaitObject::RemoveWaitingThread(Thread* thread) {
         waiting_threads.erase(itr);
 }
 
+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;
+
+    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;
+    }), 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() {
-    for (auto thread : waiting_threads)
+    // 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();
-
-    waiting_threads.clear();
-
-    HLE::Reschedule(__func__);
+        // Note: Removing the thread from the object's waitlist will be done by GetHighestPriorityReadyThread
+    }
 }
 
 const std::vector<SharedPtr<Thread>>& WaitObject::GetWaitingThreads() const {