uwb_hal: handle data_packet parsing
Bug: 328680098
Test: atest CtsUwbMultiDeviceTestCase_FiraRangingTests -s 0.0.0.0:6520 -s 0.0.0.0:6521
Change-Id: I59eae757b3f4f838959360baa24d5a5b9775db54
diff --git a/uwb/aidl/default/src/uwb_chip.rs b/uwb/aidl/default/src/uwb_chip.rs
index d1c3c67..62fe229 100644
--- a/uwb/aidl/default/src/uwb_chip.rs
+++ b/uwb/aidl/default/src/uwb_chip.rs
@@ -180,6 +180,8 @@
let mut reader = AsyncFd::new(reader).unwrap();
loop {
+ const MESSAGE_TYPE_MASK: u8 = 0b11100000;
+ const DATA_MESSAGE_TYPE: u8 = 0b000;
const UWB_HEADER_SIZE: usize = 4;
let mut buffer = vec![0; UWB_HEADER_SIZE];
@@ -224,7 +226,16 @@
// Read the remaining header bytes, if truncated.
read_exact(reader.get_mut(), &mut buffer[read_len..]).unwrap();
- let length = buffer[3] as usize + UWB_HEADER_SIZE;
+ let common_header = buffer[0];
+ let mt = (common_header & MESSAGE_TYPE_MASK) >> 5;
+ let payload_length = if mt == DATA_MESSAGE_TYPE {
+ let payload_length_fields: [u8; 2] = buffer[2..=3].try_into().unwrap();
+ u16::from_le_bytes(payload_length_fields) as usize
+ } else {
+ buffer[3] as usize
+ };
+
+ let length = payload_length + UWB_HEADER_SIZE;
buffer.resize(length, 0);
// Read the payload bytes.