Somewhat better scheduler I guess

10 files changed, 297 insertions, 256 deletions

diff --git a/Ryujinx/Cpu/AThread.cs b/Ryujinx/Cpu/AThread.cs
index d86b025a..5c032289 100644
--- a/Ryujinx/Cpu/AThread.cs
+++ b/Ryujinx/Cpu/AThread.cs
@@ -7,8 +7,8 @@ namespace ChocolArm64
 {
     public class AThread
     {
-        public AThreadState  ThreadState { get; private set; }
-        public AMemory     Memory    { get; private set; }
+        public AThreadState ThreadState { get; private set; }
+        public AMemory      Memory      { get; private set; }
 
         public long EntryPoint { get; private set; }
 
diff --git a/Ryujinx/Cpu/Memory/AMemoryHelper.cs b/Ryujinx/Cpu/Memory/AMemoryHelper.cs
index fb4316c5..219aeebf 100644
--- a/Ryujinx/Cpu/Memory/AMemoryHelper.cs
+++ b/Ryujinx/Cpu/Memory/AMemoryHelper.cs
@@ -1,6 +1,5 @@
 using System.IO;
 using System.Text;
-using System.Threading;
 
 namespace ChocolArm64.Memory
 {
@@ -21,32 +20,6 @@ namespace ChocolArm64.Memory
             }
         }
 
-        public static int ReadInt32Exclusive(AMemory Memory, long Position)
-        {
-            while (!Memory.AcquireAddress(Position))
-            {
-                Thread.Yield();
-            }
-
-            int Value = Memory.ReadInt32(Position);
-
-            Memory.ReleaseAddress(Position);
-
-            return Value;
-        }
-
-        public static void WriteInt32Exclusive(AMemory Memory, long Position, int Value)
-        {
-            while (!Memory.AcquireAddress(Position))
-            {
-                Thread.Yield();
-            }
-
-            Memory.WriteInt32(Position, Value);
-
-            Memory.ReleaseAddress(Position);
-        }
-
         public static byte[] ReadBytes(AMemory Memory, long Position, int Size)
         {
             byte[] Data = new byte[Size];
diff --git a/Ryujinx/Hid.cs b/Ryujinx/Hid.cs
index dc969f19..c344ec58 100644
--- a/Ryujinx/Hid.cs
+++ b/Ryujinx/Hid.cs
@@ -43,7 +43,7 @@ namespace Ryujinx
         }
 
         public void Init(long HidOffset)
-        {
+        {            
             unsafe
             {
                 if (HidOffset == 0 || HidOffset + Horizon.HidSize > uint.MaxValue)
diff --git a/Ryujinx/OsHle/CondVar.cs b/Ryujinx/OsHle/CondVar.cs
index eba8e4b0..91ea37bd 100644
--- a/Ryujinx/OsHle/CondVar.cs
+++ b/Ryujinx/OsHle/CondVar.cs
@@ -1,6 +1,6 @@
-using ChocolArm64.Memory;
 using Ryujinx.OsHle.Handles;
 using System.Collections.Generic;
+using System.Threading;
 
 namespace Ryujinx.OsHle
 {
@@ -11,6 +11,8 @@ namespace Ryujinx.OsHle
         private long CondVarAddress;
         private long Timeout;
 
+        private bool OwnsCondVarValue;
+
         private List<HThread> WaitingThreads;
 
         public CondVar(Process Process, long CondVarAddress, long Timeout)
@@ -24,34 +26,43 @@ namespace Ryujinx.OsHle
 
         public void WaitForSignal(HThread Thread)
         {
-            int Count = ReadCondVarValue();
+            int Count = Process.Memory.ReadInt32(CondVarAddress);
 
             if (Count <= 0)
             {
-                //FIXME: We shouldn't need to do that?
-                Process.Scheduler.Yield(Thread);
+                lock (WaitingThreads)
+                {
+                    WaitingThreads.Add(Thread);
+                }
+
+                if (Timeout == -1)
+                {
+                    Process.Scheduler.WaitForSignal(Thread);
+                }
+                else
+                {
+                    Process.Scheduler.WaitForSignal(Thread, (int)(Timeout / 1000000));
 
-                return;
+                    lock (WaitingThreads)
+                    {
+                        WaitingThreads.Remove(Thread);
+                    }
+                }
             }
 
-            WriteCondVarValue(Count - 1);
+            AcquireCondVarValue();
 
-            lock (WaitingThreads)
-            {
-                WaitingThreads.Add(Thread);
-            }
+            Count = Process.Memory.ReadInt32(CondVarAddress);
 
-            if (Timeout != -1)
-            {
-                Process.Scheduler.WaitForSignal(Thread, (int)(Timeout / 1000000));
-            }
-            else
+            if (Count > 0)
             {
-                Process.Scheduler.WaitForSignal(Thread);
+                Process.Memory.WriteInt32(CondVarAddress, Count - 1);
             }
+
+            ReleaseCondVarValue();
         }
 
-        public void SetSignal(int Count)
+        public void SetSignal(HThread Thread, int Count)
         {
             lock (WaitingThreads)
             {
@@ -59,7 +70,11 @@ namespace Ryujinx.OsHle
                 {
                     Process.Scheduler.Signal(WaitingThreads.ToArray());
 
-                    WriteCondVarValue(WaitingThreads.Count);
+                    AcquireCondVarValue();
+
+                    Process.Memory.WriteInt32(CondVarAddress, WaitingThreads.Count);
+
+                    ReleaseCondVarValue();
 
                     WaitingThreads.Clear();
                 }
@@ -85,19 +100,39 @@ namespace Ryujinx.OsHle
                         WaitingThreads.RemoveAt(HighestPrioIndex);
                     }
 
-                    WriteCondVarValue(Count);
+                    AcquireCondVarValue();
+
+                    Process.Memory.WriteInt32(CondVarAddress, Count);
+
+                    ReleaseCondVarValue();
                 }
             }
+
+            Process.Scheduler.Suspend(Thread.ProcessorId);
+            Process.Scheduler.Resume(Thread);
         }
 
-        private int ReadCondVarValue()
+        private void AcquireCondVarValue()
         {
-            return AMemoryHelper.ReadInt32Exclusive(Process.Memory, CondVarAddress);
+            if (!OwnsCondVarValue)
+            {
+                while (!Process.Memory.AcquireAddress(CondVarAddress))
+                {
+                    Thread.Yield();
+                }
+
+                OwnsCondVarValue = true;
+            }
         }
 
-        private void WriteCondVarValue(int Value)
+        private void ReleaseCondVarValue()
         {
-            AMemoryHelper.WriteInt32Exclusive(Process.Memory, CondVarAddress, Value);
+            if (OwnsCondVarValue)
+            {
+                OwnsCondVarValue = false;
+
+                Process.Memory.ReleaseAddress(CondVarAddress);
+            }
         }
     }
 }
\ No newline at end of file
diff --git a/Ryujinx/OsHle/Handles/KProcessScheduler.cs b/Ryujinx/OsHle/Handles/KProcessScheduler.cs
index ca612de9..9044987f 100644
--- a/Ryujinx/OsHle/Handles/KProcessScheduler.cs
+++ b/Ryujinx/OsHle/Handles/KProcessScheduler.cs
@@ -1,4 +1,5 @@
 using System;
+using System.Collections.Concurrent;
 using System.Collections.Generic;
 using System.Threading;
 
@@ -6,19 +7,8 @@ namespace Ryujinx.OsHle.Handles
 {
     class KProcessScheduler : IDisposable
     {
-        private enum ThreadState
-        {
-            WaitingToRun,
-            WaitingSignal,
-            Running
-        }
-
         private class SchedulerThread : IDisposable
         {
-            public bool Signaled { get; set; }
-
-            public ThreadState State { get; set; }
-
             public HThread Thread { get; private set; }
 
             public AutoResetEvent WaitEvent { get; private set; }
@@ -44,9 +34,70 @@ namespace Ryujinx.OsHle.Handles
             }
         }
 
-        private Dictionary<HThread, SchedulerThread> AllThreads;
+        private class ThreadQueue
+        {
+            private List<SchedulerThread> Threads;
+
+            public ThreadQueue()
+            {
+                Threads = new List<SchedulerThread>();
+            }
+
+            public void Push(SchedulerThread Thread)
+            {
+                lock (Threads)
+                {
+                    Threads.Add(Thread);
+                }
+            }
+
+            public SchedulerThread Pop(int MinPriority = 0x40)
+            {
+                lock (Threads)
+                {
+                    SchedulerThread SchedThread;
+
+                    int HighestPriority = MinPriority;
+
+                    int HighestPrioIndex = -1;
+
+                    for (int Index = 0; Index < Threads.Count; Index++)
+                    {
+                        SchedThread = Threads[Index];
+
+                        if (HighestPriority > SchedThread.Thread.Priority)
+                        {
+                            HighestPriority = SchedThread.Thread.Priority;
+
+                            HighestPrioIndex = Index;
+                        }
+                    }
+
+                    if (HighestPrioIndex == -1)
+                    {
+                        return null;
+                    }
+
+                    SchedThread = Threads[HighestPrioIndex];
+
+                    Threads.RemoveAt(HighestPrioIndex);
+
+                    return SchedThread;
+                }
+            }
+
+            public bool HasThread(SchedulerThread SchedThread)
+            {
+                lock (Threads)
+                {
+                    return Threads.Contains(SchedThread);
+                }
+            }
+        }
+
+        private ConcurrentDictionary<HThread, SchedulerThread> AllThreads;
 
-        private Queue<SchedulerThread>[] WaitingThreads;
+        private ThreadQueue[] WaitingToRun;
 
         private HashSet<int> ActiveProcessors;
 
@@ -54,13 +105,13 @@ namespace Ryujinx.OsHle.Handles
 
         public KProcessScheduler()
         {
-            AllThreads = new Dictionary<HThread, SchedulerThread>();
+            AllThreads = new ConcurrentDictionary<HThread, SchedulerThread>();
 
-            WaitingThreads = new Queue<SchedulerThread>[4];
+            WaitingToRun = new ThreadQueue[4];
 
-            for (int Index = 0; Index < WaitingThreads.Length; Index++)
+            for (int Index = 0; Index < 4; Index++)
             {
-                WaitingThreads[Index] = new Queue<SchedulerThread>();
+                WaitingToRun[Index] = new ThreadQueue();
             }
 
             ActiveProcessors = new HashSet<int>();
@@ -72,238 +123,191 @@ namespace Ryujinx.OsHle.Handles
         {
             lock (SchedLock)
             {
-                if (AllThreads.ContainsKey(Thread))
+                SchedulerThread SchedThread = new SchedulerThread(Thread);
+
+                if (!AllThreads.TryAdd(Thread, SchedThread))
                 {
                     return;
                 }
 
-                SchedulerThread SchedThread = new SchedulerThread(Thread);
-
-                AllThreads.Add(Thread, SchedThread);
-
                 if (!ActiveProcessors.Contains(Thread.ProcessorId))
                 {
                     ActiveProcessors.Add(Thread.ProcessorId);
 
                     Thread.Thread.Execute();
 
-                    SetThreadAsRunning(SchedThread);
-
-                    SchedThread.State = ThreadState.Running;
+                    Logging.Debug($"{GetDbgThreadInfo(Thread)} running.");
                 }
                 else
                 {
-                    InsertSorted(SchedThread);
-
-                    SchedThread.State = ThreadState.WaitingToRun;
+                    WaitingToRun[Thread.ProcessorId].Push(SchedThread);
 
                     Logging.Debug($"{GetDbgThreadInfo(SchedThread.Thread)} waiting to run.");
                 }
             }
         }
 
-        public void WaitForSignal(HThread Thread, int TimeoutMs)
+        public void Suspend(int ProcessorId)
         {
-            Logging.Debug($"{GetDbgThreadInfo(Thread)} entering signal wait state with timeout.");
+            lock (SchedLock)
+            {
+                SchedulerThread SchedThread = WaitingToRun[ProcessorId].Pop();
 
-            PutThreadToWait(Thread, ThreadState.WaitingSignal, TimeoutMs);
+                if (SchedThread != null)
+                {
+                    RunThread(SchedThread);
+                }
+                else
+                {
+                    ActiveProcessors.Remove(ProcessorId);
+                }
+            }
         }
 
-        public void WaitForSignal(HThread Thread)
+        public void Resume(HThread CurrThread)
         {
-            Logging.Debug($"{GetDbgThreadInfo(Thread)} entering signal wait state.");
-
-            PutThreadToWait(Thread, ThreadState.WaitingSignal);
-        }
+            SchedulerThread SchedThread;
 
-        public void Yield(HThread Thread)
-        {
-            Logging.Debug($"{GetDbgThreadInfo(Thread)} yielded execution.");
+            Logging.Debug($"{GetDbgThreadInfo(CurrThread)} entering ipc delay wait state.");
 
-            if (WaitingThreads[Thread.ProcessorId].Count == 0)
+            lock (SchedLock)
             {
-                Logging.Debug($"{GetDbgThreadInfo(Thread)} resumed because theres nothing to run.");
+                if (!AllThreads.TryGetValue(CurrThread, out SchedThread))
+                {
+                    Logging.Error($"{GetDbgThreadInfo(CurrThread)} was not found on the scheduler queue!");
 
-                return;
-            }            
+                    return;
+                }
+            }
 
-            PutThreadToWait(Thread, ThreadState.WaitingToRun);
+            TryResumingExecution(SchedThread);
         }
 
-        private void PutThreadToWait(HThread Thread, ThreadState State, int TimeoutMs = -1)
+        public void WaitForSignal(HThread Thread, int Timeout = -1)
         {
             SchedulerThread SchedThread;
 
+            Logging.Debug($"{GetDbgThreadInfo(Thread)} entering signal wait state.");
+
             lock (SchedLock)
             {
-                if (!AllThreads.TryGetValue(Thread, out SchedThread))
-                {
-                    return;
-                }
+                SchedThread = WaitingToRun[Thread.ProcessorId].Pop();
 
-                if (SchedThread.Signaled && SchedThread.State == ThreadState.WaitingSignal)
+                if (SchedThread != null)
                 {
-                    SchedThread.Signaled = false;
-
-                    return;
+                    RunThread(SchedThread);
                 }
-
-                ActiveProcessors.Remove(Thread.ProcessorId);
-
-                SchedThread.State = State;
-
-                TryRunningWaitingThead(SchedThread.Thread.ProcessorId);
-
-                if (State == ThreadState.WaitingSignal)
+                else
                 {
-                    InsertSorted(SchedThread);
+                    ActiveProcessors.Remove(Thread.ProcessorId);
                 }
-                else
+
+                if (!AllThreads.TryGetValue(Thread, out SchedThread))
                 {
-                    InsertAtEnd(SchedThread);
+                    Logging.Error($"{GetDbgThreadInfo(Thread)} was not found on the scheduler queue!");
+
+                    return;
                 }
             }
 
-            if (TimeoutMs >= 0)
+            if (Timeout >= 0)
             {
-                Logging.Debug($"{GetDbgThreadInfo(SchedThread.Thread)} waiting with timeout of {TimeoutMs}ms.");
+                Logging.Debug($"{GetDbgThreadInfo(Thread)} has wait timeout of {Timeout}ms.");
 
-                SchedThread.WaitEvent.WaitOne(TimeoutMs);
+                SchedThread.WaitEvent.WaitOne(Timeout);
             }
             else
             {
-                Logging.Debug($"{GetDbgThreadInfo(SchedThread.Thread)} waiting indefinitely.");
-
                 SchedThread.WaitEvent.WaitOne();
             }
 
-            while (true)
-            {
-                lock (SchedLock)
-                {
-                    Logging.Debug($"Trying to run {GetDbgThreadInfo(SchedThread.Thread)}.");
-
-                    if (!ActiveProcessors.Contains(SchedThread.Thread.ProcessorId))
-                    {
-                        SetThreadAsRunning(SchedThread);
-
-                        break;
-                    }
-                    else
-                    {
-                        SchedThread.State = ThreadState.WaitingToRun;
-
-                        Logging.Debug($"{GetDbgThreadInfo(SchedThread.Thread)} waiting to run.");
-                    }
-                }
-
-                SchedThread.WaitEvent.WaitOne();
-            }
+            TryResumingExecution(SchedThread);
         }
 
-        public void Signal(params HThread[] Threads)
+        private void TryResumingExecution(SchedulerThread SchedThread)
         {
+            HThread Thread = SchedThread.Thread;
+
             lock (SchedLock)
             {
-                HashSet<int> SignaledProcessorIds = new HashSet<int>();
-
-                foreach (HThread Thread in Threads)
+                if (ActiveProcessors.Add(Thread.ProcessorId))
                 {
-                    Logging.Debug($"{GetDbgThreadInfo(Thread)} signaled.");
+                    Logging.Debug($"{GetDbgThreadInfo(Thread)} resuming execution...");
 
-                    if (AllThreads.TryGetValue(Thread, out SchedulerThread SchedThread))
-                    {
-                        if (SchedThread.State == ThreadState.WaitingSignal)
-                        {
-                            SchedThread.State = ThreadState.WaitingToRun;
-
-                            SignaledProcessorIds.Add(Thread.ProcessorId);
-                        }
-
-                        SchedThread.Signaled = true;
-                    }
+                    return;
                 }
 
-                foreach (int ProcessorId in SignaledProcessorIds)
-                {
-                    TryRunningWaitingThead(ProcessorId);
-                }
+                WaitingToRun[Thread.ProcessorId].Push(SchedThread);
             }
+
+            SchedThread.WaitEvent.WaitOne();
+
+            Logging.Debug($"{GetDbgThreadInfo(Thread)} resuming execution...");
         }
 
-        private void TryRunningWaitingThead(int ProcessorId)
+        public void Yield(HThread Thread)
         {
-            Logging.Debug($"TryRunningWaitingThead core {ProcessorId}.");
+            SchedulerThread SchedThread;
+
+            Logging.Debug($"{GetDbgThreadInfo(Thread)} yielded execution.");
 
             lock (SchedLock)
             {
-                if (!ActiveProcessors.Contains(ProcessorId) && WaitingThreads[ProcessorId].Count > 0)
-                {
-                    SchedulerThread SchedThread = WaitingThreads[ProcessorId].Dequeue();
+                SchedThread = WaitingToRun[Thread.ProcessorId].Pop(Thread.Priority);
 
-                    Logging.Debug($"Now trying to run {GetDbgThreadInfo(SchedThread.Thread)}.");
+                if (SchedThread == null)
+                {
+                    Logging.Debug($"{GetDbgThreadInfo(Thread)} resumed because theres nothing better to run.");
 
-                    if (!SchedThread.Thread.Thread.Execute())
-                    {
-                        SchedThread.WaitEvent.Set();
-                    }
-                    else
-                    {
-                        SetThreadAsRunning(SchedThread);
-                    }
+                    return;
                 }
-                else
+                
+                RunThread(SchedThread);
+
+                if (!AllThreads.TryGetValue(Thread, out SchedThread))
                 {
-                    Logging.Debug($"Processor id {ProcessorId} already being used or no waiting threads.");
-                }
-            }
-        }
+                    Logging.Error($"{GetDbgThreadInfo(Thread)} was not found on the scheduler queue!");
 
-        private void SetThreadAsRunning(SchedulerThread SchedThread)
-        {
-            ActiveProcessors.Add(SchedThread.Thread.ProcessorId);
+                    return;
+                }
 
-            SchedThread.State = ThreadState.Running;
+                WaitingToRun[Thread.ProcessorId].Push(SchedThread);
+            }
 
-            SchedThread.Signaled = false;
+            SchedThread.WaitEvent.WaitOne();
 
-            Logging.Debug($"{GetDbgThreadInfo(SchedThread.Thread)} running.");
+            Logging.Debug($"{GetDbgThreadInfo(Thread)} resuming execution...");
         }
 
-        private void InsertSorted(SchedulerThread SchedThread)
+        private void RunThread(SchedulerThread SchedThread)
         {
-            HThread Thread = SchedThread.Thread;
-
-            Queue<SchedulerThread> CoreQueue = WaitingThreads[Thread.ProcessorId];
-
-            Queue<SchedulerThread> TempQueue = new Queue<SchedulerThread>(CoreQueue.Count);
-
-            while (CoreQueue.Count > 0)
+            if (!SchedThread.Thread.Thread.Execute())
             {
-                if (CoreQueue.Peek().Thread.Priority >= Thread.Priority)
-                {
-                    break;
-                }
-
-                TempQueue.Enqueue(CoreQueue.Dequeue());
+                SchedThread.WaitEvent.Set();
             }
-
-            CoreQueue.Enqueue(SchedThread);
-
-            while (CoreQueue.Count > 0)
-            {
-                TempQueue.Enqueue(CoreQueue.Dequeue());
-            }
-
-            while (TempQueue.Count > 0)
+            else
             {
-                CoreQueue.Enqueue(TempQueue.Dequeue());
+                Logging.Debug($"{GetDbgThreadInfo(SchedThread.Thread)} running.");
             }
         }
 
-        private void InsertAtEnd(SchedulerThread SchedThread)
+        public void Signal(params HThread[] Threads)
         {
-            WaitingThreads[SchedThread.Thread.ProcessorId].Enqueue(SchedThread);
+            lock (SchedLock)
+            {
+                foreach (HThread Thread in Threads)
+                {
+                    if (AllThreads.TryGetValue(Thread, out SchedulerThread SchedThread))
+                    {
+                        if (!WaitingToRun[Thread.ProcessorId].HasThread(SchedThread))
+                        {
+                            Logging.Debug($"{GetDbgThreadInfo(Thread)} signaled.");
+
+                            SchedThread.WaitEvent.Set();
+                        }
+                    }
+                }
+            }
         }
 
         private string GetDbgThreadInfo(HThread Thread)
@@ -320,14 +324,9 @@ namespace Ryujinx.OsHle.Handles
         {
             if (Disposing)
             {
-                foreach (Queue<SchedulerThread> SchedThreads in WaitingThreads)
+                foreach (SchedulerThread SchedThread in AllThreads.Values)
                 {
-                    foreach (SchedulerThread SchedThread in SchedThreads)
-                    {
-                        SchedThread.Dispose();
-                    }
-
-                    SchedThreads.Clear();
+                    SchedThread.Dispose();
                 }
             }
         }
diff --git a/Ryujinx/OsHle/Mutex.cs b/Ryujinx/OsHle/Mutex.cs
index f8344b6f..43862d7b 100644
--- a/Ryujinx/OsHle/Mutex.cs
+++ b/Ryujinx/OsHle/Mutex.cs
@@ -1,6 +1,6 @@
-using ChocolArm64.Memory;
 using Ryujinx.OsHle.Handles;
 using System.Collections.Concurrent;
+using System.Threading;
 
 namespace Ryujinx.OsHle
 {
@@ -12,6 +12,8 @@ namespace Ryujinx.OsHle
 
         private long MutexAddress;
 
+        private bool OwnsMutexValue;
+
         private object EnterWaitLock;
 
         private ConcurrentQueue<HThread> WaitingThreads;
@@ -30,9 +32,11 @@ namespace Ryujinx.OsHle
 
         public void WaitForLock(HThread RequestingThread, int RequestingThreadHandle)
         {
+            AcquireMutexValue();
+
             lock (EnterWaitLock)
             {
-                int CurrentThreadHandle = ReadMutexValue() & ~MutexHasListenersMask;
+                int CurrentThreadHandle = Process.Memory.ReadInt32(MutexAddress) & ~MutexHasListenersMask;
 
                 if (CurrentThreadHandle == RequestingThreadHandle ||
                     CurrentThreadHandle == 0)
@@ -40,7 +44,9 @@ namespace Ryujinx.OsHle
                     return;
                 }
 
-                WriteMutexValue(CurrentThreadHandle | MutexHasListenersMask);
+                Process.Memory.WriteInt32(MutexAddress, CurrentThreadHandle | MutexHasListenersMask);
+
+                ReleaseMutexValue();
 
                 WaitingThreads.Enqueue(RequestingThread);
             }
@@ -50,24 +56,32 @@ namespace Ryujinx.OsHle
 
         public void GiveUpLock(int ThreadHandle)
         {
+            AcquireMutexValue();
+
             lock (EnterWaitLock)
             {
-                int CurrentThread = ReadMutexValue() & ~MutexHasListenersMask;
+                int CurrentThread = Process.Memory.ReadInt32(MutexAddress) & ~MutexHasListenersMask;
 
                 if (CurrentThread == ThreadHandle)
                 {
                     Unlock();
                 }
             }
+
+            ReleaseMutexValue();
         }
 
         public void Unlock()
         {
+            AcquireMutexValue();
+
             lock (EnterWaitLock)
             {
                 int HasListeners = WaitingThreads.Count > 1 ? MutexHasListenersMask : 0;
 
-                WriteMutexValue(HasListeners);
+                Process.Memory.WriteInt32(MutexAddress, HasListeners);
+
+                ReleaseMutexValue();
 
                 HThread[] UnlockedThreads = new HThread[WaitingThreads.Count];
 
@@ -82,14 +96,27 @@ namespace Ryujinx.OsHle
             }
         }
 
-        private int ReadMutexValue()
+        private void AcquireMutexValue()
         {
-            return AMemoryHelper.ReadInt32Exclusive(Process.Memory, MutexAddress);
+            if (!OwnsMutexValue)
+            {
+                while (!Process.Memory.AcquireAddress(MutexAddress))
+                {
+                    Thread.Yield();
+                }
+
+                OwnsMutexValue = true;
+            }
         }
 
-        private void WriteMutexValue(int Value)
+        private void ReleaseMutexValue()
         {
-            AMemoryHelper.WriteInt32Exclusive(Process.Memory, MutexAddress, Value);
+            if (OwnsMutexValue)
+            {
+                OwnsMutexValue = false;
+
+                Process.Memory.ReleaseAddress(MutexAddress);
+            }
         }
     }
 }
\ No newline at end of file
diff --git a/Ryujinx/OsHle/Process.cs b/Ryujinx/OsHle/Process.cs
index f8c41dd3..4d07b94b 100644
--- a/Ryujinx/OsHle/Process.cs
+++ b/Ryujinx/OsHle/Process.cs
@@ -231,9 +231,14 @@ namespace Ryujinx.OsHle
             return (int)((Position - TlsPageAddr) / TlsSize);
         }
 
-        public bool TryGetThread(long Tpidr, out HThread Thread)
+        public HThread GetThread(long Tpidr)
         {
-            return ThreadsByTpidr.TryGetValue(Tpidr, out Thread);
+            if (!ThreadsByTpidr.TryGetValue(Tpidr, out HThread Thread))
+            {
+                Logging.Error($"Thread with TPIDR 0x{Tpidr:x16} not found!");
+            }
+
+            return Thread;
         }
 
         public void Dispose()
diff --git a/Ryujinx/OsHle/Svc/SvcSystem.cs b/Ryujinx/OsHle/Svc/SvcSystem.cs
index 9451ae07..0570ccb0 100644
--- a/Ryujinx/OsHle/Svc/SvcSystem.cs
+++ b/Ryujinx/OsHle/Svc/SvcSystem.cs
@@ -4,6 +4,7 @@ using Ryujinx.OsHle.Exceptions;
 using Ryujinx.OsHle.Handles;
 using Ryujinx.OsHle.Ipc;
 using System;
+using System.Threading;
 
 namespace Ryujinx.OsHle.Svc
 {
@@ -37,16 +38,10 @@ namespace Ryujinx.OsHle.Svc
 
             //TODO: Implement events.
 
-            //Logging.Info($"SvcWaitSynchronization Thread {ThreadState.ThreadId}");
+            HThread CurrThread = Process.GetThread(ThreadState.Tpidr);
 
-            if (Process.TryGetThread(ThreadState.Tpidr, out HThread Thread))
-            {
-                Process.Scheduler.Yield(Thread);
-            }
-            else
-            {
-                Logging.Error($"Thread with TPIDR_EL0 0x{ThreadState.Tpidr:x16} not found!");
-            }
+            Process.Scheduler.Suspend(CurrThread.ProcessorId);
+            Process.Scheduler.Resume(CurrThread);
 
             ThreadState.X0 = (int)SvcResult.Success;
         }
@@ -99,6 +94,10 @@ namespace Ryujinx.OsHle.Svc
                 Handle = (int)ThreadState.X0;
             }
 
+            HThread CurrThread = Process.GetThread(ThreadState.Tpidr);
+
+            Process.Scheduler.Suspend(CurrThread.ProcessorId);
+
             byte[] CmdData = AMemoryHelper.ReadBytes(Memory, CmdPtr, (int)Size);
 
             HSession Session = Ns.Os.Handles.GetData<HSession>(Handle);
@@ -117,6 +116,10 @@ namespace Ryujinx.OsHle.Svc
             {
                 ThreadState.X0 = (int)SvcResult.ErrBadIpcReq;
             }
+
+            Thread.Yield();
+
+            Process.Scheduler.Resume(CurrThread);
         }
 
         private void SvcBreak(AThreadState ThreadState)
diff --git a/Ryujinx/OsHle/Svc/SvcThread.cs b/Ryujinx/OsHle/Svc/SvcThread.cs
index c5ea59b9..cc2bbb1e 100644
--- a/Ryujinx/OsHle/Svc/SvcThread.cs
+++ b/Ryujinx/OsHle/Svc/SvcThread.cs
@@ -1,6 +1,5 @@
 using ChocolArm64.State;
 using Ryujinx.OsHle.Handles;
-using System.Threading;
 
 namespace Ryujinx.OsHle.Svc
 {
@@ -18,6 +17,7 @@ namespace Ryujinx.OsHle.Svc
             {
                 if (ProcessorId == -2)
                 {
+                    //TODO: Get this value from the NPDM file.
                     ProcessorId = 0;
                 }
 
@@ -55,16 +55,16 @@ namespace Ryujinx.OsHle.Svc
         {           
             ulong NanoSecs = ThreadState.X0;
 
-            if (Process.TryGetThread(ThreadState.Tpidr, out HThread CurrThread))
+            HThread CurrThread = Process.GetThread(ThreadState.Tpidr);
+            
+            if (NanoSecs == 0)
             {
                 Process.Scheduler.Yield(CurrThread);
             }
             else
             {
-                Logging.Error($"Thread with TPIDR_EL0 0x{ThreadState.Tpidr:x16} not found!");
+                Process.Scheduler.WaitForSignal(CurrThread, (int)(NanoSecs / 1000000));
             }
-
-            Thread.Sleep((int)(NanoSecs / 1000000));
         }
 
         private void SvcGetThreadPriority(AThreadState ThreadState)
diff --git a/Ryujinx/OsHle/Svc/SvcThreadSync.cs b/Ryujinx/OsHle/Svc/SvcThreadSync.cs
index b7108e8f..f342f51d 100644
--- a/Ryujinx/OsHle/Svc/SvcThreadSync.cs
+++ b/Ryujinx/OsHle/Svc/SvcThreadSync.cs
@@ -43,10 +43,11 @@ namespace Ryujinx.OsHle.Svc
 
             HThread Thread = Ns.Os.Handles.GetData<HThread>(ThreadHandle);
 
-            if (Ns.Os.Mutexes.TryGetValue(MutexAddress, out Mutex M))
-            {
-                M.GiveUpLock(ThreadHandle);
-            }
+            Mutex M = new Mutex(Process, MutexAddress, ThreadHandle);
+
+            M = Ns.Os.Mutexes.GetOrAdd(MutexAddress, M);
+
+            M.GiveUpLock(ThreadHandle);
 
             CondVar Cv = new CondVar(Process, CondVarAddress, Timeout);
 
@@ -54,10 +55,6 @@ namespace Ryujinx.OsHle.Svc
 
             Cv.WaitForSignal(Thread);
 
-            M = new Mutex(Process, MutexAddress, ThreadHandle);
-
-            M = Ns.Os.Mutexes.GetOrAdd(MutexAddress, M);
-
             M.WaitForLock(Thread, ThreadHandle);
 
             ThreadState.X0 = (int)SvcResult.Success;
@@ -68,9 +65,11 @@ namespace Ryujinx.OsHle.Svc
             long CondVarAddress = (long)ThreadState.X0;
             int  Count          =  (int)ThreadState.X1;
 
+            HThread CurrThread = Process.GetThread(ThreadState.Tpidr);
+
             if (Ns.Os.CondVars.TryGetValue(CondVarAddress, out CondVar Cv))
             {
-                Cv.SetSignal(Count);
+                Cv.SetSignal(CurrThread, Count);
             }
 
             ThreadState.X0 = (int)SvcResult.Success;