hid_io_protocol/lib.rs
1/* Copyright (C) 2017-2023 by Jacob Alexander
2 *
3 * Permission is hereby granted, free of charge, to any person obtaining a copy
4 * of this software and associated documentation files (the "Software"), to deal
5 * in the Software without restriction, including without limitation the rights
6 * to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
7 * copies of the Software, and to permit persons to whom the Software is
8 * furnished to do so, subject to the following conditions:
9 *
10 * The above copyright notice and this permission notice shall be included in
11 * all copies or substantial portions of the Software.
12 *
13 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
14 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
15 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
16 * AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
17 * LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
18 * OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
19 * THE SOFTWARE.
20 */
21
22// ----- Modules -----
23
24#![no_std]
25#![feature(associated_type_defaults)]
26
27pub mod buffer;
28pub mod commands;
29pub mod test;
30
31// ----- Crates -----
32
33use core::convert::TryFrom;
34use core::fmt;
35use heapless::Vec;
36use num_enum::{IntoPrimitive, TryFromPrimitive};
37
38#[cfg(feature = "defmt")]
39use defmt::{error, trace, warn};
40#[cfg(not(feature = "defmt"))]
41use log::{error, trace, warn};
42
43// ----- Macros -----
44
45#[cfg(not(feature = "server"))]
46macro_rules! warn {
47 (target: $target:expr, $($arg:tt)+) => {};
48 ($($arg:tt)+) => {};
49}
50
51#[cfg(not(feature = "server"))]
52macro_rules! error {
53 (target: $target:expr, $($arg:tt)+) => {};
54 ($($arg:tt)+) => {};
55}
56
57// ----- Enumerations -----
58
59/// HID-IO Packet Types
60///
61/// # Remarks
62/// Must not be larger than 0x7, 7 is reserved.
63#[derive(PartialEq, Eq, Clone, Copy, Debug)]
64#[cfg_attr(feature = "defmt", derive(defmt::Format))]
65pub enum HidIoPacketType {
66 /// Data packet
67 Data = 0,
68 /// Ack packet
69 Ack = 1,
70 /// Nak packet
71 Nak = 2,
72 /// Sync packet
73 Sync = 3,
74 /// Continued packet
75 Continued = 4,
76 /// No acknowledgement data packet
77 NaData = 5,
78 /// No acknowledgement continued packet
79 NaContinued = 6,
80}
81
82#[repr(u32)]
83#[derive(PartialEq, Eq, PartialOrd, Ord, Clone, Copy, Debug, IntoPrimitive, TryFromPrimitive)]
84#[cfg_attr(feature = "defmt", derive(defmt::Format))]
85/// Requests for to perform a specific action
86pub enum HidIoCommandId {
87 SupportedIds = 0x00,
88 GetInfo = 0x01,
89 TestPacket = 0x02,
90 ResetHidIo = 0x03,
91 Reserved = 0x04, // ... 0x0F
92
93 GetProperties = 0x10,
94 KeyState = 0x11,
95 KeyboardLayout = 0x12,
96 KeyLayout = 0x13,
97 KeyShapes = 0x14,
98 LedLayout = 0x15,
99 FlashMode = 0x16,
100 UnicodeText = 0x17,
101 UnicodeState = 0x18,
102 HostMacro = 0x19,
103 SleepMode = 0x1A,
104
105 KllState = 0x20,
106 PixelSetting = 0x21,
107 PixelSet1c8b = 0x22,
108 PixelSet3c8b = 0x23,
109 PixelSet1c16b = 0x24,
110 PixelSet3c16b = 0x25,
111 DirectSet = 0x26,
112
113 OpenUrl = 0x30,
114 TerminalCmd = 0x31,
115 GetInputLayout = 0x32,
116 SetInputLayout = 0x33,
117 TerminalOut = 0x34,
118
119 HidKeyboard = 0x40,
120 HidKeyboardLed = 0x41,
121 HidMouse = 0x42,
122 HidJoystick = 0x43,
123 HidSystemCtrl = 0x44,
124 HidConsumerCtrl = 0x45,
125
126 ManufacturingTest = 0x50,
127 ManufacturingResult = 0x51,
128
129 Unused = 0xFFFF,
130}
131
132/// HID-IO Parse Error
133///
134/// # Remarks
135/// thrown when there's an issue processing byte stream.
136#[derive(Debug)]
137#[cfg_attr(feature = "defmt", derive(defmt::Format))]
138pub enum HidIoParseError {
139 BufferNotReady,
140 BufferDataTooSmall(usize),
141 InvalidContinuedIdByte(u8),
142 InvalidHidIoCommandId(u32),
143 InvalidPacketIdWidth(u8),
144 InvalidPacketType(u8),
145 MissingContinuedIdByte,
146 MissingPacketIdWidthByte,
147 MissingPacketTypeByte,
148 MissingPayloadLengthByte,
149 NotEnoughActualBytesPacketId { len: usize, id_width: usize },
150 NotEnoughPossibleBytesPacketId { len: u32, id_width: usize },
151 PayloadAddFailed(usize),
152 SerializationError,
153 SerializationFailedResultTooSmall(usize),
154 VecAddFailed,
155 VecResizeFailed,
156}
157
158// ----- Structs -----
159
160/// HID-IO Packet Buffer Struct
161///
162/// # Remarks
163/// Used to store HID-IO data chunks. Will be chunked into individual packets on transmission.
164#[repr(C)]
165#[derive(PartialEq, Eq, Clone, Debug)]
166#[cfg_attr(feature = "defmt", derive(defmt::Format))]
167pub struct HidIoPacketBuffer<const H: usize> {
168 /// Type of packet (Continued is automatically set if needed)
169 pub ptype: HidIoPacketType,
170 /// Packet Id
171 pub id: HidIoCommandId,
172 /// Packet length for serialization (in bytes)
173 pub max_len: u32,
174 /// Payload data, chunking is done automatically by serializer
175 pub data: Vec<u8, H>,
176 /// Set False if buffer is not complete, True if it is
177 pub done: bool,
178}
179
180// ----- Utility Functions -----
181
182/// Determines the packet type from a byte stream
183///
184/// # Arguments
185/// * `packet_data` - Vector of bytes
186///
187/// # Remarks
188/// Uses a packet byte stream to determine the packet type.
189/// First three bits of data stream are used (from C-Struct):
190///
191/// ```c
192/// struct HidIo_Packet {
193/// HidIo_Packet_Type type:3;
194/// ...
195/// };
196/// ```
197pub fn packet_type(packet_data: &[u8]) -> Result<HidIoPacketType, HidIoParseError> {
198 let packet_data_len = packet_data.len();
199
200 // Check if the byte stream is large enough
201 if packet_data_len < 1 {
202 return Err(HidIoParseError::MissingPacketTypeByte);
203 }
204
205 // Extract first 3 bits from first byte
206 let ptype: u8 = (packet_data[0] & 0xE0) >> 5;
207
208 // Convert to HidIoPacketType enum
209 match ptype {
210 0 => Ok(HidIoPacketType::Data),
211 1 => Ok(HidIoPacketType::Ack),
212 2 => Ok(HidIoPacketType::Nak),
213 3 => Ok(HidIoPacketType::Sync),
214 4 => Ok(HidIoPacketType::Continued),
215 5 => Ok(HidIoPacketType::NaData),
216 6 => Ok(HidIoPacketType::NaContinued),
217 _ => Err(HidIoParseError::InvalidPacketType(ptype)),
218 }
219}
220
221/// Determines payload of packet from a byte stream
222///
223/// # Arguments
224/// * `packet_data` - Vector of bytes
225///
226/// # Remarks
227/// Uses a packet byte stream to determine payload length.
228/// This length does not include the first 2 packet bytes in the overall packet length.
229/// The length does include the bytes used for the packet Id.
230///
231/// ```c
232/// struct HidIo_Packet {
233/// ... (6 bits)
234/// uint8_t upper_len:2; // Upper 2 bits of length field (generally unused)
235/// uint8_t len; // Lower 8 bits of length field
236/// ...
237/// };
238/// ```
239pub fn payload_len(packet_data: &[u8]) -> Result<u32, HidIoParseError> {
240 let packet_data_len = packet_data.len();
241
242 // Check if the byte stream is large enough
243 if packet_data_len < 2 {
244 return Err(HidIoParseError::MissingPayloadLengthByte);
245 }
246
247 // Extract upper_len and len
248 let upper_len = u32::from(packet_data[0] & 0x3);
249 let len = u32::from(packet_data[1]);
250
251 // Merge
252 let payload_len: u32 = (upper_len << 8) | len;
253
254 Ok(payload_len)
255}
256
257/// Determines id_width from a byte stream
258///
259/// # Arguments
260/// * `packet_data` - Vector of bytes
261///
262/// # Remarks
263/// Uses a packet byte stream to determine packet id_width.
264///
265/// ```c
266/// struct HidIo_Packet {
267/// ... (4 bits)
268/// uint8_t id_width:1; // 0 - 16bits, 1 - 32bits
269/// ...
270/// };
271/// ```
272pub fn packet_id_width(packet_data: &[u8]) -> Result<usize, HidIoParseError> {
273 let packet_data_len = packet_data.len();
274
275 // Check if the byte stream is large enough
276 if packet_data_len < 2 {
277 return Err(HidIoParseError::MissingPacketIdWidthByte);
278 }
279
280 // Extract id_width
281 match packet_data[0] & 0x08 {
282 0x00 => Ok(2), // 16 bit
283 0x08 => Ok(4), // 32 bit
284 _ => Err(HidIoParseError::InvalidPacketIdWidth(packet_data[0])),
285 }
286}
287
288/// Determines packet id from a byte stream
289///
290/// # Arguments
291/// * `packet_data` - Vector of bytes
292///
293/// # Remarks
294/// Uses a packet byte stream to determine packet Id.
295///
296/// ```c
297/// struct HidIo_Packet {
298/// ... (4 bits)
299/// uint8_t id_width:1; // 0 - 16bits, 1 - 32bits
300/// ... (11 bits)
301/// uint16_t/uint32_t id; // Id field (check id_width to see which struct to use)
302/// ...
303/// };
304/// ```
305pub fn packet_id(packet_data: &[u8]) -> Result<u32, HidIoParseError> {
306 let packet_data_len = packet_data.len();
307
308 // Extract id_width
309 let id_width = packet_id_width(packet_data)?;
310
311 // Make sure there are enough possible bytes
312 if payload_len(packet_data)? < id_width as u32 {
313 return Err(HidIoParseError::NotEnoughPossibleBytesPacketId {
314 len: payload_len(packet_data)?,
315 id_width,
316 });
317 }
318
319 // Make sure there enough actual bytes
320 if packet_data_len < id_width + 2 {
321 return Err(HidIoParseError::NotEnoughActualBytesPacketId {
322 len: packet_data_len,
323 id_width,
324 });
325 }
326
327 // Iterate over bytes, constructing Id of either 16 or 32 bit width
328 let mut id: u32 = 0;
329 let offset = 2;
330 for idx in 0..id_width {
331 id |= u32::from(packet_data[offset + idx]) << (idx * 8);
332 }
333
334 Ok(id)
335}
336
337/// Determines whether there are following continued packets
338///
339/// # Arguments
340/// * `packet_data` - Vector of bytes
341///
342/// # Remarks
343/// Uses a packet byte stream to determine cont field.
344///
345/// ```c
346/// struct HidIo_Packet {
347/// ... (3 bits)
348/// uint8_t cont:1; // 0 - Only packet, 1 continued packet following
349/// ...
350/// };
351/// ```
352pub fn continued_packet(packet_data: &[u8]) -> Result<bool, HidIoParseError> {
353 let packet_data_len = packet_data.len() as u32;
354
355 // Check if the byte stream is large enough
356 if packet_data_len < 1 {
357 return Err(HidIoParseError::MissingContinuedIdByte);
358 }
359
360 // Extract cont field
361 // Determine value
362 match packet_data[0] & 0x10 {
363 0x10 => Ok(true),
364 0x00 => Ok(false),
365 _ => Err(HidIoParseError::InvalidContinuedIdByte(packet_data[0])),
366 }
367}
368
369/// Determines the starting position of the payload data
370///
371/// # Arguments
372/// * `packet_data` - Vector of bytes
373///
374/// # Remarks
375/// Uses a packet byte stream to find payload start.
376/// Please note that there may be no payload, or Id.
377/// In this case the starting position will be index 2.
378pub fn payload_start(packet_data: &[u8]) -> Result<usize, HidIoParseError> {
379 // Retrieve id_width
380 let id_width = packet_id_width(packet_data)?;
381
382 // Retrieve payload_len, if 0, then return 2 (minimum packet size)
383 if payload_len(packet_data)? == 0 {
384 return Ok(2);
385 }
386
387 // Determine starting position
388 Ok(2 + id_width)
389}
390
391// ----- Command Utility Functions -----
392
393/// Converts a HID bitmask into an array of byte codes
394///
395/// # Arguments
396/// * `bitmask` - Vector of bytes (each byte is an 8 bit bitmask)
397///
398/// # Remarks
399/// The very first byte in the bitmask represents 0->7 and the final byte ends at 255
400/// Opposite of keyboard_vec2bitmask.
401/// NOTE: The vector is currently restricted to 32 byte codes
402/// technically this could be a maximum of 256, but that
403/// is both impractical and unlikely. i.e. we only have 10
404/// fingers.
405pub fn hid_bitmask2vec(bitmask: &[u8]) -> Result<Vec<u8, 32>, HidIoParseError> {
406 let mut data: Vec<u8, 32> = Vec::new();
407
408 // Iterate over each byte of the bitmask adding a code for each found bit
409 for (byte_pos, byte) in bitmask.iter().enumerate() {
410 // Iterate over each of the bits
411 for b in 0..=7 {
412 // Check if bit is active, if so use the b position, then add byte_pos
413 let active = ((byte >> b) & 0x01) == 0x01;
414 if active {
415 let code = b + byte_pos * 8;
416 if data.push(code as u8).is_err() {
417 return Err(HidIoParseError::VecAddFailed);
418 }
419 }
420 }
421 }
422 Ok(data)
423}
424
425/// Converts a HID byte code array into a bitmask
426///
427/// # Arguments
428/// * `codes` - Vector of bytes (e.g. each byte is a HID keyboard code)
429///
430/// # Remarks
431/// Opposite of keyboard_bitmask2vec.
432pub fn hid_vec2bitmask(codes: &[u8]) -> Result<Vec<u8, 32>, HidIoParseError> {
433 let mut data: Vec<u8, 32> = Vec::new(); // Maximum of 32 bytes when dealing with 8 bit codes
434 if data.resize_default(32).is_err() {
435 return Err(HidIoParseError::VecResizeFailed);
436 }
437
438 // Iterate through codes and set each bit accordingly
439 for code in codes {
440 let byte_pos = code / 8; // Determine which byte
441 let bit_mask = 1 << (code - 8 * byte_pos); // Determine which bit
442 data[byte_pos as usize] |= bit_mask;
443 }
444 Ok(data)
445}
446
447// ----- Implementations -----
448
449impl<const H: usize> Default for HidIoPacketBuffer<H> {
450 fn default() -> Self {
451 HidIoPacketBuffer {
452 ptype: HidIoPacketType::Data,
453 id: HidIoCommandId::try_from(0).unwrap(),
454 max_len: 64, // Default size
455 data: Vec::new(),
456 done: false,
457 }
458 }
459}
460
461impl<const H: usize> HidIoPacketBuffer<H> {
462 /// Constructor for HidIoPacketBuffer
463 ///
464 /// # Remarks
465 /// Initialize as blank
466 pub fn new() -> HidIoPacketBuffer<H> {
467 HidIoPacketBuffer {
468 ..Default::default()
469 }
470 }
471
472 /// Clear Data
473 /// Sets done to false and resizes payload to 0
474 pub fn clear(&mut self) {
475 self.done = false;
476 self.data.resize_default(0).unwrap();
477 }
478
479 /// Set Data
480 pub fn set(&mut self, buf: HidIoPacketBuffer<H>) {
481 self.ptype = buf.ptype;
482 self.id = buf.id;
483 self.max_len = buf.max_len;
484 self.data = buf.data;
485 self.done = buf.done;
486 }
487
488 /// Determine id_width
489 fn id_width(&self) -> u8 {
490 match self.id as u32 {
491 0x00..=0xFFFF => 0, // 16 bit Id
492 0x01_0000..=0xFFFF_FFFF => 1, // 32 bit Id
493 }
494 }
495
496 /// Determine id_width_len
497 fn id_width_len(&self) -> u8 {
498 match self.id_width() {
499 0 => 2, // 2 bytes - 16 bit Id
500 1 => 4, // 4 bytes - 32 bit Id
501 _ => 0,
502 }
503 }
504
505 /// Determine total header length, initial and continued packets (always 2 bytes)
506 /// 1 byte for header, 1 byte for len, id_width_len for Id
507 fn hdr_len(&self) -> u8 {
508 2 + self.id_width_len()
509 }
510
511 /// Determine payload max length, initial and continued packets
512 fn payload_len(&self) -> u32 {
513 self.max_len - u32::from(self.hdr_len())
514 }
515
516 /// Serialized length of buffer
517 /// Returns the currently computed serialized length of the
518 /// buffer. Can change based on the struct fields.
519 pub fn serialized_len(&self) -> u32 {
520 // Sync packets have a serialized length of 1
521 if self.ptype == HidIoPacketType::Sync {
522 return 1;
523 }
524
525 let hdr_len = self.hdr_len();
526 let data_len = self.data.len() as u32;
527 let payload_len = self.payload_len();
528
529 let fullpackets = (data_len / payload_len) * (payload_len + u32::from(hdr_len));
530 let partialpacket = if data_len % payload_len > 0 || data_len == 0 {
531 data_len % payload_len + u32::from(hdr_len)
532 } else {
533 0
534 };
535
536 fullpackets + partialpacket
537 }
538
539 /// Append payload data
540 ///
541 /// # Arguments
542 /// * `new_data` - Vector of bytes
543 ///
544 /// # Remarks
545 /// Appends payload to HidIoPacketBuffer.
546 pub fn append_payload(&mut self, new_data: &[u8]) -> bool {
547 // Check if buffer was already finished
548 if self.done {
549 warn!("HidIoPacketBuffer is already 'done'");
550 return false;
551 }
552
553 self.data.extend_from_slice(new_data).is_ok()
554 }
555
556 /// Append packet stream
557 /// Returns the number of bytes used.
558 ///
559 /// # Arguments
560 /// * `packet_data` - Vector of bytes of packet data
561 ///
562 /// # Remarks
563 /// Does packet decoding on the fly.
564 /// Will set done parameter if this is the last packet.
565 pub fn decode_packet(&mut self, packet_data: &[u8]) -> Result<u32, HidIoParseError> {
566 // Check if buffer was already finished
567 if self.done {
568 warn!("HidIoPacketBuffer is already 'done'");
569 return Ok(0);
570 }
571
572 let packet_data_len = packet_data.len() as u32;
573
574 // Get packet type
575 let ptype = packet_type(packet_data)?;
576
577 // Check if this a sync packet
578 if ptype == HidIoPacketType::Sync {
579 self.ptype = ptype;
580 self.done = true;
581 return Ok(1);
582 }
583
584 // Get payload_len
585 let payload_len = payload_len(packet_data)?;
586 let packet_len = payload_len + 2;
587
588 // Make sure there's actually payload_len available
589 trace!(
590 "decode_packet: {:?} - ptype({:?}) payload_len({:?}) packet_len({:?})",
591 packet_data,
592 ptype,
593 payload_len,
594 packet_len
595 );
596 if packet_data_len - 2 < payload_len {
597 warn!(
598 "Dropping. Not enough bytes available in packet stream. got:{}, expected:{}",
599 packet_data_len - 2,
600 payload_len
601 );
602 return Ok(packet_data_len);
603 }
604
605 // Get packet Id
606 let id_num = packet_id(packet_data)?;
607 let id = match HidIoCommandId::try_from(id_num) {
608 Ok(id) => id,
609 Err(_) => {
610 error!("Failed to convert {} to HidIoCommandId", id_num);
611 return Err(HidIoParseError::InvalidHidIoCommandId(id_num));
612 }
613 };
614
615 // Is this a new packet?
616 // More information to set, if initializing buffer
617 if self.data.is_empty()
618 && (ptype != HidIoPacketType::Continued && ptype != HidIoPacketType::NaContinued)
619 {
620 // Set packet type
621 self.ptype = ptype;
622
623 // Set packet id
624 self.id = id;
625
626 // Make sure the current buffer matches what we're expecting
627 } else {
628 // Check for invalid packet type
629 if self.data.is_empty() && ptype == HidIoPacketType::Continued {
630 warn!("Dropping. Invalid packet type when initializing buffer, HidIoPacketType::Continued");
631 return Ok(packet_len);
632 }
633 if self.data.is_empty() && ptype == HidIoPacketType::NaContinued {
634 warn!("Dropping. Invalid packet type when initializing buffer, HidIoPacketType::NaContinued");
635 return Ok(packet_len);
636 }
637
638 // Check if not a continued packet, and we have a payload
639 if !self.data.is_empty() {
640 match ptype {
641 HidIoPacketType::Continued | HidIoPacketType::NaContinued => {}
642 _ => {
643 warn!("Dropping buffer. Invalid packet type (non-HidIoPacketType::Continued) on a already initialized buffer: {} {}", ptype, self.data.is_empty());
644 self.clear();
645 return self.decode_packet(packet_data);
646 }
647 }
648 }
649
650 // Validate that we're looking at the same Id
651 if self.id != id {
652 warn!(
653 "Dropping. Invalid incoming id:{:?}, expected:{:?}",
654 id, self.id
655 );
656 return Ok(packet_len);
657 }
658 }
659
660 // Payload start
661 let payload_start = payload_start(packet_data)?;
662
663 // Get id_width_len
664 let id_width_len = packet_id_width(packet_data)?;
665
666 // Check if this buffer will be completed
667 self.done = !continued_packet(packet_data)?;
668
669 // Add payload
670 let slice =
671 &packet_data[payload_start..payload_start + payload_len as usize - id_width_len];
672 match self.data.extend_from_slice(slice) {
673 Ok(_) => {}
674 Err(_) => {
675 return Err(HidIoParseError::PayloadAddFailed(slice.len()));
676 }
677 }
678
679 // Finished
680 Ok(packet_len)
681 }
682
683 /// Serialize HidIoPacketBuffer
684 ///
685 /// # Remarks
686 /// Determine cont, width, upper_len and len fields
687 /// According to this C-Struct:
688 ///
689 /// ```c
690 /// struct HidIo_Packet {
691 /// HidIo_Packet_Type type:3;
692 /// uint8_t cont:1; // 0 - Only packet, 1 continued packet following
693 /// uint8_t id_width:1; // 0 - 16bits, 1 - 32bits
694 /// uint8_t reserved:1; // Reserved
695 /// uint8_t upper_len:2; // Upper 2 bits of length field (generally unused)
696 /// uint8_t len; // Lower 8 bits of length field
697 /// uint8_t data[0]; // Start of data payload (may start with Id)
698 /// };
699 /// ```
700 pub fn serialize_buffer<'a>(&self, data: &'a mut [u8]) -> Result<&'a [u8], HidIoParseError> {
701 // Check if buffer is ready to serialize
702 if !self.done {
703 return Err(HidIoParseError::BufferNotReady);
704 }
705
706 // --- First Packet ---
707
708 // Keep track of the serialization buffer position
709 let mut pos = 0;
710
711 // Determine id_width
712 let id_width = self.id_width();
713
714 // Determine id_width_len
715 let id_width_len = self.id_width_len();
716
717 // Determine payload max length, initial and continued packets
718 let payload_len = self.payload_len();
719
720 // Data length
721 let data_len = self.data.len() as u32;
722
723 // Determine if a continued packet construct
724 let mut cont: bool = data_len > payload_len;
725
726 // Determine packet len
727 let packet_len: u16 = if cont {
728 // Full payload length
729 payload_len as u16 + u16::from(id_width_len)
730 } else {
731 // Calculate payload length with what's left
732 data_len as u16 + u16::from(id_width_len)
733 };
734
735 // Determine upper_len and len fields
736 let upper_len: u8 = (packet_len >> 8) as u8;
737 let len: u8 = packet_len as u8;
738
739 // Determine ptype
740 let ptype: u8 = match self.ptype {
741 HidIoPacketType::Data => 0,
742 HidIoPacketType::Ack => 1,
743 HidIoPacketType::Nak => 2,
744 HidIoPacketType::Sync => 3,
745 HidIoPacketType::Continued => 4,
746 HidIoPacketType::NaData => 5,
747 HidIoPacketType::NaContinued => 6,
748 };
749
750 // Convert Id into bytes
751 let mut id_vec: Vec<u8, 4> = Vec::new();
752 for idx in 0..id_width_len {
753 let id = (self.id as u32 >> (idx * 8)) as u8;
754 if id_vec.push(id).is_err() {
755 return Err(HidIoParseError::VecAddFailed);
756 }
757 }
758
759 // Construct header byte
760 let hdr_byte: u8 =
761 // type - 3 bits
762 (ptype << 5) |
763 // cont - 1 bit
764 (u8::from(cont) << 4) |
765 // id_width - 1 bit
766 (id_width << 3) |
767 // reserved - 1 bit
768 // (0 << 2) |
769 // upper_len - 2 bits
770 (upper_len & 0x3);
771
772 // Header byte is always serialized
773 pos = serialize_byte(data, hdr_byte, pos)?;
774
775 // Determine if this is a sync packet (much simpler serialization)
776 if self.ptype == HidIoPacketType::Sync {
777 return Ok(data);
778 }
779
780 // Serialize length
781 pos = serialize_byte(data, len, pos)?;
782
783 // Serialize id
784 pos = serialize_bytes(data, &id_vec[..id_width_len as usize], pos)?;
785
786 // Serialize payload data
787 // We can't just serialize directly (extra info is included), serialize each element of vector separately
788 let slice = if cont {
789 // Full payload length
790 &self.data[0..payload_len as usize]
791 } else {
792 // Payload that's available
793 &self.data[0..data_len as usize]
794 };
795 pos = serialize_bytes(data, slice, pos)?;
796
797 // Finish serialization if no more payload left
798 if !cont {
799 return Ok(data);
800 }
801
802 // Determine how much payload is left
803 let mut payload_left = self.data.len() as u32 - payload_len;
804 let mut last_slice_index = payload_len as usize;
805
806 // --- Additional Packets ---
807
808 while cont {
809 // Determine if continued packet construct
810 cont = payload_left > payload_len;
811
812 // Continued Packet
813 let ptype = match self.ptype {
814 HidIoPacketType::Ack | HidIoPacketType::Nak | HidIoPacketType::Data => {
815 HidIoPacketType::Continued as u8
816 }
817 HidIoPacketType::NaData => HidIoPacketType::NaContinued as u8,
818 _ => {
819 warn!("Dropping. Invalid continued packet type: {:?}", self.ptype);
820 break;
821 }
822 };
823
824 // Determine packet len
825 let packet_len: u16 = if cont {
826 // Full payload length
827 payload_len as u16 + u16::from(id_width_len)
828 } else {
829 // Calculate payload length with what's left
830 payload_left as u16 + u16::from(id_width_len)
831 };
832
833 // Determine upper_len and len fields
834 let upper_len: u8 = (packet_len >> 8) as u8;
835 let len: u8 = packet_len as u8;
836
837 // Construct header byte
838 let hdr_byte: u8 =
839 // type - 3 bits
840 (ptype << 5) |
841 // cont - 1 bit
842 (u8::from(cont) << 4) |
843 // id_width - 1 bit
844 (id_width << 3) |
845 // reserved - 1 bit
846 // (0 << 2) |
847 // upper_len - 2 bits
848 (upper_len & 0x3);
849
850 // Serialize header
851 pos = serialize_byte(data, hdr_byte, pos)?;
852
853 // Serialize length
854 pos = serialize_byte(data, len, pos)?;
855
856 // Serialize id
857 pos = serialize_bytes(data, &id_vec[..id_width_len as usize], pos)?;
858
859 // Serialize payload data
860 // We can't just serialize directly (extra info is included), serialize each element of vector separately
861 let slice_end = if cont {
862 // Full payload length
863 last_slice_index + payload_len as usize
864 } else {
865 // Payload that's available
866 data_len as usize
867 };
868 let slice = &self.data[last_slice_index..slice_end];
869 pos = serialize_bytes(data, slice, pos)?;
870
871 // Recalculate how much payload is left
872 payload_left -= (slice_end - last_slice_index) as u32;
873 last_slice_index += payload_len as usize;
874 }
875
876 // --- Finish serialization ---
877 Ok(data)
878 }
879}
880
881/// Very simple error checking function for byte serialization
882/// Returns the new serialization index pointer
883fn serialize_byte(data: &mut [u8], byte: u8, pos: usize) -> Result<usize, HidIoParseError> {
884 // Make sure buffer is large enough
885 if pos >= data.len() {
886 return Err(HidIoParseError::BufferDataTooSmall(pos - 1));
887 }
888
889 data[pos] = byte;
890 Ok(pos + 1)
891}
892
893/// Very simple error checking function for byte array serialization
894/// Returns the new serialization index pointer
895fn serialize_bytes(data: &mut [u8], bytes: &[u8], pos: usize) -> Result<usize, HidIoParseError> {
896 // Make sure buffer is large enough
897 if pos + bytes.len() > data.len() && !bytes.is_empty() {
898 return Err(HidIoParseError::BufferDataTooSmall(pos - 1));
899 }
900
901 for (i, byte) in bytes.iter().enumerate() {
902 data[pos + i] = *byte;
903 }
904 Ok(pos + bytes.len())
905}
906
907impl fmt::Display for HidIoPacketType {
908 /// Display formatter for HidIoPacketType
909 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
910 let ptype_name = match *self {
911 HidIoPacketType::Data => "HidIoPacketBuffer::Data",
912 HidIoPacketType::Ack => "HidIoPacketBuffer::Ack",
913 HidIoPacketType::Nak => "HidIoPacketBuffer::Nak",
914 HidIoPacketType::Sync => "HidIoPacketBuffer::Sync",
915 HidIoPacketType::Continued => "HidIoPacketBuffer::Continued",
916 HidIoPacketType::NaData => "HidIoPacketBuffer::NaData",
917 HidIoPacketType::NaContinued => "HidIoPacketBuffer::NaContinued",
918 };
919 write!(f, "{ptype_name}")
920 }
921}
922
923impl<const H: usize> fmt::Display for HidIoPacketBuffer<H> {
924 /// Display formatter for HidIoPacketBuffer
925 fn fmt(&self, f: &mut fmt::Formatter<'_>) -> fmt::Result {
926 write!(
927 f,
928 "\n{{\n ptype: {}\n id: {:?}\n max_len: {}\n done: {}\n data: {:#?}\n}}",
929 self.ptype, self.id, self.max_len, self.done, self.data,
930 )
931 }
932}