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}