From 191f7cee0215fe6e28a9cdd653161afcce974f54 Mon Sep 17 00:00:00 2001
From: Lior Halphon <LIJI32@gmail.com>
Date: Sat, 9 Oct 2021 14:52:28 +0300
Subject: [PATCH] Improved emulation of SGB multiplayer, fixes #405

---
 Core/joypad.c     |  2 +-
 Core/save_state.c |  8 +++++++-
 Core/sgb.c        | 19 +++++++------------
 Core/sgb.h        |  2 +-
 4 files changed, 16 insertions(+), 15 deletions(-)

diff --git a/Core/joypad.c b/Core/joypad.c
index b8d4fdb4..b30258c2 100644
--- a/Core/joypad.c
+++ b/Core/joypad.c
@@ -12,7 +12,7 @@ void GB_update_joyp(GB_gameboy_t *gb)
     previous_state = gb->io_registers[GB_IO_JOYP] & 0xF;
     key_selection = (gb->io_registers[GB_IO_JOYP] >> 4) & 3;
     gb->io_registers[GB_IO_JOYP] &= 0xF0;
-    uint8_t current_player = gb->sgb? (gb->sgb->current_player & (gb->sgb->player_count - 1) & 3) : 0;
+    uint8_t current_player = gb->sgb? gb->sgb->current_player : 0;
     switch (key_selection) {
         case 3:
             if (gb->sgb && gb->sgb->player_count > 1) {
diff --git a/Core/save_state.c b/Core/save_state.c
index 74615985..6e7ca9d2 100644
--- a/Core/save_state.c
+++ b/Core/save_state.c
@@ -355,6 +355,12 @@ static void sanitize_state(GB_gameboy_t *gb)
     if (gb->object_priority == GB_OBJECT_PRIORITY_UNDEFINED) {
         gb->object_priority = gb->cgb_mode? GB_OBJECT_PRIORITY_INDEX : GB_OBJECT_PRIORITY_X;
     }
+    if (gb->sgb) {
+        if (gb->sgb->player_count != 1 && gb->sgb->player_count != 2 && gb->sgb->player_count != 4) {
+            gb->sgb->player_count = 1;
+        }
+        gb->sgb->current_player &= gb->sgb->player_count - 1;
+    }
     if (gb->sgb && !gb->sgb->v14_3) {
 #ifdef GB_BIG_ENDIAN
         for (unsigned i = 0; i < sizeof(gb->sgb->border.raw_data) / 2; i++) {
@@ -719,7 +725,7 @@ static int save_state_internal(GB_gameboy_t *gb, virtual_file_t *file, bool appe
             bess_sgb.attribute_files = (BESS_buffer_t){LE32(sizeof(gb->sgb->attribute_files)),
                                                        LE32(sgb_offset + offsetof(GB_sgb_t, attribute_files))};
             
-            bess_sgb.multiplayer_state = (gb->sgb->player_count << 4) | (gb->sgb->current_player & (gb->sgb->player_count - 1));
+            bess_sgb.multiplayer_state = (gb->sgb->player_count << 4) | gb->sgb->current_player;
             if (file->write(file, &bess_sgb, sizeof(bess_sgb)) != sizeof(bess_sgb)) {
                 goto error;
             }
diff --git a/Core/sgb.c b/Core/sgb.c
index 894ae966..66f2f202 100644
--- a/Core/sgb.c
+++ b/Core/sgb.c
@@ -382,15 +382,12 @@ static void command_ready(GB_gameboy_t *gb)
             // Not supported, but used by almost all SGB games for hot patching, so let's mute the warning for this
             break;
         case MLT_REQ:
-            if (gb->sgb->player_count == 1) {
-                gb->sgb->current_player = 0;
-            }
             gb->sgb->player_count = (gb->sgb->command[1] & 3) + 1; /* Todo: When breaking save state comaptibility,
                                                                             fix this to be 0 based. */
             if (gb->sgb->player_count == 3) {
-                gb->sgb->current_player++;
+                gb->sgb->player_count++;
             }
-            gb->sgb->mlt_lock = true;
+            gb->sgb->current_player &= (gb->sgb->player_count - 1);
             break;
         case CHR_TRN:
             gb->sgb->vram_transfer_countdown = 2;
@@ -446,18 +443,16 @@ void GB_sgb_write(GB_gameboy_t *gb, uint8_t value)
         command_size = SGB_PACKET_SIZE * 8;
     }
     
-    if ((value & 0x20) == 0 && (gb->io_registers[GB_IO_JOYP] & 0x20) != 0) {
-        gb->sgb->mlt_lock ^= true;
+    if ((value & 0x20) != 0 && (gb->io_registers[GB_IO_JOYP] & 0x20) == 0) {
+        if ((gb->sgb->player_count & 1) == 0) {
+            gb->sgb->current_player++;
+            gb->sgb->current_player &= (gb->sgb->player_count - 1);
+        }
     }
     
     switch ((value >> 4) & 3) {
         case 3:
             gb->sgb->ready_for_pulse = true;
-            if ((gb->sgb->player_count & 1) == 0 && !gb->sgb->mlt_lock) {
-                gb->sgb->current_player++;
-                gb->sgb->current_player &= 3;
-                gb->sgb->mlt_lock = true;
-            }
             break;
             
         case 2: // Zero
diff --git a/Core/sgb.h b/Core/sgb.h
index 1e67835d..1e1a5c28 100644
--- a/Core/sgb.h
+++ b/Core/sgb.h
@@ -61,7 +61,7 @@ struct GB_sgb_s {
     uint8_t received_header[0x54];
     
     /* Multiplayer (cont) */
-    bool mlt_lock;
+    GB_PADDING(bool, mlt_lock);
     
     bool v14_3; // True on save states created on 0.14.3 or newer; Remove when breaking save state compatibility!
 };