5 files changed, 60 insertions, 42 deletions

diff --git a/src/core/hle/kernel/mutex.cpp b/src/core/hle/kernel/mutex.cpp
index 84ff65150..cef961289 100644
--- a/src/core/hle/kernel/mutex.cpp
+++ b/src/core/hle/kernel/mutex.cpp
@@ -13,38 +13,6 @@
 
 namespace Kernel {
 
-/**
- * Boost's a thread's priority to the best priority among the thread's held mutexes.
- * This prevents priority inversion via priority inheritance.
- */
-static void UpdateThreadPriority(Thread* thread) {
-    s32 best_priority = THREADPRIO_LOWEST;
-    for (auto& mutex : thread->held_mutexes) {
-        if (mutex->priority < best_priority)
-            best_priority = mutex->priority;
-    }
-
-    best_priority = std::min(best_priority, thread->nominal_priority);
-    thread->SetPriority(best_priority);
-}
-
-/**
- * Elevate the mutex priority to the best priority
- * among the priorities of all its waiting threads.
- */
-static void UpdateMutexPriority(Mutex* mutex) {
-    s32 best_priority = THREADPRIO_LOWEST;
-    for (auto& waiter : mutex->GetWaitingThreads()) {
-        if (waiter->current_priority < best_priority)
-            best_priority = waiter->current_priority;
-    }
-
-    if (best_priority != mutex->priority) {
-        mutex->priority = best_priority;
-        UpdateThreadPriority(mutex->holding_thread.get());
-    }
-}
-
 void ReleaseThreadMutexes(Thread* thread) {
     for (auto& mtx : thread->held_mutexes) {
         mtx->lock_count = 0;
@@ -83,9 +51,7 @@ void Mutex::Acquire(Thread* thread) {
         priority = thread->current_priority;
         thread->held_mutexes.insert(this);
         holding_thread = thread;
-
-        UpdateThreadPriority(thread);
-
+        thread->UpdatePriority();
         Core::System::GetInstance().PrepareReschedule();
     }
 
@@ -100,7 +66,7 @@ void Mutex::Release() {
         // Yield to the next thread only if we've fully released the mutex
         if (lock_count == 0) {
             holding_thread->held_mutexes.erase(this);
-            UpdateThreadPriority(holding_thread.get());
+            holding_thread->UpdatePriority();
             holding_thread = nullptr;
             WakeupAllWaitingThreads();
             Core::System::GetInstance().PrepareReschedule();
@@ -110,12 +76,30 @@ void Mutex::Release() {
 
 void Mutex::AddWaitingThread(SharedPtr<Thread> thread) {
     WaitObject::AddWaitingThread(thread);
-    UpdateMutexPriority(this);
+    thread->pending_mutexes.insert(this);
+    UpdatePriority();
 }
 
 void Mutex::RemoveWaitingThread(Thread* thread) {
     WaitObject::RemoveWaitingThread(thread);
-    UpdateMutexPriority(this);
+    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
diff --git a/src/core/hle/kernel/mutex.h b/src/core/hle/kernel/mutex.h
index 31f920516..c57adf400 100644
--- a/src/core/hle/kernel/mutex.h
+++ b/src/core/hle/kernel/mutex.h
@@ -39,6 +39,12 @@ public:
     std::string name;                 ///< Name of mutex (optional)
     SharedPtr<Thread> holding_thread; ///< Thread that has acquired the mutex
 
+    /**
+     * Elevate the mutex priority to the best priority
+     * among the priorities of all its waiting threads.
+     */
+    void UpdatePriority();
+
     bool ShouldWait(Thread* thread) const override;
     void Acquire(Thread* thread) override;
 
diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index d44010824..3a5a67450 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -105,15 +105,15 @@ void Thread::Stop() {
 
     WakeupAllWaitingThreads();
 
-    // Release all the mutexes that this thread holds
-    ReleaseThreadMutexes(this);
-
     // Clean up any dangling references in objects that this thread was waiting for
     for (auto& wait_object : wait_objects) {
         wait_object->RemoveWaitingThread(this);
     }
     wait_objects.clear();
 
+    // Release all the mutexes that this thread holds
+    ReleaseThreadMutexes(this);
+
     // Mark the TLS slot in the thread's page as free.
     u32 tls_page = (tls_address - Memory::TLS_AREA_VADDR) / Memory::PAGE_SIZE;
     u32 tls_slot =
@@ -515,8 +515,21 @@ void Thread::SetPriority(s32 priority) {
     nominal_priority = current_priority = priority;
 }
 
+void Thread::UpdatePriority() {
+    s32 best_priority = nominal_priority;
+    for (auto& mutex : held_mutexes) {
+        if (mutex->priority < best_priority)
+            best_priority = mutex->priority;
+    }
+    BoostPriority(best_priority);
+}
+
 void Thread::BoostPriority(s32 priority) {
-    ready_queue.move(this, current_priority, priority);
+    // If thread was ready, adjust queues
+    if (status == THREADSTATUS_READY)
+        ready_queue.move(this, current_priority, priority);
+    else
+        ready_queue.prepare(priority);
     current_priority = priority;
 }
 
diff --git a/src/core/hle/kernel/thread.h b/src/core/hle/kernel/thread.h
index f2bc1ec9c..e2f0cc831 100644
--- a/src/core/hle/kernel/thread.h
+++ b/src/core/hle/kernel/thread.h
@@ -90,6 +90,12 @@ public:
     void SetPriority(s32 priority);
 
     /**
+     * Boost's a thread's priority to the best priority among the thread's held mutexes.
+     * This prevents priority inversion via priority inheritance.
+     */
+    void UpdatePriority();
+
+    /**
      * Temporarily boosts the thread's priority until the next time it is scheduled
      * @param priority The new priority
      */
@@ -178,6 +184,9 @@ public:
     /// Mutexes currently held by this thread, which will be released when it exits.
     boost::container::flat_set<SharedPtr<Mutex>> held_mutexes;
 
+    /// Mutexes that this thread is currently waiting for.
+    boost::container::flat_set<SharedPtr<Mutex>> pending_mutexes;
+
     SharedPtr<Process> owner_process; ///< Process that owns this thread
 
     /// Objects that the thread is waiting on.
diff --git a/src/core/hle/svc.cpp b/src/core/hle/svc.cpp
index b6e34a9e9..160f27c98 100644
--- a/src/core/hle/svc.cpp
+++ b/src/core/hle/svc.cpp
@@ -611,6 +611,12 @@ static ResultCode SetThreadPriority(Kernel::Handle handle, s32 priority) {
         return ERR_INVALID_HANDLE;
 
     thread->SetPriority(priority);
+    thread->UpdatePriority();
+
+    // Update the mutexes that this thread is waiting for
+    for (auto& mutex : thread->pending_mutexes)
+        mutex->UpdatePriority();
+
     Core::System::GetInstance().PrepareReschedule();
     return RESULT_SUCCESS;
 }