From 409dcf0e0aecfdb676fd3b64223a25e47c1b1c1a Mon Sep 17 00:00:00 2001
From: Zach Hilman <zachhilman@gmail.com>
Date: Sun, 18 Nov 2018 23:44:19 -0500
Subject: svc: Implement yield types 0 and -1

---
 src/common/thread_queue_list.h    | 16 +++++++++++
 src/core/hle/kernel/scheduler.cpp | 18 ++++++++++++
 src/core/hle/kernel/scheduler.h   |  6 ++++
 src/core/hle/kernel/svc.cpp       | 27 ++++++++++++++++--
 src/core/hle/kernel/thread.cpp    | 60 +++++++++++++++++++++++++++++++++++++++
 src/core/hle/kernel/thread.h      |  5 ++++
 6 files changed, 130 insertions(+), 2 deletions(-)

diff --git a/src/common/thread_queue_list.h b/src/common/thread_queue_list.h
index 133122c5f..323eab97c 100644
--- a/src/common/thread_queue_list.h
+++ b/src/common/thread_queue_list.h
@@ -6,6 +6,7 @@
 
 #include <array>
 #include <deque>
+#include <functional>
 #include <boost/range/algorithm_ext/erase.hpp>
 
 namespace Common {
@@ -49,6 +50,21 @@ struct ThreadQueueList {
         return T();
     }
 
+    T get_first_filter(std::function<bool(T)> filter) const {
+        const Queue* cur = first;
+        while (cur != nullptr) {
+            if (!cur->data.empty()) {
+                for (const auto& item : cur->data) {
+                    if (filter(item))
+                        return item;
+                }
+            }
+            cur = cur->next_nonempty;
+        }
+
+        return T();
+    }
+
     T pop_first() {
         Queue* cur = first;
         while (cur != nullptr) {
diff --git a/src/core/hle/kernel/scheduler.cpp b/src/core/hle/kernel/scheduler.cpp
index 5a5f4cef1..fb5e14950 100644
--- a/src/core/hle/kernel/scheduler.cpp
+++ b/src/core/hle/kernel/scheduler.cpp
@@ -169,6 +169,16 @@ void Scheduler::UnscheduleThread(Thread* thread, u32 priority) {
     ready_queue.remove(priority, thread);
 }
 
+void Scheduler::RescheduleThread(Thread* thread, u32 priority) {
+    std::lock_guard<std::mutex> lock(scheduler_mutex);
+
+    // Thread is not in queue
+    ASSERT(ready_queue.contains(thread) != -1);
+
+    ready_queue.remove(priority, thread);
+    ready_queue.push_back(priority, thread);
+}
+
 void Scheduler::SetThreadPriority(Thread* thread, u32 priority) {
     std::lock_guard<std::mutex> lock(scheduler_mutex);
 
@@ -179,4 +189,12 @@ void Scheduler::SetThreadPriority(Thread* thread, u32 priority) {
         ready_queue.prepare(priority);
 }
 
+Thread* Scheduler::GetNextSuggestedThread(u32 core) {
+    std::lock_guard<std::mutex> lock(scheduler_mutex);
+
+    const auto mask = 1 << core;
+    return ready_queue.get_first_filter(
+        [&mask](Thread* thread) { return (thread->GetAffinityMask() & mask) != 0; });
+}
+
 } // namespace Kernel
diff --git a/src/core/hle/kernel/scheduler.h b/src/core/hle/kernel/scheduler.h
index c63032b7d..8444afdbc 100644
--- a/src/core/hle/kernel/scheduler.h
+++ b/src/core/hle/kernel/scheduler.h
@@ -48,9 +48,15 @@ public:
     /// Unschedules a thread that was already scheduled
     void UnscheduleThread(Thread* thread, u32 priority);
 
+    /// Moves a thread to the back of the current priority queue
+    void RescheduleThread(Thread* thread, u32 priority);
+
     /// Sets the priority of a thread in the scheduler
     void SetThreadPriority(Thread* thread, u32 priority);
 
+    /// Gets the next suggested thread for load balancing
+    Thread* GetNextSuggestedThread(u32 core);
+
     /// Returns a list of all threads managed by the scheduler
     const std::vector<SharedPtr<Thread>>& GetThreadList() const {
         return thread_list;
diff --git a/src/core/hle/kernel/svc.cpp b/src/core/hle/kernel/svc.cpp
index 75dbfc31d..467575c93 100644
--- a/src/core/hle/kernel/svc.cpp
+++ b/src/core/hle/kernel/svc.cpp
@@ -962,16 +962,39 @@ static void SleepThread(s64 nanoseconds) {
 
     // Don't attempt to yield execution if there are no available threads to run,
     // this way we avoid a useless reschedule to the idle thread.
-    if (nanoseconds == 0 && !Core::System::GetInstance().CurrentScheduler().HaveReadyThreads())
+    if (!Core::System::GetInstance().CurrentScheduler().HaveReadyThreads())
         return;
 
+    if (nanoseconds <= 0) {
+        switch (nanoseconds) {
+        case 0:
+            GetCurrentThread()->YieldNormal();
+            break;
+        case -1:
+            GetCurrentThread()->YieldWithLoadBalancing();
+            break;
+        case -2:
+            GetCurrentThread()->YieldAndWaitForLoadBalancing();
+            break;
+        default:
+            UNREACHABLE_MSG(
+                "Unimplemented sleep yield type '{:016X}'! Falling back to forced reschedule...",
+                nanoseconds);
+        }
+
+        nanoseconds = 0;
+    }
+
     // Sleep current thread and check for next thread to schedule
     WaitCurrentThread_Sleep();
 
     // Create an event to wake the thread up after the specified nanosecond delay has passed
     GetCurrentThread()->WakeAfterDelay(nanoseconds);
 
-    Core::System::GetInstance().PrepareReschedule();
+    Core::System::GetInstance().CpuCore(0).PrepareReschedule();
+    Core::System::GetInstance().CpuCore(1).PrepareReschedule();
+    Core::System::GetInstance().CpuCore(2).PrepareReschedule();
+    Core::System::GetInstance().CpuCore(3).PrepareReschedule();
 }
 
 /// Wait process wide key atomic
diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index 4ffb76818..ddc4da1c0 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -388,6 +388,66 @@ bool Thread::InvokeWakeupCallback(ThreadWakeupReason reason, SharedPtr<Thread> t
     return wakeup_callback(reason, std::move(thread), std::move(object), index);
 }
 
+void Thread::YieldNormal() {
+    // Avoid yielding if the thread isn't even running.
+    if (status != ThreadStatus::Running) {
+        return;
+    }
+
+    if (nominal_priority < THREADPRIO_COUNT) {
+        scheduler->RescheduleThread(this, nominal_priority);
+        scheduler->Reschedule();
+    }
+}
+
+void Thread::YieldWithLoadBalancing() {
+    auto priority = nominal_priority;
+    auto core = processor_id;
+
+    // Avoid yielding if the thread isn't even running.
+    if (status != ThreadStatus::Running) {
+        Core::System::GetInstance().CpuCore(processor_id).PrepareReschedule();
+        return;
+    }
+
+    SharedPtr<Thread> next;
+    const auto& threads = scheduler->GetThreadList();
+
+    if (priority < THREADPRIO_COUNT) {
+        // Reschedule thread to end of queue.
+        scheduler->RescheduleThread(this, priority);
+
+        const auto iter = std::find_if(threads.begin(), threads.end(),
+                                       [&priority](const SharedPtr<Thread>& thread) {
+                                           return thread->GetNominalPriority() == priority;
+                                       });
+
+        if (iter != threads.end())
+            next = iter->get();
+    }
+
+    Thread* suggested_thread = nullptr;
+
+    for (int i = 0; i < 4; ++i) {
+        if (i == core)
+            continue;
+
+        const auto res =
+            Core::System::GetInstance().CpuCore(i).Scheduler().GetNextSuggestedThread(core);
+        if (res != nullptr) {
+            suggested_thread = res;
+            break;
+        }
+    }
+
+    if (suggested_thread != nullptr)
+        suggested_thread->ChangeCore(core, suggested_thread->GetAffinityMask());
+}
+
+void Thread::YieldAndWaitForLoadBalancing() {
+    UNIMPLEMENTED_MSG("Wait for load balancing thread yield type is not implemented!");
+}
+
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 
 /**
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index d384d50db..e97434dd8 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -26,6 +26,7 @@ enum ThreadPriority : u32 {
     THREADPRIO_USERLAND_MAX = 24, ///< Highest thread priority for userland apps
     THREADPRIO_DEFAULT = 44,      ///< Default thread priority for userland apps
     THREADPRIO_LOWEST = 63,       ///< Lowest thread priority
+    THREADPRIO_COUNT = 64,        ///< Total number of possible thread priorities.
 };
 
 enum ThreadProcessorId : s32 {
@@ -370,6 +371,10 @@ public:
         return affinity_mask;
     }
 
+    void YieldNormal();
+    void YieldWithLoadBalancing();
+    void YieldAndWaitForLoadBalancing();
+
 private:
     explicit Thread(KernelCore& kernel);
     ~Thread() override;
-- 
cgit v1.2.3


From 820d81b9a5392951c18daa5a47d6c0ffd28baa9b Mon Sep 17 00:00:00 2001
From: Zach Hilman <zachhilman@gmail.com>
Date: Thu, 22 Nov 2018 00:33:53 -0500
Subject: scheduler: Add explanations for YieldWith and WithoutLoadBalancing

---
 src/common/thread_queue_list.h    |  4 +--
 src/core/hle/kernel/scheduler.cpp | 61 +++++++++++++++++++++++++++++++---
 src/core/hle/kernel/scheduler.h   | 70 +++++++++++++++++++++++++++++++++++++--
 src/core/hle/kernel/svc.cpp       | 21 ++++++++----
 src/core/hle/kernel/thread.cpp    | 60 ---------------------------------
 src/core/hle/kernel/thread.h      |  4 ---
 6 files changed, 141 insertions(+), 79 deletions(-)

diff --git a/src/common/thread_queue_list.h b/src/common/thread_queue_list.h
index 323eab97c..e7594db68 100644
--- a/src/common/thread_queue_list.h
+++ b/src/common/thread_queue_list.h
@@ -6,7 +6,6 @@
 
 #include <array>
 #include <deque>
-#include <functional>
 #include <boost/range/algorithm_ext/erase.hpp>
 
 namespace Common {
@@ -50,7 +49,8 @@ struct ThreadQueueList {
         return T();
     }
 
-    T get_first_filter(std::function<bool(T)> filter) const {
+    template <typename UnaryPredicate>
+    T get_first_filter(UnaryPredicate filter) const {
         const Queue* cur = first;
         while (cur != nullptr) {
             if (!cur->data.empty()) {
diff --git a/src/core/hle/kernel/scheduler.cpp b/src/core/hle/kernel/scheduler.cpp
index fb5e14950..624c841ad 100644
--- a/src/core/hle/kernel/scheduler.cpp
+++ b/src/core/hle/kernel/scheduler.cpp
@@ -9,6 +9,7 @@
 #include "common/logging/log.h"
 #include "core/arm/arm_interface.h"
 #include "core/core.h"
+#include "core/core_cpu.h"
 #include "core/core_timing.h"
 #include "core/hle/kernel/kernel.h"
 #include "core/hle/kernel/process.h"
@@ -169,7 +170,7 @@ void Scheduler::UnscheduleThread(Thread* thread, u32 priority) {
     ready_queue.remove(priority, thread);
 }
 
-void Scheduler::RescheduleThread(Thread* thread, u32 priority) {
+void Scheduler::MoveThreadToBackOfPriorityQueue(Thread* thread, u32 priority) {
     std::lock_guard<std::mutex> lock(scheduler_mutex);
 
     // Thread is not in queue
@@ -189,12 +190,64 @@ void Scheduler::SetThreadPriority(Thread* thread, u32 priority) {
         ready_queue.prepare(priority);
 }
 
-Thread* Scheduler::GetNextSuggestedThread(u32 core) {
+Thread* Scheduler::GetNextSuggestedThread(u32 core) const {
     std::lock_guard<std::mutex> lock(scheduler_mutex);
 
-    const auto mask = 1 << core;
+    const u32 mask = 1U << core;
     return ready_queue.get_first_filter(
-        [&mask](Thread* thread) { return (thread->GetAffinityMask() & mask) != 0; });
+        [mask](Thread const* thread) { return (thread->GetAffinityMask() & mask) != 0; });
+}
+
+void Scheduler::YieldWithoutLoadBalancing(Thread* thread) {
+    ASSERT(thread != nullptr);
+    // Avoid yielding if the thread isn't even running.
+    ASSERT(thread->GetStatus() == ThreadStatus::Running);
+
+    // Sanity check that the priority is valid
+    ASSERT(thread->GetPriority() < THREADPRIO_COUNT);
+
+    // Yield this thread
+    MoveThreadToBackOfPriorityQueue(thread, thread->GetPriority());
+    Reschedule();
+}
+
+void Scheduler::YieldWithLoadBalancing(Thread* thread) {
+    ASSERT(thread != nullptr);
+    const auto priority = thread->GetPriority();
+    const auto core = static_cast<u32>(thread->GetProcessorID());
+
+    // Avoid yielding if the thread isn't even running.
+    ASSERT(thread->GetStatus() == ThreadStatus::Running);
+
+    // Sanity check that the priority is valid
+    ASSERT(priority < THREADPRIO_COUNT);
+
+    // Reschedule thread to end of queue.
+    MoveThreadToBackOfPriorityQueue(thread, priority);
+
+    Thread* suggested_thread = nullptr;
+
+    // Search through all of the cpu cores (except this one) for a suggested thread.
+    // Take the first non-nullptr one
+    for (unsigned cur_core = 0; cur_core < Core::NUM_CPU_CORES; ++cur_core) {
+        if (cur_core == core)
+            continue;
+
+        const auto res =
+            Core::System::GetInstance().CpuCore(cur_core).Scheduler().GetNextSuggestedThread(core);
+        if (res != nullptr) {
+            suggested_thread = res;
+            break;
+        }
+    }
+
+    // If a suggested thread was found, queue that for this core
+    if (suggested_thread != nullptr)
+        suggested_thread->ChangeCore(core, suggested_thread->GetAffinityMask());
+}
+
+void Scheduler::YieldAndWaitForLoadBalancing(Thread* thread) {
+    UNIMPLEMENTED_MSG("Wait for load balancing thread yield type is not implemented!");
 }
 
 } // namespace Kernel
diff --git a/src/core/hle/kernel/scheduler.h b/src/core/hle/kernel/scheduler.h
index 8444afdbc..71b32589a 100644
--- a/src/core/hle/kernel/scheduler.h
+++ b/src/core/hle/kernel/scheduler.h
@@ -49,13 +49,79 @@ public:
     void UnscheduleThread(Thread* thread, u32 priority);
 
     /// Moves a thread to the back of the current priority queue
-    void RescheduleThread(Thread* thread, u32 priority);
+    void MoveThreadToBackOfPriorityQueue(Thread* thread, u32 priority);
 
     /// Sets the priority of a thread in the scheduler
     void SetThreadPriority(Thread* thread, u32 priority);
 
     /// Gets the next suggested thread for load balancing
-    Thread* GetNextSuggestedThread(u32 core);
+    Thread* GetNextSuggestedThread(u32 core) const;
+
+    /**
+     * YieldWithoutLoadBalancing -- analogous to normal yield on a system
+     * Moves the thread to the end of the ready queue for its priority, and then reschedules the
+     * system to the new head of the queue.
+     *
+     * Example (Single Core -- but can be extrapolated to multi):
+     * ready_queue[prio=0]: ThreadA, ThreadB, ThreadC (->exec order->)
+     * Currently Running: ThreadR
+     *
+     * ThreadR calls YieldWithoutLoadBalancing
+     *
+     * ThreadR is moved to the end of ready_queue[prio=0]:
+     * ready_queue[prio=0]: ThreadA, ThreadB, ThreadC, ThreadR (->exec order->)
+     * Currently Running: Nothing
+     *
+     * System is rescheduled (ThreadA is popped off of queue):
+     * ready_queue[prio=0]: ThreadB, ThreadC, ThreadR (->exec order->)
+     * Currently Running: ThreadA
+     *
+     * If the queue is empty at time of call, no yielding occurs. This does not cross between cores
+     * or priorities at all.
+     */
+    void YieldWithoutLoadBalancing(Thread* thread);
+
+    /**
+     * YieldWithLoadBalancing -- yield but with better selection of the new running thread
+     * Moves the current thread to the end of the ready queue for its priority, then selects a
+     * 'suggested thread' (a thread on a different core that could run on this core) from the
+     * scheduler, changes its core, and reschedules the current core to that thread.
+     *
+     * Example (Dual Core -- can be extrapolated to Quad Core, this is just normal yield if it were
+     * single core):
+     * ready_queue[core=0][prio=0]: ThreadA, ThreadB (affinities not pictured as irrelevant
+     * ready_queue[core=1][prio=0]: ThreadC[affinity=both], ThreadD[affinity=core1only]
+     * Currently Running: ThreadQ on Core 0 || ThreadP on Core 1
+     *
+     * ThreadQ calls YieldWithLoadBalancing
+     *
+     * ThreadQ is moved to the end of ready_queue[core=0][prio=0]:
+     * ready_queue[core=0][prio=0]: ThreadA, ThreadB
+     * ready_queue[core=1][prio=0]: ThreadC[affinity=both], ThreadD[affinity=core1only]
+     * Currently Running: ThreadQ on Core 0 || ThreadP on Core 1
+     *
+     * A list of suggested threads for each core is compiled
+     * Suggested Threads: {ThreadC on Core 1}
+     * If this were quad core (as the switch is), there could be between 0 and 3 threads in this
+     * list. If there are more than one, the thread is selected by highest prio.
+     *
+     * ThreadC is core changed to Core 0:
+     * ready_queue[core=0][prio=0]: ThreadC, ThreadA, ThreadB, ThreadQ
+     * ready_queue[core=1][prio=0]: ThreadD
+     * Currently Running: None on Core 0 || ThreadP on Core 1
+     *
+     * System is rescheduled (ThreadC is popped off of queue):
+     * ready_queue[core=0][prio=0]: ThreadA, ThreadB, ThreadQ
+     * ready_queue[core=1][prio=0]: ThreadD
+     * Currently Running: ThreadC on Core 0 || ThreadP on Core 1
+     *
+     * If no suggested threads can be found this will behave just as normal yield. If there are
+     * multiple candidates for the suggested thread on a core, the highest prio is taken.
+     */
+    void YieldWithLoadBalancing(Thread* thread);
+
+    /// Currently unknown -- asserts as unimplemented on call
+    void YieldAndWaitForLoadBalancing(Thread* thread);
 
     /// Returns a list of all threads managed by the scheduler
     const std::vector<SharedPtr<Thread>>& GetThreadList() const {
diff --git a/src/core/hle/kernel/svc.cpp b/src/core/hle/kernel/svc.cpp
index 467575c93..205706033 100644
--- a/src/core/hle/kernel/svc.cpp
+++ b/src/core/hle/kernel/svc.cpp
@@ -965,16 +965,23 @@ static void SleepThread(s64 nanoseconds) {
     if (!Core::System::GetInstance().CurrentScheduler().HaveReadyThreads())
         return;
 
+    enum class SleepType : s64 {
+        YieldWithoutLoadBalancing = 0,
+        YieldWithLoadBalancing = 1,
+        YieldAndWaitForLoadBalancing = 2,
+    };
+
     if (nanoseconds <= 0) {
-        switch (nanoseconds) {
-        case 0:
-            GetCurrentThread()->YieldNormal();
+        auto& scheduler{Core::System::GetInstance().CurrentScheduler()};
+        switch (static_cast<SleepType>(nanoseconds)) {
+        case SleepType::YieldWithoutLoadBalancing:
+            scheduler.YieldWithoutLoadBalancing(GetCurrentThread());
             break;
-        case -1:
-            GetCurrentThread()->YieldWithLoadBalancing();
+        case SleepType::YieldWithLoadBalancing:
+            scheduler.YieldWithLoadBalancing(GetCurrentThread());
             break;
-        case -2:
-            GetCurrentThread()->YieldAndWaitForLoadBalancing();
+        case SleepType::YieldAndWaitForLoadBalancing:
+            scheduler.YieldAndWaitForLoadBalancing(GetCurrentThread());
             break;
         default:
             UNREACHABLE_MSG(
diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index ddc4da1c0..4ffb76818 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -388,66 +388,6 @@ bool Thread::InvokeWakeupCallback(ThreadWakeupReason reason, SharedPtr<Thread> t
     return wakeup_callback(reason, std::move(thread), std::move(object), index);
 }
 
-void Thread::YieldNormal() {
-    // Avoid yielding if the thread isn't even running.
-    if (status != ThreadStatus::Running) {
-        return;
-    }
-
-    if (nominal_priority < THREADPRIO_COUNT) {
-        scheduler->RescheduleThread(this, nominal_priority);
-        scheduler->Reschedule();
-    }
-}
-
-void Thread::YieldWithLoadBalancing() {
-    auto priority = nominal_priority;
-    auto core = processor_id;
-
-    // Avoid yielding if the thread isn't even running.
-    if (status != ThreadStatus::Running) {
-        Core::System::GetInstance().CpuCore(processor_id).PrepareReschedule();
-        return;
-    }
-
-    SharedPtr<Thread> next;
-    const auto& threads = scheduler->GetThreadList();
-
-    if (priority < THREADPRIO_COUNT) {
-        // Reschedule thread to end of queue.
-        scheduler->RescheduleThread(this, priority);
-
-        const auto iter = std::find_if(threads.begin(), threads.end(),
-                                       [&priority](const SharedPtr<Thread>& thread) {
-                                           return thread->GetNominalPriority() == priority;
-                                       });
-
-        if (iter != threads.end())
-            next = iter->get();
-    }
-
-    Thread* suggested_thread = nullptr;
-
-    for (int i = 0; i < 4; ++i) {
-        if (i == core)
-            continue;
-
-        const auto res =
-            Core::System::GetInstance().CpuCore(i).Scheduler().GetNextSuggestedThread(core);
-        if (res != nullptr) {
-            suggested_thread = res;
-            break;
-        }
-    }
-
-    if (suggested_thread != nullptr)
-        suggested_thread->ChangeCore(core, suggested_thread->GetAffinityMask());
-}
-
-void Thread::YieldAndWaitForLoadBalancing() {
-    UNIMPLEMENTED_MSG("Wait for load balancing thread yield type is not implemented!");
-}
-
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 
 /**
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index e97434dd8..77aec099a 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -371,10 +371,6 @@ public:
         return affinity_mask;
     }
 
-    void YieldNormal();
-    void YieldWithLoadBalancing();
-    void YieldAndWaitForLoadBalancing();
-
 private:
     explicit Thread(KernelCore& kernel);
     ~Thread() override;
-- 
cgit v1.2.3


From 3476830b26b61410b633c827e985bffa1dc52528 Mon Sep 17 00:00:00 2001
From: Zach Hilman <zachhilman@gmail.com>
Date: Sun, 2 Dec 2018 00:44:40 -0500
Subject: svc: Avoid performance-degrading unnecessary reschedule

---
 src/core/hle/kernel/scheduler.cpp |  3 ++-
 src/core/hle/kernel/svc.cpp       | 11 ++++-------
 2 files changed, 6 insertions(+), 8 deletions(-)

diff --git a/src/core/hle/kernel/scheduler.cpp b/src/core/hle/kernel/scheduler.cpp
index 624c841ad..efe3551e2 100644
--- a/src/core/hle/kernel/scheduler.cpp
+++ b/src/core/hle/kernel/scheduler.cpp
@@ -207,8 +207,8 @@ void Scheduler::YieldWithoutLoadBalancing(Thread* thread) {
     ASSERT(thread->GetPriority() < THREADPRIO_COUNT);
 
     // Yield this thread
-    MoveThreadToBackOfPriorityQueue(thread, thread->GetPriority());
     Reschedule();
+    MoveThreadToBackOfPriorityQueue(thread, thread->GetPriority());
 }
 
 void Scheduler::YieldWithLoadBalancing(Thread* thread) {
@@ -223,6 +223,7 @@ void Scheduler::YieldWithLoadBalancing(Thread* thread) {
     ASSERT(priority < THREADPRIO_COUNT);
 
     // Reschedule thread to end of queue.
+    Reschedule();
     MoveThreadToBackOfPriorityQueue(thread, priority);
 
     Thread* suggested_thread = nullptr;
diff --git a/src/core/hle/kernel/svc.cpp b/src/core/hle/kernel/svc.cpp
index 205706033..c119f7be1 100644
--- a/src/core/hle/kernel/svc.cpp
+++ b/src/core/hle/kernel/svc.cpp
@@ -962,13 +962,13 @@ static void SleepThread(s64 nanoseconds) {
 
     // Don't attempt to yield execution if there are no available threads to run,
     // this way we avoid a useless reschedule to the idle thread.
-    if (!Core::System::GetInstance().CurrentScheduler().HaveReadyThreads())
+    if (nanoseconds <= 0 && !Core::System::GetInstance().CurrentScheduler().HaveReadyThreads())
         return;
 
     enum class SleepType : s64 {
         YieldWithoutLoadBalancing = 0,
-        YieldWithLoadBalancing = 1,
-        YieldAndWaitForLoadBalancing = 2,
+        YieldWithLoadBalancing = -1,
+        YieldAndWaitForLoadBalancing = -2,
     };
 
     if (nanoseconds <= 0) {
@@ -998,10 +998,7 @@ static void SleepThread(s64 nanoseconds) {
     // Create an event to wake the thread up after the specified nanosecond delay has passed
     GetCurrentThread()->WakeAfterDelay(nanoseconds);
 
-    Core::System::GetInstance().CpuCore(0).PrepareReschedule();
-    Core::System::GetInstance().CpuCore(1).PrepareReschedule();
-    Core::System::GetInstance().CpuCore(2).PrepareReschedule();
-    Core::System::GetInstance().CpuCore(3).PrepareReschedule();
+    Core::System::GetInstance().PrepareReschedule();
 }
 
 /// Wait process wide key atomic
-- 
cgit v1.2.3


From b5af41a07bebc0a378428e7d7ddc68c9c750d2d1 Mon Sep 17 00:00:00 2001
From: Zach Hilman <zachhilman@gmail.com>
Date: Mon, 3 Dec 2018 17:29:21 -0500
Subject: scheduler: Only work steal higher priority threads from other cores

---
 src/core/hle/kernel/scheduler.cpp | 38 +++++++++++++++++---------------------
 src/core/hle/kernel/scheduler.h   |  5 +----
 src/core/hle/kernel/svc.cpp       | 16 ++++++----------
 3 files changed, 24 insertions(+), 35 deletions(-)

diff --git a/src/core/hle/kernel/scheduler.cpp b/src/core/hle/kernel/scheduler.cpp
index efe3551e2..c6b7d5232 100644
--- a/src/core/hle/kernel/scheduler.cpp
+++ b/src/core/hle/kernel/scheduler.cpp
@@ -170,16 +170,6 @@ void Scheduler::UnscheduleThread(Thread* thread, u32 priority) {
     ready_queue.remove(priority, thread);
 }
 
-void Scheduler::MoveThreadToBackOfPriorityQueue(Thread* thread, u32 priority) {
-    std::lock_guard<std::mutex> lock(scheduler_mutex);
-
-    // Thread is not in queue
-    ASSERT(ready_queue.contains(thread) != -1);
-
-    ready_queue.remove(priority, thread);
-    ready_queue.push_back(priority, thread);
-}
-
 void Scheduler::SetThreadPriority(Thread* thread, u32 priority) {
     std::lock_guard<std::mutex> lock(scheduler_mutex);
 
@@ -190,12 +180,13 @@ void Scheduler::SetThreadPriority(Thread* thread, u32 priority) {
         ready_queue.prepare(priority);
 }
 
-Thread* Scheduler::GetNextSuggestedThread(u32 core) const {
+Thread* Scheduler::GetNextSuggestedThread(u32 core, u32 maximum_priority) const {
     std::lock_guard<std::mutex> lock(scheduler_mutex);
 
     const u32 mask = 1U << core;
-    return ready_queue.get_first_filter(
-        [mask](Thread const* thread) { return (thread->GetAffinityMask() & mask) != 0; });
+    return ready_queue.get_first_filter([mask, maximum_priority](Thread const* thread) {
+        return (thread->GetAffinityMask() & mask) != 0 && thread->GetPriority() < maximum_priority;
+    });
 }
 
 void Scheduler::YieldWithoutLoadBalancing(Thread* thread) {
@@ -206,9 +197,10 @@ void Scheduler::YieldWithoutLoadBalancing(Thread* thread) {
     // Sanity check that the priority is valid
     ASSERT(thread->GetPriority() < THREADPRIO_COUNT);
 
-    // Yield this thread
+    // Yield this thread -- sleep for zero time and force reschedule to different thread
+    WaitCurrentThread_Sleep();
+    GetCurrentThread()->WakeAfterDelay(0);
     Reschedule();
-    MoveThreadToBackOfPriorityQueue(thread, thread->GetPriority());
 }
 
 void Scheduler::YieldWithLoadBalancing(Thread* thread) {
@@ -222,9 +214,9 @@ void Scheduler::YieldWithLoadBalancing(Thread* thread) {
     // Sanity check that the priority is valid
     ASSERT(priority < THREADPRIO_COUNT);
 
-    // Reschedule thread to end of queue.
-    Reschedule();
-    MoveThreadToBackOfPriorityQueue(thread, priority);
+    // Sleep for zero time to be able to force reschedule to different thread
+    WaitCurrentThread_Sleep();
+    GetCurrentThread()->WakeAfterDelay(0);
 
     Thread* suggested_thread = nullptr;
 
@@ -235,16 +227,20 @@ void Scheduler::YieldWithLoadBalancing(Thread* thread) {
             continue;
 
         const auto res =
-            Core::System::GetInstance().CpuCore(cur_core).Scheduler().GetNextSuggestedThread(core);
-        if (res != nullptr) {
+            Core::System::GetInstance().CpuCore(cur_core).Scheduler().GetNextSuggestedThread(
+                core, priority);
+        if (res != nullptr &&
+            (suggested_thread == nullptr || suggested_thread->GetPriority() > res->GetPriority())) {
             suggested_thread = res;
-            break;
         }
     }
 
     // If a suggested thread was found, queue that for this core
     if (suggested_thread != nullptr)
         suggested_thread->ChangeCore(core, suggested_thread->GetAffinityMask());
+
+    // Perform actual yielding.
+    Reschedule();
 }
 
 void Scheduler::YieldAndWaitForLoadBalancing(Thread* thread) {
diff --git a/src/core/hle/kernel/scheduler.h b/src/core/hle/kernel/scheduler.h
index 71b32589a..97ced4dfc 100644
--- a/src/core/hle/kernel/scheduler.h
+++ b/src/core/hle/kernel/scheduler.h
@@ -48,14 +48,11 @@ public:
     /// Unschedules a thread that was already scheduled
     void UnscheduleThread(Thread* thread, u32 priority);
 
-    /// Moves a thread to the back of the current priority queue
-    void MoveThreadToBackOfPriorityQueue(Thread* thread, u32 priority);
-
     /// Sets the priority of a thread in the scheduler
     void SetThreadPriority(Thread* thread, u32 priority);
 
     /// Gets the next suggested thread for load balancing
-    Thread* GetNextSuggestedThread(u32 core) const;
+    Thread* GetNextSuggestedThread(u32 core, u32 minimum_priority) const;
 
     /**
      * YieldWithoutLoadBalancing -- analogous to normal yield on a system
diff --git a/src/core/hle/kernel/svc.cpp b/src/core/hle/kernel/svc.cpp
index c119f7be1..fabdedd3d 100644
--- a/src/core/hle/kernel/svc.cpp
+++ b/src/core/hle/kernel/svc.cpp
@@ -984,20 +984,16 @@ static void SleepThread(s64 nanoseconds) {
             scheduler.YieldAndWaitForLoadBalancing(GetCurrentThread());
             break;
         default:
-            UNREACHABLE_MSG(
-                "Unimplemented sleep yield type '{:016X}'! Falling back to forced reschedule...",
-                nanoseconds);
+            UNREACHABLE_MSG("Unimplemented sleep yield type '{:016X}'!", nanoseconds);
         }
+    } else {
+        // Sleep current thread and check for next thread to schedule
+        WaitCurrentThread_Sleep();
 
-        nanoseconds = 0;
+        // Create an event to wake the thread up after the specified nanosecond delay has passed
+        GetCurrentThread()->WakeAfterDelay(nanoseconds);
     }
 
-    // Sleep current thread and check for next thread to schedule
-    WaitCurrentThread_Sleep();
-
-    // Create an event to wake the thread up after the specified nanosecond delay has passed
-    GetCurrentThread()->WakeAfterDelay(nanoseconds);
-
     Core::System::GetInstance().PrepareReschedule();
 }
 
-- 
cgit v1.2.3


From ddf5903cd9c05f1fecd8a5b8e8ad702b9b20eef8 Mon Sep 17 00:00:00 2001
From: Zach Hilman <zachhilman@gmail.com>
Date: Mon, 3 Dec 2018 21:22:09 -0500
Subject: scheduler: Avoid manual Reschedule call

This will automatically occur anyway when PrepareReschedule is called
---
 src/core/hle/kernel/scheduler.cpp | 18 ++++++++----------
 src/core/hle/kernel/svc.cpp       |  4 +++-
 2 files changed, 11 insertions(+), 11 deletions(-)

diff --git a/src/core/hle/kernel/scheduler.cpp b/src/core/hle/kernel/scheduler.cpp
index c6b7d5232..df4d6cf0a 100644
--- a/src/core/hle/kernel/scheduler.cpp
+++ b/src/core/hle/kernel/scheduler.cpp
@@ -200,7 +200,6 @@ void Scheduler::YieldWithoutLoadBalancing(Thread* thread) {
     // Yield this thread -- sleep for zero time and force reschedule to different thread
     WaitCurrentThread_Sleep();
     GetCurrentThread()->WakeAfterDelay(0);
-    Reschedule();
 }
 
 void Scheduler::YieldWithLoadBalancing(Thread* thread) {
@@ -223,24 +222,23 @@ void Scheduler::YieldWithLoadBalancing(Thread* thread) {
     // Search through all of the cpu cores (except this one) for a suggested thread.
     // Take the first non-nullptr one
     for (unsigned cur_core = 0; cur_core < Core::NUM_CPU_CORES; ++cur_core) {
-        if (cur_core == core)
-            continue;
-
         const auto res =
             Core::System::GetInstance().CpuCore(cur_core).Scheduler().GetNextSuggestedThread(
                 core, priority);
-        if (res != nullptr &&
-            (suggested_thread == nullptr || suggested_thread->GetPriority() > res->GetPriority())) {
-            suggested_thread = res;
+
+        // If scheduler provides a suggested thread
+        if (res != nullptr) {
+            // And its better than the current suggested thread (or is the first valid one)
+            if (suggested_thread == nullptr ||
+                suggested_thread->GetPriority() > res->GetPriority()) {
+                suggested_thread = res;
+            }
         }
     }
 
     // If a suggested thread was found, queue that for this core
     if (suggested_thread != nullptr)
         suggested_thread->ChangeCore(core, suggested_thread->GetAffinityMask());
-
-    // Perform actual yielding.
-    Reschedule();
 }
 
 void Scheduler::YieldAndWaitForLoadBalancing(Thread* thread) {
diff --git a/src/core/hle/kernel/svc.cpp b/src/core/hle/kernel/svc.cpp
index fabdedd3d..29c2c2d03 100644
--- a/src/core/hle/kernel/svc.cpp
+++ b/src/core/hle/kernel/svc.cpp
@@ -994,7 +994,9 @@ static void SleepThread(s64 nanoseconds) {
         GetCurrentThread()->WakeAfterDelay(nanoseconds);
     }
 
-    Core::System::GetInstance().PrepareReschedule();
+    // Reschedule all CPU cores
+    for (std::size_t i = 0; i < 4; ++i)
+        Core::System::GetInstance().CpuCore(i).PrepareReschedule();
 }
 
 /// Wait process wide key atomic
-- 
cgit v1.2.3


From e6f7825a248dd0ff9f2e3fdccabdbe3631622861 Mon Sep 17 00:00:00 2001
From: Zach Hilman <zachhilman@gmail.com>
Date: Tue, 4 Dec 2018 22:11:32 -0500
Subject: svc: Avoid incorrect fast yield condition

---
 src/core/hle/kernel/svc.cpp | 7 +------
 1 file changed, 1 insertion(+), 6 deletions(-)

diff --git a/src/core/hle/kernel/svc.cpp b/src/core/hle/kernel/svc.cpp
index 29c2c2d03..e3cf3f909 100644
--- a/src/core/hle/kernel/svc.cpp
+++ b/src/core/hle/kernel/svc.cpp
@@ -960,11 +960,6 @@ static void ExitThread() {
 static void SleepThread(s64 nanoseconds) {
     LOG_TRACE(Kernel_SVC, "called nanoseconds={}", nanoseconds);
 
-    // Don't attempt to yield execution if there are no available threads to run,
-    // this way we avoid a useless reschedule to the idle thread.
-    if (nanoseconds <= 0 && !Core::System::GetInstance().CurrentScheduler().HaveReadyThreads())
-        return;
-
     enum class SleepType : s64 {
         YieldWithoutLoadBalancing = 0,
         YieldWithLoadBalancing = -1,
@@ -995,7 +990,7 @@ static void SleepThread(s64 nanoseconds) {
     }
 
     // Reschedule all CPU cores
-    for (std::size_t i = 0; i < 4; ++i)
+    for (std::size_t i = 0; i < Core::NUM_CPU_CORES; ++i)
         Core::System::GetInstance().CpuCore(i).PrepareReschedule();
 }
 
-- 
cgit v1.2.3