From 395fa2da4c60a7732474ac25bb5cd0ca45a9cf87 Mon Sep 17 00:00:00 2001
From: Vicki Pfau <vi@endrift.com>
Date: Fri, 3 Feb 2023 02:06:20 -0800
Subject: [PATCH] GBA SIO: Fix unconnected normal mode SIOCNT SI bit (fixes
 #2810)

---
 CHANGES                |  1 +
 src/gba/sio/lockstep.c | 30 ++++++++++++++++++++++++------
 2 files changed, 25 insertions(+), 6 deletions(-)

diff --git a/CHANGES b/CHANGES
index 1a3f283eb..925623fc8 100644
--- a/CHANGES
+++ b/CHANGES
@@ -3,6 +3,7 @@ Emulation fixes:
  - GBA Audio: Fix improperly deserializing GB audio registers (fixes mgba.io/i/2793)
  - GBA Memory: Make VRAM access stalls only apply to BG RAM
  - GBA SIO: Fix SIOCNT SI pin value after attaching player 2 (fixes mgba.io/i/2805)
+ - GBA SIO: Fix unconnected normal mode SIOCNT SI bit (fixes mgba.io/i/2810)
  - GBA Timers: Cascading timers don't tick when disabled (fixes mgba.io/i/2812)
 Other fixes:
  - Core: Allow sending thread requests to a crashed core (fixes mgba.io/i/2784)
diff --git a/src/gba/sio/lockstep.c b/src/gba/sio/lockstep.c
index a03e88e30..db92c50a5 100644
--- a/src/gba/sio/lockstep.c
+++ b/src/gba/sio/lockstep.c
@@ -127,7 +127,11 @@ bool GBASIOLockstepNodeLoad(struct GBASIODriver* driver) {
 		break;
 	case SIO_NORMAL_8:
 	case SIO_NORMAL_32:
-		ATOMIC_ADD(node->p->attachedNormal, 1);
+		if (ATOMIC_ADD(node->p->attachedNormal, 1) > node->id + 1 && node->id < 3) {
+			node->d.p->siocnt = GBASIONormalSetSi(node->d.p->siocnt, GBASIONormalGetIdleSo(node->p->players[node->id + 1]->d.p->siocnt));
+		} else {
+			node->d.p->siocnt = GBASIONormalFillSi(node->d.p->siocnt);
+		}
 		node->d.writeRegister = GBASIOLockstepNodeNormalWriteRegister;
 		break;
 	default:
@@ -507,9 +511,26 @@ static uint16_t GBASIOLockstepNodeNormalWriteRegister(struct GBASIODriver* drive
 
 	if (address == REG_SIOCNT) {
 		mLOG(GBA_SIO, DEBUG, "Lockstep %i: SIOCNT <- %04X", node->id, value);
+		int attached;
+		ATOMIC_LOAD(attached, node->p->attachedNormal);
 		value &= 0xFF8B;
-		if (!node->id) {
-			value = GBASIONormalClearSi(value);
+		if (node->id < 3 && attached > node->id + 1) {
+			value = GBASIONormalSetSi(value, GBASIONormalGetIdleSo(node->p->players[node->id + 1]->d.p->siocnt));
+		} else {
+			value = GBASIONormalFillSi(value);
+		}
+
+		enum mLockstepPhase transferActive;
+		ATOMIC_LOAD(transferActive, node->p->d.transferActive);
+		if (node->id > 0 && transferActive == TRANSFER_IDLE) {
+			int try;
+			for (try = 0; try < 3; ++try) {
+				GBASIONormal parentSiocnt;
+				ATOMIC_LOAD(parentSiocnt, node->p->players[node->id - 1]->d.p->siocnt);
+				if (ATOMIC_CMPXCHG(node->p->players[node->id - 1]->d.p->siocnt, parentSiocnt, GBASIONormalSetSi(parentSiocnt, GBASIONormalGetIdleSo(value)))) {
+					break;
+				}
+			}
 		}
 		if (value & 0x0080) {
 			if (!node->id) {
@@ -524,9 +545,6 @@ static uint16_t GBASIOLockstepNodeNormalWriteRegister(struct GBASIODriver* drive
 					cycles *= 4;
 				}
 
-				enum mLockstepPhase transferActive;
-				ATOMIC_LOAD(transferActive, node->p->d.transferActive);
-
 				if (transferActive == TRANSFER_IDLE) {
 					mLOG(GBA_SIO, DEBUG, "Lockstep %i: Transfer initiated", node->id);
 					ATOMIC_STORE(node->p->d.transferActive, TRANSFER_STARTING);