Thread scheduler rewrite (#393)

* Started to rewrite the thread scheduler

* Add a single core-like scheduling mode, enabled by default

* Clear exclusive monitor on context switch

* Add SetThreadActivity, misc fixes

* Implement WaitForAddress and SignalToAddress svcs, misc fixes

* Misc fixes (on SetActivity and Arbiter), other tweaks

* Rebased

* Add missing null check

* Rename multicore key on config, fix UpdatePriorityInheritance

* Make scheduling data MLQs private

* nit: Ordering

Ryujinx.HLE/HOS/Kernel/KScheduler.cs

@@ -0,0 +1,235 @@
+using System;
+using System.Collections.Generic;
+using System.Linq;
+using System.Threading;
+
+namespace Ryujinx.HLE.HOS.Kernel
+{
+    partial class KScheduler : IDisposable
+    {
+        public const int PrioritiesCount = 64;
+        public const int CpuCoresCount   = 4;
+
+        private const int PreemptionPriorityCores012 = 59;
+        private const int PreemptionPriorityCore3    = 63;
+
+        private Horizon System;
+
+        public KSchedulingData SchedulingData { get; private set; }
+
+        public KCoreContext[] CoreContexts { get; private set; }
+
+        public bool ThreadReselectionRequested { get; set; }
+
+        public KScheduler(Horizon System)
+        {
+            this.System = System;
+
+            SchedulingData = new KSchedulingData();
+
+            CoreManager = new HleCoreManager();
+
+            CoreContexts = new KCoreContext[CpuCoresCount];
+
+            for (int Core = 0; Core < CpuCoresCount; Core++)
+            {
+                CoreContexts[Core] = new KCoreContext(this, CoreManager);
+            }
+
+            Thread PreemptionThread = new Thread(PreemptCurrentThread);
+
+            KeepPreempting = true;
+
+            PreemptionThread.Start();
+        }
+
+        private void PreemptThreads()
+        {
+            System.CriticalSectionLock.Lock();
+
+            PreemptThread(PreemptionPriorityCores012, 0);
+            PreemptThread(PreemptionPriorityCores012, 1);
+            PreemptThread(PreemptionPriorityCores012, 2);
+            PreemptThread(PreemptionPriorityCore3,    3);
+
+            System.CriticalSectionLock.Unlock();
+        }
+
+        private void PreemptThread(int Prio, int Core)
+        {
+            IEnumerable<KThread> ScheduledThreads = SchedulingData.ScheduledThreads(Core);
+
+            KThread SelectedThread = ScheduledThreads.FirstOrDefault(x => x.DynamicPriority == Prio);
+
+            //Yield priority queue.
+            if (SelectedThread != null)
+            {
+                SchedulingData.Reschedule(Prio, Core, SelectedThread);
+            }
+
+            IEnumerable<KThread> SuitableCandidates()
+            {
+                foreach (KThread Thread in SchedulingData.SuggestedThreads(Core))
+                {
+                    int SrcCore = Thread.CurrentCore;
+
+                    if (SrcCore >= 0)
+                    {
+                        KThread HighestPrioSrcCore = SchedulingData.ScheduledThreads(SrcCore).FirstOrDefault();
+
+                        if (HighestPrioSrcCore != null && HighestPrioSrcCore.DynamicPriority < 2)
+                        {
+                            break;
+                        }
+
+                        if (HighestPrioSrcCore == Thread)
+                        {
+                            continue;
+                        }
+                    }
+
+                    //If the candidate was scheduled after the current thread, then it's not worth it.
+                    if (SelectedThread == null || SelectedThread.LastScheduledTicks >= Thread.LastScheduledTicks)
+                    {
+                        yield return Thread;
+                    }
+                }
+            }
+
+            //Select candidate threads that could run on this core.
+            //Only take into account threads that are not yet selected.
+            KThread Dst = SuitableCandidates().FirstOrDefault(x => x.DynamicPriority == Prio);
+
+            if (Dst != null)
+            {
+                SchedulingData.TransferToCore(Prio, Core, Dst);
+
+                SelectedThread = Dst;
+            }
+
+            //If the priority of the currently selected thread is lower than preemption priority,
+            //then allow threads with lower priorities to be selected aswell.
+            if (SelectedThread != null && SelectedThread.DynamicPriority > Prio)
+            {
+                Func<KThread, bool> Predicate = x => x.DynamicPriority >= SelectedThread.DynamicPriority;
+
+                Dst = SuitableCandidates().FirstOrDefault(Predicate);
+
+                if (Dst != null)
+                {
+                    SchedulingData.TransferToCore(Dst.DynamicPriority, Core, Dst);
+                }
+            }
+
+            ThreadReselectionRequested = true;
+        }
+
+        public void SelectThreads()
+        {
+            ThreadReselectionRequested = false;
+
+            for (int Core = 0; Core < CpuCoresCount; Core++)
+            {
+                KThread Thread = SchedulingData.ScheduledThreads(Core).FirstOrDefault();
+
+                CoreContexts[Core].SelectThread(Thread);
+            }
+
+            for (int Core = 0; Core < CpuCoresCount; Core++)
+            {
+                //If the core is not idle (there's already a thread running on it),
+                //then we don't need to attempt load balancing.
+                if (SchedulingData.ScheduledThreads(Core).Any())
+                {
+                    continue;
+                }
+
+                int[] SrcCoresHighestPrioThreads = new int[CpuCoresCount];
+
+                int SrcCoresHighestPrioThreadsCount = 0;
+
+                KThread Dst = null;
+
+                //Select candidate threads that could run on this core.
+                //Give preference to threads that are not yet selected.
+                foreach (KThread Thread in SchedulingData.SuggestedThreads(Core))
+                {
+                    if (Thread.CurrentCore < 0 || Thread != CoreContexts[Thread.CurrentCore].SelectedThread)
+                    {
+                        Dst = Thread;
+
+                        break;
+                    }
+
+                    SrcCoresHighestPrioThreads[SrcCoresHighestPrioThreadsCount++] = Thread.CurrentCore;
+                }
+
+                //Not yet selected candidate found.
+                if (Dst != null)
+                {
+                    //Priorities < 2 are used for the kernel message dispatching
+                    //threads, we should skip load balancing entirely.
+                    if (Dst.DynamicPriority >= 2)
+                    {
+                        SchedulingData.TransferToCore(Dst.DynamicPriority, Core, Dst);
+
+                        CoreContexts[Core].SelectThread(Dst);
+                    }
+
+                    continue;
+                }
+
+                //All candiates are already selected, choose the best one
+                //(the first one that doesn't make the source core idle if moved).
+                for (int Index = 0; Index < SrcCoresHighestPrioThreadsCount; Index++)
+                {
+                    int SrcCore = SrcCoresHighestPrioThreads[Index];
+
+                    KThread Src = SchedulingData.ScheduledThreads(SrcCore).ElementAtOrDefault(1);
+
+                    if (Src != null)
+                    {
+                        //Run the second thread on the queue on the source core,
+                        //move the first one to the current core.
+                        KThread OrigSelectedCoreSrc = CoreContexts[SrcCore].SelectedThread;
+
+                        CoreContexts[SrcCore].SelectThread(Src);
+
+                        SchedulingData.TransferToCore(OrigSelectedCoreSrc.DynamicPriority, Core, OrigSelectedCoreSrc);
+
+                        CoreContexts[Core].SelectThread(OrigSelectedCoreSrc);
+                    }
+                }
+            }
+        }
+
+        public KThread GetCurrentThread()
+        {
+            lock (CoreContexts)
+            {
+                for (int Core = 0; Core < CpuCoresCount; Core++)
+                {
+                    if (CoreContexts[Core].CurrentThread?.Context.IsCurrentThread() ?? false)
+                    {
+                        return CoreContexts[Core].CurrentThread;
+                    }
+                }
+            }
+
+            throw new InvalidOperationException("Current thread is not scheduled!");
+        }
+
+        public void Dispose()
+        {
+            Dispose(true);
+        }
+
+        protected virtual void Dispose(bool Disposing)
+        {
+            if (Disposing)
+            {
+                KeepPreempting = false;
+            }
+        }
+    }
+}