Fix some thread sync issues (#172)

* Fix some thread sync issues

* Remove some debug stuff

* Ensure that writes to the mutex address clears the exclusive monitor

8 files changed, 231 insertions, 181 deletions

diff --git a/ChocolArm64/AThread.cs b/ChocolArm64/AThread.cs
index 040f590e..4fc79d5e 100644
--- a/ChocolArm64/AThread.cs
+++ b/ChocolArm64/AThread.cs
@@ -46,7 +46,7 @@ namespace ChocolArm64
             {
                 Translator.ExecuteSubroutine(this, EntryPoint);
 
-                Memory.RemoveMonitor(ThreadId);
+                Memory.RemoveMonitor(ThreadState);
 
                 WorkFinished?.Invoke(this, EventArgs.Empty);
             });
diff --git a/ChocolArm64/Instruction/AInstEmitMemoryEx.cs b/ChocolArm64/Instruction/AInstEmitMemoryEx.cs
index a339bb69..ba8eeceb 100644
--- a/ChocolArm64/Instruction/AInstEmitMemoryEx.cs
+++ b/ChocolArm64/Instruction/AInstEmitMemoryEx.cs
@@ -48,6 +48,16 @@ namespace ChocolArm64.Instruction
         {
             AOpCodeMemEx Op = (AOpCodeMemEx)Context.CurrOp;
 
+            if (AccType.HasFlag(AccessType.Ordered))
+            {
+                EmitBarrier(Context);
+            }
+
+            if (AccType.HasFlag(AccessType.Exclusive))
+            {
+                EmitMemoryCall(Context, nameof(AMemory.SetExclusive), Op.Rn);
+            }
+
             Context.EmitLdarg(ATranslatedSub.MemoryArgIdx);
             Context.EmitLdint(Op.Rn);
 
@@ -67,16 +77,6 @@ namespace ChocolArm64.Instruction
 
                 Context.EmitStintzr(Op.Rt2);
             }
-
-            if (AccType.HasFlag(AccessType.Exclusive))
-            {
-                EmitMemoryCall(Context, nameof(AMemory.SetExclusive), Op.Rn);
-            }
-
-            if (AccType.HasFlag(AccessType.Ordered))
-            {
-                EmitBarrier(Context);
-            }
         }
 
         public static void Pfrm(AILEmitterCtx Context)
@@ -150,7 +150,7 @@ namespace ChocolArm64.Instruction
                 Context.EmitLdc_I8(0);
                 Context.EmitStintzr(Op.Rs);
 
-                Clrex(Context);
+                EmitMemoryCall(Context, nameof(AMemory.ClearExclusiveForStore));
             }
 
             Context.MarkLabel(LblEnd);
diff --git a/ChocolArm64/Memory/AMemory.cs b/ChocolArm64/Memory/AMemory.cs
index fe250d4b..c02bf172 100644
--- a/ChocolArm64/Memory/AMemory.cs
+++ b/ChocolArm64/Memory/AMemory.cs
@@ -6,6 +6,7 @@ using System.Runtime.CompilerServices;
 using System.Runtime.InteropServices;
 using System.Runtime.Intrinsics;
 using System.Runtime.Intrinsics.X86;
+using System.Threading;
 
 namespace ChocolArm64.Memory
 {
@@ -15,32 +16,18 @@ namespace ChocolArm64.Memory
 
         public AMemoryMgr Manager { get; private set; }
 
-        private struct ExMonitor
+        private class ArmMonitor
         {
-            public long Position { get; private set; }
-
-            private bool ExState;
-
-            public ExMonitor(long Position, bool ExState)
-            {
-                this.Position = Position;
-                this.ExState  = ExState;
-            }
+            public long Position;
+            public bool ExState;
 
             public bool HasExclusiveAccess(long Position)
             {
                 return this.Position == Position && ExState;
             }
-
-            public void Reset()
-            {
-                ExState = false;
-            }
         }
 
-        private Dictionary<int, ExMonitor> Monitors;
-
-        private HashSet<long> ExAddrs;
+        private Dictionary<int, ArmMonitor> Monitors;
 
         public IntPtr Ram { get; private set; }
 
@@ -50,9 +37,7 @@ namespace ChocolArm64.Memory
         {
             Manager = new AMemoryMgr();
 
-            Monitors = new Dictionary<int, ExMonitor>();
-
-            ExAddrs = new HashSet<long>();
+            Monitors = new Dictionary<int, ArmMonitor>();
 
             if (RuntimeInformation.IsOSPlatform(OSPlatform.Windows))
             {
@@ -66,16 +51,13 @@ namespace ChocolArm64.Memory
             RamPtr = (byte*)Ram;
         }
 
-        public void RemoveMonitor(int ThreadId)
+        public void RemoveMonitor(AThreadState State)
         {
             lock (Monitors)
             {
-                if (Monitors.TryGetValue(ThreadId, out ExMonitor Monitor))
-                {
-                    ExAddrs.Remove(Monitor.Position);
-                }
+                ClearExclusive(State);
 
-                Monitors.Remove(ThreadId);
+                Monitors.Remove(State.ThreadId);
             }
         }
 
@@ -85,66 +67,85 @@ namespace ChocolArm64.Memory
 
             lock (Monitors)
             {
-                if (Monitors.TryGetValue(ThreadState.ThreadId, out ExMonitor Monitor))
+                foreach (ArmMonitor Mon in Monitors.Values)
                 {
-                    ExAddrs.Remove(Monitor.Position);
+                    if (Mon.Position == Position && Mon.ExState)
+                    {
+                        Mon.ExState = false;
+                    }
                 }
 
-                bool ExState = ExAddrs.Add(Position);
-
-                Monitor = new ExMonitor(Position, ExState);
-
-                if (!Monitors.TryAdd(ThreadState.ThreadId, Monitor))
+                if (!Monitors.TryGetValue(ThreadState.ThreadId, out ArmMonitor ThreadMon))
                 {
-                    Monitors[ThreadState.ThreadId] = Monitor;
+                    ThreadMon = new ArmMonitor();
+
+                    Monitors.Add(ThreadState.ThreadId, ThreadMon);
                 }
+
+                ThreadMon.Position = Position;
+                ThreadMon.ExState  = true;
             }
         }
 
         public bool TestExclusive(AThreadState ThreadState, long Position)
         {
+            //Note: Any call to this method also should be followed by a
+            //call to ClearExclusiveForStore if this method returns true.
             Position &= ~ErgMask;
 
-            lock (Monitors)
+            Monitor.Enter(Monitors);
+
+            if (!Monitors.TryGetValue(ThreadState.ThreadId, out ArmMonitor ThreadMon))
             {
-                if (!Monitors.TryGetValue(ThreadState.ThreadId, out ExMonitor Monitor))
-                {
-                    return false;
-                }
+                return false;
+            }
+
+            bool ExState = ThreadMon.HasExclusiveAccess(Position);
 
-                return Monitor.HasExclusiveAccess(Position);
+            if (!ExState)
+            {
+                Monitor.Exit(Monitors);
             }
+
+            return ExState;
         }
 
-        public void ClearExclusive(AThreadState ThreadState)
+        public void ClearExclusiveForStore(AThreadState ThreadState)
         {
-            lock (Monitors)
+            if (Monitors.TryGetValue(ThreadState.ThreadId, out ArmMonitor ThreadMon))
             {
-                if (Monitors.TryGetValue(ThreadState.ThreadId, out ExMonitor Monitor))
-                {
-                    Monitor.Reset();
-                    ExAddrs.Remove(Monitor.Position);
-                }
+                ThreadMon.ExState = false;
             }
+
+            Monitor.Exit(Monitors);
         }
 
-        public bool AcquireAddress(long Position)
+        public void ClearExclusive(AThreadState ThreadState)
         {
-            Position &= ~ErgMask;
-
             lock (Monitors)
             {
-                return ExAddrs.Add(Position);
+                if (Monitors.TryGetValue(ThreadState.ThreadId, out ArmMonitor ThreadMon))
+                {
+                    ThreadMon.ExState = false;
+                }
             }
         }
 
-        public void ReleaseAddress(long Position)
+        public void WriteInt32ToSharedAddr(long Position, int Value)
         {
-            Position &= ~ErgMask;
+            long MaskedPosition = Position & ~ErgMask;
 
             lock (Monitors)
             {
-                ExAddrs.Remove(Position);
+                foreach (ArmMonitor Mon in Monitors.Values)
+                {
+                    if (Mon.Position == MaskedPosition && Mon.ExState)
+                    {
+                        Mon.ExState = false;
+                    }
+                }
+
+                WriteInt32(Position, Value);
             }
         }
 
diff --git a/Ryujinx.HLE/OsHle/Handles/KProcessScheduler.cs b/Ryujinx.HLE/OsHle/Handles/KProcessScheduler.cs
index fa414f68..722460b1 100644
--- a/Ryujinx.HLE/OsHle/Handles/KProcessScheduler.cs
+++ b/Ryujinx.HLE/OsHle/Handles/KProcessScheduler.cs
@@ -127,20 +127,32 @@ namespace Ryujinx.HLE.OsHle.Handles
             AllThreads[Thread].WaitSync.Set();
         }
 
-        public void TryToRun(KThread Thread)
+        public void ChangeCore(KThread Thread, int IdealCore, int CoreMask)
         {
             lock (SchedLock)
             {
-                if (AllThreads.TryGetValue(Thread, out SchedulerThread SchedThread))
+                if (IdealCore != -3)
                 {
+                    Thread.IdealCore = IdealCore;
+                }
+
+                Thread.CoreMask = CoreMask;
+
+                if (AllThreads.ContainsKey(Thread))
+                {
+                    SetReschedule(Thread.ActualCore);
+
+                    SchedulerThread SchedThread = AllThreads[Thread];
+
+                    //Note: Aways if the thread is on the queue first, and try
+                    //adding to a new core later, to ensure that a thread that
+                    //is already running won't be added to another core.
                     if (WaitingToRun.HasThread(SchedThread) && TryAddToCore(Thread))
                     {
+                        WaitingToRun.Remove(SchedThread);
+
                         RunThread(SchedThread);
                     }
-                    else
-                    {
-                        SetReschedule(Thread.ProcessorId);
-                    }
                 }
             }
         }
@@ -216,18 +228,18 @@ namespace Ryujinx.HLE.OsHle.Handles
 
                 SchedulerThread NewThread = WaitingToRun.Pop(ActualCore, MinPriority);
 
-                if (NewThread == null)
+                if (NewThread != null)
                 {
-                    PrintDbgThreadInfo(Thread, "resumed because theres nothing better to run.");
+                    NewThread.Thread.ActualCore = ActualCore;
 
-                    return;
-                }
-
-                NewThread.Thread.ActualCore = ActualCore;
+                    CoreThreads[ActualCore] = NewThread.Thread;
 
-                CoreThreads[ActualCore] = NewThread.Thread;
-
-                RunThread(NewThread);
+                    RunThread(NewThread);
+                }
+                else
+                {
+                    CoreThreads[ActualCore] = null;
+                }
             }
 
             Resume(Thread);
diff --git a/Ryujinx.HLE/OsHle/Handles/KThread.cs b/Ryujinx.HLE/OsHle/Handles/KThread.cs
index d26a52f0..a135bb28 100644
--- a/Ryujinx.HLE/OsHle/Handles/KThread.cs
+++ b/Ryujinx.HLE/OsHle/Handles/KThread.cs
@@ -58,25 +58,31 @@ namespace Ryujinx.HLE.OsHle.Handles
 
         public void UpdatePriority()
         {
-            int OldPriority = ActualPriority;
-
-            int CurrPriority = WantedPriority;
+            bool PriorityChanged;
 
             lock (Process.ThreadSyncLock)
             {
+                int OldPriority = ActualPriority;
+
+                int CurrPriority = WantedPriority;
+
                 foreach (KThread Thread in MutexWaiters)
                 {
-                    if (CurrPriority > Thread.WantedPriority)
+                    int WantedPriority = Thread.WantedPriority;
+
+                    if (CurrPriority > WantedPriority)
                     {
-                        CurrPriority = Thread.WantedPriority;
+                        CurrPriority = WantedPriority;
                     }
                 }
-            }
 
-            if (CurrPriority != OldPriority)
-            {
+                PriorityChanged = CurrPriority != OldPriority;
+
                 ActualPriority = CurrPriority;
+            }
 
+            if (PriorityChanged)
+            {
                 Process.Scheduler.Resort(this);
 
                 MutexOwner?.UpdatePriority();
diff --git a/Ryujinx.HLE/OsHle/Handles/ThreadQueue.cs b/Ryujinx.HLE/OsHle/Handles/ThreadQueue.cs
index 3cb82f0d..09dcb3d0 100644
--- a/Ryujinx.HLE/OsHle/Handles/ThreadQueue.cs
+++ b/Ryujinx.HLE/OsHle/Handles/ThreadQueue.cs
@@ -24,7 +24,7 @@ namespace Ryujinx.HLE.OsHle.Handles
                     return;
                 }
 
-                if (Head == null || Head.Thread.ActualPriority > Wait.Thread.ActualPriority)
+                if (Head == null || Head.Thread.ActualPriority >= Wait.Thread.ActualPriority)
                 {
                     Wait.Next = Head;
 
@@ -37,7 +37,7 @@ namespace Ryujinx.HLE.OsHle.Handles
 
                 while (Curr.Next != null)
                 {
-                    if (Curr.Next.Thread.ActualPriority > Wait.Thread.ActualPriority)
+                    if (Curr.Next.Thread.ActualPriority >= Wait.Thread.ActualPriority)
                     {
                         break;
                     }
diff --git a/Ryujinx.HLE/OsHle/Kernel/SvcThread.cs b/Ryujinx.HLE/OsHle/Kernel/SvcThread.cs
index 9dcfaaee..4501867f 100644
--- a/Ryujinx.HLE/OsHle/Kernel/SvcThread.cs
+++ b/Ryujinx.HLE/OsHle/Kernel/SvcThread.cs
@@ -83,6 +83,8 @@ namespace Ryujinx.HLE.OsHle.Kernel
         {
             ulong TimeoutNs = ThreadState.X0;
 
+            Ns.Log.PrintDebug(LogClass.KernelSvc, "Timeout = " + TimeoutNs.ToString("x16"));
+
             KThread CurrThread = Process.GetThread(ThreadState.Tpidr);
 
             if (TimeoutNs == 0)
@@ -123,6 +125,10 @@ namespace Ryujinx.HLE.OsHle.Kernel
             int Handle   = (int)ThreadState.X0;
             int Priority = (int)ThreadState.X1;
 
+            Ns.Log.PrintDebug(LogClass.KernelSvc,
+                "Handle = "   + Handle  .ToString("x8") + ", " +
+                "Priority = " + Priority.ToString("x8"));
+
             KThread Thread = GetThread(ThreadState.Tpidr, Handle);
 
             if (Thread != null)
@@ -163,13 +169,6 @@ namespace Ryujinx.HLE.OsHle.Kernel
 
         private void SvcSetThreadCoreMask(AThreadState ThreadState)
         {
-            //FIXME: This is wrong, but the "correct" way to handle
-            //this svc causes deadlocks when more often.
-            //There is probably something wrong with it still.
-            ThreadState.X0 = 0;
-
-            return;
-
             int  Handle    =  (int)ThreadState.X0;
             int  IdealCore =  (int)ThreadState.X1;
             long CoreMask  = (long)ThreadState.X2;
@@ -188,7 +187,7 @@ namespace Ryujinx.HLE.OsHle.Kernel
 
                 CoreMask = 1 << IdealCore;
             }
-            else if (IdealCore != -3)
+            else
             {
                 if ((uint)IdealCore > 3)
                 {
@@ -223,11 +222,7 @@ namespace Ryujinx.HLE.OsHle.Kernel
             //-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)
+            if (IdealCore == -3 && (CoreMask & (1 << Thread.IdealCore)) == 0)
             {
                 Ns.Log.PrintWarning(LogClass.KernelSvc, $"Invalid core mask 0x{CoreMask:x8}!");
 
@@ -236,9 +231,7 @@ namespace Ryujinx.HLE.OsHle.Kernel
                 return;
             }
 
-            Thread.CoreMask = (int)CoreMask;
-
-            Process.Scheduler.TryToRun(Thread);
+            Process.Scheduler.ChangeCore(Thread, IdealCore, (int)CoreMask);
 
             ThreadState.X0 = 0;
         }
diff --git a/Ryujinx.HLE/OsHle/Kernel/SvcThreadSync.cs b/Ryujinx.HLE/OsHle/Kernel/SvcThreadSync.cs
index 030e6e3d..ec9c40e0 100644
--- a/Ryujinx.HLE/OsHle/Kernel/SvcThreadSync.cs
+++ b/Ryujinx.HLE/OsHle/Kernel/SvcThreadSync.cs
@@ -2,9 +2,6 @@ using ChocolArm64.State;
 using Ryujinx.HLE.Logging;
 using Ryujinx.HLE.OsHle.Handles;
 using System;
-using System.Collections.Generic;
-using System.Linq;
-using System.Threading;
 
 using static Ryujinx.HLE.OsHle.ErrorCode;
 
@@ -143,11 +140,9 @@ namespace Ryujinx.HLE.OsHle.Kernel
                 return;
             }
 
-            KThread CurrThread = Process.GetThread(ThreadState.Tpidr);
-
-            MutexUnlock(CurrThread, MutexAddress);
+            KThread WaitThread = Process.GetThread(ThreadState.Tpidr);
 
-            if (!CondVarWait(CurrThread, ThreadHandle, MutexAddress, CondVarAddress, Timeout))
+            if (!CondVarWait(WaitThread, ThreadHandle, MutexAddress, CondVarAddress, Timeout))
             {
                 ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.Timeout);
 
@@ -168,7 +163,7 @@ namespace Ryujinx.HLE.OsHle.Kernel
 
             KThread CurrThread = Process.GetThread(ThreadState.Tpidr);
 
-            CondVarSignal(CurrThread, CondVarAddress, Count);
+            CondVarSignal(ThreadState, CurrThread, CondVarAddress, Count);
 
             ThreadState.X0 = 0;
         }
@@ -182,7 +177,7 @@ namespace Ryujinx.HLE.OsHle.Kernel
         {
             lock (Process.ThreadSyncLock)
             {
-                int MutexValue = Process.Memory.ReadInt32(MutexAddress);
+                int MutexValue = Memory.ReadInt32(MutexAddress);
 
                 Ns.Log.PrintDebug(LogClass.KernelSvc, "MutexValue = " + MutexValue.ToString("x8"));
 
@@ -194,7 +189,7 @@ namespace Ryujinx.HLE.OsHle.Kernel
                 CurrThread.WaitHandle   = WaitThreadHandle;
                 CurrThread.MutexAddress = MutexAddress;
 
-                InsertWaitingMutexThread(OwnerThreadHandle, WaitThread);
+                InsertWaitingMutexThreadUnsafe(OwnerThreadHandle, WaitThread);
             }
 
             Ns.Log.PrintDebug(LogClass.KernelSvc, "Entering wait state...");
@@ -208,25 +203,29 @@ namespace Ryujinx.HLE.OsHle.Kernel
             {
                 //This is the new thread that will now own the mutex.
                 //If no threads are waiting for the lock, then it should be null.
-                KThread OwnerThread = PopThread(CurrThread.MutexWaiters, x => x.MutexAddress == MutexAddress);
+                (KThread OwnerThread, int Count) = PopMutexThreadUnsafe(CurrThread, MutexAddress);
+
+                if (OwnerThread == CurrThread)
+                {
+                    throw new InvalidOperationException();
+                }
 
                 if (OwnerThread != null)
                 {
                     //Remove all waiting mutex from the old owner,
                     //and insert then on the new owner.
-                    UpdateMutexOwner(CurrThread, OwnerThread, MutexAddress);
+                    UpdateMutexOwnerUnsafe(CurrThread, OwnerThread, MutexAddress);
 
                     CurrThread.UpdatePriority();
 
-                    int HasListeners = OwnerThread.MutexWaiters.Count > 0 ? MutexHasListenersMask : 0;
+                    int HasListeners = Count >= 2 ? MutexHasListenersMask : 0;
 
-                    Process.Memory.WriteInt32(MutexAddress, HasListeners | OwnerThread.WaitHandle);
+                    Memory.WriteInt32ToSharedAddr(MutexAddress, HasListeners | OwnerThread.WaitHandle);
 
                     OwnerThread.WaitHandle     = 0;
                     OwnerThread.MutexAddress   = 0;
                     OwnerThread.CondVarAddress = 0;
-
-                    OwnerThread.MutexOwner = null;
+                    OwnerThread.MutexOwner     = null;
 
                     OwnerThread.UpdatePriority();
 
@@ -236,7 +235,7 @@ namespace Ryujinx.HLE.OsHle.Kernel
                 }
                 else
                 {
-                    Process.Memory.WriteInt32(MutexAddress, 0);
+                    Memory.WriteInt32ToSharedAddr(MutexAddress, 0);
 
                     Ns.Log.PrintDebug(LogClass.KernelSvc, "No threads waiting mutex!");
                 }
@@ -256,6 +255,8 @@ namespace Ryujinx.HLE.OsHle.Kernel
 
             lock (Process.ThreadSyncLock)
             {
+                MutexUnlock(WaitThread, MutexAddress);
+
                 WaitThread.CondVarSignaled = false;
 
                 Process.ThreadArbiterList.Add(WaitThread);
@@ -269,11 +270,15 @@ namespace Ryujinx.HLE.OsHle.Kernel
 
                 lock (Process.ThreadSyncLock)
                 {
-                    WaitThread.MutexOwner?.MutexWaiters.Remove(WaitThread);
-
                     if (!WaitThread.CondVarSignaled || WaitThread.MutexOwner != null)
                     {
-                        WaitThread.MutexOwner = null;
+                        if (WaitThread.MutexOwner != null)
+                        {
+                            WaitThread.MutexOwner.MutexWaiters.Remove(WaitThread);
+                            WaitThread.MutexOwner.UpdatePriority();
+
+                            WaitThread.MutexOwner = null;
+                        }
 
                         Process.ThreadArbiterList.Remove(WaitThread);
 
@@ -291,13 +296,17 @@ namespace Ryujinx.HLE.OsHle.Kernel
             return true;
         }
 
-        private void CondVarSignal(KThread CurrThread, long CondVarAddress, int Count)
+        private void CondVarSignal(
+            AThreadState ThreadState,
+            KThread      CurrThread,
+            long         CondVarAddress,
+            int          Count)
         {
             lock (Process.ThreadSyncLock)
             {
                 while (Count == -1 || Count-- > 0)
                 {
-                    KThread WaitThread = PopThread(Process.ThreadArbiterList, x => x.CondVarAddress == CondVarAddress);
+                    KThread WaitThread = PopCondVarThreadUnsafe(CondVarAddress);
 
                     if (WaitThread == null)
                     {
@@ -308,16 +317,37 @@ namespace Ryujinx.HLE.OsHle.Kernel
 
                     WaitThread.CondVarSignaled = true;
 
-                    AcquireMutexValue(WaitThread.MutexAddress);
+                    long MutexAddress = WaitThread.MutexAddress;
+
+                    Memory.SetExclusive(ThreadState, MutexAddress);
+
+                    int MutexValue = Memory.ReadInt32(MutexAddress);
+
+                    while (MutexValue != 0)
+                    {
+                        if (Memory.TestExclusive(ThreadState, MutexAddress))
+                        {
+                            //Wait until the lock is released.
+                            InsertWaitingMutexThreadUnsafe(MutexValue & ~MutexHasListenersMask, WaitThread);
+
+                            Memory.WriteInt32(MutexAddress, MutexValue | MutexHasListenersMask);
 
-                    int MutexValue = Process.Memory.ReadInt32(WaitThread.MutexAddress);
+                            Memory.ClearExclusiveForStore(ThreadState);
+
+                            break;
+                        }
+
+                        Memory.SetExclusive(ThreadState, MutexAddress);
+
+                        MutexValue = Memory.ReadInt32(MutexAddress);
+                    }
 
                     Ns.Log.PrintDebug(LogClass.KernelSvc, "MutexValue = " + MutexValue.ToString("x8"));
 
                     if (MutexValue == 0)
                     {
                         //Give the lock to this thread.
-                        Process.Memory.WriteInt32(WaitThread.MutexAddress, WaitThread.WaitHandle);
+                        Memory.WriteInt32ToSharedAddr(MutexAddress, WaitThread.WaitHandle);
 
                         WaitThread.WaitHandle     = 0;
                         WaitThread.MutexAddress   = 0;
@@ -329,46 +359,28 @@ namespace Ryujinx.HLE.OsHle.Kernel
 
                         Process.Scheduler.WakeUp(WaitThread);
                     }
-                    else
-                    {
-                        //Wait until the lock is released.
-                        MutexValue &= ~MutexHasListenersMask;
-
-                        InsertWaitingMutexThread(MutexValue, WaitThread);
-
-                        MutexValue |= MutexHasListenersMask;
-
-                        Process.Memory.WriteInt32(WaitThread.MutexAddress, MutexValue);
-                    }
-
-                    ReleaseMutexValue(WaitThread.MutexAddress);
                 }
             }
         }
 
-        private void UpdateMutexOwner(KThread CurrThread, KThread NewOwner, long MutexAddress)
+        private void UpdateMutexOwnerUnsafe(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.
-            lock (Process.ThreadSyncLock)
+            for (int Index = 0; Index < CurrThread.MutexWaiters.Count; Index++)
             {
-                for (int Index = 0; Index < CurrThread.MutexWaiters.Count; Index++)
-                {
-                    KThread Thread = CurrThread.MutexWaiters[Index];
-
-                    if (Thread.MutexAddress == MutexAddress)
-                    {
-                        CurrThread.MutexWaiters.RemoveAt(Index--);
+                KThread Thread = CurrThread.MutexWaiters[Index];
 
-                        Thread.MutexOwner = NewOwner;
+                if (Thread.MutexAddress == MutexAddress)
+                {
+                    CurrThread.MutexWaiters.RemoveAt(Index--);
 
-                        InsertWaitingMutexThread(NewOwner, Thread);
-                    }
+                    InsertWaitingMutexThreadUnsafe(NewOwner, Thread);
                 }
             }
         }
 
-        private void InsertWaitingMutexThread(int OwnerThreadHandle, KThread WaitThread)
+        private void InsertWaitingMutexThreadUnsafe(int OwnerThreadHandle, KThread WaitThread)
         {
             KThread OwnerThread = Process.HandleTable.GetData<KThread>(OwnerThreadHandle);
 
@@ -379,47 +391,73 @@ namespace Ryujinx.HLE.OsHle.Kernel
                 return;
             }
 
-            InsertWaitingMutexThread(OwnerThread, WaitThread);
+            InsertWaitingMutexThreadUnsafe(OwnerThread, WaitThread);
         }
 
-        private void InsertWaitingMutexThread(KThread OwnerThread, KThread WaitThread)
+        private void InsertWaitingMutexThreadUnsafe(KThread OwnerThread, KThread WaitThread)
         {
-            lock (Process.ThreadSyncLock)
-            {
-                WaitThread.MutexOwner = OwnerThread;
+            WaitThread.MutexOwner = OwnerThread;
 
-                if (!OwnerThread.MutexWaiters.Contains(WaitThread))
-                {
-                    OwnerThread.MutexWaiters.Add(WaitThread);
+            if (!OwnerThread.MutexWaiters.Contains(WaitThread))
+            {
+                OwnerThread.MutexWaiters.Add(WaitThread);
 
-                    OwnerThread.UpdatePriority();
-                }
+                OwnerThread.UpdatePriority();
             }
         }
 
-        private KThread PopThread(List<KThread> Threads, Func<KThread, bool> Predicate)
+        private (KThread, int) PopMutexThreadUnsafe(KThread OwnerThread, long MutexAddress)
         {
-            KThread Thread = Threads.OrderBy(x => x.ActualPriority).FirstOrDefault(Predicate);
+            int Count = 0;
 
-            if (Thread != null)
+            KThread WakeThread = null;
+
+            foreach (KThread Thread in OwnerThread.MutexWaiters)
             {
-                Threads.Remove(Thread);
+                if (Thread.MutexAddress != MutexAddress)
+                {
+                    continue;
+                }
+
+                if (WakeThread == null || Thread.ActualPriority < WakeThread.ActualPriority)
+                {
+                    WakeThread = Thread;
+                }
+
+                Count++;
             }
 
-            return Thread;
+            if (WakeThread != null)
+            {
+                OwnerThread.MutexWaiters.Remove(WakeThread);
+            }
+
+            return (WakeThread, Count);
         }
 
-        private void AcquireMutexValue(long MutexAddress)
+        private KThread PopCondVarThreadUnsafe(long CondVarAddress)
         {
-            while (!Process.Memory.AcquireAddress(MutexAddress))
+            KThread WakeThread = null;
+
+            foreach (KThread Thread in Process.ThreadArbiterList)
+            {
+                if (Thread.CondVarAddress != CondVarAddress)
+                {
+                    continue;
+                }
+
+                if (WakeThread == null || Thread.ActualPriority < WakeThread.ActualPriority)
+                {
+                    WakeThread = Thread;
+                }
+            }
+
+            if (WakeThread != null)
             {
-                Thread.Yield();
+                Process.ThreadArbiterList.Remove(WakeThread);
             }
-        }
 
-        private void ReleaseMutexValue(long MutexAddress)
-        {
-            Process.Memory.ReleaseAddress(MutexAddress);
+            return WakeThread;
         }
 
         private bool IsPointingInsideKernel(long Address)