From 3c0b7f7724bb9265ab7331ea9c4a4c7312061c7e Mon Sep 17 00:00:00 2001
From: refractionpcsx2 <refraction@gmail.com>
Date: Sat, 24 Jun 2023 13:15:28 +0100
Subject: [PATCH] IPU: Redo FIFO transfers, plus small IPU_TO DMA tweak

---
 pcsx2/IPU/IPU_Fifo.cpp | 78 ++++++++++++++++++++++--------------------
 pcsx2/IPU/IPU_Fifo.h   |  2 +-
 pcsx2/IPU/IPUdma.cpp   |  2 +-
 3 files changed, 42 insertions(+), 40 deletions(-)

diff --git a/pcsx2/IPU/IPU_Fifo.cpp b/pcsx2/IPU/IPU_Fifo.cpp
index 7d2666da79..3d2b30e8f0 100644
--- a/pcsx2/IPU/IPU_Fifo.cpp
+++ b/pcsx2/IPU/IPU_Fifo.cpp
@@ -72,25 +72,28 @@ std::string IPU_Fifo_Output::desc() const
 	return StringUtil::StdStringFromFormat("IPU Fifo Output: readpos = 0x%x, writepos = 0x%x, data = 0x%x", readpos, writepos, data);
 }
 
-int IPU_Fifo_Input::write(u32* pMem, int size)
+int IPU_Fifo_Input::write(const u32* pMem, int size)
 {
-	int transsize;
-	int firsttrans = std::min(size, 8 - (int)g_BP.IFC);
+	const int transfer_size = std::min(size, 8 - (int)g_BP.IFC);
+	if (!transfer_size) return 0;
 
-	g_BP.IFC += firsttrans;
-	transsize = firsttrans;
+	const int first_words = std::min((32 - writepos), transfer_size << 2);
+	const int second_words = (transfer_size << 2) - first_words;
 
-	while (transsize-- > 0)
-	{
-		CopyQWC(&data[writepos], pMem);
-		writepos = (writepos + 4) & 31;
-		pMem += 4;
-	}
+	memcpy(&data[writepos], pMem, first_words << 2);
+	pMem += first_words;
+
+	if(second_words)
+		memcpy(&data[0], pMem, second_words << 2);
+
+	writepos = (writepos + (transfer_size << 2)) & 31;
+
+	g_BP.IFC += transfer_size;
 
 	if (g_BP.IFC == 8)
 		IPU1Status.DataRequested = false;
 
-	return firsttrans;
+	return transfer_size;
 }
 
 int IPU_Fifo_Input::read(void *value)
@@ -121,26 +124,25 @@ int IPU_Fifo_Output::write(const u32 *value, uint size)
 {
 	pxAssertMsg(size>0, "Invalid size==0 when calling IPU_Fifo_Output::write");
 
-	uint origsize = size;
-	/*do {*/
-		//IPU0dma();
+	const int transfer_size = std::min(size, 8 - (uint)ipuRegs.ctrl.OFC);
+	if(!transfer_size) return 0;
 
-		uint transsize = std::min(size, 8 - (uint)ipuRegs.ctrl.OFC);
-		if(!transsize) return 0;
+	const int first_words = std::min((32 - writepos), transfer_size << 2);
+	const int second_words = (transfer_size << 2) - first_words;
+
+	memcpy(&data[writepos], value, first_words << 2);
+	value += first_words;
+	if (second_words)
+		memcpy(&data[0], value, second_words << 2);
+
+	writepos = (writepos + (transfer_size << 2)) & 31;
+
+	ipuRegs.ctrl.OFC += transfer_size;
 
-		ipuRegs.ctrl.OFC += transsize;
-		size -= transsize;
-		while (transsize > 0)
-		{
-			CopyQWC(&data[writepos], value);
-			writepos = (writepos + 4) & 31;
-			value += 4;
-			--transsize;
-		}
-	/*} while(true);*/
 	if(ipu0ch.chcr.STR)
 		IPU_INT_FROM(ipuRegs.ctrl.OFC * BIAS);
-	return origsize - size;
+
+	return transfer_size;
 }
 
 void IPU_Fifo_Output::read(void *value, uint size)
@@ -151,16 +153,16 @@ void IPU_Fifo_Output::read(void *value, uint size)
 	// Zeroing the read data is not needed, since the ringbuffer design will never read back
 	// the zero'd data anyway. --air
 
-	//__m128 zeroreg = _mm_setzero_ps();
-	while (size > 0)
-	{
-		CopyQWC(value, &data[readpos]);
-		//_mm_store_ps((float*)&data[readpos], zeroreg);
+	const int first_words = std::min((32 - readpos), static_cast<int>(size << 2));
+	const int second_words = static_cast<int>(size << 2) - first_words;
 
-		readpos = (readpos + 4) & 31;
-		value = (u128*)value + 1;
-		--size;
-	}
+	memcpy(value, &data[readpos], first_words << 2);
+	value = static_cast<u32*>(value) + first_words;
+
+	if (second_words)
+		memcpy(value, &data[0], second_words << 2);
+
+	readpos = (readpos + static_cast<int>(size << 2)) & 31;
 }
 
 void ReadFIFO_IPUout(mem128_t* out)
@@ -177,7 +179,7 @@ void WriteFIFO_IPUin(const mem128_t* value)
 	IPU_LOG( "WriteFIFO/IPUin <- 0x%08X.%08X.%08X.%08X", value->_u32[0], value->_u32[1], value->_u32[2], value->_u32[3]);
 
 	//committing every 16 bytes
-	if( ipu_fifo.in.write((u32*)value, 1) == 0 )
+	if( ipu_fifo.in.write(value->_u32, 1) == 0 )
 	{
 		if (ipuRegs.ctrl.BUSY && !CommandExecuteQueued)
 		{
diff --git a/pcsx2/IPU/IPU_Fifo.h b/pcsx2/IPU/IPU_Fifo.h
index 119a2f416b..0a8fdf6d75 100644
--- a/pcsx2/IPU/IPU_Fifo.h
+++ b/pcsx2/IPU/IPU_Fifo.h
@@ -27,7 +27,7 @@ struct IPU_Fifo_Input
 	alignas(16) u32 data[32];
 	int readpos, writepos;
 
-	int write(u32* pMem, int size);
+	int write(const u32* pMem, int size);
 	int read(void *value);
 	void clear();
 	std::string desc() const;
diff --git a/pcsx2/IPU/IPUdma.cpp b/pcsx2/IPU/IPUdma.cpp
index 132b65859c..f87bcb9205 100644
--- a/pcsx2/IPU/IPUdma.cpp
+++ b/pcsx2/IPU/IPUdma.cpp
@@ -123,7 +123,7 @@ void IPU1dma()
 		totalqwc += IPU1chain();
 
 	//Do this here to prevent double settings on Chain DMA's
-	if(totalqwc == 0 || (IPU1Status.DMAFinished && !IPU1Status.InProgress))
+	if((totalqwc == 0 && g_BP.IFC < 8) || (IPU1Status.DMAFinished && !IPU1Status.InProgress))
 	{
 		totalqwc = std::max(4, totalqwc) + tagcycles;
 		IPU_INT_TO(totalqwc * BIAS);