From 5d0bbcd51277ff6c45192e555d644daa416ca397 Mon Sep 17 00:00:00 2001
From: Vicki Pfau <vi@endrift.com>
Date: Tue, 24 Apr 2018 11:21:28 -0700
Subject: [PATCH] GB Video: Fix input iteration on SGB (fixes #1064)

---
 src/gb/io.c    | 22 +++++++++++++++++-----
 src/gb/video.c |  1 +
 2 files changed, 18 insertions(+), 5 deletions(-)

diff --git a/src/gb/io.c b/src/gb/io.c
index af2bba3a9..d66df3030 100644
--- a/src/gb/io.c
+++ b/src/gb/io.c
@@ -113,13 +113,26 @@ static void _writeSGBBits(struct GB* gb, int bits) {
 		return;
 	}
 	gb->currentSgbBits = bits;
-	if (bits == 3) {
-		gb->sgbCurrentController = (gb->sgbCurrentController + 1) & gb->sgbControllers;
-	}
 	if (gb->sgbBit == 128 && bits == 2) {
 		GBVideoWriteSGBPacket(&gb->video, gb->sgbPacket);
 		++gb->sgbBit;
 	}
+	if (gb->sgbBit > 128) {
+		switch (bits) {
+		case 1:
+			gb->sgbBit |= 2;
+			break;
+		case 2:
+			gb->sgbBit |= 4;
+			break;
+		case 3:
+			if (gb->sgbBit == 135 && bits == 3 && !(gb->video.sgbCommandHeader & 7)) {
+				gb->sgbBit &= ~6;
+				gb->sgbCurrentController = (gb->sgbCurrentController + 1) & gb->sgbControllers;
+			}
+			break;
+		}
+	}
 	if (gb->sgbBit >= 128) {
 		return;
 	}
@@ -511,8 +524,7 @@ static uint8_t _readKeys(struct GB* gb) {
 	}
 	switch (gb->memory.io[REG_JOYP] & 0x30) {
 	case 0x30:
-	// TODO: Increment
-		keys = (gb->video.sgbCommandHeader >> 3) == SGB_MLT_REQ ? 0xF - gb->sgbCurrentController : 0;
+		keys = 0xF - gb->sgbCurrentController;
 		break;
 	case 0x20:
 		keys >>= 4;
diff --git a/src/gb/video.c b/src/gb/video.c
index be92537f4..93fcf1767 100644
--- a/src/gb/video.c
+++ b/src/gb/video.c
@@ -703,6 +703,7 @@ void GBVideoWriteSGBPacket(struct GBVideo* video, uint8_t* data) {
 		break;
 	case SGB_MLT_REQ:
 		video->p->sgbControllers = video->sgbPacketBuffer[1] & 0x3;
+		video->p->sgbCurrentController = 0;
 		return;
 	case SGB_MASK_EN:
 		video->renderer->sgbRenderMode = video->sgbPacketBuffer[1] & 0x3;