Merge pull request #1 from LDj3SNuD/syncprim

Started to work in improving the sync primitives

Ryujinx.Core/OsHle/Handles/KProcessScheduler.cs

@@ -13,17 +13,23 @@ namespace Ryujinx.Core.OsHle.Handles
         {
             public KThread Thread { get; private set; }
 
-            public AutoResetEvent WaitEvent { get; private set; }
+            public ManualResetEvent SyncWaitEvent  { get; private set; }
+            public AutoResetEvent   SchedWaitEvent { get; private set; }
 
             public bool Active { get; set; }
 
+            public int SyncTimeout { get; set; }
+
             public SchedulerThread(KThread Thread)
             {
                 this.Thread = Thread;
 
-                WaitEvent = new AutoResetEvent(false);
+                SyncWaitEvent  = new ManualResetEvent(true);
+                SchedWaitEvent = new AutoResetEvent(false);
 
                 Active = true;
+
+                SyncTimeout = 0;
             }
 
             public void Dispose()
@@ -35,7 +41,8 @@ namespace Ryujinx.Core.OsHle.Handles
             {
                 if (Disposing)
                 {
-                    WaitEvent.Dispose();
+                    SyncWaitEvent.Dispose();
+                    SchedWaitEvent.Dispose();
                 }
             }
         }
@@ -194,45 +201,66 @@ namespace Ryujinx.Core.OsHle.Handles
                 throw new InvalidOperationException();
             }
 
-            lock (SchedLock)
+            SchedThread.Active = Active;
+
+            UpdateSyncWaitEvent(SchedThread);
+
+            WaitIfNeeded(SchedThread);
+        }
+
+        public bool EnterWait(KThread Thread, int Timeout = -1)
+        {
+            if (!AllThreads.TryGetValue(Thread, out SchedulerThread SchedThread))
             {
-                bool OldState = SchedThread.Active;
+                throw new InvalidOperationException();
-
-                SchedThread.Active = Active;
-
-                if (!OldState && Active)
-                {
-                    if (ActiveProcessors.Add(Thread.ProcessorId))
-                    {
-                        RunThread(SchedThread);
-                    }
-                    else
-                    {
-                        WaitingToRun[Thread.ProcessorId].Push(SchedThread);
-
-                        PrintDbgThreadInfo(Thread, "entering wait state...");
-                    }
-                }
-                else if (OldState && !Active)
-                {
-                    if (Thread.Thread.IsCurrentThread())
-                    {
-                        Suspend(Thread.ProcessorId);
-
-                        PrintDbgThreadInfo(Thread, "entering inactive wait state...");
-                    }
-                    else
-                    {
-                        WaitingToRun[Thread.ProcessorId].Remove(SchedThread);
-                    }
-                }
             }
 
-            if (!Active && Thread.Thread.IsCurrentThread())
+            SchedThread.SyncTimeout = Timeout;
-            {
-                SchedThread.WaitEvent.WaitOne();
 
-                PrintDbgThreadInfo(Thread, "resuming execution...");
+            UpdateSyncWaitEvent(SchedThread);
+
+            return WaitIfNeeded(SchedThread);
+        }
+
+        public void WakeUp(KThread Thread)
+        {
+            if (!AllThreads.TryGetValue(Thread, out SchedulerThread SchedThread))
+            {
+                throw new InvalidOperationException();
+            }
+
+            SchedThread.SyncTimeout = 0;
+
+            UpdateSyncWaitEvent(SchedThread);
+
+            WaitIfNeeded(SchedThread);
+        }
+
+        private void UpdateSyncWaitEvent(SchedulerThread SchedThread)
+        {
+            if (SchedThread.Active && SchedThread.SyncTimeout == 0)
+            {
+                SchedThread.SyncWaitEvent.Set();
+            }
+            else
+            {
+                SchedThread.SyncWaitEvent.Reset();
+            }
+        }
+
+        private bool WaitIfNeeded(SchedulerThread SchedThread)
+        {
+            KThread Thread = SchedThread.Thread;
+
+            if (!IsActive(SchedThread) && Thread.Thread.IsCurrentThread())
+            {
+                Suspend(Thread.ProcessorId);
+
+                return Resume(Thread);
+            }
+            else
+            {
+                return false;
             }
         }
 
@@ -263,64 +291,76 @@ namespace Ryujinx.Core.OsHle.Handles
             {
                 SchedulerThread SchedThread = WaitingToRun[Thread.ProcessorId].Pop(Thread.Priority);
 
-                if (SchedThread == null)
+                if (IsActive(Thread) && SchedThread == null)
                 {
                     PrintDbgThreadInfo(Thread, "resumed because theres nothing better to run.");
 
                     return;
                 }
 
-                RunThread(SchedThread);
+                if (SchedThread != null)
+                {
+                    RunThread(SchedThread);
+                }
             }
 
             Resume(Thread);
         }
 
-        public void Resume(KThread Thread)
+        public bool Resume(KThread Thread)
         {
             if (!AllThreads.TryGetValue(Thread, out SchedulerThread SchedThread))
             {
                 throw new InvalidOperationException();
             }
 
-            TryResumingExecution(SchedThread);
+            return TryResumingExecution(SchedThread);
         }
 
-        private void TryResumingExecution(SchedulerThread SchedThread)
+        private bool TryResumingExecution(SchedulerThread SchedThread)
         {
             KThread Thread = SchedThread.Thread;
 
-            if (SchedThread.Active)
+            if (!SchedThread.Active || SchedThread.SyncTimeout != 0)
-            {
-                lock (SchedLock)
-                {
-                    if (ActiveProcessors.Add(Thread.ProcessorId))
-                    {
-                        PrintDbgThreadInfo(Thread, "resuming execution...");
-
-                        return;
-                    }
-
-                    WaitingToRun[Thread.ProcessorId].Push(SchedThread);
-
-                    PrintDbgThreadInfo(Thread, "entering wait state...");
-                }
-            }
-            else
             {
                 PrintDbgThreadInfo(Thread, "entering inactive wait state...");
             }
 
-            SchedThread.WaitEvent.WaitOne();
+            bool Result = false;
+
+            if (SchedThread.SyncTimeout != 0)
+            {
+                Result = SchedThread.SyncWaitEvent.WaitOne(SchedThread.SyncTimeout);
+
+                SchedThread.SyncTimeout = 0;
+            }
+
+            lock (SchedLock)
+            {
+                if (ActiveProcessors.Add(Thread.ProcessorId))
+                {
+                    PrintDbgThreadInfo(Thread, "resuming execution...");
+
+                    return Result;
+                }
+
+                WaitingToRun[Thread.ProcessorId].Push(SchedThread);
+
+                PrintDbgThreadInfo(Thread, "entering wait state...");
+            }
+
+            SchedThread.SchedWaitEvent.WaitOne();
 
             PrintDbgThreadInfo(Thread, "resuming execution...");
+
+            return Result;
         }
 
         private void RunThread(SchedulerThread SchedThread)
         {
             if (!SchedThread.Thread.Thread.Execute())
             {
-                SchedThread.WaitEvent.Set();
+                SchedThread.SchedWaitEvent.Set();
             }
             else
             {
@@ -328,6 +368,21 @@ namespace Ryujinx.Core.OsHle.Handles
             }
         }
 
+        private bool IsActive(KThread Thread)
+        {
+            if (!AllThreads.TryGetValue(Thread, out SchedulerThread SchedThread))
+            {
+                throw new InvalidOperationException();
+            }
+
+            return IsActive(SchedThread);
+        }
+
+        private bool IsActive(SchedulerThread SchedThread)
+        {
+            return SchedThread.Active && SchedThread.SyncTimeout == 0;
+        }
+
         private void PrintDbgThreadInfo(KThread Thread, string Message)
         {
             Logging.Debug(LogClass.KernelScheduler, "(" +

Ryujinx.Core/OsHle/Handles/KThread.cs

@@ -6,10 +6,19 @@ namespace Ryujinx.Core.OsHle.Handles
     {
         public AThread Thread { get; private set; }
 
+        public KThread NextMutexThread   { get; set; }
+        public KThread NextCondVarThread { get; set; }
+
+        public long MutexAddress   { get; set; }
+        public long CondVarAddress { get; set; }
+
         public int ProcessorId  { get; private set; }
 
-        public int  Priority { get; set; }
+        public int Priority { get; private set; }
-        public int  Handle   { get; set; }
+
+        private int DesiredPriority;
+
+        public int Handle { get; set; }
 
         public int ThreadId => Thread.ThreadId;
 
@@ -17,7 +26,28 @@ namespace Ryujinx.Core.OsHle.Handles
         {
             this.Thread      = Thread;
             this.ProcessorId = ProcessorId;
-            this.Priority    = Priority;
+
+            SetPriority(Priority);
+        }
+
+        public void SetPriority(int Priority)
+        {
+            this.Priority = DesiredPriority = Priority;
+
+            UpdatePriority();
+        }
+
+        public void ResetPriority()
+        {
+            Priority = DesiredPriority;
+        }
+
+        public void UpdatePriority()
+        {
+            if (Priority > (NextMutexThread?.Priority ?? Priority))
+            {
+                Priority = NextMutexThread.Priority;
+            }
         }
     }
 }

Ryujinx.Core/OsHle/Kernel/ConditionVariable.cs

@@ -1,148 +0,0 @@
-using Ryujinx.Core.OsHle.Handles;
-using System.Collections.Generic;
-using System.Threading;
-
-namespace Ryujinx.Core.OsHle.Kernel
-{
-    class ConditionVariable
-    {
-        private Process Process;
-
-        private long CondVarAddress;
-
-        private bool OwnsCondVarValue;
-
-        private List<(KThread Thread, AutoResetEvent WaitEvent)> WaitingThreads;
-
-        public ConditionVariable(Process Process, long CondVarAddress)
-        {
-            this.Process        = Process;
-            this.CondVarAddress = CondVarAddress;
-
-            WaitingThreads = new List<(KThread, AutoResetEvent)>();
-        }
-
-        public bool WaitForSignal(KThread Thread, ulong Timeout)
-        {
-            bool Result = true;
-
-            int Count = Process.Memory.ReadInt32(CondVarAddress);
-
-            if (Count <= 0)
-            {
-                using (AutoResetEvent WaitEvent = new AutoResetEvent(false))
-                {
-                    lock (WaitingThreads)
-                    {
-                        WaitingThreads.Add((Thread, WaitEvent));
-                    }
-
-                    if (Timeout == ulong.MaxValue)
-                    {
-                        Result = WaitEvent.WaitOne();
-                    }
-                    else
-                    {
-                        Result = WaitEvent.WaitOne(NsTimeConverter.GetTimeMs(Timeout));
-
-                        lock (WaitingThreads)
-                        {
-                            WaitingThreads.Remove((Thread, WaitEvent));
-                        }
-                    }
-                }
-            }
-
-            AcquireCondVarValue();
-
-            Count = Process.Memory.ReadInt32(CondVarAddress);
-
-            if (Result && Count > 0)
-            {
-                Process.Memory.WriteInt32(CondVarAddress, Count - 1);
-            }
-
-            ReleaseCondVarValue();
-
-            return Result;
-        }
-
-        public void SetSignal(KThread Thread, int Count)
-        {
-            lock (WaitingThreads)
-            {
-                if (Count < 0)
-                {
-                    foreach ((_, AutoResetEvent WaitEvent) in WaitingThreads)
-                    {
-                        IncrementCondVarValue();
-
-                        WaitEvent.Set();
-                    }
-
-                    WaitingThreads.Clear();
-                }
-                else
-                {
-                    while (WaitingThreads.Count > 0 && Count-- > 0)
-                    {
-                        int HighestPriority  = WaitingThreads[0].Thread.Priority;
-                        int HighestPrioIndex = 0;
-
-                        for (int Index = 1; Index < WaitingThreads.Count; Index++)
-                        {
-                            if (HighestPriority > WaitingThreads[Index].Thread.Priority)
-                            {
-                                HighestPriority = WaitingThreads[Index].Thread.Priority;
-
-                                HighestPrioIndex = Index;
-                            }
-                        }
-
-                        IncrementCondVarValue();
-
-                        WaitingThreads[HighestPrioIndex].WaitEvent.Set();
-
-                        WaitingThreads.RemoveAt(HighestPrioIndex);
-                    }
-                }
-            }
-
-            Process.Scheduler.Yield(Thread);
-        }
-
-        private void IncrementCondVarValue()
-        {
-            AcquireCondVarValue();
-
-            int Count = Process.Memory.ReadInt32(CondVarAddress);
-
-            Process.Memory.WriteInt32(CondVarAddress, Count + 1);
-
-            ReleaseCondVarValue();
-        }
-
-        private void AcquireCondVarValue()
-        {
-            if (!OwnsCondVarValue)
-            {
-                while (!Process.Memory.AcquireAddress(CondVarAddress))
-                {
-                    Thread.Yield();
-                }
-
-                OwnsCondVarValue = true;
-            }
-        }
-
-        private void ReleaseCondVarValue()
-        {
-            if (OwnsCondVarValue)
-            {
-                OwnsCondVarValue = false;
-
-                Process.Memory.ReleaseAddress(CondVarAddress);
-            }
-        }
-    }
-}

Ryujinx.Core/OsHle/Kernel/KernelErr.cs

@@ -2,9 +2,11 @@ namespace Ryujinx.Core.OsHle.Kernel
 {
     static class KernelErr
     {
-        public const int InvalidMemRange = 110;
+        public const int InvalidAlignment = 102;
-        public const int InvalidHandle   = 114;
+        public const int InvalidAddress   = 106;
-        public const int Timeout         = 117;
+        public const int InvalidMemRange  = 110;
-        public const int InvalidInfo     = 120;
+        public const int InvalidHandle    = 114;
+        public const int Timeout          = 117;
+        public const int InvalidInfo      = 120;
     }
 }

Ryujinx.Core/OsHle/Kernel/MutualExclusion.cs

@@ -1,95 +0,0 @@
-using Ryujinx.Core.OsHle.Handles;
-using System.Collections.Generic;
-using System.Threading;
-
-namespace Ryujinx.Core.OsHle.Kernel
-{
-    class MutualExclusion
-    {
-        private const int MutexHasListenersMask = 0x40000000;
-
-        private Process Process;
-
-        private long MutexAddress;
-
-        private int OwnerThreadHandle;
-
-        private List<(KThread Thread, AutoResetEvent WaitEvent)> WaitingThreads;
-
-        public MutualExclusion(Process Process, long MutexAddress)
-        {
-            this.Process      = Process;
-            this.MutexAddress = MutexAddress;
-
-            WaitingThreads = new List<(KThread, AutoResetEvent)>();
-        }
-
-        public void WaitForLock(KThread RequestingThread)
-        {
-            WaitForLock(RequestingThread, OwnerThreadHandle);
-        }
-
-        public void WaitForLock(KThread RequestingThread, int OwnerThreadHandle)
-        {
-            if (OwnerThreadHandle == RequestingThread.Handle ||
-                OwnerThreadHandle == 0)
-            {
-                return;
-            }
-
-            using (AutoResetEvent WaitEvent = new AutoResetEvent(false))
-            {
-                lock (WaitingThreads)
-                {
-                    WaitingThreads.Add((RequestingThread, WaitEvent));
-                }
-
-                Process.Scheduler.Suspend(RequestingThread.ProcessorId);
-
-                WaitEvent.WaitOne();
-
-                Process.Scheduler.Resume(RequestingThread);
-            }
-        }
-
-        public void Unlock()
-        {
-            lock (WaitingThreads)
-            {
-                int HasListeners = WaitingThreads.Count > 1 ? MutexHasListenersMask : 0;
-
-                if (WaitingThreads.Count > 0)
-                {
-                    int HighestPriority  = WaitingThreads[0].Thread.Priority;
-                    int HighestPrioIndex = 0;
-
-                    for (int Index = 1; Index < WaitingThreads.Count; Index++)
-                    {
-                        if (HighestPriority > WaitingThreads[Index].Thread.Priority)
-                        {
-                            HighestPriority = WaitingThreads[Index].Thread.Priority;
-
-                            HighestPrioIndex = Index;
-                        }
-                    }
-
-                    int Handle = WaitingThreads[HighestPrioIndex].Thread.Handle;
-
-                    WaitingThreads[HighestPrioIndex].WaitEvent.Set();
-
-                    WaitingThreads.RemoveAt(HighestPrioIndex);
-
-                    Process.Memory.WriteInt32(MutexAddress, HasListeners | Handle);
-
-                    OwnerThreadHandle = Handle;
-                }
-                else
-                {
-                    Process.Memory.WriteInt32(MutexAddress, 0);
-
-                    OwnerThreadHandle = 0;
-                }
-            }
-        }
-    }
-}

Ryujinx.Core/OsHle/Kernel/SvcHandler.cs

@@ -3,7 +3,6 @@ using ChocolArm64.Memory;
 using ChocolArm64.State;
 using Ryujinx.Core.OsHle.Handles;
 using System;
-using System.Collections.Concurrent;
 using System.Collections.Generic;
 
 namespace Ryujinx.Core.OsHle.Kernel
@@ -18,9 +17,6 @@ namespace Ryujinx.Core.OsHle.Kernel
         private Process Process;
         private AMemory Memory;
 
-        private ConcurrentDictionary<long, MutualExclusion>   Mutexes;
-        private ConcurrentDictionary<long, ConditionVariable> CondVars;
-
         private HashSet<(HSharedMem, long)> MappedSharedMems;
 
         private ulong CurrentHeapSize;
@@ -71,9 +67,6 @@ namespace Ryujinx.Core.OsHle.Kernel
             this.Process = Process;
             this.Memory  = Process.Memory;
 
-            Mutexes  = new ConcurrentDictionary<long, MutualExclusion>();
-            CondVars = new ConcurrentDictionary<long, ConditionVariable>();
-
             MappedSharedMems = new HashSet<(HSharedMem, long)>();
         }
 

Ryujinx.Core/OsHle/Kernel/SvcThread.cs

@@ -110,7 +110,7 @@ namespace Ryujinx.Core.OsHle.Kernel
 
             if (CurrThread != null)
             {
-                CurrThread.Priority = Priority;
+                CurrThread.SetPriority(Priority);
 
                 ThreadState.X0 = 0;
             }

Ryujinx.Core/OsHle/Kernel/SvcThreadSync.cs

@@ -1,5 +1,7 @@
 using ChocolArm64.State;
 using Ryujinx.Core.OsHle.Handles;
+using System;
+using System.Threading;
 
 using static Ryujinx.Core.OsHle.ErrorCode;
 
@@ -7,12 +9,32 @@ namespace Ryujinx.Core.OsHle.Kernel
 {
     partial class SvcHandler
     {
+        private const int MutexHasListenersMask = 0x40000000;
+
         private void SvcArbitrateLock(AThreadState ThreadState)
         {
             int  OwnerThreadHandle      =  (int)ThreadState.X0;
             long MutexAddress           = (long)ThreadState.X1;
             int  RequestingThreadHandle =  (int)ThreadState.X2;
 
+            if (IsPointingInsideKernel(MutexAddress))
+            {
+                Logging.Warn(LogClass.KernelSvc, $"Invalid mutex address 0x{MutexAddress:x16}!");
+
+                ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidAddress);
+
+                return;
+            }
+
+            if (IsWordAddressUnaligned(MutexAddress))
+            {
+                Logging.Warn(LogClass.KernelSvc, $"Unaligned mutex address 0x{MutexAddress:x16}!");
+
+                ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidAlignment);
+
+                return;
+            }
+
             KThread OwnerThread = Process.HandleTable.GetData<KThread>(OwnerThreadHandle);
 
             if (OwnerThread == null)
@@ -35,9 +57,7 @@ namespace Ryujinx.Core.OsHle.Kernel
                 return;
             }
 
-            MutualExclusion Mutex = GetMutex(MutexAddress);
+            MutexLock(MutexAddress, RequestingThread, OwnerThreadHandle);
-
-            Mutex.WaitForLock(RequestingThread, OwnerThreadHandle);
 
             ThreadState.X0 = 0;
         }
@@ -46,9 +66,28 @@ namespace Ryujinx.Core.OsHle.Kernel
         {
             long MutexAddress = (long)ThreadState.X0;
 
-            GetMutex(MutexAddress).Unlock();
+            if (IsPointingInsideKernel(MutexAddress))
+            {
+                Logging.Warn(LogClass.KernelSvc, $"Invalid mutex address 0x{MutexAddress:x16}!");
 
-            Process.Scheduler.Yield(Process.GetThread(ThreadState.Tpidr));
+                ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidAddress);
+
+                return;
+            }
+
+            if (IsWordAddressUnaligned(MutexAddress))
+            {
+                Logging.Warn(LogClass.KernelSvc, $"Unaligned mutex address 0x{MutexAddress:x16}!");
+
+                ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidAlignment);
+
+                return;
+            }
+
+            if (MutexUnlock(Process.GetThread(ThreadState.Tpidr), MutexAddress))
+            {
+                Process.Scheduler.Yield(Process.GetThread(ThreadState.Tpidr));
+            }
 
             ThreadState.X0 = 0;
         }
@@ -60,6 +99,24 @@ namespace Ryujinx.Core.OsHle.Kernel
             int   ThreadHandle   =  (int)ThreadState.X2;
             ulong Timeout        =       ThreadState.X3;
 
+            if (IsPointingInsideKernel(MutexAddress))
+            {
+                Logging.Warn(LogClass.KernelSvc, $"Invalid mutex address 0x{MutexAddress:x16}!");
+
+                ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidAddress);
+
+                return;
+            }
+
+            if (IsWordAddressUnaligned(MutexAddress))
+            {
+                Logging.Warn(LogClass.KernelSvc, $"Unaligned mutex address 0x{MutexAddress:x16}!");
+
+                ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidAlignment);
+
+                return;
+            }
+
             KThread Thread = Process.HandleTable.GetData<KThread>(ThreadHandle);
 
             if (Thread == null)
@@ -67,24 +124,20 @@ namespace Ryujinx.Core.OsHle.Kernel
                 Logging.Warn(LogClass.KernelSvc, $"Invalid thread handle 0x{ThreadHandle:x8}!");
 
                 ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidHandle);
+
+                return;
             }
 
-            Process.Scheduler.Suspend(Thread.ProcessorId);
+            MutexUnlock(Process.GetThread(ThreadState.Tpidr), MutexAddress);
 
-            MutualExclusion Mutex = GetMutex(MutexAddress);
+            if (!CondVarWait(Thread, MutexAddress, CondVarAddress, Timeout))
-
-            Mutex.Unlock();
-
-            if (!GetCondVar(CondVarAddress).WaitForSignal(Thread, Timeout))
             {
                 ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.Timeout);
 
                 return;
             }
 
-            Mutex.WaitForLock(Thread);
+            Process.Scheduler.Yield(Process.GetThread(ThreadState.Tpidr));
-
-            Process.Scheduler.Resume(Thread);
 
             ThreadState.X0 = 0;
         }
@@ -94,31 +147,227 @@ namespace Ryujinx.Core.OsHle.Kernel
             long CondVarAddress = (long)ThreadState.X0;
             int  Count          =  (int)ThreadState.X1;
 
-            KThread CurrThread = Process.GetThread(ThreadState.Tpidr);
+            CondVarSignal(CondVarAddress, Count);
-
-            GetCondVar(CondVarAddress).SetSignal(CurrThread, Count);
 
             ThreadState.X0 = 0;
         }
 
-        private MutualExclusion GetMutex(long MutexAddress)
+        private void MutexLock(long MutexAddress, KThread RequestingThread, int OwnerThreadHandle)
         {
-            MutualExclusion MutexFactory(long Key)
+            int MutexValue = Process.Memory.ReadInt32(MutexAddress);
+
+            if (MutexValue != (OwnerThreadHandle | MutexHasListenersMask))
             {
-                return new MutualExclusion(Process, MutexAddress);
+                return;
             }
 
-            return Mutexes.GetOrAdd(MutexAddress, MutexFactory);
+            InsertWaitingMutexThread(OwnerThreadHandle, RequestingThread);
+
+            RequestingThread.MutexAddress = MutexAddress;
+
+            Process.Scheduler.EnterWait(RequestingThread);
         }
 
-        private ConditionVariable GetCondVar(long CondVarAddress)
+        private bool MutexUnlock(KThread CurrThread, long MutexAddress)
         {
-            ConditionVariable CondVarFactory(long Key)
+            if (CurrThread == null)
             {
-                return new ConditionVariable(Process, CondVarAddress);
+                Logging.Warn(LogClass.KernelSvc, $"Invalid mutex 0x{MutexAddress:x16}!");
+
+                return false;
             }
 
-            return CondVars.GetOrAdd(CondVarAddress, CondVarFactory);
+            CurrThread.MutexAddress   = 0;
+            CurrThread.CondVarAddress = 0;
+
+            CurrThread.ResetPriority();
+
+            KThread OwnerThread = CurrThread.NextMutexThread;
+
+            CurrThread.NextMutexThread = null;
+
+            if (OwnerThread != null)
+            {
+                int HasListeners = OwnerThread.NextMutexThread != null ? MutexHasListenersMask : 0;
+
+                Process.Memory.WriteInt32(MutexAddress, HasListeners | OwnerThread.Handle);
+
+                Process.Scheduler.WakeUp(OwnerThread);
+
+                return true;
+            }
+            else
+            {
+                Process.Memory.WriteInt32(MutexAddress, 0);
+
+                return false;
+            }
+        }
+
+        private bool CondVarWait(KThread WaitThread, long MutexAddress, long CondVarAddress, ulong Timeout)
+        {
+            KThread CurrThread = Process.ThreadArbiterList;
+
+            if (CurrThread != null)
+            {
+                bool DoInsert = CurrThread != WaitThread;
+
+                while (CurrThread.NextCondVarThread != null)
+                {
+                    if (CurrThread.NextCondVarThread.Priority < WaitThread.Priority)
+                    {
+                        break;
+                    }
+
+                    CurrThread = CurrThread.NextCondVarThread;
+
+                    DoInsert &= CurrThread != WaitThread;
+                }
+
+                if (DoInsert)
+                {
+                    if (WaitThread.NextCondVarThread != null)
+                    {
+                        throw new InvalidOperationException();
+                    }
+
+                    WaitThread.NextCondVarThread = CurrThread.NextCondVarThread;
+                    CurrThread.NextCondVarThread = WaitThread;
+                }
+
+                CurrThread.MutexAddress   = MutexAddress;
+                CurrThread.CondVarAddress = CondVarAddress;
+            }
+            else
+            {
+                Process.ThreadArbiterList = WaitThread;
+            }
+
+            if (Timeout != ulong.MaxValue)
+            {
+                return Process.Scheduler.EnterWait(WaitThread, NsTimeConverter.GetTimeMs(Timeout));
+            }
+            else
+            {
+                return Process.Scheduler.EnterWait(WaitThread);
+            }
+        }
+
+        private void CondVarSignal(long CondVarAddress, int Count)
+        {
+            KThread PrevThread = null;
+            KThread CurrThread = Process.ThreadArbiterList;
+
+            while (CurrThread != null && (Count == -1 || Count-- > 0))
+            {
+                if (CurrThread.CondVarAddress == CondVarAddress)
+                {
+                    if (PrevThread != null)
+                    {
+                        PrevThread.NextCondVarThread = CurrThread.NextCondVarThread;
+                    }
+                    else
+                    {
+                        Process.ThreadArbiterList = CurrThread.NextCondVarThread;
+                    }
+
+                    CurrThread.NextCondVarThread = null;
+
+                    AcquireMutexValue(CurrThread.MutexAddress);
+
+                    int MutexValue = Process.Memory.ReadInt32(CurrThread.MutexAddress);
+
+                    MutexValue &= ~MutexHasListenersMask;
+
+                    if (MutexValue == 0)
+                    {
+                        Process.Memory.WriteInt32(CurrThread.MutexAddress, CurrThread.Handle);
+
+                        CurrThread.MutexAddress   = 0;
+                        CurrThread.CondVarAddress = 0;
+
+                        CurrThread.ResetPriority();
+
+                        Process.Scheduler.WakeUp(CurrThread);
+                    }
+                    else
+                    {
+                        InsertWaitingMutexThread(MutexValue, CurrThread);
+
+                        MutexValue |= MutexHasListenersMask;
+
+                        Process.Memory.WriteInt32(CurrThread.MutexAddress, MutexValue);
+                    }
+
+                    ReleaseMutexValue(CurrThread.MutexAddress);
+                }
+
+                PrevThread = CurrThread;
+                CurrThread = CurrThread.NextCondVarThread;
+            }
+        }
+
+        private void InsertWaitingMutexThread(int OwnerThreadHandle, KThread WaitThread)
+        {
+            KThread CurrThread = Process.HandleTable.GetData<KThread>(OwnerThreadHandle);
+
+            if (CurrThread == null)
+            {
+                Logging.Warn(LogClass.KernelSvc, $"Invalid thread handle 0x{OwnerThreadHandle:x8}!");
+
+                return;
+            }
+
+            while (CurrThread.NextMutexThread != null)
+            {
+                if (CurrThread == WaitThread)
+                {
+                    return;
+                }
+
+                if (CurrThread.NextMutexThread.Priority < WaitThread.Priority)
+                {
+                    break;
+                }
+
+                CurrThread = CurrThread.NextMutexThread;
+            }
+
+            if (CurrThread != WaitThread)
+            {
+                if (WaitThread.NextCondVarThread != null)
+                {
+                    throw new InvalidOperationException();
+                }
+
+                WaitThread.NextMutexThread = CurrThread.NextMutexThread;
+                CurrThread.NextMutexThread = WaitThread;
+
+                CurrThread.UpdatePriority();
+            }
+        }
+
+        private void AcquireMutexValue(long MutexAddress)
+        {
+            while (!Process.Memory.AcquireAddress(MutexAddress))
+            {
+                Thread.Yield();
+            }
+        }
+
+        private void ReleaseMutexValue(long MutexAddress)
+        {
+            Process.Memory.ReleaseAddress(MutexAddress);
+        }
+
+        private bool IsPointingInsideKernel(long Address)
+        {
+            return ((ulong)Address + 0x1000000000) < 0xffffff000;
+        }
+
+        private bool IsWordAddressUnaligned(long Address)
+        {
+            return (Address & 3) != 0;
         }
     }
 }

Ryujinx.Core/OsHle/Process.cs

@@ -10,6 +10,7 @@ using Ryujinx.Core.OsHle.Services.Nv;
 using System;
 using System.Collections.Concurrent;
 using System.Collections.Generic;
+using System.Linq;
 
 namespace Ryujinx.Core.OsHle
 {
@@ -35,6 +36,8 @@ namespace Ryujinx.Core.OsHle
 
         public KProcessScheduler Scheduler { get; private set; }
 
+        public KThread ThreadArbiterList { get; set; }
+
         public KProcessHandleTable HandleTable { get; private set; }
 
         public AppletStateMgr AppletState { get; private set; }
@@ -43,7 +46,7 @@ namespace Ryujinx.Core.OsHle
 
         private ConcurrentDictionary<int, AThread> TlsSlots;
 
-        private ConcurrentDictionary<long, KThread> ThreadsByTpidr;
+        private ConcurrentDictionary<long, KThread> Threads;
 
         private List<Executable> Executables;
 
@@ -71,7 +74,7 @@ namespace Ryujinx.Core.OsHle
 
             TlsSlots = new ConcurrentDictionary<int, AThread>();
 
-            ThreadsByTpidr = new ConcurrentDictionary<long, KThread>();
+            Threads = new ConcurrentDictionary<long, KThread>();
 
             Executables = new List<Executable>();
 
@@ -185,34 +188,34 @@ namespace Ryujinx.Core.OsHle
                 throw new ObjectDisposedException(nameof(Process));
             }
 
-            AThread Thread = new AThread(GetTranslator(), Memory, EntryPoint);
+            AThread CpuThread = new AThread(GetTranslator(), Memory, EntryPoint);
 
-            KThread KernelThread = new KThread(Thread, ProcessorId, Priority);
+            KThread Thread = new KThread(CpuThread, ProcessorId, Priority);
 
-            int Handle = HandleTable.OpenHandle(KernelThread);
+            int Handle = HandleTable.OpenHandle(Thread);
 
-            KernelThread.Handle = Handle;
+            Thread.Handle = Handle;
 
-            int ThreadId = GetFreeTlsSlot(Thread);
+            int ThreadId = GetFreeTlsSlot(CpuThread);
 
             long Tpidr = MemoryRegions.TlsPagesAddress + ThreadId * TlsSize;
 
-            Thread.ThreadState.ProcessId = ProcessId;
+            CpuThread.ThreadState.ProcessId = ProcessId;
-            Thread.ThreadState.ThreadId  = ThreadId;
+            CpuThread.ThreadState.ThreadId  = ThreadId;
-            Thread.ThreadState.CntfrqEl0 = TickFreq;
+            CpuThread.ThreadState.CntfrqEl0 = TickFreq;
-            Thread.ThreadState.Tpidr     = Tpidr;
+            CpuThread.ThreadState.Tpidr     = Tpidr;
 
-            Thread.ThreadState.X0  = (ulong)ArgsPtr;
+            CpuThread.ThreadState.X0  = (ulong)ArgsPtr;
-            Thread.ThreadState.X1  = (ulong)Handle;
+            CpuThread.ThreadState.X1  = (ulong)Handle;
-            Thread.ThreadState.X31 = (ulong)StackTop;
+            CpuThread.ThreadState.X31 = (ulong)StackTop;
 
-            Thread.ThreadState.Break     += BreakHandler;
+            CpuThread.ThreadState.Break     += BreakHandler;
-            Thread.ThreadState.SvcCall   += SvcHandler.SvcCall;
+            CpuThread.ThreadState.SvcCall   += SvcHandler.SvcCall;
-            Thread.ThreadState.Undefined += UndefinedHandler;
+            CpuThread.ThreadState.Undefined += UndefinedHandler;
 
-            Thread.WorkFinished += ThreadFinished;
+            CpuThread.WorkFinished += ThreadFinished;
 
-            ThreadsByTpidr.TryAdd(Thread.ThreadState.Tpidr, KernelThread);
+            Threads.TryAdd(CpuThread.ThreadState.Tpidr, Thread);
 
             return Handle;
         }
@@ -324,7 +327,7 @@ namespace Ryujinx.Core.OsHle
 
         public KThread GetThread(long Tpidr)
         {
-            if (!ThreadsByTpidr.TryGetValue(Tpidr, out KThread Thread))
+            if (!Threads.TryGetValue(Tpidr, out KThread Thread))
             {
                 Logging.Error(LogClass.KernelScheduler, $"Thread with TPIDR 0x{Tpidr:x16} not found!");
             }
@@ -332,6 +335,28 @@ namespace Ryujinx.Core.OsHle
             return Thread;
         }
 
+        public IEnumerable<KThread> EnumerateThreadsWithMutex(long MutexAddress)
+        {
+            foreach (KThread Thread in Threads.Values.OrderBy(x => x.Priority))
+            {
+                if (Thread.MutexAddress == MutexAddress)
+                {
+                    yield return Thread;
+                }
+            }
+        }
+
+        public IEnumerable<KThread> EnumerateThreadsWithCondVar(long CondVarAddress)
+        {
+            foreach (KThread Thread in Threads.Values.OrderBy(x => x.Priority))
+            {
+                if (Thread.CondVarAddress == CondVarAddress)
+                {
+                    yield return Thread;
+                }
+            }
+        }
+
         public void Dispose()
         {
             Dispose(true);