FHSS: added critical states

source/Service_Libs/fhss/fhss_ws.c

@@ -28,6 +28,7 @@
 #include "eventOS_callback_timer.h"
 #include "randLIB.h"
 #include "ns_trace.h"
+#include "platform/arm_hal_interrupt.h"
 #include <string.h>
 
 #define TRACE_GROUP "fhss"
@@ -255,7 +256,11 @@ 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->callbacks.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;
+    }
     return (broadcast_interval-remaining_time) + time_to_tx;
 }
 
@@ -504,6 +509,7 @@ static int16_t fhss_ws_write_synch_info_callback(const fhss_api_t *api, uint8_t
     if (!fhss_structure || !ptr || (frame_type != FHSS_DATA_FRAME)) {
         return -1;
     }
+    platform_enter_critical();
     struct ws_ie_t header_ie;
     header_ie.id = WH_IE_ID;
     if (fhss_ws_ie_header_discover(ptr, length, &header_ie, WH_SUB_ID_UTT)) {
@@ -515,6 +521,7 @@ static int16_t fhss_ws_write_synch_info_callback(const fhss_api_t *api, uint8_t
         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);
     }
+    platform_exit_critical();
     //TODO return destination channel here
     return fhss_structure->rx_channel;
 }
@@ -660,18 +667,19 @@ 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;
     }
-
+    platform_enter_critical();
     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_structure->ws->is_on_bc_channel = false;
     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;
     // TODO: Calculate drift error
     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_set_txrx_slot_length(fhss_structure);
     fhss_structure->ws->bc_slot = bc_timing_info->broadcast_slot + slots_since_reception;
-
+    platform_exit_critical();
     //TODO: support multiple parents
     fhss_structure->ws->parent_bc_info = bc_timing_info;
     return 0;