From 86459dfc29c5c5f78c79d649d297a98d7212cc7f Mon Sep 17 00:00:00 2001
From: notaz <notasas@gmail.com>
Date: Sat, 14 Oct 2023 23:42:11 +0300
Subject: [PATCH] update vibration handling

following the nocash description
notaz/pcsx_rearmed#314
---
 include/psemu_plugin_defs.h |  4 +++-
 libpcsxcore/plugins.c       | 42 ++++++++++++++++++++++++-------------
 2 files changed, 31 insertions(+), 15 deletions(-)

diff --git a/include/psemu_plugin_defs.h b/include/psemu_plugin_defs.h
index c7a0d1c9..3f4d21b2 100644
--- a/include/psemu_plugin_defs.h
+++ b/include/psemu_plugin_defs.h
@@ -229,12 +229,14 @@ typedef struct
 		unsigned char cmd4dConfig[6];
 		unsigned int  lastUseFrame;
 		unsigned int  digitalModeFrames;
+		unsigned char configModeUsed;
+		unsigned char padding[3];
 	} ds;
 	unsigned char multitapLongModeEnabled;
 	unsigned char padding2;
 	unsigned char txData[34];
 
-	unsigned char reserved[26];
+	unsigned char reserved[22];
 } PadDataS;
 
 /*         NET PlugIn v2       */
diff --git a/libpcsxcore/plugins.c b/libpcsxcore/plugins.c
index 6e5cdbfe..868493bb 100644
--- a/libpcsxcore/plugins.c
+++ b/libpcsxcore/plugins.c
@@ -562,8 +562,9 @@ static void reqIndex2Treatment(int padIndex, u8 value) {
 			//0x43
 			if (value == 0) {
 				pads[padIndex].ds.configMode = 0;
-			} else {
+			} else if (value == 1) {
 				pads[padIndex].ds.configMode = 1;
+				pads[padIndex].ds.configModeUsed = 1;
 			}
 			break;
 		case CMD_SET_MODE_AND_LOCK :
@@ -584,15 +585,19 @@ static void reqIndex2Treatment(int padIndex, u8 value) {
 				memcpy(buf, resp4C_01, 8);
 			}
 			break;
-		case CMD_READ_DATA_AND_VIBRATE:
-			//mem the vibration value for small motor;
-			pads[padIndex].Vib[0] = value;
-			break;
 	}
 }
 
-static void vibrate(int padIndex) {
+static void ds_update_vibrate(int padIndex) {
 	PadDataS *pad = &pads[padIndex];
+	if (pad->ds.configModeUsed) {
+		pad->Vib[0] = (pad->Vib[0] == 1) ? 1 : 0;
+	}
+	else {
+		// compat mode
+		pad->Vib[0] = (pad->Vib[0] & 0xc0) == 0x40 && (pad->Vib[1] & 1);
+		pad->Vib[1] = 0;
+	}
 	if (pad->Vib[0] != pad->VibF[0] || pad->Vib[1] != pad->VibF[1]) {
 		//value is different update Value and call libretro for vibration
 		pad->VibF[0] = pad->Vib[0];
@@ -733,19 +738,28 @@ static void PADpoll_dualshock(int port, unsigned char value, int pos)
 		case 2:
 			reqIndex2Treatment(port, value);
 			break;
-		case 3:
-			if (pads[port].txData[0] == CMD_READ_DATA_AND_VIBRATE) {
-				// vibration value for the Large motor
-				pads[port].Vib[1] = value;
-
-				vibrate(port);
-			}
-			break;
 		case 7:
 			if (pads[port].txData[0] == CMD_VIBRATION_TOGGLE)
 				memcpy(pads[port].ds.cmd4dConfig, pads[port].txData + 2, 6);
 			break;
 	}
+
+	if (pads[port].txData[0] == CMD_READ_DATA_AND_VIBRATE
+	    && !pads[port].ds.configModeUsed && 2 <= pos && pos < 4)
+	{
+		// "compat" single motor mode
+		pads[port].Vib[pos - 2] = value;
+	}
+	else if (pads[port].txData[0] == CMD_READ_DATA_AND_VIBRATE
+		 && 2 <= pos && pos < 8)
+	{
+		// 0 - weak motor, 1 - strong motor
+		int dev = pads[port].ds.cmd4dConfig[pos - 2];
+		if (dev < 2)
+			pads[port].Vib[dev] = value;
+	}
+	if (pos == respSize - 1)
+		ds_update_vibrate(port);
 }
 
 static unsigned char PADpoll_(int port, unsigned char value, int pos, int *more_data) {
-- 
2.39.5