Implement SvcGetThreadContext3

10 files changed, 163 insertions, 12 deletions

diff --git a/ChocolArm64/Events/AInstExceptionEventArgs.cs b/ChocolArm64/Events/AInstExceptionEventArgs.cs
index 34f90c8e..a6853ea1 100644
--- a/ChocolArm64/Events/AInstExceptionEventArgs.cs
+++ b/ChocolArm64/Events/AInstExceptionEventArgs.cs
@@ -4,11 +4,13 @@ namespace ChocolArm64.Events
 {
     public class AInstExceptionEventArgs : EventArgs
     {
-        public int Id { get; private set; }
+        public long Position { get; private set; }
+        public int  Id       { get; private set; }
 
-        public AInstExceptionEventArgs(int Id)
+        public AInstExceptionEventArgs(long Position, int Id)
         {
-            this.Id = Id;
+            this.Position = Position;
+            this.Id       = Id;
         }
     }
 }
\ No newline at end of file
diff --git a/ChocolArm64/Instruction/AInstEmitException.cs b/ChocolArm64/Instruction/AInstEmitException.cs
index 3e444c73..73d20967 100644
--- a/ChocolArm64/Instruction/AInstEmitException.cs
+++ b/ChocolArm64/Instruction/AInstEmitException.cs
@@ -25,6 +25,7 @@ namespace ChocolArm64.Instruction
 
             Context.EmitLdarg(ATranslatedSub.StateArgIdx);
 
+            Context.EmitLdc_I8(Op.Position);
             Context.EmitLdc_I4(Op.Id);
 
             Context.EmitPrivateCall(typeof(AThreadState), MthdName);
diff --git a/ChocolArm64/State/AThreadState.cs b/ChocolArm64/State/AThreadState.cs
index a93e4cf9..a84e3242 100644
--- a/ChocolArm64/State/AThreadState.cs
+++ b/ChocolArm64/State/AThreadState.cs
@@ -51,6 +51,17 @@ namespace ChocolArm64.State
         public int Fpcr { get; set; }
         public int Fpsr { get; set; }
 
+        public int Psr
+        {
+            get
+            {
+                return (Negative ? (int)APState.N : 0) |
+                       (Zero     ? (int)APState.Z : 0) |
+                       (Carry    ? (int)APState.C : 0) |
+                       (Overflow ? (int)APState.V : 0);
+            }
+        }
+
         public uint CtrEl0   => 0x8444c004;
         public uint DczidEl0 => 0x00000004;
 
@@ -89,14 +100,14 @@ namespace ChocolArm64.State
             TickCounter.Start();
         }
 
-        internal void OnBreak(int Imm)
+        internal void OnBreak(long Position, int Imm)
         {
-            Break?.Invoke(this, new AInstExceptionEventArgs(Imm));
+            Break?.Invoke(this, new AInstExceptionEventArgs(Position, Imm));
         }
 
-        internal void OnSvcCall(int Imm)
+        internal void OnSvcCall(long Position, int Imm)
         {
-            SvcCall?.Invoke(this, new AInstExceptionEventArgs(Imm));
+            SvcCall?.Invoke(this, new AInstExceptionEventArgs(Position, Imm));
         }
 
         internal void OnUndefined(long Position, int RawOpCode)
diff --git a/Ryujinx.HLE/OsHle/Handles/KProcessScheduler.cs b/Ryujinx.HLE/OsHle/Handles/KProcessScheduler.cs
index 722460b1..011d9754 100644
--- a/Ryujinx.HLE/OsHle/Handles/KProcessScheduler.cs
+++ b/Ryujinx.HLE/OsHle/Handles/KProcessScheduler.cs
@@ -47,6 +47,8 @@ namespace Ryujinx.HLE.OsHle.Handles
 
                 if (TryAddToCore(Thread))
                 {
+                    SchedThread.IsRunning = true;
+
                     Thread.Thread.Execute();
 
                     PrintDbgThreadInfo(Thread, "running.");
@@ -94,10 +96,7 @@ namespace Ryujinx.HLE.OsHle.Handles
 
         public void SetThreadActivity(KThread Thread, bool Active)
         {
-            if (!AllThreads.TryGetValue(Thread, out SchedulerThread SchedThread))
-            {
-                throw new InvalidOperationException();
-            }
+            SchedulerThread SchedThread = AllThreads[Thread];
 
             SchedThread.IsActive = Active;
 
@@ -111,6 +110,16 @@ namespace Ryujinx.HLE.OsHle.Handles
             }
         }
 
+        public bool IsThreadRunning(KThread Thread)
+        {
+            if (!AllThreads.TryGetValue(Thread, out SchedulerThread SchedThread))
+            {
+                return false;
+            }
+
+            return SchedThread.IsRunning;
+        }
+
         public void EnterWait(KThread Thread, int TimeoutMs = Timeout.Infinite)
         {
             SchedulerThread SchedThread = AllThreads[Thread];
@@ -161,6 +170,8 @@ namespace Ryujinx.HLE.OsHle.Handles
         {
             lock (SchedLock)
             {
+                AllThreads[Thread].IsRunning = false;
+
                 PrintDbgThreadInfo(Thread, "suspended.");
 
                 int ActualCore = Thread.ActualCore;
@@ -252,6 +263,8 @@ namespace Ryujinx.HLE.OsHle.Handles
 
         private void TryResumingExecution(SchedulerThread SchedThread)
         {
+            SchedThread.IsRunning = false;
+
             KThread Thread = SchedThread.Thread;
 
             PrintDbgThreadInfo(Thread, "trying to resume...");
@@ -262,6 +275,8 @@ namespace Ryujinx.HLE.OsHle.Handles
             {
                 if (TryAddToCore(Thread))
                 {
+                    SchedThread.IsRunning = true;
+
                     PrintDbgThreadInfo(Thread, "resuming execution...");
 
                     return;
@@ -291,6 +306,8 @@ namespace Ryujinx.HLE.OsHle.Handles
             {
                 PrintDbgThreadInfo(SchedThread.Thread, "running.");
             }
+
+            SchedThread.IsRunning = true;
         }
 
         public void Resort(KThread Thread)
diff --git a/Ryujinx.HLE/OsHle/Handles/KThread.cs b/Ryujinx.HLE/OsHle/Handles/KThread.cs
index a135bb28..3db46f3d 100644
--- a/Ryujinx.HLE/OsHle/Handles/KThread.cs
+++ b/Ryujinx.HLE/OsHle/Handles/KThread.cs
@@ -29,6 +29,8 @@ namespace Ryujinx.HLE.OsHle.Handles
 
         public int WaitHandle { get; set; }
 
+        public long LastPc { get; set; }
+
         public int ThreadId => Thread.ThreadId;
 
         public KThread(
diff --git a/Ryujinx.HLE/OsHle/Handles/SchedulerThread.cs b/Ryujinx.HLE/OsHle/Handles/SchedulerThread.cs
index 5bdefe74..0b7a3c30 100644
--- a/Ryujinx.HLE/OsHle/Handles/SchedulerThread.cs
+++ b/Ryujinx.HLE/OsHle/Handles/SchedulerThread.cs
@@ -11,6 +11,8 @@ namespace Ryujinx.HLE.OsHle.Handles
 
         public bool IsActive { get; set; }
 
+        public bool IsRunning { get; set; }
+
         public AutoResetEvent   WaitSync     { get; private set; }
         public ManualResetEvent WaitActivity { get; private set; }
         public AutoResetEvent   WaitSched    { get; private set; }
diff --git a/Ryujinx.HLE/OsHle/Kernel/KernelErr.cs b/Ryujinx.HLE/OsHle/Kernel/KernelErr.cs
index 643e5f68..ad4fdfb6 100644
--- a/Ryujinx.HLE/OsHle/Kernel/KernelErr.cs
+++ b/Ryujinx.HLE/OsHle/Kernel/KernelErr.cs
@@ -13,5 +13,7 @@ namespace Ryujinx.HLE.OsHle.Kernel
         public const int Canceled         = 118;
         public const int CountOutOfRange  = 119;
         public const int InvalidInfo      = 120;
+        public const int InvalidThread    = 122;
+        public const int InvalidState     = 125;
     }
 }
\ No newline at end of file
diff --git a/Ryujinx.HLE/OsHle/Kernel/SvcHandler.cs b/Ryujinx.HLE/OsHle/Kernel/SvcHandler.cs
index f3a5fae0..5b6279e3 100644
--- a/Ryujinx.HLE/OsHle/Kernel/SvcHandler.cs
+++ b/Ryujinx.HLE/OsHle/Kernel/SvcHandler.cs
@@ -72,7 +72,8 @@ namespace Ryujinx.HLE.OsHle.Kernel
                 { 0x29, SvcGetInfo                       },
                 { 0x2c, SvcMapPhysicalMemory             },
                 { 0x2d, SvcUnmapPhysicalMemory           },
-                { 0x32, SvcSetThreadActivity             }
+                { 0x32, SvcSetThreadActivity             },
+                { 0x33, SvcGetThreadContext3             }
             };
 
             this.Ns      = Ns;
diff --git a/Ryujinx.HLE/OsHle/Kernel/SvcThread.cs b/Ryujinx.HLE/OsHle/Kernel/SvcThread.cs
index 4501867f..543d9fd2 100644
--- a/Ryujinx.HLE/OsHle/Kernel/SvcThread.cs
+++ b/Ryujinx.HLE/OsHle/Kernel/SvcThread.cs
@@ -280,5 +280,116 @@ namespace Ryujinx.HLE.OsHle.Kernel
                 ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidHandle);
             }
         }
+
+        private void SvcGetThreadContext3(AThreadState ThreadState)
+        {
+            long Position = (long)ThreadState.X0;
+            int  Handle   =  (int)ThreadState.X1;
+
+            KThread Thread = Process.HandleTable.GetData<KThread>(Handle);
+
+            if (Thread == null)
+            {
+                Ns.Log.PrintWarning(LogClass.KernelSvc, $"Invalid thread handle 0x{Handle:x8}!");
+
+                ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidHandle);
+
+                return;
+            }
+
+            if (Process.GetThread(ThreadState.Tpidr) == Thread)
+            {
+                Ns.Log.PrintWarning(LogClass.KernelSvc, $"Thread handle 0x{Handle:x8} is current thread!");
+
+                ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidThread);
+
+                return;
+            }
+
+            if (Process.Scheduler.IsThreadRunning(Thread))
+            {
+                Ns.Log.PrintWarning(LogClass.KernelSvc, $"Thread handle 0x{Handle:x8} is running!");
+
+                ThreadState.X0 = MakeError(ErrorModule.Kernel, KernelErr.InvalidState);
+
+                return;
+            }
+
+            Memory.WriteUInt64(Position + 0x0,  ThreadState.X0);
+            Memory.WriteUInt64(Position + 0x8,  ThreadState.X1);
+            Memory.WriteUInt64(Position + 0x10, ThreadState.X2);
+            Memory.WriteUInt64(Position + 0x18, ThreadState.X3);
+            Memory.WriteUInt64(Position + 0x20, ThreadState.X4);
+            Memory.WriteUInt64(Position + 0x28, ThreadState.X5);
+            Memory.WriteUInt64(Position + 0x30, ThreadState.X6);
+            Memory.WriteUInt64(Position + 0x38, ThreadState.X7);
+            Memory.WriteUInt64(Position + 0x40, ThreadState.X8);
+            Memory.WriteUInt64(Position + 0x48, ThreadState.X9);
+            Memory.WriteUInt64(Position + 0x50, ThreadState.X10);
+            Memory.WriteUInt64(Position + 0x58, ThreadState.X11);
+            Memory.WriteUInt64(Position + 0x60, ThreadState.X12);
+            Memory.WriteUInt64(Position + 0x68, ThreadState.X13);
+            Memory.WriteUInt64(Position + 0x70, ThreadState.X14);
+            Memory.WriteUInt64(Position + 0x78, ThreadState.X15);
+            Memory.WriteUInt64(Position + 0x80, ThreadState.X16);
+            Memory.WriteUInt64(Position + 0x88, ThreadState.X17);
+            Memory.WriteUInt64(Position + 0x90, ThreadState.X18);
+            Memory.WriteUInt64(Position + 0x98, ThreadState.X19);
+            Memory.WriteUInt64(Position + 0xa0, ThreadState.X20);
+            Memory.WriteUInt64(Position + 0xa8, ThreadState.X21);
+            Memory.WriteUInt64(Position + 0xb0, ThreadState.X22);
+            Memory.WriteUInt64(Position + 0xb8, ThreadState.X23);
+            Memory.WriteUInt64(Position + 0xc0, ThreadState.X24);
+            Memory.WriteUInt64(Position + 0xc8, ThreadState.X25);
+            Memory.WriteUInt64(Position + 0xd0, ThreadState.X26);
+            Memory.WriteUInt64(Position + 0xd8, ThreadState.X27);
+            Memory.WriteUInt64(Position + 0xe0, ThreadState.X28);
+            Memory.WriteUInt64(Position + 0xe8, ThreadState.X29);
+            Memory.WriteUInt64(Position + 0xf0, ThreadState.X30);
+            Memory.WriteUInt64(Position + 0xf8, ThreadState.X31);
+
+            Memory.WriteInt64(Position + 0x100, Thread.LastPc);
+
+            Memory.WriteUInt64(Position + 0x108, (ulong)ThreadState.Psr);
+
+            Memory.WriteVector128(Position + 0x110, ThreadState.V0);
+            Memory.WriteVector128(Position + 0x120, ThreadState.V1);
+            Memory.WriteVector128(Position + 0x130, ThreadState.V2);
+            Memory.WriteVector128(Position + 0x140, ThreadState.V3);
+            Memory.WriteVector128(Position + 0x150, ThreadState.V4);
+            Memory.WriteVector128(Position + 0x160, ThreadState.V5);
+            Memory.WriteVector128(Position + 0x170, ThreadState.V6);
+            Memory.WriteVector128(Position + 0x180, ThreadState.V7);
+            Memory.WriteVector128(Position + 0x190, ThreadState.V8);
+            Memory.WriteVector128(Position + 0x1a0, ThreadState.V9);
+            Memory.WriteVector128(Position + 0x1b0, ThreadState.V10);
+            Memory.WriteVector128(Position + 0x1c0, ThreadState.V11);
+            Memory.WriteVector128(Position + 0x1d0, ThreadState.V12);
+            Memory.WriteVector128(Position + 0x1e0, ThreadState.V13);
+            Memory.WriteVector128(Position + 0x1f0, ThreadState.V14);
+            Memory.WriteVector128(Position + 0x200, ThreadState.V15);
+            Memory.WriteVector128(Position + 0x210, ThreadState.V16);
+            Memory.WriteVector128(Position + 0x220, ThreadState.V17);
+            Memory.WriteVector128(Position + 0x230, ThreadState.V18);
+            Memory.WriteVector128(Position + 0x240, ThreadState.V19);
+            Memory.WriteVector128(Position + 0x250, ThreadState.V20);
+            Memory.WriteVector128(Position + 0x260, ThreadState.V21);
+            Memory.WriteVector128(Position + 0x270, ThreadState.V22);
+            Memory.WriteVector128(Position + 0x280, ThreadState.V23);
+            Memory.WriteVector128(Position + 0x290, ThreadState.V24);
+            Memory.WriteVector128(Position + 0x2a0, ThreadState.V25);
+            Memory.WriteVector128(Position + 0x2b0, ThreadState.V26);
+            Memory.WriteVector128(Position + 0x2c0, ThreadState.V27);
+            Memory.WriteVector128(Position + 0x2d0, ThreadState.V28);
+            Memory.WriteVector128(Position + 0x2e0, ThreadState.V29);
+            Memory.WriteVector128(Position + 0x2f0, ThreadState.V30);
+            Memory.WriteVector128(Position + 0x300, ThreadState.V31);
+
+            Memory.WriteInt32(Position + 0x310, ThreadState.Fpcr);
+            Memory.WriteInt32(Position + 0x314, ThreadState.Fpsr);
+            Memory.WriteInt64(Position + 0x318, ThreadState.Tpidr);
+
+            ThreadState.X0 = 0;
+        }
     }
 }
\ No newline at end of file
diff --git a/Ryujinx.HLE/OsHle/Process.cs b/Ryujinx.HLE/OsHle/Process.cs
index f01a0927..53e357ab 100644
--- a/Ryujinx.HLE/OsHle/Process.cs
+++ b/Ryujinx.HLE/OsHle/Process.cs
@@ -205,6 +205,8 @@ namespace Ryujinx.HLE.OsHle
 
             KThread Thread = new KThread(CpuThread, this, ProcessorId, Priority);
 
+            Thread.LastPc = EntryPoint;
+
             int Handle = HandleTable.OpenHandle(Thread);
 
             int ThreadId = GetFreeTlsSlot(CpuThread);