diff --git a/rootfs/usr/share/inputplumber/schema/composite_device_v1.json b/rootfs/usr/share/inputplumber/schema/composite_device_v1.json index f9523280..bff10205 100644 --- a/rootfs/usr/share/inputplumber/schema/composite_device_v1.json +++ b/rootfs/usr/share/inputplumber/schema/composite_device_v1.json @@ -65,7 +65,11 @@ "xbox-series", "deck", "ds5", + "ds5-usb", + "ds5-bt", "ds5-edge", + "ds5-edge-usb", + "ds5-edge-bt", "touchpad", "touchscreen" ] diff --git a/src/drivers/dualsense/hid_report.rs b/src/drivers/dualsense/hid_report.rs index 911f6c90..ff713557 100644 --- a/src/drivers/dualsense/hid_report.rs +++ b/src/drivers/dualsense/hid_report.rs @@ -15,6 +15,14 @@ pub enum PackedInputDataReport { } impl PackedInputDataReport { + pub fn new_bt() -> Self { + Self::Bluetooth(BluetoothPackedInputDataReport::new()) + } + + pub fn new_usb() -> Self { + Self::Usb(USBPackedInputDataReport::new()) + } + pub fn unpack(buf: &[u8], size: usize) -> Result> { let report_id = buf[0]; match report_id { @@ -544,6 +552,12 @@ pub struct BluetoothPackedInputDataReport { pub bt_crc_fail_count: u8, } +impl BluetoothPackedInputDataReport { + pub fn new() -> Self { + Self::default() + } +} + impl Default for BluetoothPackedInputDataReport { fn default() -> Self { Self { @@ -794,3 +808,44 @@ impl Default for UsbPackedOutputReportShort { } } } + +// When using bluetooth, the first byte after the reportID is uint8_t seq_tag, +// while the next one is uint8_t tag, following bytes are the same as USB. +#[derive(PackedStruct, Debug, Copy, Clone, PartialEq)] +#[packed_struct(bit_numbering = "msb0", size_bytes = "78")] +pub struct BluetoothPackedOutputReport { + // byte 0 + #[packed_field(bytes = "0")] + pub report_id: u8, // Report ID (always 0x31) + + // byte 1 + #[packed_field(bytes = "1")] + pub seq_tag: u8, + + // byte 2 + #[packed_field(bytes = "2")] + pub tag: u8, + + // byte 3-49 + #[packed_field(bytes = "3..=49")] + pub state: SetStatePackedOutputData, + + #[packed_field(bytes = "50..=73", endian = "lsb")] + pub reserved: [u8; 24], + + #[packed_field(bytes = "74..=77", endian = "lsb")] + pub crc32: u32, +} + +impl Default for BluetoothPackedOutputReport { + fn default() -> Self { + Self { + report_id: 0x31, + seq_tag: 0x00, + tag: 0x00, + state: Default::default(), + reserved: Default::default(), + crc32: Default::default(), + } + } +} diff --git a/src/input/target/dualsense.rs b/src/input/target/dualsense.rs index 61dafdae..688b27de 100644 --- a/src/input/target/dualsense.rs +++ b/src/input/target/dualsense.rs @@ -19,7 +19,8 @@ use crate::{ STICK_Y_MAX, STICK_Y_MIN, TRIGGER_MAX, }, hid_report::{ - Direction, PackedInputDataReport, USBPackedInputDataReport, UsbPackedOutputReport, + BluetoothPackedInputDataReport, BluetoothPackedOutputReport, Direction, + PackedInputDataReport, USBPackedInputDataReport, UsbPackedOutputReport, UsbPackedOutputReportShort, }, report_descriptor::{ @@ -43,6 +44,55 @@ use crate::{ use super::{InputError, OutputError, TargetInputDevice, TargetOutputDevice}; +const PS_INPUT_CRC32_SEED: u8 = 0xA1u8; +const PS_OUTPUT_CRC32_SEED: u8 = 0xA2u8; +const PS_FEATURE_CRC32_SEED: u8 = 0xA3u8; + +const CRC32_LE_TABLE: [u32; 256] = [ + 0x00000000, 0x77073096, 0xee0e612c, 0x990951ba, 0x076dc419, 0x706af48f, 0xe963a535, 0x9e6495a3, + 0x0edb8832, 0x79dcb8a4, 0xe0d5e91e, 0x97d2d988, 0x09b64c2b, 0x7eb17cbd, 0xe7b82d07, 0x90bf1d91, + 0x1db71064, 0x6ab020f2, 0xf3b97148, 0x84be41de, 0x1adad47d, 0x6ddde4eb, 0xf4d4b551, 0x83d385c7, + 0x136c9856, 0x646ba8c0, 0xfd62f97a, 0x8a65c9ec, 0x14015c4f, 0x63066cd9, 0xfa0f3d63, 0x8d080df5, + 0x3b6e20c8, 0x4c69105e, 0xd56041e4, 0xa2677172, 0x3c03e4d1, 0x4b04d447, 0xd20d85fd, 0xa50ab56b, + 0x35b5a8fa, 0x42b2986c, 0xdbbbc9d6, 0xacbcf940, 0x32d86ce3, 0x45df5c75, 0xdcd60dcf, 0xabd13d59, + 0x26d930ac, 0x51de003a, 0xc8d75180, 0xbfd06116, 0x21b4f4b5, 0x56b3c423, 0xcfba9599, 0xb8bda50f, + 0x2802b89e, 0x5f058808, 0xc60cd9b2, 0xb10be924, 0x2f6f7c87, 0x58684c11, 0xc1611dab, 0xb6662d3d, + 0x76dc4190, 0x01db7106, 0x98d220bc, 0xefd5102a, 0x71b18589, 0x06b6b51f, 0x9fbfe4a5, 0xe8b8d433, + 0x7807c9a2, 0x0f00f934, 0x9609a88e, 0xe10e9818, 0x7f6a0dbb, 0x086d3d2d, 0x91646c97, 0xe6635c01, + 0x6b6b51f4, 0x1c6c6162, 0x856530d8, 0xf262004e, 0x6c0695ed, 0x1b01a57b, 0x8208f4c1, 0xf50fc457, + 0x65b0d9c6, 0x12b7e950, 0x8bbeb8ea, 0xfcb9887c, 0x62dd1ddf, 0x15da2d49, 0x8cd37cf3, 0xfbd44c65, + 0x4db26158, 0x3ab551ce, 0xa3bc0074, 0xd4bb30e2, 0x4adfa541, 0x3dd895d7, 0xa4d1c46d, 0xd3d6f4fb, + 0x4369e96a, 0x346ed9fc, 0xad678846, 0xda60b8d0, 0x44042d73, 0x33031de5, 0xaa0a4c5f, 0xdd0d7cc9, + 0x5005713c, 0x270241aa, 0xbe0b1010, 0xc90c2086, 0x5768b525, 0x206f85b3, 0xb966d409, 0xce61e49f, + 0x5edef90e, 0x29d9c998, 0xb0d09822, 0xc7d7a8b4, 0x59b33d17, 0x2eb40d81, 0xb7bd5c3b, 0xc0ba6cad, + 0xedb88320, 0x9abfb3b6, 0x03b6e20c, 0x74b1d29a, 0xead54739, 0x9dd277af, 0x04db2615, 0x73dc1683, + 0xe3630b12, 0x94643b84, 0x0d6d6a3e, 0x7a6a5aa8, 0xe40ecf0b, 0x9309ff9d, 0x0a00ae27, 0x7d079eb1, + 0xf00f9344, 0x8708a3d2, 0x1e01f268, 0x6906c2fe, 0xf762575d, 0x806567cb, 0x196c3671, 0x6e6b06e7, + 0xfed41b76, 0x89d32be0, 0x10da7a5a, 0x67dd4acc, 0xf9b9df6f, 0x8ebeeff9, 0x17b7be43, 0x60b08ed5, + 0xd6d6a3e8, 0xa1d1937e, 0x38d8c2c4, 0x4fdff252, 0xd1bb67f1, 0xa6bc5767, 0x3fb506dd, 0x48b2364b, + 0xd80d2bda, 0xaf0a1b4c, 0x36034af6, 0x41047a60, 0xdf60efc3, 0xa867df55, 0x316e8eef, 0x4669be79, + 0xcb61b38c, 0xbc66831a, 0x256fd2a0, 0x5268e236, 0xcc0c7795, 0xbb0b4703, 0x220216b9, 0x5505262f, + 0xc5ba3bbe, 0xb2bd0b28, 0x2bb45a92, 0x5cb36a04, 0xc2d7ffa7, 0xb5d0cf31, 0x2cd99e8b, 0x5bdeae1d, + 0x9b64c2b0, 0xec63f226, 0x756aa39c, 0x026d930a, 0x9c0906a9, 0xeb0e363f, 0x72076785, 0x05005713, + 0x95bf4a82, 0xe2b87a14, 0x7bb12bae, 0x0cb61b38, 0x92d28e9b, 0xe5d5be0d, 0x7cdcefb7, 0x0bdbdf21, + 0x86d3d2d4, 0xf1d4e242, 0x68ddb3f8, 0x1fda836e, 0x81be16cd, 0xf6b9265b, 0x6fb077e1, 0x18b74777, + 0x88085ae6, 0xff0f6a70, 0x66063bca, 0x11010b5c, 0x8f659eff, 0xf862ae69, 0x616bffd3, 0x166ccf45, + 0xa00ae278, 0xd70dd2ee, 0x4e048354, 0x3903b3c2, 0xa7672661, 0xd06016f7, 0x4969474d, 0x3e6e77db, + 0xaed16a4a, 0xd9d65adc, 0x40df0b66, 0x37d83bf0, 0xa9bcae53, 0xdebb9ec5, 0x47b2cf7f, 0x30b5ffe9, + 0xbdbdf21c, 0xcabac28a, 0x53b39330, 0x24b4a3a6, 0xbad03605, 0xcdd70693, 0x54de5729, 0x23d967bf, + 0xb3667a2e, 0xc4614ab8, 0x5d681b02, 0x2a6f2b94, 0xb40bbe37, 0xc30c8ea1, 0x5a05df1b, 0x2d02ef8d, +]; + +fn crc32_le(initial_crc: u32, buf: &[u8]) -> u32 { + buf.iter().fold(initial_crc, |crc, &byte| { + (crc >> 8) ^ CRC32_LE_TABLE[((crc & 0xFF) ^ (byte as u32)) as usize] + }) +} + +fn crc32_calc(seed: u8, data: &[u8]) -> u32 { + !crc32_le(crc32_le(0xFFFFFFFFu32, &[seed]), data) +} + /// The type of DualSense device to emulate. Currently two models are supported: /// DualSense and DualSense Edge. #[derive(Debug, Copy, Clone, PartialEq)] @@ -126,7 +176,10 @@ impl DualSenseDevice { let device = DualSenseDevice::create_virtual_device(&hardware)?; Ok(Self { device, - state: PackedInputDataReport::Usb(USBPackedInputDataReport::new()), + state: match &hardware.bus_type { + &BusType::Usb => PackedInputDataReport::new_usb(), + &BusType::Bluetooth => PackedInputDataReport::new_bt(), + }, timestamp: 0, hardware, queued_events: Vec::new(), @@ -186,27 +239,32 @@ impl DualSenseDevice { /// Write the current device state to the device fn write_state(&mut self) -> Result<(), Box> { - match self.state { - PackedInputDataReport::Usb(state) => { - let data = state.pack()?; - - // Write the state to the virtual HID - if let Err(e) = self.device.write(&data) { - let err = format!("Failed to write input data report: {:?}", e); - return Err(err.into()); - } - } - PackedInputDataReport::Bluetooth(state) => { - let data = state.pack()?; + let mut data = match self.state { + PackedInputDataReport::Usb(state) => Vec::::from(state.pack()?), + PackedInputDataReport::Bluetooth(state) => Vec::::from(state.pack()?), + }; - // Write the state to the virtual HID - if let Err(e) = self.device.write(&data) { - let err = format!("Failed to write input data report: {:?}", e); - return Err(err.into()); - } + match &self.hardware.bus_type { + BusType::Bluetooth => { + let len = data.len(); + assert_eq!(len, 78); + + let crc = crc32_calc(PS_INPUT_CRC32_SEED, &data[..(len - 4)]); + + data[len - 4] = ((crc >> 0u32) & 0xFFu32) as u8; + data[len - 3] = ((crc >> 8u32) & 0xFFu32) as u8; + data[len - 2] = ((crc >> 16u32) & 0xFFu32) as u8; + data[len - 1] = ((crc >> 24u32) & 0xFFu32) as u8; } + _ => {} }; + // Write the state to the virtual HID + if let Err(e) = self.device.write(data.as_slice()) { + let err = format!("Failed to write input data report: {:?}", e); + return Err(err.into()); + } + Ok(()) } @@ -628,12 +686,6 @@ impl DualSenseDevice { /// Handle [OutputEvent::Output] events from the HIDRAW device. These are /// events which should be forwarded back to source devices. fn handle_output(&mut self, data: Vec) -> Result, Box> { - // Validate the output report size - let _expected_report_size = match self.hardware.bus_type { - BusType::Bluetooth => OUTPUT_REPORT_BT_SIZE, - BusType::Usb => OUTPUT_REPORT_USB_SIZE, - }; - // The first byte should be the report id let Some(report_id) = data.first() else { log::warn!("Received empty output report."); @@ -642,10 +694,11 @@ impl DualSenseDevice { log::debug!("Got output report with ID: {report_id}"); - match *report_id { + let state = match *report_id { OUTPUT_REPORT_USB => { - log::debug!("Received USB output report with length: {}", data.len()); - let state = match data.len() { + let report_len = data.len(); + log::debug!("Received USB output report with length: {report_len}"); + match data.len() { OUTPUT_REPORT_USB_SIZE => { let buf: [u8; OUTPUT_REPORT_USB_SIZE] = data.try_into().unwrap(); let report = UsbPackedOutputReport::unpack(&buf)?; @@ -671,40 +724,48 @@ impl DualSenseDevice { state } _ => { - log::warn!("Failed to unpack output report. Expected size {OUTPUT_REPORT_USB_SIZE} or {OUTPUT_REPORT_USB_SHORT_SIZE}, got {}.", data.len()); + log::warn!("Failed to unpack output report. Expected size {OUTPUT_REPORT_USB_SIZE} or {OUTPUT_REPORT_USB_SHORT_SIZE}, got {report_len}."); return Ok(vec![]); } - }; - - log::trace!("{}", state); - - // Send the output report to the composite device so it can - // be processed by source devices. - let event = OutputEvent::DualSense(state); - return Ok(vec![event]); + } } OUTPUT_REPORT_BT => { - log::debug!( - "Received Bluetooth output report with length: {}", - data.len() - ); - // + let report_len = data.len(); + log::debug!("Received Bluetooth output report with length: {report_len}"); + + match report_len { + OUTPUT_REPORT_BT_SIZE => { + let buf: [u8; OUTPUT_REPORT_BT_SIZE] = data.try_into().unwrap(); + let report = BluetoothPackedOutputReport::unpack(&buf)?; + report.state + } + _ => { + log::warn!("Failed to unpack bluetooth output report. Expected size {OUTPUT_REPORT_BT_SIZE}, got {report_len}."); + return Ok(vec![]); + } + } } _ => { log::debug!("Unknown output report: {report_id}"); + + return Ok(vec![]); } - } + }; + + log::trace!("{}", state); - Ok(vec![]) + // Send the output report to the composite device so it can + // be processed by source devices. + let event = OutputEvent::DualSense(state); + return Ok(vec![event]); } /// Handle [OutputEvent::GetReport] events from the HIDRAW device fn handle_get_report( &mut self, - id: u32, report_number: u8, _report_type: uhid_virt::ReportType, - ) -> Result<(), Box> { + ) -> Result, Box> { // Handle report pairing requests let data = match report_number { // Pairing information report @@ -734,11 +795,6 @@ impl DualSenseDevice { 0x00, ]; - // If this is a bluetooth gamepad, include the crc - if self.hardware.bus_type == BusType::Bluetooth { - // TODO: Handle bluetooth CRC32 - } - data } // Firmware information report @@ -812,11 +868,6 @@ impl DualSenseDevice { 0x00, ]; - // If this is a bluetooth gamepad, include the crc - if self.hardware.bus_type == BusType::Bluetooth { - // TODO: Handle bluetooth CRC32 - } - data } // Calibration report @@ -867,11 +918,6 @@ impl DualSenseDevice { 0x00, ]; - // If this is a bluetooth gamepad, include the crc - if self.hardware.bus_type == BusType::Bluetooth { - // TODO: Handle bluetooth CRC32 - } - data } _ => { @@ -880,13 +926,7 @@ impl DualSenseDevice { } }; - // Write the report reply to the HIDRAW device - if let Err(e) = self.device.write_get_report_reply(id, 0, data) { - log::warn!("Failed to write get report reply: {:?}", e); - return Err(e.to_string().into()); - } - - Ok(()) + Ok(data) } } @@ -1049,8 +1089,32 @@ impl TargetOutputDevice for DualSenseDevice { // device. You should read the payload and forward it to the device. uhid_virt::OutputEvent::Output { data } => { log::trace!("Got output data: {:?}", data); - let result = self.handle_output(data); - match result { + + match self.hardware.bus_type { + BusType::Bluetooth => { + // crc protected data is the whole data minus the final 4 bytes (because those are the crc itself) + let len = data.len(); + let crc_protected_data = &data.as_slice()[..(len - 4)]; + let crc = crc32_calc(PS_OUTPUT_CRC32_SEED, crc_protected_data); + let report_crc: u32 = ((data[len - 1] as u32) << 24u32) + | ((data[len - 2] as u32) << 16u32) + | ((data[len - 3] as u32) << 8u32) + | ((data[len - 4] as u32) << 0u32); + + // if crc does not match ignore the event + if crc != report_crc { + log::error!( + "Error in checking crc32: expected 0x{:x} got 0x{:x}", + report_crc, + crc + ); + return Ok(vec![]); + } + } + BusType::Usb => {} + }; + + match self.handle_output(data) { Ok(events) => Ok(events), Err(e) => { let err = format!("Failed process output event: {:?}", e); @@ -1079,11 +1143,34 @@ impl TargetOutputDevice for DualSenseDevice { "Received GetReport event: id: {id}, num: {report_number}, type: {:?}", report_type ); - let result = self.handle_get_report(id, report_number, report_type); - if let Err(e) = result { - let err = format!("Failed to process GetReport event: {:?}", e); - return Err(err.into()); + let mut data = match self.handle_get_report(report_number, report_type) { + Ok(data) => data, + Err(e) => { + let err = format!("Failed to process GetReport event for id {id}, report_number {report_number}: {e}"); + return Err(err.into()); + } + }; + + // If this is a bluetooth gamepad, include the crc + match &self.hardware.bus_type { + BusType::Bluetooth => { + let len = data.len(); + let crc = crc32_calc(PS_FEATURE_CRC32_SEED, &data[..(len - 4)]); + + data[len - 4] = ((crc >> 0u32) & 0xFFu32) as u8; + data[len - 3] = ((crc >> 8u32) & 0xFFu32) as u8; + data[len - 2] = ((crc >> 16u32) & 0xFFu32) as u8; + data[len - 1] = ((crc >> 24u32) & 0xFFu32) as u8; + } + _ => {} + }; + + // Write the report reply to the HIDRAW device + if let Err(e) = self.device.write_get_report_reply(id, 0, data) { + log::warn!("Failed to write get report reply: {:?}", e); + return Err(e.to_string().into()); } + Ok(vec![]) } // This is the SET_REPORT equivalent of UHID_GET_REPORT. On receipt, you shall diff --git a/src/input/target/mod.rs b/src/input/target/mod.rs index 0a87a262..3989065f 100644 --- a/src/input/target/mod.rs +++ b/src/input/target/mod.rs @@ -174,10 +174,26 @@ impl TargetDeviceTypeId { id: "ds5", name: "Sony Interactive Entertainment DualSense Wireless Controller", }, + TargetDeviceTypeId { + id: "ds5-usb", + name: "Sony Interactive Entertainment DualSense Wireless Controller", + }, + TargetDeviceTypeId { + id: "ds5-bt", + name: "Sony Interactive Entertainment DualSense Wireless Controller", + }, TargetDeviceTypeId { id: "ds5-edge", name: "Sony Interactive Entertainment DualSense Edge Wireless Controller", }, + TargetDeviceTypeId { + id: "ds5-edge-usb", + name: "Sony Interactive Entertainment DualSense Edge Wireless Controller", + }, + TargetDeviceTypeId { + id: "ds5-edge-bt", + name: "Sony Interactive Entertainment DualSense Edge Wireless Controller", + }, TargetDeviceTypeId { id: "hori-steam", name: "HORI CO.,LTD. HORIPAD STEAM",