From 8e97b10acba481437734a180703d75283c1df17c Mon Sep 17 00:00:00 2001
From: MerryMage <MerryMage@users.noreply.github.com>
Date: Sun, 10 May 2020 14:14:03 +0100
Subject: [PATCH] VFPv4: Implement VFNMS, VFNMA

---
 src/frontend/A32/decoder/vfp.inc              |  4 +-
 .../A32/disassembler/disassembler_arm.cpp     |  8 ++++
 .../A32/translate/impl/translate_arm.h        |  2 +
 src/frontend/A32/translate/impl/vfp.cpp       | 40 +++++++++++++++++++
 4 files changed, 52 insertions(+), 2 deletions(-)

diff --git a/src/frontend/A32/decoder/vfp.inc b/src/frontend/A32/decoder/vfp.inc
index 70a42a42..a3084187 100644
--- a/src/frontend/A32/decoder/vfp.inc
+++ b/src/frontend/A32/decoder/vfp.inc
@@ -8,8 +8,8 @@ INST(vfp_VNMUL,            "VNMUL",                   "cccc11100D10nnnndddd101zN
 INST(vfp_VADD,             "VADD",                    "cccc11100D11nnnndddd101zN0M0mmmm") // VFPv2
 INST(vfp_VSUB,             "VSUB",                    "cccc11100D11nnnndddd101zN1M0mmmm") // VFPv2
 INST(vfp_VDIV,             "VDIV",                    "cccc11101D00nnnndddd101zN0M0mmmm") // VFPv2
-//INST(vfp_VFNMA,            "VFNMA",                   "cccc11101D01nnnndddd101zN0M0mmmm") // VFPv4
-//INST(vfp_VFNMS,            "VFNMS",                   "cccc11101D01nnnndddd101zN1M0mmmm") // VFPv4
+INST(vfp_VFNMS,            "VFNMS",                   "cccc11101D01nnnndddd101zN0M0mmmm") // VFPv4
+INST(vfp_VFNMA,            "VFNMA",                   "cccc11101D01nnnndddd101zN1M0mmmm") // VFPv4
 //INST(vfp_VFMA,             "VFMA",                    "cccc11101D10nnnndddd101zN0M0mmmm") // VFPv4
 //INST(vfp_VFMS,             "VFMS",                    "cccc11101D10nnnndddd101zN1M0mmmm") // VFPv4
 
diff --git a/src/frontend/A32/disassembler/disassembler_arm.cpp b/src/frontend/A32/disassembler/disassembler_arm.cpp
index 75701a46..870617dc 100644
--- a/src/frontend/A32/disassembler/disassembler_arm.cpp
+++ b/src/frontend/A32/disassembler/disassembler_arm.cpp
@@ -1216,6 +1216,14 @@ public:
         return fmt::format("vdiv{}.{} {}, {}, {}", CondToString(cond), sz ? "f64" : "f32", FPRegStr(sz, Vd, D), FPRegStr(sz, Vn, N), FPRegStr(sz, Vm, M));
     }
 
+    std::string vfp_VFNMS(Cond cond, bool D, size_t Vn, size_t Vd, bool sz, bool N, bool M, size_t Vm) {
+        return fmt::format("vfnms{}.{} {}, {}, {}", CondToString(cond), sz ? "f64" : "f32", FPRegStr(sz, Vd, D), FPRegStr(sz, Vn, N), FPRegStr(sz, Vm, M));
+    }
+
+    std::string vfp_VFNMA(Cond cond, bool D, size_t Vn, size_t Vd, bool sz, bool N, bool M, size_t Vm) {
+        return fmt::format("vfmna{}.{} {}, {}, {}", CondToString(cond), sz ? "f64" : "f32", FPRegStr(sz, Vd, D), FPRegStr(sz, Vn, N), FPRegStr(sz, Vm, M));
+    }
+
     std::string vfp_VMOV_u32_f64(Cond cond, size_t Vd, Reg t, bool D){
         return fmt::format("vmov{}.32 {}, {}", CondToString(cond), FPRegStr(true, Vd, D), t);
     }
diff --git a/src/frontend/A32/translate/impl/translate_arm.h b/src/frontend/A32/translate/impl/translate_arm.h
index 6df2fa02..80dad244 100644
--- a/src/frontend/A32/translate/impl/translate_arm.h
+++ b/src/frontend/A32/translate/impl/translate_arm.h
@@ -371,6 +371,8 @@ struct ArmTranslatorVisitor final {
     bool vfp_VNMLA(Cond cond, bool D, size_t Vn, size_t Vd, bool sz, bool N, bool M, size_t Vm);
     bool vfp_VNMLS(Cond cond, bool D, size_t Vn, size_t Vd, bool sz, bool N, bool M, size_t Vm);
     bool vfp_VDIV(Cond cond, bool D, size_t Vn, size_t Vd, bool sz, bool N, bool M, size_t Vm);
+    bool vfp_VFNMS(Cond cond, bool D, size_t Vn, size_t Vd, bool sz, bool N, bool M, size_t Vm);
+    bool vfp_VFNMA(Cond cond, bool D, size_t Vn, size_t Vd, bool sz, bool N, bool M, size_t Vm);
 
     // Floating-point move instructions
     bool vfp_VMOV_u32_f64(Cond cond, size_t Vd, Reg t, bool D);
diff --git a/src/frontend/A32/translate/impl/vfp.cpp b/src/frontend/A32/translate/impl/vfp.cpp
index 1e1b991f..6785596b 100644
--- a/src/frontend/A32/translate/impl/vfp.cpp
+++ b/src/frontend/A32/translate/impl/vfp.cpp
@@ -264,6 +264,46 @@ bool ArmTranslatorVisitor::vfp_VDIV(Cond cond, bool D, size_t Vn, size_t Vd, boo
     });
 }
 
+// VFNMS<c>.F64 <Dd>, <Dn>, <Dm>
+// VFNMS<c>.F32 <Sd>, <Sn>, <Sm>
+bool ArmTranslatorVisitor::vfp_VFNMS(Cond cond, bool D, size_t Vn, size_t Vd, bool sz, bool N, bool M, size_t Vm) {
+    if (!ConditionPassed(cond)) {
+        return true;
+    }
+
+    const auto d = ToExtReg(sz, Vd, D);
+    const auto n = ToExtReg(sz, Vn, N);
+    const auto m = ToExtReg(sz, Vm, M);
+
+    return EmitVfpVectorOperation(sz, d, n, m, [this](ExtReg d, ExtReg n, ExtReg m) {
+        const auto reg_n = ir.GetExtendedRegister(n);
+        const auto reg_m = ir.GetExtendedRegister(m);
+        const auto reg_d = ir.GetExtendedRegister(d);
+        const auto result = ir.FPMulAdd(ir.FPNeg(reg_d), reg_n, reg_m, true);
+        ir.SetExtendedRegister(d, result);
+    });
+}
+
+// VFNMA<c>.F64 <Dd>, <Dn>, <Dm>
+// VFNMA<c>.F32 <Sd>, <Sn>, <Sm>
+bool ArmTranslatorVisitor::vfp_VFNMA(Cond cond, bool D, size_t Vn, size_t Vd, bool sz, bool N, bool M, size_t Vm) {
+    if (!ConditionPassed(cond)) {
+        return true;
+    }
+
+    const auto d = ToExtReg(sz, Vd, D);
+    const auto n = ToExtReg(sz, Vn, N);
+    const auto m = ToExtReg(sz, Vm, M);
+
+    return EmitVfpVectorOperation(sz, d, n, m, [this](ExtReg d, ExtReg n, ExtReg m) {
+        const auto reg_n = ir.GetExtendedRegister(n);
+        const auto reg_m = ir.GetExtendedRegister(m);
+        const auto reg_d = ir.GetExtendedRegister(d);
+        const auto result = ir.FPMulAdd(ir.FPNeg(reg_d), ir.FPNeg(reg_n), reg_m, true);
+        ir.SetExtendedRegister(d, result);
+    });
+}
+
 // VMOV<c>.32 <Dd[0]>, <Rt>
 bool ArmTranslatorVisitor::vfp_VMOV_u32_f64(Cond cond, size_t Vd, Reg t, bool D) {
     if (t == Reg::PC) {