scdsu_core/
dsu.rs

1//! Provides functionality for working with DSU protocol data.
2
3/// DSU frame representing all controller data sent over the CemuHook protocol.
4/// DSU protocol reference can be found [`here`](https://v1993.github.io/cemuhook-protocol/).
5#[derive(Debug, Clone, Copy, PartialEq)]
6pub struct DSUFrame {
7    pub dpad_left: bool,
8    pub dpad_down: bool,
9    pub dpad_right: bool,
10    pub dpad_up: bool,
11    pub options: bool,
12    pub r3: bool,
13    pub l3: bool,
14    pub share: bool,
15    pub y: bool,
16    pub b: bool,
17    pub a: bool,
18    pub x: bool,
19    pub r1: bool,
20    pub l1: bool,
21    pub r2: bool,
22    pub l2: bool,
23    pub home: bool,
24    pub touch: bool,
25    pub left_stick_x: u8,
26    pub left_stick_y: u8,
27    pub right_stick_x: u8,
28    pub right_stick_y: u8,
29    pub analog_r2: u8,
30    pub analog_l2: u8,
31    pub accel_x: f32,
32    pub accel_y: f32,
33    pub accel_z: f32,
34    pub gyro_x: f32,
35    pub gyro_y: f32,
36    pub gyro_z: f32,
37}
38
39/// Write the common CemuHook packet header into `buf`
40///
41/// `buf` must be at least 16 bytes
42/// The CRC32 field (bytes 8..12) is zeroed so the caller can compute it after
43/// filling the payload.
44fn write_header(buf: &mut [u8], payload_len: u16, client_id: u32) {
45    buf[0..4].copy_from_slice(b"DSUS");
46    buf[4..6].copy_from_slice(&1001u16.to_le_bytes());
47    buf[6..8].copy_from_slice(&payload_len.to_le_bytes());
48    buf[8..12].fill(0); // crc32 placeholder
49    buf[12..16].copy_from_slice(&client_id.to_le_bytes());
50}
51
52/// CRC32 used by the CemuHook protocol.
53/// Matches the algorithm from SteamDeckGyroDSU.
54fn crc32(data: &[u8]) -> u32 {
55    let mut crc: u32 = 0xFFFFFFFF;
56    for &byte in data {
57        crc ^= byte as u32;
58        for _ in 0..8 {
59            crc = if crc & 1 != 0 {
60                (crc >> 1) ^ 0xEDB8_8320
61            } else {
62                crc >> 1
63            };
64        }
65    }
66    !crc
67}
68
69/// Build a CemuHook protocol-version response packet into `buf`.
70/// `buf` must be at least 22 bytes.
71pub fn write_version_response(buf: &mut [u8], client_id: u32) {
72    write_header(buf, 2, client_id);
73    buf[16..20].copy_from_slice(&0x100000u32.to_le_bytes());
74    buf[20..22].copy_from_slice(&1001u16.to_le_bytes());
75
76    let c = crc32(&buf[..22]);
77    buf[8..12].copy_from_slice(&c.to_le_bytes());
78}
79
80/// Build a CemuHook controller-info response packet into `buf`.
81/// `buf` must be at least 32 bytes.
82pub fn write_info_response(buf: &mut [u8], slot: u8, client_id: u32, connected: bool) {
83    buf.fill(0);
84    write_header(buf, 16, client_id); // payload length = 32 - 16
85    buf[16..20].copy_from_slice(&0x100001u32.to_le_bytes());
86
87    // SharedResponse
88    buf[20] = slot;
89    if connected {
90        buf[21] = 2; // slotState = connected
91        buf[22] = 2; // deviceModel = full gyro
92        buf[23] = 1; // connection = USB
93    }
94    // Info response: byte 31 is a zero byte (not a connected flag).
95
96    let c = crc32(&buf[..32]);
97    buf[8..12].copy_from_slice(&c.to_le_bytes());
98}
99
100/// Build a CemuHook data-event packet (100 bytes) from a `DSUFrame`.
101pub fn write_data_event(
102    buf: &mut [u8; 100],
103    frame: &DSUFrame,
104    packet_num: u32,
105    client_id: u32,
106    slot: u8,
107    timestamp_us: u64,
108    invert_pitch: bool,
109) {
110    buf.fill(0);
111
112    write_header(buf, 84, client_id); // 100 - 16 = 84
113    buf[16..20].copy_from_slice(&0x100002u32.to_le_bytes());
114
115    // SharedResponse (11 bytes, offset 20)
116    buf[20] = slot; // slot requested by client
117    buf[21] = 2; // slotState = connected
118    buf[22] = 2; // deviceModel = full gyro
119    buf[23] = 1; // connection = USB
120    // mac1/mac2/battery already zero
121    buf[31] = 1; // connected
122
123    // packetNumber (offset 32)
124    buf[32..36].copy_from_slice(&packet_num.to_le_bytes());
125
126    // Buttons (offset 36)
127    buf[36] = get_bitmask(&[
128        (frame.dpad_left, 7),
129        (frame.dpad_down, 6),
130        (frame.dpad_right, 5),
131        (frame.dpad_up, 4),
132        (frame.options, 3),
133        (frame.r3, 2),
134        (frame.l3, 1),
135        (frame.share, 0),
136    ]);
137    buf[37] = get_bitmask(&[
138        (frame.y, 7),
139        (frame.b, 6),
140        (frame.a, 5),
141        (frame.x, 4),
142        (frame.r1, 3),
143        (frame.l1, 2),
144        (frame.r2, 1),
145        (frame.l2, 0),
146    ]);
147    buf[38] = u8::from(frame.home);
148    buf[39] = u8::from(frame.touch);
149
150    // Sticks (offset 40)
151    buf[40] = frame.left_stick_x;
152    buf[41] = frame.left_stick_y;
153    buf[42] = frame.right_stick_x;
154    buf[43] = frame.right_stick_y;
155
156    // Analog buttons (offset 44)
157    // Cemu reads these analog values even for digital buttons.
158    buf[44] = if frame.dpad_left { u8::MAX } else { 0 };
159    buf[45] = if frame.dpad_down { u8::MAX } else { 0 };
160    buf[46] = if frame.dpad_right { u8::MAX } else { 0 };
161    buf[47] = if frame.dpad_up { u8::MAX } else { 0 };
162    buf[48] = if frame.y { u8::MAX } else { 0 };
163    buf[49] = if frame.b { u8::MAX } else { 0 };
164    buf[50] = if frame.a { u8::MAX } else { 0 };
165    buf[51] = if frame.x { u8::MAX } else { 0 };
166    buf[52] = if frame.r1 { u8::MAX } else { 0 };
167    buf[53] = if frame.l1 { u8::MAX } else { 0 };
168    buf[54] = frame.analog_r2;
169    buf[55] = frame.analog_l2;
170
171    // Touch data (bytes 56-67) are already zeroed.
172
173    // MotionData timestamp (offset 68)
174    buf[68..76].copy_from_slice(&timestamp_us.to_le_bytes());
175
176    // Accelerometer in g (offset 76)
177    let acc_x = frame.accel_x;
178    let acc_y = if invert_pitch {
179        -frame.accel_y
180    } else {
181        frame.accel_y
182    };
183    let acc_z = frame.accel_z;
184
185    buf[76..80].copy_from_slice(&acc_x.to_le_bytes());
186    buf[80..84].copy_from_slice(&acc_y.to_le_bytes());
187    buf[84..88].copy_from_slice(&acc_z.to_le_bytes());
188
189    // Gyroscope in deg/s (offset 88)
190    let pitch = if invert_pitch {
191        -frame.gyro_x
192    } else {
193        frame.gyro_x
194    };
195
196    // when gravity reference is flipped with invert_pitch, this needs to be flipped too
197    let yaw = if invert_pitch {
198        -frame.gyro_y
199    } else {
200        frame.gyro_y
201    };
202
203    let roll = frame.gyro_z;
204
205    buf[88..92].copy_from_slice(&pitch.to_le_bytes());
206    buf[92..96].copy_from_slice(&yaw.to_le_bytes());
207    buf[96..100].copy_from_slice(&roll.to_le_bytes());
208
209    let c = crc32(&buf[..100]);
210    buf[8..12].copy_from_slice(&c.to_le_bytes());
211}
212
213/// Get a DSU button bitmask from a slice of bool and bit position pairs
214fn get_bitmask(bits: &[(bool, u8)]) -> u8 {
215    let mut mask = 0u8;
216    for &(on, pos) in bits {
217        if on {
218            mask |= 1u8 << pos;
219        }
220    }
221    mask
222}
223
224#[cfg(test)]
225mod tests {
226    use super::*;
227
228    #[test]
229    fn test_get_bitmask_empty() {
230        assert_eq!(get_bitmask(&[]), 0);
231    }
232
233    #[test]
234    fn test_get_bitmask_single_bit() {
235        assert_eq!(get_bitmask(&[(true, 0)]), 0b00000001);
236        assert_eq!(get_bitmask(&[(true, 7)]), 0b10000000);
237    }
238
239    #[test]
240    fn test_get_bitmask_multiple_bits() {
241        assert_eq!(get_bitmask(&[(true, 0), (true, 1), (true, 2)]), 0b00000111);
242        assert_eq!(get_bitmask(&[(true, 0), (true, 7)]), 0b10000001);
243    }
244
245    #[test]
246    fn test_write_version_response() {
247        let mut buf = [0u8; 22];
248        write_version_response(&mut buf, 0x12345678);
249
250        assert_eq!(&buf[0..4], b"DSUS");
251        assert_eq!(u16::from_le_bytes([buf[4], buf[5]]), 1001);
252        assert_eq!(u16::from_le_bytes([buf[6], buf[7]]), 2);
253        assert_eq!(
254            u32::from_le_bytes([buf[12], buf[13], buf[14], buf[15]]),
255            0x12345678
256        );
257        assert_eq!(
258            u32::from_le_bytes([buf[16], buf[17], buf[18], buf[19]]),
259            0x100000
260        );
261        assert_eq!(u16::from_le_bytes([buf[20], buf[21]]), 1001);
262    }
263
264    #[test]
265    fn test_write_info_response_connected() {
266        let mut buf = [0u8; 32];
267        write_info_response(&mut buf, 1, 0xABCD_EF01, true);
268
269        assert_eq!(&buf[0..4], b"DSUS");
270        assert_eq!(u16::from_le_bytes([buf[6], buf[7]]), 16);
271        assert_eq!(
272            u32::from_le_bytes([buf[12], buf[13], buf[14], buf[15]]),
273            0xABCD_EF01
274        );
275        assert_eq!(buf[20], 1);
276        assert_eq!(buf[21], 2);
277        assert_eq!(buf[22], 2);
278        assert_eq!(buf[23], 1);
279    }
280
281    #[test]
282    fn test_write_info_response_disconnected() {
283        let mut buf = [0u8; 32];
284        write_info_response(&mut buf, 0, 0, false);
285
286        assert_eq!(&buf[0..4], b"DSUS");
287        assert_eq!(buf[20], 0);
288        assert_eq!(buf[21], 0);
289        assert_eq!(buf[22], 0);
290        assert_eq!(buf[23], 0);
291    }
292
293    fn create_test_frame() -> DSUFrame {
294        DSUFrame {
295            dpad_left: true,
296            dpad_down: false,
297            dpad_right: true,
298            dpad_up: false,
299            options: true,
300            r3: false,
301            l3: true,
302            share: false,
303            y: true,
304            b: false,
305            a: true,
306            x: false,
307            r1: true,
308            l1: false,
309            r2: true,
310            l2: false,
311            home: true,
312            touch: false,
313            left_stick_x: 128,
314            left_stick_y: 64,
315            right_stick_x: 200,
316            right_stick_y: 50,
317            analog_r2: 200,
318            analog_l2: 100,
319            accel_x: 1.0,
320            accel_y: 0.5,
321            accel_z: -0.5,
322            gyro_x: 10.0,
323            gyro_y: -5.0,
324            gyro_z: 2.5,
325        }
326    }
327
328    #[test]
329    fn test_write_data_event_basic() {
330        let mut buf = [0u8; 100];
331        let frame = create_test_frame();
332        write_data_event(&mut buf, &frame, 12345, 0xDEAD_BEEF, 0, 1_000_000, false);
333
334        assert_eq!(&buf[0..4], b"DSUS");
335        assert_eq!(u16::from_le_bytes([buf[6], buf[7]]), 84);
336        assert_eq!(
337            u32::from_le_bytes([buf[12], buf[13], buf[14], buf[15]]),
338            0xDEAD_BEEF
339        );
340        assert_eq!(buf[20], 0);
341        assert_eq!(
342            u32::from_le_bytes([buf[32], buf[33], buf[34], buf[35]]),
343            12345
344        );
345    }
346
347    #[test]
348    fn test_write_data_event_buttons() {
349        let mut buf = [0u8; 100];
350        let frame = create_test_frame();
351        write_data_event(&mut buf, &frame, 0, 0, 0, 0, false);
352
353        assert_eq!(buf[36], 0b1010_1010);
354        assert_eq!(buf[37], 0b1010_1010);
355        assert_eq!(buf[38], 1);
356        assert_eq!(buf[39], 0);
357    }
358
359    #[test]
360    fn test_write_data_event_sticks() {
361        let mut buf = [0u8; 100];
362        let frame = create_test_frame();
363        write_data_event(&mut buf, &frame, 0, 0, 0, 0, false);
364
365        assert_eq!(buf[40], 128);
366        assert_eq!(buf[41], 64);
367        assert_eq!(buf[42], 200);
368        assert_eq!(buf[43], 50);
369    }
370
371    #[test]
372    fn test_write_data_event_analog_buttons() {
373        let mut buf = [0u8; 100];
374        let frame = create_test_frame();
375        write_data_event(&mut buf, &frame, 0, 0, 0, 0, false);
376
377        assert_eq!(buf[44], 255);
378        assert_eq!(buf[45], 0);
379        assert_eq!(buf[46], 255);
380        assert_eq!(buf[47], 0);
381        assert_eq!(buf[48], 255);
382        assert_eq!(buf[49], 0);
383        assert_eq!(buf[50], 255);
384        assert_eq!(buf[51], 0);
385        assert_eq!(buf[52], 255);
386        assert_eq!(buf[53], 0);
387        assert_eq!(buf[54], 200);
388        assert_eq!(buf[55], 100);
389    }
390
391    #[test]
392    fn test_write_data_event_timestamp() {
393        let mut buf = [0u8; 100];
394        let frame = create_test_frame();
395        write_data_event(&mut buf, &frame, 0, 0, 0, 9_876_543_210, false);
396
397        assert_eq!(
398            u64::from_le_bytes(buf[68..76].try_into().unwrap()),
399            9_876_543_210
400        );
401    }
402
403    #[test]
404    fn test_write_data_event_accel_normal() {
405        let mut buf = [0u8; 100];
406        let frame = create_test_frame();
407        write_data_event(&mut buf, &frame, 0, 0, 0, 0, false);
408
409        assert_eq!(f32::from_le_bytes(buf[76..80].try_into().unwrap()), 1.0);
410        assert_eq!(f32::from_le_bytes(buf[80..84].try_into().unwrap()), 0.5);
411        assert_eq!(f32::from_le_bytes(buf[84..88].try_into().unwrap()), -0.5);
412    }
413
414    #[test]
415    fn test_write_data_event_accel_inverted() {
416        let mut buf = [0u8; 100];
417        let frame = create_test_frame();
418        write_data_event(&mut buf, &frame, 0, 0, 0, 0, true);
419
420        assert_eq!(f32::from_le_bytes(buf[76..80].try_into().unwrap()), 1.0);
421        assert_eq!(f32::from_le_bytes(buf[80..84].try_into().unwrap()), -0.5);
422        assert_eq!(f32::from_le_bytes(buf[84..88].try_into().unwrap()), -0.5);
423    }
424
425    #[test]
426    fn test_write_data_event_gyro_normal() {
427        let mut buf = [0u8; 100];
428        let frame = create_test_frame();
429        write_data_event(&mut buf, &frame, 0, 0, 0, 0, false);
430
431        assert_eq!(f32::from_le_bytes(buf[88..92].try_into().unwrap()), 10.0);
432        assert_eq!(f32::from_le_bytes(buf[92..96].try_into().unwrap()), -5.0);
433        assert_eq!(f32::from_le_bytes(buf[96..100].try_into().unwrap()), 2.5);
434    }
435
436    #[test]
437    fn test_write_data_event_gyro_inverted() {
438        let mut buf = [0u8; 100];
439        let frame = create_test_frame();
440        write_data_event(&mut buf, &frame, 0, 0, 0, 0, true);
441
442        assert_eq!(f32::from_le_bytes(buf[88..92].try_into().unwrap()), -10.0);
443        assert_eq!(f32::from_le_bytes(buf[92..96].try_into().unwrap()), 5.0);
444        assert_eq!(f32::from_le_bytes(buf[96..100].try_into().unwrap()), 2.5);
445    }
446}
scdsu_core/dsu.rs

scdsu_core/
dsu.rs
