Kernel: Implemented mutex priority inheritance.

Verified with a hwtest and implemented based on reverse engineering.

Thread A's priority will get bumped to the highest priority among all the threads that are waiting for a mutex that A holds.
Once A releases the mutex and ownership is transferred to B, A's priority will return to normal and B's priority will be bumped.

1 files changed, 39 insertions, 2 deletions

diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index 16d9b9e36..36222d45f 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -129,6 +129,11 @@ static void ThreadWakeupCallback(u64 thread_handle, int cycles_late) {
         thread->mutex_wait_address = 0;
         thread->condvar_wait_address = 0;
         thread->wait_handle = 0;
+
+        auto lock_owner = thread->lock_owner;
+        // Threads waking up by timeout from WaitProcessWideKey do not perform priority inheritance
+        // and don't have a lock owner.
+        ASSERT(lock_owner == nullptr);
     }
 
     if (resume)
@@ -325,8 +330,8 @@ ResultVal<SharedPtr<Thread>> Thread::Create(std::string name, VAddr entry_point,
 void Thread::SetPriority(u32 priority) {
     ASSERT_MSG(priority <= THREADPRIO_LOWEST && priority >= THREADPRIO_HIGHEST,
                "Invalid priority value.");
-    Core::System::GetInstance().Scheduler().SetThreadPriority(this, priority);
-    nominal_priority = current_priority = priority;
+    nominal_priority = priority;
+    UpdatePriority();
 }
 
 void Thread::BoostPriority(u32 priority) {
@@ -376,6 +381,38 @@ VAddr Thread::GetCommandBufferAddress() const {
     return GetTLSAddress() + CommandHeaderOffset;
 }
 
+void Thread::AddMutexWaiter(SharedPtr<Thread> thread) {
+    thread->lock_owner = this;
+    wait_mutex_threads.emplace_back(std::move(thread));
+    UpdatePriority();
+}
+
+void Thread::RemoveMutexWaiter(SharedPtr<Thread> thread) {
+    boost::remove_erase(wait_mutex_threads, thread);
+    thread->lock_owner = nullptr;
+    UpdatePriority();
+}
+
+void Thread::UpdatePriority() {
+    // Find the highest priority among all the threads that are waiting for this thread's lock
+    u32 new_priority = nominal_priority;
+    for (const auto& thread : wait_mutex_threads) {
+        if (thread->nominal_priority < new_priority)
+            new_priority = thread->nominal_priority;
+    }
+
+    if (new_priority == current_priority)
+        return;
+
+    Core::System::GetInstance().Scheduler().SetThreadPriority(this, new_priority);
+
+    current_priority = new_priority;
+
+    // Recursively update the priority of the thread that depends on the priority of this one.
+    if (lock_owner)
+        lock_owner->UpdatePriority();
+}
+
 ////////////////////////////////////////////////////////////////////////////////////////////////////
 
 /**