From f679f8d0635b89175110334dad97bcbcf799cb14 Mon Sep 17 00:00:00 2001
From: Connor McLaughlin <stenzek@gmail.com>
Date: Sat, 9 May 2020 13:48:13 +1000
Subject: [PATCH] GTE: Fix buggy MVMVA instruction

---
 src/core/gte.cpp | 32 +++++++++++++++++++++++++-------
 src/core/gte.h   |  1 +
 2 files changed, 26 insertions(+), 7 deletions(-)

diff --git a/src/core/gte.cpp b/src/core/gte.cpp
index 454623177..e9bb0e948 100644
--- a/src/core/gte.cpp
+++ b/src/core/gte.cpp
@@ -405,6 +405,26 @@ void Core::MulMatVec(const s16 M[3][3], const s32 T[3], const s16 Vx, const s16
 #undef dot3
 }
 
+void Core::MulMatVecBuggy(const s16 M[3][3], const s32 T[3], const s16 Vx, const s16 Vy, const s16 Vz, u8 shift,
+                          bool lm)
+{
+#define dot3(i)                                                                                                        \
+  do                                                                                                                   \
+  {                                                                                                                    \
+    TruncateAndSetIR<i + 1>(                                                                                           \
+      SignExtendMACResult<i + 1>(SignExtendMACResult<i + 1>((s64(T[i]) << 12) + (s64(M[i][0]) * s64(Vx)))) >> shift,   \
+      false);                                                                                                          \
+    TruncateAndSetMACAndIR<i + 1>(SignExtendMACResult<i + 1>((s64(M[i][1]) * s64(Vy))) + (s64(M[i][2]) * s64(Vz)),     \
+                                  shift, lm);                                                                          \
+  } while (0)
+
+  dot3(0);
+  dot3(1);
+  dot3(2);
+
+#undef dot3
+}
+
 void Core::Execute_MVMVA(Instruction inst)
 {
   m_regs.FLAG.Clear();
@@ -463,25 +483,23 @@ void Core::Execute_MVMVA(Instruction inst)
       break;
   }
 
-  s32 T[3];
+  static const s32 zero_T[3] = {};
   switch (inst.mvmva_translation_vector)
   {
     case 0:
-      std::memcpy(T, m_regs.TR, sizeof(T));
+      MulMatVec(M, m_regs.TR, Vx, Vy, Vz, inst.GetShift(), inst.lm);
       break;
     case 1:
-      std::memcpy(T, m_regs.BK, sizeof(T));
+      MulMatVec(M, m_regs.BK, Vx, Vy, Vz, inst.GetShift(), inst.lm);
       break;
     case 2:
-      std::memcpy(T, m_regs.FC, sizeof(T));
+      MulMatVecBuggy(M, m_regs.FC, Vx, Vy, Vz, inst.GetShift(), inst.lm);
       break;
     default:
-      std::fill_n(T, countof(T), s32(0));
+      MulMatVec(M, zero_T, Vx, Vy, Vz, inst.GetShift(), inst.lm);
       break;
   }
 
-  MulMatVec(M, T, Vx, Vy, Vz, inst.GetShift(), inst.lm);
-
   m_regs.FLAG.UpdateError();
 }
 
diff --git a/src/core/gte.h b/src/core/gte.h
index 1b80609fa..bd5a91e60 100644
--- a/src/core/gte.h
+++ b/src/core/gte.h
@@ -74,6 +74,7 @@ private:
 
   // 3x3 matrix * 3x1 vector with translation, updates MAC[1-3] and IR[1-3]
   void MulMatVec(const s16 M[3][3], const s32 T[3], const s16 Vx, const s16 Vy, const s16 Vz, u8 shift, bool lm);
+  void MulMatVecBuggy(const s16 M[3][3], const s32 T[3], const s16 Vx, const s16 Vy, const s16 Vz, u8 shift, bool lm);
 
   // Interpolate colour, or as in nocash "MAC+(FC-MAC)*IR0".
   void InterpolateColor(s64 in_MAC1, s64 in_MAC2, s64 in_MAC3, u8 shift, bool lm);