Merge pull request ARMmbed#1885 from ARMmbed/IOTTHD-2752

Iotthd 2752

Author: Jarkko Paso

source/Service_Libs/fhss/fhss_ws.c

@@ -42,7 +42,18 @@
 void (*fhss_uc_switch)(void) = NULL;
 void (*fhss_bc_switch)(void) = NULL;
 #endif /*FHSS_CHANNEL_DEBUG_CBS*/
-
+// Seconds to milliseconds
+#define S_TO_MS(x) (((uint32_t)x)*1000)
+// Milliseconds to seconds
+#define MS_TO_S(x) divide_integer(x, 1000)
+// Seconds to microseconds
+#define S_TO_US(x) (((uint32_t)x)*1000000)
+// Microseconds to seconds
+#define US_TO_S(x) divide_integer(x, 1000000)
+// Milliseconds to microseconds
+#define MS_TO_US(x) (((uint32_t)x)*1000)
+// Microseconds to milliseconds
+#define US_TO_MS(x) divide_integer(x, 1000)
 #define DEF_2E24 0x1000000
 #define IE_HEADER_LENGTH_MASK 0x007f
 #define IE_HEADER_ID_MASK 0x7f80
@@ -71,6 +82,16 @@ static uint32_t divide_integer(uint32_t dividend, uint32_t divisor)
     return (dividend + divisor/2) / divisor;
 }
 
+static uint32_t get_remaining_slots_us(fhss_structure_t *fhss_structure, void (*callback)(const fhss_api_t *api, uint16_t), uint32_t max_timeout_us)
+{
+    uint32_t remaining_time_us = fhss_structure->platform_functions.fhss_get_remaining_slots(callback, fhss_structure->fhss_api);
+    // When remaining time goes negative, use 0.
+    if (remaining_time_us > max_timeout_us) {
+        remaining_time_us = 0;
+    }
+    return remaining_time_us;
+}
+
 fhss_structure_t *fhss_ws_enable(fhss_api_t *fhss_api, const fhss_ws_configuration_t *fhss_configuration, const fhss_timer_t *fhss_timer)
 {
     if (!fhss_api || !fhss_configuration || !fhss_timer) {
@@ -156,7 +177,7 @@ static int32_t fhss_ws_calc_bc_channel(fhss_structure_t *fhss_structure)
         }
     }
 #ifdef FHSS_CHANNEL_DEBUG
-    tr_info("%"PRIu32" BC %u %u", fhss_structure->platform_functions.fhss_get_timestamp(fhss_structure->fhss_api), next_channel, fhss_structure->ws->bc_slot);
+    tr_info("%"PRIu32" BC %u %u", fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api), next_channel, fhss_structure->ws->bc_slot);
 #endif /*FHSS_CHANNEL_DEBUG*/
     return next_channel;
 }
@@ -175,7 +196,7 @@ static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
         return;
     }
     if (fhss_structure->ws->is_on_bc_channel == false) {
-        fhss_start_timer(fhss_structure, fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval*1000 - (delay * fhss_structure->platform_functions.fhss_resolution_divider), fhss_broadcast_handler);
+        fhss_start_timer(fhss_structure, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval) - (delay * fhss_structure->platform_functions.fhss_resolution_divider), fhss_broadcast_handler);
         fhss_structure->ws->is_on_bc_channel = true;
         next_channel = fhss_structure->ws->bc_channel = fhss_ws_calc_bc_channel(fhss_structure);
 
@@ -184,12 +205,12 @@ static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
          * Max random is 1/10 of the channel dwell interval.
          * Event timer resolution is 50us.
          */
-        uint32_t bc_dwell_us = fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval*1000;
+        uint32_t bc_dwell_us = MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval);
         uint16_t bc_min_random = (bc_dwell_us / 50) / 50;
         uint16_t bc_max_random = (bc_dwell_us / 10) / 50;
         eventOS_callback_timer_start(fhss_structure->fhss_event_timer, randLIB_get_random_in_range(bc_min_random, bc_max_random));
     } else {
-        uint32_t timeout = (fhss_structure->ws->fhss_configuration.fhss_broadcast_interval - fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval) * 1000;
+        uint32_t timeout = MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_broadcast_interval - fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval);
         fhss_start_timer(fhss_structure, timeout - (delay * fhss_structure->platform_functions.fhss_resolution_divider), fhss_broadcast_handler);
         fhss_structure->ws->is_on_bc_channel = false;
         // Should return to own (unicast) listening channel after broadcast channel
@@ -199,7 +220,7 @@ static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
          * Max random is 1/10 of the TX slot length.
          * Event timer resolution is 50us.
          */
-        uint32_t txrx_slot_length_us = fhss_structure->ws->txrx_slot_length_ms * 1000;
+        uint32_t txrx_slot_length_us = MS_TO_US(fhss_structure->ws->txrx_slot_length_ms);
         uint16_t uc_min_random = (txrx_slot_length_us / 30) / 50;
         uint16_t uc_max_random = (txrx_slot_length_us / 10) / 50;
         bool tx_allowed = fhss_ws_check_tx_allowed(fhss_structure);
@@ -210,7 +231,7 @@ static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
         eventOS_callback_timer_start(fhss_structure->fhss_event_timer, randLIB_get_random_in_range(uc_min_random, uc_max_random));
 
 #ifdef FHSS_CHANNEL_DEBUG
-    tr_info("%"PRIu32" UC %u", fhss_structure->platform_functions.fhss_get_timestamp(fhss_structure->fhss_api), fhss_structure->rx_channel);
+    tr_info("%"PRIu32" UC %u", fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api), fhss_structure->rx_channel);
 #endif /*FHSS_CHANNEL_DEBUG*/
     }
     fhss_structure->callbacks.change_channel(fhss_structure->fhss_api, next_channel);
@@ -246,8 +267,8 @@ static void fhss_event_timer_cb(int8_t timer_id, uint16_t slots)
         queue_size = fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, true);
     } else {
         // On unicast, start timer to trigger polling event on next TX slot
-        uint32_t delay_between_tx_slots_us = fhss_structure->ws->txrx_slot_length_ms*1000*2;
-        if (delay_between_tx_slots_us < fhss_structure->platform_functions.fhss_get_remaining_slots(fhss_broadcast_handler, fhss_structure->fhss_api)) {
+        uint32_t delay_between_tx_slots_us = MS_TO_US(fhss_structure->ws->txrx_slot_length_ms)*2;
+        if (delay_between_tx_slots_us < get_remaining_slots_us(fhss_structure, fhss_broadcast_handler, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_broadcast_interval))) {
             eventOS_callback_timer_start(fhss_structure->fhss_event_timer, delay_between_tx_slots_us/50);
         }
         queue_size = fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, false);
@@ -265,16 +286,13 @@ static uint32_t fhss_ws_calculate_ufsi(fhss_structure_t *fhss_structure, uint32_
         cur_slot = fhss_structure->number_of_channels;
     }
     cur_slot--;
-    uint32_t remaining_time = (fhss_structure->platform_functions.fhss_get_remaining_slots(fhss_unicast_handler, fhss_structure->fhss_api) / 1000);
-    if (remaining_time > dwell_time) {
-        remaining_time = 0;
-    }
+    uint32_t remaining_time_ms = US_TO_MS(get_remaining_slots_us(fhss_structure, fhss_unicast_handler, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_uc_dwell_interval)));
     uint32_t time_to_tx = 0;
     uint32_t cur_time = fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api);
     if (cur_time < tx_time) {
-        time_to_tx = (tx_time - cur_time) / 1000;
+        time_to_tx = US_TO_MS(tx_time - cur_time);
     }
-    uint64_t ms_since_seq_start = (cur_slot * dwell_time) + (dwell_time-remaining_time) + time_to_tx;
+    uint64_t ms_since_seq_start = (cur_slot * dwell_time) + (dwell_time-remaining_time_ms) + time_to_tx;
     uint32_t seq_length = 0x10000;
     if (fhss_structure->ws->fhss_configuration.ws_uc_channel_function == WS_TR51CF) {
         ms_since_seq_start %= (dwell_time*fhss_structure->number_of_channels);
@@ -287,17 +305,16 @@ static uint32_t fhss_ws_calculate_broadcast_interval_offset(fhss_structure_t *fh
 {
     uint8_t dwell_time = fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval;
     uint32_t broadcast_interval = fhss_structure->ws->fhss_configuration.fhss_broadcast_interval;
-
-    uint32_t remaining_time = divide_integer(fhss_structure->platform_functions.fhss_get_remaining_slots(fhss_broadcast_handler, fhss_structure->fhss_api), 1000);
+    uint32_t remaining_time_ms = US_TO_MS(get_remaining_slots_us(fhss_structure, fhss_broadcast_handler, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_broadcast_interval)));
     if (fhss_structure->ws->is_on_bc_channel == true) {
-        remaining_time += (broadcast_interval - dwell_time);
+        remaining_time_ms += (broadcast_interval - dwell_time);
     }
     uint32_t time_to_tx = 0;
     uint32_t cur_time = fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api);
     if (cur_time < tx_time) {
-        time_to_tx = divide_integer(tx_time - cur_time, 1000);
+        time_to_tx = US_TO_MS(tx_time - cur_time);
     }
-    return (broadcast_interval-remaining_time) + time_to_tx;
+    return (broadcast_interval-remaining_time_ms) + time_to_tx;
 }
 
 static uint16_t fhss_ws_calculate_destination_slot(fhss_ws_neighbor_timing_info_t *neighbor_timing_info, uint32_t tx_time)
@@ -310,12 +327,12 @@ static uint16_t fhss_ws_calculate_destination_slot(fhss_ws_neighbor_timing_info_
         seq_length = neighbor_timing_info->uc_timing_info.unicast_number_of_channels;
     }
     uint32_t dest_ms_since_seq_start = own_ceil((float)((uint64_t)ufsi*seq_length*dwell_time) / DEF_2E24);
-    return (own_floor(((float)((tx_time - ufsi_timestamp)/1000 + dest_ms_since_seq_start) / dwell_time)) % seq_length);
+    return (own_floor(((float)(US_TO_MS(tx_time - ufsi_timestamp) + dest_ms_since_seq_start) / dwell_time)) % seq_length);
 }
 
 static uint32_t fhss_ws_get_sf_timeout_callback(fhss_structure_t *fhss_structure)
 {
-    return fhss_structure->ws->fhss_configuration.fhss_uc_dwell_interval * 1000;
+    return MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_uc_dwell_interval);
 }
 
 static int16_t fhss_ws_synch_state_set_callback(const fhss_api_t *api, fhss_states fhss_state, uint16_t pan_id)
@@ -336,7 +353,7 @@ static int16_t fhss_ws_synch_state_set_callback(const fhss_api_t *api, fhss_stat
         // Start unicast schedule
         if ((fhss_structure->ws->fhss_configuration.ws_uc_channel_function != WS_FIXED_CHANNEL)) {
             fhss_ws_update_uc_channel_callback(fhss_structure);
-            fhss_start_timer(fhss_structure, fhss_structure->ws->fhss_configuration.fhss_uc_dwell_interval*1000, fhss_unicast_handler);
+            fhss_start_timer(fhss_structure, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_uc_dwell_interval), fhss_unicast_handler);
             fhss_structure->ws->unicast_timer_running = true;
         }
     }
@@ -374,7 +391,7 @@ static void fhss_ws_update_uc_channel_callback(fhss_structure_t *fhss_structure)
         return;
     }
 #ifdef FHSS_CHANNEL_DEBUG
-    tr_info("%"PRIu32" UC %u %u", fhss_structure->platform_functions.fhss_get_timestamp(fhss_structure->fhss_api), next_channel, fhss_structure->ws->uc_slot);
+    tr_info("%"PRIu32" UC %u %u", fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api), next_channel, fhss_structure->ws->uc_slot);
 #endif /*FHSS_CHANNEL_DEBUG*/
     fhss_structure->callbacks.change_channel(fhss_structure->fhss_api, next_channel);
 #ifdef FHSS_CHANNEL_DEBUG_CBS
@@ -466,7 +483,8 @@ static bool fhss_ws_check_tx_allowed(fhss_structure_t *fhss_structure)
     if (!number_of_tx_slots) {
         return true;
     }
-    uint32_t remaining_time_ms = fhss_structure->platform_functions.fhss_get_remaining_slots(fhss_broadcast_handler, fhss_structure->fhss_api) / 1000;
+
+    uint32_t remaining_time_ms = get_remaining_slots_us(fhss_structure, fhss_broadcast_handler, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_broadcast_interval)) / 1000;
     uint32_t tx_slot_begin = (fhss_structure->ws->fhss_configuration.fhss_broadcast_interval - fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval) - (fhss_structure->ws->txrx_slot_length_ms * (fhss_structure->own_hop & 1));
     uint32_t rx_slot_begin = tx_slot_begin - fhss_structure->ws->txrx_slot_length_ms;
     uint8_t n_o_tx_slots = number_of_tx_slots;
@@ -488,8 +506,8 @@ static bool fhss_ws_check_tx_time(fhss_structure_t *fhss_structure, uint16_t tx_
         return true;
     }
     uint32_t tx_time = fhss_get_tx_time(fhss_structure, tx_length, phy_header_length, phy_tail_length);
-    uint32_t time_to_bc_channel = fhss_structure->platform_functions.fhss_get_remaining_slots(fhss_broadcast_handler, fhss_structure->fhss_api);
-    if (tx_time > time_to_bc_channel) {
+    uint32_t time_to_bc_channel_us = get_remaining_slots_us(fhss_structure, fhss_broadcast_handler, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_broadcast_interval));
+    if (tx_time > time_to_bc_channel_us) {
         return false;
     }
     return true;
@@ -709,21 +727,23 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
     if (!bc_timing_info->broadcast_interval || !bc_timing_info->broadcast_dwell_interval) {
         return -1;
     }
-    if ((((uint32_t)fhss_structure->ws->min_synch_interval*1000000) > (fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api) - fhss_structure->ws->synchronization_time)) && !force_synch) {
+    if ((S_TO_US(fhss_structure->ws->min_synch_interval) > (fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api) - fhss_structure->ws->synchronization_time)) && !force_synch) {
         return 0;
     }
-    fhss_structure->ws->synchronization_time = fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api);
     platform_enter_critical();
+    uint32_t prev_synchronization_time = fhss_structure->ws->synchronization_time;
+    fhss_structure->ws->synchronization_time = fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api);
+    uint32_t own_bc_interval_offset = fhss_ws_calculate_broadcast_interval_offset(fhss_structure, fhss_structure->ws->synchronization_time);
     fhss_stop_timer(fhss_structure, fhss_broadcast_handler);
-    uint32_t time_from_reception_ms = divide_integer(fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api) - bc_timing_info->bt_rx_timestamp, 1000);
+    uint32_t time_from_reception_ms = US_TO_MS(fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api) - bc_timing_info->bt_rx_timestamp);
     uint32_t true_bc_interval_offset = (bc_timing_info->broadcast_interval_offset + time_from_reception_ms) % bc_timing_info->broadcast_interval;
     if (true_bc_interval_offset >= bc_timing_info->broadcast_dwell_interval) {
         fhss_structure->ws->is_on_bc_channel = false;
     }
-    uint32_t timeout = ((bc_timing_info->broadcast_interval-true_bc_interval_offset)*1000);
+    uint32_t timeout = MS_TO_US(bc_timing_info->broadcast_interval-true_bc_interval_offset);
 
     if (fhss_structure->ws->is_on_bc_channel) {
-        timeout -= ((bc_timing_info->broadcast_interval - bc_timing_info->broadcast_dwell_interval) *1000);
+        timeout -= MS_TO_US(bc_timing_info->broadcast_interval-bc_timing_info->broadcast_dwell_interval);
     }
     fhss_start_timer(fhss_structure, timeout, fhss_broadcast_handler);
     uint16_t slots_since_reception = (bc_timing_info->broadcast_interval_offset + time_from_reception_ms) / bc_timing_info->broadcast_interval;
@@ -738,6 +758,9 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
     platform_exit_critical();
     //TODO: support multiple parents
     fhss_structure->ws->parent_bc_info = bc_timing_info;
+    if (prev_synchronization_time) {
+        tr_debug("synch to parent: %s, drift: %ims in %u seconds", trace_array(eui64, 8), true_bc_interval_offset-own_bc_interval_offset, US_TO_S(fhss_structure->ws->synchronization_time-prev_synchronization_time));
+    }
     return 0;
 }
 
@@ -763,7 +786,7 @@ int fhss_ws_configuration_set(fhss_structure_t *fhss_structure, const fhss_ws_co
         fhss_structure->ws->unicast_timer_running = false;
     }
     if ((fhss_structure->ws->unicast_timer_running == false) && (fhss_configuration->ws_uc_channel_function != WS_FIXED_CHANNEL) && fhss_configuration->fhss_uc_dwell_interval) {
-        fhss_start_timer(fhss_structure, fhss_configuration->fhss_uc_dwell_interval*1000, fhss_unicast_handler);
+        fhss_start_timer(fhss_structure, MS_TO_US(fhss_configuration->fhss_uc_dwell_interval), fhss_unicast_handler);
         fhss_structure->ws->unicast_timer_running = true;
     }
     fhss_structure->ws->fhss_configuration = *fhss_configuration;