Merge pull request ARMmbed#1584 from ARMmbed/IOTTHD-2055

FHSS: Synch callback to write ufsi

Author: Jarkko Paso

nanostack/fhss_api.h

@@ -38,15 +38,6 @@ typedef struct fhss_callback fhss_callback_t;
 #define FHSS_SYNCH_REQUEST_FRAME    1   /**< FHSS synchronization request frame */
 #define FHSS_DATA_FRAME             2   /**< FHSS data frame */
 
-/**
- * @brief FHSS information element types.
- */
-#define FHSS_UTT_IE              0   /**< UTT-IE */
-#define FHSS_BT_IE               1   /**< BT-IE */
-#define FHSS_US_IE               2   /**< US-IE */
-#define FHSS_BS_IE               3   /**< BS-IE */
-#define FHSS_PLAIN_SYNCH_INFO    4   /**< Plain synchronization information */
-
 /**
  * @brief FHSS synchronization info length.
  */
@@ -91,7 +82,6 @@ typedef bool fhss_use_broadcast_queue(const fhss_api_t *api, bool is_broadcast_a
  * @return -1 Transmission of the packet is currently not allowed, try again.
  * @return -2 Invalid api.
  * @return -3 Broadcast packet on Unicast channel (not allowed), push packet back to queue.
- * @return -4 Synchronization info missing.
  */
 typedef int fhss_tx_handle(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);
 
@@ -164,13 +154,13 @@ typedef uint16_t fhss_get_retry_period(const fhss_api_t *api, uint8_t *destinati
 /**
  * @brief Write synchronization info to given pointer.
  * @param api FHSS instance.
- * @param info_ptr Pointer to written data.
- * @param info_type Type of the written info (UTT-IE, BT-IE, US-IE, BS-IE).
- * @param frame_type_id Frame type of packet which will be written in info element.
+ * @param ptr Pointer to data. Start of written data for Synch frame. Start of IE header for Data frame.
+ * @param length Length of IE header. Ignored when Synch frame.
+ * @param frame_type Frame type of packet (Frames types are defined by FHSS api).
  * @param tx_time TX time must be referenced to the first symbol following the SFD of the transmitted frame.
  * @return -1 on fail, write length otherwise.
  */
-typedef int16_t fhss_write_synch_info(const fhss_api_t *api, uint8_t *info_ptr, int info_type, int frame_type_id, uint32_t tx_time);
+typedef int16_t fhss_write_synch_info(const fhss_api_t *api, uint8_t *ptr, uint8_t length, int frame_type, uint32_t tx_time);
 
 /**
  * @brief Initialize MAC functions.

nanostack/platform/arm_hal_phy.h

@@ -56,7 +56,7 @@ typedef enum {
     PHY_EXTENSION_READ_LINK_STATUS, /**< Net library could read link status. */
     PHY_EXTENSION_CONVERT_SIGNAL_INFO, /**< Convert signal info. */
     PHY_EXTENSION_ACCEPT_ANY_BEACON, /**< Set boolean true or false for accept beacon from other Pan-ID than configured. Default value should be false */
-    PHY_EXTENSION_SET_TX_TIME,  /**< Net library sets transmission time based on global time stamp. Max. 65ms from setting to TX */
+    PHY_EXTENSION_SET_TX_TIME,  /**< Net library sets transmission time based on global time stamp. Max. 65ms from setting to TX. If TX time is set to zero, it should be ignored.*/
     PHY_EXTENSION_READ_RX_TIME, /**< Read the time of last reception based on global time stamp. */
 } phy_extension_type_e;
 

source/MAC/IEEE802_15_4/mac_header_helper_functions.c

@@ -20,6 +20,7 @@
 #include "ns_trace.h"
 #include "mlme.h"
 #include "mac_api.h"
+#include "fhss_api.h"
 #include "common_functions.h"
 #include "mac_common_defines.h"
 #include "MAC/IEEE802_15_4/mac_defines.h"
@@ -483,7 +484,7 @@ static uint8_t *mac_security_interface_aux_security_header_write(uint8_t *ptr, c
     return ptr;
 }
 
-uint8_t * mac_generic_packet_write(uint8_t *ptr, const mac_pre_build_frame_t *buffer)
+uint8_t * mac_generic_packet_write(struct protocol_interface_rf_mac_setup *rf_ptr, uint8_t *ptr, const mac_pre_build_frame_t *buffer)
 {
     ptr = mac_header_write_fcf_dsn(&buffer->fcf_dsn, ptr);
 
@@ -507,13 +508,16 @@ uint8_t * mac_generic_packet_write(uint8_t *ptr, const mac_pre_build_frame_t *bu
     if (buffer->fcf_dsn.securityEnabled) {
         ptr = mac_security_interface_aux_security_header_write(ptr, &buffer->aux_header);
     }
-
+    uint8_t *ie_start = ptr;
     //Copy Payload and set IE Elemets
     ptr = mac_header_information_elements_write(buffer, ptr);
     if (buffer->mac_payload_length) {
         memcpy(ptr, buffer->mac_payload, buffer->mac_payload_length );
         ptr += buffer->mac_payload_length;
     }
+    if (rf_ptr->fhss_api) {
+        rf_ptr->fhss_api->write_synch_info(rf_ptr->fhss_api, ie_start, buffer->headerIeLength, FHSS_DATA_FRAME, buffer->tx_time);
+    }
     return ptr;
 }
 

source/MAC/IEEE802_15_4/mac_header_helper_functions.h

@@ -25,6 +25,7 @@ struct mac_pre_parsed_frame_s;
 struct mac_pre_build_frame;
 struct mac_aux_security_header_s;
 struct ns_ie_iovec;
+struct protocol_interface_rf_mac_setup;
 
 uint8_t mac_security_mic_length_get(uint8_t security_level);
 uint8_t mac_header_security_aux_header_length(uint8_t security_level, uint8_t keyIdmode);
@@ -64,7 +65,7 @@ uint8_t mcps_mac_command_frame_id_get(const struct mac_pre_parsed_frame_s *buffe
 uint8_t *mcps_mac_payload_pointer_get(const struct mac_pre_parsed_frame_s *buffer);
 uint8_t *mcps_security_mic_pointer_get(const struct mac_pre_parsed_frame_s *buffer);
 /* Write Mac Header and payload */
-uint8_t * mac_generic_packet_write(uint8_t *ptr, const struct mac_pre_build_frame *buffer);
+uint8_t * mac_generic_packet_write(struct protocol_interface_rf_mac_setup *rf_ptr, uint8_t *ptr, const struct mac_pre_build_frame *buffer);
 
 
 /** get pointer to Mac header start point*/

source/MAC/IEEE802_15_4/mac_mcps_sap.c

@@ -51,6 +51,9 @@
 
 #define TRACE_GROUP "mMCp"
 
+// Used to set TX time (us) with FHSS. Must be <= 65ms.
+#define MAC_TX_PROCESSING_DELAY_INITIAL 2000
+
 typedef struct {
     uint8_t address[8];
     unsigned addr_type:2;
@@ -1362,6 +1365,17 @@ static void mac_security_authentication_data_params_set(ccm_globals_t *ccm_ptr,
     }
 }
 
+static uint32_t mcps_calculate_tx_time(protocol_interface_rf_mac_setup_s *rf_mac_setup, uint16_t time_to_tx)
+{
+    if (!rf_mac_setup->fhss_api) {
+        return 0;
+    }
+    // Max. time to TX is 65ms
+    if (time_to_tx > 65000) {
+        time_to_tx = 65000;
+    }
+    return rf_mac_setup->fhss_api->read_timestamp(rf_mac_setup->fhss_api) + time_to_tx;
+}
 
 static int8_t mcps_generic_packet_build(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer) {
     phy_device_driver_s *dev_driver = rf_ptr->dev_driver->phy_driver;
@@ -1435,7 +1449,8 @@ static int8_t mcps_generic_packet_build(protocol_interface_rf_mac_setup_s *rf_pt
     tx_buf->len = frame_length;
 
     uint8_t *mhr_start = ptr;
-    ptr = mac_generic_packet_write(ptr, buffer);
+    buffer->tx_time = mcps_calculate_tx_time(rf_ptr, MAC_TX_PROCESSING_DELAY_INITIAL);
+    ptr = mac_generic_packet_write(rf_ptr, ptr, buffer);
 
     if (buffer->fcf_dsn.securityEnabled) {
         mac_security_data_params_set(ccm_ptr, (mhr_start + (buffer->mac_header_length_with_security + open_payload)), (mac_payload_length - open_payload));

source/MAC/IEEE802_15_4/mac_mcps_sap.h

@@ -125,6 +125,7 @@ typedef struct mac_pre_build_frame{
     uint8_t *mac_payload;
     uint8_t status;
     uint8_t asynch_channel;
+    uint32_t tx_time;
     bool upper_layer_request;
     bool mac_allocated_payload_ptr:1;
     bool asynch_request:1;

source/MAC/IEEE802_15_4/mac_pd_sap.c

@@ -163,28 +163,19 @@ int8_t mac_pd_sap_req(protocol_interface_rf_mac_setup_s *rf_mac_setup)
 /**
  * Set PHY TX time.
  *
- * \param rf_mac_setup pointer to MAC
- * \param time_to_tx Time from present time to wanted TX time (us)
- * \return 0 if success, -1 when FHSS not registered
+ * \param rf_mac_setup pointer to MAC.
+ * \param tx_time TX timestamp to be set.
  *
  */
-static int mac_pd_sap_set_tx_time(protocol_interface_rf_mac_setup_s *rf_mac_setup, uint16_t time_to_tx)
+static void mac_pd_sap_set_phy_tx_time(protocol_interface_rf_mac_setup_s *rf_mac_setup, uint32_t tx_time)
 {
-    if (rf_mac_setup->fhss_api) {
-        // Max. TX time is 65ms
-        if (time_to_tx > 65000) {
-            time_to_tx = 65000;
-        }
-        uint8_t tx_time_buffer[4];
-        uint32_t tx_time = rf_mac_setup->fhss_api->read_timestamp(rf_mac_setup->fhss_api) + time_to_tx;
-        if (!tx_time) {
-            tx_time++;
-        }
-        common_write_32_bit(tx_time, tx_time_buffer);
-        rf_mac_setup->dev_driver->phy_driver->extension(PHY_EXTENSION_SET_TX_TIME, tx_time_buffer);
-        return 0;
+    // With TX time set to zero, PHY sends immediately
+    if (!tx_time) {
+        tx_time++;
     }
-    return -1;
+    uint8_t tx_time_buffer[4];
+    common_write_32_bit(tx_time, tx_time_buffer);
+    rf_mac_setup->dev_driver->phy_driver->extension(PHY_EXTENSION_SET_TX_TIME, tx_time_buffer);
 }
 
 /**
@@ -194,7 +185,7 @@ static int mac_pd_sap_set_tx_time(protocol_interface_rf_mac_setup_s *rf_mac_setu
  * \return Timestamp of last PHY reception
  *
  */
-static uint32_t mac_pd_sap_get_rx_time(protocol_interface_rf_mac_setup_s *rf_mac_setup)
+static uint32_t mac_pd_sap_get_phy_rx_time(protocol_interface_rf_mac_setup_s *rf_mac_setup)
 {
     uint8_t rx_time_buffer[4];
     rf_mac_setup->dev_driver->phy_driver->extension(PHY_EXTENSION_READ_RX_TIME, rx_time_buffer);
@@ -216,12 +207,12 @@ void mac_pd_sap_state_machine(protocol_interface_rf_mac_setup_s *rf_mac_setup)
                 if (!active_buf) {
                     return;
                 }
+                mac_pd_sap_set_phy_tx_time(rf_mac_setup, active_buf->tx_time);
                 if (active_buf->fcf_dsn.frametype == FC_BEACON_FRAME) {
                     // FHSS synchronization info is written in the end of transmitted (Beacon) buffer
                     dev_driver_tx_buffer_s *tx_buf = &rf_mac_setup->dev_driver_tx_buffer;
                     synch_info = tx_buf->buf + rf_mac_setup->dev_driver->phy_driver->phy_header_length + tx_buf->len - FHSS_SYNCH_INFO_LENGTH;
-                    //TODO: Add proper frame type when they are defined
-                    rf_mac_setup->fhss_api->write_synch_info(rf_mac_setup->fhss_api, synch_info, FHSS_PLAIN_SYNCH_INFO, 0, 0);
+                    rf_mac_setup->fhss_api->write_synch_info(rf_mac_setup->fhss_api, synch_info, 0, FHSS_SYNCH_FRAME, 0);
                 }
                 // Change to destination channel and write synchronization info to Beacon frames here
                 int tx_handle_retval = rf_mac_setup->fhss_api->tx_handle(rf_mac_setup->fhss_api, !mac_is_ack_request_set(active_buf),

source/Service_Libs/fhss/fhss.c

@@ -929,22 +929,15 @@ static void fhss_update_channel_callback(fhss_structure_t *fhss_structure)
     }
 }
 
-static int16_t fhss_write_synch_info_callback(const fhss_api_t *api, uint8_t *info_ptr, int info_type, int frame_type_id, uint32_t tx_time)
+static int16_t fhss_write_synch_info_callback(const fhss_api_t *api, uint8_t *ptr, uint8_t length, int frame_type, uint32_t tx_time)
 {
     fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
-    (void) frame_type_id;
     (void) tx_time;
-    if (!fhss_structure) {
+    if (!fhss_structure || !ptr || (frame_type != FHSS_SYNCH_FRAME)) {
         return -1;
     }
-    if (info_type == FHSS_PLAIN_SYNCH_INFO) {
-        if (!info_ptr) {
-            return -1;
-        }
-        fhss_beacon_build(fhss_structure, info_ptr);
-        return FHSS_SYNCH_INFO_LENGTH;
-    }
-    return -1;
+    fhss_beacon_build(fhss_structure, ptr);
+    return FHSS_SYNCH_INFO_LENGTH;
 }
 
 static void fhss_data_tx_done_callback(const fhss_api_t *api, bool waiting_ack, bool tx_completed, uint8_t handle)

source/Service_Libs/fhss/fhss_ws.c

@@ -25,6 +25,7 @@
 #include "fhss_ws.h"
 #include "fhss_mac_interface.h"
 #include "nsdynmemLIB.h"
+#include "common_functions.h"
 #include "ns_trace.h"
 #include <string.h>
 
@@ -33,7 +34,17 @@
 // Enable this flag to use channel traces
 #define FHSS_CHANNEL_DEBUG
 
-#define DEF_2E16 16777216
+#define DEF_2E24 0x1000000
+#define IE_HEADER_LENGTH_MASK 0x007f
+#define IE_HEADER_ID_MASK 0x7f80
+#define WH_IE_ID        0x2a
+#define WH_SUB_ID_UTT   1
+
+struct ws_ie_t {
+    uint8_t *content_ptr;
+    unsigned length:7;
+    uint8_t id;
+};
 
 static void fhss_ws_update_uc_channel_callback(fhss_structure_t *fhss_structure);
 static void fhss_unicast_handler(const fhss_api_t *fhss_api, uint16_t delay);
@@ -137,14 +148,13 @@ 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 ms_since_seq_start = (cur_slot * dwell_time) + (dwell_time-remaining_time) + time_to_tx;
+    uint64_t ms_since_seq_start = (cur_slot * dwell_time) + (dwell_time-remaining_time) + time_to_tx;
     ms_since_seq_start %= (dwell_time*fhss_structure->number_of_channels);
     uint32_t seq_length = 0x10000;
     if (fhss_structure->ws->fhss_configuration.ws_channel_function == WS_TR51CF) {
         seq_length = fhss_structure->number_of_channels;
     }
-
-    return own_floor((float)(ms_since_seq_start * DEF_2E16) / (seq_length*dwell_time));
+    return own_floor((float)(ms_since_seq_start * DEF_2E24) / (seq_length*dwell_time));
 }
 
 static uint16_t fhss_ws_calculate_destination_slot(fhss_structure_t *fhss_structure, uint32_t ufsi, uint32_t ufsi_timestamp, uint8_t dwell_time, uint32_t tx_time)
@@ -153,7 +163,7 @@ static uint16_t fhss_ws_calculate_destination_slot(fhss_structure_t *fhss_struct
     if (fhss_structure->ws->fhss_configuration.ws_channel_function  == WS_TR51CF) {
         seq_length = fhss_structure->number_of_channels;
     }
-    uint32_t dest_ms_since_seq_start = own_ceil((float)(ufsi*seq_length*dwell_time) / DEF_2E16);
+    uint32_t dest_ms_since_seq_start = own_ceil((float)(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);
 }
 
@@ -262,18 +272,43 @@ static bool fhss_ws_check_tx_conditions_callback(const fhss_api_t *api, bool is_
     return true;
 }
 
-static int16_t fhss_ws_write_synch_info_callback(const fhss_api_t *api, uint8_t *info_ptr, int info_type, int frame_type_id, uint32_t tx_time)
+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)
+{
+    struct ws_ie_t ie_element;
+    uint8_t *sub_id_ptr;
+    uint16_t ie_dummy;
+    while (length > 2) {
+        ie_dummy = common_read_16_bit_inverse(header_ptr);
+        ie_element.length = (ie_dummy & IE_HEADER_LENGTH_MASK);
+        ie_element.id = ((ie_dummy & IE_HEADER_ID_MASK) >> 7);
+        ie_element.content_ptr = header_ptr + 2;
+        sub_id_ptr = ie_element.content_ptr;
+        if (ie_element.length && (header_ie->id == ie_element.id) && (*sub_id_ptr == sub_id)) {
+            sub_id_ptr++;
+            ie_element.length--;
+            header_ie->content_ptr = sub_id_ptr;
+            header_ie->length = ie_element.length;
+            return ie_element.length;
+        }
+        length -= ie_element.length + 2;
+        header_ptr += ie_element.length + 2;
+    }
+    return 0;
+}
+
+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)
 {
     fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
-    (void) frame_type_id;
-    (void) tx_time;
-    (void) info_ptr;
-    (void) info_type;
-    if (!fhss_structure) {
+    if (!fhss_structure || !ptr || (frame_type != FHSS_DATA_FRAME)) {
         return -1;
     }
-
-    return -1;
+    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)) {
+        uint32_t ufsi = fhss_ws_calculate_ufsi(fhss_structure, tx_time);
+        common_write_24_bit_inverse(ufsi, header_ie.content_ptr+1);
+    }
+    return 0;
 }
 
 static void fhss_ws_data_tx_done_callback(const fhss_api_t *api, bool waiting_ack, bool tx_completed, uint8_t handle)