From f6e9003ddc744c009a8451a05a02fcdcc47daded Mon Sep 17 00:00:00 2001
From: Vicki Pfau <vi@endrift.com>
Date: Tue, 2 Mar 2021 18:43:55 -0800
Subject: [PATCH] SI/DeviceGBA: Ensure data socket isn't backed up

When reading a reply from a message sent to the data socket there is
the possibility that the other side gets sent multiple messages
before replying to any of them, which can lead to multiple replies
sent in a row. Though this only happens when things time out, it's
quite possible for these timeouts to happen or build up over time,
especially when initiating the connection.

This change makes sure to flush any pending bytes that have not been
read yet out of the socket after a successful POLL reply is received,
since that is the most common time when backups occur, and as well as
using the exact number of bytes in an expected reply, to ensure
the received data and the message it's replying to do not get out of
sync.
---
 Source/Core/Core/HW/SI/SI_DeviceGBA.cpp | 38 ++++++++++++++++++++++---
 Source/Core/Core/HW/SI/SI_DeviceGBA.h   |  3 +-
 2 files changed, 36 insertions(+), 5 deletions(-)

diff --git a/Source/Core/Core/HW/SI/SI_DeviceGBA.cpp b/Source/Core/Core/HW/SI/SI_DeviceGBA.cpp
index 9efb6e0172..934d8fd78a 100644
--- a/Source/Core/Core/HW/SI/SI_DeviceGBA.cpp
+++ b/Source/Core/Core/HW/SI/SI_DeviceGBA.cpp
@@ -253,7 +253,7 @@ void GBASockServer::Send(const u8* si_buffer)
     Disconnect();
 }
 
-int GBASockServer::Receive(u8* si_buffer)
+int GBASockServer::Receive(u8* si_buffer, u8 bytes)
 {
   if (!m_client)
     return 0;
@@ -267,8 +267,7 @@ int GBASockServer::Receive(u8* si_buffer)
 
   size_t num_received = 0;
   std::array<u8, RECV_MAX_SIZE> recv_data;
-  sf::Socket::Status recv_stat =
-      m_client->receive(recv_data.data(), recv_data.size(), num_received);
+  sf::Socket::Status recv_stat = m_client->receive(recv_data.data(), bytes, num_received);
   if (recv_stat == sf::Socket::Disconnected)
   {
     Disconnect();
@@ -286,6 +285,21 @@ int GBASockServer::Receive(u8* si_buffer)
   return static_cast<int>(std::min(num_received, recv_data.size()));
 }
 
+void GBASockServer::Flush()
+{
+  if (!m_client || !m_booted)
+    return;
+
+  size_t num_received = 1;
+  u8 byte;
+  while (num_received)
+  {
+    sf::Socket::Status recv_stat = m_client->receive(&byte, 1, num_received);
+    if (recv_stat == sf::Socket::Disconnected)
+      break;
+  }
+}
+
 CSIDevice_GBA::CSIDevice_GBA(SIDevices device, int device_number) : ISIDevice(device, device_number)
 {
 }
@@ -329,7 +343,23 @@ int CSIDevice_GBA::RunBuffer(u8* buffer, int request_length)
 
   case NextAction::ReceiveResponse:
   {
-    int num_data_received = m_sock_server.Receive(buffer);
+    u8 bytes = 1;
+    switch (m_last_cmd)
+    {
+    case CMD_RESET:
+    case CMD_STATUS:
+      bytes = 3;
+      break;
+    case CMD_READ:
+      bytes = 5;
+      break;
+    default:
+      break;
+    }
+    int num_data_received = m_sock_server.Receive(buffer, bytes);
+    if (m_last_cmd == CMD_STATUS && num_data_received == 3)
+      m_sock_server.Flush();
+
     m_next_action = NextAction::SendCommand;
     if (num_data_received == 0)
     {
diff --git a/Source/Core/Core/HW/SI/SI_DeviceGBA.h b/Source/Core/Core/HW/SI/SI_DeviceGBA.h
index c08ab954e0..770ec87d13 100644
--- a/Source/Core/Core/HW/SI/SI_DeviceGBA.h
+++ b/Source/Core/Core/HW/SI/SI_DeviceGBA.h
@@ -28,7 +28,8 @@ public:
   bool IsConnected();
   void ClockSync();
   void Send(const u8* si_buffer);
-  int Receive(u8* si_buffer);
+  int Receive(u8* si_buffer, u8 bytes);
+  void Flush();
 
 private:
   void Disconnect();