From 914d8798116c4f713c19ad050394d195417c047d Mon Sep 17 00:00:00 2001
From: Vicki Pfau <vi@endrift.com>
Date: Fri, 24 May 2024 03:01:44 -0700
Subject: [PATCH] GBA SIO: Allow seamless mode switching if driver supports it

---
 src/gba/sio.c          | 22 +++++++++++++++-------
 src/gba/sio/lockstep.c | 10 +---------
 2 files changed, 16 insertions(+), 16 deletions(-)

diff --git a/src/gba/sio.c b/src/gba/sio.c
index 710cd9d45..54fff7ea1 100644
--- a/src/gba/sio.c
+++ b/src/gba/sio.c
@@ -60,16 +60,24 @@ static void _switchMode(struct GBASIO* sio) {
 		newMode = (enum GBASIOMode) (mode & 0xC);
 	}
 	if (newMode != sio->mode) {
-		if (sio->activeDriver && sio->activeDriver->unload) {
-			sio->activeDriver->unload(sio->activeDriver);
-		}
+		struct GBASIODriver* driver = _lookupDriver(sio, newMode);
 		if (sio->mode != (enum GBASIOMode) -1) {
 			mLOG(GBA_SIO, DEBUG, "Switching mode from %s to %s", _modeName(sio->mode), _modeName(newMode));
 		}
-		sio->mode = newMode;
-		sio->activeDriver = _lookupDriver(sio, sio->mode);
-		if (sio->activeDriver && sio->activeDriver->load) {
-			sio->activeDriver->load(sio->activeDriver);
+		if (driver != sio->activeDriver || (driver && !driver->setMode)) {
+			if (sio->activeDriver && sio->activeDriver->unload) {
+				sio->activeDriver->unload(sio->activeDriver);
+			}
+			sio->mode = newMode;
+			sio->activeDriver = driver;
+			if (sio->activeDriver && sio->activeDriver->load) {
+				sio->activeDriver->load(sio->activeDriver);
+			}
+		} else {
+			sio->mode = newMode;
+			if (sio->activeDriver && sio->activeDriver->setMode) {
+				sio->activeDriver->setMode(sio->activeDriver, newMode);
+			}
 		}
 
 		int id = 0;
diff --git a/src/gba/sio/lockstep.c b/src/gba/sio/lockstep.c
index 30092c3ec..f7b82f089 100644
--- a/src/gba/sio/lockstep.c
+++ b/src/gba/sio/lockstep.c
@@ -15,7 +15,6 @@ static bool GBASIOLockstepNodeInit(struct GBASIODriver* driver);
 static void GBASIOLockstepNodeDeinit(struct GBASIODriver* driver);
 static bool GBASIOLockstepNodeLoad(struct GBASIODriver* driver);
 static bool GBASIOLockstepNodeUnload(struct GBASIODriver* driver);
-static void GBASIOLockstepNodeSetMode(struct GBASIODriver* driver, enum GBASIOMode mode);
 static bool GBASIOLockstepNodeHandlesMode(struct GBASIODriver* driver, enum GBASIOMode mode);
 static int GBASIOLockstepNodeConnectedDevices(struct GBASIODriver* driver);
 static int GBASIOLockstepNodeDeviceId(struct GBASIODriver* driver);
@@ -42,7 +41,7 @@ void GBASIOLockstepNodeCreate(struct GBASIOLockstepNode* node) {
 	node->d.deinit = GBASIOLockstepNodeDeinit;
 	node->d.load = GBASIOLockstepNodeLoad;
 	node->d.unload = GBASIOLockstepNodeUnload;
-	node->d.setMode = GBASIOLockstepNodeSetMode;
+	node->d.setMode = NULL;
 	node->d.handlesMode = GBASIOLockstepNodeHandlesMode;
 	node->d.connectedDevices = GBASIOLockstepNodeConnectedDevices;
 	node->d.deviceId = GBASIOLockstepNodeDeviceId;
@@ -190,13 +189,6 @@ bool GBASIOLockstepNodeUnload(struct GBASIODriver* driver) {
 	return true;
 }
 
-static void GBASIOLockstepNodeSetMode(struct GBASIODriver* driver, enum GBASIOMode mode) {
-	struct GBASIOLockstepNode* node = (struct GBASIOLockstepNode*) driver;
-	mLockstepLock(&node->p->d);
-	node->mode = mode;
-	mLockstepUnlock(&node->p->d);
-}
-
 static bool GBASIOLockstepNodeHandlesMode(struct GBASIODriver* driver, enum GBASIOMode mode) {
 	UNUSED(driver);
 	switch (mode) {