From 4fcc921b46e515b2909fe76eb22ef46f1118353e Mon Sep 17 00:00:00 2001
From: Lior Halphon <LIJI32@gmail.com>
Date: Fri, 13 Sep 2019 13:13:28 +0300
Subject: [PATCH] Fix SGB multiplayer, improve multiplayer accuracy

---
 Core/gb.c     |  2 +-
 Core/joypad.c |  4 ++--
 Core/memory.c |  8 +++++---
 Core/sgb.c    | 13 ++++++++-----
 Core/sgb.h    |  3 +++
 5 files changed, 19 insertions(+), 11 deletions(-)

diff --git a/Core/gb.c b/Core/gb.c
index e91420c..6604e4f 100644
--- a/Core/gb.c
+++ b/Core/gb.c
@@ -858,7 +858,7 @@ void GB_reset(GB_gameboy_t *gb)
     gb->mbc_rom_bank = 1;
     gb->last_rtc_second = time(NULL);
     gb->cgb_ram_bank = 1;
-    gb->io_registers[GB_IO_JOYP] = 0xF;
+    gb->io_registers[GB_IO_JOYP] = 0xCF;
     gb->mbc_ram_size = mbc_ram_size;
     if (GB_is_cgb(gb)) {
         gb->ram_size = 0x1000 * 8;
diff --git a/Core/joypad.c b/Core/joypad.c
index 124d908..91f6ae3 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 : 0;
+    uint8_t current_player = gb->sgb? (gb->sgb->current_player & (gb->sgb->player_count - 1)) : 0;
     switch (key_selection) {
         case 3:
             if (gb->sgb && gb->sgb->player_count > 1) {
@@ -73,7 +73,7 @@ void GB_icd_set_joyp(GB_gameboy_t *gb, uint8_t value)
     if (previous_state & ~(gb->io_registers[GB_IO_JOYP] & 0xF)) {
         gb->io_registers[GB_IO_IF] |= 0x10;
     }
-
+    gb->io_registers[GB_IO_JOYP] |= 0xC0;
 }
 
 void GB_set_key_state(GB_gameboy_t *gb, GB_key_t index, bool pressed)
diff --git a/Core/memory.c b/Core/memory.c
index 40ea00f..a218f8e 100644
--- a/Core/memory.c
+++ b/Core/memory.c
@@ -746,9 +746,11 @@ static void write_high_memory(GB_gameboy_t *gb, uint16_t addr, uint8_t value)
                 return;
 
             case GB_IO_JOYP:
-                gb->io_registers[GB_IO_JOYP] = value & 0xF0;
-                GB_sgb_write(gb, value);
-                GB_update_joyp(gb);
+                if ((gb->io_registers[GB_IO_JOYP] & 0x30) != (value & 0x30)) {
+                    gb->io_registers[GB_IO_JOYP] = value & 0xF0;
+                    GB_sgb_write(gb, value);
+                    GB_update_joyp(gb);
+                }
                 return;
 
             case GB_IO_BIOS:
diff --git a/Core/sgb.c b/Core/sgb.c
index c36bc3c..d0ac472 100644
--- a/Core/sgb.c
+++ b/Core/sgb.c
@@ -338,7 +338,6 @@ static void command_ready(GB_gameboy_t *gb)
             break;
         case MLT_REQ:
             gb->sgb->player_count = (uint8_t[]){1, 2, 1, 4}[gb->sgb->command[1] & 3];
-            gb->sgb->current_player = gb->sgb->player_count - 1;
             break;
         case CHR_TRN:
             gb->sgb->vram_transfer_countdown = 2;
@@ -382,13 +381,16 @@ void GB_sgb_write(GB_gameboy_t *gb, uint8_t value)
     if (gb->joyp_write_callback) {
         gb->joyp_write_callback(gb, value);
     }
+    
     if (!GB_is_sgb(gb)) return;
     if (!GB_is_hle_sgb(gb)) {
         /* Notify via callback */
         return;
     }
     if (gb->sgb->disable_commands) return;
-    if (gb->sgb->command_write_index >= sizeof(gb->sgb->command) * 8) return;
+    if (gb->sgb->command_write_index >= sizeof(gb->sgb->command) * 8) {
+        return;
+    }
     
     uint16_t command_size = (gb->sgb->command[0] & 7 ?: 1) * SGB_PACKET_SIZE * 8;
     if ((gb->sgb->command[0] & 0xF1) == 0xF1) {
@@ -398,10 +400,10 @@ void GB_sgb_write(GB_gameboy_t *gb, uint8_t value)
     switch ((value >> 4) & 3) {
         case 3:
             gb->sgb->ready_for_pulse = true;
-            /* TODO: This is the logic used by BGB which *should* work for most/all games, but a proper test ROM is needed */
-            if (gb->sgb->player_count > 1 && (gb->io_registers[GB_IO_JOYP] & 0x30) == 0x10) {
+            if (gb->sgb->player_count > 1 && !gb->sgb->mlt_lock) {
                 gb->sgb->current_player++;
-                gb->sgb->current_player &= gb->sgb->player_count - 1;
+                gb->sgb->current_player &= 3;
+                gb->sgb->mlt_lock = true;
             }
             break;
             
@@ -426,6 +428,7 @@ 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");
diff --git a/Core/sgb.h b/Core/sgb.h
index 49bf6d9..df90253 100644
--- a/Core/sgb.h
+++ b/Core/sgb.h
@@ -52,6 +52,9 @@ struct GB_sgb_s {
     
     /* GB Header */
     uint8_t received_header[0x54];
+    
+    /* Multiplayer (cont) */
+    bool mlt_lock;
 };
 
 void GB_sgb_write(GB_gameboy_t *gb, uint8_t value);