Kernel/Mutex: Implemented priority inheritance.

The implementation is based on reverse engineering of the 3DS's kernel.

A mutex holder's priority will be temporarily boosted to the best priority among any threads that want to acquire any of its held mutexes.
When the holder releases the mutex, it's priority will be boosted to the best priority among the threads that want to acquire any of its remaining held mutexes.

4 files changed, 51 insertions, 22 deletions

diff --git a/src/core/hle/kernel/kernel.h b/src/core/hle/kernel/kernel.h
index 67eae93f2..2680f89c9 100644
--- a/src/core/hle/kernel/kernel.h
+++ b/src/core/hle/kernel/kernel.h
@@ -145,7 +145,7 @@ public:
      * Add a thread to wait on this object
      * @param thread Pointer to thread to add
      */
-    void AddWaitingThread(SharedPtr<Thread> thread);
+    virtual void AddWaitingThread(SharedPtr<Thread> thread);
 
     /**
      * Removes a thread from waiting on this object (e.g. if it was resumed already)
diff --git a/src/core/hle/kernel/mutex.cpp b/src/core/hle/kernel/mutex.cpp
index 072e4e7c1..e83717e80 100644
--- a/src/core/hle/kernel/mutex.cpp
+++ b/src/core/hle/kernel/mutex.cpp
@@ -6,6 +6,7 @@
 #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"
@@ -13,19 +14,25 @@
 namespace Kernel {
 
 /**
- * Resumes a thread waiting for the specified mutex
- * @param mutex The mutex that some thread is waiting on
+ * 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 ResumeWaitingThread(Mutex* mutex) {
-    // Reset mutex lock thread handle, nothing is waiting
-    mutex->lock_count = 0;
-    mutex->holding_thread = nullptr;
-    mutex->WakeupAllWaitingThreads();
+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);
 }
 
 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();
 }
@@ -54,27 +61,52 @@ bool Mutex::ShouldWait(Thread* thread) const {
 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;
+
+        UpdateThreadPriority(thread);
+
+        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);
+            UpdateThreadPriority(holding_thread.get());
+            holding_thread = nullptr;
+            WakeupAllWaitingThreads();
             Core::System::GetInstance().PrepareReschedule();
         }
     }
 }
 
+void Mutex::AddWaitingThread(SharedPtr<Thread> thread) {
+    WaitObject::AddWaitingThread(thread);
+
+    // Elevate the mutex priority to the best priority
+    // among the priorities of all its waiting threads.
+
+    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;
+        UpdateThreadPriority(holding_thread.get());
+    }
+}
+
 } // namespace
diff --git a/src/core/hle/kernel/mutex.h b/src/core/hle/kernel/mutex.h
index 98b3d40b5..3e6adeb17 100644
--- a/src/core/hle/kernel/mutex.h
+++ b/src/core/hle/kernel/mutex.h
@@ -35,18 +35,15 @@ public:
     }
 
     int lock_count;                   ///< Number of times the mutex has been acquired
+    u32 priority;                     ///< The priority of the mutex, used for priority inheritance.
     std::string name;                 ///< Name of mutex (optional)
     SharedPtr<Thread> holding_thread; ///< Thread that has acquired the mutex
 
     bool ShouldWait(Thread* thread) const override;
     void Acquire(Thread* thread) override;
 
+    void AddWaitingThread(SharedPtr<Thread> thread) override;
 
-    /**
-     * Acquires the specified mutex for the specified thread
-     * @param thread Thread that will acquire the mutex
-     */
-    void Acquire(SharedPtr<Thread> thread);
     void Release();
 
 private:
diff --git a/src/core/hle/kernel/thread.cpp b/src/core/hle/kernel/thread.cpp
index 7d03a2cf7..d44010824 100644
--- a/src/core/hle/kernel/thread.cpp
+++ b/src/core/hle/kernel/thread.cpp
@@ -90,9 +90,6 @@ static bool CheckWait_AddressArbiter(const Thread* thread, VAddr wait_address) {
 }
 
 void Thread::Stop() {
-    // Release all the mutexes that this thread holds
-    ReleaseThreadMutexes(this);
-
     // Cancel any outstanding wakeup events for this thread
     CoreTiming::UnscheduleEvent(ThreadWakeupEventType, callback_handle);
     wakeup_callback_handle_table.Close(callback_handle);
@@ -108,6 +105,9 @@ 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);