fhss_ws.c

00001 /*
00002  * Copyright (c) 2018-2019, Arm Limited and affiliates.
00003  * SPDX-License-Identifier: Apache-2.0
00004  *
00005  * Licensed under the Apache License, Version 2.0 (the "License");
00006  * you may not use this file except in compliance with the License.
00007  * You may obtain a copy of the License at
00008  *
00009  *     http://www.apache.org/licenses/LICENSE-2.0
00010  *
00011  * Unless required by applicable law or agreed to in writing, software
00012  * distributed under the License is distributed on an "AS IS" BASIS,
00013  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014  * See the License for the specific language governing permissions and
00015  * limitations under the License.
00016  */
00017 #include "nsconfig.h"
00018 #include "ns_types.h"
00019 #include "fhss_api.h "
00020 #include "fhss_config.h "
00021 #include "fhss.h"
00022 #include "fhss_common.h"
00023 #include "fhss_statistics.h"
00024 #include "channel_list.h"
00025 #include "channel_functions.h"
00026 #include "fhss_ws.h"
00027 #include "nsdynmemLIB.h"
00028 #include "common_functions.h"
00029 #include "eventOS_callback_timer.h"
00030 #include "randLIB.h"
00031 #include "ns_trace.h"
00032 #include "platform/arm_hal_interrupt.h"
00033 #include <string.h>
00034 
00035 #define TRACE_GROUP "fhss"
00036 
00037 // Enable this flag to use channel traces
00038 // #define FHSS_CHANNEL_DEBUG
00039 // Enable this flag to use debug callbacks
00040 // #define FHSS_CHANNEL_DEBUG_CBS
00041 
00042 #ifdef FHSS_CHANNEL_DEBUG_CBS
00043 void (*fhss_uc_switch)(void) = NULL;
00044 void (*fhss_bc_switch)(void) = NULL;
00045 #endif /*FHSS_CHANNEL_DEBUG_CBS*/
00046 // Seconds to milliseconds
00047 #define S_TO_MS(x) (((int64_t)x)*1000)
00048 // Milliseconds to seconds
00049 #define MS_TO_S(x) divide_integer(x, 1000)
00050 // Seconds to microseconds
00051 #define S_TO_US(x) (((int64_t)x)*1000000)
00052 // Microseconds to seconds
00053 #define US_TO_S(x) divide_integer(x, 1000000)
00054 // Milliseconds to microseconds
00055 #define MS_TO_US(x) (((int64_t)x)*1000)
00056 // Microseconds to milliseconds
00057 #define US_TO_MS(x) divide_integer(x, 1000)
00058 // Milliseconds to nanoseconds
00059 #define MS_TO_NS(x) (((int64_t)x)*1000000)
00060 // Nanoseconds to milliseconds
00061 #define NS_TO_MS(x) divide_integer(x, 1000000)
00062 // Microseconds to nanoseconds
00063 #define US_TO_NS(x) (((int64_t)x)*1000)
00064 // Nanoseconds to microseconds
00065 #define NS_TO_US(x) divide_integer(x, 1000)
00066 #define DEF_2E24 0x1000000
00067 #define IE_HEADER_LENGTH_MASK 0x007f
00068 #define IE_HEADER_ID_MASK 0x7f80
00069 #define WH_IE_ID        0x2a
00070 #define WH_SUB_ID_UTT   1
00071 #define WH_SUB_ID_BT    2
00072 
00073 #ifdef HAVE_WS
00074 
00075 struct ws_ie_t {
00076     uint8_t *content_ptr;
00077     unsigned length: 7;
00078     uint8_t id;
00079 };
00080 
00081 static int fhss_ws_manage_channel_table_allocation(fhss_structure_t *fhss_structure, uint16_t channel_count);
00082 static void fhss_event_timer_cb(int8_t timer_id, uint16_t slots);
00083 static void fhss_ws_update_uc_channel_callback(fhss_structure_t *fhss_structure);
00084 static void fhss_unicast_handler(const fhss_api_t *fhss_api, uint16_t delay);
00085 static bool fhss_ws_check_tx_allowed(fhss_structure_t *fhss_structure);
00086 static uint32_t fhss_set_txrx_slot_length(fhss_structure_t *fhss_structure);
00087 
00088 // This function supports rounding up
00089 static int64_t divide_integer(int64_t dividend, int32_t divisor)
00090 {
00091     if (!divisor) {
00092         return dividend;
00093     }
00094     if (dividend < 0) {
00095         return (dividend - divisor / 2) / divisor;
00096     }
00097     return (dividend + divisor / 2) / divisor;
00098 }
00099 
00100 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)
00101 {
00102     uint32_t remaining_time_us = fhss_structure->platform_functions.fhss_get_remaining_slots(callback, fhss_structure->fhss_api);
00103     // When remaining time goes negative, use 0.
00104     if (remaining_time_us > max_timeout_us) {
00105         remaining_time_us = 0;
00106     }
00107     return remaining_time_us;
00108 }
00109 
00110 void fhss_ws_start_timer(fhss_structure_t *fhss_structure, uint32_t time, void (*callback)(const fhss_api_t *fhss_api, uint16_t))
00111 {
00112     // Number of millisecond slots in timeout(us)
00113     int32_t time_in_ms = divide_integer(time, 1000);
00114     // Reduce the compensation (per millisecond) from timeout.
00115     time -= NS_TO_US(time_in_ms * fhss_structure->ws->drift_per_millisecond_ns);
00116     fhss_start_timer(fhss_structure, time, callback);
00117 }
00118 
00119 fhss_structure_t *fhss_ws_enable(fhss_api_t *fhss_api, const fhss_ws_configuration_t *fhss_configuration, const fhss_timer_t *fhss_timer)
00120 {
00121     if (!fhss_api || !fhss_configuration || !fhss_timer) {
00122         tr_err("Invalid FHSS enable configuration");
00123         return NULL;
00124     }
00125     int channel_count = channel_list_count_channels(fhss_configuration->channel_mask);
00126     if (channel_count <= 0) {
00127         // There must be at least one configured channel in channel list
00128         return NULL;
00129     }
00130     fhss_structure_t *fhss_struct = fhss_allocate_instance(fhss_api, fhss_timer);
00131     if (!fhss_struct) {
00132         return NULL;
00133     }
00134     fhss_struct->ws = ns_dyn_mem_alloc(sizeof(fhss_ws_t));
00135     if (!fhss_struct->ws) {
00136         fhss_free_instance(fhss_api);
00137         return NULL;
00138     }
00139     memset(fhss_struct->ws, 0, sizeof(fhss_ws_t));
00140     if (fhss_ws_manage_channel_table_allocation(fhss_struct, channel_count)) {
00141         ns_dyn_mem_free(fhss_struct->ws);
00142         fhss_free_instance(fhss_api);
00143         tr_error("Failed to allocate channel tables");
00144         return NULL;
00145     }
00146 
00147     fhss_struct->fhss_event_timer = eventOS_callback_timer_register(fhss_event_timer_cb);
00148     fhss_struct->ws->fhss_configuration = *fhss_configuration;
00149     fhss_struct->number_of_channels = channel_count;
00150     fhss_ws_set_hop_count(fhss_struct, 0xff);
00151     fhss_struct->rx_channel = fhss_configuration->unicast_fixed_channel;
00152     fhss_struct->ws->min_synch_interval = DEFAULT_MIN_SYNCH_INTERVAL;
00153     fhss_set_txrx_slot_length(fhss_struct);
00154     ns_list_init(&fhss_struct->fhss_failed_tx_list);
00155     return fhss_struct;
00156 }
00157 
00158 static int fhss_ws_manage_channel_table_allocation(fhss_structure_t *fhss_structure, uint16_t channel_count)
00159 {
00160     // Must allocate channel table for TR51
00161     if (!fhss_structure->ws->tr51_channel_table && !fhss_structure->ws->tr51_output_table) {
00162         fhss_structure->ws->tr51_channel_table = ns_dyn_mem_alloc(sizeof(int16_t) * tr51_calc_nearest_prime_number(channel_count));
00163         if (!fhss_structure->ws->tr51_channel_table) {
00164             return -1;
00165         }
00166         fhss_structure->ws->tr51_output_table = ns_dyn_mem_alloc(sizeof(int16_t) * channel_count);
00167         if (!fhss_structure->ws->tr51_output_table) {
00168             ns_dyn_mem_free(fhss_structure->ws->tr51_channel_table);
00169             return -1;
00170         }
00171         tr51_init_channel_table(fhss_structure->ws->tr51_channel_table, channel_count);
00172     }
00173     return 0;
00174 }
00175 
00176 static uint32_t fhss_set_txrx_slot_length(fhss_structure_t *fhss_structure)
00177 {
00178     uint32_t number_of_tx_slots = ((fhss_structure->ws->fhss_configuration.fhss_broadcast_interval - fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval) / WS_MAX_TXRX_SLOT_LEN_MS) / 2;
00179     if (!number_of_tx_slots) {
00180         return 0;
00181     }
00182     fhss_structure->ws->txrx_slot_length_ms = (fhss_structure->ws->fhss_configuration.fhss_broadcast_interval - fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval) / (number_of_tx_slots * 2);
00183     return number_of_tx_slots;
00184 }
00185 
00186 static int32_t fhss_ws_calc_bc_channel(fhss_structure_t *fhss_structure)
00187 {
00188     int32_t next_channel = fhss_structure->ws->fhss_configuration.broadcast_fixed_channel;
00189 
00190     if (fhss_structure->ws->fhss_configuration.ws_bc_channel_function == WS_TR51CF) {
00191         next_channel = tr51_get_bc_channel_index(fhss_structure->ws->tr51_channel_table, fhss_structure->ws->tr51_output_table, fhss_structure->ws->bc_slot, fhss_structure->ws->fhss_configuration.bsi, fhss_structure->number_of_channels, NULL);
00192         if (++fhss_structure->ws->bc_slot == fhss_structure->number_of_channels) {
00193             fhss_structure->ws->bc_slot = 0;
00194         }
00195     } else if (fhss_structure->ws->fhss_configuration.ws_bc_channel_function == WS_DH1CF) {
00196         next_channel = dh1cf_get_bc_channel_index(fhss_structure->ws->bc_slot, fhss_structure->ws->fhss_configuration.bsi, fhss_structure->number_of_channels);
00197         fhss_structure->ws->bc_slot++;
00198     } else if (fhss_structure->ws->fhss_configuration.ws_bc_channel_function == WS_VENDOR_DEF_CF) {
00199         if (fhss_structure->ws->fhss_configuration.vendor_defined_cf) {
00200             next_channel = fhss_structure->ws->fhss_configuration.vendor_defined_cf(fhss_structure->fhss_api, fhss_structure->ws->bc_slot, NULL, fhss_structure->ws->fhss_configuration.bsi, fhss_structure->number_of_channels);
00201         }
00202     }
00203 #ifdef FHSS_CHANNEL_DEBUG
00204     tr_info("%"PRIu32" BC %u %u %u", fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api), next_channel, fhss_structure->ws->bc_slot, fhss_structure->ws->fhss_configuration.bsi);
00205 #endif /*FHSS_CHANNEL_DEBUG*/
00206     return next_channel;
00207 }
00208 
00209 static uint8_t calc_own_tx_trig_slot(uint8_t own_hop)
00210 {
00211     if (own_hop == 0xff) {
00212         return 0;
00213     }
00214     if (own_hop & 1) {
00215         own_hop--;
00216     }
00217     own_hop /= 2;
00218     return (own_hop & 1);
00219 }
00220 
00221 static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
00222 {
00223     int32_t next_channel;
00224     fhss_structure_t *fhss_structure = fhss_get_object_with_api(fhss_api);
00225     if (!fhss_structure) {
00226         return;
00227     }
00228 
00229     if (fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval == 0 || fhss_structure->ws->fhss_configuration.fhss_broadcast_interval == 0) {
00230         // stop broadcast schedule
00231         fhss_structure->ws->is_on_bc_channel = false;
00232         fhss_structure->ws->synchronization_time = 0;
00233         return;
00234     }
00235     if (fhss_structure->ws->is_on_bc_channel == false) {
00236         fhss_ws_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);
00237         fhss_structure->ws->is_on_bc_channel = true;
00238         next_channel = fhss_structure->ws->bc_channel = fhss_ws_calc_bc_channel(fhss_structure);
00239 
00240         /* Start timer with random timeout to trigger broadcast TX queue poll event.
00241          * Min random is 1/50 of the channel dwell interval.
00242          * Max random is 1/10 of the channel dwell interval.
00243          * Event timer resolution is 50us.
00244          */
00245         uint32_t bc_dwell_us = MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval);
00246         uint16_t bc_min_random = (bc_dwell_us / 50) / 50;
00247         uint16_t bc_max_random = (bc_dwell_us / 10) / 50;
00248         eventOS_callback_timer_start(fhss_structure->fhss_event_timer, randLIB_get_random_in_range(bc_min_random, bc_max_random));
00249     } else {
00250         fhss_structure->ws->unicast_start_time_us = fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api);
00251         uint32_t timeout = MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_broadcast_interval - fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval);
00252         fhss_ws_start_timer(fhss_structure, timeout - (delay * fhss_structure->platform_functions.fhss_resolution_divider), fhss_broadcast_handler);
00253         fhss_structure->ws->is_on_bc_channel = false;
00254         // Should return to own (unicast) listening channel after broadcast channel
00255         next_channel = fhss_structure->rx_channel;
00256         /* Start timer with random timeout to trigger unicast TX queue poll event.
00257          * For hops 0,1,4,5,8,9,...
00258          * Min random is 1/100 of the TX slot length.
00259          * Max random is 1/5 of the TX slot length.
00260          *
00261          * For hops 2,3,6,7,10,11,...
00262          * Min random is 1/100 of the TX slot length plus 0.5*TX slot length.
00263          * Max random is 1/10 of the TX slot length plus 0.5*TX slot length.
00264          * Event timer resolution is 50us.
00265          */
00266         // returns 1 if polling of TX queue is done on latter half of the TX slot
00267         uint8_t own_tx_trig_slot = calc_own_tx_trig_slot(fhss_structure->own_hop);
00268         uint32_t txrx_slot_length_us = MS_TO_US(fhss_structure->ws->txrx_slot_length_ms);
00269         uint16_t uc_min_random = (((txrx_slot_length_us / 2) * own_tx_trig_slot) / 50) + ((txrx_slot_length_us / 100) / 50);
00270         uint16_t uc_max_random = (((txrx_slot_length_us / 2) * own_tx_trig_slot) / 50) + ((txrx_slot_length_us / 5) / 50);
00271         bool tx_allowed = fhss_ws_check_tx_allowed(fhss_structure);
00272         if (!tx_allowed) {
00273             uc_min_random += (txrx_slot_length_us) / 50;
00274             uc_max_random += (txrx_slot_length_us) / 50;
00275         }
00276         eventOS_callback_timer_start(fhss_structure->fhss_event_timer, randLIB_get_random_in_range(uc_min_random, uc_max_random));
00277 
00278 #ifdef FHSS_CHANNEL_DEBUG
00279         tr_info("%"PRIu32" UC %u", fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api), fhss_structure->rx_channel);
00280 #endif /*FHSS_CHANNEL_DEBUG*/
00281     }
00282     fhss_structure->callbacks.change_channel(fhss_structure->fhss_api, next_channel);
00283 #ifdef FHSS_CHANNEL_DEBUG_CBS
00284     if (fhss_bc_switch) {
00285         fhss_bc_switch();
00286     }
00287 #endif /*FHSS_CHANNEL_DEBUG_CBS*/
00288 }
00289 
00290 static int own_floor(float value)
00291 {
00292     return (int)value;
00293 }
00294 
00295 static int own_ceil(float value)
00296 {
00297     int ivalue = (int)value;
00298     if (value == (float)ivalue) {
00299         return ivalue;
00300     }
00301     return ivalue + 1;
00302 }
00303 
00304 static void fhss_event_timer_cb(int8_t timer_id, uint16_t slots)
00305 {
00306     (void) slots;
00307     uint16_t queue_size = 0;
00308     fhss_structure_t *fhss_structure = fhss_get_object_with_timer_id(timer_id);
00309 
00310 
00311     if (fhss_structure->ws->is_on_bc_channel == true) {
00312         queue_size = fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, true);
00313     } else {
00314         // On unicast, start timer to trigger polling event on next TX slot
00315         uint32_t delay_between_tx_slots_us = MS_TO_US(fhss_structure->ws->txrx_slot_length_ms) * 2;
00316         // Timer could drift to RX slot when broadcast interval is high. Return timer to TX slot.
00317         if (fhss_ws_check_tx_allowed(fhss_structure) == false) {
00318             delay_between_tx_slots_us -= MS_TO_US(fhss_structure->ws->txrx_slot_length_ms - (calc_own_tx_trig_slot(fhss_structure->own_hop) * (fhss_structure->ws->txrx_slot_length_ms / 2)));
00319         }
00320         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))) {
00321             eventOS_callback_timer_start(fhss_structure->fhss_event_timer, delay_between_tx_slots_us / 50);
00322         }
00323         queue_size = fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, false);
00324     }
00325     if (queue_size) {
00326         fhss_structure->callbacks.tx_poll(fhss_structure->fhss_api);
00327     }
00328 }
00329 
00330 static uint32_t fhss_ws_calculate_ufsi(fhss_structure_t *fhss_structure, uint32_t tx_time)
00331 {
00332     uint8_t dwell_time = fhss_structure->ws->fhss_configuration.fhss_uc_dwell_interval;
00333     uint16_t cur_slot = fhss_structure->ws->uc_slot;
00334     if (cur_slot == 0) {
00335         cur_slot = fhss_structure->number_of_channels;
00336     }
00337     cur_slot--;
00338     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)));
00339     uint32_t time_to_tx = 0;
00340     uint32_t cur_time = fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api);
00341     if (cur_time < tx_time) {
00342         time_to_tx = US_TO_MS(tx_time - cur_time);
00343     }
00344     uint64_t ms_since_seq_start = (cur_slot * dwell_time) + (dwell_time - remaining_time_ms) + time_to_tx;
00345     uint32_t seq_length = 0x10000;
00346     if (fhss_structure->ws->fhss_configuration.ws_uc_channel_function == WS_TR51CF) {
00347         ms_since_seq_start %= (dwell_time * fhss_structure->number_of_channels);
00348         seq_length = fhss_structure->number_of_channels;
00349     }
00350     return own_floor((float)(ms_since_seq_start * DEF_2E24) / (seq_length * dwell_time));
00351 }
00352 
00353 static uint32_t fhss_ws_calculate_broadcast_interval_offset(fhss_structure_t *fhss_structure, uint32_t tx_time)
00354 {
00355     uint8_t dwell_time = fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval;
00356     uint32_t broadcast_interval = fhss_structure->ws->fhss_configuration.fhss_broadcast_interval;
00357     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)));
00358     if (fhss_structure->ws->is_on_bc_channel == true) {
00359         remaining_time_ms += (broadcast_interval - dwell_time);
00360     }
00361     uint32_t time_to_tx = 0;
00362     uint32_t cur_time = fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api);
00363     if (cur_time < tx_time) {
00364         time_to_tx = US_TO_MS(tx_time - cur_time);
00365     }
00366     return (broadcast_interval - remaining_time_ms) + time_to_tx;
00367 }
00368 
00369 static uint16_t fhss_ws_calculate_destination_slot(fhss_ws_neighbor_timing_info_t *neighbor_timing_info, uint32_t tx_time)
00370 {
00371     uint_fast24_t ufsi = neighbor_timing_info->uc_timing_info.ufsi;
00372     uint32_t ufsi_timestamp = neighbor_timing_info->uc_timing_info.utt_rx_timestamp;
00373     uint8_t dwell_time = neighbor_timing_info->uc_timing_info.unicast_dwell_interval;
00374     uint32_t seq_length = 0x10000;
00375     if (neighbor_timing_info->uc_timing_info.unicast_channel_function  == WS_TR51CF) {
00376         seq_length = neighbor_timing_info->uc_timing_info.unicast_number_of_channels;
00377     }
00378     uint32_t dest_ms_since_seq_start = own_ceil((float)((uint64_t)ufsi * seq_length * dwell_time) / DEF_2E24);
00379     return (own_floor(((float)(US_TO_MS(tx_time - ufsi_timestamp) + dest_ms_since_seq_start) / dwell_time)) % seq_length);
00380 }
00381 
00382 static uint32_t fhss_ws_get_sf_timeout_callback(fhss_structure_t *fhss_structure)
00383 {
00384     return MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_uc_dwell_interval);
00385 }
00386 
00387 static int16_t fhss_ws_synch_state_set_callback(const fhss_api_t *api, fhss_states fhss_state, uint16_t pan_id)
00388 {
00389     (void) pan_id;
00390     fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
00391     if (!fhss_structure) {
00392         return -1;
00393     }
00394     if (fhss_state == FHSS_SYNCHRONIZED) {
00395         uint32_t fhss_broadcast_interval = fhss_structure->ws->fhss_configuration.fhss_broadcast_interval;
00396         uint8_t fhss_bc_dwell_interval = fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval;
00397 
00398         // Start broadcast schedule when BC intervals are known
00399         if (fhss_broadcast_interval && fhss_bc_dwell_interval) {
00400             fhss_broadcast_handler(fhss_structure->fhss_api, 0);
00401         }
00402         // Start unicast schedule
00403         if ((fhss_structure->ws->fhss_configuration.ws_uc_channel_function != WS_FIXED_CHANNEL)) {
00404             fhss_ws_update_uc_channel_callback(fhss_structure);
00405             fhss_ws_start_timer(fhss_structure, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_uc_dwell_interval), fhss_unicast_handler);
00406             fhss_structure->ws->unicast_timer_running = true;
00407         }
00408     } else if (fhss_state == FHSS_UNSYNCHRONIZED) {
00409         fhss_structure->ws->synchronization_time = 0;
00410     }
00411 
00412     fhss_structure->fhss_state = fhss_state;
00413     int16_t current_channel = fhss_structure->rx_channel;
00414     if (fhss_structure->ws->is_on_bc_channel == true) {
00415         current_channel = fhss_structure->ws->bc_channel;
00416     }
00417     return current_channel;
00418 }
00419 
00420 static void fhss_ws_update_uc_channel_callback(fhss_structure_t *fhss_structure)
00421 {
00422     uint8_t mac_address[8];
00423     int32_t next_channel = fhss_structure->rx_channel;
00424     fhss_structure->callbacks.read_mac_address(fhss_structure->fhss_api, mac_address);
00425     if (fhss_structure->ws->fhss_configuration.ws_uc_channel_function == WS_FIXED_CHANNEL) {
00426         return;
00427     } else if (fhss_structure->ws->fhss_configuration.ws_uc_channel_function == WS_TR51CF) {
00428         next_channel = fhss_structure->rx_channel = tr51_get_uc_channel_index(fhss_structure->ws->tr51_channel_table, fhss_structure->ws->tr51_output_table, fhss_structure->ws->uc_slot, mac_address, fhss_structure->number_of_channels, NULL);
00429         if (++fhss_structure->ws->uc_slot == fhss_structure->number_of_channels) {
00430             fhss_structure->ws->uc_slot = 0;
00431         }
00432     } else if (fhss_structure->ws->fhss_configuration.ws_uc_channel_function == WS_DH1CF) {
00433         next_channel = fhss_structure->rx_channel = dh1cf_get_uc_channel_index(fhss_structure->ws->uc_slot, mac_address, fhss_structure->number_of_channels);
00434         fhss_structure->ws->uc_slot++;
00435     } else if (fhss_structure->ws->fhss_configuration.ws_uc_channel_function == WS_VENDOR_DEF_CF) {
00436         if (fhss_structure->ws->fhss_configuration.vendor_defined_cf) {
00437             next_channel = fhss_structure->rx_channel = fhss_structure->ws->fhss_configuration.vendor_defined_cf(fhss_structure->fhss_api, fhss_structure->ws->bc_slot, mac_address, fhss_structure->ws->fhss_configuration.bsi, fhss_structure->number_of_channels);
00438         }
00439     }
00440     // Do not switch unicast channel when broadcast channel is active.
00441     if (fhss_structure->ws->is_on_bc_channel == true) {
00442         return;
00443     }
00444 #ifdef FHSS_CHANNEL_DEBUG
00445     tr_info("%"PRIu32" UC %u %u", fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api), next_channel, fhss_structure->ws->uc_slot);
00446 #endif /*FHSS_CHANNEL_DEBUG*/
00447     fhss_structure->callbacks.change_channel(fhss_structure->fhss_api, next_channel);
00448 #ifdef FHSS_CHANNEL_DEBUG_CBS
00449     if (fhss_uc_switch) {
00450         fhss_uc_switch();
00451     }
00452 #endif /*FHSS_CHANNEL_DEBUG_CBS*/
00453 }
00454 
00455 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)
00456 {
00457     (void) frame_type;
00458     (void) frame_length;
00459     (void) phy_header_length;
00460     (void) phy_tail_length;
00461     fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
00462     if (!fhss_structure) {
00463         return -1;
00464     }
00465     if (is_broadcast_addr) {
00466         return 0;
00467     }
00468     // Do not allow unicast destination on broadcast channel
00469     if (!is_broadcast_addr && (fhss_structure->ws->is_on_bc_channel == true)) {
00470         return -1;
00471     }
00472     // Check TX/RX slot
00473     if (!fhss_ws_check_tx_allowed(fhss_structure)) {
00474         return -1;
00475     }
00476     if (fhss_structure->fhss_state == FHSS_SYNCHRONIZED) {
00477         fhss_ws_neighbor_timing_info_t *neighbor_timing_info = fhss_structure->ws->get_neighbor_info(api, destination_address);
00478         if (!neighbor_timing_info) {
00479             fhss_stats_update(fhss_structure, STATS_FHSS_UNKNOWN_NEIGHBOR, 1);
00480             return -2;
00481         }
00482         // TODO: WS bootstrap has to store neighbors number of channels
00483         if (neighbor_timing_info->uc_timing_info.unicast_number_of_channels == 0) {
00484             neighbor_timing_info->uc_timing_info.unicast_number_of_channels = fhss_structure->number_of_channels;
00485         }
00486         uint16_t destination_slot = fhss_ws_calculate_destination_slot(neighbor_timing_info, tx_time);
00487         int32_t tx_channel = neighbor_timing_info->uc_timing_info.fixed_channel;
00488         if (neighbor_timing_info->uc_timing_info.unicast_channel_function == WS_TR51CF) {
00489             tx_channel = tr51_get_uc_channel_index(fhss_structure->ws->tr51_channel_table, fhss_structure->ws->tr51_output_table, destination_slot, destination_address, neighbor_timing_info->uc_timing_info.unicast_number_of_channels, NULL);
00490         } else if (neighbor_timing_info->uc_timing_info.unicast_channel_function == WS_DH1CF) {
00491             tx_channel = dh1cf_get_uc_channel_index(destination_slot, destination_address, neighbor_timing_info->uc_timing_info.unicast_number_of_channels);
00492         } else if (neighbor_timing_info->uc_timing_info.unicast_channel_function == WS_VENDOR_DEF_CF) {
00493             if (fhss_structure->ws->fhss_configuration.vendor_defined_cf) {
00494                 tx_channel = fhss_structure->ws->fhss_configuration.vendor_defined_cf(fhss_structure->fhss_api, fhss_structure->ws->bc_slot, destination_address, fhss_structure->ws->fhss_configuration.bsi, neighbor_timing_info->uc_timing_info.unicast_number_of_channels);
00495             } else {
00496                 tr_err("FHSS: vendor defined configuration failed");
00497                 return -1;
00498             }
00499         }
00500 #ifdef FHSS_CHANNEL_DEBUG
00501         tr_debug("TX channel: %u %u %s", tx_channel, destination_slot + 1, trace_array(destination_address, 8));
00502 #endif /*FHSS_CHANNEL_DEBUG*/
00503         fhss_structure->callbacks.change_channel(fhss_structure->fhss_api, tx_channel);
00504     }
00505     return 0;
00506 }
00507 
00508 static bool fhss_check_bad_channel(fhss_structure_t *fhss_structure, uint8_t handle)
00509 {
00510     // When operating on fixed channel, we must ignore the bad channel check.
00511     if (fhss_structure->ws->fhss_configuration.ws_uc_channel_function == WS_FIXED_CHANNEL) {
00512         return true;
00513     }
00514     fhss_failed_tx_t *failed_tx = fhss_failed_handle_find(fhss_structure, handle);
00515     if (!failed_tx) {
00516         return true;
00517     }
00518     if (failed_tx->bad_channel == fhss_structure->rx_channel) {
00519         return false;
00520     }
00521     return true;
00522 }
00523 
00524 static bool fhss_ws_check_tx_allowed(fhss_structure_t *fhss_structure)
00525 {
00526     // Ignore TX/RX slots
00527     if (fhss_structure->own_hop == 0xff) {
00528         return true;
00529     }
00530     // Currently on broadcast channel
00531     if (fhss_structure->ws->is_on_bc_channel == true) {
00532         return true;
00533     }
00534     uint32_t number_of_tx_slots = fhss_set_txrx_slot_length(fhss_structure);
00535     // Allow transmission when broadcast interval is very short comparing to MAX slot length
00536     if (!number_of_tx_slots) {
00537         return true;
00538     }
00539 
00540     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;
00541     uint32_t tx_slot_begin_ms = (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));
00542     tx_slot_begin_ms = tx_slot_begin_ms - (((tx_slot_begin_ms - remaining_time_ms) / (2 * fhss_structure->ws->txrx_slot_length_ms)) * (2 * fhss_structure->ws->txrx_slot_length_ms));
00543     uint32_t rx_slot_begin_ms = tx_slot_begin_ms - fhss_structure->ws->txrx_slot_length_ms;
00544     // Check if we are currently on TX slot.
00545     if ((remaining_time_ms <= tx_slot_begin_ms) && (remaining_time_ms > rx_slot_begin_ms)) {
00546         return true;
00547     }
00548 
00549     return false;
00550 }
00551 
00552 static bool fhss_ws_check_tx_time(fhss_structure_t *fhss_structure, uint16_t tx_length, uint8_t phy_header_length, uint8_t phy_tail_length)
00553 {
00554     if (!fhss_structure->ws->fhss_configuration.fhss_broadcast_interval || !fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval) {
00555         return true;
00556     }
00557     uint32_t tx_time = fhss_get_tx_time(fhss_structure, tx_length, phy_header_length, phy_tail_length);
00558     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));
00559     if (tx_time > time_to_bc_channel_us) {
00560         return false;
00561     }
00562     return true;
00563 }
00564 
00565 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)
00566 {
00567     (void) frame_type;
00568     fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
00569     if (!fhss_structure) {
00570         return true;
00571     }
00572     // Do not allow broadcast destination on unicast channel
00573     if (is_broadcast_addr && (fhss_structure->ws->is_on_bc_channel == false)) {
00574         return false;
00575     }
00576     // Do not allow unicast destination on broadcast channel
00577     if (!is_broadcast_addr && (fhss_structure->ws->is_on_bc_channel == true)) {
00578         return false;
00579     }
00580     // This condition will check that message is not sent on bad channel
00581     if (fhss_check_bad_channel(fhss_structure, handle) == false) {
00582         return false;
00583     }
00584     // Check that there is enough unicast TX time before next broadcast channel. We try to avoid delaying the change to broadcast channel because of ongoing transmission.
00585     if (!is_broadcast_addr && !fhss_ws_check_tx_time(fhss_structure, frame_length, phy_header_length, phy_tail_length)) {
00586         return false;
00587     }
00588     // Check TX/RX slot for unicast frames
00589     if (!is_broadcast_addr && !fhss_ws_check_tx_allowed(fhss_structure)) {
00590         return false;
00591     }
00592     return true;
00593 }
00594 
00595 static uint8_t fhss_ws_ie_header_discover(uint8_t *header_ptr, uint16_t length, struct ws_ie_t *header_ie, uint8_t sub_id)
00596 {
00597     struct ws_ie_t ie_element;
00598     uint8_t *sub_id_ptr;
00599     uint16_t ie_dummy;
00600     while (length > 2) {
00601         ie_dummy = common_read_16_bit_inverse(header_ptr);
00602         ie_element.length = (ie_dummy & IE_HEADER_LENGTH_MASK);
00603         ie_element.id = ((ie_dummy & IE_HEADER_ID_MASK) >> 7);
00604         ie_element.content_ptr = header_ptr + 2;
00605         sub_id_ptr = ie_element.content_ptr;
00606         if (ie_element.length && (header_ie->id == ie_element.id) && (*sub_id_ptr == sub_id)) {
00607             sub_id_ptr++;
00608             ie_element.length--;
00609             header_ie->content_ptr = sub_id_ptr;
00610             header_ie->length = ie_element.length;
00611             return ie_element.length;
00612         }
00613         length -= ie_element.length + 2;
00614         header_ptr += ie_element.length + 2;
00615     }
00616     return 0;
00617 }
00618 
00619 static int16_t fhss_ws_write_synch_info_callback(const fhss_api_t *api, uint8_t *ptr, uint8_t length, int frame_type, uint32_t tx_time)
00620 {
00621     fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
00622     if (!fhss_structure || !ptr || (frame_type != FHSS_DATA_FRAME)) {
00623         return -1;
00624     }
00625     platform_enter_critical();
00626     struct ws_ie_t header_ie;
00627     header_ie.id = WH_IE_ID;
00628     if (fhss_ws_ie_header_discover(ptr, length, &header_ie, WH_SUB_ID_UTT)) {
00629         uint32_t ufsi = fhss_ws_calculate_ufsi(fhss_structure, tx_time);
00630         common_write_24_bit_inverse(ufsi, header_ie.content_ptr + 1);
00631     }
00632     if (fhss_ws_ie_header_discover(ptr, length, &header_ie, WH_SUB_ID_BT)) {
00633         uint32_t broadcast_interval_offset = fhss_ws_calculate_broadcast_interval_offset(fhss_structure, tx_time);
00634         common_write_16_bit_inverse(fhss_structure->ws->bc_slot, header_ie.content_ptr);
00635         common_write_24_bit_inverse(broadcast_interval_offset, header_ie.content_ptr + 2);
00636     }
00637     platform_exit_critical();
00638     //TODO return destination channel here
00639     return fhss_structure->rx_channel;
00640 }
00641 
00642 static void fhss_ws_data_tx_done_callback(const fhss_api_t *api, bool waiting_ack, bool tx_completed, uint8_t handle)
00643 {
00644     fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
00645     if (!fhss_structure) {
00646         return;
00647     }
00648     if (fhss_structure->fhss_state == FHSS_SYNCHRONIZED) {
00649         if (waiting_ack == false) {
00650             if (fhss_structure->ws->is_on_bc_channel == false) {
00651                 fhss_structure->callbacks.change_channel(fhss_structure->fhss_api, fhss_structure->rx_channel);
00652             } else {
00653                 fhss_structure->callbacks.change_channel(fhss_structure->fhss_api, fhss_structure->ws->bc_channel);
00654             }
00655         }
00656     }
00657     // Buffer was successfully transmitted. Remove stored failure handle if exists.
00658     if (tx_completed == true) {
00659         fhss_failed_tx_t *fhss_failed_tx = fhss_failed_handle_find(fhss_structure, handle);
00660         if (fhss_failed_tx) {
00661             fhss_failed_handle_remove(fhss_structure, handle);
00662         }
00663     }
00664 }
00665 
00666 static bool fhss_ws_data_tx_fail_callback(const fhss_api_t *api, uint8_t handle, int frame_type)
00667 {
00668     fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
00669     if (!fhss_structure) {
00670         return false;
00671     }
00672     // Only use channel retries when device is synchronized
00673     if (fhss_structure->fhss_state == FHSS_UNSYNCHRONIZED) {
00674         return false;
00675     }
00676 
00677     // Use channel retries only for data frames
00678     if (FHSS_DATA_FRAME != frame_type) {
00679         return false;
00680     }
00681 
00682     fhss_failed_tx_t *fhss_failed_tx = fhss_failed_handle_find(fhss_structure, handle);
00683     if (fhss_failed_tx) {
00684         fhss_failed_tx->retries_done++;
00685         if (fhss_failed_tx->retries_done >= WS_NUMBER_OF_CHANNEL_RETRIES) {
00686             // No more retries. Return false to stop retransmitting.
00687             fhss_failed_handle_remove(fhss_structure, handle);
00688             return false;
00689         }
00690         fhss_failed_tx->bad_channel = fhss_structure->rx_channel;
00691     } else {
00692         // Create new failure handle and return true to retransmit
00693         fhss_failed_handle_add(fhss_structure, handle, fhss_structure->rx_channel);
00694     }
00695     fhss_stats_update(fhss_structure, STATS_FHSS_CHANNEL_RETRY, 1);
00696     return true;
00697 }
00698 
00699 static void fhss_ws_receive_frame_callback(const fhss_api_t *api, uint16_t pan_id, uint8_t *source_address, uint32_t timestamp, uint8_t *synch_info, int frame_type)
00700 {
00701     (void) api;
00702     (void) pan_id;
00703     (void) source_address;
00704     (void) timestamp;
00705     (void) synch_info;
00706     (void) frame_type;
00707 }
00708 
00709 static bool fhss_ws_is_broadcast_channel_callback(const fhss_api_t *api)
00710 {
00711     fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
00712     if (!fhss_structure) {
00713         return true;
00714     }
00715     return fhss_structure->ws->is_on_bc_channel;
00716 }
00717 
00718 static bool fhss_ws_use_broadcast_queue_cb(const fhss_api_t *api, bool is_broadcast_addr, int frame_type)
00719 {
00720     (void) frame_type;
00721     fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
00722     if (!fhss_structure) {
00723         return false;
00724     }
00725     // Broadcast packets are stored in broadcast queue
00726     return is_broadcast_addr;
00727 }
00728 
00729 static uint32_t fhss_ws_get_retry_period_callback(const fhss_api_t *api, uint8_t *destination_address, uint16_t phy_mtu)
00730 {
00731     (void) destination_address;
00732     (void) phy_mtu;
00733     uint32_t return_value = 0;
00734     fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
00735     if (!fhss_structure) {
00736         return return_value;
00737     }
00738     if (fhss_structure->own_hop == 0xff) {
00739         return return_value;
00740     }
00741     if (fhss_structure->ws->is_on_bc_channel == true) {
00742         return return_value;
00743     }
00744 
00745     uint32_t txrx_slot_length_us = MS_TO_US(fhss_structure->ws->txrx_slot_length_ms);
00746     uint32_t unicast_start_us = fhss_structure->ws->unicast_start_time_us;
00747     uint32_t cur_time_us = fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api);
00748     uint32_t tx_trig_offset_us = (txrx_slot_length_us / 2) * calc_own_tx_trig_slot(fhss_structure->own_hop);
00749 
00750     uint32_t next_tx_trig_slot_start_us = unicast_start_us + (txrx_slot_length_us * !fhss_ws_check_tx_allowed(fhss_structure)) + tx_trig_offset_us;
00751     uint32_t next_tx_trig_slot_end_us = next_tx_trig_slot_start_us + (txrx_slot_length_us / 2);
00752     while ((next_tx_trig_slot_start_us < cur_time_us) || ((next_tx_trig_slot_start_us - cur_time_us) > (uint32_t) MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_broadcast_interval))) {
00753         if (cur_time_us < next_tx_trig_slot_end_us) {
00754             return 0;
00755         }
00756         next_tx_trig_slot_start_us += (txrx_slot_length_us * 2);
00757         next_tx_trig_slot_end_us = next_tx_trig_slot_start_us + (txrx_slot_length_us / 2);
00758     }
00759     return_value = next_tx_trig_slot_start_us - cur_time_us;
00760     uint32_t time_to_bc_channel = get_remaining_slots_us(fhss_structure, fhss_broadcast_handler, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_broadcast_interval));
00761     if (time_to_bc_channel <= return_value) {
00762         return_value += MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval);
00763     }
00764 
00765     return return_value;
00766 }
00767 
00768 static void fhss_unicast_handler(const fhss_api_t *fhss_api, uint16_t delay)
00769 {
00770     uint32_t timeout = 0;
00771     fhss_structure_t *fhss_structure = fhss_get_object_with_api(fhss_api);
00772     if (!fhss_structure) {
00773         return;
00774     }
00775     timeout = fhss_ws_get_sf_timeout_callback(fhss_structure);
00776     if (!timeout) {
00777         fhss_stop_timer(fhss_structure, fhss_unicast_handler);
00778         fhss_structure->ws->unicast_timer_running = false;
00779         return;
00780     }
00781     fhss_ws_start_timer(fhss_structure, timeout - (delay * fhss_structure->platform_functions.fhss_resolution_divider), fhss_unicast_handler);
00782     fhss_structure->ws->unicast_timer_running = true;
00783     fhss_ws_update_uc_channel_callback(fhss_structure);
00784     // Unless we have broadcast schedule, we have to poll unicast queue when changing channel. This is randomized by the unicast schedule.
00785     if (!fhss_structure->ws->fhss_configuration.fhss_broadcast_interval || !fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval) {
00786         if (fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, false)) {
00787             fhss_structure->callbacks.tx_poll(fhss_structure->fhss_api);
00788         }
00789     }
00790 }
00791 
00792 int fhss_ws_set_callbacks(fhss_structure_t *fhss_structure)
00793 {
00794     // Set external API
00795     fhss_structure->fhss_api->is_broadcast_channel = &fhss_ws_is_broadcast_channel_callback;
00796     fhss_structure->fhss_api->use_broadcast_queue = &fhss_ws_use_broadcast_queue_cb;
00797     fhss_structure->fhss_api->tx_handle = &fhss_ws_tx_handle_callback;
00798     fhss_structure->fhss_api->check_tx_conditions = &fhss_ws_check_tx_conditions_callback;
00799     fhss_structure->fhss_api->receive_frame = &fhss_ws_receive_frame_callback;
00800     fhss_structure->fhss_api->data_tx_done = &fhss_ws_data_tx_done_callback;
00801     fhss_structure->fhss_api->data_tx_fail = &fhss_ws_data_tx_fail_callback;
00802     fhss_structure->fhss_api->synch_state_set = &fhss_ws_synch_state_set_callback;
00803     fhss_structure->fhss_api->read_timestamp = NULL;
00804     fhss_structure->fhss_api->get_retry_period = &fhss_ws_get_retry_period_callback;
00805     fhss_structure->fhss_api->write_synch_info = &fhss_ws_write_synch_info_callback;
00806     fhss_structure->fhss_api->init_callbacks = &fhss_init_callbacks_cb;
00807     return 0;
00808 }
00809 
00810 int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8], const broadcast_timing_info_t *bc_timing_info, const bool force_synch)
00811 {
00812     (void) eui64;
00813     if (!fhss_structure->ws) {
00814         return -1;
00815     }
00816     if (!bc_timing_info->broadcast_interval || !bc_timing_info->broadcast_dwell_interval) {
00817         return -1;
00818     }
00819     if (((uint32_t)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) {
00820         return 0;
00821     }
00822     platform_enter_critical();
00823     uint16_t own_bc_slot = fhss_structure->ws->bc_slot;
00824     uint32_t prev_synchronization_time = fhss_structure->ws->synchronization_time;
00825     fhss_structure->ws->synchronization_time = fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api);
00826     uint32_t time_since_last_synch_us = fhss_structure->ws->synchronization_time - prev_synchronization_time;
00827     uint32_t own_bc_interval_offset = fhss_ws_calculate_broadcast_interval_offset(fhss_structure, fhss_structure->ws->synchronization_time);
00828     fhss_stop_timer(fhss_structure, fhss_broadcast_handler);
00829     uint32_t time_from_reception_ms = US_TO_MS(fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api) - bc_timing_info->bt_rx_timestamp);
00830     uint32_t true_bc_interval_offset = (bc_timing_info->broadcast_interval_offset + time_from_reception_ms) % bc_timing_info->broadcast_interval;
00831     if (true_bc_interval_offset >= bc_timing_info->broadcast_dwell_interval) {
00832         fhss_structure->ws->is_on_bc_channel = false;
00833     }
00834     uint32_t timeout = MS_TO_US(bc_timing_info->broadcast_interval - true_bc_interval_offset);
00835 
00836     if (fhss_structure->ws->is_on_bc_channel) {
00837         timeout -= MS_TO_US(bc_timing_info->broadcast_interval - bc_timing_info->broadcast_dwell_interval);
00838     }
00839     fhss_ws_start_timer(fhss_structure, timeout, fhss_broadcast_handler);
00840     uint16_t slots_since_reception = (bc_timing_info->broadcast_interval_offset + time_from_reception_ms) / bc_timing_info->broadcast_interval;
00841     // TODO: Calculate drift error
00842     fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval = bc_timing_info->broadcast_dwell_interval;
00843     fhss_structure->ws->fhss_configuration.fhss_broadcast_interval = bc_timing_info->broadcast_interval;
00844     fhss_set_txrx_slot_length(fhss_structure);
00845     if (fhss_structure->ws->fhss_configuration.ws_bc_channel_function == WS_FIXED_CHANNEL) {
00846         fhss_structure->ws->bc_slot = 0;
00847     } else {
00848         fhss_structure->ws->bc_slot = bc_timing_info->broadcast_slot + slots_since_reception;
00849     }
00850     if (fhss_structure->ws->fhss_configuration.ws_bc_channel_function == WS_TR51CF) {
00851         fhss_structure->ws->bc_slot %= fhss_structure->number_of_channels;
00852     }
00853     platform_exit_critical();
00854     //TODO: support multiple parents
00855     fhss_structure->ws->parent_bc_info = bc_timing_info;
00856     if (prev_synchronization_time && fhss_structure->ws->fhss_configuration.ws_bc_channel_function != WS_FIXED_CHANNEL) {
00857         //TODO: Compensation for fixed channel configuration
00858         if (SYNCH_COMPENSATION_MIN_INTERVAL <= US_TO_S(time_since_last_synch_us)) {
00859             // Update clock drift
00860             int32_t drift_per_ms_tmp = divide_integer((int32_t)MS_TO_NS((true_bc_interval_offset - own_bc_interval_offset) + ((int32_t)(fhss_structure->ws->bc_slot - own_bc_slot) * bc_timing_info->broadcast_interval)), US_TO_MS(time_since_last_synch_us));
00861             if (drift_per_ms_tmp > MAX_DRIFT_COMPENSATION_STEP) {
00862                 drift_per_ms_tmp = MAX_DRIFT_COMPENSATION_STEP;
00863             } else if (drift_per_ms_tmp < -MAX_DRIFT_COMPENSATION_STEP) {
00864                 drift_per_ms_tmp = -MAX_DRIFT_COMPENSATION_STEP;
00865             }
00866             fhss_structure->ws->drift_per_millisecond_ns += drift_per_ms_tmp;
00867             fhss_stats_update(fhss_structure, STATS_FHSS_DRIFT_COMP, NS_TO_US((int64_t)(fhss_structure->ws->drift_per_millisecond_ns * bc_timing_info->broadcast_dwell_interval)));
00868         }
00869         tr_debug("synch to parent: %s, drift: %"PRIi32"ms in %"PRIu64" seconds, compensation: %"PRIi32"ns per ms", trace_array(eui64, 8), true_bc_interval_offset - own_bc_interval_offset + ((int32_t)(fhss_structure->ws->bc_slot - own_bc_slot) * bc_timing_info->broadcast_interval), US_TO_S(time_since_last_synch_us), fhss_structure->ws->drift_per_millisecond_ns);
00870     }
00871     fhss_stats_update(fhss_structure, STATS_FHSS_SYNCH_INTERVAL, US_TO_S(time_since_last_synch_us));
00872     return 0;
00873 }
00874 
00875 int fhss_ws_remove_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8])
00876 {
00877     (void) eui64;
00878     if (!fhss_structure->ws) {
00879         return -1;
00880     }
00881     fhss_structure->ws->parent_bc_info = NULL;
00882     return 0;
00883 }
00884 
00885 int fhss_ws_configuration_set(fhss_structure_t *fhss_structure, const fhss_ws_configuration_t *fhss_configuration)
00886 {
00887     int channel_count = channel_list_count_channels(fhss_configuration->channel_mask);
00888     if (channel_count <= 0) {
00889         return -1;
00890     }
00891     platform_enter_critical();
00892     if (fhss_configuration->ws_uc_channel_function == WS_FIXED_CHANNEL || fhss_configuration->fhss_uc_dwell_interval == 0) {
00893         fhss_stop_timer(fhss_structure, fhss_unicast_handler);
00894         fhss_structure->ws->unicast_timer_running = false;
00895     }
00896     if (fhss_configuration->ws_bc_channel_function == WS_FIXED_CHANNEL || (fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval == 0 || fhss_structure->ws->fhss_configuration.fhss_broadcast_interval == 0)) {
00897         fhss_structure->ws->synchronization_time = 0;
00898     }
00899 
00900     if ((fhss_structure->ws->unicast_timer_running == false) && (fhss_configuration->ws_uc_channel_function != WS_FIXED_CHANNEL) && fhss_configuration->fhss_uc_dwell_interval) {
00901         fhss_ws_start_timer(fhss_structure, MS_TO_US(fhss_configuration->fhss_uc_dwell_interval), fhss_unicast_handler);
00902         fhss_structure->ws->unicast_timer_running = true;
00903     }
00904     fhss_structure->ws->fhss_configuration = *fhss_configuration;
00905     fhss_structure->number_of_channels = channel_count;
00906     if (fhss_configuration->ws_uc_channel_function == WS_FIXED_CHANNEL) {
00907         fhss_structure->rx_channel = fhss_configuration->unicast_fixed_channel;
00908     }
00909     platform_exit_critical();
00910     tr_info("fhss Configuration set, UC channel: %d, BC channel: %d, UC CF: %d, BC CF: %d, channels: %d, uc dwell: %d, bc dwell: %d, bc interval: %"PRIu32", bsi:%d",
00911             fhss_structure->ws->fhss_configuration.unicast_fixed_channel,
00912             fhss_structure->ws->fhss_configuration.broadcast_fixed_channel,
00913             fhss_structure->ws->fhss_configuration.ws_uc_channel_function,
00914             fhss_structure->ws->fhss_configuration.ws_bc_channel_function,
00915             fhss_structure->number_of_channels,
00916             fhss_structure->ws->fhss_configuration.fhss_uc_dwell_interval,
00917             fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval,
00918             fhss_structure->ws->fhss_configuration.fhss_broadcast_interval,
00919             fhss_structure->ws->fhss_configuration.bsi);
00920     return 0;
00921 }
00922 
00923 int fhss_ws_set_hop_count(fhss_structure_t *fhss_structure, const uint8_t hop_count)
00924 {
00925     fhss_structure->own_hop = hop_count;
00926     fhss_stats_update(fhss_structure, STATS_FHSS_HOP_COUNT, fhss_structure->own_hop);
00927     return 0;
00928 }
00929 
00930 #endif // HAVE_WS