WS: Calculate UFSI drift and trace (ARMmbed#2430)

Jarkko Paso · web-flow · commit 0a472aefb243 · 2020-08-28T10:28:59.000+03:00
diff --git a/source/6LoWPAN/ws/ws_bootstrap.c b/source/6LoWPAN/ws/ws_bootstrap.c
@@ -1475,7 +1475,7 @@ static void ws_bootstrap_pan_config_analyse(struct protocol_interface_info_entry
     if (neighbour_pointer_valid) {
         etx_lqi_dbm_update(cur->id, data->mpduLinkQuality, data->signal_dbm, neighbor_info.neighbor->index, neighbor_info.neighbor->mac64);
         //Update Neighbor Broadcast and Unicast Parameters
-        ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, ws_utt, data->timestamp);
+        ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, ws_utt, data->timestamp, (uint8_t *) data->SrcAddr);
         ws_neighbor_class_neighbor_unicast_schedule_set(neighbor_info.ws_neighbor, ws_us, &cur->ws_info->hopping_schdule);
         ws_neighbor_class_neighbor_broadcast_time_info_update(neighbor_info.ws_neighbor, &ws_bt_ie, data->timestamp);
         ws_neighbor_class_neighbor_broadcast_schedule_set(neighbor_info.ws_neighbor, &ws_bs_ie);
@@ -1559,7 +1559,7 @@ static void ws_bootstrap_pan_config_solicit_analyse(struct protocol_interface_in
     llc_neighbour_req_t neighbor_info;
     if (ws_bootstrap_neighbor_info_request(cur, data->SrcAddr, &neighbor_info, false)) {
         etx_lqi_dbm_update(cur->id, data->mpduLinkQuality, data->signal_dbm, neighbor_info.neighbor->index, neighbor_info.neighbor->mac64);
-        ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, ws_utt, data->timestamp);
+        ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, ws_utt, data->timestamp, (uint8_t *) data->SrcAddr);
         ws_neighbor_class_neighbor_unicast_schedule_set(neighbor_info.ws_neighbor, ws_us, &cur->ws_info->hopping_schdule);
     }
 
@@ -3268,7 +3268,7 @@ static int8_t ws_bootstrap_neighbor_set(protocol_interface_info_entry_t *cur, pa
         return -1;
     }
     ws_bootstrap_neighbor_set_stable(cur, parent_ptr->addr);
-    ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, &parent_ptr->ws_utt, parent_ptr->timestamp);
+    ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, &parent_ptr->ws_utt, parent_ptr->timestamp, parent_ptr->addr);
     ws_neighbor_class_neighbor_unicast_schedule_set(neighbor_info.ws_neighbor, &parent_ptr->ws_us, &cur->ws_info->hopping_schdule);
     return 0;
 }
diff --git a/source/6LoWPAN/ws/ws_llc_data_service.c b/source/6LoWPAN/ws/ws_llc_data_service.c
@@ -493,7 +493,7 @@ static void ws_llc_mac_confirm_cb(const mac_api_t *api, const mcps_data_conf_t *
                             neighbor_info.neighbor->lifetime = neighbor_info.neighbor->link_lifetime;
                         }
 
-                        ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, &ws_utt, data->timestamp);
+                        ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, &ws_utt, data->timestamp, neighbor_info.neighbor->mac64);
                     }
 
                     int8_t rsl;
@@ -702,7 +702,7 @@ static void ws_llc_data_indication_cb(const mac_api_t *api, const mcps_data_ind_
         return;
     }
 
-    ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, &ws_utt, data->timestamp);
+    ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, &ws_utt, data->timestamp, (uint8_t *) data->SrcAddr);
     if (us_ie_inline) {
         ws_neighbor_class_neighbor_unicast_schedule_set(neighbor_info.ws_neighbor, &us_ie, &interface->ws_info->hopping_schdule);
     }
@@ -808,7 +808,7 @@ static void ws_llc_eapol_indication_cb(const mac_api_t *api, const mcps_data_ind
         temp_entry->signal_dbm = data->signal_dbm;
     }
     uint8_t auth_eui64[8];
-    ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, &ws_utt, data->timestamp);
+    ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, &ws_utt, data->timestamp, (uint8_t *) data->SrcAddr);
     if (us_ie_inline) {
         ws_neighbor_class_neighbor_unicast_schedule_set(neighbor_info.ws_neighbor, &us_ie, &interface->ws_info->hopping_schdule);
     }
diff --git a/source/6LoWPAN/ws/ws_neighbor_class.c b/source/6LoWPAN/ws/ws_neighbor_class.c
@@ -89,8 +89,44 @@ void ws_neighbor_class_entry_remove(ws_neighbor_class_t *class_data, uint8_t att
     }
 }
 
-void ws_neighbor_class_neighbor_unicast_time_info_update(ws_neighbor_class_entry_t *ws_neighbor, ws_utt_ie_t *ws_utt, uint32_t timestamp)
+#ifdef FEA_TRACE_SUPPORT
+static int own_ceil(float value)
 {
+    int ivalue = (int)value;
+    if (value == (float)ivalue) {
+        return ivalue;
+    }
+    return ivalue + 1;
+}
+
+static void ws_neighbor_calculate_ufsi_drift(ws_neighbor_class_entry_t *ws_neighbor, ws_utt_ie_t *ws_utt, uint32_t timestamp, uint8_t address[8])
+{
+    if (ws_neighbor->fhss_data.uc_timing_info.utt_rx_timestamp && ws_neighbor->fhss_data.uc_timing_info.ufsi) {
+        uint32_t seq_length = 0x10000;
+        if (ws_neighbor->fhss_data.uc_timing_info.unicast_channel_function  == WS_TR51CF) {
+            seq_length = ws_neighbor->fhss_data.uc_timing_info.unicast_number_of_channels;
+        }
+        // Convert 24-bit UFSI to real time before drift calculation
+        uint32_t time_since_seq_start_prev_ms = own_ceil((float)((uint64_t)ws_neighbor->fhss_data.uc_timing_info.ufsi * seq_length * ws_neighbor->fhss_data.uc_timing_info.unicast_dwell_interval) / 0x1000000);
+        uint32_t time_since_seq_start_cur_ms = own_ceil((float)((uint64_t)ws_utt->ufsi * seq_length * ws_neighbor->fhss_data.uc_timing_info.unicast_dwell_interval) / 0x1000000);
+        uint32_t time_since_last_ufsi_us = timestamp - ws_neighbor->fhss_data.uc_timing_info.utt_rx_timestamp;
+        uint32_t ufsi_diff_ms = time_since_seq_start_cur_ms - time_since_seq_start_prev_ms;
+        int32_t ufsi_drift_ms = (int32_t)(time_since_last_ufsi_us / 1000 - ufsi_diff_ms);
+        // Only trace if there is significant error
+        if (ufsi_drift_ms < -5 || ufsi_drift_ms > 5) {
+            tr_debug("UFSI updated: %s, drift: %"PRIi32"ms in %"PRIu32" seconds", trace_array(address, 8), ufsi_drift_ms, time_since_last_ufsi_us / 1000000);
+        }
+    }
+}
+#endif
+
+void ws_neighbor_class_neighbor_unicast_time_info_update(ws_neighbor_class_entry_t *ws_neighbor, ws_utt_ie_t *ws_utt, uint32_t timestamp, uint8_t address[8])
+{
+#ifdef FEA_TRACE_SUPPORT
+    ws_neighbor_calculate_ufsi_drift(ws_neighbor, ws_utt, timestamp, address);
+#else
+    (void) address;
+#endif
     ws_neighbor->fhss_data.uc_timing_info.utt_rx_timestamp = timestamp;
     ws_neighbor->fhss_data.uc_timing_info.ufsi = ws_utt->ufsi;
 }
diff --git a/source/6LoWPAN/ws/ws_neighbor_class.h b/source/6LoWPAN/ws/ws_neighbor_class.h
@@ -106,7 +106,7 @@ void ws_neighbor_class_entry_remove(ws_neighbor_class_t *class_data, uint8_t att
  * \param timestamp timestamp for received data
  *
  */
-void ws_neighbor_class_neighbor_unicast_time_info_update(ws_neighbor_class_entry_t *ws_neighbor, ws_utt_ie_t *ws_utt, uint32_t timestamp);
+void ws_neighbor_class_neighbor_unicast_time_info_update(ws_neighbor_class_entry_t *ws_neighbor, ws_utt_ie_t *ws_utt, uint32_t timestamp, uint8_t address[8]);
 
 /**
  * ws_neighbor_class_neighbor_unicast_schedule_set a function for update neighbor unicast shedule information
diff --git a/test/nanostack/unittest/stub/ws_neighbour_class_stub.c b/test/nanostack/unittest/stub/ws_neighbour_class_stub.c
@@ -69,7 +69,7 @@ void ws_neighbor_class_entry_remove(ws_neighbor_class_t *class_data, uint8_t att
 
 }
 
-void ws_neighbor_class_neighbor_unicast_time_info_update(ws_neighbor_class_entry_t *ws_neighbor, ws_utt_ie_t *ws_utt, uint32_t timestamp)
+void ws_neighbor_class_neighbor_unicast_time_info_update(ws_neighbor_class_entry_t *ws_neighbor, ws_utt_ie_t *ws_utt, uint32_t timestamp, uint8_t address[8])
 {
 
 }

Original file line number	Diff line number	Diff line change
`@@ -493,7 +493,7 @@ static void ws_llc_mac_confirm_cb(const mac_api_t api, const mcps_data_conf_t `
`493`	`493`	`neighbor_info.neighbor->lifetime = neighbor_info.neighbor->link_lifetime;`
`494`	`494`	`}`
`495`	`495`
`496`		`- ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, &ws_utt, data->timestamp);`
	`496`	`+ ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, &ws_utt, data->timestamp, neighbor_info.neighbor->mac64);`
`497`	`497`	`}`
`498`	`498`
`499`	`499`	`int8_t rsl;`
`@@ -702,7 +702,7 @@ static void ws_llc_data_indication_cb(const mac_api_t *api, const mcps_data_ind_`
`702`	`702`	`return;`
`703`	`703`	`}`
`704`	`704`
`705`		`- ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, &ws_utt, data->timestamp);`
	`705`	`+ ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, &ws_utt, data->timestamp, (uint8_t *) data->SrcAddr);`
`706`	`706`	`if (us_ie_inline) {`
`707`	`707`	`ws_neighbor_class_neighbor_unicast_schedule_set(neighbor_info.ws_neighbor, &us_ie, &interface->ws_info->hopping_schdule);`
`708`	`708`	`}`
`@@ -808,7 +808,7 @@ static void ws_llc_eapol_indication_cb(const mac_api_t *api, const mcps_data_ind`
`808`	`808`	`temp_entry->signal_dbm = data->signal_dbm;`
`809`	`809`	`}`
`810`	`810`	`uint8_t auth_eui64[8];`
`811`		`- ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, &ws_utt, data->timestamp);`
	`811`	`+ ws_neighbor_class_neighbor_unicast_time_info_update(neighbor_info.ws_neighbor, &ws_utt, data->timestamp, (uint8_t *) data->SrcAddr);`
`812`	`812`	`if (us_ie_inline) {`
`813`	`813`	`ws_neighbor_class_neighbor_unicast_schedule_set(neighbor_info.ws_neighbor, &us_ie, &interface->ws_info->hopping_schdule);`
`814`	`814`	`}`
Original file line number	Diff line number	Diff line change
`@@ -106,7 +106,7 @@ void ws_neighbor_class_entry_remove(ws_neighbor_class_t *class_data, uint8_t att`
`106`	`106`	`* \param timestamp timestamp for received data`
`107`	`107`	`*`
`108`	`108`	`*/`
`109`		`-void ws_neighbor_class_neighbor_unicast_time_info_update(ws_neighbor_class_entry_t ws_neighbor, ws_utt_ie_t ws_utt, uint32_t timestamp);`
	`109`	`+void ws_neighbor_class_neighbor_unicast_time_info_update(ws_neighbor_class_entry_t ws_neighbor, ws_utt_ie_t ws_utt, uint32_t timestamp, uint8_t address[8]);`
`110`	`110`
`111`	`111`	`/**`
`112`	`112`	`* ws_neighbor_class_neighbor_unicast_schedule_set a function for update neighbor unicast shedule information`
Original file line number	Diff line number	Diff line change
`@@ -69,7 +69,7 @@ void ws_neighbor_class_entry_remove(ws_neighbor_class_t *class_data, uint8_t att`
`69`	`69`
`70`	`70`	`}`
`71`	`71`
`72`		`-void ws_neighbor_class_neighbor_unicast_time_info_update(ws_neighbor_class_entry_t ws_neighbor, ws_utt_ie_t ws_utt, uint32_t timestamp)`
	`72`	`+void ws_neighbor_class_neighbor_unicast_time_info_update(ws_neighbor_class_entry_t ws_neighbor, ws_utt_ie_t ws_utt, uint32_t timestamp, uint8_t address[8])`
`73`	`73`	`{`
`74`	`74`
`75`	`75`	`}`