Implement Frsqrte_S (#72)

* Implement Frsqrte_S

* Implement Frsqrte_V

* Add Frsqrte_S test

5 files changed, 149 insertions, 0 deletions

diff --git a/ChocolArm64/AOpCodeTable.cs b/ChocolArm64/AOpCodeTable.cs
index 87c30218..3042dbc4 100644
--- a/ChocolArm64/AOpCodeTable.cs
+++ b/ChocolArm64/AOpCodeTable.cs
@@ -225,6 +225,8 @@ namespace ChocolArm64
             Set("0>0011101<100001100010xxxxxxxxxx", AInstEmit.Frintp_V,      typeof(AOpCodeSimd));
             Set("000111100x100111010000xxxxxxxxxx", AInstEmit.Frintx_S,      typeof(AOpCodeSimd));
             Set("0>1011100<100001100110xxxxxxxxxx", AInstEmit.Frintx_V,      typeof(AOpCodeSimd));
+            Set("011111101x100001110110xxxxxxxxxx", AInstEmit.Frsqrte_S,     typeof(AOpCodeSimd));
+            Set("0>1011101<100001110110xxxxxxxxxx", AInstEmit.Frsqrte_V,     typeof(AOpCodeSimd));
             Set("000111100x100001110000xxxxxxxxxx", AInstEmit.Fsqrt_S,       typeof(AOpCodeSimd));
             Set("000111100x1xxxxx001110xxxxxxxxxx", AInstEmit.Fsub_S,        typeof(AOpCodeSimdReg));
             Set("0>0011101<1xxxxx110101xxxxxxxxxx", AInstEmit.Fsub_V,        typeof(AOpCodeSimdReg));
diff --git a/ChocolArm64/Instruction/AInstEmitSimdArithmetic.cs b/ChocolArm64/Instruction/AInstEmitSimdArithmetic.cs
index 0b94554d..4ed5f063 100644
--- a/ChocolArm64/Instruction/AInstEmitSimdArithmetic.cs
+++ b/ChocolArm64/Instruction/AInstEmitSimdArithmetic.cs
@@ -476,6 +476,22 @@ namespace ChocolArm64.Instruction
             });
         }
 
+        public static void Frsqrte_S(AILEmitterCtx Context)
+        {
+            EmitScalarUnaryOpF(Context, () =>
+            {
+                EmitUnarySoftFloatCall(Context, nameof(ASoftFloat.InvSqrtEstimate));
+            });
+        }
+
+        public static void Frsqrte_V(AILEmitterCtx Context)
+        {
+            EmitVectorUnaryOpF(Context, () =>
+            {
+                EmitUnarySoftFloatCall(Context, nameof(ASoftFloat.InvSqrtEstimate));
+            });
+        }
+
         public static void Fsqrt_S(AILEmitterCtx Context)
         {
             EmitScalarUnaryOpF(Context, () =>
diff --git a/ChocolArm64/Instruction/AInstEmitSimdHelper.cs b/ChocolArm64/Instruction/AInstEmitSimdHelper.cs
index 9a749ec6..b66419bd 100644
--- a/ChocolArm64/Instruction/AInstEmitSimdHelper.cs
+++ b/ChocolArm64/Instruction/AInstEmitSimdHelper.cs
@@ -100,6 +100,26 @@ namespace ChocolArm64.Instruction
             Context.EmitCall(MthdInfo);
         }
 
+        public static void EmitUnarySoftFloatCall(AILEmitterCtx Context, string Name)
+        {
+            IAOpCodeSimd Op = (IAOpCodeSimd)Context.CurrOp;
+
+            int SizeF = Op.Size & 1;
+
+            MethodInfo MthdInfo;
+
+            if (SizeF == 0)
+            {
+                MthdInfo = typeof(ASoftFloat).GetMethod(Name, new Type[] { typeof(float) });
+            }
+            else /* if (SizeF == 1) */
+            {
+                MthdInfo = typeof(ASoftFloat).GetMethod(Name, new Type[] { typeof(double) });
+            }
+
+            Context.EmitCall(MthdInfo);
+        }
+
         public static void EmitScalarUnaryOpSx(AILEmitterCtx Context, Action Emit)
         {
             EmitScalarOp(Context, Emit, OperFlags.Rn, true);
diff --git a/ChocolArm64/Instruction/ASoftFloat.cs b/ChocolArm64/Instruction/ASoftFloat.cs
new file mode 100644
index 00000000..7bee69ba
--- /dev/null
+++ b/ChocolArm64/Instruction/ASoftFloat.cs
@@ -0,0 +1,103 @@
+using System;
+
+namespace ChocolArm64.Instruction
+{
+    static class ASoftFloat
+    {
+        static ASoftFloat()
+        {
+            InvSqrtEstimateTable = BuildInvSqrtEstimateTable();
+        }
+
+        private static readonly byte[] InvSqrtEstimateTable;
+
+        private static byte[] BuildInvSqrtEstimateTable()
+        {
+            byte[] Table = new byte[512];
+            for (ulong index = 128; index < 512; index++)
+            {
+                ulong a = index;
+                if (a < 256)
+                {
+                    a = (a << 1) + 1;
+                }
+                else
+                {
+                    a = (a | 1) << 1;
+                }
+
+                ulong b = 256;
+                while (a * (b + 1) * (b + 1) < (1ul << 28))
+                {
+                    b++;
+                }
+                b = (b + 1) >> 1;
+
+                Table[index] = (byte)(b & 0xFF);
+            }
+            return Table;
+        }
+
+        public static float InvSqrtEstimate(float x)
+        {
+            return (float)InvSqrtEstimate((double)x);
+        }
+
+        public static double InvSqrtEstimate(double x)
+        {
+            ulong x_bits = (ulong)BitConverter.DoubleToInt64Bits(x);
+            ulong x_sign = x_bits & 0x8000000000000000;
+            long x_exp = (long)((x_bits >> 52) & 0x7FF);
+            ulong scaled = x_bits & ((1ul << 52) - 1);
+
+            if (x_exp == 0x7ff)
+            {
+                if (scaled == 0)
+                {
+                    // Infinity -> Zero
+                    return BitConverter.Int64BitsToDouble((long)x_sign);
+                }
+
+                // NaN
+                return BitConverter.Int64BitsToDouble((long)(x_bits | 0x0008000000000000));
+            }
+
+            if (x_exp == 0)
+            {
+                if (scaled == 0)
+                {
+                    // Zero -> Infinity
+                    return BitConverter.Int64BitsToDouble((long)(x_sign | 0x7ff0000000000000));
+                }
+
+                // Denormal
+                while ((scaled & (1 << 51)) == 0)
+                {
+                    scaled <<= 1;
+                    x_exp--;
+                }
+                scaled <<= 1;
+            }
+
+            if (((ulong)x_exp & 1) == 1)
+            {
+                scaled >>= 45;
+                scaled &= 0xFF;
+                scaled |= 0x80;
+            }
+            else
+            {
+                scaled >>= 44;
+                scaled &= 0xFF;
+                scaled |= 0x100;
+            }
+
+            ulong result_exp = ((ulong)(3068 - x_exp) / 2) & 0x7FF;
+            ulong estimate = (ulong)InvSqrtEstimateTable[scaled];
+            ulong fraction = estimate << 44;
+
+            ulong result = x_sign | (result_exp << 52) | fraction;
+            return BitConverter.Int64BitsToDouble((long)result);
+        }
+    }
+}
\ No newline at end of file
diff --git a/Ryujinx.Tests/Cpu/CpuTestSimdArithmetic.cs b/Ryujinx.Tests/Cpu/CpuTestSimdArithmetic.cs
index bbac9e16..7765253b 100644
--- a/Ryujinx.Tests/Cpu/CpuTestSimdArithmetic.cs
+++ b/Ryujinx.Tests/Cpu/CpuTestSimdArithmetic.cs
@@ -618,5 +618,13 @@ namespace Ryujinx.Tests.Cpu
                 Assert.AreEqual(Result1, ThreadState.V0.X1);
             });
         }
+
+        [TestCase(0x41200000u, 0x3EA18000u)]
+        public void Frsqrte_S(uint A, uint Result)
+        {
+            AVec V1 = new AVec { X0 = A };
+            AThreadState ThreadState = SingleOpcode(0x7EA1D820, V1: V1);
+            Assert.AreEqual(Result, ThreadState.V0.X0);
+        }
     }
 }