diff --git a/features/nanostack/sal-stack-nanostack/nanostack/dhcp_service_api.h b/features/nanostack/sal-stack-nanostack/nanostack/dhcp_service_api.h
index 1a3be195aa..88a5150458 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/dhcp_service_api.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/dhcp_service_api.h
@@ -142,6 +142,15 @@ uint16_t dhcp_service_init(int8_t interface_id, dhcp_instance_type_e instance_ty
 */
 void dhcp_service_relay_instance_enable(uint16_t instance, uint8_t *server_address);
 
+/**
+* \brief Enable DHCPv6 Relay Agent to add interface ID option to relay frame. Default is disabled.
+*
+*
+* \param instance The instance ID of the registered server.
+* \param enable true add interface option
+*/
+void dhcp_service_relay_interface_id_option_enable(uint16_t instance, bool enable);
+
 /**
 * \brief Get DHCPv6 Relay Agent address pointer.
 *
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/fhss_api.h b/features/nanostack/sal-stack-nanostack/nanostack/fhss_api.h
index be5a90c1d2..e6aa23270e 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/fhss_api.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/fhss_api.h
@@ -49,6 +49,7 @@ typedef struct fhss_callback fhss_callback_t;
 typedef enum {
     FHSS_UNSYNCHRONIZED,
     FHSS_SYNCHRONIZED,
+    FHSS_EXPEDITED_FORWARDING
 } fhss_states;
 
 /**
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/fhss_ws_extension.h b/features/nanostack/sal-stack-nanostack/nanostack/fhss_ws_extension.h
index ac2e1b446f..d98b5ace97 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/fhss_ws_extension.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/fhss_ws_extension.h
@@ -118,6 +118,27 @@ extern int ns_fhss_set_neighbor_info_fp(const fhss_api_t *fhss_api, fhss_get_nei
  */
 extern int ns_fhss_ws_set_hop_count(const fhss_api_t *fhss_api, const uint8_t hop_count);
 
+/**
+ * @brief WS TX allowance levels.
+ */
+typedef enum {
+    /** Allow transmitting only on TX slots. */
+    WS_TX_SLOT,
+    /** Allow transmitting only on TX and RX slots. */
+    WS_TX_AND_RX_SLOT,
+    /** Allow transmitting always. Also unicast on broadcast channel. */
+    WS_TX_ALWAYS
+} fhss_ws_tx_allow_level;
+
+/**
+ * @brief Set node unicast TX allowance level. Allows device to use the unicast and broadcast channel for unicast transmission as described by fhss_ws_tx_allow_level.
+ * @param fhss_api FHSS instance.
+ * @param global_level Level of TX allowance in normal mode.
+ * @param ef_level Level of TX allowance in expedited forwarding mode.
+ * @return 0 on success, -1 on fail.
+ */
+extern int ns_fhss_ws_set_tx_allowance_level(const fhss_api_t *fhss_api, const fhss_ws_tx_allow_level global_level, const fhss_ws_tx_allow_level ef_level);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/mac_api.h b/features/nanostack/sal-stack-nanostack/nanostack/mac_api.h
index 8e545987ea..bc5b13fcef 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/mac_api.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/mac_api.h
@@ -128,10 +128,11 @@ typedef void mcps_data_request(const mac_api_t *api, const mcps_data_req_t *data
  * @param data MCPS-DATA.request specific values
  * @param ie_ext Information element list to MCPS-DATA.request
  * @param asynch_channel_list Optional channel list to asynch data request. Give NULL when normal data request.
+ * @param priority Data request priority level
  *
  * Asynch data request is mac standard extension. asynch_channel_list include channel mask which channel message is requested to send.
  */
-typedef void mcps_data_request_ext(const mac_api_t *api, const mcps_data_req_t *data, const mcps_data_req_ie_list_t *ie_ext, const struct channel_list_s *asynch_channel_list);
+typedef void mcps_data_request_ext(const mac_api_t *api, const mcps_data_req_t *data, const mcps_data_req_ie_list_t *ie_ext, const struct channel_list_s *asynch_channel_list, mac_data_priority_t priority);
 
 /**
  * @brief mcps_purge_request MCPS_PURGE request call
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/mac_mcps.h b/features/nanostack/sal-stack-nanostack/nanostack/mac_mcps.h
index 614a01e326..8e99c2f431 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/mac_mcps.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/mac_mcps.h
@@ -192,5 +192,17 @@ typedef struct mcps_edfe_response_s {
     bool use_message_handle_to_discover: 1; /**< EDFE Data request message ID is valid at message_handle.  */
 } mcps_edfe_response_t;
 
+/**
+ * @brief enum mac_data_priority_t Data request priority level
+ *
+ * Data request priority level may affect CCA process and MAC queue process
+ */
+typedef enum mac_data_priority_e {
+    MAC_DATA_NORMAL_PRIORITY = 0,   /**< Normal MCPS DATA REQ */
+    MAC_DATA_MEDIUM_PRIORITY = 1,   /**< Indirect Data which is polled */
+    MAC_DATA_HIGH_PRIORITY = 2,     /**< MAC command usually use this and beacon */
+    MAC_DATA_EXPEDITE_FORWARD = 3   /**< Expedite forward level give highest priority */
+} mac_data_priority_t;
+
 
 #endif // MAC_MCPS_H
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/mlme.h b/features/nanostack/sal-stack-nanostack/nanostack/mlme.h
index 859457d161..992046dfde 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/mlme.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/mlme.h
@@ -264,6 +264,7 @@ typedef enum {
     macAutoRequestKeyIndex = 0x7b,  /*<The index of the key used for automatic data*/
     macDefaultKeySource = 0x7c,      /*<Default key source*/
     //NON standard extension
+    macRequestRestart = 0xf1,       /*< Configure failed packet data request restart */
     macEdfeForceStop = 0xf2,          /*< Use this command for Data wait timeout at LLC: Mac stop Edfe session data wait and enable normal FHSS mode */
     macSetDataWhitening = 0xf3,     /*< Enable or disable data whitening, boolean true for enable, false for disable */
     macCCAThresholdStart = 0xf4,    /*< Start automatic CCA threshold */
@@ -517,4 +518,16 @@ typedef struct mlme_multi_csma_ca_s {
     uint16_t multi_cca_interval;        /**< Length of the additional CSMA-CA period(s) in microseconds */
 } mlme_multi_csma_ca_param_t;
 
+/**
+ * @brief struct mlme_request_restart_config_s Set failed packet request restart configuration
+ *
+ * Non standard extension to restart data request after failed CCA or TX attempts
+ */
+typedef struct mlme_request_restart_config_s {
+    uint8_t cca_failure_restart_max;    /**< Max number of restarts after CCA failure */
+    uint8_t tx_failure_restart_max;     /**< Max number of restarts after TX failure */
+    uint16_t blacklist_min_ms;          /**< Blacklist min, which is doubled by every restart */
+    uint16_t blacklist_max_ms;          /**< Blacklist max, largest allowed blacklist time */
+} mlme_request_restart_config_t;
+
 #endif /* MLME_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/platform/arm_hal_phy.h b/features/nanostack/sal-stack-nanostack/nanostack/platform/arm_hal_phy.h
index ec065edc80..62c46f8552 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/platform/arm_hal_phy.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/platform/arm_hal_phy.h
@@ -50,6 +50,7 @@ typedef enum {
     PHY_LINK_TX_SUCCESS,        /**< MAC TX complete. MAC will a make decision to enter wait ACK or TX done state. */
     PHY_LINK_TX_FAIL,           /**< Link TX process fail. */
     PHY_LINK_CCA_FAIL,          /**< RF link CCA process fail. */
+    PHY_LINK_CCA_FAIL_RX,       /**< RF link CCA process failed because of packet reception. */
     PHY_LINK_CCA_OK,            /**< RF link CCA process ok. */
     PHY_LINK_CCA_PREPARE,       /**< Prepare for CCA after CSMA-CA: changes to CCA channel and gives permission to TX. See PHY_LINK_CCA_PREPARE status definitions for return values */
 } phy_link_tx_status_e;
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/socket_api.h b/features/nanostack/sal-stack-nanostack/nanostack/socket_api.h
index e5323c27d3..0b5db6c825 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/socket_api.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/socket_api.h
@@ -138,10 +138,41 @@ extern "C" {
  * After the successful state change, data can be sent using socket_send().
  * The connection can be shut down in either direction with socket_shutdown() function - shutting down write signals end-of-data to the peer.
  *
+ *
  * \section socket-udpicmp How to use UDP and RAW socket:
  *
  * A UDP socket is ready to receive and send data immediately after a successful call of socket_open() and a NET_READY event is received.
  * Data can be transmitted with the socket_sendto() function. An ICMP socket works with same function call.
+ *
+ * \section socket-trafficpriority How to set socket message priority to improve Quality of Service (QoS):
+ *
+ * IPv6 header has a field traffic class that contains a 6-bit Differentiated Services Code Point (DSCP) field that is used for packet
+ * classification. By default the packet class level is set to 0 NS_DSCP_DEFAULT.
+ *
+ * Recommend QoS levels:
+ *
+ * |     Level        |Description                                                                                          |
+ * | :--------------: | :-------------------------------------------------------------------------------------------------: |
+ * | NS_DSCP_DEFAULT  | Default level for normal data usage                                                                 |
+ * | NS_DSCP_AF11     | Higher Application data service for prioritize packet forwarding.                                   |
+ * | NS_DSCP_EF       | Expedited Forwarding (EF) for short messages. Allows low loss, low delay, and low jitter services.  |
+ * |                  | This is meant for very important messages like alerts. EF packet length should be kept in           |
+ * |                  | minimum. This should not be used for any other purpose as it will block other network traffic       |
+ * | NS_DSCP_CS6      | Network protocol message Priority. Application should not use this.                                 |
+ *
+ * High priority messages can be set to use higher than default class by using socket_setsockopt() and
+ * socket_option_traffic_class_dsc_set() helper.
+ *
+ * Example to send a message using Expedited Forwarding class:
+ *
+ * //Set EF class to high priority messages
+ * int16_t traffic_class = socket_option_traffic_class_dsc_set(NS_DSCP_EF);
+ * socket_setsockopt(socket_id, SOCKET_IPPROTO_IPV6, SOCKET_IPV6_TCLASS, &traffic_class, sizeof traffic_class);
+ *
+ * //Set traffic class back to default
+ * traffic_class = socket_option_traffic_class_dsc_set(NS_DSCP_DEFAULT);
+ * socket_setsockopt(socket_id, SOCKET_IPPROTO_IPV6, SOCKET_IPV6_TCLASS, &traffic_class, sizeof traffic_class);
+ *
  */
 
 #include "ns_address.h"
@@ -250,6 +281,24 @@ typedef struct ns_in6_pktinfo {
     int8_t  ipi6_ifindex;    /**< send/recv interface index */
 } ns_in6_pktinfo_t;
 
+/** \name Socket DSCP (Differentiated Services Code Point) QoS level examples.
+ * \anchor MSG_QOS_LEVELS
+ */
+///@{
+/** Standard priority and it is socket default */
+#define NS_DSCP_DEFAULT 0
+/** Application high priority service: Stack priorities these messages over the default priority messages */
+#define NS_DSCP_AF11 10
+/** Expedited Forwarding (EF) QoS level enable high priority state: low loss, low delay, and low jitter services */
+#define NS_DSCP_EF 46
+/** Network protocol traffic allocated QoS level stack may internally use that */
+#define NS_DSCP_CS6 48
+///@}
+
+/** Helper Traffic class Differentiated Services Code for QoS  0-63. 0 is default which define Lowest Priority
+ *
+ * */
+#define socket_option_traffic_class_dsc_set(x)  (uint8_t)((x & 63) << 2)
 
 /** \name Alignment macros for control message headers
 * \anchor CMSG_ALIGN_FLAGS
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/ws_management_api.h b/features/nanostack/sal-stack-nanostack/nanostack/ws_management_api.h
index f506c37995..7094d09077 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/ws_management_api.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/ws_management_api.h
@@ -90,6 +90,19 @@ extern "C" {
 #define NETWORK_SIZE_XLARGE         0x19    /**< 2500+ devices */
 #define NETWORK_SIZE_AUTOMATIC      0xFF    /**< Automatic network size */
 
+/**
+ * \brief Neighbor type to differentiate the Role of the neighbor.
+ */
+
+typedef enum {
+    WS_OTHER = 0,            /**< temporary or soon to be removed neighbor*/
+    WS_PRIMARY_PARENT,       /**< Primary parent used for upward packets and used from Border router downwards*/
+    WS_SECONDARY_PARENT,     /**< Secondary parent reported to border router and might be used as alternate route*/
+    WS_CANDIDATE_PARENT,     /**< Candidate neighbor that is considered as parent if there is problem with active parents*/
+    WS_CHILD                 /**< Child with registered address*/
+} ws_management_neighbor_type_e;
+
+
 /** Temporary API change flag. this will be removed when new version of API is implemented on applications
  *
  */
@@ -127,6 +140,47 @@ typedef struct ws_statistics {
     uint32_t asynch_tx_count;
     /** Asynch RX counter */
     uint32_t asynch_rx_count;
+
+
+    /** Time spent in individual Wi-SUN join state 1 Discovery*/
+    uint32_t join_state_1;
+    /** Time spent in individual Wi-SUN join state 2 Authentication*/
+    uint32_t join_state_2;
+    /** Time spent in individual Wi-SUN join state 3 Configuration learn*/
+    uint32_t join_state_3;
+    /** Time spent in individual Wi-SUN join state 4 RPL parent discovery*/
+    uint32_t join_state_4;
+    /** Time spent in individual Wi-SUN join state 5 Active state*/
+    uint32_t join_state_5;
+
+
+    /** Amount of Wi-SUN Pan Advertisement Solicit Message sent*/
+    uint32_t sent_PAS;
+    /** Amount of Wi-SUN Pan Advertisement Message sent*/
+    uint32_t sent_PA;
+    /** Amount of Wi-SUN Pan Configuration Solicit Message sent*/
+    uint32_t sent_PCS;
+    /** Amount of Wi-SUN Pan Configuration Message sent*/
+    uint32_t sent_PC;
+
+    /** Amount of Wi-SUN Pan Advertisement Solicit Message sent*/
+    uint32_t recv_PAS;
+    /** Amount of Wi-SUN Pan Advertisement Message sent*/
+    uint32_t recv_PA;
+    /** Amount of Wi-SUN Pan Configuration Solicit Message sent*/
+    uint32_t recv_PCS;
+    /** Amount of Wi-SUN Pan Configuration Message sent*/
+    uint32_t recv_PC;
+
+    /** New Neighbours found */
+    uint32_t Neighbour_add;
+    /** New Neighbours Removed */
+    uint32_t Neighbour_remove;
+    /** New Child added */
+    uint32_t Child_add;
+    /** Child lost */
+    uint32_t child_remove;
+
 } ws_statistics_t;
 
 /**
@@ -149,6 +203,28 @@ typedef struct ws_stack_info {
     uint8_t join_state;
 } ws_stack_info_t;
 
+/**
+ * \brief Struct ws_neighbour_info_t Gives the neighbour information.
+ */
+typedef struct ws_neighbour_info {
+    /** Link local address*/
+    uint8_t link_local_address[16];
+    /** Global address if it is known set to 0 if not available*/
+    uint8_t global_address[16];
+    /** parent RSSI Out measured RSSI value calculated using EWMA specified by Wi-SUN from range of -174 (0) to +80 (254) dBm.*/
+    uint8_t rsl_out;
+    /** parent RSSI in measured RSSI value calculated using EWMA specified by Wi-SUN from range of -174 (0) to +80 (254) dBm.*/
+    uint8_t rsl_in;
+    /** RPL Rank value for parents 0xffff for neighbors RANK is unknown*/
+    uint16_t rpl_rank;
+    /** Measured ETX value if known set to 0xFFFF if not known or Child*/
+    uint16_t etx;
+    /** Remaining lifetime Link lifetime for parents and ARO lifetime for children*/
+    uint32_t lifetime;
+    /** Neighbour type (Primary Parent, Secondary Parent, Candidate parent, child, other(Temporary neighbours))*/
+    ws_management_neighbor_type_e type;
+} ws_neighbour_info_t;
+
 /**
  * Initialize Wi-SUN stack.
  *
@@ -752,6 +828,25 @@ int ws_stack_info_get(
     int8_t interface_id,
     ws_stack_info_t *info_ptr);
 
+/**
+ * Get Neighbor table information from stack.
+ *
+ * To allocate correct amount of memory first use the API with NULL to get current amount
+ * of neighbors. Then Allocate the memory and call the function to fill the table.
+ *
+ * \param interface_id Network interface ID.
+ * \param neighbor_ptr Pointer to memory where Neighbor table entries can be written.
+ * \param count amount of neighbor table entries allocated to memory.
+ *
+ * \return >=0 Success with amount of entries written in table.
+ * \return >=0 if neighbor_ptr is NULL returns the amount of neighbors currently.
+ * \return <0 Failure.
+ */
+int ws_neighbor_info_get(
+    int8_t interface_id,
+    ws_neighbour_info_t *neighbor_ptr,
+    uint16_t count);
+
 /**
  * Set minimum RF sensitivity acceptable for the parent selection
  *
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan.c
index a8279994eb..6c6a0ca445 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan.c
@@ -83,7 +83,10 @@
 #define TRACE_GROUP "6lo"
 
 /* Data rate for application used in Stagger calculation */
-#define STAGGER_DATARATE_FOR_APPL(n) ((n)*25/100)
+#define STAGGER_DATARATE_FOR_APPL(n) ((n)*75/100)
+
+/* Time after network is considered stable and smaller stagger values can be given*/
+#define STAGGER_STABLE_NETWORK_TIME 3600*4
 
 #ifdef HAVE_RPL
 rpl_domain_t *protocol_6lowpan_rpl_domain;
@@ -839,7 +842,7 @@ bool protocol_6lowpan_stagger_estimate_get(int8_t interface_id, uint32_t data_am
         datarate = 250000;
     } else if (ws_info(cur_interface)) {
         network_size = ws_common_network_size_estimate_get(cur_interface);
-        datarate = ws_common_datarate_get(cur_interface);
+        datarate = ws_common_usable_application_datarate_get(cur_interface);
     } else {
         // 6LoWPAN ND
         network_size = 1000;
@@ -850,11 +853,23 @@ bool protocol_6lowpan_stagger_estimate_get(int8_t interface_id, uint32_t data_am
         // If no data amount given, use 1kB
         data_amount = 1;
     }
+    if (datarate < 25000) {
+        // Minimum data rate used in calculations is 25kbs to prevent invalid values
+        datarate = 25000;
+    }
 
     /*
      * Do not occupy whole bandwidth, leave space for network formation etc...
      */
-    datarate = STAGGER_DATARATE_FOR_APPL(datarate);
+    if (ws_info(cur_interface) &&
+            (ws_common_connected_time_get(cur_interface) > STAGGER_STABLE_NETWORK_TIME || ws_common_authentication_time_get(cur_interface) == 0)) {
+        // After four hours of network connected full bandwidth is given to application
+        // Authentication has not been required during bootstrap so network load is much smaller
+    } else {
+        // Smaller data rate allowed as we have just joined to the network and Authentication was made
+        datarate = STAGGER_DATARATE_FOR_APPL(datarate);
+    }
+
     stagger_value = 1 + ((data_amount * 1024 * 8 * network_size) / datarate);
     /**
      * Example:
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.c
index 86fc64ebde..73d6690f97 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.c
@@ -964,6 +964,62 @@ int8_t mac_helper_mac_mlme_max_retry_set(int8_t interface_id, uint8_t mac_retry_
     return 0;
 }
 
+int8_t mac_helper_mac_mlme_max_csma_backoffs_set(int8_t interface_id, uint8_t csma_backoffs)
+{
+    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
+
+    if (!cur || !cur->mac_api) {
+        return -1;
+    }
+    mlme_set_t set_req;
+    set_req.attr = macMaxCSMABackoffs;
+    set_req.attr_index = 0;
+    set_req.value_pointer = &csma_backoffs;
+    set_req.value_size = 1;
+    cur->mac_api->mlme_req(cur->mac_api, MLME_SET, &set_req);
+
+    return 0;
+}
+
+int8_t mac_helper_mac_mlme_be_set(int8_t interface_id, uint8_t min_be, uint8_t max_be)
+{
+    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
+
+    if (!cur || !cur->mac_api) {
+        return -1;
+    }
+    mlme_set_t set_req;
+    set_req.attr = macMinBE;
+    set_req.attr_index = 0;
+    set_req.value_pointer = &min_be;
+    set_req.value_size = 1;
+    cur->mac_api->mlme_req(cur->mac_api, MLME_SET, &set_req);
+
+    set_req.attr = macMaxBE;
+    set_req.attr_index = 0;
+    set_req.value_pointer = &max_be;
+    set_req.value_size = 1;
+    cur->mac_api->mlme_req(cur->mac_api, MLME_SET, &set_req);
+
+    return 0;
+}
+
+int8_t mac_helper_mac_mlme_data_request_restart_set(int8_t interface_id, mlme_request_restart_config_t *request_restart_config)
+{
+    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
+
+    if (!cur || !cur->mac_api) {
+        return -1;
+    }
+    mlme_set_t set_req;
+    set_req.attr = macRequestRestart;
+    set_req.attr_index = 0;
+    set_req.value_pointer = (void *)request_restart_config;
+    set_req.value_size = sizeof(mlme_request_restart_config_t);
+    cur->mac_api->mlme_req(cur->mac_api, MLME_SET, &set_req);
+
+    return 0;
+}
 
 int8_t mac_helper_mac_device_description_pan_id_update(int8_t interface_id, uint16_t pan_id)
 {
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.h
index fd1eb19d86..f85916bcf7 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.h
@@ -130,6 +130,12 @@ void mac_helper_devicetable_direct_set(struct mac_api_s *mac_api, const mlme_dev
 
 int8_t mac_helper_mac_mlme_max_retry_set(int8_t interface_id, uint8_t mac_retry_set);
 
+int8_t mac_helper_mac_mlme_max_csma_backoffs_set(int8_t interface_id, uint8_t csma_backoffs);
+
+int8_t mac_helper_mac_mlme_be_set(int8_t interface_id, uint8_t min_be, uint8_t max_be);
+
+int8_t mac_helper_mac_mlme_data_request_restart_set(int8_t interface_id, mlme_request_restart_config_t *request_restart_config);
+
 int8_t mac_helper_mac_device_description_pan_id_update(int8_t interface_id, uint16_t pan_id);
 
 int8_t mac_helper_start_auto_cca_threshold(int8_t interface_id, uint8_t number_of_channels, int8_t default_dbm, int8_t high_limit, int8_t low_limit);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mpx_api.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mpx_api.h
index e4c2ee6407..b210e4a395 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mpx_api.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mpx_api.h
@@ -17,7 +17,7 @@
 
 #ifndef MPX_API_H_
 #define MPX_API_H_
-
+#include "mac_mcps.h"
 struct mcps_data_req_s;
 struct mcps_data_conf_s;
 struct mcps_data_ind_s;
@@ -30,9 +30,10 @@ typedef struct mpx_api_s mpx_api_t;
  * @param api API to handle the request
  * @param data MCPS-DATA.request specific values
  * @param user_id MPX user ID
+ * @param priority priority level
  *
  */
-typedef void mpx_data_request(const mpx_api_t *api, const struct mcps_data_req_s *data, uint16_t user_id);
+typedef void mpx_data_request(const mpx_api_t *api, const struct mcps_data_req_s *data, uint16_t user_id, mac_data_priority_t priority);
 
 /**
  * @brief mpx_data_queue_clean clean MPX user data
@@ -82,6 +83,22 @@ typedef uint16_t mpx_header_size_get(const mpx_api_t *api, uint16_t user_id);
  */
 typedef int8_t mpx_data_cb_register(const mpx_api_t *api, mpx_data_confirm *confirm_cb, mpx_data_indication *indication_cb, uint16_t user_id);
 
+/**
+ * @brief mpx_eui64_purge_request Purge EUI-64 related data from MPX
+ * @param api The API which handled the response
+ * @param eui64 EUI-64 to purge
+ *
+ */
+typedef void mpx_eui64_purge_request(const mpx_api_t *api, const uint8_t *eui64);
+
+/**
+ * \brief mpx_high_priority_mode_set Enable/Disable MPX high priority mode for TX process
+ * @param api The API which handled the response
+ * @param enable_mode True for enable High Priority mode, False disable
+ *
+ */
+typedef void mpx_high_priority_mode_set(const mpx_api_t *api, bool enable_mode);
+
 /**
  * \brief Struct mpx_api_s defines functions for MPX user for register call backs and send data.
  */
@@ -90,6 +107,8 @@ struct mpx_api_s {
     mpx_data_purge_request *mpx_data_purge;            /**< MPX data Purge. */
     mpx_header_size_get *mpx_headroom_size_get;     /**< MPX headroom size get in bytes. */
     mpx_data_cb_register *mpx_user_registration;    /**< MPX User cb registration must be call before enable to send or RX data*/
+    mpx_eui64_purge_request *mpx_eui64_purge;       /**< MPX Purge EUI-64 related data */
+    mpx_high_priority_mode_set *mpx_priority_mode_set; /**< MPX request to enable / disable High Priority mode */
 };
 
 
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/adaptation_interface.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/adaptation_interface.c
index 74d783c8e6..0374731751 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/adaptation_interface.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/adaptation_interface.c
@@ -101,6 +101,7 @@ typedef struct {
     uint16_t indirect_big_packet_threshold;
     uint16_t max_indirect_big_packets_total;
     uint16_t max_indirect_small_packets_per_child;
+    uint32_t last_rx_high_priority;
     bool fragmenter_active; /*!< Fragmenter state */
     adaptation_etx_update_cb *etx_update_cb;
     mpx_api_t *mpx_api;
@@ -109,6 +110,7 @@ typedef struct {
 } fragmenter_interface_t;
 
 #define LOWPAN_ACTIVE_UNICAST_ONGOING_MAX 10
+#define LOWPAN_HIGH_PRIORITY_STATE_LENGTH 50 //5 seconds 100us ticks
 
 /* Minimum buffer amount and memory size to ensure operation even in out of memory situation
  */
@@ -116,6 +118,7 @@ typedef struct {
 #define LOWPAN_MEM_LIMIT_MIN_MEMORY 10000
 #define LOWPAN_MEM_LIMIT_REMOVE_NORMAL 3000 // Remove when approaching memory limit
 #define LOWPAN_MEM_LIMIT_REMOVE_MAX 10000 // Remove when at memory limit
+#define LOWPAN_MEM_LIMIT_REMOVE_EF_MODE 20000 // Remove when out of memory and we are in EF mode
 
 
 
@@ -155,6 +158,7 @@ static bool lowpan_adaptation_indirect_queue_free_message(struct protocol_interf
 static fragmenter_tx_entry_t *lowpan_adaptation_indirect_mac_data_request_active(fragmenter_interface_t *interface_ptr, fragmenter_tx_entry_t *tx_ptr);
 
 static bool lowpan_buffer_tx_allowed(fragmenter_interface_t *interface_ptr, buffer_t *buf);
+static bool lowpan_adaptation_purge_from_mac(struct protocol_interface_info_entry *cur, fragmenter_interface_t *interface_ptr,  uint8_t msduhandle);
 
 static void lowpan_adaptation_etx_update_cb(protocol_interface_info_entry_t *cur, buffer_t *buf, const mcps_data_conf_t *confirm)
 {
@@ -251,6 +255,28 @@ static void lowpan_adaptation_tx_queue_write(fragmenter_interface_t *interface_p
     protocol_stats_update(STATS_AL_TX_QUEUE_SIZE, interface_ptr->directTxQueue_size);
 }
 
+static void lowpan_adaptation_tx_queue_write_to_front(fragmenter_interface_t *interface_ptr, buffer_t *buf)
+{
+    buffer_t *lower_priority_buf = NULL;
+
+    ns_list_foreach(buffer_t, cur, &interface_ptr->directTxQueue) {
+
+        if (cur->priority <= buf->priority) {
+            lower_priority_buf = cur;
+            break;
+        }
+    }
+
+    if (lower_priority_buf) {
+        ns_list_add_before(&interface_ptr->directTxQueue, lower_priority_buf, buf);
+    } else {
+        ns_list_add_to_end(&interface_ptr->directTxQueue, buf);
+    }
+    interface_ptr->directTxQueue_size++;
+    lowpan_adaptation_tx_queue_level_update(interface_ptr);
+    protocol_stats_update(STATS_AL_TX_QUEUE_SIZE, interface_ptr->directTxQueue_size);
+}
+
 static buffer_t *lowpan_adaptation_tx_queue_read(fragmenter_interface_t *interface_ptr)
 {
     // Currently this function is called only when data confirm is received for previously sent packet.
@@ -258,6 +284,12 @@ static buffer_t *lowpan_adaptation_tx_queue_read(fragmenter_interface_t *interfa
         return NULL;
     }
     ns_list_foreach_safe(buffer_t, buf, &interface_ptr->directTxQueue) {
+
+        if (buf->link_specific.ieee802_15_4.requestAck && interface_ptr->last_rx_high_priority &&  buf->priority < QOS_EXPEDITE_FORWARD) {
+            //Stop reading at this point when Priority is not enough big
+            return NULL;
+        }
+
         if (lowpan_buffer_tx_allowed(interface_ptr, buf)) {
             ns_list_remove(&interface_ptr->directTxQueue, buf);
             interface_ptr->directTxQueue_size--;
@@ -460,6 +492,7 @@ int8_t lowpan_adaptation_interface_reset(int8_t interface_id)
     buffer_free_list(&interface_ptr->directTxQueue);
     interface_ptr->directTxQueue_size = 0;
     interface_ptr->directTxQueue_level = 0;
+    interface_ptr->last_rx_high_priority = 0;
 
     return 0;
 }
@@ -506,12 +539,30 @@ void lowpan_adaptation_free_heap(bool full_gc)
 {
     ns_list_foreach(fragmenter_interface_t, interface_ptr, &fragmenter_interface_list) {
         // Go through all interfaces and free small amount of memory
-        // This is not very radical, but gives time to recover wthout causing too harsh changes
-        lowpan_adaptation_free_low_priority_packets(interface_ptr->interface_id, full_gc ? LOWPAN_MEM_LIMIT_REMOVE_MAX : LOWPAN_MEM_LIMIT_REMOVE_NORMAL);
+        // This is not very radical, but gives time to recover without causing too harsh changes
+        buffer_priority_t priority = QOS_NORMAL;
+        uint32_t amount = LOWPAN_MEM_LIMIT_REMOVE_NORMAL;
+
+        if (full_gc && interface_ptr->last_rx_high_priority) {
+            // We have encountered out of memory in EF state We handle this as Critical state
+            // Large amount of memory is freed and packets lower than EF priority can be dropped
+            priority = QOS_NETWORK_CTRL;
+            amount = LOWPAN_MEM_LIMIT_REMOVE_EF_MODE;
+        } else if (interface_ptr->last_rx_high_priority) {
+            // We are running out of memory and we are in EF state. We handle this as severe state
+            // Some amount of memory is freed and also lower than EF priority packets are dropped
+            priority = QOS_NETWORK_CTRL;
+            amount = LOWPAN_MEM_LIMIT_REMOVE_MAX;
+        } else if (full_gc) {
+            // We have encountered out of memory. we need to remove more packets, but we only remove normal priority packets
+            amount = LOWPAN_MEM_LIMIT_REMOVE_MAX;
+        }
+
+        lowpan_adaptation_free_low_priority_packets(interface_ptr->interface_id, priority, amount);
     }
 }
 
-int8_t lowpan_adaptation_free_low_priority_packets(int8_t interface_id, uint32_t requested_amount)
+int8_t lowpan_adaptation_free_low_priority_packets(int8_t interface_id, buffer_priority_t max_priority, uint32_t requested_amount)
 {
     fragmenter_interface_t *interface_ptr = lowpan_adaptation_interface_discover(interface_id);
 
@@ -543,7 +594,7 @@ int8_t lowpan_adaptation_free_low_priority_packets(int8_t interface_id, uint32_t
 
     //Only remove last entries from TX queue with low priority
     ns_list_foreach_reverse_safe(buffer_t, entry, &interface_ptr->directTxQueue) {
-        if (entry->priority == QOS_NORMAL) {
+        if (entry->priority <= max_priority) {
             memory_freed += sizeof(buffer_t) + entry->size;
             packets_freed++;
             ns_list_remove(&interface_ptr->directTxQueue, entry);
@@ -1028,12 +1079,34 @@ static void lowpan_data_request_to_mac(protocol_interface_info_entry_t *cur, buf
             buf->link_specific.ieee802_15_4.rf_channel_switch = false;
         }
     }
+    //Define data priority
+    mac_data_priority_t data_priority;
+
+    switch (buf->priority) {
+        case QOS_HIGH:
+            data_priority = MAC_DATA_MEDIUM_PRIORITY;
+            break;
+        case QOS_NETWORK_CTRL:
+            data_priority = MAC_DATA_HIGH_PRIORITY;
+            break;
+        case QOS_EXPEDITE_FORWARD:
+            data_priority = MAC_DATA_EXPEDITE_FORWARD;
+            break;
+        case QOS_MAC_BEACON:
+            data_priority = MAC_DATA_HIGH_PRIORITY;
+            break;
+        default:
+            data_priority = MAC_DATA_NORMAL_PRIORITY;
+            break;
+    }
 
     if (interface_ptr->mpx_api) {
         dataReq.ExtendedFrameExchange = buf->options.edfe_mode;
-        interface_ptr->mpx_api->mpx_data_request(interface_ptr->mpx_api, &dataReq, interface_ptr->mpx_user_id);
+        interface_ptr->mpx_api->mpx_data_request(interface_ptr->mpx_api, &dataReq, interface_ptr->mpx_user_id, data_priority);
     } else {
-        cur->mac_api->mcps_data_req(cur->mac_api, &dataReq);
+        mcps_data_req_ie_list_t ie_list;
+        memset(&ie_list, 0, sizeof(mcps_data_req_ie_list_t));
+        cur->mac_api->mcps_data_req_ext(cur->mac_api, &dataReq, &ie_list, NULL, data_priority);
     }
 }
 
@@ -1075,9 +1148,119 @@ static bool lowpan_buffer_tx_allowed(fragmenter_interface_t *interface_ptr, buff
     if (is_unicast && lowpan_adaptation_is_destination_tx_active(&interface_ptr->activeUnicastList, buf)) {
         return false;
     }
+
+    if (is_unicast && interface_ptr->last_rx_high_priority &&  buf->priority < QOS_EXPEDITE_FORWARD) {
+        return false;
+    }
     return true;
 }
 
+static uint32_t lowpan_adaptation_time_stamp_diff(uint32_t compare_stamp)
+{
+    if (protocol_core_monotonic_time < compare_stamp) {
+        return compare_stamp - protocol_core_monotonic_time;
+    }
+    return protocol_core_monotonic_time - compare_stamp;
+}
+
+static bool lowpan_adaptation_high_priority_state_exit(fragmenter_interface_t *interface_ptr)
+{
+    if (!interface_ptr->last_rx_high_priority || lowpan_adaptation_time_stamp_diff(interface_ptr->last_rx_high_priority) < LOWPAN_HIGH_PRIORITY_STATE_LENGTH) {
+        return false;
+    }
+
+    //Check First buffer_from tx queue
+    buffer_t *buf = ns_list_get_first(&interface_ptr->directTxQueue);
+    if (buf && buf->priority == QOS_EXPEDITE_FORWARD) {
+        //TX queue must not include any
+        return false;
+    }
+
+    //Check If we have a Any active TX process still active
+    ns_list_foreach(fragmenter_tx_entry_t, entry, &interface_ptr->activeUnicastList) {
+        if (entry->buf->priority == QOS_EXPEDITE_FORWARD) {
+            return false;
+        }
+    }
+
+    //Disable High Priority Mode
+    if (interface_ptr->mpx_api) {
+        interface_ptr->mpx_api->mpx_priority_mode_set(interface_ptr->mpx_api, false);
+    }
+    interface_ptr->last_rx_high_priority = 0;
+    return true;
+}
+
+static void lowpan_adaptation_high_priority_state_enable(protocol_interface_info_entry_t *cur, fragmenter_interface_t *interface_ptr)
+{
+
+    if (!interface_ptr->last_rx_high_priority) {
+        // MPX enaled stack must inform MPX to priority enable
+        if (interface_ptr->mpx_api) {
+            interface_ptr->mpx_api->mpx_priority_mode_set(interface_ptr->mpx_api, true);
+        }
+        //Purge Active tx queue's all possible's
+        if (!interface_ptr->fragmenter_active) {
+            //Purge Only When Fragmenter is not active
+            ns_list_foreach_reverse_safe(fragmenter_tx_entry_t, entry, &interface_ptr->activeUnicastList) {
+
+                if (lowpan_adaptation_purge_from_mac(cur, interface_ptr, entry->buf->seq)) {
+                    buffer_t *buf = entry->buf;
+                    ns_list_remove(&interface_ptr->activeUnicastList, entry);
+                    interface_ptr->activeTxList_size--;
+                    ns_dyn_mem_free(entry);
+                    //Add message to tx queue front based on priority. Now same priority at buf is prioritised at order
+                    lowpan_adaptation_tx_queue_write_to_front(interface_ptr, buf);
+                    random_early_detetction_aq_calc(cur->random_early_detection, interface_ptr->directTxQueue_size);
+                }
+            }
+        }
+    }
+
+    //Store timestamp for indicate last RX High Priority message
+    interface_ptr->last_rx_high_priority = protocol_core_monotonic_time ? protocol_core_monotonic_time : 1;
+
+}
+
+
+static void lowpan_adaptation_priority_status_update(protocol_interface_info_entry_t *cur, fragmenter_interface_t *interface_ptr, buffer_priority_t priority)
+{
+    if (priority == QOS_EXPEDITE_FORWARD) {
+        lowpan_adaptation_high_priority_state_enable(cur, interface_ptr);
+    } else {
+        //Let check can we disable possible High Priority state
+        lowpan_adaptation_high_priority_state_exit(interface_ptr);
+    }
+}
+
+void lowpan_adaptation_expedite_forward_enable(protocol_interface_info_entry_t *cur)
+{
+    fragmenter_interface_t *interface_ptr = lowpan_adaptation_interface_discover(cur->id);
+    if (!interface_ptr) {
+        return;
+    }
+    lowpan_adaptation_high_priority_state_enable(cur, interface_ptr);
+}
+
+void lowpan_adaptation_interface_slow_timer(protocol_interface_info_entry_t *cur)
+{
+    fragmenter_interface_t *interface_ptr = lowpan_adaptation_interface_discover(cur->id);
+    if (!interface_ptr) {
+        return;
+    }
+
+    if (lowpan_adaptation_high_priority_state_exit(interface_ptr)) {
+        //Activate Packets from TX queue
+        buffer_t *buf_from_queue = lowpan_adaptation_tx_queue_read(interface_ptr);
+        while (buf_from_queue) {
+            lowpan_adaptation_interface_tx(cur, buf_from_queue);
+            buf_from_queue = lowpan_adaptation_tx_queue_read(interface_ptr);
+        }
+        //Update Average QUEUE
+        random_early_detetction_aq_calc(cur->random_early_detection, interface_ptr->directTxQueue_size);
+    }
+}
+
 int8_t lowpan_adaptation_interface_tx(protocol_interface_info_entry_t *cur, buffer_t *buf)
 {
     bool is_room_for_new_message;
@@ -1105,6 +1288,8 @@ int8_t lowpan_adaptation_interface_tx(protocol_interface_info_entry_t *cur, buff
         buffer_priority_set(buf, QOS_HIGH);
     }
 
+    //Update priority status
+    lowpan_adaptation_priority_status_update(cur, interface_ptr, buf->priority);
 
     //Check packet size
     bool fragmented_needed = lowpan_adaptation_request_longer_than_mtu(cur, buf, interface_ptr);
@@ -1409,6 +1594,8 @@ int8_t lowpan_adaptation_interface_tx_confirm(protocol_interface_info_entry_t *c
     }
     // When confirmation is for direct transmission, push all allowed buffers to MAC
     if (active_direct_confirm == true) {
+        //Check Possibility for exit from High Priority state
+        lowpan_adaptation_high_priority_state_exit(interface_ptr);
         buffer_t *buf_from_queue = lowpan_adaptation_tx_queue_read(interface_ptr);
         while (buf_from_queue) {
             lowpan_adaptation_interface_tx(cur, buf_from_queue);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/lowpan_adaptation_interface.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/lowpan_adaptation_interface.h
index 13401df1f0..5b88e28723 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/lowpan_adaptation_interface.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/lowpan_adaptation_interface.h
@@ -38,7 +38,7 @@ int8_t lowpan_adaptation_interface_mpx_register(int8_t interface_id, struct mpx_
 
 void lowpan_adaptation_free_heap(bool full_gc);
 
-int8_t lowpan_adaptation_free_low_priority_packets(int8_t interface_id, uint32_t requested_amount);
+int8_t lowpan_adaptation_free_low_priority_packets(int8_t interface_id, buffer_priority_t max_priority, uint32_t requested_amount);
 
 
 /**
@@ -62,4 +62,8 @@ int8_t lowpan_adaptation_free_messages_from_queues_by_address(struct protocol_in
 
 int8_t lowpan_adaptation_indirect_queue_params_set(struct protocol_interface_info_entry *cur, uint16_t indirect_big_packet_threshold, uint16_t max_indirect_big_packets_total, uint16_t max_indirect_small_packets_per_child);
 
+void lowpan_adaptation_expedite_forward_enable(struct protocol_interface_info_entry *cur);
+
+void lowpan_adaptation_interface_slow_timer(struct protocol_interface_info_entry *cur);
+
 #endif /* LOWPAN_ADAPTATION_INTERFACE_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_bbr_api.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_bbr_api.c
index 7a86817450..1fc5a5940c 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_bbr_api.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_bbr_api.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018-2019, Arm Limited and affiliates.
+ * Copyright (c) 2018-2021, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -38,16 +38,18 @@
 #include "RPL/rpl_data.h"
 #include "Common_Protocols/icmpv6.h"
 #include "Common_Protocols/icmpv6_radv.h"
+#include "Common_Protocols/ip.h"
 #include "ws_management_api.h"
 #include "net_rpl.h"
 #include "Service_Libs/nd_proxy/nd_proxy.h"
 #include "6LoWPAN/ws/ws_bbr_api_internal.h"
 #include "6LoWPAN/ws/ws_pae_controller.h"
+#include "6LoWPAN/lowpan_adaptation_interface.h"
 #include "DHCPv6_Server/DHCPv6_server_service.h"
 #include "DHCPv6_client/dhcpv6_client_api.h"
 #include "libDHCPv6/libDHCPv6_vendordata.h"
 #include "libNET/src/net_dns_internal.h"
-
+#include "platform/os_whiteboard.h"
 
 #include "ws_bbr_api.h"
 
@@ -450,6 +452,7 @@ static void ws_bbr_dodag_get(uint8_t *local_prefix_ptr, uint8_t *global_prefix_p
     memcpy(global_prefix_ptr, global_address, 8);
     return;
 }
+
 static void wisun_bbr_na_send(int8_t interface_id, const uint8_t target[static 16])
 {
     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
@@ -461,6 +464,8 @@ static void wisun_bbr_na_send(int8_t interface_id, const uint8_t target[static 1
         return;
     }
 
+    whiteboard_os_modify(target, ADD);
+
     buffer_t *buffer = icmpv6_build_na(cur, false, true, true, target, NULL, ADDR_UNSPECIFIED);
     protocol_push(buffer);
     return;
@@ -514,7 +519,6 @@ static void ws_bbr_dhcp_server_dns_info_update(protocol_interface_info_entry_t *
         for (int n = 0; n < MAX_DNS_RESOLUTIONS; n++) {
             if (pre_resolved_dns_queries[n].domain_name != NULL) {
                 ptr = net_dns_option_vendor_option_data_dns_query_write(ptr, pre_resolved_dns_queries[n].address, pre_resolved_dns_queries[n].domain_name);
-                tr_info("set DNS query result for %s, addr: %s", pre_resolved_dns_queries[n].domain_name, tr_ipv6(pre_resolved_dns_queries[n].address));
             }
         }
     }
@@ -522,6 +526,15 @@ static void ws_bbr_dhcp_server_dns_info_update(protocol_interface_info_entry_t *
     DHCPv6_server_service_set_vendor_data(cur->id, global_id, ARM_ENTERPRISE_NUMBER, dhcp_vendor_data_ptr, dhcp_vendor_data_len);
 }
 
+static void wisun_dhcp_address_remove_cb(int8_t interfaceId, uint8_t *targetAddress, void *prefix_info)
+{
+    (void) interfaceId;
+    (void) prefix_info;
+    if (targetAddress) {
+        whiteboard_os_modify(targetAddress, REMOVE);
+    }
+}
+
 static void ws_bbr_dhcp_server_start(protocol_interface_info_entry_t *cur, uint8_t *global_id, uint32_t dhcp_address_lifetime)
 {
     uint8_t ll[16];
@@ -535,7 +548,7 @@ static void ws_bbr_dhcp_server_start(protocol_interface_info_entry_t *cur, uint8
         tr_error("DHCPv6 Server create fail");
         return;
     }
-    DHCPv6_server_service_callback_set(cur->id, global_id, NULL, wisun_dhcp_address_add_cb);
+    DHCPv6_server_service_callback_set(cur->id, global_id, wisun_dhcp_address_remove_cb, wisun_dhcp_address_add_cb);
     //Check for anonymous mode
     bool anonymous = (configuration & BBR_DHCP_ANONYMOUS) ? true : false;
 
@@ -896,6 +909,18 @@ bool ws_bbr_ready_to_start(protocol_interface_info_entry_t *cur)
     return true;
 }
 
+static void ws_bbr_forwarding_cb(protocol_interface_info_entry_t *interface, buffer_t *buf)
+{
+    uint8_t traffic_class = buf->options.traffic_class >> IP_TCLASS_DSCP_SHIFT;
+
+    if (traffic_class == IP_DSCP_EF) {
+        //indicate EF forwarding to adaptation
+        lowpan_adaptation_expedite_forward_enable(interface);
+    }
+}
+
+
+
 void ws_bbr_init(protocol_interface_info_entry_t *interface)
 {
     (void) interface;
@@ -908,6 +933,7 @@ void ws_bbr_init(protocol_interface_info_entry_t *interface)
         ws_bbr_fhss_bsi = ws_bbr_bsi_read(&bbr_info_nvm_tlv);
         tr_debug("Read BSI %u from NVM", ws_bbr_fhss_bsi);
     }
+    interface->if_common_forwarding_out_cb = &ws_bbr_forwarding_cb;
 }
 
 
@@ -1447,6 +1473,7 @@ int ws_bbr_dns_query_result_set(int8_t interface_id, const uint8_t address[16],
             if (address) {
                 // Update address
                 memcpy(pre_resolved_dns_queries[n].address, address, 16);
+                tr_info("Update DNS query result for %s, addr: %s", pre_resolved_dns_queries[n].domain_name, tr_ipv6(pre_resolved_dns_queries[n].address));
             } else {
                 // delete entry
                 memset(pre_resolved_dns_queries[n].address, 0, 16);
@@ -1469,6 +1496,7 @@ int ws_bbr_dns_query_result_set(int8_t interface_id, const uint8_t address[16],
                 }
                 memcpy(pre_resolved_dns_queries[n].address, address, 16);
                 strcpy(pre_resolved_dns_queries[n].domain_name, domain_name_ptr);
+                tr_info("set DNS query result for %s, addr: %s", pre_resolved_dns_queries[n].domain_name, tr_ipv6(pre_resolved_dns_queries[n].address));
                 goto update_information;
             }
         }
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_bootstrap.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_bootstrap.c
index d9f5dcd97d..013c3c6e99 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_bootstrap.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_bootstrap.c
@@ -33,6 +33,7 @@
 #include "6LoWPAN/Bootstraps/protocol_6lowpan.h"
 #include "6LoWPAN/Bootstraps/protocol_6lowpan_interface.h"
 #include "ipv6_stack/protocol_ipv6.h"
+#include "ipv6_stack/ipv6_routing_table.h"
 #include "6LoWPAN/MAC/mac_helper.h"
 #include "6LoWPAN/MAC/mac_data_poll.h"
 #include "6LoWPAN/MAC/mpx_api.h"
@@ -104,7 +105,7 @@ static void ws_bootstrap_nw_frame_counter_read(protocol_interface_info_entry_t *
 static void ws_bootstrap_nw_info_updated(protocol_interface_info_entry_t *interface_ptr, uint16_t pan_id, uint16_t pan_version, char *network_name);
 static void ws_bootstrap_authentication_completed(protocol_interface_info_entry_t *cur, auth_result_e result, uint8_t *target_eui_64);
 static const uint8_t *ws_bootstrap_authentication_next_target(protocol_interface_info_entry_t *cur, const uint8_t *previous_eui_64, uint16_t *pan_id);
-static bool ws_bootstrap_congestion_get(protocol_interface_info_entry_t *interface_ptr, uint16_t active_supp);
+static bool ws_bootstrap_eapol_congestion_get(protocol_interface_info_entry_t *interface_ptr, uint16_t active_supp);
 static void ws_bootstrap_pan_version_increment(protocol_interface_info_entry_t *cur);
 static ws_nud_table_entry_t *ws_nud_entry_discover(protocol_interface_info_entry_t *cur, void *neighbor);
 static void ws_nud_entry_remove(protocol_interface_info_entry_t *cur, mac_neighbor_table_entry_t *entry_ptr);
@@ -123,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*/
@@ -137,15 +140,11 @@ static void ws_bootsrap_create_ll_address(uint8_t *ll_address, const uint8_t *ma
     ll_address[8] ^= 2;
 }
 
-mac_neighbor_table_entry_t *ws_bootstrap_mac_neighbor_add(struct protocol_interface_info_entry *interface, const uint8_t *src64)
 
+
+static mac_neighbor_table_entry_t *ws_bootstrap_mac_neighbor_allocate(struct protocol_interface_info_entry *interface, const uint8_t *src64)
 {
-    mac_neighbor_table_entry_t *neighbor = mac_neighbor_table_address_discover(mac_neighbor_info(interface), src64, MAC_ADDR_MODE_64_BIT);
-    if (neighbor) {
-        return neighbor;
-    }
-
-    neighbor = mac_neighbor_table_entry_allocate(mac_neighbor_info(interface), src64);
+    mac_neighbor_table_entry_t *neighbor = mac_neighbor_table_entry_allocate(mac_neighbor_info(interface), src64);
     if (!neighbor) {
         return NULL;
     }
@@ -159,6 +158,17 @@ mac_neighbor_table_entry_t *ws_bootstrap_mac_neighbor_add(struct protocol_interf
     return neighbor;
 }
 
+mac_neighbor_table_entry_t *ws_bootstrap_mac_neighbor_add(struct protocol_interface_info_entry *interface, const uint8_t *src64)
+
+{
+    mac_neighbor_table_entry_t *neighbor = mac_neighbor_table_address_discover(mac_neighbor_info(interface), src64, MAC_ADDR_MODE_64_BIT);
+    if (neighbor) {
+        return neighbor;
+    }
+
+    return ws_bootstrap_mac_neighbor_allocate(interface, src64);
+}
+
 void ws_bootstrap_neighbor_set_stable(struct protocol_interface_info_entry *interface, const uint8_t *src64)
 {
     mac_neighbor_table_entry_t *neighbor = mac_neighbor_table_address_discover(mac_neighbor_info(interface), src64, MAC_ADDR_MODE_64_BIT);
@@ -199,13 +209,13 @@ static void ws_bootstap_eapol_neigh_entry_allocate(struct protocol_interface_inf
     if (!mac_entry) {
         return;
     }
-    ws_bootstrap_neighbor_set_stable(interface, mac_64);
     mac_entry->lifetime = 0xffffffff;
     mac_entry->link_lifetime = 0xffffffff;
     ws_neighbor_class_entry_t *ws_neigh = ws_neighbor_class_entry_get(&interface->ws_info->neighbor_storage, mac_entry->index);
     if (!ws_neigh) {
         return;
     }
+
     interface->ws_info->eapol_tx_index = mac_entry->index;
 }
 
@@ -301,18 +311,25 @@ static void ws_bootstrap_address_notification_cb(struct protocol_interface_info_
     }
 }
 
-static void ws_bootstrap_configure_max_retries(protocol_interface_info_entry_t *cur, uint8_t max_mac_retries, uint8_t max_channel_retries)
+static void ws_bootstrap_configure_max_retries(protocol_interface_info_entry_t *cur, uint8_t max_mac_retries)
 {
     mac_helper_mac_mlme_max_retry_set(cur->id, max_mac_retries);
+}
 
-    const fhss_ws_configuration_t *fhss_configuration_cur = ns_fhss_ws_configuration_get(cur->ws_info->fhss_api);
-    if (fhss_configuration_cur && fhss_configuration_cur->config_parameters.number_of_channel_retries != max_channel_retries) {
-        fhss_ws_configuration_t fhss_configuration;
-        memset(&fhss_configuration, 0, sizeof(fhss_ws_configuration_t));
-        memcpy(&fhss_configuration, ns_fhss_ws_configuration_get(cur->ws_info->fhss_api), sizeof(fhss_ws_configuration_t));
-        fhss_configuration.config_parameters.number_of_channel_retries = max_channel_retries;
-        ns_fhss_ws_configuration_set(cur->ws_info->fhss_api, &fhss_configuration);
-    }
+static void ws_bootstrap_configure_csma_ca_backoffs(protocol_interface_info_entry_t *cur, uint8_t max_backoffs, uint8_t min_be, uint8_t max_be)
+{
+    mac_helper_mac_mlme_max_csma_backoffs_set(cur->id, max_backoffs);
+    mac_helper_mac_mlme_be_set(cur->id, min_be, max_be);
+}
+
+static void ws_bootstrap_configure_data_request_restart(protocol_interface_info_entry_t *cur, uint8_t cca_failure_restart_max, uint8_t tx_failure_restart_max, uint16_t blacklist_min_ms, uint16_t blacklist_max_ms)
+{
+    mlme_request_restart_config_t request_restart_config;
+    request_restart_config.cca_failure_restart_max = cca_failure_restart_max;
+    request_restart_config.tx_failure_restart_max = tx_failure_restart_max;
+    request_restart_config.blacklist_min_ms = blacklist_min_ms;
+    request_restart_config.blacklist_max_ms = blacklist_max_ms;
+    mac_helper_mac_mlme_data_request_restart_set(cur->id, &request_restart_config);
 }
 
 static int ws_bootstrap_tasklet_init(protocol_interface_info_entry_t *cur)
@@ -662,6 +679,8 @@ static int8_t ws_fhss_initialize(protocol_interface_info_entry_t *cur)
             return -1;
         }
         ns_sw_mac_fhss_register(cur->mac_api, fhss_api);
+        // Allow transmitting unicast frames only on TX slots in normal and expedited forwarding mode
+        ns_fhss_ws_set_tx_allowance_level(fhss_api, WS_TX_SLOT, WS_TX_SLOT);
     } else {
         // Read defaults from the configuration to help FHSS testing
         const fhss_ws_configuration_t *fhss_configuration_copy = ns_fhss_ws_configuration_get(fhss_api);
@@ -713,18 +732,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;
     }
@@ -749,8 +783,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);
@@ -999,7 +1033,8 @@ static void ws_bootstrap_dhcp_info_notify_cb(int8_t interface, dhcp_option_notif
             break;
 
         case DHCPV6_OPTION_DNS_SERVERS:
-            while (options->option.generic.data_length) {
+            while (options->option.generic.data_length && options->option.generic.data_length >= 16 && options->option.generic.data_length % 16 == 0) {
+                // Validate payload to have full 16 byte length addresses without any extra bytes
                 net_dns_server_address_set(interface, server_ll64, options->option.generic.data, server_info->life_time);
                 options->option.generic.data_length -= 16;
                 options->option.generic.data += 16;
@@ -1099,6 +1134,10 @@ static int8_t ws_bootstrap_up(protocol_interface_info_entry_t *cur)
     ws_nud_table_reset(cur);
 
     ws_bootstrap_candidate_table_reset(cur);
+    // Zero uptime counters
+    cur->ws_info->uptime = 0;
+    cur->ws_info->authentication_time = 0;
+    cur->ws_info->connected_time = 0;
 
     blacklist_params_set(
         WS_BLACKLIST_ENTRY_LIFETIME,
@@ -1121,6 +1160,8 @@ void ws_bootstrap_disconnect(protocol_interface_info_entry_t *cur, ws_bootsrap_e
         //Already moved to leaving state.
         return;
     }
+    // We are no longer connected
+    cur->ws_info->connected_time = 0;
 
     if (cur->rpl_domain && cur->nwk_bootstrap_state == ER_BOOTSRAP_DONE) {
         //Stop Asych Timer
@@ -1175,6 +1216,7 @@ static int8_t ws_bootstrap_down(protocol_interface_info_entry_t *cur)
     ws_pae_controller_stop(cur);
     ws_bootstrap_candidate_table_reset(cur);
     blacklist_clear();
+    cur->if_common_forwarding_out_cb = NULL;
 
     return nwk_6lowpan_down(cur);
 }
@@ -1902,6 +1944,7 @@ static void ws_bootstrap_asynch_ind(struct protocol_interface_info_entry *cur, c
         // Not from long address
         return;
     }
+    ws_stats_update(cur, STATS_WS_ASYNCH_RX, 1);
     //Validate network name
     switch (message_type) {
         case WS_FT_PAN_ADVERT:
@@ -1918,7 +1961,6 @@ static void ws_bootstrap_asynch_ind(struct protocol_interface_info_entry *cur, c
         default:
             return;
     }
-    ws_stats_update(cur, STATS_WS_ASYNCH_RX, 1);
     //UTT-IE and US-IE are mandatory for all Asynch Messages
     ws_utt_ie_t ws_utt;
     if (!ws_wh_utt_read(ie_ext->headerIeList, ie_ext->headerIeListLength, &ws_utt)) {
@@ -1941,20 +1983,26 @@ static void ws_bootstrap_asynch_ind(struct protocol_interface_info_entry *cur, c
     switch (message_type) {
         case WS_FT_PAN_ADVERT:
             // Analyse Advertisement
+            ws_stats_update(cur, STATS_WS_ASYNCH_RX_PA, 1);
             tr_info("received ADVERT Src:%s panid:%x rssi:%d", trace_array(data->SrcAddr, 8), data->SrcPANId, data->signal_dbm);
             ws_bootstrap_pan_advertisement_analyse(cur, data, ie_ext, &ws_utt, &ws_us);
             break;
         case WS_FT_PAN_ADVERT_SOL:
+            ws_stats_update(cur, STATS_WS_ASYNCH_RX_PAS, 1);
             tr_info("received ADVERT SOL Src:%s rssi:%d", trace_array(data->SrcAddr, 8), data->signal_dbm);
             ws_bootstrap_pan_advertisement_solicit_analyse(cur, data, &ws_utt, &ws_us);
             break;
         case WS_FT_PAN_CONF:
+            ws_stats_update(cur, STATS_WS_ASYNCH_RX_PC, 1);
             tr_info("received CONFIG Src:%s rssi:%d", trace_array(data->SrcAddr, 8), data->signal_dbm);
             ws_bootstrap_pan_config_analyse(cur, data, ie_ext, &ws_utt, &ws_us);
             break;
-        default:
+        case WS_FT_PAN_CONF_SOL:
+            ws_stats_update(cur, STATS_WS_ASYNCH_RX_PCS, 1);
             tr_info("received CONFIG SOL Src:%s rssi:%d", trace_array(data->SrcAddr, 8), data->signal_dbm);
             ws_bootstrap_pan_config_solicit_analyse(cur, data, &ws_utt, &ws_us);
+        default:
+            // Unknown message do not process
             break;
     }
 }
@@ -1989,6 +2037,13 @@ static void ws_bootstrap_neighbor_table_clean(struct protocol_interface_info_ent
         // Enough neighbor entries
         return;
     }
+    uint32_t temp_link_min_timeout;
+    if (mac_neighbor_info(interface)->neighbour_list_size == mac_neighbor_info(interface)->list_total_size) {
+        temp_link_min_timeout = 1; //Accept 1 second time from last
+    } else {
+        temp_link_min_timeout = interface->ws_info->cfg->timing.temp_link_min_timeout;
+    }
+
     memcpy(ll_target, ADDR_LINK_LOCAL_PREFIX, 8);
 
     uint32_t current_time_stamp = ns_sw_mac_read_current_timestamp(interface->mac_api);
@@ -2001,17 +2056,14 @@ static void ws_bootstrap_neighbor_table_clean(struct protocol_interface_info_ent
             continue;
         }
 
-        if (cur->link_lifetime < WS_NEIGHBOUR_TEMPORARY_NEIGH_MAX_LIFETIME) {
-            continue;
-        }
-
         if (cur->link_role == PRIORITY_PARENT_NEIGHBOUR) {
             //This is our primary parent we cannot delete
             continue;
         }
 
-        if (cur->nud_active || ws_neighbor->negative_aro_send) {
+        if (cur->nud_active) {
             //If NUD process is active do not trig
+            // or Negative ARO is active
             continue;
         }
 
@@ -2020,6 +2072,11 @@ static void ws_bootstrap_neighbor_table_clean(struct protocol_interface_info_ent
             continue;
         }
 
+        if (cur->link_lifetime > WS_NEIGHBOUR_TEMPORARY_ENTRY_LIFETIME && cur->link_lifetime <= WS_NEIGHBOUR_TEMPORARY_NEIGH_MAX_LIFETIME) {
+            //Do not permit to remove configured temp life time
+            continue;
+        }
+
         if (cur->trusted_device) {
 
             if (ipv6_neighbour_has_registered_by_eui64(&interface->ipv6_neighbour_cache, cur->mac64)) {
@@ -2038,7 +2095,7 @@ static void ws_bootstrap_neighbor_table_clean(struct protocol_interface_info_ent
 
         //Read current timestamp
         uint32_t time_from_last_unicast_shedule = ws_time_from_last_unicast_traffic(current_time_stamp, ws_neighbor);
-        if (time_from_last_unicast_shedule > interface->ws_info->cfg->timing.temp_link_min_timeout) {
+        if (time_from_last_unicast_shedule >= temp_link_min_timeout) {
             //Accept only Enough Old Device
             if (!neighbor_entry_ptr) {
                 //Accept first compare
@@ -2087,6 +2144,7 @@ static bool ws_bootstrap_neighbor_info_request(struct protocol_interface_info_en
         mac_neighbor_table_neighbor_remove(mac_neighbor_info(interface), neighbor_buffer->neighbor);
         return false;
     }
+    ws_stats_update(interface, STATS_WS_NEIGHBOUR_ADD, 1);
     return true;
 }
 
@@ -2102,6 +2160,11 @@ static void ws_neighbor_entry_remove_notify(mac_neighbor_table_entry_t *entry_pt
 
     //TODO State machine check here
 
+    if (ipv6_neighbour_has_registered_by_eui64(&cur->ipv6_neighbour_cache, entry_ptr->mac64)) {
+        // Child entry deleted
+        ws_stats_update(cur, STATS_WS_CHILD_REMOVE, 1);
+    }
+
     if (entry_ptr->ffd_device) {
         protocol_6lowpan_release_short_link_address_from_neighcache(cur, entry_ptr->mac16);
         protocol_6lowpan_release_long_link_address_from_neighcache(cur, entry_ptr->mac64);
@@ -2111,6 +2174,8 @@ static void ws_neighbor_entry_remove_notify(mac_neighbor_table_entry_t *entry_pt
     ws_nud_entry_remove(cur, entry_ptr);
 
     ws_bootstrap_neighbor_delete(cur, entry_ptr);
+    ws_stats_update(cur, STATS_WS_NEIGHBOUR_REMOVE, 1);
+
 }
 
 static uint32_t ws_probe_init_time_get(protocol_interface_info_entry_t *cur)
@@ -2128,12 +2193,14 @@ static bool ws_neighbor_entry_nud_notify(mac_neighbor_table_entry_t *entry_ptr,
     uint8_t ll_address[16];
     bool nud_proces = false;
     bool activate_nud = false;
+    bool child;
+    bool candidate_parent;
     protocol_interface_info_entry_t *cur = user_data;
 
     ws_neighbor_class_entry_t *ws_neighbor = ws_neighbor_class_entry_get(&cur->ws_info->neighbor_storage, entry_ptr->index);
     etx_storage_t *etx_entry = etx_storage_entry_get(cur->id, entry_ptr->index);
 
-    if (!entry_ptr->trusted_device || !ws_neighbor || !etx_entry || ws_neighbor->negative_aro_send || entry_ptr->link_lifetime < WS_NEIGHBOUR_TEMPORARY_NEIGH_MAX_LIFETIME) {
+    if (!entry_ptr->trusted_device || !ws_neighbor || !etx_entry || entry_ptr->link_lifetime <= WS_NEIGHBOUR_TEMPORARY_NEIGH_MAX_LIFETIME) {
         return false;
     }
 
@@ -2141,12 +2208,17 @@ static bool ws_neighbor_entry_nud_notify(mac_neighbor_table_entry_t *entry_ptr,
 
     if (time_from_start > WS_NEIGHBOR_NUD_TIMEOUT) {
 
+        child = ipv6_neighbour_has_registered_by_eui64(&cur->ipv6_neighbour_cache, entry_ptr->mac64);
+        candidate_parent = rpl_control_is_dodag_parent_candidate(cur, ll_address, cur->ws_info->cfg->gen.rpl_parent_candidate_max);
         /* For parents ARO registration is sent in link timeout times
          * For candidate parents NUD is needed
          * For children NUD is sent only at very close to end
          */
-        if (ipv6_neighbour_has_registered_by_eui64(&cur->ipv6_neighbour_cache, entry_ptr->mac64) &&
-                (time_from_start < WS_NEIGHBOR_NUD_TIMEOUT * 1.8)) {
+        if (!child && !candidate_parent) {
+            // No need for keep alive
+            return false;
+        }
+        if (child && (time_from_start < WS_NEIGHBOR_NUD_TIMEOUT * 1.8)) {
             /* This is our child with valid ARO registration send NUD if we are close to delete
              *
              * if ARO was received link is considered active so this is only in case of very long ARO registration times
@@ -2157,11 +2229,6 @@ static bool ws_neighbor_entry_nud_notify(mac_neighbor_table_entry_t *entry_ptr,
             return false;
         }
 
-        if (!rpl_control_is_dodag_parent_candidate(cur, ll_address, cur->ws_info->cfg->gen.rpl_parent_candidate_max)) {
-            //NUD Not needed for if neighbour is not parent candidate
-            return false;
-        }
-
         if (time_from_start > WS_NEIGHBOR_NUD_TIMEOUT * 1.5) {
             activate_nud = true;
         } else {
@@ -2331,7 +2398,7 @@ int ws_bootstrap_init(int8_t interface_id, net_6lowpan_mode_e bootstrap_mode)
         ret_val =  -4;
         goto init_fail;
     }
-    if (ws_pae_controller_cb_register(cur, &ws_bootstrap_authentication_completed, &ws_bootstrap_authentication_next_target, &ws_bootstrap_nw_key_set, &ws_bootstrap_nw_key_clear, &ws_bootstrap_nw_key_index_set, &ws_bootstrap_nw_frame_counter_set, &ws_bootstrap_nw_frame_counter_read, &ws_bootstrap_pan_version_increment, &ws_bootstrap_nw_info_updated, &ws_bootstrap_congestion_get) < 0) {
+    if (ws_pae_controller_cb_register(cur, &ws_bootstrap_authentication_completed, &ws_bootstrap_authentication_next_target, &ws_bootstrap_nw_key_set, &ws_bootstrap_nw_key_clear, &ws_bootstrap_nw_key_index_set, &ws_bootstrap_nw_frame_counter_set, &ws_bootstrap_nw_frame_counter_read, &ws_bootstrap_pan_version_increment, &ws_bootstrap_nw_info_updated, &ws_bootstrap_eapol_congestion_get) < 0) {
         ret_val =  -4;
         goto init_fail;
     }
@@ -2587,6 +2654,13 @@ static void ws_set_fhss_hop(protocol_interface_info_entry_t *cur)
     // Calculate own hop count. This method gets inaccurate when hop count increases.
     uint8_t own_hop = (own_rank - rank_inc) / rank_inc;
     ns_fhss_ws_set_hop_count(cur->ws_info->fhss_api, own_hop);
+    if (own_hop == 1) {
+        // Allow transmitting unicast frames only on TX slots in normal mode and always in expedited forwarding mode for first hop
+        ns_fhss_ws_set_tx_allowance_level(cur->ws_info->fhss_api, WS_TX_SLOT, WS_TX_ALWAYS);
+    } else {
+        // Allow transmitting unicast frames only on TX slots in normal and expedited forwarding mode for other hops
+        ns_fhss_ws_set_tx_allowance_level(cur->ws_info->fhss_api, WS_TX_SLOT, WS_TX_SLOT);
+    }
     tr_debug("own hop: %u, own rank: %u, rank inc: %u", own_hop, own_rank, rank_inc);
 }
 
@@ -2729,7 +2803,9 @@ static void ws_bootstrap_rpl_callback(rpl_event_t event, void *handle)
 
         ws_set_fhss_hop(cur);
         // Set retry configuration for bootstrap ready state
-        ws_bootstrap_configure_max_retries(cur, WS_MAX_FRAME_RETRIES, WS_NUMBER_OF_CHANNEL_RETRIES);
+        ws_bootstrap_configure_max_retries(cur, WS_MAX_FRAME_RETRIES);
+        // Set TX failure request restart configuration
+        ws_bootstrap_configure_data_request_restart(cur, WS_CCA_REQUEST_RESTART_MAX, WS_TX_REQUEST_RESTART_MAX, WS_REQUEST_RESTART_BLACKLIST_MIN, WS_REQUEST_RESTART_BLACKLIST_MAX);
     } else if (event == RPL_EVENT_LOCAL_REPAIR_NO_MORE_DIS) {
         /*
          * RPL goes to passive mode, but does not require any extra changed
@@ -3167,10 +3243,12 @@ static void ws_bootstrap_nw_info_updated(protocol_interface_info_entry_t *cur, u
 
 static void ws_bootstrap_authentication_completed(protocol_interface_info_entry_t *cur, auth_result_e result, uint8_t *target_eui_64)
 {
-    (void) target_eui_64;
-
     if (result == AUTH_RESULT_OK) {
-        tr_debug("authentication success");
+        tr_debug("authentication success eui64:%s", trace_array(target_eui_64, 8));
+        if (target_eui_64) {
+            // Authentication was made contacting the authenticator
+            cur->ws_info->authentication_time = cur->ws_info->uptime;
+        }
         ws_bootstrap_event_configuration_start(cur);
     } else if (result == AUTH_RESULT_ERR_TX_ERR) {
         // eapol parent selected is not working
@@ -3214,15 +3292,41 @@ static const uint8_t *ws_bootstrap_authentication_next_target(protocol_interface
     return previous_eui_64;
 }
 
-static bool ws_bootstrap_congestion_get(protocol_interface_info_entry_t *interface_ptr, uint16_t active_supp)
+static void ws_bootstrap_eapol_congestion_init(protocol_interface_info_entry_t *cur)
 {
-    if (interface_ptr == NULL || interface_ptr->random_early_detection == NULL) {
+    random_early_detection_free(cur->llc_random_early_detection);
+    cur->llc_random_early_detection = NULL;
+
+    if (cur->llc_random_early_detection == NULL) {
+        cur->llc_random_early_detection = random_early_detection_create(
+                                              cur->ws_info->cfg->sec_prot.max_simult_sec_neg_tx_queue_min,
+                                              cur->ws_info->cfg->sec_prot.max_simult_sec_neg_tx_queue_max,
+                                              100, RED_AVERAGE_WEIGHT_EIGHTH);
+    }
+
+    random_early_detection_free(cur->llc_eapol_random_early_detection);
+    cur->llc_eapol_random_early_detection = NULL;
+
+    if (cur->llc_eapol_random_early_detection == NULL) {
+        cur->llc_eapol_random_early_detection = random_early_detection_create(
+                                                    cur->ws_info->cfg->sec_prot.max_simult_sec_neg_tx_queue_min,
+                                                    cur->ws_info->cfg->sec_prot.max_simult_sec_neg_tx_queue_max,
+                                                    100, RED_AVERAGE_WEIGHT_EIGHTH);
+    }
+}
+
+static bool ws_bootstrap_eapol_congestion_get(protocol_interface_info_entry_t *cur, uint16_t active_supp)
+{
+    if (cur == NULL || cur->random_early_detection == NULL || cur->llc_random_early_detection == NULL || cur->llc_eapol_random_early_detection == NULL) {
         return false;
     }
 
     bool return_value = false;
     static struct red_info_s *red_info = NULL;
-    uint16_t average = 0;
+    uint16_t adaptation_average = 0;
+    uint16_t llc_average = 0;
+    uint16_t llc_eapol_average = 0;
+    uint16_t average_sum = 0;
     uint8_t active_max = 0;
 
     //TODO implement API for HEAP info request
@@ -3247,6 +3351,13 @@ static bool ws_bootstrap_congestion_get(protocol_interface_info_entry_t *interfa
         active_max = 50;
     }
 
+    // Read the values for adaptation and LLC queues
+    adaptation_average = random_early_detetction_aq_read(cur->random_early_detection);
+    llc_average = random_early_detetction_aq_read(cur->llc_random_early_detection);
+    llc_eapol_average  = random_early_detetction_aq_read(cur->llc_eapol_random_early_detection);
+    // Calculate combined average
+    average_sum = adaptation_average + llc_average + llc_eapol_average;
+
     // Maximum for active supplicants based on memory reached, fail
     if (active_supp >= active_max) {
         return_value = true;
@@ -3260,20 +3371,20 @@ static bool ws_bootstrap_congestion_get(protocol_interface_info_entry_t *interfa
 
     if (red_info == NULL) {
         red_info = random_early_detection_create(
-                       interface_ptr->ws_info->cfg->sec_prot.max_simult_sec_neg_tx_queue_min,
-                       interface_ptr->ws_info->cfg->sec_prot.max_simult_sec_neg_tx_queue_max,
+                       cur->ws_info->cfg->sec_prot.max_simult_sec_neg_tx_queue_min,
+                       cur->ws_info->cfg->sec_prot.max_simult_sec_neg_tx_queue_max,
                        100, RED_AVERAGE_WEIGHT_DISABLED);
     }
     if (red_info == NULL) {
         goto congestion_get_end;
     }
 
-    average = random_early_detetction_aq_read(interface_ptr->random_early_detection);
-    average = random_early_detetction_aq_calc(red_info, average);
+    // Check drop probability
+    average_sum = random_early_detetction_aq_calc(red_info, average_sum);
     return_value = random_early_detection_congestion_check(red_info);
 
 congestion_get_end:
-    tr_info("Active supplicant limit, active: %i max: %i averageQ: %i drop: %s", active_supp, active_max, average, return_value ? "T" : "F");
+    tr_info("Active supplicant limit, active: %i max: %i summed averageQ: %i adapt averageQ: %i LLC averageQ: %i LLC EAPOL averageQ: %i drop: %s", active_supp, active_max, average_sum, adaptation_average, llc_average, llc_eapol_average, return_value ? "T" : "F");
 
     return return_value;
 }
@@ -3378,6 +3489,7 @@ static void ws_bootstrap_pan_advert_solicit(protocol_interface_info_entry_t *cur
 
     async_req.security.SecurityLevel = 0;
 
+    ws_stats_update(cur, STATS_WS_ASYNCH_TX_PAS, 1);
     ws_llc_asynch_request(cur, &async_req);
 }
 
@@ -3394,6 +3506,7 @@ static void ws_bootstrap_pan_config_solicit(protocol_interface_info_entry_t *cur
     ws_set_asynch_channel_list(cur, &async_req);
     async_req.security.SecurityLevel = 0;
 
+    ws_stats_update(cur, STATS_WS_ASYNCH_TX_PCS, 1);
     ws_llc_asynch_request(cur, &async_req);
 }
 
@@ -3423,9 +3536,9 @@ static uint16_t ws_bootstrap_routing_cost_calculate(protocol_interface_info_entr
 
     uint16_t etx = etx_local_etx_read(cur->id, mac_neighbor->index);
     if (etx == 0) {
-        etx = WS_ETX_MAX; //SET maxium value here if ETX is unknown
+        etx = WS_ETX_MAX; //SET maximum value here if ETX is unknown
     } else {
-        //Scale to 128 based ETX (local read retur 0x100 - 0xffff
+        //Scale to 128 based ETX (local read return 0x100 - 0xffff
         etx = etx >> 1;
     }
     // Make the 0xffff as maximum value
@@ -3483,7 +3596,7 @@ static void ws_bootstrap_pan_advert(protocol_interface_info_entry_t *cur)
         cur->ws_info->pan_information.routing_cost = ws_bootstrap_routing_cost_calculate(cur);
     }
 
-
+    ws_stats_update(cur, STATS_WS_ASYNCH_TX_PA, 1);
     ws_llc_asynch_request(cur, &async_req);
 }
 
@@ -3512,6 +3625,7 @@ static void ws_bootstrap_pan_config(protocol_interface_info_entry_t *cur)
         async_req.security.KeyIndex = mac_helper_default_key_index_get(cur);
     }
 
+    ws_stats_update(cur, STATS_WS_ASYNCH_TX_PC, 1);
     ws_llc_asynch_request(cur, &async_req);
 }
 
@@ -3551,6 +3665,8 @@ static void ws_bootstrap_event_handler(arm_event_s *event)
             cur->ws_info->pending_key_index_info.state = NO_PENDING_PROCESS;
             cur->mac_parameters->mac_default_key_index = 0;
 
+            ipv6_destination_cache_clean(cur->id);
+
             // Clear parent blacklist
             blacklist_clear();
 
@@ -3576,9 +3692,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 {
@@ -3625,8 +3738,17 @@ static void ws_bootstrap_event_handler(arm_event_s *event)
                 // Set PAE port to 10254 and authenticator relay to 10253 (and to own ll address)
                 ws_pae_controller_authenticator_start(cur, PAE_AUTH_SOCKET_PORT, ll_addr, EAPOL_RELAY_SOCKET_PORT);
 
+                // Initialize eapol congestion tracking
+                ws_bootstrap_eapol_congestion_init(cur);
+
                 // Set retry configuration for bootstrap ready state
-                ws_bootstrap_configure_max_retries(cur, WS_MAX_FRAME_RETRIES, WS_NUMBER_OF_CHANNEL_RETRIES);
+                ws_bootstrap_configure_max_retries(cur, WS_MAX_FRAME_RETRIES);
+
+                // Set TX failure request restart configuration
+                ws_bootstrap_configure_data_request_restart(cur, WS_CCA_REQUEST_RESTART_MAX, WS_TX_REQUEST_RESTART_MAX, WS_REQUEST_RESTART_BLACKLIST_MIN, WS_REQUEST_RESTART_BLACKLIST_MAX);
+
+                // Set CSMA-CA backoff configuration
+                ws_bootstrap_configure_csma_ca_backoffs(cur, WS_MAX_CSMA_BACKOFFS, WS_MAC_MIN_BE, WS_MAC_MAX_BE);
 
                 ws_bootstrap_event_operation_start(cur);
                 break;
@@ -3638,7 +3760,11 @@ static void ws_bootstrap_event_handler(arm_event_s *event)
             ws_bootstrap_network_discovery_configure(cur);
             ws_bootstrap_fhss_activate(cur);
             // Set retry configuration for discovery state
-            ws_bootstrap_configure_max_retries(cur, WS_MAX_FRAME_RETRIES_BOOTSTRAP, WS_NUMBER_OF_CHANNEL_RETRIES_BOOTSTRAP);
+            ws_bootstrap_configure_max_retries(cur, WS_MAX_FRAME_RETRIES_BOOTSTRAP);
+            // Set TX failure request restart configuration for discovery state
+            ws_bootstrap_configure_data_request_restart(cur, WS_CCA_REQUEST_RESTART_MAX, WS_TX_REQUEST_RESTART_MAX_BOOTSTRAP, WS_REQUEST_RESTART_BLACKLIST_MIN, WS_REQUEST_RESTART_BLACKLIST_MAX);
+            // Set CSMA-CA backoff configuration
+            ws_bootstrap_configure_csma_ca_backoffs(cur, WS_MAX_CSMA_BACKOFFS, WS_MAC_MIN_BE, WS_MAC_MAX_BE);
             // Start network scan
             ws_bootstrap_start_discovery(cur);
             break;
@@ -3782,6 +3908,7 @@ void ws_bootstrap_rpl_wait_process(protocol_interface_info_entry_t *cur)
 
     if (cur->ws_info->rpl_state == RPL_EVENT_DAO_DONE) {
         // RPL routing is ready
+        cur->ws_info->connected_time = cur->ws_info->uptime;
         ws_bootstrap_event_routing_ready(cur);
     } else if (!rpl_control_have_dodag(cur->rpl_domain)) {
         // RPL not ready send DIS message if possible
@@ -3995,6 +4122,19 @@ void ws_bootstrap_seconds_timer(protocol_interface_info_entry_t *cur, uint32_t s
             cur->ws_info->ws_bsi_block.old_bsi = 0;
         }
     }
+    /*Update join state statistics*/
+    if (ws_bootstrap_state_discovery(cur)) {
+        ws_stats_update(cur, STATS_WS_STATE_1, 1);
+    } else if (ws_bootstrap_state_authenticate(cur)) {
+        ws_stats_update(cur, STATS_WS_STATE_2, 1);
+    } else if (ws_bootstrap_state_configure(cur)) {
+        ws_stats_update(cur, STATS_WS_STATE_3, 1);
+    } else if (ws_bootstrap_state_wait_rpl(cur)) {
+        ws_stats_update(cur, STATS_WS_STATE_4, 1);
+    } else if (ws_bootstrap_state_active(cur)) {
+        ws_stats_update(cur, STATS_WS_STATE_5, 1);
+    }
+    cur->ws_info->uptime++;
 
     ws_llc_timer_seconds(cur, seconds);
 
@@ -4026,7 +4166,7 @@ void ws_bootstrap_secondary_parent_update(protocol_interface_info_entry_t *inter
     }
 }
 
-int ws_bootstrap_get_info(protocol_interface_info_entry_t *cur, struct ws_stack_info *info_ptr)
+int ws_bootstrap_stack_info_get(protocol_interface_info_entry_t *cur, struct ws_stack_info *info_ptr)
 {
 
     ws_neighbor_class_entry_t *ws_neighbour = NULL;
@@ -4036,14 +4176,11 @@ int ws_bootstrap_get_info(protocol_interface_info_entry_t *cur, struct ws_stack_
 
     if (mac_parent) {
         ws_neighbour = ws_neighbor_class_entry_get(&cur->ws_info->neighbor_storage, mac_parent->index);
-
-        memcpy(info_ptr->parent, ADDR_LINK_LOCAL_PREFIX, 8);
-        memcpy(info_ptr->parent + 8, mac_parent->mac64, 8);
-        info_ptr->parent[8] ^= 2;
+        ws_bootsrap_create_ll_address(info_ptr->parent, mac_parent->mac64);
     }
     if (ws_neighbour) {
-        info_ptr->rsl_in = ws_neighbour->rsl_in;
-        info_ptr->rsl_out = ws_neighbour->rsl_out;
+        info_ptr->rsl_in = ws_neighbor_class_rsl_in_get(ws_neighbour);
+        info_ptr->rsl_out = ws_neighbor_class_rsl_out_get(ws_neighbour);
         info_ptr->routing_cost = ws_neighbour->routing_cost;
     }
     info_ptr->device_min_sens = DEVICE_MIN_SENS;
@@ -4062,6 +4199,78 @@ int ws_bootstrap_get_info(protocol_interface_info_entry_t *cur, struct ws_stack_
 
     return 0;
 }
+int ws_bootstrap_neighbor_info_get(protocol_interface_info_entry_t *cur, ws_neighbour_info_t *neighbor_ptr, uint16_t table_max)
+{
+    uint8_t count = 0;
+    if (!neighbor_ptr) {
+        // Return the aount of neighbors.
+        for (int n = 0; n < mac_neighbor_info(cur)->list_total_size; n++) {
+            mac_neighbor_table_entry_t *mac_entry = mac_neighbor_table_attribute_discover(mac_neighbor_info(cur), n);
+            if (mac_entry && mac_entry->lifetime && mac_entry->lifetime != 0xffffffff) {
+                count++;
+            }
+        }
+        return count;
+    }
+
+    if (table_max > mac_neighbor_info(cur)->list_total_size) {
+        table_max = mac_neighbor_info(cur)->list_total_size;
+    }
+
+    for (int n = 0; n < mac_neighbor_info(cur)->list_total_size; n++) {
+        if (count > table_max) {
+            break;
+        }
+
+        mac_neighbor_table_entry_t *mac_entry = mac_neighbor_table_attribute_discover(mac_neighbor_info(cur), n);
+        ws_neighbor_class_entry_t *ws_neighbor =  ws_neighbor_class_entry_get(&cur->ws_info->neighbor_storage, n);
+        if (mac_entry && ws_neighbor && mac_entry->lifetime && mac_entry->lifetime != 0xffffffff) {
+            // Active neighbor entry
+            uint8_t ll_address[16];
+            memset(neighbor_ptr + count, 0, sizeof(ws_neighbour_info_t));
+            neighbor_ptr[count].lifetime = mac_entry->lifetime;
+
+            neighbor_ptr[count].rsl_in = ws_neighbor_class_rsl_in_get(ws_neighbor);
+            neighbor_ptr[count].rsl_out = ws_neighbor_class_rsl_out_get(ws_neighbor);
+
+            // ETX is shown calculated as 8 bit integer, but more common way is to use 7 bit such that 128 means ETX:1.0
+            neighbor_ptr[count].etx = etx_local_etx_read(cur->id, mac_entry->index);
+            if (neighbor_ptr[count].etx != 0xffff) {
+                neighbor_ptr[count].etx = neighbor_ptr[count].etx >> 1;
+            }
+            ws_bootsrap_create_ll_address(ll_address, mac_entry->mac64);
+            memcpy(neighbor_ptr[count].link_local_address, ll_address, 16);
+
+            if (rpl_control_is_dodag_parent_candidate(cur, ll_address, cur->ws_info->cfg->gen.rpl_parent_candidate_max)) {
+                neighbor_ptr[count].type = WS_CANDIDATE_PARENT;
+            }
+            neighbor_ptr[count].rpl_rank = rpl_control_neighbor_info_get(cur, ll_address, neighbor_ptr[count].global_address);
+
+            if (mac_entry->link_role == PRIORITY_PARENT_NEIGHBOUR) {
+                neighbor_ptr[count].type = WS_PRIMARY_PARENT;
+            }
+            if (mac_entry->link_role == SECONDARY_PARENT_NEIGHBOUR) {
+                neighbor_ptr[count].type = WS_SECONDARY_PARENT;
+            }
+            if (mac_entry->link_role == CHILD_NEIGHBOUR) {
+                neighbor_ptr[count].type = WS_CHILD;
+            }
+
+            ipv6_neighbour_t *IPv6_neighbor = ipv6_neighbour_get_registered_by_eui64(&cur->ipv6_neighbour_cache, mac_entry->mac64);
+            if (IPv6_neighbor) {
+                //This is a child
+                neighbor_ptr[count].type = WS_CHILD;
+                memcpy(neighbor_ptr[count].global_address, IPv6_neighbor->ip_address, 16);
+                // Child lifetimes are based on Registration times not a link time
+                neighbor_ptr[count].lifetime = IPv6_neighbor->lifetime;
+            }
+            count++;
+        }
+    }
+
+    // Go through list
+    return count;
+}
 
 //Calculate max_packet queue size
 static uint16_t ws_bootstrap_define_congestin_max_threshold(uint32_t heap_total_size, uint16_t packet_size, uint16_t packet_per_seconds, uint32_t max_delay, uint16_t min_packet_queue_size, uint16_t max_packet_queue_size)
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_bootstrap.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_bootstrap.h
index 8d40c63eee..535092501d 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_bootstrap.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_bootstrap.h
@@ -36,6 +36,7 @@ struct ws_us_ie;
 struct ws_bs_ie;
 struct ws_neighbor_class_entry;
 struct ws_stack_info;
+struct ws_neighbour_info;
 
 int ws_bootstrap_init(int8_t interface_id, net_6lowpan_mode_e bootstrap_mode);
 
@@ -96,7 +97,9 @@ void ws_bootstrap_eapol_tx_temporary_clear(struct protocol_interface_info_entry
 
 void ws_bootstrap_neighbor_set_stable(struct protocol_interface_info_entry *interface, const uint8_t *src64);
 
-int ws_bootstrap_get_info(protocol_interface_info_entry_t *cur, struct ws_stack_info *info_ptr);
+int ws_bootstrap_stack_info_get(protocol_interface_info_entry_t *cur, struct ws_stack_info *info_ptr);
+
+int ws_bootstrap_neighbor_info_get(protocol_interface_info_entry_t *cur, struct ws_neighbour_info *neighbor_ptr, uint16_t table_max);
 
 void ws_bootstrap_mac_neighbor_short_time_set(struct protocol_interface_info_entry *interface, const uint8_t *src64, uint32_t valid_time);
 
@@ -110,7 +113,8 @@ void ws_bootstrap_mac_neighbor_short_time_set(struct protocol_interface_info_ent
 #define ws_bootstrap_neighbor_set_stable(interface, src64)
 #define ws_bootstrap_primary_parent_update(interface, neighbor)
 #define ws_bootstrap_secondary_parent_update(interface)
-#define ws_bootstrap_get_info(cur, info_ptr)
+#define ws_bootstrap_stack_info_get(cur, info_ptr)
+#define ws_bootstrap_neighbor_info_get(cur, neighbor_ptr, count)
 
 
 #endif //HAVE_WS
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_cfg_settings.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_cfg_settings.c
index 0c81ba7acc..32254b214b 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_cfg_settings.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_cfg_settings.c
@@ -98,10 +98,10 @@ typedef struct cfg_devices_in_config {
  *
  */
 const cfg_devices_in_config_t devices_by_datarate[] = {
-    { 1,  4, 10,  25}, // Configuration for 50 -100kbs
-    { 1,  8, 15,  25}, // Configuration for 150kbs - 200kbs
-    { 2, 15, 25,  50}, // Configuration for 300kbs
-    { 3, 20, 40, 100}, // Configuration for 600kbs - 2400kbs
+    { 1,  5, 10,  25}, // Configuration for 50 -100kbs
+    { 1, 10, 20,  40}, // Configuration for 150kbs - 200kbs
+    { 1, 15, 30,  60}, // Configuration for 300kbs
+    { 2, 20, 50, 100}, // Configuration for 600kbs - 2400kbs
 };
 
 
@@ -344,6 +344,39 @@ int8_t ws_cfg_network_size_set(protocol_interface_info_entry_t *cur, ws_gen_cfg_
     return CFG_SETTINGS_OK;
 }
 
+static uint8_t ws_cfg_config_get_by_size(protocol_interface_info_entry_t *cur, uint8_t network_size)
+{
+    (void)cur;
+
+    ws_phy_cfg_t phy_cfg;
+    if (ws_cfg_phy_get(&phy_cfg, NULL) < 0) {
+        return CONFIG_SMALL;
+    }
+    uint32_t data_rate = ws_common_datarate_get_from_phy_mode(phy_cfg.phy_mode_id, phy_cfg.operating_mode);
+
+    uint8_t index;
+    if (data_rate < 150000) {
+        index = 0;
+    } else if (data_rate < 300000) {
+        index = 1;
+    } else if (data_rate < 600000) {
+        index = 2;
+    } else {
+        index = 3;
+    }
+
+    if (network_size == NETWORK_SIZE_CERTIFICATE) {
+        return CONFIG_CERTIFICATE;
+    } else if (network_size <= devices_by_datarate[index].max_for_small) {
+        return CONFIG_SMALL;
+    } else if (network_size <= devices_by_datarate[index].max_for_medium) {
+        return CONFIG_MEDIUM;
+    } else if (network_size <= devices_by_datarate[index].max_for_large) {
+        return CONFIG_LARGE;
+    }
+    return CONFIG_XLARGE;
+}
+
 int8_t ws_cfg_network_size_configure(protocol_interface_info_entry_t *cur, uint16_t network_size)
 {
     // Read settings that are affected by network size
@@ -364,18 +397,18 @@ int8_t ws_cfg_network_size_configure(protocol_interface_info_entry_t *cur, uint1
         memcpy(nw_size_external_cfg, &new_nw_size_cfg, sizeof(ws_cfg_nw_size_t));
     }
 
-    // Small
-    if (network_size < 100) {
-        // Automatic
+    network_size = network_size / 100;
+    if (network_size == 0) {
+        network_size = 1;
+    }
+
+    if (ws_cfg_config_get_by_size(cur, network_size) == CONFIG_SMALL) {
         ws_cfg_network_size_config_set_small(&new_nw_size_cfg);
-    } else if (network_size < 800) {
-        // Medium
+    } else if (ws_cfg_config_get_by_size(cur, network_size) == CONFIG_MEDIUM) {
         ws_cfg_network_size_config_set_medium(&new_nw_size_cfg);
-    } else if (network_size < 1500) {
-        // Medium
+    } else if (ws_cfg_config_get_by_size(cur, network_size) == CONFIG_LARGE) {
         ws_cfg_network_size_config_set_large(&new_nw_size_cfg);
     } else {
-        // Large
         ws_cfg_network_size_config_set_xlarge(&new_nw_size_cfg);
     }
 
@@ -400,33 +433,8 @@ cfg_network_size_type_e ws_cfg_network_config_get(protocol_interface_info_entry_
     if (ws_cfg_gen_get(&cfg, NULL) < 0) {
         return CONFIG_SMALL;
     }
-    ws_phy_cfg_t phy_cfg;
-    if (ws_cfg_phy_get(&phy_cfg, NULL) < 0) {
-        return CONFIG_SMALL;
-    }
 
-    uint32_t data_rate = ws_get_datarate_using_operating_mode(phy_cfg.operating_mode);
-    uint8_t index;
-    if (data_rate < 150000) {
-        index = 0;
-    } else if (data_rate < 300000) {
-        index = 1;
-    } else if (data_rate < 600000) {
-        index = 2;
-    } else {
-        index = 3;
-    }
-
-    if (cfg.network_size == NETWORK_SIZE_CERTIFICATE) {
-        return CONFIG_CERTIFICATE;
-    } else if (cfg.network_size <= devices_by_datarate[index].max_for_small) {
-        return CONFIG_SMALL;
-    } else if (cfg.network_size <= devices_by_datarate[index].max_for_medium) {
-        return CONFIG_MEDIUM;
-    } else if (cfg.network_size <= devices_by_datarate[index].max_for_large) {
-        return CONFIG_LARGE;
-    }
-    return CONFIG_XLARGE;
+    return ws_cfg_config_get_by_size(cur, cfg.network_size);
 }
 
 
@@ -522,7 +530,7 @@ static void ws_cfg_network_size_config_set_large(ws_cfg_nw_size_t *cfg)
     cfg->gen.rpl_selected_parent_max = WS_RPL_SELECTED_PARENT_MAX;
 
     // Configure the Wi-SUN timing trickle parameters
-    cfg->timing.disc_trickle_imin = TRICKLE_IMIN_60_SECS << 2;       // 240 seconds
+    cfg->timing.disc_trickle_imin = TRICKLE_IMIN_60_SECS << 1;       // 120 seconds
     cfg->timing.disc_trickle_imax = 1536;      // 1536 seconds; 25 minutes
     cfg->timing.disc_trickle_k = 1;
     cfg->timing.pan_timeout = PAN_VERSION_LARGE_NETWORK_TIMEOUT;
@@ -530,7 +538,7 @@ static void ws_cfg_network_size_config_set_large(ws_cfg_nw_size_t *cfg)
     cfg->timing.temp_eapol_min_timeout = WS_EAPOL_TEMPORARY_ENTRY_LARGE_TIMEOUT;
 
     // RPL configuration
-    cfg->bbr.dio_interval_min = WS_RPL_DIO_IMIN_LARGE;               // 18; 262s, 4.5min
+    cfg->bbr.dio_interval_min = WS_RPL_DIO_IMIN_LARGE;               // 18; 256s, 4.5min
     cfg->bbr.dio_interval_doublings = WS_RPL_DIO_DOUBLING_LARGE;     // 3; 2048s, 34min
     cfg->bbr.dio_redundancy_constant = WS_RPL_DIO_REDUNDANCY_LARGE;  // 10
     cfg->bbr.dag_max_rank_increase = WS_RPL_MAX_HOP_RANK_INCREASE;
@@ -1441,7 +1449,7 @@ uint32_t ws_cfg_neighbour_temporary_lifetime_get(void)
 }
 void ws_cfg_neighbour_temporary_lifetime_set(uint32_t lifetime)
 {
-    if (lifetime >= WS_NEIGHBOUR_TEMPORARY_NEIGH_MAX_LIFETIME || lifetime == 0) {
+    if (lifetime > WS_NEIGHBOUR_TEMPORARY_NEIGH_MAX_LIFETIME || lifetime == 0) {
         if (lifetime > WS_NEIGHBOR_LINK_TIMEOUT) {
             lifetime = WS_NEIGHBOR_LINK_TIMEOUT;
         }
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common.c
index a11af08d18..4a77f37bab 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common.c
@@ -34,6 +34,7 @@
 #include "6LoWPAN/ws/ws_bbr_api_internal.h"
 #include "6LoWPAN/ws/ws_pae_controller.h"
 #include "6LoWPAN/ws/ws_cfg_settings.h"
+#include "6LoWPAN/ws/ws_stats.h"
 #include "Service_Libs/etx/etx.h"
 #include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 #include "Service_Libs/blacklist/blacklist.h"
@@ -223,7 +224,7 @@ phy_modulation_index_e ws_get_modulation_index_using_operating_mode(uint8_t oper
 int8_t ws_common_regulatory_domain_config(protocol_interface_info_entry_t *cur, ws_hopping_schedule_t *hopping_schdule)
 {
     (void)cur;
-    if (ws_get_datarate_using_operating_mode(hopping_schdule->operating_mode) == 0) {
+    if (ws_common_datarate_get_from_phy_mode(hopping_schdule->phy_mode_id, hopping_schdule->operating_mode) == 0) {
         //Unsupported operation mode
         return -1;
     }
@@ -629,6 +630,7 @@ uint8_t ws_common_allow_child_registration(protocol_interface_info_entry_t *inte
     ws_common_neighbour_address_reg_link_update(interface, eui64, link_lifetime);
     tr_info("Child registration allowed %d/%d, max:%d", child_count, max_child_count, mac_neighbor_info(interface)->list_total_size);
 
+    ws_stats_update(interface, STATS_WS_CHILD_ADD, 1);
     return ARO_SUCCESS;
 }
 
@@ -639,9 +641,7 @@ bool ws_common_negative_aro_mark(protocol_interface_info_entry_t *interface, con
         return false;
     }
 
-    ws_neighbor_class_entry_t *ws_neighbor = ws_neighbor_class_entry_get(&interface->ws_info->neighbor_storage, neighbour->index);
-    ws_neighbor->negative_aro_send = true;
-    neighbour->lifetime = WS_NEIGHBOR_TEMPORARY_LINK_MIN_TIMEOUT_SMALL; //Remove anyway if Packet is freed before MAC push
+    ws_bootstrap_mac_neighbor_short_time_set(interface, eui64, WS_NEIGHBOUR_TEMPORARY_NEIGH_MAX_LIFETIME);
     return true;
 }
 
@@ -663,11 +663,37 @@ uint32_t ws_common_latency_estimate_get(protocol_interface_info_entry_t *cur)
     return latency;
 }
 
+uint32_t ws_common_datarate_get_from_phy_mode(uint8_t phy_mode_id, uint8_t operating_mode)
+{
+    if (((phy_mode_id >= 34) && (phy_mode_id <= 38)) ||
+            ((phy_mode_id >= 51) && (phy_mode_id <= 54)) ||
+            ((phy_mode_id >= 68) && (phy_mode_id <= 70)) ||
+            ((phy_mode_id >= 84) && (phy_mode_id <= 86))) {
+        return ws_get_datarate_using_phy_mode_id(phy_mode_id);
+    }
+    return ws_get_datarate_using_operating_mode(operating_mode);
+}
+
 uint32_t ws_common_datarate_get(protocol_interface_info_entry_t *cur)
 {
-    return ws_get_datarate_using_operating_mode(cur->ws_info->hopping_schdule.operating_mode);
+    return ws_common_datarate_get_from_phy_mode(cur->ws_info->hopping_schdule.phy_mode_id, cur->ws_info->hopping_schdule.operating_mode);
 }
 
+uint32_t ws_common_usable_application_datarate_get(protocol_interface_info_entry_t *cur)
+{
+    /* Usable data rate is a available data rate when removed ACK and wait times required to send a packet
+     *
+     * Estimated to be around 70% with following assumptions with 150kbs data rate
+     * Average ACK size 48 bytes
+     * Average tACK 2ms
+     * Average CCA check time + processing 7ms
+     * Delays in bytes with 150kbs data rate 168 + 48 bytes for ACK 216 bytes
+     * Usable data rate is 1 - 216/(216 + 500) about 70%
+     */
+    return 70 * ws_common_datarate_get_from_phy_mode(cur->ws_info->hopping_schdule.phy_mode_id, cur->ws_info->hopping_schdule.operating_mode) / 100;
+}
+
+
 uint32_t ws_common_network_size_estimate_get(protocol_interface_info_entry_t *cur)
 {
     uint32_t network_size_estimate = 100;
@@ -680,6 +706,30 @@ uint32_t ws_common_network_size_estimate_get(protocol_interface_info_entry_t *cu
     return network_size_estimate;
 }
 
+uint32_t ws_common_connected_time_get(protocol_interface_info_entry_t *cur)
+{
+    if (!ws_info(cur)) {
+        return 0;
+    }
+    if (cur->ws_info->connected_time == 0) {
+        // We are not connected
+        return 0;
+    }
+    return cur->ws_info->uptime - cur->ws_info->connected_time;
+}
+
+uint32_t ws_common_authentication_time_get(protocol_interface_info_entry_t *cur)
+{
+    if (!ws_info(cur)) {
+        return 0;
+    }
+    if (cur->ws_info->authentication_time == 0) {
+        // Authentication was not done when joined to network so time is not known
+        return 0;
+    }
+    return cur->ws_info->uptime - cur->ws_info->authentication_time;
+}
+
 void ws_common_primary_parent_update(protocol_interface_info_entry_t *interface, mac_neighbor_table_entry_t *neighbor)
 {
     ws_bootstrap_primary_parent_update(interface, neighbor);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common.h
index 2761164482..2dda3beac8 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common.h
@@ -97,7 +97,10 @@ typedef struct ws_info_s {
     ws_bsi_block_t ws_bsi_block;
     uint16_t aro_registration_timer;       /**< Aro registration timer */
     uint16_t rpl_version_timer;            /**< RPL version update timeout */
-    uint32_t pan_timeout_timer;    /**< routers will fallback to previous state after this */
+    uint32_t pan_timeout_timer;            /**< routers will fallback to previous state after this */
+    uint32_t uptime;                       /**< Seconds after interface has been started */
+    uint32_t authentication_time;          /**< When the last authentication was performed */
+    uint32_t connected_time;               /**< Time we have been connected to network */
     uint32_t pan_config_sol_max_timeout;
     uint8_t gtkhash[32];
     uint16_t network_pan_id;
@@ -164,10 +167,18 @@ uint32_t ws_common_version_timeout_get(uint8_t config);
 
 uint32_t ws_common_latency_estimate_get(protocol_interface_info_entry_t *cur);
 
+uint32_t ws_common_datarate_get_from_phy_mode(uint8_t phy_mode_id, uint8_t operating_mode);
+
 uint32_t ws_common_datarate_get(protocol_interface_info_entry_t *cur);
 
+uint32_t ws_common_usable_application_datarate_get(protocol_interface_info_entry_t *cur);
+
 uint32_t ws_common_network_size_estimate_get(protocol_interface_info_entry_t *cur);
 
+uint32_t ws_common_connected_time_get(protocol_interface_info_entry_t *cur);
+
+uint32_t ws_common_authentication_time_get(protocol_interface_info_entry_t *cur);
+
 void ws_common_primary_parent_update(protocol_interface_info_entry_t *interface, mac_neighbor_table_entry_t *neighbor);
 
 void ws_common_secondary_parent_update(protocol_interface_info_entry_t *interface);
@@ -187,8 +198,12 @@ void ws_common_border_router_alive_update(protocol_interface_info_entry_t *inter
 #define ws_common_allow_child_registration(cur, eui64, aro_timeout) (2)
 #define ws_common_negative_aro_mark(interface, eui64)(false)
 #define ws_common_latency_estimate_get(cur) 0
+#define ws_common_datarate_get_from_phy_mode(phy_mode_id, operating_mode) 0
 #define ws_common_datarate_get(cur) 0
+#define ws_common_usable_application_datarate_get(cur) 0
 #define ws_common_network_size_estimate_get(cur) 0
+#define ws_common_connected_time_get(cur) 0
+#define ws_common_authentication_time_get(cur) 0
 #define ws_common_primary_parent_update(interface, neighbor)
 #define ws_common_secondary_parent_update(interface)
 #define ws_common_border_router_alive_update(interface) ((void) 0)
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common_defines.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common_defines.h
index cb400ba302..1657d9ee6b 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common_defines.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common_defines.h
@@ -326,10 +326,10 @@ typedef struct ws_bs_ie {
 
 
 /* WS requires at least 19 MAC retransmissions (total 1+19=20 attempts). Default 802.15.4 macMaxFrameRetries is 3 (total 1+3=4 attempts).
- * At least 4 channel retries must be used: (Initial channel + WS_NUMBER_OF_CHANNEL_RETRIES) * MAC attempts = (1+4)*4=20 attempts
+ * At least 4 request restarts must be used: (Initial channel + WS_TX_REQUEST_RESTART_MAX) * MAC attempts = (1+4)*4=20 attempts
  *
  * Valid settings could be for example:
- * WS_MAX_FRAME_RETRIES     WS_NUMBER_OF_CHANNEL_RETRIES    Total attempts
+ * WS_MAX_FRAME_RETRIES     WS_TX_REQUEST_RESTART_MAX       Total attempts
  * 0                        19                              1+0*1+19=20
  * 1                        9                               1+1*1+9=20
  * 2                        6                               1+2*1+6=21
@@ -338,16 +338,28 @@ typedef struct ws_bs_ie {
  */
 // This configuration is used when bootstrap is ready
 #define WS_MAX_FRAME_RETRIES            3
-#define WS_NUMBER_OF_CHANNEL_RETRIES    4
 // This configuration is used during bootstrap
-#define WS_MAX_FRAME_RETRIES_BOOTSTRAP          0
-#define WS_NUMBER_OF_CHANNEL_RETRIES_BOOTSTRAP  19
+#define WS_MAX_FRAME_RETRIES_BOOTSTRAP  0
 
+// Configuring data request restart allows MAC to push failed packet back to MAC TX queue up to WS_CCA_REQUEST_RESTART_MAX times for CCA failure and WS_TX_REQUEST_RESTART_MAX for TX failure.
+// Packet cannot be taken back to transmission before it has finished the blacklist period.
+#define WS_CCA_REQUEST_RESTART_MAX          9
+#define WS_TX_REQUEST_RESTART_MAX           4
+#define WS_TX_REQUEST_RESTART_MAX_BOOTSTRAP 19
+#define WS_REQUEST_RESTART_BLACKLIST_MIN    20
+#define WS_REQUEST_RESTART_BLACKLIST_MAX    300
 
-#if (1 + WS_MAX_FRAME_RETRIES) * (1 + WS_NUMBER_OF_CHANNEL_RETRIES) < 20
+#if (1 + WS_MAX_FRAME_RETRIES) * (1 + WS_TX_REQUEST_RESTART_MAX) < 20
 #warning "MAX frame retries set too low"
 #endif
 
+// Total CCA attempts: 1 + WS_MAX_CSMA_BACKOFFS
+#define WS_MAX_CSMA_BACKOFFS    0
+
+// Use minimum possible CSMA-CA backoffs
+#define WS_MAC_MIN_BE   1
+#define WS_MAC_MAX_BE   1
+
 /*
  * Automatic CCA threshold: default threshold and range in dBm.
  */
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_config.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_config.h
index ce1e1217b0..fb2e23ca27 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_config.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_config.h
@@ -95,9 +95,9 @@
  *
  */
 
-#define PAN_VERSION_SMALL_NETWORK_TIMEOUT 32*60
+#define PAN_VERSION_SMALL_NETWORK_TIMEOUT 30*60
 
-#define PAN_VERSION_MEDIUM_NETWORK_TIMEOUT 64*60
+#define PAN_VERSION_MEDIUM_NETWORK_TIMEOUT 60*60
 
 #define PAN_VERSION_LARGE_NETWORK_TIMEOUT 90*60
 
@@ -290,7 +290,7 @@ extern uint8_t DEVICE_MIN_SENS;
 // Large network trickle values for sending of initial EAPOL-key
 #define LARGE_NW_INITIAL_KEY_TRICKLE_IMIN_SECS         600   /* 10 to 20 minutes */
 #define LARGE_NW_INITIAL_KEY_TRICKLE_IMAX_SECS         1200
-#define LARGE_NW_INITIAL_KEY_RETRY_COUNT               4
+#define LARGE_NW_INITIAL_KEY_RETRY_COUNT               3
 
 // Very slow network values for sending of initial EAPOL-key
 #define EXTRA_LARGE_NW_INITIAL_KEY_TRICKLE_IMIN_SECS   600   /* 10 to 20 minutes */
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_eapol_pdu.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_eapol_pdu.c
index b32892eaed..e5c2f64c84 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_eapol_pdu.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_eapol_pdu.c
@@ -214,7 +214,19 @@ int8_t ws_eapol_pdu_send_to_mpx(protocol_interface_info_entry_t *interface_ptr,
 
     eapol_pdu_data->msdu_handle++;
 
-    eapol_pdu_data->mpx_api->mpx_data_request(eapol_pdu_data->mpx_api, &data_request, eapol_pdu_data->mpx_user_id);
+    eapol_pdu_data->mpx_api->mpx_data_request(eapol_pdu_data->mpx_api, &data_request, eapol_pdu_data->mpx_user_id, MAC_DATA_NORMAL_PRIORITY);
+    return 0;
+}
+
+int8_t ws_eapol_pdu_mpx_eui64_purge(protocol_interface_info_entry_t *interface_ptr, const uint8_t *eui_64)
+{
+    eapol_pdu_data_t *eapol_pdu_data = ws_eapol_pdu_data_get(interface_ptr);
+    if (!eapol_pdu_data) {
+        return -1;
+    }
+
+    eapol_pdu_data->mpx_api->mpx_eui64_purge(eapol_pdu_data->mpx_api, eui_64);
+
     return 0;
 }
 
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_eapol_pdu.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_eapol_pdu.h
index c385e9313c..5c5320c0a8 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_eapol_pdu.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_eapol_pdu.h
@@ -161,4 +161,16 @@ typedef int8_t ws_eapol_pdu_tx_status(protocol_interface_info_entry_t *interface
  */
 int8_t ws_eapol_pdu_send_to_mpx(protocol_interface_info_entry_t *interface_ptr, const uint8_t *eui_64, void *data, uint16_t size, void *buffer, ws_eapol_pdu_tx_status tx_status, uint8_t tx_identifier);
 
+/**
+ *  ws_eapol_pdu_mpx_eui64_purge purge EUI-64 from MPX
+ *
+ * \param interface_ptr interface
+ * \param eui_64 EUI-64 to purge
+ *
+ * \return < 0 failure
+ * \return >= 0 success
+ *
+ */
+int8_t ws_eapol_pdu_mpx_eui64_purge(protocol_interface_info_entry_t *interface_ptr, const uint8_t *eui_64);
+
 #endif /* WS_EAPOL_PDU_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_eapol_relay.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_eapol_relay.c
index 41e01be8f1..2c2a6a3b93 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_eapol_relay.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_eapol_relay.c
@@ -189,6 +189,13 @@ static void ws_eapol_relay_socket_cb(void *cb)
         return;
     }
 
+    // EAPOL PDU data length is zero (message contains only supplicant EUI-64 and KMP ID)
+    if (cb_data->d_len == 9) {
+        ws_eapol_pdu_mpx_eui64_purge(eapol_relay->interface_ptr, socket_pdu);
+        ns_dyn_mem_free(socket_pdu);
+        return;
+    }
+
     //First 8 byte is EUID64 and rsr payload
     if (ws_eapol_pdu_send_to_mpx(eapol_relay->interface_ptr, socket_pdu, socket_pdu + 8, cb_data->d_len - 8, socket_pdu, NULL, 0) < 0) {
         ns_dyn_mem_free(socket_pdu);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_empty_functions.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_empty_functions.c
index c70f2e36c5..3f6279685d 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_empty_functions.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_empty_functions.c
@@ -452,6 +452,17 @@ int ws_stack_info_get(int8_t interface_id, ws_stack_info_t *info_ptr)
     return -1;
 }
 
+int ws_neighbor_info_get(
+    int8_t interface_id,
+    ws_neighbour_info_t *neighbor_ptr,
+    uint16_t count)
+{
+    (void) interface_id;
+    (void) neighbor_ptr;
+    (void) count;
+    return -1;
+}
+
 int ws_device_min_sens_set(
     int8_t interface_id,
     uint8_t device_min_sens)
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_llc_data_service.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_llc_data_service.c
index 1f6f15abad..c83f9b89b1 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_llc_data_service.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_llc_data_service.c
@@ -41,6 +41,7 @@
 #include "Security/eapol/eapol_helper.h"
 #include "Service_Libs/etx/etx.h"
 #include "fhss_ws_extension.h"
+#include "Service_Libs/random_early_detection/random_early_detection_api.h"
 
 #ifdef HAVE_WS
 
@@ -90,6 +91,7 @@ typedef struct {
     uint8_t         mpx_user_handle;    /**< This MPX user defined handle */
     ns_ie_iovec_t   ie_vector_list[3];  /**< IE vectors: 1 for Header's, 1 for Payload and for MPX payload */
     mcps_data_req_ie_list_t ie_ext;
+    mac_data_priority_t priority;
     ns_list_link_t  link;               /**< List link entry */
     uint8_t         ie_buffer[];        /**< Trailing buffer data */
 } llc_message_t;
@@ -109,6 +111,7 @@ typedef struct {
     ws_neighbor_temp_list_t         active_eapol_temp_neigh;
     ws_neighbor_temp_list_t         free_temp_neigh;
     llc_message_list_t              llc_eap_pending_list;           /**< Active Message list */
+    uint16_t                        llc_eap_pending_list_size;      /**< EAPOL active Message list size */
     bool                            active_eapol_session: 1;        /**< Indicating active EAPOL message */
 } temp_entriest_t;
 
@@ -117,6 +120,8 @@ typedef struct {
 #define ENHANCED_FRAME_RESPONSE (WH_IE_ELEMENT_HEADER_LENGTH + 2 + WH_IE_ELEMENT_HEADER_LENGTH + 4 + WH_IE_ELEMENT_HEADER_LENGTH + 1 + WH_IE_ELEMENT_HEADER_LENGTH + 5)
 
 typedef struct {
+    ns_list_link_t                  link;                           /**< List link entry */
+
     uint8_t                         mac_handle_base;                /**< Mac handle id base this will be updated by 1 after use */
     uint8_t                         llc_message_list_size;          /**< llc_message_list list size */
     uint16_t                        edfe_rx_wait_timer;
@@ -131,9 +136,8 @@ typedef struct {
     ws_neighbor_info_request        *ws_neighbor_info_request_cb;   /**< LLC Neighbour discover API*/
     uint8_t                         ws_enhanced_response_elements[ENHANCED_FRAME_RESPONSE];
     ns_ie_iovec_t                   ws_header_vector;
+    bool                            high_priority_mode;
     protocol_interface_info_entry_t *interface_ptr;                 /**< List link entry */
-
-    ns_list_link_t                  link;                           /**< List link entry */
 } llc_data_base_t;
 
 static NS_LIST_DEFINE(llc_data_base_list, llc_data_base_t, link);
@@ -159,7 +163,7 @@ static llc_data_base_t *ws_llc_base_allocate(void);
 static void ws_llc_mac_confirm_cb(const mac_api_t *api, const mcps_data_conf_t *data, const mcps_data_conf_payload_t *conf_data);
 static void ws_llc_mac_indication_cb(const mac_api_t *api, const mcps_data_ind_t *data, const mcps_data_ie_list_t *ie_ext);
 static uint16_t ws_mpx_header_size_get(llc_data_base_t *base, uint16_t user_id);
-static void ws_llc_mpx_data_request(const mpx_api_t *api, const struct mcps_data_req_s *data, uint16_t user_id);
+static void ws_llc_mpx_data_request(const mpx_api_t *api, const struct mcps_data_req_s *data, uint16_t user_id, mac_data_priority_t priority);
 static int8_t ws_llc_mpx_data_cb_register(const mpx_api_t *api, mpx_data_confirm *confirm_cb, mpx_data_indication *indication_cb, uint16_t user_id);
 static uint16_t ws_llc_mpx_header_size_get(const mpx_api_t *api, uint16_t user_id);
 static uint8_t ws_llc_mpx_data_purge_request(const mpx_api_t *api, struct mcps_purge_s *purge, uint16_t user_id);
@@ -245,6 +249,7 @@ static void llc_message_free(llc_message_t *message, llc_data_base_t *llc_base)
     ns_list_remove(&llc_base->llc_message_list, message);
     ns_dyn_mem_free(message);
     llc_base->llc_message_list_size--;
+    random_early_detetction_aq_calc(llc_base->interface_ptr->llc_random_early_detection, llc_base->llc_message_list_size);
 }
 
 static void llc_message_id_allocate(llc_message_t *message, llc_data_base_t *llc_base, bool mpx_user)
@@ -286,6 +291,7 @@ static llc_message_t *llc_message_allocate(uint16_t ie_buffer_size, llc_data_bas
     }
     message->ack_requested = false;
     message->eapol_temporary = false;
+    message->priority = MAC_DATA_NORMAL_PRIORITY;
     return message;
 }
 
@@ -534,10 +540,12 @@ static void ws_llc_mac_confirm_cb(const mac_api_t *api, const mcps_data_conf_t *
             if (message) {
                 //Start A pending EAPOL
                 ns_list_remove(&base->temp_entries->llc_eap_pending_list, message);
+                base->temp_entries->llc_eap_pending_list_size--;
+                random_early_detetction_aq_calc(base->interface_ptr->llc_eapol_random_early_detection, base->temp_entries->llc_eap_pending_list_size);
                 ws_llc_mpx_eapol_send(base, message);
             }
         } else {
-            if (neighbor_info.ws_neighbor && neighbor_info.neighbor && neighbor_info.neighbor->link_lifetime <= WS_NEIGHBOUR_DHCP_ENTRY_LIFETIME) {
+            if (neighbor_info.ws_neighbor && neighbor_info.neighbor && neighbor_info.neighbor->link_lifetime <= WS_NEIGHBOUR_TEMPORARY_NEIGH_MAX_LIFETIME) {
                 //Remove temp neighbour
                 tr_debug("Remove Temp Entry by TX confirm");
                 mac_neighbor_table_neighbor_remove(mac_neighbor_info(interface), neighbor_info.neighbor);
@@ -969,7 +977,7 @@ static void ws_llc_lowpan_mpx_header_set(llc_message_t *message, uint16_t user_i
     message->ie_vector_list[1].iovLen = ptr - (uint8_t *)message->ie_vector_list[1].ieBase;
 }
 
-static void ws_llc_lowpan_mpx_data_request(llc_data_base_t *base, mpx_user_t *user_cb, const struct mcps_data_req_s *data)
+static void ws_llc_lowpan_mpx_data_request(llc_data_base_t *base, mpx_user_t *user_cb, const struct mcps_data_req_s *data, mac_data_priority_t priority)
 {
     wh_ie_sub_list_t ie_header_mask;
     memset(&ie_header_mask, 0, sizeof(wh_ie_sub_list_t));
@@ -1020,6 +1028,8 @@ static void ws_llc_lowpan_mpx_data_request(llc_data_base_t *base, mpx_user_t *us
     //Add To active list
     llc_message_id_allocate(message, base, true);
     base->llc_message_list_size++;
+    message->priority = priority;
+    random_early_detetction_aq_calc(base->interface_ptr->llc_random_early_detection, base->llc_message_list_size);
     ns_list_add_to_end(&base->llc_message_list, message);
 
     mcps_data_req_t data_req;
@@ -1097,7 +1107,7 @@ static void ws_llc_lowpan_mpx_data_request(llc_data_base_t *base, mpx_user_t *us
         message->ie_ext.payloadIovLength = 0; //Set Back 2 at response handler
     }
 
-    base->interface_ptr->mac_api->mcps_data_req_ext(base->interface_ptr->mac_api, &data_req, &message->ie_ext, NULL);
+    base->interface_ptr->mac_api->mcps_data_req_ext(base->interface_ptr->mac_api, &data_req, &message->ie_ext, NULL, message->priority);
 }
 
 static bool ws_llc_eapol_temp_entry_set(llc_data_base_t *base, const uint8_t *mac64)
@@ -1147,15 +1157,16 @@ static void ws_llc_mpx_eapol_send(llc_data_base_t *base, llc_message_t *message)
     mcps_data_req_t data_req;
     llc_message_id_allocate(message, base, true);
     base->llc_message_list_size++;
+    random_early_detetction_aq_calc(base->interface_ptr->llc_random_early_detection, base->llc_message_list_size);
     ns_list_add_to_end(&base->llc_message_list, message);
     message->eapol_temporary = ws_llc_eapol_temp_entry_set(base, message->dst_address);
     ws_llc_eapol_data_req_init(&data_req, message);
     base->temp_entries->active_eapol_session = true;
-    base->interface_ptr->mac_api->mcps_data_req_ext(base->interface_ptr->mac_api, &data_req, &message->ie_ext, NULL);
+    base->interface_ptr->mac_api->mcps_data_req_ext(base->interface_ptr->mac_api, &data_req, &message->ie_ext, NULL, message->priority);
 }
 
 
-static void ws_llc_mpx_eapol_request(llc_data_base_t *base, mpx_user_t *user_cb, const struct mcps_data_req_s *data)
+static void ws_llc_mpx_eapol_request(llc_data_base_t *base, mpx_user_t *user_cb, const struct mcps_data_req_s *data, mac_data_priority_t priority)
 {
     wh_ie_sub_list_t ie_header_mask;
     memset(&ie_header_mask, 0, sizeof(wh_ie_sub_list_t));
@@ -1190,6 +1201,7 @@ static void ws_llc_mpx_eapol_request(llc_data_base_t *base, mpx_user_t *user_cb,
         user_cb->data_confirm(&base->mpx_data_base.mpx_api, &data_conf);
         return;
     }
+    message->priority = priority;
     message->mpx_user_handle = data->msduHandle;
     message->ack_requested = data->TxAckReq;
 
@@ -1240,6 +1252,8 @@ static void ws_llc_mpx_eapol_request(llc_data_base_t *base, mpx_user_t *user_cb,
     if (base->temp_entries->active_eapol_session) {
         //Move to pending list
         ns_list_add_to_end(&base->temp_entries->llc_eap_pending_list, message);
+        base->temp_entries->llc_eap_pending_list_size++;
+        random_early_detetction_aq_calc(base->interface_ptr->llc_eapol_random_early_detection, base->temp_entries->llc_eap_pending_list_size);
     } else {
         ws_llc_mpx_eapol_send(base, message);
     }
@@ -1247,7 +1261,7 @@ static void ws_llc_mpx_eapol_request(llc_data_base_t *base, mpx_user_t *user_cb,
 }
 
 
-static void ws_llc_mpx_data_request(const mpx_api_t *api, const struct mcps_data_req_s *data, uint16_t user_id)
+static void ws_llc_mpx_data_request(const mpx_api_t *api, const struct mcps_data_req_s *data, uint16_t user_id, mac_data_priority_t priority)
 {
     llc_data_base_t *base = ws_llc_discover_by_mpx(api);
     if (!base) {
@@ -1270,12 +1284,21 @@ static void ws_llc_mpx_data_request(const mpx_api_t *api, const struct mcps_data
     }
 
     if (user_id == MPX_KEY_MANAGEMENT_ENC_USER_ID) {
-        ws_llc_mpx_eapol_request(base, user_cb, data);
+        ws_llc_mpx_eapol_request(base, user_cb, data, priority);
     } else if (user_id == MPX_LOWPAN_ENC_USER_ID) {
-        ws_llc_lowpan_mpx_data_request(base, user_cb, data);
+        ws_llc_lowpan_mpx_data_request(base, user_cb, data, priority);
     }
 }
 
+static void ws_llc_mpx_eui64_purge_request(const mpx_api_t *api, const uint8_t *eui64)
+{
+    llc_data_base_t *base = ws_llc_discover_by_mpx(api);
+    if (!base) {
+        return;
+    }
+    tr_info("LLC purge EAPOL temporary entry: %s", tr_array(eui64, 8));
+    ws_llc_release_eapol_temp_entry(base->temp_entries, eui64);
+}
 
 static int8_t ws_llc_mpx_data_cb_register(const mpx_api_t *api, mpx_data_confirm *confirm_cb, mpx_data_indication *indication_cb, uint16_t user_id)
 {
@@ -1325,6 +1348,15 @@ static uint8_t ws_llc_mpx_data_purge_request(const mpx_api_t *api, struct mcps_p
     return purge_status;
 }
 
+static void wc_llc_mpx_priority_set_request(const mpx_api_t *api, bool enable_mode)
+{
+    llc_data_base_t *base = ws_llc_discover_by_mpx(api);
+    if (!base) {
+        return;
+    }
+    base->high_priority_mode = enable_mode;
+}
+
 static void ws_llc_mpx_init(mpx_class_t *mpx_class)
 {
     //Init Mbed Class and API
@@ -1334,6 +1366,8 @@ static void ws_llc_mpx_init(mpx_class_t *mpx_class)
     mpx_class->mpx_api.mpx_user_registration = &ws_llc_mpx_data_cb_register;
     mpx_class->mpx_api.mpx_data_request = &ws_llc_mpx_data_request;
     mpx_class->mpx_api.mpx_data_purge = &ws_llc_mpx_data_purge_request;
+    mpx_class->mpx_api.mpx_eui64_purge = &ws_llc_mpx_eui64_purge_request;
+    mpx_class->mpx_api.mpx_priority_mode_set = &wc_llc_mpx_priority_set_request;
 }
 
 static void ws_llc_clean(llc_data_base_t *base)
@@ -1351,10 +1385,13 @@ static void ws_llc_clean(llc_data_base_t *base)
         ns_list_remove(&base->temp_entries->llc_eap_pending_list, message);
         ns_dyn_mem_free(message);
     }
+    base->temp_entries->llc_eap_pending_list_size = 0;
     base->temp_entries->active_eapol_session = false;
     memset(&base->ie_params, 0, sizeof(llc_ie_params_t));
 
     ws_llc_temp_neigh_info_table_reset(base->temp_entries);
+    //Disable High Priority mode
+    base->high_priority_mode = false;
 }
 
 static void ws_llc_temp_neigh_info_table_reset(temp_entriest_t *base)
@@ -1657,6 +1694,11 @@ int8_t ws_llc_asynch_request(struct protocol_interface_info_entry *interface, as
         return -1;
     }
 
+    if (base->high_priority_mode) {
+        //Drop asynch messages at High Priority mode
+        return -1;
+    }
+
 
     //Calculate IE Buffer size
     request->wh_requested_ie_list.fc_ie = false; //Never should not be a part Asynch message
@@ -1683,6 +1725,7 @@ int8_t ws_llc_asynch_request(struct protocol_interface_info_entry *interface, as
     //Add To active list
     llc_message_id_allocate(message, base, false);
     base->llc_message_list_size++;
+    random_early_detetction_aq_calc(base->interface_ptr->llc_random_early_detection, base->llc_message_list_size);
     ns_list_add_to_end(&base->llc_message_list, message);
     message->messsage_type = request->message_type;
 
@@ -1761,7 +1804,7 @@ int8_t ws_llc_asynch_request(struct protocol_interface_info_entry *interface, as
         }
     }
 
-    base->interface_ptr->mac_api->mcps_data_req_ext(base->interface_ptr->mac_api, &data_req, &message->ie_ext, &request->channel_list);
+    base->interface_ptr->mac_api->mcps_data_req_ext(base->interface_ptr->mac_api, &data_req, &message->ie_ext, &request->channel_list, message->priority);
 
     return 0;
 }
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_management_api.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_management_api.c
index b7ff5e7a8d..d1bb845d8e 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_management_api.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_management_api.c
@@ -927,7 +927,20 @@ int ws_stack_info_get(int8_t interface_id, ws_stack_info_t *info_ptr)
     if (!cur || !ws_info(cur) || !info_ptr) {
         return -1;
     }
-    return ws_bootstrap_get_info(cur, info_ptr);
+    return ws_bootstrap_stack_info_get(cur, info_ptr);
+}
+
+int ws_neighbor_info_get(
+    int8_t interface_id,
+    ws_neighbour_info_t *neighbor_ptr,
+    uint16_t count)
+{
+    protocol_interface_info_entry_t *cur;
+    cur = protocol_stack_interface_info_get_by_id(interface_id);
+    if (!cur || !ws_info(cur)) {
+        return -1;
+    }
+    return ws_bootstrap_neighbor_info_get(cur, neighbor_ptr, count);
 }
 
 int ws_device_min_sens_set(
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_neighbor_class.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_neighbor_class.h
index aa63fa181f..516196abdf 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_neighbor_class.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_neighbor_class.h
@@ -33,7 +33,6 @@ typedef struct ws_neighbor_class_entry {
     bool broadcast_timing_info_stored: 1;
     bool broadcast_shedule_info_stored: 1;
     bool synch_done : 1;
-    bool negative_aro_send : 1;
     bool unicast_data_rx : 1;
 } ws_neighbor_class_entry_t;
 
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_auth.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_auth.c
index 4e6db848fe..452f3c5015 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_auth.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_auth.c
@@ -133,8 +133,8 @@ static bool ws_pae_auth_active_limit_reached(uint16_t active_supp, pae_auth_t *p
 static kmp_api_t *ws_pae_auth_kmp_incoming_ind(kmp_service_t *service, uint8_t msg_if_instance_id, kmp_type_e type, const kmp_addr_t *addr, const void *pdu, uint16_t size);
 static void ws_pae_auth_kmp_api_create_confirm(kmp_api_t *kmp, kmp_result_e result);
 static void ws_pae_auth_kmp_api_create_indication(kmp_api_t *kmp, kmp_type_e type, kmp_addr_t *addr);
-static void ws_pae_auth_kmp_api_finished_indication(kmp_api_t *kmp, kmp_result_e result, kmp_sec_keys_t *sec_keys);
-static void ws_pae_auth_next_kmp_trigger(pae_auth_t *pae_auth, supp_entry_t *supp_entry);
+static bool ws_pae_auth_kmp_api_finished_indication(kmp_api_t *kmp, kmp_result_e result, kmp_sec_keys_t *sec_keys);
+static bool ws_pae_auth_next_kmp_trigger(pae_auth_t *pae_auth, supp_entry_t *supp_entry);
 static kmp_type_e ws_pae_auth_next_protocol_get(pae_auth_t *pae_auth, supp_entry_t *supp_entry);
 static kmp_api_t *ws_pae_auth_kmp_create_and_start(kmp_service_t *service, kmp_type_e type, uint8_t socked_msg_if_instance_id, supp_entry_t *supp_entry, sec_cfg_t *sec_cfg);
 static void ws_pae_auth_kmp_api_finished(kmp_api_t *kmp);
@@ -1116,36 +1116,36 @@ static void ws_pae_auth_kmp_api_create_indication(kmp_api_t *kmp, kmp_type_e typ
     kmp_api_create_response(kmp, KMP_RESULT_OK);
 }
 
-static void ws_pae_auth_kmp_api_finished_indication(kmp_api_t *kmp, kmp_result_e result, kmp_sec_keys_t *sec_keys)
+static bool ws_pae_auth_kmp_api_finished_indication(kmp_api_t *kmp, kmp_result_e result, kmp_sec_keys_t *sec_keys)
 {
     (void) sec_keys;
 
     // For now, just ignore if not ok
     if (result != KMP_RESULT_OK) {
-        return;
+        return false;
     }
 
     supp_entry_t *supp_entry = kmp_api_data_get(kmp);
     if (!supp_entry) {
         // Should not be possible
-        return;
+        return false;
     }
     kmp_service_t *service = kmp_api_service_get(kmp);
     pae_auth_t *pae_auth = ws_pae_auth_by_kmp_service_get(service);
     if (!pae_auth) {
         // Should not be possible
-        return;
+        return false;
     }
 
     // Ensures that supplicant is in active supplicant list before initiating next KMP
     if (!ws_pae_lib_supp_list_entry_is_in_list(&pae_auth->active_supp_list, supp_entry)) {
-        return;
+        return false;
     }
 
-    ws_pae_auth_next_kmp_trigger(pae_auth, supp_entry);
+    return ws_pae_auth_next_kmp_trigger(pae_auth, supp_entry);
 }
 
-static void ws_pae_auth_next_kmp_trigger(pae_auth_t *pae_auth, supp_entry_t *supp_entry)
+static bool ws_pae_auth_next_kmp_trigger(pae_auth_t *pae_auth, supp_entry_t *supp_entry)
 {
     // Get next protocol based on what keys supplicant has
     kmp_type_e next_type = ws_pae_auth_next_protocol_get(pae_auth, supp_entry);
@@ -1154,7 +1154,7 @@ static void ws_pae_auth_next_kmp_trigger(pae_auth_t *pae_auth, supp_entry_t *sup
         // Supplicant goes inactive after 15 seconds
         ws_pae_lib_supp_timer_ticks_set(supp_entry, WAIT_AFTER_AUTHENTICATION_TICKS);
         // All done
-        return;
+        return true;
     } else {
         kmp_api_t *api = ws_pae_lib_kmp_list_type_get(&supp_entry->kmp_list, next_type);
         if (api != NULL) {
@@ -1162,7 +1162,7 @@ static void ws_pae_auth_next_kmp_trigger(pae_auth_t *pae_auth, supp_entry_t *sup
                for GTK there can be previous terminating and the new one for next key index */
             if (next_type != IEEE_802_11_GKH) {
                 tr_info("KMP already ongoing; ignored, eui-64: %s", trace_array(supp_entry->addr.eui_64, 8));
-                return;
+                return false;
             }
         }
     }
@@ -1173,7 +1173,7 @@ static void ws_pae_auth_next_kmp_trigger(pae_auth_t *pae_auth, supp_entry_t *sup
     // Create new instance
     kmp_api_t *new_kmp = ws_pae_auth_kmp_create_and_start(pae_auth->kmp_service, next_type, pae_auth->relay_socked_msg_if_instance_id, supp_entry, pae_auth->sec_cfg);
     if (!new_kmp) {
-        return;
+        return false;
     }
 
     // For radius EAP-TLS create also radius client in addition to EAP-TLS
@@ -1181,12 +1181,12 @@ static void ws_pae_auth_next_kmp_trigger(pae_auth_t *pae_auth, supp_entry_t *sup
         if (ws_pae_lib_kmp_list_type_get(&supp_entry->kmp_list, RADIUS_CLIENT_PROT) != NULL) {
             // Radius client already exists, wait for it to be deleted
             ws_pae_lib_kmp_list_delete(&supp_entry->kmp_list, new_kmp);
-            return;
+            return false;
         }
         // Create radius client instance */
         if (ws_pae_auth_kmp_create_and_start(pae_auth->kmp_service, RADIUS_CLIENT_PROT, pae_auth->radius_socked_msg_if_instance_id, supp_entry, pae_auth->sec_cfg) == NULL) {
             ws_pae_lib_kmp_list_delete(&supp_entry->kmp_list, new_kmp);
-            return;
+            return false;
         }
     }
     // For EAP-TLS create also TLS client in addition to EAP-TLS
@@ -1194,16 +1194,17 @@ static void ws_pae_auth_next_kmp_trigger(pae_auth_t *pae_auth, supp_entry_t *sup
         if (ws_pae_lib_kmp_list_type_get(&supp_entry->kmp_list, TLS_PROT) != NULL) {
             // TLS already exists, wait for it to be deleted
             ws_pae_lib_kmp_list_delete(&supp_entry->kmp_list, new_kmp);
-            return;
+            return false;
         }
         // Create TLS instance */
         if (ws_pae_auth_kmp_create_and_start(pae_auth->kmp_service, TLS_PROT, pae_auth->relay_socked_msg_if_instance_id, supp_entry, pae_auth->sec_cfg) == NULL) {
             ws_pae_lib_kmp_list_delete(&supp_entry->kmp_list, new_kmp);
-            return;
+            return false;
         }
     }
 
     kmp_api_create_request(new_kmp, next_type, &supp_entry->addr, &supp_entry->sec_keys);
+    return false;
 }
 
 static kmp_type_e ws_pae_auth_next_protocol_get(pae_auth_t *pae_auth, supp_entry_t *supp_entry)
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_controller.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_controller.c
index da74695ee3..0f299ce55d 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_controller.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_controller.c
@@ -141,8 +141,8 @@ static int8_t ws_pae_controller_frame_counter_read(pae_controller_t *controller)
 static void ws_pae_controller_frame_counter_reset(frame_counters_t *frame_counters);
 static void ws_pae_controller_frame_counter_index_reset(frame_counters_t *frame_counters, uint8_t index);
 static int8_t ws_pae_controller_nw_info_read(pae_controller_t *controller, sec_prot_gtk_keys_t *gtks);
-static int8_t ws_pae_controller_nvm_nw_info_write(protocol_interface_info_entry_t *interface_ptr, uint16_t pan_id, char *network_name, sec_prot_gtk_keys_t *gtks);
-static int8_t ws_pae_controller_nvm_nw_info_read(protocol_interface_info_entry_t *interface_ptr, uint16_t *pan_id, char *network_name, sec_prot_gtk_keys_t *gtks);
+static int8_t ws_pae_controller_nvm_nw_info_write(protocol_interface_info_entry_t *interface_ptr, uint16_t pan_id, char *network_name, uint8_t *gtk_eui64, sec_prot_gtk_keys_t *gtks);
+static int8_t ws_pae_controller_nvm_nw_info_read(protocol_interface_info_entry_t *interface_ptr, uint16_t *pan_id, char *network_name, uint8_t *gtk_eui64, sec_prot_gtk_keys_t *gtks);
 
 
 static const char *FRAME_COUNTER_FILE = FRAME_COUNTER_FILE_NAME;
@@ -371,7 +371,13 @@ static void ws_pae_controller_nw_info_updated_check(protocol_interface_info_entr
     }
 
     if (controller->sec_keys_nw_info.updated || sec_prot_keys_gtks_are_updated(controller->sec_keys_nw_info.gtks)) {
-        ws_pae_controller_nvm_nw_info_write(interface_ptr, controller->sec_keys_nw_info.key_pan_id, controller->sec_keys_nw_info.network_name, controller->sec_keys_nw_info.gtks);
+        // Get own EUI-64
+        uint8_t gtk_eui64[8] = {0};
+        link_layer_address_s mac_params;
+        if (arm_nwk_mac_address_read(interface_ptr->id, &mac_params) >= 0) {
+            memcpy(gtk_eui64, mac_params.mac_long, 8);
+        }
+        ws_pae_controller_nvm_nw_info_write(interface_ptr, controller->sec_keys_nw_info.key_pan_id, controller->sec_keys_nw_info.network_name, gtk_eui64, controller->sec_keys_nw_info.gtks);
         controller->sec_keys_nw_info.updated = false;
         sec_prot_keys_gtks_updated_reset(controller->sec_keys_nw_info.gtks);
     }
@@ -844,32 +850,46 @@ static void ws_pae_controller_frame_counter_index_reset(frame_counters_t *frame_
 
 static int8_t ws_pae_controller_nw_info_read(pae_controller_t *controller, sec_prot_gtk_keys_t *gtks)
 {
-    if (ws_pae_controller_nvm_nw_info_read(controller->interface_ptr, &controller->sec_keys_nw_info.key_pan_id, controller->sec_keys_nw_info.network_name, gtks) < 0) {
+    uint8_t nvm_gtk_eui64[8];
+    if (ws_pae_controller_nvm_nw_info_read(controller->interface_ptr, &controller->sec_keys_nw_info.key_pan_id, controller->sec_keys_nw_info.network_name, nvm_gtk_eui64, gtks) < 0) {
         // If no stored GTKs and network info (pan_id and network name) exits
         return -1;
     }
 
+    /* Get own EUI-64 and compare to the one read from the NVM. In case of mismatch delete GTKs and make
+       full authentication to update keys with new EUI-64 and in case of authenticator to update new
+       authenticator EUI-64 to the network. */
+    uint8_t gtk_eui64[8] = {0};
+    link_layer_address_s mac_params;
+    if (arm_nwk_mac_address_read(controller->interface_ptr->id, &mac_params) >= 0) {
+        memcpy(gtk_eui64, mac_params.mac_long, 8);
+    }
+    if (memcmp(nvm_gtk_eui64, gtk_eui64, 8) != 0) {
+        tr_warn("NVM EUI-64 mismatch, current: %s stored: %s", tr_array(gtk_eui64, 8), tr_array(nvm_gtk_eui64, 8));
+        sec_prot_keys_gtks_clear(gtks);
+    }
+
     // Sets also new pan_id used for pan_id set by bootstrap
     controller->sec_keys_nw_info.new_pan_id = controller->sec_keys_nw_info.key_pan_id;
 
     return 0;
 }
 
-static int8_t ws_pae_controller_nvm_nw_info_write(protocol_interface_info_entry_t *interface_ptr, uint16_t pan_id, char *network_name, sec_prot_gtk_keys_t *gtks)
+static int8_t ws_pae_controller_nvm_nw_info_write(protocol_interface_info_entry_t *interface_ptr, uint16_t pan_id, char *network_name, uint8_t *gtk_eui64, sec_prot_gtk_keys_t *gtks)
 {
     nw_info_nvm_tlv_t *tlv = (nw_info_nvm_tlv_t *) ws_pae_controller_nvm_tlv_get(interface_ptr);
     if (!tlv) {
         return -1;
     }
 
-    ws_pae_nvm_store_nw_info_tlv_create(tlv, pan_id, network_name, gtks);
+    ws_pae_nvm_store_nw_info_tlv_create(tlv, pan_id, network_name, gtk_eui64, gtks);
 
     ws_pae_nvm_store_tlv_file_write(NW_INFO_FILE, (nvm_tlv_t *) tlv);
 
     return 0;
 }
 
-static int8_t ws_pae_controller_nvm_nw_info_read(protocol_interface_info_entry_t *interface_ptr, uint16_t *pan_id, char *network_name, sec_prot_gtk_keys_t *gtks)
+static int8_t ws_pae_controller_nvm_nw_info_read(protocol_interface_info_entry_t *interface_ptr, uint16_t *pan_id, char *network_name, uint8_t *gtk_eui64, sec_prot_gtk_keys_t *gtks)
 {
     nw_info_nvm_tlv_t *tlv_entry = (nw_info_nvm_tlv_t *) ws_pae_controller_nvm_tlv_get(interface_ptr);
     if (!tlv_entry) {
@@ -882,7 +902,7 @@ static int8_t ws_pae_controller_nvm_nw_info_read(protocol_interface_info_entry_t
         return -1;
     }
 
-    if (ws_pae_nvm_store_nw_info_tlv_read(tlv_entry, pan_id, network_name, gtks) < 0) {
+    if (ws_pae_nvm_store_nw_info_tlv_read(tlv_entry, pan_id, network_name, gtk_eui64, gtks) < 0) {
         return -1;
     }
 
@@ -964,7 +984,8 @@ int8_t ws_pae_controller_auth_init(protocol_interface_info_entry_t *interface_pt
         }
         if (!read_gtks_to || sec_prot_keys_gtk_count(read_gtks_to) == 0) {
             // Key material invalid or GTKs are expired, delete GTKs from NVM
-            ws_pae_controller_nvm_nw_info_write(controller->interface_ptr, controller->sec_keys_nw_info.key_pan_id, controller->sec_keys_nw_info.network_name, NULL);
+            uint8_t gtk_eui64[8] = {0}; // Set GTK EUI-64 to zero
+            ws_pae_controller_nvm_nw_info_write(controller->interface_ptr, controller->sec_keys_nw_info.key_pan_id, controller->sec_keys_nw_info.network_name, gtk_eui64, NULL);
         }
     }
 
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_nvm_data.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_nvm_data.c
index fa26ab3f5c..ba575c2acb 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_nvm_data.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_nvm_data.c
@@ -67,7 +67,7 @@ void ws_pae_nvm_store_generic_tlv_free(nvm_tlv_t *tlv_entry)
     ns_dyn_mem_free(tlv_entry);
 }
 
-void ws_pae_nvm_store_nw_info_tlv_create(nw_info_nvm_tlv_t *tlv_entry, uint16_t pan_id, char *nw_name, sec_prot_gtk_keys_t *gtks)
+void ws_pae_nvm_store_nw_info_tlv_create(nw_info_nvm_tlv_t *tlv_entry, uint16_t pan_id, char *nw_name, uint8_t *gtk_eui64, sec_prot_gtk_keys_t *gtks)
 {
     int len;
     tlv_entry->tag = PAE_NVM_NW_INFO_TAG;
@@ -87,6 +87,9 @@ void ws_pae_nvm_store_nw_info_tlv_create(nw_info_nvm_tlv_t *tlv_entry, uint16_t
     memcpy((char *)tlv, nw_name, len);
     tlv += 33;
 
+    memcpy((char *)tlv, gtk_eui64, 8);
+    tlv += 8;
+
     uint64_t current_time = ws_pae_current_time_get();
 
     for (uint8_t i = 0; i < GTK_NUM; i++) {
@@ -119,7 +122,7 @@ void ws_pae_nvm_store_nw_info_tlv_create(nw_info_nvm_tlv_t *tlv_entry, uint16_t
 
 }
 
-int8_t ws_pae_nvm_store_nw_info_tlv_read(nw_info_nvm_tlv_t *tlv_entry, uint16_t *pan_id, char *nw_name, sec_prot_gtk_keys_t *gtks)
+int8_t ws_pae_nvm_store_nw_info_tlv_read(nw_info_nvm_tlv_t *tlv_entry, uint16_t *pan_id, char *nw_name, uint8_t *gtk_eui64, sec_prot_gtk_keys_t *gtks)
 {
     if (!tlv_entry || !pan_id || !nw_name) {
         return -1;
@@ -144,6 +147,9 @@ int8_t ws_pae_nvm_store_nw_info_tlv_read(nw_info_nvm_tlv_t *tlv_entry, uint16_t
     }
     tlv += 33;
 
+    memcpy(gtk_eui64, (char *)tlv, 8);
+    tlv += 8;
+
     uint64_t current_time = ws_pae_current_time_get();
 
     tr_debug("NVM NW_INFO current time: %"PRIi64, current_time);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_nvm_data.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_nvm_data.h
index 23f164d4d8..3d6e643231 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_nvm_data.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_nvm_data.h
@@ -35,8 +35,8 @@
 #define PAE_NVM_KEY_STORAGE_INDEX_TAG    4
 #define PAE_NVM_KEY_STORAGE_TAG          5
 
-// pan_id (2) +  network name (33) + (GTK set (1) + GTK expiry timestamp (8) + status (1) + install order (1) + GTK (16)) * 4
-#define PAE_NVM_NW_INFO_LEN              2 + 33 + (1 + 8 + 1 + 1 + GTK_LEN) * GTK_NUM
+// pan_id (2) +  network name (33) + GTK EUI-64 (own EUI-64) (8) + (GTK set (1) + GTK expiry timestamp (8) + status (1) + install order (1) + GTK (16)) * 4
+#define PAE_NVM_NW_INFO_LEN              2 + 33 + 8 + (1 + 8 + 1 + 1 + GTK_LEN) * GTK_NUM
 
 // PTK EUI-64 set (1) + PTK EUI-64 (8) + PMK set (1) + PMK lifetime (4) + PMK (32) + PMK replay counter (8) + PTK set (1) + PTK lifetime (4) + PTK (48)
 #define PAE_NVM_KEYS_LEN                 1 + 8 + 1 + 4 + PMK_LEN + 8 + 1 + 4 + PTK_LEN
@@ -95,7 +95,7 @@ void ws_pae_nvm_store_generic_tlv_free(nvm_tlv_t *tlv_entry);
  * \return TLV entry or NULL
  *
  */
-void ws_pae_nvm_store_nw_info_tlv_create(nw_info_nvm_tlv_t *tlv_entry, uint16_t pan_id, char *nw_name, sec_prot_gtk_keys_t *gtks);
+void ws_pae_nvm_store_nw_info_tlv_create(nw_info_nvm_tlv_t *tlv_entry, uint16_t pan_id, char *nw_name, uint8_t *gtk_eui64, sec_prot_gtk_keys_t *gtks);
 
 /**
  * ws_pae_nvm_store_nw_info_tlv_read read from NVM network info TLV
@@ -109,7 +109,7 @@ void ws_pae_nvm_store_nw_info_tlv_create(nw_info_nvm_tlv_t *tlv_entry, uint16_t
  * \return >= 0 success
  *
  */
-int8_t ws_pae_nvm_store_nw_info_tlv_read(nw_info_nvm_tlv_t *tlv_entry, uint16_t *pan_id, char *nw_name, sec_prot_gtk_keys_t *gtks);
+int8_t ws_pae_nvm_store_nw_info_tlv_read(nw_info_nvm_tlv_t *tlv_entry, uint16_t *pan_id, char *nw_name, uint8_t *gtk_eui64, sec_prot_gtk_keys_t *gtks);
 
 /**
  * ws_pae_nvm_store_keys_tlv_create create NVM keys TLV
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_supp.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_supp.c
index c8aeee7267..d73de75a2b 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_supp.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_pae_supp.c
@@ -151,7 +151,7 @@ static int8_t ws_pae_supp_gtk_hash_mismatch_check(pae_supp_t *pae_supp);
 
 static void ws_pae_supp_kmp_api_create_confirm(kmp_api_t *kmp, kmp_result_e result);
 static void ws_pae_supp_kmp_api_create_indication(kmp_api_t *kmp, kmp_type_e type, kmp_addr_t *addr);
-static void ws_pae_supp_kmp_api_finished_indication(kmp_api_t *kmp, kmp_result_e result, kmp_sec_keys_t *sec_keys);
+static bool ws_pae_supp_kmp_api_finished_indication(kmp_api_t *kmp, kmp_result_e result, kmp_sec_keys_t *sec_keys);
 static void ws_pae_supp_kmp_api_finished(kmp_api_t *kmp);
 
 
@@ -429,11 +429,7 @@ static void ws_pae_supp_authenticate_response(pae_supp_t *pae_supp, auth_result_
     pae_supp->auth_trickle_running = false;
     if (pae_supp->auth_requested && pae_supp->auth_completed) {
         pae_supp->auth_requested = false;
-        uint8_t *target_eui_64 = NULL;
-        if (result != AUTH_RESULT_OK) {
-            target_eui_64 = pae_supp->target_addr.eui_64;
-        }
-        pae_supp->auth_completed(pae_supp->interface_ptr, result, target_eui_64);
+        pae_supp->auth_completed(pae_supp->interface_ptr, result, pae_supp->target_addr.eui_64);
     }
 }
 
@@ -1226,12 +1222,12 @@ static void ws_pae_supp_kmp_api_create_indication(kmp_api_t *kmp, kmp_type_e typ
     kmp_api_create_response(kmp, KMP_RESULT_OK);
 }
 
-static void ws_pae_supp_kmp_api_finished_indication(kmp_api_t *kmp, kmp_result_e result, kmp_sec_keys_t *sec_keys)
+static bool ws_pae_supp_kmp_api_finished_indication(kmp_api_t *kmp, kmp_result_e result, kmp_sec_keys_t *sec_keys)
 {
     kmp_service_t *service = kmp_api_service_get(kmp);
     pae_supp_t *pae_supp = ws_pae_supp_by_kmp_service_get(service);
     if (!pae_supp) {
-        return;
+        return false;
     }
 
     kmp_type_e type = kmp_api_type_get(kmp);
@@ -1263,6 +1259,8 @@ static void ws_pae_supp_kmp_api_finished_indication(kmp_api_t *kmp, kmp_result_e
             pae_supp->tx_failure_on_initial_key = true;
         }
     }
+
+    return false;
 }
 
 static void ws_pae_supp_kmp_api_finished(kmp_api_t *kmp)
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_stats.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_stats.c
index 85f2511221..a6e794e4b5 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_stats.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_stats.c
@@ -51,20 +51,69 @@ int ws_statistics_stop(int8_t interface_id)
 
 void ws_stats_update(protocol_interface_info_entry_t *cur, ws_stats_type_t type, uint32_t update_val)
 {
-    if (!cur || !ws_info(cur)) {
+    if (!cur || !ws_info(cur) || !cur->ws_info->stored_stats_ptr) {
         return;
     }
     ws_statistics_t *stored_stats = cur->ws_info->stored_stats_ptr;
 
-    if (stored_stats) {
-        switch (type) {
-            case STATS_WS_ASYNCH_TX:
-                stored_stats->asynch_tx_count += update_val;
-                break;
-            case STATS_WS_ASYNCH_RX:
-                stored_stats->asynch_rx_count += update_val;
-                break;
-        }
+    switch (type) {
+        case STATS_WS_ASYNCH_TX:
+            stored_stats->asynch_tx_count += update_val;
+            break;
+        case STATS_WS_ASYNCH_RX:
+            stored_stats->asynch_rx_count += update_val;
+            break;
+        case STATS_WS_ASYNCH_TX_PAS:
+            stored_stats->sent_PAS += update_val;
+            break;
+        case STATS_WS_ASYNCH_TX_PA:
+            stored_stats->sent_PA += update_val;
+            break;
+        case STATS_WS_ASYNCH_TX_PCS:
+            stored_stats->sent_PCS += update_val;
+            break;
+        case STATS_WS_ASYNCH_TX_PC:
+            stored_stats->sent_PC += update_val;
+            break;
+        case STATS_WS_ASYNCH_RX_PAS:
+            stored_stats->recv_PAS += update_val;
+            break;
+        case STATS_WS_ASYNCH_RX_PA:
+            stored_stats->recv_PA += update_val;
+            break;
+        case STATS_WS_ASYNCH_RX_PCS:
+            stored_stats->recv_PCS += update_val;
+            break;
+        case STATS_WS_ASYNCH_RX_PC:
+            stored_stats->recv_PC += update_val;
+            break;
+        case STATS_WS_STATE_1:
+            stored_stats->join_state_1 += update_val;
+            break;
+        case STATS_WS_STATE_2:
+            stored_stats->join_state_2 += update_val;
+            break;
+        case STATS_WS_STATE_3:
+            stored_stats->join_state_3 += update_val;
+            break;
+        case STATS_WS_STATE_4:
+            stored_stats->join_state_4 += update_val;
+            break;
+        case STATS_WS_STATE_5:
+            stored_stats->join_state_5 += update_val;
+            break;
+        case STATS_WS_NEIGHBOUR_ADD:
+            stored_stats->Neighbour_add += update_val;
+            break;
+        case STATS_WS_NEIGHBOUR_REMOVE:
+            stored_stats->Neighbour_remove += update_val;
+            break;
+        case STATS_WS_CHILD_ADD:
+            stored_stats->Child_add += update_val;
+            break;
+        case STATS_WS_CHILD_REMOVE:
+            stored_stats->child_remove += update_val;
+            break;
     }
 }
 #endif // HAVE_WS
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_stats.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_stats.h
index 8f7776d97d..5a315556f8 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_stats.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_stats.h
@@ -22,7 +22,24 @@
 
 typedef enum {
     STATS_WS_ASYNCH_TX,
-    STATS_WS_ASYNCH_RX
+    STATS_WS_ASYNCH_RX,
+    STATS_WS_ASYNCH_RX_PAS,
+    STATS_WS_ASYNCH_RX_PA,
+    STATS_WS_ASYNCH_RX_PCS,
+    STATS_WS_ASYNCH_RX_PC,
+    STATS_WS_ASYNCH_TX_PAS,
+    STATS_WS_ASYNCH_TX_PA,
+    STATS_WS_ASYNCH_TX_PCS,
+    STATS_WS_ASYNCH_TX_PC,
+    STATS_WS_STATE_1,
+    STATS_WS_STATE_2,
+    STATS_WS_STATE_3,
+    STATS_WS_STATE_4,
+    STATS_WS_STATE_5,
+    STATS_WS_NEIGHBOUR_ADD,
+    STATS_WS_NEIGHBOUR_REMOVE,
+    STATS_WS_CHILD_ADD,
+    STATS_WS_CHILD_REMOVE,
 } ws_stats_type_t;
 
 void ws_stats_update(protocol_interface_info_entry_t *cur, ws_stats_type_t type, uint32_t update_val);
diff --git a/features/nanostack/sal-stack-nanostack/source/Common_Protocols/ipv6.c b/features/nanostack/sal-stack-nanostack/source/Common_Protocols/ipv6.c
index 8ceae7c4c8..045e6c471c 100644
--- a/features/nanostack/sal-stack-nanostack/source/Common_Protocols/ipv6.c
+++ b/features/nanostack/sal-stack-nanostack/source/Common_Protocols/ipv6.c
@@ -212,10 +212,12 @@ buffer_routing_info_t *ipv6_buffer_route_to(buffer_t *buf, const uint8_t *next_h
      * reduce an existing entry if route has changed) */
     if (dest_entry->pmtu > outgoing_if->ipv6_neighbour_cache.link_mtu) {
         dest_entry->pmtu = outgoing_if->ipv6_neighbour_cache.link_mtu;
+        dest_entry->pmtu_lifetime = outgoing_if->pmtu_lifetime;
     }
     /* Route can also limit PMTU */
     if (dest_entry->pmtu > route->route_info.pmtu) {
         dest_entry->pmtu = route->route_info.pmtu;
+        dest_entry->pmtu_lifetime = outgoing_if->pmtu_lifetime;
     }
     /* Buffer then gets this PMTU (overwriting what we wrote from the route) */
     route->route_info.pmtu = dest_entry->pmtu;
@@ -1360,6 +1362,9 @@ buffer_t *ipv6_forwarding_up(buffer_t *buf)
         }
     } else { /* unicast */
         if (!for_us) {
+            if (cur->if_common_forwarding_out_cb) {
+                cur->if_common_forwarding_out_cb(cur, buf);
+            }
             return ipv6_consider_forwarding_unicast_packet(buf, cur, &ll_src);
         }
     }
diff --git a/features/nanostack/sal-stack-nanostack/source/Core/ns_socket.c b/features/nanostack/sal-stack-nanostack/source/Core/ns_socket.c
index 7bd2e5e30d..60b5c96b75 100644
--- a/features/nanostack/sal-stack-nanostack/source/Core/ns_socket.c
+++ b/features/nanostack/sal-stack-nanostack/source/Core/ns_socket.c
@@ -1119,7 +1119,7 @@ int16_t socket_buffer_sendmsg(int8_t sid, buffer_t *buf, const struct ns_msghdr
 
 #ifndef NO_TCP
     if (socket_ptr->type == SOCKET_TYPE_STREAM) {
-        tcp_session_t *tcp_info = tcp_info(inet_pcb);
+        tcp_session_t *tcp_info = inet_pcb->session;
         if (!tcp_info) {
             tr_warn("No TCP session for cur Socket");
             ret_val = -3;
diff --git a/features/nanostack/sal-stack-nanostack/source/DHCPv6_client/dhcpv6_client_api.h b/features/nanostack/sal-stack-nanostack/source/DHCPv6_client/dhcpv6_client_api.h
index 57025e76ef..b7429c6125 100644
--- a/features/nanostack/sal-stack-nanostack/source/DHCPv6_client/dhcpv6_client_api.h
+++ b/features/nanostack/sal-stack-nanostack/source/DHCPv6_client/dhcpv6_client_api.h
@@ -139,6 +139,8 @@ void dhcp_client_global_address_delete(int8_t interface, uint8_t *dhcp_addr, uin
 
 void dhcp_relay_agent_enable(int8_t interface, uint8_t border_router_address[static 16]);
 
+void dhcp_relay_agent_interface_id_option_enable(int8_t interface, bool enable);
+
 int dhcp_client_server_address_update(int8_t interface, uint8_t *prefix, uint8_t server_address[static 16]);
 
 
diff --git a/features/nanostack/sal-stack-nanostack/source/DHCPv6_client/dhcpv6_client_service.c b/features/nanostack/sal-stack-nanostack/source/DHCPv6_client/dhcpv6_client_service.c
index 559b6ccf1d..758ce3977b 100644
--- a/features/nanostack/sal-stack-nanostack/source/DHCPv6_client/dhcpv6_client_service.c
+++ b/features/nanostack/sal-stack-nanostack/source/DHCPv6_client/dhcpv6_client_service.c
@@ -175,6 +175,16 @@ void dhcp_relay_agent_enable(int8_t interface, uint8_t border_router_address[sta
     dhcp_service_relay_instance_enable(dhcp_client->relay_instance, border_router_address);
 }
 
+void dhcp_relay_agent_interface_id_option_enable(int8_t interface, bool enable)
+{
+    dhcp_client_class_t *dhcp_client = dhcpv6_client_entry_discover(interface);
+    if (!dhcp_client) {
+        return;
+    }
+
+    dhcp_service_relay_interface_id_option_enable(dhcp_client->relay_instance, enable);
+}
+
 void dhcp_client_delete(int8_t interface)
 {
     protocol_interface_info_entry_t *cur = NULL;
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_data_buffer.h b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_data_buffer.h
index 3e5a7629ab..ec694540da 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_data_buffer.h
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_data_buffer.h
@@ -93,6 +93,11 @@ typedef struct mac_pre_build_frame {
     uint8_t fhss_cca_retry_count;
     uint8_t stored_retry_cnt;
     uint8_t stored_cca_cnt;
+    uint8_t cca_request_restart_cnt;
+    uint8_t tx_request_restart_cnt;
+    uint8_t priority;
+    uint32_t blacklist_start_time_us;
+    uint16_t blacklist_period_ms;
     uint16_t initial_tx_channel;
     uint32_t tx_time;
     bool upper_layer_request: 1;
@@ -103,7 +108,6 @@ typedef struct mac_pre_build_frame {
     bool ExtendedFrameExchange: 1;
     bool WaitResponse: 1;
     unsigned security_mic_len: 5;   //Max possible lengths 0, 4, 8, 16 bytes
-    unsigned priority: 2;
     unsigned stored_priority: 2;
     struct mac_pre_build_frame *next; //Pointer for queue purpose
 } mac_pre_build_frame_t;
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_defines.h b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_defines.h
index 3012e5c056..c13c708e08 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_defines.h
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_defines.h
@@ -124,6 +124,7 @@ typedef struct dev_driver_tx_buffer {
 #define MAC_CCA_MAX 8
 #define MAC_DEF_MIN_BE 3
 #define MAC_DEF_MAX_BE 5
+#define MAC_PRIORITY_EF_BACKOFF_MULTIPLIER  2
 
 typedef struct mac_active_scan {
     uint8_t pan_id[2];
@@ -246,6 +247,10 @@ typedef struct protocol_interface_rf_mac_setup {
     uint8_t mac_sequence;
     uint8_t mac_tx_retry;
     uint8_t mac_cca_retry;
+    uint8_t cca_failure_restart_max;
+    uint8_t tx_failure_restart_max;
+    uint16_t blacklist_min_ms;
+    uint16_t blacklist_max_ms;
     uint16_t mac_ack_wait_duration;
     uint8_t mac_mlme_retry_max;
     uint8_t aUnitBackoffPeriod;
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.c b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.c
index 40b0319e89..9d65c24e6f 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.c
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.c
@@ -35,6 +35,7 @@
 #include "platform/arm_hal_interrupt.h"
 #include "common_functions.h"
 #include "Core/include/ns_monitor.h"
+#include "randLIB.h"
 
 #include "MAC/IEEE802_15_4/sw_mac_internal.h"
 #include "MAC/IEEE802_15_4/mac_defines.h"
@@ -153,7 +154,7 @@ void mcps_sap_data_req_handler(protocol_interface_rf_mac_setup_s *rf_mac_setup,
 {
     mcps_data_req_ie_list_t ie_list;
     memset(&ie_list, 0, sizeof(mcps_data_req_ie_list_t));
-    mcps_sap_data_req_handler_ext(rf_mac_setup, data_req, &ie_list, NULL);
+    mcps_sap_data_req_handler_ext(rf_mac_setup, data_req, &ie_list, NULL, MAC_DATA_NORMAL_PRIORITY);
 }
 
 static bool mac_ie_vector_length_validate(ns_ie_iovec_t *ie_vector, uint16_t iov_length,  uint16_t *length_out)
@@ -192,7 +193,7 @@ static bool mac_ie_vector_length_validate(ns_ie_iovec_t *ie_vector, uint16_t iov
 }
 
 
-void mcps_sap_data_req_handler_ext(protocol_interface_rf_mac_setup_s *rf_mac_setup, const mcps_data_req_t *data_req, const mcps_data_req_ie_list_t *ie_list, const channel_list_s *asynch_channel_list)
+void mcps_sap_data_req_handler_ext(protocol_interface_rf_mac_setup_s *rf_mac_setup, const mcps_data_req_t *data_req, const mcps_data_req_ie_list_t *ie_list, const channel_list_s *asynch_channel_list, mac_data_priority_t priority)
 {
     uint8_t status = MLME_SUCCESS;
     mac_pre_build_frame_t *buffer = NULL;
@@ -264,6 +265,27 @@ void mcps_sap_data_req_handler_ext(protocol_interface_rf_mac_setup_s *rf_mac_set
         buffer->asynch_request = true;
     }
 
+    //Set Priority level
+    switch (priority) {
+        case MAC_DATA_EXPEDITE_FORWARD:
+            buffer->priority = MAC_PD_DATA_EF_PRIORITY;
+            // Enable FHSS expedited forwarding
+            if (rf_mac_setup->fhss_api) {
+                rf_mac_setup->fhss_api->synch_state_set(rf_mac_setup->fhss_api, FHSS_EXPEDITED_FORWARDING, 0);
+            }
+            break;
+        case MAC_DATA_HIGH_PRIORITY:
+            buffer->priority = MAC_PD_DATA_HIGH_PRIOTITY;
+            break;
+        case MAC_DATA_MEDIUM_PRIORITY:
+            buffer->priority = MAC_PD_DATA_MEDIUM_PRIORITY;
+            break;
+        default:
+            buffer->priority = MAC_PD_DATA_NORMAL_PRIORITY;
+            break;
+    }
+
+
     buffer->upper_layer_request = true;
     buffer->fcf_dsn.frametype = FC_DATA_FRAME;
     buffer->ExtendedFrameExchange = data_req->ExtendedFrameExchange;
@@ -353,6 +375,12 @@ void mcps_sap_data_req_handler_ext(protocol_interface_rf_mac_setup_s *rf_mac_set
     buffer->mac_header_length_with_security += mac_header_address_length(&buffer->fcf_dsn);
     buffer->mac_payload = data_req->msdu;
     buffer->mac_payload_length = data_req->msduLength;
+    buffer->cca_request_restart_cnt = rf_mac_setup->cca_failure_restart_max;
+    // Multiply number of backoffs for higher priority packets
+    if (buffer->priority == MAC_PD_DATA_EF_PRIORITY) {
+        buffer->cca_request_restart_cnt *= MAC_PRIORITY_EF_BACKOFF_MULTIPLIER;
+    }
+    buffer->tx_request_restart_cnt = rf_mac_setup->tx_failure_restart_max;
     //check that header + payload length is not bigger than MAC MTU
 
     if (data_req->InDirectTx) {
@@ -1498,6 +1526,66 @@ static bool mcps_buffer_edfe_data_failure(protocol_interface_rf_mac_setup_s *rf_
     return false;
 }
 
+static void mcps_set_packet_blacklist(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer, uint8_t number_of_restarts)
+{
+    /*
+     * Random min = configured blacklist min << attempt count, but never larger than configured blacklist max / 2.
+     * Random max = random min * 2, but never larger than blacklist max.
+     * Example:
+     * blacklist_min_ms: 20ms
+     * blacklist_max_ms: 300ms
+     * blacklist_retry_attempts: 4
+     *
+     * Packet is blacklisted:
+     * 20ms - 40ms after 1st failure
+     * 40ms - 80ms after 2nd failure
+     * 80ms - 160ms after 3rd failure
+     * 150ms - 300ms after 4th failure
+     */
+    uint8_t i = 0;
+    uint32_t blacklist_min_ms = 0;
+    while (i < number_of_restarts) {
+        blacklist_min_ms = rf_ptr->blacklist_min_ms << i;
+        if (blacklist_min_ms > (rf_ptr->blacklist_max_ms / 2)) {
+            break;
+        }
+        i++;
+    }
+    uint32_t blacklist_max_ms = blacklist_min_ms * 2;
+    if (blacklist_min_ms > (rf_ptr->blacklist_max_ms / 2)) {
+        blacklist_min_ms = (rf_ptr->blacklist_max_ms / 2);
+    }
+    if (blacklist_max_ms > rf_ptr->blacklist_max_ms) {
+        blacklist_max_ms = rf_ptr->blacklist_max_ms;
+    }
+    buffer->blacklist_period_ms = randLIB_get_random_in_range(blacklist_min_ms, blacklist_max_ms);
+    if (!buffer->blacklist_period_ms) {
+        buffer->blacklist_period_ms++;
+    }
+    buffer->blacklist_start_time_us = mac_mcps_sap_get_phy_timestamp(rf_ptr);
+}
+
+static bool mcps_update_packet_request_restart(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer)
+{
+    // Function returns true when buffer needs to be requeued
+    if (!rf_ptr || !buffer) {
+        return false;
+    }
+    if (rf_ptr->mac_tx_result == MAC_CCA_FAIL && buffer->cca_request_restart_cnt) {
+        buffer->cca_request_restart_cnt--;
+        if (buffer->priority == MAC_PD_DATA_EF_PRIORITY) {
+            mcps_set_packet_blacklist(rf_ptr, buffer, (rf_ptr->cca_failure_restart_max * MAC_PRIORITY_EF_BACKOFF_MULTIPLIER) - buffer->cca_request_restart_cnt);
+        } else {
+            mcps_set_packet_blacklist(rf_ptr, buffer, rf_ptr->cca_failure_restart_max - buffer->cca_request_restart_cnt);
+        }
+        return true;
+    } else if (rf_ptr->mac_tx_result == MAC_TX_FAIL && buffer->tx_request_restart_cnt) {
+        buffer->tx_request_restart_cnt--;
+        mcps_set_packet_blacklist(rf_ptr, buffer, rf_ptr->tx_failure_restart_max - buffer->tx_request_restart_cnt);
+        return true;
+    }
+    return false;
+}
 
 static void mcps_data_confirm_handle(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer, mac_pre_parsed_frame_t *ack_buf)
 {
@@ -1506,9 +1594,9 @@ static void mcps_data_confirm_handle(protocol_interface_rf_mac_setup_s *rf_ptr,
     sw_mac_stats_update(rf_ptr, STAT_MAC_TX_RETRY, rf_ptr->mac_tx_status.retry);
     mcps_data_conf_t confirm;
     if (rf_ptr->fhss_api && !buffer->asynch_request) {
-        // FHSS checks if this failed buffer needs to be pushed back to TX queue and retransmitted
         if (!mcps_buffer_edfe_data_failure(rf_ptr, buffer) && ((rf_ptr->mac_tx_result == MAC_TX_FAIL) || (rf_ptr->mac_tx_result == MAC_CCA_FAIL) || (rf_ptr->mac_tx_result == MAC_RETURN_TO_QUEUE))) {
-            if ((rf_ptr->mac_tx_result == MAC_RETURN_TO_QUEUE) || rf_ptr->fhss_api->data_tx_fail(rf_ptr->fhss_api, buffer->msduHandle, mac_convert_frame_type_to_fhss(buffer->fcf_dsn.frametype), rf_ptr->mac_tx_start_channel) == true) {
+            // Packet has return to queue status or it needs to be blacklisted and queued
+            if ((rf_ptr->mac_tx_result == MAC_RETURN_TO_QUEUE) || mcps_update_packet_request_restart(rf_ptr, buffer) == true) {
                 if (rf_ptr->mac_tx_result == MAC_TX_FAIL) {
                     buffer->fhss_retry_count += 1 + rf_ptr->mac_tx_status.retry;
                 } else if (rf_ptr->mac_tx_result == MAC_RETURN_TO_QUEUE) {
@@ -2211,6 +2299,17 @@ int mac_convert_frame_type_to_fhss(uint8_t frame_type)
     return FHSS_DATA_FRAME;
 }
 
+static bool mcps_check_packet_blacklist(protocol_interface_rf_mac_setup_s *rf_mac_setup, mac_pre_build_frame_t *buffer)
+{
+    if (!buffer->blacklist_period_ms) {
+        return false;
+    }
+    if ((mac_mcps_sap_get_phy_timestamp(rf_mac_setup) - buffer->blacklist_start_time_us) >= (buffer->blacklist_period_ms * 1000)) {
+        return false;
+    }
+    return true;
+}
+
 void mcps_sap_pd_req_queue_write(protocol_interface_rf_mac_setup_s *rf_mac_setup, mac_pre_build_frame_t *buffer)
 {
     if (!rf_mac_setup || !buffer) {
@@ -2229,9 +2328,9 @@ void mcps_sap_pd_req_queue_write(protocol_interface_rf_mac_setup_s *rf_mac_setup
         }
         if (rf_mac_setup->fhss_api && (buffer->asynch_request == false)) {
             uint16_t frame_length = buffer->mac_payload_length + buffer->headerIeLength + buffer->payloadsIeLength;
-            if (rf_mac_setup->fhss_api->check_tx_conditions(rf_mac_setup->fhss_api, !mac_is_ack_request_set(buffer),
-                                                            buffer->msduHandle, mac_convert_frame_type_to_fhss(buffer->fcf_dsn.frametype), frame_length,
-                                                            rf_mac_setup->dev_driver->phy_driver->phy_header_length, rf_mac_setup->dev_driver->phy_driver->phy_tail_length) == false) {
+            if ((mcps_check_packet_blacklist(rf_mac_setup, buffer) == true) || rf_mac_setup->fhss_api->check_tx_conditions(rf_mac_setup->fhss_api, !mac_is_ack_request_set(buffer),
+                                                                                                                           buffer->msduHandle, mac_convert_frame_type_to_fhss(buffer->fcf_dsn.frametype), frame_length,
+                                                                                                                           rf_mac_setup->dev_driver->phy_driver->phy_header_length, rf_mac_setup->dev_driver->phy_driver->phy_tail_length) == false) {
                 if (buffer->ExtendedFrameExchange) {
                     rf_mac_setup->mac_edfe_info->state = MAC_EDFE_FRAME_IDLE;
                 }
@@ -2334,9 +2433,12 @@ static mac_pre_build_frame_t *mcps_sap_pd_req_queue_read(protocol_interface_rf_m
     // With FHSS, check TX conditions
     if (rf_mac_setup->fhss_api) {
         while (buffer) {
-            if (buffer->asynch_request || (flush == true) || (rf_mac_setup->fhss_api->check_tx_conditions(rf_mac_setup->fhss_api, !mac_is_ack_request_set(buffer),
-                                                                                                          buffer->msduHandle, mac_convert_frame_type_to_fhss(buffer->fcf_dsn.frametype), buffer->mac_payload_length,
-                                                                                                          rf_mac_setup->dev_driver->phy_driver->phy_header_length, rf_mac_setup->dev_driver->phy_driver->phy_tail_length) == true)) {
+            if (buffer->asynch_request ||
+                    (flush == true) ||
+                    ((mcps_check_packet_blacklist(rf_mac_setup, buffer) == false) &&
+                     (rf_mac_setup->fhss_api->check_tx_conditions(rf_mac_setup->fhss_api, !mac_is_ack_request_set(buffer),
+                                                                  buffer->msduHandle, mac_convert_frame_type_to_fhss(buffer->fcf_dsn.frametype), buffer->mac_payload_length,
+                                                                  rf_mac_setup->dev_driver->phy_driver->phy_header_length, rf_mac_setup->dev_driver->phy_driver->phy_tail_length) == true))) {
                 break;
             }
             prev = buffer;
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.h b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.h
index 8368f4eac2..d79343546e 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.h
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.h
@@ -49,7 +49,8 @@ typedef enum {
 #define MAC_PD_DATA_NORMAL_PRIORITY 0    //Normal MCPS DATA REQ
 #define MAC_PD_DATA_MEDIUM_PRIORITY 1    //Indirect Data which is polled
 #define MAC_PD_DATA_HIGH_PRIOTITY   2    //Beacon request Beacon response
-#define MAC_PD_DATA_TX_IMMEDIATELY  3    //Only for packets whose transmission was interrupted by wrong channel type. E.g. unicast on broadcast channel.
+#define MAC_PD_DATA_EF_PRIORITY     3    //Expedited forwarding
+#define MAC_PD_DATA_TX_IMMEDIATELY  4    //Only for packets whose transmission was interrupted by wrong channel type. E.g. unicast on broadcast channel.
 
 #define MCPS_SAP_DATA_IND_EVENT         1
 #define MCPS_SAP_DATA_CNF_EVENT         2
@@ -122,7 +123,7 @@ int8_t mac_virtual_sap_data_cb(void *identifier, struct arm_phy_sap_msg_s *messa
 
 void mcps_sap_data_req_handler(struct protocol_interface_rf_mac_setup *rf_mac_setup, const struct mcps_data_req_s *data_req);
 
-void mcps_sap_data_req_handler_ext(struct protocol_interface_rf_mac_setup *rf_mac_setup, const struct mcps_data_req_s *data_req, const struct mcps_data_req_ie_list *ie_list, const channel_list_s *asynch_channel_list);
+void mcps_sap_data_req_handler_ext(struct protocol_interface_rf_mac_setup *rf_mac_setup, const struct mcps_data_req_s *data_req, const struct mcps_data_req_ie_list *ie_list, const channel_list_s *asynch_channel_list, mac_data_priority_t priority);
 
 void mac_mcps_trig_buffer_from_queue(struct protocol_interface_rf_mac_setup *rf_mac_setup);
 
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mlme.c b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mlme.c
index 5530c6b3ae..c6c0d01cc4 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mlme.c
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mlme.c
@@ -449,6 +449,8 @@ int8_t mac_mlme_reset(protocol_interface_rf_mac_setup_s *rf_mac_setup, const mlm
     rf_mac_setup->macWaitingData = false;
     rf_mac_setup->macDataPollReq = false;
     rf_mac_setup->macRxDataAtPoll = false;
+    rf_mac_setup->macTxProcessActive = false;
+    rf_mac_setup->mac_ack_tx_active = false;
     //Clean MAC
     if (reset->SetDefaultPIB) {
         tr_debug("RESET MAC PIB");
@@ -596,7 +598,7 @@ static int8_t mac_mlme_8bit_set(protocol_interface_rf_mac_setup_s *rf_mac_setup,
             break;
 
         case macMaxBE:
-            if (value > 8 || value < 3) {
+            if (value > 8 || value < 1) {
                 return -1;
             }
             rf_mac_setup->macMaxBE = value;
@@ -747,6 +749,18 @@ static int8_t mac_mlme_set_multi_csma_parameters(protocol_interface_rf_mac_setup
     return 0;
 }
 
+static int8_t mac_mlme_set_data_request_restart_config(protocol_interface_rf_mac_setup_s *rf_mac_setup, const mlme_set_t *set_req)
+{
+    mlme_request_restart_config_t request_restart_config;
+    memcpy(&request_restart_config, set_req->value_pointer, sizeof(mlme_request_restart_config_t));
+    rf_mac_setup->cca_failure_restart_max = request_restart_config.cca_failure_restart_max;
+    rf_mac_setup->tx_failure_restart_max = request_restart_config.tx_failure_restart_max;
+    rf_mac_setup->blacklist_min_ms = request_restart_config.blacklist_min_ms;
+    rf_mac_setup->blacklist_max_ms = request_restart_config.blacklist_max_ms;
+    tr_debug("Request restart config: CCA %u, TX %u, min %u, max %u", rf_mac_setup->cca_failure_restart_max, rf_mac_setup->tx_failure_restart_max, rf_mac_setup->blacklist_min_ms, rf_mac_setup->blacklist_max_ms);
+    return 0;
+}
+
 int8_t mac_mlme_set_req(protocol_interface_rf_mac_setup_s *rf_mac_setup, const mlme_set_t *set_req)
 {
     if (!set_req || !rf_mac_setup || !rf_mac_setup->dev_driver || !rf_mac_setup->dev_driver->phy_driver) {
@@ -816,6 +830,8 @@ int8_t mac_mlme_set_req(protocol_interface_rf_mac_setup_s *rf_mac_setup, const m
             return 0;
         case macMultiCSMAParameters:
             return mac_mlme_set_multi_csma_parameters(rf_mac_setup, set_req);
+        case macRequestRestart:
+            return mac_mlme_set_data_request_restart_config(rf_mac_setup, set_req);
         case macRfConfiguration:
             rf_mac_setup->dev_driver->phy_driver->extension(PHY_EXTENSION_SET_RF_CONFIGURATION, (uint8_t *) set_req->value_pointer);
             mac_mlme_set_symbol_rate(rf_mac_setup);
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_pd_sap.c b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_pd_sap.c
index 08a5604237..2a61f10ee9 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_pd_sap.c
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_pd_sap.c
@@ -455,7 +455,7 @@ static int8_t mac_data_interface_tx_done_cb(protocol_interface_rf_mac_setup_s *r
     }
 
 #ifdef TIMING_TOOL_TRACES
-    if ((status == PHY_LINK_CCA_FAIL) || (status == PHY_LINK_CCA_PREPARE)) {
+    if ((status == PHY_LINK_CCA_FAIL) || (status == PHY_LINK_CCA_FAIL_RX) || (status == PHY_LINK_CCA_PREPARE)) {
         tr_info("%u CSMA_done", mac_mcps_sap_get_phy_timestamp(rf_ptr));
     }
 #endif
@@ -559,7 +559,8 @@ VALIDATE_TX_TIME:
 
         // Do not update CCA count when Ack is received, it was already updated with PHY_LINK_TX_SUCCESS event
         // Do not update CCA count when CCA_OK is received, PHY_LINK_TX_SUCCESS will update it
-        if ((status != PHY_LINK_TX_DONE) && (status != PHY_LINK_TX_DONE_PENDING) && (status != PHY_LINK_CCA_OK)) {
+        // Do not update CCA count when CCA fail was because of active reception, MAC will restart CCA check in this case
+        if ((status != PHY_LINK_TX_DONE) && (status != PHY_LINK_TX_DONE_PENDING) && (status != PHY_LINK_CCA_OK) && (status != PHY_LINK_CCA_FAIL_RX)) {
             /* For PHY_LINK_TX_SUCCESS and PHY_LINK_CCA_FAIL cca_retry must always be > 0.
              * PHY_LINK_TX_FAIL either happened during transmission or when waiting Ack -> we must use the CCA count given by PHY.
              */
@@ -593,6 +594,9 @@ VALIDATE_TX_TIME:
         } else if (status == PHY_LINK_CCA_FAIL) {
             waiting_ack = false;
             tx_completed = false;
+        } else if (status == PHY_LINK_CCA_FAIL_RX) {
+            waiting_ack = false;
+            tx_completed = false;
         } else if (status == PHY_LINK_CCA_OK) {
             waiting_ack = false;
             tx_completed = false;
@@ -620,6 +624,7 @@ VALIDATE_TX_TIME:
             break;
 
         case PHY_LINK_CCA_FAIL:
+        case PHY_LINK_CCA_FAIL_RX:
             mac_sap_cca_fail_cb(rf_ptr, failed_channel);
             break;
 
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_security_mib.c b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_security_mib.c
index 0897b78bb9..ed06964ab5 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_security_mib.c
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_security_mib.c
@@ -600,7 +600,7 @@ void mac_sec_mib_key_device_frame_counter_set(mlme_key_descriptor_t *key_descpri
     if (key_descpription_table->KeyFrameCounterPerKey) {
         uint32_t *counter_ptr = key_descpription_table->KeyDeviceFrameCounterList + attribute_index;
         *counter_ptr = frame_counter;
-    } else {
+    } else if (device_info) {
         device_info->FrameCounter = frame_counter;
     }
 }
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/sw_mac.c b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/sw_mac.c
index 7fd08c6d25..7e287ee514 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/sw_mac.c
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/sw_mac.c
@@ -58,7 +58,7 @@ static int8_t ns_sw_mac_api_enable_edfe_ext(mac_api_t *api, mcps_edfe_handler *e
 
 static void mlme_req(const mac_api_t *api, mlme_primitive id, const void *data);
 static void mcps_req(const mac_api_t *api, const mcps_data_req_t *data);
-static void mcps_req_ext(const mac_api_t *api, const mcps_data_req_t *data, const mcps_data_req_ie_list_t *ie_ext, const channel_list_s *asynch_channel_list);
+static void mcps_req_ext(const mac_api_t *api, const mcps_data_req_t *data, const mcps_data_req_ie_list_t *ie_ext, const channel_list_s *asynch_channel_list,  mac_data_priority_t priority);
 static uint8_t purge_req(const mac_api_t *api, const mcps_purge_t *data);
 static int8_t macext_mac64_address_set(const mac_api_t *api, const uint8_t *mac64);
 static int8_t macext_mac64_address_get(const mac_api_t *api, mac_extended_address_type type, uint8_t *mac64_buf);
@@ -575,22 +575,22 @@ void mlme_req(const mac_api_t *api, mlme_primitive id, const void *data)
     }
 }
 
-void mcps_req(const mac_api_t *api, const mcps_data_req_t *data)
+static void mcps_req(const mac_api_t *api, const mcps_data_req_t *data)
 {
     //TODO: Populate linked list when present
     if (mac_store.mac_api == api) {
         /* Call direct new API but without IE extensions */
         mcps_data_req_ie_list_t ie_list;
         memset(&ie_list, 0, sizeof(mcps_data_req_ie_list_t));
-        mcps_sap_data_req_handler_ext(mac_store.setup, data, &ie_list, NULL);
+        mcps_sap_data_req_handler_ext(mac_store.setup, data, &ie_list, NULL, MAC_DATA_NORMAL_PRIORITY);
     }
 }
 
-void mcps_req_ext(const mac_api_t *api, const mcps_data_req_t *data, const mcps_data_req_ie_list_t *ie_ext, const channel_list_s *asynch_channel_list)
+static void mcps_req_ext(const mac_api_t *api, const mcps_data_req_t *data, const mcps_data_req_ie_list_t *ie_ext, const channel_list_s *asynch_channel_list, mac_data_priority_t priority)
 {
 //TODO: Populate linked list when present
     if (mac_store.mac_api == api) {
-        mcps_sap_data_req_handler_ext(mac_store.setup, data, ie_ext, asynch_channel_list);
+        mcps_sap_data_req_handler_ext(mac_store.setup, data, ie_ext, asynch_channel_list, priority);
     }
 }
 
diff --git a/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol.h b/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol.h
index 81d44631da..0ca9c30a65 100644
--- a/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol.h
+++ b/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol.h
@@ -448,6 +448,8 @@ struct protocol_interface_info_entry {
     br_info_t *border_router_setup;
     struct load_balance_api *lb_api;
     struct red_info_s *random_early_detection;
+    struct red_info_s *llc_random_early_detection;
+    struct red_info_s *llc_eapol_random_early_detection;
     neigh_cache_s neigh_cache;
     pan_blaclist_cache_s pan_blaclist_cache;
     pan_coordinator_blaclist_cache_s pan_cordinator_black_list;
@@ -472,6 +474,7 @@ struct protocol_interface_info_entry {
     int8_t (*if_down)(struct protocol_interface_info_entry *cur);
     int8_t (*if_up)(struct protocol_interface_info_entry *cur);
     void (*if_stack_buffer_handler)(buffer_t *);
+    void (*if_common_forwarding_out_cb)(struct protocol_interface_info_entry *, buffer_t *);
     bool (*if_ns_transmit)(struct protocol_interface_info_entry *cur, ipv6_neighbour_t *neighCacheEntry, bool unicast, uint8_t seq);
     bool (*if_map_ip_to_link_addr)(struct protocol_interface_info_entry *cur, const uint8_t *ip_addr, addrtype_t *ll_type, const uint8_t **ll_addr_out);
     bool (*if_map_link_addr_to_ip)(struct protocol_interface_info_entry *cur, addrtype_t ll_type, const uint8_t *ll_addr, uint8_t *ip_addr_out);
diff --git a/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/protocol_core.c b/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/protocol_core.c
index 168293a4b7..45f0c41384 100644
--- a/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/protocol_core.c
+++ b/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/protocol_core.c
@@ -265,6 +265,7 @@ void core_timer_event_handle(uint16_t ticksUpdate)
                         cur->nwk_wpan_nvm_api->nvm_params_update_cb(cur->nwk_wpan_nvm_api, false);
                     }
                     etx_cache_timer(cur->id, seconds);
+                    lowpan_adaptation_interface_slow_timer(cur);
                 }
             } else if (cur->nwk_id == IF_IPV6) {
                 //Slow Pointer Update
@@ -461,6 +462,7 @@ static void protocol_core_base_init(protocol_interface_info_entry_t *entry, nwk_
     entry->ipv6_configure = NULL;
     entry->if_lowpan_security_params = NULL;
     entry->if_ns_transmit = NULL;
+    entry->if_common_forwarding_out_cb = NULL;
     entry->if_special_forwarding = NULL;
     entry->if_snoop = NULL;
     entry->if_icmp_handler = NULL;
diff --git a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.c b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.c
index 7c51210cee..522d85107a 100644
--- a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.c
+++ b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.c
@@ -307,6 +307,25 @@ bool rpl_control_probe_parent_candidate(protocol_interface_info_entry_t *interfa
     return false;
 }
 
+uint16_t rpl_control_neighbor_info_get(struct protocol_interface_info_entry *interface, const uint8_t ll_addr[16], uint8_t *global_address)
+{
+
+    if (!interface->rpl_domain) {
+        return 0xffff;
+    }
+    ns_list_foreach(struct rpl_instance, instance, &interface->rpl_domain->instances) {
+        rpl_neighbour_t *neighbour = rpl_lookup_neighbour_by_ll_address(instance, ll_addr, interface->id);
+        if (neighbour) {
+            const uint8_t *global_address_ptr = rpl_neighbour_global_address(neighbour);
+            if (global_address && global_address_ptr) {
+                memcpy(global_address, global_address_ptr, 16);
+            }
+            return rpl_instance_candidate_rank(neighbour);
+        }
+    }
+    return 0xffff;
+}
+
 bool rpl_possible_better_candidate(struct protocol_interface_info_entry *interface, rpl_instance_t *rpl_instance, const uint8_t ll_addr[16], uint16_t candidate_rank, uint16_t etx)
 {
     if (!interface->rpl_domain) {
diff --git a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.h b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.h
index e0cd4ef09f..e13f552a09 100644
--- a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.h
+++ b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.h
@@ -170,6 +170,7 @@ void rpl_control_unpublish_address(rpl_domain_t *domain, const uint8_t addr[16])
 bool rpl_control_is_dodag_parent(struct protocol_interface_info_entry *interface, const uint8_t ll_addr[16]);
 bool rpl_control_is_dodag_parent_candidate(struct protocol_interface_info_entry *interface, const uint8_t ll_addr[16], uint16_t candidate_cmp_limiter);
 bool rpl_control_probe_parent_candidate(struct protocol_interface_info_entry *interface, const uint8_t ll_addr[16]);
+uint16_t rpl_control_neighbor_info_get(struct protocol_interface_info_entry *interface, const uint8_t ll_addr[16], uint8_t *global_address);
 bool rpl_possible_better_candidate(struct protocol_interface_info_entry *interface, struct rpl_instance *rpl_instance, const uint8_t ll_addr[16], uint16_t candidate_rank, uint16_t etx);
 uint16_t rpl_control_parent_candidate_list_size(struct protocol_interface_info_entry *interface, bool parent_list);
 uint16_t rpl_control_candidate_list_size(struct protocol_interface_info_entry *interface, struct rpl_instance *rpl_instance);
diff --git a/features/nanostack/sal-stack-nanostack/source/Security/kmp/kmp_api.c b/features/nanostack/sal-stack-nanostack/source/Security/kmp/kmp_api.c
index 19be4a7f7a..296f76362d 100644
--- a/features/nanostack/sal-stack-nanostack/source/Security/kmp/kmp_api.c
+++ b/features/nanostack/sal-stack-nanostack/source/Security/kmp/kmp_api.c
@@ -100,7 +100,7 @@ static uint8_t kmp_instance_identifier = 0;
 static kmp_msg_if_entry_t *kmp_api_msg_if_get(kmp_service_t *service, uint8_t msg_if_instance_id);
 static void kmp_api_sec_prot_create_confirm(sec_prot_t *prot, sec_prot_result_e result);
 static void kmp_api_sec_prot_create_indication(sec_prot_t *prot);
-static void kmp_api_sec_prot_finished_indication(sec_prot_t *prot, sec_prot_result_e result, sec_prot_keys_t *sec_keys);
+static bool kmp_api_sec_prot_finished_indication(sec_prot_t *prot, sec_prot_result_e result, sec_prot_keys_t *sec_keys);
 static void kmp_api_sec_prot_finished(sec_prot_t *prot);
 static int8_t kmp_sec_prot_send(sec_prot_t *prot, void *pdu, uint16_t size);
 static int8_t kmp_sec_prot_conn_send(sec_prot_t *prot, void *pdu, uint16_t size, uint8_t conn_number, uint8_t flags);
@@ -234,10 +234,10 @@ static void kmp_api_sec_prot_create_indication(sec_prot_t *prot)
     kmp->create_ind((kmp_api_t *)kmp, kmp->type, kmp->addr);
 }
 
-static void kmp_api_sec_prot_finished_indication(sec_prot_t *prot, sec_prot_result_e result, sec_prot_keys_t *sec_keys)
+static bool kmp_api_sec_prot_finished_indication(sec_prot_t *prot, sec_prot_result_e result, sec_prot_keys_t *sec_keys)
 {
     kmp_api_t *kmp = kmp_api_get_from_prot(prot);
-    kmp->finished_ind((kmp_api_t *)kmp, (kmp_result_e) result, sec_keys);
+    return kmp->finished_ind((kmp_api_t *)kmp, (kmp_result_e) result, sec_keys);
 }
 
 static void kmp_api_sec_prot_finished(sec_prot_t *prot)
@@ -598,8 +598,7 @@ int8_t kmp_service_msg_if_receive(kmp_service_t *service, uint8_t instance_id, k
     int8_t ret = -1;
     if (kmp->sec_prot.receive != NULL) {
         ret = kmp->sec_prot.receive(&kmp->sec_prot, pdu, size);
-    }
-    if (kmp->sec_prot.conn_receive != NULL) {
+    } else if (kmp->sec_prot.conn_receive != NULL) {
         ret = kmp->sec_prot.conn_receive(&kmp->sec_prot, pdu, size, connection_num);
     }
 
diff --git a/features/nanostack/sal-stack-nanostack/source/Security/kmp/kmp_api.h b/features/nanostack/sal-stack-nanostack/source/Security/kmp/kmp_api.h
index 2dea6d6333..3c880de292 100644
--- a/features/nanostack/sal-stack-nanostack/source/Security/kmp/kmp_api.h
+++ b/features/nanostack/sal-stack-nanostack/source/Security/kmp/kmp_api.h
@@ -115,8 +115,10 @@ void kmp_api_create_response(kmp_api_t *kmp, kmp_result_e result);
  * \param result ok or fail
  * \param sec_keys security keys
  *
+ * \return TRUE authentication has been completed, FALSE authentication continues
+ *
  */
-typedef void kmp_api_finished_indication(kmp_api_t *kmp, kmp_result_e result, kmp_sec_keys_t *sec_keys);
+typedef bool kmp_api_finished_indication(kmp_api_t *kmp, kmp_result_e result, kmp_sec_keys_t *sec_keys);
 
 /**
  * kmp_api_finished will be called when KMP has finished and is ready for delete
diff --git a/features/nanostack/sal-stack-nanostack/source/Security/protocols/eap_tls_sec_prot/auth_eap_tls_sec_prot.c b/features/nanostack/sal-stack-nanostack/source/Security/protocols/eap_tls_sec_prot/auth_eap_tls_sec_prot.c
index 84d3c013fa..122cfe4e02 100644
--- a/features/nanostack/sal-stack-nanostack/source/Security/protocols/eap_tls_sec_prot/auth_eap_tls_sec_prot.c
+++ b/features/nanostack/sal-stack-nanostack/source/Security/protocols/eap_tls_sec_prot/auth_eap_tls_sec_prot.c
@@ -89,7 +89,7 @@ static int8_t auth_eap_tls_sec_prot_receive(sec_prot_t *prot, void *pdu, uint16_
 static void auth_eap_tls_sec_prot_state_machine(sec_prot_t *prot);
 
 static int8_t auth_eap_tls_sec_prot_message_handle(sec_prot_t *prot);
-static int8_t auth_eap_tls_sec_prot_message_send(sec_prot_t *prot, uint8_t eap_code, uint8_t eap_type, uint8_t tls_state);
+static int8_t auth_eap_tls_sec_prot_message_send(sec_prot_t *prot, uint8_t eap_code, uint8_t eap_type, uint8_t tls_state, bool retry);
 
 static void auth_eap_tls_sec_prot_timer_timeout(sec_prot_t *prot, uint16_t ticks);
 static int8_t auth_eap_tls_sec_prot_init_tls(sec_prot_t *prot);
@@ -247,7 +247,7 @@ static int8_t auth_eap_tls_sec_prot_message_handle(sec_prot_t *prot)
     return eap_tls_sec_prot_lib_message_handle(data_ptr, length, new_seq_id, &data->tls_send, &data->tls_recv);
 }
 
-static int8_t auth_eap_tls_sec_prot_message_send(sec_prot_t *prot, uint8_t eap_code, uint8_t eap_type, uint8_t tls_state)
+static int8_t auth_eap_tls_sec_prot_message_send(sec_prot_t *prot, uint8_t eap_code, uint8_t eap_type, uint8_t tls_state, bool retry)
 {
     eap_tls_sec_prot_int_t *data = eap_tls_sec_prot_get(prot);
 
@@ -275,9 +275,9 @@ static int8_t auth_eap_tls_sec_prot_message_send(sec_prot_t *prot, uint8_t eap_c
         return -1;
     }
 
-    tr_info("EAP-TLS: send %s type %s id %i flags %x len %i, eui-64: %s", eap_msg_trace[eap_code - 1],
-            eap_type == EAP_IDENTITY ? "IDENTITY" : "TLS", data->eap_id_seq, flags, eapol_pdu_size,
-            trace_array(sec_prot_remote_eui_64_addr_get(prot), 8));
+    tr_info("EAP-TLS: %s %s type %s id %i flags %x len %i, eui-64: %s", retry ? "retry" : "send",
+            eap_msg_trace[eap_code - 1], eap_type == EAP_IDENTITY ? "IDENTITY" : "TLS",
+            data->eap_id_seq, flags, eapol_pdu_size, trace_array(sec_prot_remote_eui_64_addr_get(prot), 8));
 
     if (prot->send(prot, eapol_decoded_data, eapol_pdu_size + prot->header_size) < 0) {
         return -1;
@@ -305,13 +305,13 @@ static void auth_eap_tls_sec_prot_tls_create_indication(sec_prot_t *tls_prot)
     tls_prot->create_resp(tls_prot, SEC_RESULT_OK);
 }
 
-static void auth_eap_tls_sec_prot_tls_finished_indication(sec_prot_t *tls_prot, sec_prot_result_e result, sec_prot_keys_t *sec_keys)
+static bool auth_eap_tls_sec_prot_tls_finished_indication(sec_prot_t *tls_prot, sec_prot_result_e result, sec_prot_keys_t *sec_keys)
 {
     (void) sec_keys;
 
     sec_prot_t *prot = tls_prot->type_get(tls_prot, SEC_PROT_TYPE_EAP_TLS);
     if (!prot) {
-        return;
+        return false;
     }
 
     eap_tls_sec_prot_int_t *data = eap_tls_sec_prot_get(prot);
@@ -333,6 +333,8 @@ static void auth_eap_tls_sec_prot_tls_finished_indication(sec_prot_t *tls_prot,
         // On fatal error terminate right away
         prot->state_machine_call(prot);
     }
+
+    return false;
 }
 
 static int8_t auth_eap_tls_sec_prot_tls_send(sec_prot_t *tls_prot, void *pdu, uint16_t size)
@@ -418,7 +420,7 @@ static void auth_eap_tls_sec_prot_state_machine(sec_prot_t *prot)
             auth_eap_tls_sec_prot_seq_id_update(prot);
 
             // Sends EAP request, Identity
-            auth_eap_tls_sec_prot_message_send(prot, EAP_REQ, EAP_IDENTITY, EAP_TLS_EXCHANGE_NONE);
+            auth_eap_tls_sec_prot_message_send(prot, EAP_REQ, EAP_IDENTITY, EAP_TLS_EXCHANGE_NONE, false);
 
             // Start trickle timer to re-send if no response
             sec_prot_timer_trickle_start(&data->common, &prot->sec_cfg->prot_cfg.sec_prot_trickle_params);
@@ -432,7 +434,7 @@ static void auth_eap_tls_sec_prot_state_machine(sec_prot_t *prot)
             // On timeout
             if (sec_prot_result_timeout_check(&data->common)) {
                 // Re-sends EAP request, Identity
-                auth_eap_tls_sec_prot_message_send(prot, EAP_REQ, EAP_IDENTITY, EAP_TLS_EXCHANGE_NONE);
+                auth_eap_tls_sec_prot_message_send(prot, EAP_REQ, EAP_IDENTITY, EAP_TLS_EXCHANGE_NONE, true);
                 return;
             }
 
@@ -442,7 +444,7 @@ static void auth_eap_tls_sec_prot_state_machine(sec_prot_t *prot)
             }
 
             // Sends EAP request, TLS EAP start
-            auth_eap_tls_sec_prot_message_send(prot, EAP_REQ, EAP_TLS, EAP_TLS_EXCHANGE_START);
+            auth_eap_tls_sec_prot_message_send(prot, EAP_REQ, EAP_TLS, EAP_TLS_EXCHANGE_START, false);
 
             // Start trickle timer to re-send if no response
             sec_prot_timer_trickle_start(&data->common, &prot->sec_cfg->prot_cfg.sec_prot_trickle_params);
@@ -456,12 +458,13 @@ static void auth_eap_tls_sec_prot_state_machine(sec_prot_t *prot)
 
             // On timeout
             if (sec_prot_result_timeout_check(&data->common)) {
+
                 if (sec_prot_state_get(&data->common) == EAP_TLS_STATE_RESPONSE_START) {
                     // Re-sends EAP request, TLS EAP start
-                    auth_eap_tls_sec_prot_message_send(prot, EAP_REQ, EAP_TLS, EAP_TLS_EXCHANGE_START);
+                    auth_eap_tls_sec_prot_message_send(prot, EAP_REQ, EAP_TLS, EAP_TLS_EXCHANGE_START, true);
                 } else {
                     // Re-sends EAP request, TLS EAP
-                    auth_eap_tls_sec_prot_message_send(prot, EAP_REQ, EAP_TLS, EAP_TLS_EXCHANGE_ONGOING);
+                    auth_eap_tls_sec_prot_message_send(prot, EAP_REQ, EAP_TLS, EAP_TLS_EXCHANGE_ONGOING, true);
                 }
                 return;
             }
@@ -475,7 +478,7 @@ static void auth_eap_tls_sec_prot_state_machine(sec_prot_t *prot)
                 }
                 if (result == EAP_TLS_MSG_IDENTITY) {
                     // If received EAP response, Identity: re-sends EAP request, TLS EAP start
-                    auth_eap_tls_sec_prot_message_send(prot, EAP_REQ, EAP_TLS, EAP_TLS_EXCHANGE_START);
+                    auth_eap_tls_sec_prot_message_send(prot, EAP_REQ, EAP_TLS, EAP_TLS_EXCHANGE_START, true);
                     return;
                 }
 
@@ -524,7 +527,7 @@ static void auth_eap_tls_sec_prot_state_machine(sec_prot_t *prot)
                 data->send_pending = false;
 
                 // Sends EAP request, TLS EAP, TLS exchange
-                auth_eap_tls_sec_prot_message_send(prot, EAP_REQ, EAP_TLS, EAP_TLS_EXCHANGE_ONGOING);
+                auth_eap_tls_sec_prot_message_send(prot, EAP_REQ, EAP_TLS, EAP_TLS_EXCHANGE_ONGOING, false);
 
                 // Start trickle timer to re-send if no response
                 sec_prot_timer_trickle_start(&data->common, &prot->sec_cfg->prot_cfg.sec_prot_trickle_params);
@@ -534,10 +537,10 @@ static void auth_eap_tls_sec_prot_state_machine(sec_prot_t *prot)
                     // Supplicant PMK is now valid
                     sec_prot_keys_pmk_mismatch_reset(prot->sec_keys);
                     // Sends EAP success
-                    auth_eap_tls_sec_prot_message_send(prot, EAP_SUCCESS, 0, EAP_TLS_EXCHANGE_NONE);
+                    auth_eap_tls_sec_prot_message_send(prot, EAP_SUCCESS, 0, EAP_TLS_EXCHANGE_NONE, false);
                 } else {
                     // Sends EAP failure
-                    auth_eap_tls_sec_prot_message_send(prot, EAP_FAILURE, 0, EAP_TLS_EXCHANGE_NONE);
+                    auth_eap_tls_sec_prot_message_send(prot, EAP_FAILURE, 0, EAP_TLS_EXCHANGE_NONE, false);
                     sec_prot_result_set(&data->common, SEC_RESULT_ERROR);
                 }
 
diff --git a/features/nanostack/sal-stack-nanostack/source/Security/protocols/eap_tls_sec_prot/supp_eap_tls_sec_prot.c b/features/nanostack/sal-stack-nanostack/source/Security/protocols/eap_tls_sec_prot/supp_eap_tls_sec_prot.c
index 8e0952c162..3384716879 100644
--- a/features/nanostack/sal-stack-nanostack/source/Security/protocols/eap_tls_sec_prot/supp_eap_tls_sec_prot.c
+++ b/features/nanostack/sal-stack-nanostack/source/Security/protocols/eap_tls_sec_prot/supp_eap_tls_sec_prot.c
@@ -295,13 +295,13 @@ static void supp_eap_tls_sec_prot_tls_create_confirm(sec_prot_t *tls_prot, sec_p
     }
 }
 
-static void supp_eap_tls_sec_prot_tls_finished_indication(sec_prot_t *tls_prot, sec_prot_result_e result, sec_prot_keys_t *sec_keys)
+static bool supp_eap_tls_sec_prot_tls_finished_indication(sec_prot_t *tls_prot, sec_prot_result_e result, sec_prot_keys_t *sec_keys)
 {
     (void) sec_keys;
 
     sec_prot_t *prot = tls_prot->type_get(tls_prot, SEC_PROT_TYPE_EAP_TLS);
     if (!prot) {
-        return;
+        return false;
     }
 
     eap_tls_sec_prot_int_t *data = eap_tls_sec_prot_get(prot);
@@ -324,6 +324,8 @@ static void supp_eap_tls_sec_prot_tls_finished_indication(sec_prot_t *tls_prot,
         // On fatal error and on success calls state machine to sent empty EAP-TLS message
         prot->state_machine_call(prot);
     }
+
+    return false;
 }
 
 static int8_t supp_eap_tls_sec_prot_tls_send(sec_prot_t *tls_prot, void *pdu, uint16_t size)
diff --git a/features/nanostack/sal-stack-nanostack/source/Security/protocols/fwh_sec_prot/auth_fwh_sec_prot.c b/features/nanostack/sal-stack-nanostack/source/Security/protocols/fwh_sec_prot/auth_fwh_sec_prot.c
index 1162eb9c35..ec89a9a09e 100644
--- a/features/nanostack/sal-stack-nanostack/source/Security/protocols/fwh_sec_prot/auth_fwh_sec_prot.c
+++ b/features/nanostack/sal-stack-nanostack/source/Security/protocols/fwh_sec_prot/auth_fwh_sec_prot.c
@@ -80,7 +80,8 @@ static int8_t auth_fwh_sec_prot_receive(sec_prot_t *prot, void *pdu, uint16_t si
 static fwh_sec_prot_msg_e auth_fwh_sec_prot_message_get(eapol_pdu_t *eapol_pdu, sec_prot_keys_t *sec_keys);
 static void auth_fwh_sec_prot_state_machine(sec_prot_t *prot);
 
-static int8_t auth_fwh_sec_prot_message_send(sec_prot_t *prot, fwh_sec_prot_msg_e msg);
+static int8_t auth_fwh_sec_prot_message_send(sec_prot_t *prot, fwh_sec_prot_msg_e msg, bool retry);
+static int8_t auth_fwh_sec_prot_auth_completed_send(sec_prot_t *prot);
 static void auth_fwh_sec_prot_timer_timeout(sec_prot_t *prot, uint16_t ticks);
 
 static int8_t auth_fwh_sec_prot_ptk_generate(sec_prot_t *prot, sec_prot_keys_t *sec_keys);
@@ -204,7 +205,7 @@ static fwh_sec_prot_msg_e auth_fwh_sec_prot_message_get(eapol_pdu_t *eapol_pdu,
     return msg;
 }
 
-static int8_t auth_fwh_sec_prot_message_send(sec_prot_t *prot, fwh_sec_prot_msg_e msg)
+static int8_t auth_fwh_sec_prot_message_send(sec_prot_t *prot, fwh_sec_prot_msg_e msg, bool retry)
 {
     fwh_sec_prot_int_t *data = fwh_sec_prot_get(prot);
 
@@ -301,7 +302,9 @@ static int8_t auth_fwh_sec_prot_message_send(sec_prot_t *prot, fwh_sec_prot_msg_
         return -1;
     }
 
-    tr_info("4WH: send %s, eui-64: %s", msg == FWH_MESSAGE_1 ? "Message 1" : "Message 3", trace_array(sec_prot_remote_eui_64_addr_get(prot), 8));
+    tr_info("4WH: %s %s, eui-64: %s", retry ? "retry" : "send",
+            msg == FWH_MESSAGE_1 ? "Message 1" : "Message 3",
+            trace_array(sec_prot_remote_eui_64_addr_get(prot), 8));
 
     if (prot->send(prot, eapol_pdu_frame, eapol_pdu_size + prot->header_size) < 0) {
         return -1;
@@ -310,6 +313,18 @@ static int8_t auth_fwh_sec_prot_message_send(sec_prot_t *prot, fwh_sec_prot_msg_
     return 0;
 }
 
+static int8_t auth_fwh_sec_prot_auth_completed_send(sec_prot_t *prot)
+{
+    uint8_t *eapol_pdu_frame = ns_dyn_mem_temporary_alloc(prot->header_size);
+
+    // Send zero length message to relay which requests LLC to remove EAPOL temporary entry based on EUI-64
+    if (prot->send(prot, eapol_pdu_frame, prot->header_size) < 0) {
+        return -1;
+    }
+
+    return 0;
+}
+
 static void auth_fwh_sec_prot_timer_timeout(sec_prot_t *prot, uint16_t ticks)
 {
     fwh_sec_prot_int_t *data = fwh_sec_prot_get(prot);
@@ -347,7 +362,7 @@ static void auth_fwh_sec_prot_state_machine(sec_prot_t *prot)
 
             // Sends 4WH Message 1
             sec_prot_lib_nonce_generate(data->nonce);
-            auth_fwh_sec_prot_message_send(prot, FWH_MESSAGE_1);
+            auth_fwh_sec_prot_message_send(prot, FWH_MESSAGE_1, false);
 
             // Start trickle timer to re-send if no response
             sec_prot_timer_trickle_start(&data->common, &prot->sec_cfg->prot_cfg.sec_prot_trickle_params);
@@ -360,7 +375,7 @@ static void auth_fwh_sec_prot_state_machine(sec_prot_t *prot)
             if (sec_prot_result_timeout_check(&data->common)) {
                 // Re-sends 4WH Message 1
                 sec_prot_lib_nonce_generate(data->nonce);
-                auth_fwh_sec_prot_message_send(prot, FWH_MESSAGE_1);
+                auth_fwh_sec_prot_message_send(prot, FWH_MESSAGE_1, true);
             } else {
                 if (data->recv_msg != FWH_MESSAGE_2) {
                     return;
@@ -375,7 +390,7 @@ static void auth_fwh_sec_prot_state_machine(sec_prot_t *prot)
                 }
 
                 // Sends 4WH Message 3
-                auth_fwh_sec_prot_message_send(prot, FWH_MESSAGE_3);
+                auth_fwh_sec_prot_message_send(prot, FWH_MESSAGE_3, false);
 
                 // Start trickle timer to re-send if no response
                 sec_prot_timer_trickle_start(&data->common, &prot->sec_cfg->prot_cfg.sec_prot_trickle_params);
@@ -388,7 +403,7 @@ static void auth_fwh_sec_prot_state_machine(sec_prot_t *prot)
         case FWH_STATE_MESSAGE_4:
             if (sec_prot_result_timeout_check(&data->common)) {
                 // Re-sends 4WH Message 3
-                auth_fwh_sec_prot_message_send(prot, FWH_MESSAGE_3);
+                auth_fwh_sec_prot_message_send(prot, FWH_MESSAGE_3, true);
             } else {
                 if (data->recv_msg != FWH_MESSAGE_4) {
                     return;
@@ -416,7 +431,11 @@ static void auth_fwh_sec_prot_state_machine(sec_prot_t *prot)
             tr_info("4WH: finish, eui-64: %s", trace_array(sec_prot_remote_eui_64_addr_get(prot), 8));
 
             // KMP-FINISHED.indication,
-            prot->finished_ind(prot, sec_prot_result_get(&data->common), 0);
+            if (prot->finished_ind(prot, sec_prot_result_get(&data->common), 0)) {
+                // Authentication completed (all GTKs inserted)
+                auth_fwh_sec_prot_auth_completed_send(prot);
+            }
+
             sec_prot_state_set(prot, &data->common, FWH_STATE_FINISHED);
             break;
 
diff --git a/features/nanostack/sal-stack-nanostack/source/Security/protocols/gkh_sec_prot/auth_gkh_sec_prot.c b/features/nanostack/sal-stack-nanostack/source/Security/protocols/gkh_sec_prot/auth_gkh_sec_prot.c
index d1101d2292..4424b55a2c 100644
--- a/features/nanostack/sal-stack-nanostack/source/Security/protocols/gkh_sec_prot/auth_gkh_sec_prot.c
+++ b/features/nanostack/sal-stack-nanostack/source/Security/protocols/gkh_sec_prot/auth_gkh_sec_prot.c
@@ -71,7 +71,8 @@ static int8_t auth_gkh_sec_prot_receive(sec_prot_t *prot, void *pdu, uint16_t si
 static gkh_sec_prot_msg_e auth_gkh_sec_prot_message_get(eapol_pdu_t *eapol_pdu, sec_prot_keys_t *sec_keys);
 static void auth_gkh_sec_prot_state_machine(sec_prot_t *prot);
 
-static int8_t auth_gkh_sec_prot_message_send(sec_prot_t *prot, gkh_sec_prot_msg_e msg);
+static int8_t auth_gkh_sec_prot_message_send(sec_prot_t *prot, gkh_sec_prot_msg_e msg, bool retry);
+static int8_t auth_gkh_sec_prot_auth_completed_send(sec_prot_t *prot);
 static void auth_gkh_sec_prot_timer_timeout(sec_prot_t *prot, uint16_t ticks);
 static int8_t auth_gkh_sec_prot_mic_validate(sec_prot_t *prot);
 
@@ -180,7 +181,7 @@ static gkh_sec_prot_msg_e auth_gkh_sec_prot_message_get(eapol_pdu_t *eapol_pdu,
     return msg;
 }
 
-static int8_t auth_gkh_sec_prot_message_send(sec_prot_t *prot, gkh_sec_prot_msg_e msg)
+static int8_t auth_gkh_sec_prot_message_send(sec_prot_t *prot, gkh_sec_prot_msg_e msg, bool retry)
 {
     uint16_t kde_len = 0;
 
@@ -249,7 +250,8 @@ static int8_t auth_gkh_sec_prot_message_send(sec_prot_t *prot, gkh_sec_prot_msg_
         return -1;
     }
 
-    tr_info("GKH: send Message 1, eui-64: %s", trace_array(sec_prot_remote_eui_64_addr_get(prot), 8));
+    tr_info("GKH: %s Message 1, eui-64: %s", retry ? "retry" : "send",
+            trace_array(sec_prot_remote_eui_64_addr_get(prot), 8));
 
     if (prot->send(prot, eapol_pdu_frame, eapol_pdu_size + prot->header_size) < 0) {
         return -1;
@@ -258,6 +260,18 @@ static int8_t auth_gkh_sec_prot_message_send(sec_prot_t *prot, gkh_sec_prot_msg_
     return 0;
 }
 
+static int8_t auth_gkh_sec_prot_auth_completed_send(sec_prot_t *prot)
+{
+    uint8_t *eapol_pdu_frame = ns_dyn_mem_temporary_alloc(prot->header_size);
+
+    // Send zero length message to relay which requests LLC to remove EAPOL temporary entry based on EUI-64
+    if (prot->send(prot, eapol_pdu_frame, prot->header_size) < 0) {
+        return -1;
+    }
+
+    return 0;
+}
+
 static void auth_gkh_sec_prot_timer_timeout(sec_prot_t *prot, uint16_t ticks)
 {
     gkh_sec_prot_int_t *data = gkh_sec_prot_get(prot);
@@ -287,7 +301,7 @@ static void auth_gkh_sec_prot_state_machine(sec_prot_t *prot)
             prot->create_conf(prot, SEC_RESULT_OK);
 
             // Sends GKH Message 1
-            auth_gkh_sec_prot_message_send(prot, GKH_MESSAGE_1);
+            auth_gkh_sec_prot_message_send(prot, GKH_MESSAGE_1, false);
 
             // Start trickle timer to re-send if no response
             sec_prot_timer_trickle_start(&data->common, &prot->sec_cfg->prot_cfg.sec_prot_trickle_params);
@@ -303,7 +317,7 @@ static void auth_gkh_sec_prot_state_machine(sec_prot_t *prot)
 
             if (sec_prot_result_timeout_check(&data->common)) {
                 // Re-sends GKH Message 1
-                auth_gkh_sec_prot_message_send(prot, GKH_MESSAGE_1);
+                auth_gkh_sec_prot_message_send(prot, GKH_MESSAGE_1, true);
             } else {
                 if (auth_gkh_sec_prot_mic_validate(prot) < 0) {
                     return;
@@ -319,7 +333,10 @@ static void auth_gkh_sec_prot_state_machine(sec_prot_t *prot)
             tr_info("GKH finish, eui-64: %s", trace_array(sec_prot_remote_eui_64_addr_get(prot), 8));
 
             // KMP-FINISHED.indication,
-            prot->finished_ind(prot, sec_prot_result_get(&data->common), 0);
+            if (prot->finished_ind(prot, sec_prot_result_get(&data->common), 0)) {
+                // Authentication completed (all GTKs inserted)
+                auth_gkh_sec_prot_auth_completed_send(prot);
+            }
 
             sec_prot_state_set(prot, &data->common, GKH_STATE_FINISHED);
             break;
diff --git a/features/nanostack/sal-stack-nanostack/source/Security/protocols/sec_prot.h b/features/nanostack/sal-stack-nanostack/source/Security/protocols/sec_prot.h
index cf7aa54101..ac283a00c1 100644
--- a/features/nanostack/sal-stack-nanostack/source/Security/protocols/sec_prot.h
+++ b/features/nanostack/sal-stack-nanostack/source/Security/protocols/sec_prot.h
@@ -106,8 +106,10 @@ typedef void sec_prot_create_indication(sec_prot_t *prot);
  * \param result result
  * \param sec_keys security keys
  *
+ * \return TRUE authentication has been completed, FALSE authentication continues
+ *
  */
-typedef void sec_prot_finished_indication(sec_prot_t *prot, sec_prot_result_e result, sec_prot_keys_t *sec_keys);
+typedef bool sec_prot_finished_indication(sec_prot_t *prot, sec_prot_result_e result, sec_prot_keys_t *sec_keys);
 
 /**
  * sec_prot_finished Security protocol has finished and is ready for delete
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_configuration_interface.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_configuration_interface.c
index 5ac0f81a4e..df247c80b5 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_configuration_interface.c
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_configuration_interface.c
@@ -140,6 +140,15 @@ int ns_fhss_ws_set_hop_count(const fhss_api_t *fhss_api, const uint8_t hop_count
     return fhss_ws_set_hop_count(fhss_structure, hop_count);
 }
 
+int ns_fhss_ws_set_tx_allowance_level(const fhss_api_t *fhss_api, const fhss_ws_tx_allow_level global_level, const fhss_ws_tx_allow_level ef_level)
+{
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(fhss_api);
+    if (!fhss_structure || !fhss_structure->ws) {
+        return -1;
+    }
+    return fhss_ws_set_tx_allowance_level(fhss_structure, global_level, ef_level);
+}
+
 int ns_fhss_statistics_start(const fhss_api_t *fhss_api, fhss_statistics_t *fhss_statistics)
 {
     fhss_structure_t *fhss_structure = fhss_get_object_with_api(fhss_api);
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws.c
index de2a3385ec..6c4e62e0d9 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws.c
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws.c
@@ -84,6 +84,8 @@ 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);
 static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay);
 static bool fhss_ws_check_tx_allowed(fhss_structure_t *fhss_structure);
+static bool fhss_allow_transmitting_on_rx_slot(fhss_structure_t *fhss_structure);
+static bool fhss_allow_unicast_on_broadcast_channel(fhss_structure_t *fhss_structure);
 static int32_t fhss_channel_index_from_mask(const uint32_t *channel_mask, int32_t channel_index, uint16_t number_of_channels);
 
 // This function supports rounding up
@@ -167,6 +169,10 @@ fhss_structure_t *fhss_ws_enable(fhss_api_t *fhss_api, const fhss_ws_configurati
     fhss_ws_set_hop_count(fhss_struct, 0xff);
     fhss_struct->rx_channel = fhss_configuration->unicast_fixed_channel;
     fhss_struct->ws->min_synch_interval = DEFAULT_MIN_SYNCH_INTERVAL;
+    // By default, allow transmitting unicast data only on TX slots.
+    fhss_struct->ws->tx_level = WS_TX_SLOT;
+    // By default, allow always transmitting unicast data in expedited forwarding mode.
+    fhss_struct->ws->ef_tx_level = WS_TX_ALWAYS;
     ns_list_init(&fhss_struct->fhss_failed_tx_list);
     return fhss_struct;
 }
@@ -341,6 +347,7 @@ static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
             fhss_structure->ws->is_on_bc_channel = false;
         }
         delay_us %= MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_broadcast_interval);
+        delay_us %= MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_broadcast_interval - fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval);
     }
 
     if (fhss_structure->ws->fhss_configuration.ws_bc_channel_function == WS_TR51CF) {
@@ -351,16 +358,19 @@ static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
         fhss_ws_start_timer(fhss_structure, MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval) - ((int64_t) delay_us * fhss_structure->platform_functions.fhss_resolution_divider), fhss_broadcast_handler);
         fhss_structure->ws->is_on_bc_channel = true;
         next_channel = fhss_structure->ws->bc_channel = fhss_ws_calc_bc_channel(fhss_structure);
-
-        /* Start timer with random timeout to trigger broadcast TX queue poll event.
-         * Min random is 1/50 of the channel dwell interval.
-         * Max random is 3/4 of the channel dwell interval.
-         * Event timer resolution is 50us.
-         */
-        uint32_t bc_dwell_us = MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval);
-        uint16_t bc_min_random = (bc_dwell_us / 50) / 50;
-        uint16_t bc_max_random = (bc_dwell_us - (bc_dwell_us / 4)) / 50;
-        eventOS_callback_timer_start(fhss_structure->fhss_event_timer, randLIB_get_random_in_range(bc_min_random, bc_max_random));
+        if (fhss_structure->ws->expedited_forwarding_enabled_us) {
+            eventOS_callback_timer_start(fhss_structure->fhss_event_timer, EXPEDITED_FORWARDING_POLL_PERIOD);
+        } else {
+            /* Start timer with random timeout to trigger broadcast TX queue poll event.
+             * Min random is 1/50 of the channel dwell interval.
+             * Max random is 3/4 of the channel dwell interval.
+             * Event timer resolution is 50us.
+             */
+            uint32_t bc_dwell_us = MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval);
+            uint16_t bc_min_random = (bc_dwell_us / 50) / 50;
+            uint16_t bc_max_random = (bc_dwell_us - (bc_dwell_us / 4)) / 50;
+            eventOS_callback_timer_start(fhss_structure->fhss_event_timer, randLIB_get_random_in_range(bc_min_random, bc_max_random));
+        }
     } else {
         fhss_structure->ws->unicast_start_time_us = fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api);
         uint32_t timeout = MS_TO_US(fhss_structure->ws->fhss_configuration.fhss_broadcast_interval - fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval);
@@ -368,27 +378,31 @@ static void fhss_broadcast_handler(const fhss_api_t *fhss_api, uint16_t delay)
         fhss_structure->ws->is_on_bc_channel = false;
         // Should return to own (unicast) listening channel after broadcast channel
         next_channel = fhss_structure->rx_channel;
-        /* Start timer with random timeout to trigger unicast TX queue poll event.
-         * For hops 0,1,4,5,8,9,...
-         * Min random is 1/100 of the TX slot length.
-         * Max random is 1/5 of the TX slot length.
-         *
-         * For hops 2,3,6,7,10,11,...
-         * Min random is 1/100 of the TX slot length plus 0.5*TX slot length.
-         * Max random is 1/10 of the TX slot length plus 0.5*TX slot length.
-         * Event timer resolution is 50us.
-         */
-        // returns 1 if polling of TX queue is done on latter half of the TX slot
-        uint8_t own_tx_trig_slot = calc_own_tx_trig_slot(fhss_structure->own_hop);
-        uint32_t txrx_slot_length_us = MS_TO_US(fhss_structure->ws->txrx_slot_length_ms);
-        uint16_t uc_min_random = (((txrx_slot_length_us / 2) * own_tx_trig_slot) / 50) + ((txrx_slot_length_us / 100) / 50);
-        uint16_t uc_max_random = (((txrx_slot_length_us / 2) * own_tx_trig_slot) / 50) + ((txrx_slot_length_us / 5) / 50);
-        bool tx_allowed = fhss_ws_check_tx_allowed(fhss_structure);
-        if (!tx_allowed) {
-            uc_min_random += (txrx_slot_length_us) / 50;
-            uc_max_random += (txrx_slot_length_us) / 50;
+        if (fhss_structure->ws->expedited_forwarding_enabled_us) {
+            eventOS_callback_timer_start(fhss_structure->fhss_event_timer, EXPEDITED_FORWARDING_POLL_PERIOD);
+        } else {
+            /* Start timer with random timeout to trigger unicast TX queue poll event.
+             * For hops 0,1,4,5,8,9,...
+             * Min random is 1/100 of the TX slot length.
+             * Max random is 1/5 of the TX slot length.
+             *
+             * For hops 2,3,6,7,10,11,...
+             * Min random is 1/100 of the TX slot length plus 0.5*TX slot length.
+             * Max random is 1/10 of the TX slot length plus 0.5*TX slot length.
+             * Event timer resolution is 50us.
+             */
+            // returns 1 if polling of TX queue is done on latter half of the TX slot
+            uint8_t own_tx_trig_slot = calc_own_tx_trig_slot(fhss_structure->own_hop);
+            uint32_t txrx_slot_length_us = MS_TO_US(fhss_structure->ws->txrx_slot_length_ms);
+            uint16_t uc_min_random = (((txrx_slot_length_us / 2) * own_tx_trig_slot) / 50) + ((txrx_slot_length_us / 100) / 50);
+            uint16_t uc_max_random = (((txrx_slot_length_us / 2) * own_tx_trig_slot) / 50) + ((txrx_slot_length_us / 5) / 50);
+            bool tx_allowed = fhss_ws_check_tx_allowed(fhss_structure);
+            if (!tx_allowed) {
+                uc_min_random += (txrx_slot_length_us) / 50;
+                uc_max_random += (txrx_slot_length_us) / 50;
+            }
+            eventOS_callback_timer_start(fhss_structure->fhss_event_timer, randLIB_get_random_in_range(uc_min_random, uc_max_random));
         }
-        eventOS_callback_timer_start(fhss_structure->fhss_event_timer, randLIB_get_random_in_range(uc_min_random, uc_max_random));
 
 #ifdef FHSS_CHANNEL_DEBUG
         tr_info("%"PRIu32" UC %u", fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api), fhss_structure->rx_channel);
@@ -437,19 +451,35 @@ static void fhss_event_timer_cb(int8_t timer_id, uint16_t slots)
         return;
     }
 
-    if (fhss_structure->ws->is_on_bc_channel == true) {
-        queue_size = fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, true);
-    } else {
-        // On unicast, start timer to trigger polling event on next TX slot
-        uint32_t delay_between_tx_slots_us = MS_TO_US(fhss_structure->ws->txrx_slot_length_ms) * 2;
-        // Timer could drift to RX slot when broadcast interval is high. Return timer to TX slot.
-        if (fhss_ws_check_tx_allowed(fhss_structure) == false) {
-            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)));
+    if (fhss_structure->ws->expedited_forwarding_enabled_us) {
+        if ((fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api) - fhss_structure->ws->expedited_forwarding_enabled_us) > S_TO_US(EXPEDITED_FORWARDING_PERIOD)) {
+            fhss_structure->ws->expedited_forwarding_enabled_us = 0;
         }
-        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))) {
-            eventOS_callback_timer_start(fhss_structure->fhss_event_timer, delay_between_tx_slots_us / 50);
+        eventOS_callback_timer_start(fhss_structure->fhss_event_timer, EXPEDITED_FORWARDING_POLL_PERIOD);
+        if (fhss_structure->ws->is_on_bc_channel == true) {
+            queue_size = fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, true);
+        } else {
+            queue_size = fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, false);
         }
+    } else if ((fhss_structure->ws->unicast_timer_running == false) && (fhss_structure->ws->broadcast_timer_running == false)) {
+        // No one would poll TX queue if schedule timers were not started. Start poll timer with default interval.
+        eventOS_callback_timer_start(fhss_structure->fhss_event_timer, DEFAULT_POLL_PERIOD);
         queue_size = fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, false);
+    } else {
+        if (fhss_structure->ws->is_on_bc_channel == true) {
+            queue_size = fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, true);
+        } else {
+            // On unicast, start timer to trigger polling event on next TX slot
+            uint32_t delay_between_tx_slots_us = MS_TO_US(fhss_structure->ws->txrx_slot_length_ms) * 2;
+            // Timer could drift to RX slot when broadcast interval is high. Return timer to TX slot.
+            if (fhss_ws_check_tx_allowed(fhss_structure) == false) {
+                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)));
+            }
+            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))) {
+                eventOS_callback_timer_start(fhss_structure->fhss_event_timer, delay_between_tx_slots_us / 50);
+            }
+            queue_size = fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, false);
+        }
     }
     if (queue_size) {
         fhss_structure->callbacks.tx_poll(fhss_structure->fhss_api);
@@ -528,6 +558,16 @@ static int16_t fhss_ws_synch_state_set_callback(const fhss_api_t *api, fhss_stat
     if (!fhss_structure) {
         return -1;
     }
+    if (fhss_state == FHSS_EXPEDITED_FORWARDING) {
+        if (!fhss_structure->ws->expedited_forwarding_enabled_us) {
+            eventOS_callback_timer_start(fhss_structure->fhss_event_timer, EXPEDITED_FORWARDING_POLL_PERIOD);
+        }
+        fhss_structure->ws->expedited_forwarding_enabled_us = fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api);
+        if (!fhss_structure->ws->expedited_forwarding_enabled_us) {
+            fhss_structure->ws->expedited_forwarding_enabled_us++;
+        }
+        return 0;
+    }
     if (fhss_state == FHSS_SYNCHRONIZED) {
         uint32_t fhss_broadcast_interval = fhss_structure->ws->fhss_configuration.fhss_broadcast_interval;
         uint8_t fhss_bc_dwell_interval = fhss_structure->ws->fhss_configuration.fhss_bc_dwell_interval;
@@ -619,12 +659,15 @@ static int fhss_ws_tx_handle_callback(const fhss_api_t *api, bool is_broadcast_a
     if (is_broadcast_addr && (fhss_structure->ws->is_on_bc_channel == false)) {
         return -3;
     }
-    // Do not allow unicast destination on broadcast channel
+    // Do not allow unicast destination on broadcast channel unless it is specifically enabled
     if (!is_broadcast_addr && (fhss_structure->ws->is_on_bc_channel == true)) {
+        if (fhss_allow_unicast_on_broadcast_channel(fhss_structure)) {
+            return 0;
+        }
         return -3;
     }
     // Check TX/RX slot
-    if (!fhss_ws_check_tx_allowed(fhss_structure)) {
+    if (!fhss_allow_transmitting_on_rx_slot(fhss_structure) && !fhss_ws_check_tx_allowed(fhss_structure)) {
         return -1;
     }
     if (fhss_structure->fhss_state == FHSS_SYNCHRONIZED) {
@@ -739,6 +782,30 @@ static bool fhss_ws_check_tx_time(fhss_structure_t *fhss_structure, uint16_t tx_
 #endif
 }
 
+static bool fhss_allow_transmitting_on_rx_slot(fhss_structure_t *fhss_structure)
+{
+    if (fhss_structure->ws->tx_level >= WS_TX_AND_RX_SLOT) {
+        return true;
+    }
+    // This is allowed only for devices in expedited forwarding mode
+    if (fhss_structure->ws->expedited_forwarding_enabled_us && (fhss_structure->ws->ef_tx_level >= WS_TX_AND_RX_SLOT)) {
+        return true;
+    }
+    return false;
+}
+
+static bool fhss_allow_unicast_on_broadcast_channel(fhss_structure_t *fhss_structure)
+{
+    if (fhss_structure->ws->tx_level >= WS_TX_ALWAYS) {
+        return true;
+    }
+    // This is allowed only for devices in expedited forwarding mode
+    if (fhss_structure->ws->expedited_forwarding_enabled_us && (fhss_structure->ws->ef_tx_level >= WS_TX_ALWAYS)) {
+        return true;
+    }
+    return false;
+}
+
 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)
 {
     (void) frame_type;
@@ -750,8 +817,8 @@ static bool fhss_ws_check_tx_conditions_callback(const fhss_api_t *api, bool is_
     if (is_broadcast_addr && (fhss_structure->ws->is_on_bc_channel == false)) {
         return false;
     }
-    // Do not allow unicast destination on broadcast channel
-    if (!is_broadcast_addr && (fhss_structure->ws->is_on_bc_channel == true)) {
+    // Do not allow unicast destination on broadcast channel unless it is specifically enabled
+    if (!is_broadcast_addr && (fhss_structure->ws->is_on_bc_channel == true) && !fhss_allow_unicast_on_broadcast_channel(fhss_structure)) {
         return false;
     }
     // This condition will check that message is not sent on bad channel
@@ -763,7 +830,7 @@ static bool fhss_ws_check_tx_conditions_callback(const fhss_api_t *api, bool is_
         return false;
     }
     // Check TX/RX slot for unicast frames
-    if (!is_broadcast_addr && !fhss_ws_check_tx_allowed(fhss_structure)) {
+    if (!is_broadcast_addr && !fhss_allow_transmitting_on_rx_slot(fhss_structure) && !fhss_ws_check_tx_allowed(fhss_structure)) {
         return false;
     }
     return true;
@@ -925,6 +992,10 @@ static uint32_t fhss_ws_get_retry_period_callback(const fhss_api_t *api, uint8_t
     if (fhss_structure->ws->is_on_bc_channel == true) {
         return return_value;
     }
+    // No need to wait until TX slot when sending on RX slot is allowed
+    if (fhss_allow_transmitting_on_rx_slot(fhss_structure) == true) {
+        return return_value;
+    }
 
     uint32_t txrx_slot_length_us = MS_TO_US(fhss_structure->ws->txrx_slot_length_ms);
     uint32_t unicast_start_us = fhss_structure->ws->unicast_start_time_us;
@@ -1023,6 +1094,7 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
         return 0;
     }
     platform_enter_critical();
+
     uint16_t own_bc_slot = fhss_structure->ws->bc_slot;
     uint32_t prev_synchronization_time = fhss_structure->ws->synchronization_time;
     fhss_structure->ws->synchronization_time = fhss_structure->callbacks.read_timestamp(fhss_structure->fhss_api);
@@ -1131,6 +1203,11 @@ int fhss_ws_configuration_set(fhss_structure_t *fhss_structure, const fhss_ws_co
         channel_count_uc = channel_count_bc;
     }
 
+    // No one would poll TX queue if schedule timers were not started. Start poll timer with default interval.
+    if ((fhss_structure->ws->unicast_timer_running == false) && (fhss_structure->ws->broadcast_timer_running == false)) {
+        eventOS_callback_timer_start(fhss_structure->fhss_event_timer, DEFAULT_POLL_PERIOD);
+    }
+
     fhss_structure->number_of_channels = fhss_configuration->channel_mask_size;
     fhss_structure->number_of_bc_channels = channel_count_bc;
     fhss_structure->number_of_uc_channels = channel_count_uc;
@@ -1161,4 +1238,11 @@ int fhss_ws_set_hop_count(fhss_structure_t *fhss_structure, const uint8_t hop_co
     return 0;
 }
 
+int fhss_ws_set_tx_allowance_level(fhss_structure_t *fhss_structure, const fhss_ws_tx_allow_level global_level, const fhss_ws_tx_allow_level ef_level)
+{
+    fhss_structure->ws->tx_level = global_level;
+    fhss_structure->ws->ef_tx_level = ef_level;
+    return 0;
+}
+
 #endif // HAVE_WS
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws.h b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws.h
index e9751bb151..6e3b3d5d48 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws.h
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws.h
@@ -18,17 +18,23 @@
 #define FHSS_WS_H_
 
 // TX slot length is optimised to this packet length
-#define OPTIMAL_PACKET_LENGTH   500
+#define OPTIMAL_PACKET_LENGTH               500
 // Max TX/RX slot length in milliseconds. Is used when datarate is not given by PHY or calculated slot length exceeds maximum allowed.
-#define WS_TXRX_SLOT_LEN_MS_MAX         100
+#define WS_TXRX_SLOT_LEN_MS_MAX             100
 // Min TX/RX slot length in milliseconds. Is used when calculated slot length is under minimum allowed.
-#define WS_TXRX_SLOT_LEN_MS_MIN         13
+#define WS_TXRX_SLOT_LEN_MS_MIN             13
 // Default minimum broadcast synchronization interval in seconds
-#define DEFAULT_MIN_SYNCH_INTERVAL      60
+#define DEFAULT_MIN_SYNCH_INTERVAL          60
 // Drift compensation allowed if at least SYNCH_COMPENSATION_MIN_INTERVAL (seconds) since last synchronization
-#define SYNCH_COMPENSATION_MIN_INTERVAL 60
+#define SYNCH_COMPENSATION_MIN_INTERVAL     60
 // MAX compensation per received synchronization info in ns
-#define MAX_DRIFT_COMPENSATION_STEP     10
+#define MAX_DRIFT_COMPENSATION_STEP         10
+// Length of expedited forwarding period in seconds
+#define EXPEDITED_FORWARDING_PERIOD         5
+// TX poll interval for expedited forwarding in 50us slots
+#define EXPEDITED_FORWARDING_POLL_PERIOD    (5000 / 50)
+// TX poll interval used when channel schedules are not yet started (50us slots)
+#define DEFAULT_POLL_PERIOD    (10000 / 50)
 typedef struct fhss_ws fhss_ws_t;
 
 struct fhss_ws {
@@ -44,9 +50,12 @@ struct fhss_ws {
     uint8_t *tr51_output_table;
     uint32_t next_uc_timeout;
     uint32_t next_bc_timeout;
+    uint32_t expedited_forwarding_enabled_us;
     bool unicast_timer_running;
     bool broadcast_timer_running;
     bool is_on_bc_channel;
+    fhss_ws_tx_allow_level tx_level;
+    fhss_ws_tx_allow_level ef_tx_level;
     struct fhss_ws_configuration fhss_configuration;
     const struct broadcast_timing_info *parent_bc_info;
     fhss_get_neighbor_info *get_neighbor_info;
@@ -58,6 +67,7 @@ int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8],
 int fhss_ws_remove_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8]);
 int fhss_ws_configuration_set(fhss_structure_t *fhss_structure, const fhss_ws_configuration_t *fhss_configuration);
 int fhss_ws_set_hop_count(fhss_structure_t *fhss_structure, const uint8_t hop_count);
+int fhss_ws_set_tx_allowance_level(fhss_structure_t *fhss_structure, const fhss_ws_tx_allow_level global_level, const fhss_ws_tx_allow_level ef_level);
 void fhss_set_txrx_slot_length(fhss_structure_t *fhss_structure);
 
 #endif /*FHSS_WS_H_*/
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws_empty_functions.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws_empty_functions.c
index 11896b55c2..4a68275472 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws_empty_functions.c
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws_empty_functions.c
@@ -80,5 +80,14 @@ int fhss_ws_set_hop_count(fhss_structure_t *fhss_structure, const uint8_t hop_co
     return -1;
 }
 
+int fhss_ws_set_tx_allowance_level(fhss_structure_t *fhss_structure, const fhss_ws_tx_allow_level global_level, const fhss_ws_tx_allow_level ef_level)
+{
+    (void) fhss_structure;
+    (void) global_level;
+    (void) ef_level;
+
+    return -1;
+}
+
 #endif // HAVE_WS
 
diff --git a/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.c b/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.c
index e253f0b36f..0cbc0a4065 100644
--- a/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.c
+++ b/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.c
@@ -96,7 +96,7 @@ static NS_LIST_DEFINE(ipv6_routing_table, ipv6_route_t, link);
 static ipv6_destination_t *ipv6_destination_lookup(const uint8_t *address, int8_t interface_id);
 static void ipv6_destination_cache_forget_router(ipv6_neighbour_cache_t *cache, const uint8_t neighbour_addr[16]);
 static void ipv6_destination_cache_forget_neighbour(const ipv6_neighbour_t *neighbour);
-static void ipv6_destination_release(ipv6_destination_t *dest);
+static bool ipv6_destination_release(ipv6_destination_t *dest);
 static void ipv6_route_table_remove_router(int8_t interface_id, const uint8_t *addr, ipv6_route_src_t source);
 static uint16_t total_metric(const ipv6_route_t *route);
 static uint8_t ipv6_route_table_count_source(int8_t interface_id, ipv6_route_src_t source);
@@ -481,6 +481,16 @@ bool ipv6_neighbour_has_registered_by_eui64(ipv6_neighbour_cache_t *cache, const
     return false;
 }
 
+ipv6_neighbour_t *ipv6_neighbour_get_registered_by_eui64(ipv6_neighbour_cache_t *cache, const uint8_t *eui64)
+{
+    ns_list_foreach_safe(ipv6_neighbour_t, cur, &cache->list) {
+        if (cur->type != IP_NEIGHBOUR_GARBAGE_COLLECTIBLE && memcmp(ipv6_neighbour_eui64(cache, cur), eui64, 8) == 0) {
+            return cur;
+        }
+    }
+    return NULL;
+}
+
 void ipv6_neighbour_set_state(ipv6_neighbour_cache_t *cache, ipv6_neighbour_t *entry, ip_neighbour_cache_state_t state)
 {
     if (!ipv6_neighbour_state_is_probably_reachable(entry->state) &&
@@ -863,7 +873,7 @@ void ipv6_destination_cache_print(route_print_fn_t *print_fn)
     print_fn("Destination Cache:");
     ns_list_foreach(ipv6_destination_t, entry, &ipv6_destination_cache) {
         ROUTE_PRINT_ADDR_STR_BUFFER_INIT(addr_str);
-        print_fn(" %s (life %u)", ROUTE_PRINT_ADDR_STR_FORMAT(addr_str, entry->destination), entry->lifetime);
+        print_fn(" %s (%d id) (life %u)", ROUTE_PRINT_ADDR_STR_FORMAT(addr_str, entry->destination), entry->interface_id, entry->lifetime);
 #ifdef HAVE_IPV6_ND
         if (entry->redirected) {
             print_fn("     Redirect %s%%%u", ROUTE_PRINT_ADDR_STR_FORMAT(addr_str, entry->redirect_addr), entry->interface_id);
@@ -939,7 +949,6 @@ ipv6_destination_t *ipv6_destination_lookup_or_create(const uint8_t *address, in
     if (!entry) {
         if (count > destination_cache_config.max_entries) {
             entry = ns_list_get_last(&ipv6_destination_cache);
-            ns_list_remove(&ipv6_destination_cache, entry);
             ipv6_destination_release(entry);
         }
 
@@ -1082,18 +1091,31 @@ void ipv6_destination_cache_forced_gc(bool full_gc)
      **/
     ns_list_foreach_reverse_safe(ipv6_destination_t, entry, &ipv6_destination_cache) {
         if (entry->lifetime == 0 || gc_count > destination_cache_config.long_term_entries || full_gc) {
-            ns_list_remove(&ipv6_destination_cache, entry);
-            ipv6_destination_release(entry);
-            gc_count--;
+            if (ipv6_destination_release(entry)) {
+                gc_count--;
+            }
         }
     }
 }
 
-static void ipv6_destination_release(ipv6_destination_t *dest)
+void ipv6_destination_cache_clean(int8_t interface_id)
+{
+    ns_list_foreach_reverse_safe(ipv6_destination_t, entry, &ipv6_destination_cache) {
+        if (entry->interface_id == interface_id) {
+            ipv6_destination_release(entry);
+        }
+    }
+}
+
+static bool ipv6_destination_release(ipv6_destination_t *dest)
 {
     if (--dest->refcount == 0) {
+        ns_list_remove(&ipv6_destination_cache, dest);
+        tr_debug("Destination cache remove: %s", trace_ipv6(dest->destination));
         ns_dyn_mem_free(dest);
+        return true;
     }
+    return false;
 }
 
 static void ipv6_destination_cache_gc_periodic(void)
@@ -1137,9 +1159,11 @@ static void ipv6_destination_cache_gc_periodic(void)
      */
     ns_list_foreach_reverse_safe(ipv6_destination_t, entry, &ipv6_destination_cache) {
         if (entry->lifetime == 0 || gc_count > destination_cache_config.short_term_entries) {
-            ns_list_remove(&ipv6_destination_cache, entry);
-            ipv6_destination_release(entry);
-            if (--gc_count <= destination_cache_config.long_term_entries) {
+            if (ipv6_destination_release(entry)) {
+                gc_count--;
+            }
+
+            if (gc_count <= destination_cache_config.long_term_entries) {
                 break;
             }
         }
diff --git a/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.h b/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.h
index 944439843e..ce7eb22476 100644
--- a/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.h
+++ b/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.h
@@ -162,6 +162,7 @@ extern bool ipv6_neighbour_ll_addr_match(const ipv6_neighbour_t *entry, addrtype
 extern void ipv6_neighbour_invalidate_ll_addr(ipv6_neighbour_cache_t *cache, addrtype_t ll_type, const uint8_t *ll_address);
 extern void ipv6_neighbour_delete_registered_by_eui64(ipv6_neighbour_cache_t *cache, const uint8_t *eui64);
 extern bool ipv6_neighbour_has_registered_by_eui64(ipv6_neighbour_cache_t *cache, const uint8_t *eui64);
+extern ipv6_neighbour_t *ipv6_neighbour_get_registered_by_eui64(ipv6_neighbour_cache_t *cache, const uint8_t *eui64);
 extern void ipv6_neighbour_entry_update_unsolicited(ipv6_neighbour_cache_t *cache, ipv6_neighbour_t *entry, addrtype_t type, const uint8_t *ll_address/*, bool tentative*/);
 extern ipv6_neighbour_t *ipv6_neighbour_update_unsolicited(ipv6_neighbour_cache_t *cache, const uint8_t *ip_address, addrtype_t ll_type, const uint8_t *ll_address);
 extern void ipv6_neighbour_reachability_confirmation(const uint8_t ip_address[__static 16], int8_t interface_id);
@@ -223,6 +224,7 @@ void ipv6_destination_cache_timer(uint8_t ticks);
 void ipv6_destination_redirect(const uint8_t *dest_addr, const uint8_t *sender_addr, const uint8_t *redirect_addr, int8_t interface_id, addrtype_t ll_type, const uint8_t *ll_address);
 #endif
 void ipv6_destination_cache_forced_gc(bool full_gc);
+void ipv6_destination_cache_clean(int8_t interface_id);
 
 /* Combined Routing Table (RFC 4191) and Prefix List (RFC 4861) */
 /* On-link prefixes have the on_link flag set and next_hop is unset */
diff --git a/features/nanostack/sal-stack-nanostack/source/libDHCPv6/dhcp_service_api.c b/features/nanostack/sal-stack-nanostack/source/libDHCPv6/dhcp_service_api.c
index 66cda2e6d4..78dddce5ad 100644
--- a/features/nanostack/sal-stack-nanostack/source/libDHCPv6/dhcp_service_api.c
+++ b/features/nanostack/sal-stack-nanostack/source/libDHCPv6/dhcp_service_api.c
@@ -60,6 +60,7 @@ typedef struct {
     int8_t interface_id;
     uint8_t server_address[16];
     bool    relay_activated;
+    bool    add_interface_id_option;
     ns_list_link_t link;
 } relay_instance_t;
 typedef NS_LIST_HEAD(relay_instance_t, link) relay_instance_list_t;
@@ -83,6 +84,8 @@ typedef struct {
     uint8_t *msg_ptr;
     uint16_t msg_len;
     uint8_t *relay_start;
+    uint8_t *opt_interface_id;
+    uint16_t opt_interface_id_length;
     ns_list_link_t link;
 } msg_tr_t;
 typedef NS_LIST_HEAD(msg_tr_t, link) tr_list_t;
@@ -335,6 +338,8 @@ void recv_dhcp_server_msg(void *cb_res)
         }
         //Update Source and data
         msg_tr_ptr->relay_start = msg_ptr;
+        msg_tr_ptr->opt_interface_id = relay_msg.relay_interface_id.msg_ptr;
+        msg_tr_ptr->opt_interface_id_length = relay_msg.relay_interface_id.len;
         memcpy(msg_tr_ptr->addr.address, relay_msg.peer_address, 16);
         msg_ptr = relay_msg.relay_options.msg_ptr;
         msg_len = relay_msg.relay_options.len;
@@ -408,35 +413,37 @@ void recv_dhcp_relay_msg(void *cb_res)
     }
     ns_address_t src_address;
 
-    uint8_t relay_frame[DHCPV6_RELAY_LENGTH + 4];
-    ns_iovec_t msg_iov[2];
-    msg_iov[0].iov_base = relay_frame;
-    msg_iov[0].iov_len = 34;
-    msg_iov[1].iov_base = ns_dyn_mem_temporary_alloc(sckt_data->d_len);
-    msg_iov[1].iov_len = sckt_data->d_len;
-    if (msg_iov[1].iov_base == NULL) {
+    //Relay vector added space for relay frame + Interface ID
+    uint8_t relay_frame[DHCPV6_RELAY_LENGTH + 4 + 5];
+
+
+    uint8_t *socket_data = ns_dyn_mem_temporary_alloc(sckt_data->d_len);
+
+    if (socket_data == NULL) {
         // read actual message
         tr_error("Out of resources");
         goto cleanup;
     }
 
+
     ns_msghdr_t msghdr;
+    ns_iovec_t msg_data;
+    msg_data.iov_base = socket_data;
+    msg_data.iov_len = sckt_data->d_len;
     //Set messages name buffer
     msghdr.msg_name = &src_address;
     msghdr.msg_namelen = sizeof(src_address);
-    msghdr.msg_iov = &msg_iov[1];
+    msghdr.msg_iov = &msg_data;
     msghdr.msg_iovlen = 1;
     msghdr.msg_control = NULL;
     msghdr.msg_controllen = 0;
 
     msg_len = socket_recvmsg(sckt_data->socket_id, &msghdr, NS_MSG_LEGACY0);
 
-
     tr_debug("dhcp Relay recv msg");
 
     //Parse type
-    uint8_t *ptr = msg_iov[1].iov_base;
-    uint8_t msg_type = *ptr;
+    uint8_t msg_type = *socket_data;
 
     int16_t tc = 0;
     if (msg_type == DHCPV6_RELAY_FORWARD) {
@@ -446,7 +453,7 @@ void recv_dhcp_relay_msg(void *cb_res)
     } else if (msg_type == DHCPV6_RELAY_REPLY) {
         //Parse and validate Relay
         dhcpv6_relay_msg_t relay_msg;
-        if (!libdhcpv6_relay_msg_read(ptr, msg_len, &relay_msg)) {
+        if (!libdhcpv6_relay_msg_read(socket_data, msg_len, &relay_msg)) {
             tr_error("Not valid relay");
             goto cleanup;
         }
@@ -458,14 +465,14 @@ void recv_dhcp_relay_msg(void *cb_res)
         memcpy(src_address.address, relay_msg.peer_address, 16);
         src_address.type = ADDRESS_IPV6;
         src_address.identifier = DHCPV6_CLIENT_PORT;
-        msghdr.msg_iov = &msg_iov[0];
+        msghdr.msg_iov = &msg_data;
         msghdr.msg_iovlen = 1;
-        msg_iov[0].iov_base = relay_msg.relay_options.msg_ptr;
-        msg_iov[0].iov_len = relay_msg.relay_options.len;
+        msg_data.iov_base = relay_msg.relay_options.msg_ptr;
+        msg_data.iov_len = relay_msg.relay_options.len;
         tr_debug("Forward Original relay msg to client");
 
     } else {
-        if (0 != libdhcpv6_message_malformed_check(ptr, msg_len)) {
+        if (0 != libdhcpv6_message_malformed_check(socket_data, msg_len)) {
             tr_error("Malformed packet");
             goto cleanup;
         }
@@ -476,10 +483,24 @@ void recv_dhcp_relay_msg(void *cb_res)
             tr_error("No GP address");
             goto cleanup;
         }
-
+        ns_iovec_t msg_iov[2];
+        uint8_t *ptr = relay_frame;
         //Build
-        libdhcpv6_dhcp_relay_msg_write(relay_frame, DHCPV6_RELAY_FORWARD, 0, src_address.address, gp_address);
-        libdhcpv6_dhcp_option_header_write(relay_frame + 34, msg_len);
+        //ADD relay frame vector front of original data
+        msghdr.msg_iov = &msg_iov[0];
+        msg_iov[0].iov_base = relay_frame;
+        msghdr.msg_iovlen = 2;
+        //SET Original Data
+        msg_iov[1].iov_base = socket_data;
+        msg_iov[1].iov_len = msg_len;
+
+        ptr = libdhcpv6_dhcp_relay_msg_write(ptr, DHCPV6_RELAY_FORWARD, 0, src_address.address, gp_address);
+        if (relay_srv->add_interface_id_option) {
+            ptr = libdhcpv6_option_interface_id_write(ptr, sckt_data->interface_id);
+        }
+        ptr = libdhcpv6_dhcp_option_header_write(ptr, DHCPV6_OPTION_RELAY, msg_len);
+        //Update length of relay vector
+        msg_iov[0].iov_len = ptr - relay_frame;
 
         //Update Neighbour table if necessary
         relay_notify_t *neigh_notify = dhcp_service_notify_find(sckt_data->interface_id);
@@ -491,12 +512,6 @@ void recv_dhcp_relay_msg(void *cb_res)
         memcpy(src_address.address, relay_srv->server_address, 16);
         src_address.type = ADDRESS_IPV6;
         src_address.identifier = DHCPV6_SERVER_PORT;
-        //ADD relay frame vector front of original data
-        msghdr.msg_iov = &msg_iov[0];
-        msghdr.msg_iovlen = 2;
-        msg_iov[0].iov_base = relay_frame;
-        msg_iov[0].iov_len = 38;
-        msg_iov[1].iov_len = msg_len;
         tr_debug("Forward Client msg to server");
         tc = IP_DSCP_CS6 << IP_TCLASS_DSCP_SHIFT;
 
@@ -504,7 +519,7 @@ void recv_dhcp_relay_msg(void *cb_res)
     socket_setsockopt(sckt_data->socket_id, SOCKET_IPPROTO_IPV6, SOCKET_IPV6_TCLASS, &tc, sizeof(tc));
     socket_sendmsg(sckt_data->socket_id, &msghdr, NS_MSG_LEGACY0);
 cleanup:
-    ns_dyn_mem_free(msg_iov[1].iov_base);
+    ns_dyn_mem_free(socket_data);
 
     return;
 }
@@ -635,6 +650,7 @@ uint16_t dhcp_service_init(int8_t interface_id, dhcp_instance_type_e instance_ty
         relay_srv->instance_id = id;
         relay_srv->interface_id = interface_id;
         relay_srv->relay_activated = false;
+        relay_srv->add_interface_id_option = false;
 
     }
 
@@ -655,6 +671,14 @@ void dhcp_service_relay_instance_enable(uint16_t instance, uint8_t *server_addre
     }
 }
 
+void dhcp_service_relay_interface_id_option_enable(uint16_t instance, bool enable)
+{
+    relay_instance_t *relay_srv = dhcp_service_relay_find(instance);
+    if (relay_srv) {
+        relay_srv->add_interface_id_option = enable;
+    }
+}
+
 uint8_t *dhcp_service_relay_global_addres_get(uint16_t instance)
 {
     relay_instance_t *relay_srv = dhcp_service_relay_find(instance);
@@ -851,30 +875,42 @@ void dhcp_service_send_message(msg_tr_t *msg_tr_ptr)
         //Build Relay Reply only server do this
         int16_t tc = IP_DSCP_CS6 << IP_TCLASS_DSCP_SHIFT;
         socket_setsockopt(msg_tr_ptr->socket, SOCKET_IPPROTO_IPV6, SOCKET_IPV6_TCLASS, &tc, sizeof(tc));
-        ns_iovec_t data_vector[2];
+        ns_iovec_t data_vector[4];
+        uint8_t relay_header[4];
+        libdhcpv6_dhcp_option_header_write(relay_header, DHCPV6_OPTION_RELAY, msg_tr_ptr->msg_len);
         ns_msghdr_t msghdr;
+        msghdr.msg_iovlen = 0;
         memcpy(msg_tr_ptr->addr.address, msg_tr_ptr->relay_start + 2, 16);
         msg_tr_ptr->addr.identifier = DHCPV6_SERVER_PORT;
         //SET IOV vectors
         //Relay Reply
-        data_vector[0].iov_base = (void *) msg_tr_ptr->relay_start;
-        data_vector[0].iov_len = DHCPV6_RELAY_LENGTH + 4;
+        data_vector[msghdr.msg_iovlen].iov_base = (void *) msg_tr_ptr->relay_start;
+        data_vector[msghdr.msg_iovlen].iov_len = DHCPV6_RELAY_LENGTH;
+        msghdr.msg_iovlen++;
+        if (msg_tr_ptr->opt_interface_id) {
+            data_vector[msghdr.msg_iovlen].iov_base = (void *)(msg_tr_ptr->opt_interface_id - 4);
+            data_vector[msghdr.msg_iovlen].iov_len = msg_tr_ptr->opt_interface_id_length + 4;
+            msghdr.msg_iovlen++;
+        }
+        //Relay reply header
+        data_vector[msghdr.msg_iovlen].iov_base = (void *) relay_header;
+        data_vector[msghdr.msg_iovlen].iov_len = 4;
+        msghdr.msg_iovlen++;
         //DHCPV normal message vector
-        data_vector[1].iov_base = (void *) msg_tr_ptr->msg_ptr;
-        data_vector[1].iov_len = msg_tr_ptr->msg_len;
+        data_vector[msghdr.msg_iovlen].iov_base = (void *) msg_tr_ptr->msg_ptr;
+        data_vector[msghdr.msg_iovlen].iov_len = msg_tr_ptr->msg_len;
+        msghdr.msg_iovlen++;
 
         //Set message name
         msghdr.msg_name = (void *) &msg_tr_ptr->addr;
         msghdr.msg_namelen = sizeof(ns_address_t);
         msghdr.msg_iov = &data_vector[0];
-        msghdr.msg_iovlen = 2;
         //No ancillary data
         msghdr.msg_control = NULL;
         msghdr.msg_controllen = 0;
 
         uint8_t *ptr = msg_tr_ptr->relay_start;
         *ptr = DHCPV6_RELAY_REPLY;
-        libdhcpv6_dhcp_option_header_write(ptr + 34, msg_tr_ptr->msg_len);
         retval = socket_sendmsg(msg_tr_ptr->socket, &msghdr, NS_MSG_LEGACY0);
 
     } else {
@@ -972,6 +1008,12 @@ void dhcp_service_relay_instance_enable(uint16_t instance, uint8_t *server_addre
     (void)server_address;
 }
 
+void dhcp_service_relay_interface_id_option_enable(uint16_t instance, bool enable)
+{
+    (void)instance;
+    (void)enable;
+}
+
 int dhcp_service_send_resp(uint32_t msg_tr_id, uint8_t options, uint8_t *msg_ptr, uint16_t msg_len)
 {
     (void)msg_tr_id;
diff --git a/features/nanostack/sal-stack-nanostack/source/libDHCPv6/libDHCPv6.c b/features/nanostack/sal-stack-nanostack/source/libDHCPv6/libDHCPv6.c
index 722e41f46e..abc29b2272 100644
--- a/features/nanostack/sal-stack-nanostack/source/libDHCPv6/libDHCPv6.c
+++ b/features/nanostack/sal-stack-nanostack/source/libDHCPv6/libDHCPv6.c
@@ -859,9 +859,16 @@ uint8_t *libdhcpv6_dhcp_relay_msg_write(uint8_t *ptr, uint8_t type, uint8_t hop_
     return ptr;
 }
 
-uint8_t *libdhcpv6_dhcp_option_header_write(uint8_t *ptr, uint16_t length)
+uint8_t *libdhcpv6_option_interface_id_write(uint8_t *ptr, int8_t interface_id)
 {
-    ptr = common_write_16_bit(DHCPV6_OPTION_RELAY, ptr);
+    ptr = libdhcpv6_dhcp_option_header_write(ptr, DHCPV6_OPTION_INTERFACE_ID, 1);
+    *ptr++ = interface_id;
+    return ptr;
+}
+
+uint8_t *libdhcpv6_dhcp_option_header_write(uint8_t *ptr, uint16_t option_type, uint16_t length)
+{
+    ptr = common_write_16_bit(option_type, ptr);
     ptr = common_write_16_bit(length, ptr);
     return ptr;
 }
@@ -878,6 +885,11 @@ bool libdhcpv6_relay_msg_read(uint8_t *ptr, uint16_t length, dhcpv6_relay_msg_t
     relay_msg->peer_address = ptr + 16;
     ptr += 32;
     //Discover
+    if (libdhcpv6_message_option_discover(ptr, length - 34, DHCPV6_OPTION_INTERFACE_ID, &relay_msg->relay_interface_id) != 0) {
+        relay_msg->relay_interface_id.len = 0;
+        relay_msg->relay_interface_id.msg_ptr = NULL;
+    }
+
     if (libdhcpv6_message_option_discover(ptr, length - 34, DHCPV6_OPTION_RELAY, &relay_msg->relay_options) != 0) {
         return false;
     }
diff --git a/features/nanostack/sal-stack-nanostack/source/libDHCPv6/libDHCPv6.h b/features/nanostack/sal-stack-nanostack/source/libDHCPv6/libDHCPv6.h
index dd2bd6309f..5910bab190 100644
--- a/features/nanostack/sal-stack-nanostack/source/libDHCPv6/libDHCPv6.h
+++ b/features/nanostack/sal-stack-nanostack/source/libDHCPv6/libDHCPv6.h
@@ -125,6 +125,7 @@ typedef struct dhcpv6_relay_msg {
     uint8_t    hop_limit;
     uint8_t    *link_address;
     uint8_t    *peer_address;
+    dhcp_options_msg_t relay_interface_id;
     dhcp_options_msg_t relay_options;
 } dhcpv6_relay_msg_t;
 
@@ -237,6 +238,7 @@ typedef struct dhcpv6_relay_msg {
 
 #define DHCPV6_RELAY_LENGTH 34
 #define DHCPV6_OPTION_RELAY 0x0009
+#define DHCPV6_OPTION_INTERFACE_ID 0x0012
 
 
 
@@ -293,7 +295,7 @@ uint16_t libdhcpv6_address_reply_message_len(uint16_t clientDUIDLength, uint16_t
 
 uint8_t *libdhcpv6_generic_nontemporal_address_message_write(uint8_t *ptr, dhcpv6_solication_base_packet_s *packet, dhcpv6_ia_non_temporal_address_s *nonTemporalAddress, dhcp_duid_options_params_t *serverLink);
 uint8_t *libdhcpv6_dhcp_relay_msg_write(uint8_t *ptr, uint8_t type, uint8_t hop_limit,  uint8_t *peer_addres, uint8_t *link_address);
-uint8_t *libdhcpv6_dhcp_option_header_write(uint8_t *ptr, uint16_t length);
+uint8_t *libdhcpv6_dhcp_option_header_write(uint8_t *ptr, uint16_t option_type, uint16_t length);
 
 int libdhcpv6_get_IA_address(uint8_t *ptr, uint16_t data_length, dhcp_ia_non_temporal_params_t *params);
 int libdhcpv6_get_duid_by_selected_type_id_opt(uint8_t *ptr, uint16_t data_length, uint16_t type, dhcp_duid_options_params_t *params);
@@ -341,6 +343,8 @@ uint8_t *libdhcpv6_rapid_commit_option_write(uint8_t *ptr);
  */
 uint8_t *libdhcvp6_request_option_write(uint8_t *ptr, uint8_t optionCnt, uint16_t *optionPtr);
 
+uint8_t *libdhcpv6_option_interface_id_write(uint8_t *ptr, int8_t interface_id);
+
 
 
 /**
diff --git a/features/nanostack/sal-stack-nanostack/source/libNET/src/net_dns.c b/features/nanostack/sal-stack-nanostack/source/libNET/src/net_dns.c
index 434bd4d6d3..7975ba89b6 100644
--- a/features/nanostack/sal-stack-nanostack/source/libNET/src/net_dns.c
+++ b/features/nanostack/sal-stack-nanostack/source/libNET/src/net_dns.c
@@ -112,10 +112,11 @@ int8_t net_dns_server_address_set(int8_t interface_id, const uint8_t address[16]
 
     if (!info_ptr) {
         info_ptr = dns_server_info_create(interface_id, address);
+        // Trace only when new entry is created
+        tr_info("DNS Server: %s from: %s Lifetime: %lu", trace_ipv6(dns_server_address), trace_ipv6(info_ptr->address), (unsigned long) lifetime);
     }
     info_ptr->lifetime = lifetime;
     memcpy(info_ptr->dns_server_address, dns_server_address, 16);
-    tr_info("DNS Server: %s from: %s Lifetime: %lu", trace_ipv6(info_ptr->dns_server_address), trace_ipv6(info_ptr->address), (unsigned long) info_ptr->lifetime);
     return 0;
 }
 
@@ -148,22 +149,22 @@ int8_t net_dns_server_search_list_set(int8_t interface_id, const uint8_t address
     if (info_ptr->dns_search_list_ptr && info_ptr->dns_search_list_len != dns_search_list_len) {
         ns_dyn_mem_free(info_ptr->dns_search_list_ptr);
         info_ptr->dns_search_list_ptr = NULL;
+        info_ptr->dns_search_list_len = 0;
     }
 
     if (dns_search_list_len) {
         if (!info_ptr->dns_search_list_ptr) {
             info_ptr->dns_search_list_ptr = ns_dyn_mem_alloc(dns_search_list_len);
+
+            tr_info("DNS Search List: %s Lifetime: %lu", trace_array(dns_search_list_ptr, dns_search_list_len), (unsigned long) info_ptr->lifetime);
         }
 
         if (!info_ptr->dns_search_list_ptr) {
             return -2;
         }
         memcpy(info_ptr->dns_search_list_ptr, dns_search_list_ptr, dns_search_list_len);
+        info_ptr->dns_search_list_len = dns_search_list_len;
     }
-
-    info_ptr->dns_search_list_len = dns_search_list_len;
-    tr_info("DNS Search List: %s Lifetime: %lu", trace_array(info_ptr->dns_search_list_ptr, info_ptr->dns_search_list_len), (unsigned long) info_ptr->lifetime);
-
     return 0;
 }