1 files changed, 49 insertions, 35 deletions

diff --git a/src/core/hle/kernel/mutex.cpp b/src/core/hle/kernel/mutex.cpp
index 736944bae..cef961289 100644
--- a/src/core/hle/kernel/mutex.cpp
+++ b/src/core/hle/kernel/mutex.cpp
@@ -6,26 +6,18 @@
 #include <vector>
 #include <boost/range/algorithm_ext/erase.hpp>
 #include "common/assert.h"
+#include "core/core.h"
 #include "core/hle/kernel/kernel.h"
 #include "core/hle/kernel/mutex.h"
 #include "core/hle/kernel/thread.h"
 
 namespace Kernel {
 
-/**
- * Resumes a thread waiting for the specified mutex
- * @param mutex The mutex that some thread is waiting on
- */
-static void ResumeWaitingThread(Mutex* mutex) {
-    // Reset mutex lock thread handle, nothing is waiting
-    mutex->lock_count = 0;
-    mutex->holding_thread = nullptr;
-    mutex->WakeupAllWaitingThreads();
-}
-
 void ReleaseThreadMutexes(Thread* thread) {
     for (auto& mtx : thread->held_mutexes) {
-        ResumeWaitingThread(mtx.get());
+        mtx->lock_count = 0;
+        mtx->holding_thread = nullptr;
+        mtx->WakeupAllWaitingThreads();
     }
     thread->held_mutexes.clear();
 }
@@ -40,52 +32,74 @@ SharedPtr<Mutex> Mutex::Create(bool initial_locked, std::string name) {
     mutex->name = std::move(name);
     mutex->holding_thread = nullptr;
 
-    // Acquire mutex with current thread if initialized as locked...
+    // Acquire mutex with current thread if initialized as locked
     if (initial_locked)
-        mutex->Acquire();
+        mutex->Acquire(GetCurrentThread());
 
     return mutex;
 }
 
-bool Mutex::ShouldWait() {
-    auto thread = GetCurrentThread();
-    bool wait = lock_count > 0 && holding_thread != thread;
-
-    // If the holding thread of the mutex is lower priority than this thread, that thread should
-    // temporarily inherit this thread's priority
-    if (wait && thread->current_priority < holding_thread->current_priority)
-        holding_thread->BoostPriority(thread->current_priority);
-
-    return wait;
-}
-
-void Mutex::Acquire() {
-    Acquire(GetCurrentThread());
+bool Mutex::ShouldWait(Thread* thread) const {
+    return lock_count > 0 && thread != holding_thread;
 }
 
-void Mutex::Acquire(SharedPtr<Thread> thread) {
-    ASSERT_MSG(!ShouldWait(), "object unavailable!");
+void Mutex::Acquire(Thread* thread) {
+    ASSERT_MSG(!ShouldWait(thread), "object unavailable!");
 
-    // Actually "acquire" the mutex only if we don't already have it...
+    // Actually "acquire" the mutex only if we don't already have it
     if (lock_count == 0) {
+        priority = thread->current_priority;
         thread->held_mutexes.insert(this);
-        holding_thread = std::move(thread);
+        holding_thread = thread;
+        thread->UpdatePriority();
+        Core::System::GetInstance().PrepareReschedule();
     }
 
     lock_count++;
 }
 
 void Mutex::Release() {
-    // Only release if the mutex is held...
+    // Only release if the mutex is held
     if (lock_count > 0) {
         lock_count--;
 
-        // Yield to the next thread only if we've fully released the mutex...
+        // Yield to the next thread only if we've fully released the mutex
         if (lock_count == 0) {
             holding_thread->held_mutexes.erase(this);
-            ResumeWaitingThread(this);
+            holding_thread->UpdatePriority();
+            holding_thread = nullptr;
+            WakeupAllWaitingThreads();
+            Core::System::GetInstance().PrepareReschedule();
         }
     }
 }
 
+void Mutex::AddWaitingThread(SharedPtr<Thread> thread) {
+    WaitObject::AddWaitingThread(thread);
+    thread->pending_mutexes.insert(this);
+    UpdatePriority();
+}
+
+void Mutex::RemoveWaitingThread(Thread* thread) {
+    WaitObject::RemoveWaitingThread(thread);
+    thread->pending_mutexes.erase(this);
+    UpdatePriority();
+}
+
+void Mutex::UpdatePriority() {
+    if (!holding_thread)
+        return;
+
+    s32 best_priority = THREADPRIO_LOWEST;
+    for (auto& waiter : GetWaitingThreads()) {
+        if (waiter->current_priority < best_priority)
+            best_priority = waiter->current_priority;
+    }
+
+    if (best_priority != priority) {
+        priority = best_priority;
+        holding_thread->UpdatePriority();
+    }
+}
+
 } // namespace