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.