Made initial implementation of the thread scheduler, refactor Svc to avoid passing many arguments

19 files changed, 739 insertions, 251 deletions

diff --git a/GLScreen.cs b/GLScreen.cs
index 98281693..cd650f2c 100644
--- a/GLScreen.cs
+++ b/GLScreen.cs
@@ -191,7 +191,7 @@ void main(void) {
             ScreenTex = new ScreenTexture(Ns, Renderer, 1280, 720);
         }
 
-        protected override void OnLoad (EventArgs e)
+        protected override void OnLoad(EventArgs e)
         {
             VSync = VSyncMode.On;
 
diff --git a/Program.cs b/Program.cs
index df1e7c5a..3d4481aa 100644
--- a/Program.cs
+++ b/Program.cs
@@ -53,7 +53,7 @@ namespace Ryujinx
                 Screen.Run(60.0);
             }
 
-            Ns.Os.StopAllProcesses();
+            Ns.Os.FinalizeAllProcesses();
 
             Ns.Dispose();
         }
diff --git a/Ryujinx/Cpu/AThread.cs b/Ryujinx/Cpu/AThread.cs
index 2fafcdd5..93331825 100644
--- a/Ryujinx/Cpu/AThread.cs
+++ b/Ryujinx/Cpu/AThread.cs
@@ -10,8 +10,13 @@ namespace ChocolArm64
         public ARegisters  Registers { get; private set; }
         public AMemory     Memory    { get; private set; }
 
+        public long EntryPoint { get; private set; }
+
         private ATranslator Translator;
-        private Thread      Work;
+
+        private ThreadPriority Priority;
+
+        private Thread Work;
 
         public event EventHandler WorkFinished;
 
@@ -19,25 +24,35 @@ namespace ChocolArm64
 
         public bool IsAlive => Work.IsAlive;
 
-        public long EntryPoint { get; private set; }
-        public int  Priority   { get; private set; }
+        private bool IsExecuting;
+
+        private object ExecuteLock;
 
-        public AThread(AMemory Memory, long EntryPoint = 0, int Priority = 0)
+        public AThread(AMemory Memory, ThreadPriority Priority, long EntryPoint)
         {
             this.Memory     = Memory;
-            this.EntryPoint = EntryPoint;
             this.Priority   = Priority;
+            this.EntryPoint = EntryPoint;
 
-            Registers  = new ARegisters();
-            Translator = new ATranslator(this);
+            Registers   = new ARegisters();
+            Translator  = new ATranslator(this);
+            ExecuteLock = new object();
         }
 
         public void StopExecution() => Translator.StopExecution();
 
-        public void Execute() => Execute(EntryPoint);
-
-        public void Execute(long EntryPoint)
+        public bool Execute()
         {
+            lock (ExecuteLock)
+            {
+                if (IsExecuting)
+                {
+                    return false;
+                }
+
+                IsExecuting = true;
+            }
+
             Work = new Thread(delegate()
             {
                 Translator.ExecuteSubroutine(EntryPoint);
@@ -47,28 +62,11 @@ namespace ChocolArm64
                 WorkFinished?.Invoke(this, EventArgs.Empty);
             });
 
-            if (Priority < 12)
-            {
-                Work.Priority = ThreadPriority.Highest;
-            }
-            else if (Priority < 24)
-            {
-                Work.Priority = ThreadPriority.AboveNormal;
-            }
-            else if (Priority < 36)
-            {
-                Work.Priority = ThreadPriority.Normal;
-            }
-            else if (Priority < 48)
-            {
-                Work.Priority = ThreadPriority.BelowNormal;
-            }
-            else
-            {
-                Work.Priority = ThreadPriority.Lowest;
-            }
+            Work.Priority = Priority;
 
             Work.Start();
+
+            return true;
         }
     }
 }
\ No newline at end of file
diff --git a/Ryujinx/Cpu/Instruction/ASoftFallback.cs b/Ryujinx/Cpu/Instruction/ASoftFallback.cs
index 8fe31a22..2bd25793 100644
--- a/Ryujinx/Cpu/Instruction/ASoftFallback.cs
+++ b/Ryujinx/Cpu/Instruction/ASoftFallback.cs
@@ -197,38 +197,38 @@ namespace ChocolArm64.Instruction
             return ValueF;
         }
 
-        public static float Int32ToDouble(int Value, int FBits)
+        public static double Int32ToDouble(int Value, int FBits)
         {
-            float ValueF = Value;
+            double ValueF = Value;
 
-            if (FBits != 0) ValueF *= 1 / MathF.Pow(2, FBits);
+            if (FBits != 0) ValueF *= 1 / Math.Pow(2, FBits);
 
             return ValueF;
         }
 
-        public static float Int64ToDouble(long Value, int FBits)
+        public static double Int64ToDouble(long Value, int FBits)
         {
-            float ValueF = Value;
+            double ValueF = Value;
 
-            if (FBits != 0) ValueF *= 1 / MathF.Pow(2, FBits);
+            if (FBits != 0) ValueF *= 1 / Math.Pow(2, FBits);
 
             return ValueF;
         }
 
-        public static float UInt32ToDouble(uint Value, int FBits)
+        public static double UInt32ToDouble(uint Value, int FBits)
         {
-            float ValueF = Value;
+            double ValueF = Value;
 
-            if (FBits != 0) ValueF *= 1 / MathF.Pow(2, FBits);
+            if (FBits != 0) ValueF *= 1 / Math.Pow(2, FBits);
 
             return ValueF;
         }
 
-        public static float UInt64ToDouble(ulong Value, int FBits)
+        public static double UInt64ToDouble(ulong Value, int FBits)
         {
-            float ValueF = Value;
+            double ValueF = Value;
 
-            if (FBits != 0) ValueF *= 1 / MathF.Pow(2, FBits);
+            if (FBits != 0) ValueF *= 1 / Math.Pow(2, FBits);
 
             return ValueF;
         }
diff --git a/Ryujinx/Cpu/Memory/AMemory.cs b/Ryujinx/Cpu/Memory/AMemory.cs
index 7952186d..ead99f3f 100644
--- a/Ryujinx/Cpu/Memory/AMemory.cs
+++ b/Ryujinx/Cpu/Memory/AMemory.cs
@@ -66,10 +66,10 @@ namespace ChocolArm64.Memory
 
         public void SetExclusive(ARegisters Registers, long Position)
         {
+            Position &= ~ErgMask;
+
             lock (Monitors)
             {
-                Position &= ~ErgMask;
-
                 if (Monitors.TryGetValue(Registers.ThreadId, out ExMonitor Monitor))
                 {
                     ExAddrs.Remove(Monitor.Position);
@@ -88,10 +88,10 @@ namespace ChocolArm64.Memory
 
         public bool TestExclusive(ARegisters Registers, long Position)
         {
+            Position &= ~ErgMask;
+
             lock (Monitors)
             {
-                Position &= ~ErgMask;
-
                 if (!Monitors.TryGetValue(Registers.ThreadId, out ExMonitor Monitor))
                 {
                     return false;
@@ -113,6 +113,26 @@ namespace ChocolArm64.Memory
             }
         }
 
+        public bool AcquireAddress(long Position)
+        {
+            Position &= ~ErgMask;
+
+            lock (Monitors)
+            {
+                return ExAddrs.Add(Position);
+            }
+        }
+
+        public void ReleaseAddress(long Position)
+        {
+            Position &= ~ErgMask;
+
+            lock (Monitors)
+            {
+                ExAddrs.Remove(Position);
+            }
+        }
+
         public sbyte ReadSByte(long Position) => (sbyte)ReadByte  (Position);
         public short ReadInt16(long Position) => (short)ReadUInt16(Position);
         public int   ReadInt32(long Position) =>   (int)ReadUInt32(Position);
diff --git a/Ryujinx/Cpu/Memory/AMemoryHelper.cs b/Ryujinx/Cpu/Memory/AMemoryHelper.cs
index 219aeebf..fb4316c5 100644
--- a/Ryujinx/Cpu/Memory/AMemoryHelper.cs
+++ b/Ryujinx/Cpu/Memory/AMemoryHelper.cs
@@ -1,5 +1,6 @@
 using System.IO;
 using System.Text;
+using System.Threading;
 
 namespace ChocolArm64.Memory
 {
@@ -20,6 +21,32 @@ 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/OsHle/CondVar.cs b/Ryujinx/OsHle/CondVar.cs
index 02fb8ba3..eba8e4b0 100644
--- a/Ryujinx/OsHle/CondVar.cs
+++ b/Ryujinx/OsHle/CondVar.cs
@@ -1,86 +1,103 @@
 using ChocolArm64.Memory;
-using System.Collections.Concurrent;
-using System.Threading;
+using Ryujinx.OsHle.Handles;
+using System.Collections.Generic;
 
 namespace Ryujinx.OsHle
 {
     class CondVar
     {
-        private AMemory Memory;
+        private Process Process;
 
         private long CondVarAddress;
         private long Timeout;
 
-        private class WaitingThread
-        {
-            public int Handle;
-
-            public ManualResetEvent Event;
-
-            public WaitingThread(int Handle, ManualResetEvent Event)
-            {
-                this.Handle = Handle;
-                this.Event  = Event;
-            }
-        }
+        private List<HThread> WaitingThreads;
 
-        private ConcurrentQueue<WaitingThread> WaitingThreads;
-
-        public CondVar(AMemory Memory, long CondVarAddress, long Timeout)
+        public CondVar(Process Process, long CondVarAddress, long Timeout)
         {
-            this.Memory         = Memory;
+            this.Process        = Process;
             this.CondVarAddress = CondVarAddress;
             this.Timeout        = Timeout;
 
-            WaitingThreads = new ConcurrentQueue<WaitingThread>();
+            WaitingThreads = new List<HThread>();
         }
 
-        public void WaitForSignal(int ThreadHandle)
+        public void WaitForSignal(HThread Thread)
         {
-            int Count = Memory.ReadInt32(CondVarAddress);
+            int Count = ReadCondVarValue();
 
             if (Count <= 0)
             {
+                //FIXME: We shouldn't need to do that?
+                Process.Scheduler.Yield(Thread);
+
                 return;
             }
 
-            Memory.WriteInt32(CondVarAddress, Count - 1);
+            WriteCondVarValue(Count - 1);
 
-            ManualResetEvent Event = new ManualResetEvent(false);
-
-            WaitingThreads.Enqueue(new WaitingThread(ThreadHandle, Event));
+            lock (WaitingThreads)
+            {
+                WaitingThreads.Add(Thread);
+            }
 
             if (Timeout != -1)
             {
-                Event.WaitOne((int)(Timeout / 1000000));
+                Process.Scheduler.WaitForSignal(Thread, (int)(Timeout / 1000000));
             }
             else
             {
-                Event.WaitOne();
+                Process.Scheduler.WaitForSignal(Thread);
             }
         }
 
         public void SetSignal(int Count)
         {
-            if (Count == -1)
+            lock (WaitingThreads)
             {
-                while (WaitingThreads.TryDequeue(out WaitingThread Thread))
+                if (Count == -1)
                 {
-                    Thread.Event.Set();
-                }
+                    Process.Scheduler.Signal(WaitingThreads.ToArray());
 
-                Memory.WriteInt32(CondVarAddress, WaitingThreads.Count);
-            }
-            else
-            {
-                //TODO: Threads with the highest priority needs to be signaled first.
-                if (WaitingThreads.TryDequeue(out WaitingThread Thread))
-                {
-                    Thread.Event.Set();
+                    WriteCondVarValue(WaitingThreads.Count);
+
+                    WaitingThreads.Clear();
                 }
+                else
+                {
+                    if (WaitingThreads.Count > 0)
+                    {
+                        int HighestPriority  = WaitingThreads[0].Priority;
+                        int HighestPrioIndex = 0;
+
+                        for (int Index = 1; Index < WaitingThreads.Count; Index++)
+                        {
+                            if (HighestPriority > WaitingThreads[Index].Priority)
+                            {
+                                HighestPriority = WaitingThreads[Index].Priority;
+
+                                HighestPrioIndex = Index;
+                            }
+                        }
+
+                        Process.Scheduler.Signal(WaitingThreads[HighestPrioIndex]);
+
+                        WaitingThreads.RemoveAt(HighestPrioIndex);
+                    }
 
-                Memory.WriteInt32(CondVarAddress, Count);
+                    WriteCondVarValue(Count);
+                }
             }
         }
+
+        private int ReadCondVarValue()
+        {
+            return AMemoryHelper.ReadInt32Exclusive(Process.Memory, CondVarAddress);
+        }
+
+        private void WriteCondVarValue(int Value)
+        {
+            AMemoryHelper.WriteInt32Exclusive(Process.Memory, CondVarAddress, Value);
+        }
     }
 }
\ No newline at end of file
diff --git a/Ryujinx/OsHle/Handles/HSessionObj.cs b/Ryujinx/OsHle/Handles/HSessionObj.cs
index d2b2b0d7..a871a11f 100644
--- a/Ryujinx/OsHle/Handles/HSessionObj.cs
+++ b/Ryujinx/OsHle/Handles/HSessionObj.cs
@@ -18,9 +18,9 @@ namespace Ryujinx.OsHle.Handles
 
         protected virtual void Dispose(bool Disposing)
         {
-            if(Disposing && Obj != null)
+            if (Disposing && Obj != null)
             {
-                if(Obj is IDisposable DisposableObj)
+                if (Obj is IDisposable DisposableObj)
                 {
                     DisposableObj.Dispose();
                 }
diff --git a/Ryujinx/OsHle/Handles/HThread.cs b/Ryujinx/OsHle/Handles/HThread.cs
index 9fb0b57b..fef2faf2 100644
--- a/Ryujinx/OsHle/Handles/HThread.cs
+++ b/Ryujinx/OsHle/Handles/HThread.cs
@@ -6,9 +6,16 @@ namespace Ryujinx.OsHle.Handles
     {
         public AThread Thread { get; private set; }
 
-        public HThread(AThread Thread)
+        public int ProcessorId { get; private set; }
+        public int Priority    { get; private set; }
+
+        public int ThreadId => Thread.ThreadId;
+
+        public HThread(AThread Thread, int ProcessorId, int Priority)
         {
-            this.Thread = Thread;
+            this.Thread      = Thread;
+            this.ProcessorId = ProcessorId;
+            this.Priority    = Priority;
         }
     }
 }
\ No newline at end of file
diff --git a/Ryujinx/OsHle/Handles/KProcessScheduler.cs b/Ryujinx/OsHle/Handles/KProcessScheduler.cs
new file mode 100644
index 00000000..ca612de9
--- /dev/null
+++ b/Ryujinx/OsHle/Handles/KProcessScheduler.cs
@@ -0,0 +1,335 @@
+using System;
+using System.Collections.Generic;
+using System.Threading;
+
+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; }
+
+            public SchedulerThread(HThread Thread)
+            {
+                this.Thread = Thread;
+
+                WaitEvent = new AutoResetEvent(false);
+            }
+
+            public void Dispose()
+            {
+                Dispose(true);
+            }
+
+            protected virtual void Dispose(bool Disposing)
+            {
+                if (Disposing)
+                {
+                    WaitEvent.Dispose();
+                }
+            }
+        }
+
+        private Dictionary<HThread, SchedulerThread> AllThreads;
+
+        private Queue<SchedulerThread>[] WaitingThreads;
+
+        private HashSet<int> ActiveProcessors;
+
+        private object SchedLock;
+
+        public KProcessScheduler()
+        {
+            AllThreads = new Dictionary<HThread, SchedulerThread>();
+
+            WaitingThreads = new Queue<SchedulerThread>[4];
+
+            for (int Index = 0; Index < WaitingThreads.Length; Index++)
+            {
+                WaitingThreads[Index] = new Queue<SchedulerThread>();
+            }
+
+            ActiveProcessors = new HashSet<int>();
+
+            SchedLock = new object();
+        }
+
+        public void StartThread(HThread Thread)
+        {
+            lock (SchedLock)
+            {
+                if (AllThreads.ContainsKey(Thread))
+                {
+                    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;
+                }
+                else
+                {
+                    InsertSorted(SchedThread);
+
+                    SchedThread.State = ThreadState.WaitingToRun;
+
+                    Logging.Debug($"{GetDbgThreadInfo(SchedThread.Thread)} waiting to run.");
+                }
+            }
+        }
+
+        public void WaitForSignal(HThread Thread, int TimeoutMs)
+        {
+            Logging.Debug($"{GetDbgThreadInfo(Thread)} entering signal wait state with timeout.");
+
+            PutThreadToWait(Thread, ThreadState.WaitingSignal, TimeoutMs);
+        }
+
+        public void WaitForSignal(HThread Thread)
+        {
+            Logging.Debug($"{GetDbgThreadInfo(Thread)} entering signal wait state.");
+
+            PutThreadToWait(Thread, ThreadState.WaitingSignal);
+        }
+
+        public void Yield(HThread Thread)
+        {
+            Logging.Debug($"{GetDbgThreadInfo(Thread)} yielded execution.");
+
+            if (WaitingThreads[Thread.ProcessorId].Count == 0)
+            {
+                Logging.Debug($"{GetDbgThreadInfo(Thread)} resumed because theres nothing to run.");
+
+                return;
+            }            
+
+            PutThreadToWait(Thread, ThreadState.WaitingToRun);
+        }
+
+        private void PutThreadToWait(HThread Thread, ThreadState State, int TimeoutMs = -1)
+        {
+            SchedulerThread SchedThread;
+
+            lock (SchedLock)
+            {
+                if (!AllThreads.TryGetValue(Thread, out SchedThread))
+                {
+                    return;
+                }
+
+                if (SchedThread.Signaled && SchedThread.State == ThreadState.WaitingSignal)
+                {
+                    SchedThread.Signaled = false;
+
+                    return;
+                }
+
+                ActiveProcessors.Remove(Thread.ProcessorId);
+
+                SchedThread.State = State;
+
+                TryRunningWaitingThead(SchedThread.Thread.ProcessorId);
+
+                if (State == ThreadState.WaitingSignal)
+                {
+                    InsertSorted(SchedThread);
+                }
+                else
+                {
+                    InsertAtEnd(SchedThread);
+                }
+            }
+
+            if (TimeoutMs >= 0)
+            {
+                Logging.Debug($"{GetDbgThreadInfo(SchedThread.Thread)} waiting with timeout of {TimeoutMs}ms.");
+
+                SchedThread.WaitEvent.WaitOne(TimeoutMs);
+            }
+            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();
+            }
+        }
+
+        public void Signal(params HThread[] Threads)
+        {
+            lock (SchedLock)
+            {
+                HashSet<int> SignaledProcessorIds = new HashSet<int>();
+
+                foreach (HThread Thread in Threads)
+                {
+                    Logging.Debug($"{GetDbgThreadInfo(Thread)} signaled.");
+
+                    if (AllThreads.TryGetValue(Thread, out SchedulerThread SchedThread))
+                    {
+                        if (SchedThread.State == ThreadState.WaitingSignal)
+                        {
+                            SchedThread.State = ThreadState.WaitingToRun;
+
+                            SignaledProcessorIds.Add(Thread.ProcessorId);
+                        }
+
+                        SchedThread.Signaled = true;
+                    }
+                }
+
+                foreach (int ProcessorId in SignaledProcessorIds)
+                {
+                    TryRunningWaitingThead(ProcessorId);
+                }
+            }
+        }
+
+        private void TryRunningWaitingThead(int ProcessorId)
+        {
+            Logging.Debug($"TryRunningWaitingThead core {ProcessorId}.");
+
+            lock (SchedLock)
+            {
+                if (!ActiveProcessors.Contains(ProcessorId) && WaitingThreads[ProcessorId].Count > 0)
+                {
+                    SchedulerThread SchedThread = WaitingThreads[ProcessorId].Dequeue();
+
+                    Logging.Debug($"Now trying to run {GetDbgThreadInfo(SchedThread.Thread)}.");
+
+                    if (!SchedThread.Thread.Thread.Execute())
+                    {
+                        SchedThread.WaitEvent.Set();
+                    }
+                    else
+                    {
+                        SetThreadAsRunning(SchedThread);
+                    }
+                }
+                else
+                {
+                    Logging.Debug($"Processor id {ProcessorId} already being used or no waiting threads.");
+                }
+            }
+        }
+
+        private void SetThreadAsRunning(SchedulerThread SchedThread)
+        {
+            ActiveProcessors.Add(SchedThread.Thread.ProcessorId);
+
+            SchedThread.State = ThreadState.Running;
+
+            SchedThread.Signaled = false;
+
+            Logging.Debug($"{GetDbgThreadInfo(SchedThread.Thread)} running.");
+        }
+
+        private void InsertSorted(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 (CoreQueue.Peek().Thread.Priority >= Thread.Priority)
+                {
+                    break;
+                }
+
+                TempQueue.Enqueue(CoreQueue.Dequeue());
+            }
+
+            CoreQueue.Enqueue(SchedThread);
+
+            while (CoreQueue.Count > 0)
+            {
+                TempQueue.Enqueue(CoreQueue.Dequeue());
+            }
+
+            while (TempQueue.Count > 0)
+            {
+                CoreQueue.Enqueue(TempQueue.Dequeue());
+            }
+        }
+
+        private void InsertAtEnd(SchedulerThread SchedThread)
+        {
+            WaitingThreads[SchedThread.Thread.ProcessorId].Enqueue(SchedThread);
+        }
+
+        private string GetDbgThreadInfo(HThread Thread)
+        {
+            return $"Thread {Thread.ThreadId} (core {Thread.ProcessorId}) prio {Thread.Priority}";
+        }
+
+        public void Dispose()
+        {
+            Dispose(true);
+        }
+
+        protected virtual void Dispose(bool Disposing)
+        {
+            if (Disposing)
+            {
+                foreach (Queue<SchedulerThread> SchedThreads in WaitingThreads)
+                {
+                    foreach (SchedulerThread SchedThread in SchedThreads)
+                    {
+                        SchedThread.Dispose();
+                    }
+
+                    SchedThreads.Clear();
+                }
+            }
+        }
+    }
+}
\ No newline at end of file
diff --git a/Ryujinx/OsHle/Horizon.cs b/Ryujinx/OsHle/Horizon.cs
index 04744bdf..dd578730 100644
--- a/Ryujinx/OsHle/Horizon.cs
+++ b/Ryujinx/OsHle/Horizon.cs
@@ -127,11 +127,12 @@ namespace Ryujinx.OsHle
             Processes.TryAdd(ProcessId, MainProcess);
         }
 
-        public void StopAllProcesses()
+        public void FinalizeAllProcesses()
         {
             foreach (Process Process in Processes.Values)
             {
                 Process.StopAllThreads();
+                Process.Dispose();
             }
         }
 
diff --git a/Ryujinx/OsHle/Mutex.cs b/Ryujinx/OsHle/Mutex.cs
index 99d12b28..60870baa 100644
--- a/Ryujinx/OsHle/Mutex.cs
+++ b/Ryujinx/OsHle/Mutex.cs
@@ -1,5 +1,6 @@
-using ChocolArm64;
 using ChocolArm64.Memory;
+using Ryujinx.OsHle.Handles;
+using System.Collections.Concurrent;
 using System.Threading;
 
 namespace Ryujinx.OsHle
@@ -8,33 +9,31 @@ namespace Ryujinx.OsHle
     {
         private const int MutexHasListenersMask = 0x40000000;
 
-        private AMemory Memory;
+        private Process Process;
 
         private long MutexAddress;
 
-        private int CurrRequestingThreadHandle;
-
-        private int HighestPriority;
-
-        private ManualResetEvent ThreadEvent;
-
         private object EnterWaitLock;
 
-        public Mutex(AMemory Memory, long MutexAddress)
+        private ConcurrentQueue<HThread> WaitingThreads;
+
+        public Mutex(Process Process, long MutexAddress, int OwnerThreadHandle)
         {
-            this.Memory       = Memory;
+            this.Process      = Process;
             this.MutexAddress = MutexAddress;
 
-            ThreadEvent = new ManualResetEvent(false);
+            //Process.Memory.WriteInt32(MutexAddress, OwnerThreadHandle);
 
             EnterWaitLock = new object();
+
+            WaitingThreads = new ConcurrentQueue<HThread>();
         }
 
-        public void WaitForLock(AThread RequestingThread, int RequestingThreadHandle)
+        public void WaitForLock(HThread RequestingThread, int RequestingThreadHandle)
         {
             lock (EnterWaitLock)
-            {               
-                int CurrentThreadHandle = Memory.ReadInt32(MutexAddress) & ~MutexHasListenersMask;
+            {
+                int CurrentThreadHandle = ReadMutexValue() & ~MutexHasListenersMask;
 
                 if (CurrentThreadHandle == RequestingThreadHandle ||
                     CurrentThreadHandle == 0)
@@ -42,23 +41,19 @@ namespace Ryujinx.OsHle
                     return;
                 }
 
-                if (CurrRequestingThreadHandle == 0 || RequestingThread.Priority < HighestPriority)
-                {
-                    CurrRequestingThreadHandle = RequestingThreadHandle;
+                WriteMutexValue(CurrentThreadHandle | MutexHasListenersMask);
 
-                    HighestPriority = RequestingThread.Priority;
-                }
+                WaitingThreads.Enqueue(RequestingThread);
             }
 
-            ThreadEvent.Reset();
-            ThreadEvent.WaitOne();
+            Process.Scheduler.WaitForSignal(RequestingThread);
         }
 
         public void GiveUpLock(int ThreadHandle)
         {
             lock (EnterWaitLock)
             {
-                int CurrentThread = Memory.ReadInt32(MutexAddress) & ~MutexHasListenersMask;
+                int CurrentThread = ReadMutexValue() & ~MutexHasListenersMask;
 
                 if (CurrentThread == ThreadHandle)
                 {
@@ -71,19 +66,31 @@ namespace Ryujinx.OsHle
         {
             lock (EnterWaitLock)
             {
-                if (CurrRequestingThreadHandle != 0)
-                {
-                    Memory.WriteInt32(MutexAddress, CurrRequestingThreadHandle);
-                }
-                else
+                int HasListeners = WaitingThreads.Count > 1 ? MutexHasListenersMask : 0;
+
+                WriteMutexValue(HasListeners);
+
+                HThread[] UnlockedThreads = new HThread[WaitingThreads.Count];
+
+                int Index = 0;
+
+                while (WaitingThreads.TryDequeue(out HThread Thread))
                 {
-                    Memory.WriteInt32(MutexAddress, 0);
+                    UnlockedThreads[Index++] = Thread;
                 }
 
-                CurrRequestingThreadHandle = 0;
-
-                ThreadEvent.Set();
+                Process.Scheduler.Signal(UnlockedThreads);
             }
         }
+
+        private int ReadMutexValue()
+        {
+            return AMemoryHelper.ReadInt32Exclusive(Process.Memory, MutexAddress);
+        }
+
+        private void WriteMutexValue(int Value)
+        {
+            AMemoryHelper.WriteInt32Exclusive(Process.Memory, MutexAddress, Value);
+        }
     }
 }
\ No newline at end of file
diff --git a/Ryujinx/OsHle/Process.cs b/Ryujinx/OsHle/Process.cs
index 66823846..624ab654 100644
--- a/Ryujinx/OsHle/Process.cs
+++ b/Ryujinx/OsHle/Process.cs
@@ -9,10 +9,11 @@ using Ryujinx.OsHle.Svc;
 using System;
 using System.Collections.Concurrent;
 using System.Collections.Generic;
+using System.Threading;
 
 namespace Ryujinx.OsHle
 {
-    class Process
+    class Process : IDisposable
     {
         private const int  MaxStackSize  = 8 * 1024 * 1024;
 
@@ -27,14 +28,18 @@ namespace Ryujinx.OsHle
 
         public AMemory Memory { get; private set; }
 
-        private SvcHandler SvcHandler;
+        public KProcessScheduler Scheduler { get; private set; }
 
-        private AThread MainThread;
+        private SvcHandler SvcHandler;       
 
         private ConcurrentDictionary<int, AThread> TlsSlots;
 
+        private ConcurrentDictionary<long, HThread> ThreadsByTpidr;
+
         private List<Executable> Executables;
 
+        private HThread MainThread;
+
         private long ImageBase;
 
         public Process(Switch Ns, AMemoryAlloc Allocator, int ProcessId)
@@ -42,9 +47,16 @@ namespace Ryujinx.OsHle
             this.Ns        = Ns;
             this.ProcessId = ProcessId;
 
-            Memory      = new AMemory(Ns.Ram, Allocator);
-            SvcHandler  = new SvcHandler(Ns, Memory);
-            TlsSlots    = new ConcurrentDictionary<int, AThread>();
+            Memory = new AMemory(Ns.Ram, Allocator);
+
+            Scheduler = new KProcessScheduler();
+
+            SvcHandler = new SvcHandler(Ns, this);
+
+            TlsSlots = new ConcurrentDictionary<int, AThread>();
+
+            ThreadsByTpidr = new ConcurrentDictionary<long, HThread>();
+
             Executables = new List<Executable>();
 
             ImageBase = 0x8000000;
@@ -86,16 +98,16 @@ namespace Ryujinx.OsHle
 
             Memory.Manager.MapPhys(StackBot, MaxStackSize, (int)MemoryType.Normal, AMemoryPerm.RW);
 
-            int Handle = MakeThread(Executables[0].ImageBase, TlsPageAddr, 0, 48, 0);
+            int Handle = MakeThread(Executables[0].ImageBase, TlsPageAddr, 0, 0, 0);
 
             if (Handle == -1)
             {
                 return false;
             }
 
-            MainThread = Ns.Os.Handles.GetData<HThread>(Handle).Thread;
+            MainThread = Ns.Os.Handles.GetData<HThread>(Handle);
 
-            MainThread.Execute();
+            Scheduler.StartThread(MainThread);
 
             return true;
         }
@@ -104,9 +116,9 @@ namespace Ryujinx.OsHle
         {
             if (MainThread != null)
             {
-                while (MainThread.IsAlive)
+                while (MainThread.Thread.IsAlive)
                 {
-                    MainThread.StopExecution();
+                    MainThread.Thread.StopExecution();
                 }
             }
 
@@ -126,11 +138,36 @@ namespace Ryujinx.OsHle
             int  Priority,
             int  ProcessorId)
         {
-            AThread Thread = new AThread(Memory, EntryPoint, Priority);
+            ThreadPriority ThreadPrio;
 
-            int TlsSlot = GetFreeTlsSlot(Thread);
+            if (Priority < 12)
+            {
+                ThreadPrio = ThreadPriority.Highest;
+            }
+            else if (Priority < 24)
+            {
+                ThreadPrio = ThreadPriority.AboveNormal;
+            }
+            else if (Priority < 36)
+            {
+                ThreadPrio = ThreadPriority.Normal;
+            }
+            else if (Priority < 48)
+            {
+                ThreadPrio = ThreadPriority.BelowNormal;
+            }
+            else
+            {
+                ThreadPrio = ThreadPriority.Lowest;
+            }
+
+            AThread Thread = new AThread(Memory, ThreadPrio, EntryPoint);
+
+            HThread ThreadHnd = new HThread(Thread, ProcessorId, Priority);
 
-            int Handle = Ns.Os.Handles.GenerateId(new HThread(Thread));
+            int Handle = Ns.Os.Handles.GenerateId(ThreadHnd);
+
+            int TlsSlot = GetFreeTlsSlot(Thread);
 
             if (TlsSlot == -1 || Handle  == -1)
             {
@@ -149,6 +186,8 @@ namespace Ryujinx.OsHle
 
             Thread.WorkFinished += ThreadFinished;
 
+            ThreadsByTpidr.TryAdd(Thread.Registers.Tpidr, ThreadHnd);
+
             return Handle;
         }
 
@@ -189,5 +228,23 @@ namespace Ryujinx.OsHle
         {
             return (int)((Position - TlsPageAddr) / TlsSize);
         }
+
+        public bool TryGetThread(long Tpidr, out HThread Thread)
+        {
+            return ThreadsByTpidr.TryGetValue(Tpidr, out Thread);
+        }
+
+        public void Dispose()
+        {
+            Dispose(true);
+        }
+
+        protected virtual void Dispose(bool Disposing)
+        {
+            if (Disposing)
+            {
+                Scheduler.Dispose();
+            }
+        }
     }
 }
\ No newline at end of file
diff --git a/Ryujinx/OsHle/Svc/SvcHandler.cs b/Ryujinx/OsHle/Svc/SvcHandler.cs
index 44bd88ee..ad228c45 100644
--- a/Ryujinx/OsHle/Svc/SvcHandler.cs
+++ b/Ryujinx/OsHle/Svc/SvcHandler.cs
@@ -7,55 +7,50 @@ namespace Ryujinx.OsHle.Svc
 {
     partial class SvcHandler
     {
-        private delegate void SvcFunc(Switch Ns, ARegisters Registers, AMemory Memory);
+        private delegate void SvcFunc(ARegisters Registers);
 
-        private Dictionary<int, SvcFunc> SvcFuncs = new Dictionary<int, SvcFunc>()
-        {
-            { 0x01, SvcSetHeapSize                   },
-            { 0x03, SvcSetMemoryAttribute            },
-            { 0x04, SvcMapMemory                     },
-            { 0x06, SvcQueryMemory                   },
-            { 0x08, SvcCreateThread                  },
-            { 0x09, SvcStartThread                   },
-            { 0x0b, SvcSleepThread                   },
-            { 0x0c, SvcGetThreadPriority             },
-            { 0x13, SvcMapSharedMemory               },
-            { 0x14, SvcUnmapSharedMemory             },
-            { 0x15, SvcCreateTransferMemory          },
-            { 0x16, SvcCloseHandle                   },
-            { 0x17, SvcResetSignal                   },
-            { 0x18, SvcWaitSynchronization           },
-            { 0x1a, SvcArbitrateLock                 },
-            { 0x1b, SvcArbitrateUnlock               },
-            { 0x1c, SvcWaitProcessWideKeyAtomic      },
-            { 0x1d, SvcSignalProcessWideKey          },
-            { 0x1e, SvcGetSystemTick                 },
-            { 0x1f, SvcConnectToNamedPort            },
-            { 0x21, SvcSendSyncRequest               },
-            { 0x22, SvcSendSyncRequestWithUserBuffer },
-            { 0x26, SvcBreak                         },
-            { 0x27, SvcOutputDebugString             },
-            { 0x29, SvcGetInfo                       }
-        };
-
-        enum SvcResult
-        {
-            Success      = 0,
-            ErrBadHandle = 0xe401,
-            ErrTimeout   = 0xea01,
-            ErrBadInfo   = 0xf001,
-            ErrBadIpcReq = 0xf601
-        }
+        private Dictionary<int, SvcFunc> SvcFuncs;
 
         private Switch  Ns;
+        private Process Process;
         private AMemory Memory;
 
         private static Random Rng;
 
-        public SvcHandler(Switch Ns, AMemory Memory)
+        public SvcHandler(Switch Ns, Process Process)
         {
-            this.Ns     = Ns;
-            this.Memory = Memory;
+            SvcFuncs = new Dictionary<int, SvcFunc>()
+            {
+                { 0x01, SvcSetHeapSize                   },
+                { 0x03, SvcSetMemoryAttribute            },
+                { 0x04, SvcMapMemory                     },
+                { 0x06, SvcQueryMemory                   },
+                { 0x08, SvcCreateThread                  },
+                { 0x09, SvcStartThread                   },
+                { 0x0b, SvcSleepThread                   },
+                { 0x0c, SvcGetThreadPriority             },
+                { 0x13, SvcMapSharedMemory               },
+                { 0x14, SvcUnmapSharedMemory             },
+                { 0x15, SvcCreateTransferMemory          },
+                { 0x16, SvcCloseHandle                   },
+                { 0x17, SvcResetSignal                   },
+                { 0x18, SvcWaitSynchronization           },
+                { 0x1a, SvcArbitrateLock                 },
+                { 0x1b, SvcArbitrateUnlock               },
+                { 0x1c, SvcWaitProcessWideKeyAtomic      },
+                { 0x1d, SvcSignalProcessWideKey          },
+                { 0x1e, SvcGetSystemTick                 },
+                { 0x1f, SvcConnectToNamedPort            },
+                { 0x21, SvcSendSyncRequest               },
+                { 0x22, SvcSendSyncRequestWithUserBuffer },
+                { 0x26, SvcBreak                         },
+                { 0x27, SvcOutputDebugString             },
+                { 0x29, SvcGetInfo                       }
+            };
+
+            this.Ns      = Ns;
+            this.Process = Process;
+            this.Memory  = Process.Memory;
         }
 
         static SvcHandler()
@@ -69,9 +64,11 @@ namespace Ryujinx.OsHle.Svc
 
             if (SvcFuncs.TryGetValue(e.Id, out SvcFunc Func))
             {
-                Logging.Trace($"{Func.Method.Name} called.");
-                Func(Ns, Registers, Memory);
-                Logging.Trace($"{Func.Method.Name} ended.");
+                Logging.Trace($"(Thread {Registers.ThreadId}) {Func.Method.Name} called.");
+
+                Func(Registers);
+
+                Logging.Trace($"(Thread {Registers.ThreadId}) {Func.Method.Name} ended.");
             }
             else
             {
diff --git a/Ryujinx/OsHle/Svc/SvcMemory.cs b/Ryujinx/OsHle/Svc/SvcMemory.cs
index 185cac89..f6cef4cc 100644
--- a/Ryujinx/OsHle/Svc/SvcMemory.cs
+++ b/Ryujinx/OsHle/Svc/SvcMemory.cs
@@ -6,7 +6,7 @@ namespace Ryujinx.OsHle.Svc
 {
     partial class SvcHandler
     {
-        private static void SvcSetHeapSize(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcSetHeapSize(ARegisters Registers)
         {
             uint Size = (uint)Registers.X1;
 
@@ -16,7 +16,7 @@ namespace Ryujinx.OsHle.Svc
             Registers.X1 = (ulong)Memory.Manager.HeapAddr;
         }
 
-        private static void SvcSetMemoryAttribute(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcSetMemoryAttribute(ARegisters Registers)
         {
             long Position = (long)Registers.X0;
             long Size     = (long)Registers.X1;
@@ -28,7 +28,7 @@ namespace Ryujinx.OsHle.Svc
             Registers.X0 = (int)SvcResult.Success;
         }
 
-        private static void SvcMapMemory(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcMapMemory(ARegisters Registers)
         {
             long Dst  = (long)Registers.X0;
             long Src  = (long)Registers.X1;
@@ -39,7 +39,7 @@ namespace Ryujinx.OsHle.Svc
             Registers.X0 = (int)SvcResult.Success;
         }
 
-        private static void SvcQueryMemory(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcQueryMemory(ARegisters Registers)
         {
             long InfoPtr  = (long)Registers.X0;
             long Position = (long)Registers.X2;
@@ -63,7 +63,7 @@ namespace Ryujinx.OsHle.Svc
             Registers.X1 = 0;
         }
 
-        private static void SvcMapSharedMemory(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcMapSharedMemory(ARegisters Registers)
         {
             int  Handle   =  (int)Registers.X0;
             long Position = (long)Registers.X1;
@@ -87,7 +87,7 @@ namespace Ryujinx.OsHle.Svc
             //TODO: Error codes.
         }
 
-        private static void SvcUnmapSharedMemory(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcUnmapSharedMemory(ARegisters Registers)
         {
             int  Handle   =  (int)Registers.X0;
             long Position = (long)Registers.X1;
@@ -103,7 +103,7 @@ namespace Ryujinx.OsHle.Svc
             //TODO: Error codes.
         }
 
-        private static void SvcCreateTransferMemory(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcCreateTransferMemory(ARegisters Registers)
         {
             long Position = (long)Registers.X1;
             long Size     = (long)Registers.X2;
diff --git a/Ryujinx/OsHle/Svc/SvcResult.cs b/Ryujinx/OsHle/Svc/SvcResult.cs
new file mode 100644
index 00000000..4f0c4f1f
--- /dev/null
+++ b/Ryujinx/OsHle/Svc/SvcResult.cs
@@ -0,0 +1,11 @@
+namespace Ryujinx.OsHle.Svc
+{
+    enum SvcResult
+    {
+        Success      = 0,
+        ErrBadHandle = 0xe401,
+        ErrTimeout   = 0xea01,
+        ErrBadInfo   = 0xf001,
+        ErrBadIpcReq = 0xf601
+    }
+}
\ No newline at end of file
diff --git a/Ryujinx/OsHle/Svc/SvcSystem.cs b/Ryujinx/OsHle/Svc/SvcSystem.cs
index 5721c53b..098ddb2f 100644
--- a/Ryujinx/OsHle/Svc/SvcSystem.cs
+++ b/Ryujinx/OsHle/Svc/SvcSystem.cs
@@ -1,5 +1,6 @@
 using ChocolArm64.Memory;
 using ChocolArm64.State;
+using Ryujinx.OsHle.Exceptions;
 using Ryujinx.OsHle.Handles;
 using Ryujinx.OsHle.Ipc;
 using System;
@@ -8,7 +9,7 @@ namespace Ryujinx.OsHle.Svc
 {
     partial class SvcHandler
     {
-        private static void SvcCloseHandle(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcCloseHandle(ARegisters Registers)
         {
             int Handle = (int)Registers.X0;
 
@@ -17,7 +18,7 @@ namespace Ryujinx.OsHle.Svc
             Registers.X0 = (int)SvcResult.Success;
         }
 
-        private static void SvcResetSignal(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcResetSignal(ARegisters Registers)
         {
             int Handle = (int)Registers.X0;
 
@@ -26,7 +27,7 @@ namespace Ryujinx.OsHle.Svc
             Registers.X0 = (int)SvcResult.Success;
         }
 
-        private static void SvcWaitSynchronization(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcWaitSynchronization(ARegisters Registers)
         {
             long HandlesPtr   = (long)Registers.X0;
             int  HandlesCount =  (int)Registers.X2;
@@ -34,15 +35,26 @@ namespace Ryujinx.OsHle.Svc
 
             //TODO: Implement events.
 
+            //Logging.Info($"SvcWaitSynchronization Thread {Registers.ThreadId}");
+
+            if (Process.TryGetThread(Registers.Tpidr, out HThread Thread))
+            {
+                Process.Scheduler.Yield(Thread);
+            }
+            else
+            {
+                Logging.Error($"Thread with TPIDR_EL0 0x{Registers.Tpidr:x16} not found!");
+            }
+
             Registers.X0 = (int)SvcResult.Success;
         }
 
-        private static void SvcGetSystemTick(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcGetSystemTick(ARegisters Registers)
         {
             Registers.X0 = (ulong)Registers.CntpctEl0;
         }
 
-        private static void SvcConnectToNamedPort(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcConnectToNamedPort(ARegisters Registers)
         {
             long StackPtr = (long)Registers.X0;
             long NamePtr  = (long)Registers.X1;
@@ -58,23 +70,23 @@ namespace Ryujinx.OsHle.Svc
             Registers.X0 = (int)SvcResult.Success;
         }
 
-        private static void SvcSendSyncRequest(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcSendSyncRequest(ARegisters Registers)
         {
-            SendSyncRequest(Ns, Registers, Memory, false);
+            SendSyncRequest(Registers, false);
         }
 
-        private static void SvcSendSyncRequestWithUserBuffer(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcSendSyncRequestWithUserBuffer(ARegisters Registers)
         {
-            SendSyncRequest(Ns, Registers, Memory, true);
+            SendSyncRequest(Registers, true);
         }
 
-        private static void SendSyncRequest(Switch Ns, ARegisters Registers, AMemory Memory, bool IsUser)
+        private void SendSyncRequest(ARegisters Registers, bool UserBuffer)
         {
             long CmdPtr = Registers.Tpidr;
             long Size   = 0x100;
             int  Handle = 0;
 
-            if (IsUser)
+            if (UserBuffer)
             {
                 CmdPtr = (long)Registers.X0;
                 Size   = (long)Registers.X1;
@@ -105,16 +117,16 @@ namespace Ryujinx.OsHle.Svc
             }
         }
 
-        private static void SvcBreak(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcBreak(ARegisters Registers)
         {
             long Reason  = (long)Registers.X0;
             long Unknown = (long)Registers.X1;
             long Info    = (long)Registers.X2;
 
-            throw new Exception($"SvcBreak: {Reason} {Unknown} {Info}");
+            throw new GuestBrokeExecutionException();
         }
 
-        private static void SvcOutputDebugString(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcOutputDebugString(ARegisters Registers)
         {
             long Position = (long)Registers.X0;
             long Size     = (long)Registers.X1;
@@ -126,7 +138,7 @@ namespace Ryujinx.OsHle.Svc
             Registers.X0 = (int)SvcResult.Success;
         }
 
-        private static void SvcGetInfo(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcGetInfo(ARegisters Registers)
         {
             long StackPtr = (long)Registers.X0;
             int  InfoType =  (int)Registers.X1;
@@ -148,8 +160,8 @@ namespace Ryujinx.OsHle.Svc
                 case 3:  Registers.X1 = GetMapRegionSize();      break;
                 case 4:  Registers.X1 = GetHeapRegionBaseAddr(); break;
                 case 5:  Registers.X1 = GetHeapRegionSize();     break;
-                case 6:  Registers.X1 = GetTotalMem(Memory);     break;
-                case 7:  Registers.X1 = GetUsedMem(Memory);      break;
+                case 6:  Registers.X1 = GetTotalMem();           break;
+                case 7:  Registers.X1 = GetUsedMem();            break;
                 case 11: Registers.X1 = GetRnd64();              break;
                 case 12: Registers.X1 = GetAddrSpaceBaseAddr();  break;
                 case 13: Registers.X1 = GetAddrSpaceSize();      break;
@@ -162,47 +174,47 @@ namespace Ryujinx.OsHle.Svc
             Registers.X0 = (int)SvcResult.Success;
         }
 
-        private static ulong GetTotalMem(AMemory Memory)
+        private ulong GetTotalMem()
         {
             return (ulong)Memory.Manager.GetTotalMemorySize();
         }
 
-        private static ulong GetUsedMem(AMemory Memory)
+        private ulong GetUsedMem()
         {
             return (ulong)Memory.Manager.GetUsedMemorySize();
         }
 
-        private static ulong GetRnd64()
+        private ulong GetRnd64()
         {
             return (ulong)Rng.Next() + ((ulong)Rng.Next() << 32);
         }
 
-        private static ulong GetAddrSpaceBaseAddr()
+        private ulong GetAddrSpaceBaseAddr()
         {
             return 0x08000000;
         }
 
-        private static ulong GetAddrSpaceSize()
+        private ulong GetAddrSpaceSize()
         {
             return AMemoryMgr.AddrSize - GetAddrSpaceBaseAddr();
         }
 
-        private static ulong GetMapRegionBaseAddr()
+        private ulong GetMapRegionBaseAddr()
         {
             return 0x80000000;
         }
 
-        private static ulong GetMapRegionSize()
+        private ulong GetMapRegionSize()
         {
             return 0x40000000;
         }
 
-        private static ulong GetHeapRegionBaseAddr()
+        private ulong GetHeapRegionBaseAddr()
         {
             return GetMapRegionBaseAddr() + GetMapRegionSize();
         }
 
-        private static ulong GetHeapRegionSize()
+        private ulong GetHeapRegionSize()
         {
             return 0x40000000;
         }
diff --git a/Ryujinx/OsHle/Svc/SvcThread.cs b/Ryujinx/OsHle/Svc/SvcThread.cs
index 895cb247..ac1ba745 100644
--- a/Ryujinx/OsHle/Svc/SvcThread.cs
+++ b/Ryujinx/OsHle/Svc/SvcThread.cs
@@ -1,4 +1,3 @@
-using ChocolArm64.Memory;
 using ChocolArm64.State;
 using Ryujinx.OsHle.Handles;
 using System.Threading;
@@ -7,7 +6,7 @@ namespace Ryujinx.OsHle.Svc
 {
     partial class SvcHandler
     {
-        private static void SvcCreateThread(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcCreateThread(ARegisters Registers)
         {
             long EntryPoint  = (long)Registers.X1;
             long ArgsPtr     = (long)Registers.X2;
@@ -17,12 +16,17 @@ namespace Ryujinx.OsHle.Svc
 
             if (Ns.Os.TryGetProcess(Registers.ProcessId, out Process Process))
             {
+                if (ProcessorId == -2)
+                {
+                    ProcessorId = 0;
+                }
+
                 int Handle = Process.MakeThread(
                     EntryPoint,
                     StackTop,
                     ArgsPtr,
                     Priority,
-                    ProcessorId);                
+                    ProcessorId);
 
                 Registers.X0 = (int)SvcResult.Success;
                 Registers.X1 = (ulong)Handle;
@@ -31,15 +35,15 @@ namespace Ryujinx.OsHle.Svc
             //TODO: Error codes.
         }
 
-        private static void SvcStartThread(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcStartThread(ARegisters Registers)
         {
             int Handle = (int)Registers.X0;
 
-            HThread HndData = Ns.Os.Handles.GetData<HThread>(Handle);
+            HThread Thread = Ns.Os.Handles.GetData<HThread>(Handle);
 
-            if (HndData != null)
+            if (Thread != null)
             {
-                HndData.Thread.Execute();
+                Process.Scheduler.StartThread(Thread);
 
                 Registers.X0 = (int)SvcResult.Success;
             }
@@ -47,29 +51,31 @@ namespace Ryujinx.OsHle.Svc
             //TODO: Error codes.
         }
 
-        private static void SvcSleepThread(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcSleepThread(ARegisters Registers)
         {           
             ulong NanoSecs = Registers.X0;
 
-            if (NanoSecs == 0)
+            if (Process.TryGetThread(Registers.Tpidr, out HThread CurrThread))
             {
-                Thread.Yield();
+                Process.Scheduler.Yield(CurrThread);
             }
             else
             {
-                Thread.Sleep((int)(NanoSecs / 1000000));
+                Logging.Error($"Thread with TPIDR_EL0 0x{Registers.Tpidr:x16} not found!");
             }
+
+            Thread.Sleep((int)(NanoSecs / 1000000));
         }
 
-        private static void SvcGetThreadPriority(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcGetThreadPriority(ARegisters Registers)
         {
             int Handle = (int)Registers.X1;
 
-            HThread HndData = Ns.Os.Handles.GetData<HThread>(Handle);
+            HThread Thread = Ns.Os.Handles.GetData<HThread>(Handle);
 
-            if (HndData != null)
+            if (Thread != null)
             {
-                Registers.X1 = (ulong)HndData.Thread.Priority;
+                Registers.X1 = (ulong)Thread.Priority;
                 Registers.X0 = (int)SvcResult.Success;
             }
 
diff --git a/Ryujinx/OsHle/Svc/SvcThreadSync.cs b/Ryujinx/OsHle/Svc/SvcThreadSync.cs
index 1df75625..96681df1 100644
--- a/Ryujinx/OsHle/Svc/SvcThreadSync.cs
+++ b/Ryujinx/OsHle/Svc/SvcThreadSync.cs
@@ -1,5 +1,4 @@
 using ChocolArm64;
-using ChocolArm64.Memory;
 using ChocolArm64.State;
 using Ryujinx.OsHle.Handles;
 
@@ -7,27 +6,24 @@ namespace Ryujinx.OsHle.Svc
 {
     partial class SvcHandler
     {
-        private static void SvcArbitrateLock(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcArbitrateLock(ARegisters Registers)
         {
             int  OwnerThreadHandle      =  (int)Registers.X0;
             long MutexAddress           = (long)Registers.X1;
             int  RequestingThreadHandle =  (int)Registers.X2;
 
-            AThread RequestingThread = Ns.Os.Handles.GetData<HThread>(RequestingThreadHandle).Thread;
+            HThread RequestingThread = Ns.Os.Handles.GetData<HThread>(RequestingThreadHandle);
 
-            Mutex M = new Mutex(Memory, MutexAddress);
+            Mutex M = new Mutex(Process, MutexAddress, OwnerThreadHandle);
 
             M = Ns.Os.Mutexes.GetOrAdd(MutexAddress, M);
 
-            //FIXME
-            //M.WaitForLock(RequestingThread, RequestingThreadHandle);
-
-            Memory.WriteInt32(MutexAddress, 0);
+            M.WaitForLock(RequestingThread, RequestingThreadHandle);
 
             Registers.X0 = (int)SvcResult.Success;
         }
 
-        private static void SvcArbitrateUnlock(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcArbitrateUnlock(ARegisters Registers)
         {
             long MutexAddress = (long)Registers.X0;
 
@@ -39,39 +35,36 @@ namespace Ryujinx.OsHle.Svc
             Registers.X0 = (int)SvcResult.Success;
         }
 
-        private static void SvcWaitProcessWideKeyAtomic(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcWaitProcessWideKeyAtomic(ARegisters Registers)
         {
             long MutexAddress   = (long)Registers.X0;
             long CondVarAddress = (long)Registers.X1;
             int  ThreadHandle   =  (int)Registers.X2;
             long Timeout        = (long)Registers.X3;
 
-            AThread Thread = Ns.Os.Handles.GetData<HThread>(ThreadHandle).Thread;
+            HThread Thread = Ns.Os.Handles.GetData<HThread>(ThreadHandle);
 
             if (Ns.Os.Mutexes.TryGetValue(MutexAddress, out Mutex M))
             {
                 M.GiveUpLock(ThreadHandle);
             }
 
-            CondVar Signal = new CondVar(Memory, CondVarAddress, Timeout);
+            CondVar Cv = new CondVar(Process, CondVarAddress, Timeout);
 
-            Signal = Ns.Os.CondVars.GetOrAdd(CondVarAddress, Signal);
+            Cv = Ns.Os.CondVars.GetOrAdd(CondVarAddress, Cv);
 
-            Signal.WaitForSignal(ThreadHandle);
+            Cv.WaitForSignal(Thread);
 
-            M = new Mutex(Memory, MutexAddress);
+            M = new Mutex(Process, MutexAddress, ThreadHandle);
 
             M = Ns.Os.Mutexes.GetOrAdd(MutexAddress, M);
 
-            //FIXME
-            //M.WaitForLock(Thread, ThreadHandle);
-
-            Memory.WriteInt32(MutexAddress, 0);
+            M.WaitForLock(Thread, ThreadHandle);
 
             Registers.X0 = (int)SvcResult.Success;
         }
 
-        private static void SvcSignalProcessWideKey(Switch Ns, ARegisters Registers, AMemory Memory)
+        private void SvcSignalProcessWideKey(ARegisters Registers)
         {
             long CondVarAddress = (long)Registers.X0;
             int  Count          =  (int)Registers.X1;