Merge pull request ARMmbed#1644 from ARMmbed/ws_start_bc

Ws start bc

Author: Jarkko Paso

source/6LoWPAN/ws/ws_bootstrap.c

@@ -532,8 +532,8 @@ static void ws_bootstrap_pan_config_analyse(struct protocol_interface_info_entry
     cur->ws_info->hopping_schdule.fhss_bc_dwell_interval = ws_bs_ie.dwell_interval;
 
     cur->ws_info->hopping_schdule.channel_function = ws_bs_ie.channel_function;
-    // TODO: Restart unicast schedule if configuration changed, start broadcast schedule (FHSS)
-
+    // TODO: Restart unicast schedule if configuration changed
+    ns_fhss_ws_set_parent(cur->ws_info->fhss_api, data->SrcAddr, &neighbor->fhss_data.bc_timing_info);
 
     if (!cur->ws_info->configuration_learned) {
         // Generate own hopping schedules Follow first parent broadcast and plans and also use same unicast dwell

source/MAC/IEEE802_15_4/mac_mcps_sap.c

@@ -1507,7 +1507,7 @@ void mcps_sap_pd_req_queue_write(protocol_interface_rf_mac_setup_s *rf_mac_setup
         if ((rf_mac_setup->macBroadcastDisabled == true) && !mac_is_ack_request_set(buffer)) {
             goto push_to_queue;
         }
-        if (rf_mac_setup->fhss_api) {
+        if (rf_mac_setup->fhss_api && (buffer->asynch_request == false)) {
             if (rf_mac_setup->fhss_api->check_tx_conditions(rf_mac_setup->fhss_api, !mac_is_ack_request_set(buffer),
             	buffer->msduHandle, mac_convert_frame_type_to_fhss(buffer->fcf_dsn.frametype), buffer->mac_payload_length,
             	rf_mac_setup->dev_driver->phy_driver->phy_header_length, rf_mac_setup->dev_driver->phy_driver->phy_tail_length) == false) {

source/MAC/IEEE802_15_4/mac_pd_sap.c

@@ -340,7 +340,9 @@ static int8_t mac_data_interface_tx_done_cb(protocol_interface_rf_mac_setup_s *r
         if (rf_ptr->mac_tx_result == MAC_TX_DONE) {
             tx_is_done = true;
         }
-        rf_ptr->fhss_api->data_tx_done(rf_ptr->fhss_api, waiting_ack, tx_is_done, rf_ptr->active_pd_data_request->msduHandle);
+        if (rf_ptr->active_pd_data_request->asynch_request == false) {
+            rf_ptr->fhss_api->data_tx_done(rf_ptr->fhss_api, waiting_ack, tx_is_done, rf_ptr->active_pd_data_request->msduHandle);
+        }
     }
     return 0;
 }

source/Service_Libs/fhss/fhss_ws.c

@@ -38,6 +38,7 @@
 #define IE_HEADER_ID_MASK 0x7f80
 #define WH_IE_ID        0x2a
 #define WH_SUB_ID_UTT   1
+#define WH_SUB_ID_BT    2
 
 struct ws_ie_t {
     uint8_t *content_ptr;
@@ -106,10 +107,13 @@ static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
     if (!fhss_structure) {
         return;
     }
+    // TODO: check parent broadcast timing constantly and calculate drift error
     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_structure->ws->is_on_bc_channel = true;
         next_channel = fhss_ws_calc_bc_channel(fhss_structure);
+        // TODO: Randomize polling TX queue when on broadcast channel
+        fhss_structure->callbacks.tx_poll(fhss_structure->fhss_api);
     } 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);
@@ -156,6 +160,19 @@ static uint32_t fhss_ws_calculate_ufsi(fhss_structure_t *fhss_structure, uint32_
     return own_floor((float)(ms_since_seq_start * DEF_2E24) / (seq_length*dwell_time));
 }
 
+static uint32_t fhss_ws_calculate_broadcast_interval_offset(fhss_structure_t *fhss_structure, uint32_t tx_time)
+{
+    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 = (fhss_structure->platform_functions.fhss_get_remaining_slots(fhss_broadcast_handler, fhss_structure->fhss_api) / 1000);
+    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;
+    return (broadcast_interval-remaining_time) + time_to_tx;
+}
+
 static uint16_t fhss_ws_calculate_destination_slot(fhss_ws_neighbor_timing_info_t *neighbor_timing_info, uint32_t tx_time)
 {
     uint_fast24_t ufsi = neighbor_timing_info->uc_timing_info.ufsi;
@@ -238,7 +255,7 @@ static int fhss_ws_tx_handle_callback(const fhss_api_t *api, bool is_broadcast_a
     if (!fhss_structure) {
         return -1;
     }
-    if (is_broadcast_addr) {
+    if (is_broadcast_addr || (fhss_structure->ws->is_on_bc_channel == true)) {
         return 0;
     }
     if (fhss_structure->fhss_state == FHSS_SYNCHRONIZED) {
@@ -274,13 +291,22 @@ static int fhss_ws_tx_handle_callback(const fhss_api_t *api, bool is_broadcast_a
 
 static bool fhss_ws_check_tx_conditions_callback(const fhss_api_t *api, bool is_broadcast_addr, uint8_t handle, int frame_type, uint16_t frame_length, uint8_t phy_header_length, uint8_t phy_tail_length)
 {
-    (void) api;
-    (void) is_broadcast_addr;
     (void) handle;
     (void) frame_type;
     (void) frame_length;
     (void) phy_header_length;
     (void) phy_tail_length;
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
+    if (!fhss_structure) {
+        return true;
+    }
+    if (fhss_structure->ws->fhss_configuration.ws_channel_function == WS_FIXED_CHANNEL) {
+        return true;
+    }
+    // Do not allow broadcast destination on unicast channel
+    if (is_broadcast_addr && (fhss_structure->ws->is_on_bc_channel == false)) {
+        return false;
+    }
     return true;
 }
 
@@ -320,6 +346,11 @@ static int16_t fhss_ws_write_synch_info_callback(const fhss_api_t *api, uint8_t
         uint32_t ufsi = fhss_ws_calculate_ufsi(fhss_structure, tx_time);
         common_write_24_bit_inverse(ufsi, header_ie.content_ptr+1);
     }
+    if (fhss_ws_ie_header_discover(ptr, length, &header_ie, WH_SUB_ID_BT)) {
+        uint32_t broadcast_interval_offset = fhss_ws_calculate_broadcast_interval_offset(fhss_structure, tx_time);
+        common_write_16_bit_inverse(fhss_structure->ws->bc_slot, header_ie.content_ptr);
+        common_write_24_bit_inverse(broadcast_interval_offset, header_ie.content_ptr+2);
+    }
     return 0;
 }
 
@@ -402,6 +433,16 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
     if (!fhss_structure->ws) {
         return -1;
     }
+    if (fhss_structure->ws->fhss_configuration.ws_channel_function == WS_FIXED_CHANNEL) {
+        return 0;
+    }
+    // Start broadcast schedule when BC intervals are known
+    if (bc_timing_info->broadcast_interval && bc_timing_info->broadcast_dwell_interval) {
+        fhss_start_timer(fhss_structure, (bc_timing_info->broadcast_interval-bc_timing_info->broadcast_interval_offset)*1000, fhss_broadcast_handler);
+        fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval = bc_timing_info->broadcast_dwell_interval;
+        fhss_structure->ws->fhss_configuration.fhss_broadcast_interval = bc_timing_info->broadcast_interval;
+        fhss_structure->ws->bc_slot = bc_timing_info->broadcast_slot;
+    }
     //TODO: support multiple parents
     fhss_structure->ws->parent_bc_info = bc_timing_info;
     return 0;

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

@@ -57,6 +57,13 @@ uint16_t common_read_16_bit_inverse(const uint8_t data_buf[__static 2])
     return temp_16;
 }
 
+uint8_t *common_write_16_bit_inverse(uint16_t value, uint8_t ptr[__static 2])
+{
+    *ptr++ = value;
+    *ptr++ = value >> 8;
+    return ptr;
+}
+
 uint8_t *common_write_24_bit_inverse(uint_fast24_t value, uint8_t ptr[__static 3])
 {
     *ptr++ = value;
@@ -134,6 +141,7 @@ static fhss_api_t *test_generate_fhss_api(void)
     fhss_common_stub.fhss_struct.ws->is_on_bc_channel = false;
     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.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);
     return &fhss_api;
@@ -352,7 +360,7 @@ bool test_fhss_ws_write_synch_info_callback()
 {
     fhss_api_t *api = test_generate_fhss_api();
     fhss_common_stub.fhss_struct.ws->fhss_configuration.ws_channel_function = WS_TR51CF;
-    uint8_t synch_info[16] = {0x05, 0x15, 0x02, 0x00, 0x00, 0x00, 0x00, 0x05, 0x15, 0x01, 0x04, 0x00, 0x00, 0x00, 0x00, 0x3f};
+    uint8_t synch_info[100] = {0x05, 0x15, 0x02, 0x00, 0x00, 0x00, 0x00, 0x05, 0x15, 0x01, 0x04, 0x00, 0x00, 0x00, 0x00, 0x3f};
     // Test when FHSS struct not found
     disable_fhss_struct();
     if (fhss_common_stub.fhss_struct.fhss_api->write_synch_info(api, synch_info, sizeof(synch_info), DEFAULT_FRAME_TYPE, DEFAULT_TX_TIME) != -1) {
@@ -517,6 +525,7 @@ bool test_fhss_ws_set_parent()
     broadcast_timing_info_t bc_timing_info;
     memset(&bc_timing_info, 0, sizeof(broadcast_timing_info_t));
     fhss_api_t *api = test_generate_fhss_api();
+    fhss_common_stub.fhss_struct.ws->fhss_configuration.ws_channel_function = WS_TR51CF;
 
     // Test without WS enabled FHSS
     fhss_structure_t fake_fhss_structure;