diff --git a/ChocolArm64/AOpCodeTable.cs b/ChocolArm64/AOpCodeTable.cs index 87c30218b9..3042dbc4ef 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 0b94554d59..4ed5f06379 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 9a749ec68f..b66419bd41 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 0000000000..7bee69baea --- /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 bbac9e16c8..7765253ba7 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); + } } }