Some improvements to SetThreadCoreMask, simplified implementation of wait lists

2 files changed, 124 insertions, 152 deletions

diff --git a/Ryujinx.Core/OsHle/Kernel/SvcThread.cs b/Ryujinx.Core/OsHle/Kernel/SvcThread.cs
index 94912f53..597e42a6 100644
--- a/Ryujinx.Core/OsHle/Kernel/SvcThread.cs
+++ b/Ryujinx.Core/OsHle/Kernel/SvcThread.cs
@@ -146,11 +146,16 @@ namespace Ryujinx.Core.OsHle.Kernel
             int  IdealCore =  (int)ThreadState.X1;
             long CoreMask  = (long)ThreadState.X2;
 
+            Ns.Log.PrintDebug(LogClass.KernelSvc,
+                "Handle = "    + Handle   .ToString("x8") + ", " +
+                "IdealCore = " + IdealCore.ToString("x8") + ", " +
+                "CoreMask = "  + CoreMask .ToString("x16"));
+
             KThread Thread = GetThread(ThreadState.Tpidr, Handle);
 
             if (IdealCore == -2)
             {
-                //TODO: Get this value from the NPDM file.
+                //TODO: Get this valcdue from the NPDM file.
                 IdealCore = 0;
 
                 CoreMask = 1 << IdealCore;
@@ -159,14 +164,16 @@ namespace Ryujinx.Core.OsHle.Kernel
             {
                 if ((uint)IdealCore > 3)
                 {
-                    Ns.Log.PrintWarning(LogClass.KernelSvc, $"Invalid core id 0x{IdealCore:x8}!");
+                    if ((IdealCore | 2) != -1)
+                    {
+                        Ns.Log.PrintWarning(LogClass.KernelSvc, $"Invalid core id 0x{IdealCore:x8}!");
 
-                    ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidCoreId);
+                        ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidCoreId);
 
-                    return;
+                        return;
+                    }
                 }
-
-                if ((CoreMask & (1 << IdealCore)) == 0)
+                else if ((CoreMask & (1 << IdealCore)) == 0)
                 {
                     Ns.Log.PrintWarning(LogClass.KernelSvc, $"Invalid core mask 0x{CoreMask:x8}!");
 
@@ -185,27 +192,32 @@ namespace Ryujinx.Core.OsHle.Kernel
                 return;
             }
 
-            if (IdealCore == -3)
+            //-1 is used as "don't care", so the IdealCore value is ignored.
+            //-2 is used as "use NPDM default core id" (handled above).
+            //-3 is used as "don't update", the old IdealCore value is kept.
+            if (IdealCore != -3)
             {
-                if ((CoreMask & (1 << Thread.IdealCore)) == 0)
-                {
-                    Ns.Log.PrintWarning(LogClass.KernelSvc, $"Invalid core mask 0x{CoreMask:x8}!");
-
-                    ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidCoreMask);
-
-                    return;
-                }
+                Thread.IdealCore = IdealCore;
             }
-            else
+            else if ((CoreMask & (1 << Thread.IdealCore)) == 0)
             {
-                Thread.IdealCore = IdealCore;
+                Ns.Log.PrintWarning(LogClass.KernelSvc, $"Invalid core mask 0x{CoreMask:x8}!");
+
+                ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidCoreMask);
+
+                return;
             }
 
             Thread.CoreMask = (int)CoreMask;
 
             KThread CurrThread = Process.GetThread(ThreadState.Tpidr);
 
+            //Try yielding execution, for the case where the new
+            //core mask allows the thread to run on the current core.
             Process.Scheduler.Yield(CurrThread);
+
+            //Try running the modified thread, for the case where one
+            //of the cores specified on the core mask is free.
             Process.Scheduler.TryRunning(Thread);
 
             ThreadState.X0 = 0;
diff --git a/Ryujinx.Core/OsHle/Kernel/SvcThreadSync.cs b/Ryujinx.Core/OsHle/Kernel/SvcThreadSync.cs
index 0ca2a5f9..280065ec 100644
--- a/Ryujinx.Core/OsHle/Kernel/SvcThreadSync.cs
+++ b/Ryujinx.Core/OsHle/Kernel/SvcThreadSync.cs
@@ -2,6 +2,8 @@ using ChocolArm64.State;
 using Ryujinx.Core.Logging;
 using Ryujinx.Core.OsHle.Handles;
 using System;
+using System.Collections.Generic;
+using System.Linq;
 using System.Threading;
 
 using static Ryujinx.Core.OsHle.ErrorCode;
@@ -110,10 +112,10 @@ namespace Ryujinx.Core.OsHle.Kernel
             ulong Timeout        =       ThreadState.X3;
 
             Ns.Log.PrintDebug(LogClass.KernelSvc,
-                "OwnerThreadHandle = " + MutexAddress  .ToString("x16") + ", " +
-                "MutexAddress = "      + CondVarAddress.ToString("x16") + ", " +
-                "WaitThreadHandle = "  + ThreadHandle  .ToString("x8")  + ", " +
-                "Timeout = "           + Timeout       .ToString("x16"));
+                "MutexAddress = "   + MutexAddress  .ToString("x16") + ", " +
+                "CondVarAddress = " + CondVarAddress.ToString("x16") + ", " +
+                "ThreadHandle = "   + ThreadHandle  .ToString("x8")  + ", " +
+                "Timeout = "        + Timeout       .ToString("x16"));
 
             if (IsPointingInsideKernel(MutexAddress))
             {
@@ -181,15 +183,18 @@ namespace Ryujinx.Core.OsHle.Kernel
 
             Ns.Log.PrintDebug(LogClass.KernelSvc, "MutexValue = " + MutexValue.ToString("x8"));
 
-            if (MutexValue != (OwnerThreadHandle | MutexHasListenersMask))
+            lock (Process.ThreadSyncLock)
             {
-                return;
-            }
+                if (MutexValue != (OwnerThreadHandle | MutexHasListenersMask))
+                {
+                    return;
+                }
 
-            CurrThread.WaitHandle   = WaitThreadHandle;
-            CurrThread.MutexAddress = MutexAddress;
+                CurrThread.WaitHandle   = WaitThreadHandle;
+                CurrThread.MutexAddress = MutexAddress;
 
-            InsertWaitingMutexThread(OwnerThreadHandle, WaitThread);
+                InsertWaitingMutexThread(OwnerThreadHandle, WaitThread);
+            }
 
             Ns.Log.PrintDebug(LogClass.KernelSvc, "Entering wait state...");
 
@@ -205,26 +210,22 @@ namespace Ryujinx.Core.OsHle.Kernel
                 return false;
             }
 
-            lock (CurrThread)
+            lock (Process.ThreadSyncLock)
             {
                 //This is the new thread that will not own the mutex.
                 //If no threads are waiting for the lock, then it should be null.
-                KThread OwnerThread = CurrThread.NextMutexThread;
+                KThread OwnerThread = GetHighestPriority(CurrThread.MutexWaiters, MutexAddress);
 
-                while (OwnerThread != null && OwnerThread.MutexAddress != MutexAddress)
+                if (OwnerThread != null)
                 {
-                    OwnerThread = OwnerThread.NextMutexThread;
-                }
-
-                UpdateMutexOwner(CurrThread, OwnerThread, MutexAddress);
-
-                CurrThread.NextMutexThread = null;
+                    //Remove all waiting mutex from the old owner,
+                    //and insert then on the new owner.
+                    UpdateMutexOwner(CurrThread, OwnerThread, MutexAddress);
 
-                CurrThread.UpdatePriority();
+                    CurrThread.UpdatePriority();
+                    OwnerThread.UpdatePriority();
 
-                if (OwnerThread != null)
-                {
-                    int HasListeners = OwnerThread.NextMutexThread != null ? MutexHasListenersMask : 0;
+                    int HasListeners = OwnerThread.MutexWaiters.Count > 0 ? MutexHasListenersMask : 0;
 
                     Process.Memory.WriteInt32(MutexAddress, HasListeners | OwnerThread.WaitHandle);
 
@@ -238,12 +239,16 @@ namespace Ryujinx.Core.OsHle.Kernel
 
                     Process.Scheduler.WakeUp(OwnerThread);
 
+                    Ns.Log.PrintDebug(LogClass.KernelSvc, "Gave mutex to thread id " + OwnerThread.ThreadId + "!");
+
                     return true;
                 }
                 else
                 {
                     Process.Memory.WriteInt32(MutexAddress, 0);
 
+                    Ns.Log.PrintDebug(LogClass.KernelSvc, "No threads waiting mutex!");
+
                     return false;
                 }
             }
@@ -262,43 +267,7 @@ namespace Ryujinx.Core.OsHle.Kernel
 
             lock (Process.ThreadArbiterListLock)
             {
-                KThread CurrThread = Process.ThreadArbiterListHead;
-
-                if (CurrThread == null || CurrThread.ActualPriority > WaitThread.ActualPriority)
-                {
-                    WaitThread.NextCondVarThread = Process.ThreadArbiterListHead;
-
-                    Process.ThreadArbiterListHead = WaitThread;
-                }
-                else
-                {
-                    bool DoInsert = CurrThread != WaitThread;
-
-                    while (CurrThread.NextCondVarThread != null)
-                    {
-                        if (CurrThread.NextCondVarThread.ActualPriority > WaitThread.ActualPriority)
-                        {
-                            break;
-                        }
-
-                        CurrThread = CurrThread.NextCondVarThread;
-
-                        DoInsert &= CurrThread != WaitThread;
-                    }
-
-                    //Only insert if the node doesn't already exist in the list.
-                    //This prevents circular references.
-                    if (DoInsert)
-                    {
-                        if (WaitThread.NextCondVarThread != null)
-                        {
-                            throw new InvalidOperationException();
-                        }
-
-                        WaitThread.NextCondVarThread = CurrThread.NextCondVarThread;
-                        CurrThread.NextCondVarThread = WaitThread;
-                    }
-                }
+                Process.ThreadArbiterList.Add(WaitThread);
             }
 
             Ns.Log.PrintDebug(LogClass.KernelSvc, "Entering wait state...");
@@ -319,62 +288,44 @@ namespace Ryujinx.Core.OsHle.Kernel
         {
             lock (Process.ThreadArbiterListLock)
             {
-                KThread PrevThread = null;
-                KThread CurrThread = Process.ThreadArbiterListHead;
+                KThread CurrThread = GetHighestPriority(CondVarAddress);
 
-                while (CurrThread != null && (Count == -1 || Count > 0))
+                while (CurrThread != null && (Count == -1 || Count-- > 0))
                 {
-                    if (CurrThread.CondVarAddress == CondVarAddress)
-                    {
-                        if (PrevThread != null)
-                        {
-                            PrevThread.NextCondVarThread = CurrThread.NextCondVarThread;
-                        }
-                        else
-                        {
-                            Process.ThreadArbiterListHead = CurrThread.NextCondVarThread;
-                        }
-
-                        CurrThread.NextCondVarThread = null;
-
-                        AcquireMutexValue(CurrThread.MutexAddress);
-
-                        int MutexValue = Process.Memory.ReadInt32(CurrThread.MutexAddress);
-
-                        if (MutexValue == 0)
-                        {
-                            //Give the lock to this thread.
-                            Process.Memory.WriteInt32(CurrThread.MutexAddress, CurrThread.WaitHandle);
+                    AcquireMutexValue(CurrThread.MutexAddress);
 
-                            CurrThread.WaitHandle     = 0;
-                            CurrThread.MutexAddress   = 0;
-                            CurrThread.CondVarAddress = 0;
+                    int MutexValue = Process.Memory.ReadInt32(CurrThread.MutexAddress);
 
-                            CurrThread.MutexOwner?.UpdatePriority();
+                    if (MutexValue == 0)
+                    {
+                        //Give the lock to this thread.
+                        Process.Memory.WriteInt32(CurrThread.MutexAddress, CurrThread.WaitHandle);
 
-                            CurrThread.MutexOwner = null;
+                        CurrThread.WaitHandle     = 0;
+                        CurrThread.MutexAddress   = 0;
+                        CurrThread.CondVarAddress = 0;
 
-                            Process.Scheduler.WakeUp(CurrThread);
-                        }
-                        else
-                        {
-                            //Wait until the lock is released.
-                            MutexValue &= ~MutexHasListenersMask;
+                        CurrThread.MutexOwner?.UpdatePriority();
 
-                            InsertWaitingMutexThread(MutexValue, CurrThread);
+                        CurrThread.MutexOwner = null;
 
-                            MutexValue |= MutexHasListenersMask;
+                        Process.Scheduler.WakeUp(CurrThread);
+                    }
+                    else
+                    {
+                        //Wait until the lock is released.
+                        MutexValue &= ~MutexHasListenersMask;
 
-                            Process.Memory.WriteInt32(CurrThread.MutexAddress, MutexValue);
-                        }
+                        InsertWaitingMutexThread(MutexValue, CurrThread);
 
-                        ReleaseMutexValue(CurrThread.MutexAddress);
+                        MutexValue |= MutexHasListenersMask;
 
-                        Count--;
+                        Process.Memory.WriteInt32(CurrThread.MutexAddress, MutexValue);
                     }
 
-                    PrevThread = CurrThread;
-                    CurrThread = CurrThread.NextCondVarThread;
+                    ReleaseMutexValue(CurrThread.MutexAddress);
+
+                    CurrThread = GetHighestPriority(CondVarAddress);
                 }
             }
         }
@@ -390,57 +341,66 @@ namespace Ryujinx.Core.OsHle.Kernel
                 return;
             }
 
-            WaitThread.MutexOwner = OwnerThread;
+            InsertWaitingMutexThread(OwnerThread, WaitThread);
+        }
 
-            lock (OwnerThread)
+        private void InsertWaitingMutexThread(KThread OwnerThread, KThread WaitThread)
+        {
+            lock (Process.ThreadSyncLock)
             {
-                KThread CurrThread = OwnerThread;
+                WaitThread.MutexOwner = OwnerThread;
 
-                while (CurrThread.NextMutexThread != null)
+                if (!OwnerThread.MutexWaiters.Contains(WaitThread))
                 {
-                    if (CurrThread == WaitThread)
-                    {
-                        return;
-                    }
-
-                    if (CurrThread.NextMutexThread.ActualPriority > WaitThread.ActualPriority)
-                    {
-                        break;
-                    }
+                    OwnerThread.MutexWaiters.Add(WaitThread);
 
-                    CurrThread = CurrThread.NextMutexThread;
+                    OwnerThread.UpdatePriority();
                 }
+            }
+        }
 
-                if (CurrThread != WaitThread)
+        private void UpdateMutexOwner(KThread CurrThread, KThread NewOwner, long MutexAddress)
+        {
+            //Go through all threads waiting for the mutex,
+            //and update the MutexOwner field to point to the new owner.
+            lock (Process.ThreadSyncLock)
+            {
+                for (int Index = 0; Index < CurrThread.MutexWaiters.Count; Index++)
                 {
-                    if (WaitThread.NextMutexThread != null)
+                    KThread Thread = CurrThread.MutexWaiters[Index];
+
+                    if (Thread.MutexAddress == MutexAddress)
                     {
-                        throw new InvalidOperationException();
-                    }
+                        CurrThread.MutexWaiters.RemoveAt(Index--);
 
-                    WaitThread.NextMutexThread = CurrThread.NextMutexThread;
-                    CurrThread.NextMutexThread = WaitThread;
+                        Thread.MutexOwner = NewOwner;
+
+                        NewOwner.MutexWaiters.Add(Thread);
+                    }
                 }
             }
+        }
 
-            OwnerThread.UpdatePriority();
+        private KThread GetHighestPriority(List<KThread> Threads, long MutexAddress)
+        {
+            return GetHighestPriority(Threads, x => x.MutexAddress == MutexAddress);
         }
 
-        private void UpdateMutexOwner(KThread CurrThread, KThread NewOwner, long MutexAddress)
+        private KThread GetHighestPriority(long CondVarAddress)
         {
-            //Go through all threads waiting for the mutex,
-            //and update the MutexOwner field to point to the new owner.
-            CurrThread = CurrThread.NextMutexThread;
+            return GetHighestPriority(Process.ThreadArbiterList, x => x.CondVarAddress == CondVarAddress);
+        }
 
-            while (CurrThread != null)
-            {
-                if (CurrThread.MutexAddress == MutexAddress)
-                {
-                    CurrThread.MutexOwner = NewOwner;
-                }
+        private KThread GetHighestPriority(List<KThread> Threads, Func<KThread, bool> Predicate)
+        {
+            KThread Thread = Threads.OrderBy(x => x.ActualPriority).FirstOrDefault(Predicate);
 
-                CurrThread = CurrThread.NextMutexThread;
+            if (Thread != null)
+            {
+                Threads.Remove(Thread);
             }
+
+            return Thread;
         }
 
         private void AcquireMutexValue(long MutexAddress)