Merge pull request ARMmbed#1800 from ARMmbed/IOTTHD-2504

Iotthd 2504

Author: Jarkko Paso

nanostack/fhss_api.h

@@ -215,6 +215,13 @@ typedef int mac_read_mac_address(const fhss_api_t *fhss_api, uint8_t *mac_addres
  */
 typedef uint32_t mac_read_datarate(const fhss_api_t *fhss_api);
 
+/**
+ * @brief Read 32-bit timestamp.
+ * @param fhss_api FHSS instance.
+ * @return Timestamp.
+ */
+typedef uint32_t mac_read_timestamp(const fhss_api_t *fhss_api);
+
 /**
  * @brief Change channel.
  * @param fhss_api FHSS instance.
@@ -287,6 +294,7 @@ struct fhss_callback {
     mac_read_tx_queue_size *read_tx_queue_size;                 /**< Read MAC TX queue size. */
     mac_read_mac_address *read_mac_address;                     /**< Read MAC address. */
     mac_read_datarate *read_datarate;                           /**< Read PHY datarate. */
+    mac_read_timestamp *read_timestamp;                         /**< Read timestamp. */
     mac_change_channel *change_channel;                         /**< Change channel. */
     mac_send_fhss_frame *send_fhss_frame;                       /**< Send FHSS frame. */
     mac_synch_lost_notification *synch_lost_notification;       /**< Send notification when FHSS synchronization is lost. */

source/MAC/IEEE802_15_4/mac_fhss_callbacks.c

@@ -65,6 +65,17 @@ uint32_t mac_read_phy_datarate(const fhss_api_t *fhss_api)
     return datarate;
 }
 
+uint32_t mac_read_phy_timestamp(const fhss_api_t *fhss_api)
+{
+    protocol_interface_rf_mac_setup_s *mac_setup = get_sw_mac_ptr_by_fhss_api(fhss_api);
+    if (!mac_setup) {
+        return 0;
+    }
+    uint32_t timestamp;
+    mac_setup->dev_driver->phy_driver->extension(PHY_EXTENSION_GET_TIMESTAMP, (uint8_t *)&timestamp);
+    return timestamp;
+}
+
 int mac_set_channel(const fhss_api_t *fhss_api, uint8_t channel_number)
 {
     protocol_interface_rf_mac_setup_s *mac_setup = get_sw_mac_ptr_by_fhss_api(fhss_api);

source/MAC/IEEE802_15_4/mac_fhss_callbacks.h

@@ -21,6 +21,7 @@
 uint16_t mac_read_tx_queue_sizes(const fhss_api_t *fhss_api, bool broadcast_queue);
 int mac_read_64bit_mac_address(const fhss_api_t *fhss_api, uint8_t *mac_address);
 uint32_t mac_read_phy_datarate(const fhss_api_t *fhss_api);
+uint32_t mac_read_phy_timestamp(const fhss_api_t *fhss_api);
 int mac_set_channel(const fhss_api_t *fhss_api, uint8_t channel_number);
 int mac_fhss_frame_tx(const fhss_api_t *fhss_api, int frame_type);
 int mac_synch_lost(const fhss_api_t *fhss_api);

source/MAC/IEEE802_15_4/sw_mac.c

@@ -176,6 +176,7 @@ int ns_sw_mac_fhss_register(mac_api_t *mac_api, fhss_api_t *fhss_api)
     fhss_callback_t callbacks;
     callbacks.read_tx_queue_size = &mac_read_tx_queue_sizes;
     callbacks.read_datarate = &mac_read_phy_datarate;
+    callbacks.read_timestamp = &mac_read_phy_timestamp;
     callbacks.read_mac_address = &mac_read_64bit_mac_address;
     callbacks.change_channel = &mac_set_channel;
     callbacks.send_fhss_frame = &mac_fhss_frame_tx;

source/Service_Libs/fhss/fhss_ws.c

@@ -34,6 +34,13 @@
 
 // Enable this flag to use channel traces
 // #define FHSS_CHANNEL_DEBUG
+// Enable this flag to use debug callbacks
+// #define FHSS_CHANNEL_DEBUG_CBS
+
+#ifdef FHSS_CHANNEL_DEBUG_CBS
+void (*fhss_uc_switch)(void) = NULL;
+void (*fhss_bc_switch)(void) = NULL;
+#endif /*FHSS_CHANNEL_DEBUG_CBS*/
 
 #define DEF_2E24 0x1000000
 #define IE_HEADER_LENGTH_MASK 0x007f
@@ -108,7 +115,6 @@ static int32_t fhss_ws_calc_bc_channel(fhss_structure_t *fhss_structure)
 
 static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
 {
-    (void) delay;
     int32_t next_channel;
     fhss_structure_t *fhss_structure = fhss_get_object_with_api(fhss_api);
     if (!fhss_structure) {
@@ -121,12 +127,12 @@ 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, fhss_broadcast_handler);
+        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_structure->ws->is_on_bc_channel = true;
         next_channel = fhss_structure->ws->bc_channel = fhss_ws_calc_bc_channel(fhss_structure);
     } else {
         uint32_t timeout = (fhss_structure->ws->fhss_configuration.fhss_broadcast_interval - fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval) * 1000;
-        fhss_start_timer(fhss_structure, timeout, fhss_broadcast_handler);
+        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
         next_channel = fhss_structure->rx_channel;
@@ -135,6 +141,11 @@ static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
 #endif /*FHSS_CHANNEL_DEBUG*/
     }
     fhss_structure->callbacks.change_channel(fhss_structure->fhss_api, next_channel);
+#ifdef FHSS_CHANNEL_DEBUG_CBS
+        if (fhss_bc_switch) {
+            fhss_bc_switch();
+        }
+#endif /*FHSS_CHANNEL_DEBUG_CBS*/
     fhss_ws_start_tx_poll_timer(fhss_structure, fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, true),
             fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval);
 }
@@ -162,7 +173,11 @@ static uint32_t fhss_ws_calculate_ufsi(fhss_structure_t *fhss_structure, uint32_
     }
     cur_slot--;
     uint32_t remaining_time = (fhss_structure->platform_functions.fhss_get_remaining_slots(fhss_unicast_handler, fhss_structure->fhss_api) / 1000);
-    uint32_t time_to_tx = (tx_time - fhss_structure->fhss_api->read_timestamp(fhss_structure->fhss_api)) / 1000;
+    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;
+    }
     uint64_t ms_since_seq_start = (cur_slot * dwell_time) + (dwell_time-remaining_time) + time_to_tx;
     uint32_t seq_length = 0x10000;
     if (fhss_structure->ws->fhss_configuration.ws_channel_function == WS_TR51CF) {
@@ -181,7 +196,7 @@ static uint32_t fhss_ws_calculate_broadcast_interval_offset(fhss_structure_t *fh
     if (fhss_structure->ws->is_on_bc_channel == true) {
         remaining_time += (broadcast_interval - dwell_time);
     }
-    uint32_t time_to_tx = (tx_time - fhss_structure->fhss_api->read_timestamp(fhss_structure->fhss_api)) / 1000;
+    uint32_t time_to_tx = (tx_time - fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api)) / 1000;
     return (broadcast_interval-remaining_time) + time_to_tx;
 }
 
@@ -194,7 +209,7 @@ static uint16_t fhss_ws_calculate_destination_slot(fhss_ws_neighbor_timing_info_
     if (neighbor_timing_info->uc_timing_info.unicast_channel_function  == WS_TR51CF) {
         seq_length = neighbor_timing_info->uc_timing_info.unicast_number_of_channels;
     }
-    uint32_t dest_ms_since_seq_start = own_ceil((float)(ufsi*seq_length*dwell_time) / DEF_2E24);
+    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);
 }
 
@@ -230,7 +245,7 @@ static void fhss_ws_synch_state_set_callback(const fhss_api_t *api, fhss_states
 static void fhss_ws_update_uc_channel_callback(fhss_structure_t *fhss_structure)
 {
     uint8_t mac_address[8];
-    int32_t next_channel;
+    int32_t next_channel = fhss_structure->rx_channel;
     fhss_structure->callbacks.read_mac_address(fhss_structure->fhss_api, mac_address);
     if (fhss_structure->ws->fhss_configuration.ws_channel_function == WS_FIXED_CHANNEL) {
         return;
@@ -255,6 +270,11 @@ static void fhss_ws_update_uc_channel_callback(fhss_structure_t *fhss_structure)
     tr_info("%"PRIu32" UC %u %u", fhss_structure->platform_functions.fhss_get_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
+    if (fhss_uc_switch) {
+        fhss_uc_switch();
+    }
+#endif /*FHSS_CHANNEL_DEBUG_CBS*/
 }
 
 static int fhss_ws_tx_handle_callback(const fhss_api_t *api, bool is_broadcast_addr, uint8_t *destination_address, int frame_type, uint16_t frame_length, uint8_t phy_header_length, uint8_t phy_tail_length, uint32_t tx_time)
@@ -575,7 +595,7 @@ int fhss_ws_set_callbacks(fhss_structure_t *fhss_structure)
     fhss_structure->fhss_api->data_tx_done = &fhss_ws_data_tx_done_callback;
     fhss_structure->fhss_api->data_tx_fail = &fhss_ws_data_tx_fail_callback;
     fhss_structure->fhss_api->synch_state_set = &fhss_ws_synch_state_set_callback;
-    fhss_structure->fhss_api->read_timestamp = &fhss_read_timestamp_cb;
+    fhss_structure->fhss_api->read_timestamp = NULL;
     fhss_structure->fhss_api->get_retry_period = NULL;
     fhss_structure->fhss_api->write_synch_info = &fhss_ws_write_synch_info_callback;
     fhss_structure->fhss_api->init_callbacks = &fhss_init_callbacks_cb;
@@ -595,7 +615,7 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
         return -1;
     }
 
-    uint32_t time_from_reception_ms = (fhss_structure->fhss_api->read_timestamp(fhss_structure->fhss_api) - bc_timing_info->bt_rx_timestamp)/1000;
+    uint32_t time_from_reception_ms = (fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api) - bc_timing_info->bt_rx_timestamp)/1000;
     uint32_t true_bc_interval_offset = (bc_timing_info->broadcast_interval_offset + time_from_reception_ms) % bc_timing_info->broadcast_interval;
     uint32_t timeout = ((bc_timing_info->broadcast_interval-true_bc_interval_offset)*1000);
     fhss_start_timer(fhss_structure, timeout, fhss_broadcast_handler);

test/nanostack/unittest/service_libs/fhss_ws/test_fhss_ws.c

@@ -144,6 +144,7 @@ static fhss_api_t *test_generate_fhss_api(void)
     fhss_common_stub.fhss_struct.callbacks.change_channel = &mac_set_channel;
     fhss_common_stub.fhss_struct.callbacks.read_mac_address = &mac_read_64bit_mac_address;
     fhss_common_stub.fhss_struct.callbacks.read_tx_queue_size = &mac_read_tx_queue_sizes;
+    fhss_common_stub.fhss_struct.callbacks.read_timestamp = &mac_read_phy_timestamp;
     fhss_common_stub.fhss_struct.callbacks.tx_poll = &mac_poll_tx_queue;
     test_set_platform_api(&fhss_common_stub.fhss_struct.platform_functions);
     fhss_ws_set_callbacks(&fhss_common_stub.fhss_struct);
@@ -469,24 +470,24 @@ bool test_fhss_ws_write_synch_info_callback()
         return false;
     }
     fhss_platform_stub.remaining_slots_value = 100000;
-    fhss_common_stub.uint32_value = 1000000;
+    fhss_callbacks_stub.uint32_value = 1000000;
     /* Test the UFSI field in synch info
      * slot:           |               0(200ms)        |               1(200ms)        |               2(200ms)        |               3(200ms)        |
      *                 |                      ufsi(597ms) -> 1001599                                                  |
      * timestamps:                                                                written at 1000000us|TX at 1097000us|
      */
-    if ((fhss_common_stub.fhss_struct.fhss_api->write_synch_info(api, synch_info, sizeof(synch_info), DEFAULT_FRAME_TYPE, fhss_common_stub.uint32_value + 97000) != 0)
+    if ((fhss_common_stub.fhss_struct.fhss_api->write_synch_info(api, synch_info, sizeof(synch_info), DEFAULT_FRAME_TYPE, fhss_callbacks_stub.uint32_value + 97000) != 0)
             || (test_ufsi(&synch_info[11], 1001599) != true)) {
         return false;
     }
     // Test when timestamp overflows
-    fhss_common_stub.uint32_value = 4294960000;
+    fhss_callbacks_stub.uint32_value = 4294960000;
     /* Test the UFSI field in synch info
      * slot:           |               0(200ms)        |               1(200ms)        |               2(200ms)        |               3(200ms)        |
      *                 |                      ufsi(597ms) -> 1001599                                                  |
      * timestamps:                                                             written at 4294960000us|  TX at 89704us|
      */
-    if ((fhss_common_stub.fhss_struct.fhss_api->write_synch_info(api, synch_info, sizeof(synch_info), DEFAULT_FRAME_TYPE, fhss_common_stub.uint32_value + 97000) != 0)
+    if ((fhss_common_stub.fhss_struct.fhss_api->write_synch_info(api, synch_info, sizeof(synch_info), DEFAULT_FRAME_TYPE, fhss_callbacks_stub.uint32_value + 97000) != 0)
             || (test_ufsi(&synch_info[11], 1001599) != true)) {
         return false;
     }

test/nanostack/unittest/stub/fhss_callbacks_stub.c

@@ -35,6 +35,11 @@ uint32_t mac_read_phy_datarate(const fhss_api_t *fhss_api)
     return 250000;
 }
 
+uint32_t mac_read_phy_timestamp(const fhss_api_t *fhss_api)
+{
+    return fhss_callbacks_stub.uint32_value;
+}
+
 int mac_set_channel(const fhss_api_t *fhss_api, uint8_t channel_number)
 {
     fhss_callbacks_stub.uint8_value = channel_number;

test/nanostack/unittest/stub/fhss_callbacks_stub.h

@@ -24,6 +24,7 @@ extern "C" {
 typedef struct {
     uint8_t uint8_value;
     int8_t int8_value;
+    uint32_t uint32_value;
     uint8_t address[8];
 } fhss_callbacks_stub_def;