From ee0b14ba0811a76a306c1546be90de128a17f5cc Mon Sep 17 00:00:00 2001
From: gdkchan <gab.dark.100@gmail.com>
Date: Mon, 14 May 2018 03:01:10 -0300
Subject: [PATCH] Some improvements to SetThreadCoreMask, simplified
 implementation of wait lists

---
 .../OsHle/Handles/KProcessScheduler.cs        |  78 +++---
 Ryujinx.Core/OsHle/Handles/KThread.cs         | 150 +----------
 Ryujinx.Core/OsHle/Kernel/SvcThread.cs        |  48 ++--
 Ryujinx.Core/OsHle/Kernel/SvcThreadSync.cs    | 240 ++++++++----------
 Ryujinx.Core/OsHle/Process.cs                 |   8 +-
 5 files changed, 199 insertions(+), 325 deletions(-)

diff --git a/Ryujinx.Core/OsHle/Handles/KProcessScheduler.cs b/Ryujinx.Core/OsHle/Handles/KProcessScheduler.cs
index 33f7fb1449..487a8147f6 100644
--- a/Ryujinx.Core/OsHle/Handles/KProcessScheduler.cs
+++ b/Ryujinx.Core/OsHle/Handles/KProcessScheduler.cs
@@ -66,17 +66,21 @@ namespace Ryujinx.Core.OsHle.Handles
                     SchedThread.Dispose();
                 }
 
-                SchedulerThread NewThread = WaitingToRun.Pop(Thread.ActualCore);
+                int ActualCore = Thread.ActualCore;
+
+                SchedulerThread NewThread = WaitingToRun.Pop(ActualCore);
 
                 if (NewThread == null)
                 {
-                    Log.PrintDebug(LogClass.KernelScheduler, $"Nothing to run on core {Thread.ActualCore}!");
+                    Log.PrintDebug(LogClass.KernelScheduler, $"Nothing to run on core {ActualCore}!");
 
-                    RemoveActiveCore(Thread.ActualCore);
+                    RemoveActiveCore(ActualCore);
 
                     return;
                 }
 
+                NewThread.Thread.ActualCore = ActualCore;
+
                 RunThread(NewThread);
             }
         }
@@ -146,17 +150,21 @@ namespace Ryujinx.Core.OsHle.Handles
             {
                 PrintDbgThreadInfo(Thread, "suspended.");
 
-                SchedulerThread SchedThread = WaitingToRun.Pop(Thread.ActualCore);
+                int ActualCore = Thread.ActualCore;
+
+                SchedulerThread SchedThread = WaitingToRun.Pop(ActualCore);
 
                 if (SchedThread != null)
                 {
+                    SchedThread.Thread.ActualCore = ActualCore;
+
                     RunThread(SchedThread);
                 }
                 else
                 {
                     Log.PrintDebug(LogClass.KernelScheduler, $"Nothing to run on core {Thread.ActualCore}!");
 
-                    RemoveActiveCore(Thread.ActualCore);
+                    RemoveActiveCore(ActualCore);
                 }
             }
         }
@@ -169,9 +177,9 @@ namespace Ryujinx.Core.OsHle.Handles
             {
                 lock (SchedLock)
                 {
-                    SchedulerThread SchedThread = WaitingToRun.Pop(
-                        Thread.ActualCore,
-                        Thread.ActualPriority);
+                    int ActualCore = Thread.ActualCore;
+
+                    SchedulerThread SchedThread = WaitingToRun.Pop(ActualCore, Thread.ActualPriority);
 
                     if (SchedThread == null)
                     {
@@ -182,6 +190,8 @@ namespace Ryujinx.Core.OsHle.Handles
 
                     if (SchedThread != null)
                     {
+                        SchedThread.Thread.ActualCore = ActualCore;
+
                         RunThread(SchedThread);
                     }
                 }
@@ -198,24 +208,24 @@ namespace Ryujinx.Core.OsHle.Handles
 
         public bool TryRunning(KThread Thread)
         {
-            if (!AllThreads.TryGetValue(Thread, out SchedulerThread SchedThread))
+            //Failing to get the thread here is fine,
+            //the thread may not have been started yet.
+            if (AllThreads.TryGetValue(Thread, out SchedulerThread SchedThread))
             {
-                throw new InvalidOperationException();
-            }
-
-            lock (SchedLock)
-            {
-                if (WaitingToRun.HasThread(SchedThread) && AddActiveCore(Thread))
+                lock (SchedLock)
                 {
-                    WaitingToRun.Remove(SchedThread);
+                    if (WaitingToRun.HasThread(SchedThread) && AddActiveCore(Thread))
+                    {
+                        WaitingToRun.Remove(SchedThread);
 
-                    RunThread(SchedThread);
+                        RunThread(SchedThread);
 
-                    return true;
+                        return true;
+                    }
                 }
-
-                return false;
             }
+
+            return false;
         }
 
         public void Resume(KThread Thread)
@@ -289,18 +299,25 @@ namespace Ryujinx.Core.OsHle.Handles
 
         private bool AddActiveCore(KThread Thread)
         {
+            int CoreMask;
+
             lock (SchedLock)
             {
                 //First, try running it on Ideal Core.
-                int CoreMask = 1 << Thread.IdealCore;
+                int IdealCore = Thread.IdealCore;
 
-                if ((ActiveCores & CoreMask) == 0)
+                if (IdealCore != -1)
                 {
-                    ActiveCores |= CoreMask;
+                    CoreMask = 1 << IdealCore;
 
-                    Thread.ActualCore = Thread.IdealCore;
+                    if ((ActiveCores & CoreMask) == 0)
+                    {
+                        ActiveCores |= CoreMask;
 
-                    return true;
+                        Thread.ActualCore = IdealCore;
+
+                        return true;
+                    }
                 }
 
                 //If that fails, then try running on any core allowed by Core Mask.
@@ -340,11 +357,12 @@ namespace Ryujinx.Core.OsHle.Handles
         private void PrintDbgThreadInfo(KThread Thread, string Message)
         {
             Log.PrintDebug(LogClass.KernelScheduler, "(" +
-                "ThreadId = "       + Thread.ThreadId       + ", " +
-                "ActualCore = "     + Thread.ActualCore     + ", " +
-                "IdealCore = "      + Thread.IdealCore      + ", " +
-                "ActualPriority = " + Thread.ActualPriority + ", " +
-                "WantedPriority = " + Thread.WantedPriority + ") " + Message);
+                "ThreadId = "       + Thread.ThreadId                + ", " +
+                "CoreMask = 0x"     + Thread.CoreMask.ToString("x1") + ", " +
+                "ActualCore = "     + Thread.ActualCore              + ", " +
+                "IdealCore = "      + Thread.IdealCore               + ", " +
+                "ActualPriority = " + Thread.ActualPriority          + ", " +
+                "WantedPriority = " + Thread.WantedPriority          + ") " + Message);
         }
 
         public void Dispose()
diff --git a/Ryujinx.Core/OsHle/Handles/KThread.cs b/Ryujinx.Core/OsHle/Handles/KThread.cs
index a430974c1a..ce9c3db23a 100644
--- a/Ryujinx.Core/OsHle/Handles/KThread.cs
+++ b/Ryujinx.Core/OsHle/Handles/KThread.cs
@@ -1,5 +1,5 @@
 using ChocolArm64;
-using System;
+using System.Collections.Generic;
 
 namespace Ryujinx.Core.OsHle.Handles
 {
@@ -14,16 +14,15 @@ namespace Ryujinx.Core.OsHle.Handles
 
         private Process Process;
 
-        public KThread NextMutexThread   { get; set; }
-        public KThread NextCondVarThread { get; set; }
+        public List<KThread> MutexWaiters { get; private set; }
 
         public KThread MutexOwner { get; set; }
 
         public int ActualPriority { get; private set; }
         public int WantedPriority { get; private set; }
 
-        public int IdealCore  { get; set; }
         public int ActualCore { get; set; }
+        public int IdealCore  { get; set; }
 
         public int WaitHandle { get; set; }
 
@@ -39,6 +38,8 @@ namespace Ryujinx.Core.OsHle.Handles
             this.Process   = Process;
             this.IdealCore = IdealCore;
 
+            MutexWaiters = new List<KThread>();
+
             CoreMask = 1 << IdealCore;
 
             ActualPriority = WantedPriority = Priority;
@@ -57,148 +58,25 @@ namespace Ryujinx.Core.OsHle.Handles
 
             int CurrPriority = WantedPriority;
 
-            if (NextMutexThread != null && CurrPriority > NextMutexThread.WantedPriority)
+            lock (Process.ThreadSyncLock)
             {
-                CurrPriority = NextMutexThread.WantedPriority;
+                foreach (KThread Thread in MutexWaiters)
+                {
+                    if (CurrPriority > Thread.WantedPriority)
+                    {
+                        CurrPriority = Thread.WantedPriority;
+                    }
+                }
             }
 
             if (CurrPriority != OldPriority)
             {
                 ActualPriority = CurrPriority;
 
-                UpdateWaitLists();
+                Process.Scheduler.Resort(this);
 
                 MutexOwner?.UpdatePriority();
             }
         }
-
-        private void UpdateWaitLists()
-        {
-            UpdateMutexList();
-            UpdateCondVarList();
-
-            Process.Scheduler.Resort(this);
-        }
-
-        private void UpdateMutexList()
-        {
-            KThread OwnerThread = MutexOwner;
-
-            if (OwnerThread == null)
-            {
-                return;
-            }
-
-            //The MutexOwner field should only be non-null when the thread is
-            //waiting for the lock, and the lock belongs to another thread.
-            if (OwnerThread == this)
-            {
-                throw new InvalidOperationException();
-            }
-
-            lock (OwnerThread)
-            {
-                //Remove itself from the list.
-                KThread CurrThread = OwnerThread;
-
-                while (CurrThread.NextMutexThread != null)
-                {
-                    if (CurrThread.NextMutexThread == this)
-                    {
-                        CurrThread.NextMutexThread = NextMutexThread;
-
-                        break;
-                    }
-
-                    CurrThread = CurrThread.NextMutexThread;
-                }
-
-                //Re-add taking new priority into account.
-                CurrThread = OwnerThread;
-
-                while (CurrThread.NextMutexThread != null)
-                {
-                    if (CurrThread.NextMutexThread.ActualPriority > ActualPriority)
-                    {
-                        break;
-                    }
-
-                    CurrThread = CurrThread.NextMutexThread;
-                }
-
-                NextMutexThread = CurrThread.NextMutexThread;
-
-                CurrThread.NextMutexThread = this;
-            }
-        }
-
-        private void UpdateCondVarList()
-        {
-            lock (Process.ThreadArbiterListLock)
-            {
-                if (Process.ThreadArbiterListHead == null)
-                {
-                    return;
-                }
-
-                //Remove itself from the list.
-                bool Found;
-
-                KThread CurrThread = Process.ThreadArbiterListHead;
-
-                if (Found = (Process.ThreadArbiterListHead == this))
-                {
-                    Process.ThreadArbiterListHead = Process.ThreadArbiterListHead.NextCondVarThread;
-                }
-                else
-                {
-                    while (CurrThread.NextCondVarThread != null)
-                    {
-                        if (CurrThread.NextCondVarThread == this)
-                        {
-                            CurrThread.NextCondVarThread = NextCondVarThread;
-
-                            Found = true;
-
-                            break;
-                        }
-
-                        CurrThread = CurrThread.NextCondVarThread;
-                    }
-                }
-
-                if (!Found)
-                {
-                    return;
-                }
-
-                //Re-add taking new priority into account.
-                if (Process.ThreadArbiterListHead == null ||
-                    Process.ThreadArbiterListHead.ActualPriority > ActualPriority)
-                {
-                    NextCondVarThread = Process.ThreadArbiterListHead;
-
-                    Process.ThreadArbiterListHead = this;
-
-                    return;
-                }
-
-                CurrThread = Process.ThreadArbiterListHead;
-
-                while (CurrThread.NextCondVarThread != null)
-                {
-                    if (CurrThread.NextCondVarThread.ActualPriority > ActualPriority)
-                    {
-                        break;
-                    }
-
-                    CurrThread = CurrThread.NextCondVarThread;
-                }
-
-                NextCondVarThread = CurrThread.NextCondVarThread;
-
-                CurrThread.NextCondVarThread = this;
-            }
-        }
     }
 }
\ No newline at end of file
diff --git a/Ryujinx.Core/OsHle/Kernel/SvcThread.cs b/Ryujinx.Core/OsHle/Kernel/SvcThread.cs
index 94912f536f..597e42a6d7 100644
--- a/Ryujinx.Core/OsHle/Kernel/SvcThread.cs
+++ b/Ryujinx.Core/OsHle/Kernel/SvcThread.cs
@@ -146,11 +146,16 @@ namespace Ryujinx.Core.OsHle.Kernel
             int  IdealCore =  (int)ThreadState.X1;
             long CoreMask  = (long)ThreadState.X2;
 
+            Ns.Log.PrintDebug(LogClass.KernelSvc,
+                "Handle = "    + Handle   .ToString("x8") + ", " +
+                "IdealCore = " + IdealCore.ToString("x8") + ", " +
+                "CoreMask = "  + CoreMask .ToString("x16"));
+
             KThread Thread = GetThread(ThreadState.Tpidr, Handle);
 
             if (IdealCore == -2)
             {
-                //TODO: Get this value from the NPDM file.
+                //TODO: Get this valcdue from the NPDM file.
                 IdealCore = 0;
 
                 CoreMask = 1 << IdealCore;
@@ -159,14 +164,16 @@ namespace Ryujinx.Core.OsHle.Kernel
             {
                 if ((uint)IdealCore > 3)
                 {
-                    Ns.Log.PrintWarning(LogClass.KernelSvc, $"Invalid core id 0x{IdealCore:x8}!");
+                    if ((IdealCore | 2) != -1)
+                    {
+                        Ns.Log.PrintWarning(LogClass.KernelSvc, $"Invalid core id 0x{IdealCore:x8}!");
 
-                    ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidCoreId);
+                        ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidCoreId);
 
-                    return;
+                        return;
+                    }
                 }
-
-                if ((CoreMask & (1 << IdealCore)) == 0)
+                else if ((CoreMask & (1 << IdealCore)) == 0)
                 {
                     Ns.Log.PrintWarning(LogClass.KernelSvc, $"Invalid core mask 0x{CoreMask:x8}!");
 
@@ -185,27 +192,32 @@ namespace Ryujinx.Core.OsHle.Kernel
                 return;
             }
 
-            if (IdealCore == -3)
-            {
-                if ((CoreMask & (1 << Thread.IdealCore)) == 0)
-                {
-                    Ns.Log.PrintWarning(LogClass.KernelSvc, $"Invalid core mask 0x{CoreMask:x8}!");
-
-                    ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidCoreMask);
-
-                    return;
-                }
-            }
-            else
+            //-1 is used as "don't care", so the IdealCore value is ignored.
+            //-2 is used as "use NPDM default core id" (handled above).
+            //-3 is used as "don't update", the old IdealCore value is kept.
+            if (IdealCore != -3)
             {
                 Thread.IdealCore = IdealCore;
             }
+            else if ((CoreMask & (1 << Thread.IdealCore)) == 0)
+            {
+                Ns.Log.PrintWarning(LogClass.KernelSvc, $"Invalid core mask 0x{CoreMask:x8}!");
+
+                ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidCoreMask);
+
+                return;
+            }
 
             Thread.CoreMask = (int)CoreMask;
 
             KThread CurrThread = Process.GetThread(ThreadState.Tpidr);
 
+            //Try yielding execution, for the case where the new
+            //core mask allows the thread to run on the current core.
             Process.Scheduler.Yield(CurrThread);
+
+            //Try running the modified thread, for the case where one
+            //of the cores specified on the core mask is free.
             Process.Scheduler.TryRunning(Thread);
 
             ThreadState.X0 = 0;
diff --git a/Ryujinx.Core/OsHle/Kernel/SvcThreadSync.cs b/Ryujinx.Core/OsHle/Kernel/SvcThreadSync.cs
index 0ca2a5f928..280065ec4c 100644
--- a/Ryujinx.Core/OsHle/Kernel/SvcThreadSync.cs
+++ b/Ryujinx.Core/OsHle/Kernel/SvcThreadSync.cs
@@ -2,6 +2,8 @@ using ChocolArm64.State;
 using Ryujinx.Core.Logging;
 using Ryujinx.Core.OsHle.Handles;
 using System;
+using System.Collections.Generic;
+using System.Linq;
 using System.Threading;
 
 using static Ryujinx.Core.OsHle.ErrorCode;
@@ -110,10 +112,10 @@ namespace Ryujinx.Core.OsHle.Kernel
             ulong Timeout        =       ThreadState.X3;
 
             Ns.Log.PrintDebug(LogClass.KernelSvc,
-                "OwnerThreadHandle = " + MutexAddress  .ToString("x16") + ", " +
-                "MutexAddress = "      + CondVarAddress.ToString("x16") + ", " +
-                "WaitThreadHandle = "  + ThreadHandle  .ToString("x8")  + ", " +
-                "Timeout = "           + Timeout       .ToString("x16"));
+                "MutexAddress = "   + MutexAddress  .ToString("x16") + ", " +
+                "CondVarAddress = " + CondVarAddress.ToString("x16") + ", " +
+                "ThreadHandle = "   + ThreadHandle  .ToString("x8")  + ", " +
+                "Timeout = "        + Timeout       .ToString("x16"));
 
             if (IsPointingInsideKernel(MutexAddress))
             {
@@ -181,16 +183,19 @@ namespace Ryujinx.Core.OsHle.Kernel
 
             Ns.Log.PrintDebug(LogClass.KernelSvc, "MutexValue = " + MutexValue.ToString("x8"));
 
-            if (MutexValue != (OwnerThreadHandle | MutexHasListenersMask))
+            lock (Process.ThreadSyncLock)
             {
-                return;
+                if (MutexValue != (OwnerThreadHandle | MutexHasListenersMask))
+                {
+                    return;
+                }
+
+                CurrThread.WaitHandle   = WaitThreadHandle;
+                CurrThread.MutexAddress = MutexAddress;
+
+                InsertWaitingMutexThread(OwnerThreadHandle, WaitThread);
             }
 
-            CurrThread.WaitHandle   = WaitThreadHandle;
-            CurrThread.MutexAddress = MutexAddress;
-
-            InsertWaitingMutexThread(OwnerThreadHandle, WaitThread);
-
             Ns.Log.PrintDebug(LogClass.KernelSvc, "Entering wait state...");
 
             Process.Scheduler.EnterWait(CurrThread);
@@ -205,26 +210,22 @@ namespace Ryujinx.Core.OsHle.Kernel
                 return false;
             }
 
-            lock (CurrThread)
+            lock (Process.ThreadSyncLock)
             {
                 //This is the new thread that will not own the mutex.
                 //If no threads are waiting for the lock, then it should be null.
-                KThread OwnerThread = CurrThread.NextMutexThread;
-
-                while (OwnerThread != null && OwnerThread.MutexAddress != MutexAddress)
-                {
-                    OwnerThread = OwnerThread.NextMutexThread;
-                }
-
-                UpdateMutexOwner(CurrThread, OwnerThread, MutexAddress);
-
-                CurrThread.NextMutexThread = null;
-
-                CurrThread.UpdatePriority();
+                KThread OwnerThread = GetHighestPriority(CurrThread.MutexWaiters, MutexAddress);
 
                 if (OwnerThread != null)
                 {
-                    int HasListeners = OwnerThread.NextMutexThread != null ? MutexHasListenersMask : 0;
+                    //Remove all waiting mutex from the old owner,
+                    //and insert then on the new owner.
+                    UpdateMutexOwner(CurrThread, OwnerThread, MutexAddress);
+
+                    CurrThread.UpdatePriority();
+                    OwnerThread.UpdatePriority();
+
+                    int HasListeners = OwnerThread.MutexWaiters.Count > 0 ? MutexHasListenersMask : 0;
 
                     Process.Memory.WriteInt32(MutexAddress, HasListeners | OwnerThread.WaitHandle);
 
@@ -238,12 +239,16 @@ namespace Ryujinx.Core.OsHle.Kernel
 
                     Process.Scheduler.WakeUp(OwnerThread);
 
+                    Ns.Log.PrintDebug(LogClass.KernelSvc, "Gave mutex to thread id " + OwnerThread.ThreadId + "!");
+
                     return true;
                 }
                 else
                 {
                     Process.Memory.WriteInt32(MutexAddress, 0);
 
+                    Ns.Log.PrintDebug(LogClass.KernelSvc, "No threads waiting mutex!");
+
                     return false;
                 }
             }
@@ -262,43 +267,7 @@ namespace Ryujinx.Core.OsHle.Kernel
 
             lock (Process.ThreadArbiterListLock)
             {
-                KThread CurrThread = Process.ThreadArbiterListHead;
-
-                if (CurrThread == null || CurrThread.ActualPriority > WaitThread.ActualPriority)
-                {
-                    WaitThread.NextCondVarThread = Process.ThreadArbiterListHead;
-
-                    Process.ThreadArbiterListHead = WaitThread;
-                }
-                else
-                {
-                    bool DoInsert = CurrThread != WaitThread;
-
-                    while (CurrThread.NextCondVarThread != null)
-                    {
-                        if (CurrThread.NextCondVarThread.ActualPriority > WaitThread.ActualPriority)
-                        {
-                            break;
-                        }
-
-                        CurrThread = CurrThread.NextCondVarThread;
-
-                        DoInsert &= CurrThread != WaitThread;
-                    }
-
-                    //Only insert if the node doesn't already exist in the list.
-                    //This prevents circular references.
-                    if (DoInsert)
-                    {
-                        if (WaitThread.NextCondVarThread != null)
-                        {
-                            throw new InvalidOperationException();
-                        }
-
-                        WaitThread.NextCondVarThread = CurrThread.NextCondVarThread;
-                        CurrThread.NextCondVarThread = WaitThread;
-                    }
-                }
+                Process.ThreadArbiterList.Add(WaitThread);
             }
 
             Ns.Log.PrintDebug(LogClass.KernelSvc, "Entering wait state...");
@@ -319,62 +288,44 @@ namespace Ryujinx.Core.OsHle.Kernel
         {
             lock (Process.ThreadArbiterListLock)
             {
-                KThread PrevThread = null;
-                KThread CurrThread = Process.ThreadArbiterListHead;
+                KThread CurrThread = GetHighestPriority(CondVarAddress);
 
-                while (CurrThread != null && (Count == -1 || Count > 0))
+                while (CurrThread != null && (Count == -1 || Count-- > 0))
                 {
-                    if (CurrThread.CondVarAddress == CondVarAddress)
+                    AcquireMutexValue(CurrThread.MutexAddress);
+
+                    int MutexValue = Process.Memory.ReadInt32(CurrThread.MutexAddress);
+
+                    if (MutexValue == 0)
                     {
-                        if (PrevThread != null)
-                        {
-                            PrevThread.NextCondVarThread = CurrThread.NextCondVarThread;
-                        }
-                        else
-                        {
-                            Process.ThreadArbiterListHead = CurrThread.NextCondVarThread;
-                        }
+                        //Give the lock to this thread.
+                        Process.Memory.WriteInt32(CurrThread.MutexAddress, CurrThread.WaitHandle);
 
-                        CurrThread.NextCondVarThread = null;
+                        CurrThread.WaitHandle     = 0;
+                        CurrThread.MutexAddress   = 0;
+                        CurrThread.CondVarAddress = 0;
 
-                        AcquireMutexValue(CurrThread.MutexAddress);
+                        CurrThread.MutexOwner?.UpdatePriority();
 
-                        int MutexValue = Process.Memory.ReadInt32(CurrThread.MutexAddress);
+                        CurrThread.MutexOwner = null;
 
-                        if (MutexValue == 0)
-                        {
-                            //Give the lock to this thread.
-                            Process.Memory.WriteInt32(CurrThread.MutexAddress, CurrThread.WaitHandle);
+                        Process.Scheduler.WakeUp(CurrThread);
+                    }
+                    else
+                    {
+                        //Wait until the lock is released.
+                        MutexValue &= ~MutexHasListenersMask;
 
-                            CurrThread.WaitHandle     = 0;
-                            CurrThread.MutexAddress   = 0;
-                            CurrThread.CondVarAddress = 0;
+                        InsertWaitingMutexThread(MutexValue, CurrThread);
 
-                            CurrThread.MutexOwner?.UpdatePriority();
+                        MutexValue |= MutexHasListenersMask;
 
-                            CurrThread.MutexOwner = null;
-
-                            Process.Scheduler.WakeUp(CurrThread);
-                        }
-                        else
-                        {
-                            //Wait until the lock is released.
-                            MutexValue &= ~MutexHasListenersMask;
-
-                            InsertWaitingMutexThread(MutexValue, CurrThread);
-
-                            MutexValue |= MutexHasListenersMask;
-
-                            Process.Memory.WriteInt32(CurrThread.MutexAddress, MutexValue);
-                        }
-
-                        ReleaseMutexValue(CurrThread.MutexAddress);
-
-                        Count--;
+                        Process.Memory.WriteInt32(CurrThread.MutexAddress, MutexValue);
                     }
 
-                    PrevThread = CurrThread;
-                    CurrThread = CurrThread.NextCondVarThread;
+                    ReleaseMutexValue(CurrThread.MutexAddress);
+
+                    CurrThread = GetHighestPriority(CondVarAddress);
                 }
             }
         }
@@ -390,59 +341,68 @@ namespace Ryujinx.Core.OsHle.Kernel
                 return;
             }
 
-            WaitThread.MutexOwner = OwnerThread;
+            InsertWaitingMutexThread(OwnerThread, WaitThread);
+        }
 
-            lock (OwnerThread)
+        private void InsertWaitingMutexThread(KThread OwnerThread, KThread WaitThread)
+        {
+            lock (Process.ThreadSyncLock)
             {
-                KThread CurrThread = OwnerThread;
+                WaitThread.MutexOwner = OwnerThread;
 
-                while (CurrThread.NextMutexThread != null)
+                if (!OwnerThread.MutexWaiters.Contains(WaitThread))
                 {
-                    if (CurrThread == WaitThread)
-                    {
-                        return;
-                    }
+                    OwnerThread.MutexWaiters.Add(WaitThread);
 
-                    if (CurrThread.NextMutexThread.ActualPriority > WaitThread.ActualPriority)
-                    {
-                        break;
-                    }
-
-                    CurrThread = CurrThread.NextMutexThread;
-                }
-
-                if (CurrThread != WaitThread)
-                {
-                    if (WaitThread.NextMutexThread != null)
-                    {
-                        throw new InvalidOperationException();
-                    }
-
-                    WaitThread.NextMutexThread = CurrThread.NextMutexThread;
-                    CurrThread.NextMutexThread = WaitThread;
+                    OwnerThread.UpdatePriority();
                 }
             }
-
-            OwnerThread.UpdatePriority();
         }
 
         private void UpdateMutexOwner(KThread CurrThread, KThread NewOwner, long MutexAddress)
         {
             //Go through all threads waiting for the mutex,
             //and update the MutexOwner field to point to the new owner.
-            CurrThread = CurrThread.NextMutexThread;
-
-            while (CurrThread != null)
+            lock (Process.ThreadSyncLock)
             {
-                if (CurrThread.MutexAddress == MutexAddress)
+                for (int Index = 0; Index < CurrThread.MutexWaiters.Count; Index++)
                 {
-                    CurrThread.MutexOwner = NewOwner;
-                }
+                    KThread Thread = CurrThread.MutexWaiters[Index];
 
-                CurrThread = CurrThread.NextMutexThread;
+                    if (Thread.MutexAddress == MutexAddress)
+                    {
+                        CurrThread.MutexWaiters.RemoveAt(Index--);
+
+                        Thread.MutexOwner = NewOwner;
+
+                        NewOwner.MutexWaiters.Add(Thread);
+                    }
+                }
             }
         }
 
+        private KThread GetHighestPriority(List<KThread> Threads, long MutexAddress)
+        {
+            return GetHighestPriority(Threads, x => x.MutexAddress == MutexAddress);
+        }
+
+        private KThread GetHighestPriority(long CondVarAddress)
+        {
+            return GetHighestPriority(Process.ThreadArbiterList, x => x.CondVarAddress == CondVarAddress);
+        }
+
+        private KThread GetHighestPriority(List<KThread> Threads, Func<KThread, bool> Predicate)
+        {
+            KThread Thread = Threads.OrderBy(x => x.ActualPriority).FirstOrDefault(Predicate);
+
+            if (Thread != null)
+            {
+                Threads.Remove(Thread);
+            }
+
+            return Thread;
+        }
+
         private void AcquireMutexValue(long MutexAddress)
         {
             while (!Process.Memory.AcquireAddress(MutexAddress))
diff --git a/Ryujinx.Core/OsHle/Process.cs b/Ryujinx.Core/OsHle/Process.cs
index 8520b9ad93..c804bd20a0 100644
--- a/Ryujinx.Core/OsHle/Process.cs
+++ b/Ryujinx.Core/OsHle/Process.cs
@@ -38,10 +38,12 @@ namespace Ryujinx.Core.OsHle
 
         public KProcessScheduler Scheduler { get; private set; }
 
-        public KThread ThreadArbiterListHead { get; set; }
+        public List<KThread> ThreadArbiterList { get; private set; }
 
         public object ThreadArbiterListLock { get; private set; }
 
+        public object ThreadSyncLock { get; private set; }
+
         public KProcessHandleTable HandleTable { get; private set; }
 
         public AppletStateMgr AppletState { get; private set; }
@@ -72,8 +74,12 @@ namespace Ryujinx.Core.OsHle
 
             Memory = new AMemory();
 
+            ThreadArbiterList = new List<KThread>();
+
             ThreadArbiterListLock = new object();
 
+            ThreadSyncLock = new object();
+
             HandleTable = new KProcessHandleTable();
 
             AppletState = new AppletStateMgr();