From 7f1c73ce0c3c2b1ab5f9817e39ebfb9a72d80a64 Mon Sep 17 00:00:00 2001 From: LDj3SNuD <35856442+LDj3SNuD@users.noreply.github.com> Date: Fri, 28 Sep 2018 20:23:12 +0200 Subject: [PATCH] Update ASoftFloat.cs --- ChocolArm64/Instruction/ASoftFloat.cs | 1519 ++++++++++++++++++++----- 1 file changed, 1220 insertions(+), 299 deletions(-) diff --git a/ChocolArm64/Instruction/ASoftFloat.cs b/ChocolArm64/Instruction/ASoftFloat.cs index e3f067ed50..164a5cd5c7 100644 --- a/ChocolArm64/Instruction/ASoftFloat.cs +++ b/ChocolArm64/Instruction/ASoftFloat.cs @@ -1,4 +1,8 @@ +using ChocolArm64.State; + using System; +using System.Diagnostics; +using System.Runtime.CompilerServices; namespace ChocolArm64.Instruction { @@ -6,13 +10,29 @@ namespace ChocolArm64.Instruction { static ASoftFloat() { + RecipEstimateTable = BuildRecipEstimateTable(); InvSqrtEstimateTable = BuildInvSqrtEstimateTable(); - RecipEstimateTable = BuildRecipEstimateTable(); } private static readonly byte[] RecipEstimateTable; private static readonly byte[] InvSqrtEstimateTable; + private static byte[] BuildRecipEstimateTable() + { + byte[] Table = new byte[256]; + for (ulong index = 0; index < 256; index++) + { + ulong a = index | 0x100; + + a = (a << 1) + 1; + ulong b = 0x80000 / a; + b = (b + 1) >> 1; + + Table[index] = (byte)(b & 0xFF); + } + return Table; + } + private static byte[] BuildInvSqrtEstimateTable() { byte[] Table = new byte[512]; @@ -40,22 +60,75 @@ namespace ChocolArm64.Instruction return Table; } - private static byte[] BuildRecipEstimateTable() + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float RecipEstimate(float x) { - byte[] Table = new byte[256]; - for (ulong index = 0; index < 256; index++) - { - ulong a = index | 0x100; - - a = (a << 1) + 1; - ulong b = 0x80000 / a; - b = (b + 1) >> 1; - - Table[index] = (byte)(b & 0xFF); - } - return Table; + return (float)RecipEstimate((double)x); } + public static double RecipEstimate(double x) + { + ulong x_bits = (ulong)BitConverter.DoubleToInt64Bits(x); + ulong x_sign = x_bits & 0x8000000000000000; + ulong x_exp = (x_bits >> 52) & 0x7FF; + ulong scaled = x_bits & ((1ul << 52) - 1); + + if (x_exp >= 2045) + { + if (x_exp == 0x7ff && scaled != 0) + { + // NaN + return BitConverter.Int64BitsToDouble((long)(x_bits | 0x0008000000000000)); + } + + // Infinity, or Out of range -> Zero + return BitConverter.Int64BitsToDouble((long)x_sign); + } + + if (x_exp == 0) + { + if (scaled == 0) + { + // Zero -> Infinity + return BitConverter.Int64BitsToDouble((long)(x_sign | 0x7FF0000000000000)); + } + + // Denormal + if ((scaled & (1ul << 51)) == 0) + { + x_exp = ~0ul; + scaled <<= 2; + } + else + { + scaled <<= 1; + } + } + + scaled >>= 44; + scaled &= 0xFF; + + ulong result_exp = (2045 - x_exp) & 0x7FF; + ulong estimate = (ulong)RecipEstimateTable[scaled]; + ulong fraction = estimate << 44; + + if (result_exp == 0) + { + fraction >>= 1; + fraction |= 1ul << 51; + } + else if (result_exp == 0x7FF) + { + result_exp = 0; + fraction >>= 2; + fraction |= 1ul << 50; + } + + ulong result = x_sign | (result_exp << 52) | fraction; + return BitConverter.Int64BitsToDouble((long)result); + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] public static float InvSqrtEstimate(float x) { return (float)InvSqrtEstimate((double)x); @@ -124,108 +197,6 @@ namespace ChocolArm64.Instruction return BitConverter.Int64BitsToDouble((long)result); } - public static float RecipEstimate(float x) - { - return (float)RecipEstimate((double)x); - } - - public static double RecipEstimate(double x) - { - ulong x_bits = (ulong)BitConverter.DoubleToInt64Bits(x); - ulong x_sign = x_bits & 0x8000000000000000; - ulong x_exp = (x_bits >> 52) & 0x7FF; - ulong scaled = x_bits & ((1ul << 52) - 1); - - if (x_exp >= 2045) - { - if (x_exp == 0x7ff && scaled != 0) - { - // NaN - return BitConverter.Int64BitsToDouble((long)(x_bits | 0x0008000000000000)); - } - - // Infinity, or Out of range -> Zero - return BitConverter.Int64BitsToDouble((long)x_sign); - } - - if (x_exp == 0) - { - if (scaled == 0) - { - // Zero -> Infinity - return BitConverter.Int64BitsToDouble((long)(x_sign | 0x7FF0000000000000)); - } - - // Denormal - if ((scaled & (1ul << 51)) == 0) - { - x_exp = ~0ul; - scaled <<= 2; - } - else - { - scaled <<= 1; - } - } - - scaled >>= 44; - scaled &= 0xFF; - - ulong result_exp = (2045 - x_exp) & 0x7FF; - ulong estimate = (ulong)RecipEstimateTable[scaled]; - ulong fraction = estimate << 44; - - if (result_exp == 0) - { - fraction >>= 1; - fraction |= 1ul << 51; - } - else if (result_exp == 0x7FF) - { - result_exp = 0; - fraction >>= 2; - fraction |= 1ul << 50; - } - - ulong result = x_sign | (result_exp << 52) | fraction; - return BitConverter.Int64BitsToDouble((long)result); - } - - public static float RecipStep(float op1, float op2) - { - return (float)RecipStep((double)op1, (double)op2); - } - - public static double RecipStep(double op1, double op2) - { - op1 = -op1; - - ulong op1_bits = (ulong)BitConverter.DoubleToInt64Bits(op1); - ulong op2_bits = (ulong)BitConverter.DoubleToInt64Bits(op2); - - ulong op1_sign = op1_bits & 0x8000000000000000; - ulong op2_sign = op2_bits & 0x8000000000000000; - ulong op1_other = op1_bits & 0x7FFFFFFFFFFFFFFF; - ulong op2_other = op2_bits & 0x7FFFFFFFFFFFFFFF; - - bool inf1 = op1_other == 0x7FF0000000000000; - bool inf2 = op2_other == 0x7FF0000000000000; - bool zero1 = op1_other == 0; - bool zero2 = op2_other == 0; - - if ((inf1 && zero2) || (zero1 && inf2)) - { - return 2.0; - } - else if (inf1 || inf2) - { - // Infinity - return BitConverter.Int64BitsToDouble((long)(0x7FF0000000000000 | (op1_sign ^ op2_sign))); - } - - return 2.0 + op1 * op2; - } - public static float ConvertHalfToSingle(ushort x) { uint x_sign = (uint)(x >> 15) & 0x0001; @@ -261,277 +232,1227 @@ namespace ChocolArm64.Instruction uint new_exp = (uint)((exponent + 127) & 0xFF) << 23; return BitConverter.Int32BitsToSingle((int)((x_sign << 31) | new_exp | (x_mantissa << 13))); } + } - public static float MaxNum(float op1, float op2) + static class ASoftFloat_32 + { + public static float FPAdd(float Value1, float Value2, AThreadState State) { - uint op1_bits = (uint)BitConverter.SingleToInt32Bits(op1); - uint op2_bits = (uint)BitConverter.SingleToInt32Bits(op2); + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_32.FPAdd: State.Fpcr = 0x{State.Fpcr:X8}"); - if (IsQNaN(op1_bits) && !IsQNaN(op2_bits)) + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out uint Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out uint Op2); + + float Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) { - op1 = float.NegativeInfinity; - } - else if (!IsQNaN(op1_bits) && IsQNaN(op2_bits)) - { - op2 = float.NegativeInfinity; + bool Inf1 = Type1 == FPType.Infinity; bool Zero1 = Type1 == FPType.Zero; + bool Inf2 = Type2 == FPType.Infinity; bool Zero2 = Type2 == FPType.Zero; + + if (Inf1 && Inf2 && Sign1 == !Sign2) + { + Result = FPDefaultNaN(); + + FPProcessException(FPExc.InvalidOp, State); + } + else if ((Inf1 && !Sign1) || (Inf2 && !Sign2)) + { + Result = FPInfinity(false); + } + else if ((Inf1 && Sign1) || (Inf2 && Sign2)) + { + Result = FPInfinity(true); + } + else if (Zero1 && Zero2 && Sign1 == Sign2) + { + Result = FPZero(Sign1); + } + else + { + Result = Value1 + Value2; + + /*if (Result == 0f) // -Zero, +Zero + { + Result = FPZero(FPRoundingMode(State) == FPRounding.NEGINF); + }*/ + } } - return Max(op1, op2); + return Result; } - public static double MaxNum(double op1, double op2) + public static float FPDiv(float Value1, float Value2, AThreadState State) { - ulong op1_bits = (ulong)BitConverter.DoubleToInt64Bits(op1); - ulong op2_bits = (ulong)BitConverter.DoubleToInt64Bits(op2); + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_32.FPDiv: State.Fpcr = 0x{State.Fpcr:X8}"); - if (IsQNaN(op1_bits) && !IsQNaN(op2_bits)) + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out uint Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out uint Op2); + + float Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) { - op1 = double.NegativeInfinity; - } - else if (!IsQNaN(op1_bits) && IsQNaN(op2_bits)) - { - op2 = double.NegativeInfinity; + bool Inf1 = Type1 == FPType.Infinity; bool Zero1 = Type1 == FPType.Zero; + bool Inf2 = Type2 == FPType.Infinity; bool Zero2 = Type2 == FPType.Zero; + + if ((Inf1 && Inf2) || (Zero1 && Zero2)) + { + Result = FPDefaultNaN(); + + FPProcessException(FPExc.InvalidOp, State); + } + else if (Inf1 || Zero2) + { + Result = FPInfinity(Sign1 ^ Sign2); + + if (!Inf1) FPProcessException(FPExc.DivideByZero, State); + } + else if (Zero1 || Inf2) + { + Result = FPZero(Sign1 ^ Sign2); + } + else + { + Result = Value1 / Value2; + } } - return Max(op1, op2); + return Result; } - public static float Max(float op1, float op2) + public static float FPMax(float Value1, float Value2, AThreadState State) { - // Fast path - if (op1 > op2) + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_32.FPMax: State.Fpcr = 0x{State.Fpcr:X8}"); + + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out uint Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out uint Op2); + + float Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) { - return op1; + if (Value1 > Value2) + { + if (Type1 == FPType.Infinity) + { + Result = FPInfinity(Sign1); + } + else if (Type1 == FPType.Zero) + { + Result = FPZero(Sign1 && Sign2); + } + else + { + Result = Value1; + } + } + else + { + if (Type2 == FPType.Infinity) + { + Result = FPInfinity(Sign2); + } + else if (Type2 == FPType.Zero) + { + Result = FPZero(Sign1 && Sign2); + } + else + { + Result = Value2; + } + } } - if (op1 < op2 || (op1 == op2 && op2 != 0)) - { - return op2; - } - - uint op1_bits = (uint)BitConverter.SingleToInt32Bits(op1); - uint op2_bits = (uint)BitConverter.SingleToInt32Bits(op2); - - // Handle NaN cases - if (ProcessNaNs(op1_bits, op2_bits, out uint op_bits)) - { - return BitConverter.Int32BitsToSingle((int)op_bits); - } - - // Return the most positive zero - if ((op1_bits & op2_bits) == 0x80000000u) - { - return BitConverter.Int32BitsToSingle(int.MinValue); - } - - return 0; + return Result; } - public static double Max(double op1, double op2) + public static float FPMaxNum(float Value1, float Value2, AThreadState State) { - // Fast path - if (op1 > op2) + Debug.WriteIf(State.Fpcr != 0, "ASoftFloat_32.FPMaxNum: "); + + Value1.FPUnpack(out FPType Type1, out bool Sign1, out uint Op1); + Value2.FPUnpack(out FPType Type2, out bool Sign2, out uint Op2); + + if (Type1 == FPType.QNaN && Type2 != FPType.QNaN) { - return op1; + Value1 = FPInfinity(true); + } + else if (Type1 != FPType.QNaN && Type2 == FPType.QNaN) + { + Value2 = FPInfinity(true); } - if (op1 < op2 || (op1 == op2 && op2 != 0)) - { - return op2; - } - - ulong op1_bits = (ulong)BitConverter.DoubleToInt64Bits(op1); - ulong op2_bits = (ulong)BitConverter.DoubleToInt64Bits(op2); - - // Handle NaN cases - if (ProcessNaNs(op1_bits, op2_bits, out ulong op_bits)) - { - return BitConverter.Int64BitsToDouble((long)op_bits); - } - - // Return the most positive zero - if ((op1_bits & op2_bits) == 0x8000000000000000ul) - { - return BitConverter.Int64BitsToDouble(long.MinValue); - } - - return 0; + return FPMax(Value1, Value2, State); } - public static float MinNum(float op1, float op2) + public static float FPMin(float Value1, float Value2, AThreadState State) { - uint op1_bits = (uint)BitConverter.SingleToInt32Bits(op1); - uint op2_bits = (uint)BitConverter.SingleToInt32Bits(op2); + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_32.FPMin: State.Fpcr = 0x{State.Fpcr:X8}"); - if (IsQNaN(op1_bits) && !IsQNaN(op2_bits)) + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out uint Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out uint Op2); + + float Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) { - op1 = float.PositiveInfinity; - } - else if (!IsQNaN(op1_bits) && IsQNaN(op2_bits)) - { - op2 = float.PositiveInfinity; + if (Value1 < Value2) + { + if (Type1 == FPType.Infinity) + { + Result = FPInfinity(Sign1); + } + else if (Type1 == FPType.Zero) + { + Result = FPZero(Sign1 || Sign2); + } + else + { + Result = Value1; + } + } + else + { + if (Type2 == FPType.Infinity) + { + Result = FPInfinity(Sign2); + } + else if (Type2 == FPType.Zero) + { + Result = FPZero(Sign1 || Sign2); + } + else + { + Result = Value2; + } + } } - return Min(op1, op2); + return Result; } - public static double MinNum(double op1, double op2) + public static float FPMinNum(float Value1, float Value2, AThreadState State) { - ulong op1_bits = (ulong)BitConverter.DoubleToInt64Bits(op1); - ulong op2_bits = (ulong)BitConverter.DoubleToInt64Bits(op2); + Debug.WriteIf(State.Fpcr != 0, "ASoftFloat_32.FPMinNum: "); - if (IsQNaN(op1_bits) && !IsQNaN(op2_bits)) + Value1.FPUnpack(out FPType Type1, out bool Sign1, out uint Op1); + Value2.FPUnpack(out FPType Type2, out bool Sign2, out uint Op2); + + if (Type1 == FPType.QNaN && Type2 != FPType.QNaN) { - op1 = double.PositiveInfinity; + Value1 = FPInfinity(false); } - else if (!IsQNaN(op1_bits) && IsQNaN(op2_bits)) + else if (Type1 != FPType.QNaN && Type2 == FPType.QNaN) { - op2 = double.PositiveInfinity; + Value2 = FPInfinity(false); } - return Min(op1, op2); + return FPMin(Value1, Value2, State); } - public static float Min(float op1, float op2) + public static float FPMul(float Value1, float Value2, AThreadState State) { - // Fast path - if (op1 < op2) + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_32.FPMul: State.Fpcr = 0x{State.Fpcr:X8}"); + + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out uint Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out uint Op2); + + float Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) { - return op1; + bool Inf1 = Type1 == FPType.Infinity; bool Zero1 = Type1 == FPType.Zero; + bool Inf2 = Type2 == FPType.Infinity; bool Zero2 = Type2 == FPType.Zero; + + if ((Inf1 && Zero2) || (Zero1 && Inf2)) + { + Result = FPDefaultNaN(); + + FPProcessException(FPExc.InvalidOp, State); + } + else if (Inf1 || Inf2) + { + Result = FPInfinity(Sign1 ^ Sign2); + } + else if (Zero1 || Zero2) + { + Result = FPZero(Sign1 ^ Sign2); + } + else + { + Result = Value1 * Value2; + } } - if (op1 > op2 || (op1 == op2 && op2 != 0)) - { - return op2; - } - - uint op1_bits = (uint)BitConverter.SingleToInt32Bits(op1); - uint op2_bits = (uint)BitConverter.SingleToInt32Bits(op2); - - // Handle NaN cases - if (ProcessNaNs(op1_bits, op2_bits, out uint op_bits)) - { - return BitConverter.Int32BitsToSingle((int)op_bits); - } - - // Return the most negative zero - if ((op1_bits | op2_bits) == 0x80000000u) - { - return BitConverter.Int32BitsToSingle(int.MinValue); - } - - return 0; + return Result; } - public static double Min(double op1, double op2) + public static float FPMulAdd(float ValueA, float Value1, float Value2, AThreadState State) { - // Fast path - if (op1 < op2) + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_32.FPMulAdd: State.Fpcr = 0x{State.Fpcr:X8}"); + + ValueA = ValueA.FPUnpack(out FPType TypeA, out bool SignA, out uint Addend); + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out uint Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out uint Op2); + + bool Inf1 = Type1 == FPType.Infinity; bool Zero1 = Type1 == FPType.Zero; + bool Inf2 = Type2 == FPType.Infinity; bool Zero2 = Type2 == FPType.Zero; + + float Result = FPProcessNaNs3(TypeA, Type1, Type2, Addend, Op1, Op2, State, out bool Done); + + if (TypeA == FPType.QNaN && ((Inf1 && Zero2) || (Zero1 && Inf2))) { - return op1; + Result = FPDefaultNaN(); + + FPProcessException(FPExc.InvalidOp, State); } - if (op1 > op2 || (op1 == op2 && op2 != 0)) + if (!Done) { - return op2; + bool InfA = TypeA == FPType.Infinity; bool ZeroA = TypeA == FPType.Zero; + + bool SignP = Sign1 ^ Sign2; + bool InfP = Inf1 || Inf2; + bool ZeroP = Zero1 || Zero2; + + if ((Inf1 && Zero2) || (Zero1 && Inf2) || (InfA && InfP && SignA != SignP)) + { + Result = FPDefaultNaN(); + + FPProcessException(FPExc.InvalidOp, State); + } + else if ((InfA && !SignA) || (InfP && !SignP)) + { + Result = FPInfinity(false); + } + else if ((InfA && SignA) || (InfP && SignP)) + { + Result = FPInfinity(true); + } + else if (ZeroA && ZeroP && SignA == SignP) + { + Result = FPZero(SignA); + } + else + { + // https://github.com/dotnet/corefx/issues/31903 + Result = ValueA + (Value1 * Value2); + + /*if (Result == 0f) // -Zero, +Zero + { + Result = FPZero(FPRoundingMode(State) == FPRounding.NEGINF); + }*/ + } } - ulong op1_bits = (ulong)BitConverter.DoubleToInt64Bits(op1); - ulong op2_bits = (ulong)BitConverter.DoubleToInt64Bits(op2); - - // Handle NaN cases - if (ProcessNaNs(op1_bits, op2_bits, out ulong op_bits)) - { - return BitConverter.Int64BitsToDouble((long)op_bits); - } - - // Return the most negative zero - if ((op1_bits | op2_bits) == 0x8000000000000000ul) - { - return BitConverter.Int64BitsToDouble(long.MinValue); - } - - return 0; + return Result; } - private static bool ProcessNaNs(uint op1_bits, uint op2_bits, out uint op_bits) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static float FPMulSub(float ValueA, float Value1, float Value2, AThreadState State) { - if (IsSNaN(op1_bits)) + Debug.WriteIf(State.Fpcr != 0, "ASoftFloat_32.FPMulSub: "); + + Value1 = Value1.FPNeg(); + + return FPMulAdd(ValueA, Value1, Value2, State); + } + + public static float FPRecipStepFused(float Value1, float Value2, AThreadState State) + { + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_32.FPRecipStepFused: State.Fpcr = 0x{State.Fpcr:X8}"); + + Value1 = Value1.FPNeg(); + + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out uint Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out uint Op2); + + float Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) { - op_bits = op1_bits | (1u << 22); // op1 is SNaN, return QNaN op1 + bool Inf1 = Type1 == FPType.Infinity; bool Zero1 = Type1 == FPType.Zero; + bool Inf2 = Type2 == FPType.Infinity; bool Zero2 = Type2 == FPType.Zero; + + if ((Inf1 && Zero2) || (Zero1 && Inf2)) + { + Result = FPTwo(false); + } + else if (Inf1 || Inf2) + { + Result = FPInfinity(Sign1 ^ Sign2); + } + else + { + // https://github.com/dotnet/corefx/issues/31903 + Result = 2f + (Value1 * Value2); + + /*if (Result == 0f) // -Zero, +Zero + { + Result = FPZero(FPRoundingMode(State) == FPRounding.NEGINF); + }*/ + } } - else if (IsSNaN(op2_bits)) + + return Result; + } + + public static float FPRSqrtStepFused(float Value1, float Value2, AThreadState State) + { + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_32.FPRSqrtStepFused: State.Fpcr = 0x{State.Fpcr:X8}"); + + Value1 = Value1.FPNeg(); + + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out uint Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out uint Op2); + + float Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) { - op_bits = op2_bits | (1u << 22); // op2 is SNaN, return QNaN op2 + bool Inf1 = Type1 == FPType.Infinity; bool Zero1 = Type1 == FPType.Zero; + bool Inf2 = Type2 == FPType.Infinity; bool Zero2 = Type2 == FPType.Zero; + + if ((Inf1 && Zero2) || (Zero1 && Inf2)) + { + Result = FPOnePointFive(false); + } + else if (Inf1 || Inf2) + { + Result = FPInfinity(Sign1 ^ Sign2); + } + else + { + // https://github.com/dotnet/corefx/issues/31903 + Result = (3f + (Value1 * Value2)) / 2f; + + /*if (Result == 0f) // -Zero, +Zero + { + Result = FPZero(FPRoundingMode(State) == FPRounding.NEGINF); + }*/ + } } - else if (IsQNaN(op1_bits)) + + return Result; + } + + public static float FPSub(float Value1, float Value2, AThreadState State) + { + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_32.FPSub: State.Fpcr = 0x{State.Fpcr:X8}"); + + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out uint Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out uint Op2); + + float Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) { - op_bits = op1_bits; // op1 is QNaN, return QNaN op1 + bool Inf1 = Type1 == FPType.Infinity; bool Zero1 = Type1 == FPType.Zero; + bool Inf2 = Type2 == FPType.Infinity; bool Zero2 = Type2 == FPType.Zero; + + if (Inf1 && Inf2 && Sign1 == Sign2) + { + Result = FPDefaultNaN(); + + FPProcessException(FPExc.InvalidOp, State); + } + else if ((Inf1 && !Sign1) || (Inf2 && Sign2)) + { + Result = FPInfinity(false); + } + else if ((Inf1 && Sign1) || (Inf2 && !Sign2)) + { + Result = FPInfinity(true); + } + else if (Zero1 && Zero2 && Sign1 == !Sign2) + { + Result = FPZero(Sign1); + } + else + { + Result = Value1 - Value2; + + /*if (Result == 0f) // -Zero, +Zero + { + Result = FPZero(FPRoundingMode(State) == FPRounding.NEGINF); + }*/ + } } - else if (IsQNaN(op2_bits)) + + return Result; + } + + private enum FPType { Nonzero, Zero, Infinity, QNaN, SNaN }; + private enum FPExc { InvalidOp, DivideByZero, Overflow, Underflow, Inexact, InputDenorm = 7 }; + private enum FPRounding { TIEEVEN, POSINF, NEGINF, ZERO }; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] private static float FPDefaultNaN() => -float.NaN; + [MethodImpl(MethodImplOptions.AggressiveInlining)] private static float FPInfinity(bool Sign) => Sign ? float.NegativeInfinity : float.PositiveInfinity; + [MethodImpl(MethodImplOptions.AggressiveInlining)] private static float FPZero(bool Sign) => Sign ? -0f : +0f; + [MethodImpl(MethodImplOptions.AggressiveInlining)] private static float FPTwo(bool Sign) => Sign ? -2f : +2f; + [MethodImpl(MethodImplOptions.AggressiveInlining)] private static float FPOnePointFive(bool Sign) => Sign ? -1.5f : +1.5f; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] private static float FPNeg(this float Value) => -Value; + + private static float FPUnpack(this float Value, out FPType Type, out bool Sign, out uint ValueBits) + { + ValueBits = (uint)BitConverter.SingleToInt32Bits(Value); + + Sign = (~ValueBits & 0x80000000u) == 0u; + + if ((ValueBits & 0x7F800000u) == 0u) { - op_bits = op2_bits; // op2 is QNaN, return QNaN op2 + if ((ValueBits & 0x007FFFFFu) == 0u) + { + Type = FPType.Zero; + } + else + { + Type = FPType.Nonzero; + } + } + else if ((~ValueBits & 0x7F800000u) == 0u) + { + if ((ValueBits & 0x007FFFFFu) == 0u) + { + Type = FPType.Infinity; + } + else + { + Type = (~ValueBits & 0x00400000u) == 0u + ? FPType.QNaN + : FPType.SNaN; + + return FPZero(Sign); + } } else { - op_bits = 0; - - return false; + Type = FPType.Nonzero; } - return true; + return Value; } - private static bool ProcessNaNs(ulong op1_bits, ulong op2_bits, out ulong op_bits) + private static float FPProcessNaNs( + FPType Type1, FPType Type2, + uint Op1, uint Op2, + AThreadState State, + out bool Done) { - if (IsSNaN(op1_bits)) + Done = true; + + if (Type1 == FPType.SNaN) { - op_bits = op1_bits | (1ul << 51); // op1 is SNaN, return QNaN op1 + return FPProcessNaN(Type1, Op1, State); } - else if (IsSNaN(op2_bits)) + else if (Type2 == FPType.SNaN) { - op_bits = op2_bits | (1ul << 51); // op2 is SNaN, return QNaN op2 + return FPProcessNaN(Type2, Op2, State); } - else if (IsQNaN(op1_bits)) + else if (Type1 == FPType.QNaN) { - op_bits = op1_bits; // op1 is QNaN, return QNaN op1 + return FPProcessNaN(Type1, Op1, State); } - else if (IsQNaN(op2_bits)) + else if (Type2 == FPType.QNaN) { - op_bits = op2_bits; // op2 is QNaN, return QNaN op2 + return FPProcessNaN(Type2, Op2, State); + } + + Done = false; + + return FPZero(false); + } + + private static float FPProcessNaNs3( + FPType Type1, FPType Type2, FPType Type3, + uint Op1, uint Op2, uint Op3, + AThreadState State, + out bool Done) + { + Done = true; + + if (Type1 == FPType.SNaN) + { + return FPProcessNaN(Type1, Op1, State); + } + else if (Type2 == FPType.SNaN) + { + return FPProcessNaN(Type2, Op2, State); + } + else if (Type3 == FPType.SNaN) + { + return FPProcessNaN(Type3, Op3, State); + } + else if (Type1 == FPType.QNaN) + { + return FPProcessNaN(Type1, Op1, State); + } + else if (Type2 == FPType.QNaN) + { + return FPProcessNaN(Type2, Op2, State); + } + else if (Type3 == FPType.QNaN) + { + return FPProcessNaN(Type3, Op3, State); + } + + Done = false; + + return FPZero(false); + } + + private static float FPProcessNaN(FPType Type, uint Op, AThreadState State) + { + const int DNBit = 25; // Default NaN mode control bit. + + if (Type == FPType.SNaN) + { + Op |= 1u << 22; + + FPProcessException(FPExc.InvalidOp, State); + } + + if ((State.Fpcr & (1 << DNBit)) != 0) + { + return FPDefaultNaN(); + } + + return BitConverter.Int32BitsToSingle((int)Op); + } + + private static void FPProcessException(FPExc Exc, AThreadState State) + { + int Enable = (int)Exc + 8; + + if ((State.Fpcr & (1 << Enable)) != 0) + { + throw new NotImplementedException("floating-point trap handling"); } else { - op_bits = 0; - - return false; + State.Fpsr |= 1 << (int)Exc; } - - return true; } - private static bool IsQNaN(uint op_bits) + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static FPRounding FPRoundingMode(AThreadState State) { - return (op_bits & 0x007FFFFF) != 0 && - (op_bits & 0x7FC00000) == 0x7FC00000; - } + const int RModeBits = 22; // Rounding Mode control field. - private static bool IsQNaN(ulong op_bits) - { - return (op_bits & 0x000FFFFFFFFFFFFF) != 0 && - (op_bits & 0x7FF8000000000000) == 0x7FF8000000000000; - } - - private static bool IsSNaN(uint op_bits) - { - return (op_bits & 0x007FFFFF) != 0 && - (op_bits & 0x7FC00000) == 0x7F800000; - } - - private static bool IsSNaN(ulong op_bits) - { - return (op_bits & 0x000FFFFFFFFFFFFF) != 0 && - (op_bits & 0x7FF8000000000000) == 0x7FF0000000000000; + return (FPRounding)((State.Fpcr >> RModeBits) & 0b11); } } -} \ No newline at end of file + + static class ASoftFloat_64 + { + public static double FPAdd(double Value1, double Value2, AThreadState State) + { + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_64.FPAdd: State.Fpcr = 0x{State.Fpcr:X8}"); + + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out ulong Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out ulong Op2); + + double Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) + { + bool Inf1 = Type1 == FPType.Infinity; bool Zero1 = Type1 == FPType.Zero; + bool Inf2 = Type2 == FPType.Infinity; bool Zero2 = Type2 == FPType.Zero; + + if (Inf1 && Inf2 && Sign1 == !Sign2) + { + Result = FPDefaultNaN(); + + FPProcessException(FPExc.InvalidOp, State); + } + else if ((Inf1 && !Sign1) || (Inf2 && !Sign2)) + { + Result = FPInfinity(false); + } + else if ((Inf1 && Sign1) || (Inf2 && Sign2)) + { + Result = FPInfinity(true); + } + else if (Zero1 && Zero2 && Sign1 == Sign2) + { + Result = FPZero(Sign1); + } + else + { + Result = Value1 + Value2; + + /*if (Result == 0f) // -Zero, +Zero + { + Result = FPZero(FPRoundingMode(State) == FPRounding.NEGINF); + }*/ + } + } + + return Result; + } + + public static double FPDiv(double Value1, double Value2, AThreadState State) + { + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_64.FPDiv: State.Fpcr = 0x{State.Fpcr:X8}"); + + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out ulong Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out ulong Op2); + + double Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) + { + bool Inf1 = Type1 == FPType.Infinity; bool Zero1 = Type1 == FPType.Zero; + bool Inf2 = Type2 == FPType.Infinity; bool Zero2 = Type2 == FPType.Zero; + + if ((Inf1 && Inf2) || (Zero1 && Zero2)) + { + Result = FPDefaultNaN(); + + FPProcessException(FPExc.InvalidOp, State); + } + else if (Inf1 || Zero2) + { + Result = FPInfinity(Sign1 ^ Sign2); + + if (!Inf1) FPProcessException(FPExc.DivideByZero, State); + } + else if (Zero1 || Inf2) + { + Result = FPZero(Sign1 ^ Sign2); + } + else + { + Result = Value1 / Value2; + } + } + + return Result; + } + + public static double FPMax(double Value1, double Value2, AThreadState State) + { + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_64.FPMax: State.Fpcr = 0x{State.Fpcr:X8}"); + + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out ulong Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out ulong Op2); + + double Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) + { + if (Value1 > Value2) + { + if (Type1 == FPType.Infinity) + { + Result = FPInfinity(Sign1); + } + else if (Type1 == FPType.Zero) + { + Result = FPZero(Sign1 && Sign2); + } + else + { + Result = Value1; + } + } + else + { + if (Type2 == FPType.Infinity) + { + Result = FPInfinity(Sign2); + } + else if (Type2 == FPType.Zero) + { + Result = FPZero(Sign1 && Sign2); + } + else + { + Result = Value2; + } + } + } + + return Result; + } + + public static double FPMaxNum(double Value1, double Value2, AThreadState State) + { + Debug.WriteIf(State.Fpcr != 0, "ASoftFloat_64.FPMaxNum: "); + + Value1.FPUnpack(out FPType Type1, out bool Sign1, out ulong Op1); + Value2.FPUnpack(out FPType Type2, out bool Sign2, out ulong Op2); + + if (Type1 == FPType.QNaN && Type2 != FPType.QNaN) + { + Value1 = FPInfinity(true); + } + else if (Type1 != FPType.QNaN && Type2 == FPType.QNaN) + { + Value2 = FPInfinity(true); + } + + return FPMax(Value1, Value2, State); + } + + public static double FPMin(double Value1, double Value2, AThreadState State) + { + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_64.FPMin: State.Fpcr = 0x{State.Fpcr:X8}"); + + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out ulong Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out ulong Op2); + + double Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) + { + if (Value1 < Value2) + { + if (Type1 == FPType.Infinity) + { + Result = FPInfinity(Sign1); + } + else if (Type1 == FPType.Zero) + { + Result = FPZero(Sign1 || Sign2); + } + else + { + Result = Value1; + } + } + else + { + if (Type2 == FPType.Infinity) + { + Result = FPInfinity(Sign2); + } + else if (Type2 == FPType.Zero) + { + Result = FPZero(Sign1 || Sign2); + } + else + { + Result = Value2; + } + } + } + + return Result; + } + + public static double FPMinNum(double Value1, double Value2, AThreadState State) + { + Debug.WriteIf(State.Fpcr != 0, "ASoftFloat_64.FPMinNum: "); + + Value1.FPUnpack(out FPType Type1, out bool Sign1, out ulong Op1); + Value2.FPUnpack(out FPType Type2, out bool Sign2, out ulong Op2); + + if (Type1 == FPType.QNaN && Type2 != FPType.QNaN) + { + Value1 = FPInfinity(false); + } + else if (Type1 != FPType.QNaN && Type2 == FPType.QNaN) + { + Value2 = FPInfinity(false); + } + + return FPMin(Value1, Value2, State); + } + + public static double FPMul(double Value1, double Value2, AThreadState State) + { + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_64.FPMul: State.Fpcr = 0x{State.Fpcr:X8}"); + + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out ulong Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out ulong Op2); + + double Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) + { + bool Inf1 = Type1 == FPType.Infinity; bool Zero1 = Type1 == FPType.Zero; + bool Inf2 = Type2 == FPType.Infinity; bool Zero2 = Type2 == FPType.Zero; + + if ((Inf1 && Zero2) || (Zero1 && Inf2)) + { + Result = FPDefaultNaN(); + + FPProcessException(FPExc.InvalidOp, State); + } + else if (Inf1 || Inf2) + { + Result = FPInfinity(Sign1 ^ Sign2); + } + else if (Zero1 || Zero2) + { + Result = FPZero(Sign1 ^ Sign2); + } + else + { + Result = Value1 * Value2; + } + } + + return Result; + } + + public static double FPMulAdd(double ValueA, double Value1, double Value2, AThreadState State) + { + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_64.FPMulAdd: State.Fpcr = 0x{State.Fpcr:X8}"); + + ValueA = ValueA.FPUnpack(out FPType TypeA, out bool SignA, out ulong Addend); + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out ulong Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out ulong Op2); + + bool Inf1 = Type1 == FPType.Infinity; bool Zero1 = Type1 == FPType.Zero; + bool Inf2 = Type2 == FPType.Infinity; bool Zero2 = Type2 == FPType.Zero; + + double Result = FPProcessNaNs3(TypeA, Type1, Type2, Addend, Op1, Op2, State, out bool Done); + + if (TypeA == FPType.QNaN && ((Inf1 && Zero2) || (Zero1 && Inf2))) + { + Result = FPDefaultNaN(); + + FPProcessException(FPExc.InvalidOp, State); + } + + if (!Done) + { + bool InfA = TypeA == FPType.Infinity; bool ZeroA = TypeA == FPType.Zero; + + bool SignP = Sign1 ^ Sign2; + bool InfP = Inf1 || Inf2; + bool ZeroP = Zero1 || Zero2; + + if ((Inf1 && Zero2) || (Zero1 && Inf2) || (InfA && InfP && SignA != SignP)) + { + Result = FPDefaultNaN(); + + FPProcessException(FPExc.InvalidOp, State); + } + else if ((InfA && !SignA) || (InfP && !SignP)) + { + Result = FPInfinity(false); + } + else if ((InfA && SignA) || (InfP && SignP)) + { + Result = FPInfinity(true); + } + else if (ZeroA && ZeroP && SignA == SignP) + { + Result = FPZero(SignA); + } + else + { + // https://github.com/dotnet/corefx/issues/31903 + Result = ValueA + (Value1 * Value2); + + /*if (Result == 0d) // -Zero, +Zero + { + Result = FPZero(FPRoundingMode(State) == FPRounding.NEGINF); + }*/ + } + } + + return Result; + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + public static double FPMulSub(double ValueA, double Value1, double Value2, AThreadState State) + { + Debug.WriteIf(State.Fpcr != 0, "ASoftFloat_64.FPMulSub: "); + + Value1 = Value1.FPNeg(); + + return FPMulAdd(ValueA, Value1, Value2, State); + } + + public static double FPRecipStepFused(double Value1, double Value2, AThreadState State) + { + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_64.FPRecipStepFused: State.Fpcr = 0x{State.Fpcr:X8}"); + + Value1 = Value1.FPNeg(); + + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out ulong Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out ulong Op2); + + double Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) + { + bool Inf1 = Type1 == FPType.Infinity; bool Zero1 = Type1 == FPType.Zero; + bool Inf2 = Type2 == FPType.Infinity; bool Zero2 = Type2 == FPType.Zero; + + if ((Inf1 && Zero2) || (Zero1 && Inf2)) + { + Result = FPTwo(false); + } + else if (Inf1 || Inf2) + { + Result = FPInfinity(Sign1 ^ Sign2); + } + else + { + // https://github.com/dotnet/corefx/issues/31903 + Result = 2d + (Value1 * Value2); + + /*if (Result == 0d) // -Zero, +Zero + { + Result = FPZero(FPRoundingMode(State) == FPRounding.NEGINF); + }*/ + } + } + + return Result; + } + + public static double FPRSqrtStepFused(double Value1, double Value2, AThreadState State) + { + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_64.FPRSqrtStepFused: State.Fpcr = 0x{State.Fpcr:X8}"); + + Value1 = Value1.FPNeg(); + + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out ulong Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out ulong Op2); + + double Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) + { + bool Inf1 = Type1 == FPType.Infinity; bool Zero1 = Type1 == FPType.Zero; + bool Inf2 = Type2 == FPType.Infinity; bool Zero2 = Type2 == FPType.Zero; + + if ((Inf1 && Zero2) || (Zero1 && Inf2)) + { + Result = FPOnePointFive(false); + } + else if (Inf1 || Inf2) + { + Result = FPInfinity(Sign1 ^ Sign2); + } + else + { + // https://github.com/dotnet/corefx/issues/31903 + Result = (3d + (Value1 * Value2)) / 2d; + + /*if (Result == 0d) // -Zero, +Zero + { + Result = FPZero(FPRoundingMode(State) == FPRounding.NEGINF); + }*/ + } + } + + return Result; + } + + public static double FPSub(double Value1, double Value2, AThreadState State) + { + Debug.WriteLineIf(State.Fpcr != 0, $"ASoftFloat_64.FPSub: State.Fpcr = 0x{State.Fpcr:X8}"); + + Value1 = Value1.FPUnpack(out FPType Type1, out bool Sign1, out ulong Op1); + Value2 = Value2.FPUnpack(out FPType Type2, out bool Sign2, out ulong Op2); + + double Result = FPProcessNaNs(Type1, Type2, Op1, Op2, State, out bool Done); + + if (!Done) + { + bool Inf1 = Type1 == FPType.Infinity; bool Zero1 = Type1 == FPType.Zero; + bool Inf2 = Type2 == FPType.Infinity; bool Zero2 = Type2 == FPType.Zero; + + if (Inf1 && Inf2 && Sign1 == Sign2) + { + Result = FPDefaultNaN(); + + FPProcessException(FPExc.InvalidOp, State); + } + else if ((Inf1 && !Sign1) || (Inf2 && Sign2)) + { + Result = FPInfinity(false); + } + else if ((Inf1 && Sign1) || (Inf2 && !Sign2)) + { + Result = FPInfinity(true); + } + else if (Zero1 && Zero2 && Sign1 == !Sign2) + { + Result = FPZero(Sign1); + } + else + { + Result = Value1 - Value2; + + /*if (Result == 0f) // -Zero, +Zero + { + Result = FPZero(FPRoundingMode(State) == FPRounding.NEGINF); + }*/ + } + } + + return Result; + } + + private enum FPType { Nonzero, Zero, Infinity, QNaN, SNaN }; + private enum FPExc { InvalidOp, DivideByZero, Overflow, Underflow, Inexact, InputDenorm = 7 }; + private enum FPRounding { TIEEVEN, POSINF, NEGINF, ZERO }; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] private static double FPDefaultNaN() => -double.NaN; + [MethodImpl(MethodImplOptions.AggressiveInlining)] private static double FPInfinity(bool Sign) => Sign ? double.NegativeInfinity : double.PositiveInfinity; + [MethodImpl(MethodImplOptions.AggressiveInlining)] private static double FPZero(bool Sign) => Sign ? -0d : +0d; + [MethodImpl(MethodImplOptions.AggressiveInlining)] private static double FPTwo(bool Sign) => Sign ? -2d : +2d; + [MethodImpl(MethodImplOptions.AggressiveInlining)] private static double FPOnePointFive(bool Sign) => Sign ? -1.5d : +1.5d; + + [MethodImpl(MethodImplOptions.AggressiveInlining)] private static double FPNeg(this double Value) => -Value; + + private static double FPUnpack(this double Value, out FPType Type, out bool Sign, out ulong ValueBits) + { + ValueBits = (ulong)BitConverter.DoubleToInt64Bits(Value); + + Sign = (~ValueBits & 0x8000000000000000ul) == 0ul; + + if ((ValueBits & 0x7FF0000000000000ul) == 0ul) + { + if ((ValueBits & 0x000FFFFFFFFFFFFFul) == 0ul) + { + Type = FPType.Zero; + } + else + { + Type = FPType.Nonzero; + } + } + else if ((~ValueBits & 0x7FF0000000000000ul) == 0ul) + { + if ((ValueBits & 0x000FFFFFFFFFFFFFul) == 0ul) + { + Type = FPType.Infinity; + } + else + { + Type = (~ValueBits & 0x8000000000000ul) == 0ul + ? FPType.QNaN + : FPType.SNaN; + + return FPZero(Sign); + } + } + else + { + Type = FPType.Nonzero; + } + + return Value; + } + + private static double FPProcessNaNs( + FPType Type1, FPType Type2, + ulong Op1, ulong Op2, + AThreadState State, + out bool Done) + { + Done = true; + + if (Type1 == FPType.SNaN) + { + return FPProcessNaN(Type1, Op1, State); + } + else if (Type2 == FPType.SNaN) + { + return FPProcessNaN(Type2, Op2, State); + } + else if (Type1 == FPType.QNaN) + { + return FPProcessNaN(Type1, Op1, State); + } + else if (Type2 == FPType.QNaN) + { + return FPProcessNaN(Type2, Op2, State); + } + + Done = false; + + return FPZero(false); + } + + private static double FPProcessNaNs3( + FPType Type1, FPType Type2, FPType Type3, + ulong Op1, ulong Op2, ulong Op3, + AThreadState State, + out bool Done) + { + Done = true; + + if (Type1 == FPType.SNaN) + { + return FPProcessNaN(Type1, Op1, State); + } + else if (Type2 == FPType.SNaN) + { + return FPProcessNaN(Type2, Op2, State); + } + else if (Type3 == FPType.SNaN) + { + return FPProcessNaN(Type3, Op3, State); + } + else if (Type1 == FPType.QNaN) + { + return FPProcessNaN(Type1, Op1, State); + } + else if (Type2 == FPType.QNaN) + { + return FPProcessNaN(Type2, Op2, State); + } + else if (Type3 == FPType.QNaN) + { + return FPProcessNaN(Type3, Op3, State); + } + + Done = false; + + return FPZero(false); + } + + private static double FPProcessNaN(FPType Type, ulong Op, AThreadState State) + { + const int DNBit = 25; // Default NaN mode control bit. + + if (Type == FPType.SNaN) + { + Op |= 1ul << 51; + + FPProcessException(FPExc.InvalidOp, State); + } + + if ((State.Fpcr & (1 << DNBit)) != 0) + { + return FPDefaultNaN(); + } + + return BitConverter.Int64BitsToDouble((long)Op); + } + + private static void FPProcessException(FPExc Exc, AThreadState State) + { + int Enable = (int)Exc + 8; + + if ((State.Fpcr & (1 << Enable)) != 0) + { + throw new NotImplementedException("floating-point trap handling"); + } + else + { + State.Fpsr |= 1 << (int)Exc; + } + } + + [MethodImpl(MethodImplOptions.AggressiveInlining)] + private static FPRounding FPRoundingMode(AThreadState State) + { + const int RModeBits = 22; // Rounding Mode control field. + + return (FPRounding)((State.Fpcr >> RModeBits) & 0b11); + } + } +}