From 652e52df3dbe5b36e5352a18f50aadf1479dd3c7 Mon Sep 17 00:00:00 2001
From: Lior Halphon <LIJI32@gmail.com>
Date: Sat, 14 Sep 2019 12:31:52 +0300
Subject: [PATCH] Pass the SGB multiplayer tests

---
 Core/joypad.c |  2 +-
 Core/memory.c |  2 +-
 Core/sgb.c    | 17 ++++++++++++++---
 3 files changed, 16 insertions(+), 5 deletions(-)

diff --git a/Core/joypad.c b/Core/joypad.c
index 91f6ae3..b8d4fdb 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)) : 0;
+    uint8_t current_player = gb->sgb? (gb->sgb->current_player & (gb->sgb->player_count - 1) & 3) : 0;
     switch (key_selection) {
         case 3:
             if (gb->sgb && gb->sgb->player_count > 1) {
diff --git a/Core/memory.c b/Core/memory.c
index a218f8e..4c4a702 100644
--- a/Core/memory.c
+++ b/Core/memory.c
@@ -747,8 +747,8 @@ static void write_high_memory(GB_gameboy_t *gb, uint16_t addr, uint8_t value)
 
             case GB_IO_JOYP:
                 if ((gb->io_registers[GB_IO_JOYP] & 0x30) != (value & 0x30)) {
-                    gb->io_registers[GB_IO_JOYP] = value & 0xF0;
                     GB_sgb_write(gb, value);
+                    gb->io_registers[GB_IO_JOYP] = value & 0xF0;
                     GB_update_joyp(gb);
                 }
                 return;
diff --git a/Core/sgb.c b/Core/sgb.c
index 18daa47..8539238 100644
--- a/Core/sgb.c
+++ b/Core/sgb.c
@@ -337,7 +337,15 @@ 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:
-            gb->sgb->player_count = (uint8_t[]){1, 2, 1, 4}[gb->sgb->command[1] & 3];
+            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->mlt_lock = true;
             break;
         case CHR_TRN:
             gb->sgb->vram_transfer_countdown = 2;
@@ -397,10 +405,14 @@ 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;
+    }
+    
     switch ((value >> 4) & 3) {
         case 3:
             gb->sgb->ready_for_pulse = true;
-            if (gb->sgb->player_count > 1 && !gb->sgb->mlt_lock) {
+            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;
@@ -428,7 +440,6 @@ void GB_sgb_write(GB_gameboy_t *gb, uint8_t value)
             }
             break;
         case 1: // One
-            gb->sgb->mlt_lock ^= true;
             if (!gb->sgb->ready_for_pulse || !gb->sgb->ready_for_write) return;
             if (gb->sgb->ready_for_stop) {
                 GB_log(gb, "Corrupt SGB command.\n");