diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/nanostack/net_interface.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/nanostack/net_interface.h
index 5287224831..e4b07583a0 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/nanostack/net_interface.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/nanostack/net_interface.h
@@ -987,6 +987,23 @@ void arm_print_protocols2(void (*print_fn)(const char *fmt, ...), char sep);
   *
   */
 extern void net_get_version_information(uint8_t *ptr);
+
+/**
+ * \brief Set buffer size for sleepy device parent.
+ *
+ * This function can be used to set sleepy device parent buffer size and packet threshold.
+ *
+ * Note! In Thread mode parent buffer size is automatically set during Thread initialization.
+ *
+ * \param big_packet_threshold Indicate how long packets are considered big. For Thread, must be 106 bytes.
+ * \param small_packets_per_child_count Number of small packets stored for each sleepy children. For Thread, must be at least 1.
+ * \param big_packets_total_count Total number of big packets parent can store for all sleepy children. For Thread, must be at least 1.
+ * \return 0 on success, <0 on errors.
+ */
+
+extern int arm_nwk_sleepy_device_parent_buffer_size_set(int8_t interface_id, uint16_t big_packet_threshold, uint16_t small_packets_per_child_count, uint16_t big_packets_total_count);
+
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/nanostack/thread_management_if.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/nanostack/thread_management_if.h
index 82e95d8de6..c193385881 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/nanostack/thread_management_if.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/nanostack/thread_management_if.h
@@ -74,17 +74,12 @@ typedef struct link_configuration {
     uint8_t master_key[16]; /**< Master key of the thread network*/
     uint8_t PSKc[16]; /**< PSKc value that is calculated from commissioning credentials credentials,XPANID and network name*/
     uint8_t mesh_local_ula_prefix[8]; /**< Mesh local ula prefix*/
-    uint8_t mesh_local_eid[8]; /**< Mesh local extented id*/
     uint8_t extented_pan_id[8]; /**< Extended pan id*/
-    uint8_t extended_random_mac[8]; /**< Extended random mac which is generated during commissioning*/
     uint8_t channel_mask[8]; /**< channel page and mask only supported is page 0*/
     uint8_t channel_page;/**< channel page supported pages 0*/
-    char *PSKc_ptr; /**< Commissioning credentials.  TODO! think if we need the actual credentials*/
-    uint8_t PSKc_len; /**< Length of PSKc */
     uint16_t key_rotation; /**< Key rotation time in hours*/
     uint32_t key_sequence; /**< Key sequence counter */
     uint16_t panId; /**< network id*/
-    uint8_t Protocol_id; /**< current protocol id*/
     uint8_t version; /**< current protocol version*/
     uint16_t rfChannel; /**< current rf channel*/
     uint8_t securityPolicy; /**< Commission Security Policy*/
@@ -108,6 +103,8 @@ typedef struct link_configuration {
  */
 typedef struct {
     uint8_t eui64[8];/**< eui64 of the device. This field is used to identify device when joining to network Mandatory*/
+    uint8_t mesh_local_eid[8]; /**< Mesh local extented id*/
+    uint8_t extended_random_mac[8]; /**< Extended random mac which is generated during commissioning*/
     uint8_t *PSKd_ptr;/**< Device credentials used to authenticate device to commissioner Mandatory  length 6-32*/
     uint8_t PSKd_len;/**< Length of PSKd_ptr*/
     char *provisioning_uri_ptr;/**< Provisioning url max 64 bytes*/
@@ -200,6 +197,22 @@ link_configuration_s *thread_management_configuration_get(int8_t interface_id);
  */
 int thread_management_link_configuration_store(int8_t interface_id, link_configuration_s *link_config);
 
+/** Configure extra TLVs in nanostack .
+ *
+ * Storing is asynchronous operation and this method makes a request to store link
+ * configuration settings. Operation will be completed in the background.
+ * Once settings has been stored the Thread network will be restarted with new
+ * configuration settings.
+ *
+ * /param interface Id of network interface. -1 if interface_id is not available.
+ * /param additional_ptr Pointer to the extra TLV that is to be configured in nanostack
+ * /param additional_len Length of the additional TLV
+ *
+ * /return 0 if store request is successful.
+ * /return < 0 if request is failed.
+ */
+int thread_management_link_configuration_add(int8_t interface_id, uint8_t *additional_ptr, uint8_t additional_len);
+
 /** Delete Thread network link configuration settings.
  *
  * Deletion is asynchronous operation and this method makes a request to delete link
@@ -399,6 +412,34 @@ int thread_management_device_certificate_set(int8_t interface_id, const unsigned
  */
 int thread_management_network_certificate_set(int8_t interface_id, const unsigned char *network_certificate_ptr, uint16_t network_certificate_len, const unsigned char *priv_key_ptr, uint16_t priv_key_len);
 
+/**
+ * Set Thread partition weighting.
+ *
+ * This function sets weighting value for Thread network partition. Interface will be restarted if interface is active and
+ * new weighting value is different than previous weighting value.
+ *
+ * \param interface_id Network interface ID.
+ * \param partition_weighting New weighting value for Thread partition
+ *
+ * \return 0, OK.
+ * \return <0 fail.
+ */
+int thread_management_partition_weighting_set(int8_t interface_id, uint8_t partition_weighting);
+
+/**
+ * Set Thread Sleepy End Device parent packet buffer size.
+ *
+ * This function can be used to adjust count of packets SED parent is storing.
+ *
+ * \param interface_id Network interface ID.
+ * \param small_packets_per_child_count Number of small packets parent is storing for each SED.
+ * \param big_packets_total_count Number of big packets parent can store for all SEDs.
+ *
+ * \return 0, OK.
+ * \return <0 fail.
+ */
+int thread_management_sed_parent_buffer_size_set(int8_t interface_id, uint16_t small_packets_per_child_count, uint16_t big_packets_total_count);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan_bootstrap.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan_bootstrap.c
index c2baadd50d..06dbf49f41 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan_bootstrap.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan_bootstrap.c
@@ -100,6 +100,7 @@ static void protocol_6lowpan_neighbor_information_remove(int8_t interface_id, ml
 static int8_t protocol_6lowpan_host_challenge(int8_t interface_id, const uint8_t *mac64);
 static int8_t protocol_6lowpan_router_challenge(int8_t interface_id, const uint8_t *mac64);
 static void protocol_6lowpan_address_reg_ready(protocol_interface_info_entry_t *cur_interface);
+static void coordinator_black_list(protocol_interface_info_entry_t *cur);
 
 static mle_6lowpan_data_t *mle_6lowpan_data;
 
@@ -663,17 +664,17 @@ static uint16_t mle_router_synch(protocol_interface_info_entry_t *cur, const uin
     if (retrans) {
         if (destAddress) {
             timeout.retrans_max = 3;
-            timeout.timeout_init = 1;
-            timeout.timeout_max = 3;
+            timeout.timeout_init = 2;
+            timeout.timeout_max = 4;
         } else {
             timeout.retrans_max = 2;
-            timeout.timeout_init = 2;
+            timeout.timeout_init = 4;
             timeout.timeout_max = 4;
         }
     } else {
         timeout.retrans_max = 1;
-        timeout.timeout_init = 1;
-        timeout.timeout_max = 3;
+        timeout.timeout_init = 2;
+        timeout.timeout_max = 4;
     }
 
     timeout.delay = delay;
@@ -706,7 +707,7 @@ static int mle_router_accept_request_build(protocol_interface_info_entry_t *cur,
         bufId =  mle_service_msg_allocate(cur->id, 64, true,type);
         timeout.retrans_max = 2;
         timeout.timeout_init = 2;
-        timeout.timeout_max = 2;
+        timeout.timeout_max = 4;
     }
 
     if (bufId == 0) {
@@ -814,6 +815,7 @@ static bool mle_parent_link_req_cb(int8_t interface_id, uint16_t msgId, bool use
         if (cur->nwk_bootstrap_state == ER_MLE_LINK_REQ) {
             //Enter ND scan
             bootsrap_next_state_kick(ER_SCAN, cur);
+            pan_coordinator_blacklist_free(&cur->pan_cordinator_black_list);
         }
 #ifdef HAVE_RPL
         else if (cur->nwk_bootstrap_state == ER_ROUTER_SYNCH) {
@@ -840,12 +842,19 @@ static bool mle_parent_link_req_cb(int8_t interface_id, uint16_t msgId, bool use
     if (usedAllRetries) {
         switch (cur->nwk_bootstrap_state) {
             case ER_MLE_LINK_REQ:
-            case ER_ROUTER_SYNCH:
             case ER_MLE_LINK_ADDRESS_SYNCH:
             case ER_MLE_LINK_SHORT_SYNCH:
+                if (cur->nwk_bootstrap_state == ER_MLE_LINK_REQ) {
+                    coordinator_black_list(cur);
+                }
                 tr_debug("Link synch fail %u", cur->nwk_bootstrap_state);
                 bootsrap_next_state_kick(ER_BOOTSTRAP_CONNECTION_DOWN, cur);
                 break;
+#ifdef HAVE_RPL
+            case ER_ROUTER_SYNCH:
+                bootsrap_next_state_kick(ER_RPL_MC, cur);
+                break;
+#endif // HAVE_RPL
             default:
                 break;
         }
@@ -869,6 +878,7 @@ static bool mle_accept_request_cb(int8_t interface_id, uint16_t msgId, bool used
         //If message has been sent by MLE service sends MLE reject to clear link
         if (mle_service_check_msg_sent(msgId)) {
             uint8_t *address_ptr = mle_service_get_msg_destination_address_pointer(msgId);
+            tr_debug("No accept for Accept/Request");
             mle_service_reject_message_build(cur->id, address_ptr, false);
         }
         return false;
@@ -2278,15 +2288,8 @@ static void nwk_6lowpan_bootsrap_pana_authentication_start(protocol_interface_in
 
 #endif
 
-static void nwk_6lowpan_network_authentication_fail(protocol_interface_info_entry_t *cur)
+static void coordinator_black_list(protocol_interface_info_entry_t *cur)
 {
-    nwk_scan_params_t *scan_params =
-            &cur->mac_parameters->nwk_scan_params;
-
-    tr_warn("Pana Auhth er");
-
-    scan_params->nwk_cur_active = mac_helper_free_pan_descriptions(scan_params->nwk_cur_active);
-    //Black List coordinator
     uint8_t coord_pan_address[10];
     addrtype_t cord_adr_type = mac_helper_coordinator_address_get(cur, coord_pan_address +2);
 
@@ -2299,6 +2302,18 @@ static void nwk_6lowpan_network_authentication_fail(protocol_interface_info_entr
 
         pan_cordinator_blacklist_pan_set(&cur->pan_cordinator_black_list, coord_pan_address, 300);
     }
+}
+
+static void nwk_6lowpan_network_authentication_fail(protocol_interface_info_entry_t *cur)
+{
+    nwk_scan_params_t *scan_params =
+            &cur->mac_parameters->nwk_scan_params;
+
+    tr_warn("Pana Auhth er");
+
+    scan_params->nwk_cur_active = mac_helper_free_pan_descriptions(scan_params->nwk_cur_active);
+    //Black List coordinator
+    coordinator_black_list(cur);
 
     nwk_bootsrap_state_update(ARM_NWK_AUHTENTICATION_FAIL, cur);
 }
@@ -2374,6 +2389,7 @@ static void nwk_6lowpan_network_authentication_done(protocol_interface_info_entr
             tr_debug("Link request start fail");
         }
 #else
+        pan_coordinator_blacklist_free(&cur->pan_cordinator_black_list);
         cur->nwk_bootstrap_state = ER_SCAN;
         nwk_6lowpan_router_scan_state(cur);
 #endif
@@ -2538,7 +2554,7 @@ void protocol_6lowpan_mac_scan_confirm(int8_t if_id, const mlme_scan_conf_t* con
 
 void bootstrap_timer_handle(uint16_t ticks)
 {
-    ticks;
+    (void)ticks;
     ns_list_foreach(protocol_interface_info_entry_t, cur, &protocol_interface_info_list) {
         if (cur->nwk_id == IF_6LoWPAN) {
             if (cur->nwk_bootstrap_state == ER_ACTIVE_SCAN || cur->nwk_bootstrap_state == ER_WARM_ACTIVE_SCAN) {
@@ -2728,6 +2744,7 @@ static void protocol_6lowpan_generate_link_reject(protocol_interface_info_entry_
         address[8] ^= 2;
     }
     if (mac_helper_default_security_level_get(cur)) {
+        tr_debug("Drop link by asymmetric security");
         mle_service_reject_message_build(cur->id, address, false);
         return;
     }
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Mesh/mesh.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Mesh/mesh.c
index 4f274a6934..16c74e41ac 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Mesh/mesh.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Mesh/mesh.c
@@ -243,6 +243,7 @@ intercept:
     memcpy(buf->dst_sa.address + 2, route.address, route.addr_len);
 
     /* Set src MAC address from our interface */
+    buf->src_sa.addr_type = ADDR_NONE;
     if (!mac_helper_write_our_addr(cur, &buf->src_sa)) {
         goto drop;
     }
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api.c
index 4360f0a88b..acdaa2c453 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api.c
@@ -60,6 +60,7 @@
 #include "thread_management_server.h"
 #include "socket_api.h"
 #include "coap_service_api.h"
+#include "Common_Protocols/icmpv6.h"
 
 #define TRACE_GROUP "tBBR"
 
@@ -511,6 +512,7 @@ static int thread_border_relay_to_leader_cb(int8_t service_id, uint8_t source_ad
     return -1;
 }
 
+#ifdef HAVE_THREAD_BORDER_ROUTER
 static bool thread_bbr_i_host_prefix(struct protocol_interface_info_entry *cur, uint8_t prefix_ptr[8], uint8_t *br_count, bool *i_am_lowest)
 {
     bool i_host_this_prefix = false;
@@ -567,6 +569,7 @@ static void thread_bbr_network_data_remove(thread_bbr_t *this)
     memset(this->bbr_prefix,0,8);
     this->br_info_published = false;
 }
+
 static void thread_bbr_network_data_send(thread_bbr_t *this, uint8_t prefix[8], uint8_t eui64[8])
 {
     thread_border_router_info_t br_info = { 0 };
@@ -593,8 +596,8 @@ static void thread_bbr_network_data_send(thread_bbr_t *this, uint8_t prefix[8],
     thread_border_router_prefix_add(this->interface_id, this->bbr_prefix, 64, &br_info);
     thread_border_router_publish(this->interface_id);
     this->br_info_published = true;
-
 }
+
 static void thread_bbr_routing_enable(thread_bbr_t *this)
 {
     if (this->routing_enabled) {
@@ -684,23 +687,24 @@ static void thread_bbr_status_check(thread_bbr_t *this, uint32_t seconds)
         // Check states when we need to remove our BR from network
         if (this->br_hosted && this->br_count > 1) {
             // Race condition More border routers than there should trigger random delay to remove BR
-            // our implementation prefers lowest RLOC to drop out to reduce problem time
+            // our implementation prefers lowest RLOC to to stay to reduce problem time
             if (br_lowest_host) {
-                this->br_delete_timer = randLIB_get_random_in_range(5,10);
-            } else {
                 this->br_delete_timer = randLIB_get_random_in_range(20,60);
+            } else {
+                this->br_delete_timer = randLIB_get_random_in_range(5,10);
             }
-            tr_info("br: too many BRs start remove jitter: %d", this->br_delete_timer);
+            tr_info("br: too many BRs start remove jitter:%"PRIu32, this->br_delete_timer);
             return;
         }
         if (this->br_info_published && !bbr_prefix_ptr ) {
             // Need to disable ND proxy will give a 20 second delay for it We could also disable routing immediately
             this->br_delete_timer = 20;
-            tr_info("br: can not be border router need to remove after: %d", this->br_delete_timer);
+            tr_info("br: can not be border router need to remove after: %"PRIu32, this->br_delete_timer);
             return;
         }
     }
 }
+
 static bool thread_bbr_activated(thread_bbr_t *this, uint32_t seconds)
 {
     protocol_interface_info_entry_t *cur;
@@ -739,7 +743,6 @@ static bool thread_bbr_activated(thread_bbr_t *this, uint32_t seconds)
     return false;
 }
 
-#ifdef HAVE_THREAD_BORDER_ROUTER
 bool thread_bbr_routing_enabled(protocol_interface_info_entry_t *cur)
 {
     thread_bbr_t *this = thread_bbr_find_by_interface(cur->thread_info->interface_id);
@@ -755,7 +758,7 @@ void thread_bbr_network_data_update_notify(protocol_interface_info_entry_t *cur)
     (void) cur;
     thread_mdns_network_data_update_notify();
 }
-#endif
+#endif /* HAVE_THREAD_BORDER_ROUTER*/
 
 static void thread_bbr_udp_proxy_service_stop(int8_t interface_id)
 {
@@ -765,7 +768,6 @@ static void thread_bbr_udp_proxy_service_stop(int8_t interface_id)
         tr_error("Failed to find BA instance");
         return;
     }
-    tr_debug("thread_border_router_udp_proxy_service_stop %d", interface_id);
 
     socket_close(this->udp_proxy_socket);
     this->udp_proxy_socket = -1;
@@ -786,19 +788,18 @@ int thread_bbr_commissioner_proxy_service_update(int8_t interface_id)
         goto return_fail;
     }
 
-    if (cur->thread_info->registered_commissioner.commissioner_valid) {
-        // relay is needed
+    if (!cur->thread_info->registered_commissioner.commissioner_valid) {
+        // commissioner not enabled
         if (this->udp_proxy_socket != -1) {
-            // UDP service is already running
-            return 0;
+            thread_bbr_udp_proxy_service_stop(interface_id);
         }
-    } else if (this->udp_proxy_socket != -1) {
-        // UDP service is running and need to delete
-        thread_bbr_udp_proxy_service_stop(interface_id);
         return 0;
     }
-    tr_debug("thread_border_router_udp_proxy_service_start %d", interface_id);
 
+    if (this->udp_proxy_socket != -1) {
+        // commissioner is valid and UDP service is already running
+        return 0;
+    }
 
     // Set source parameters, if commissioner is available
     ret_val = thread_management_get_commissioner_address(this->interface_id, &ns_source_addr.address[0], 0);
@@ -942,6 +943,48 @@ void thread_bbr_seconds_timer(int8_t interface_id, uint32_t seconds)
 #endif // HAVE_THREAD_ROUTER
 
 #ifdef HAVE_THREAD_BORDER_ROUTER
+static int thread_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);
+    if (!cur) {
+        return -1;
+    }
+
+    buffer_t *buffer = icmpv6_build_na(cur, false, true, true, target, NULL, ADDR_UNSPECIFIED);
+    protocol_push(buffer);
+    return 0;
+
+}
+int thread_bbr_nd_entry_add (int8_t interface_id, const uint8_t *addr_data_ptr,  uint32_t lifetime, void *info, const uint8_t *mleid_ptr) {
+    (void) mleid_ptr;
+    thread_bbr_t *this = thread_bbr_find_by_interface(interface_id);
+    if (!this || this->backbone_interface_id < 0) {
+        tr_err("bbr not ready");
+        return -1;
+    }
+    ipv6_route_t *route = ipv6_route_add_with_info(addr_data_ptr, 128, interface_id, NULL, ROUTE_THREAD_PROXIED_HOST, info, 0, lifetime, 0);
+    // We are using route info field to store sequence number
+    if (!route) {
+        // Direct route to host allows ND proxying to work
+        tr_err("out of resources");
+        return -2;
+    }
+    // send NA
+    thread_bbr_na_send(this->backbone_interface_id, addr_data_ptr);
+
+    return 0;
+}
+
+int thread_bbr_nd_entry_find(int8_t interface_id, const uint8_t *addr_data_ptr) {
+    ipv6_route_t *route = ipv6_route_choose_next_hop(addr_data_ptr, interface_id, NULL);
+    if (!route || route->prefix_len < 128 || !route->on_link || route->info.source != ROUTE_THREAD_PROXIED_HOST ) {
+        //Not found
+        return -1;
+    }
+    //TODO get information to route to parameters eq mleid, timeout
+    return 0;
+}
+
 int thread_bbr_proxy_state_update(int8_t caller_interface_id , int8_t handler_interface_id, bool status)
 {
     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(handler_interface_id);
@@ -998,6 +1041,7 @@ int thread_bbr_start(int8_t interface_id, int8_t backbone_interface_id)
 #ifdef HAVE_THREAD_BORDER_ROUTER
     thread_bbr_t *this = thread_bbr_find_by_interface(interface_id);
     link_configuration_s *link_configuration_ptr = thread_joiner_application_get_config(interface_id);
+    uint8_t *extended_random_mac = thread_joiner_application_random_mac_get(interface_id);
     char service_name[30] = {0};
     char *ptr;
 
@@ -1009,10 +1053,10 @@ int thread_bbr_start(int8_t interface_id, int8_t backbone_interface_id)
 
     this->backbone_interface_id = backbone_interface_id;
     ptr = service_name;
-    *ptr++ = 'a' + link_configuration_ptr->extended_random_mac[0] % 26;
-    *ptr++ = 'a' + link_configuration_ptr->extended_random_mac[1] % 26;
-    *ptr++ = 'a' + link_configuration_ptr->extended_random_mac[2] % 26;
-    *ptr++ = 'a' + link_configuration_ptr->extended_random_mac[3] % 26;
+    *ptr++ = 'a' + extended_random_mac[0] % 26;
+    *ptr++ = 'a' + extended_random_mac[1] % 26;
+    *ptr++ = 'a' + extended_random_mac[2] % 26;
+    *ptr++ = 'a' + extended_random_mac[3] % 26;
     memcpy(ptr,"-ARM-",5);
     ptr += 5;
     memcpy(ptr,link_configuration_ptr->name,16);
@@ -1020,7 +1064,8 @@ int thread_bbr_start(int8_t interface_id, int8_t backbone_interface_id)
     // Start mdns service
     thread_mdns_start(this->interface_id, this->backbone_interface_id, service_name);
     multicast_fwd_set_proxy_upstream(this->backbone_interface_id);
-    multicast_fwd_full_for_scope(this->interface_id, IPV6_SCOPE_SITE_LOCAL);
+    multicast_fwd_full_for_scope(this->interface_id, 0);
+    multicast_fwd_full_for_scope(this->backbone_interface_id, 0);
     // By default multicast forwarding is not enabled as it causes multicast loops
     multicast_fwd_set_forwarding(this->interface_id, false);
 
@@ -1070,6 +1115,9 @@ void thread_bbr_stop(int8_t interface_id)
         return;
     }
     thread_extension_bbr_delete(interface_id);
+    thread_bbr_network_data_remove(this);
+    thread_bbr_routing_disable(this);
+    thread_border_router_publish(interface_id);
     thread_mdns_stop();
     this->backbone_interface_id = -1;
 
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api_internal.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api_internal.h
index 98b7169c9e..8930e0190f 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api_internal.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api_internal.h
@@ -65,6 +65,7 @@ void thread_bbr_seconds_timer(int8_t interface_id, uint32_t tics);
  */
 int thread_bbr_commissioner_proxy_service_update(int8_t interface_id);
 
+
 #else
 #define thread_bbr_init(interface_id, external_commisssioner_port)
 #define thread_bbr_delete(interface_id)
@@ -94,10 +95,26 @@ bool thread_bbr_routing_enabled(protocol_interface_info_entry_t *cur);
  */
 void thread_bbr_network_data_update_notify(protocol_interface_info_entry_t *cur);
 
+/**
+ * \brief Add new nd entry to bbr
+ *
+ * \param interface_id addr_data_ptr lifetime info mleid_ptr
+ */
+int thread_bbr_nd_entry_add(int8_t interface_id, const uint8_t *addr_data_ptr,  uint32_t lifetime, void *info, const uint8_t *mleid_ptr);
+
+/**
+ * \brief Find if bbr has nd entry
+ *
+ * \param interface_id addr_data_ptr
+ */
+int thread_bbr_nd_entry_find(int8_t interface_id, const uint8_t *addr_data_ptr);
+
 #else
 #define thread_bbr_proxy_state_update(caller_interface_id , handler_interface_id, status) (NULL)
 #define thread_bbr_routing_enabled(cur) false
 #define thread_bbr_network_data_update_notify(cur)
+#define thread_bbr_nd_entry_add(interface_id, addr_data_ptr, lifetime, info, mleid_ptr) (0)
+#define thread_bbr_nd_entry_find(interface_id, addr_data_ptr) (0)
 #endif //HAVE_THREAD_BORDER_ROUTER
 
 
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bootstrap.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bootstrap.c
index c3a109eeeb..2095381936 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bootstrap.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bootstrap.c
@@ -115,21 +115,6 @@ static void thread_bootsrap_network_join_start(struct protocol_interface_info_en
 static int8_t thread_child_keep_alive(int8_t interface_id, const uint8_t *mac64);
 
 
-int thread_bootstrap_reset_child_info(protocol_interface_info_entry_t *cur, mle_neigh_table_entry_t *child)
-{
-    thread_dynamic_storage_child_info_clear(cur->id, child);
-
-    // If Child's RLOC16 appears in the Network Data send the RLOC16 to the Leader
-    if (thread_network_data_services_registered(&cur->thread_info->networkDataStorage, child->short_adr)) {
-        tr_debug("Remove references to Child's RLOC16 from the Network Data");
-        thread_management_client_network_data_unregister(cur->id, child->short_adr);
-    }
-
-    // Clear all (sleepy) child registrations to multicast groups
-    thread_child_mcast_entries_remove(cur, child->mac64);
-
-    return 0;
-}
 
 static bool thread_interface_is_active(int8_t interface_id) {
     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
@@ -146,57 +131,7 @@ static void thread_neighbor_remove(int8_t interface_id, mle_neigh_table_entry_t
     if (!cur_interface) {
         return;
     }
-    if (thread_info(cur_interface)->thread_device_mode == THREAD_DEVICE_MODE_END_DEVICE || thread_info(cur_interface)->thread_device_mode == THREAD_DEVICE_MODE_SLEEPY_END_DEVICE) {
-        thread_parent_info_t *thread_endnode_parent = thread_info(cur_interface)->thread_endnode_parent;
-        //Compare Parent
-        if (thread_endnode_parent) {
-            if (thread_endnode_parent->shortAddress == cur->short_adr) {
-                tr_warn("End device lost Parent!\n");
-                thread_bootstrap_connection_error(cur_interface->id, CON_PARENT_CONNECT_DOWN, NULL);
-            }
-        }
-    }
-    else {
-        if (thread_info(cur_interface)->thread_attached_state == THREAD_STATE_CONNECTED)
-        {
-            thread_parent_info_t *thread_endnode_parent = thread_info(cur_interface)->thread_endnode_parent;
-            if (thread_endnode_parent->shortAddress == cur->short_adr) {
-                tr_warn("REED has lost Parent!\n");
-                thread_routing_remove_link(cur_interface, cur->short_adr);
-                if(cur_interface->nwk_bootstrap_state != ER_CHILD_ID_REQ) {
-                    thread_bootstrap_connection_error(cur_interface->id, CON_PARENT_CONNECT_DOWN, NULL);
-                }
-            }
-            else{
-                tr_debug("Delete REED Neighbor");
-                if (thread_is_router_addr(cur->short_adr)) {
-                tr_debug("Router Free");
-                thread_routing_remove_link(cur_interface, cur->short_adr);
-                }
-            }
-        }
-        else if (thread_info(cur_interface)->thread_attached_state == THREAD_STATE_CONNECTED_ROUTER)
-        {
-            tr_debug("Delete Router Neighbor");
-            if (thread_is_router_addr(cur->short_adr)) {
-                tr_debug("Router Free");
-                thread_routing_remove_link(cur_interface, cur->short_adr);
-            } else if (thread_addr_is_child(mac_helper_mac16_address_get(cur_interface), cur->short_adr)) {
-                tr_debug("Child Free");
-                /* 16-bit neighbour cache entries are mesh addresses, so remain potentially valid even if an
-                 * MLE link fails. This is the only exception - if it was the link from us as a router to a
-                 * child. That means that device must be off the mesh (at that 16-bit address, at least).
-                 * This will actually clear either a GC cache entry for a FTD or a registered entry
-                 * for a MTD.
-                 */
-                protocol_6lowpan_release_short_link_address_from_neighcache(cur_interface, cur->short_adr);
-                thread_bootstrap_reset_child_info(cur_interface, cur);
-            }
-        }
-    }
-
-    protocol_6lowpan_release_long_link_address_from_neighcache(cur_interface, cur->mac64);
-    mac_helper_devicetable_remove(cur_interface->mac_api, cur->attribute_index);
+    thread_reset_neighbour_info(cur_interface, cur);
 }
 
 
@@ -247,7 +182,7 @@ int8_t thread_mle_class_init(int8_t interface_id)
         return -1;
     }
 
-    if (mle_class_init(interface_id, buffer.device_decription_table_size, &thread_neighbor_remove, &thread_child_keep_alive, &thread_interface_is_active) != 0) {
+    if (mle_class_init(interface_id, buffer.device_decription_table_size - 1, &thread_neighbor_remove, &thread_child_keep_alive, &thread_interface_is_active) != 0) {
         return -1;
     }
 
@@ -448,8 +383,10 @@ static int thread_router_check_previous_partition_info(protocol_interface_info_e
         //check for parameters
         return -1;
     }
-    if ((leaderData->partitionId == cur->thread_info->previous_partition_info.partitionId) && (routeTlv->dataPtr[0] == cur->thread_info->previous_partition_info.idSequence)) {
-        //drop the advertisement
+    if ((leaderData->partitionId == cur->thread_info->previous_partition_info.partitionId) &&
+        (leaderData->weighting == cur->thread_info->previous_partition_info.weighting) &&
+        (routeTlv->dataPtr[0] == cur->thread_info->previous_partition_info.idSequence)) {
+        //drop the advertisement from previuos partition
         return 1;
     }
     else {
@@ -487,6 +424,14 @@ int thread_bootstrap_partition_process(protocol_interface_info_entry_t *cur, uin
         tr_debug("Heard a REED and I am a singleton - merge");
         return 2;
     }
+    /*Rule 0: If we are going to form Higher partition than heard we dont try to attach to lower ones
+     */
+    if (thread_extension_enabled(cur) &&
+        thread_info(cur)->thread_device_mode == THREAD_DEVICE_MODE_ROUTER &&
+        heard_partition_leader_data->weighting < thread_info(cur)->partition_weighting) {
+        return -2;
+    }
+
     //Rule 1: A non-singleton Thread Network Partition always has higher priority than a singleton Thread Network Partition
     if (heard_partition_routers > 1 && active_routers == 1) {
         tr_debug("Heard a nonsingleton and i am a singleton");
@@ -499,6 +444,7 @@ int thread_bootstrap_partition_process(protocol_interface_info_entry_t *cur, uin
     /*Rule 2: When comparing two singleton or two non-singleton Thread Network Partitions,
     the one with the higher 8-bit weight value has higher priority. */
     if (heard_partition_leader_data->weighting > current_leader_data->weighting) {
+        tr_debug("Heard a greater weighting");
         return 2;
     }
 
@@ -526,7 +472,8 @@ int thread_leader_data_validation(protocol_interface_info_entry_t *cur, thread_l
     if (!thread_info(cur)->thread_leader_data) {
         return -1;
     }
-    if (thread_info(cur)->thread_leader_data->partitionId != leaderData->partitionId) {
+    if ((thread_info(cur)->thread_leader_data->partitionId != leaderData->partitionId) ||
+        (thread_info(cur)->thread_leader_data->weighting != leaderData->weighting)) {
         uint8_t routers_in_route_tlv = thread_get_router_count_from_route_tlv(routeTlv);
         //partition checks
         return thread_bootstrap_partition_process(cur,routers_in_route_tlv,leaderData, routeTlv);
@@ -535,10 +482,10 @@ int thread_leader_data_validation(protocol_interface_info_entry_t *cur, thread_l
     //Should check is there new version numbers
     if (common_serial_number_greater_8(leaderData->dataVersion, thread_info(cur)->thread_leader_data->dataVersion) ||
         common_serial_number_greater_8(leaderData->stableDataVersion, thread_info(cur)->thread_leader_data->stableDataVersion)) {
-        // Version number increased
+        // Version number increased by some-one else -> there is leader in the network
         if (thread_info(cur)->leader_private_data) {
-            tr_error("SEq synch error");
-            // MUST  restart partition
+            tr_error("Another leader detected -> bootstrap");
+            thread_bootstrap_reset_restart(cur->id);
             return -1;
             }
         tr_debug("NEW Network Data available");
@@ -790,7 +737,9 @@ static void thread_bootstrap_ml_address_update(protocol_interface_info_entry_t *
     }
 
     // Set new ML-EID and ULA prefix
-    memcpy(cur->iid_slaac, conf->mesh_local_eid, 8);
+    uint8_t *ml_eid = thread_joiner_application_ml_eid_get(cur->id);
+    memcpy(cur->iid_slaac, ml_eid, 8);
+
     arm_thread_private_ula_prefix_set(cur, conf->mesh_local_ula_prefix);
 
     // Generate new ML64 address
@@ -864,7 +813,7 @@ int thread_link_configuration_activate(protocol_interface_info_entry_t *cur, lin
         return -1;
     }
 
-    if (thread_configuration_mac_activate(cur, linkConfiguration->rfChannel, linkConfiguration->panId,linkConfiguration->extended_random_mac)) {
+    if (thread_configuration_mac_activate(cur, linkConfiguration->rfChannel, linkConfiguration->panId,thread_joiner_application_random_mac_get(cur->id))) {
         return -1;
     }
 
@@ -1153,10 +1102,12 @@ void thread_tasklet(arm_event_s *event)
             thread_router_bootstrap_active_router_attach(cur);
             thread_bootstrap_child_id_request(cur);
             if (thread_nd_own_service_list_data_size(&cur->thread_info->localServerDataBase)) {
-                // We publish our services if we have some BUG leader cannot remove old ones
+                // publish our services to allow leader to remove old ones
                 thread_border_router_publish(cur->id);
             }
             thread_router_bootstrap_address_change_notify_send(cur);
+            // Validate network data after a short period
+            thread_border_router_resubmit_timer_set(cur->id, 5);
             break;
         case THREAD_ATTACH_ROUTER_ID_GET_FAIL:
             tr_debug_extra("Thread SM THREAD_ATTACH_ROUTER_ID_GET_FAIL");
@@ -1171,7 +1122,7 @@ void thread_tasklet(arm_event_s *event)
         case THREAD_ATTACH_ROUTER_ID_RELEASED:
             tr_debug_extra("Thread SM THREAD_ATTACH_ROUTER_ID_RELEASED");
             if (thread_nd_own_service_list_data_size(&cur->thread_info->localServerDataBase)) {
-                // We publish our services if we have some BUG leader cannot remove old ones
+                // publish our services to allow leader to remove old ones
                 thread_border_router_publish(cur->id);
             }
             break;
@@ -1342,6 +1293,7 @@ static int thread_bootstrap_attach_start(int8_t interface_id, thread_bootsrap_st
             tr_debug("Thread ReAttach");
             //save the current partition id and sequence number before trying reattach
             cur->thread_info->previous_partition_info.partitionId = cur->thread_info->thread_leader_data->partitionId;
+            cur->thread_info->previous_partition_info.weighting = cur->thread_info->thread_leader_data->weighting;
             cur->thread_info->previous_partition_info.idSequence = cur->thread_info->routing.router_id_sequence;
             cur->thread_info->routerShortAddress = mac_helper_mac16_address_get(cur);
             if(cur->thread_info->thread_attached_state != THREAD_STATE_REATTACH_RETRY){
@@ -1410,9 +1362,9 @@ static void thread_bootstrap_generate_leader_and_link(protocol_interface_info_en
 static int8_t thread_bootstrap_attempt_attach_with_pending_set(protocol_interface_info_entry_t *cur)
 {
     tr_debug("Attempting to attach with pending set");
-    uint32_t pending_timestamp = thread_joiner_application_pending_config_timeout_get(cur->id);
-    if (pending_timestamp > 0) {
-        tr_debug("We have a pending timestamp running");
+    uint32_t pending_delay_timer = thread_joiner_application_pending_config_timeout_get(cur->id);
+    if (pending_delay_timer > 0 && thread_joiner_application_pending_delay_timer_in_sync(cur->id)) {
+        tr_debug("We have a pending delay timer running");
         //we already have a pending set that can be activated so return
         return -1;
     }
@@ -1769,7 +1721,11 @@ void thread_bootstrap_attached_finish(protocol_interface_info_entry_t *cur)
     if (cur->thread_info->releaseRouterId) {
         thread_router_bootstrap_router_id_release(cur);
     }
-    thread_nvm_store_link_info_file_write(cur);
+    uint8_t *parent_mac_addr = NULL;
+    if (cur->thread_info->thread_endnode_parent) {
+        parent_mac_addr = cur->thread_info->thread_endnode_parent->mac64;
+    }
+    thread_nvm_store_link_info_write(parent_mac_addr, mac_helper_mac16_address_get(cur));
     thread_bootstrap_ready(cur);
 
     if(thread_is_router_addr(mac_helper_mac16_address_get(cur))) {
@@ -2219,10 +2175,15 @@ static bool thread_bootstrap_sync_after_reset_start(protocol_interface_info_entr
     uint16_t my_short_address;
     uint8_t parent_mac64[8];
 
-    if (!thread_nvm_store_link_info_get(parent_mac64, &my_short_address)) {
+    int link_info_err = thread_nvm_store_link_info_get(parent_mac64, &my_short_address);
+    if ( link_info_err!= THREAD_NVM_FILE_SUCCESS) {
+        tr_warning("thread_nvm_store_link_info_get returned %d", link_info_err);
         return false;
     }
-    thread_nvm_store_link_info_clear();
+    link_info_err = thread_nvm_store_link_info_clear();
+    if ( link_info_err!= THREAD_NVM_FILE_SUCCESS) {
+        tr_warning("thread_nvm_store_link_info_clear returned %d", link_info_err);
+        }
     if (thread_is_router_addr(my_short_address)) {
         thread_info(cur)->routerShortAddress = my_short_address;
         thread_dynamic_storage_build_mle_table(cur->id);
@@ -2230,6 +2191,7 @@ static bool thread_bootstrap_sync_after_reset_start(protocol_interface_info_entr
         return true;
     }
     if (!thread_parent_data_allocate(cur->thread_info)) {
+        tr_info("parent alloc failed");
         return false;
     }
 
@@ -2292,16 +2254,6 @@ void thread_bootstrap_state_machine(protocol_interface_info_entry_t *cur)
             tr_debug("Thread SM:Active Scan");
 
             thread_joiner_application_nvm_link_configuration_load(cur->id);
-
-            if (thread_joiner_application_nvm_operation_in_progress(cur->id)) {
-                /*
-                 *  joiner application has pending NVM operation in progress,
-                 *  wait it to complete before continuing startup
-                 */
-                cur->bootsrap_state_machine_cnt = 1;
-                return;
-            }
-
             linkConfiguration = thread_joiner_application_get_config(cur->id);
             if (!linkConfiguration) {
                 thread_bootstrap_start_network_discovery(cur);
@@ -2310,7 +2262,7 @@ void thread_bootstrap_state_machine(protocol_interface_info_entry_t *cur)
 
             //SET Link by Static configuration
             tr_info("thread network attach start");
-            if (thread_mle_service_register(cur->id,linkConfiguration->extended_random_mac) != 0 ||
+            if (thread_mle_service_register(cur->id,thread_joiner_application_random_mac_get(cur->id)) != 0 ||
                 thread_link_configuration_activate(cur, linkConfiguration) != 0) {
                 tr_error("Network Bootsrap Start Fail");
                 bootsrap_next_state_kick(ER_BOOTSTRAP_SCAN_FAIL, cur);
@@ -3021,10 +2973,10 @@ void thread_bootstrap_dynamic_configuration_save(protocol_interface_info_entry_t
     // in error situation this returns 0 !!!!
     uint32_t mle_frame_counter = mle_service_security_get_frame_counter(cur->id);
     if (linkConfiguration) {
-            thread_nvm_store_fast_data_check_and_store(mac_frame_counter, mle_frame_counter, linkConfiguration->key_sequence);
+            thread_nvm_store_fast_data_check_and_write(mac_frame_counter, mle_frame_counter, linkConfiguration->key_sequence);
     }
     else {
-        thread_nvm_store_frame_counters_check_and_store(mac_frame_counter, mle_frame_counter);
+        thread_nvm_store_frame_counters_check_and_write(mac_frame_counter, mle_frame_counter);
     }
 }
 
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_border_router_api.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_border_router_api.c
index a9d6f9ca0a..00bdc84841 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_border_router_api.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_border_router_api.c
@@ -82,6 +82,7 @@ typedef struct {
 /* Neighbor discovery options according to RFC6106 (rfc4861) */
 #define RFC6106_RECURSIVE_DNS_SERVER_OPTION     25
 #define RFC6106_DNS_SEARCH_LIST_OPTION          31
+
 static NS_LIST_DEFINE(border_router_instance_list, thread_border_router_t, link);
 
 
@@ -296,6 +297,59 @@ static bool thread_border_router_local_network_data_prefix_match(thread_network_
     return true;
 }
 
+static void thread_border_router_child_network_data_clean(uint8_t interface_id, uint16_t child_id)
+{
+    uint8_t addr16_buf[2];
+
+    common_write_16_bit(child_id, addr16_buf);
+    if (mle_class_get_by_link_address(interface_id, addr16_buf, ADDR_802_15_4_SHORT)) {
+        /* Child is available in mle, do nothing */
+        return;
+    }
+
+    // Child is not our child => network data contains data from lost children, remove it
+    tr_debug("Remove nwk data from lost child: %04x", child_id);
+    thread_management_client_network_data_unregister(interface_id, child_id);
+}
+
+static void thread_border_router_lost_children_nwk_data_validate(protocol_interface_info_entry_t *cur, uint16_t router_short_addr)
+{
+    if (!thread_is_router_addr(router_short_addr)) {
+        // not validating children nwk data
+        return;
+    }
+
+    thread_network_data_cache_entry_t *network_data = &cur->thread_info->networkDataStorage;
+
+    ns_list_foreach(thread_network_data_prefix_cache_entry_t, curLP, &network_data->localPrefixList) {
+        /* Go throgh all routes */
+        ns_list_foreach(thread_network_server_data_entry_t, curRoute, &curLP->routeList) {
+            if (thread_addr_is_child(router_short_addr, curRoute->routerID)) {
+                // Router children found
+                thread_border_router_child_network_data_clean(cur->id, curRoute->routerID);
+            }
+        }
+
+        /* Go through all BR's */
+        ns_list_foreach(thread_network_server_data_entry_t, curBR, &curLP->borderRouterList) {
+            if (thread_addr_is_child(router_short_addr, curBR->routerID)) {
+                // Router children found
+                thread_border_router_child_network_data_clean(cur->id, curBR->routerID);
+            }
+        }
+    }
+
+    /* Go throgh all services */
+    ns_list_foreach(thread_network_data_service_cache_entry_t, service, &network_data->service_list) {
+        ns_list_foreach(thread_network_data_service_server_entry_t, server, &service->server_list) {
+            if (thread_addr_is_child(router_short_addr, server->router_id)) {
+                // Router children found
+                thread_border_router_child_network_data_clean(cur->id, server->router_id);
+            }
+        }
+    }
+}
+
 static bool thread_border_router_local_network_data_service_match(thread_network_local_data_cache_entry_t *local_data, thread_network_data_service_cache_entry_t *service, uint16_t router_id)
 {
     bool instance_found = false;
@@ -371,9 +425,12 @@ static bool thread_border_router_local_srv_data_in_network_data_check(protocol_i
         }
     }
 
+    thread_border_router_lost_children_nwk_data_validate(cur, router_id);
+
     return true;
 }
 
+#ifdef HAVE_THREAD_BORDER_ROUTER
 static int thread_border_router_recursive_dns_server_option_store(int8_t interface_id, uint8_t *recursive_dns_server_option, uint16_t recursive_dns_server_option_len)
 {
     thread_border_router_t *this = thread_border_router_find_by_interface(interface_id);
@@ -393,7 +450,9 @@ static int thread_border_router_recursive_dns_server_option_store(int8_t interfa
     }
     return 0;
 }
+#endif
 
+#ifdef HAVE_THREAD_BORDER_ROUTER
 static int thread_border_router_dns_search_list_option_store(int8_t interface_id, uint8_t *dns_search_list_option, uint16_t search_list_option_len)
 {
     thread_border_router_t *this = thread_border_router_find_by_interface(interface_id);
@@ -412,6 +471,7 @@ static int thread_border_router_dns_search_list_option_store(int8_t interface_id
     }
     return 0;
 }
+#endif
 
 int8_t thread_border_router_init(int8_t interface_id)
 {
@@ -457,6 +517,7 @@ void thread_border_router_delete(int8_t interface_id)
     ns_dyn_mem_free(this->recursive_dns_server_option);
     ns_dyn_mem_free(this);
 }
+
 void thread_border_router_seconds_timer(int8_t interface_id, uint32_t seconds)
 {
     thread_border_router_t *this = thread_border_router_find_by_interface(interface_id);
@@ -783,17 +844,19 @@ static void thread_tmf_client_network_data_set_cb(int8_t interface_id, int8_t st
     if (!cur) {
         return;
     }
-    // Save the old address
-    cur->thread_info->localServerDataBase.registered_rloc16 = mac_helper_mac16_address_get(cur);
-    cur->thread_info->localServerDataBase.release_old_address = false;
+
     cur->thread_info->localServerDataBase.publish_active = false;
 
-    tr_debug("border router status %s, addr: %x",status?"Fail":"Ok", cur->thread_info->localServerDataBase.registered_rloc16);
+    tr_debug("BR a/sd response status: %s, addr: %x",status?"Fail":"OK", cur->thread_info->localServerDataBase.registered_rloc16);
 
     if (cur->thread_info->localServerDataBase.publish_pending) {
         cur->thread_info->localServerDataBase.publish_pending = false;
         thread_border_router_publish(cur->id);
     }
+
+    // always update RLOC to new one. If COAP response fails then resubmit timer will trigger new a/sd
+    cur->thread_info->localServerDataBase.registered_rloc16 = mac_helper_mac16_address_get(cur);
+    cur->thread_info->localServerDataBase.release_old_address = false;
 }
 #endif
 
@@ -817,6 +880,9 @@ int thread_border_router_publish(int8_t interface_id)
         return -2;
     }
 
+    rloc16 = mac_helper_mac16_address_get(cur);
+    tr_debug("Border router old: %x, new: %x", cur->thread_info->localServerDataBase.registered_rloc16, rloc16);
+
     if (cur->thread_info->localServerDataBase.publish_active) {
         cur->thread_info->localServerDataBase.publish_pending = true;
         tr_debug("Activate pending status for publish");
@@ -831,20 +897,18 @@ int thread_border_router_publish(int8_t interface_id)
     if (!ptr) {
         return -3;
     }
-    rloc16 = mac_helper_mac16_address_get(cur);
 
     ptr = thread_tmfcop_tlv_data_write_header(ptr, TMFCOP_TLV_NETWORK_DATA, network_data_len);
     ptr = thread_nd_own_service_list_data_write(&cur->thread_info->localServerDataBase, ptr, rloc16);
 
-    tr_debug("Border router old: %x, new: %x", cur->thread_info->localServerDataBase.registered_rloc16, rloc16);
-
     if (cur->thread_info->localServerDataBase.registered_rloc16 != 0xffff &&
             cur->thread_info->localServerDataBase.release_old_address &&
             cur->thread_info->localServerDataBase.registered_rloc16 != rloc16) {
         // Our address has changed so we must register our network with new address and remove the old address
-        tr_debug("Border router Address changed remove old");
+        tr_debug("BR address changed - remove old %x", cur->thread_info->localServerDataBase.registered_rloc16);
         ptr = thread_tmfcop_tlv_data_write_uint16(ptr,TMFCOP_TLV_RLOC16,cur->thread_info->localServerDataBase.registered_rloc16);
     }
+
     cur->thread_info->localServerDataBase.registered_rloc16 = rloc16;
     ret_val = thread_management_client_network_data_register(cur->id, payload_ptr, ptr - payload_ptr, thread_tmf_client_network_data_set_cb);
     if (payload_ptr) {
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.c
index 5ff34b204f..1ef5e4e60d 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.c
@@ -245,7 +245,8 @@ int8_t thread_bootstrap_down(protocol_interface_info_entry_t *cur)
         }
         mac_helper_link_frame_counter_read(cur->id, &fast_data.mac_frame_counter);
         fast_data.mle_frame_counter=mle_service_security_get_frame_counter(cur->id);
-        thread_nvm_store_fast_data_store(&fast_data);
+        thread_nvm_store_fast_data_write(&fast_data);
+        thread_joiner_application_configuration_nvm_save(cur->id);
         mac_pairwise_key_flush_list(cur->id);
         thread_discovery_reset(cur->id);
         thread_bootstrap_stop(cur);
@@ -981,6 +982,7 @@ void thread_seconds_timer(protocol_interface_info_entry_t *cur, uint32_t ticks)
     thread_border_router_seconds_timer(cur->id, ticks);
     thread_bbr_seconds_timer(cur->id, ticks);
     thread_lowpower_timer(cur, ticks);
+    thread_nvm_store_seconds_timer(ticks);
 
     if (cur->lowpan_info & INTERFACE_NWK_BOOTSRAP_ACTIVE) {
         nwk_bootsrap_state_update(ARM_NWK_BOOTSTRAP_READY, cur);
@@ -1658,7 +1660,6 @@ bool thread_pending_operational_dataset_process(protocol_interface_info_entry_t
         tr_error("pending set creation failed");
         return false;
     }
-
     tr_debug("updating pending dataset");
     thread_joiner_application_pending_config_timestamp_set(cur->id,mle_pending_timestamp);
     thread_joiner_application_pending_config_enable(cur->id,delay_timer);
@@ -1836,36 +1837,27 @@ static void thread_tx_failure_handler(int8_t nwk_id, uint8_t accumulated_failure
         return;
     }
 
-    if (thread_i_am_router(cur)) {
-       if (thread_addr_is_child(mac_helper_mac16_address_get(cur), neighbor->short_adr)) {
-           if (accumulated_failures < THREAD_MAC_TRANSMISSIONS*THREAD_FAILED_CHILD_TRANSMISSIONS) {
-               return;
-           }
+    if (accumulated_failures >= THREAD_MAC_TRANSMISSIONS*THREAD_FAILED_CHILD_TRANSMISSIONS) {
+        thread_reset_neighbour_info(cur, neighbor);
+    }
+}
 
-           tr_debug("Free the Child node, mac16=%d", neighbor->short_adr);
-           thread_bootstrap_reset_child_info(cur, neighbor);
+/* Called when MLE link to neighbour lost, or ETX callback says link is bad */
+void thread_reset_neighbour_info(protocol_interface_info_entry_t *cur, mle_neigh_table_entry_t *neighbour)
+{
+    thread_parent_info_t *thread_endnode_parent = thread_info(cur)->thread_endnode_parent;
 
-           protocol_6lowpan_release_short_link_address_from_neighcache(cur, neighbor->short_adr);
-           protocol_6lowpan_release_long_link_address_from_neighcache(cur, neighbor->mac64);
-           mac_helper_devicetable_remove(cur->mac_api, neighbor->attribute_index);
-       } else if (thread_is_router_addr(neighbor->short_adr)) {
-           if (accumulated_failures < THREAD_MAC_TRANSMISSIONS*THREAD_FAILED_ROUTER_TRANSMISSIONS) {
-               return;
-           }
+    if (!thread_i_am_router(cur) && thread_endnode_parent && thread_endnode_parent->shortAddress == neighbour->short_adr) {
+        tr_warn("End device lost Parent!\n");
+        if(cur->nwk_bootstrap_state != ER_CHILD_ID_REQ) {
+            thread_bootstrap_connection_error(cur->id, CON_PARENT_CONNECT_DOWN, NULL);
+        }
+    }
 
-           tr_debug("Set link quality to neighbor router to zero...");
-           thread_routing_force_link_margin(cur, neighbor->short_adr, 0);
-       }
-   } else { // We are a Child
-       if (thread_check_is_this_my_parent(cur, neighbor)) {
-           if (accumulated_failures < THREAD_MAC_TRANSMISSIONS*THREAD_FAILED_CHILD_TRANSMISSIONS) {
-               return;
-           }
-
-           tr_debug("Consider the parent gone...");
-           thread_bootstrap_connection_error(cur->id, CON_PARENT_CONNECT_DOWN, NULL);
-       }
-   }
+    thread_routing_remove_link(cur, neighbour->short_adr);
+    thread_router_bootstrap_reset_child_info(cur, neighbour);
+    protocol_6lowpan_release_long_link_address_from_neighcache(cur, neighbour->mac64);
+    mac_helper_devicetable_remove(cur->mac_api, neighbour->attribute_index);
 }
 
 uint8_t thread_get_router_count_from_route_tlv(mle_tlv_info_t *routeTlv)
@@ -1928,14 +1920,17 @@ void thread_mcast_group_change(struct protocol_interface_info_entry *interface,
     }
 }
 
-void thread_old_partition_data_purge(thread_info_t *thread_info)
+void thread_old_partition_data_purge(protocol_interface_info_entry_t *cur)
 {
     /* Partition has been changed. Wipe out data related to old partition */
-    thread_management_client_pending_coap_request_kill(thread_info->interface_id);
+    thread_management_client_pending_coap_request_kill(cur->id);
+
+    /* Reset previous routing information */
+    thread_routing_reset(&cur->thread_info->routing);
+
+    /* Flush address cache */
+    ipv6_neighbour_cache_flush(&cur->ipv6_neighbour_cache);
 
-    /* Reset/init previous routing information */
-    thread_routing_reset(&thread_info->routing);
-    thread_routing_init(&thread_info->routing);
 }
 
 #endif
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.h
index 86cdc1a6eb..9cadc1b5ed 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.h
@@ -70,9 +70,9 @@ extern uint16_t thread_joiner_port;
 
 typedef enum {
     THREAD_STATE_NETWORK_DISCOVER,      // Not commissioned to Thread network
-    THREAD_STATE_REATTACH,              // Connected to thread network, searching for better partition
-    THREAD_STATE_REATTACH_RETRY,        // Connected to thread network, searching for better partition with REED bit is set
-    THREAD_STATE_ATTACH_ANY,            // Connected to thread network, searching for all partitions with leader connectivity
+    THREAD_STATE_REATTACH,              // Connection to leader lost, searching for new parent
+    THREAD_STATE_REATTACH_RETRY,        // Connection to leader lost, searching for new parent with REED bit is set
+    THREAD_STATE_ATTACH_ANY,            // Searching for all partitions with leader connectivity
     THREAD_STATE_CONNECTED,             // Attached to Thread network - can't route
     THREAD_STATE_CONNECTED_ROUTER,      // Attached to Thread network - Routing enabled
 } thread_attach_state_e;
@@ -253,6 +253,7 @@ struct thread_extension_credentials;
 typedef struct thread_previous_partition_info_s {
     uint32_t partitionId; //partition ID of the previous partition
     uint8_t idSequence;   //idSequence last heard from the previous partition
+    uint8_t weighting;    //weighting last heard from the previous partition
 } thread_previous_partition_t;
 
 
@@ -294,6 +295,7 @@ typedef struct thread_info_s {
     uint8_t version;
     uint8_t testMaxActiveRouterIdLimit; //Default for this is 32
     uint8_t maxChildCount; //Default for this is 24
+    uint8_t partition_weighting;
     bool rfc6775: 1;
     bool requestFullNetworkData: 1;
     bool leaderCab: 1;
@@ -302,6 +304,7 @@ typedef struct thread_info_s {
     bool networkDataRequested: 1;
     bool end_device_link_synch: 1;
     bool router_mc_addrs_registered: 1;
+    bool leader_synced:1; // flag used by leader after restart
 } thread_info_t;
 
 #ifdef HAVE_THREAD
@@ -356,6 +359,7 @@ uint16_t thread_network_data_generate_stable_set(protocol_interface_info_entry_t
 
 void thread_set_active_router(protocol_interface_info_entry_t *cur, if_address_entry_t *address_entry, uint8_t *routerId);
 uint8_t thread_get_router_count_from_route_tlv(mle_tlv_info_t *routeTlv);
+void thread_reset_neighbour_info(protocol_interface_info_entry_t *cur, mle_neigh_table_entry_t *neighbour);
 
 void thread_child_id_request_entry_clean(protocol_interface_info_entry_t *cur);
 thread_pending_child_id_req_t *thread_child_id_request_entry_get(protocol_interface_info_entry_t *cur, uint8_t *euid64);
@@ -408,7 +412,7 @@ uint8_t thread_pending_timestamp_tlv_size(protocol_interface_info_entry_t *cur);
 void thread_calculate_key_guard_timer(protocol_interface_info_entry_t *cur, link_configuration_s *linkConfiguration, bool is_init);
 void thread_set_link_local_address(protocol_interface_info_entry_t *cur);
 void thread_mcast_group_change(struct protocol_interface_info_entry *interface, struct if_group_entry *group, bool group_added);
-void thread_old_partition_data_purge(thread_info_t *thread_info);
+void thread_old_partition_data_purge(protocol_interface_info_entry_t *cur);
 
 #else // HAVE_THREAD
 
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_config.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_config.h
index 9ddfb15a82..55243dc4a8 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_config.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_config.h
@@ -237,12 +237,12 @@
  * least one (1) 106-octet IPv6 datagram per attached SED".
  *
  * The defines below tell how many small (i.e. up to the big packet
- * threshold) packets total and big (i.e. over the big packet threshold)
- * packets per sleepy child we buffer in the indirect TX queue. The
- * minimum values are 1 for both, but here we use 2 for better
- * performance.
+ * threshold) packets per sleepy child and big (i.e. over the big 
+ * packet threshold) packets total we buffer in the indirect TX 
+ * queue. The minimum values are 1 for both, but here we use larger
+ * value for better performance.
  */
-#define THREAD_INDIRECT_BIG_PACKETS_TOTAL 2
+#define THREAD_INDIRECT_BIG_PACKETS_TOTAL 10
 #define THREAD_INDIRECT_SMALL_PACKETS_PER_CHILD 2
 
 /**
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_discovery.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_discovery.c
index 82af6acce6..ab552ccd17 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_discovery.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_discovery.c
@@ -559,7 +559,7 @@ static int thread_discovery_response_send(thread_discovery_class_t *class, threa
     *ptr++ = message_length;
     uint16_t discover_response_tlv = thread_discover_tlv_get(class->version, (linkConfiguration->securityPolicy & SECURITY_POLICY_NATIVE_COMMISSIONING_ALLOWED));
 
-    discover_response_tlv = thread_extension_discover_response_tlv_write(discover_response_tlv, class->version, thread_extension_security_policy_enabled(linkConfiguration->securityPolicy));
+    thread_extension_discover_response_tlv_write(&discover_response_tlv, class->version, linkConfiguration->securityPolicy);
 
     ptr = thread_meshcop_tlv_data_write_uint16(ptr, MESHCOP_TLV_DISCOVERY_RESPONSE, discover_response_tlv);
     ptr = thread_meshcop_tlv_data_write(ptr, MESHCOP_TLV_XPANID, 8, linkConfiguration->extented_pan_id);
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_extension.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_extension.h
index fc4eaf461f..8e7a555e01 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_extension.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_extension.h
@@ -53,7 +53,7 @@ struct discovery_response_list;
 #define thread_extension_mcast_subscrition_change(interface, group, added)
 #define thread_extension_route_set(cur)
 #define thread_extension_activate(cur)
-#define thread_extension_security_policy_enabled(securityPolicy) (false)
+#define thread_extension_enabled(cur) (false)
 #define thread_extension_version_check(version) (false)
 #define thread_extension_discover_response_read(nwk_info, discover_response_tlv, data_ptr, data_len)
 #define thread_extension_discover_response_tlv_write(data, version, extension_bit) (data)
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_host_bootstrap.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_host_bootstrap.c
index c8afa75abd..9f5e5bd5dc 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_host_bootstrap.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_host_bootstrap.c
@@ -63,7 +63,6 @@
 #include "6LoWPAN/Thread/thread_management_client.h"
 #include "6LoWPAN/Thread/thread_network_data_lib.h"
 #include "6LoWPAN/Thread/thread_tmfcop_lib.h"
-#include "6LoWPAN/Thread/thread_nvm_store.h"
 #include "thread_management_if.h"
 #include "Common_Protocols/ipv6.h"
 #include "MPL/mpl.h"
@@ -231,7 +230,9 @@ static int thread_parent_request_build(protocol_interface_info_entry_t *cur)
     }
 
     if (cur->thread_info->thread_attached_state == THREAD_STATE_REATTACH ||
-        cur->thread_info->thread_attached_state == THREAD_STATE_REATTACH_RETRY) {
+        cur->thread_info->thread_attached_state == THREAD_STATE_REATTACH_RETRY ||
+        cur->thread_info->thread_attached_state == THREAD_STATE_CONNECTED ||
+        cur->thread_info->thread_attached_state == THREAD_STATE_CONNECTED_ROUTER) {
         // When doing re-attach End devices are immediately accepted as parents
         scanMask |= 0x40;
     }
@@ -408,9 +409,9 @@ static void thread_child_synch_receive_cb(int8_t interface_id, mle_message_t *ml
         tr_debug("End device synch Possible");
 
         cur->thread_info->thread_attached_state = THREAD_STATE_CONNECTED;
-
+        // read network data, and leader data check. Send data request sent if pending set is not in sync
         if (mle_tlv_read_tlv(MLE_TYPE_NETWORK_DATA, mle_msg->data_ptr, mle_msg->data_length, &networkDataTlv) &&
-            thread_leader_data_parse(mle_msg->data_ptr, mle_msg->data_length, &leaderData)) {
+            thread_leader_data_parse(mle_msg->data_ptr, mle_msg->data_length, &leaderData) && thread_joiner_application_pending_delay_timer_in_sync(cur->id)) {
             thread_bootstrap_network_data_save(cur, &leaderData, networkDataTlv.dataPtr, networkDataTlv.tlvLen);
         } else {
             thread_bootstrap_parent_network_data_request(cur, true);
@@ -533,7 +534,9 @@ void thread_mle_parent_discover_receive_cb(int8_t interface_id, mle_message_t *m
             if (thread_info(cur)->thread_attached_state == THREAD_STATE_REATTACH || thread_info(cur)->thread_attached_state == THREAD_STATE_REATTACH_RETRY) {
                 tr_debug("Reattach");
                 if (thread_info(cur)->thread_leader_data) {
-                    if (thread_info(cur)->thread_leader_data->partitionId != leaderData.partitionId) { //accept only same ID at reattach phase
+                    if ((thread_info(cur)->thread_leader_data->partitionId != leaderData.partitionId) ||
+                        (thread_info(cur)->thread_leader_data->weighting != leaderData.weighting)) {
+                        //accept only same ID at reattach phase
                         return;
                     }
                     //Compare ID - when downgrading, accept all
@@ -550,7 +553,9 @@ void thread_mle_parent_discover_receive_cb(int8_t interface_id, mle_message_t *m
                     thread_info(cur)->thread_attached_state == THREAD_STATE_CONNECTED ||
                     thread_info(cur)->thread_attached_state == THREAD_STATE_CONNECTED_ROUTER) {
                 if (thread_info(cur)->thread_leader_data) {
-                    if (thread_info(cur)->thread_leader_data->partitionId == leaderData.partitionId) { //accept only different ID at anyattach phase
+                    if ((thread_info(cur)->thread_leader_data->partitionId == leaderData.partitionId) &&
+                        (thread_info(cur)->thread_leader_data->weighting == leaderData.weighting)) {
+                        //accept only different ID at anyattach phase
                         tr_debug("Drop old partition");
                         return;
                     }
@@ -585,6 +590,13 @@ void thread_mle_parent_discover_receive_cb(int8_t interface_id, mle_message_t *m
                 }
             }
 
+            if (thread_extension_enabled(cur) &&
+                thread_info(cur)->thread_device_mode == THREAD_DEVICE_MODE_ROUTER &&
+                leaderData.weighting < thread_info(cur)->partition_weighting) {
+                // Only applies to extensions and only routers that can form new partitions can ignore lower weight
+                tr_debug("Drop parent due weighting %d<%d", leaderData.weighting, thread_info(cur)->partition_weighting);
+                return;
+            }
 
             if (accept_response) {
                 if (thread_info(cur)->thread_attach_scanned_parent == NULL) {
@@ -597,12 +609,17 @@ void thread_mle_parent_discover_receive_cb(int8_t interface_id, mle_message_t *m
                     tr_debug("Partition %"PRIu32, leaderData.partitionId);
                 } else {
                     uint32_t currentPartitionId = thread_info(cur)->thread_attach_scanned_parent->leader_data.partitionId;
-                    tr_debug("Current %"PRIu32" RX %"PRIu32, currentPartitionId, leaderData.partitionId);
+                    uint8_t currentWeighting = thread_info(cur)->thread_attach_scanned_parent->leader_data.weighting;
+                    tr_debug("Current partition %"PRIu32" old:%"PRIu32" weighting %"PRIu8" old:%"PRIu8,
+                        currentPartitionId, leaderData.partitionId, currentWeighting, leaderData.weighting);
 
-                    if (leaderData.partitionId != currentPartitionId) {
+                    if ((leaderData.partitionId != currentPartitionId) ||
+                        (leaderData.weighting != currentWeighting)) {
                         int retVal = thread_bootstrap_partition_process(cur, connectivityTlv.activeRouters, &leaderData,NULL);
-                        if (retVal > 0)
+                        if (retVal > 0) {
+                            // New partition is Higher
                             scan_result = thread_info(cur)->thread_attach_scanned_parent;
+                        }
                     }
                     else if (leaderData.partitionId == currentPartitionId) {
                         thread_link_quality_e currentLqi;
@@ -820,6 +837,9 @@ static void thread_mle_child_request_receive_cb(int8_t interface_id, mle_message
             thread_info(cur)->thread_attached_state = THREAD_STATE_CONNECTED;
 
             thread_bootstrap_update_ml16_address(cur, childId);
+            if (!thread_is_router_addr(thread_info(cur)->routerShortAddress)) {
+                thread_info(cur)->routerShortAddress = 0xfffe;
+            }
 
             mle_service_msg_free(scan_result->child_id_request_id);
 
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.c
index 72e7d802b8..231502f058 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.c
@@ -215,26 +215,19 @@ typedef struct {
     bool configuration_valid: 1; //TODO this should???? be put in link configuration
     bool finalisation_done: 1;
     bool nvm_link_configuration_load: 1; // load link configuration settings from NVM in restart
-    uint8_t nvm_operation_count;
     int8_t interface_id;
     int8_t coap_service_id;
     int8_t secure_coap_service_id;
+    bool pending_set_in_sync:1;
     ns_list_link_t link;
 } thread_joiner_t;
 
-typedef struct {
-    uint8_t *nvm_buffer;
-    uint16_t nvm_buffer_length;
-    int8_t interface_id;
-} nvm_data_t;
 
 static NS_LIST_DEFINE(instance_list, thread_joiner_t, link);
 
-static const char *thread_nvm_key_static_link_conf = "com.arm.nanostack.thread.static_link_cfg";
-
 static int thread_joiner_application_nvm_link_config_read(thread_joiner_t *this);
-static int thread_joiner_application_nvm_link_config_write(thread_joiner_t *this, configuration_set_t *configuration_ptr);
-static void thread_joiner_application_validate_settings(thread_joiner_t *this);
+static bool thread_joiner_application_validate_settings(thread_joiner_t *this);
+static uint8_t *thread_joiner_application_write_channel(uint8_t *ptr, uint16_t data);
 
 static int stringlen(const char *s, int n)
 {
@@ -335,14 +328,11 @@ static void link_configuration_copy(link_configuration_s *this, link_configurati
     if (!this || !configuration_ptr) {
         return;
     }
-    //@TODO Totaly wrong place
     memcpy(this->name, configuration_ptr->name, 16);
     memcpy(this->PSKc, configuration_ptr->PSKc, 16);
     memcpy(this->master_key, configuration_ptr->master_key, 16);
     memcpy(this->mesh_local_ula_prefix, configuration_ptr->mesh_local_ula_prefix, 8);
-    memcpy(this->mesh_local_eid, configuration_ptr->mesh_local_eid, 8);
     memcpy(this->extented_pan_id, configuration_ptr->extented_pan_id, 8);
-    memcpy(this->extended_random_mac, configuration_ptr->extended_random_mac, 8);
     memcpy(this->channel_mask, configuration_ptr->channel_mask,5);
     this->key_rotation = configuration_ptr->key_rotation;
     this->key_sequence = configuration_ptr->key_sequence;
@@ -416,8 +406,6 @@ static void link_configuration_trace(link_configuration_s *this)
     }
     tr_debug("NwkName: %s", trace_array(this->name, 16));
     tr_debug("Mesh ula: %s", trace_array(this->mesh_local_ula_prefix, 8));
-    tr_debug("ML-EID: %s", trace_array(this->mesh_local_eid, 8));
-    tr_debug("Random MAC: %s", trace_array(this->extended_random_mac, 8));
     tr_debug("Extendend PanId: %s", trace_array(this->extented_pan_id, 8));
     tr_debug("panid: %"PRIu16", Channel:%"PRIu16", keyRot:%"PRIu16", keySeq:%"PRIu32, this->panId, this->rfChannel, this->key_rotation, this->key_sequence);
     return;
@@ -432,6 +420,7 @@ static device_configuration_s *device_configuration_create(void)
     memset(this, 0, sizeof(device_configuration_s));
     return this;
 }
+
 static void device_configuration_delete(device_configuration_s *this)
 {
     if (!this) {
@@ -451,6 +440,8 @@ static void device_configuration_copy(device_configuration_s *this, device_confi
         return;
     }
     memcpy(this->eui64, configuration_ptr->eui64,8);
+    memcpy(this->extended_random_mac, configuration_ptr->extended_random_mac,8);
+    memcpy(this->mesh_local_eid, configuration_ptr->mesh_local_eid,8);
     memcpy(this->vendor_stack_version, configuration_ptr->vendor_stack_version,6);
     //TODO: count PSKc instead and use that
     if( configuration_ptr->PSKd_ptr && configuration_ptr->PSKd_len > 0 ){
@@ -487,6 +478,8 @@ static void device_configuration_trace(device_configuration_s *this)
     if (!this) {
         return;
     }
+    tr_debug("Mesh local eid: %s", trace_array(this->mesh_local_eid, 8));
+    tr_debug("extended random: %s", trace_array(this->extended_random_mac, 8));
     tr_debug("uri: %s", this->provisioning_uri_ptr ? this->provisioning_uri_ptr: "(none)");
     tr_debug("name: %s", this->vendor_name_ptr);
     tr_debug("mode: %s", this->vendor_model_ptr);
@@ -721,10 +714,7 @@ static void configuration_set_generate(configuration_set_t *destination_ptr, lin
     }
     response_ptr = destination_ptr->data;
 
-    *response_ptr++ = MESHCOP_TLV_CHANNEL; // type
-    *response_ptr++ = 3; // length
-    *response_ptr++ = 0; // channel page
-    response_ptr = common_write_16_bit(configuration_ptr->rfChannel, response_ptr);
+    response_ptr = thread_joiner_application_write_channel(response_ptr, configuration_ptr->rfChannel);
     *response_ptr++ = MESHCOP_TLV_CHANNEL_MASK; // type
     *response_ptr++ = 6; // length
     *response_ptr++ = configuration_ptr->channel_page; // channel page
@@ -746,145 +736,52 @@ static void configuration_set_generate(configuration_set_t *destination_ptr, lin
     destination_ptr->length = response_ptr - destination_ptr->data;
 }
 
-void thread_joiner_application_lc_nvm_read_callback(int status, void *context)
+static bool configuration_set_validate(uint8_t *configuration_set, uint16_t configuration_set_len, bool make_full_validation)
 {
-    tr_debug("thread_joiner_application_lc_nvm_read_callback() status=%d, ctx=%p", status, context);
-    nvm_data_t *nvm_data_ptr = (nvm_data_t*)context;
-    thread_joiner_t *this = thread_joiner_find(nvm_data_ptr->interface_id);
-
-    if (!this) {
-        tr_error("Could not find instance for %d", nvm_data_ptr->interface_id);
-        ns_dyn_mem_free(nvm_data_ptr->nvm_buffer);
-        ns_dyn_mem_free(nvm_data_ptr);
-        return;
+    if (thread_meshcop_tlv_find(configuration_set, configuration_set_len, MESHCOP_TLV_NETWORK_MASTER_KEY, NULL) < 16 ||
+        thread_meshcop_tlv_find(configuration_set, configuration_set_len, MESHCOP_TLV_NETWORK_MESH_LOCAL_ULA, NULL) < 8 ||
+        thread_meshcop_tlv_find(configuration_set, configuration_set_len, MESHCOP_TLV_XPANID, NULL) < 8 ||
+        thread_meshcop_tlv_find(configuration_set, configuration_set_len, MESHCOP_TLV_NETWORK_NAME, NULL) == 0 ||
+        thread_meshcop_tlv_find(configuration_set, configuration_set_len, MESHCOP_TLV_PSKC, NULL) < 16 ) {
+            // Absolutely minimum we must have is master secret to attach.
+            // If commissioner wants to be connected we must have PSKc,Name,Xpanid
+            // If there is some fields missing we could attach, but timestamp must be set to 0 which will then cause synchronization
+            tr_debug("Not all TLv's included");
+            return false;
     }
 
-    this->nvm_operation_count--;
-
-    if (status == NS_NVM_OK) {
-        ns_dyn_mem_free(this->active_configuration_ptr);
-        this->active_configuration_ptr = (configuration_set_t*)nvm_data_ptr->nvm_buffer;
-        link_configuration_update(this->configuration_ptr, this->active_configuration_ptr->data, this->active_configuration_ptr->length);
-
-        thread_nvm_fast_data_t fast_data;
-        memset(&fast_data,0,sizeof(thread_nvm_fast_data_t));
-        int ret = thread_nvm_store_fast_data_read(&fast_data);
-        tr_info("From NVM");
-        tr_info("mac-counter %"PRIu32,fast_data.mac_frame_counter);
-        tr_info("mle-counter %"PRIu32,fast_data.mle_frame_counter);
-        tr_info("seq-counter %"PRIu32,fast_data.seq_counter);
-
-        if (!ret) {
-            // nvm working
-            this->configuration_ptr->key_sequence = fast_data.seq_counter;
-        }
-        else {
-            mac_helper_link_frame_counter_read(this->interface_id, &fast_data.mac_frame_counter);
-            fast_data.mle_frame_counter=mle_service_security_get_frame_counter(this->interface_id);
-        }
-
-        fast_data.mac_frame_counter += MAC_FRAME_COUNTER_LIMIT;
-        fast_data.mle_frame_counter += MLE_FRAME_COUNTER_LIMIT;
-        thread_nvm_store_fast_data_store(&fast_data);
-
-        thread_dynamic_storage_device_configuration_read(this->interface_id,this->configuration_ptr->extended_random_mac,this->configuration_ptr->mesh_local_eid);
-        thread_nvm_store_link_info_file_read();
-        thread_joiner_application_validate_settings(this);// Generate all random information
-        this->configuration_valid = true;
-        link_configuration_trace(this->configuration_ptr);
-
-        //Add Security to MLE service
-        uint8_t key_material[32];
-        uint8_t key_index;
-        //Define KEY's
-        thread_key_get(this->configuration_ptr->master_key, key_material, this->configuration_ptr->key_sequence);
-        key_index = THREAD_KEY_INDEX(this->configuration_ptr->key_sequence);
-        //Set Keys
-        // coverity[returned_null] for ignoring protocol_stack_interface_info_get_by_id NULl return
-        protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(this->interface_id);
-        mac_helper_security_default_key_set(cur, &key_material[16], key_index, MAC_KEY_ID_MODE_IDX);
-        mle_service_security_set_security_key(this->interface_id, key_material, key_index, true);
-        // update counters
-        mle_service_security_set_frame_counter(this->interface_id, fast_data.mle_frame_counter);
-        mac_helper_link_frame_counter_set(this->interface_id, fast_data.mac_frame_counter);
-
-        //the pending set needs to be read from NVM
-        if (thread_dynamic_storage_pending_configuration_exists(this->interface_id)) {
-            if (this->pending_configuration_ptr) {
-                ns_dyn_mem_free(this->pending_configuration_ptr);
-            }
-            this->pending_configuration_ptr = ns_dyn_mem_alloc(sizeof(configuration_set_t));
-            if (this->pending_configuration_ptr) {
-                thread_dynamic_storage_pending_configuration_read(this->interface_id, this->pending_configuration_ptr,sizeof(configuration_set_t));
-                this->pending_configuration_ptr->timeout_in_ms = 0; // We have lost the timer value
-            }
-        }
-    } else {
-        // data was not found or error during read
-        ns_dyn_mem_free(nvm_data_ptr->nvm_buffer);
-        tr_debug("No data in NVM read, err=%d", status);
-        if (this->configuration_valid == true) {
-            // The configuration is valid, but some values might be randomized we need to write those
-            thread_joiner_application_configuration_nvm_save(this->interface_id);
+    if (make_full_validation) {
+        if ((thread_meshcop_tlv_find(configuration_set, configuration_set_len, MESHCOP_TLV_CHANNEL, NULL) == 0) ||
+            (thread_meshcop_tlv_find(configuration_set, configuration_set_len, MESHCOP_TLV_PANID, NULL) == 0) ||
+            (thread_meshcop_tlv_find(configuration_set, configuration_set_len, MESHCOP_TLV_XPANID, NULL) == 0)) {
+                tr_debug("Not all TLv's included");
+                return false;
         }
     }
-
-    ns_dyn_mem_free(nvm_data_ptr);
+    return true;
 }
 
-void thread_joiner_application_nvm_write_callback(int status, void *context)
+static bool thread_joiner_application_validate_settings(thread_joiner_t *this)
 {
-    tr_debug("thread_joiner_application_nvm_write_callback() status=%d ctx=%p", status, context);
-    nvm_data_t *nvm_data_ptr = (nvm_data_t*)context;
-
-    thread_joiner_t *this = thread_joiner_find(nvm_data_ptr->interface_id);
-    if (this) {
-        this->nvm_operation_count--;
-    }
-
-    if (status != NS_NVM_OK) {
-        tr_error("Data write to NVM failed, err=%d", status);
-    }
-    ns_dyn_mem_free(nvm_data_ptr->nvm_buffer);
-    ns_dyn_mem_free(nvm_data_ptr);
-}
-
-void thread_joiner_application_nvm_delete_callback(int status, void *context)
-{
-    tr_debug("thread_joiner_application_nvm_delete_callback() status=%d ctx=%p", status, context);
-    nvm_data_t *nvm_data_ptr = (nvm_data_t*)context;
-
-    thread_joiner_t *this = thread_joiner_find(nvm_data_ptr->interface_id);
-    if (this) {
-        this->nvm_operation_count--;
-    }
-
-    if (status != NS_NVM_OK) {
-        tr_error("Data delete from NVM failed, err=%d", status);
-    }
-    ns_dyn_mem_free(nvm_data_ptr);
-}
-
-static void thread_joiner_application_validate_settings(thread_joiner_t *this)
-{
-
-    if (memcmp(this->configuration_ptr->extended_random_mac,ADDR_UNSPECIFIED, 8) == 0) {
-        randLIB_get_n_bytes_random(this->configuration_ptr->extended_random_mac, 8);
-
-        this->configuration_ptr->extended_random_mac[0] |= 2; //Set Local Bit
-        this->configuration_ptr->extended_random_mac[0] &= ~1; //Clear multicast bit
+    bool new_value_generated=0;
+    if (memcmp(this->device_configuration_ptr->extended_random_mac,ADDR_UNSPECIFIED, 8) == 0) {
+        randLIB_get_n_bytes_random(this->device_configuration_ptr->extended_random_mac, 8);
+        this->device_configuration_ptr->extended_random_mac[0] |= 2; //Set Local Bit
+        this->device_configuration_ptr->extended_random_mac[0] &= ~1; //Clear multicast bit
+        new_value_generated = 1;
         tr_info("Generating Random MAC");
     }
-    while (addr_iid_reserved(this->configuration_ptr->mesh_local_eid) ||
-           memcmp(this->configuration_ptr->mesh_local_eid, ADDR_SHORT_ADR_SUFFIC,6) == 0 ) {
+    while (addr_iid_reserved(this->device_configuration_ptr->mesh_local_eid) ||
+        memcmp(this->device_configuration_ptr->mesh_local_eid, ADDR_SHORT_ADR_SUFFIC,6) == 0 ) {
         // addr_iid_reserved checks the all zeroes case.
-        randLIB_get_n_bytes_random(this->configuration_ptr->mesh_local_eid, 8);
+        randLIB_get_n_bytes_random(this->device_configuration_ptr->mesh_local_eid, 8);
+        new_value_generated = 1;
         tr_info("Generating Random ML-EID");
     }
-
     if (this->configuration_ptr->key_rotation < 3600) {
         this->configuration_ptr->key_rotation = 3600;
     }
+    return new_value_generated;
 }
 
 int thread_joiner_application_init(int8_t interface_id, device_configuration_s *device_configuration_ptr, link_configuration_s *default_configuration_ptr)
@@ -906,12 +803,6 @@ int thread_joiner_application_init(int8_t interface_id, device_configuration_s *
         return -3;
     }
 
-    int ret = thread_nvm_store_init();
-
-    if(ret) {
-        tr_error("thread_nvm_store_init, NVM error %d", ret);
-    }
-
     if ( !this->device_configuration_ptr ) {
         this->device_configuration_ptr = device_configuration_create();
     }
@@ -1023,102 +914,112 @@ link_configuration_s *thread_joiner_application_get_config(int8_t interface_id)
     if (this->configuration_valid == false) {
         return NULL;
     }
-    //link_configuration_trace(this->configuration_ptr);
-
     return this->configuration_ptr;
 }
 
-static int thread_joiner_application_nvm_link_config_read(thread_joiner_t *this)
-{
-    int thread_nvm_status;
-    nvm_data_t *nvm_data_ptr = ns_dyn_mem_alloc(sizeof(nvm_data_t));
-    if (!nvm_data_ptr) {
+static int thread_joiner_application_nvm_link_config_read(thread_joiner_t *this) {
+
+    // read config from NVM, in case of failure current settings are unchanged.
+    int nvm_read_status = thread_nvm_store_active_configuration_read(this->active_configuration_ptr, sizeof(configuration_set_t));
+    tr_debug("active conf read %d", nvm_read_status);
+    // validate that active configuration settings are valid, even if we couldn't read from nvm.
+    if(!configuration_set_validate(this->active_configuration_ptr->data, this->active_configuration_ptr->length, true)) {
+        tr_debug("No active configuration avail");
         return -1;
     }
-    nvm_data_ptr->nvm_buffer = ns_dyn_mem_alloc(sizeof(configuration_set_t));
-    if (!nvm_data_ptr->nvm_buffer) {
-        ns_dyn_mem_free(nvm_data_ptr);
-        return -1;
+    link_configuration_update(this->configuration_ptr, this->active_configuration_ptr->data, this->active_configuration_ptr->length);
+
+    thread_nvm_fast_data_t fast_data;
+    memset(&fast_data,0,sizeof(thread_nvm_fast_data_t));
+    int fast_data_read_ret = thread_nvm_store_fast_data_read(&fast_data);
+    tr_info("From NVM %d", fast_data_read_ret);
+    tr_info("mac-counter %"PRIu32,fast_data.mac_frame_counter);
+    tr_info("mle-counter %"PRIu32,fast_data.mle_frame_counter);
+    tr_info("seq-counter %"PRIu32,fast_data.seq_counter);
+
+    if (THREAD_NVM_FILE_SUCCESS == fast_data_read_ret) {
+        if (this->configuration_ptr->key_sequence < fast_data.seq_counter) {
+            this->configuration_ptr->key_sequence = fast_data.seq_counter;
+        }
     }
 
-    nvm_data_ptr->nvm_buffer_length = sizeof(configuration_set_t);
-    nvm_data_ptr->interface_id = this->interface_id;
-    thread_nvm_status = ns_nvm_data_read(thread_joiner_application_lc_nvm_read_callback, thread_nvm_key_static_link_conf, nvm_data_ptr->nvm_buffer, &nvm_data_ptr->nvm_buffer_length, nvm_data_ptr);
-    if (thread_nvm_status != NS_NVM_OK) {
-        ns_dyn_mem_free(nvm_data_ptr->nvm_buffer);
-        ns_dyn_mem_free(nvm_data_ptr);
-        return -1;
-    }
-    this->nvm_operation_count++;
-    return 0;
-}
+    fast_data.mac_frame_counter += MAC_FRAME_COUNTER_LIMIT;
+    fast_data.mle_frame_counter += MLE_FRAME_COUNTER_LIMIT;
+    thread_nvm_store_fast_data_write(&fast_data);
 
-static int thread_joiner_application_nvm_link_config_write(thread_joiner_t *this, configuration_set_t *configuration_ptr)
-{
-    /* write data to NVM */
-    int thread_nvm_status;
-    nvm_data_t *nvm_data_ptr = ns_dyn_mem_alloc(sizeof(nvm_data_t));
-    if (!nvm_data_ptr) {
-        return -1;
-    }
-    nvm_data_ptr->nvm_buffer = ns_dyn_mem_alloc(sizeof(configuration_set_t));
-    if (!nvm_data_ptr->nvm_buffer) {
-        ns_dyn_mem_free(nvm_data_ptr);
-        return -1;
+    thread_nvm_store_device_configuration_read(this->device_configuration_ptr->extended_random_mac,this->device_configuration_ptr->mesh_local_eid);
+    thread_nvm_store_link_info_read();
+    // Generate all random information, if device configuration read failed then the random mac and eid are created.
+    bool new_value_generated = thread_joiner_application_validate_settings(this);
+    if (new_value_generated) {
+        thread_nvm_store_device_configuration_write(this->device_configuration_ptr->extended_random_mac,this->device_configuration_ptr->mesh_local_eid);
     }
 
-    nvm_data_ptr->nvm_buffer_length = sizeof(configuration_set_t);
-    if (this) {
-        nvm_data_ptr->interface_id = this->interface_id;
-    } else {
-        nvm_data_ptr->interface_id = -1;
-    }
-    memcpy(nvm_data_ptr->nvm_buffer, configuration_ptr, sizeof(configuration_set_t));
-    thread_nvm_status = ns_nvm_data_write(thread_joiner_application_nvm_write_callback, thread_nvm_key_static_link_conf, nvm_data_ptr->nvm_buffer, &nvm_data_ptr->nvm_buffer_length, nvm_data_ptr);
-    if (thread_nvm_status != NS_NVM_OK) {
-        ns_dyn_mem_free(nvm_data_ptr->nvm_buffer);
-        ns_dyn_mem_free(nvm_data_ptr);
-        return -1;
-    }
+    this->configuration_valid = true;
+    link_configuration_trace(this->configuration_ptr);
 
-    if (this) {
-        this->nvm_operation_count++;
-        this->nvm_link_configuration_load = true;
-    }
+    //Add Security to MLE service
+    uint8_t key_material[32];
+    uint8_t key_index;
+    //Define KEY's
 
+    thread_key_get(this->configuration_ptr->master_key, key_material, this->configuration_ptr->key_sequence);
+    key_index = THREAD_KEY_INDEX(this->configuration_ptr->key_sequence);
+    //Set Keys
+    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(this->interface_id);
+    mac_helper_security_default_key_set(cur, &key_material[16], key_index, MAC_KEY_ID_MODE_IDX);
+    mle_service_security_set_security_key(this->interface_id, key_material, key_index, true);
+    // update counters
+    mle_service_security_set_frame_counter(this->interface_id, fast_data.mle_frame_counter);
+    mac_helper_link_frame_counter_set(this->interface_id, fast_data.mac_frame_counter);
+
+    // this saves all configurations
+    if (THREAD_NVM_FILE_SUCCESS!=nvm_read_status) {
+        thread_joiner_application_configuration_nvm_save(this->interface_id);
+    }
+    else {
+        tr_info("Reading pending set");
+        configuration_set_t *pend_conf_ptr = ns_dyn_mem_alloc(sizeof(configuration_set_t));
+        if (pend_conf_ptr) {
+            memset(pend_conf_ptr, 0, sizeof(configuration_set_t));
+            int pending_ret = thread_nvm_store_pending_configuration_read(pend_conf_ptr, sizeof(configuration_set_t));
+            if(THREAD_NVM_FILE_SUCCESS==pending_ret) {
+                if (this->pending_configuration_ptr) {
+                    ns_dyn_mem_free(this->pending_configuration_ptr);
+                }
+                this->pending_configuration_ptr = pend_conf_ptr;
+                this->pending_configuration_ptr->timeout_in_ms = 0;
+                this->pending_set_in_sync = false;
+            }
+            else {
+                tr_info("Reading pending from NVM error:%d", pending_ret);
+                ns_dyn_mem_free(pend_conf_ptr);
+            }
+        }
+    }
+    this->nvm_link_configuration_load = false;
     return 0;
 }
 
 static int thread_joiner_application_nvm_link_config_delete(thread_joiner_t *this)
 {
-    /* write data to NVM */
-    int thread_nvm_status;
-
     if (!this) {
         return -1;
     }
+    int ret = thread_nvm_store_pending_configuration_remove();
 
-    nvm_data_t *nvm_data_ptr = ns_dyn_mem_alloc(sizeof(nvm_data_t));
-    if (!nvm_data_ptr) {
-        return -1;
-    }
-    nvm_data_ptr->nvm_buffer = NULL;
-    nvm_data_ptr->nvm_buffer_length = 0;;
-    nvm_data_ptr->interface_id = this->interface_id;
-
-    thread_nvm_status = ns_nvm_key_delete(thread_joiner_application_nvm_delete_callback, thread_nvm_key_static_link_conf, nvm_data_ptr);
-    if (thread_nvm_status != NS_NVM_OK) {
-        ns_dyn_mem_free(nvm_data_ptr);
-        return -1;
+    if (ret!=THREAD_NVM_FILE_SUCCESS) {
+        tr_error("Pending configuration delete error: %d", ret);
     }
 
-    thread_dynamic_storage_pending_configuration_store(this->interface_id, NULL, 0);
+    ret = thread_nvm_store_active_configuration_remove();
+
+    if (ret!=THREAD_NVM_FILE_SUCCESS) {
+        tr_error("Active configuration delete error: %d", ret);
+    }
 
-    this->nvm_operation_count++;
     // delete link configuration from cache, device will be restarted after settings are deleted
     this->configuration_valid = false;
-    this->nvm_link_configuration_load = true;
-
     return 0;
 }
 
@@ -1132,6 +1033,34 @@ uint64_t thread_joiner_application_active_timestamp_get(int8_t interface_id)
     return this->configuration_ptr->timestamp;
 }
 
+uint8_t thread_joiner_application_security_policy_get(int8_t interface_id)
+{
+    thread_joiner_t *this = thread_joiner_find(interface_id);
+    if (!this) {
+        return 0;
+    }
+
+    return this->configuration_ptr->securityPolicy;
+}
+
+uint8_t *thread_joiner_application_random_mac_get(int8_t interface_id)
+{
+    thread_joiner_t *this = thread_joiner_find(interface_id);
+    if (!this || !this->device_configuration_ptr) {
+        tr_error("thread_joiner_application_random_mac_get NULL parameter ");
+        return (uint8_t*)ADDR_UNSPECIFIED;
+    }
+    return this->device_configuration_ptr->extended_random_mac;
+}
+uint8_t *thread_joiner_application_ml_eid_get(int8_t interface_id)
+{
+    thread_joiner_t *this = thread_joiner_find(interface_id);
+    if (!this || !this->device_configuration_ptr) {
+        tr_error("thread_joiner_application_ml_eid_get parameter NULL");
+        return (uint8_t*)ADDR_UNSPECIFIED;
+    }
+    return this->device_configuration_ptr->mesh_local_eid;
+}
 void thread_joiner_application_active_timestamp_set(int8_t interface_id, uint64_t timestamp)
 {
     thread_joiner_t *this = thread_joiner_find(interface_id);
@@ -1195,7 +1124,8 @@ void thread_joiner_pending_config_activate(int8_t interface_id)
 
     if (master_key_ptr && memcmp(master_key_ptr,link_configuration->master_key,16) != 0) {
         this->configuration_ptr->key_sequence = 0;
-        thread_nvm_store_seq_counter_store(this->configuration_ptr->key_sequence);
+        // if write fails, keep going...
+        (void)thread_nvm_store_seq_counter_write(this->configuration_ptr->key_sequence);
     }
 
     tr_info("*** Activating pending configuration.");
@@ -1206,7 +1136,7 @@ void thread_joiner_pending_config_activate(int8_t interface_id)
     this->active_configuration_ptr->timestamp = pending_active_timestamp;
     // All information is copied from old configuration so if configuration is corrupt we dont change anything.
     this->pending_configuration_ptr = NULL;
-
+    (void)thread_nvm_store_pending_configuration_remove();
     configuration_set_copy_mandatory(this->active_configuration_ptr, this->old_active_configuration_ptr);
     link_configuration_update(this->configuration_ptr,this->active_configuration_ptr->data, this->active_configuration_ptr->length);
     link_configuration_trace(this->configuration_ptr);
@@ -1259,9 +1189,7 @@ int thread_joiner_application_pending_config_create(int8_t interface_id, uint8_t
         tr_error("pending configuration creation failed");
         return -2;
     }
-    this->pending_configuration_ptr->length = 0;
-    this->pending_configuration_ptr->timeout_in_ms = 0;
-    this->pending_configuration_ptr->timestamp = 0;
+    memset(this->pending_configuration_ptr, 0, sizeof(configuration_set_t));
     configuration_set_add_all_fields(this->pending_configuration_ptr,data_ptr,data_len, meshcop_pending_set_ignore, sizeof(meshcop_pending_set_ignore));
     return 0;
 }
@@ -1275,10 +1203,19 @@ bool thread_joiner_application_pending_config_exists(int8_t interface_id)
     return true;
 }
 
-uint64_t thread_joiner_application_pending_config_timestamp_get(int8_t interface_id)
+bool thread_joiner_application_pending_delay_timer_in_sync(int8_t interface_id)
 {
     thread_joiner_t *this = thread_joiner_find(interface_id);
     if (!this || !this->pending_configuration_ptr) {
+        return false;
+    }
+    return this->pending_set_in_sync;
+}
+
+uint64_t thread_joiner_application_pending_config_timestamp_get(int8_t interface_id)
+{
+    thread_joiner_t *this = thread_joiner_find(interface_id);
+    if (!this || !this->pending_configuration_ptr || !this->pending_set_in_sync) {
         return 0;
     }
     return this->pending_configuration_ptr->timestamp;
@@ -1302,6 +1239,7 @@ int thread_joiner_application_pending_config_enable(int8_t interface_id, uint32_
         return -1;
     }
     this->pending_configuration_ptr->timeout_in_ms = timeout_in_ms;
+    this->pending_set_in_sync = true;
 
     if(this->pending_configuration_ptr->timeout_in_ms > THREAD_MAX_DELAY_TIMER_SECONDS*1000) {
         this->pending_configuration_ptr->timeout_in_ms = THREAD_MAX_DELAY_TIMER_SECONDS*1000;
@@ -1605,17 +1543,6 @@ int thread_joiner_application_link_configuration_delete(int8_t interface_id)
     return thread_joiner_application_nvm_link_config_delete(this);
 }
 
-bool thread_joiner_application_nvm_operation_in_progress(int8_t interface_id)
-{
-    thread_joiner_t *this = thread_joiner_find(interface_id);
-    if (this) {
-        if (this->nvm_operation_count == 0) {
-            return false;
-        }
-    }
-    return true;
-}
-
 bool thread_joiner_application_nvm_link_configuration_load(int8_t interface_id)
 {
     thread_joiner_t *this = thread_joiner_find(interface_id);
@@ -1811,6 +1738,14 @@ static int thread_joiner_application_send_finalisation(thread_joiner_t *this, ui
     return 0;
 }
 
+static uint8_t *thread_joiner_application_write_channel(uint8_t *ptr, uint16_t data)
+{
+    *ptr++ = MESHCOP_TLV_CHANNEL; // type
+    *ptr++ = 3; // length
+    *ptr++ = 0; // channel page
+    return common_write_16_bit(data, ptr);
+}
+
 static int thread_joiner_application_entrust_recv_cb(int8_t service_id, uint8_t source_address[16], uint16_t source_port, sn_coap_hdr_s *request_ptr)
 {
     thread_joiner_t *this = thread_joiner_find_by_service(service_id);
@@ -1846,7 +1781,7 @@ static int thread_joiner_application_entrust_recv_cb(int8_t service_id, uint8_t
         result_ptr = thread_meshcop_tlv_data_write(result_ptr,MESHCOP_TLV_XPANID, 8, this->configuration_ptr->extented_pan_id );
     }
     if (thread_meshcop_tlv_find(this->active_configuration_ptr->data, this->active_configuration_ptr->length, MESHCOP_TLV_CHANNEL, NULL) == 0) {
-        result_ptr = thread_meshcop_tlv_data_write_uint16(result_ptr,MESHCOP_TLV_CHANNEL, this->configuration_ptr->rfChannel );
+        result_ptr = thread_joiner_application_write_channel(result_ptr, this->configuration_ptr->rfChannel);
     }
     if (thread_meshcop_tlv_find(this->active_configuration_ptr->data, this->active_configuration_ptr->length, MESHCOP_TLV_PANID, NULL) == 0) {
         result_ptr = thread_meshcop_tlv_data_write_uint16(result_ptr,MESHCOP_TLV_PANID, this->configuration_ptr->panId );
@@ -1944,17 +1879,20 @@ int thread_joiner_application_pskd_commission_start(int8_t interface_id, uint8_t
 
 int thread_joiner_application_configuration_nvm_save(int8_t interface_id)
 {
+    tr_info("thread_joiner_application_configuration_nvm_save");
 
     thread_joiner_t *this = thread_joiner_find(interface_id);
     if (!this) {
         return -1;
     }
 
-    thread_dynamic_storage_device_configuration_store(interface_id,this->configuration_ptr->extended_random_mac,this->configuration_ptr->mesh_local_eid);
-    thread_dynamic_storage_pending_configuration_store(interface_id,this->pending_configuration_ptr, sizeof(configuration_set_t));
-    thread_nvm_store_seq_counter_store(this->configuration_ptr->key_sequence);
-    thread_joiner_application_nvm_link_config_write(this, this->active_configuration_ptr);
+    thread_nvm_store_device_configuration_write(this->device_configuration_ptr->extended_random_mac,this->device_configuration_ptr->mesh_local_eid);
+    thread_nvm_store_pending_configuration_write(this->pending_configuration_ptr, sizeof(configuration_set_t));
+    thread_nvm_store_seq_counter_write(this->configuration_ptr->key_sequence);
+    thread_nvm_store_active_configuration_write(this->active_configuration_ptr, sizeof(configuration_set_t));
 
+    /* allow configuration to be read in bootstrap */
+    this->nvm_link_configuration_load = true;
     return 0;
 }
 
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.h
index 9a892d147f..3939b5de52 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.h
@@ -67,6 +67,7 @@ void thread_joiner_application_deinit(int8_t interface_id);
 struct link_configuration *thread_joiner_application_get_config(int8_t interface_id);
 
 uint64_t thread_joiner_application_active_timestamp_get(int8_t interface_id);
+uint8_t thread_joiner_application_security_policy_get(int8_t interface_id);
 void thread_joiner_application_active_timestamp_set(int8_t interface_id, uint64_t timestamp);
 uint8_t *thread_joiner_application_active_config_tlv_list_get(uint8_t interface_id, uint16_t *length);
 
@@ -91,6 +92,11 @@ device_configuration_s *thread_joiner_application_get_device_config(int8_t inter
  *\ return 0 if store request has been delivered to lower layers.
  *\ return -1 if store failed (store request not made)
  */
+
+uint8_t *thread_joiner_application_random_mac_get(int8_t interface_id);
+
+uint8_t *thread_joiner_application_ml_eid_get(int8_t interface_id);
+
 int thread_joiner_application_link_configuration_store(int8_t interface_id, link_configuration_s *link_config);
 
 /** Delete link configuration from platform NVM
@@ -106,14 +112,6 @@ int thread_joiner_application_link_configuration_store(int8_t interface_id, link
  */
 int thread_joiner_application_link_configuration_delete(int8_t interface_id);
 
-/** Check in joiner application has pending NVM operations
- *
- * /param interface interface id of this thread instance.
- *
- * /return true if NVM operation is active, false otherwise
- */
-bool thread_joiner_application_nvm_operation_in_progress(int8_t interface_id);
-
 /** Check if link configuration settings should be loaded from NVM and make a read
  * request if settings needs to be loaded.
  *
@@ -261,7 +259,7 @@ bool thread_joiner_application_next_pending_config_exists(int8_t interface_id);
 uint16_t thread_joiner_application_next_pending_config_length(int8_t interface_id);
 uint8_t *thread_joiner_application_next_pending_config_build(int8_t interface_id, uint8_t *ptr);
 void thread_joiner_application_next_pending_config_delete(int8_t interface_id);
-
+bool thread_joiner_application_pending_delay_timer_in_sync(int8_t interface_id);
 
 
 void thread_joiner_application_next_active_config_save(int8_t interface_id);
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_leader_service.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_leader_service.c
index 6a74f22031..622f9de18e 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_leader_service.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_leader_service.c
@@ -54,6 +54,7 @@
 #include "6LoWPAN/Thread/thread_leader_service.h"
 #include "6LoWPAN/Thread/thread_router_bootstrap.h"
 #include "6LoWPAN/Thread/thread_network_synch.h"
+#include "6LoWPAN/Thread/thread_nvm_store.h"
 #include "Service_Libs/mle_service/mle_service_api.h"
 #include "6LoWPAN/Thread/thread_host_bootstrap.h"
 #include "6LoWPAN/Thread/thread_border_router_api_internal.h"
@@ -1121,7 +1122,7 @@ send_error_response:
 
 static int thread_leader_service_network_data_register(protocol_interface_info_entry_t *cur, uint8_t *network_data_ptr, uint16_t network_data_length, uint16_t routerId)
 {
-    tr_debug("thread leader Register request");
+    tr_debug("Register network data for %x", routerId);
     // Mark all data to be cleared
     thread_network_data_router_id_mark_delete(&cur->thread_info->networkDataStorage, routerId, true);
 
@@ -1175,8 +1176,8 @@ static int thread_leader_service_register_cb(int8_t service_id, uint8_t source_a
 
     if (2 <= thread_tmfcop_tlv_data_get_uint16(payload_ptr, payload_len, TMFCOP_TLV_RLOC16, &old_rloc)) {
         // This will delete everything from old rloc
+        tr_warn("Remove network data from: %x", old_rloc);
         ret = thread_leader_service_network_data_register(cur, NULL, 0, old_rloc);
-        tr_warn("Removed network data from: %x", old_rloc);
     }
 
     if (!thread_tmfcop_tlv_exist(payload_ptr, payload_len, TMFCOP_TLV_NETWORK_DATA)) {
@@ -1256,6 +1257,9 @@ static int thread_leader_service_leader_init(protocol_interface_info_entry_t *cu
 {
     //Clean All allocated stuff's
     thread_info_t *thread_info = cur->thread_info;
+    // mark and delete previous leader network data information
+    thread_network_data_router_id_mark_delete(&thread_info->networkDataStorage,thread_router_addr_from_id(cur->thread_info->thread_leader_data->leaderRouterId), true);
+    thread_network_data_router_id_free(&thread_info->networkDataStorage, false, cur);
     thread_leader_service_leader_data_free(thread_info);
 
     thread_info->rfc6775 = false;
@@ -1273,14 +1277,16 @@ static int thread_leader_service_leader_init(protocol_interface_info_entry_t *cu
     return thread_leader_service_leader_start(cur);
 }
 
-static void thread_leader_service_leader_data_initialize(thread_leader_data_t *leader_data, uint8_t routerId)
+static void thread_leader_service_leader_data_initialize(protocol_interface_info_entry_t *cur, uint8_t routerId)
 {
-    if (leader_data) {
+    thread_leader_data_t *leader_data = cur->thread_info->thread_leader_data;
+
+    if (cur->thread_info->thread_leader_data) {
         leader_data->partitionId = randLIB_get_32bit(); //Generate Random Instance
         leader_data->leaderRouterId = routerId; //Set leader data to zero by Default
         leader_data->dataVersion = randLIB_get_8bit();
         leader_data->stableDataVersion = randLIB_get_8bit();
-        leader_data->weighting = THREAD_DEFAULT_WEIGHTING;
+        leader_data->weighting = cur->thread_info->partition_weighting;
     }
 }
 
@@ -1302,7 +1308,7 @@ static void thread_leader_service_interface_setup_activate(protocol_interface_in
 
     routerId = thread_router_id_from_addr(cur->thread_info->routerShortAddress);
 
-    thread_leader_service_leader_data_initialize(cur->thread_info->thread_leader_data, routerId);
+    thread_leader_service_leader_data_initialize(cur, routerId);
     // Test code
     if(cur->thread_info->testRandomPartitionId != 0){
         cur->thread_info->thread_leader_data->partitionId = cur->thread_info->testRandomPartitionId;
@@ -1311,7 +1317,7 @@ static void thread_leader_service_interface_setup_activate(protocol_interface_in
     thread_leader_service_private_routemask_init(private);
     //SET Router ID
     thread_leader_allocate_router_id_by_allocated_id(private, routerId, cur->mac);
-    thread_old_partition_data_purge(cur->thread_info);
+    thread_old_partition_data_purge(cur);
     cur->lowpan_address_mode = NET_6LOWPAN_GP16_ADDRESS;
     thread_bootstrap_update_ml16_address(cur, cur->thread_info->routerShortAddress);
     thread_generate_ml64_address(cur);
@@ -1507,11 +1513,10 @@ void thread_leader_service_leader_data_free(thread_info_t *thread_info)
 
 void thread_leader_service_thread_partitition_generate(int8_t interface_id, bool newPartition)
 {
-    uint16_t leaderRloc;
     protocol_interface_info_entry_t *cur;
-    link_configuration_s *linkConfiguration;
-    linkConfiguration = thread_joiner_application_get_config(interface_id);
-
+    link_configuration_s *linkConfiguration = thread_joiner_application_get_config(interface_id);
+    uint8_t *parent_mac_addr = NULL;
+    uint16_t leaderRloc;
 
     cur = protocol_stack_interface_info_get_by_id(interface_id);
     if (!cur || !cur->thread_info) {
@@ -1580,7 +1585,10 @@ void thread_leader_service_thread_partitition_generate(int8_t interface_id, bool
     cur->nwk_mode = ARM_NWK_GP_IP_MODE;
     thread_bootstrap_ready(cur);
     thread_configuration_mle_activate(cur);
-    thread_nvm_store_link_info_file_write(cur);
+    if (cur->thread_info->thread_endnode_parent) {
+        parent_mac_addr = cur->thread_info->thread_endnode_parent->mac64;
+    }
+    thread_nvm_store_link_info_write(parent_mac_addr, mac_helper_mac16_address_get(cur));
 
     if (thread_nd_own_service_list_data_size(&cur->thread_info->localServerDataBase)) {
         // We publish our services if we have some BUG leader cannot remove old ones
@@ -1629,7 +1637,6 @@ int thread_leader_service_thread_partitition_restart(int8_t interface_id, mle_tl
     // Clear network data (if exists) and propagate new empty network data
     thread_network_data_free_and_clean(&cur->thread_info->networkDataStorage);
     thread_network_data_base_init(&cur->thread_info->networkDataStorage);
-    thread_leader_service_network_data_changed(cur, true, true);
     return 0;
 }
 
@@ -1807,5 +1814,4 @@ void thread_leader_service_router_state_changed(thread_info_t *thread_info, uint
     }
 }
 
-
 #endif // HAVE_THREAD_LEADER_SERVICE */
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_client.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_client.c
index 74e1d95aef..3d1f7a812f 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_client.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_client.c
@@ -614,8 +614,9 @@ void thread_management_client_pending_coap_request_kill(int8_t interface_id)
         return;
     }
 
+    cur->thread_info->localServerDataBase.publish_active = false;
+
     if (this->coap_asd_msg_id != 0) {
-        cur->thread_info->localServerDataBase.publish_active = false;
         coap_service_request_delete(this->coap_service_id, this->coap_asd_msg_id);
         this->coap_asd_msg_id = 0;
     }
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_if.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_if.c
index 110060049c..58ea54aea3 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_if.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_if.c
@@ -47,6 +47,7 @@
 #include "6LoWPAN/Thread/thread_routing.h"
 #include "6LoWPAN/Thread/thread_network_data_lib.h"
 #include "6LoWPAN/Thread/thread_network_data_storage.h"
+#include "6LoWPAN/Thread/thread_leader_service.h"
 #include "6LoWPAN/Thread/thread_nd.h"
 #include "thread_diagnostic.h"
 #include "6LoWPAN/Thread/thread_dhcpv6_client.h"
@@ -361,7 +362,7 @@ uint8_t *thread_management_key_request(int8_t interface_id, uint8_t keyId)
                     //Get Default key
                     keyPtr =  mle_service_security_default_key_get(interface_id);
                 }
-                thread_nvm_store_seq_counter_store(linkConfiguration->key_sequence);
+                thread_nvm_store_seq_counter_write(linkConfiguration->key_sequence);
             }
         }
     }
@@ -493,7 +494,7 @@ int thread_management_key_sets_calc(protocol_interface_info_entry_t *cur, link_c
         fast_data.seq_counter = thrKeySequenceCounter;
         fast_data.mac_frame_counter = 0;
         fast_data.mle_frame_counter = mle_service_security_get_frame_counter(cur->interface_mode);
-        thread_nvm_store_fast_data_store(&fast_data);
+        thread_nvm_store_fast_data_write(&fast_data);
         mac_helper_link_frame_counter_set(cur->id, 0);
         thread_security_key_generate(cur,linkConfiguration->master_key,linkConfiguration->key_sequence);
         thread_security_next_key_generate(cur,linkConfiguration->master_key,linkConfiguration->key_sequence);
@@ -915,6 +916,9 @@ int thread_management_node_init(
     /* Thread will manage the address query timing, and report negatives. Set this high so as not to interfere. */
     cur->ipv6_neighbour_cache.retrans_timer = 10000;
 
+    // Set default partition weighting
+    cur->thread_info->partition_weighting = THREAD_DEFAULT_WEIGHTING;
+
     /* IP forwarding is off by default */
     cur->ip_forwarding = false;
 
@@ -1065,6 +1069,36 @@ int thread_management_link_configuration_store(int8_t interface_id, link_configu
 #endif
 }
 
+int thread_management_link_configuration_add(int8_t interface_id, uint8_t *additional_ptr, uint8_t additional_len)
+{
+#ifdef HAVE_THREAD
+    if (interface_id < 0) {
+        return -1;
+    }
+
+    int ret = thread_joiner_application_update_configuration(interface_id, additional_ptr, additional_len, true);
+    if (ret != 0) {
+        return ret;
+    }
+
+    protocol_interface_info_entry_t *cur =  protocol_stack_interface_info_get_by_id(interface_id);
+    if (!cur  || !cur->thread_info) {
+        return -2;
+    }
+    if ((cur->lowpan_info & INTERFACE_NWK_ACTIVE) !=  0) {
+        // take new settings into use after restart
+        thread_bootstrap_reset_restart(interface_id);
+    }
+
+    return ret;
+#else
+    (void) interface_id;
+    (void) additional_ptr;
+    (void) additional_len;
+    return -1;
+#endif
+}
+
 int thread_management_link_configuration_delete(int8_t interface_id)
 {
 #ifdef HAVE_THREAD
@@ -1344,11 +1378,6 @@ int8_t thread_management_get_request_full_nwk_data(int8_t interface_id, bool *fu
 
 int thread_management_device_certificate_set(int8_t interface_id, const unsigned char *device_certificate_ptr, uint16_t device_certificate_len, const unsigned char *priv_key_ptr, uint16_t priv_key_len)
 {
-    (void) interface_id;
-    (void) device_certificate_ptr;
-    (void) device_certificate_len;
-    (void) priv_key_ptr;
-    (void) priv_key_len;
 #ifdef HAVE_THREAD
     protocol_interface_info_entry_t *cur;
 
@@ -1361,16 +1390,16 @@ int thread_management_device_certificate_set(int8_t interface_id, const unsigned
     return thread_extension_bootstrap_device_certificate_set(cur, device_certificate_ptr, device_certificate_len, priv_key_ptr, priv_key_len);
 
 #else
+    (void) interface_id;
+    (void) device_certificate_ptr;
+    (void) device_certificate_len;
+    (void) priv_key_ptr;
+    (void) priv_key_len;
     return -1;
 #endif
 }
 int thread_management_network_certificate_set(int8_t interface_id, const unsigned char *network_certificate_ptr, uint16_t network_certificate_len, const unsigned char *priv_key_ptr, uint16_t priv_key_len)
 {
-    (void) interface_id;
-    (void) network_certificate_ptr;
-    (void) network_certificate_len;
-    (void) priv_key_ptr;
-    (void) priv_key_len;
 #ifdef HAVE_THREAD
     protocol_interface_info_entry_t *cur;
 
@@ -1386,7 +1415,40 @@ int thread_management_network_certificate_set(int8_t interface_id, const unsigne
 
     return thread_extension_bootstrap_network_private_key_set(cur, priv_key_ptr, priv_key_len);
 #else
+    (void) interface_id;
+    (void) network_certificate_ptr;
+    (void) network_certificate_len;
+    (void) priv_key_ptr;
+    (void) priv_key_len;
     return -1;
 #endif
 }
 
+int thread_management_partition_weighting_set(int8_t interface_id, uint8_t partition_weighting)
+{
+#ifdef HAVE_THREAD
+    protocol_interface_info_entry_t *cur;
+
+    cur = protocol_stack_interface_info_get_by_id(interface_id);
+    if (!cur || !cur->thread_info) {
+        tr_debug("Invalid interface id");
+        return -1;
+    }
+
+    if (cur->thread_info->partition_weighting == partition_weighting) {
+        return 0;
+    }
+
+    cur->thread_info->partition_weighting = partition_weighting;
+    if (cur->lowpan_info & INTERFACE_NWK_ACTIVE) {
+        // bootstrap active and weighting has changed
+        thread_bootstrap_reset_restart(interface_id);
+    }
+
+    return 0;
+#else
+    (void) interface_id;
+    (void) partition_weighting;
+    return -1;
+#endif
+}
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_server.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_server.c
index 68aabc4b71..329c26c14a 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_server.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_server.c
@@ -68,7 +68,7 @@
 #include "6LoWPAN/MAC/mac_data_poll.h"
 #include "mlme.h"
 
-#ifdef HAVE_THREAD_ROUTER
+#ifdef HAVE_THREAD
 
 typedef struct scan_query {
     int8_t coap_service_id;
@@ -857,6 +857,7 @@ static int thread_management_server_panid_query_cb(int8_t service_id, uint8_t so
     this->scan_ptr->timer = eventOS_timeout_ms(thread_panid_scan_timeout_cb, 500, this);// Delay for the confirm response message
     if (!this->scan_ptr->timer) {
         ns_dyn_mem_free(this->scan_ptr);
+        this->scan_ptr = NULL;
         response_code = COAP_MSG_CODE_RESPONSE_INTERNAL_SERVER_ERROR;
         goto error_exit;
     }
@@ -875,7 +876,6 @@ static int thread_management_server_panid_query_cb(int8_t service_id, uint8_t so
     }
     return -1;
 error_exit:
-    this->scan_ptr = NULL;
     if (request_ptr->msg_type == COAP_MSG_TYPE_CONFIRMABLE ){
         coap_service_response_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, request_ptr, response_code, COAP_CT_OCTET_STREAM, NULL, 0);
         return 0;
@@ -1134,6 +1134,7 @@ int thread_management_server_init(int8_t interface_id)
     this->announce_ptr = NULL;
     this->external_commissioner_port = THREAD_COMMISSIONING_PORT;
 
+#ifdef HAVE_THREAD_ROUTER
     if (thread_border_router_init(this->interface_id) != 0) {
         ns_dyn_mem_free(this);
         return -5;
@@ -1143,19 +1144,20 @@ int thread_management_server_init(int8_t interface_id)
         ns_dyn_mem_free(this);
         return -5;
     }
-
+#endif
     this->coap_service_id = coap_service_initialize(this->interface_id, THREAD_MANAGEMENT_PORT, COAP_SERVICE_OPTIONS_NONE, NULL, NULL);
     if (this->coap_service_id < 0) {
         tr_warn("Thread management init failed");
         ns_dyn_mem_free(this);
         return -3;
     }
-
+#ifdef HAVE_THREAD_ROUTER
     if (thread_leader_service_init(interface_id, this->coap_service_id) != 0) {
         tr_warn("Thread leader service init failed");
         ns_dyn_mem_free(this);
         return -3;
     }
+#endif
     thread_extension_init(interface_id, this->coap_service_id);
 
     // All thread devices
@@ -1556,7 +1558,7 @@ int thread_management_server_tmf_get_request_handler(int8_t interface_id, int8_t
     return ret_val;
 }
 
-#else // HAVE_THREAD_ROUTER
+#else // HAVE_THREAD
 
 int thread_management_server_init(int8_t interface_id)
 {
@@ -1571,7 +1573,7 @@ void thread_management_server_delete(int8_t interface_id)
 
 int thread_management_server_joiner_router_init(int8_t interface_id)
 {
-	(void)interface_id;
+    (void)interface_id;
     return 0;
 }
 
@@ -1601,4 +1603,4 @@ int thread_management_server_tmf_get_request_handler(int8_t interface_id, int8_t
     (void)request_ptr;
     return -1;
 }
-#endif //HAVE_THREAD_ROUTER
+#endif //HAVE_THREAD
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_mle_message_handler.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_mle_message_handler.c
index a8783cfba8..e564752791 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_mle_message_handler.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_mle_message_handler.c
@@ -42,8 +42,10 @@
 #include "6LoWPAN/Thread/thread_bootstrap.h"
 #include "6LoWPAN/Thread/thread_management_internal.h"
 #include "6LoWPAN/Thread/thread_joiner_application.h"
+#include "6LoWPAN/Thread/thread_leader_service.h"
 #include "6LoWPAN/Thread/thread_tmfcop_lib.h"
 #include "6LoWPAN/Thread/thread_host_bootstrap.h"
+#include "6LoWPAN/Thread/thread_extension.h"
 #include "6LoWPAN/Thread/thread_router_bootstrap.h"
 #include "6LoWPAN/Thread/thread_network_synch.h"
 #include "6LoWPAN/MAC/mac_helper.h"
@@ -237,6 +239,29 @@ static bool thread_router_leader_data_process(protocol_interface_info_entry_t *c
     return true;
 }
 
+static bool thread_reed_partitions_merge(protocol_interface_info_entry_t *cur, uint16_t shortAddress, thread_leader_data_t heard_partition_leader_data)
+{
+    if (thread_is_router_addr(shortAddress)) {
+        return false;
+    }
+    if (thread_extension_version_check(thread_info(cur)->version)) {
+        // lower weighting heard
+        if (thread_info(cur)->thread_leader_data->weighting > heard_partition_leader_data.weighting) {
+            return false;
+        }
+        // lower/same partition id heard
+        if (thread_info(cur)->thread_leader_data->weighting == heard_partition_leader_data.weighting &&
+                thread_info(cur)->thread_leader_data->partitionId >= heard_partition_leader_data.partitionId ) {
+            return false;
+        }
+    } else if (thread_info(cur)->thread_leader_data->partitionId >= heard_partition_leader_data.partitionId){
+        return false;
+    }
+    // can merge to a higher weighting/partition id
+    thread_bootstrap_connection_error(cur->id, CON_ERROR_PARTITION_MERGE, NULL);
+    return true;
+}
+
 static bool thread_router_advertiment_tlv_analyze(uint8_t *ptr, uint16_t data_length, thread_leader_data_t *leaderData, uint16_t *shortAddress, mle_tlv_info_t *routeTlv)
 {
     //Read Leader Data and verify connectivity
@@ -323,9 +348,10 @@ static void thread_parse_advertisement(protocol_interface_info_entry_t *cur, mle
         //processing for non routers
         if (thread_check_is_this_my_parent(cur, entry_temp)) {
             //advertisement from parent
-            if (thread_info(cur)->thread_leader_data->partitionId != leaderData.partitionId) {
-                //parent changed partition - reset own routing information
-                thread_old_partition_data_purge(cur->thread_info);
+            if ((thread_info(cur)->thread_leader_data->partitionId != leaderData.partitionId) ||
+                (thread_info(cur)->thread_leader_data->weighting != leaderData.weighting)) {
+                //parent changed partition/weight - reset own routing information
+                thread_old_partition_data_purge(cur);
             }
             //check if network data needs to be requested
             if (!thread_bootstrap_request_network_data(cur, &leaderData, shortAddress)) {
@@ -333,7 +359,6 @@ static void thread_parse_advertisement(protocol_interface_info_entry_t *cur, mle
                 thread_bootstrap_connection_error(cur->id, CON_PARENT_CONNECT_DOWN, NULL);
                 return;
             }
-
         }
     }
 
@@ -345,7 +370,8 @@ static void thread_parse_advertisement(protocol_interface_info_entry_t *cur, mle
         if (!thread_attach_active_router(cur)) {
             // REED and FED
             if (!entry_temp && thread_bootstrap_link_create_check(cur, shortAddress) && thread_bootstrap_link_create_allowed(cur, shortAddress, mle_msg->packet_src_address)) {
-                if (thread_info(cur)->thread_leader_data->partitionId == leaderData.partitionId) {
+                if ((thread_info(cur)->thread_leader_data->partitionId == leaderData.partitionId) &&
+                        (thread_info(cur)->thread_leader_data->weighting == leaderData.weighting)) {
                     // Create link to new neighbor no other processing allowed
                     thread_link_request_start(cur, mle_msg->packet_src_address);
                     return;
@@ -355,6 +381,10 @@ static void thread_parse_advertisement(protocol_interface_info_entry_t *cur, mle
                     return;
                 }
             }
+            // process REED advertisement from higher partition
+            if (thread_reed_partitions_merge(cur, shortAddress, leaderData)) {
+                return;
+            }
         } else {
             //Router
             if (!thread_router_leader_data_process(cur, mle_msg->packet_src_address, &leaderData, &routeTlv, entry_temp) ) {
@@ -547,7 +577,7 @@ static void thread_parse_data_response(protocol_interface_info_entry_t *cur, mle
     if (thread_info(cur)->thread_leader_data->partitionId != leaderData.partitionId) {
         thread_info(cur)->thread_leader_data->leaderRouterId = leaderData.leaderRouterId;
         thread_info(cur)->thread_leader_data->partitionId = leaderData.partitionId;
-        thread_old_partition_data_purge(cur->thread_info);
+        thread_old_partition_data_purge(cur);
         accept_new_data = true;
     }
 
@@ -593,6 +623,12 @@ static void thread_parse_data_response(protocol_interface_info_entry_t *cur, mle
             tr_debug("SET NWK data Request state");
         }
     }
+
+    // leader has finished synching network data after reset/restart
+    if (cur->thread_info->leader_synced) {
+        cur->thread_info->leader_synced = false;
+        thread_leader_service_network_data_changed(cur, true, true);
+    }
 }
 static int thread_host_child_update_response_send(protocol_interface_info_entry_t *cur, uint8_t *dst_address, mle_tlv_info_t *challengeTlv, mle_tlv_info_t *requestTlv)
 {
@@ -680,7 +716,7 @@ static void thread_host_child_update_request_process(protocol_interface_info_ent
     if (thread_info(cur)->thread_leader_data->partitionId != leaderData.partitionId) {
         thread_info(cur)->thread_leader_data->leaderRouterId = leaderData.leaderRouterId;
         thread_info(cur)->thread_leader_data->partitionId = leaderData.partitionId;
-        thread_old_partition_data_purge(cur->thread_info);
+        thread_old_partition_data_purge(cur);
     }
     //Check Network Data TLV
     if (mle_tlv_read_tlv(MLE_TYPE_NETWORK_DATA, mle_msg->data_ptr, mle_msg->data_length, &networkDataTlv)) {
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.c
index affa4a1bfc..2ba5226c2d 100755
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.c
@@ -2451,7 +2451,7 @@ uint8_t *thread_network_data_service_set_write(thread_network_data_cache_entry_t
         tlvLength = thread_nd_service_based_on_list_entry_size(cur, true);
         if (tlvLength) {
             ptr = thread_nd_service_header_write(ptr, cur, tlvLength);
-            tr_debug("Service: sid:%d e:%d data:%s",cur->S_id, cur->S_enterprise_number, trace_array(cur->S_service_data, cur->S_service_data_length));
+            tr_debug("Service: sid:%d e:%"PRIu32" data:%s",cur->S_id, cur->S_enterprise_number, trace_array(cur->S_service_data, cur->S_service_data_length));
             ptr = thread_nd_service_server_list_write(&cur->server_list, ptr, true);
         }
     }
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.h
index ac65b415eb..a4d2bf0176 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.h
@@ -185,8 +185,8 @@ typedef struct thread_network_local_data_cache_entry_s {
     thread_network_data_service_list_t service_list;
     uint16_t registered_rloc16;/*!< Address used for latest registration */
     bool release_old_address:1;/*!< true if release of old address is needed */
-    bool publish_active:1;
-    bool publish_pending:1;
+    bool publish_active:1;/*!< true when publish is active */
+    bool publish_pending:1;/*!< true when publish attempt made during active publish */
 } thread_network_local_data_cache_entry_t;
 
 /**
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.c
index 0f7d2aa7e1..5867627603 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.c
@@ -78,13 +78,6 @@ typedef struct thread_network_dynamic_data_store {
     thread_leader_info_t *leader_private_data;
     thread_parent_info_t *thread_endnode_parent;
     thread_attach_state_e thread_attached_state;
-    void *pending_configuration;
-    uint64_t pending_timestamp;
-    uint32_t mle_frame_counter;
-    uint32_t mac_frame_counter;
-    uint32_t network_sequence_counter;
-    uint8_t  mleid[8];
-    uint8_t  mac[8];
     uint16_t shortAddress;
     thread_sync_child_info_t children[THREAD_MAX_CHILD_COUNT];
 } thread_network_dynamic_data_store_t;
@@ -110,11 +103,6 @@ thread_network_dynamic_data_entry_t *thread_network_synch_create(int8_t interfac
         newEntry->networ_dynamic_data_parameters.shortAddress = 0xfffe;
         newEntry->networ_dynamic_data_parameters.leader_private_data = NULL;
         newEntry->networ_dynamic_data_parameters.thread_endnode_parent = NULL;
-        newEntry->networ_dynamic_data_parameters.pending_configuration = NULL;
-        newEntry->networ_dynamic_data_parameters.pending_timestamp = 0;
-        newEntry->networ_dynamic_data_parameters.mac_frame_counter = 0;
-        newEntry->networ_dynamic_data_parameters.mle_frame_counter = 0;
-        newEntry->networ_dynamic_data_parameters.network_sequence_counter = 0;
         for (int i = 0; i < THREAD_MAX_CHILD_COUNT; ++i) {
             memset(&newEntry->networ_dynamic_data_parameters.children[i], 0, sizeof(thread_sync_child_info_t));
         }
@@ -162,6 +150,7 @@ int thread_network_synch_data_free(int8_t interface_id)
 {
     return thread_network_synch_delete(interface_id);
 }
+
 /*
  * Dynamic network data storage.
  */
@@ -288,143 +277,4 @@ void thread_dynamic_storage_build_mle_table(int8_t interface_id)
         }
     }
 }
-
-
-
-/*Pending configuration storage*/
-void thread_dynamic_storage_pending_configuration_store(int8_t interface_id, void *data, uint16_t size)
-{
-    thread_network_dynamic_data_entry_t *storeEntry = thread_network_synch_find(interface_id);
-    if (!storeEntry) {
-        storeEntry = thread_network_synch_create(interface_id);
-    }
-    if (!storeEntry) {
-        return;
-    }
-
-    tr_info("saved pending configuration to store");
-    ns_dyn_mem_free(storeEntry->networ_dynamic_data_parameters.pending_configuration);
-    storeEntry->networ_dynamic_data_parameters.pending_configuration = NULL;
-    if (!data || !size) {
-        return;
-    }
-
-    storeEntry->networ_dynamic_data_parameters.pending_configuration = ns_dyn_mem_alloc(size);
-    if (!storeEntry->networ_dynamic_data_parameters.pending_configuration) {
-        return;
-    }
-    memcpy(storeEntry->networ_dynamic_data_parameters.pending_configuration, data,size);
-
-}
-
-bool thread_dynamic_storage_pending_configuration_exists(int8_t interface_id)
-{
-    thread_network_dynamic_data_entry_t *storeEntry = thread_network_synch_find(interface_id);
-    if (!storeEntry || !storeEntry->networ_dynamic_data_parameters.pending_configuration) {
-        tr_debug("no pending config found in store");
-        return false;
-    }
-    return true;
-}
-void thread_dynamic_storage_pending_configuration_read(int8_t interface_id, void *data, uint16_t size)
-{
-    thread_network_dynamic_data_entry_t *storeEntry = thread_network_synch_find(interface_id);
-    if (!storeEntry || !storeEntry->networ_dynamic_data_parameters.pending_configuration) {
-        tr_debug("no pending config found in store");
-        return;
-    }
-    memcpy(data,storeEntry->networ_dynamic_data_parameters.pending_configuration, size);
-    return;
-}
-
-int thread_pending_data_delete(int8_t interfaceId)
-{
-    thread_network_dynamic_data_entry_t *newEntry = thread_network_synch_find(interfaceId);
-    if (newEntry) {
-      //Free
-        newEntry->networ_dynamic_data_parameters.pending_configuration = NULL;
-        newEntry->networ_dynamic_data_parameters.pending_timestamp = 0;
-        return 0;
-    }
-    return -1;
-}
-
-void thread_dynamic_storage_device_configuration_read(int8_t interface_id, uint8_t *mac_ptr, uint8_t *mleid_ptr)
-{
-    thread_network_dynamic_data_entry_t *storeEntry = thread_network_synch_find(interface_id);
-    if (!storeEntry) {
-        tr_error("no device configuration in store");
-        return;
-    }
-    memcpy(mac_ptr,storeEntry->networ_dynamic_data_parameters.mac, 8);
-    memcpy(mleid_ptr,storeEntry->networ_dynamic_data_parameters.mleid, 8);
-    return;
-}
-
-void thread_dynamic_storage_device_configuration_store(int8_t interface_id, uint8_t *mac_ptr, uint8_t *mleid_ptr)
-{
-    thread_network_dynamic_data_entry_t *storeEntry = thread_network_synch_find(interface_id);
-    if (!storeEntry) {
-        storeEntry = thread_network_synch_create(interface_id);
-    }
-    if (!storeEntry) {
-        return;
-    }
-    tr_debug("store device configuration");
-    memcpy(storeEntry->networ_dynamic_data_parameters.mac, mac_ptr, 8);
-    memcpy(storeEntry->networ_dynamic_data_parameters.mleid,mleid_ptr, 8);
-    return;
-}
-
-static uint8_t loaded = false;
-static uint8_t mac[8] = {0};
-static uint16_t short_addr = 0xffff;
-
-void thread_nvm_store_link_info_file_read()
-{
-    if (!memcmp(mac,ADDR_UNSPECIFIED,8) && short_addr == 0xffff) {
-        return;
-    }
-    // File exists at bootup
-    loaded = true;
-}
-bool thread_nvm_store_link_info_get(uint8_t *parent_mac64, uint16_t *my_short_address)
-{
-    if (!memcmp(mac,ADDR_UNSPECIFIED,8) && short_addr == 0xffff) {
-        return false;
-    }
-    if (!loaded) {
-        return false;
-    }
-    if (parent_mac64) {
-        memcpy(parent_mac64,mac,8);
-    }
-    if (my_short_address) {
-        *my_short_address = short_addr;
-    }
-
-    // File was found and values read
-    return true;
-}
-
-void thread_nvm_store_link_info_clear()
-{
-    memset(mac,0,8);
-    short_addr = 0xffff;
-    loaded = false;
-    // synchronised settings were used and now are deleted
-    // it is only allowed to use synch settings during the first boot
-}
-
-void thread_nvm_store_link_info_file_write(protocol_interface_info_entry_t *cur)
-{
-    if(cur->thread_info->thread_endnode_parent) {
-        memcpy(mac,cur->thread_info->thread_endnode_parent->mac64,8);
-    } else {
-        memset(mac,0,8);
-    }
-    short_addr = mac_helper_mac16_address_get(cur);
-    // Settings are changed, but values should not be saved yet only after certain
-    // grace period. But the values are not returned in get.
-}
 #endif
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.h
index 2aa57a9649..eae9f1807c 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.h
@@ -58,9 +58,4 @@ int thread_pending_data_delete(int8_t interfaceId);
 void thread_dynamic_storage_device_configuration_read(int8_t interface_id, uint8_t *mac_ptr, uint8_t *mleid_ptr);
 void thread_dynamic_storage_device_configuration_store(int8_t interface_id, uint8_t *mac_ptr, uint8_t *mleid_ptr);
 
-void thread_nvm_store_link_info_file_read();
-bool thread_nvm_store_link_info_get(uint8_t *parent_mac64, uint16_t *my_short_address);
-void thread_nvm_store_link_info_clear();
-void thread_nvm_store_link_info_file_write(protocol_interface_info_entry_t *cur);
-
 #endif /* THREAD_NETWORK_SYNCH_H_ */
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.c
index c42a212dab..fdd644e846 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.c
@@ -31,33 +31,72 @@
  *
  */
 
-#include "thread_nvm_store.h"
-#include "ns_trace.h"
+#include "nsconfig.h"
+
 #include <string.h>
 #include <stdio.h>
+#include "Core/include/address.h"
 #include "ns_file_system.h"
 #include "thread_config.h"
+#include "thread_common.h"
+#include "thread_nvm_store.h"
+#include "ns_trace.h"
 
 #define TRACE_GROUP "tnvm"
-#define FAST_DATA_FILE "f_d"
-#define FAST_DATA_WORD_SIZE 4
+const char *FAST_DATA_FILE = "f_d";
 #define FAST_DATA_VERSION 1
+#define LINK_INFO_WRITE_DELAY 2
+#define LINK_INFO_SHORT_ADDR_NOT_SET 0xffff
+#define LINK_INFO_WRITE_DONE 0xffff
+
+const char *LINK_INFO_FILE = "l_i";
+#define LINK_INFO_DATA_VERSION 1
+
+typedef struct {
+    uint8_t mac[8];
+    uint16_t short_addr;
+} nvm_link_info_t;
+
+typedef struct {
+    nvm_link_info_t nvm_link_info;
+    uint16_t write_delay;
+    bool loaded;
+} thread_nvm_store_link_info_t;
+
+const char *THREAD_NVM_ACTIVE_CONF_FILE = "a_c";
+#define ACTIVE_CONF_DATA_VERSION 1
+
+const char *DEVICE_CONF_FILE = "s_d";
+#define DEVICE_CONF_VERSION 1
+
+const char *THREAD_NVM_PENDING_CONF_FILE = "p_c";
+#define PENDING_CONF_DATA_VERSION 1
 
 static const char* thread_nvm_store_get_root_path(void);
-static int root_path_valid();
-static int thread_nvm_store_fast_data_read_version(uint32_t* version);
-static int thread_nvm_store_fast_data_create(void);
-static int thread_nvm_store_read(const char *file_name, void *data, uint32_t data_size);
-static int thread_nvm_store_write(const char *file_name, void *data, uint32_t data_size);
-static void create_fast_data_path (char* fast_data_path);
+static int root_path_valid(void);
+static int thread_nvm_store_read(const char *file_name, void *data, uint32_t data_size, uint32_t *version);
+static int thread_nvm_store_write(const char *file_name, void *data, uint32_t data_size, uint32_t version);
+static void thread_nvm_store_create_path(char* fast_data_path, const char* file_name);
 static int thread_nvm_store_fast_data_save(thread_nvm_fast_data_t* fast_data_to_set);
 static int thread_nvm_store_all_counters_store(uint32_t mac_frame_counter, uint32_t mle_frame_counter, uint32_t seq_counter);
+static void thread_nvm_store_link_info_delayed_write(uint32_t seconds);
 
-#define MAX_ROOT_PATH_LEN 100
+#define MAX_ROOT_PATH_LEN 150
 
 #define FAST_DATA_STRING_LEN (strlen(FAST_DATA_FILE)+strlen(thread_nvm_store_get_root_path())+1)
+#define ACTIVE_CONF_STRING_LEN (strlen(THREAD_NVM_ACTIVE_CONF_FILE)+strlen(thread_nvm_store_get_root_path())+1)
+#define DEVICE_CONF_STRING_LEN (strlen(DEVICE_CONF_FILE)+strlen(thread_nvm_store_get_root_path())+1)
+#define PENDING_CONF_STRING_LEN (strlen(THREAD_NVM_PENDING_CONF_FILE)+strlen(thread_nvm_store_get_root_path())+1)
+#define LINK_INFO_STRING_LEN (strlen(LINK_INFO_FILE)+strlen(thread_nvm_store_get_root_path())+1)
+
 
 thread_nvm_fast_data_t cached_fast_data;
+thread_nvm_store_link_info_t cached_link_info = {
+    .nvm_link_info.short_addr = LINK_INFO_SHORT_ADDR_NOT_SET,
+    .nvm_link_info.mac = {0,0,0,0,0,0,0,0},
+    .write_delay = LINK_INFO_WRITE_DELAY,
+    .loaded = false
+};
 
 static const char* thread_nvm_store_get_root_path(void)
 {
@@ -68,8 +107,9 @@ static const char* thread_nvm_store_get_root_path(void)
     return path;
 }
 
-static int root_path_valid() {
-    if (NULL==thread_nvm_store_get_root_path())
+static int root_path_valid(void)
+{
+    if (NULL==ns_file_system_get_root_path())
         return 0;
     int path_len = strlen(thread_nvm_store_get_root_path());
     if(path_len==0 || path_len>MAX_ROOT_PATH_LEN) {
@@ -77,30 +117,133 @@ static int root_path_valid() {
     }
     return 1;
 }
-
-int thread_nvm_store_init(void)
+int thread_nvm_store_device_configuration_write(uint8_t *mac_ptr, uint8_t *mleid_ptr)
 {
-    uint32_t version=0;
+   thread_nvm_device_conf_t d_c;
+   if (!root_path_valid()) {
+       return THREAD_NVM_FILE_ROOT_PATH_INVALID;
+   }
+   memcpy(d_c.mac, mac_ptr, sizeof(d_c.mac));
+   memcpy(d_c.mle_id, mleid_ptr, sizeof(d_c.mle_id));
+   char device_conf_path[DEVICE_CONF_STRING_LEN];
+   thread_nvm_store_create_path(device_conf_path, DEVICE_CONF_FILE);
+   return thread_nvm_store_write(device_conf_path, &d_c, sizeof(thread_nvm_device_conf_t), DEVICE_CONF_VERSION);
+}
 
+int thread_nvm_store_device_configuration_read(uint8_t *mac_ptr, uint8_t *mleid_ptr)
+{
+    int ret = THREAD_NVM_FILE_READ_ERROR;
+    if (!root_path_valid()) {
+        return THREAD_NVM_FILE_ROOT_PATH_INVALID;
+    }
+    char device_conf_path[DEVICE_CONF_STRING_LEN];
+    thread_nvm_store_create_path(device_conf_path, DEVICE_CONF_FILE);
+    uint32_t version;
+    thread_nvm_device_conf_t d_c;
+
+    ret = thread_nvm_store_read(device_conf_path, &d_c, sizeof(thread_nvm_device_conf_t), &version);
+    if(THREAD_NVM_FILE_SUCCESS==ret) {
+        if (THREAD_NVM_FILE_SUCCESS==ret && DEVICE_CONF_VERSION!=version) {
+            tr_info("fast data version mismatch %"PRIu32, version);
+            ret = THREAD_NVM_FILE_VERSION_WRONG;
+        }
+        else {
+            memcpy(mac_ptr, d_c.mac, sizeof(d_c.mac));
+            memcpy(mleid_ptr, d_c.mle_id, sizeof(d_c.mle_id));
+        }
+    }
+    return ret;
+}
+
+int thread_nvm_store_pending_configuration_write(void *data, uint16_t size)
+{
+    char pc_data_path[PENDING_CONF_STRING_LEN];
+    if (NULL==data) {
+        return THREAD_NVM_FILE_PARAMETER_INVALID;
+    }
+    if (!root_path_valid()) {
+        return THREAD_NVM_FILE_ROOT_PATH_INVALID;
+    }
+    thread_nvm_store_create_path(pc_data_path, THREAD_NVM_PENDING_CONF_FILE);
+    return thread_nvm_store_write(pc_data_path, data, size, PENDING_CONF_DATA_VERSION);
+}
+
+int thread_nvm_store_pending_configuration_read(void *data, uint16_t size)
+{
+    char pc_data_path[PENDING_CONF_STRING_LEN];
+    uint32_t version;
+    if (NULL==data) {
+        return THREAD_NVM_FILE_PARAMETER_INVALID;
+    }
+    if (!root_path_valid()) {
+        return THREAD_NVM_FILE_ROOT_PATH_INVALID;
+    }
+    thread_nvm_store_create_path(pc_data_path, THREAD_NVM_PENDING_CONF_FILE);
+
+    int ret = thread_nvm_store_read(pc_data_path, data, size, &version);
+    if (THREAD_NVM_FILE_SUCCESS==ret && PENDING_CONF_DATA_VERSION!=version) {
+        tr_info("Pending configuration version mismatch %"PRIu32, version);
+        return THREAD_NVM_FILE_VERSION_WRONG;
+    }
+    return ret;
+}
+
+int thread_nvm_store_active_configuration_write(void *data, uint16_t data_size)
+{
+    char ac_data_path[ACTIVE_CONF_STRING_LEN];
+    if (NULL==data) {
+        return THREAD_NVM_FILE_PARAMETER_INVALID;
+    }
     if (!root_path_valid()) {
         return THREAD_NVM_FILE_ROOT_PATH_INVALID;
     }
 
-    int ret = thread_nvm_store_fast_data_read_version(&version);
-    if(THREAD_NVM_FILE_READ_ERROR==ret) {
-        tr_info("need to create a new fastdata");
-        return thread_nvm_store_fast_data_create();
-    }
-
-    if (version!=FAST_DATA_VERSION) {
-        //handle new version format here
-        //if version is different do the reformatting and save new format.....
-        tr_info("Fast data version mismatch %u\n", (unsigned int)version);
-    }
-    return 0;
+    thread_nvm_store_create_path(ac_data_path, THREAD_NVM_ACTIVE_CONF_FILE);
+    return thread_nvm_store_write(ac_data_path, data, data_size, ACTIVE_CONF_DATA_VERSION);
 }
 
-int thread_nvm_store_seq_counter_store(uint32_t network_seq_counter)
+int thread_nvm_store_active_configuration_read(void *data, uint16_t data_size)
+{
+    char ac_data_path[ACTIVE_CONF_STRING_LEN];
+    uint32_t version;
+    if (NULL==data) {
+        return THREAD_NVM_FILE_PARAMETER_INVALID;
+    }
+    if (!root_path_valid()) {
+        return THREAD_NVM_FILE_ROOT_PATH_INVALID;
+    }
+    thread_nvm_store_create_path(ac_data_path, THREAD_NVM_ACTIVE_CONF_FILE);
+
+    int ret = thread_nvm_store_read(ac_data_path, data, data_size, &version);
+    if (THREAD_NVM_FILE_SUCCESS==ret && ACTIVE_CONF_DATA_VERSION!=version) {
+        tr_info("active configuration version mismatch %"PRIu32, version);
+        return THREAD_NVM_FILE_VERSION_WRONG;
+    }
+    return ret;
+}
+
+int thread_nvm_store_active_configuration_remove(void)
+{
+    if (!root_path_valid()) {
+        return THREAD_NVM_FILE_ROOT_PATH_INVALID;
+    }
+    char ac_data_path[ACTIVE_CONF_STRING_LEN];
+    thread_nvm_store_create_path(ac_data_path, THREAD_NVM_ACTIVE_CONF_FILE);
+    return remove(ac_data_path);
+}
+
+int thread_nvm_store_pending_configuration_remove(void)
+{
+    if (!root_path_valid()) {
+        return THREAD_NVM_FILE_ROOT_PATH_INVALID;
+    }
+    char ac_data_path[PENDING_CONF_STRING_LEN];
+    thread_nvm_store_create_path(ac_data_path, THREAD_NVM_PENDING_CONF_FILE);
+    return remove(ac_data_path);
+}
+
+
+int thread_nvm_store_seq_counter_write(uint32_t network_seq_counter)
 {
     int ret = THREAD_NVM_FILE_SUCCESS;
     if (cached_fast_data.seq_counter!=network_seq_counter) {
@@ -110,7 +253,7 @@ int thread_nvm_store_seq_counter_store(uint32_t network_seq_counter)
     return ret;
 }
 
-int thread_nvm_store_fast_data_check_and_store(uint32_t mac_frame_counter, uint32_t mle_frame_counter, uint32_t network_seq_counter)
+int thread_nvm_store_fast_data_check_and_write(uint32_t mac_frame_counter, uint32_t mle_frame_counter, uint32_t network_seq_counter)
 {
     int ret = THREAD_NVM_FILE_SUCCESS;
     if( ((int)(mac_frame_counter - cached_fast_data.mac_frame_counter) > MAC_FRAME_COUNTER_LIMIT) ||
@@ -125,7 +268,7 @@ int thread_nvm_store_fast_data_check_and_store(uint32_t mac_frame_counter, uint3
 }
 
 
-int thread_nvm_store_frame_counters_check_and_store(uint32_t mac_frame_counter, uint32_t mle_frame_counter)
+int thread_nvm_store_frame_counters_check_and_write(uint32_t mac_frame_counter, uint32_t mle_frame_counter)
 {
     int ret = THREAD_NVM_FILE_SUCCESS;
     if( ((int)(mac_frame_counter - cached_fast_data.mac_frame_counter) > MAC_FRAME_COUNTER_LIMIT) ||
@@ -143,7 +286,6 @@ static int thread_nvm_store_all_counters_store(uint32_t mac_frame_counter, uint3
     fast_data.mac_frame_counter = mac_frame_counter;
     fast_data.mle_frame_counter = mle_frame_counter;
     fast_data.seq_counter = network_seq_counter;
-    fast_data.version = FAST_DATA_VERSION;
     if (root_path_valid()) {
         return thread_nvm_store_fast_data_save(&fast_data);
     }
@@ -152,14 +294,13 @@ static int thread_nvm_store_all_counters_store(uint32_t mac_frame_counter, uint3
     }
 }
 
-int thread_nvm_store_fast_data_store(thread_nvm_fast_data_t* fast_data)
+int thread_nvm_store_fast_data_write(thread_nvm_fast_data_t* fast_data)
 {
     cached_fast_data.mac_frame_counter = fast_data->mac_frame_counter;
     cached_fast_data.mle_frame_counter = fast_data->mle_frame_counter;
     cached_fast_data.seq_counter = fast_data->seq_counter;
 
     if (root_path_valid()) {
-        fast_data->version=FAST_DATA_VERSION;
         return thread_nvm_store_fast_data_save(fast_data);
     }
     else {
@@ -167,44 +308,25 @@ int thread_nvm_store_fast_data_store(thread_nvm_fast_data_t* fast_data)
     }
 }
 
-static void create_fast_data_path (char* fast_data_path)
+static void thread_nvm_store_create_path(char* fast_data_path, const char* file_name)
 {
     strcpy(fast_data_path, thread_nvm_store_get_root_path());
-    strcat(fast_data_path, FAST_DATA_FILE);
-}
-
-static int thread_nvm_store_fast_data_read_version(uint32_t* version)
-{
-    thread_nvm_fast_data_t fast_data;
-    char fast_data_path[FAST_DATA_STRING_LEN];
-    create_fast_data_path(fast_data_path);
-
-    int ret = thread_nvm_store_read(fast_data_path, &fast_data, FAST_DATA_WORD_SIZE*4);
-
-    if(ret<0) {
-        return THREAD_NVM_FILE_READ_ERROR;
-    }
-    *version = fast_data.version;
-    return THREAD_NVM_FILE_SUCCESS;
-}
-
-static int thread_nvm_store_fast_data_create(void)
-{
-    thread_nvm_fast_data_t fast_data;
-    memset(&fast_data, 0, FAST_DATA_WORD_SIZE*4);
-    fast_data.version = FAST_DATA_VERSION;
-    char fast_data_path[FAST_DATA_STRING_LEN];
-    create_fast_data_path(fast_data_path);
-    return thread_nvm_store_write(fast_data_path, &fast_data, FAST_DATA_WORD_SIZE*4);
+    strcat(fast_data_path, file_name);
 }
 
 int thread_nvm_store_fast_data_read(thread_nvm_fast_data_t* fast_data)
 {
     int ret = THREAD_NVM_FILE_SUCCESS;
+
     if (root_path_valid()) {
         char fast_data_path[FAST_DATA_STRING_LEN];
-        create_fast_data_path(fast_data_path);
-        ret = thread_nvm_store_read(fast_data_path, fast_data, FAST_DATA_WORD_SIZE*4);
+        thread_nvm_store_create_path(fast_data_path, FAST_DATA_FILE);
+        uint32_t version;
+        ret = thread_nvm_store_read(fast_data_path, fast_data, sizeof(thread_nvm_fast_data_t), &version);
+        if (THREAD_NVM_FILE_SUCCESS==ret && FAST_DATA_VERSION!=version) {
+            tr_info("fast data version mismatch %"PRIu32, version);
+            return THREAD_NVM_FILE_VERSION_WRONG;
+        }
     }
     else {
         fast_data->mac_frame_counter = cached_fast_data.mac_frame_counter;
@@ -217,20 +339,29 @@ int thread_nvm_store_fast_data_read(thread_nvm_fast_data_t* fast_data)
 static int thread_nvm_store_fast_data_save(thread_nvm_fast_data_t* fast_data_to_set)
 {
     char fast_data_path[FAST_DATA_STRING_LEN];
-    create_fast_data_path(fast_data_path);
-    return thread_nvm_store_write(fast_data_path, fast_data_to_set, FAST_DATA_WORD_SIZE*4);
+    thread_nvm_store_create_path(fast_data_path, FAST_DATA_FILE);
+    return thread_nvm_store_write(fast_data_path, fast_data_to_set, sizeof(thread_nvm_fast_data_t), FAST_DATA_VERSION);
 }
 
-static int thread_nvm_store_write(const char *file_name, void *data, uint32_t data_size)
+static int thread_nvm_store_write(const char *file_name, void *data, uint32_t data_size, uint32_t version)
 {
     FILE *fp = fopen(file_name, "w");
     if(fp == NULL) {
+        tr_error("NVM open error: %s", file_name);
         return THREAD_NVM_FILE_CANNOT_OPEN;
     }
-    size_t n_bytes = fwrite(data, 1, data_size, fp);
+
+    size_t n_bytes = fwrite(&version, 1, sizeof(uint32_t), fp);
+    if (n_bytes!=sizeof(uint32_t)) {
+        tr_warning("NVM version write error");
+        fclose(fp);
+        return THREAD_NVM_FILE_WRITE_ERROR;
+    }
+
+    n_bytes = fwrite(data, 1, data_size, fp);
     fclose(fp);
     if (n_bytes!=data_size) {
-        tr_error("NVM write failed");
+        tr_error("NVM write error %s", file_name);
         return THREAD_NVM_FILE_WRITE_ERROR;
     }
     else {
@@ -239,19 +370,164 @@ static int thread_nvm_store_write(const char *file_name, void *data, uint32_t da
 }
 
 // returns 0 when ok
-static int thread_nvm_store_read(const char *file_name, void *data, uint32_t data_size)
+static int thread_nvm_store_read(const char *file_name, void *data, uint32_t data_size, uint32_t *version)
 {
     FILE *fp = fopen(file_name, "r");
     if(fp == NULL) {
+        tr_warning("File not found: %s", file_name);
         return THREAD_NVM_FILE_CANNOT_OPEN;
     }
-    size_t n_bytes = fread(data, 1, data_size, fp);
+
+    size_t n_bytes = fread(version, 1, sizeof(uint32_t), fp);
+    if (n_bytes!=sizeof(uint32_t)) {
+        tr_warning("NVM version read error %s", file_name);
+        fclose(fp);
+        return THREAD_NVM_FILE_READ_ERROR;
+    }
+
+    n_bytes = fread(data, 1, data_size, fp);
     fclose(fp);
     if (n_bytes!=data_size) {
-        tr_error("NVM read failed");
+        tr_error("NVM read error %s", file_name);
         return THREAD_NVM_FILE_READ_ERROR;
     }
     else {
         return THREAD_NVM_FILE_SUCCESS; // return how many bytes was written.
     }
 }
+
+int thread_nvm_store_link_info_read(void)
+{
+    nvm_link_info_t nvm_link_info_tmp;
+    int status;
+
+    if (!ns_file_system_get_root_path()) {
+        if (!memcmp(cached_link_info.nvm_link_info.mac, ADDR_UNSPECIFIED, 8) &&
+            cached_link_info.nvm_link_info.short_addr == LINK_INFO_SHORT_ADDR_NOT_SET) {
+            tr_info("link info not cached");
+            return THREAD_NVM_FILE_READ_ERROR;
+        }
+    }
+    cached_link_info.loaded = true;
+    char link_info_path[LINK_INFO_STRING_LEN];
+    strcpy(link_info_path, thread_nvm_store_get_root_path());
+    strcat(link_info_path, LINK_INFO_FILE);
+
+    uint32_t version=0;
+    status = thread_nvm_store_read(link_info_path, &nvm_link_info_tmp, sizeof(nvm_link_info_t), &version);
+
+    if (status != THREAD_NVM_FILE_SUCCESS) {
+        if (!memcmp(cached_link_info.nvm_link_info.mac, ADDR_UNSPECIFIED, 8) &&
+            cached_link_info.nvm_link_info.short_addr == LINK_INFO_SHORT_ADDR_NOT_SET) {
+            tr_info("link info not cached and read error %d", status);
+            cached_link_info.loaded = false;
+            return THREAD_NVM_FILE_READ_ERROR;
+        }
+        return status;
+    }
+    else if (ACTIVE_CONF_DATA_VERSION != version) {
+        tr_info("link info version mismatch %"PRIu32, version);
+        return THREAD_NVM_FILE_VERSION_WRONG;
+    }
+    memcpy(cached_link_info.nvm_link_info.mac, nvm_link_info_tmp.mac, 8);
+    cached_link_info.nvm_link_info.short_addr = nvm_link_info_tmp.short_addr;
+    tr_info("info read: %s parent short addr: %"PRIu16, trace_array(cached_link_info.nvm_link_info.mac, 8), cached_link_info.nvm_link_info.short_addr);
+    return THREAD_NVM_FILE_SUCCESS;
+}
+
+int thread_nvm_store_link_info_get(uint8_t *parent_mac64, uint16_t *my_short_address)
+{
+    if (!memcmp(cached_link_info.nvm_link_info.mac, ADDR_UNSPECIFIED, 8) &&
+        cached_link_info.nvm_link_info.short_addr == LINK_INFO_SHORT_ADDR_NOT_SET) {
+        tr_info("thread_nvm_store_link_info_get addr zeros");
+        return THREAD_NVM_FILE_READ_ERROR;
+    }
+
+    if (!cached_link_info.loaded) {
+        return THREAD_NVM_FILE_READ_ERROR;
+    }
+    // read data from cache if cached data is available
+    if (parent_mac64) {
+        memcpy(parent_mac64, cached_link_info.nvm_link_info.mac, 8);
+    }
+    if (my_short_address) {
+        *my_short_address = cached_link_info.nvm_link_info.short_addr;
+    }
+    return THREAD_NVM_FILE_SUCCESS;
+}
+
+int thread_nvm_store_link_info_clear(void)
+{
+    int status;
+    tr_info("thread_nvm_store_link_info_clear");
+    memset(cached_link_info.nvm_link_info.mac, 0, 8);
+    cached_link_info.nvm_link_info.short_addr = LINK_INFO_SHORT_ADDR_NOT_SET;
+
+    cached_link_info.loaded = false;
+
+    if (!ns_file_system_get_root_path()) {
+        return THREAD_NVM_FILE_ROOT_PATH_INVALID;
+    }
+
+    char link_info_path[LINK_INFO_STRING_LEN];
+    strcpy(link_info_path, thread_nvm_store_get_root_path());
+    strcat(link_info_path, LINK_INFO_FILE);
+
+    status = remove(link_info_path);
+
+    if (status != 0) {
+      return THREAD_NVM_FILE_REMOVE_ERROR;
+    }
+
+    return THREAD_NVM_FILE_SUCCESS;
+}
+
+int thread_nvm_store_link_info_write(uint8_t *parent_mac, uint16_t short_addr)
+{
+    //tr_info("write mac: %s parent short addr: %"PRIu16, trace_array(parent_mac, 8), short_addr);
+    if (parent_mac) {
+        memcpy(cached_link_info.nvm_link_info.mac, parent_mac, 8);
+    } else {
+        memset(cached_link_info.nvm_link_info.mac, 0, 8);
+        tr_info("setting to zero");
+    }
+    // when router parent is zeros, but my short address is the actual routing address.
+    cached_link_info.nvm_link_info.short_addr = short_addr;
+
+    if (cached_link_info.write_delay == LINK_INFO_WRITE_DONE) {
+        cached_link_info.write_delay = LINK_INFO_WRITE_DELAY; // delay writing some seconds
+    }
+
+    if (!ns_file_system_get_root_path()) {
+        return THREAD_NVM_FILE_ROOT_PATH_INVALID;
+    }
+
+    return THREAD_NVM_FILE_SUCCESS;
+}
+
+static void thread_nvm_store_link_info_delayed_write(uint32_t seconds)
+{
+    if (cached_link_info.write_delay == LINK_INFO_WRITE_DONE) {
+        return;
+    }
+    else if (cached_link_info.write_delay > seconds) {
+        cached_link_info.write_delay -= seconds;
+        return;
+    }
+    cached_link_info.write_delay = LINK_INFO_WRITE_DONE;
+
+    if (!ns_file_system_get_root_path()) {
+        return;
+    }
+
+    char link_info_path[LINK_INFO_STRING_LEN];
+    strcpy(link_info_path, thread_nvm_store_get_root_path());
+    strcat(link_info_path, LINK_INFO_FILE);
+    tr_info("link info write parent mac: %s parent short addr: %"PRIu16, trace_array(cached_link_info.nvm_link_info.mac, 8), cached_link_info.nvm_link_info.short_addr);
+    thread_nvm_store_write(link_info_path, &cached_link_info.nvm_link_info, sizeof(nvm_link_info_t), LINK_INFO_DATA_VERSION);
+}
+
+void thread_nvm_store_seconds_timer(uint32_t seconds)
+{
+    thread_nvm_store_link_info_delayed_write(seconds);
+}
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.h
index b5a386d5fd..d01a1719f2 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.h
@@ -53,26 +53,55 @@ extern "C" {
 #define THREAD_NVM_FILE_VERSION_WRONG     -3
 #define THREAD_NVM_FILE_CANNOT_OPEN       -4
 #define THREAD_NVM_FILE_ROOT_PATH_INVALID -5
+#define THREAD_NVM_FILE_PARAMETER_INVALID -6
+#define THREAD_NVM_FILE_REMOVE_ERROR      -7
 
 typedef struct {
-    uint32_t version;
     uint32_t mle_frame_counter;
     uint32_t mac_frame_counter;
     uint32_t seq_counter;
 } thread_nvm_fast_data_t;
 
-/* Creates nvm files if they don't exist. Checks file versions */
-int thread_nvm_store_init(void);
-/* reads all fast data from nvm */
+typedef struct {
+    uint8_t mac[8];
+    uint8_t mle_id[8];
+} thread_nvm_device_conf_t;
+
+/* reads all fast data from nvm, if the return values is THREAD_NVM_FILE_ROOT_PATH_INVALID, the cached values are returned.  */
 int thread_nvm_store_fast_data_read(thread_nvm_fast_data_t* fast_data);
 /* stores all fast data to nvm */
-int thread_nvm_store_fast_data_store(thread_nvm_fast_data_t* fast_data);
+int thread_nvm_store_fast_data_write(thread_nvm_fast_data_t* fast_data);
 /* stores new frame counters to nvm only if the any frame counter threshold is passed*/
-int thread_nvm_store_frame_counters_check_and_store(uint32_t mac_frame_counter, uint32_t mle_frame_counter);
+int thread_nvm_store_frame_counters_check_and_write(uint32_t mac_frame_counter, uint32_t mle_frame_counter);
 /* stores the frame counter and seq counter to nvm only if any threshold is passed*/
-int thread_nvm_store_fast_data_check_and_store(uint32_t mac_frame_counter, uint32_t mle_frame_counter, uint32_t network_seq_counter);
+int thread_nvm_store_fast_data_check_and_write(uint32_t mac_frame_counter, uint32_t mle_frame_counter, uint32_t network_seq_counter);
 /* stores the value to nvm only if it has changed */
+
+int thread_nvm_store_seq_counter_write(uint32_t network_seq_counter);
+/* stores the active configuration */
+int thread_nvm_store_active_configuration_write(void *data, uint16_t data_size);
+/* Reads the active configuration */
+int thread_nvm_store_active_configuration_read(void *data, uint16_t data_size);
+/* Removes the active configuration */
+int thread_nvm_store_active_configuration_remove(void);
+
+int thread_nvm_store_device_configuration_write(uint8_t *mac_ptr, uint8_t *mleid_ptr);
+int thread_nvm_store_device_configuration_read(uint8_t *mac_ptr, uint8_t *mleid_ptr);
+int thread_nvm_store_pending_configuration_write(void *data, uint16_t size);
+int thread_nvm_store_pending_configuration_read(void *data, uint16_t size);
+int thread_nvm_store_pending_configuration_remove(void);
+
 int thread_nvm_store_seq_counter_store(uint32_t network_seq_counter);
+/* read link info to cache */
+int thread_nvm_store_link_info_read(void);
+/* get link information */
+int thread_nvm_store_link_info_get(uint8_t *parent_mac64, uint16_t *my_short_address);
+/* clear link information */
+int thread_nvm_store_link_info_clear(void);
+/* write link information, will use caching and delayed writing */
+int thread_nvm_store_link_info_write(uint8_t *parent_mac, uint16_t short_addr);
+/* second timer for NVM store to delay operations */
+void thread_nvm_store_seconds_timer(uint32_t seconds);
 
 #ifdef __cplusplus
 }
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.c
index 9550628487..c6e8cd185f 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.c
@@ -100,7 +100,7 @@ static void thread_bootstrap_client_router_id_release_cb(int8_t interface_id, in
 static int thread_router_synch_accept_request_build(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, uint16_t shortAddress, uint8_t *challenge, uint8_t chalLen, uint8_t *tlvReq, uint8_t tlvReqLen);
 static int thread_router_accept_to_endevice(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, uint8_t *challenge, uint8_t chalLen);
 static int thread_router_accept_request_build(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, uint16_t shortAddress, uint8_t *challenge, uint8_t chalLen, uint8_t type, bool rssi_tlv, uint8_t rssi);
-static int thread_child_update_response(protocol_interface_info_entry_t *cur, uint8_t *dst_address, uint8_t mode, uint16_t short_address, uint32_t timeout, mle_tlv_info_t *addressRegisterTlv,mle_tlv_info_t *tlvReq, mle_tlv_info_t *challengeTlv);
+static int thread_child_update_response(protocol_interface_info_entry_t *cur, uint8_t *dst_address, uint8_t mode, uint16_t short_address, uint32_t timeout, mle_tlv_info_t *addressRegisterTlv,mle_tlv_info_t *tlvReq, mle_tlv_info_t *challengeTlv, uint64_t active_timestamp, uint64_t pending_timestamp);
 static int mle_build_and_send_data_response_msg(protocol_interface_info_entry_t *cur, uint8_t *dst_address, uint8_t *data_ptr, uint16_t data_len, mle_tlv_info_t *request_tlv, uint8_t mode);
 static int thread_attach_parent_response_build(protocol_interface_info_entry_t *cur, uint8_t *dstAddress, uint8_t *challenge, uint16_t chalLen, uint8_t linkMargin, uint8_t scanMask, uint8_t mode);
 static int mle_attach_child_id_response_build(protocol_interface_info_entry_t *cur, uint8_t *dstAddress,thread_pending_child_id_req_t *child_req,mle_neigh_table_entry_t *neigh_info);
@@ -399,21 +399,22 @@ static void thread_router_synch_receive_cb(int8_t interface_id, mle_message_t *m
                 if (thread_leader_service_thread_partitition_restart(interface_id, &routing)) {
                     return;
                 }
-                tr_info("Router synch success as Leader of network");
+                thread_network_data_request_send(cur, mle_msg->packet_src_address, false);
+                // force leader to learn active/pending sets from data response
+                cur->thread_info->leader_synced = true;
+                // Prevent the Leader to learn network data from data response
+                cur->thread_info->networkDataRequested = false;
+                tr_info("Router synch OK as Leader");
             } else {
-               // Remove possibly registered network data from leader
-               thread_management_client_network_data_unregister(cur->id, address16);
-
                // Decrement data version and request network data to be updated
                cur->thread_info->thread_leader_data->dataVersion--;
                cur->thread_info->thread_leader_data->stableDataVersion--;
                thread_network_data_request_send(cur, mle_msg->packet_src_address, true);
-               tr_info("Router synch success as Router");
+               tr_info("Router synch OK as Router");
             }
 
             thread_router_bootstrap_route_tlv_push(cur, routing.dataPtr, routing.tlvLen , linkMargin, entry_temp);
             thread_bootstrap_attached_ready(cur);
-
         }
 
         break;
@@ -680,16 +681,39 @@ static uint8_t *thread_tlv_add(protocol_interface_info_entry_t *cur, uint8_t *pt
     return ptr;
 }
 
-static int thread_child_update_response(protocol_interface_info_entry_t *cur, uint8_t *dst_address, uint8_t mode, uint16_t short_address, uint32_t timeout, mle_tlv_info_t *addressRegisterTlv,mle_tlv_info_t *tlvReq, mle_tlv_info_t *challengeTlv)
+static int thread_child_update_response(protocol_interface_info_entry_t *cur, uint8_t *dst_address, uint8_t mode, uint16_t short_address, uint32_t timeout, mle_tlv_info_t *addressRegisterTlv,mle_tlv_info_t *tlvReq, mle_tlv_info_t *challengeTlv, uint64_t active_timestamp, uint64_t pending_timestamp)
 {
     uint16_t i;
     uint16_t len = 64;
     uint32_t keySequence;
+    bool add_active_configuration = false;
+    bool add_pending_configuration = false;
+    uint64_t own_pending_timestamp = 0;
     uint8_t *ptr;
     if (!thread_info(cur)) {
         return -1;
     }
+    link_configuration_s *link_configuration;
+    link_configuration = thread_joiner_application_get_config(cur->id);
 
+    if (!link_configuration) {
+        return -1;
+    }
+
+    len += 10; // active timestamp tlv size
+    len += thread_pending_timestamp_tlv_size(cur);
+
+    if (!active_timestamp || active_timestamp != link_configuration->timestamp) {
+        len += thread_active_operational_dataset_size(cur);
+        add_active_configuration = true;
+    }
+    own_pending_timestamp = thread_joiner_application_pending_config_timestamp_get(cur->id);
+    // if pending config is not in sync from requested device
+    if (!pending_timestamp ||
+            (own_pending_timestamp && own_pending_timestamp != pending_timestamp)) {
+        len += thread_pending_operational_dataset_size(cur);
+        add_pending_configuration = true;
+    }
     if (tlvReq && tlvReq->tlvLen) {
         mle_tlv_ignore(tlvReq->dataPtr, tlvReq->tlvLen, MLE_TYPE_LL_FRAME_COUNTER);
         len += thread_tlv_len(cur, tlvReq->dataPtr, tlvReq->tlvLen, mode);
@@ -742,6 +766,16 @@ static int thread_child_update_response(protocol_interface_info_entry_t *cur, ui
         if (mle_tlv_requested(tlvReq->dataPtr, tlvReq->tlvLen, MLE_TYPE_ADDRESS16)) {
             ptr = mle_tlv_write_short_address(ptr, short_address);
         }
+        if (mle_tlv_requested(tlvReq->dataPtr, tlvReq->tlvLen, MLE_TYPE_NETWORK_DATA)) {
+            ptr = thread_active_timestamp_write(cur,ptr);
+            ptr = thread_pending_timestamp_write(cur,ptr);
+            if (add_active_configuration) {
+                ptr = thread_active_operational_dataset_write(cur, ptr);
+            }
+            if (add_pending_configuration) {
+                ptr = thread_pending_operational_dataset_write(cur, ptr);
+            }
+        }
     }
 
     if (mle_service_update_length_by_ptr(bufId,ptr)!= 0) {
@@ -777,13 +811,14 @@ static int mle_build_and_send_data_response_msg(protocol_interface_info_entry_t
 
     length += thread_pending_timestamp_tlv_size(cur);
 
-
     if (!active_timestamp_present_in_request || active_timestamp != link_configuration->timestamp) {
         length += thread_active_operational_dataset_size(cur);
         add_active_configuration = true;
     }
     own_pending_timestamp = thread_joiner_application_pending_config_timestamp_get(cur->id);
-    if (!pending_timestamp_present_in_request || (own_pending_timestamp && own_pending_timestamp != pending_timestamp)) {
+    // if pending config is not in sync from requested device
+    if (!pending_timestamp_present_in_request ||
+            (own_pending_timestamp && own_pending_timestamp != pending_timestamp)) {
         length += thread_pending_operational_dataset_size(cur);
         add_pending_configuration = true;
     }
@@ -912,34 +947,76 @@ static int thread_attach_parent_response_build(protocol_interface_info_entry_t *
     return 0;
 }
 
+int thread_router_bootstrap_reset_child_info(protocol_interface_info_entry_t *cur, mle_neigh_table_entry_t *child)
+{
+    /* Cleanup occurs for /any/ link we lose to something that looks like a child address,
+     * not just links that are /now/ our children.
+     * Due to REED/partition transitions the address may not look like a current child address;
+     * we could be holding a child entry for future repromotion to router with same ID.
+     */
+    if (thread_is_router_addr(child->short_adr) || child->short_adr >= 0xfffe) {
+        return -1;
+    }
+    tr_debug("Child free %x", child->short_adr);
+    thread_dynamic_storage_child_info_clear(cur->id, child);
+
+    /* As we are losing a link to a child address, we can assume that if we have an IP neighbour cache
+     * mapping to that address, it is no longer valid. We must have been their parent, and they must be
+     * finding a new parent, and hence a new 16-bit address. (Losing a link to a router address would not
+     * invalidate our IP->16-bit mapping.)
+     */
+    protocol_6lowpan_release_short_link_address_from_neighcache(cur, child->short_adr);
+
+    // If Child's RLOC16 appears in the Network Data send the RLOC16 to the Leader
+    if (thread_network_data_services_registered(&cur->thread_info->networkDataStorage, child->short_adr)) {
+        tr_debug("Remove references to Child's RLOC16 from the Network Data");
+        thread_management_client_network_data_unregister(cur->id, child->short_adr);
+    }
+
+    // Clear all (sleepy) child registrations to multicast groups
+    thread_child_mcast_entries_remove(cur, child->mac64);
+
+    return 0;
+}
+
 void thread_router_bootstrap_child_information_clear(protocol_interface_info_entry_t *cur)
 {
+    /* make sure that the child info (from previous partition if any)
+        is cleared if no router address is got from leader */
+
     if (!cur->thread_info) {
         return;
     }
 
-    if (cur->thread_info->routerShortAddress == 0xfffe) {
-        return;
-    }
-
-    // Remove registered entries in IP neighbour cache
-    ns_list_foreach_safe(ipv6_neighbour_t, neighbour, &cur->ipv6_neighbour_cache.list) {
-        if (neighbour->type == IP_NEIGHBOUR_REGISTERED) {
-            ipv6_neighbour_entry_remove(&cur->ipv6_neighbour_cache, neighbour);
-        }
-    }
-
     // Remove mle neighbour entries for children in previous partition
     mle_neigh_table_list_t *entry_list = mle_class_active_list_get(cur->id);
     if (!entry_list) {
         return;
     }
     ns_list_foreach_safe(mle_neigh_table_entry_t, table_entry, entry_list) {
-        if (!thread_is_router_addr(table_entry->short_adr) && thread_router_addr_from_addr(table_entry->short_adr) == cur->thread_info->routerShortAddress) {
+        if (table_entry->short_adr < 0xfffe && !thread_is_router_addr(table_entry->short_adr)) {
             mle_class_remove_entry(cur->id, table_entry);
         }
     }
+}
+static void thread_router_bootstrap_invalid_child_information_clear(protocol_interface_info_entry_t *cur, uint16_t router_rloc)
+{
 
+    tr_debug("Thread Short address changed old: %x new: %x", cur->thread_info->routerShortAddress, router_rloc);
+
+    mle_neigh_table_list_t *entry_list = mle_class_active_list_get(cur->id);
+    if (!entry_list) {
+        return;
+    }
+
+    // scrub neighbours with child addresses that are not ours
+    ns_list_foreach_safe(mle_neigh_table_entry_t, table_entry, entry_list) {
+        if (table_entry->short_adr < 0xfffe &&
+                !thread_is_router_addr(table_entry->short_adr) &&
+                thread_router_addr_from_addr(table_entry->short_adr) != router_rloc) {
+            mle_class_remove_entry(cur->id, table_entry);
+        }
+    }
 }
 
 static void thread_bootstrap_client_router_id_cb(int8_t interface_id, int8_t status, uint16_t router_rloc, const uint8_t router_mask_ptr[9])
@@ -976,10 +1053,14 @@ static void thread_bootstrap_client_router_id_cb(int8_t interface_id, int8_t sta
         parent_router_id = cur->thread_info->thread_endnode_parent->router_id;
     }
 
+    thread_router_bootstrap_invalid_child_information_clear(cur,router_rloc);
+
+    // Release network data from old address
+    cur->thread_info->localServerDataBase.release_old_address = true;
+
     //ADD New ML16
     // This should be used thread_bootstrap_update_ml16_address(cur, router_rloc);
     thread_clean_old_16_bit_address_based_addresses(cur);
-
     mac_helper_mac16_address_set(cur, router_rloc);
     cur->thread_info->routerShortAddress = router_rloc;
     memcpy(ml16, cur->thread_info->threadPrivatePrefixInfo.ulaPrefix, 8);
@@ -993,16 +1074,6 @@ static void thread_bootstrap_client_router_id_cb(int8_t interface_id, int8_t sta
         return;
     }
 
-    tr_info("Thread Short address changed old: %x new: %x", cur->thread_info->routerShortAddress, router_rloc);
-    /*the default routerShortAddress if nothing is requested is 0xfffe so eliminate this case
-        Also make sure that the child info (from previous partition if any)
-        is cleared if the router address requested is not what is got from leader */
-    if (cur->thread_info->routerShortAddress != router_rloc) {
-        thread_router_bootstrap_child_information_clear(cur);
-    }
-    // Release network data from old address
-    cur->thread_info->localServerDataBase.release_old_address = true;
-
     //  /* XXX Is short_src_adr ever reset? Is it undefined if info not in msg? */
     tr_debug("Route seq %d Router mask: %s", routeId, trace_array(router_mask_ptr, 8));
     cur->thread_info->routing.router_id_sequence_valid = false;
@@ -1194,7 +1265,7 @@ static bool thread_child_id_request(protocol_interface_info_entry_t *cur, mle_ne
         }
 
         //allocate child address if current is router, 0xffff or not our child
-        if (!thread_addr_is_equal_or_child(mac_helper_mac16_address_get(cur), entry_temp->short_adr)) {
+        if (!thread_addr_is_child(mac_helper_mac16_address_get(cur), entry_temp->short_adr)) {
             entry_temp->short_adr = thread_router_bootstrap_child_address_generate(cur);
         }
 
@@ -1824,6 +1895,8 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
                     uint8_t mode;
                     uint8_t status;
                     uint32_t timeout = 0;
+                    uint64_t active_timestamp = 0;
+                    uint64_t pending_timestamp = 0;
                     mle_tlv_info_t addressRegisterTlv = {0};
                     mle_tlv_info_t challengeTlv = {0};
                     mle_tlv_info_t tlv_req = {0};
@@ -1857,6 +1930,8 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
 
                     addressRegisterTlv.tlvType = MLE_TYPE_UNASSIGNED;
                     mle_tlv_read_tlv(MLE_TYPE_ADDRESS_REGISTRATION, mle_msg->data_ptr, mle_msg->data_length, &addressRegisterTlv);
+                    mle_tlv_read_64_bit_tlv(MLE_TYPE_ACTIVE_TIMESTAMP, mle_msg->data_ptr, mle_msg->data_length, &active_timestamp);
+                    mle_tlv_read_64_bit_tlv(MLE_TYPE_PENDING_TIMESTAMP, mle_msg->data_ptr, mle_msg->data_length, &pending_timestamp);
 
                     mle_tlv_read_tlv(MLE_TYPE_TLV_REQUEST, mle_msg->data_ptr, mle_msg->data_length, &tlv_req);
 
@@ -1879,7 +1954,7 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
 
                     tr_debug("Keep-Alive -->Respond Child");
                     //Response
-                    thread_child_update_response(cur, mle_msg->packet_src_address, mode, entry_temp->short_adr, timeout, &addressRegisterTlv, &tlv_req, &challengeTlv);
+                    thread_child_update_response(cur, mle_msg->packet_src_address, mode, entry_temp->short_adr, timeout, &addressRegisterTlv, &tlv_req, &challengeTlv, active_timestamp, pending_timestamp);
 
                 }
                 break;
@@ -2119,6 +2194,7 @@ void thread_router_bootstrap_child_id_reject(protocol_interface_info_entry_t *cu
 
 void thread_router_bootstrap_active_router_attach(protocol_interface_info_entry_t *cur)
 {
+    uint8_t *parent_mac_addr = NULL;
     arm_nwk_6lowpan_thread_test_print_routing_database(cur->id);
     uint16_t address16 = mac_helper_mac16_address_get(cur);
     cur->thread_info->thread_attached_state = THREAD_STATE_CONNECTED_ROUTER;
@@ -2136,7 +2212,10 @@ void thread_router_bootstrap_active_router_attach(protocol_interface_info_entry_
     thread_bootstrap_network_prefixes_process(cur);
     thread_nd_service_activate(cur->id);
     thread_router_bootstrap_mle_advertise(cur);
-    thread_nvm_store_link_info_file_write(cur);
+    if (cur->thread_info->thread_endnode_parent) {
+        parent_mac_addr = cur->thread_info->thread_endnode_parent->mac64;
+    }
+    thread_nvm_store_link_info_write(parent_mac_addr, mac_helper_mac16_address_get(cur));
 }
 
 static int thread_validate_own_routeid_from_new_mask(const uint8_t *master_router_id_mask, uint8_t router_id)
@@ -2267,6 +2346,7 @@ static void thread_reed_advertisements_cb(void* arg)
     if (cur->thread_info->thread_attached_state == THREAD_STATE_CONNECTED &&
             cur->thread_info->thread_device_mode == THREAD_DEVICE_MODE_ROUTER){
         thread_reed_advertise(cur);
+        thread_router_bootstrap_child_information_clear(cur);
         cur->thread_info->routerSelectParameters.reedAdvertisementTimeout = eventOS_timeout_ms(thread_reed_advertisements_cb, thread_reed_timeout_calculate(&cur->thread_info->routerSelectParameters) * 1000, cur);
     }
 }
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.h
index 9ef946699e..823655879d 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.h
@@ -48,6 +48,7 @@ void thread_router_bootstrap_reed_advertisements_start(protocol_interface_info_e
 int thread_router_bootstrap_mle_advertise(struct protocol_interface_info_entry *cur);
 
 void thread_router_bootstrap_child_information_clear(protocol_interface_info_entry_t *cur);
+int thread_router_bootstrap_reset_child_info(protocol_interface_info_entry_t *cur, mle_neigh_table_entry_t *child);
 uint16_t thread_router_bootstrap_child_count_get(protocol_interface_info_entry_t *cur);
 void thread_router_bootstrap_child_id_handler(struct protocol_interface_info_entry *cur);
 void thread_router_bootstrap_child_id_reject(struct protocol_interface_info_entry *cur);
@@ -93,6 +94,7 @@ void thread_router_bootstrap_address_change_notify_send(protocol_interface_info_
 #define thread_router_bootstrap_timer(cur, ticks)
 #define thread_router_bootstrap_random_upgrade_jitter() 0;
 #define thread_router_bootstrap_advertiment_analyze(cur, src_address, entry_temp, shortAddress)
+#define thread_router_bootstrap_reset_child_info(cur, child) 0
 
 #define thread_router_bootstrap_multicast_forwarder_enable(cur, buf) NULL
 #define thread_router_bootstrap_anycast_address_register(cur)
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.c
index 431637ba86..ec672ae26a 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.c
@@ -457,6 +457,8 @@ void thread_routing_update_id_set(protocol_interface_info_entry_t *cur, uint8_t
                 set_fast_route_entry(thread, i, FAST_ROUTE_INVALID_ID, THREAD_COST_INFINITE);
                 tr_info("Remove router (ID: %d)", i);
                 thread_nd_flush_neighbour_cache_for_short_addr(cur, thread_router_addr_from_id(i), true);
+                thread_routing_remove_link(cur, thread_router_addr_from_id(i));
+                thread_delete_route_entry_by_id(thread, i);
             }
         }
     }
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.h
index 93e6254c3f..9896e982c0 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.h
@@ -163,6 +163,9 @@ THREAD_ROUTING_FN bool thread_is_router_addr(uint16_t addr)
 /* Return true if b is a child of a */
 THREAD_ROUTING_FN bool thread_addr_is_child(uint16_t a, uint16_t b)
 {
+    if (thread_is_router_addr(b)) {
+        return false;
+    }
     return thread_router_addr_from_addr(b) == a;
 }
 
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/adaptation_interface.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/adaptation_interface.c
index 50da2fedb4..3cb43a30a2 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/adaptation_interface.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/6LoWPAN/adaptation_interface.c
@@ -45,6 +45,13 @@
 
 #define TRACE_GROUP "6lAd"
 
+// #define EXTRA_DEBUG_EXTRA
+#ifdef EXTRA_DEBUG_EXTRA
+#define tr_debug_extra(...) tr_debug(__VA_ARGS__)
+#else
+#define tr_debug_extra(...)
+#endif
+
 typedef struct {
     uint16_t tag;   /*!< Fragmentation datagram TAG ID */
     uint16_t size;  /*!< Datagram Total Size (uncompressed) */
@@ -57,7 +64,8 @@ typedef struct {
     uint8_t unfrag_len; /*!< Length of headers that precede the FRAG header */
     bool fragmented_data:1;
     bool first_fragment:1;
-    bool indirectData:1;
+    bool indirect_data:1;
+    bool indirect_data_cached:1; /* Data cached for delayed transmission as mac request is already active */
     buffer_t *buf;
     uint8_t *fragmenter_buf;
     ns_list_link_t      link; /*!< List link entry */
@@ -116,6 +124,8 @@ static int8_t lowpan_message_fragmentation_init(buffer_t *buf, fragmenter_tx_ent
 static bool lowpan_message_fragmentation_message_write(const fragmenter_tx_entry_t *frag_entry, mcps_data_req_t *dataReq);
 static void lowpan_adaptation_indirect_queue_free_message(struct protocol_interface_info_entry *cur, fragmenter_interface_t *interface_ptr, fragmenter_tx_entry_t *tx_ptr);
 
+static fragmenter_tx_entry_t* lowpan_adaptation_indirect_mac_data_request_active(fragmenter_interface_t *interface_ptr, fragmenter_tx_entry_t *tx_ptr);
+
 //Discover
 static fragmenter_interface_t *lowpan_adaptation_interface_discover(int8_t interfaceId)
 {
@@ -362,6 +372,7 @@ static fragmenter_tx_entry_t *lowpan_indirect_entry_allocate(uint16_t fragment_b
     indirec_entry->buf = NULL;
     indirec_entry->fragmented_data = false;
     indirec_entry->first_fragment = true;
+    indirec_entry->indirect_data_cached = false;
 
     return indirec_entry;
 }
@@ -480,7 +491,7 @@ static fragmenter_tx_entry_t * lowpan_adaptation_tx_process_init(fragmenter_inte
 
     lowpan_active_buffer_state_reset(tx_entry);
 
-    tx_entry->indirectData = indirect;
+    tx_entry->indirect_data = indirect;
 
     return tx_entry;
 }
@@ -589,6 +600,80 @@ static void lowpan_adaptation_data_request_primitiv_set(const buffer_t *buf, mcp
     }
 }
 
+static bool lowpan_adaptation_indirect_cache_sanity_check(protocol_interface_info_entry_t *cur, fragmenter_interface_t *interface_ptr)
+{
+    fragmenter_tx_entry_t *active_tx_entry;
+    ns_list_foreach(fragmenter_tx_entry_t, fragmenter_tx_entry, &interface_ptr->indirect_tx_queue) {
+        if (fragmenter_tx_entry->indirect_data_cached == false) {
+            // active entry, jump to next one
+            continue;
+        }
+
+        // cached entry found, check if it has pending data reguest
+        active_tx_entry = lowpan_adaptation_indirect_mac_data_request_active(interface_ptr, fragmenter_tx_entry);
+
+        if (active_tx_entry == NULL) {
+            // entry is in cache and is not sent to mac => trigger this
+            tr_debug_extra("sanity check, push seq %d to addr %s", fragmenter_tx_entry->buf->seq, trace_ipv6(fragmenter_tx_entry->buf->dst_sa.address));
+            fragmenter_tx_entry->indirect_data_cached = false;
+            lowpan_data_request_to_mac(cur, fragmenter_tx_entry->buf, fragmenter_tx_entry);
+            return true;
+        }
+    }
+
+    return false;
+}
+
+static bool lowpan_adaptation_indirect_cache_trigger(protocol_interface_info_entry_t *cur, fragmenter_interface_t *interface_ptr, fragmenter_tx_entry_t *tx_ptr)
+{
+    tr_debug_extra("lowpan_adaptation_indirect_cache_trigger()");
+
+    if (ns_list_count(&interface_ptr->indirect_tx_queue) == 0) {
+        return false;
+    }
+
+    /* Trigger first cached entry */
+    ns_list_foreach(fragmenter_tx_entry_t, fragmenter_tx_entry, &interface_ptr->indirect_tx_queue) {
+        if (fragmenter_tx_entry->indirect_data_cached) {
+            if (addr_ipv6_equal(tx_ptr->buf->dst_sa.address, fragmenter_tx_entry->buf->dst_sa.address)) {
+                tr_debug_extra("pushing seq %d to addr %s", fragmenter_tx_entry->buf->seq, trace_ipv6(fragmenter_tx_entry->buf->dst_sa.address));
+                fragmenter_tx_entry->indirect_data_cached = false;
+                lowpan_data_request_to_mac(cur, fragmenter_tx_entry->buf, fragmenter_tx_entry);
+                return true;
+            }
+        }
+    }
+
+    /* Sanity check, If nothing can be triggered from own address, check cache queue */
+    return lowpan_adaptation_indirect_cache_sanity_check(cur, interface_ptr);
+}
+
+static fragmenter_tx_entry_t* lowpan_adaptation_indirect_mac_data_request_active(fragmenter_interface_t *interface_ptr, fragmenter_tx_entry_t *tx_ptr)
+{
+    ns_list_foreach(fragmenter_tx_entry_t, fragmenter_tx_entry, &interface_ptr->indirect_tx_queue) {
+        if (fragmenter_tx_entry->indirect_data_cached == false) {
+            if (addr_ipv6_equal(tx_ptr->buf->dst_sa.address, fragmenter_tx_entry->buf->dst_sa.address)) {
+                tr_debug_extra("active seq: %d", fragmenter_tx_entry->buf->seq);
+                return fragmenter_tx_entry;
+            }
+        }
+    }
+    return NULL;
+}
+
+static fragmenter_tx_entry_t* lowpan_adaptation_indirect_first_cached_request_get(fragmenter_interface_t *interface_ptr, fragmenter_tx_entry_t *tx_ptr)
+{
+    ns_list_foreach(fragmenter_tx_entry_t, fragmenter_tx_entry, &interface_ptr->indirect_tx_queue) {
+        if (fragmenter_tx_entry->indirect_data_cached == true) {
+            if (addr_ipv6_equal(tx_ptr->buf->dst_sa.address, fragmenter_tx_entry->buf->dst_sa.address)) {
+                tr_debug_extra("first cached seq: %d", fragmenter_tx_entry->buf->seq);
+                return fragmenter_tx_entry;
+            }
+        }
+    }
+    return NULL;
+}
+
 static void lowpan_adaptation_make_room_for_small_packet(protocol_interface_info_entry_t *cur, fragmenter_interface_t *interface_ptr, mle_neigh_table_entry_t *neighbour_to_count)
 {
     if (interface_ptr->max_indirect_small_packets_per_child == 0) {
@@ -618,6 +703,7 @@ static void lowpan_adaptation_make_room_for_big_packet(struct protocol_interface
     ns_list_foreach_reverse_safe(fragmenter_tx_entry_t, tx_entry, &interface_ptr->indirect_tx_queue) {
         if (buffer_data_length(tx_entry->buf) > interface_ptr->indirect_big_packet_threshold) {
             if (++count >= interface_ptr->max_indirect_big_packets_total) {
+                tr_debug_extra("free seq: %d", tx_entry->buf->seq);
                 lowpan_adaptation_indirect_queue_free_message(cur, interface_ptr, tx_entry);
             }
         }
@@ -714,21 +800,45 @@ int8_t lowpan_adaptation_interface_tx(protocol_interface_info_entry_t *cur, buff
 
     if (indirect) {
         //Add to indirectQUue
+        fragmenter_tx_entry_t *tx_ptr_cached;
         mle_neigh_table_entry_t *mle_entry = mle_class_get_by_link_address(cur->id, buf->dst_sa.address + 2, buf->dst_sa.addr_type);
-
-        if (buffer_data_length(buf) <= interface_ptr->indirect_big_packet_threshold) {
-            lowpan_adaptation_make_room_for_small_packet(cur, interface_ptr, mle_entry);
-        } else {
-            lowpan_adaptation_make_room_for_big_packet(cur, interface_ptr);
-        }
-
-        ns_list_add_to_end(&interface_ptr->indirect_tx_queue, tx_ptr);
         if (mle_entry) {
             buf->link_specific.ieee802_15_4.indirectTTL = (uint32_t) mle_entry->timeout_rx * MLE_TIMER_TICKS_MS;
         } else {
             buf->link_specific.ieee802_15_4.indirectTTL = cur->mac_parameters->mac_in_direct_entry_timeout;
         }
 
+        tr_debug_extra("indirect seq: %d, addr=%s", tx_ptr->buf->seq, trace_ipv6(buf->dst_sa.address));
+
+        // Make room for new message if needed */
+        if (buffer_data_length(buf) <= interface_ptr->indirect_big_packet_threshold) {
+            lowpan_adaptation_make_room_for_small_packet(cur, interface_ptr, mle_entry);
+        } else {
+            lowpan_adaptation_make_room_for_big_packet(cur, interface_ptr);
+        }
+
+        if (lowpan_adaptation_indirect_mac_data_request_active(interface_ptr, tx_ptr)) {
+            // mac is handling previous data request, add new one to be cached */
+            tr_debug_extra("caching seq: %d", tx_ptr->buf->seq);
+            tx_ptr->indirect_data_cached = true;
+        }
+
+        ns_list_add_to_end(&interface_ptr->indirect_tx_queue, tx_ptr);
+
+        // Check if current message can be delivered to MAC or should some cached message be delivered first
+        tx_ptr_cached = lowpan_adaptation_indirect_first_cached_request_get(interface_ptr, tx_ptr);
+        if (tx_ptr->indirect_data_cached == false && tx_ptr_cached) {
+            tr_debug_extra("sending cached seq: %d", tx_ptr_cached->buf->seq);
+            // set current message to cache
+            tx_ptr->indirect_data_cached = true;
+            // swap entries
+            tx_ptr = tx_ptr_cached;
+            tx_ptr->indirect_data_cached = false;
+            buf = tx_ptr_cached->buf;
+        } else if (tx_ptr->indirect_data_cached == true) {
+            // There is mac data request ongoing and new req was sent to cache
+            return 0;
+        }
     }
 
     lowpan_data_request_to_mac(cur, buf, tx_ptr);
@@ -741,7 +851,6 @@ tx_error_handler:
 
 }
 
-
 static bool lowpan_adaptation_tx_process_ready(fragmenter_tx_entry_t *tx_ptr)
 {
     if (!tx_ptr->fragmented_data) {
@@ -880,11 +989,21 @@ int8_t lowpan_adaptation_interface_tx_confirm(protocol_interface_info_entry_t *c
 
             //Check is there more packets
             if (lowpan_adaptation_tx_process_ready(tx_ptr)) {
+                bool triggered_from_indirect_cache = false;
                 if (tx_ptr->fragmented_data && active_direct_confirm) {
                     //Clean
                     interface_ptr->fragmenter_active = false;
                 }
+
+                if (tx_ptr->buf->link_specific.ieee802_15_4.indirectTxProcess) {
+                    triggered_from_indirect_cache = lowpan_adaptation_indirect_cache_trigger(cur, interface_ptr, tx_ptr);
+                }
+
                 lowpan_adaptation_data_process_clean(interface_ptr, tx_ptr, map_mlme_status_to_socket_event(confirm->status));
+
+                if (triggered_from_indirect_cache) {
+                    return 0;
+                }
             } else {
                 lowpan_data_request_to_mac(cur, buf, tx_ptr);
             }
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Common_Protocols/ipv6.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Common_Protocols/ipv6.c
index 6d241a8bbe..bf19d1a568 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Common_Protocols/ipv6.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Common_Protocols/ipv6.c
@@ -1075,9 +1075,6 @@ static buffer_t *ipv6_consider_forwarding_multicast_packet(buffer_t *buf, protoc
         cur->if_special_multicast_forwarding(cur, buf);
     }
 
-    uint_fast8_t group_scope = addr_ipv6_multicast_scope(buf->dst_sa.address);
-    uint_fast8_t src_scope = addr_ipv6_scope(buf->src_sa.address, cur);
-
 #ifdef HAVE_MPL
     /* MPL does its own thing - we do not perform any "native" forwarding */
     if (buf->options.ip_extflags & IPEXT_HBH_MPL) {
@@ -1086,6 +1083,9 @@ static buffer_t *ipv6_consider_forwarding_multicast_packet(buffer_t *buf, protoc
 #endif
 
 #ifdef MULTICAST_FORWARDING
+    uint_fast8_t group_scope = addr_ipv6_multicast_scope(buf->dst_sa.address);
+    uint_fast8_t src_scope = addr_ipv6_scope(buf->src_sa.address, cur);
+
     /* Look at reverse path - check our route to the source address */
     ipv6_route_t *route = ipv6_route_choose_next_hop(buf->src_sa.address, cur->id, NULL);
 
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/DHCPv6_Server/DHCPv6_Server_service.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/DHCPv6_Server/DHCPv6_Server_service.c
index 3ee586b396..e33f3e361e 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/DHCPv6_Server/DHCPv6_Server_service.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/DHCPv6_Server/DHCPv6_Server_service.c
@@ -34,6 +34,7 @@
 #include "DHCPv6_Server/DHCPv6_server_service.h"
 #include "common_functions.h"
 #include "NWK_INTERFACE/Include/protocol.h"
+#include "6LoWPAN/Thread/thread_bbr_api_internal.h"
 #include "Common_Protocols/icmpv6.h"
 #include "dhcp_service_api.h"
 
@@ -105,13 +106,16 @@ int DHCPv6_server_respond_client(dhcpv6_gua_server_entry_s *serverBase, dhcpv6_r
             // coverity[returned_null] for ignoring protocol_stack_interface_info_get_by_id NULL return
             DHCPV6_server_service_remove_GUA_from_neighcache(protocol_stack_interface_info_get_by_id(serverBase->interfaceId), nonTemporalAddress.requestedAddress);
         }
+        if (thread_bbr_nd_entry_add(serverBase->interfaceId,dhcp_allocated_address->nonTemporalAddress, nonTemporalAddress.validLifeTime, serverBase->guaPrefix, NULL) == -1) {
+            // No nanostack BBR present we will put entry for application implemented BBR
+            ipv6_route_t *route = ipv6_route_add_with_info(dhcp_allocated_address->nonTemporalAddress, 128, serverBase->interfaceId, NULL, ROUTE_THREAD_PROXIED_HOST,serverBase->guaPrefix,0, nonTemporalAddress.validLifeTime, 0);
+            if (!route) {
+                address_allocated = false;
+                libdhcpv6_address_rm_from_allocated_list(serverBase,dhcp_allocated_address->nonTemporalAddress);
+            }
+
 
-        ipv6_route_t *route = ipv6_route_add_with_info(dhcp_allocated_address->nonTemporalAddress, 128, serverBase->interfaceId, NULL, ROUTE_THREAD_PROXIED_HOST,serverBase->guaPrefix,0, nonTemporalAddress.validLifeTime, 0);
-        if (!route) {
-            address_allocated = false;
-            libdhcpv6_address_rm_from_allocated_list(serverBase,dhcp_allocated_address->nonTemporalAddress);
         }
-
     }
 
     response->responseLength = libdhcpv6_address_reply_message_len(replyPacket->clientDUID.linkType, replyPacket->serverDUID.linkType, 0, replyPacket->rapidCommit, address_allocated);
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/MPL/mpl.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/MPL/mpl.c
index 1292e98302..b72600da69 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/MPL/mpl.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/MPL/mpl.c
@@ -1128,9 +1128,6 @@ static buffer_t *mpl_exthdr_provider(buffer_t *buf, ipv6_exthdr_stage_t stage, i
                 buf->options.ip_extflags &=~ IPEXT_HBH_MPL_UNFILLED;
                 buf->mpl_option_data_offset = IPV6_HDRLEN + 4;
                 mpl_forwarder_process_message(buf, domain, true);
-            } else {
-                uint8_t *iphdr = buffer_data_pointer(buf);
-                uint8_t *ext = iphdr + IPV6_HDRLEN;
             }
             *result = 0;
             return buf;
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol.h
index 9c298eab92..c2bb96fabd 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol.h
@@ -102,7 +102,9 @@ typedef enum icmp_state {
     ER_MLE_LINK_REQ = 13,
     ER_MLE_LINK_SHORT_SYNCH = 14,
     ER_MLE_LINK_ADDRESS_SYNCH = 15,
+#ifdef HAVE_RPL
     ER_ROUTER_SYNCH = 17,
+#endif
     ER_PANA_PING = 18,
     ER_PARENT_SYNCH_LOST = 19,
     ER_MLE_SCAN = 20,
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/RPL/rpl_data.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/RPL/rpl_data.c
index 2f42a64102..4c3a84a57e 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/RPL/rpl_data.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/RPL/rpl_data.c
@@ -51,6 +51,7 @@
 
 #define RPL_DATA_SR_INIT_SIZE (16*4)
 
+#ifdef HAVE_RPL_ROOT
 typedef struct rpl_data_sr {
     rpl_dao_target_t *target;   /* Target - note may be a prefix */
     uint16_t iaddr_size;
@@ -60,6 +61,7 @@ typedef struct rpl_data_sr {
 } rpl_data_sr_t;
 
 static rpl_data_sr_t *rpl_data_sr;
+#endif
 
 static const uint8_t *rpl_data_get_dodagid(const buffer_t *buf);
 
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/RPL/rpl_downward.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/RPL/rpl_downward.c
index b0b133f542..2f7d9c95d4 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/RPL/rpl_downward.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/RPL/rpl_downward.c
@@ -170,19 +170,6 @@ void rpl_downward_convert_dodag_preferences_to_dao_path_control(rpl_dodag_t *dod
     }
 }
 
-static uint_fast8_t rpl_downward_path_control_to_preference(uint8_t pc)
-{
-    if (pc >= 0x40) {
-        return 1;
-    } else if (pc >= 0x10) {
-        return 2;
-    } else if (pc >= 0x04) {
-        return 3;
-    } else {
-        return 4;
-    }
-}
-
 static void rpl_downward_target_refresh(rpl_dao_target_t *target)
 {
     target->need_seq_inc = true;
@@ -826,6 +813,19 @@ void rpl_instance_send_dao_no_path(rpl_instance_t *instance, rpl_dao_target_t *t
 #endif
 
 #ifdef HAVE_RPL_ROOT
+static uint_fast8_t rpl_downward_path_control_to_preference(uint8_t pc)
+{
+    if (pc >= 0x40) {
+        return 1;
+    } else if (pc >= 0x10) {
+        return 2;
+    } else if (pc >= 0x04) {
+        return 3;
+    } else {
+        return 4;
+    }
+}
+
 static rpl_dao_root_transit_t *rpl_downward_add_root_transit(rpl_dao_target_t *target, const uint8_t parent[16], uint8_t path_control)
 {
     //rpl_dao_root_transit_t *old_first = ns_list_get_first(&target->info.root.transits);
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/Common/sec_lib_definitions.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/Common/sec_lib_definitions.h
index 13c929e9b9..7c3ca3739b 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/Common/sec_lib_definitions.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/Common/sec_lib_definitions.h
@@ -317,6 +317,7 @@ typedef struct pana_session_t {
     bool session_ready:1;
     bool key_warp:1;
     bool user_server:1;
+    bool packet_delivered:1;
     /* Define Relay usage */
     uint8_t address_status;
     uint8_t session_relay_address[16];
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/Common/security_lib.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/Common/security_lib.c
index de642c030a..d000b4fb97 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/Common/security_lib.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/Common/security_lib.c
@@ -748,12 +748,12 @@ int8_t sec_suite_remove(sec_suite_t *cur) {
         return -1;
     }
 
-    if (cur->pana_session.pana_heap) {
-        ns_dyn_mem_free(cur->pana_session.pana_heap);
-        cur->pana_session.pana_heap = NULL;
-    }
+    pana_free_dynamic_ram(cur);
 
     sec_suite_tls_free(cur, true);
+#ifdef ECC
+    sec_ecc_state_free(cur);
+#endif
     ns_list_remove(&sec_suite_list, cur);
     ns_dyn_mem_free(cur);
     return 0;
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/PANA/eap_protocol.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/PANA/eap_protocol.c
index 704e579041..d28dfba0c2 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/PANA/eap_protocol.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/PANA/eap_protocol.c
@@ -42,6 +42,37 @@
 
 const uint8_t EAP_ANYMOUS[9] = {'a', 'n', 'o', 'n', 'y', 'm', 'o', 'u', 's'};
 
+static bool force_frag_last_retry = false;
+
+static bool force_frag_start_fail = false;
+
+static bool force_frag_timeout = false;
+
+static void eap_seq_back_to_accept(sec_suite_t *suite)
+{
+    if (suite->pana_session.eap_id_seq == 0) {
+        suite->pana_session.eap_id_seq = 0xff;
+    } else {
+        suite->pana_session.eap_id_seq--;
+    }
+}
+
+void pana_eap_fragmetation_start_filter(bool state)
+{
+    tr_debug("Set start state %u", state);
+    force_frag_start_fail = state;
+}
+
+void pana_eap_fragmetation_force_timeout(bool state)
+{
+    force_frag_timeout = state;
+}
+
+void pana_eap_fragmetation_force_retry(bool state)
+{
+    force_frag_last_retry = state;
+}
+
 static buffer_t *eap_common_headroom_get_to_buffer(buffer_t *buf, uint16_t header_size)
 {
     if ((buf = buffer_headroom(buf, header_size)) == 0) {
@@ -145,7 +176,7 @@ bool pana_eap_frag_re_tx(sec_suite_t *suite)
             buffer_data_length_set(f_buf, suite->pana_session.last_assy_size);
             goto success_push;
         }
-    } else if (suite->pana_session.eap_frag_buf) {
+    } else if (suite->pana_session.eap_frag_buf || suite->pana_session.packet_delivered) {
         f_buf = buffer_get(127);
         if (f_buf) {
 
@@ -241,9 +272,10 @@ buffer_t *eap_down(buffer_t *buf, sec_suite_t *suite)
 
 buffer_t *eap_up(buffer_t *buf, sec_suite_t *suite)
 {
+    eap_header_t header;
     uint8_t *ptr = buffer_data_pointer(buf);
     uint16_t payload_length = buffer_data_length(buf);
-    eap_header_t header;
+    uint8_t response_counter = suite->retry_counter;
     if (!eap_header_parse(ptr, payload_length, &header)) {
         return buffer_free(buf);
     }
@@ -337,17 +369,27 @@ buffer_t *eap_up(buffer_t *buf, sec_suite_t *suite)
                         if (suite->pana_session.eap_assy_buf) {
                             tr_debug("Free Frag Buf");
                             buffer_free(suite->pana_session.eap_assy_buf);
-                            suite->pana_session.eap_assy_buf  = 0;
+                            suite->pana_session.eap_assy_buf  = NULL;
                         }
                         suite->pana_session.assy_length = 0;
                         suite->pana_session.assy_off_set = 0;
                         suite->pana_session.last_assy_size = 0;
+                        suite->pana_session.packet_delivered = true;
+                        suite->retry_counter = 0;
                     }
                 }
             }
 
             if ((eap_tls_header.eap_tls_flags & EAP_TLS_MORE_FRAGMENTS) == 0) {
                 if (suite->pana_session.frag_length) {
+                    if (force_frag_last_retry || force_frag_timeout) {
+                        force_frag_last_retry = false;
+                        if (header.eap_code == EAP_RESPONSE) {
+                            suite->retry_counter = response_counter;
+                        }
+                        eap_seq_back_to_accept(suite);
+                        return buffer_free(buf);
+                    }
                     buffer_t *t_buf = suite->pana_session.eap_frag_buf;
 
                     uint16_t check_len = suite->pana_session.frag_off_set;
@@ -462,8 +504,14 @@ buffer_t *eap_up(buffer_t *buf, sec_suite_t *suite)
                 //Check did we have a already action
                 if (suite->pana_session.frag_length == 0) {
 
-                    buffer_t *f_buf = buffer_get(eap_tls_header.tls_length);
-                    tr_debug("First Fragment");
+                    buffer_t *f_buf = NULL;
+                    if (force_frag_start_fail) {
+                        tr_debug("Force to drop fragment");
+                        force_frag_start_fail = false;
+                    } else {
+                        tr_debug("First Fragment");
+                        f_buf = buffer_get(eap_tls_header.tls_length);
+                    }
                     if (f_buf) {
                         buffer_data_length_set(f_buf, eap_tls_header.tls_length);
                         memcpy(buffer_data_pointer(f_buf), eap_tls_header.data_ptr, eap_tls_header.tls_frame_length);
@@ -532,4 +580,20 @@ buffer_t *eap_up(buffer_t *buf, sec_suite_t *suite)
             return buffer_free(buf);
     }
 }
+#else
+void pana_eap_fragmetation_start_filter(bool state)
+{
+    (void) state;
+}
+
+void pana_eap_fragmetation_force_timeout(bool state)
+{
+    (void) state;
+}
+
+void pana_eap_fragmetation_force_retry(bool state)
+{
+    (void) state;
+}
+
 #endif
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/PANA/pana_client.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/PANA/pana_client.c
index c0cd102d59..8fb25cbb4b 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/PANA/pana_client.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/PANA/pana_client.c
@@ -657,9 +657,11 @@ static void pana_client_state_machine_func(sec_suite_t *suite)
 
             switch (suite->state) {
                 case TLS_KEY_CHANGE:
-                case TLS_INIT:
                     sec_lib_state_machine_trig(suite, TLS_ALERT_INTERNAL);
                     break;
+                case TLS_INIT: //Trig pana failure if not get any response from server
+                    sec_lib_state_machine_trig(suite, PANA_FAILURE);
+                    break;
 
                 default:
                     pana_client_pana_error_handler(suite);
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/PANA/pana_server.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/PANA/pana_server.c
index e697d68133..9d2b065fb4 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/PANA/pana_server.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Security/PANA/pana_server.c
@@ -741,13 +741,14 @@ static void pana_server_state_machine_func(sec_suite_t *suite)
 #ifdef ECC
             if (sec_auth_re_check(suite)) {
                 bool tx_start_OK = false;
-                if (suite->pana_session.assy_length && suite->pana_session.frag_length) {
+                if (suite->pana_session.assy_length || suite->pana_session.frag_length || suite->pana_session.packet_delivered) {
                     //Build next EAP Packet
                     //tr_debug("TX same again fragment piece");
                     tx_start_OK = pana_eap_frag_re_tx(suite);
 
                 } else {
                     if (tls_pana_server_exchange_build(suite)) {
+                        suite->pana_session.packet_delivered = false;
                         tx_start_OK = true;
                     }
 
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/fhss/fhss.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/fhss/fhss.c
index 532b21393f..9294007c6f 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/fhss/fhss.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/fhss/fhss.c
@@ -586,7 +586,7 @@ static int fhss_update_synch_monitor(fhss_structure_t *fhss_structure, const fhs
     /* If superframe changed during synchronization but remaining time to next superframe is high, it is likely that
      * superframe change is not valid anymore. Don't use this Beacon for syncronization monitoring.
      */
-    if (!(super_frame_changed && ((configuration->fhss_superframe_length - remaining_time_own) < CLOSE_TO_SUPERFRAME_LENGTH))) {
+    if ((configuration->fhss_superframe_length - remaining_time_own) > CLOSE_TO_SUPERFRAME_LENGTH) {
         remaining_time_own += (int32_t) configuration->fhss_superframe_length * super_frame_changed;
 
         int32_t prev_synch_fix = (time_to_next_superframe - remaining_time_own);
@@ -1001,6 +1001,7 @@ int fhss_add_beacon_info(fhss_structure_t *fhss_structure, uint16_t pan_id, uint
         beacon_info = fhss_create_beacon_info(fhss_structure);
     }
     if (!beacon_info) {
+        tr_error("Beacon data not allocated");
         return -2;
     }
     fhss_write_beacon_info(beacon_info, pan_id, source_address, timestamp, synch_info);
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/fhss/fhss_beacon_tasklet.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/fhss/fhss_beacon_tasklet.c
index 158782ec35..2ba416cf90 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/fhss/fhss_beacon_tasklet.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/fhss/fhss_beacon_tasklet.c
@@ -26,6 +26,8 @@
 #include "fhss.h"
 #include "fhss_beacon.h"
 #include "fhss_statistics.h"
+#include "fhss_mac_interface.h"
+#include "platform/arm_hal_interrupt.h"
 
 #include <string.h> // memset
 
@@ -121,15 +123,16 @@ static void fhss_beacon_tasklet_func(arm_event_s* event)
     // Update Beacon info lifetimes
     else if(event->event_type == FHSS_UPDATE_SYNCH_INFO_STORAGE)
     {
-        fhss_update_beacon_info_lifetimes(fhss_structure, fhss_structure->platform_functions.fhss_get_timestamp(fhss_structure->fhss_api));
+        fhss_update_beacon_info_lifetimes(fhss_structure, fhss_read_timestamp_cb(fhss_structure->fhss_api));
     }
 }
 
 void fhss_beacon_build(fhss_structure_t *fhss_structure, uint8_t* dest)
 {
     fhss_synchronization_beacon_payload_s temp_payload;
-
+    platform_enter_critical();
     fhss_beacon_update_payload(fhss_structure, &temp_payload);
+    platform_exit_critical();
     fhss_beacon_encode_raw(dest, &temp_payload);
 }
 
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/fhss/fhss_mac_interface.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/fhss/fhss_mac_interface.c
index 79461d0bae..ae839f3eda 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/fhss/fhss_mac_interface.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/fhss/fhss_mac_interface.c
@@ -22,6 +22,7 @@
 #include "Service_Libs/fhss/fhss.h"
 #include "Service_Libs/fhss/fhss_channel.h"
 #include "Service_Libs/fhss/fhss_beacon.h"
+#include "platform/arm_hal_interrupt.h"
 #include "randLIB.h"
 #include "ns_trace.h"
 
@@ -124,10 +125,12 @@ void fhss_receive_frame_cb(const fhss_api_t *api, uint16_t pan_id, uint8_t *sour
             if (!fhss_compare_with_synch_parent_address(fhss_structure, source_address)) {
                 // Synch parent address needs to be updated in case parent has changed
                 fhss_update_synch_parent_address(fhss_structure);
+                platform_enter_critical();
                 // Calculate time since the Beacon was received
                 uint32_t elapsed_time = api->read_timestamp(api) - timestamp;
                 // Synchronize to given PAN
                 fhss_beacon_received(fhss_structure, synch_info, elapsed_time);
+                platform_exit_critical();
             }
         }
     } else if (FHSS_SYNCH_REQUEST_FRAME == frame_type) {
@@ -201,14 +204,17 @@ void fhss_synch_state_set_cb(const fhss_api_t *api, fhss_states fhss_state, uint
 
     // State is already set
     if (fhss_structure->fhss_state == fhss_state) {
+        tr_debug("Synch same state %u", fhss_state);
         return;
     }
 
     if (fhss_state == FHSS_UNSYNCHRONIZED) {
+        tr_debug("FHSS down");
         fhss_down(fhss_structure);
     } else {
         // Do not synchronize to current pan
         if (fhss_structure->synch_panid == pan_id) {
+            tr_debug("Synch same panid %u", pan_id);
             return;
         }
         uint32_t datarate = fhss_structure->callbacks.read_datarate(api);
@@ -220,16 +226,20 @@ void fhss_synch_state_set_cb(const fhss_api_t *api, fhss_states fhss_state, uint
         fhss_beacon_info_t *beacon_info = fhss_get_beacon_info(fhss_structure, pan_id);
         if (beacon_info) {
             memcpy(fhss_structure->synch_parent, beacon_info->source_address, 8);
+            platform_enter_critical();
             // Calculate time since the Beacon was received
             uint32_t elapsed_time = api->read_timestamp(api) - beacon_info->timestamp;
             // Synchronize to given PAN
             fhss_beacon_received(fhss_structure, beacon_info->synch_info, elapsed_time);
+            platform_exit_critical();
             // Delete stored Beacon infos
             fhss_flush_beacon_info_storage(fhss_structure);
             fhss_structure->synch_panid = pan_id;
         } else if (fhss_is_synch_root(fhss_structure) == true) {
             // Synch root will start new network
             fhss_start_timer(fhss_structure, fhss_structure->synch_configuration.fhss_superframe_length, fhss_superframe_handler);
+        } else {
+            tr_error("Synch info not find");
         }
     }
     fhss_structure->fhss_state = fhss_state;
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/pan_blacklist/pan_blacklist.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/pan_blacklist/pan_blacklist.c
index 64887a1bcd..b6897a7e06 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/pan_blacklist/pan_blacklist.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/pan_blacklist/pan_blacklist.c
@@ -91,7 +91,13 @@ void pan_coordinator_blacklist_time_update(pan_coordinator_blaclist_cache_s *lis
     }
 }
 
-
+void pan_coordinator_blacklist_free(pan_coordinator_blaclist_cache_s *list_ptr)
+{
+    ns_list_foreach_safe(pan_coordinator_blacklist_entry_t, cur_ptr, &list_ptr->head) {
+        ns_list_remove(&list_ptr->head, cur_ptr);
+        ns_dyn_mem_free(cur_ptr);
+    }
+}
 
 bool pan_blacklist_filter(pan_blaclist_cache_s *list_ptr, uint16_t panid)
 {
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/pan_blacklist/pan_blacklist_api.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/pan_blacklist/pan_blacklist_api.h
index 9911bc5d75..f71cd80014 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/pan_blacklist/pan_blacklist_api.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/Service_Libs/pan_blacklist/pan_blacklist_api.h
@@ -57,6 +57,8 @@ void pan_blacklist_cache_init(pan_blaclist_cache_s *blacklist_cache);
 
 void pan_coordinator_blacklist_cache_init(pan_coordinator_blaclist_cache_s *blacklist_cache);
 
+void pan_coordinator_blacklist_free(pan_coordinator_blaclist_cache_s *list_ptr);
+
 void pan_blacklist_pan_set(pan_blaclist_cache_s *list_ptr, uint16_t panid, uint16_t timeout);
 
 void pan_cordinator_blacklist_pan_set(pan_coordinator_blaclist_cache_s *list_ptr, uint8_t *cordinator_data, uint16_t timeout);
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.c
index bb8f3d45e1..4c30a33209 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.c
@@ -401,6 +401,15 @@ void ipv6_neighbour_invalidate_ll_addr(ipv6_neighbour_cache_t *cache, addrtype_t
     }
 }
 
+void ipv6_neighbour_delete_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) {
+            ipv6_neighbour_entry_remove(cache, cur);
+        }
+    }
+}
+
 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) &&
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.h b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.h
index 391cb2b0b7..86d08569e7 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.h
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.h
@@ -154,6 +154,7 @@ extern bool ipv6_neighbour_is_probably_reachable(ipv6_neighbour_cache_t *cache,
 extern bool ipv6_neighbour_addr_is_probably_reachable(ipv6_neighbour_cache_t *cache, const uint8_t *address);
 extern bool ipv6_neighbour_ll_addr_match(const ipv6_neighbour_t *entry, addrtype_t ll_type, const uint8_t *ll_address);
 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 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);
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/libNET/src/net_load_balance.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/libNET/src/net_load_balance.c
index ddc0723f2a..ad79c2bd5f 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/libNET/src/net_load_balance.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/libNET/src/net_load_balance.c
@@ -263,22 +263,25 @@ void net_load_balance_internal_state_activate(protocol_interface_info_entry_t *i
 
 }
 
-
+#ifdef HAVE_RPL
+#ifdef HAVE_6LOWPAN_BORDER_ROUTER
 static int8_t net_load_balance_api_get_node_count_cb(void *lb_user, uint16_t *node_count)
 {
-#ifdef HAVE_RPL
     protocol_interface_info_entry_t *interface_ptr = lb_user;
     if (rpl_control_get_instance_dao_target_count(interface_ptr->rpl_domain, 1, NULL, node_count) ) {
         return 0;
     }
-#endif
+
     return -1;
 }
+#endif
+#endif
 
+#ifdef HAVE_RPL
+#ifdef HAVE_6LOWPAN_BORDER_ROUTER
 static int8_t net_load_balance_api_get_set_load_level_cb(void *lb_user, uint8_t load_level)
 {
     //Call DODAG preference
-#ifdef HAVE_RPL
     protocol_interface_info_entry_t *interface_ptr = lb_user;
     if (!interface_ptr->rpl_domain || interface_ptr->rpl_domain != protocol_6lowpan_rpl_domain || !protocol_6lowpan_rpl_root_dodag) {
         return -1;
@@ -290,12 +293,9 @@ static int8_t net_load_balance_api_get_set_load_level_cb(void *lb_user, uint8_t
 
     rpl_control_set_dodag_pref(protocol_6lowpan_rpl_root_dodag, RPL_DODAG_PREF_MASK - load_level);
     return 0;
-
-#else
-    return -1;
-#endif
 }
-
+#endif
+#endif
 
 int8_t net_load_balance_load_level_update_enable(int8_t interface_id, uint16_t expected_device_count)
 {
diff --git a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/libNET/src/ns_net.c b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/libNET/src/ns_net.c
index 7a1f9e29ec..88ee5ef95f 100644
--- a/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/libNET/src/ns_net.c
+++ b/features/nanostack/FEATURE_NANOSTACK/sal-stack-nanostack/source/libNET/src/ns_net.c
@@ -36,6 +36,7 @@
 #include "RPL/rpl_data.h"
 #endif
 #include "ccmLIB.h"
+#include "6LoWPAN/lowpan_adaptation_interface.h"
 #include "6LoWPAN/Bootstraps/network_lib.h"
 #include "6LoWPAN/Bootstraps/protocol_6lowpan.h"
 #include "6LoWPAN/Bootstraps/protocol_6lowpan_bootstrap.h"
@@ -1415,3 +1416,16 @@ void arm_ncache_flush(void)
 {
     nwk_interface_flush_neigh_cache();
 }
+
+int arm_nwk_sleepy_device_parent_buffer_size_set(int8_t interface_id, uint16_t big_packet_threshold, uint16_t small_packets_per_child_count, uint16_t big_packets_total_count)
+{
+    protocol_interface_info_entry_t *cur;
+
+    cur = protocol_stack_interface_info_get_by_id(interface_id);
+    if (cur) {
+        return lowpan_adaptation_indirect_queue_params_set(cur, big_packet_threshold,
+            big_packets_total_count, small_packets_per_child_count);
+    }
+    return -1;
+}
+