Implement SvcWaitForAddress 0x34 (#289)

* Implement SvcWaitForAddress 0x34

Currently needed by Sonic Mania Plus

* Fix mistake

* read-decrement-write locked

6 files changed, 171 insertions, 5 deletions

diff --git a/Ryujinx.HLE/OsHle/Handles/KThread.cs b/Ryujinx.HLE/OsHle/Handles/KThread.cs
index 3db46f3d..2b980d17 100644
--- a/Ryujinx.HLE/OsHle/Handles/KThread.cs
+++ b/Ryujinx.HLE/OsHle/Handles/KThread.cs
@@ -9,10 +9,12 @@ namespace Ryujinx.HLE.OsHle.Handles
 
         public int CoreMask { get; set; }
 
-        public long MutexAddress   { get; set; }
-        public long CondVarAddress { get; set; }
+        public long MutexAddress       { get; set; }
+        public long CondVarAddress     { get; set; }
+        public long ArbiterWaitAddress { get; set; }
 
         public bool CondVarSignaled { get; set; }
+        public bool ArbiterSignaled { get; set; }
 
         private Process Process;
 
diff --git a/Ryujinx.HLE/OsHle/Kernel/AddressArbiter.cs b/Ryujinx.HLE/OsHle/Kernel/AddressArbiter.cs
new file mode 100644
index 00000000..ce9ef0cd
--- /dev/null
+++ b/Ryujinx.HLE/OsHle/Kernel/AddressArbiter.cs
@@ -0,0 +1,112 @@
+using ChocolArm64.Memory;
+using ChocolArm64.State;
+using Ryujinx.HLE.OsHle.Handles;
+
+using static Ryujinx.HLE.OsHle.ErrorCode;
+
+namespace Ryujinx.HLE.OsHle.Kernel
+{
+    static class AddressArbiter
+    {
+        static ulong WaitForAddress(Process Process, AThreadState ThreadState, long Address, ulong Timeout)
+        {
+            KThread CurrentThread = Process.GetThread(ThreadState.Tpidr);
+
+            Process.Scheduler.SetReschedule(CurrentThread.ProcessorId);
+
+            CurrentThread.ArbiterWaitAddress = Address;
+            CurrentThread.ArbiterSignaled    = false;
+
+            Process.Scheduler.EnterWait(CurrentThread, NsTimeConverter.GetTimeMs(Timeout));
+
+            if (!CurrentThread.ArbiterSignaled)
+            {
+                return MakeError(ErrorModule.Kernel, KernelErr.Timeout);
+            }
+
+            return 0;
+        }
+
+        public static ulong WaitForAddressIfLessThan(Process      Process, 
+                                                     AThreadState ThreadState, 
+                                                     AMemory      Memory, 
+                                                     long         Address, 
+                                                     int          Value, 
+                                                     ulong        Timeout, 
+                                                     bool         ShouldDecrement)
+        {
+            Memory.SetExclusive(ThreadState, Address);
+
+            int CurrentValue = Memory.ReadInt32(Address);
+
+            while (true)
+            {
+                if (Memory.TestExclusive(ThreadState, Address))
+                {
+                    if (CurrentValue < Value)
+                    {
+                        if (ShouldDecrement)
+                        {
+                            Memory.WriteInt32(Address, CurrentValue - 1);
+                        }
+
+                        Memory.ClearExclusiveForStore(ThreadState);
+                    }
+                    else
+                    {
+                        Memory.ClearExclusiveForStore(ThreadState);
+
+                        return MakeError(ErrorModule.Kernel, KernelErr.InvalidState);
+                    }
+
+                    break;
+                }
+
+                Memory.SetExclusive(ThreadState, Address);
+
+                CurrentValue = Memory.ReadInt32(Address);
+            }
+
+            if (Timeout == 0)
+            {
+                return MakeError(ErrorModule.Kernel, KernelErr.Timeout);
+            }
+
+            return WaitForAddress(Process, ThreadState, Address, Timeout);
+        }
+
+        public static ulong WaitForAddressIfEqual(Process      Process, 
+                                                  AThreadState ThreadState, 
+                                                  AMemory      Memory, 
+                                                  long         Address, 
+                                                  int          Value, 
+                                                  ulong        Timeout)
+        {
+            if (Memory.ReadInt32(Address) != Value)
+            {
+                return MakeError(ErrorModule.Kernel, KernelErr.InvalidState);
+            }
+
+            if (Timeout == 0)
+            {
+                return MakeError(ErrorModule.Kernel, KernelErr.Timeout);
+            }
+
+            return WaitForAddress(Process, ThreadState, Address, Timeout);
+        }
+    }
+
+    enum ArbitrationType : int
+    {
+        WaitIfLessThan,
+        DecrementAndWaitIfLessThan,
+        WaitIfEqual
+    }
+
+    enum SignalType : int
+    {
+        Signal,
+        IncrementAndSignalIfEqual,
+        ModifyByWaitingCountAndSignalIfEqual
+    }
+}
diff --git a/Ryujinx.HLE/OsHle/Kernel/KernelErr.cs b/Ryujinx.HLE/OsHle/Kernel/KernelErr.cs
index ad4fdfb6..bbae5325 100644
--- a/Ryujinx.HLE/OsHle/Kernel/KernelErr.cs
+++ b/Ryujinx.HLE/OsHle/Kernel/KernelErr.cs
@@ -12,7 +12,7 @@ namespace Ryujinx.HLE.OsHle.Kernel
         public const int Timeout          = 117;
         public const int Canceled         = 118;
         public const int CountOutOfRange  = 119;
-        public const int InvalidInfo      = 120;
+        public const int InvalidEnumValue = 120;
         public const int InvalidThread    = 122;
         public const int InvalidState     = 125;
     }
diff --git a/Ryujinx.HLE/OsHle/Kernel/SvcHandler.cs b/Ryujinx.HLE/OsHle/Kernel/SvcHandler.cs
index e05073fd..e816c44e 100644
--- a/Ryujinx.HLE/OsHle/Kernel/SvcHandler.cs
+++ b/Ryujinx.HLE/OsHle/Kernel/SvcHandler.cs
@@ -73,7 +73,8 @@ namespace Ryujinx.HLE.OsHle.Kernel
                 { 0x2c, SvcMapPhysicalMemory             },
                 { 0x2d, SvcUnmapPhysicalMemory           },
                 { 0x32, SvcSetThreadActivity             },
-                { 0x33, SvcGetThreadContext3             }
+                { 0x33, SvcGetThreadContext3             },
+                { 0x34, SvcWaitForAddress                }
             };
 
             this.Ns      = Ns;
diff --git a/Ryujinx.HLE/OsHle/Kernel/SvcSystem.cs b/Ryujinx.HLE/OsHle/Kernel/SvcSystem.cs
index a32b2d86..08305522 100644
--- a/Ryujinx.HLE/OsHle/Kernel/SvcSystem.cs
+++ b/Ryujinx.HLE/OsHle/Kernel/SvcSystem.cs
@@ -294,7 +294,7 @@ namespace Ryujinx.HLE.OsHle.Kernel
                 InfoType == 19 ||
                 InfoType == 20)
             {
-                ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidInfo);
+                ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidEnumValue);
 
                 return;
             }
diff --git a/Ryujinx.HLE/OsHle/Kernel/SvcThreadSync.cs b/Ryujinx.HLE/OsHle/Kernel/SvcThreadSync.cs
index ec9c40e0..9fc42617 100644
--- a/Ryujinx.HLE/OsHle/Kernel/SvcThreadSync.cs
+++ b/Ryujinx.HLE/OsHle/Kernel/SvcThreadSync.cs
@@ -197,6 +197,57 @@ namespace Ryujinx.HLE.OsHle.Kernel
             Process.Scheduler.EnterWait(CurrThread);
         }
 
+        private void SvcWaitForAddress(AThreadState ThreadState)
+        {
+            long            Address = (long)ThreadState.X0;
+            ArbitrationType Type    = (ArbitrationType)ThreadState.X1;
+            int             Value   = (int)ThreadState.X2;
+            ulong           Timeout = ThreadState.X3;
+
+            Ns.Log.PrintDebug(LogClass.KernelSvc,
+                "Address = "         + Address.ToString("x16") + ", " +
+                "ArbitrationType = " + Type   .ToString()      + ", " +
+                "Value = "           + Value  .ToString("x8")  + ", " +
+                "Timeout = "         + Timeout.ToString("x16"));
+
+            if (IsPointingInsideKernel(Address))
+            {
+                Ns.Log.PrintWarning(LogClass.KernelSvc, $"Invalid address 0x{Address:x16}!");
+
+                ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidAddress);
+
+                return;
+            }
+
+            if (IsWordAddressUnaligned(Address))
+            {
+                Ns.Log.PrintWarning(LogClass.KernelSvc, $"Unaligned address 0x{Address:x16}!");
+
+                ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidAlignment);
+
+                return;
+            }
+
+            switch (Type)
+            {
+                case ArbitrationType.WaitIfLessThan:
+                    ThreadState.X0 = AddressArbiter.WaitForAddressIfLessThan(Process, ThreadState, Memory, Address, Value, Timeout, false);
+                    break;
+
+                case ArbitrationType.DecrementAndWaitIfLessThan:
+                    ThreadState.X0 = AddressArbiter.WaitForAddressIfLessThan(Process, ThreadState, Memory, Address, Value, Timeout, true);
+                    break;
+
+                case ArbitrationType.WaitIfEqual:
+                    ThreadState.X0 = AddressArbiter.WaitForAddressIfEqual(Process, ThreadState, Memory, Address, Value, Timeout);
+                    break;
+
+                default:
+                    ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidEnumValue);
+                    break;
+            }
+        }
+
         private void MutexUnlock(KThread CurrThread, long MutexAddress)
         {
             lock (Process.ThreadSyncLock)