Merge "Vendor response packets support"

1 files changed, 118 insertions, 5 deletions

diff --git a/src/rust/uwb_uci_packets/src/lib.rs b/src/rust/uwb_uci_packets/src/lib.rs
index 68b93b8..c5bb3f5 100644
--- a/src/rust/uwb_uci_packets/src/lib.rs
+++ b/src/rust/uwb_uci_packets/src/lib.rs
@@ -39,8 +39,19 @@ pub const UCI_PACKET_HEADER_LEN: usize = 7;
 const UCI_DATA_SND_PACKET_HEADER_LEN: usize = 6;
 
 // Opcode field byte position (within UCI packet header) and mask (of bits to be used).
-const UCI_CONTROL_PACKET_HEADER_OPCODE_BYTE_POSITION: usize = 1;
-const UCI_CONTROL_PACKET_HEADER_OPCODE_MASK: u8 = 0x3F;
+const UCI_HEADER_MT_BYTE_POSITION: usize = 0;
+const UCI_HEADER_MT_BIT_SHIFT: u8 = 5;
+const UCI_HEADER_MT_MASK: u8 = 0x7;
+
+const UCI_HEADER_PBF_BYTE_POSITION: usize = 0;
+const UCI_HEADER_PBF_BIT_SHIFT: u8 = 4;
+const UCI_HEADER_PBF_MASK: u8 = 0x1;
+
+const UCI_CONTROL_HEADER_GID_BYTE_POSITION: usize = 0;
+const UCI_CONTROL_HEADER_GID_MASK: u8 = 0xF;
+
+const UCI_CONTROL_HEADER_OID_BYTE_POSITION: usize = 1;
+const UCI_CONTROL_HEADER_OID_MASK: u8 = 0x3F;
 
 #[derive(Debug, Clone, PartialEq, FromPrimitive)]
 pub enum TimeStampLength {
@@ -295,12 +306,28 @@ impl UciControlPacketHeader {
     }
 }
 
+// Helper methods to extract the UCI Packet header fields.
+fn get_mt_from_uci_packet(packet: &[u8]) -> u8 {
+    (packet[UCI_HEADER_MT_BYTE_POSITION] >> UCI_HEADER_MT_BIT_SHIFT) & UCI_HEADER_MT_MASK
+}
+
+fn get_pbf_from_uci_packet(packet: &[u8]) -> u8 {
+    (packet[UCI_HEADER_PBF_BYTE_POSITION] >> UCI_HEADER_PBF_BIT_SHIFT) & UCI_HEADER_PBF_MASK
+}
+
+fn get_gid_from_uci_control_packet(packet: &[u8]) -> u8 {
+    packet[UCI_CONTROL_HEADER_GID_BYTE_POSITION] & UCI_CONTROL_HEADER_GID_MASK
+}
+
+fn get_oid_from_uci_control_packet(packet: &[u8]) -> u8 {
+    packet[UCI_CONTROL_HEADER_OID_BYTE_POSITION] & UCI_CONTROL_HEADER_OID_MASK
+}
+
 // This function parses the packet bytes to return the Control Packet Opcode (OID) field. The
 // caller should check that the packet bytes represent a UCI control packet. The code will not
 // panic because UciPacketHal::to_bytes() should always be larger then the place we access.
 fn get_opcode_from_uci_control_packet(packet: &UciPacketHal) -> u8 {
-    packet.clone().to_bytes()[UCI_CONTROL_PACKET_HEADER_OPCODE_BYTE_POSITION]
-        & UCI_CONTROL_PACKET_HEADER_OPCODE_MASK
+    get_oid_from_uci_control_packet(&packet.clone().to_bytes())
 }
 
 fn is_uci_control_packet(message_type: MessageType) -> bool {
@@ -386,6 +413,25 @@ impl TryFrom<Vec<UciPacketHal>> for UciControlPacket {
     }
 }
 
+#[derive(Debug, Clone)]
+pub struct RawUciControlPacket {
+    pub mt: u8,
+    pub gid: u8,
+    pub oid: u8,
+    pub payload: Vec<u8>,
+}
+
+impl RawUciControlPacket {
+    // Match the GID and OID to confirm the UCI packet (represented by header) is
+    // the same as the stored signature. We don't match the MT because they can be
+    // different (eg: CMD/RSP pair).
+    pub fn is_same_signature_bytes(&self, header: &[u8]) -> bool {
+        let gid = get_gid_from_uci_control_packet(header);
+        let oid = get_oid_from_uci_control_packet(header);
+        gid == self.gid && oid == self.oid
+    }
+}
+
 // UCI Data packet functions.
 fn is_uci_data_rcv_packet(message_type: MessageType, data_packet_format: DataPacketFormat) -> bool {
     message_type == MessageType::Data && data_packet_format == DataPacketFormat::DataRcv
@@ -543,15 +589,42 @@ pub struct PacketDefrager {
     control_fragment_cache: Vec<UciPacketHal>,
     // TODO(b/261762781): Prefer this to be UciDataPacketHal
     data_fragment_cache: Vec<UciPacketHal>,
+    // Raw packet payload bytes cache
+    raw_fragment_cache: Vec<u8>,
 }
 
 pub enum UciDefragPacket {
     Control(UciControlPacket),
     Data(UciDataPacket),
+    Raw(Result<()>, RawUciControlPacket),
 }
 
 impl PacketDefrager {
-    pub fn defragment_packet(&mut self, msg: &[u8]) -> Option<UciDefragPacket> {
+    pub fn defragment_packet(
+        &mut self,
+        msg: &[u8],
+        last_raw_cmd: Option<RawUciControlPacket>,
+    ) -> Option<UciDefragPacket> {
+        if let Some(raw_cmd) = last_raw_cmd {
+            let mt_u8 = get_mt_from_uci_packet(msg);
+            match MessageType::try_from(u8::from(mt_u8)) {
+                Ok(mt) => match mt {
+                    // Parse only a UCI response packet as a Raw packet.
+                    MessageType::Response => {
+                        return self.defragment_raw_uci_response_packet(msg, raw_cmd);
+                    }
+                    _ => { /* Fallthrough to de-frag as a normal UCI packet below */ }
+                },
+                Err(_) => {
+                    error!("Rx packet from HAL has unrecognized MT={}", mt_u8);
+                    return Some(UciDefragPacket::Raw(
+                        Err(Error::InvalidPacketError),
+                        RawUciControlPacket { mt: mt_u8, gid: 0, oid: 0, payload: Vec::new() },
+                    ));
+                }
+            };
+        }
+
         let packet = UciPacketHal::parse(msg)
             .or_else(|e| {
                 error!("Failed to parse packet: {:?}", e);
@@ -598,6 +671,46 @@ impl PacketDefrager {
             }
         }
     }
+
+    fn defragment_raw_uci_response_packet(
+        &mut self,
+        msg: &[u8],
+        raw_cmd: RawUciControlPacket,
+    ) -> Option<UciDefragPacket> {
+        let mt_u8 = get_mt_from_uci_packet(msg);
+        let pbf = get_pbf_from_uci_packet(msg);
+        let gid = get_gid_from_uci_control_packet(msg);
+        let oid = get_oid_from_uci_control_packet(msg);
+        if raw_cmd.is_same_signature_bytes(msg) {
+            // Store only the packet payload bytes (UCI header should not be stored).
+            self.raw_fragment_cache.extend_from_slice(&msg[UCI_PACKET_HAL_HEADER_LEN..]);
+
+            if pbf == u8::from(PacketBoundaryFlag::NotComplete) {
+                return None;
+            }
+
+            // All fragments received, defragment and return the Raw packet's payload bytes.
+            return Some(UciDefragPacket::Raw(
+                Ok(()),
+                RawUciControlPacket {
+                    mt: mt_u8,
+                    gid,
+                    oid,
+                    payload: self.raw_fragment_cache.drain(..).collect(),
+                },
+            ));
+        } else {
+            error!(
+                "Rx packet from HAL (MT={}, PBF={}, GID={}, OID={}) has non-matching\
+                   RawCmd signature",
+                mt_u8, pbf, gid, oid
+            );
+            return Some(UciDefragPacket::Raw(
+                Err(Error::InvalidPacketError),
+                RawUciControlPacket { mt: mt_u8, gid, oid, payload: Vec::new() },
+            ));
+        }
+    }
 }
 
 #[allow(dead_code)]