From 8c40a6b0c74a92754bc3918e35e40072dcc5cc92 Mon Sep 17 00:00:00 2001
From: Sam Lantinga <slouken@libsdl.org>
Date: Mon, 26 Sep 2022 22:47:21 -0700
Subject: [PATCH] Drop PS5 Bluetooth reports that fail CRC check

---
 src/joystick/hidapi/SDL_hidapi_ps5.c | 73 +++++++++++++++++++++-------
 1 file changed, 56 insertions(+), 17 deletions(-)

diff --git a/src/joystick/hidapi/SDL_hidapi_ps5.c b/src/joystick/hidapi/SDL_hidapi_ps5.c
index 1bd1a2669..93b59cea1 100644
--- a/src/joystick/hidapi/SDL_hidapi_ps5.c
+++ b/src/joystick/hidapi/SDL_hidapi_ps5.c
@@ -45,6 +45,10 @@
 #define BLUETOOTH_DISCONNECT_TIMEOUT_MS 500
 
 #define LOAD16(A, B)  (Sint16)((Uint16)(A) | (((Uint16)(B)) << 8))
+#define LOAD32(A, B, C, D) ((((Uint32)(A)) << 0)    | \
+                            (((Uint32)(B)) << 8)    | \
+                            (((Uint32)(C)) << 16)   | \
+                            (((Uint32)(D)) << 24))
 
 typedef enum
 {
@@ -1256,6 +1260,40 @@ HIDAPI_DriverPS5_HandleStatePacketAlt(SDL_Joystick *joystick, SDL_hid_device *de
     SDL_memcpy(&ctx->last_state, packet, sizeof(ctx->last_state));
 }
 
+static SDL_bool
+VerifyCRC(Uint8 *data, int size)
+{
+    Uint8 ubHdr = 0xA1; /* hidp header is part of the CRC calculation */
+    Uint32 unCRC, unPacketCRC;
+    Uint8 *packetCRC = data + size - sizeof(unPacketCRC);
+    unCRC = SDL_crc32(0, &ubHdr, 1);
+    unCRC = SDL_crc32(unCRC, data, (size_t)(size - sizeof(unCRC)));
+
+    unPacketCRC = LOAD32(packetCRC[0],
+                         packetCRC[1],
+                         packetCRC[2],
+                         packetCRC[3]);
+    return (unCRC == unPacketCRC) ? SDL_TRUE : SDL_FALSE;
+}
+
+static SDL_bool
+HIDAPI_DriverPS5_IsPacketValid(SDL_DriverPS5_Context *ctx, Uint8 *data, int size)
+{
+    switch (data[0]) {
+    case k_EPS5ReportIdState:
+        return SDL_TRUE;
+
+    case k_EPS5ReportIdBluetoothState:
+        if (VerifyCRC(data, size)) {
+            return SDL_TRUE;
+        }
+        break;
+    default:
+        break;
+    }
+    return SDL_FALSE;
+}
+
 static SDL_bool
 HIDAPI_DriverPS5_UpdateDevice(SDL_HIDAPI_Device *device)
 {
@@ -1265,25 +1303,18 @@ HIDAPI_DriverPS5_UpdateDevice(SDL_HIDAPI_Device *device)
     int size;
     int packet_count = 0;
 
-    /* Reconnect the Bluetooth device once the USB device is gone */
-    if (device->num_joysticks == 0 &&
-        device->is_bluetooth &&
-        !HIDAPI_HasConnectedUSBDevice(device->serial)) {
-        if (SDL_hid_read_timeout(device->dev, data, sizeof(data), 0) > 0) {
-            HIDAPI_JoystickConnected(device, NULL);
-        }
-    }
-
     if (device->num_joysticks > 0) {
         joystick = SDL_JoystickFromInstanceID(device->joysticks[0]);
-    } else {
-        return SDL_FALSE;
     }
 
     while ((size = SDL_hid_read_timeout(device->dev, data, sizeof(data), 0)) > 0) {
 #ifdef DEBUG_PS5_PROTOCOL
         HIDAPI_DumpPacket("PS5 packet: size = %d", data, size);
 #endif
+        if (!HIDAPI_DriverPS5_IsPacketValid(ctx, data, size)) {
+            continue;
+        }
+
         ++packet_count;
         ctx->last_packet = SDL_GetTicks();
 
@@ -1327,15 +1358,23 @@ HIDAPI_DriverPS5_UpdateDevice(SDL_HIDAPI_Device *device)
         }
     }
 
-    if (device->is_bluetooth && packet_count == 0) {
-        /* Check to see if it looks like the device disconnected */
-        if (SDL_TICKS_PASSED(SDL_GetTicks(), ctx->last_packet + BLUETOOTH_DISCONNECT_TIMEOUT_MS)) {
-            /* Send an empty output report to tickle the Bluetooth stack */
-            HIDAPI_DriverPS5_TickleBluetooth(device);
+    if (device->is_bluetooth) {
+        if (packet_count == 0) {
+            /* Check to see if it looks like the device disconnected */
+            if (SDL_TICKS_PASSED(SDL_GetTicks(), ctx->last_packet + BLUETOOTH_DISCONNECT_TIMEOUT_MS)) {
+                /* Send an empty output report to tickle the Bluetooth stack */
+                HIDAPI_DriverPS5_TickleBluetooth(device);
+            }
+        } else {
+            /* Reconnect the Bluetooth device once the USB device is gone */
+            if (device->num_joysticks == 0 &&
+                !HIDAPI_HasConnectedUSBDevice(device->serial)) {
+                HIDAPI_JoystickConnected(device, NULL);
+            }
         }
     }
 
-    if (size < 0) {
+    if (size < 0 && device->num_joysticks > 0) {
         /* Read error, device is disconnected */
         HIDAPI_JoystickDisconnected(device, device->joysticks[0]);
     }