3 files changed, 145 insertions, 46 deletions
diff --git a/Ryujinx.HLE/HOS/Kernel/Threading/KPriorityQueue.cs b/Ryujinx.HLE/HOS/Kernel/Threading/KPriorityQueue.cs
index 2c9d7574..14fba704 100644
--- a/Ryujinx.HLE/HOS/Kernel/Threading/KPriorityQueue.cs
+++ b/Ryujinx.HLE/HOS/Kernel/Threading/KPriorityQueue.cs
@@ -5,11 +5,11 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
 {
     class KPriorityQueue
     {
-        private LinkedList<KThread>[][] _scheduledThreadsPerPrioPerCore;
-        private LinkedList<KThread>[][] _suggestedThreadsPerPrioPerCore;
+        private readonly LinkedList<KThread>[][] _scheduledThreadsPerPrioPerCore;
+        private readonly LinkedList<KThread>[][] _suggestedThreadsPerPrioPerCore;
 
-        private long[] _scheduledPrioritiesPerCore;
-        private long[] _suggestedPrioritiesPerCore;
+        private readonly long[] _scheduledPrioritiesPerCore;
+        private readonly long[] _suggestedPrioritiesPerCore;
 
         public KPriorityQueue()
         {
@@ -32,41 +32,132 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
             _suggestedPrioritiesPerCore = new long[KScheduler.CpuCoresCount];
         }
 
-        public IEnumerable<KThread> SuggestedThreads(int core)
+        public readonly ref struct KThreadEnumerable
         {
-            return Iterate(_suggestedThreadsPerPrioPerCore, _suggestedPrioritiesPerCore, core);
+            readonly LinkedList<KThread>[][] _listPerPrioPerCore;
+            readonly long[] _prios;
+            readonly int _core;
+
+            public KThreadEnumerable(LinkedList<KThread>[][] listPerPrioPerCore, long[] prios, int core)
+            {
+                _listPerPrioPerCore = listPerPrioPerCore;
+                _prios = prios;
+                _core = core;
+            }
+
+            public Enumerator GetEnumerator()
+            {
+                return new Enumerator(_listPerPrioPerCore, _prios, _core);
+            }
+
+            public ref struct Enumerator
+            {
+                private readonly LinkedList<KThread>[][] _listPerPrioPerCore;
+                private readonly int _core;
+                private long _prioMask;
+                private int _prio;
+                private LinkedList<KThread> _list;
+                private LinkedListNode<KThread> _node;
+
+                public Enumerator(LinkedList<KThread>[][] listPerPrioPerCore, long[] prios, int core)
+                {
+                    _listPerPrioPerCore = listPerPrioPerCore;
+                    _core = core;
+                    _prioMask = prios[core];
+                    _prio = BitOperations.TrailingZeroCount(_prioMask);
+                    _prioMask &= ~(1L << _prio);
+                }
+
+                public KThread Current => _node?.Value;
+
+                public bool MoveNext()
+                {
+                    _node = _node?.Next;
+
+                    if (_node == null)
+                    {
+                        if (!MoveNextListAndFirstNode())
+                        {
+                            return false;
+                        }
+                    }
+
+                    return _node != null;
+                }
+
+                private bool MoveNextListAndFirstNode()
+                {
+                    if (_prio < KScheduler.PrioritiesCount)
+                    {
+                        _list = _listPerPrioPerCore[_prio][_core];
+
+                        _node = _list.First;
+
+                        _prio = BitOperations.TrailingZeroCount(_prioMask);
+
+                        _prioMask &= ~(1L << _prio);
+
+                        return true;
+                    }
+                    else
+                    {
+                        _list = null;
+                        _node = null;
+                        return false;
+                    }
+                }
+            }
         }
 
-        public IEnumerable<KThread> ScheduledThreads(int core)
+        public KThreadEnumerable ScheduledThreads(int core)
         {
-            return Iterate(_scheduledThreadsPerPrioPerCore, _scheduledPrioritiesPerCore, core);
+            return new KThreadEnumerable(_scheduledThreadsPerPrioPerCore, _scheduledPrioritiesPerCore, core);
         }
 
-        private IEnumerable<KThread> Iterate(LinkedList<KThread>[][] listPerPrioPerCore, long[] prios, int core)
+        public KThreadEnumerable SuggestedThreads(int core)
         {
-            long prioMask = prios[core];
-
-            int prio = BitOperations.TrailingZeroCount(prioMask);
+            return new KThreadEnumerable(_suggestedThreadsPerPrioPerCore, _suggestedPrioritiesPerCore, core);
+        }
 
-            prioMask &= ~(1L << prio);
+        public KThread ScheduledThreadsFirstOrDefault(int core)
+        {
+            return ScheduledThreadsElementAtOrDefault(core, 0);
+        }
 
-            while (prio < KScheduler.PrioritiesCount)
+        public KThread ScheduledThreadsElementAtOrDefault(int core, int index)
+        {
+            int currentIndex = 0;
+            foreach (var scheduledThread in ScheduledThreads(core))
             {
-                LinkedList<KThread> list = listPerPrioPerCore[prio][core];
+                if (currentIndex == index)
+                {
+                    return scheduledThread;
+                }
+                else
+                {
+                    currentIndex++;
+                }
+            }
 
-                LinkedListNode<KThread> node = list.First;
+            return null;
+        }
 
-                while (node != null)
+        public KThread ScheduledThreadsWithDynamicPriorityFirstOrDefault(int core, int dynamicPriority)
+        {
+            foreach (var scheduledThread in ScheduledThreads(core))
+            {
+                if (scheduledThread.DynamicPriority == dynamicPriority)
                 {
-                    yield return node.Value;
-
-                    node = node.Next;
+                    return scheduledThread;
                 }
+            }
 
-                prio = BitOperations.TrailingZeroCount(prioMask);
+            return null;
+        }
 
-                prioMask &= ~(1L << prio);
-            }
+        public bool HasScheduledThreads(int core)
+        {
+            return ScheduledThreadsFirstOrDefault(core) != null;
         }
 
         public void TransferToCore(int prio, int dstCore, KThread thread)
diff --git a/Ryujinx.HLE/HOS/Kernel/Threading/KScheduler.cs b/Ryujinx.HLE/HOS/Kernel/Threading/KScheduler.cs
index 0c51b7b9..b9de7d9c 100644
--- a/Ryujinx.HLE/HOS/Kernel/Threading/KScheduler.cs
+++ b/Ryujinx.HLE/HOS/Kernel/Threading/KScheduler.cs
@@ -1,8 +1,6 @@
 using Ryujinx.Common;
 using Ryujinx.HLE.HOS.Kernel.Process;
 using System;
-using System.Collections.Generic;
-using System.Linq;
 using System.Numerics;
 using System.Threading;
 
@@ -17,6 +15,8 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
 
         private static readonly int[] PreemptionPriorities = new int[] { 59, 59, 59, 63 };
 
+        private static readonly int[] _srcCoresHighestPrioThreads = new int[CpuCoresCount];
+
         private readonly KernelContext _context;
         private readonly int _coreId;
 
@@ -86,7 +86,7 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
 
             for (int core = 0; core < CpuCoresCount; core++)
             {
-                KThread thread = context.PriorityQueue.ScheduledThreads(core).FirstOrDefault();
+                KThread thread = context.PriorityQueue.ScheduledThreadsFirstOrDefault(core);
 
                 if (thread != null &&
                     thread.Owner != null &&
@@ -115,12 +115,12 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
             {
                 // If the core is not idle (there's already a thread running on it),
                 // then we don't need to attempt load balancing.
-                if (context.PriorityQueue.ScheduledThreads(core).Any())
+                if (context.PriorityQueue.HasScheduledThreads(core))
                 {
                     continue;
                 }
 
-                int[] srcCoresHighestPrioThreads = new int[CpuCoresCount];
+                Array.Fill(_srcCoresHighestPrioThreads, 0);
 
                 int srcCoresHighestPrioThreadsCount = 0;
 
@@ -136,7 +136,7 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
                         break;
                     }
 
-                    srcCoresHighestPrioThreads[srcCoresHighestPrioThreadsCount++] = suggested.ActiveCore;
+                    _srcCoresHighestPrioThreads[srcCoresHighestPrioThreadsCount++] = suggested.ActiveCore;
                 }
 
                 // Not yet selected candidate found.
@@ -158,9 +158,9 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
                 // (the first one that doesn't make the source core idle if moved).
                 for (int index = 0; index < srcCoresHighestPrioThreadsCount; index++)
                 {
-                    int srcCore = srcCoresHighestPrioThreads[index];
+                    int srcCore = _srcCoresHighestPrioThreads[index];
 
-                    KThread src = context.PriorityQueue.ScheduledThreads(srcCore).ElementAtOrDefault(1);
+                    KThread src = context.PriorityQueue.ScheduledThreadsElementAtOrDefault(srcCore, 1);
 
                     if (src != null)
                     {
@@ -422,9 +422,7 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
 
         private static void RotateScheduledQueue(KernelContext context, int core, int prio)
         {
-            IEnumerable<KThread> scheduledThreads = context.PriorityQueue.ScheduledThreads(core);
-
-            KThread selectedThread = scheduledThreads.FirstOrDefault(x => x.DynamicPriority == prio);
+            KThread selectedThread = context.PriorityQueue.ScheduledThreadsWithDynamicPriorityFirstOrDefault(core, prio);
             KThread nextThread = null;
 
             // Yield priority queue.
@@ -433,14 +431,14 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
                 nextThread = context.PriorityQueue.Reschedule(prio, core, selectedThread);
             }
 
-            IEnumerable<KThread> SuitableCandidates()
+            static KThread FirstSuitableCandidateOrDefault(KernelContext context, int core, KThread selectedThread, KThread nextThread, Predicate< KThread> predicate)
             {
                 foreach (KThread suggested in context.PriorityQueue.SuggestedThreads(core))
                 {
                     int suggestedCore = suggested.ActiveCore;
                     if (suggestedCore >= 0)
                     {
-                        KThread selectedSuggestedCore = context.PriorityQueue.ScheduledThreads(suggestedCore).FirstOrDefault();
+                        KThread selectedSuggestedCore = context.PriorityQueue.ScheduledThreadsFirstOrDefault(suggestedCore);
 
                         if (selectedSuggestedCore == suggested || (selectedSuggestedCore != null && selectedSuggestedCore.DynamicPriority < 2))
                         {
@@ -453,14 +451,19 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
                         nextThread == null ||
                         nextThread.LastScheduledTime >= suggested.LastScheduledTime)
                     {
-                        yield return suggested;
+                        if (predicate(suggested))
+                        {
+                            return suggested;
+                        }
                     }
                 }
+
+                return null;
             }
 
             // 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);
+            KThread dst = FirstSuitableCandidateOrDefault(context, core, selectedThread, nextThread, x => x.DynamicPriority == prio);
 
             if (dst != null)
             {
@@ -469,11 +472,11 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
 
             // If the priority of the currently selected thread is lower or same as the preemption priority,
             // then try to migrate a thread with lower priority.
-            KThread bestCandidate = context.PriorityQueue.ScheduledThreads(core).FirstOrDefault();
+            KThread bestCandidate = context.PriorityQueue.ScheduledThreadsFirstOrDefault(core);
 
             if (bestCandidate != null && bestCandidate.DynamicPriority >= prio)
             {
-                dst = SuitableCandidates().FirstOrDefault(x => x.DynamicPriority < bestCandidate.DynamicPriority);
+                dst = FirstSuitableCandidateOrDefault(context, core, selectedThread, nextThread, x => x.DynamicPriority < bestCandidate.DynamicPriority);
 
                 if (dst != null)
                 {
@@ -534,7 +537,7 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
             // Move current thread to the end of the queue.
             KThread nextThread = context.PriorityQueue.Reschedule(prio, core, currentThread);
 
-            IEnumerable<KThread> SuitableCandidates()
+            static KThread FirstSuitableCandidateOrDefault(KernelContext context, int core, KThread nextThread, int lessThanOrEqualPriority)
             {
                 foreach (KThread suggested in context.PriorityQueue.SuggestedThreads(core))
                 {
@@ -554,12 +557,17 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
                     if (suggested.LastScheduledTime <= nextThread.LastScheduledTime ||
                         suggested.DynamicPriority < nextThread.DynamicPriority)
                     {
-                        yield return suggested;
+                        if (suggested.DynamicPriority <= lessThanOrEqualPriority)
+                        {
+                            return suggested;
+                        }
                     }
                 }
+
+                return null;
             }
 
-            KThread dst = SuitableCandidates().FirstOrDefault(x => x.DynamicPriority <= prio);
+            KThread dst = FirstSuitableCandidateOrDefault(context, core, nextThread, prio);
 
             if (dst != null)
             {
@@ -596,7 +604,7 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
 
             context.PriorityQueue.TransferToCore(currentThread.DynamicPriority, -1, currentThread);
 
-            if (!context.PriorityQueue.ScheduledThreads(core).Any())
+            if (!context.PriorityQueue.HasScheduledThreads(core))
             {
                 KThread selectedThread = null;
 
@@ -609,7 +617,7 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
                         continue;
                     }
 
-                    KThread firstCandidate = context.PriorityQueue.ScheduledThreads(suggestedCore).FirstOrDefault();
+                    KThread firstCandidate = context.PriorityQueue.ScheduledThreadsFirstOrDefault(suggestedCore);
 
                     if (firstCandidate == suggested)
                     {
diff --git a/Ryujinx.HLE/HOS/Kernel/Threading/KSynchronization.cs b/Ryujinx.HLE/HOS/Kernel/Threading/KSynchronization.cs
index 01b65f55..973d5f6a 100644
--- a/Ryujinx.HLE/HOS/Kernel/Threading/KSynchronization.cs
+++ b/Ryujinx.HLE/HOS/Kernel/Threading/KSynchronization.cs
@@ -59,7 +59,7 @@ namespace Ryujinx.HLE.HOS.Kernel.Threading
             }
             else
             {
-                LinkedListNode<KThread>[] syncNodes = new LinkedListNode<KThread>[syncObjs.Length];
+                LinkedListNode<KThread>[] syncNodes = syncObjs.Length == 0 ? Array.Empty<LinkedListNode<KThread>>() : new LinkedListNode<KThread>[syncObjs.Length];
 
                 for (int index = 0; index < syncObjs.Length; index++)
                 {