Add FRSQRTS and FCM* instructions

3 files changed, 208 insertions, 18 deletions

diff --git a/ChocolArm64/AOpCodeTable.cs b/ChocolArm64/AOpCodeTable.cs
index 3042dbc4..483594e2 100644
--- a/ChocolArm64/AOpCodeTable.cs
+++ b/ChocolArm64/AOpCodeTable.cs
@@ -168,6 +168,22 @@ namespace ChocolArm64
             Set("0>1011100<1xxxxx110101xxxxxxxxxx", AInstEmit.Faddp_V,       typeof(AOpCodeSimdReg));
             Set("000111100x1xxxxxxxxx01xxxxx0xxxx", AInstEmit.Fccmp_S,       typeof(AOpCodeSimdFcond));
             Set("000111100x1xxxxxxxxx01xxxxx1xxxx", AInstEmit.Fccmpe_S,      typeof(AOpCodeSimdFcond));
+            Set("010111100x1xxxxx111001xxxxxxxxxx", AInstEmit.Fcmeq_S,       typeof(AOpCodeSimdReg));
+            Set("0>0011100<1xxxxx111001xxxxxxxxxx", AInstEmit.Fcmeq_V,       typeof(AOpCodeSimdReg));
+            Set("010111101x100000110110xxxxxxxxxx", AInstEmit.Fcmeq_S,       typeof(AOpCodeSimd));
+            Set("0>0011101<100000110110xxxxxxxxxx", AInstEmit.Fcmeq_V,       typeof(AOpCodeSimd));
+            Set("011111100x1xxxxx111001xxxxxxxxxx", AInstEmit.Fcmge_S,       typeof(AOpCodeSimdReg));
+            Set("0>1011100<1xxxxx111001xxxxxxxxxx", AInstEmit.Fcmge_V,       typeof(AOpCodeSimdReg));
+            Set("011111101x100000110010xxxxxxxxxx", AInstEmit.Fcmge_S,       typeof(AOpCodeSimd));
+            Set("0>1011101<100000110010xxxxxxxxxx", AInstEmit.Fcmge_V,       typeof(AOpCodeSimd));
+            Set("011111101x1xxxxx111001xxxxxxxxxx", AInstEmit.Fcmgt_S,       typeof(AOpCodeSimdReg));
+            Set("0>1011101<1xxxxx111001xxxxxxxxxx", AInstEmit.Fcmgt_V,       typeof(AOpCodeSimdReg));
+            Set("010111101x100000110010xxxxxxxxxx", AInstEmit.Fcmgt_S,       typeof(AOpCodeSimd));
+            Set("0>0011101<100000110010xxxxxxxxxx", AInstEmit.Fcmgt_V,       typeof(AOpCodeSimd));
+            Set("011111101x100000110110xxxxxxxxxx", AInstEmit.Fcmle_S,       typeof(AOpCodeSimd));
+            Set("0>1011101<100000110110xxxxxxxxxx", AInstEmit.Fcmle_V,       typeof(AOpCodeSimd));
+            Set("010111101x100000111010xxxxxxxxxx", AInstEmit.Fcmlt_S,       typeof(AOpCodeSimd));
+            Set("0>0011101<100000111010xxxxxxxxxx", AInstEmit.Fcmlt_V,       typeof(AOpCodeSimd));
             Set("000111100x1xxxxx001000xxxxx0x000", AInstEmit.Fcmp_S,        typeof(AOpCodeSimdReg));
             Set("000111100x1xxxxx001000xxxxx1x000", AInstEmit.Fcmpe_S,       typeof(AOpCodeSimdReg));
             Set("000111100x1xxxxxxxxx11xxxxxxxxxx", AInstEmit.Fcsel_S,       typeof(AOpCodeSimdFcond));
@@ -227,6 +243,8 @@ namespace ChocolArm64
             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("010111101x1xxxxx111111xxxxxxxxxx", AInstEmit.Frsqrts_S,     typeof(AOpCodeSimdReg));
+            Set("0>0011101<1xxxxx111111xxxxxxxxxx", AInstEmit.Frsqrts_V,     typeof(AOpCodeSimdReg));
             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 4ed5f063..9fb33878 100644
--- a/ChocolArm64/Instruction/AInstEmitSimdArithmetic.cs
+++ b/ChocolArm64/Instruction/AInstEmitSimdArithmetic.cs
@@ -492,6 +492,47 @@ namespace ChocolArm64.Instruction
             });
         }
 
+        public static void Frsqrts_S(AILEmitterCtx Context)
+        {
+            EmitScalarBinaryOpF(Context, () => EmitFrsqrts(Context));
+        }
+
+        public static void Frsqrts_V(AILEmitterCtx Context)
+        {
+            EmitVectorBinaryOpF(Context, () => EmitFrsqrts(Context));
+        }
+
+        private static void EmitFrsqrts(AILEmitterCtx Context)
+        {
+            IAOpCodeSimd Op = (IAOpCodeSimd)Context.CurrOp;
+
+            int SizeF = Op.Size & 1;
+
+            Context.Emit(OpCodes.Mul);
+
+            if (SizeF == 0)
+            {
+                Context.EmitLdc_R4(3);
+            }
+            else /* if (SizeF == 1) */
+            {
+                Context.EmitLdc_R8(3);
+            }
+
+            Context.Emit(OpCodes.Add);
+
+            if (SizeF == 0)
+            {
+                Context.EmitLdc_R4(0.5f);
+            }
+            else /* if (SizeF == 1) */
+            {
+                Context.EmitLdc_R8(0.5);
+            }
+
+            Context.Emit(OpCodes.Mul);
+        }
+
         public static void Fsqrt_S(AILEmitterCtx Context)
         {
             EmitScalarUnaryOpF(Context, () =>
diff --git a/ChocolArm64/Instruction/AInstEmitSimdCmp.cs b/ChocolArm64/Instruction/AInstEmitSimdCmp.cs
index 76861b73..43e8e949 100644
--- a/ChocolArm64/Instruction/AInstEmitSimdCmp.cs
+++ b/ChocolArm64/Instruction/AInstEmitSimdCmp.cs
@@ -110,13 +110,84 @@ namespace ChocolArm64.Instruction
             Fccmp_S(Context);
         }
 
+        public static void Fcmeq_S(AILEmitterCtx Context)
+        {
+            EmitScalarFcmp(Context, OpCodes.Beq_S);
+        }
+
+        public static void Fcmeq_V(AILEmitterCtx Context)
+        {
+            EmitVectorFcmp(Context, OpCodes.Beq_S);
+        }
+
+        public static void Fcmge_S(AILEmitterCtx Context)
+        {
+            EmitScalarFcmp(Context, OpCodes.Bge_S);
+        }
+
+        public static void Fcmge_V(AILEmitterCtx Context)
+        {
+            EmitVectorFcmp(Context, OpCodes.Bge_S);
+        }
+
+        public static void Fcmgt_S(AILEmitterCtx Context)
+        {
+            EmitScalarFcmp(Context, OpCodes.Bgt_S);
+        }
+
+        public static void Fcmgt_V(AILEmitterCtx Context)
+        {
+            EmitVectorFcmp(Context, OpCodes.Bgt_S);
+        }
+
+        public static void Fcmhi_S(AILEmitterCtx Context)
+        {
+            EmitScalarFcmp(Context, OpCodes.Bgt_Un_S);
+        }
+
+        public static void Fcmhi_V(AILEmitterCtx Context)
+        {
+            EmitVectorFcmp(Context, OpCodes.Bgt_Un_S);
+        }
+
+        public static void Fcmhs_S(AILEmitterCtx Context)
+        {
+            EmitScalarFcmp(Context, OpCodes.Bge_Un_S);
+        }
+
+        public static void Fcmhs_V(AILEmitterCtx Context)
+        {
+            EmitVectorFcmp(Context, OpCodes.Bge_Un_S);
+        }
+
+        public static void Fcmle_S(AILEmitterCtx Context)
+        {
+            EmitScalarFcmp(Context, OpCodes.Ble_S);
+        }
+
+        public static void Fcmle_V(AILEmitterCtx Context)
+        {
+            EmitVectorFcmp(Context, OpCodes.Ble_S);
+        }
+
+        public static void Fcmlt_S(AILEmitterCtx Context)
+        {
+            EmitScalarFcmp(Context, OpCodes.Blt_S);
+        }
+
+        public static void Fcmlt_V(AILEmitterCtx Context)
+        {
+            EmitVectorFcmp(Context, OpCodes.Blt_S);
+        }
+
         public static void Fcmp_S(AILEmitterCtx Context)
         {
             AOpCodeSimdReg Op = (AOpCodeSimdReg)Context.CurrOp;
 
             bool CmpWithZero = !(Op is AOpCodeSimdFcond) ? Op.Bit3 : false;
 
-            //Handle NaN case. If any number is NaN, then NZCV = 0011.
+            //Handle NaN case.
+            //If any number is NaN, then NZCV = 0011.
             if (CmpWithZero)
             {
                 EmitNaNCheck(Context, Op.Rn);
@@ -140,7 +211,14 @@ namespace ChocolArm64.Instruction
 
                 if (CmpWithZero)
                 {
-                    EmitLdcImmF(Context, 0, Op.Size);
+                    if (Op.Size == 0)
+                    {
+                        Context.EmitLdc_R4(0);
+                    }
+                    else /* if (SizeF == 1) */
+                    {
+                        Context.EmitLdc_R8(0);
+                    }
                 }
                 else
                 {
@@ -190,22 +268,6 @@ namespace ChocolArm64.Instruction
             Fcmp_S(Context);
         }
 
-        private static void EmitLdcImmF(AILEmitterCtx Context, double ImmF, int Size)
-        {
-            if (Size == 0)
-            {
-                Context.EmitLdc_R4((float)ImmF);
-            }
-            else if (Size == 1)
-            {
-                Context.EmitLdc_R8(ImmF);
-            }
-            else
-            {
-                throw new ArgumentOutOfRangeException(nameof(Size));
-            }
-        }
-
         private static void EmitNaNCheck(AILEmitterCtx Context, int Reg)
         {
             IAOpCodeSimd Op = (IAOpCodeSimd)Context.CurrOp;
@@ -268,5 +330,74 @@ namespace ChocolArm64.Instruction
                 EmitVectorZeroUpper(Context, Op.Rd);
             }
         }
+
+        private static void EmitScalarFcmp(AILEmitterCtx Context, OpCode ILOp)
+        {
+            AOpCodeSimd Op = (AOpCodeSimd)Context.CurrOp;
+
+            int SizeF = Op.Size & 1;
+
+            EmitFcmp(Context, ILOp, 0);
+
+            EmitScalarSetF(Context, Op.Rd, SizeF);
+        }
+
+        private static void EmitVectorFcmp(AILEmitterCtx Context, OpCode ILOp)
+        {
+            AOpCodeSimd Op = (AOpCodeSimd)Context.CurrOp;
+
+            int SizeF = Op.Size & 1;
+
+            int Bytes = Context.CurrOp.GetBitsCount() >> 3;
+
+            for (int Index = 0; Index < Bytes >> SizeF + 2; Index++)
+            {
+                EmitFcmp(Context, ILOp, Index);
+            }
+
+            if (Op.RegisterSize == ARegisterSize.SIMD64)
+            {
+                EmitVectorZeroUpper(Context, Op.Rd);
+            }
+        }
+
+        private static void EmitFcmp(AILEmitterCtx Context, OpCode ILOp, int Index)
+        {
+            AOpCodeSimd Op = (AOpCodeSimd)Context.CurrOp;
+
+            int SizeF = Op.Size & 1;
+
+            ulong SzMask = ulong.MaxValue >> (64 - (32 << SizeF));
+
+            EmitVectorExtractF(Context, Op.Rn, Index, SizeF);
+
+            if (Op is AOpCodeSimdReg BinOp)
+            {
+                EmitVectorExtractF(Context, BinOp.Rm, Index, SizeF);
+            }
+            else if (SizeF == 0)
+            {
+                Context.EmitLdc_R4(0);
+            }
+            else /* if (SizeF == 1) */
+            {
+                Context.EmitLdc_R8(0);
+            }
+
+            AILLabel LblTrue = new AILLabel();
+            AILLabel LblEnd  = new AILLabel();
+
+            Context.Emit(ILOp, LblTrue);
+
+            EmitVectorInsert(Context, Op.Rd, Index, SizeF + 2, 0);
+
+            Context.Emit(OpCodes.Br_S, LblEnd);
+
+            Context.MarkLabel(LblTrue);
+
+            EmitVectorInsert(Context, Op.Rd, Index, SizeF + 2, (long)SzMask);
+
+            Context.MarkLabel(LblEnd);
+        }
     }
 }
\ No newline at end of file