From 7c600ed6fa1ece347eb7c464b4e82cb4bd7ea3a0 Mon Sep 17 00:00:00 2001
From: Connor McLaughlin <mclaughc@outlook.com>
Date: Fri, 4 Oct 2019 17:38:31 +1000
Subject: [PATCH] GTE: Implement CDP

---
 src/core/gte.cpp | 30 ++++++++++++++++++++++++++++++
 src/core/gte.h   |  1 +
 2 files changed, 31 insertions(+)

diff --git a/src/core/gte.cpp b/src/core/gte.cpp
index 8fc6f3326..06b6d54e9 100644
--- a/src/core/gte.cpp
+++ b/src/core/gte.cpp
@@ -286,6 +286,10 @@ void Core::ExecuteInstruction(Instruction inst)
       Execute_NCDS(inst);
       break;
 
+    case 0x14:
+      Execute_CDP(inst);
+      break;
+
     case 0x16:
       Execute_NCCT(inst);
       break;
@@ -827,6 +831,32 @@ void Core::Execute_CC(Instruction inst)
   m_regs.FLAG.UpdateError();
 }
 
+void Core::Execute_CDP(Instruction inst)
+{
+  m_regs.FLAG.Clear();
+
+  const u8 shift = inst.GetShift();
+  const bool lm = inst.lm;
+
+  // [IR1,IR2,IR3] = [MAC1,MAC2,MAC3] = (BK*1000h + LCM*IR) SAR (sf*12)
+  MulMatVec(m_regs.LCM, m_regs.BK, m_regs.IR1, m_regs.IR2, m_regs.IR3, shift, lm);
+
+  // No need to assign these to MAC[1-3], as it'll never overflow.
+  // [MAC1,MAC2,MAC3] = [R*IR1,G*IR2,B*IR3] SHL 4
+  const s32 in_MAC1 = (s32(ZeroExtend32(m_regs.RGBC[0])) * s32(m_regs.IR1)) << 4;
+  const s32 in_MAC2 = (s32(ZeroExtend32(m_regs.RGBC[1])) * s32(m_regs.IR2)) << 4;
+  const s32 in_MAC3 = (s32(ZeroExtend32(m_regs.RGBC[2])) * s32(m_regs.IR3)) << 4;
+
+  // [MAC1,MAC2,MAC3] = MAC+(FC-MAC)*IR0                   ;<--- for CDP only
+  // [MAC1, MAC2, MAC3] = [MAC1, MAC2, MAC3] SAR(sf * 12)
+  InterpolateColor(in_MAC1, in_MAC2, in_MAC3, shift, lm);
+
+  // Color FIFO = [MAC1/16,MAC2/16,MAC3/16,CODE], [IR1,IR2,IR3] = [MAC1,MAC2,MAC3]
+  PushRGBFromMAC();
+
+  m_regs.FLAG.UpdateError();
+}
+
 void Core::DPCS(const u8 color[3], u8 shift, bool lm)
 {
   // In: [IR1,IR2,IR3]=Vector, FC=Far Color, IR0=Interpolation value, CODE=MSB of RGBC
diff --git a/src/core/gte.h b/src/core/gte.h
index a98c9350d..dcd5cba5c 100644
--- a/src/core/gte.h
+++ b/src/core/gte.h
@@ -90,6 +90,7 @@ private:
   void Execute_NCDS(Instruction inst);
   void Execute_NCDT(Instruction inst);
   void Execute_CC(Instruction inst);
+  void Execute_CDP(Instruction inst);
   void Execute_DPCS(Instruction inst);
   void Execute_DPCT(Instruction inst);
   void Execute_DCPL(Instruction inst);