Kernel/Arbiters: When doing ArbitrateAddress(Signal), always pick the highest priority thread, using the first one that was put to sleep if more than one thread with the same highest priority exists.

This is consistent with hardware behavior as shown by this test https://gist.github.com/ds84182/40e46129bd38b46a5100f15f96ba5eaf

src/core/hle/kernel/address_arbiter.cpp

@@ -2,6 +2,7 @@
 // Licensed under GPLv2 or any later version
 // Refer to the license.txt file included.
 
+#include <algorithm>
 #include "common/common_types.h"
 #include "common/logging/log.h"
 #include "core/hle/kernel/address_arbiter.h"
@@ -14,6 +15,55 @@
 
 namespace Kernel {
 
+void AddressArbiter::WaitThread(SharedPtr<Thread> thread, VAddr wait_address) {
+    thread->wait_address = wait_address;
+    thread->status = THREADSTATUS_WAIT_ARB;
+    waiting_threads.emplace_back(std::move(thread));
+}
+
+void AddressArbiter::ResumeAllThreads(VAddr address) {
+    // Determine which threads are waiting on this address, those should be woken up.
+    auto itr = std::stable_partition(waiting_threads.begin(), waiting_threads.end(),
+                                     [address](const auto& thread) {
+                                         ASSERT_MSG(thread->status == THREADSTATUS_WAIT_ARB,
+                                                    "Inconsistent AddressArbiter state");
+                                         return thread->wait_address != address;
+                                     });
+
+    // Wake up all the found threads
+    std::for_each(itr, waiting_threads.end(), [](auto& thread) { thread->ResumeFromWait(); });
+
+    // Remove the woken up threads from the wait list.
+    waiting_threads.erase(itr, waiting_threads.end());
+}
+
+SharedPtr<Thread> AddressArbiter::ResumeHighestPriorityThread(VAddr address) {
+    // Determine which threads are waiting on this address, those should be considered for wakeup.
+    auto matches_start = std::stable_partition(
+        waiting_threads.begin(), waiting_threads.end(), [address](const auto& thread) {
+            ASSERT_MSG(thread->status == THREADSTATUS_WAIT_ARB,
+                       "Inconsistent AddressArbiter state");
+            return thread->wait_address != address;
+        });
+
+    // Iterate through threads, find highest priority thread that is waiting to be arbitrated.
+    // Note: The real kernel will pick the first thread in the list if more than one have the
+    // same highest priority value. Lower priority values mean higher priority.
+    auto itr = std::min_element(matches_start, waiting_threads.end(),
+                                [](const auto& lhs, const auto& rhs) {
+                                    return lhs->current_priority < rhs->current_priority;
+                                });
+
+    if (itr == waiting_threads.end())
+        return nullptr;
+
+    auto thread = *itr;
+    thread->ResumeFromWait();
+
+    waiting_threads.erase(itr);
+    return thread;
+}
+
 AddressArbiter::AddressArbiter() {}
 AddressArbiter::~AddressArbiter() {}
 
@@ -25,32 +75,32 @@ SharedPtr<AddressArbiter> AddressArbiter::Create(std::string name) {
     return address_arbiter;
 }
 
-ResultCode AddressArbiter::ArbitrateAddress(ArbitrationType type, VAddr address, s32 value,
-                                            u64 nanoseconds) {
+ResultCode AddressArbiter::ArbitrateAddress(SharedPtr<Thread> thread, ArbitrationType type,
+                                            VAddr address, s32 value, u64 nanoseconds) {
     switch (type) {
 
     // Signal thread(s) waiting for arbitrate address...
     case ArbitrationType::Signal:
         // Negative value means resume all threads
         if (value < 0) {
-            ArbitrateAllThreads(address);
+            ResumeAllThreads(address);
         } else {
             // Resume first N threads
             for (int i = 0; i < value; i++)
-                ArbitrateHighestPriorityThread(address);
+                ResumeHighestPriorityThread(address);
         }
         break;
 
     // Wait current thread (acquire the arbiter)...
     case ArbitrationType::WaitIfLessThan:
         if ((s32)Memory::Read32(address) < value) {
-            Kernel::WaitCurrentThread_ArbitrateAddress(address);
+            WaitThread(std::move(thread), address);
         }
         break;
     case ArbitrationType::WaitIfLessThanWithTimeout:
         if ((s32)Memory::Read32(address) < value) {
-            Kernel::WaitCurrentThread_ArbitrateAddress(address);
-            GetCurrentThread()->WakeAfterDelay(nanoseconds);
+            thread->WakeAfterDelay(nanoseconds);
+            WaitThread(std::move(thread), address);
         }
         break;
     case ArbitrationType::DecrementAndWaitIfLessThan: {
@@ -58,7 +108,7 @@ ResultCode AddressArbiter::ArbitrateAddress(ArbitrationType type, VAddr address,
         if (memory_value < value) {
             // Only change the memory value if the thread should wait
             Memory::Write32(address, (s32)memory_value - 1);
-            Kernel::WaitCurrentThread_ArbitrateAddress(address);
+            WaitThread(std::move(thread), address);
         }
         break;
     }
@@ -67,8 +117,8 @@ ResultCode AddressArbiter::ArbitrateAddress(ArbitrationType type, VAddr address,
         if (memory_value < value) {
             // Only change the memory value if the thread should wait
             Memory::Write32(address, (s32)memory_value - 1);
-            Kernel::WaitCurrentThread_ArbitrateAddress(address);
-            GetCurrentThread()->WakeAfterDelay(nanoseconds);
+            thread->WakeAfterDelay(nanoseconds);
+            WaitThread(std::move(thread), address);
         }
         break;
     }

src/core/hle/kernel/address_arbiter.h

@@ -4,6 +4,7 @@
 
 #pragma once
 
+#include <vector>
 #include "common/common_types.h"
 #include "core/hle/kernel/kernel.h"
 #include "core/hle/result.h"
@@ -11,13 +12,15 @@
 // Address arbiters are an underlying kernel synchronization object that can be created/used via
 // supervisor calls (SVCs). They function as sort of a global lock. Typically, games/other CTR
 // applications use them as an underlying mechanism to implement thread-safe barriers, events, and
-// semphores.
+// semaphores.
 
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 // Kernel namespace
 
 namespace Kernel {
 
+class Thread;
+
 enum class ArbitrationType : u32 {
     Signal,
     WaitIfLessThan,
@@ -50,11 +53,25 @@ public:
 
     std::string name; ///< Name of address arbiter object (optional)
 
-    ResultCode ArbitrateAddress(ArbitrationType type, VAddr address, s32 value, u64 nanoseconds);
+    ResultCode ArbitrateAddress(SharedPtr<Thread> thread, ArbitrationType type, VAddr address,
+                                s32 value, u64 nanoseconds);
 
 private:
     AddressArbiter();
     ~AddressArbiter() override;
+
+    /// Puts the thread to wait on the specified arbitration address under this address arbiter.
+    void WaitThread(SharedPtr<Thread> thread, VAddr wait_address);
+
+    /// Resume all threads found to be waiting on the address under this address arbiter
+    void ResumeAllThreads(VAddr address);
+
+    /// Resume one thread found to be waiting on the address under this address arbiter and return
+    /// the resumed thread.
+    SharedPtr<Thread> ResumeHighestPriorityThread(VAddr address);
+
+    /// Threads waiting for the address arbiter to be signaled.
+    std::vector<SharedPtr<Thread>> waiting_threads;
 };
 
-} // namespace FileSys
+} // namespace Kernel

src/core/hle/kernel/thread.cpp

@@ -67,16 +67,6 @@ Thread* GetCurrentThread() {
     return current_thread.get();
 }
 
-/**
- * Check if the specified thread is waiting on the specified address to be arbitrated
- * @param thread The thread to test
- * @param wait_address The address to test against
- * @return True if the thread is waiting, false otherwise
- */
-static bool CheckWait_AddressArbiter(const Thread* thread, VAddr wait_address) {
-    return thread->status == THREADSTATUS_WAIT_ARB && wait_address == thread->wait_address;
-}
-
 void Thread::Stop() {
     // Cancel any outstanding wakeup events for this thread
     CoreTiming::UnscheduleEvent(ThreadWakeupEventType, callback_handle);
@@ -109,40 +99,6 @@ void Thread::Stop() {
     Kernel::g_current_process->tls_slots[tls_page].reset(tls_slot);
 }
 
-Thread* ArbitrateHighestPriorityThread(u32 address) {
-    Thread* highest_priority_thread = nullptr;
-    u32 priority = THREADPRIO_LOWEST;
-
-    // Iterate through threads, find highest priority thread that is waiting to be arbitrated...
-    for (auto& thread : thread_list) {
-        if (!CheckWait_AddressArbiter(thread.get(), address))
-            continue;
-
-        if (thread == nullptr)
-            continue;
-
-        if (thread->current_priority <= priority) {
-            highest_priority_thread = thread.get();
-            priority = thread->current_priority;
-        }
-    }
-
-    // If a thread was arbitrated, resume it
-    if (nullptr != highest_priority_thread) {
-        highest_priority_thread->ResumeFromWait();
-    }
-
-    return highest_priority_thread;
-}
-
-void ArbitrateAllThreads(u32 address) {
-    // Resume all threads found to be waiting on the address
-    for (auto& thread : thread_list) {
-        if (CheckWait_AddressArbiter(thread.get(), address))
-            thread->ResumeFromWait();
-    }
-}
-
 /**
  * Switches the CPU's active thread context to that of the specified thread
  * @param new_thread The thread to switch to
@@ -220,12 +176,6 @@ void WaitCurrentThread_Sleep() {
     thread->status = THREADSTATUS_WAIT_SLEEP;
 }
 
-void WaitCurrentThread_ArbitrateAddress(VAddr wait_address) {
-    Thread* thread = GetCurrentThread();
-    thread->wait_address = wait_address;
-    thread->status = THREADSTATUS_WAIT_ARB;
-}
-
 void ExitCurrentThread() {
     Thread* thread = GetCurrentThread();
     thread->Stop();

src/core/hle/svc.cpp

@@ -629,7 +629,8 @@ static ResultCode ArbitrateAddress(Kernel::Handle handle, u32 address, u32 type,
     if (arbiter == nullptr)
         return ERR_INVALID_HANDLE;
 
-    auto res = arbiter->ArbitrateAddress(static_cast<Kernel::ArbitrationType>(type), address, value,
+    auto res = arbiter->ArbitrateAddress(Kernel::GetCurrentThread(),
+                                         static_cast<Kernel::ArbitrationType>(type), address, value,
                                          nanoseconds);
 
     // TODO(Subv): Identify in which specific cases this call should cause a reschedule.