Validate randomized fixed channel (ARMmbed#2592)

source/6LoWPAN/ws/ws_bootstrap.c

@@ -124,6 +124,8 @@ static void ws_bootstrap_asynch_trickle_stop(protocol_interface_info_entry_t *cu
 static void ws_bootstrap_advertise_start(protocol_interface_info_entry_t *cur);
 static void ws_bootstrap_rpl_scan_start(protocol_interface_info_entry_t *cur);
 
+static uint16_t ws_randomize_fixed_channel(uint16_t configured_fixed_channel, uint8_t number_of_channels, uint32_t *channel_mask);
+
 typedef enum {
     WS_PARENT_SOFT_SYNCH = 0,  /**< let FHSS make decision if synchronization is needed*/
     WS_PARENT_HARD_SYNCH,      /**< Synch FHSS with latest synch information*/
@@ -728,18 +730,33 @@ static int8_t ws_fhss_border_router_configure(protocol_interface_info_entry_t *c
 
     //GET BSI from BBR module
     fhss_configuration.bsi = ws_bbr_bsi_generate(cur);
-    ws_fhss_set_defaults(cur, &fhss_configuration);
     ws_fhss_configure_channel_masks(cur, &fhss_configuration);
+    // Randomize fixed channels. Only used if channel plan is fixed.
+    cur->ws_info->cfg->fhss.fhss_uc_fixed_channel = ws_randomize_fixed_channel(cur->ws_info->cfg->fhss.fhss_uc_fixed_channel, cur->ws_info->hopping_schdule.number_of_channels, fhss_configuration.channel_mask);
+    cur->ws_info->cfg->fhss.fhss_bc_fixed_channel = ws_randomize_fixed_channel(cur->ws_info->cfg->fhss.fhss_bc_fixed_channel, cur->ws_info->hopping_schdule.number_of_channels, fhss_configuration.channel_mask);
+    ws_fhss_set_defaults(cur, &fhss_configuration);
     ns_fhss_ws_configuration_set(cur->ws_info->fhss_api, &fhss_configuration);
     ws_bootstrap_llc_hopping_update(cur, &fhss_configuration);
 
     return 0;
 }
 
-static uint16_t ws_randomize_fixed_channel(uint16_t configured_fixed_channel, uint8_t number_of_channels)
+static bool ws_channel_allowed(uint8_t channel, uint32_t *channel_mask)
+{
+    if ((1 << (channel % 32)) & (channel_mask[channel / 32])) {
+        return true;
+    }
+    return false;
+}
+
+static uint16_t ws_randomize_fixed_channel(uint16_t configured_fixed_channel, uint8_t number_of_channels, uint32_t *channel_mask)
 {
     if (configured_fixed_channel == 0xFFFF) {
-        return randLIB_get_random_in_range(0, number_of_channels - 1);
+        uint16_t random_channel = randLIB_get_random_in_range(0, number_of_channels - 1);
+        while (ws_channel_allowed(random_channel, channel_mask) == false) {
+            random_channel = randLIB_get_random_in_range(0, number_of_channels - 1);
+        }
+        return random_channel;
     } else {
         return configured_fixed_channel;
     }
@@ -764,8 +781,8 @@ static int8_t ws_fhss_configure(protocol_interface_info_entry_t *cur, bool disco
     }
     fhss_configuration.ws_bc_channel_function = WS_FIXED_CHANNEL;
     fhss_configuration.fhss_broadcast_interval = 0;
-    uint8_t tmp_uc_fixed_channel = ws_randomize_fixed_channel(cur->ws_info->cfg->fhss.fhss_uc_fixed_channel, cur->ws_info->hopping_schdule.number_of_channels);
-    uint8_t tmp_bc_fixed_channel = ws_randomize_fixed_channel(cur->ws_info->cfg->fhss.fhss_bc_fixed_channel, cur->ws_info->hopping_schdule.number_of_channels);
+    uint8_t tmp_uc_fixed_channel = ws_randomize_fixed_channel(cur->ws_info->cfg->fhss.fhss_uc_fixed_channel, cur->ws_info->hopping_schdule.number_of_channels, fhss_configuration.channel_mask);
+    uint8_t tmp_bc_fixed_channel = ws_randomize_fixed_channel(cur->ws_info->cfg->fhss.fhss_bc_fixed_channel, cur->ws_info->hopping_schdule.number_of_channels, fhss_configuration.channel_mask);
     fhss_configuration.unicast_fixed_channel = tmp_uc_fixed_channel;
     fhss_configuration.broadcast_fixed_channel = tmp_bc_fixed_channel;
     ns_fhss_ws_configuration_set(cur->ws_info->fhss_api, &fhss_configuration);
@@ -3655,9 +3672,6 @@ static void ws_bootstrap_event_handler(arm_event_s *event)
                 ws_bootstrap_ip_stack_reset(cur);
                 ws_pae_controller_auth_init(cur);
 
-                // Randomize fixed channels. Only used if channel plan is fixed.
-                cur->ws_info->cfg->fhss.fhss_uc_fixed_channel = ws_randomize_fixed_channel(cur->ws_info->cfg->fhss.fhss_uc_fixed_channel, cur->ws_info->hopping_schdule.number_of_channels);
-                cur->ws_info->cfg->fhss.fhss_bc_fixed_channel = ws_randomize_fixed_channel(cur->ws_info->cfg->fhss.fhss_bc_fixed_channel, cur->ws_info->hopping_schdule.number_of_channels);
                 if (cur->ws_info->cfg->gen.network_pan_id == 0xffff) {
                     cur->ws_info->network_pan_id = randLIB_get_random_in_range(0, 0xfffd);
                 } else {