From bb2f43591e2735f58362cab06ff3cb1f0201a138 Mon Sep 17 00:00:00 2001 From: Charlie Boutier Date: Wed, 13 Mar 2024 18:04:47 +0000 Subject: [PATCH] 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 --- uwb/aidl/default/src/uwb_chip.rs | 13 ++++++++++++- 1 file changed, 12 insertions(+), 1 deletion(-) diff --git a/uwb/aidl/default/src/uwb_chip.rs b/uwb/aidl/default/src/uwb_chip.rs index d1c3c67f76..62fe22925a 100644 --- a/uwb/aidl/default/src/uwb_chip.rs +++ b/uwb/aidl/default/src/uwb_chip.rs @@ -180,6 +180,8 @@ impl IUwbChipAsyncServer for UwbChip { 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 @@ impl IUwbChipAsyncServer for UwbChip { // 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.