Implemented setting the LED color on the PS5 controller

src/joystick/hidapi/SDL_hidapi_ps4.c

@@ -106,10 +106,10 @@ typedef struct {
     SDL_bool is_dongle;
     SDL_bool is_bluetooth;
     SDL_bool audio_supported;
-    SDL_bool rumble_supported;
+    SDL_bool effects_supported;
     int player_index;
-    Uint16 rumble_left;
+    Uint8 rumble_left;
-    Uint16 rumble_right;
+    Uint8 rumble_right;
     SDL_bool color_set;
     Uint8 led_red;
     Uint8 led_green;
@@ -120,27 +120,6 @@ typedef struct {
 } SDL_DriverPS4_Context;
 
 
-/* Public domain CRC implementation adapted from:
-   http://home.thep.lu.se/~bjorn/crc/crc32_simple.c
-*/
-static Uint32 crc32_for_byte(Uint32 r)
-{
-    int i;
-    for(i = 0; i < 8; ++i) {
-        r = (r & 1? 0: (Uint32)0xEDB88320L) ^ r >> 1;
-    }
-    return r ^ (Uint32)0xFF000000L;
-}
-
-static Uint32 crc32(Uint32 crc, const void *data, int count)
-{
-    int i;
-    for(i = 0; i < count; ++i) {
-        crc = crc32_for_byte((Uint8)crc ^ ((const Uint8*)data)[i]) ^ crc >> 8;
-    }
-    return crc;
-}
-
 static SDL_bool
 HIDAPI_DriverPS4_IsSupportedDevice(const char *name, SDL_GameControllerType type, Uint16 vendor_id, Uint16 product_id, Uint16 version, int interface_number, int interface_class, int interface_subclass, int interface_protocol)
 {
@@ -235,7 +214,62 @@ HIDAPI_DriverPS4_GetDevicePlayerIndex(SDL_HIDAPI_Device *device, SDL_JoystickID
     return -1;
 }
 
-static int HIDAPI_DriverPS4_RumbleJoystick(SDL_HIDAPI_Device *device, SDL_Joystick *joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble);
+static int
+HIDAPI_DriverPS4_UpdateEffects(SDL_HIDAPI_Device *device)
+{
+    SDL_DriverPS4_Context *ctx = (SDL_DriverPS4_Context *)device->context;
+    DS4EffectsState_t *effects;
+    Uint8 data[78];
+    int report_size, offset;
+
+    if (!ctx->effects_supported) {
+        return SDL_Unsupported();
+    }
+
+    SDL_zero(data);
+
+    if (ctx->is_bluetooth) {
+        data[0] = k_EPS4ReportIdBluetoothEffects;
+        data[1] = 0xC0 | 0x04;  /* Magic value HID + CRC, also sets interval to 4ms for samples */
+        data[3] = 0x03;  /* 0x1 is rumble, 0x2 is lightbar, 0x4 is the blink interval */
+
+        report_size = 78;
+        offset = 6;
+    } else {
+        data[0] = k_EPS4ReportIdUsbEffects;
+        data[1] = 0x07;  /* Magic value */
+
+        report_size = 32;
+        offset = 4;
+    }
+    effects = (DS4EffectsState_t *)&data[offset];
+
+    effects->ucRumbleLeft = ctx->rumble_left;
+    effects->ucRumbleRight = ctx->rumble_right;
+
+    /* Populate the LED state with the appropriate color from our lookup table */
+    if (ctx->color_set) {
+        effects->ucLedRed = ctx->led_red;
+        effects->ucLedGreen = ctx->led_green;
+        effects->ucLedBlue = ctx->led_blue;
+    } else {
+        SetLedsForPlayerIndex(effects, ctx->player_index);
+    }
+
+    if (ctx->is_bluetooth) {
+        /* Bluetooth reports need a CRC at the end of the packet (at least on Linux) */
+        Uint8 ubHdr = 0xA2; /* hidp header is part of the CRC calculation */
+        Uint32 unCRC;
+        unCRC = crc32(0, &ubHdr, 1);
+        unCRC = crc32(unCRC, data, (Uint32)(report_size - sizeof(unCRC)));
+        SDL_memcpy(&data[report_size - sizeof(unCRC)], &unCRC, sizeof(unCRC));
+    }
+
+    if (SDL_HIDAPI_SendRumble(device, data, report_size) != report_size) {
+        return SDL_SetError("Couldn't send rumble packet");
+    }
+    return 0;
+}
 
 static void
 HIDAPI_DriverPS4_SetDevicePlayerIndex(SDL_HIDAPI_Device *device, SDL_JoystickID instance_id, int player_index)
@@ -249,7 +283,7 @@ HIDAPI_DriverPS4_SetDevicePlayerIndex(SDL_HIDAPI_Device *device, SDL_JoystickID
     ctx->player_index = player_index;
 
     /* This will set the new LED state based on the new player index */
-    HIDAPI_DriverPS4_RumbleJoystick(device, SDL_JoystickFromInstanceID(instance_id), 0, 0);
+    HIDAPI_DriverPS4_UpdateEffects(device);
 }
 
 static SDL_bool
@@ -293,9 +327,9 @@ HIDAPI_DriverPS4_OpenJoystick(SDL_HIDAPI_Device *device, SDL_Joystick *joystick)
 
     if (HIDAPI_DriverPS4_CanRumble(device->vendor_id, device->product_id)) {
         if (ctx->is_bluetooth) {
-            ctx->rumble_supported = SDL_GetHintBoolean(SDL_HINT_JOYSTICK_HIDAPI_PS4_RUMBLE, SDL_FALSE);
+            ctx->effects_supported = SDL_GetHintBoolean(SDL_HINT_JOYSTICK_HIDAPI_PS4_RUMBLE, SDL_FALSE);
         } else {
-            ctx->rumble_supported = SDL_TRUE;
+            ctx->effects_supported = SDL_TRUE;
         }
     }
 
@@ -303,7 +337,7 @@ HIDAPI_DriverPS4_OpenJoystick(SDL_HIDAPI_Device *device, SDL_Joystick *joystick)
     ctx->player_index = SDL_JoystickGetPlayerIndex(joystick);
 
     /* Initialize LED and effect state */
-    HIDAPI_DriverPS4_RumbleJoystick(device, joystick, 0, 0);
+    HIDAPI_DriverPS4_UpdateEffects(device);
 
     /* Initialize the joystick capabilities */
     joystick->nbuttons = 16;
@@ -319,60 +353,11 @@ static int
 HIDAPI_DriverPS4_RumbleJoystick(SDL_HIDAPI_Device *device, SDL_Joystick *joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble)
 {
     SDL_DriverPS4_Context *ctx = (SDL_DriverPS4_Context *)device->context;
-    DS4EffectsState_t *effects;
-    Uint8 data[78];
-    int report_size, offset;
 
-    if (!ctx->rumble_supported) {
+    ctx->rumble_left = (low_frequency_rumble >> 8);
-        return SDL_Unsupported();
+    ctx->rumble_right = (high_frequency_rumble >> 8);
-    }
 
-    /* In order to send rumble, we have to send a complete effect packet */
+    return HIDAPI_DriverPS4_UpdateEffects(device);
-    SDL_zero(data);
-
-    if (ctx->is_bluetooth) {
-        data[0] = k_EPS4ReportIdBluetoothEffects;
-        data[1] = 0xC0 | 0x04;  /* Magic value HID + CRC, also sets interval to 4ms for samples */
-        data[3] = 0x03;  /* 0x1 is rumble, 0x2 is lightbar, 0x4 is the blink interval */
-
-        report_size = 78;
-        offset = 6;
-    } else {
-        data[0] = k_EPS4ReportIdUsbEffects;
-        data[1] = 0x07;  /* Magic value */
-
-        report_size = 32;
-        offset = 4;
-    }
-    effects = (DS4EffectsState_t *)&data[offset];
-
-    ctx->rumble_left = low_frequency_rumble;
-    ctx->rumble_right = high_frequency_rumble;
-    effects->ucRumbleLeft = (low_frequency_rumble >> 8);
-    effects->ucRumbleRight = (high_frequency_rumble >> 8);
-
-    /* Populate the LED state with the appropriate color from our lookup table */
-    if (ctx->color_set) {
-        effects->ucLedRed = ctx->led_red;
-        effects->ucLedGreen = ctx->led_green;
-        effects->ucLedBlue = ctx->led_blue;
-    } else {
-        SetLedsForPlayerIndex(effects, ctx->player_index);
-    }
-
-    if (ctx->is_bluetooth) {
-        /* Bluetooth reports need a CRC at the end of the packet (at least on Linux) */
-        Uint8 ubHdr = 0xA2; /* hidp header is part of the CRC calculation */
-        Uint32 unCRC;
-        unCRC = crc32(0, &ubHdr, 1);
-        unCRC = crc32(unCRC, data, (Uint32)(report_size - sizeof(unCRC)));
-        SDL_memcpy(&data[report_size - sizeof(unCRC)], &unCRC, sizeof(unCRC));
-    }
-
-    if (SDL_HIDAPI_SendRumble(device, data, report_size) != report_size) {
-        return SDL_SetError("Couldn't send rumble packet");
-    }
-    return 0;
 }
 
 static int
@@ -397,8 +382,7 @@ HIDAPI_DriverPS4_SetJoystickLED(SDL_HIDAPI_Device *device, SDL_Joystick *joystic
     ctx->led_green = green;
     ctx->led_blue = blue;
 
-    /* FIXME: Is there a better way to send this without sending another rumble packet? */
+    return HIDAPI_DriverPS4_UpdateEffects(device);
-    return HIDAPI_DriverPS4_RumbleJoystick(device, joystick, ctx->rumble_left, ctx->rumble_right);
 }
 
 static void

src/joystick/hidapi/SDL_hidapi_ps5.c

@@ -39,7 +39,8 @@
 typedef enum
 {
     k_EPS5ReportIdState = 0x01,
-    k_EPS5ReportIDOutput = 0x02,
+    k_EPS5ReportIdUsbEffects = 0x02,
+    k_EPS5ReportIdBluetoothEffects = 0x02,
     k_EPS5ReportIdBluetoothState = 0x31,
 } EPS5ReportId;
 
@@ -82,29 +83,41 @@ typedef struct
     Uint8 rgucUnknown1[8];              /* 40 */
     Uint8 rgucTimer2[4];                /* 48 - 32 bit little endian */
     Uint8 ucBatteryLevel;               /* 52 */
-    Uint8 ucConnectState;               /* 53 - 0x08 = USB, 0x03 = headphone */
+    Uint8 ucConnectState;               /* 53 - 0x08 = USB, 0x01 = headphone */
 
     /* There's more unknown data at the end, and a 32-bit CRC on Bluetooth */
 } PS5StatePacket_t;
 
-
+typedef struct
-static void ReadFeatureReport(hid_device *dev, Uint8 report_id)
 {
-    Uint8 report[USB_PACKET_LENGTH + 1];
+    Uint8 ucEnableBits;
-    int size;
+    Uint8 ucRumbleRight;
+    Uint8 ucRumbleLeft;
+    Uint8 rgucUnknown1[6];
+    Uint8 rgucRightTriggerEffect[11];
+    Uint8 rgucLeftTriggerEffect[11];
+    Uint8 rgucUnknown2[6];
+    Uint8 ucLedFlags;
+    Uint8 rgucUnknown3[2];
+    Uint8 ucLedAnim;
+    Uint8 ucLedBrightness;
+    Uint8 ucLedToggles;
+    Uint8 ucLedRed;
+    Uint8 ucLedGreen;
+    Uint8 ucLedBlue;
+} DS5EffectsState_t;
 
-    SDL_memset(report, 0, sizeof(report));
+SDL_COMPILE_TIME_ASSERT(X, sizeof(DS5EffectsState_t) == 46);
-    report[0] = report_id;
-    size = hid_get_feature_report(dev, report, sizeof(report));
-    if (size > 0) {
-#ifdef DEBUG_PS5_PROTOCOL
-        SDL_Log("Report %d\n", report_id);
-        HIDAPI_DumpPacket("Report: size = %d", report, size);
-#endif
-    }
-}
 
 typedef struct {
+    SDL_bool is_bluetooth;
+    int player_index;
+    Uint16 rumble_left;
+    Uint16 rumble_right;
+    SDL_bool color_set;
+    Uint8 led_red;
+    Uint8 led_green;
+    Uint8 led_blue;
     union
     {
         PS5SimpleStatePacket_t simple;
@@ -128,6 +141,45 @@ HIDAPI_DriverPS5_GetDeviceName(Uint16 vendor_id, Uint16 product_id)
     return NULL;
 }
 
+static SDL_bool ReadFeatureReport(hid_device *dev, Uint8 report_id)
+{
+    Uint8 report[USB_PACKET_LENGTH + 1];
+
+    SDL_memset(report, 0, sizeof(report));
+    report[0] = report_id;
+    if (hid_get_feature_report(dev, report, sizeof(report)) < 0) {
+        return SDL_FALSE;
+    }
+    return SDL_TRUE;
+}
+
+static void
+SetLedsForPlayerIndex(DS5EffectsState_t *effects, int player_index)
+{
+    /* This list is the same as what hid-sony.c uses in the Linux kernel.
+       The first 4 values correspond to what the PS4 assigns.
+    */
+    static const Uint8 colors[7][3] = {
+        { 0x00, 0x00, 0x40 }, /* Blue */
+        { 0x40, 0x00, 0x00 }, /* Red */
+        { 0x00, 0x40, 0x00 }, /* Green */
+        { 0x20, 0x00, 0x20 }, /* Pink */
+        { 0x02, 0x01, 0x00 }, /* Orange */
+        { 0x00, 0x01, 0x01 }, /* Teal */
+        { 0x01, 0x01, 0x01 }  /* White */
+    };
+
+    if (player_index >= 0) {
+        player_index %= SDL_arraysize(colors);
+    } else {
+        player_index = 0;
+    }
+
+    effects->ucLedRed = colors[player_index][0];
+    effects->ucLedGreen = colors[player_index][1];
+    effects->ucLedBlue = colors[player_index][2];
+}
+
 static SDL_bool
 HIDAPI_DriverPS5_InitDevice(SDL_HIDAPI_Device *device)
 {
@@ -140,9 +192,74 @@ HIDAPI_DriverPS5_GetDevicePlayerIndex(SDL_HIDAPI_Device *device, SDL_JoystickID
     return -1;
 }
 
+static int
+HIDAPI_DriverPS5_UpdateEffects(SDL_HIDAPI_Device *device)
+{
+    SDL_DriverPS5_Context *ctx = (SDL_DriverPS5_Context *)device->context;
+    DS5EffectsState_t *effects;
+    Uint8 data[78];
+    int report_size, offset;
+
+    SDL_zero(data);
+
+    if (ctx->is_bluetooth) {
+        /* FIXME: This doesn't work yet */
+        data[0] = k_EPS5ReportIdBluetoothEffects;
+        data[1] = 0x07;  /* Magic value */
+
+        report_size = 75;
+        offset = 2;
+    } else {
+        data[0] = k_EPS5ReportIdUsbEffects;
+        data[1] = 0x07;  /* Magic value */
+
+        report_size = 48;
+        offset = 2;
+    }
+    effects = (DS5EffectsState_t *)&data[offset];
+
+    effects->ucEnableBits = 0x04;   /* Enable LED color */
+
+    effects->ucRumbleLeft = ctx->rumble_left;
+    effects->ucRumbleRight = ctx->rumble_right;
+
+    /* Populate the LED state with the appropriate color from our lookup table */
+    if (ctx->color_set) {
+        effects->ucLedRed = ctx->led_red;
+        effects->ucLedGreen = ctx->led_green;
+        effects->ucLedBlue = ctx->led_blue;
+    } else {
+        SetLedsForPlayerIndex(effects, ctx->player_index);
+    }
+
+    if (ctx->is_bluetooth) {
+        /* Bluetooth reports need a CRC at the end of the packet (at least on Linux) */
+        Uint8 ubHdr = 0xA2; /* hidp header is part of the CRC calculation */
+        Uint32 unCRC;
+        unCRC = crc32(0, &ubHdr, 1);
+        unCRC = crc32(unCRC, data, (Uint32)(report_size - sizeof(unCRC)));
+        SDL_memcpy(&data[report_size - sizeof(unCRC)], &unCRC, sizeof(unCRC));
+    }
+
+    if (SDL_HIDAPI_SendRumble(device, data, report_size) != report_size) {
+        return SDL_SetError("Couldn't send rumble packet");
+    }
+    return 0;
+}
+
 static void
 HIDAPI_DriverPS5_SetDevicePlayerIndex(SDL_HIDAPI_Device *device, SDL_JoystickID instance_id, int player_index)
 {
+    SDL_DriverPS5_Context *ctx = (SDL_DriverPS5_Context *)device->context;
+
+    if (!ctx) {
+        return;
+    }
+
+    ctx->player_index = player_index;
+
+    /* This will set the new LED state based on the new player index */
+    HIDAPI_DriverPS5_UpdateEffects(device);
 }
 
 static SDL_bool
@@ -169,6 +286,12 @@ HIDAPI_DriverPS5_OpenJoystick(SDL_HIDAPI_Device *device, SDL_Joystick *joystick)
     */
     ReadFeatureReport(device->dev, k_EPS5FeatureReportIdSerialNumber);
 
+    /* Initialize player index (needed for setting LEDs) */
+    ctx->player_index = SDL_JoystickGetPlayerIndex(joystick);
+
+    /* Initialize LED and effect state */
+    HIDAPI_DriverPS5_UpdateEffects(device);
+
     /* Initialize the joystick capabilities */
     joystick->nbuttons = 17;
     joystick->naxes = SDL_CONTROLLER_AXIS_MAX;
@@ -182,19 +305,12 @@ HIDAPI_DriverPS5_OpenJoystick(SDL_HIDAPI_Device *device, SDL_Joystick *joystick)
 static int
 HIDAPI_DriverPS5_RumbleJoystick(SDL_HIDAPI_Device *device, SDL_Joystick *joystick, Uint16 low_frequency_rumble, Uint16 high_frequency_rumble)
 {
-    Uint8 data[5];
+    SDL_DriverPS5_Context *ctx = (SDL_DriverPS5_Context *)device->context;
 
-    /* This works over USB, not over Bluetooth */
+    ctx->rumble_left = (low_frequency_rumble >> 8);
-    SDL_zero(data);
+    ctx->rumble_right = (high_frequency_rumble >> 8);
-    data[0] = k_EPS5ReportIDOutput;
-    data[1] = 0x7;  /* Magic value */
-    data[3] = (high_frequency_rumble >> 8);
-    data[4] = (low_frequency_rumble >> 8);
 
-    if (SDL_HIDAPI_SendRumble(device, data, sizeof(data)) != sizeof(data)) {
+    return HIDAPI_DriverPS5_UpdateEffects(device);
-        return SDL_SetError("Couldn't send rumble packet");
-    }
-    return 0;
 }
 
 static int
@@ -212,7 +328,14 @@ HIDAPI_DriverPS5_HasJoystickLED(SDL_HIDAPI_Device *device, SDL_Joystick *joystic
 static int
 HIDAPI_DriverPS5_SetJoystickLED(SDL_HIDAPI_Device *device, SDL_Joystick *joystick, Uint8 red, Uint8 green, Uint8 blue)
 {
-    return SDL_Unsupported();
+    SDL_DriverPS5_Context *ctx = (SDL_DriverPS5_Context *)device->context;
+
+    ctx->color_set = SDL_TRUE;
+    ctx->led_red = red;
+    ctx->led_green = green;
+    ctx->led_blue = blue;
+
+    return HIDAPI_DriverPS5_UpdateEffects(device);
 }
 
 static void
@@ -431,6 +554,9 @@ HIDAPI_DriverPS5_HandleStatePacket(SDL_Joystick *joystick, hid_device *dev, SDL_
     touchpad_y = (packet->rgucTouchpadData2[1] >> 4) | ((int)packet->rgucTouchpadData2[2] << 4);
     SDL_PrivateJoystickTouchpad(joystick, 0, 1, touchpad_state, touchpad_x * TOUCHPAD_SCALEX, touchpad_y * TOUCHPAD_SCALEY, touchpad_state ? 1.0f : 0.0f);
 
+    /* Update whether the controller is in USB or Bluetooth data state */
+    ctx->is_bluetooth = (packet->ucConnectState & 0x80) ? SDL_FALSE : SDL_TRUE;
+
     SDL_memcpy(&ctx->last_state.state, packet, sizeof(ctx->last_state.state));
 }
 

src/joystick/hidapi/SDL_hidapijoystick.c

@@ -390,6 +390,27 @@ HIDAPI_ShutdownDiscovery()
 #endif
 }
 
+/* Public domain CRC implementation adapted from:
+   http://home.thep.lu.se/~bjorn/crc/crc32_simple.c
+*/
+static Uint32 crc32_for_byte(Uint32 r)
+{
+    int i;
+    for(i = 0; i < 8; ++i) {
+        r = (r & 1? 0: (Uint32)0xEDB88320L) ^ r >> 1;
+    }
+    return r ^ (Uint32)0xFF000000L;
+}
+
+Uint32 crc32(Uint32 crc, const void *data, int count)
+{
+    int i;
+    for(i = 0; i < count; ++i) {
+        crc = crc32_for_byte((Uint8)crc ^ ((const Uint8*)data)[i]) ^ crc >> 8;
+    }
+    return crc;
+}
+
 void
 HIDAPI_DumpPacket(const char *prefix, Uint8 *data, int size)
 {

src/joystick/hidapi/SDL_hidapijoystick_c.h

@@ -127,6 +127,8 @@ extern void HIDAPI_JoystickDisconnected(SDL_HIDAPI_Device *device, SDL_JoystickI
 
 extern void HIDAPI_DumpPacket(const char *prefix, Uint8 *data, int size);
 
+Uint32 crc32(Uint32 crc, const void *data, int count);
+
 #endif /* SDL_JOYSTICK_HIDAPI_H */
 
 /* vi: set ts=4 sw=4 expandtab: */