1 // Copyright 2022, The Android Open Source Project
2 //
3 // Licensed under the Apache License, Version 2.0 (the "License");
4 // you may not use this file except in compliance with the License.
5 // You may obtain a copy of the License at
6 //
7 // http://www.apache.org/licenses/LICENSE-2.0
8 //
9 // Unless required by applicable law or agreed to in writing, software
10 // distributed under the License is distributed on an "AS IS" BASIS,
11 // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
12 // See the License for the specific language governing permissions and
13 // limitations under the License.
14
15 #![allow(clippy::all)]
16 #![allow(non_upper_case_globals)]
17 #![allow(non_camel_case_types)]
18 #![allow(non_snake_case)]
19 #![allow(unused)]
20 #![allow(missing_docs)]
21
22 use std::cmp;
23
24 use log::error;
25 use num_derive::FromPrimitive;
26 use num_traits::FromPrimitive;
27 use zeroize::Zeroize;
28
29 mod debug_display;
30
31 include!(concat!(env!("OUT_DIR"), "/uci_packets.rs"));
32
33 const MAX_PAYLOAD_LEN: usize = 255;
34 // TODO: Use a PDL struct to represent the headers and avoid hardcoding
35 // lengths below.
36 // Real UCI packet header len.
37 pub const UCI_PACKET_HAL_HEADER_LEN: usize = 4;
38 // Unfragmented UCI packet header len.
39 pub const UCI_PACKET_HEADER_LEN: usize = 7;
40 // Unfragmented UCI DATA_MESSAGE_SND packet header len.
41 const UCI_DATA_SND_PACKET_HEADER_LEN: usize = 6;
42
43 // Opcode field byte position (within UCI packet header) and mask (of bits to be used).
44 const UCI_HEADER_MT_BYTE_POSITION: usize = 0;
45 const UCI_HEADER_MT_BIT_SHIFT: u8 = 5;
46 const UCI_HEADER_MT_MASK: u8 = 0x7;
47
48 const UCI_HEADER_PBF_BYTE_POSITION: usize = 0;
49 const UCI_HEADER_PBF_BIT_SHIFT: u8 = 4;
50 const UCI_HEADER_PBF_MASK: u8 = 0x1;
51
52 const UCI_CONTROL_HEADER_GID_BYTE_POSITION: usize = 0;
53 const UCI_CONTROL_HEADER_GID_MASK: u8 = 0xF;
54
55 const UCI_CONTROL_HEADER_OID_BYTE_POSITION: usize = 1;
56 const UCI_CONTROL_HEADER_OID_MASK: u8 = 0x3F;
57
58 // Radar field lengths
59 pub const UCI_RADAR_SEQUENCE_NUMBER_LEN: usize = 4;
60 pub const UCI_RADAR_TIMESTAMP_LEN: usize = 4;
61 pub const UCI_RADAR_VENDOR_DATA_LEN_LEN: usize = 1;
62
63 #[derive(Debug, Clone, PartialEq, FromPrimitive)]
64 pub enum TimeStampLength {
65 Timestamp40Bit = 0x0,
66 Timestamp64Bit = 0x1,
67 }
68
69 #[derive(Debug, Clone, PartialEq, FromPrimitive)]
70 pub enum DTAnchorLocationType {
71 NotIncluded = 0x0,
72 Wgs84 = 0x1,
73 Relative = 0x2,
74 }
75
76 #[allow(dead_code)]
77 #[derive(Debug, Clone, PartialEq)]
78 pub struct DlTdoaRangingMeasurement {
79 pub status: u8,
80 pub message_type: u8,
81 pub message_control: u16,
82 pub block_index: u16,
83 pub round_index: u8,
84 pub nlos: u8,
85 pub aoa_azimuth: u16,
86 pub aoa_azimuth_fom: u8,
87 pub aoa_elevation: u16,
88 pub aoa_elevation_fom: u8,
89 pub rssi: u8,
90 pub tx_timestamp: u64,
91 pub rx_timestamp: u64,
92 pub anchor_cfo: u16,
93 pub cfo: u16,
94 pub initiator_reply_time: u32,
95 pub responder_reply_time: u32,
96 pub initiator_responder_tof: u16,
97 pub dt_anchor_location: Vec<u8>,
98 pub ranging_rounds: Vec<u8>,
99 total_size: usize,
100 }
101
102 impl DlTdoaRangingMeasurement {
parse_one(bytes: &[u8]) -> Option<Self>103 pub fn parse_one(bytes: &[u8]) -> Option<Self> {
104 let mut ptr = 0;
105 let status = extract_u8(bytes, &mut ptr, 1)?;
106 let message_type = extract_u8(bytes, &mut ptr, 1)?;
107 let message_control = extract_u16(bytes, &mut ptr, 2)?;
108 let block_index = extract_u16(bytes, &mut ptr, 2)?;
109 let round_index = extract_u8(bytes, &mut ptr, 1)?;
110 let nlos = extract_u8(bytes, &mut ptr, 1)?;
111 let aoa_azimuth = extract_u16(bytes, &mut ptr, 2)?;
112 let aoa_azimuth_fom = extract_u8(bytes, &mut ptr, 1)?;
113 let aoa_elevation = extract_u16(bytes, &mut ptr, 2)?;
114 let aoa_elevation_fom = extract_u8(bytes, &mut ptr, 1)?;
115 let rssi = extract_u8(bytes, &mut ptr, 1)?;
116 let tx_timestamp_length = (message_control >> 1) & 0x1;
117 let tx_timestamp = match TimeStampLength::from_u16(tx_timestamp_length)? {
118 TimeStampLength::Timestamp40Bit => extract_u64(bytes, &mut ptr, 5)?,
119 TimeStampLength::Timestamp64Bit => extract_u64(bytes, &mut ptr, 8)?,
120 };
121 let rx_timestamp_length = (message_control >> 3) & 0x1;
122 let rx_timestamp = match TimeStampLength::from_u16(rx_timestamp_length)? {
123 TimeStampLength::Timestamp40Bit => extract_u64(bytes, &mut ptr, 5)?,
124 TimeStampLength::Timestamp64Bit => extract_u64(bytes, &mut ptr, 8)?,
125 };
126 let anchor_cfo = extract_u16(bytes, &mut ptr, 2)?;
127 let cfo = extract_u16(bytes, &mut ptr, 2)?;
128 let initiator_reply_time = extract_u32(bytes, &mut ptr, 4)?;
129 let responder_reply_time = extract_u32(bytes, &mut ptr, 4)?;
130 let initiator_responder_tof = extract_u16(bytes, &mut ptr, 2)?;
131 let dt_location_type = (message_control >> 5) & 0x3;
132 let dt_anchor_location = match DTAnchorLocationType::from_u16(dt_location_type)? {
133 DTAnchorLocationType::Wgs84 => extract_vec(bytes, &mut ptr, 12)?,
134 DTAnchorLocationType::Relative => extract_vec(bytes, &mut ptr, 10)?,
135 _ => vec![],
136 };
137 let active_ranging_rounds = ((message_control >> 7) & 0xf) as u8;
138 let ranging_round = extract_vec(bytes, &mut ptr, active_ranging_rounds as usize)?;
139
140 Some(DlTdoaRangingMeasurement {
141 status,
142 message_type,
143 message_control,
144 block_index,
145 round_index,
146 nlos,
147 aoa_azimuth,
148 aoa_azimuth_fom,
149 aoa_elevation,
150 aoa_elevation_fom,
151 rssi,
152 tx_timestamp,
153 rx_timestamp,
154 anchor_cfo,
155 cfo,
156 initiator_reply_time,
157 responder_reply_time,
158 initiator_responder_tof,
159 dt_anchor_location: dt_anchor_location.to_vec(),
160 ranging_rounds: ranging_round.to_vec(),
161 total_size: ptr,
162 })
163 }
get_total_size(&self) -> usize164 pub fn get_total_size(&self) -> usize {
165 self.total_size
166 }
167 }
168
169 #[derive(Debug, Clone, PartialEq)]
170 pub struct ShortAddressDlTdoaRangingMeasurement {
171 pub mac_address: u16,
172 pub measurement: DlTdoaRangingMeasurement,
173 }
174
175 impl ShortAddressDlTdoaRangingMeasurement {
176 /// Parse the `payload` byte buffer from PDL to the vector of measurement.
parse(bytes: &[u8], no_of_ranging_measurement: u8) -> Option<Vec<Self>>177 pub fn parse(bytes: &[u8], no_of_ranging_measurement: u8) -> Option<Vec<Self>> {
178 let mut ptr = 0;
179 let mut measurements = vec![];
180 let mut count = 0;
181 while (count < no_of_ranging_measurement) {
182 let mac_address = extract_u16(bytes, &mut ptr, 2)?;
183 let rem = &bytes[ptr..];
184 let measurement = DlTdoaRangingMeasurement::parse_one(rem);
185 match measurement {
186 Some(measurement) => {
187 ptr += measurement.get_total_size();
188 measurements
189 .push(ShortAddressDlTdoaRangingMeasurement { mac_address, measurement });
190 count = count + 1;
191 }
192 None => return None,
193 }
194 }
195 Some(measurements)
196 }
197 }
198
199 #[derive(Debug, Clone, PartialEq)]
200 pub struct ExtendedAddressDlTdoaRangingMeasurement {
201 pub mac_address: u64,
202 pub measurement: DlTdoaRangingMeasurement,
203 }
204
205 impl ExtendedAddressDlTdoaRangingMeasurement {
206 /// Parse the `payload` byte buffer from PDL to the vector of measurement.
parse(bytes: &[u8], no_of_ranging_measurement: u8) -> Option<Vec<Self>>207 pub fn parse(bytes: &[u8], no_of_ranging_measurement: u8) -> Option<Vec<Self>> {
208 let mut ptr = 0;
209 let mut measurements = vec![];
210 let mut count = 0;
211 while (count < no_of_ranging_measurement) {
212 let mac_address = extract_u64(bytes, &mut ptr, 8)?;
213 let rem = &bytes[ptr..];
214 let measurement = DlTdoaRangingMeasurement::parse_one(rem);
215 match measurement {
216 Some(measurement) => {
217 ptr += measurement.get_total_size();
218 measurements
219 .push(ExtendedAddressDlTdoaRangingMeasurement { mac_address, measurement });
220 count = count + 1;
221 }
222 None => return None,
223 }
224 }
225 Some(measurements)
226 }
227 }
228
extract_vec(bytes: &[u8], ptr: &mut usize, consumed_size: usize) -> Option<Vec<u8>>229 pub fn extract_vec(bytes: &[u8], ptr: &mut usize, consumed_size: usize) -> Option<Vec<u8>> {
230 if bytes.len() < *ptr + consumed_size {
231 return None;
232 }
233
234 let res = bytes[*ptr..*ptr + consumed_size].to_vec();
235 *ptr += consumed_size;
236 Some(res)
237 }
238
239 /// Generate the function that extracts the value from byte buffers.
240 macro_rules! generate_extract_func {
241 ($func_name:ident, $type:ty) => {
242 /// Extract the value from |byte[ptr..ptr + consumed_size]| in little endian.
243 fn $func_name(bytes: &[u8], ptr: &mut usize, consumed_size: usize) -> Option<$type> {
244 const type_size: usize = std::mem::size_of::<$type>();
245 if consumed_size > type_size {
246 return None;
247 }
248
249 let extracted_bytes = extract_vec(bytes, ptr, consumed_size)?;
250 let mut le_bytes = [0; type_size];
251 le_bytes[0..consumed_size].copy_from_slice(&extracted_bytes);
252 Some(<$type>::from_le_bytes(le_bytes))
253 }
254 };
255 }
256
257 generate_extract_func!(extract_u8, u8);
258 generate_extract_func!(extract_u16, u16);
259 generate_extract_func!(extract_u32, u32);
260 generate_extract_func!(extract_u64, u64);
261
262 // The GroupIdOrDataPacketFormat enum has all the values defined in both the GroupId and
263 // DataPacketFormat enums. It represents the same bits in UCI packet header - the GID field in
264 // a UCI control packet, and the DataPacketFormat field in a UCI data packet. Hence the unwrap()
265 // calls in the conversions below should always succeed (as long as care is taken in future, to
266 // keep the two enums in sync, for any additional values defined in the UCI spec).
267 impl From<GroupId> for GroupIdOrDataPacketFormat {
from(gid: GroupId) -> Self268 fn from(gid: GroupId) -> Self {
269 GroupIdOrDataPacketFormat::try_from(u8::from(gid)).unwrap()
270 }
271 }
272
273 impl From<GroupIdOrDataPacketFormat> for GroupId {
from(gid_or_dpf: GroupIdOrDataPacketFormat) -> Self274 fn from(gid_or_dpf: GroupIdOrDataPacketFormat) -> Self {
275 GroupId::try_from(u8::from(gid_or_dpf)).unwrap()
276 }
277 }
278
279 impl From<DataPacketFormat> for GroupIdOrDataPacketFormat {
from(dpf: DataPacketFormat) -> Self280 fn from(dpf: DataPacketFormat) -> Self {
281 GroupIdOrDataPacketFormat::try_from(u8::from(dpf)).unwrap()
282 }
283 }
284
285 // The GroupIdOrDataPacketFormat enum has more values defined (for the GroupId bits) than the
286 // DataPacketFormat enum. Hence this is implemented as TryFrom() instead of From().
287 impl TryFrom<GroupIdOrDataPacketFormat> for DataPacketFormat {
288 type Error = DecodeError;
289
try_from(gid_or_dpf: GroupIdOrDataPacketFormat) -> Result<Self, DecodeError>290 fn try_from(gid_or_dpf: GroupIdOrDataPacketFormat) -> Result<Self, DecodeError> {
291 DataPacketFormat::try_from(u8::from(gid_or_dpf)).or(Err(DecodeError::InvalidPacketError))
292 }
293 }
294
295 // Container for UCI packet header fields.
296 struct UciControlPacketHeader {
297 message_type: MessageType,
298 group_id: GroupId,
299 opcode: u8,
300 }
301
302 impl UciControlPacketHeader {
new(message_type: MessageType, group_id: GroupId, opcode: u8) -> Result<Self, DecodeError>303 fn new(message_type: MessageType, group_id: GroupId, opcode: u8) -> Result<Self, DecodeError> {
304 if !is_uci_control_packet(message_type) {
305 return Err(DecodeError::InvalidPacketError);
306 }
307
308 Ok(UciControlPacketHeader {
309 message_type: message_type,
310 group_id: group_id,
311 opcode: opcode,
312 })
313 }
314 }
315
316 // Helper methods to extract the UCI Packet header fields.
get_mt_from_uci_packet(packet: &[u8]) -> u8317 fn get_mt_from_uci_packet(packet: &[u8]) -> u8 {
318 (packet[UCI_HEADER_MT_BYTE_POSITION] >> UCI_HEADER_MT_BIT_SHIFT) & UCI_HEADER_MT_MASK
319 }
320
get_pbf_from_uci_packet(packet: &[u8]) -> u8321 fn get_pbf_from_uci_packet(packet: &[u8]) -> u8 {
322 (packet[UCI_HEADER_PBF_BYTE_POSITION] >> UCI_HEADER_PBF_BIT_SHIFT) & UCI_HEADER_PBF_MASK
323 }
324
get_gid_from_uci_control_packet(packet: &[u8]) -> u8325 fn get_gid_from_uci_control_packet(packet: &[u8]) -> u8 {
326 packet[UCI_CONTROL_HEADER_GID_BYTE_POSITION] & UCI_CONTROL_HEADER_GID_MASK
327 }
328
get_oid_from_uci_control_packet(packet: &[u8]) -> u8329 fn get_oid_from_uci_control_packet(packet: &[u8]) -> u8 {
330 packet[UCI_CONTROL_HEADER_OID_BYTE_POSITION] & UCI_CONTROL_HEADER_OID_MASK
331 }
332
333 // This function parses the packet bytes to return the Control Packet Opcode (OID) field. The
334 // caller should check that the packet bytes represent a UCI control packet. The code will not
335 // panic because UciPacketHal::encode_to_bytes() should always be larger then the place we access.
get_opcode_from_uci_control_packet(packet: &UciPacketHal) -> u8336 fn get_opcode_from_uci_control_packet(packet: &UciPacketHal) -> u8 {
337 get_oid_from_uci_control_packet(&packet.encode_to_bytes().unwrap())
338 }
339
is_uci_control_packet(message_type: MessageType) -> bool340 fn is_uci_control_packet(message_type: MessageType) -> bool {
341 match message_type {
342 MessageType::Command
343 | MessageType::Response
344 | MessageType::Notification
345 | MessageType::ReservedForTesting1
346 | MessageType::ReservedForTesting2 => true,
347 _ => false,
348 }
349 }
350
build_uci_control_packet( message_type: MessageType, group_id: GroupId, opcode: u8, payload: Option<Bytes>, ) -> Option<UciControlPacket>351 pub fn build_uci_control_packet(
352 message_type: MessageType,
353 group_id: GroupId,
354 opcode: u8,
355 payload: Option<Bytes>,
356 ) -> Option<UciControlPacket> {
357 if !is_uci_control_packet(message_type) {
358 error!("Only control packets are allowed, MessageType: {message_type:?}");
359 return None;
360 }
361 Some(UciControlPacketBuilder { group_id, message_type, opcode, payload }.build())
362 }
363
364 // Ensure that the new packet fragment belong to the same packet.
is_same_control_packet(header: &UciControlPacketHeader, packet: &UciPacketHal) -> bool365 fn is_same_control_packet(header: &UciControlPacketHeader, packet: &UciPacketHal) -> bool {
366 is_uci_control_packet(header.message_type)
367 && header.message_type == packet.get_message_type()
368 && header.group_id == packet.get_group_id_or_data_packet_format().into()
369 && header.opcode == get_opcode_from_uci_control_packet(packet)
370 }
371
is_device_state_err_control_packet(packet: &UciPacketHal) -> bool372 fn is_device_state_err_control_packet(packet: &UciPacketHal) -> bool {
373 packet.get_message_type() == MessageType::Notification.into()
374 && packet.get_group_id_or_data_packet_format() == GroupIdOrDataPacketFormat::Core.into()
375 && get_opcode_from_uci_control_packet(packet) == CoreOpCode::CoreDeviceStatusNtf.into()
376 && packet.encode_to_vec().unwrap()[UCI_PACKET_HAL_HEADER_LEN]
377 == DeviceState::DeviceStateError.into()
378 }
379
380 impl UciControlPacket {
381 // For some usage, we need to get the raw payload.
to_raw_payload(self) -> Vec<u8>382 pub fn to_raw_payload(self) -> Vec<u8> {
383 self.encode_to_bytes().unwrap().slice(UCI_PACKET_HEADER_LEN..).to_vec()
384 }
385 }
386
387 // Helper to convert from vector of |UciPacketHal| to |UciControlPacket|. An example
388 // usage is to convert a list UciPacketHAL fragments to one UciPacket, during de-fragmentation.
389 impl TryFrom<Vec<UciPacketHal>> for UciControlPacket {
390 type Error = DecodeError;
391
try_from(packets: Vec<UciPacketHal>) -> Result<Self, DecodeError>392 fn try_from(packets: Vec<UciPacketHal>) -> Result<Self, DecodeError> {
393 if packets.is_empty() {
394 return Err(DecodeError::InvalidPacketError);
395 }
396
397 // Store header info from the first packet.
398 let header = UciControlPacketHeader::new(
399 packets[0].get_message_type(),
400 packets[0].get_group_id_or_data_packet_format().into(),
401 get_opcode_from_uci_control_packet(&packets[0]),
402 )?;
403
404 // Create the reassembled payload.
405 let mut payload_buf = BytesMut::new();
406 for packet in packets {
407 // Ensure that the new fragment is part of the same packet.
408 if !is_same_control_packet(&header, &packet) {
409 // if DEVICE_STATE_ERROR notification is received while waiting for remaining fragments,
410 // process it and send to upper layer for device recovery
411 if is_device_state_err_control_packet(&packet) {
412 error!("Received device reset error: {:?}", packet);
413 return UciControlPacket::parse(
414 &UciControlPacketBuilder {
415 message_type: packet.get_message_type(),
416 group_id: packet.get_group_id_or_data_packet_format().into(),
417 opcode: get_opcode_from_uci_control_packet(&packet),
418 payload: Some(
419 packet
420 .encode_to_bytes()
421 .unwrap()
422 .slice(UCI_PACKET_HAL_HEADER_LEN..),
423 ),
424 }
425 .build()
426 .encode_to_bytes()
427 .unwrap(),
428 );
429 }
430 error!("Received unexpected fragment: {:?}", packet);
431 return Err(DecodeError::InvalidPacketError);
432 }
433 // get payload by stripping the header.
434 payload_buf.extend_from_slice(
435 &packet.encode_to_bytes().unwrap().slice(UCI_PACKET_HAL_HEADER_LEN..),
436 )
437 }
438
439 // Create assembled |UciControlPacket| and convert to bytes again since we need to
440 // reparse the packet after defragmentation to get the appropriate message.
441 UciControlPacket::parse(
442 &UciControlPacketBuilder {
443 message_type: header.message_type,
444 group_id: header.group_id,
445 opcode: header.opcode,
446 payload: Some(payload_buf.into()),
447 }
448 .build()
449 .encode_to_bytes()
450 .unwrap(),
451 )
452 }
453 }
454
455 #[derive(Debug, Clone)]
456 pub struct RawUciControlPacket {
457 pub mt: u8,
458 pub gid: u8,
459 pub oid: u8,
460 pub payload: Vec<u8>,
461 }
462
463 impl RawUciControlPacket {
464 // Match the GID and OID to confirm the UCI packet (represented by header) is
465 // the same as the stored signature. We don't match the MT because they can be
466 // different (eg: CMD/RSP pair).
is_same_signature_bytes(&self, header: &[u8]) -> bool467 pub fn is_same_signature_bytes(&self, header: &[u8]) -> bool {
468 let gid = get_gid_from_uci_control_packet(header);
469 let oid = get_oid_from_uci_control_packet(header);
470 gid == self.gid && oid == self.oid
471 }
472 }
473
is_uci_data_packet(message_type: MessageType) -> bool474 fn is_uci_data_packet(message_type: MessageType) -> bool {
475 message_type == MessageType::Data
476 }
477
is_data_rcv_or_radar_format(data_packet_format: DataPacketFormat) -> bool478 fn is_data_rcv_or_radar_format(data_packet_format: DataPacketFormat) -> bool {
479 data_packet_format == DataPacketFormat::DataRcv
480 || data_packet_format == DataPacketFormat::RadarDataMessage
481 }
482
try_into_data_payload( packet: UciPacketHal, expected_data_packet_format: DataPacketFormat, ) -> Result<Bytes, DecodeError>483 fn try_into_data_payload(
484 packet: UciPacketHal,
485 expected_data_packet_format: DataPacketFormat,
486 ) -> Result<Bytes, DecodeError> {
487 let dpf: DataPacketFormat = packet.get_group_id_or_data_packet_format().try_into()?;
488 if is_uci_data_packet(packet.get_message_type()) && dpf == expected_data_packet_format {
489 Ok(packet.encode_to_bytes().unwrap().slice(UCI_PACKET_HAL_HEADER_LEN..))
490 } else {
491 error!("Received unexpected data packet fragment: {:?}", packet);
492 Err(DecodeError::InvalidPacketError)
493 }
494 }
495
496 // Helper to convert from vector of |UciPacketHal| to |UciDataPacket|. An example
497 // usage is to convert a list UciPacketHAL fragments to one UciPacket, during de-fragmentation.
498 impl TryFrom<Vec<UciPacketHal>> for UciDataPacket {
499 type Error = DecodeError;
500
try_from(packets: Vec<UciPacketHal>) -> Result<Self, DecodeError>501 fn try_from(packets: Vec<UciPacketHal>) -> Result<Self, DecodeError> {
502 if packets.is_empty() {
503 return Err(DecodeError::InvalidPacketError);
504 }
505
506 let dpf: DataPacketFormat = packets[0].get_group_id_or_data_packet_format().try_into()?;
507 if !is_data_rcv_or_radar_format(dpf) {
508 error!("Unexpected data packet format {:?}", dpf);
509 }
510
511 // Create the reassembled payload.
512 let mut payload_buf = Bytes::new();
513 for packet in packets {
514 // Ensure that the fragment is a Data Rcv packet.
515 // Get payload by stripping the header.
516 payload_buf = [payload_buf, try_into_data_payload(packet, dpf)?].concat().into();
517 }
518
519 // Create assembled |UciDataPacket| and convert to bytes again since we need to
520 // reparse the packet after defragmentation to get the appropriate message.
521 UciDataPacket::parse(
522 &UciDataPacketBuilder {
523 message_type: MessageType::Data,
524 data_packet_format: dpf,
525 payload: Some(payload_buf.into()),
526 }
527 .build()
528 .encode_to_bytes()
529 .unwrap(),
530 )
531 }
532 }
533
534 // Helper to convert from |UciControlPacket| to vector of |UciControlPacketHal|s. An
535 // example usage is to do this conversion for fragmentation (from Host to UWBS).
536 impl From<UciControlPacket> for Vec<UciControlPacketHal> {
from(packet: UciControlPacket) -> Self537 fn from(packet: UciControlPacket) -> Self {
538 // Store header info.
539 let header = match UciControlPacketHeader::new(
540 packet.get_message_type(),
541 packet.get_group_id(),
542 packet.get_opcode(),
543 ) {
544 Ok(hdr) => hdr,
545 _ => {
546 error!(
547 "Unable to parse UciControlPacketHeader from UciControlPacket: {:?}",
548 packet
549 );
550 return Vec::new();
551 }
552 };
553
554 let mut fragments = Vec::new();
555 // get payload by stripping the header.
556 let payload = packet.encode_to_bytes().unwrap().slice(UCI_PACKET_HEADER_LEN..);
557 if payload.is_empty() {
558 fragments.push(
559 UciControlPacketHalBuilder {
560 message_type: header.message_type,
561 group_id_or_data_packet_format: header.group_id.into(),
562 opcode: header.opcode,
563 packet_boundary_flag: PacketBoundaryFlag::Complete,
564 payload: None,
565 }
566 .build(),
567 );
568 } else {
569 let mut fragments_iter = payload.chunks(MAX_PAYLOAD_LEN).peekable();
570 while let Some(fragment) = fragments_iter.next() {
571 // Set the last fragment complete if this is last fragment.
572 let pbf = if let Some(nxt_fragment) = fragments_iter.peek() {
573 PacketBoundaryFlag::NotComplete
574 } else {
575 PacketBoundaryFlag::Complete
576 };
577 fragments.push(
578 UciControlPacketHalBuilder {
579 message_type: header.message_type,
580 group_id_or_data_packet_format: header.group_id.into(),
581 opcode: header.opcode,
582 packet_boundary_flag: pbf,
583 payload: Some(Bytes::from(fragment.to_owned())),
584 }
585 .build(),
586 );
587 }
588 }
589 fragments
590 }
591 }
592
593 // Helper to convert From<UciDataSnd> into Vec<UciDataPacketHal>. An
594 // example usage is for fragmentation in the Data Packet Tx flow.
fragment_data_msg_send(packet: UciDataSnd, max_payload_len: usize) -> Vec<UciDataPacketHal>595 pub fn fragment_data_msg_send(packet: UciDataSnd, max_payload_len: usize) -> Vec<UciDataPacketHal> {
596 let mut fragments = Vec::new();
597 let dpf = packet.get_data_packet_format().into();
598
599 // get payload by stripping the header.
600 let payload = packet.encode_to_bytes().unwrap().slice(UCI_DATA_SND_PACKET_HEADER_LEN..);
601 if payload.is_empty() {
602 fragments.push(
603 UciDataPacketHalBuilder {
604 group_id_or_data_packet_format: dpf,
605 packet_boundary_flag: PacketBoundaryFlag::Complete,
606 payload: None,
607 }
608 .build(),
609 );
610 } else {
611 let mut fragments_iter = payload.chunks(max_payload_len).peekable();
612 while let Some(fragment) = fragments_iter.next() {
613 // Set the last fragment complete if this is last fragment.
614 let pbf = if let Some(nxt_fragment) = fragments_iter.peek() {
615 PacketBoundaryFlag::NotComplete
616 } else {
617 PacketBoundaryFlag::Complete
618 };
619 fragments.push(
620 UciDataPacketHalBuilder {
621 group_id_or_data_packet_format: dpf,
622 packet_boundary_flag: pbf,
623 payload: Some(Bytes::from(fragment.to_owned())),
624 }
625 .build(),
626 );
627 }
628 }
629 fragments
630 }
631
632 #[derive(Default, Debug)]
633 pub struct PacketDefrager {
634 // Cache to store incoming fragmented packets in the middle of reassembly.
635 // Will be empty if there is no reassembly in progress.
636 // TODO(b/261762781): Prefer this to be UciControlPacketHal
637 control_fragment_cache: Vec<UciPacketHal>,
638 // TODO(b/261762781): Prefer this to be UciDataPacketHal
639 data_fragment_cache: Vec<UciPacketHal>,
640 // Raw packet payload bytes cache
641 raw_fragment_cache: Vec<u8>,
642 }
643
644 pub enum UciDefragPacket {
645 Control(UciControlPacket),
646 Data(UciDataPacket),
647 Raw(Result<(), DecodeError>, RawUciControlPacket),
648 }
649
650 impl PacketDefrager {
defragment_packet( &mut self, msg: &[u8], last_raw_cmd: Option<RawUciControlPacket>, ) -> Option<UciDefragPacket>651 pub fn defragment_packet(
652 &mut self,
653 msg: &[u8],
654 last_raw_cmd: Option<RawUciControlPacket>,
655 ) -> Option<UciDefragPacket> {
656 if let Some(raw_cmd) = last_raw_cmd {
657 let mt_u8 = get_mt_from_uci_packet(msg);
658 match MessageType::try_from(u8::from(mt_u8)) {
659 Ok(mt) => match mt {
660 // Parse only a UCI response packet as a Raw packet.
661 MessageType::Response => {
662 return self.defragment_raw_uci_response_packet(msg, raw_cmd);
663 }
664 _ => { /* Fallthrough to de-frag as a normal UCI packet below */ }
665 },
666 Err(_) => {
667 error!("Rx packet from HAL has unrecognized MT={}", mt_u8);
668 return Some(UciDefragPacket::Raw(
669 Err(DecodeError::InvalidPacketError),
670 RawUciControlPacket { mt: mt_u8, gid: 0, oid: 0, payload: Vec::new() },
671 ));
672 }
673 };
674 }
675
676 let packet = UciPacketHal::parse(msg)
677 .or_else(|e| {
678 error!("Failed to parse packet: {:?}", e);
679 Err(e)
680 })
681 .ok()?;
682
683 let pbf = packet.get_packet_boundary_flag();
684
685 // TODO(b/261762781): The current implementation allows for the possibility that we receive
686 // interleaved Control/Data HAL packets, and so uses separate caches for them. In the
687 // future, if we determine that interleaving is not possible, this can be simplified.
688 if is_uci_control_packet(packet.get_message_type()) {
689 // Add the incoming fragment to the control packet cache.
690 self.control_fragment_cache.push(packet);
691 if pbf == PacketBoundaryFlag::NotComplete {
692 // Wait for remaining fragments.
693 return None;
694 }
695
696 // All fragments received, defragment the control packet.
697 match self.control_fragment_cache.drain(..).collect::<Vec<_>>().try_into() {
698 Ok(packet) => Some(UciDefragPacket::Control(packet)),
699 Err(e) => {
700 error!("Failed to defragment control packet: {:?}", e);
701 None
702 }
703 }
704 } else {
705 // Add the incoming fragment to the data packet cache.
706 self.data_fragment_cache.push(packet);
707 if pbf == PacketBoundaryFlag::NotComplete {
708 // Wait for remaining fragments.
709 return None;
710 }
711
712 // All fragments received, defragment the data packet.
713 match self.data_fragment_cache.drain(..).collect::<Vec<_>>().try_into() {
714 Ok(packet) => Some(UciDefragPacket::Data(packet)),
715 Err(e) => {
716 error!("Failed to defragment data packet: {:?}", e);
717 None
718 }
719 }
720 }
721 }
722
defragment_raw_uci_response_packet( &mut self, msg: &[u8], raw_cmd: RawUciControlPacket, ) -> Option<UciDefragPacket>723 fn defragment_raw_uci_response_packet(
724 &mut self,
725 msg: &[u8],
726 raw_cmd: RawUciControlPacket,
727 ) -> Option<UciDefragPacket> {
728 let mt_u8 = get_mt_from_uci_packet(msg);
729 let pbf = get_pbf_from_uci_packet(msg);
730 let gid = get_gid_from_uci_control_packet(msg);
731 let oid = get_oid_from_uci_control_packet(msg);
732 if raw_cmd.is_same_signature_bytes(msg) {
733 // Store only the packet payload bytes (UCI header should not be stored).
734 self.raw_fragment_cache.extend_from_slice(&msg[UCI_PACKET_HAL_HEADER_LEN..]);
735
736 if pbf == u8::from(PacketBoundaryFlag::NotComplete) {
737 return None;
738 }
739
740 // All fragments received, defragment and return the Raw packet's payload bytes.
741 return Some(UciDefragPacket::Raw(
742 Ok(()),
743 RawUciControlPacket {
744 mt: mt_u8,
745 gid,
746 oid,
747 payload: self.raw_fragment_cache.drain(..).collect(),
748 },
749 ));
750 } else {
751 error!(
752 "Rx packet from HAL (MT={}, PBF={}, GID={}, OID={}) has non-matching\
753 RawCmd signature",
754 mt_u8, pbf, gid, oid
755 );
756 return Some(UciDefragPacket::Raw(
757 Err(DecodeError::InvalidPacketError),
758 RawUciControlPacket { mt: mt_u8, gid, oid, payload: Vec::new() },
759 ));
760 }
761 }
762 }
763
764 #[allow(dead_code)]
765 #[derive(Debug, Clone)]
766 pub struct ParsedDiagnosticNtfPacket {
767 session_token: u32,
768 sequence_number: u32,
769 frame_reports: Vec<ParsedFrameReport>,
770 }
771
772 #[allow(dead_code)]
773 #[derive(Clone)]
774 pub struct ParsedFrameReport {
775 uwb_msg_id: u8,
776 action: u8,
777 antenna_set: u8,
778 rssi: Vec<u8>,
779 aoa: Vec<AoaMeasurement>,
780 cir: Vec<CirValue>,
781 segment_metrics: Vec<SegmentMetricsValue>,
782 }
783
parse_diagnostics_ntf( evt: AndroidRangeDiagnosticsNtf, ) -> Result<ParsedDiagnosticNtfPacket, DecodeError>784 pub fn parse_diagnostics_ntf(
785 evt: AndroidRangeDiagnosticsNtf,
786 ) -> Result<ParsedDiagnosticNtfPacket, DecodeError> {
787 let session_token = evt.get_session_token();
788 let sequence_number = evt.get_sequence_number();
789 let mut parsed_frame_reports = Vec::new();
790 for report in evt.get_frame_reports() {
791 let mut rssi_vec = Vec::new();
792 let mut aoa_vec = Vec::new();
793 let mut cir_vec = Vec::new();
794 let mut segment_metrics_vec = Vec::new();
795 for tlv in &report.frame_report_tlvs {
796 match FrameReportTlvPacket::parse(
797 &[vec![tlv.t as u8, tlv.v.len() as u8, (tlv.v.len() >> 8) as u8], tlv.v.clone()]
798 .concat(),
799 ) {
800 Ok(pkt) => match pkt.specialize() {
801 FrameReportTlvPacketChild::Rssi(rssi) => {
802 rssi_vec.append(&mut rssi.get_rssi().clone())
803 }
804 FrameReportTlvPacketChild::Aoa(aoa) => {
805 aoa_vec.append(&mut aoa.get_aoa().clone())
806 }
807 FrameReportTlvPacketChild::Cir(cir) => {
808 cir_vec.append(&mut cir.get_cir_value().clone())
809 }
810 FrameReportTlvPacketChild::SegmentMetrics(sm) => {
811 segment_metrics_vec.append(&mut sm.get_segment_metrics().clone())
812 }
813 _ => return Err(DecodeError::InvalidPacketError),
814 },
815 Err(e) => {
816 error!("Failed to parse the packet {:?}", e);
817 return Err(DecodeError::InvalidPacketError);
818 }
819 }
820 }
821 parsed_frame_reports.push(ParsedFrameReport {
822 uwb_msg_id: report.uwb_msg_id,
823 action: report.action,
824 antenna_set: report.antenna_set,
825 rssi: rssi_vec,
826 aoa: aoa_vec,
827 cir: cir_vec,
828 segment_metrics: segment_metrics_vec,
829 });
830 }
831 Ok(ParsedDiagnosticNtfPacket {
832 session_token,
833 sequence_number,
834 frame_reports: parsed_frame_reports,
835 })
836 }
837
838 #[derive(Debug, Clone, PartialEq)]
839 pub enum Controlees {
840 NoSessionKey(Vec<Controlee>),
841 ShortSessionKey(Vec<Controlee_V2_0_16_Byte_Version>),
842 LongSessionKey(Vec<Controlee_V2_0_32_Byte_Version>),
843 }
844
845 // TODO(ziyiw): Replace these functions after making uwb_uci_packets::Controlee::write_to() public.
write_controlee(controlee: &Controlee) -> BytesMut846 pub fn write_controlee(controlee: &Controlee) -> BytesMut {
847 let mut buffer = BytesMut::new();
848 buffer.extend_from_slice(&controlee.short_address);
849 let subsession_id = controlee.subsession_id;
850 buffer.extend_from_slice(&subsession_id.to_le_bytes()[0..4]);
851 buffer
852 }
853
write_controlee_2_0_16byte(controlee: &Controlee_V2_0_16_Byte_Version) -> BytesMut854 pub fn write_controlee_2_0_16byte(controlee: &Controlee_V2_0_16_Byte_Version) -> BytesMut {
855 let mut buffer = BytesMut::new();
856 buffer.extend_from_slice(&controlee.short_address);
857 let subsession_id = controlee.subsession_id;
858 buffer.extend_from_slice(&subsession_id.to_le_bytes()[0..4]);
859 buffer.extend_from_slice(&controlee.subsession_key);
860 buffer
861 }
862
write_controlee_2_0_32byte(controlee: &Controlee_V2_0_32_Byte_Version) -> BytesMut863 pub fn write_controlee_2_0_32byte(controlee: &Controlee_V2_0_32_Byte_Version) -> BytesMut {
864 let mut buffer = BytesMut::new();
865 buffer.extend_from_slice(&controlee.short_address);
866 let subsession_id = controlee.subsession_id;
867 buffer.extend_from_slice(&subsession_id.to_le_bytes()[0..4]);
868 buffer.extend_from_slice(&controlee.subsession_key);
869 buffer
870 }
871
872 /// Generate the SessionUpdateControllerMulticastListCmd packet.
873 ///
874 /// This function can build the packet with/without message control, which
875 /// is indicated by action parameter.
build_session_update_controller_multicast_list_cmd( session_token: u32, action: UpdateMulticastListAction, controlees: Controlees, ) -> Result<SessionUpdateControllerMulticastListCmd, DecodeError>876 pub fn build_session_update_controller_multicast_list_cmd(
877 session_token: u32,
878 action: UpdateMulticastListAction,
879 controlees: Controlees,
880 ) -> Result<SessionUpdateControllerMulticastListCmd, DecodeError> {
881 let mut controlees_buf = BytesMut::new();
882 match controlees {
883 Controlees::NoSessionKey(controlee_v1) => {
884 controlees_buf.extend_from_slice(&(controlee_v1.len() as u8).to_le_bytes());
885 for controlee in controlee_v1 {
886 controlees_buf.extend_from_slice(&write_controlee(&controlee));
887 }
888 }
889 Controlees::ShortSessionKey(controlee_v2)
890 if action == UpdateMulticastListAction::AddControleeWithShortSubSessionKey =>
891 {
892 controlees_buf.extend_from_slice(&(controlee_v2.len() as u8).to_le_bytes());
893 for controlee in controlee_v2 {
894 controlees_buf.extend_from_slice(&write_controlee_2_0_16byte(&controlee));
895 }
896 }
897 Controlees::LongSessionKey(controlee_v2)
898 if action == UpdateMulticastListAction::AddControleeWithLongSubSessionKey =>
899 {
900 controlees_buf.extend_from_slice(&(controlee_v2.len() as u8).to_le_bytes());
901 for controlee in controlee_v2 {
902 controlees_buf.extend_from_slice(&write_controlee_2_0_32byte(&controlee));
903 }
904 }
905 _ => return Err(DecodeError::InvalidPacketError),
906 }
907 Ok(SessionUpdateControllerMulticastListCmdBuilder {
908 session_token,
909 action,
910 payload: Some(controlees_buf.freeze()),
911 }
912 .build())
913 }
914
915 /// building Data transfer phase config command
build_data_transfer_phase_config_cmd( session_token: u32, dtpcm_repetition: u8, data_transfer_control: u8, dtpml_size: u8, mac_address: Vec<u8>, slot_bitmap: Vec<u8>, ) -> Result<SessionDataTransferPhaseConfigCmd, DecodeError>916 pub fn build_data_transfer_phase_config_cmd(
917 session_token: u32,
918 dtpcm_repetition: u8,
919 data_transfer_control: u8,
920 dtpml_size: u8,
921 mac_address: Vec<u8>,
922 slot_bitmap: Vec<u8>,
923 ) -> Result<SessionDataTransferPhaseConfigCmd, DecodeError> {
924 let mut dtpml_buffer = BytesMut::new();
925
926 //calculate mac address mode from data transfer control
927 let mac_address_mode = data_transfer_control & 0x01;
928
929 // Calculate mac address size based on address mode
930 let mac_address_size = match mac_address_mode {
931 SHORT_ADDRESS => 2,
932 EXTENDED_ADDRESS => 8,
933 _ => return Err(DecodeError::InvalidPacketError),
934 };
935
936 // Calculate slot bitmap size from data transfer control
937 let slot_bitmap_size = 1 << ((data_transfer_control & 0x0F) >> 1);
938
939 // Prepare segmented vectors for mac_address
940 let mac_address_vec: Vec<_> =
941 mac_address.chunks(mac_address_size).map(|chunk| chunk.to_owned()).collect();
942
943 // Prepare segmented vectors for slot_bitmap
944 let slot_bitmap_vec: Vec<_> =
945 slot_bitmap.chunks(slot_bitmap_size).map(|chunk| chunk.to_owned()).collect();
946
947 // Validate sizes of mac_address and slot_bitmap
948 if slot_bitmap_vec.len() != dtpml_size.into() || mac_address_vec.len() != dtpml_size.into() {
949 return Err(DecodeError::InvalidPacketError);
950 }
951
952 // Combine segmented vectors into dtpml_buffer
953 for (elem1, elem2) in mac_address_vec.into_iter().zip(slot_bitmap_vec.into_iter()) {
954 dtpml_buffer.extend_from_slice(&elem1);
955 dtpml_buffer.extend_from_slice(&elem2);
956 }
957
958 Ok(SessionDataTransferPhaseConfigCmdBuilder {
959 session_token,
960 dtpcm_repetition,
961 data_transfer_control,
962 dtpml_size,
963 payload: Some(dtpml_buffer.freeze()),
964 }
965 .build())
966 }
967
968 impl Drop for AppConfigTlv {
drop(&mut self)969 fn drop(&mut self) {
970 if self.cfg_id == AppConfigTlvType::VendorId || self.cfg_id == AppConfigTlvType::StaticStsIv
971 {
972 self.v.zeroize();
973 }
974 }
975 }
976
977 #[derive(Debug, Clone, PartialEq)]
978 pub enum PhaseList {
979 ShortMacAddress(Vec<PhaseListShortMacAddress>),
980 ExtendedMacAddress(Vec<PhaseListExtendedMacAddress>),
981 }
982
983 /// Generate the SessionSetHybridControllerConfig packet.
build_session_set_hybrid_controller_config_cmd( session_token: u32, message_control: u8, number_of_phases: u8, update_time: [u8; 8], phase_list: PhaseList, ) -> Result<SessionSetHybridControllerConfigCmd, DecodeError>984 pub fn build_session_set_hybrid_controller_config_cmd(
985 session_token: u32,
986 message_control: u8,
987 number_of_phases: u8,
988 update_time: [u8; 8],
989 phase_list: PhaseList,
990 ) -> Result<SessionSetHybridControllerConfigCmd, DecodeError> {
991 let mut phase_list_buffer = BytesMut::new();
992 match phase_list {
993 PhaseList::ShortMacAddress(phaseListShortMacAddressVec) => {
994 for phaseListShortMacAddress in phaseListShortMacAddressVec {
995 phase_list_buffer.extend_from_slice(
996 &(phaseListShortMacAddress.session_token.to_le_bytes()[0..4]),
997 );
998 phase_list_buffer.extend_from_slice(
999 &(phaseListShortMacAddress.start_slot_index.to_le_bytes()[0..2]),
1000 );
1001 phase_list_buffer.extend_from_slice(
1002 &(phaseListShortMacAddress.end_slot_index.to_le_bytes()[0..2]),
1003 );
1004 phase_list_buffer.extend_from_slice(std::slice::from_ref(
1005 &phaseListShortMacAddress.phase_participation,
1006 ));
1007 phase_list_buffer.extend_from_slice(&phaseListShortMacAddress.mac_address);
1008 }
1009 }
1010 PhaseList::ExtendedMacAddress(phaseListExtendedMacAddressVec) => {
1011 for phaseListExtendedMacAddress in phaseListExtendedMacAddressVec {
1012 phase_list_buffer.extend_from_slice(
1013 &(phaseListExtendedMacAddress.session_token.to_le_bytes()[0..4]),
1014 );
1015 phase_list_buffer.extend_from_slice(
1016 &(phaseListExtendedMacAddress.start_slot_index.to_le_bytes()[0..2]),
1017 );
1018 phase_list_buffer.extend_from_slice(
1019 &(phaseListExtendedMacAddress.end_slot_index.to_le_bytes()[0..2]),
1020 );
1021 phase_list_buffer.extend_from_slice(std::slice::from_ref(
1022 &phaseListExtendedMacAddress.phase_participation,
1023 ));
1024 phase_list_buffer.extend_from_slice(&phaseListExtendedMacAddress.mac_address);
1025 }
1026 }
1027 _ => return Err(DecodeError::InvalidPacketError),
1028 }
1029 Ok(SessionSetHybridControllerConfigCmdBuilder {
1030 session_token,
1031 message_control,
1032 number_of_phases,
1033 update_time,
1034 payload: Some(phase_list_buffer.freeze()),
1035 }
1036 .build())
1037 }
1038
1039 // Radar data 'bits per sample' field isn't a raw value, instead it's an enum
1040 // that maps to the raw value. We need this mapping to get the max sample size
1041 // length.
radar_bytes_per_sample_value(bps: BitsPerSample) -> u81042 pub fn radar_bytes_per_sample_value(bps: BitsPerSample) -> u8 {
1043 match bps {
1044 BitsPerSample::Value32 => 4,
1045 BitsPerSample::Value48 => 6,
1046 BitsPerSample::Value64 => 8,
1047 }
1048 }
1049
1050 #[cfg(test)]
1051 mod tests {
1052 use super::*;
1053
1054 #[test]
test_parse_diagnostics_ntf()1055 fn test_parse_diagnostics_ntf() {
1056 let rssi_vec = vec![0x01, 0x02, 0x03];
1057 let rssi = RssiBuilder { rssi: rssi_vec.clone() }.build();
1058 let aoa_1 = AoaMeasurement { tdoa: 1, pdoa: 2, aoa: 3, fom: 4, t: 1 };
1059 let aoa_2 = AoaMeasurement { tdoa: 5, pdoa: 6, aoa: 7, fom: 8, t: 2 };
1060 let aoa = AoaBuilder { aoa: vec![aoa_1.clone(), aoa_2.clone()] }.build();
1061 let cir_vec = vec![CirValue {
1062 first_path_index: 1,
1063 first_path_snr: 2,
1064 first_path_ns: 3,
1065 peak_path_index: 4,
1066 peak_path_snr: 5,
1067 peak_path_ns: 6,
1068 first_path_sample_offset: 7,
1069 samples_number: 2,
1070 sample_window: vec![0, 1, 2, 3],
1071 }];
1072 let cir = CirBuilder { cir_value: cir_vec.clone() }.build();
1073 let segment_metrics_vec = vec![SegmentMetricsValue {
1074 receiver_and_segment: ReceiverAndSegmentValue::parse(&[1]).unwrap(),
1075 rf_noise_floor: 2,
1076 segment_rsl: 3,
1077 first_path: PathSample { index: 4, rsl: 5, time_ns: 6 },
1078 peak_path: PathSample { index: 7, rsl: 8, time_ns: 9 },
1079 }];
1080 let segment_metrics =
1081 SegmentMetricsBuilder { segment_metrics: segment_metrics_vec.clone() }.build();
1082 let mut frame_reports = Vec::new();
1083 let tlvs = vec![
1084 FrameReportTlv { t: rssi.get_t(), v: rssi.get_rssi().to_vec() },
1085 FrameReportTlv { t: aoa.get_t(), v: aoa.encode_to_vec().unwrap()[3..].to_vec() },
1086 FrameReportTlv { t: cir.get_t(), v: cir.encode_to_vec().unwrap()[3..].to_vec() },
1087 FrameReportTlv {
1088 t: segment_metrics.get_t(),
1089 v: segment_metrics.encode_to_vec().unwrap()[3..].to_vec(),
1090 },
1091 ];
1092 let frame_report =
1093 FrameReport { uwb_msg_id: 1, action: 1, antenna_set: 1, frame_report_tlvs: tlvs };
1094 frame_reports.push(frame_report);
1095 let packet = AndroidRangeDiagnosticsNtfBuilder {
1096 session_token: 1,
1097 sequence_number: 1,
1098 frame_reports,
1099 }
1100 .build();
1101 let mut parsed_packet = parse_diagnostics_ntf(packet).unwrap();
1102 let parsed_frame_report = parsed_packet.frame_reports.pop().unwrap();
1103 assert_eq!(rssi_vec, parsed_frame_report.rssi);
1104 assert_eq!(aoa_1, parsed_frame_report.aoa[0]);
1105 assert_eq!(aoa_2, parsed_frame_report.aoa[1]);
1106 assert_eq!(cir_vec, parsed_frame_report.cir);
1107 assert_eq!(segment_metrics_vec, parsed_frame_report.segment_metrics);
1108 }
1109
1110 #[test]
test_write_controlee()1111 fn test_write_controlee() {
1112 let short_address: [u8; 2] = [2, 3];
1113 let controlee: Controlee = Controlee { short_address, subsession_id: 3 };
1114 let bytes = write_controlee(&controlee);
1115 let parsed_controlee = Controlee::parse(&bytes).unwrap();
1116 assert_eq!(controlee, parsed_controlee);
1117 }
1118
1119 #[test]
test_build_multicast_update_packet()1120 fn test_build_multicast_update_packet() {
1121 let short_address: [u8; 2] = [0x12, 0x34];
1122 let controlee = Controlee { short_address, subsession_id: 0x1324_3546 };
1123 let packet: UciControlPacket = build_session_update_controller_multicast_list_cmd(
1124 0x1425_3647,
1125 UpdateMulticastListAction::AddControlee,
1126 Controlees::NoSessionKey(vec![controlee; 1]),
1127 )
1128 .unwrap()
1129 .into();
1130 let packet_fragments: Vec<UciControlPacketHal> = packet.into();
1131 let uci_packet = packet_fragments[0].encode_to_vec();
1132 assert_eq!(
1133 uci_packet,
1134 Ok(vec![
1135 0x21, 0x07, 0x00, 0x0c, // 2(packet info), RFU, payload length(12)
1136 0x47, 0x36, 0x25, 0x14, // 4(session id (LE))
1137 0x00, 0x01, 0x12, 0x34, // action, # controlee, 2(short address (LE))
1138 0x46, 0x35, 0x24, 0x13, // 4(subsession id (LE))
1139 ])
1140 );
1141 }
1142
1143 #[test]
test_build_multicast_update_packet_v2_short_session_key()1144 fn test_build_multicast_update_packet_v2_short_session_key() {
1145 let short_address: [u8; 2] = [0x12, 0x34];
1146 let controlee = Controlee_V2_0_16_Byte_Version {
1147 short_address,
1148 subsession_id: 0x1324_3546,
1149 subsession_key: [
1150 0x12, 0x34, 0x56, 0x78, 0x90, 0xab, 0xcd, 0xef, 0x12, 0x34, 0x56, 0x78, 0x90, 0xab,
1151 0xcd, 0xef,
1152 ],
1153 };
1154 let packet: UciControlPacket = build_session_update_controller_multicast_list_cmd(
1155 0x1425_3647,
1156 UpdateMulticastListAction::AddControleeWithShortSubSessionKey,
1157 Controlees::ShortSessionKey(vec![controlee; 1]),
1158 )
1159 .unwrap()
1160 .into();
1161 let packet_fragments: Vec<UciControlPacketHal> = packet.into();
1162 let uci_packet = packet_fragments[0].encode_to_vec();
1163 assert_eq!(
1164 uci_packet,
1165 Ok(vec![
1166 0x21, 0x07, 0x00, 0x1c, // 2(packet info), RFU, payload length(28)
1167 0x47, 0x36, 0x25, 0x14, // 4(session id (LE))
1168 0x02, 0x01, 0x12, 0x34, // action, # controlee, 2(short address (LE))
1169 0x46, 0x35, 0x24, 0x13, // 4(subsession id (LE))
1170 0x12, 0x34, 0x56, 0x78, 0x90, 0xab, 0xcd, 0xef, 0x12, 0x34, 0x56, 0x78, 0x90, 0xab,
1171 0xcd, 0xef, // 16(subsession key(LE))
1172 ])
1173 );
1174 }
1175
1176 #[test]
test_build_multicast_update_packet_v2_long_session_key()1177 fn test_build_multicast_update_packet_v2_long_session_key() {
1178 let short_address: [u8; 2] = [0x12, 0x34];
1179 let controlee = Controlee_V2_0_32_Byte_Version {
1180 short_address,
1181 subsession_id: 0x1324_3546,
1182 subsession_key: [
1183 0x12, 0x34, 0x56, 0x78, 0x90, 0xab, 0xcd, 0xef, 0x12, 0x34, 0x56, 0x78, 0x90, 0xab,
1184 0xcd, 0xef, 0x12, 0x34, 0x56, 0x78, 0x90, 0xab, 0xcd, 0xef, 0x12, 0x34, 0x56, 0x78,
1185 0x90, 0xab, 0xcd, 0xef,
1186 ],
1187 };
1188 let packet: UciControlPacket = build_session_update_controller_multicast_list_cmd(
1189 0x1425_3647,
1190 UpdateMulticastListAction::AddControleeWithLongSubSessionKey,
1191 Controlees::LongSessionKey(vec![controlee; 1]),
1192 )
1193 .unwrap()
1194 .into();
1195 let packet_fragments: Vec<UciControlPacketHal> = packet.into();
1196 let uci_packet = packet_fragments[0].encode_to_vec();
1197 assert_eq!(
1198 uci_packet,
1199 Ok(vec![
1200 0x21, 0x07, 0x00, 0x2c, // 2(packet info), RFU, payload length(44)
1201 0x47, 0x36, 0x25, 0x14, // 4(session id (LE))
1202 0x03, 0x01, 0x12, 0x34, // action, # controlee, 2(short address (LE))
1203 0x46, 0x35, 0x24, 0x13, // 4(subsession id (LE))
1204 0x12, 0x34, 0x56, 0x78, 0x90, 0xab, 0xcd, 0xef, 0x12, 0x34, 0x56, 0x78, 0x90, 0xab,
1205 0xcd, 0xef, 0x12, 0x34, 0x56, 0x78, 0x90, 0xab, 0xcd, 0xef, 0x12, 0x34, 0x56, 0x78,
1206 0x90, 0xab, 0xcd, 0xef, // 32(subsession key(LE))
1207 ])
1208 );
1209 }
1210
1211 #[test]
test_to_raw_payload()1212 fn test_to_raw_payload() {
1213 let payload = vec![0x11, 0x22, 0x33];
1214 let payload_clone = payload.clone();
1215 let packet = UciControlPacketBuilder {
1216 group_id: GroupId::Test,
1217 message_type: MessageType::Response,
1218 opcode: 0x5,
1219 payload: Some(payload_clone.into()),
1220 }
1221 .build();
1222
1223 assert_eq!(payload, packet.to_raw_payload());
1224 }
1225
1226 #[test]
test_to_raw_payload_empty()1227 fn test_to_raw_payload_empty() {
1228 let payload: Vec<u8> = vec![];
1229 let packet = UciControlPacketBuilder {
1230 group_id: GroupId::Test,
1231 message_type: MessageType::Response,
1232 opcode: 0x5,
1233 payload: None,
1234 }
1235 .build();
1236
1237 assert_eq!(payload, packet.to_raw_payload());
1238 }
1239
1240 #[cfg(test)]
1241 mod tests {
1242 use crate::{extract_u16, extract_u32, extract_u64, extract_u8, extract_vec};
1243 #[test]
test_extract_func()1244 fn test_extract_func() {
1245 let bytes = [0x1, 0x3, 0x5, 0x7, 0x9, 0x2, 0x4, 0x05, 0x07, 0x09, 0x0a];
1246 let mut ptr = 0;
1247
1248 let u8_val = extract_u8(&bytes, &mut ptr, 1);
1249 assert_eq!(u8_val, Some(0x1));
1250 assert_eq!(ptr, 1);
1251
1252 let u16_val = extract_u16(&bytes, &mut ptr, 2);
1253 assert_eq!(u16_val, Some(0x0503));
1254 assert_eq!(ptr, 3);
1255
1256 let u32_val = extract_u32(&bytes, &mut ptr, 3);
1257 assert_eq!(u32_val, Some(0x020907));
1258 assert_eq!(ptr, 6);
1259
1260 let u64_val = extract_u64(&bytes, &mut ptr, 5);
1261 assert_eq!(u64_val, Some(0x0a09070504));
1262 assert_eq!(ptr, 11);
1263
1264 let vec = extract_vec(&bytes, &mut ptr, 3);
1265 assert_eq!(vec, None);
1266 assert_eq!(ptr, 11);
1267 }
1268 }
1269
1270 #[test]
test_short_dltdoa_ranging_measurement()1271 fn test_short_dltdoa_ranging_measurement() {
1272 let bytes = [
1273 // All Fields in Little Endian (LE)
1274 // First measurement
1275 0x0a, 0x01, 0x33, 0x05, // 2(Mac address), Status, Message Type
1276 0x53, 0x05, 0x02, 0x05, // 2(Message control), 2(Block Index)
1277 0x07, 0x09, 0x0a, 0x01, // Round Index, NLoS, 2(AoA Azimuth)
1278 0x02, 0x05, 0x07, 0x09, // AoA Azimuth FOM, 2(AoA Elevation), AoA Elevation FOM
1279 0x0a, 0x01, 0x02, 0x05, // RSSI, 3(Tx Timestamp..)
1280 0x07, 0x09, 0x0a, 0x01, // 4(Tx Timestamp..)
1281 0x02, 0x05, 0x07, 0x09, // Tx Timestamp, 3(Rx Timestamp..)
1282 0x05, 0x07, 0x09, 0x0a, // 2(Rx Timestamp), 2(Anchor Cfo)
1283 0x01, 0x02, 0x05, 0x07, // 2(Cfo), 2(Initiator Reply Time..)
1284 0x09, 0x05, 0x07, 0x09, // 2(Initiator Reply Time), 2(Responder Reply Time..)
1285 0x0a, 0x01, 0x02, 0x05, // 2(Responder Reply Time), 2(Initiator-Responder ToF)
1286 0x07, 0x09, 0x07, 0x09, // 4(Anchor Location..)
1287 0x05, 0x07, 0x09, 0x0a, // 4(Anchor Location..)
1288 0x01, 0x02, 0x05, 0x07, // 2(Anchor Location..), 2(Active Ranging Rounds..)
1289 0x09, 0x0a, 0x01, 0x02, // 4(Active Ranging Rounds..)
1290 0x05, 0x07, 0x09, 0x05, // 4(Active Ranging Rounds)
1291 // Second measurement
1292 0x0a, 0x01, 0x33, 0x05, // 2(Mac address), Status, Message Type
1293 0x33, 0x05, 0x02, 0x05, // 2(Message control), 2(Block Index)
1294 0x07, 0x09, 0x0a, 0x01, // Round Index, NLoS, 2(AoA Azimuth)
1295 0x02, 0x05, 0x07, 0x09, // AoA Azimuth FOM, 2(AoA Elevation), AoA Elevation FOM
1296 0x0a, 0x01, 0x02, 0x05, // RSSI, 3(Tx Timestamp..)
1297 0x07, 0x09, 0x0a, 0x01, // 4(Tx Timestamp..)
1298 0x02, 0x05, 0x07, 0x09, // Tx Timestamp, 3(Rx Timestamp..)
1299 0x05, 0x07, 0x09, 0x0a, // 2(Rx Timestamp), 2(Anchor Cfo)
1300 0x01, 0x02, 0x05, 0x07, // 2(Cfo), 2(Initiator Reply Time..)
1301 0x09, 0x05, 0x07, 0x09, // 2(Initiator Reply Time), 2(Responder Reply Time..)
1302 0x0a, 0x01, 0x02, 0x05, // 2(Responder Reply Time), 2(Initiator-Responder ToF)
1303 0x07, 0x09, 0x07, 0x09, // 4(Anchor Location..)
1304 0x05, 0x07, 0x09, 0x0a, // 4(Anchor Location..)
1305 0x01, 0x02, 0x01, 0x02, // 4(Anchor Location)
1306 0x05, 0x07, 0x09, 0x0a, // 4(Active Ranging Rounds..)
1307 0x01, 0x02, 0x05, 0x07, // 4(Active Ranging Rounds..)
1308 0x09, 0x05, // 2(Active Ranging Rounds)
1309 ];
1310
1311 let measurements = ShortAddressDlTdoaRangingMeasurement::parse(&bytes, 2).unwrap();
1312 assert_eq!(measurements.len(), 2);
1313 let measurement_1 = &measurements[0].measurement;
1314 let mac_address_1 = &measurements[0].mac_address;
1315 assert_eq!(*mac_address_1, 0x010a);
1316 assert_eq!(measurement_1.status, 0x33);
1317 assert_eq!(measurement_1.message_type, 0x05);
1318 assert_eq!(measurement_1.message_control, 0x0553);
1319 assert_eq!(measurement_1.block_index, 0x0502);
1320 assert_eq!(measurement_1.round_index, 0x07);
1321 assert_eq!(measurement_1.nlos, 0x09);
1322 assert_eq!(measurement_1.aoa_azimuth, 0x010a);
1323 assert_eq!(measurement_1.aoa_azimuth_fom, 0x02);
1324 assert_eq!(measurement_1.aoa_elevation, 0x0705);
1325 assert_eq!(measurement_1.aoa_elevation_fom, 0x09);
1326 assert_eq!(measurement_1.rssi, 0x0a);
1327 assert_eq!(measurement_1.tx_timestamp, 0x02010a0907050201);
1328 assert_eq!(measurement_1.rx_timestamp, 0x0705090705);
1329 assert_eq!(measurement_1.anchor_cfo, 0x0a09);
1330 assert_eq!(measurement_1.cfo, 0x0201);
1331 assert_eq!(measurement_1.initiator_reply_time, 0x05090705);
1332 assert_eq!(measurement_1.responder_reply_time, 0x010a0907);
1333 assert_eq!(measurement_1.initiator_responder_tof, 0x0502);
1334 assert_eq!(
1335 measurement_1.dt_anchor_location,
1336 vec![0x07, 0x09, 0x07, 0x09, 0x05, 0x07, 0x09, 0x0a, 0x01, 0x02]
1337 );
1338 assert_eq!(
1339 measurement_1.ranging_rounds,
1340 vec![0x05, 0x07, 0x09, 0x0a, 0x01, 0x02, 0x05, 0x07, 0x09, 0x05,]
1341 );
1342
1343 let measurement_2 = &measurements[1].measurement;
1344 let mac_address_2 = &measurements[1].mac_address;
1345 assert_eq!(*mac_address_2, 0x010a);
1346 assert_eq!(measurement_2.status, 0x33);
1347 assert_eq!(measurement_2.message_type, 0x05);
1348 assert_eq!(measurement_2.message_control, 0x0533);
1349 assert_eq!(measurement_2.block_index, 0x0502);
1350 assert_eq!(measurement_2.round_index, 0x07);
1351 assert_eq!(measurement_2.nlos, 0x09);
1352 assert_eq!(measurement_2.aoa_azimuth, 0x010a);
1353 assert_eq!(measurement_2.aoa_azimuth_fom, 0x02);
1354 assert_eq!(measurement_2.aoa_elevation, 0x0705);
1355 assert_eq!(measurement_2.aoa_elevation_fom, 0x09);
1356 assert_eq!(measurement_2.rssi, 0x0a);
1357 assert_eq!(measurement_2.tx_timestamp, 0x02010a0907050201);
1358 assert_eq!(measurement_2.rx_timestamp, 0x0705090705);
1359 assert_eq!(measurement_2.anchor_cfo, 0x0a09);
1360 assert_eq!(measurement_2.cfo, 0x0201);
1361 assert_eq!(measurement_2.initiator_reply_time, 0x05090705);
1362 assert_eq!(measurement_2.responder_reply_time, 0x010a0907);
1363 assert_eq!(measurement_2.initiator_responder_tof, 0x0502);
1364 assert_eq!(
1365 measurement_2.dt_anchor_location,
1366 vec![0x07, 0x09, 0x07, 0x09, 0x05, 0x07, 0x09, 0x0a, 0x01, 0x02, 0x01, 0x02]
1367 );
1368 assert_eq!(
1369 measurement_2.ranging_rounds,
1370 vec![0x05, 0x07, 0x09, 0x0a, 0x01, 0x02, 0x05, 0x07, 0x09, 0x05,]
1371 );
1372 }
1373
1374 #[test]
test_extended_dltdoa_ranging_measurement()1375 fn test_extended_dltdoa_ranging_measurement() {
1376 let bytes = [
1377 // All Fields in Little Endian (LE)
1378 /* First measurement */
1379 0x0a, 0x01, 0x33, 0x05, // 4(Mac address..)
1380 0x33, 0x05, 0x02, 0x05, // 4(Mac address)
1381 0x07, 0x09, 0x0a, 0x01, // Status, Message Type, 2(Message control),
1382 0x02, 0x05, 0x07, 0x09, // 2(Block Index), Round Index, NLoS,
1383 0x0a, 0x01, 0x02, 0x05, // 2(AoA Azimuth), AoA Azimuth FOM, 1(AoA Elevation..)
1384 0x07, 0x09, 0x0a, // 1(AoA Elevation), AoA Elevation FOM, RSSI,
1385 0x01, 0x02, 0x05, 0x07, // 4(Tx Timestamp..)
1386 0x09, 0x05, 0x07, 0x09, // 4(Tx Timestamp),
1387 0x0a, 0x01, 0x02, 0x05, // 4(Rx Timestamp..)
1388 0x07, 0x09, 0x05, 0x07, // 4(Rx Timestamp)
1389 0x09, 0x0a, 0x01, 0x02, // 2(Anchor Cfo), 2(Cfo),
1390 0x05, 0x07, 0x09, 0x05, // 4(Initiator Reply Time)
1391 0x07, 0x09, 0x0a, 0x01, // 4(Responder Reply Time),
1392 0x02, 0x05, 0x02, 0x05, // 2(Initiator-Responder ToF), 2(Active Ranging Rounds)
1393 ];
1394
1395 let measurements = ExtendedAddressDlTdoaRangingMeasurement::parse(&bytes, 1).unwrap();
1396 assert_eq!(measurements.len(), 1);
1397 let measurement = &measurements[0].measurement;
1398 let mac_address = &measurements[0].mac_address;
1399 assert_eq!(*mac_address, 0x050205330533010a);
1400 assert_eq!(measurement.message_control, 0x010a);
1401 assert_eq!(measurement.block_index, 0x0502);
1402 assert_eq!(measurement.round_index, 0x07);
1403 assert_eq!(measurement.nlos, 0x09);
1404 assert_eq!(measurement.aoa_azimuth, 0x010a);
1405 assert_eq!(measurement.aoa_azimuth_fom, 0x02);
1406 assert_eq!(measurement.aoa_elevation, 0x0705);
1407 assert_eq!(measurement.aoa_elevation_fom, 0x09);
1408 assert_eq!(measurement.rssi, 0x0a);
1409 assert_eq!(measurement.tx_timestamp, 0x0907050907050201);
1410 assert_eq!(measurement.rx_timestamp, 0x070509070502010a);
1411 assert_eq!(measurement.anchor_cfo, 0x0a09);
1412 assert_eq!(measurement.cfo, 0x0201);
1413 assert_eq!(measurement.initiator_reply_time, 0x05090705);
1414 assert_eq!(measurement.responder_reply_time, 0x010a0907);
1415 assert_eq!(measurement.initiator_responder_tof, 0x0502);
1416 assert_eq!(measurement.dt_anchor_location, vec![]);
1417 assert_eq!(measurement.ranging_rounds, vec![0x02, 0x05]);
1418 }
1419
1420 #[test]
test_build_data_transfer_phase_config_cmd()1421 fn test_build_data_transfer_phase_config_cmd() {
1422 let packet: UciControlPacket =
1423 build_data_transfer_phase_config_cmd(0x1234_5678, 0x0, 0x2, 1, vec![0, 1], vec![2, 3])
1424 .unwrap()
1425 .into();
1426 let packet_fragments: Vec<UciControlPacketHal> = packet.into();
1427 let uci_packet = packet_fragments[0].encode_to_vec();
1428 assert_eq!(
1429 uci_packet,
1430 Ok(vec![
1431 0x21, 0x0e, 0x00, 0x0b, // 2(packet info), RFU, payload length(11)
1432 0x78, 0x56, 0x34, 0x12, // 4(session id (LE))
1433 0x00, 0x02, 0x01, // dtpcm_repetition, data_transfer_control, dtpml_size
1434 0x00, 0x01, 0x02, 0x03, // payload
1435 ])
1436 );
1437 }
1438
1439 #[test]
test_build_session_set_hybrid_controller_config_cmd_short_address()1440 fn test_build_session_set_hybrid_controller_config_cmd_short_address() {
1441 let phase_list_short_mac_address = PhaseListShortMacAddress {
1442 session_token: 0x1324_3546,
1443 start_slot_index: 0x1111,
1444 end_slot_index: 0x1121,
1445 phase_participation: 0x0,
1446 mac_address: [0x1, 0x2],
1447 };
1448 let packet: UciControlPacket = build_session_set_hybrid_controller_config_cmd(
1449 0x1234_5678,
1450 0x0,
1451 0x0,
1452 [1; 8],
1453 PhaseList::ShortMacAddress(vec![phase_list_short_mac_address]),
1454 )
1455 .unwrap()
1456 .into();
1457 let packet_fragments: Vec<UciControlPacketHal> = packet.into();
1458 let uci_packet = packet_fragments[0].encode_to_vec();
1459 assert_eq!(
1460 uci_packet,
1461 Ok(vec![
1462 0x21, 0x0c, 0x00, 0x19, // 2(packet info), RFU, payload length(25)
1463 0x78, 0x56, 0x34, 0x12, // 4(session id (LE))
1464 0x00, 0x00, // message_control, number_of_phases
1465 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, // update_time
1466 0x46, 0x35, 0x24, 0x13, // session id (LE)
1467 0x11, 0x11, // start slot index (LE)
1468 0x21, 0x11, // end slot index (LE)
1469 0x00, // phase_participation
1470 0x01, 0x02, // mac address
1471 ])
1472 );
1473 }
1474
1475 #[test]
test_build_session_set_hybrid_controller_config_cmd_extended_address()1476 fn test_build_session_set_hybrid_controller_config_cmd_extended_address() {
1477 let phase_list_extended_mac_address = PhaseListExtendedMacAddress {
1478 session_token: 0x1324_3546,
1479 start_slot_index: 0x1111,
1480 end_slot_index: 0x1121,
1481 phase_participation: 0x0,
1482 mac_address: [0x1, 0x2, 0x3, 0x4, 0x5, 0x6, 0x7, 0x8],
1483 };
1484 let packet: UciControlPacket = build_session_set_hybrid_controller_config_cmd(
1485 0x1234_5678,
1486 0x0,
1487 0x0,
1488 [1; 8],
1489 PhaseList::ExtendedMacAddress(vec![phase_list_extended_mac_address]),
1490 )
1491 .unwrap()
1492 .into();
1493 let packet_fragments: Vec<UciControlPacketHal> = packet.into();
1494 let uci_packet = packet_fragments[0].encode_to_vec();
1495 assert_eq!(
1496 uci_packet,
1497 Ok(vec![
1498 0x21, 0x0c, 0x00, 0x1f, // 2(packet info), RFU, payload length(31)
1499 0x78, 0x56, 0x34, 0x12, // 4(session id (LE))
1500 0x00, 0x00, // message_control, number_of_phases
1501 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, 0x01, // update_time
1502 0x46, 0x35, 0x24, 0x13, // session id (LE)
1503 0x11, 0x11, // start slot index (LE)
1504 0x21, 0x11, // end slot index (LE)
1505 0x00, // phase_participation
1506 0x01, 0x02, 0x03, 0x04, 0x05, 0x06, 0x07, 0x08 // mac address
1507 ])
1508 );
1509 }
1510 }
1511