diff --git a/nanostack/fhss_api.h b/nanostack/fhss_api.h
index 362743e53fc..5395c01e510 100644
--- a/nanostack/fhss_api.h
+++ b/nanostack/fhss_api.h
@@ -86,13 +86,14 @@ typedef bool fhss_use_broadcast_queue(const fhss_api_t *api, bool is_broadcast_a
  * @param frame_length MSDU length of the frame.
  * @param phy_header_length PHY header length.
  * @param phy_tail_length PHY tail length.
+ * @param tx_time TX time.
  * @return  0 Success.
  * @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);
+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);
 
 /**
  * @brief Check TX permission.
diff --git a/source/MAC/IEEE802_15_4/mac_pd_sap.c b/source/MAC/IEEE802_15_4/mac_pd_sap.c
index c7224568c5d..b81c92c66a6 100644
--- a/source/MAC/IEEE802_15_4/mac_pd_sap.c
+++ b/source/MAC/IEEE802_15_4/mac_pd_sap.c
@@ -227,7 +227,7 @@ void mac_pd_sap_state_machine(protocol_interface_rf_mac_setup_s *rf_mac_setup)
                 int tx_handle_retval = rf_mac_setup->fhss_api->tx_handle(rf_mac_setup->fhss_api, !mac_is_ack_request_set(active_buf),
                         active_buf->DstAddr, mac_convert_frame_type_to_fhss(active_buf->fcf_dsn.frametype),
                         active_buf->mac_payload_length, rf_mac_setup->dev_driver->phy_driver->phy_header_length,
-                        rf_mac_setup->dev_driver->phy_driver->phy_tail_length);
+                        rf_mac_setup->dev_driver->phy_driver->phy_tail_length, 0);
                 // When FHSS TX handle returns -1, transmission of the packet is currently not allowed -> restart CCA timer
                 if (tx_handle_retval == -1) {
                     timer_mac_start(rf_mac_setup, MAC_TIMER_CCA, randLIB_get_random_in_range(20, 400) + 1);
diff --git a/source/Service_Libs/fhss/fhss.c b/source/Service_Libs/fhss/fhss.c
index 48f6453394a..a4d7e020694 100644
--- a/source/Service_Libs/fhss/fhss.c
+++ b/source/Service_Libs/fhss/fhss.c
@@ -841,8 +841,9 @@ static void fhss_superframe_callback(fhss_structure_t *fhss_structure)
     }
 }
 
-static int fhss_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)
+static int fhss_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)
 {
+    (void) tx_time;
     fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
     if (!fhss_structure) {
         return -2;
diff --git a/source/Service_Libs/fhss/fhss_ws.c b/source/Service_Libs/fhss/fhss_ws.c
index e80a1a82b13..b48e63022cb 100644
--- a/source/Service_Libs/fhss/fhss_ws.c
+++ b/source/Service_Libs/fhss/fhss_ws.c
@@ -33,6 +33,8 @@
 // Enable this flag to use channel traces
 #define FHSS_CHANNEL_DEBUG
 
+#define DEF_2E16 16777216
+
 static void fhss_ws_update_uc_channel_callback(fhss_structure_t *fhss_structure);
 
 fhss_structure_t *fhss_ws_enable(fhss_api_t *fhss_api, const fhss_ws_configuration_t *fhss_configuration, const fhss_timer_t *fhss_timer)
@@ -102,6 +104,50 @@ static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
     }
 }
 
+static int own_floor(float value)
+{
+    return (int)value;
+}
+
+static int own_ceil(float value)
+{
+    int ivalue = (int)value;
+    if (value == (float)ivalue) {
+        return ivalue;
+    }
+    return ivalue + 1;
+}
+
+static uint32_t fhss_ws_calculate_ufsi(fhss_structure_t *fhss_structure, uint32_t tx_time)
+{
+    uint8_t dwell_time = fhss_structure->fhss_conf.fhss_ws_configuration.fhss_uc_dwell_interval;
+    uint16_t cur_slot = fhss_structure->ws->uc_slot;
+    if (cur_slot == 0) {
+        cur_slot = fhss_structure->number_of_channels;
+    }
+    cur_slot--;
+    uint32_t remaining_time = (fhss_structure->platform_functions.fhss_get_remaining_slots(fhss_superframe_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;
+    ms_since_seq_start %= (dwell_time*fhss_structure->number_of_channels);
+    uint32_t seq_length = 0x10000;
+    if (fhss_structure->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));
+}
+
+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)
+{
+    uint32_t seq_length = 0x10000;
+    if (fhss_structure->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);
+    return (own_floor(((float)((tx_time - ufsi_timestamp)/1000 + dest_ms_since_seq_start) / dwell_time)) % seq_length);
+}
+
 static uint32_t fhss_ws_get_sf_timeout_callback(fhss_structure_t *fhss_structure)
 {
     return fhss_structure->fhss_conf.fhss_ws_configuration.fhss_uc_dwell_interval * 1000;
@@ -159,7 +205,7 @@ static void fhss_ws_update_uc_channel_callback(fhss_structure_t *fhss_structure)
     fhss_structure->callbacks.change_channel(fhss_structure->fhss_api, next_channel);
 }
 
-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)
+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)
 {
     (void) is_broadcast_addr;
     (void) frame_type;
@@ -172,8 +218,8 @@ static int fhss_ws_tx_handle_callback(const fhss_api_t *api, bool is_broadcast_a
     }
     if (fhss_structure->fhss_state == FHSS_SYNCHRONIZED) {
         int32_t tx_channel;
-        //TODO: Compute destination slot using neighbour table
-        uint16_t destination_slot = fhss_structure->ws->uc_slot;
+        //TODO: Get destination UFSI, timestamp and dwell time from neighbour table
+        uint16_t destination_slot = fhss_ws_calculate_destination_slot(fhss_structure, 0, 0, fhss_structure->fhss_conf.fhss_ws_configuration.fhss_uc_dwell_interval, tx_time);
         if (fhss_structure->ws->channel_function == WS_TR51CF) {
             tx_channel = tr51_get_uc_channel_index(destination_slot, destination_address, fhss_structure->number_of_channels);
         } else if(fhss_structure->ws->channel_function == WS_DH1CF) {
diff --git a/test/nanostack/unittest/mac/mac_mcps_sap/test_mac_mcps_sap.c b/test/nanostack/unittest/mac/mac_mcps_sap/test_mac_mcps_sap.c
index 8ea6ded7317..ba4fb13e40f 100644
--- a/test/nanostack/unittest/mac/mac_mcps_sap/test_mac_mcps_sap.c
+++ b/test/nanostack/unittest/mac/mac_mcps_sap/test_mac_mcps_sap.c
@@ -261,7 +261,7 @@ static protocol_interface_rf_mac_setup_s * test_mac_rf_mac_class_allocate(void)
     return rf_mac_setup;
 }
 
-static int fhss_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)
+static int fhss_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)
 {
     return 0;
 }
diff --git a/test/nanostack/unittest/mac/mac_pd_sap/test_mac_pd_sap.c b/test/nanostack/unittest/mac/mac_pd_sap/test_mac_pd_sap.c
index 8d2cb9bb284..0cef817058c 100644
--- a/test/nanostack/unittest/mac/mac_pd_sap/test_mac_pd_sap.c
+++ b/test/nanostack/unittest/mac/mac_pd_sap/test_mac_pd_sap.c
@@ -51,7 +51,7 @@ static int8_t test_rf_virtual_tx(const virtual_data_req_t *data_req,int8_t drive
     return 0;
 }
 
-static int fhss_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)
+static int fhss_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)
 {
     return 0;
 }
diff --git a/test/nanostack/unittest/service_libs/fhss/test_fhss.c b/test/nanostack/unittest/service_libs/fhss/test_fhss.c
index 242b9b74d29..358ae4c3b1d 100644
--- a/test/nanostack/unittest/service_libs/fhss/test_fhss.c
+++ b/test/nanostack/unittest/service_libs/fhss/test_fhss.c
@@ -729,27 +729,27 @@ bool test_fhss_tx_handle_callback()
     fhss_struct->synch_configuration.fhss_number_of_tx_slots = 1;
     fhss_set_callbacks(fhss_struct);
     // Broadcast data on unicast channel should return -3
-    if (fhss_struct->fhss_api->tx_handle(&fhss_api, true, NULL, FHSS_DATA_FRAME, 0, 0, 0) != -3) {
+    if (fhss_struct->fhss_api->tx_handle(&fhss_api, true, NULL, FHSS_DATA_FRAME, 0, 0, 0, 0) != -3) {
         return false;
     }
     // Test sending of Beacon
-    if (fhss_struct->fhss_api->tx_handle(&fhss_api, true, NULL, FHSS_SYNCH_FRAME, 0, 0, 0) != 0) {
+    if (fhss_struct->fhss_api->tx_handle(&fhss_api, true, NULL, FHSS_SYNCH_FRAME, 0, 0, 0, 0) != 0) {
         return false;
     }
     // When TX is not allowed, should return -1 for data frame
     fhss_struct->tx_allowed = false;
     fhss_struct->fhss_state = FHSS_SYNCHRONIZED;
-    if (fhss_struct->fhss_api->tx_handle(&fhss_api, false, NULL, FHSS_DATA_FRAME, 0, 0, 0) != -1) {
+    if (fhss_struct->fhss_api->tx_handle(&fhss_api, false, NULL, FHSS_DATA_FRAME, 0, 0, 0, 0) != -1) {
         return false;
     }
     // When TX is allowed, should return 0 for data frame
     fhss_struct->tx_allowed = true;
     fhss_struct->fhss_state = FHSS_UNSYNCHRONIZED;
-    if (fhss_struct->fhss_api->tx_handle(&fhss_api, false, NULL, FHSS_DATA_FRAME, 0, 0, 0) != 0) {
+    if (fhss_struct->fhss_api->tx_handle(&fhss_api, false, NULL, FHSS_DATA_FRAME, 0, 0, 0, 0) != 0) {
         return false;
     }
     // Test changing to parent channel to send synch request
-    if (fhss_struct->fhss_api->tx_handle(&fhss_api, false, NULL, FHSS_SYNCH_REQUEST_FRAME, 0, 0, 0) != 0) {
+    if (fhss_struct->fhss_api->tx_handle(&fhss_api, false, NULL, FHSS_SYNCH_REQUEST_FRAME, 0, 0, 0, 0) != 0) {
         return false;
     }
     free(fhss_struct);
diff --git a/test/nanostack/unittest/stub/fhss_mac_interface_stub.h b/test/nanostack/unittest/stub/fhss_mac_interface_stub.h
index 28890a7fe3c..cfcf27c4384 100644
--- a/test/nanostack/unittest/stub/fhss_mac_interface_stub.h
+++ b/test/nanostack/unittest/stub/fhss_mac_interface_stub.h
@@ -34,7 +34,7 @@ extern fhss_mac_interface_stub_def fhss_mac_interface_stub;
 
 bool fhss_is_broadcast_channel_cb(const fhss_api_t *api);
 bool fhss_use_broadcast_queue_cb(const fhss_api_t *api, bool is_broadcast_addr, int frame_type);
-int fhss_tx_handle_cb(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);
+int fhss_tx_handle_cb(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);
 bool fhss_check_tx_conditions_cb(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 fhss_receive_frame_cb(const fhss_api_t *api, uint16_t pan_id, uint8_t *source_address, uint32_t timestamp, uint8_t *synch_info, int frame_type);
 void fhss_data_tx_done_cb(const fhss_api_t *api, bool waiting_ack, bool tx_completed, uint8_t handle);