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.

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)

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

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:

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);

src/core/hle/svc.cpp

@@ -278,9 +278,6 @@ static ResultCode WaitSynchronization1(Kernel::Handle handle, s64 nano_seconds)
             return ERR_SYNC_TIMEOUT;
 
         object->AddWaitingThread(thread);
-        // TODO(Subv): Perform things like update the mutex lock owner's priority to
-        // prevent priority inversion. Currently this is done in Mutex::ShouldWait,
-        // but it should be moved to a function that is called from here.
         thread->status = THREADSTATUS_WAIT_SYNCH;
 
         // Create an event to wake the thread up after the specified nanosecond delay has passed
@@ -359,9 +356,6 @@ static ResultCode WaitSynchronizationN(s32* out, Kernel::Handle* handles, s32 ha
         // Add the thread to each of the objects' waiting threads.
         for (auto& object : objects) {
             object->AddWaitingThread(thread);
-            // TODO(Subv): Perform things like update the mutex lock owner's priority to
-            // prevent priority inversion. Currently this is done in Mutex::ShouldWait,
-            // but it should be moved to a function that is called from here.
         }
 
         // Set the thread's waitlist to the list of objects passed to WaitSynchronizationN
@@ -409,9 +403,6 @@ static ResultCode WaitSynchronizationN(s32* out, Kernel::Handle* handles, s32 ha
             // Set the index of this object in the mapping of Objects -> index for this thread.
             thread->wait_objects_index[object->GetObjectId()] = static_cast<int>(i);
             object->AddWaitingThread(thread);
-            // TODO(Subv): Perform things like update the mutex lock owner's priority to
-            // prevent priority inversion. Currently this is done in Mutex::ShouldWait,
-            // but it should be moved to a function that is called from here.
         }
 
         // Note: If no handles and no timeout were given, then the thread will deadlock, this is