Fix FRSQRTS and FCM* (scalar) instructions

1 files changed, 30 insertions, 7 deletions

diff --git a/ChocolArm64/Instruction/AInstEmitSimdArithmetic.cs b/ChocolArm64/Instruction/AInstEmitSimdArithmetic.cs
index 772b7955..bf119a18 100644
--- a/ChocolArm64/Instruction/AInstEmitSimdArithmetic.cs
+++ b/ChocolArm64/Instruction/AInstEmitSimdArithmetic.cs
@@ -512,22 +512,34 @@ namespace ChocolArm64.Instruction
 
         public static void Frsqrts_S(AILEmitterCtx Context)
         {
-            EmitScalarBinaryOpF(Context, () => EmitFrsqrts(Context));
+            EmitFrsqrts(Context, 0, Scalar: true);
         }
 
         public static void Frsqrts_V(AILEmitterCtx Context)
         {
-            EmitVectorBinaryOpF(Context, () => EmitFrsqrts(Context));
+            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++)
+            {
+                EmitFrsqrts(Context, Index, Scalar: false);
+            }
+
+            if (Op.RegisterSize == ARegisterSize.SIMD64)
+            {
+                EmitVectorZeroUpper(Context, Op.Rd);
+            }
         }
 
-        private static void EmitFrsqrts(AILEmitterCtx Context)
+        private static void EmitFrsqrts(AILEmitterCtx Context, int Index, bool Scalar)
         {
-            IAOpCodeSimd Op = (IAOpCodeSimd)Context.CurrOp;
+            AOpCodeSimdReg Op = (AOpCodeSimdReg)Context.CurrOp;
 
             int SizeF = Op.Size & 1;
 
-            Context.Emit(OpCodes.Mul);
-
             if (SizeF == 0)
             {
                 Context.EmitLdc_R4(3);
@@ -537,7 +549,11 @@ namespace ChocolArm64.Instruction
                 Context.EmitLdc_R8(3);
             }
 
-            Context.Emit(OpCodes.Add);
+            EmitVectorExtractF(Context, Op.Rn, Index, SizeF);
+            EmitVectorExtractF(Context, Op.Rm, Index, SizeF);
+
+            Context.Emit(OpCodes.Mul);
+            Context.Emit(OpCodes.Sub);
 
             if (SizeF == 0)
             {
@@ -549,6 +565,13 @@ namespace ChocolArm64.Instruction
             }
 
             Context.Emit(OpCodes.Mul);
+
+            if (Scalar)
+            {
+                EmitVectorZeroAll(Context, Op.Rd);
+            }
+
+            EmitVectorInsertF(Context, Op.Rd, Index, SizeF);
         }
 
         public static void Fsqrt_S(AILEmitterCtx Context)