translate_arm/vfp2: Implement VSTM (A1, A2)

src/frontend/decoder/vfp2.h

@@ -95,13 +95,12 @@ boost::optional<const VFP2Matcher<V>&> DecodeVFP2(u32 instruction) {
     // VCVTR
 
     // Extension register load-store instructions
-    // VSTR
-    // VSTM
-    // VSTMDB
     INST(&V::vfp2_VPUSH,          "VPUSH",                   "cccc11010D101101dddd101zvvvvvvvv"),
     INST(&V::vfp2_VPOP,           "VPOP",                    "cccc11001D111101dddd101zvvvvvvvv"),
     INST(&V::vfp2_VLDR,           "VLDR",                    "cccc1101UD01nnnndddd101zvvvvvvvv"),
     INST(&V::vfp2_VSTR,           "VSTR",                    "cccc1101UD00nnnndddd101zvvvvvvvv"),
+    INST(&V::vfp2_VSTM_a1,        "VSTM (A1)",               "cccc110puDw0nnnndddd1011vvvvvvvv"),
+    INST(&V::vfp2_VSTM_a2,        "VSTM (A2)",               "cccc110puDw0nnnndddd1010vvvvvvvv"),
     INST(&V::vfp2_VLDM_a1,        "VLDM (A1)",               "cccc110puDw1nnnndddd1011vvvvvvvv"),
     INST(&V::vfp2_VLDM_a2,        "VLDM (A2)",               "cccc110puDw1nnnndddd1010vvvvvvvv"),
 

src/frontend/disassembler/disassembler_arm.cpp

@@ -889,6 +889,20 @@ public:
         return Common::StringFromFormat("vstr%s %s, [%s, #%c%u]", CondToString(cond), FPRegStr(sz, Vd, D).c_str(), RegToString(n), U ? '+' : '-', imm32);
     }
 
+    std::string vfp2_VSTM_a1(Cond cond, bool p, bool u, bool D, bool w, Reg n, size_t Vd, Imm8 imm8) {
+        const char* mode = "<invalid mode>";
+        if (!p && u) mode = "ia";
+        if (p && !u) mode = "db";
+        return Common::StringFromFormat("vstm%s%s.f64 %s%s, %s(+%u)", mode, CondToString(cond), RegToString(n), w ? "!" : "", FPRegStr(true, Vd, D).c_str(), imm8);
+    }
+
+    std::string vfp2_VSTM_a2(Cond cond, bool p, bool u, bool D, bool w, Reg n, size_t Vd, Imm8 imm8) {
+        const char* mode = "<invalid mode>";
+        if (!p && u) mode = "ia";
+        if (p && !u) mode = "db";
+        return Common::StringFromFormat("vstm%s%s.f32 %s%s, %s(+%u)", mode, CondToString(cond), RegToString(n), w ? "!" : "", FPRegStr(false, Vd, D).c_str(), imm8);
+    }
+
     std::string vfp2_VLDM_a1(Cond cond, bool p, bool u, bool D, bool w, Reg n, size_t Vd, Imm8 imm8) {
         const char* mode = "<invalid mode>";
         if (!p && u) mode = "ia";

src/frontend/translate/translate_arm/translate_arm.h

@@ -363,6 +363,8 @@ struct ArmTranslatorVisitor final {
     bool vfp2_VSTR(Cond cond, bool U, bool D, Reg n, size_t Vd, bool sz, Imm8 imm8);
     bool vfp2_VPOP(Cond cond, bool D, size_t Vd, bool sz, Imm8 imm8);
     bool vfp2_VPUSH(Cond cond, bool D, size_t Vd, bool sz, Imm8 imm8);
+    bool vfp2_VSTM_a1(Cond cond, bool p, bool u, bool D, bool w, Reg n, size_t Vd, Imm8 imm8);
+    bool vfp2_VSTM_a2(Cond cond, bool p, bool u, bool D, bool w, Reg n, size_t Vd, Imm8 imm8);
     bool vfp2_VLDM_a1(Cond cond, bool p, bool u, bool D, bool w, Reg n, size_t Vd, Imm8 imm8);
     bool vfp2_VLDM_a2(Cond cond, bool p, bool u, bool D, bool w, Reg n, size_t Vd, Imm8 imm8);
 };

src/frontend/translate/translate_arm/vfp2.cpp

@@ -467,6 +467,73 @@ bool ArmTranslatorVisitor::vfp2_VSTR(Cond cond, bool U, bool D, Reg n, size_t Vd
     return true;
 }
 
+bool ArmTranslatorVisitor::vfp2_VSTM_a1(Cond cond, bool p, bool u, bool D, bool w, Reg n, size_t Vd, Imm8 imm8) {
+    if (!p && !u && !w)
+        ASSERT_MSG(false, "Decode error");
+    if (p && !w)
+        ASSERT_MSG(false, "Decode error");
+    if (p == u && w)
+        return arm_UDF();
+    if (n == Reg::PC && w)
+        return UnpredictableInstruction();
+
+    ExtReg d = ToExtReg(true, Vd, D);
+    u32 imm32 = imm8 << 2;
+    size_t regs = imm8 / 2;
+
+    if (regs == 0 || regs > 16 || Arm::RegNumber(d)+regs > 32)
+        return UnpredictableInstruction();
+
+    // VSTM<mode>.F64 <Rn>{!}, <list of double registers>
+    if (ConditionPassed(cond)) {
+        auto address = u ? ir.GetRegister(n) : ir.Sub(ir.GetRegister(n), ir.Imm32(imm32));
+        if (w)
+            ir.SetRegister(n, u ? ir.Add(address, ir.Imm32(imm32)) : address);
+        for (size_t i = 0; i < regs; i++) {
+            auto value = ir.TransferFromFP64(ir.GetExtendedRegister(d + i));
+            auto word1 = ir.LeastSignificantWord(value);
+            auto word2 = ir.MostSignificantWord(value).result;
+            if (ir.current_location.EFlag()) std::swap(word1, word2);
+            ir.WriteMemory32(address, word1);
+            address = ir.Add(address, ir.Imm32(4));
+            ir.WriteMemory32(address, word2);
+            address = ir.Add(address, ir.Imm32(4));
+        }
+    }
+    return true;
+}
+
+bool ArmTranslatorVisitor::vfp2_VSTM_a2(Cond cond, bool p, bool u, bool D, bool w, Reg n, size_t Vd, Imm8 imm8) {
+    if (!p && !u && !w)
+        ASSERT_MSG(false, "Decode error");
+    if (p && !w)
+        ASSERT_MSG(false, "Decode error");
+    if (p == u && w)
+        return arm_UDF();
+    if (n == Reg::PC && w)
+        return UnpredictableInstruction();
+
+    ExtReg d = ToExtReg(false, Vd, D);
+    u32 imm32 = imm8 << 2;
+    size_t regs = imm8;
+
+    if (regs == 0 || Arm::RegNumber(d)+regs > 32)
+        return UnpredictableInstruction();
+
+    // VSTM<mode>.F32 <Rn>{!}, <list of single registers>
+    if (ConditionPassed(cond)) {
+        auto address = u ? ir.GetRegister(n) : ir.Sub(ir.GetRegister(n), ir.Imm32(imm32));
+        if (w)
+            ir.SetRegister(n, u ? ir.Add(address, ir.Imm32(imm32)) : address);
+        for (size_t i = 0; i < regs; i++) {
+            auto word = ir.TransferFromFP32(ir.GetExtendedRegister(d + i));
+            ir.WriteMemory32(address, word);
+            address = ir.Add(address, ir.Imm32(4));
+        }
+    }
+    return true;
+}
+
 bool ArmTranslatorVisitor::vfp2_VLDM_a1(Cond cond, bool p, bool u, bool D, bool w, Reg n, size_t Vd, Imm8 imm8) {
     if (!p && !u && !w)
         ASSERT_MSG(false, "Decode error");
@@ -479,7 +546,7 @@ bool ArmTranslatorVisitor::vfp2_VLDM_a1(Cond cond, bool p, bool u, bool D, bool
 
     ExtReg d = ToExtReg(true, Vd, D);
     u32 imm32 = imm8 << 2;
-    size_t regs = imm8;
+    size_t regs = imm8 / 2;
 
     if (regs == 0 || regs > 16 || Arm::RegNumber(d)+regs > 32)
         return UnpredictableInstruction();
@@ -532,6 +599,5 @@ bool ArmTranslatorVisitor::vfp2_VLDM_a2(Cond cond, bool p, bool u, bool D, bool
     return true;
 }
 
-
 } // namespace Arm
 } // namespace Dynarmic