merge from upstream/master into main at 2023-01-23T02:14:43.997892Z

Change-Id: If6fa13ebc8a805ef08cfcec001cc2705f48885b0
diff --git a/src/include/uwb_types.h b/src/include/uwb_types.h
index 410f072..5e141b2 100755
--- a/src/include/uwb_types.h
+++ b/src/include/uwb_types.h
@@ -165,6 +165,14 @@
            ((((uint32_t)(*((p) + 3)))) << 24));                     \
     (p) += 4;                                                       \
   }
+#define STREAM_TO_UINT40(u40, p)                                    \
+  {                                                                 \
+    u40 = (((uint64_t)(*(p))) + ((((uint64_t)(*((p) + 1)))) << 8) + \
+           ((((uint64_t)(*((p) + 2)))) << 16) +                     \
+           ((((uint64_t)(*((p) + 3)))) << 24) +                     \
+           ((((uint64_t)(*((p) + 4)))) << 32));                     \
+    (p) += 5;                                                       \
+  }
 #define STREAM_TO_UINT64(u64, p)                                    \
   {                                                                 \
     u64 = (((uint64_t)(*(p))) + ((((uint64_t)(*((p) + 1)))) << 8) + \
diff --git a/src/rust/uwb_core/protos/uwb_service.proto b/src/rust/uwb_core/protos/uwb_service.proto
index 35e989e..c2f6eb7 100644
--- a/src/rust/uwb_core/protos/uwb_service.proto
+++ b/src/rust/uwb_core/protos/uwb_service.proto
@@ -414,6 +414,31 @@
   OwrAoaStatusCode owr_aoa_status_code = 17;
 }
 
+// Represent uwb_uci_packets::ShortAddressDlTdoaRangingMeasurement or
+// uwb_uci_packets::ExtendedAddressDlTdoaRangingMeasurement.
+message DlTDoARangingMeasurement {
+  uint64 mac_address = 1;
+  StatusCode status = 2;
+  uint32 message_control = 3;
+  uint32 block_index = 4;
+  uint32 round_index = 5;
+  uint32 nlos = 6;
+  uint32 aoa_azimuth = 7;
+  uint32 aoa_azimuth_fom = 8;
+  uint32 aoa_elevation = 9;
+  uint32 aoa_elevation_fom = 10;
+  uint32 rssi = 11;
+  uint64 tx_timestamp = 12;
+  uint64 rx_timestamp = 13;
+  uint32 anchor_cfo = 14;
+  uint32 cfo = 15;
+  uint32 initiator_reply_time = 16;
+  uint32 responder_reply_time = 17;
+  uint32 initiator_responder_tof = 18;
+  repeated uint32 dt_anchor_location = 19;
+  repeated uint32 ranging_rounds = 20;
+}
+
 // Represent uwb_core::uci::notification::SessionRangeData;
 message SessionRangeData {
   uint32 sequence_number = 1;
@@ -421,6 +446,7 @@
   uint32 current_ranging_interval_ms = 3;
   RangingMeasurementType ranging_measurement_type = 4;
   repeated RangingMeasurement ranging_measurements = 5;
+  repeated DlTDoARangingMeasurement dltdoa_ranging_measurements = 6;
 }
 
 // Represent uwb_uci_packets::PowerStats;
diff --git a/src/rust/uwb_core/src/proto/mappings.rs b/src/rust/uwb_core/src/proto/mappings.rs
index 1750ce8..7da6c3c 100644
--- a/src/rust/uwb_core/src/proto/mappings.rs
+++ b/src/rust/uwb_core/src/proto/mappings.rs
@@ -16,6 +16,7 @@
 
 use std::convert::{TryFrom, TryInto};
 
+use num_traits::FromPrimitive;
 use protobuf::RepeatedField;
 use zeroize::Zeroize;
 
@@ -28,22 +29,24 @@
     ScheduledMode, StsConfig, StsLength, TxAdaptivePayloadPower, UwbAddress, UwbChannel,
 };
 use crate::params::uci_packets::{
-    Controlee, DeviceState, ExtendedAddressOwrAoaRangingMeasurement,
-    ExtendedAddressTwoWayRangingMeasurement, OwrAoaStatusCode, PowerStats, RangingMeasurementType,
-    ReasonCode, SessionState, SessionType, ShortAddressOwrAoaRangingMeasurement,
+    Controlee, DeviceState, ExtendedAddressDlTdoaRangingMeasurement,
+    ExtendedAddressOwrAoaRangingMeasurement, ExtendedAddressTwoWayRangingMeasurement,
+    OwrAoaStatusCode, PowerStats, RangingMeasurementType, ReasonCode, SessionState, SessionType,
+    ShortAddressDlTdoaRangingMeasurement, ShortAddressOwrAoaRangingMeasurement,
     ShortAddressTwoWayRangingMeasurement, StatusCode, UpdateMulticastListAction,
 };
 use crate::params::AppConfigParams;
 use crate::proto::bindings::{
     AoaResultRequest as ProtoAoaResultRequest, BprfPhrDataRate as ProtoBprfPhrDataRate,
     Controlee as ProtoControlee, DeviceRole as ProtoDeviceRole, DeviceState as ProtoDeviceState,
-    DeviceType as ProtoDeviceType, FiraAppConfigParams as ProtoFiraAppConfigParams,
-    HoppingMode as ProtoHoppingMode, KeyRotation as ProtoKeyRotation,
-    MacAddressMode as ProtoMacAddressMode, MacFcsType as ProtoMacFcsType,
-    MultiNodeMode as ProtoMultiNodeMode, OwrAoaStatusCode as ProtoOwrAoaStatusCode,
-    PowerStats as ProtoPowerStats, PreambleDuration as ProtoPreambleDuration,
-    PrfMode as ProtoPrfMode, PsduDataRate as ProtoPsduDataRate,
-    RangeDataNtfConfig as ProtoRangeDataNtfConfig, RangingMeasurement as ProtoRangingMeasurement,
+    DeviceType as ProtoDeviceType, DlTDoARangingMeasurement as ProtoDlTDoARangingMeasurement,
+    FiraAppConfigParams as ProtoFiraAppConfigParams, HoppingMode as ProtoHoppingMode,
+    KeyRotation as ProtoKeyRotation, MacAddressMode as ProtoMacAddressMode,
+    MacFcsType as ProtoMacFcsType, MultiNodeMode as ProtoMultiNodeMode,
+    OwrAoaStatusCode as ProtoOwrAoaStatusCode, PowerStats as ProtoPowerStats,
+    PreambleDuration as ProtoPreambleDuration, PrfMode as ProtoPrfMode,
+    PsduDataRate as ProtoPsduDataRate, RangeDataNtfConfig as ProtoRangeDataNtfConfig,
+    RangingMeasurement as ProtoRangingMeasurement,
     RangingMeasurementType as ProtoRangingMeasurementType,
     RangingRoundControl as ProtoRangingRoundControl, RangingRoundUsage as ProtoRangingRoundUsage,
     RangingTimeStruct as ProtoRangingTimeStruct, ReasonCode as ProtoReasonCode,
@@ -354,6 +357,11 @@
     ADD_CONTROLEE_WITH_LONG_SUB_SESSION_KEY => AddControleeWithLongSubSessionKey,
 }
 
+pub enum ProtoRangingMeasurements {
+    Ranging(Vec<ProtoRangingMeasurement>),
+    DlTDoa(Vec<ProtoDlTDoARangingMeasurement>),
+}
+
 impl<T> From<Result<T>> for ProtoStatus {
     fn from(item: Result<T>) -> Self {
         match item {
@@ -445,6 +453,84 @@
     }
 }
 
+impl From<ShortAddressDlTdoaRangingMeasurement> for ProtoDlTDoARangingMeasurement {
+    fn from(item: ShortAddressDlTdoaRangingMeasurement) -> Self {
+        let mut result = Self::new();
+        result.set_mac_address(item.mac_address.into());
+        result.set_status(
+            StatusCode::from_u8(item.measurement.status)
+                .unwrap_or(StatusCode::UciStatusFailed)
+                .into(),
+        );
+        result.set_message_control(item.measurement.message_control.into());
+        result.set_block_index(item.measurement.block_index.into());
+        result.set_round_index(item.measurement.round_index.into());
+        result.set_nlos(item.measurement.nlos.into());
+        result.set_aoa_azimuth(item.measurement.aoa_azimuth.into());
+        result.set_aoa_azimuth_fom(item.measurement.aoa_azimuth_fom.into());
+        result.set_aoa_elevation(item.measurement.aoa_elevation.into());
+        result.set_aoa_elevation_fom(item.measurement.aoa_elevation_fom.into());
+        result.set_rssi(item.measurement.rssi.into());
+        result.set_tx_timestamp(item.measurement.tx_timestamp);
+        result.set_rx_timestamp(item.measurement.rx_timestamp);
+        result.set_anchor_cfo(item.measurement.anchor_cfo.into());
+        result.set_cfo(item.measurement.cfo.into());
+        result.set_initiator_reply_time(item.measurement.initiator_reply_time);
+        result.set_responder_reply_time(item.measurement.responder_reply_time);
+        result.set_initiator_responder_tof(item.measurement.initiator_responder_tof.into());
+        result.set_dt_anchor_location(
+            item.measurement
+                .dt_anchor_location
+                .into_iter()
+                .map(|val| val as u32)
+                .collect::<Vec<u32>>(),
+        );
+        result.set_ranging_rounds(
+            item.measurement.ranging_rounds.into_iter().map(|val| val as u32).collect::<Vec<u32>>(),
+        );
+        result
+    }
+}
+
+impl From<ExtendedAddressDlTdoaRangingMeasurement> for ProtoDlTDoARangingMeasurement {
+    fn from(item: ExtendedAddressDlTdoaRangingMeasurement) -> Self {
+        let mut result = Self::new();
+        result.set_mac_address(item.mac_address);
+        result.set_status(
+            StatusCode::from_u8(item.measurement.status)
+                .unwrap_or(StatusCode::UciStatusFailed)
+                .into(),
+        );
+        result.set_message_control(item.measurement.message_control.into());
+        result.set_block_index(item.measurement.block_index.into());
+        result.set_round_index(item.measurement.round_index.into());
+        result.set_nlos(item.measurement.nlos.into());
+        result.set_aoa_azimuth(item.measurement.aoa_azimuth.into());
+        result.set_aoa_azimuth_fom(item.measurement.aoa_azimuth_fom.into());
+        result.set_aoa_elevation(item.measurement.aoa_elevation.into());
+        result.set_aoa_elevation_fom(item.measurement.aoa_elevation_fom.into());
+        result.set_rssi(item.measurement.rssi.into());
+        result.set_tx_timestamp(item.measurement.tx_timestamp);
+        result.set_rx_timestamp(item.measurement.rx_timestamp);
+        result.set_anchor_cfo(item.measurement.anchor_cfo.into());
+        result.set_cfo(item.measurement.cfo.into());
+        result.set_initiator_reply_time(item.measurement.initiator_reply_time);
+        result.set_responder_reply_time(item.measurement.responder_reply_time);
+        result.set_initiator_responder_tof(item.measurement.initiator_responder_tof.into());
+        result.set_dt_anchor_location(
+            item.measurement
+                .dt_anchor_location
+                .into_iter()
+                .map(|val| val as u32)
+                .collect::<Vec<u32>>(),
+        );
+        result.set_ranging_rounds(
+            item.measurement.ranging_rounds.into_iter().map(|val| val as u32).collect::<Vec<u32>>(),
+        );
+        result
+    }
+}
+
 impl From<SessionRangeData> for ProtoSessionRangeData {
     fn from(item: SessionRangeData) -> Self {
         let mut result = Self::new();
@@ -452,29 +538,38 @@
         result.set_session_id(item.session_id);
         result.set_current_ranging_interval_ms(item.current_ranging_interval_ms);
         result.set_ranging_measurement_type(item.ranging_measurement_type.into());
-        result.set_ranging_measurements(RepeatedField::from_vec(to_proto_ranging_measurements(
-            item.ranging_measurements,
-        )));
+        match to_proto_ranging_measurements(item.ranging_measurements) {
+            ProtoRangingMeasurements::Ranging(ranging_measurements) => {
+                result.set_ranging_measurements(RepeatedField::from_vec(ranging_measurements))
+            }
+            ProtoRangingMeasurements::DlTDoa(dltdoa_measurements) => {
+                result.set_dltdoa_ranging_measurements(RepeatedField::from_vec(dltdoa_measurements))
+            }
+        }
         result
     }
 }
 
-fn to_proto_ranging_measurements(item: RangingMeasurements) -> Vec<ProtoRangingMeasurement> {
+fn to_proto_ranging_measurements(item: RangingMeasurements) -> ProtoRangingMeasurements {
     match item {
         RangingMeasurements::ShortAddressTwoWay(arr) => {
-            arr.into_iter().map(|item| item.into()).collect()
+            ProtoRangingMeasurements::Ranging(arr.into_iter().map(|item| item.into()).collect())
         }
         RangingMeasurements::ExtendedAddressTwoWay(arr) => {
-            arr.into_iter().map(|item| item.into()).collect()
+            ProtoRangingMeasurements::Ranging(arr.into_iter().map(|item| item.into()).collect())
         }
         RangingMeasurements::ShortAddressOwrAoa(arr) => {
-            arr.into_iter().map(|item| item.into()).collect()
+            ProtoRangingMeasurements::Ranging(arr.into_iter().map(|item| item.into()).collect())
         }
         RangingMeasurements::ExtendedAddressOwrAoa(arr) => {
-            arr.into_iter().map(|item| item.into()).collect()
+            ProtoRangingMeasurements::Ranging(arr.into_iter().map(|item| item.into()).collect())
         }
-        // TODO(b/260499366): Add support for DlTDoA.
-        _ => todo!(),
+        RangingMeasurements::ShortAddressDltdoa(arr) => {
+            ProtoRangingMeasurements::DlTDoa(arr.into_iter().map(|item| item.into()).collect())
+        }
+        RangingMeasurements::ExtendedAddressDltdoa(arr) => {
+            ProtoRangingMeasurements::DlTDoa(arr.into_iter().map(|item| item.into()).collect())
+        }
     }
 }
 
diff --git a/src/rust/uwb_core/src/uci/notification.rs b/src/rust/uwb_core/src/uci/notification.rs
index 95f70bf..2463cb4 100644
--- a/src/rust/uwb_core/src/uci/notification.rs
+++ b/src/rust/uwb_core/src/uci/notification.rs
@@ -109,10 +109,10 @@
     ExtendedAddressTwoWay(Vec<ExtendedAddressTwoWayRangingMeasurement>),
 
     /// Dl-TDoA measurement with short address.
-    ShortDltdoa(Vec<ShortAddressDlTdoaRangingMeasurement>),
+    ShortAddressDltdoa(Vec<ShortAddressDlTdoaRangingMeasurement>),
 
     /// Dl-TDoA measurement with extended address.
-    ExtendedDltdoa(Vec<ExtendedAddressDlTdoaRangingMeasurement>),
+    ExtendedAddressDltdoa(Vec<ExtendedAddressDlTdoaRangingMeasurement>),
 
     /// OWR for AoA measurement with short address.
     ShortAddressOwrAoa(Vec<ShortAddressOwrAoaRangingMeasurement>),
@@ -296,7 +296,7 @@
                 match ShortAddressDlTdoaRangingMeasurement::parse(&evt.clone().to_vec()) {
                     Some(v) => {
                         if v.len() == evt.get_no_of_ranging_measurements().into() {
-                            RangingMeasurements::ShortDltdoa(v)
+                            RangingMeasurements::ShortAddressDltdoa(v)
                         } else {
                             error!("Wrong count of ranging measurements {:?}", evt);
                             return Err(Error::BadParameters);
@@ -309,7 +309,7 @@
                 match ExtendedAddressDlTdoaRangingMeasurement::parse(&evt.clone().to_vec()) {
                     Some(v) => {
                         if v.len() == evt.get_no_of_ranging_measurements().into() {
-                            RangingMeasurements::ExtendedDltdoa(v)
+                            RangingMeasurements::ExtendedAddressDltdoa(v)
                         } else {
                             error!("Wrong count of ranging measurements {:?}", evt);
                             return Err(Error::BadParameters);
diff --git a/src/uci/include/uci_defs.h b/src/uci/include/uci_defs.h
index 7943771..8579e63 100755
--- a/src/uci/include/uci_defs.h
+++ b/src/uci/include/uci_defs.h
@@ -168,6 +168,9 @@
 #define UCI_MSG_SESSION_GET_COUNT 0x05
 #define UCI_MSG_SESSION_GET_STATE 0x06
 #define UCI_MSG_SESSION_UPDATE_CONTROLLER_MULTICAST_LIST 0x07
+#define UCI_MSG_SESSION_UPDATE_ACTIVE_ROUNDS_OF_DT_ANCHOR 0x08
+#define UCI_MSG_SESSION_UPDATE_ACTIVE_ROUNDS_OF_DT_TAG 0x09
+#define UCI_MSG_SESSION_CONFIGURE_DT_ANCHOR_RR_RDM_LIST 0x0A
 
 /* Pay load size for each command*/
 #define UCI_MSG_SESSION_INIT_CMD_SIZE 0x05
diff --git a/src/uwa/include/uwa_dm_int.h b/src/uwa/include/uwa_dm_int.h
index 077c45b..160501e 100755
--- a/src/uwa/include/uwa_dm_int.h
+++ b/src/uwa/include/uwa_dm_int.h
@@ -223,7 +223,6 @@
 typedef struct {
   UWB_HDR hdr;
   uint32_t session_id;
-  uint8_t addr_len;
   uint8_t p_addr[EXTENDED_ADDRESS_LEN];
   uint8_t dest_end_point;
   uint8_t sequence_num;
diff --git a/src/uwb/include/uwb_api.h b/src/uwb/include/uwb_api.h
index 145aa8d..e75e924 100755
--- a/src/uwb/include/uwb_api.h
+++ b/src/uwb/include/uwb_api.h
@@ -306,8 +306,9 @@
   uint8_t aoa_azimuth_FOM;
   uint16_t aoa_elevation;
   uint8_t aoa_elevation_FOM;
-  uint8_t txTimeStamp[8];
-  uint8_t rxTimeStamp[8];
+  uint8_t rssi;
+  uint64_t txTimeStamp;
+  uint64_t rxTimeStamp;
   uint16_t cfo_anchor;
   uint16_t cfo;
   uint32_t initiator_reply_time;
@@ -315,7 +316,7 @@
   uint16_t initiator_responder_TOF;
   uint8_t anchor_location[12];
   uint8_t active_ranging_round[15];
-} tUWB_DLTDOA_RANGING_MEASR
+} tUWB_DLTDOA_RANGING_MEASR;
 
 typedef struct {
   uint8_t mac_addr[8];
diff --git a/src/uwb/include/uwb_int.h b/src/uwb/include/uwb_int.h
index 1629d38..da2952e 100755
--- a/src/uwb/include/uwb_int.h
+++ b/src/uwb/include/uwb_int.h
@@ -193,7 +193,7 @@
                                        uint16_t len);
 
 /* APIs for handling data transfer */
-extern void uwb_ucif_send_data_frame(uint32_t session_id,uint8_t addr_len, uint8_t* p_addr,
+extern void uwb_ucif_send_data_frame(uint32_t session_id, uint8_t* p_addr,
                                            uint16_t data_len, uint8_t* p_data);
 extern void uwb_ucif_proc_data_credit_ntf(uint8_t* p_buf, uint16_t len);
 extern void uwb_ucif_proc_data_transfer_status_ntf(uint8_t* p_buf, uint16_t len);
diff --git a/src/uwb/uci/uci_hmsgs.cc b/src/uwb/uci/uci_hmsgs.cc
index 42b0934..8e57954 100755
--- a/src/uwb/uci/uci_hmsgs.cc
+++ b/src/uwb/uci/uci_hmsgs.cc
@@ -962,8 +962,8 @@
       UINT32_TO_STREAM(pp, session_id);
       ARRAY_TO_STREAM(pp, p_addr, EXTENDED_ADDRESS_LEN);
       UINT8_TO_STREAM(pp, dest_end_point);
-      UINT8_TO_STREAM(pp, (uint8_t)sequence_num);
-      uci_pkt_len -= (sizeof(session_id) + EXTENDED_ADDRESS_LEN  + sizeof(dest_end_point) +
+      UINT8_TO_STREAM(pp, sequence_num);
+      uci_pkt_len -= (sizeof(session_id) + EXTENDED_ADDRESS_LEN + sizeof(dest_end_point) +
                       sizeof(data_len) + sizeof(uint8_t));
     }
     if(!isFirstSegment){
diff --git a/src/uwb/uwb/uwb_main.cc b/src/uwb/uwb/uwb_main.cc
index 8ae0b25..b79de22 100755
--- a/src/uwb/uwb/uwb_main.cc
+++ b/src/uwb/uwb/uwb_main.cc
@@ -1015,7 +1015,7 @@
 tUWB_STATUS UWB_SendData(uint32_t session_id, uint8_t* p_addr,
                          uint8_t dest_end_point, uint8_t sequence_num,
                          uint16_t data_len, uint8_t* p_data) {
-  return uci_send_data_frame(session_id, addr_len, p_addr, dest_end_point, sequence_num, data_len, p_data);
+  return uci_send_data_frame(session_id, p_addr, dest_end_point, sequence_num, data_len, p_data);
 }
 
 /*******************************************************************************
diff --git a/src/uwb/uwb/uwb_ucif.cc b/src/uwb/uwb/uwb_ucif.cc
index f7a4303..04cbd77 100755
--- a/src/uwb/uwb/uwb_ucif.cc
+++ b/src/uwb/uwb/uwb_ucif.cc
@@ -35,10 +35,6 @@
 
 #define NORMAL_MODE_LENGTH_OFFSET 0x03
 #define DATA_PACKET_LEN_SHIFT 0x08
-#define EXTENDED_MODE_LEN_OFFSET 0x02
-#define EXTENDED_MODE_LEN_SHIFT 0x08
-#define EXTND_LEN_INDICATOR_OFFSET 0x01
-#define EXTND_LEN_INDICATOR_OFFSET_MASK  0x80
 #define TDOA_TX_TIMESTAMP_OFFSET         0x00FF
 #define TDOA_TX_TIMESTAMP_OFFSET_MASK    0x06
 #define TDOA_RX_TIMESTAMP_OFFSET         0x00FF
@@ -152,7 +148,6 @@
     uwb_cb.cmd_retry_count++;
   } else {
     uwb_ucif_event_status(UWB_UWBS_RESP_TIMEOUT_REVT, UWB_STATUS_FAILED);
-    uwb_ucif_uwb_recovery();
   }
 }
 
@@ -260,12 +255,9 @@
         uwb_cb.rawCmdCbflag = true;
       }
 
-      if(pbf) {
-        uwb_cb.rawCmdCbflag = false;
-      } else {
-        uwb_cb.uci_cmd_window--;
-      }
-      uwb_cb.is_resp_pending = !pbf;
+       /* Indicate command is pending */
+      uwb_cb.uci_cmd_window--;
+      uwb_cb.is_resp_pending = true;
       uwb_cb.cmd_retry_count = 0;
 
       /* send to HAL */
@@ -1127,10 +1119,6 @@
   uwb_cb.device_state = status;
 
   (*uwb_cb.p_resp_cback)(UWB_DEVICE_STATUS_REVT, &uwb_response);
-  if (status == UWBS_STATUS_ERROR || status == UWBS_STATUS_TIMEOUT) {
-    uwb_stop_quick_timer(&uwb_cb.uci_wait_rsp_timer);
-    uwb_ucif_uwb_recovery();
-  }
 }
 
 /*******************************************************************************
@@ -1291,20 +1279,21 @@
       STREAM_TO_UINT8(dltdoa_range_measr->aoa_azimuth_FOM, p);
       STREAM_TO_UINT16(dltdoa_range_measr->aoa_elevation, p);
       STREAM_TO_UINT8(dltdoa_range_measr->aoa_elevation_FOM, p);
+      STREAM_TO_UINT8(dltdoa_range_measr->rssi, p);
       txTimeStampValue = ((dltdoa_range_measr->message_control & TDOA_TX_TIMESTAMP_OFFSET ) & (TDOA_TX_TIMESTAMP_OFFSET_MASK));
       if(txTimeStampValue == TDOA_TX_TIMESTAMP_40BITS) {
-        STREAM_TO_ARRAY(&dltdoa_range_measr->txTimeStamp[0], p, TDOA_TIMESTAMP_LEN_40BITS);
+        STREAM_TO_UINT40(dltdoa_range_measr->txTimeStamp, p);
       } else if(txTimeStampValue == TDOA_TX_TIMESTAMP_64BITS) {
-        STREAM_TO_ARRAY(&dltdoa_range_measr->txTimeStamp[0], p, TDOA_TIMESTAMP_LEN_64BITS);
+        STREAM_TO_UINT64(dltdoa_range_measr->txTimeStamp, p);
       } else {
         UCI_TRACE_E("%s: Invalid txTimeStamp value", __func__);
         return;
       }
       rxTimeStampValue = ((dltdoa_range_measr->message_control & TDOA_RX_TIMESTAMP_OFFSET ) & (TDOA_RX_TIMESTAMP_OFFSET_MASK));
       if(rxTimeStampValue == TDOA_RX_TIMESTAMP_40BITS) {
-        STREAM_TO_ARRAY(&dltdoa_range_measr->rxTimeStamp[0], p, TDOA_TIMESTAMP_LEN_40BITS);
+        STREAM_TO_UINT40(dltdoa_range_measr->rxTimeStamp, p);
       } else if(rxTimeStampValue == TDOA_RX_TIMESTAMP_64BITS) {
-        STREAM_TO_ARRAY(&dltdoa_range_measr->rxTimeStamp[0], p, TDOA_TIMESTAMP_LEN_64BITS);
+        STREAM_TO_UINT64(dltdoa_range_measr->rxTimeStamp, p);
       } else {
         UCI_TRACE_E("%s: Invalid rxTimeStamp value", __func__);
         return;