From c6c116e84710984eeb0c7863bda3171594e101ae Mon Sep 17 00:00:00 2001 From: Sam Lantinga Date: Mon, 16 Nov 2020 10:51:13 -0800 Subject: [PATCH] Implemented setting the LED color on the PS5 controller --- src/joystick/hidapi/SDL_hidapi_ps4.c | 150 ++++++++--------- src/joystick/hidapi/SDL_hidapi_ps5.c | 182 +++++++++++++++++---- src/joystick/hidapi/SDL_hidapijoystick.c | 21 +++ src/joystick/hidapi/SDL_hidapijoystick_c.h | 2 + 4 files changed, 244 insertions(+), 111 deletions(-) diff --git a/src/joystick/hidapi/SDL_hidapi_ps4.c b/src/joystick/hidapi/SDL_hidapi_ps4.c index f48f54eff..515d88d3f 100644 --- a/src/joystick/hidapi/SDL_hidapi_ps4.c +++ b/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; - Uint16 rumble_right; + Uint8 rumble_left; + 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) { - return SDL_Unsupported(); - } + ctx->rumble_left = (low_frequency_rumble >> 8); + ctx->rumble_right = (high_frequency_rumble >> 8); - /* In order to send rumble, we have to send a complete effect packet */ - 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; + return HIDAPI_DriverPS4_UpdateEffects(device); } 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_RumbleJoystick(device, joystick, ctx->rumble_left, ctx->rumble_right); + return HIDAPI_DriverPS4_UpdateEffects(device); } static void diff --git a/src/joystick/hidapi/SDL_hidapi_ps5.c b/src/joystick/hidapi/SDL_hidapi_ps5.c index 82bdfdf6b..cd5612f20 100644 --- a/src/joystick/hidapi/SDL_hidapi_ps5.c +++ b/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; - -static void ReadFeatureReport(hid_device *dev, Uint8 report_id) +typedef struct { - Uint8 report[USB_PACKET_LENGTH + 1]; - int size; + Uint8 ucEnableBits; + 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)); - 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 - } -} +SDL_COMPILE_TIME_ASSERT(X, sizeof(DS5EffectsState_t) == 46); 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 */ - SDL_zero(data); - data[0] = k_EPS5ReportIDOutput; - data[1] = 0x7; /* Magic value */ - data[3] = (high_frequency_rumble >> 8); - data[4] = (low_frequency_rumble >> 8); + ctx->rumble_left = (low_frequency_rumble >> 8); + ctx->rumble_right = (high_frequency_rumble >> 8); - if (SDL_HIDAPI_SendRumble(device, data, sizeof(data)) != sizeof(data)) { - return SDL_SetError("Couldn't send rumble packet"); - } - return 0; + return HIDAPI_DriverPS5_UpdateEffects(device); } 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)); } diff --git a/src/joystick/hidapi/SDL_hidapijoystick.c b/src/joystick/hidapi/SDL_hidapijoystick.c index c732cbaa2..1d819d920 100644 --- a/src/joystick/hidapi/SDL_hidapijoystick.c +++ b/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) { diff --git a/src/joystick/hidapi/SDL_hidapijoystick_c.h b/src/joystick/hidapi/SDL_hidapijoystick_c.h index 4ea3f6029..498c08f31 100644 --- a/src/joystick/hidapi/SDL_hidapijoystick_c.h +++ b/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: */