From a90b0a1c93686d1a132110ed2b27dcf1db534490 Mon Sep 17 00:00:00 2001
From: JosJuice <josjuice@gmail.com>
Date: Sat, 31 Jul 2021 12:34:04 +0200
Subject: [PATCH] JitArm64: Implement mtfsfx

The sixth and final part of implementing the FPSCR system register
instructions.
---
 Source/Core/Common/Arm64Emitter.cpp           |  8 ++++
 Source/Core/Common/Arm64Emitter.h             |  2 +
 Source/Core/Core/PowerPC/JitArm64/Jit.h       |  1 +
 .../JitArm64/JitArm64_SystemRegisters.cpp     | 41 +++++++++++++++++++
 .../Core/PowerPC/JitArm64/JitArm64_Tables.cpp | 12 +++---
 5 files changed, 58 insertions(+), 6 deletions(-)

diff --git a/Source/Core/Common/Arm64Emitter.cpp b/Source/Core/Common/Arm64Emitter.cpp
index 4e6ecc450e..ce3c0a5103 100644
--- a/Source/Core/Common/Arm64Emitter.cpp
+++ b/Source/Core/Common/Arm64Emitter.cpp
@@ -3008,6 +3008,14 @@ void ARM64FloatEmitter::BIC(ARM64Reg Rd, ARM64Reg Rn, ARM64Reg Rm)
 {
   EmitThreeSame(0, 1, 3, Rd, Rn, Rm);
 }
+void ARM64FloatEmitter::BIF(ARM64Reg Rd, ARM64Reg Rn, ARM64Reg Rm)
+{
+  EmitThreeSame(1, 3, 3, Rd, Rn, Rm);
+}
+void ARM64FloatEmitter::BIT(ARM64Reg Rd, ARM64Reg Rn, ARM64Reg Rm)
+{
+  EmitThreeSame(1, 2, 3, Rd, Rn, Rm);
+}
 void ARM64FloatEmitter::BSL(ARM64Reg Rd, ARM64Reg Rn, ARM64Reg Rm)
 {
   EmitThreeSame(1, 1, 3, Rd, Rn, Rm);
diff --git a/Source/Core/Common/Arm64Emitter.h b/Source/Core/Common/Arm64Emitter.h
index c5ccf41c74..a5a4c03e4e 100644
--- a/Source/Core/Common/Arm64Emitter.h
+++ b/Source/Core/Common/Arm64Emitter.h
@@ -1248,6 +1248,8 @@ public:
   void ADD(u8 size, ARM64Reg Rd, ARM64Reg Rn, ARM64Reg Rm);
   void AND(ARM64Reg Rd, ARM64Reg Rn, ARM64Reg Rm);
   void BIC(ARM64Reg Rd, ARM64Reg Rn, ARM64Reg Rm);
+  void BIF(ARM64Reg Rd, ARM64Reg Rn, ARM64Reg Rm);
+  void BIT(ARM64Reg Rd, ARM64Reg Rn, ARM64Reg Rm);
   void BSL(ARM64Reg Rd, ARM64Reg Rn, ARM64Reg Rm);
   void DUP(u8 size, ARM64Reg Rd, ARM64Reg Rn, u8 index);
   void FABS(u8 size, ARM64Reg Rd, ARM64Reg Rn);
diff --git a/Source/Core/Core/PowerPC/JitArm64/Jit.h b/Source/Core/Core/PowerPC/JitArm64/Jit.h
index 50ccbcea3a..7adea7973e 100644
--- a/Source/Core/Core/PowerPC/JitArm64/Jit.h
+++ b/Source/Core/Core/PowerPC/JitArm64/Jit.h
@@ -122,6 +122,7 @@ public:
   void mtfsb0x(UGeckoInstruction inst);
   void mtfsb1x(UGeckoInstruction inst);
   void mtfsfix(UGeckoInstruction inst);
+  void mtfsfx(UGeckoInstruction inst);
 
   // LoadStore
   void lXX(UGeckoInstruction inst);
diff --git a/Source/Core/Core/PowerPC/JitArm64/JitArm64_SystemRegisters.cpp b/Source/Core/Core/PowerPC/JitArm64/JitArm64_SystemRegisters.cpp
index e0cd9d36a9..47283eb10d 100644
--- a/Source/Core/Core/PowerPC/JitArm64/JitArm64_SystemRegisters.cpp
+++ b/Source/Core/Core/PowerPC/JitArm64/JitArm64_SystemRegisters.cpp
@@ -852,3 +852,44 @@ void JitArm64::mtfsfix(UGeckoInstruction inst)
   if (inst.CRFD == 7)
     UpdateRoundingMode();
 }
+
+void JitArm64::mtfsfx(UGeckoInstruction inst)
+{
+  INSTRUCTION_START
+  JITDISABLE(bJITSystemRegistersOff);
+  FALLBACK_IF(inst.Rc);
+
+  u32 mask = 0;
+  for (int i = 0; i < 8; i++)
+  {
+    if (inst.FM & (1 << i))
+      mask |= 0xFU << (4 * i);
+  }
+
+  if (mask == 0xFFFFFFFF)
+  {
+    ARM64Reg VB = fpr.R(inst.FB, RegType::LowerPair);
+
+    m_float_emit.STR(32, IndexType::Unsigned, VB, PPC_REG, PPCSTATE_OFF(fpscr));
+  }
+  else if (mask != 0)
+  {
+    ARM64Reg VB = fpr.R(inst.FB, RegType::LowerPair);
+
+    ARM64Reg V0 = fpr.GetReg();
+    ARM64Reg V1 = fpr.GetReg();
+    ARM64Reg WA = gpr.GetReg();
+
+    m_float_emit.LDR(32, IndexType::Unsigned, V0, PPC_REG, PPCSTATE_OFF(fpscr));
+    MOVI2R(WA, mask);
+    m_float_emit.FMOV(EncodeRegToSingle(V1), WA);
+    m_float_emit.BIT(EncodeRegToDouble(V0), EncodeRegToDouble(VB), EncodeRegToDouble(V1));
+    m_float_emit.STR(32, IndexType::Unsigned, V0, PPC_REG, PPCSTATE_OFF(fpscr));
+
+    fpr.Unlock(V0, V1);
+    gpr.Unlock(WA);
+  }
+
+  if (inst.FM & 1)
+    UpdateRoundingMode();
+}
diff --git a/Source/Core/Core/PowerPC/JitArm64/JitArm64_Tables.cpp b/Source/Core/Core/PowerPC/JitArm64/JitArm64_Tables.cpp
index aa6255c0ff..dc7f82a6a4 100644
--- a/Source/Core/Core/PowerPC/JitArm64/JitArm64_Tables.cpp
+++ b/Source/Core/Core/PowerPC/JitArm64/JitArm64_Tables.cpp
@@ -314,12 +314,12 @@ constexpr std::array<GekkoOPTemplate, 15> table63{{
     {40, &JitArm64::fp_logic},               // fnegx
     {12, &JitArm64::frspx},                  // frspx
 
-    {64, &JitArm64::mcrfs},                   // mcrfs
-    {583, &JitArm64::mffsx},                  // mffsx
-    {70, &JitArm64::mtfsb0x},                 // mtfsb0x
-    {38, &JitArm64::mtfsb1x},                 // mtfsb1x
-    {134, &JitArm64::mtfsfix},                // mtfsfix
-    {711, &JitArm64::FallBackToInterpreter},  // mtfsfx
+    {64, &JitArm64::mcrfs},     // mcrfs
+    {583, &JitArm64::mffsx},    // mffsx
+    {70, &JitArm64::mtfsb0x},   // mtfsb0x
+    {38, &JitArm64::mtfsb1x},   // mtfsb1x
+    {134, &JitArm64::mtfsfix},  // mtfsfix
+    {711, &JitArm64::mtfsfx},   // mtfsfx
 }};
 
 constexpr std::array<GekkoOPTemplate, 10> table63_2{{