1use super::*;
2use std::io::{Cursor, Result, Error, ErrorKind};
3use std::io::prelude::*;
4use byteorder::{ReadBytesExt, WriteBytesExt, LittleEndian};
5
6fn invalid_data_error(message: &str) -> Error {
7 Error::new(ErrorKind::InvalidData, message)
8}
9
10pub fn encode_message_header(writer: &mut Vec<u8>, message_header: MessageHeader) -> Result<()> {
11 match message_header.source {
12 MessageSource::Server => writer.write(b"DSUS")?,
13 MessageSource::Client => writer.write(b"DSUC")?
14 };
15
16 writer.write_u16::<LittleEndian>(message_header.protocol_version)?;
17 writer.write_u16::<LittleEndian>(message_header.message_length)?;
18 writer.write_u32::<LittleEndian>(message_header.checksum)?;
19 writer.write_u32::<LittleEndian>(message_header.source_id)
20}
21
22pub fn parse_message_header(reader: &mut Cursor<&[u8]>) -> Result<MessageHeader> {
23 let source = {
24 let mut buffer = [0 as u8; 4];
25 reader.read(&mut buffer)?;
26 let magic_string = {
27 let string = std::str::from_utf8(&buffer);
28
29 match string {
30 Ok(str) => Ok(str),
31 _ => Err(invalid_data_error("Magic string is not a valid UTF-8 string"))
32 }
33 }?;
34
35 match magic_string {
36 "DSUS" => Ok(MessageSource::Server),
37 "DSUC" => Ok(MessageSource::Client),
38 _ => Err(invalid_data_error("Unrecognized magic string"))
39 }?
40 };
41
42 let protocol_version = reader.read_u16::<LittleEndian>()?;
43 let message_length = reader.read_u16::<LittleEndian>()?;
44 let checksum = reader.read_u32::<LittleEndian>()?;
45 let source_id = reader.read_u32::<LittleEndian>()?;
46
47 Ok(MessageHeader {
48 source,
49 protocol_version,
50 message_length,
51 checksum,
52 source_id
53 })
54}
55
56pub fn encode_message_type(writer: &mut Vec<u8>, message_type: MessageType) -> Result<()> {
57 let encoded = match message_type {
58 MessageType::ProtocolVersion => 0x100000,
59 MessageType::ConnectedControllers => 0x100001,
60 MessageType::ControllerData => 0x100002,
61 };
62
63 writer.write_u32::<LittleEndian>(encoded)
64}
65
66pub fn parse_message_type(reader: &mut Cursor<&[u8]>) -> Result<MessageType> {
67 let message_type = {
68 let message_type = reader.read_u32::<LittleEndian>()?;
69
70 match message_type {
71 0x100000 => Ok(MessageType::ProtocolVersion),
72 0x100001 => Ok(MessageType::ConnectedControllers),
73 0x100002 => Ok(MessageType::ControllerData),
74 _ => Err(invalid_data_error("Invalid message type"))
75 }
76 }?;
77
78 Ok(message_type)
79}
80
81pub fn encode_controller_info(writer: &mut Vec<u8>, controller_info: ControllerInfo) -> Result<()> {
82 writer.write_u8(controller_info.slot)?;
83
84 let slot_state = match controller_info.slot_state {
85 SlotState::NotConnected => 0,
86 SlotState::Reserved => 1,
87 SlotState::Connected => 2
88 };
89 writer.write_u8(slot_state)?;
90
91 let device_type = match controller_info.device_type {
92 DeviceType::NotApplicable => 0,
93 DeviceType::PartialGyro => 1,
94 DeviceType::FullGyro => 2
95 };
96 writer.write_u8(device_type)?;
97
98 let connection_type = match controller_info.connection_type {
99 ConnectionType::NotApplicable => 0,
100 ConnectionType::USB => 1,
101 ConnectionType::Bluetooth => 2
102 };
103 writer.write_u8(connection_type)?;
104
105 writer.write_u48::<LittleEndian>(controller_info.mac_address)?;
106
107 let battery_status = match controller_info.battery_status {
108 BatteryStatus::NotApplicable => 0x00,
109 BatteryStatus::Dying => 0x01,
110 BatteryStatus::Low => 0x02,
111 BatteryStatus::Medium => 0x03,
112 BatteryStatus::High => 0x04,
113 BatteryStatus::Full => 0x05,
114 BatteryStatus::Charging => 0xEE,
115 BatteryStatus::Charged => 0xEF
116 };
117 writer.write_u8(battery_status)
118}
119
120pub fn parse_controller_info(reader: &mut Cursor<&[u8]>) -> Result<ControllerInfo> {
121 let slot = reader.read_u8()?;
122 if slot > 4 {
123 return Err(invalid_data_error("Invalid slot number"));
124 }
125
126 let slot_state = {
127 let slot_state = reader.read_u8()?;
128
129 match slot_state {
130 0 => Ok(SlotState::NotConnected),
131 1 => Ok(SlotState::Reserved),
132 2 => Ok(SlotState::Connected),
133 _ => Err(invalid_data_error("Invalid slot number"))
134 }
135 }?;
136
137 let device_type = {
138 let device_type = reader.read_u8()?;
139
140 match device_type {
141 0 => Ok(DeviceType::NotApplicable),
142 1 => Ok(DeviceType::PartialGyro),
143 2 => Ok(DeviceType::FullGyro),
144 _ => Err(invalid_data_error("Invalid device type"))
145 }
146 }?;
147
148 let connection_type = {
149 let connection_type = reader.read_u8()?;
150
151 match connection_type {
152 0 => Ok(ConnectionType::NotApplicable),
153 1 => Ok(ConnectionType::USB),
154 2 => Ok(ConnectionType::Bluetooth),
155 _ => Err(invalid_data_error("Invalid connection type"))
156 }
157 }?;
158
159 let mac_address = reader.read_u48::<LittleEndian>()?;
160
161 let battery_status = {
162 let battery_status = reader.read_u8()?;
163
164 match battery_status {
165 0x00 => Ok(BatteryStatus::NotApplicable),
166 0x01 => Ok(BatteryStatus::Dying),
167 0x02 => Ok(BatteryStatus::Low),
168 0x03 => Ok(BatteryStatus::Medium),
169 0x04 => Ok(BatteryStatus::High),
170 0x05 => Ok(BatteryStatus::Full),
171 0xEE => Ok(BatteryStatus::Charging),
172 0xEF => Ok(BatteryStatus::Charged),
173 _ => Err(invalid_data_error("Invalid battery status"))
174 }
175 }?;
176
177 Ok(ControllerInfo {
178 slot,
179 slot_state,
180 device_type,
181 connection_type,
182 mac_address,
183 battery_status
184 })
185}
186
187pub fn encode_touch_data(writer: &mut Vec<u8>, touch_data: TouchData) -> Result<()> {
188 match touch_data.active {
189 false => writer.write_u8(0),
190 true => writer.write_u8(1),
191 }?;
192
193 writer.write_u8(touch_data.id)?;
194
195 writer.write_u16::<LittleEndian>(touch_data.position_x)?;
196 writer.write_u16::<LittleEndian>(touch_data.position_y)
197}
198
199pub fn parse_touch_data(reader: &mut Cursor<&[u8]>) -> Result<TouchData> {
200 let active = {
201 let active = reader.read_u8()?;
202
203 match active {
204 0 => Ok(false),
205 1 | 255 => Ok(true),
206 _ => Err(invalid_data_error("Invalid touch active value"))
207 }
208 }?;
209
210 let id = reader.read_u8()?;
211
212 let position_x = reader.read_u16::<LittleEndian>()?;
213 let position_y = reader.read_u16::<LittleEndian>()?;
214
215 Ok(TouchData {
216 active,
217 id,
218 position_x,
219 position_y
220 })
221}
222
223pub fn encode_controller_data_request(writer: &mut Vec<u8>,
224 request: ControllerDataRequest) -> Result<()> {
225 match request {
226 ControllerDataRequest::ReportAll => {
227 writer.write_u8(0)?;
228 writer.write_u8(0)?;
229 writer.write_u48::<LittleEndian>(0)
230 },
231 ControllerDataRequest::SlotNumber(slot) => {
232 writer.write_u8(1)?;
233 writer.write_u8(slot)?;
234 writer.write_u48::<LittleEndian>(0)
235 },
236 ControllerDataRequest::MAC(mac) => {
237 writer.write_u8(2)?;
238 writer.write_u8(0)?;
239 writer.write_u48::<LittleEndian>(mac)
240 }
241 }
242}
243
244pub fn parse_controller_data_request(reader: &mut Cursor<&[u8]>) -> Result<ControllerDataRequest> {
245 let request_type = reader.read_u8()?;
246
247 match request_type {
248 0 => Ok(ControllerDataRequest::ReportAll),
249 1 => {
250 let slot_number = reader.read_u8()?;
251 if slot_number >= 4 {
252 return Err(invalid_data_error("Invalid slot number requested"))
253 }
254
255 Ok(ControllerDataRequest::SlotNumber(slot_number))
256 }
257 2 => {
258 let mac_address = reader.read_u48::<LittleEndian>()?;
259
260 Ok(ControllerDataRequest::MAC(mac_address))
261 },
262 _ => Err(invalid_data_error("Invalid controller data request type"))
263 }
264}
265
266fn bit_array_to_u8(input: [bool; 8]) -> u8 {
267 let mut result = 0;
268
269 result |= (input[0] as u8) * 0b10000000;
270 result |= (input[1] as u8) * 0b01000000;
271 result |= (input[2] as u8) * 0b00100000;
272 result |= (input[3] as u8) * 0b00010000;
273 result |= (input[4] as u8) * 0b00001000;
274 result |= (input[5] as u8) * 0b00000100;
275 result |= (input[6] as u8) * 0b00000010;
276 result |= (input[7] as u8) * 0b00000001;
277
278 result
279}
280
281pub fn encode_controller_data(writer: &mut Vec<u8>,
282 packet_number: u32,
283 controller_data: ControllerData) -> Result<()> {
284 let connected = match controller_data.connected {
285 false => 0,
286 true => 1
287 };
288 writer.write_u8(connected)?;
289
290 writer.write_u32::<LittleEndian>(packet_number)?;
291
292 let button_data = [controller_data.d_pad_left,
293 controller_data.d_pad_down,
294 controller_data.d_pad_right,
295 controller_data.d_pad_up,
296 controller_data.start,
297 controller_data.right_stick_button,
298 controller_data.left_stick_button,
299 controller_data.select];
300 writer.write_u8(bit_array_to_u8(button_data))?;
301
302 let button_data = [controller_data.square,
303 controller_data.cross,
304 controller_data.circle,
305 controller_data.triangle,
306 controller_data.r1,
307 controller_data.l1,
308 controller_data.r2,
309 controller_data.l2];
310 writer.write_u8(bit_array_to_u8(button_data))?;
311
312 writer.write_u8(controller_data.ps)?;
313
314 writer.write_u8(controller_data.touch)?;
315
316 writer.write_u8(controller_data.left_stick_x)?;
317 writer.write_u8(controller_data.left_stick_y)?;
318
319 writer.write_u8(controller_data.right_stick_x)?;
320 writer.write_u8(controller_data.right_stick_y)?;
321
322 writer.write_u8(controller_data.analog_d_pad_left)?;
323 writer.write_u8(controller_data.analog_d_pad_down)?;
324 writer.write_u8(controller_data.analog_d_pad_right)?;
325 writer.write_u8(controller_data.analog_d_pad_up)?;
326
327 writer.write_u8(controller_data.analog_square)?;
328 writer.write_u8(controller_data.analog_cross)?;
329 writer.write_u8(controller_data.analog_circle)?;
330 writer.write_u8(controller_data.analog_triangle)?;
331
332 writer.write_u8(controller_data.analog_r1)?;
333 writer.write_u8(controller_data.analog_l1)?;
334 writer.write_u8(controller_data.analog_r2)?;
335 writer.write_u8(controller_data.analog_l2)?;
336
337 encode_touch_data(writer, controller_data.first_touch)?;
338 encode_touch_data(writer, controller_data.second_touch)?;
339
340 writer.write_u64::<LittleEndian>(controller_data.motion_data_timestamp)?;
341
342 writer.write_f32::<LittleEndian>(controller_data.accelerometer_x)?;
343 writer.write_f32::<LittleEndian>(controller_data.accelerometer_y)?;
344 writer.write_f32::<LittleEndian>(controller_data.accelerometer_z)?;
345
346 writer.write_f32::<LittleEndian>(controller_data.gyroscope_pitch)?;
347 writer.write_f32::<LittleEndian>(controller_data.gyroscope_yaw)?;
348 writer.write_f32::<LittleEndian>(controller_data.gyroscope_roll)
349}
350
351pub fn parse_controller_data(reader: &mut Cursor<&[u8]>) -> Result<(u32, ControllerData)> {
352 let connected = {
353 let connected = reader.read_u8()?;
354
355 match connected {
356 0 => Ok(false),
357 1 => Ok(true),
358 _ => Err(invalid_data_error("Invalid connected value"))
359 }
360 }?;
361
362 let packet_number = reader.read_u32::<LittleEndian>()?;
363
364 let button_data = reader.read_u8()?;
365 let d_pad_left = (button_data & 0b10000000) != 0;
366 let d_pad_down = (button_data & 0b01000000) != 0;
367 let d_pad_right = (button_data & 0b00100000) != 0;
368 let d_pad_up = (button_data & 0b00010000) != 0;
369 let start = (button_data & 0b00001000) != 0;
370 let right_stick_button = (button_data & 0b00000100) != 0;
371 let left_stick_button = (button_data & 0b00000010) != 0;
372 let select = (button_data & 0b00000001) != 0;
373
374 let button_data = reader.read_u8()?;
375 let square = (button_data & 0b10000000) != 0;
376 let cross = (button_data & 0b01000000) != 0;
377 let circle = (button_data & 0b00100000) != 0;
378 let triangle = (button_data & 0b00010000) != 0;
379 let r1 = (button_data & 0b00001000) != 0;
380 let l1 = (button_data & 0b00000100) != 0;
381 let r2 = (button_data & 0b00000010) != 0;
382 let l2 = (button_data & 0b00000001) != 0;
383
384 let ps = reader.read_u8()?;
385
386 let touch = reader.read_u8()?;
387
388 let left_stick_x = reader.read_u8()?;
389 let left_stick_y = reader.read_u8()?;
390
391 let right_stick_x = reader.read_u8()?;
392 let right_stick_y = reader.read_u8()?;
393
394 let analog_d_pad_left = reader.read_u8()?;
395 let analog_d_pad_down = reader.read_u8()?;
396 let analog_d_pad_right = reader.read_u8()?;
397 let analog_d_pad_up = reader.read_u8()?;
398
399 let analog_square = reader.read_u8()?;
400 let analog_cross = reader.read_u8()?;
401 let analog_circle = reader.read_u8()?;
402 let analog_triangle = reader.read_u8()?;
403
404 let analog_r1 = reader.read_u8()?;
405 let analog_l1 = reader.read_u8()?;
406 let analog_r2 = reader.read_u8()?;
407 let analog_l2 = reader.read_u8()?;
408
409 let first_touch = parse_touch_data(reader)?;
410 let second_touch = parse_touch_data(reader)?;
411
412 let motion_data_timestamp = reader.read_u64::<LittleEndian>()?;
413
414 let accelerometer_x = reader.read_f32::<LittleEndian>()?;
415 let accelerometer_y = reader.read_f32::<LittleEndian>()?;
416 let accelerometer_z = reader.read_f32::<LittleEndian>()?;
417
418 let gyroscope_pitch = reader.read_f32::<LittleEndian>()?;
419 let gyroscope_yaw = reader.read_f32::<LittleEndian>()?;
420 let gyroscope_roll = reader.read_f32::<LittleEndian>()?;
421
422 Ok((packet_number, ControllerData {
423 connected,
424 d_pad_left,
425 d_pad_down,
426 d_pad_right,
427 d_pad_up,
428 start,
429 right_stick_button,
430 left_stick_button,
431 select,
432 square,
433 cross,
434 circle,
435 triangle,
436 r1,
437 l1,
438 r2,
439 l2,
440 ps,
441 touch,
442 left_stick_x,
443 left_stick_y,
444 right_stick_x,
445 right_stick_y,
446 analog_d_pad_left,
447 analog_d_pad_down,
448 analog_d_pad_right,
449 analog_d_pad_up,
450 analog_square,
451 analog_triangle,
452 analog_circle,
453 analog_cross,
454 analog_r1,
455 analog_l1,
456 analog_r2,
457 analog_l2,
458 first_touch,
459 second_touch,
460 motion_data_timestamp,
461 accelerometer_x,
462 accelerometer_y,
463 accelerometer_z,
464 gyroscope_pitch,
465 gyroscope_yaw,
466 gyroscope_roll
467 }))
468}
469
470pub fn encode_message_payload(writer: &mut Vec<u8>,
471 message_payload: MessagePayload) -> Result<()> {
472 match message_payload {
473 MessagePayload::None => Ok(()),
474 MessagePayload::ProtocolVersion(version) => {
475 writer.write_u16::<LittleEndian>(version)
476 },
477 MessagePayload::ConnectedControllersRequest { amount, slot_numbers } => {
478 if amount < 0 || amount > 4 {
479 return Err(invalid_data_error("Invalid amount of ports to report"))
480 };
481
482 writer.write_i32::<LittleEndian>(amount)?;
483
484 for i in 0..amount {
485 writer.write_u8(slot_numbers[i as usize])?;
486 }
487
488 Ok(())
489 },
490 MessagePayload::ConnectedControllerResponse { controller_info } => {
491 encode_controller_info(writer, controller_info)?;
492 writer.write_u8(0)
493 },
494 MessagePayload::ControllerDataRequest(request) => {
495 encode_controller_data_request(writer, request)
496 },
497 MessagePayload::ControllerData { packet_number,
498 controller_info,
499 controller_data } => {
500 encode_controller_info(writer, controller_info)?;
501 encode_controller_data(writer, packet_number, controller_data)
502 }
503 }
504}
505
506pub fn parse_message_payload(reader: &mut Cursor<&[u8]>,
507 message_source: MessageSource,
508 message_type: MessageType) -> Result<MessagePayload> {
509 match message_source {
510 MessageSource::Server => {
511 match message_type {
512 MessageType::ProtocolVersion => {
513 let protocol_version = reader.read_u16::<LittleEndian>()?;
514 Ok(MessagePayload::ProtocolVersion(protocol_version))
515 },
516 MessageType::ConnectedControllers => {
517 let controller_info = parse_controller_info(reader)?;
518 let terminating_byte = reader.read_u8()?;
519 if terminating_byte != 0 {
520 Err(invalid_data_error("Message was not properly terminated"))
521 } else {
522 Ok(MessagePayload::ConnectedControllerResponse {
523 controller_info
524 })
525 }
526 },
527 MessageType::ControllerData => {
528 let controller_info = parse_controller_info(reader)?;
529 let (packet_number, controller_data) = parse_controller_data(reader)?;
530
531 Ok(MessagePayload::ControllerData {
532 packet_number,
533 controller_info,
534 controller_data
535 })
536 }
537 }
538 },
539 MessageSource::Client => {
540 match message_type {
541 MessageType::ProtocolVersion => {
542 Ok(MessagePayload::None)
543 }
544 MessageType::ConnectedControllers => {
545 let amount = reader.read_i32::<LittleEndian>()?;
546 if amount < 0 || amount > 4 {
547 return Err(invalid_data_error("Invalid amount of ports to report"))
548 };
549
550 let mut slot_numbers = [0; 4];
551 for i in 0..amount {
552 let slot_number = reader.read_u8()?;
553 if slot_number >= 4 {
554 return Err(invalid_data_error("Invalid slot number"))
555 }
556
557 slot_numbers[i as usize] = slot_number;
558 }
559
560 Ok(MessagePayload::ConnectedControllersRequest {
561 amount,
562 slot_numbers
563 })
564 }
565 MessageType::ControllerData => {
566 let controller_data_request = parse_controller_data_request(reader)?;
567
568 Ok(MessagePayload::ControllerDataRequest(controller_data_request))
569 }
570 }
571 }
572 }
573}