diff --git a/features/nanostack/sal-stack-nanostack/nanostack/ccmLIB.h b/features/nanostack/sal-stack-nanostack/nanostack/ccmLIB.h
index ff4e2b5a4f..50fa8872e2 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/ccmLIB.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/ccmLIB.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -18,6 +18,7 @@
 #define CCMLIB_H_
 
 #include "ns_types.h"
+#include "platform/arm_hal_aes.h"
 
 /**
  *
@@ -25,11 +26,11 @@
  * \brief CCM Library API.
  *
  *  \section ccm-api CCM Library API:
- *  - ccm_sec_init(), A function to init CCM library.
+ *  - ccm_sec_init(), A function to init CCM context.
  *  - ccm_process_run(), A function to run configured CCM process.
  *
  *  \section ccm-instruction CCM process sequence:
- *  1. Init CCM library by, ccm key, ccm_sec_init()
+ *  1. Init CCM context by, ccm key, ccm_sec_init()
  *      - security level
  *      - 128-bit CCM key
  *      - mode: AES_CCM_ENCRYPT or AES_CCM_DECRYPT
@@ -41,11 +42,6 @@
  *      -If 0 Process ok
  *      -< 0 MIC fail or parameter fail
  *
- *  \section ccm-mutex CCM Mutex for Multi Thread System
- *  If you are running a multi thread system and the CCM library will be used for multiple thread, do the following:
- *  1. Add compiler flag to library build process CCM_USE_MUTEX.
- *  2. Define OS-specific mutex at the application.
- *  3. Implement arm_ccm_mutex_lock() arm_ccm_mutex_unlock() function for using the generated and initialized mutex.
  */
 #ifdef __cplusplus
 extern "C" {
@@ -63,41 +59,39 @@ extern "C" {
 #define AES_CCM_DECRYPT                 0x01    /**< Decryption mode */
 
 
-/**
- * \brief A function for locking CCM mutex if the OS is multi thread. If you are using single thread create an empty function.
- */
-extern void arm_ccm_mutex_lock(void);
-/**
- * \brief A function for unlocking CCM mutex if the OS is multi thread. If you are using single thread create an empty function
- */
-extern void arm_ccm_mutex_unlock(void);
-
 /*!
  * \struct ccm_globals_t
  * \brief CCM global structure.
  * The structure is used for configuring NONCE, adata and data before calling ccm_process_run().
  */
 typedef struct {
-    uint8_t exp_nonce[15];  /**< CCM NONCE buffer Nonce. */
-    uint8_t *data_ptr;      /**< Pointer to data IN. */
-    uint16_t data_len;      /**< Length of data IN. */
+    uint8_t exp_nonce[15];      /**< CCM NONCE buffer Nonce. */
+    uint8_t *data_ptr;          /**< Pointer to data IN. */
+    uint16_t data_len;          /**< Length of data IN. */
     const uint8_t *adata_ptr;   /**< Pointer to authentication data. */
-    uint16_t adata_len;     /**< Length of authentication data. */
-    uint8_t mic_len;        /**< ccm_sec_init() sets here the length of MIC. */
-    uint8_t *mic;           /**< Encrypt process writes MIC. Decrypt reads it and compares it with the MIC obtained from data. */
+    uint16_t adata_len;         /**< Length of authentication data. */
+    unsigned ccm_encode_mode:1; /**< Encryption modeAES_CCM_ENCRYPT or AES_CCM_DECRYPT. */
+    unsigned ccm_sec_level:3;   /**< Encryption operation security level 0-7. */
+    unsigned ccm_l_param:4;     /**< Can be 2 or 3. 2 when NONCE length is 13 and 3 when 12*/
+    uint8_t mic_len;            /**< ccm_sec_init() sets here the length of MIC. */
+    uint8_t *mic;               /**< Encrypt process writes MIC. Decrypt reads it and compares it with the MIC obtained from data. */
+    const uint8_t *key_ptr;     /**< Encyption key pointer to 128-bit key. */
+    arm_aes_context_t *aes_context; /**< Allocated AES context. */
 } ccm_globals_t;
 
+
 /**
- * \brief A function to initialize the CCM library.
+ * \brief A function to initialize the CCM context.
+ * \param ccm_context pointer to initialized XXM context
  * \param sec_level Used CCM security level (0-7).
  * \param ccm_key Pointer to 128-key.
  * \param mode AES_CCM_ENCRYPT or AES_CCM_DECRYPT.
  * \param ccm_l Can be 2 or 3. 2 when NONCE length is 13 and 3 when 12. (NONCE length = (15-ccm_l))
  *
- * \return Pointer to Global CCM parameter buffer.
- * \return 0 When parameter fails or CCM is busy.
+ * \return true when AES context allocation is OK and given parameters.
+ * \return false CCM parameters or AES context allocation fail.
  */
-extern ccm_globals_t *ccm_sec_init(uint8_t sec_level, const uint8_t *ccm_key, uint8_t mode, uint8_t ccm_l);
+extern bool ccm_sec_init(ccm_globals_t *ccm_context, uint8_t sec_level, const uint8_t *ccm_key, uint8_t mode, uint8_t ccm_l);
 
 /**
  * \brief A function to run the configured CCM process.
@@ -109,6 +103,14 @@ extern ccm_globals_t *ccm_sec_init(uint8_t sec_level, const uint8_t *ccm_key, ui
  * \return -2 Null pointer given to function.
  */
 extern int8_t ccm_process_run(ccm_globals_t *ccm_params);
+
+/**
+ * \brief A function to free aes context. Call only if ccm_process_run() is not called
+ * \param ccm_params CCM parameters
+ *
+ */
+extern void ccm_free(ccm_globals_t *ccm_params);
+
 #ifdef __cplusplus
 }
 #endif
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/fhss_api.h b/features/nanostack/sal-stack-nanostack/nanostack/fhss_api.h
index 7f16f0cc05..16820aeaae 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/fhss_api.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/fhss_api.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -74,17 +74,16 @@ typedef bool fhss_use_broadcast_queue(const fhss_api_t *api, bool is_broadcast_a
  * @param is_broadcast_addr Destination address type of packet (true if broadcast address).
  * @param destination_address Destination MAC address.
  * @param frame_type Frame type of packet (Frames types are defined by FHSS api).
- * @param synch_info Pointer to where FHSS synchronization info is written (if synch frame).
  * @param frame_length MSDU length of the frame.
  * @param phy_header_length PHY header length.
  * @param phy_tail_length PHY tail length.
+ * @param tx_time TX time.
  * @return  0 Success.
  * @return -1 Transmission of the packet is currently not allowed, try again.
  * @return -2 Invalid api.
  * @return -3 Broadcast packet on Unicast channel (not allowed), push packet back to queue.
- * @return -4 Synchronization info missing.
  */
-typedef int fhss_tx_handle(const fhss_api_t *api, bool is_broadcast_addr, uint8_t *destination_address, int frame_type, uint8_t *synch_info, uint16_t frame_length, uint8_t phy_header_length, uint8_t phy_tail_length);
+typedef int fhss_tx_handle(const fhss_api_t *api, bool is_broadcast_addr, uint8_t *destination_address, int frame_type, uint16_t frame_length, uint8_t phy_header_length, uint8_t phy_tail_length, uint32_t tx_time);
 
 /**
  * @brief Check TX permission.
@@ -152,6 +151,17 @@ typedef uint32_t fhss_read_timestamp(const fhss_api_t *api);
  */
 typedef uint16_t fhss_get_retry_period(const fhss_api_t *api, uint8_t *destination_address, uint16_t phy_mtu);
 
+/**
+ * @brief Write synchronization info to given pointer.
+ * @param api FHSS instance.
+ * @param ptr Pointer to data. Start of written data for Synch frame. Start of IE header for Data frame.
+ * @param length Length of IE header. Ignored when Synch frame.
+ * @param frame_type Frame type of packet (Frames types are defined by FHSS api).
+ * @param tx_time TX time must be referenced to the first symbol following the SFD of the transmitted frame.
+ * @return -1 on fail, write length otherwise.
+ */
+typedef int16_t fhss_write_synch_info(const fhss_api_t *api, uint8_t *ptr, uint8_t length, int frame_type, uint32_t tx_time);
+
 /**
  * @brief Initialize MAC functions.
  * @param api FHSS instance.
@@ -177,6 +187,7 @@ struct fhss_api {
     fhss_synch_state_set *synch_state_set;              /**< Change synchronization state. */
     fhss_read_timestamp *read_timestamp;                /**< Read timestamp. */
     fhss_get_retry_period *get_retry_period;            /**< Get retransmission period. */
+    fhss_write_synch_info *write_synch_info;            /**< Write synchronization info to TX frame*/
     fhss_init_callbacks *init_callbacks;                /**< Initialize MAC functions. */
 };
 
@@ -256,6 +267,18 @@ typedef int mac_broadcast_notify(const fhss_api_t *fhss_api, uint32_t broadcast_
  */
 typedef int mac_read_coordinator_mac_address(const fhss_api_t *fhss_api, uint8_t *mac_address);
 
+/**
+ * @brief Read synchronization info for a specific peer.
+ * @param fhss_api FHSS instance.
+ * @param info_ptr Pointer to info data.
+ * @param mac_address MAC address pointer.
+ * @param info_type Type of the read info (UTT-IE, BT-IE, US-IE, BS-IE).
+ * @param rx_timestamp Time of reception of the element.
+ * @return  >=0 Length of the info.
+ * @return -1 Fail.
+ */
+typedef int mac_read_synch_info(const fhss_api_t *fhss_api, uint8_t *info_ptr, uint8_t *mac_address, int info_type, uint32_t rx_timestamp);
+
 /**
  * \brief Struct fhss_callback defines functions that software MAC needs to implement.
  * Function pointers are passed to FHSS using fhss_init_callbacks function.
@@ -270,6 +293,7 @@ struct fhss_callback {
     mac_tx_poll *tx_poll;                                       /**< Poll TX queue. */
     mac_broadcast_notify *broadcast_notify;                     /**< Broadcast channel notification from FHSS. */
     mac_read_coordinator_mac_address *read_coord_mac_address;   /**< Read coordinator MAC address. */
+    mac_read_synch_info *read_synch_info;                       /**< Read information element for a specific MAC address. */
 };
 
 #ifdef __cplusplus
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/fhss_config.h b/features/nanostack/sal-stack-nanostack/nanostack/fhss_config.h
index 5cfad5dd1f..5695367659 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/fhss_config.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/fhss_config.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -28,6 +28,23 @@
 extern "C" {
 #endif
 
+#include "fhss_ws_extension.h"
+
+/**
+ * @brief WS channel functions.
+ */
+typedef enum
+{
+    /** Fixed channel. */
+    WS_FIXED_CHANNEL,
+    /** TR51 channel function. */
+    WS_TR51CF,
+    /** Direct Hash channel function. */
+    WS_DH1CF,
+    /** Vendor Defined channel function. */
+    WS_VENDOR_DEF_CF
+} fhss_ws_channel_functions;
+
 /**
  * \brief Struct fhss_tuning_parameter defines FHSS tuning parameters.
  * All delays are given in microseconds.
@@ -63,20 +80,59 @@ typedef struct fhss_configuration
 
 } fhss_configuration_t;
 
+/**
+ * @brief Get channel using vendor defined channel function.
+ * @param api FHSS instance.
+ * @param slot Slot number in channel schedule.
+ * @param eui64 EUI-64 address of node for which the (unicast) schedule is calculated. NULL for broadcast schedule.
+ * @param bsi Broadcast schedule identifier used in (broadcast) schedule calculation.
+ * @param number_of_channels Number of channels in schedule.
+ * @return Channel.
+ */
+typedef int32_t fhss_vendor_defined_cf(const fhss_api_t *api, uint16_t slot, uint8_t eui64[8], uint16_t bsi, uint16_t number_of_channels);
+
+/**
+ * \brief Struct fhss_ws_configuration defines configuration of WS FHSS.
+ */
+typedef struct fhss_ws_configuration
+{
+    /** WS channel function. */
+    fhss_ws_channel_functions ws_channel_function;
+
+    /** Broadcast schedule identifier. */
+    uint16_t bsi;
+
+    /** Unicast dwell interval. Range: 15-250 milliseconds. */
+    uint8_t fhss_uc_dwell_interval;
+
+    /** Broadcast interval. Duration between broadcast dwell intervals. Range: 0-16777216 milliseconds. */
+    uint32_t fhss_broadcast_interval;
+
+    /** Broadcast dwell interval. Range: 15-250 milliseconds. */
+    uint8_t fhss_bc_dwell_interval;
+
+    /** Channel mask. */
+    uint32_t channel_mask[8];
+
+    /** Vendor defined channel function. */
+    fhss_vendor_defined_cf *vendor_defined_cf;
+
+} fhss_ws_configuration_t;
+
 /**
  * \brief Struct fhss_timer defines interface between FHSS and FHSS platform timer.
  * Application must implement FHSS timer driver which is then used by FHSS with this interface.
  */
 typedef struct fhss_timer
 {
-    /** Start timeout (1us) */
+    /** Start timeout (1us). Timer must support multiple simultaneous timeouts */
     int (*fhss_timer_start)(uint32_t, void (*fhss_timer_callback)(const fhss_api_t *fhss_api, uint16_t), const fhss_api_t *fhss_api);
 
     /** Stop timeout */
-    int (*fhss_timer_stop)(const fhss_api_t *fhss_api);
+    int (*fhss_timer_stop)(void (*fhss_timer_callback)(const fhss_api_t *fhss_api, uint16_t), const fhss_api_t *fhss_api);
 
     /** Get remaining time of started timeout*/
-    uint32_t (*fhss_get_remaining_slots)(const fhss_api_t *fhss_api);
+    uint32_t (*fhss_get_remaining_slots)(void (*fhss_timer_callback)(const fhss_api_t *fhss_api, uint16_t), const fhss_api_t *fhss_api);
 
     /** Get timestamp since initialization of driver. Overflow of 32-bit counter is allowed (1us) */
     uint32_t (*fhss_get_timestamp)(const fhss_api_t *fhss_api);
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/fhss_ws_extension.h b/features/nanostack/sal-stack-nanostack/nanostack/fhss_ws_extension.h
new file mode 100644
index 0000000000..c440451204
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/nanostack/fhss_ws_extension.h
@@ -0,0 +1,113 @@
+/*
+ * Copyright (c) 2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+/**
+ * \file fhss_ws_extension.h
+ * \brief
+ */
+
+#ifndef FHSS_WS_EXT_H
+#define FHSS_WS_EXT_H
+
+#include "ns_types.h"
+#include "fhss_api.h"
+
+#ifdef __cplusplus
+extern "C" {
+#endif
+
+/**
+ * @brief unicast_timing_info Unicast timing/hopping schedule information structure.
+ */
+typedef struct unicast_timing_info {
+    unsigned unicast_channel_function:3;        /**< Unicast schedule channel function */
+    uint8_t unicast_dwell_interval;             /**< Unicast dwell interval */
+    uint16_t unicast_number_of_channels;        /**< Unicast number of channels */
+    uint_fast24_t ufsi;                         /**< Unicast fractional sequence interval */
+    uint32_t utt_rx_timestamp;                  /**< UTT-IE reception timestamp */
+} unicast_timing_info_t;
+
+/**
+ * @brief broadcast_timing_info Broadcast timing/hopping schedule information structure.
+ */
+typedef struct broadcast_timing_info {
+    unsigned broadcast_channel_function:3;      /**< Broadcast schedule channel function */
+    uint8_t broadcast_dwell_interval;           /**< Broadcast dwell interval */
+    uint16_t broadcast_slot;                    /**< Broadcast slot number */
+    uint16_t broadcast_schedule_id;             /**< Broadcast schedule identifier */
+    uint_fast24_t broadcast_interval_offset;    /**< Broadcast interval offset */
+    uint32_t broadcast_interval;                /**< Broadcast interval */
+    uint32_t bt_rx_timestamp;                   /**< BT-IE reception timestamp */
+} broadcast_timing_info_t;
+
+/**
+ * @brief fhss_ws_neighbor_timing_info Neighbor timing/hopping schedule information structure.
+ */
+typedef struct fhss_ws_neighbor_timing_info {
+    uint8_t clock_drift;                        /**< Neighbor clock drift */
+    uint8_t timing_accuracy;                    /**< Neighbor timing accuracy */
+    unicast_timing_info_t uc_timing_info;       /**< Neighbor unicast timing info */
+    broadcast_timing_info_t bc_timing_info;     /**< Neighbor broadcast timing info */
+    uint32_t *excluded_channels;                /**< Neighbor excluded channels (bit mask) */
+} fhss_ws_neighbor_timing_info_t;
+
+/**
+ * @brief Get neighbor timing/hopping schedule.
+ * @param api FHSS instance.
+ * @param eui64 EUI-64 address of node for which the info is requested.
+ * @return Neighbor timing/hopping schedule.
+ */
+typedef fhss_ws_neighbor_timing_info_t *fhss_get_neighbor_info(const fhss_api_t *api, uint8_t eui64[8]);
+
+/**
+ * @brief Set parent which broadcast channels must be listened by FHSS.
+ * @param fhss_api FHSS instance.
+ * @param eui64 EUI-64 address of parent.
+ * @param bc_timing_info Pointer to parent broadcast timing/hopping schedule info.
+ * @return 0 on success, -1 on fail.
+ */
+extern int ns_fhss_ws_set_parent(const fhss_api_t *fhss_api, const uint8_t eui64[8], const broadcast_timing_info_t *bc_timing_info);
+
+/**
+ * @brief Remove parent which was set by ns_fhss_ws_set_parent function.
+ * @param fhss_api FHSS instance.
+ * @param eui64 EUI-64 address of parent.
+ * @return 0 on success, -1 on fail.
+ */
+extern int ns_fhss_ws_remove_parent(const fhss_api_t *fhss_api, const uint8_t eui64[8]);
+
+/**
+ * @brief Set neighbor timing/hopping schedule request function.
+ * @param fhss_api FHSS instance.
+ * @param get_neighbor_info Neighbor info function pointer.
+ * @return 0 on success, -1 on fail.
+ */
+extern int ns_fhss_set_neighbor_info_fp(const fhss_api_t *fhss_api, fhss_get_neighbor_info *get_neighbor_info);
+
+/**
+ * @brief Set node hop count. Hop count is used to specify TX/RX slot. When hop count is set to 0xFF, TX/RX slots are ignored.
+ * @param fhss_api FHSS instance.
+ * @param hop_count Hop count to be set.
+ * @return 0 on success, -1 on fail.
+ */
+extern int ns_fhss_ws_set_hop_count(const fhss_api_t *fhss_api, const uint8_t hop_count);
+
+#ifdef __cplusplus
+}
+#endif
+
+#endif // FHSS_WS_EXT_H
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/mac_api.h b/features/nanostack/sal-stack-nanostack/nanostack/mac_api.h
index 079d533de9..b889ee25a7 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/mac_api.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/mac_api.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -32,6 +32,8 @@
 extern "C" {
 #endif
 
+struct channel_list_s;
+
 typedef struct mac_api_s mac_api_t;
 
 /**
@@ -120,6 +122,17 @@ typedef void mlme_request(const mac_api_t* api, mlme_primitive id, const void *d
  */
 typedef void mcps_data_request(const mac_api_t* api, const mcps_data_req_t *data);
 
+/**
+ * @brief mcps_request MCPS_DATA with IE extions request call
+ * @param api API to handle the request
+ * @param data MCPS-DATA.request specific values
+ * @param ie_ext Information element list to MCPS-DATA.request
+ * @param asynch_channel_list Optional channel list to asynch data request. Give NULL when normal data request.
+ *
+ * Asynch data request is mac standard extension. asynch_channel_list include channel mask which channel message is requested to send.
+ */
+typedef void mcps_data_request_ext(const mac_api_t* api, const mcps_data_req_t *data, const mcps_data_req_ie_list_t *ie_ext, const struct channel_list_s *asynch_channel_list);
+
 /**
  * @brief mcps_purge_request MCPS_PURGE request call
  * @param api API to handle the request
@@ -136,6 +149,14 @@ typedef void mcps_purge_request(const mac_api_t* api, const mcps_purge_t *data);
  */
 typedef void mcps_data_confirm(const mac_api_t* api, const mcps_data_conf_t *data);
 
+/**
+ * @brief mcps_data_confirm_ext MCPS-DATA confirm with Enhanced ACK payload is called as a response to MCPS-DATA request
+ * @param api The API which handled the response
+ * @param data MCPS-DATA.confirm specific values
+ * @param conf_data Possible Confirmation Data
+ */
+typedef void mcps_data_confirm_ext(const mac_api_t* api, const mcps_data_conf_t *data, const mcps_data_conf_payload_t *conf_data);
+
 /**
  * @brief mcps_data_indication MCPS-DATA indication is called when MAC layer has received data
  * @param api The API which handled the response
@@ -143,6 +164,24 @@ typedef void mcps_data_confirm(const mac_api_t* api, const mcps_data_conf_t *dat
  */
 typedef void mcps_data_indication(const mac_api_t* api, const mcps_data_ind_t *data);
 
+/**
+ * @brief mcps_data_indication MCPS-DATA indication is called when MAC layer has received data
+ * @param api The API which handled the response
+ * @param data MCPS-DATA.indication specific values
+ * @param ie_ext Information element list
+ */
+typedef void mcps_data_indication_ext(const mac_api_t* api, const mcps_data_ind_t *data, const mcps_data_ie_list_t *ie_ext);
+
+/**
+ * @brief mcps_ack_data_req_ext Callback for request IE elements and payload to enhanced ACK
+ * @param api The API which handled the response
+ * @param data Pointer where MAC user set Payload and IE element pointers and length
+ * @param rssi Signal strength for received packet
+ * @param lqi Link quality to neighbor
+ */
+typedef void mcps_ack_data_req_ext(const mac_api_t* api, mcps_ack_data_payload_t *data, int8_t rssi, uint8_t lqi);
+
+
 /**
  * @brief mcps_purge_confirm MCPS-PURGE confirm is called as a response to MCPS-PURGE request
  * @param api The API which handled the request
@@ -201,6 +240,19 @@ typedef int8_t mac_api_initialize(mac_api_t *api, mcps_data_confirm *data_conf_c
                                   mcps_data_indication *data_ind_cb, mcps_purge_confirm *purge_conf_cb, mlme_confirm *mlme_conf_cb,
                                   mlme_indication *mlme_ind_cb, int8_t parent_id);
 
+/**
+ * @brief mac_api_enable_mcps_ext Initialises MAC 2015 extension for MCPS layer into use, callbacks must be non-NULL.
+ * @param api mac_api_t pointer, which is created by application.
+ * @param data_ind_cb Upper layer function to handle MCPS indications
+ * @param data_cnf_cb Upper layer function to handle MCPS confirmation
+ * @param ack_data_req_cb Upper layer function for set Enhanced ACK payload
+ * @return -1 if error, -2 if OOM, 0 otherwise
+ */
+typedef int8_t mac_api_enable_mcps_ext(mac_api_t *api,
+                                       mcps_data_indication_ext *data_ind_cb,
+                                       mcps_data_confirm_ext *data_cnf_cb,
+                                       mcps_ack_data_req_ext *ack_data_req_cb);
+
 /**
  * \brief Struct mac_api_s defines functions for two-way communications between external MAC and Upper layer.
  * Application creates mac_api_t object by calling external MAC's creator function.
@@ -209,14 +261,19 @@ typedef int8_t mac_api_initialize(mac_api_t *api, mcps_data_confirm *data_conf_c
  */
 struct mac_api_s {
     mac_api_initialize          *mac_initialize;                /**< MAC initialize function to use */
+    mac_api_enable_mcps_ext     *mac_mcps_extension_enable;     /**< MAC MCPS IE extension enable function, optional feature */
     //External MAC callbacks
     mlme_request                *mlme_req;                      /**< MAC MLME request function to use */
     mcps_data_request           *mcps_data_req;                 /**< MAC MCPS data request function to use */
+    mcps_data_request_ext       *mcps_data_req_ext;             /**< MAC MCPS data request with Information element extension function to use */
     mcps_purge_request          *mcps_purge_req;                /**< MAC MCPS purge request function to use */
 
     //Upper layer callbacksMLME_ASSOCIATE
     mcps_data_confirm           *data_conf_cb;                  /**< MAC MCPS data confirm callback function */
+    mcps_data_confirm_ext       *data_conf_ext_cb;              /**< MAC MCPS data confirm with payload callback function */
     mcps_data_indication        *data_ind_cb;                   /**< MAC MCPS data indication callback function */
+    mcps_data_indication_ext    *data_ind_ext_cb;               /**< MAC MCPS data indication with IE extension's callback function */
+    mcps_ack_data_req_ext       *enhanced_ack_data_req_cb;      /**< Enhanced ACK IE element and payload request from MAC user */
     mcps_purge_confirm          *purge_conf_cb;                 /**< MAC MCPS purge confirm callback function */
     mlme_confirm                *mlme_conf_cb;                  /**< MAC MLME confirm callback function */
     mlme_indication             *mlme_ind_cb;                   /**< MAC MLME indication callback function */
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/mac_common_defines.h b/features/nanostack/sal-stack-nanostack/nanostack/mac_common_defines.h
index 2cb970662d..dfa8d0f6cc 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/mac_common_defines.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/mac_common_defines.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -30,6 +30,7 @@
 
 #define MAC_FRAME_VERSION_2003         0        /**< FCF - IEEE 802.15.4-2003 compatible */
 #define MAC_FRAME_VERSION_2006         1        /**< FCF - IEEE 802.15.4-2006 (big payload or new security) */
+#define MAC_FRAME_VERSION_2015         2        /**< FCF - IEEE 802.15.4-2015 (IE element support) */
 
 //See IEEE standard 802.15.4-2006 (table 96) for more details about identifiers
 #define MAC_KEY_ID_MODE_IMPLICIT    0           /**< Key identifier mode implicit */
@@ -66,4 +67,36 @@ typedef struct mlme_security_s {
     uint8_t Keysource[8];       /**< Key source */
 } mlme_security_t;
 
+#define MAC_HEADER_VENDOR_SPESIFIC_IE_ID          0x00  /**< Vendor specific Header IE element */
+#define MAC_HEADER_ASSIGNED_EXTERNAL_ORG_IE_ID    0x2a  /**< External organisation defined Header IE element */
+#define MAC_HEADER_TERMINATION1_IE_ID             0x7e  /**< Header IE element termination when Payload element is following Header IE */
+#define MAC_HEADER_TERMINATION2_IE_ID             0x7f  /**< Header IE element termination when no Payload element is following Header IE but normal payload is */
+
+
+/**
+ * @brief struct mac_header_IE_t Mac Header information element structure for parsing or write operation
+ * This structure encapsulates security related variables,
+ */
+typedef struct mac_header_IE_s {
+    uint8_t *content_ptr;   /**< Content data */
+    unsigned length:7;      /**< Element length 0- 127 */
+    uint8_t id;             /**< Element ID */
+} mac_header_IE_t;
+
+#define MAC_PAYLOAD_IE_ESDU_GROUP_ID            0x00    /**< Encapsulated Service Data Unit (ESDU) Payload IE element's */
+#define MAC_PAYLOAD_MLME_IE_GROUP_ID            0x01    /**< MLME nested Payload IE element's */
+#define MAC_PAYLOAD_VENDOR_IE_GROUP_ID          0x02    /**< Vendor specific nested Payload IE element's */
+#define MAC_PAYLOAD_MPX_IE_GROUP_ID             0x03    /**< MPX Payload IE element, IEEE 802.15.9 defined */
+#define MAC_PAYLOAD_TERMINATION_IE_GROUP_ID     0x0f    /**< Payload IE element terminator. Mandatory when normal payload is coming after IE element part */
+
+/**
+ * @brief struct mac_header_IE_t Mac Payload information element structure for parsing or write operation
+ * This structure encapsulates security related variables,
+ */
+typedef struct mac_payload_IE_s {
+    uint8_t *content_ptr;   /**< Content data */
+    unsigned length:11;     /**< Element length 0- 2047 */
+    unsigned id:4;          /**< Group ID */
+} mac_payload_IE_t;
+
 #endif /* MAC_COMMON_DEFINES_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/mac_mcps.h b/features/nanostack/sal-stack-nanostack/nanostack/mac_mcps.h
index 629f8a1451..fab3f2abdf 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/mac_mcps.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/mac_mcps.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -25,6 +25,7 @@
 #include <inttypes.h>
 #include "mac_common_defines.h"
 
+
 /**
  * @brief struct mcps_data_req_t Data request structure
  *
@@ -41,6 +42,8 @@ typedef struct mcps_data_req_s {
     bool TxAckReq: 1;               /**< Specifies whether ACK is needed or not */
     bool InDirectTx:1;              /**< Specifies whether indirect or direct transmission is used */
     bool PendingBit: 1;             /**< Specifies whether more fragments are to be sent or not */
+    bool SeqNumSuppressed:1;        /**< True suppress sequence number from frame. This will be only checked when 2015 extension is enabled */
+    bool PanIdSuppressed:1;         /**< True suppress PAN-id is done when possible from frame. This will be only checked when 2015 extension is enabled */
     mlme_security_t Key;            /**< Security key */
 } mcps_data_req_t;
 
@@ -58,6 +61,21 @@ typedef struct mcps_data_conf_s {
     uint8_t tx_retries;     /**< Number of retries done during sending, 0 means no retries */
 } mcps_data_conf_t;
 
+
+/**
+ * @brief struct mcps_data_conf_payload_t Data confirmatio IE extension list and payload from enhanced ACK
+ *
+ */
+typedef struct mcps_data_conf_payload_s {
+    uint8_t *headerIeList;              /**< Header information IE's list without terminator*/
+    uint8_t *payloadIeList;             /**< Payload information IE's list without terminator*/
+    uint8_t *payloadPtr;                /**< Ack payload pointer */
+    uint16_t headerIeListLength;        /**< Header information IE's list length in bytes */
+    uint16_t payloadIeListLength;       /**< Payload information IE's list length in bytes */
+    uint16_t payloadLength;             /**< Payload length in bytes */
+} mcps_data_conf_payload_t;
+
+
 /**
  * @brief struct mcps_data_ind_t Data indication structure
  *
@@ -79,6 +97,54 @@ typedef struct mcps_data_ind_s {
     uint8_t *msdu_ptr;          /**< Data unit */
 } mcps_data_ind_t;
 
+/**
+ * @brief struct mcps_data_ie_list_t MCPS data Information element list stucture
+ *
+ * Structure for IEEE 802.15.4-2015 MCPS data extension to Indication
+ */
+typedef struct mcps_data_ie_list {
+    uint8_t *headerIeList;              /**< Header information IE's list without terminator*/
+    uint8_t *payloadIeList;             /**< Payload information IE's list without terminator*/
+    uint16_t headerIeListLength;        /**< Header information IE's list length in bytes */
+    uint16_t payloadIeListLength;       /**< Payload information IE's list length in bytes */
+} mcps_data_ie_list_t;
+
+/** \brief Scatter-gather descriptor for MCPS request IE Element list
+ *
+ * Slightly optimised for small platforms - we assume we won't need any
+ * element bigger than 64K.
+ */
+typedef struct ns_ie_iovec {
+    void *ieBase;              /**< IE element pointer */
+    uint_fast16_t iovLen;      /**< IE element length */
+} ns_ie_iovec_t;
+
+
+/**
+ * @brief struct mcps_data_req_ie_list MCPS data Information element list stuctrure
+ *
+ * Structure for IEEE 802.15.4-2015 MCPS data extension to Request
+ *
+ * IE element could be divided to multiple vector which MAC just write to message direct.
+ */
+typedef struct mcps_data_req_ie_list {
+    ns_ie_iovec_t *headerIeVectorList;   /**< Header IE element list */
+    ns_ie_iovec_t *payloadIeVectorList;  /**< Payload IE element list */
+    uint16_t headerIovLength;            /**< Header IE element list size, set 0 when no elements */
+    uint16_t payloadIovLength;           /**< Payload IE element list size, set 0 when no elements */
+} mcps_data_req_ie_list_t;
+
+
+/**
+ * @brief struct mcps_ack_data_payload_t IE extension list and payload for enhanced ACK
+ *
+ */
+typedef struct mcps_ack_data_payload {
+    struct mcps_data_req_ie_list ie_elements;   /**< IE hader and payload's elements */
+    uint8_t *payloadPtr;                        /**< Ack payload pointer */
+    uint16_t payloadLength;                     /**< Payload length in bytes */
+} mcps_ack_data_payload_t;
+
 /**
  * @brief struct mcps_purge_t Purge request structure
  *
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/mlme.h b/features/nanostack/sal-stack-nanostack/nanostack/mlme.h
index 04fac15764..1119630192 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/mlme.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/mlme.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2017, Arm Limited and affiliates.
+ * Copyright (c) 2013-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -264,6 +264,7 @@ typedef enum {
     macAutoRequestKeyIndex = 0x7b,  /*<The index of the key used for automatic data*/
     macDefaultKeySource = 0x7c,      /*<Default key source*/
     //NON standard extension
+    macAcceptByPassUnknowDevice = 0xfc,  /*< Accept data trough MAC if packet is data can be authenticated by group key nad MIC. Security enforsment point must be handled carefully these packets */
     macLoadBalancingBeaconTx = 0xfd,  /*< Trig Beacon from load balance module periodic */
     macLoadBalancingAcceptAnyBeacon = 0xfe, /*<Beacon accept state control from other network. Value size bool, data true=Enable, false=disable .*/
     macThreadForceLongAddressForBeacon = 0xff /*<Thread standard force beacon source address for extended 64-bit*/
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/net_fhss.h b/features/nanostack/sal-stack-nanostack/nanostack/net_fhss.h
index e240f70124..2223a5af3b 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/net_fhss.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/net_fhss.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -38,6 +38,15 @@ extern "C" {
  */
 extern fhss_api_t *ns_fhss_create(const fhss_configuration_t *fhss_configuration, const fhss_timer_t *fhss_timer, fhss_statistics_t *fhss_statistics);
 
+/**
+ * @brief TODO: description.
+ * @param fhss_configuration Basic FHSS configuration.
+ * @param fhss_timer FHSS platform timer interface and configuration.
+ * @param fhss_statistics FHSS statistics storage.
+ * @return New FHSS instance if successful, NULL otherwise.
+ */
+extern fhss_api_t *ns_fhss_ws_create(const fhss_ws_configuration_t *fhss_configuration, const fhss_timer_t *fhss_timer);
+
 /**
  * @brief Set synchronization time configuration for FHSS network. Should be only called from Border router.
  * @param fhss_api FHSS instance.
@@ -46,6 +55,21 @@ extern fhss_api_t *ns_fhss_create(const fhss_configuration_t *fhss_configuration
  */
 extern int ns_fhss_configuration_set(fhss_api_t *fhss_api, const fhss_synch_configuration_t *fhss_synch_configuration);
 
+/**
+ * @brief Get WS configuration.
+ * @param fhss_api FHSS instance.
+ * @return WS configuration.
+ */
+extern const fhss_ws_configuration_t *ns_fhss_ws_configuration_get(const fhss_api_t *fhss_api);
+
+/**
+ * @brief Set WS configuration.
+ * @param fhss_api FHSS instance.
+ * @param fhss_configuration Basic FHSS configuration.
+ * @return 0 on success, -1 on fail.
+ */
+extern int ns_fhss_ws_configuration_set(const fhss_api_t *fhss_api, const fhss_ws_configuration_t *fhss_configuration);
+
 /**
  * @brief Deletes a FHSS API instance and removes it from software MAC.
  * @param fhss_api FHSS instance.
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/net_interface.h b/features/nanostack/sal-stack-nanostack/nanostack/net_interface.h
index ac3a48c4c6..01676781f7 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/net_interface.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/net_interface.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -166,6 +166,7 @@ typedef enum {
     NET_6LOWPAN_ND_WITHOUT_MLE,         /**< **UNSUPPORTED** */
     NET_6LOWPAN_ND_WITH_MLE,            /**< 6LoWPAN ND with MLE. */
     NET_6LOWPAN_THREAD,                 /**< 6LoWPAN Thread with MLE attached. */
+    NET_6LOWPAN_WS    ,                 /**< WS. */
     NET_6LOWPAN_ZIGBEE_IP               /**< **UNSUPPORTED** */
 } net_6lowpan_mode_extension_e;
 
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/net_test_api.h b/features/nanostack/sal-stack-nanostack/nanostack/net_test_api.h
index 78b2898da2..35322069b5 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/net_test_api.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/net_test_api.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/net_thread_test.h b/features/nanostack/sal-stack-nanostack/nanostack/net_thread_test.h
index 648fb79a3d..fe299a3770 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/net_thread_test.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/net_thread_test.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/platform/arm_hal_aes.h b/features/nanostack/sal-stack-nanostack/nanostack/platform/arm_hal_aes.h
index eeb1c6d871..891e03931b 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/platform/arm_hal_aes.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/platform/arm_hal_aes.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -36,19 +36,33 @@
 
 #include "ns_types.h"
 
+
 #ifdef __cplusplus
 extern "C" {
 #endif
 
+/* This API must be able to provide this many simultaneous contexts */
+#if 1 /* config option for 802.15.4-2015? */
+#define ARM_AES_MBEDTLS_CONTEXT_MIN 2 /**</ event loop and driver rx callback context */
+#else
+#define ARM_AES_MBEDTLS_CONTEXT_MIN 1 /**</ event loop use only */
+#endif
+
+typedef struct arm_aes_context arm_aes_context_t;
+
 /**
  * \brief Set the AES key
  *
  * This function sets the 128-bit AES key that will be used for future
  * calls to arm_aes_encrypt(). The key must be copied by the function.
+ * Function must return unique pointer for every started AES context.
  *
  * \param key pointer to 128-bit AES key
+ *
+ * \return Pointer to aes context
+ * \return NULL if aes context allocation fail
  */
-void arm_aes_start(const uint8_t key[__static 16]);
+arm_aes_context_t *arm_aes_start(const uint8_t key[__static 16]);
 
 /**
  * \brief This function performs dst=E[preset key,src] (Simple ECB block).
@@ -57,10 +71,12 @@ void arm_aes_start(const uint8_t key[__static 16]);
  * It is called between arm_aes_start() and arm_aes_finish().
  * Note that src and dst pointers may be equal.
  *
+ * \param aes_context Pointer for allocated
  * \param src pointer to 128-bit plaintext in
  * \param dst pointer for 128-bit ciphertext out
  */
 extern void arm_aes_encrypt(
+    arm_aes_context_t *aes_context,
     const uint8_t src[__static 16],
     uint8_t dst[__static 16]);
 
@@ -69,9 +85,9 @@ extern void arm_aes_encrypt(
  *
  * This function is called to terminate a series of AES operations.
  * It may be a no-op, or it may disable AES hardware. Use of the preset key is
- * no longer valid after this call.
+ * no longer valid after this call and aes context will be freed.
  */
-void arm_aes_finish(void);
+void arm_aes_finish(arm_aes_context_t *aes_context);
 
 #ifdef __cplusplus
 }
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/platform/arm_hal_phy.h b/features/nanostack/sal-stack-nanostack/nanostack/platform/arm_hal_phy.h
index 879003bc61..75c72ed3c4 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/platform/arm_hal_phy.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/platform/arm_hal_phy.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -45,6 +45,7 @@ typedef enum {
     PHY_LINK_TX_SUCCESS,        /**< MAC TX complete. MAC will a make decision to enter wait ACK or TX done state. */
     PHY_LINK_TX_FAIL,           /**< Link TX process fail. */
     PHY_LINK_CCA_FAIL,          /**< RF link CCA process fail. */
+    PHY_LINK_CCA_PREPARE,       /**< RX Tx timeout prepare operation like channel switch to Tx channel from Receive one If operation fail must return not zero*/
 } phy_link_tx_status_e;
 
 /** Extension types */
@@ -56,6 +57,13 @@ typedef enum {
     PHY_EXTENSION_READ_LINK_STATUS, /**< Net library could read link status. */
     PHY_EXTENSION_CONVERT_SIGNAL_INFO, /**< Convert signal info. */
     PHY_EXTENSION_ACCEPT_ANY_BEACON, /**< Set boolean true or false for accept beacon from other Pan-ID than configured. Default value should be false */
+    PHY_EXTENSION_SET_TX_TIME,  /**< Net library sets transmission time based on global time stamp. Max. 65ms from setting to TX. If TX time is set to zero, it should be ignored.*/
+    PHY_EXTENSION_READ_RX_TIME, /**< Read the time of last reception based on global micro seconds time stamp. */
+    PHY_EXTENSION_READ_TX_FINNISH_TIME, /**< Read the time of last finished TX micro seconds based on global time stamp. */
+    PHY_EXTENSION_DYNAMIC_RF_SUPPORTED, /**< Read status for support Radio driver support for set TX time, CCA and Timestamp read. Also PHY_LINK_CCA_PREPARE tx status must be supported also*/
+    PHY_EXTENSION_GET_TIMESTAMP, /**<  Read 32-bit constant monotonic time stamp in us */
+    PHY_EXTENSION_SET_CSMA_PARAMETERS, /**< CSMA parameter's are given by phy_csma_params_t structure remember type cast uint8_t pointer to structure type*/
+    PHY_EXTENSION_GET_SYMBOLS_PER_SECOND, /**<  Read Symbols per seconds which will help to convert symbol time to real time  */
 } phy_extension_type_e;
 
 /** Address types */
@@ -110,6 +118,12 @@ typedef struct phy_signal_info_s {
     uint16_t result;                /**< Resulting signal information. */
 } phy_signal_info_s;
 
+/** CSMA-CA parameters */
+typedef struct phy_csma_params {
+    uint32_t backoff_time;           /**< CSMA Backoff us time before start CCA & TX. 0 should disable current backoff*/
+    bool cca_enabled;                /**< True will affect CCA check false start TX direct after backoff */
+} phy_csma_params_t;
+
 /** PHY modulation scheme */
 typedef enum phy_modulation_e
 {
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/sw_mac.h b/features/nanostack/sal-stack-nanostack/nanostack/sw_mac.h
index bda9201839..3ebbd15277 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/sw_mac.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/sw_mac.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -66,6 +66,13 @@ extern int8_t ns_sw_mac_virtual_client_unregister(struct mac_api_s *api);
  */
 extern int ns_sw_mac_fhss_register(struct mac_api_s *mac_api, struct fhss_api *fhss_api);
 
+/**
+ * @brief Request registered FHSS API instance from software MAC instance.
+ * @param mac_api MAC instance.
+ * @return FHSS api.
+ */
+extern struct fhss_api *ns_sw_mac_get_fhss_api(struct mac_api_s *mac_api);
+
 /**
  * @brief Start collecting statistics from software MAC.
  * @param mac_api MAC instance.
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/thread_bbr_api.h b/features/nanostack/sal-stack-nanostack/nanostack/thread_bbr_api.h
index 66018bbfe4..91e697baad 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/thread_bbr_api.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/thread_bbr_api.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017, Arm Limited and affiliates.
+ * Copyright (c) 2017-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -75,6 +75,19 @@ int thread_bbr_start(int8_t interface_id, int8_t backbone_interface_id);
  *
  */
 int thread_bbr_timeout_set(int8_t interface_id, uint32_t timeout_a, uint32_t timeout_b, uint32_t delay);
+/**
+ * Set prefix to be used as combining multiple thread networks on backbone.
+ *
+ * update prefix value
+ *
+ * \param interface_id interface ID of the Thread network.
+ * \param prefix value must be buffer at the size of 8 bytes
+ *
+ * \return 0 on success
+ * \return <0 in case of errors
+ *
+ */
+int thread_bbr_prefix_set(int8_t interface_id, uint8_t *prefix);
 
 /**
  * Set the Thread validation interface destination address.
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/thread_border_router_api.h b/features/nanostack/sal-stack-nanostack/nanostack/thread_border_router_api.h
index b43ea9c456..fb539b69c1 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/thread_border_router_api.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/thread_border_router_api.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -54,6 +54,7 @@ typedef struct thread_border_router_info_t {
     bool                P_default_route: 1;   /**< This device provides the default route. */
     bool                P_on_mesh: 1;         /**< This prefix is considered to be on-mesh */
     bool                P_nd_dns: 1;          /**< this border router is able to provide DNS information */
+    bool                P_res1: 1;            /**< First reserved bit */
     bool                stableData: 1;        /**< This data is stable and expected to be available at least 48h. */
 } thread_border_router_info_t;
 
diff --git a/features/nanostack/sal-stack-nanostack/nanostack/thread_management_if.h b/features/nanostack/sal-stack-nanostack/nanostack/thread_management_if.h
index c193385881..c6d6c3324e 100644
--- a/features/nanostack/sal-stack-nanostack/nanostack/thread_management_if.h
+++ b/features/nanostack/sal-stack-nanostack/nanostack/thread_management_if.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -96,7 +96,7 @@ typedef struct link_configuration {
 #define SECURITY_POLICY_EXTERNAL_COMMISSIONER_ALLOWED           0x10       /**< This indicates that external Commissioner authentication is allowed using PSKc. */
 #define SECURITY_POLICY_BEACON_PAYLOAD_ENABLED                  0x08       /**< Thread 1.x Beacons are enabled when this is set. */
 
-/*
+/**
  * Mandatory device information
  *
  * This information is required if commissioning is enabled for this device.
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan.c
index 56ef416fa6..c14b97c7ab 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2017, Arm Limited and affiliates.
+ * Copyright (c) 2013-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -69,8 +69,11 @@
 #endif
 #include "sw_mac.h"
 #include "6LoWPAN/MAC/mac_data_poll.h"
+#include "6LoWPAN/MAC/mpx_api.h"
 #include "6LoWPAN/lowpan_adaptation_interface.h"
 #include "6LoWPAN/Fragmentation/cipv6_fragmenter.h"
+#include "Service_Libs/etx/etx.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 
 
 #define TRACE_GROUP_LOWPAN "6lo"
@@ -164,7 +167,7 @@ void protocol_6lowpan_stack(buffer_t *b)
                 case B_TO_PHY:
                     b = lowpan_adaptation_data_process_tx_preprocess(cur, b);
                     if (lowpan_adaptation_interface_tx(cur, b) != 0) {
-                        tr_error("Adaptaion Layer Data req fail");
+                        tr_error("Adaptation Layer Data req fail");
                     }
                     b = NULL;
                     break;
@@ -347,7 +350,6 @@ void protocol_6lowpan_host_init(protocol_interface_info_entry_t *cur, bool sleep
     }
     //Clear always INTERFACE_NWK_ROUTER_DEVICE, INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE
     cur->lowpan_info &= ~(INTERFACE_NWK_ROUTER_DEVICE | INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE);
-    mle_class_mode_set(cur->id, MLE_CLASS_END_DEVICE);
     mac_helper_pib_boolean_set(cur, macRxOnWhenIdle, true);
     mac_data_poll_init(cur);
     arm_nwk_6lowpan_borderrouter_data_free(cur);
@@ -358,7 +360,6 @@ void protocol_6lowpan_router_init(protocol_interface_info_entry_t *cur)
     cur->bootsrap_mode = ARM_NWK_BOOTSRAP_MODE_6LoWPAN_ROUTER;
     cur->lowpan_info |= INTERFACE_NWK_ROUTER_DEVICE;
     cur->lowpan_info &= ~INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE;
-    mle_class_mode_set(cur->id, MLE_CLASS_ROUTER);
     mac_data_poll_init(cur);
     arm_nwk_6lowpan_borderrouter_data_free(cur);
 }
@@ -398,6 +399,7 @@ void protocol_6lowpan_register_handlers(protocol_interface_info_entry_t *cur)
      * for routers, as RPL doesn't deal with it) */
     cur->ipv6_neighbour_cache.send_addr_reg = true;
     cur->ipv6_neighbour_cache.recv_na_aro = true;
+    cur->ipv6_neighbour_cache.use_eui64_as_slla_in_aro = false;
 }
 void protocol_6lowpan_release_short_link_address_from_neighcache(protocol_interface_info_entry_t *cur, uint16_t shortAddress)
 {
@@ -430,28 +432,27 @@ void protocol_6lowpan_release_long_link_address_from_neighcache(protocol_interfa
 }
 #ifdef HAVE_6LOWPAN_ND
 
-static int8_t  mle_set_link_priority(int8_t interface_id, const uint8_t *address, bool priority)
+static int8_t  mle_set_link_priority(protocol_interface_info_entry_t *cur, const uint8_t *address, bool priority)
 {
     uint8_t mac64[8];
-    mle_neigh_table_entry_t *mle_entry;
-
+    mac_neighbor_table_entry_t *entry;
     if (!memcmp(address, ADDR_SHORT_ADR_SUFFIC, 6)) {
-        mle_entry = mle_class_get_by_link_address(interface_id, address + 6, ADDR_802_15_4_SHORT);
+        entry = mac_neighbor_table_address_discover(mac_neighbor_info(cur), address + 6, ADDR_802_15_4_SHORT);
     } else {
 
         memcpy(mac64, address, 8);
         mac64[0] ^= 2;
-        mle_entry = mle_class_get_by_link_address(interface_id, mac64, ADDR_802_15_4_LONG);
+        entry = mac_neighbor_table_address_discover(mac_neighbor_info(cur), mac64, ADDR_802_15_4_LONG);
     }
 
-    if (!mle_entry) {
+    if (!entry) {
         return -1;
     }
 
     if (priority) {
-        mle_entry->priorityFlag = 1;
+        entry->link_role = PRIORITY_PARENT_NEIGHBOUR;
     } else {
-        mle_entry->priorityFlag = 0;
+        entry->link_role = NORMAL_NEIGHBOUR;
     }
     return 0;
 }
@@ -461,11 +462,11 @@ void protocol_6lowpan_neighbor_priority_update(protocol_interface_info_entry_t *
     if (cur->lowpan_info & INTERFACE_NWK_BOOTSRAP_MLE) {
 #ifndef NO_MLE
         if (removed_priority) {
-            mle_set_link_priority(cur->id,removed_priority, false);
+            mle_set_link_priority(cur,removed_priority, false);
         }
 
         if (updated_priority) {
-            mle_set_link_priority(cur->id, updated_priority, true);
+            mle_set_link_priority(cur, updated_priority, true);
         }
 #endif
     }
@@ -476,30 +477,30 @@ void protocol_6lowpan_neighbor_priority_update(protocol_interface_info_entry_t *
 
 uint16_t protocol_6lowpan_neighbor_priority_set(int8_t interface_id, addrtype_t addr_type, const uint8_t *addr_ptr)
 {
-    mle_neigh_table_entry_t *neigh_table_ptr;
+    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
 
-    if (!addr_ptr) {
+    if (!cur || !addr_ptr) {
         return 0;
     }
 
-    neigh_table_ptr = mle_class_get_by_link_address(interface_id, addr_ptr + PAN_ID_LEN, addr_type);
+    mac_neighbor_table_entry_t * entry = mac_neighbor_table_address_discover(mac_neighbor_info(cur), addr_ptr + PAN_ID_LEN, addr_type);
 
-    if (neigh_table_ptr) {
+    if (entry) {
+        etx_storage_t *etx_entry = etx_storage_entry_get(interface_id, entry->index);
         // If primary parent has changed clears priority from previous parent
-        if (!neigh_table_ptr->priorityFlag) {
+        if (entry->link_role != PRIORITY_PARENT_NEIGHBOUR) {
             protocol_6lowpan_neighbor_priority_clear_all(interface_id, PRIORITY_1ST);
         }
-        neigh_table_ptr->priorityFlag = 1;
+        entry->link_role = PRIORITY_PARENT_NEIGHBOUR;
 
-        protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
-        if (cur) {
-            uint8_t temp[2];
-            common_write_16_bit(neigh_table_ptr->short_adr, temp);
-            mac_helper_coordinator_address_set(cur, ADDR_802_15_4_SHORT, temp);
-            mac_helper_coordinator_address_set(cur, ADDR_802_15_4_LONG, neigh_table_ptr->mac64);
+
+        uint8_t temp[2];
+        common_write_16_bit(entry->mac16, temp);
+        mac_helper_coordinator_address_set(cur, ADDR_802_15_4_SHORT, temp);
+        mac_helper_coordinator_address_set(cur, ADDR_802_15_4_LONG, entry->mac64);
+        if (etx_entry) {
+            protocol_stats_update(STATS_ETX_1ST_PARENT, etx_entry->etx >> 4);
         }
-
-        protocol_stats_update(STATS_ETX_1ST_PARENT, neigh_table_ptr->etx >> 4);
         return 1;
     } else {
         return 0;
@@ -508,21 +509,26 @@ uint16_t protocol_6lowpan_neighbor_priority_set(int8_t interface_id, addrtype_t
 
 uint16_t protocol_6lowpan_neighbor_second_priority_set(int8_t interface_id, addrtype_t addr_type, const uint8_t *addr_ptr)
 {
-    mle_neigh_table_entry_t *neigh_table_ptr;
 
-    if (!addr_ptr) {
+    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
+
+    if (!cur || !addr_ptr) {
         return 0;
     }
 
-    neigh_table_ptr = mle_class_get_by_link_address(interface_id, addr_ptr + PAN_ID_LEN, addr_type);
+    mac_neighbor_table_entry_t * entry = mac_neighbor_table_address_discover(mac_neighbor_info(cur), addr_ptr + PAN_ID_LEN, addr_type);
 
-    if (neigh_table_ptr) {
+    if (entry) {
+        etx_storage_t *etx_entry = etx_storage_entry_get(interface_id, entry->index);
         // If secondary parent has changed clears priority from previous parent
-        if (neigh_table_ptr->second_priority_flag == 0) {
+        if (entry->link_role != SECONDARY_PARENT_NEIGHBOUR) {
             protocol_6lowpan_neighbor_priority_clear_all(interface_id, PRIORITY_2ND);
         }
-        neigh_table_ptr->second_priority_flag = 1;
-        protocol_stats_update(STATS_ETX_2ND_PARENT, neigh_table_ptr->etx >> 4);
+        entry->link_role = SECONDARY_PARENT_NEIGHBOUR;
+
+        if (etx_entry) {
+            protocol_stats_update(STATS_ETX_2ND_PARENT, etx_entry->etx >> 4);
+        }
         return 1;
     } else {
         return 0;
@@ -531,21 +537,22 @@ uint16_t protocol_6lowpan_neighbor_second_priority_set(int8_t interface_id, addr
 
 void protocol_6lowpan_neighbor_priority_clear_all(int8_t interface_id, neighbor_priority priority)
 {
-    mle_neigh_table_list_t *mle_neigh_table;
+    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
 
-    mle_neigh_table = mle_class_active_list_get(interface_id);
-    if (!mle_neigh_table) {
+    if (!cur) {
         return;
     }
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
 
-    ns_list_foreach(mle_neigh_table_entry_t, entry, mle_neigh_table) {
-        if (priority == PRIORITY_1ST) {
-            entry->priorityFlag = 0;
+    ns_list_foreach(mac_neighbor_table_entry_t, entry, mac_table_list) {
+        if (priority == PRIORITY_1ST && entry->link_role == PRIORITY_PARENT_NEIGHBOUR) {
+            entry->link_role = NORMAL_NEIGHBOUR;
         } else {
-            if (entry->second_priority_flag) {
+            if (entry->link_role == SECONDARY_PARENT_NEIGHBOUR) {
                 protocol_stats_update(STATS_ETX_2ND_PARENT, 0);
+                entry->link_role = NORMAL_NEIGHBOUR;
             }
-            entry->second_priority_flag = 0;
+
         }
     }
 }
@@ -558,43 +565,33 @@ void protocol_6lowpan_neighbor_priority_clear_all(int8_t interface_id, neighbor_
 int8_t protocol_6lowpan_neighbor_address_state_synch(protocol_interface_info_entry_t *cur, const uint8_t eui64[8], const uint8_t iid[8])
 {
     int8_t ret_val = -1;
-    if (cur->lowpan_info & INTERFACE_NWK_BOOTSRAP_MLE) {
-#ifndef NO_MLE
-        mle_neigh_table_entry_t *mle_entry = 0;
-        mle_entry =  mle_class_get_by_link_address(cur->id, eui64, ADDR_802_15_4_LONG);
-        if (mle_entry && !mle_entry->threadNeighbor) {
-            if (memcmp(iid, ADDR_SHORT_ADR_SUFFIC, 6) == 0) {
-                iid += 6;
-                //Set Short Address to MLE
-                mle_entry->short_adr = common_read_16_bit(iid);
-            }
-            if ((mle_entry->mode & MLE_DEV_MASK) == MLE_RFD_DEV) {
-                if (mle_entry->handshakeReady) {
-                    mle_entry_timeout_refresh(mle_entry);
-                }
-                ret_val = 1;
-            } else {
-                ret_val = 0;
-            }
+
+    mac_neighbor_table_entry_t * entry = mac_neighbor_table_address_discover(mac_neighbor_info(cur), eui64, ADDR_802_15_4_LONG);
+    if (entry) {
+        if (memcmp(iid, ADDR_SHORT_ADR_SUFFIC, 6) == 0) {
+            iid += 6;
+            //Set Short Address to MLE
+            entry->mac16 = common_read_16_bit(iid);
+        }
+        if (!entry->ffd_device) {
+            if (entry->connected_device) {
+                mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry, entry->link_lifetime);
+            }
+            ret_val = 1;
+        } else {
+            ret_val = 0;
         }
-#endif
-    } else {
-        ret_val = 0;
     }
     return ret_val;
 }
 
 int8_t protocol_6lowpan_neighbor_remove(protocol_interface_info_entry_t *cur, uint8_t *address_ptr, addrtype_t type)
 {
-    int8_t ret_val = 0;
-    if (cur->lowpan_info & INTERFACE_NWK_BOOTSRAP_MLE) {
-#ifndef NO_MLE
-        mle_class_remove_neighbour(cur->id, address_ptr, type);
-#endif
-    } else {
-        //REMOVE Something else
+    mac_neighbor_table_entry_t * entry = mac_neighbor_table_address_discover(mac_neighbor_info(cur), address_ptr, type);
+    if (entry) {
+        mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), entry);
     }
-    return ret_val;
+    return 0;
 }
 
 void protocol_6lowpan_allocate_mac16(protocol_interface_info_entry_t *cur)
@@ -739,7 +736,7 @@ uint8_t protocol_6lowpan_beacon_join_priority_tx(int8_t interface_id)
     mle_6lowpan_data_t *mle_6lowpan_data = protocol_6lowpan_mle_data_get();
 
     if (mle_6lowpan_data && mle_6lowpan_data->nbr_of_neigh_max != 0) {
-        uint16_t mle_neigh_cnt = mle_class_active_neigh_counter(interface_id);
+        uint16_t mle_neigh_cnt = mle_class_active_neigh_counter(cur);
 
         if (mle_neigh_cnt > mle_6lowpan_data->nbr_of_neigh_lower_threshold) {
             uint16_t mle_neigh_limit;
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan_bootstrap.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan_bootstrap.c
index 4be6abcc28..e8f4fe9577 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan_bootstrap.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan_bootstrap.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -69,7 +69,9 @@
 #include "mac_api.h"
 #include "6LoWPAN/MAC/mac_data_poll.h"
 #include "libNET/src/net_load_balance_internal.h"
+#include "6LoWPAN/lowpan_adaptation_interface.h"
 #include "6LoWPAN/NVM/nwk_nvm.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 
 
 /* Fixed-point randomisation limits for randlib_randomise_base() - RFC 3315
@@ -90,15 +92,16 @@ static void protocol_6lowpan_bootstrap_rpl_callback(rpl_event_t event, void *han
 #endif
 
 static void protocol_6lowpan_mle_purge_neighbors(struct protocol_interface_info_entry *cur_interface, uint8_t entry_count, uint8_t force_priority);
-static uint8_t protocol_6lowpan_mle_order_last_entries(mle_neigh_table_list_t *mle_neigh_table, uint8_t entry_count);
+static uint8_t protocol_6lowpan_mle_order_last_entries(int8_t interface_id,mac_neighbor_table_list_t *mac_neigh_table, uint8_t entry_count);
 static uint8_t protocol_6lowpan_mle_data_allocate(void);
 static bool mle_accept_request_cb(int8_t interface_id, uint16_t msgId, bool usedAllRetries);
 static void lowpan_comm_status_indication_cb(int8_t if_id, const mlme_comm_status_t* status);
 
-static void protocol_6lowpan_priority_neighbor_remove(protocol_interface_info_entry_t *cur_interface, mle_neigh_table_entry_t *cur);
-static void protocol_6lowpan_neighbor_information_remove(int8_t interface_id, mle_neigh_table_entry_t *cur);
-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_priority_neighbor_remove(protocol_interface_info_entry_t *cur_interface, mac_neighbor_table_entry_t *cur);
+static void lowpan_neighbor_entry_remove_notify(mac_neighbor_table_entry_t *entry_ptr, void *user_data);
+static bool lowpan_neighbor_entry_nud_notify(mac_neighbor_table_entry_t *entry_ptr, void *user_data);
+static bool protocol_6lowpan_router_challenge(protocol_interface_info_entry_t *cur_interface, const uint8_t *mac64);
+static bool protocol_6lowpan_host_challenge(protocol_interface_info_entry_t *cur, 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);
 
@@ -158,9 +161,9 @@ uint8_t *mle_general_write_timeout(uint8_t *ptr, protocol_interface_info_entry_t
 
 }
 
-static void protocol_6lowpan_priority_neighbor_remove(protocol_interface_info_entry_t *cur_interface, mle_neigh_table_entry_t *cur)
+static void protocol_6lowpan_priority_neighbor_remove(protocol_interface_info_entry_t *cur_interface, mac_neighbor_table_entry_t *cur)
 {
-    if (!cur->priorityFlag ||
+    if (cur->link_role != PRIORITY_PARENT_NEIGHBOUR ||
         !(cur_interface->lowpan_info & INTERFACE_NWK_ACTIVE) ||
         cur_interface->bootsrap_mode == ARM_NWK_BOOTSRAP_MODE_6LoWPAN_BORDER_ROUTER) {
         return;
@@ -176,9 +179,9 @@ static void protocol_6lowpan_priority_neighbor_remove(protocol_interface_info_en
         }
     } else {
         //Call Priority parent loose
-        if (cur->short_adr != 0xffff) {
+        if (cur->mac16 != 0xffff) {
             memcpy(mac64, ADDR_SHORT_ADR_SUFFIC, 6);
-            common_write_16_bit(cur->short_adr, &mac64[6]);
+            common_write_16_bit(cur->mac16, &mac64[6]);
         } else {
             memcpy(mac64,cur->mac64 , 8);
             mac64[0] ^= 2;
@@ -192,64 +195,42 @@ static void protocol_6lowpan_priority_neighbor_remove(protocol_interface_info_en
     }
 }
 
-static void protocol_6lowpan_neighbor_information_remove(int8_t interface_id, mle_neigh_table_entry_t *cur)
+static bool protocol_6lowpan_challenge_callback(int8_t interface_id, uint16_t msgId, bool usedAllRetries)
 {
     protocol_interface_info_entry_t *cur_interface = protocol_stack_interface_info_get_by_id(interface_id);
     if (!cur_interface) {
-        return;
+        return false;
     }
 
-    // Sleepy host
-    if (cur_interface->lowpan_info & INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE) {
-        mac_data_poll_protocol_poll_mode_decrement(cur_interface);
-    }
-
-    protocol_6lowpan_priority_neighbor_remove(cur_interface, cur);
-
-    if (cur->mode & MLE_FFD_DEV) {
-        protocol_6lowpan_release_short_link_address_from_neighcache(cur_interface, cur->short_adr);
-        protocol_6lowpan_release_long_link_address_from_neighcache(cur_interface, cur->mac64);
-    }
-    mac_helper_devicetable_remove(cur_interface->mac_api, cur->attribute_index);
-}
-
-static bool protocol_6lowpan_challenge_callback(int8_t interface_id, uint16_t msgId, bool usedAllRetries)
-{
     uint8_t mac64[8];
     uint8_t *ll64_ptr = mle_service_get_msg_destination_address_pointer(msgId);
 
     memcpy(mac64, ll64_ptr + 8, 8);
     mac64[0] ^= 2;
 
-    mle_neigh_table_entry_t *neig_info = mle_class_get_by_link_address(interface_id, mac64, ADDR_802_15_4_LONG);
+    mac_neighbor_table_entry_t * neig_info = mac_neighbor_table_address_discover(mac_neighbor_info(cur_interface), mac64, ADDR_802_15_4_LONG);
 
     if (!neig_info) {
         return false;//Why entry is removed before timeout??
     }
 
 
-    if (neig_info->ttl > MLE_TABLE_CHALLENGE_TIMER) {
+    if (!neig_info->nud_active) {
         return false;
     }
 
 
     if (usedAllRetries) {
         //GET entry
-        mle_class_remove_entry(interface_id, neig_info);
+        mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur_interface), neig_info);
         return false;
     }
 
     return true;
 }
 
-static int8_t protocol_6lowpan_host_challenge(int8_t interface_id, const uint8_t *mac64)
+static bool protocol_6lowpan_host_challenge(protocol_interface_info_entry_t *cur, const uint8_t *mac64)
 {
-    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
-    if (!cur) {
-        return false;
-    }
-
-
     uint16_t bufId;
     mle_message_timeout_params_t timeout;
     uint8_t ll64[16];
@@ -261,7 +242,7 @@ static int8_t protocol_6lowpan_host_challenge(int8_t interface_id, const uint8_t
     tr_debug("Link REQUEST");
     bufId = mle_service_msg_allocate(cur->id, 32, true,MLE_COMMAND_REQUEST);
     if (bufId == 0) {
-        return -1;
+        return false;
     }
 
     uint8_t *ptr = mle_service_get_data_pointer(bufId);
@@ -293,16 +274,11 @@ static int8_t protocol_6lowpan_host_challenge(int8_t interface_id, const uint8_t
     mle_service_set_msg_timeout_parameters(bufId, &timeout);
 
     mle_service_send_message(bufId);
-    return 0;
+    return true;
 }
 
-static int8_t protocol_6lowpan_router_challenge(int8_t interface_id, const uint8_t *mac64)
+static bool protocol_6lowpan_router_challenge(protocol_interface_info_entry_t *cur, const uint8_t *mac64)
 {
-    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
-    if (!cur) {
-        return false;
-    }
-
 
     uint16_t bufId;
     mle_message_timeout_params_t timeout;
@@ -315,7 +291,7 @@ static int8_t protocol_6lowpan_router_challenge(int8_t interface_id, const uint8
     tr_debug("Link REQUEST");
     bufId = mle_service_msg_allocate(cur->id, 32, true,MLE_COMMAND_REQUEST);
     if (bufId == 0) {
-        return -1;
+        return false;
     }
 
     uint8_t *ptr = mle_service_get_data_pointer(bufId);
@@ -342,16 +318,16 @@ static int8_t protocol_6lowpan_router_challenge(int8_t interface_id, const uint8
     mle_service_set_msg_timeout_parameters(bufId, &timeout);
 
     mle_service_send_message(bufId);
-    return 0;
+    return true;
 }
 
 
-static uint8_t mle_advert_neigh_cnt(int8_t interface_id, bool short_adr) {
+static uint8_t mle_advert_neigh_cnt(protocol_interface_info_entry_t *cur_interface, bool short_adr) {
 
     uint8_t advert_neigh_cnt;
     uint8_t neighb_max;
 
-    uint8_t mle_neigh_cnt = mle_class_active_neigh_counter(interface_id);
+    uint8_t mle_neigh_cnt = mle_class_active_neigh_counter(cur_interface);
 
     if (short_adr == true) {
         neighb_max = 16;
@@ -413,88 +389,94 @@ static uint8_t mle_link_quality_tlv_parse(uint8_t *mac64, uint16_t short_address
     return 0;
 }
 
-static uint8_t *mle_table_set_neighbours(int8_t interface_id, uint8_t *ptr)
+static bool neighbor_list_short_address_available(mac_neighbor_table_t *table_class)
+{
+    ns_list_foreach(mac_neighbor_table_entry_t, cur_entry, &table_class->neighbour_list) {
+        if (cur_entry->connected_device && cur_entry->mac16 == 0xffff) {
+            return false;
+        }
+    }
+    return true;
+}
+
+
+static uint8_t *mle_table_set_neighbours(protocol_interface_info_entry_t *cur, uint8_t *ptr)
 {
     uint8_t *len_ptr = 0;
-    uint8_t short_temp[2] = {0xff,0xff};
     uint8_t neigh_count = 0;
     uint8_t neigh_count_max = 0;
     uint8_t *link_flags_ptr;
-    mle_neigh_table_entry_t *first_entry_ptr = NULL;
-    bool loop_list = false;
+    mac_neighbor_table_entry_t *first_entry_ptr = NULL;
 
-    mle_neigh_table_list_t * neigh_list = mle_class_active_list_get(interface_id);
-    if (!neigh_list) {
-        return ptr;
-    }
+    mac_neighbor_table_list_t * neigh_list = &cur->mac_parameters->mac_neighbor_table->neighbour_list;
 
     *ptr++ = MLE_TYPE_LINK_QUALITY;
     len_ptr = ptr++;
     *len_ptr = 1;
-    // defaults: complete, 2 bytes long link-layer address
-    link_flags_ptr = ptr++;
-    *link_flags_ptr = 0x81;
 
-    if (mle_class_get_by_link_address(interface_id, short_temp,ADDR_802_15_4_SHORT)) {
-        *link_flags_ptr |= 0x07;
-        neigh_count_max = mle_advert_neigh_cnt(interface_id, false);
+    link_flags_ptr = ptr++;
+    //*link_flags_ptr = 0x81;
+    bool use_short_address_compression = neighbor_list_short_address_available(mac_neighbor_info(cur));
+    if (use_short_address_compression) {
+        //complete, 2 bytes long link-layer address
+        *link_flags_ptr = 0x81;
     } else {
-        neigh_count_max = mle_advert_neigh_cnt(interface_id, true);
+        //complete, 8 bytes long link-layer address
+        *link_flags_ptr = 0x87;
+
+    }
+    neigh_count_max = mle_advert_neigh_cnt(cur, use_short_address_compression);
+
+    bool clean_entries = false;
+    ns_list_foreach(mac_neighbor_table_entry_t, cur_entry, neigh_list)
+    {
+
+        if ((cur_entry->connected_device) && (cur_entry->advertisment == false)) {
+
+            // If looping list, stops adding entries when at first sent entry again
+            if (first_entry_ptr == cur_entry) {
+                break;
+            } else if (first_entry_ptr == NULL) {
+                first_entry_ptr = cur_entry;
+            }
+
+            // Limits the number of entries that are sent
+            if (++neigh_count > neigh_count_max) {
+                *link_flags_ptr &= 0x7f;
+                break;
+            }
+
+            if (cur_entry->link_role == PRIORITY_PARENT_NEIGHBOUR) {
+                *ptr++ = MLE_NEIGHBOR_PRIORITY_LINK | MLE_NEIGHBOR_INCOMING_LINK | MLE_NEIGHBOR_OUTGOING_LINK;
+            } else {
+                *ptr++ = MLE_NEIGHBOR_INCOMING_LINK | MLE_NEIGHBOR_OUTGOING_LINK;
+            }
+
+            *ptr++ = etx_local_incoming_idr_read(cur->id, cur_entry->index) >> 3;
+
+            if (use_short_address_compression) {
+                ptr = common_write_16_bit(cur_entry->mac16, ptr);
+                *len_ptr += 4;
+            } else {
+                memcpy(ptr, cur_entry->mac64, 8);
+                ptr += 8;
+                *len_ptr += 10;
+            }
+
+            // If end of the neighbor list, Mark a clean advertisment from the list
+            if (cur_entry->link.next == 0) {
+                clean_entries = true;
+            }
+            cur_entry->advertisment = true;
+        }
     }
 
-    do {
-        ns_list_foreach(mle_neigh_table_entry_t, cur, neigh_list)
-        {
-
-            loop_list = false;
-
-            if ((cur->handshakeReady) && (cur->link_q_adv_sent == false)) {
-
-                // If looping list, stops adding entries when at first sent entry again
-                if (first_entry_ptr == cur) {
-                    break;
-                } else if (first_entry_ptr == NULL) {
-                    first_entry_ptr = cur;
-                }
-
-                // Limits the number of entries that are sent
-                if (++neigh_count > neigh_count_max) {
-                    *link_flags_ptr &= 0x7f;
-                    break;
-                }
-
-                if (cur->priorityFlag) {
-                    *ptr++ = MLE_NEIGHBOR_PRIORITY_LINK | MLE_NEIGHBOR_INCOMING_LINK | MLE_NEIGHBOR_OUTGOING_LINK;
-                } else {
-                    *ptr++ = MLE_NEIGHBOR_INCOMING_LINK | MLE_NEIGHBOR_OUTGOING_LINK;
-                }
-
-                *ptr++ = etx_local_incoming_idr_read(interface_id, cur) >> 3;
-
-                if ((*link_flags_ptr & 0x07) == 1) {
-                    ptr = common_write_16_bit(cur->short_adr, ptr);
-                    *len_ptr += 4;
-                } else {
-                    memcpy(ptr, cur->mac64, 8);
-                    ptr += 8;
-                    *len_ptr += 10;
-                }
-
-                // If end of the neighbor list, start adding entries from start again
-                if (cur->link.next == 0) {
-                    loop_list = true;
-                    mle_neigh_table_list_t * neigh_temp = mle_class_active_list_get(interface_id);
-                    ns_list_foreach(mle_neigh_table_entry_t, temp, neigh_temp)
-                    {
-                        // Marks entries not sent
-                        temp->link_q_adv_sent = false;
-                    }
-                } else {
-                    cur->link_q_adv_sent = true;
-                }
-            }
+    if (clean_entries) {
+        ns_list_foreach(mac_neighbor_table_entry_t, temp, neigh_list) {
+            // Marks entries not sent
+            temp->advertisment = false;
         }
-    } while (loop_list);
+    }
 
     return ptr;
 }
@@ -515,10 +497,10 @@ static int protocol_6lowpan_mle_neigh_advertise(protocol_interface_info_entry_t
         return 0;
     }
 
-    if (mle_class_get_by_link_address(cur->id, short_temp,ADDR_802_15_4_SHORT)) {
-        neig_cache_size += mle_advert_neigh_cnt(cur->id, false) * 10;
+    if (mac_neighbor_table_address_discover(mac_neighbor_info(cur), short_temp,ADDR_802_15_4_SHORT)) {
+        neig_cache_size += mle_advert_neigh_cnt(cur, false) * 10;
     } else {
-        neig_cache_size += mle_advert_neigh_cnt(cur->id, true) << 2;
+        neig_cache_size += mle_advert_neigh_cnt(cur, true) << 2;
     }
 
     uint16_t bufId = mle_service_msg_allocate(cur->id, neig_cache_size, false, MLE_COMMAND_ADVERTISEMENT);
@@ -539,7 +521,7 @@ static int protocol_6lowpan_mle_neigh_advertise(protocol_interface_info_entry_t
     ptr = mle_service_get_data_pointer(bufId);
     ptr = mle_general_write_source_address(ptr, cur);
     ptr = mle_tlv_write_mode(ptr, lowpan_mode_get_by_interface_ptr(cur));
-    ptr = mle_table_set_neighbours(cur->id, ptr);
+    ptr = mle_table_set_neighbours(cur, ptr);
 
     if (mle_service_update_length_by_ptr(bufId,ptr)!= 0) {
         tr_debug("Buffer overflow at message write");
@@ -549,15 +531,6 @@ static int protocol_6lowpan_mle_neigh_advertise(protocol_interface_info_entry_t
 }
 #endif
 
-static uint8_t compute_link_margin(int8_t rssi)
-{
-    if (rssi < -94) {
-        return 0;
-    }
-
-    return (rssi + 94);
-}
-
 static int mle_validate_6lowpan_link_request_message(uint8_t *ptr, uint16_t data_len, mle_tlv_info_t *tlv_info)
 {
     /**
@@ -571,8 +544,11 @@ static int mle_validate_6lowpan_link_request_message(uint8_t *ptr, uint16_t data
     return 0;
 }
 
-static void mle_neigh_time_and_mode_update(mle_neigh_table_entry_t *entry_temp, uint8_t *tlv_ptr, uint16_t tlv_length)
+static void mle_neigh_time_and_mode_update(mac_neighbor_table_entry_t *entry_temp, mle_message_t *mle_msg)
 {
+    uint8_t *tlv_ptr = mle_msg->data_ptr;
+    uint16_t tlv_length = mle_msg->data_length;
+
     mle_tlv_info_t mle_tlv_info;
     uint32_t timeout_tlv;
 
@@ -580,42 +556,44 @@ static void mle_neigh_time_and_mode_update(mle_neigh_table_entry_t *entry_temp,
         return;
     }
 
+    protocol_interface_info_entry_t *cur = mle_msg->interface_ptr;
+
     if (mle_tlv_option_discover(tlv_ptr, tlv_length, MLE_TYPE_MODE, &mle_tlv_info) > 0) {
         uint8_t *t_ptr = mle_tlv_info.dataPtr;
-        entry_temp->mode = *t_ptr;
+        mle_mode_parse_to_mac_entry(entry_temp, *t_ptr);
     }
 
     if (mle_tlv_option_discover(tlv_ptr, tlv_length, MLE_TYPE_TIMEOUT, &mle_tlv_info) > 0) {
         timeout_tlv = common_read_32_bit(mle_tlv_info.dataPtr);
     } else {
-        if (entry_temp->mode & MLE_FFD_DEV) {
+        if (entry_temp->ffd_device) {
             timeout_tlv = mle_6lowpan_data->router_lifetime;
         } else {
             timeout_tlv = mle_6lowpan_data->host_lifetime;
         }
     }
-    mle_entry_timeout_update(entry_temp, timeout_tlv);
+    mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_temp, timeout_tlv);
 }
 
-static void mle_neigh_entry_update_by_mle_tlv_list(int8_t interface_id, mle_neigh_table_entry_t *entry_temp, uint8_t *tlv_ptr, uint16_t tlv_length, uint8_t *mac64, uint16_t short_address)
+static void mle_neigh_entry_update_by_mle_tlv_list(int8_t interface_id, mac_neighbor_table_entry_t *entry_temp, uint8_t *tlv_ptr, uint16_t tlv_length, uint8_t *mac64, uint16_t short_address)
 {
     mle_tlv_info_t mle_tlv_info;
 
     if (tlv_length) {
         if (mle_tlv_option_discover(tlv_ptr, tlv_length, MLE_TYPE_SRC_ADDRESS, &mle_tlv_info) > 0) {
-            entry_temp->short_adr = common_read_16_bit(mle_tlv_info.dataPtr);
+            entry_temp->mac16 = common_read_16_bit(mle_tlv_info.dataPtr);
         }
 
         if (mle_tlv_option_discover(tlv_ptr, tlv_length, MLE_TYPE_LINK_QUALITY, &mle_tlv_info) > 0) {
             uint8_t link_idr;
             uint8_t iop_flags;
             if (mle_link_quality_tlv_parse(mac64, short_address, mle_tlv_info.dataPtr, mle_tlv_info.tlvLen, &iop_flags, &link_idr)) {
-                etx_remote_incoming_idr_update(interface_id, link_idr, entry_temp);
+                etx_remote_incoming_idr_update(interface_id, link_idr, entry_temp->index);
 
                 if ((iop_flags & MLE_NEIGHBOR_PRIORITY_LINK) == MLE_NEIGHBOR_PRIORITY_LINK) {
-                    entry_temp->priority_child_flag = true;
-                } else {
-                    entry_temp->priority_child_flag = false;
+                    entry_temp->link_role = CHILD_NEIGHBOUR;
+                } else if (entry_temp->link_role == CHILD_NEIGHBOUR) {
+                    entry_temp->link_role = NORMAL_NEIGHBOUR;
                 }
             }
         }
@@ -758,10 +736,10 @@ static int mle_router_accept_request_build(protocol_interface_info_entry_t *cur,
 
 static void protocol_6lowpan_link_reject_handler(protocol_interface_info_entry_t *cur, uint8_t *ll64)
 {
-    mle_neigh_table_entry_t *entry_temp = mle_class_get_entry_by_ll64(cur->id, 0, ll64, false, NULL);
+    mac_neighbor_table_entry_t *mac_entry = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), ll64, false, NULL);
     tr_debug("MLE link reject");
-    if (entry_temp) {
-        mle_class_remove_entry(cur->id, entry_temp);
+    if (mac_entry) {
+        mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), mac_entry);
     }
 }
 
@@ -984,14 +962,16 @@ int protocol_6lowpan_router_synch_to_new_router(protocol_interface_info_entry_t
 }
 
 
-static uint8_t mle_calculate_idr(int8_t interface_id, mle_message_t *mle_msg, mle_neigh_table_entry_t *entry_temp)
+static uint8_t mle_calculate_idr(int8_t interface_id, mle_message_t *mle_msg, mac_neighbor_table_entry_t *entry_temp)
 {
-
-    return etx_lqi_dbm_update(interface_id, mle_msg->lqi, mle_msg->dbm, entry_temp) >> 3;
+    if (!entry_temp) {
+        return etx_lqi_dbm_update(-2, mle_msg->lqi, mle_msg->dbm, 0) >> 3;
+    }
+    return etx_lqi_dbm_update(interface_id, mle_msg->lqi, mle_msg->dbm, entry_temp->index) >> 3;
 
 }
 
-static bool mle_6lowpan_neighbor_limit_check(int8_t interface_id, mle_message_t *mle_msg, uint8_t only_max_limit_chk)
+static bool mle_6lowpan_neighbor_limit_check(mle_message_t *mle_msg, uint8_t only_max_limit_chk)
 {
     uint16_t mle_neigh_cnt;
     bool link_quality = false;
@@ -1000,7 +980,7 @@ static bool mle_6lowpan_neighbor_limit_check(int8_t interface_id, mle_message_t
         return true;
     }
 
-    mle_neigh_cnt = mle_class_active_neigh_counter(interface_id);
+    mle_neigh_cnt = mle_class_active_neigh_counter(mle_msg->interface_ptr);
 
     // Neighbor max limit
     if (mle_neigh_cnt >= mle_6lowpan_data->nbr_of_neigh_max) {
@@ -1048,17 +1028,11 @@ void mle_6lowpan_message_handler(int8_t interface_id, mle_message_t *mle_msg, ml
     uint8_t mode = 0x0a;
     mle_tlv_info_t mle_tlv_info;
     mle_tlv_info_t mle_challenge;
-    mle_neigh_table_entry_t *entry_temp;
-    uint8_t linkMargin;
+    mac_neighbor_table_entry_t *entry_temp;
     uint8_t incoming_idr;
     uint16_t responseId, own_mac16;
-    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
-    if (!cur) {
-        return;
-    }
+    protocol_interface_info_entry_t *cur = mle_msg->interface_ptr;
 
-    //Calculate link margin
-    linkMargin = compute_link_margin(mle_msg->dbm);
 
     own_mac16 = mac_helper_mac16_address_get(cur);
 
@@ -1081,13 +1055,13 @@ void mle_6lowpan_message_handler(int8_t interface_id, mle_message_t *mle_msg, ml
                 mle_6lowpan_data->link_req_token_bucket--;
             } else {
                 //Update only old information based on link request
-                entry_temp = mle_class_get_entry_by_ll64(interface_id, linkMargin, mle_msg->packet_src_address, false, NULL);
+                entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, false, NULL);
                 if (entry_temp) {
-                    mle_neigh_time_and_mode_update(entry_temp,mle_msg->data_ptr, mle_msg->data_length);
+                    mle_neigh_time_and_mode_update(entry_temp,mle_msg);
                     mle_neigh_entry_update_by_mle_tlv_list(interface_id, entry_temp, mle_msg->data_ptr, mle_msg->data_length, cur->mac, own_mac16);
                     mle_neigh_entry_frame_counter_update(entry_temp, mle_msg->data_ptr, mle_msg->data_length, cur, security_headers->KeyIndex);
                 } else {
-                    if (!mle_6lowpan_neighbor_limit_check(interface_id, mle_msg, false)) {
+                    if (!mle_6lowpan_neighbor_limit_check(mle_msg, false)) {
                         return;
                     }
                 }
@@ -1095,7 +1069,7 @@ void mle_6lowpan_message_handler(int8_t interface_id, mle_message_t *mle_msg, ml
 
             incoming_idr = mle_calculate_idr(interface_id, mle_msg, entry_temp);
 
-            if (entry_temp && entry_temp->handshakeReady) {
+            if (entry_temp && entry_temp->connected_device) {
                 response_type = MLE_COMMAND_ACCEPT;
             } else {
                 response_type = MLE_COMMAND_ACCEPT_AND_REQUEST;
@@ -1117,13 +1091,11 @@ void mle_6lowpan_message_handler(int8_t interface_id, mle_message_t *mle_msg, ml
             }
 
             tr_debug("Accept & Request");
-
-            entry_temp = mle_class_get_entry_by_ll64(interface_id, linkMargin, mle_msg->packet_src_address, false, NULL);
-
+            entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, false, NULL);
             if (!entry_temp) {
                 // If there is space for neighbors try to allocate new entry
-                if (mle_6lowpan_neighbor_limit_check(interface_id, mle_msg, true)) {
-                    entry_temp = mle_class_get_entry_by_ll64(interface_id, linkMargin, mle_msg->packet_src_address, true, NULL);
+                if (mle_6lowpan_neighbor_limit_check(mle_msg, true)) {
+                    entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, true, NULL);
                 }
             }
 
@@ -1137,23 +1109,22 @@ void mle_6lowpan_message_handler(int8_t interface_id, mle_message_t *mle_msg, ml
 
             //Response state set now timeout know positive state
             mle_service_set_msg_response_true(responseId);
-
-            entry_temp->threadNeighbor = false;
-            entry_temp->handshakeReady = 1;
+            entry_temp->connected_device = 1;
 
             mac_data_poll_protocol_poll_mode_decrement(cur);
 
             //Read Source address and Challenge
-            mle_neigh_time_and_mode_update(entry_temp,mle_msg->data_ptr, mle_msg->data_length);
+            mle_neigh_time_and_mode_update(entry_temp,mle_msg);
             if (mle_msg->message_type == MLE_COMMAND_ACCEPT_AND_REQUEST) {
                 // If no global address set priority (bootstrap ongoing)
                 if (!cur->global_address_available) {
-                    entry_temp->priorityFlag = true;
+                    entry_temp->link_role = PRIORITY_PARENT_NEIGHBOUR;
                 }
 
                 mle_neigh_entry_update_by_mle_tlv_list(cur->id, entry_temp, mle_msg->data_ptr, mle_msg->data_length, cur->mac, own_mac16);
                 incoming_idr = mle_calculate_idr(cur->id, mle_msg, entry_temp);
-                mle_router_accept_request_build(cur, mle_msg, mle_challenge.dataPtr, mle_challenge.tlvLen, MLE_COMMAND_ACCEPT, incoming_idr, entry_temp->priorityFlag);
+                uint8_t priority = (entry_temp->link_role == PRIORITY_PARENT_NEIGHBOUR);
+                mle_router_accept_request_build(cur, mle_msg, mle_challenge.dataPtr, mle_challenge.tlvLen, MLE_COMMAND_ACCEPT, incoming_idr, priority);
             } else {
                 mle_neigh_entry_update_by_mle_tlv_list(cur->id, entry_temp, mle_msg->data_ptr, mle_msg->data_length, cur->mac, own_mac16);
                 incoming_idr = mle_calculate_idr(cur->id, mle_msg, entry_temp);
@@ -1161,7 +1132,7 @@ void mle_6lowpan_message_handler(int8_t interface_id, mle_message_t *mle_msg, ml
             mle_neigh_entry_frame_counter_update(entry_temp, mle_msg->data_ptr, mle_msg->data_length, cur, security_headers->KeyIndex);
             // If MLE frame counter was invalid update its value since three way handshake is complete
             if (security_headers->invalid_frame_counter) {
-                entry_temp->mle_frame_counter = security_headers->frameCounter;
+                mle_service_frame_counter_entry_add(interface_id, entry_temp->index, security_headers->frameCounter);
             }
             break;
 
@@ -1184,12 +1155,11 @@ void mle_6lowpan_message_handler(int8_t interface_id, mle_message_t *mle_msg, ml
                     t_ptr = mle_tlv_info.dataPtr;
                     mode = *t_ptr;
                 }
-
-                entry_temp = mle_class_get_entry_by_ll64(interface_id, linkMargin, mle_msg->packet_src_address, false, NULL);
+                entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, false, NULL);
                 if (!entry_temp) {
                     if ((mode & MLE_DEV_MASK) == MLE_FFD_DEV) {
                         // If there is space for neighbors synchronizes to new router
-                        if (mle_6lowpan_neighbor_limit_check(interface_id, mle_msg, false)) {
+                        if (mle_6lowpan_neighbor_limit_check(mle_msg, false)) {
                             // Checks blacklist
                             if (blacklist_reject(mle_msg->packet_src_address)) {
                                 return;
@@ -1220,7 +1190,7 @@ void mle_6lowpan_message_handler(int8_t interface_id, mle_message_t *mle_msg, ml
                                 //Possible remove
                                 if ((mode & MLE_DEV_MASK) == MLE_RFD_DEV) {
                                     //Remove Entry
-                                    mle_class_remove_entry(cur->id, entry_temp);
+                                    mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), entry_temp);
                                     tr_error("MLE adv: Own address not found");
                                     return;
                                 }
@@ -1232,8 +1202,8 @@ void mle_6lowpan_message_handler(int8_t interface_id, mle_message_t *mle_msg, ml
                 //UPDATE
                 mle_neigh_entry_update_by_mle_tlv_list(cur->id,entry_temp, mle_msg->data_ptr, mle_msg->data_length, cur->mac, own_mac16);
                 mle_neigh_entry_frame_counter_update(entry_temp, mle_msg->data_ptr, mle_msg->data_length, cur, security_headers->KeyIndex);
-                if (entry_temp->handshakeReady) {
-                    mle_entry_timeout_refresh(entry_temp);
+                if (entry_temp->connected_device) {
+                    mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_temp, entry_temp->link_lifetime);
                 }
             }
             break;
@@ -1250,7 +1220,7 @@ int8_t arm_6lowpan_mle_service_ready_for_security_init(protocol_interface_info_e
         //validate MLE service
         if (!mle_service_interface_registeration_validate(cur->id)) {
             //Register
-            if (mle_service_interface_register(cur->id,mle_6lowpan_message_handler, cur->mac,8) != 0) {
+            if (mle_service_interface_register(cur->id,cur, mle_6lowpan_message_handler, cur->mac,8) != 0) {
                 tr_error("Mle Service init Fail");
                 return -1;
             }
@@ -1301,29 +1271,24 @@ mle_6lowpan_data_t *protocol_6lowpan_mle_data_get(void)
 
 static void protocol_6lowpan_mle_purge_neighbors(struct protocol_interface_info_entry *cur_interface, uint8_t entry_count, uint8_t force_priority)
 {
-    mle_neigh_table_list_t *mle_neigh_table;
+
     uint8_t count = 0;
     uint8_t ll64[16];
 
     if (!cur_interface) {
         return;
     }
+    mac_neighbor_table_list_t *mac_table_list = &cur_interface->mac_parameters->mac_neighbor_table->neighbour_list;
 
-    mle_neigh_table = mle_class_active_list_get(cur_interface->id);
+    entry_count = protocol_6lowpan_mle_order_last_entries(cur_interface->id, mac_table_list, entry_count);
 
-    if (!mle_neigh_table) {
-        return;
-    }
-
-    entry_count = protocol_6lowpan_mle_order_last_entries(mle_neigh_table, entry_count);
-
-    ns_list_foreach_reverse_safe(mle_neigh_table_entry_t, entry, mle_neigh_table) {
+    ns_list_foreach_reverse_safe(mac_neighbor_table_entry_t, entry, mac_table_list) {
         if (++count > entry_count) {
             break;
         }
 
         if (!force_priority) {
-            if (entry->priorityFlag || entry->priority_child_flag) {
+            if (entry->link_role ==  PRIORITY_PARENT_NEIGHBOUR || entry->link_role == CHILD_NEIGHBOUR) {
                 break;
             }
         }
@@ -1336,23 +1301,23 @@ static void protocol_6lowpan_mle_purge_neighbors(struct protocol_interface_info_
 
         // Sends REJECT
         mle_service_reject_message_build(cur_interface->id, ll64, false);
-        mle_class_remove_entry(cur_interface->id, entry);
+        mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur_interface), entry);
 
         // Adds purged neighbor to blacklist so that it is not added right away back from advertisement
         blacklist_update(ll64, false);
     }
 }
 
-static uint8_t protocol_6lowpan_mle_order_last_entries(mle_neigh_table_list_t *mle_neigh_table, uint8_t entry_count)
+static uint8_t protocol_6lowpan_mle_order_last_entries(int8_t interface_id, mac_neighbor_table_list_t *mac_neigh_table, uint8_t entry_count)
 {
-    mle_neigh_table_entry_t *last;
-    mle_neigh_table_entry_t *first_ordered = NULL;
+    mac_neighbor_table_entry_t *last;
+    mac_neighbor_table_entry_t *first_ordered = NULL;
+    etx_storage_t * etx_last, *etx_cur;
     uint8_t count = 0;
-
     do {
         last = NULL;
 
-        ns_list_foreach(mle_neigh_table_entry_t, entry, mle_neigh_table) {
+        ns_list_foreach(mac_neighbor_table_entry_t, entry, mac_neigh_table) {
 
             if (entry == first_ordered) {
                 break;
@@ -1363,37 +1328,27 @@ static uint8_t protocol_6lowpan_mle_order_last_entries(mle_neigh_table_list_t *m
                 continue;
             }
 
-            // Primary parent (parent selected for bootstrap or RPL primary parent)
-            if (entry->priorityFlag && !last->priorityFlag) {
+            if (entry->link_role > last->link_role) { //Bigger link role is allways better
                 continue;
+            } else if (entry->link_role == last->link_role) {
+                // Compare ETX when Link role is same
+                etx_cur = etx_storage_entry_get(interface_id, entry->index);
+                etx_last = etx_storage_entry_get(interface_id, last->index);
+                if (etx_cur && etx_last && etx_cur->etx <= etx_last->etx) {
+                    continue;
+                }
             }
-
-            // Secondary parent (RPL secondary parent)
-            if (entry->second_priority_flag && !last->second_priority_flag) {
-                continue;
-            }
-
-            // Uses this node as parent
-            if (entry->priority_child_flag && !last->priority_child_flag) {
-                continue;
-            }
-
-            // Better ETX
-            if (entry->etx <= last->etx) {
-                continue;
-            }
-
             last = entry;
         }
 
         // Sets last to end of list
         if (last) {
-            ns_list_remove(mle_neigh_table, last);
+            ns_list_remove(mac_neigh_table, last);
 
             if (first_ordered) {
-                ns_list_add_before(mle_neigh_table, first_ordered, last);
+                ns_list_add_before(mac_neigh_table, first_ordered, last);
             } else {
-                ns_list_add_to_end(mle_neigh_table, last);
+                ns_list_add_to_end(mac_neigh_table, last);
             }
 
             first_ordered = last;
@@ -1431,10 +1386,11 @@ static int8_t arm_6lowpan_bootstrap_down(protocol_interface_info_entry_t *cur)
 
 static void lowpan_mle_receive_security_bypass_cb(int8_t interface_id, mle_message_t *mle_msg)
 {
+    (void) interface_id;
 #ifdef PANA
-    protocol_interface_info_entry_t *interface = protocol_stack_interface_info_get_by_id(interface_id);
+    protocol_interface_info_entry_t *interface = mle_msg->interface_ptr;
     //Accept Only Link Reject
-    if (interface && mle_msg->message_type == MLE_COMMAND_REJECT) {
+    if (mle_msg->message_type == MLE_COMMAND_REJECT) {
 
         if ((interface->lowpan_info & (INTERFACE_NWK_BOOTSRAP_ACTIVE | INTERFACE_NWK_BOOTSRAP_PANA_AUTHENTICATION)) != (INTERFACE_NWK_BOOTSRAP_ACTIVE | INTERFACE_NWK_BOOTSRAP_PANA_AUTHENTICATION)) {
                 return;
@@ -1508,7 +1464,7 @@ static int8_t arm_6lowpan_bootstrap_up(protocol_interface_info_entry_t *cur)
             if (!mle_service_interface_registeration_validate(cur->id)) {
                 //Register
 
-                if (mle_service_interface_register(cur->id,mle_6lowpan_message_handler, cur->mac,8) != 0) {
+                if (mle_service_interface_register(cur->id, cur, mle_6lowpan_message_handler, cur->mac,8) != 0) {
                     tr_error("Mle Service init Fail");
                     return -1;
                 }
@@ -1594,7 +1550,6 @@ int8_t arm_network_processor_up(protocol_interface_info_entry_t *cur)
     } else {
         protocol_6lowpan_register_handlers(cur);
         mac_helper_pib_boolean_set(cur, macRxOnWhenIdle, true);
-        mle_class_mode_set(cur->id, MLE_CLASS_ROUTER);
         mac_helper_default_security_level_set(cur, SEC_NONE);
 
         if (cur->bootsrap_mode == ARM_NWK_BOOTSRAP_MODE_6LoWPAN_RF_SNIFFER) {
@@ -1621,15 +1576,6 @@ int8_t arm_network_processor_up(protocol_interface_info_entry_t *cur)
     return ret_val;
 }
 
-static bool lowpan_interface_is_active(int8_t interface_id) {
-    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
-    if (!cur || !(cur->lowpan_info & INTERFACE_NWK_ACTIVE)) {
-        return false;
-    }
-
-    return true;
-}
-
 static void arm_6lowpan_security_key_update_cb(protocol_interface_info_entry_t *cur, const mlme_security_t *security_params)
 {
     if (cur->mac_parameters->mac_next_key_index && (security_params->KeyIndex == cur->mac_parameters->mac_next_key_index)) {
@@ -1641,6 +1587,66 @@ static void arm_6lowpan_security_key_update_cb(protocol_interface_info_entry_t *
         }
     }
 }
+
+static void lowpan_neighbor_entry_remove_notify(mac_neighbor_table_entry_t *entry_ptr, void *user_data)
+{
+
+    protocol_interface_info_entry_t *cur_interface = user_data;
+    lowpan_adaptation_remove_free_indirect_table(cur_interface, entry_ptr);
+    // Sleepy host
+    if (cur_interface->lowpan_info & INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE) {
+        mac_data_poll_protocol_poll_mode_decrement(cur_interface);
+    }
+
+    protocol_6lowpan_priority_neighbor_remove(cur_interface, entry_ptr);
+
+    if (entry_ptr->ffd_device) {
+        protocol_6lowpan_release_short_link_address_from_neighcache(cur_interface, entry_ptr->mac16);
+        protocol_6lowpan_release_long_link_address_from_neighcache(cur_interface, entry_ptr->mac64);
+    }
+    mac_helper_devicetable_remove(cur_interface->mac_api, entry_ptr->index);
+    //Removes ETX neighbor
+    etx_neighbor_remove(cur_interface->id, entry_ptr->index);
+    //Remove MLE frame counter info
+    mle_service_frame_counter_entry_delete(cur_interface->id, entry_ptr->index);
+
+}
+
+
+static bool lowpan_neighbor_entry_nud_notify(mac_neighbor_table_entry_t *entry_ptr, void *user_data)
+{
+
+    // Sleepy host
+    protocol_interface_info_entry_t *cur_interface = user_data;
+
+    if (cur_interface->lowpan_info & INTERFACE_NWK_ROUTER_DEVICE) {
+        //Trig middle way challenge if Broadcast message  have been missed
+        if (!entry_ptr->ffd_device) {
+            return false; //End device must do this
+        }
+
+        if (entry_ptr->lifetime > (entry_ptr->link_lifetime / 2)) {
+            return false; //Trig only when midway is overed
+        }
+        return protocol_6lowpan_router_challenge(cur_interface, entry_ptr->mac64);
+    }
+
+    if (entry_ptr->link_role != PRIORITY_PARENT_NEIGHBOUR) {
+        return false; //Do not never challenge than priority parent
+    }
+
+    if (cur_interface->lowpan_info & INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE) {
+        return false; //Sleepy end device should not never challenge
+    }
+
+    if (entry_ptr->lifetime  > MLE_TABLE_CHALLENGE_TIMER) {
+        return false;
+    }
+
+    return protocol_6lowpan_host_challenge(cur_interface, entry_ptr->mac64);
+}
+
+
 int8_t arm_6lowpan_bootstarp_bootstrap_set(int8_t interface_id, net_6lowpan_mode_e bootstrap_mode, net_6lowpan_mode_extension_e net_6lowpan_mode_extension)
 {
     int8_t ret_val = -1;
@@ -1665,20 +1671,29 @@ int8_t arm_6lowpan_bootstarp_bootstrap_set(int8_t interface_id, net_6lowpan_mode
     cur->mac_security_key_usage_update_cb = arm_6lowpan_security_key_update_cb;
     //Allocate MLE class here
     //Deallocate old here
+    mac_neighbor_table_delete(mac_neighbor_info(cur));
+    mac_description_storage_size_t buffer;
+    //Read MAC device table sizes
+    if (cur->mac_api->mac_storage_sizes_get(cur->mac_api, &buffer) != 0) {
+        return -1;
+    }
 
-    mle_class_deallocate(interface_id);
+    mac_neighbor_info(cur) = mac_neighbor_table_create(buffer.device_decription_table_size, lowpan_neighbor_entry_remove_notify
+                                                                        , lowpan_neighbor_entry_nud_notify, cur);
+    if (!mac_neighbor_info(cur)) {
+        return -1;
+    }
 
     if (enable_mle_protocol) {
+        if (mle_service_frame_counter_table_allocate(interface_id, buffer.device_decription_table_size)) {
+            return -1;
+        }
 
-        mac_description_storage_size_t buffer;
-        //Read MAC device table sizes
-        if (cur->mac_api->mac_storage_sizes_get(cur->mac_api, &buffer) != 0) {
+        if (!etx_storage_list_allocate(cur->id, buffer.device_decription_table_size)) {
             return -1;
         }
-        if (mle_class_init(interface_id, buffer.device_decription_table_size, &protocol_6lowpan_neighbor_information_remove, &protocol_6lowpan_host_challenge, &lowpan_interface_is_active) != 0) {
-            return -1;
-        }
-        mle_class_router_challenge(interface_id, protocol_6lowpan_router_challenge);
+
+        lowpan_adaptation_interface_etx_update_enable(cur->id);
     }
 
     mle_service_interface_unregister(cur->id);
@@ -1704,7 +1719,7 @@ int8_t arm_6lowpan_bootstarp_bootstrap_set(int8_t interface_id, net_6lowpan_mode
             return -2;
 #else
             cur->comm_status_ind_cb = lowpan_comm_status_indication_cb;
-            if (mle_service_interface_register(cur->id,mle_6lowpan_message_handler, cur->mac,8) != 0) {
+            if (mle_service_interface_register(cur->id, cur, mle_6lowpan_message_handler, cur->mac,8) != 0) {
                 tr_error("Mle Service init Fail");
                 return -1;
             }
@@ -1906,7 +1921,7 @@ void protocol_6lowpan_link_advertise_handle(nd_router_t *cur, protocol_interface
                     cur->mle_purge_timer -= 1;
                 } else {
                     if (mle_6lowpan_data && mle_6lowpan_data->nbr_of_neigh_max != 0) {
-                        uint16_t mle_neigh_cnt = mle_class_active_neigh_counter(cur_interface->id);
+                        uint16_t mle_neigh_cnt = mle_class_active_neigh_counter(cur_interface);
                         if (mle_neigh_cnt > (mle_6lowpan_data->nbr_of_neigh_max - MLE_NEIGHBOR_PURGE_NBR)) {
                             protocol_6lowpan_mle_purge_neighbors(cur_interface, MLE_NEIGHBOR_PURGE_NBR, true);
                         }
@@ -1946,15 +1961,15 @@ static void protocol_6lowpan_nd_ready(protocol_interface_info_entry_t *cur)
         if ((cur->lowpan_info & (INTERFACE_NWK_ROUTER_DEVICE | INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE | INTERFACE_NWK_BOOTSRAP_MLE)) == INTERFACE_NWK_BOOTSRAP_MLE) {
             //TRIG Only Normal Host
 #ifndef NO_MLE
-            //GET Cordinaotor MLE Entry
+            //GET Cordinator MLE Entry
             addrtype_t addrType;
             uint8_t tempAddr[8];
             addrType = mac_helper_coordinator_address_get(cur, tempAddr);
+            mac_neighbor_table_entry_t * neig_info = mac_neighbor_table_address_discover(mac_neighbor_info(cur), tempAddr, addrType);
 
-            mle_neigh_table_entry_t *entry_t = mle_class_get_by_link_address(cur->id, tempAddr, addrType);
-            if (entry_t) {
-                if (entry_t->ttl > MLE_TABLE_CHALLENGE_TIMER) {
-                    entry_t->ttl = (MLE_TABLE_CHALLENGE_TIMER + 1);
+            if (neig_info) {
+                if (neig_info->lifetime > MLE_TABLE_CHALLENGE_TIMER) {
+                    neig_info->lifetime  = (MLE_TABLE_CHALLENGE_TIMER + 1);
                 }
             }
 #endif
@@ -2175,7 +2190,6 @@ void nwk_6lowpan_nd_address_registartion_ready(protocol_interface_info_entry_t *
             if (cur->bootsrap_mode == ARM_NWK_BOOTSRAP_MODE_6LoWPAN_SLEEPY_HOST) {
                 cur->lowpan_info |= INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE;
                 tr_debug("Enable Poll state");
-                mle_class_mode_set(cur->id, MLE_CLASS_SLEEPY_END_DEVICE);
                 mac_helper_pib_boolean_set(cur, macRxOnWhenIdle, false);
                 mac_data_poll_init(cur);
                 mac_data_poll_init_protocol_poll(cur);
@@ -2753,12 +2767,13 @@ static void protocol_6lowpan_generate_link_reject(protocol_interface_info_entry_
 
 static void lowpan_comm_status_indication_cb(int8_t if_id, const mlme_comm_status_t* status)
 {
-#ifndef NO_MLE
     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(if_id);
     if (!cur) {
         return;
     }
 
+    mac_neighbor_table_entry_t * entry_ptr;
+
     switch (status->status) {
         case MLME_UNSUPPORTED_SECURITY:
         case MLME_UNAVAILABLE_KEY:
@@ -2775,29 +2790,33 @@ static void lowpan_comm_status_indication_cb(int8_t if_id, const mlme_comm_statu
 
             break;
         case MLME_DATA_POLL_NOTIFICATION:
-            mle_refresh_entry_timeout(if_id, status->SrcAddr, (addrtype_t)status->SrcAddrMode, false);
+            entry_ptr = mac_neighbor_table_address_discover(mac_neighbor_info(cur), status->SrcAddr, status->SrcAddrMode);
+            if (entry_ptr) {
+                // Refresh Timeout
+                mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_ptr, entry_ptr->link_lifetime);
+            }
             break;
         default:
             break;
     }
-#endif
 }
 
 bool lowpan_neighbour_data_clean(int8_t interface_id, const uint8_t *link_local_address)
 {
+
+    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
+    if (!cur) {
+        return false;
+    }
     bool return_value = false;
-#ifndef NO_MLE
-    mle_neigh_table_entry_t * neigh_entry = mle_class_get_entry_by_ll64(interface_id, 0, link_local_address, false, NULL);
+    mac_neighbor_table_entry_t *neigh_entry = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), link_local_address, false, NULL);
     if (neigh_entry) {
         //Remove entry
-        if (neigh_entry->priorityFlag) {
-            return_value = true;
-        } else if (neigh_entry->second_priority_flag) {
+        if (neigh_entry->link_role == PRIORITY_PARENT_NEIGHBOUR || neigh_entry->link_role == SECONDARY_PARENT_NEIGHBOUR) {
             return_value = true;
         }
-        mle_class_remove_entry(interface_id, neigh_entry);
+        mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), neigh_entry);
     }
-#endif
     return return_value;
 }
 
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan_interface.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan_interface.c
index 478d86ecaf..d3b0d16520 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan_interface.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/Generic/protocol_6lowpan_interface.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -37,6 +37,7 @@
 #include "6LoWPAN/Bootstraps/protocol_6lowpan_bootstrap.h"
 #include "Service_Libs/blacklist/blacklist.h"
 #include "6LoWPAN/MAC/mac_helper.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 #include "mac_api.h"
 
 #ifdef HAVE_RPL
@@ -66,6 +67,7 @@
 #include "platform/arm_hal_interrupt.h"
 #include "common_functions.h"
 #include "mac_api.h"
+#include "6LoWPAN/MAC/mpx_api.h"
 #include "6LoWPAN/lowpan_adaptation_interface.h"
 #include "6LoWPAN/Fragmentation/cipv6_fragmenter.h"
 #include "libNET/src/net_load_balance_internal.h"
@@ -80,12 +82,14 @@ static int8_t set_6lowpan_nwk_down(protocol_interface_info_entry_t *cur)
         /* Change Active -> Idle */
         /* Disable Protocols Timers */
         if (!thread_info(cur)) {
-            if (cur->lowpan_info & INTERFACE_NWK_BOOTSRAP_MLE) {
+            mac_neighbor_table_neighbor_list_clean(mac_neighbor_info(cur));
 #ifndef NO_MLE
-                mle_class_list_clean(cur->id);
+            if (cur->lowpan_info & INTERFACE_NWK_BOOTSRAP_MLE) {
+
                 blacklist_clear();
-#endif
+
             }
+#endif
         }
         if (cur->lowpan_info & INTERFACE_NWK_BOOTSRAP_PANA_AUTHENTICATION) {
             pana_reset_values(cur->mac_parameters->pan_id);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/protocol_6lowpan.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/protocol_6lowpan.h
index a27c6d2e26..ebd3ccf5b8 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/protocol_6lowpan.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/protocol_6lowpan.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -35,13 +35,11 @@ extern struct rpl_domain *protocol_6lowpan_rpl_domain;
 extern struct rpl_dodag *protocol_6lowpan_rpl_root_dodag;
 
 #ifdef HAVE_RPL
-#ifndef NO_MLE
 typedef enum {
     PRIORITY_1ST,
     PRIORITY_2ND,
 } neighbor_priority;
 #endif
-#endif
 
 void protocol_6lowpan_interface_common_init(struct protocol_interface_info_entry *cur);
 void protocol_6lowpan_host_init(struct protocol_interface_info_entry *cur,  bool sleepy_host);
@@ -53,12 +51,10 @@ int protocol_6lowpan_child_update(struct protocol_interface_info_entry *cur);
 void protocol_6lowpan_neighbor_priority_update(struct protocol_interface_info_entry *cur, uint8_t *removed_priority, uint8_t *updated_priority);
 
 #ifdef HAVE_RPL
-#ifndef NO_MLE
 uint16_t protocol_6lowpan_neighbor_priority_set(int8_t interface_id, addrtype_t addr_type, const uint8_t *addr_ptr);
 uint16_t protocol_6lowpan_neighbor_second_priority_set(int8_t interface_id, addrtype_t addr_type, const uint8_t *addr_ptr);
 void protocol_6lowpan_neighbor_priority_clear_all(int8_t interface_id, neighbor_priority priority);
 #endif
-#endif
 
 #else
 #define protocol_6lowpan_child_update(cur) (-1)
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/protocol_6lowpan_bootstrap.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/protocol_6lowpan_bootstrap.h
index e1b6e3cdc7..14bb828496 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/protocol_6lowpan_bootstrap.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Bootstraps/protocol_6lowpan_bootstrap.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -27,7 +27,6 @@
 
 struct protocol_interface_info_entry;
 struct nd_router;
-struct mle_neigh_table_entry_t;
 
 #define MLE_NEIGHBOR_PURGE_NBR                3
 #define MLE_NEIGHBOR_PURGE_TIMER_TIMEOUT      4       // Times advertisement timeout
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/IPHC_Decode/6lowpan_iphc.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/IPHC_Decode/6lowpan_iphc.c
index 10473ab585..b02b372fc1 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/IPHC_Decode/6lowpan_iphc.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/IPHC_Decode/6lowpan_iphc.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2017, Arm Limited and affiliates.
+ * Copyright (c) 2013-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -77,12 +77,7 @@ buffer_t *lowpan_down(buffer_t *buf)
             return NULL;
         }
         if (thread_info(cur)) {
-            mle_neigh_table_entry_t *mle_entry;
-            mle_entry = mle_class_get_by_link_address(cur->id, buf->dst_sa.address + 2, buf->dst_sa.addr_type);
-            if (mle_entry && thread_addr_is_child(mac_helper_mac16_address_get(cur), mle_entry->short_adr)) {
-                /* Check if the child can handle only stable network data (e.g. sleepy device) */
-                stable_only = !(mle_entry->mode & MLE_THREAD_REQ_FULL_DATA_SET);
-            }
+            stable_only = thread_stable_context_check(cur, buf);
         }
     }
 
@@ -169,7 +164,7 @@ buffer_t *lowpan_down(buffer_t *buf)
     /* RFC 6282+4944 require that we limit compression to the first fragment.
      * This check is slightly conservative - always allow 4 for first-fragment header
      */
-    uint_fast8_t overhead = mac_helper_frame_overhead(cur, buf);
+    uint_fast16_t overhead = mac_helper_frame_overhead(cur, buf);
     uint_fast16_t max_iphc_size = mac_helper_max_payload_size(cur, overhead) - mesh_size - 4;
 
     buf = iphc_compress(&cur->lowpan_contexts, buf, max_iphc_size, stable_only);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_data_poll.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_data_poll.c
index 340716bffb..bb1f90e259 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_data_poll.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_data_poll.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -45,6 +45,7 @@
 #endif
 #include "6LoWPAN/MAC/mac_helper.h"
 #include "6LoWPAN/MAC/mac_data_poll.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 
 #define TRACE_GROUP "mPol"
 
@@ -281,6 +282,23 @@ void mac_poll_timer_trig(uint32_t poll_time, protocol_interface_info_entry_t *cu
         }
     }
 }
+static mac_neighbor_table_entry_t *neighbor_data_poll_referesh(protocol_interface_info_entry_t *cur, uint8_t *address, addrtype_t type)
+{
+    mac_neighbor_table_entry_t *entry = mac_neighbor_table_address_discover(mac_neighbor_info(cur), address, type);
+
+    if (!entry) {
+        return NULL;
+    }
+
+    if (!entry->connected_device) {
+        return NULL;
+    }
+
+    if (!entry->nud_active) {
+        entry->lifetime = entry->link_lifetime;
+    }
+    return entry;
+}
 
 void mac_mlme_poll_confirm(protocol_interface_info_entry_t *cur, const mlme_poll_conf_t *confirm)
 {
@@ -295,20 +313,21 @@ void mac_mlme_poll_confirm(protocol_interface_info_entry_t *cur, const mlme_poll
     }
 
     rf_ptr->pollActive = false;
+    mac_neighbor_table_entry_t *entry = NULL;
 
     switch (confirm->status) {
         case MLME_SUCCESS:
             //tr_debug("Poll Confirm: Data with Data");
             rf_ptr->nwk_parent_poll_fail = 0;
             //Trig new Data Poll immediately
-            mle_refresh_entry_timeout(cur->id, rf_ptr->poll_req.CoordAddress, (addrtype_t)rf_ptr->poll_req.CoordAddrMode, true);
+            entry = neighbor_data_poll_referesh(cur, rf_ptr->poll_req.CoordAddress, (addrtype_t)rf_ptr->poll_req.CoordAddrMode);
             poll_time = 1;
             break;
 
         case MLME_NO_DATA:
             //Start next case timer
             rf_ptr->nwk_parent_poll_fail = 0;
-            mle_refresh_entry_timeout(cur->id, rf_ptr->poll_req.CoordAddress, (addrtype_t)rf_ptr->poll_req.CoordAddrMode, true);
+            entry = neighbor_data_poll_referesh(cur, rf_ptr->poll_req.CoordAddress, (addrtype_t)rf_ptr->poll_req.CoordAddrMode);
             //tr_debug("Poll Confirm: No Data");
 
             if (rf_ptr->protocol_poll == 0) {
@@ -333,6 +352,9 @@ void mac_mlme_poll_confirm(protocol_interface_info_entry_t *cur, const mlme_poll
             }
             break;
     }
+    if (thread_info(cur) && entry) {
+        thread_neighbor_communication_update(cur, entry->index);
+    }
 
     mac_poll_timer_trig(poll_time, cur);
 
@@ -423,7 +445,6 @@ int8_t mac_data_poll_host_mode_set(struct protocol_interface_info_entry *cur, ne
                 new_poll_time = (poll_time * 1000);
                 if (rf_ptr->host_mode == NET_HOST_RX_ON_IDLE) {
                     tr_debug("Init Poll timer and period");
-                    mle_class_mode_set(cur->id, MLE_CLASS_SLEEPY_END_DEVICE);
                 }
 
                 rf_ptr->nwk_app_poll_time = new_poll_time;
@@ -449,7 +470,6 @@ int8_t mac_data_poll_host_mode_set(struct protocol_interface_info_entry *cur, ne
                     }
                 }
                 tr_debug("Enable Poll By APP");
-                mle_class_mode_set(cur->id, MLE_CLASS_SLEEPY_END_DEVICE);
                 mac_helper_pib_boolean_set(cur, macRxOnWhenIdle, false);
                 mac_poll_timer_trig(1, cur);
                 rf_ptr->nwk_app_poll_time = 300;
@@ -525,14 +545,12 @@ void mac_data_poll_init(struct protocol_interface_info_entry *cur)
     if (cur->mac_parameters->RxOnWhenIdle) {
         tr_debug("Set Non-Sleepy HOST");
         rfd_ptr->host_mode = NET_HOST_RX_ON_IDLE;
-        mle_class_mode_set(cur->id, MLE_CLASS_END_DEVICE);
     } else {
 
         rfd_ptr->protocol_poll = 1;
         mac_poll_timer_trig(200, cur);
         tr_debug("Set Sleepy HOST configure");
         rfd_ptr->host_mode = NET_HOST_FAST_POLL_MODE;
-        mle_class_mode_set(cur->id, MLE_CLASS_SLEEPY_END_DEVICE);
         rfd_ptr->slow_poll_rate_seconds = 3;
         rfd_ptr->timeOutInSeconds = 32;
         rfd_ptr->nwk_app_poll_time = 300;
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.c
index faf954010d..97ab748da5 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -25,7 +25,6 @@
 #include "net_nwk_scan.h"
 #include "ns_trace.h"
 #include "common_functions.h"
-#include "MLE/mle_tlv.h"
 #include "mac_api.h"
 
 #define TRACE_GROUP "MACh"
@@ -685,7 +684,7 @@ int8_t mac_helper_mac64_set(protocol_interface_info_entry_t *interface, const ui
  * Given a buffer, with address and security flags set, compute the maximum
  * MAC payload that could be put in that buffer.
  */
-uint_fast16_t mac_helper_max_payload_size(protocol_interface_info_entry_t *cur, uint_fast8_t frame_overhead)
+uint_fast16_t mac_helper_max_payload_size(protocol_interface_info_entry_t *cur, uint_fast16_t frame_overhead)
 {
     uint16_t max;
 
@@ -833,7 +832,17 @@ void mac_helper_devicetable_remove(mac_api_t *mac_api, uint8_t attribute_index)
     mac_api->mlme_req(mac_api,MLME_SET , &set_req);
 }
 
-void mac_helper_devicetable_set(mle_neigh_table_entry_t *entry_temp, protocol_interface_info_entry_t *cur, uint32_t frame_counter, uint8_t keyID, bool force_set)
+void mac_helper_device_description_write(protocol_interface_info_entry_t *cur, mlme_device_descriptor_t *device_desc, uint8_t *mac64, uint16_t mac16, uint32_t frame_counter, bool exempt)
+{
+    memcpy(device_desc->ExtAddress, mac64, 8);
+    device_desc->ShortAddress = mac16;
+    device_desc->PANId = mac_helper_panid_get(cur);
+    device_desc->Exempt = exempt;
+    device_desc->FrameCounter = frame_counter;
+}
+
+void mac_helper_devicetable_set(const mlme_device_descriptor_t *device_desc, protocol_interface_info_entry_t *cur, uint8_t attribute_index, uint8_t keyID, bool force_set)
+
 {
     if (!cur->mac_api) {
         return;
@@ -844,18 +853,10 @@ void mac_helper_devicetable_set(mle_neigh_table_entry_t *entry_temp, protocol_in
         return;
     }
 
-    mlme_device_descriptor_t device_desc;
     mlme_set_t set_req;
-    device_desc.FrameCounter = frame_counter;
-    device_desc.Exempt = false;
-    device_desc.ShortAddress = entry_temp->short_adr;
-    memcpy(device_desc.ExtAddress, entry_temp->mac64, 8);
-    device_desc.PANId = mac_helper_panid_get(cur);
-
-
     set_req.attr = macDeviceTable;
-    set_req.attr_index = entry_temp->attribute_index;
-    set_req.value_pointer = (void*)&device_desc;
+    set_req.attr_index = attribute_index;
+    set_req.value_pointer = (void*)device_desc;
     set_req.value_size = sizeof(mlme_device_descriptor_t);
     tr_debug("Register Device");
     cur->mac_api->mlme_req(cur->mac_api,MLME_SET , &set_req);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.h
index a1fe37cd68..d95f22e2d2 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_helper.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -29,7 +29,6 @@ struct protocol_interface_info_entry;
 struct ns_sockaddr;
 struct buffer;
 struct mac_api_s;
-struct mle_neigh_table_entry_t;
 
 
 void mac_create_scan_request(mac_scan_type_t type, struct channel_list_s *chanlist, uint8_t scan_duration, struct mlme_scan_s *request);
@@ -100,7 +99,7 @@ bool mac_helper_write_our_addr(struct protocol_interface_info_entry *interface,
 
 int8_t mac_helper_mac64_set(struct protocol_interface_info_entry *interface, const uint8_t *mac64);
 
-uint_fast16_t mac_helper_max_payload_size(struct protocol_interface_info_entry *cur, uint_fast8_t frame_overhead);
+uint_fast16_t mac_helper_max_payload_size(struct protocol_interface_info_entry *cur, uint_fast16_t frame_overhead);
 
 uint_fast8_t mac_helper_frame_overhead(struct protocol_interface_info_entry *cur, const struct buffer *buf);
 
@@ -110,8 +109,9 @@ int8_t mac_helper_link_frame_counter_set(int8_t interface_id, uint32_t seq_ptr);
 
 void mac_helper_devicetable_remove(struct mac_api_s *mac_api, uint8_t attribute_index);
 
-void mac_helper_devicetable_set(struct mle_neigh_table_entry_t *entry_temp, struct protocol_interface_info_entry *cur, uint32_t frame_counter, uint8_t keyID, bool force_set);
+void mac_helper_device_description_write(struct protocol_interface_info_entry *cur, mlme_device_descriptor_t *device_desc, uint8_t *mac64, uint16_t mac16, uint32_t frame_counter, bool exempt);
 
+void mac_helper_devicetable_set(const mlme_device_descriptor_t *device_dec, struct protocol_interface_info_entry *cur, uint8_t attribute_index, uint8_t keyID, bool force_set);
 int8_t mac_helper_mac_mlme_max_retry_set(int8_t interface_id, uint8_t mac_retry_set);
 
 #endif // MAC_HELPER_H
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_ie_lib.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_ie_lib.c
new file mode 100644
index 0000000000..037b64527f
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_ie_lib.c
@@ -0,0 +1,185 @@
+/*
+ * Copyright (c) 2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "nsconfig.h"
+#include "ns_types.h"
+#include "string.h"
+#include "common_functions.h"
+#include "mac_common_defines.h"
+#include "mac_ie_lib.h"
+
+
+#define MAC_IE_HEADER_LENGTH_MASK 0x007f
+#define MAC_IE_HEADER_ID_MASK 0x7f80
+#define MAC_IE_PAYLOAD_LENGTH_MASK 0x07ff
+#define MAC_IE_PAYLOAD_ID_MASK 0x7800
+#define MAC_IE_TYPE_PAYLOAD_MASK 0x8000
+
+#define MAC_NESTED_LONG_IE_PAYLOAD_LENGTH_MASK 0x07ff
+#define MAC_NESTED_LONG_IE_PAYLOAD_ID_MASK 0x7800
+#define MAC_NESTED_SHORT_IE_PAYLOAD_LENGTH_MASK 0x00ff
+#define MAC_NESTED_SHORT_IE_PAYLOAD_ID_MASK 0x7f00
+#define MAC_NESTED_IE_TYPE_LONG_MASK 0x8000
+
+static void mac_ie_header_parse(mac_header_IE_t *header_element, uint8_t *ptr)
+{
+    uint16_t ie_dummy = common_read_16_bit_inverse(ptr);
+    header_element->length = (ie_dummy & MAC_IE_HEADER_LENGTH_MASK);
+    header_element->id = ((ie_dummy & MAC_IE_HEADER_ID_MASK  ) >> 7 );
+    header_element->content_ptr = ptr + 2;
+}
+
+static void mac_ie_payload_parse(mac_payload_IE_t *payload_element, uint8_t *ptr)
+{
+    uint16_t ie_dummy = common_read_16_bit_inverse(ptr);
+    payload_element->length = (ie_dummy & MAC_IE_PAYLOAD_LENGTH_MASK);
+    payload_element->id = ((ie_dummy & MAC_IE_PAYLOAD_ID_MASK  ) >> 11);
+    payload_element->content_ptr = ptr + 2;
+}
+
+static void mac_ie_nested_id_parse(mac_nested_payload_IE_t *element, uint8_t *ptr)
+{
+    uint16_t ie_dummy = common_read_16_bit_inverse(ptr);
+
+    if (ie_dummy & MAC_NESTED_IE_TYPE_LONG_MASK) {
+        element->type_long = true;
+        element->length = (ie_dummy & MAC_NESTED_LONG_IE_PAYLOAD_LENGTH_MASK);
+        element->id = ((ie_dummy & MAC_NESTED_LONG_IE_PAYLOAD_ID_MASK  ) >> 11);
+    } else {
+        element->type_long = false;
+        element->length = (ie_dummy & MAC_NESTED_SHORT_IE_PAYLOAD_LENGTH_MASK);
+        element->id = ((ie_dummy & MAC_NESTED_SHORT_IE_PAYLOAD_ID_MASK  ) >> 8);
+    }
+
+    element->content_ptr = ptr + 2;
+}
+
+
+uint8_t *mac_ie_header_base_write(uint8_t *ptr, uint8_t type, uint16_t length)
+{
+    uint16_t ie_dummy = 0; //Header Type
+    ie_dummy |= (length & MAC_IE_HEADER_LENGTH_MASK);
+    ie_dummy |= ((type << 7 ) &  MAC_IE_HEADER_ID_MASK);
+    return common_write_16_bit_inverse(ie_dummy, ptr);
+}
+
+uint8_t *mac_ie_payload_base_write(uint8_t *ptr, uint8_t type, uint16_t length)
+{
+
+    uint16_t ie_dummy = MAC_IE_TYPE_PAYLOAD_MASK; //Payload type
+    ie_dummy |= (length & MAC_IE_PAYLOAD_LENGTH_MASK);
+    ie_dummy |= ((type << 11 ) &  MAC_IE_PAYLOAD_ID_MASK);
+    return common_write_16_bit_inverse(ie_dummy, ptr);
+}
+
+uint8_t *mac_ie_nested_ie_long_base_write(uint8_t *ptr, uint8_t sub_id, uint16_t length)
+{
+    uint16_t ie_dummy = MAC_NESTED_IE_TYPE_LONG_MASK;
+    ie_dummy |= (length & MAC_NESTED_LONG_IE_PAYLOAD_LENGTH_MASK);
+    ie_dummy |= ((sub_id << 11 ) &  MAC_NESTED_LONG_IE_PAYLOAD_ID_MASK);
+
+    return common_write_16_bit_inverse(ie_dummy, ptr);
+}
+
+uint8_t *mac_ie_nested_ie_short_base_write(uint8_t *ptr, uint8_t sub_id, uint16_t length)
+{
+    uint16_t ie_dummy = 0;
+    ie_dummy |= (length & MAC_NESTED_SHORT_IE_PAYLOAD_LENGTH_MASK);
+    ie_dummy |= ((sub_id << 8 ) &  MAC_NESTED_SHORT_IE_PAYLOAD_ID_MASK);
+
+    return common_write_16_bit_inverse(ie_dummy, ptr);
+}
+
+uint16_t mac_ie_payload_discover(uint8_t *payload_ptr, uint16_t length, mac_payload_IE_t * payload_ie)
+{
+    mac_payload_IE_t ie_element;
+    while (length >= 2) {
+        mac_ie_payload_parse(&ie_element, payload_ptr);
+        if (payload_ie->id == ie_element.id) {
+            payload_ie->content_ptr = ie_element.content_ptr;
+            payload_ie->length = ie_element.length;
+            return ie_element.length;
+        }
+
+        length -= ie_element.length + 2;
+
+        payload_ptr += ie_element.length + 2;
+    }
+    return 0;
+}
+
+uint16_t mac_ie_nested_discover(uint8_t *payload_ptr, uint16_t length, mac_nested_payload_IE_t * nested_ie)
+{
+    mac_nested_payload_IE_t ie_element;
+    while (length >= 2) {
+        mac_ie_nested_id_parse(&ie_element, payload_ptr);
+
+        if (length < ie_element.length + 2) {
+            return 0;
+        }
+
+        if (nested_ie->id == ie_element.id && nested_ie->type_long == ie_element.type_long) {
+            nested_ie->content_ptr = ie_element.content_ptr;
+            nested_ie->length = ie_element.length;
+            return ie_element.length;
+        }
+
+        length -= ie_element.length + 2;
+
+        payload_ptr += ie_element.length + 2;
+    }
+    return 0;
+}
+
+uint8_t mac_ie_header_discover(uint8_t *header_ptr, uint16_t length, mac_header_IE_t * header_ie)
+{
+    mac_header_IE_t ie_element;
+    while (length >= 2) {
+        mac_ie_header_parse(&ie_element, header_ptr);
+        if (header_ie->id == ie_element.id) {
+            header_ie->content_ptr = ie_element.content_ptr;
+            header_ie->length = ie_element.length;
+            return ie_element.length;
+        }
+
+        length -= ie_element.length + 2;
+
+        header_ptr += ie_element.length + 2;
+    }
+    return 0;
+}
+
+uint8_t mac_ie_header_sub_id_discover(uint8_t *header_ptr, uint16_t length, mac_header_IE_t * header_ie, uint8_t sub_id)
+{
+    mac_header_IE_t ie_element;
+    uint8_t *sub_id_ptr;
+    while (length > 2) {
+        mac_ie_header_parse(&ie_element, header_ptr);
+        sub_id_ptr = ie_element.content_ptr;
+        if (ie_element.length && header_ie->id == ie_element.id && *sub_id_ptr == sub_id) {
+            sub_id_ptr++;
+            ie_element.length--;
+            header_ie->content_ptr = sub_id_ptr;
+            header_ie->length = ie_element.length;
+            return ie_element.length;
+        }
+
+        length -= ie_element.length + 2;
+
+        header_ptr += ie_element.length + 2;
+    }
+    return 0;
+}
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_ie_lib.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_ie_lib.h
new file mode 100644
index 0000000000..dbdf8c2eea
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_ie_lib.h
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef MAC_IE_LIB_H_
+#define MAC_IE_LIB_H_
+
+struct mac_payload_IE_s;
+struct mac_payload_IE_s;
+
+/**
+ * @brief struct mac_nested_payload_IE_t Mac Nested IE Payload information element structure for parsing or write operation
+ */
+typedef struct mac_nested_payload_IE_s {
+    uint8_t *content_ptr;   /**< Content data */
+    uint16_t length;        /**< Element length 0- 2047 when type_long true and for short 0- 255*/
+    unsigned id:7;          /**< Group ID 4-bit for long and 7 bit for short type */
+    bool type_long:1;        /**< True when Nested IE long format and false for short */
+} mac_nested_payload_IE_t;
+
+/** IE header element generic header write */
+uint8_t *mac_ie_header_base_write(uint8_t *ptr, uint8_t type, uint16_t length);
+
+/** IE payload element generic header write */
+uint8_t *mac_ie_payload_base_write(uint8_t *ptr, uint8_t type, uint16_t length);
+
+/** Nested IE long header write */
+uint8_t *mac_ie_nested_ie_long_base_write(uint8_t *ptr, uint8_t sub_id, uint16_t length);
+
+/** Nested IE short header write */
+uint8_t *mac_ie_nested_ie_short_base_write(uint8_t *ptr, uint8_t sub_id, uint16_t length);
+
+/** Payload IE discover for spesific group ID */
+uint16_t mac_ie_payload_discover(uint8_t *payload_ptr, uint16_t length, struct mac_payload_IE_s * payload_ie);
+
+/** Nested IE element discover inside parsed payload element */
+uint16_t mac_ie_nested_discover(uint8_t *payload_ptr, uint16_t length, mac_nested_payload_IE_t * nested_ie);
+
+/** Header IE elemnt discover */
+uint8_t mac_ie_header_discover(uint8_t *header_ptr, uint16_t length, struct mac_header_IE_s * header_ie);
+
+/** Header IE elemnt discover with sub id */
+uint8_t mac_ie_header_sub_id_discover(uint8_t *header_ptr, uint16_t length, mac_header_IE_t * header_ie, uint8_t sub_id);
+
+#endif /* MAC_IE_LIB_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_pairwise_key.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_pairwise_key.c
index bb37e137fd..96bf1f79a0 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_pairwise_key.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_pairwise_key.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -29,6 +29,7 @@
 #include "6LoWPAN/MAC/mac_pairwise_key.h"
 #include "MLE/mle.h"
 #include "NWK_INTERFACE/Include/protocol.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 
 #define TRACE_GROUP "mPKe"
 
@@ -171,12 +172,12 @@ static mac_pairwise_interface_entry_t *mac_pairwise_key_main_class(uint8_t key_l
 
 static void mac_pairwise_key_list_free(protocol_interface_info_entry_t *interface, mac_pairwise_interface_entry_t *main_list) {
     //Delete mle entries & Keys
-    mle_neigh_table_entry_t *cur_entry;
+    mac_neighbor_table_entry_t *cur_entry;
     mac_pairwise_key_info_t *cur = main_list->mac_pairwise_key_table;
     for (uint8_t i = 0; i< main_list->key_table_size; i++) {
-        cur_entry = mle_class_get_by_device_attribute_id(interface->id, cur->device_descriptor_attribute);
+        cur_entry = mac_neighbor_table_attribute_discover(mac_neighbor_info(interface), cur->device_descriptor_attribute);
         if (cur_entry) {
-            mle_class_remove_entry(interface->id, cur_entry);
+            mac_neighbor_table_neighbor_remove(mac_neighbor_info(interface), cur_entry);
         }
         mac_helper_security_pairwisekey_set(interface, NULL, NULL, cur->key_decriptor_attribute);
     }
@@ -266,30 +267,31 @@ int mac_pairwise_key_add(int8_t interface_id, uint32_t valid_life_time, const ui
     }
 
     //Allocate mle entry
-    mle_neigh_table_entry_t *mle_entry = mle_class_get_entry_by_mac64(interface_id, 0, eui64, true, &new_entry_created);
-    if (!mle_entry) {
+    mac_neighbor_table_entry_t *mac_entry = mac_neighbor_entry_get_by_mac64(mac_neighbor_info(interface), eui64, true, &new_entry_created);
+    if (!mac_entry) {
         return -1;
     }
-    mle_entry->thread_commission = true;
-    mle_entry->short_adr = 0xffff;
-    mle_entry->ttl = 20;
+
+    mac_neighbor_table_trusted_neighbor(mac_neighbor_info(interface), mac_entry, true);
+    mac_entry->mac16 = 0xffff;
 
     //Allocate key description
 
-    mac_pairwise_key_info_t *key_desc = mac_pairwise_key_info_get(main_list, mle_entry->attribute_index);
+    mac_pairwise_key_info_t *key_desc = mac_pairwise_key_info_get(main_list, mac_entry->index);
 
     if (!key_desc) {
-        mle_class_remove_entry(interface_id, mle_entry);
+        mac_neighbor_table_neighbor_remove(mac_neighbor_info(interface), mac_entry);
         return -1;
     }
 
-    //Set device descriptor
-    mac_helper_devicetable_set(mle_entry, interface, 0, interface->mac_parameters->mac_default_key_index, new_entry_created);
+    mlme_device_descriptor_t device_desc;
+    mac_helper_device_description_write(interface, &device_desc, mac_entry->mac64, mac_entry->mac16,0, false);
+    mac_helper_devicetable_set(&device_desc, interface,mac_entry->index, interface->mac_parameters->mac_default_key_index, new_entry_created);
 
     //set key descriptor
     if (mac_helper_security_pairwisekey_set(interface, key, eui64, key_desc->key_decriptor_attribute) != 0) {
         main_list->key_table_size--;
-        mle_class_remove_entry(interface_id, mle_entry);
+        mac_neighbor_table_neighbor_remove(mac_neighbor_info(interface), mac_entry);
         return -1;
     }
 
@@ -309,23 +311,23 @@ int mac_pairwise_key_del(int8_t interface_id, const uint8_t eui64[static 8])
     if (!main_list) {
         return -1;
     }
-    //Get from mle
-    mle_neigh_table_entry_t *mle_entry = mle_class_get_entry_by_mac64(interface_id, 0, eui64, true, NULL);
-    if (!mle_entry) {
+    //Get from mac
+    mac_neighbor_table_entry_t *mac_entry = mac_neighbor_entry_get_by_mac64(mac_neighbor_info(interface), eui64, true, NULL);
+    if (!mac_entry) {
         return -1;
     }
 
     //discover by mle entry attribute
     uint8_t key_attribute;
 
-    if (!mac_pairwise_key_info_delete(main_list, mle_entry->attribute_index, &key_attribute)) {
+    if (!mac_pairwise_key_info_delete(main_list, mac_entry->index, &key_attribute)) {
         return -1;
     }
 
     //kill Entry & overwrite key
     mac_helper_security_pairwisekey_set(interface, NULL, NULL, key_attribute);
 
-    mle_class_remove_entry(interface_id, mle_entry);
+    mac_neighbor_table_neighbor_remove(mac_neighbor_info(interface), mac_entry);
 
     return 0;
 }
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_response_handler.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_response_handler.c
index 9a24f36bfe..0c21681c2f 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_response_handler.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mac_response_handler.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -31,9 +31,9 @@
 #include "mac_mcps.h"
 #include "6LoWPAN/MAC/mac_data_poll.h"
 #include "6LoWPAN/MAC/mac_response_handler.h"
+#include "6LoWPAN/MAC/mpx_api.h"
 #include "6LoWPAN/lowpan_adaptation_interface.h"
-
-static bool mac_data_is_broadcast_addr(const sockaddr_t *addr);
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 
 #define TRACE_GROUP "MRsH"
 
@@ -49,15 +49,16 @@ static void mac_mlme_device_table_confirmation_handle(protocol_interface_info_en
 
     if (confirmation->status == MLME_SUCCESS) {
         //GET ME table by extended mac64 address
-        mle_neigh_table_entry_t * entry = mle_class_get_by_link_address(info_entry->id, descpription->ExtAddress, ADDR_802_15_4_LONG);
+        mac_neighbor_table_entry_t *entry = mac_neighbor_table_address_discover(mac_neighbor_info(info_entry), descpription->ExtAddress, ADDR_802_15_4_LONG);
+
         if (!entry) {
             return;
         }
 
-        if (entry->short_adr != descpription->ShortAddress) {
+        if (entry->mac16 != descpription->ShortAddress) {
             //Refresh Short ADDRESS
             mlme_set_t set_request;
-            descpription->ShortAddress = entry->short_adr;
+            descpription->ShortAddress = entry->mac16;
 
             //CALL MLME-SET
             set_request.attr = macDeviceTable;
@@ -107,62 +108,10 @@ void mcps_data_confirm_handler( const mac_api_t* api, const mcps_data_conf_t *da
     lowpan_adaptation_interface_tx_confirm(info_entry, data);
 }
 
-static bool mcps_data_indication_neighbor_validate(int8_t interface_id, const sockaddr_t *addr)
-{
-    /* If MLE is enabled, we will talk if we have an MLE association */
-    mle_neigh_table_entry_t *mle_entry = mle_class_get_by_link_address(interface_id, addr->address + 2, addr->addr_type);
-    if (mle_entry && (mle_entry->handshakeReady ||  mle_entry->thread_commission)) {
-        return true;
-    }
-
-    /* Otherwise, we don't know them */
-    return false;
-
-}
-
 void mcps_data_indication_handler( const mac_api_t* api, const mcps_data_ind_t *data_ind )
 {
     protocol_interface_info_entry_t *info_entry = protocol_stack_interface_info_get_by_id(api->parent_id);
-    buffer_t *buf = buffer_get(data_ind->msduLength);
-    if (!buf || !info_entry) {
-        return;
-    }
-    uint8_t *ptr;
-    buffer_data_add(buf, data_ind->msdu_ptr, data_ind->msduLength);
-    //tr_debug("MAC Paylod size %u %s",data_ind->msduLength, trace_array(data_ind->msdu_ptr, 8));
-    buf->options.lqi = data_ind->mpduLinkQuality;
-    buf->options.dbm = data_ind->signal_dbm;
-    buf->src_sa.addr_type = (addrtype_t)data_ind->SrcAddrMode;
-    ptr = common_write_16_bit(data_ind->SrcPANId, buf->src_sa.address);
-    memcpy(ptr, data_ind->SrcAddr, 8);
-    buf->dst_sa.addr_type = (addrtype_t)data_ind->DstAddrMode;
-    ptr = common_write_16_bit(data_ind->DstPANId, buf->dst_sa.address);
-    memcpy(ptr, data_ind->DstAddr, 8);
-    //Set Link spesific stuff to seperately
-    buf->link_specific.ieee802_15_4.srcPanId = data_ind->SrcPANId;
-    buf->link_specific.ieee802_15_4.dstPanId = data_ind->DstPANId;
-
-    if (mac_data_is_broadcast_addr(&buf->dst_sa)) {
-        buf->options.ll_broadcast_rx = true;
-    }
-    buf->interface = info_entry;
-    if (data_ind->Key.SecurityLevel) {
-        buf->link_specific.ieee802_15_4.fc_security = true;
-
-        if (info_entry->mac_security_key_usage_update_cb) {
-            info_entry->mac_security_key_usage_update_cb(info_entry, &data_ind->Key);
-        }
-    } else {
-        buf->link_specific.ieee802_15_4.fc_security = false;
-        if (mac_helper_default_security_level_get(info_entry) ||
-                !mcps_data_indication_neighbor_validate(info_entry->id, &buf->src_sa)) {
-            //SET By Pass
-            buf->options.ll_security_bypass_rx = true;
-        }
-    }
-
-    buf->info = (buffer_info_t)(B_TO_IPV6_TXRX | B_FROM_MAC | B_DIR_UP);
-    protocol_push(buf);
+    lowpan_adaptation_interface_data_ind(info_entry, data_ind);
 }
 
 void mcps_purge_confirm_handler( const mac_api_t* api, mcps_purge_conf_t *data )
@@ -306,9 +255,3 @@ void mlme_indication_handler( const mac_api_t* api, mlme_primitive id, const voi
     }
 }
 
-bool mac_data_is_broadcast_addr(const sockaddr_t *addr)
-{
-    return (addr->addr_type == ADDR_802_15_4_SHORT) &&
-           (addr->address[2] == 0xFF && addr->address[3] == 0xFF);
-}
-
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mpx_api.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mpx_api.h
new file mode 100644
index 0000000000..a61e3e0053
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/MAC/mpx_api.h
@@ -0,0 +1,95 @@
+/*
+ * Copyright (c) 2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef MPX_API_H_
+#define MPX_API_H_
+
+struct mcps_data_req_s;
+struct mcps_data_conf_s;
+struct mcps_data_ind_s;
+struct mcps_purge_s;
+
+typedef struct mpx_api_s mpx_api_t;
+
+/**
+ * @brief mpx_data_request MPX_DATA request with user ID
+ * @param api API to handle the request
+ * @param data MCPS-DATA.request specific values
+ * @param user_id MPX user ID
+ *
+ */
+typedef void mpx_data_request(const mpx_api_t *api, const struct mcps_data_req_s *data, uint16_t user_id);
+
+/**
+ * @brief mpx_data_queue_clean clean MPX user data
+ * @param api API to handle the request
+ * @param purge MCPS-purge request
+ * @param user_id MPX user ID
+ *
+ */
+typedef void mpx_data_purge_request(const mpx_api_t *api, struct mcps_purge_s *purge, uint16_t user_id);
+
+/**
+ * @brief mpx_data_confirm MPX-DATA confirm is called as a response to MPX-DATA request
+ * @param api The API which handled the response
+ * @param data MCPS-DATA.confirm specific values
+ * @param user_id MPX user ID
+ */
+typedef void mpx_data_confirm(const mpx_api_t* api, const struct mcps_data_conf_s *data);
+
+/**
+ * @brief mpx_data_indication MPX-DATA confirm is called as a response to MPX-DATA request
+ * @param api The API which handled the response
+ * @param data MCPS-DATA.indication specific values
+ * @param user_id MPX user ID
+ */
+typedef void mpx_data_indication(const mpx_api_t* api, const struct mcps_data_ind_s *data);
+
+/**
+ * @brief mpx_header_size_get Function for request MPX user head room size
+ * @param api The API which handled the response
+ * @param user_id MPX user ID
+ *
+ * @return >0 Head room size in bytes
+ * @return 0 When Unknown User Id
+ */
+typedef uint16_t mpx_header_size_get(const mpx_api_t * api, uint16_t user_id);
+
+/**
+ * @brief mpx_data_cb_register MPX-DATA confirm cb register by user
+ * @param api The API which handled the response
+ * @param confirm_cb MPX Data Confirm call back
+ * @param indication_cb MPX Data indication
+ * @param user_id MPX user ID
+ *
+ * @return 0 register OK
+ * @return -1 Unknown User ID
+ */
+typedef int8_t mpx_data_cb_register(const mpx_api_t* api, mpx_data_confirm *confirm_cb, mpx_data_indication *indication_cb, uint16_t user_id);
+
+/**
+ * \brief Struct mpx_api_s defines functions for MPX user for register call backs and send data.
+ */
+struct mpx_api_s {
+    mpx_data_request * mpx_data_request;            /**< MPX data request. */
+    mpx_data_purge_request *mpx_data_purge;            /**< MPX data Purge. */
+    mpx_header_size_get * mpx_headroom_size_get;    /**< MPX headroom size get in bytes. */
+    mpx_data_cb_register * mpx_user_registration;   /**< MPX User cb registration must be call before enable to send or RX data*/
+};
+
+
+#endif /* MPX_API_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ND/nd_router_object.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ND/nd_router_object.c
index d52645a849..35f9230420 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ND/nd_router_object.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ND/nd_router_object.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2017, Arm Limited and affiliates.
+ * Copyright (c) 2013-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -41,6 +41,8 @@
 #include "BorderRouter/border_router.h"
 #include "Service_Libs/pan_blacklist/pan_blacklist_api.h"
 #include "6LoWPAN/MAC/mac_data_poll.h"
+#include "6LoWPAN/ws/ws_common.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 
 #define TRACE_GROUP "loND"
 
@@ -845,17 +847,18 @@ static void nd_update_registration(protocol_interface_info_entry_t *cur_interfac
         ipv6_neighbour_set_state(&cur_interface->ipv6_neighbour_cache, neigh, IP_NEIGHBOUR_STALE);
         /* Register with 2 seconds off the lifetime - don't want the NCE to expire before the route */
         ipv6_route_add(neigh->ip_address, 128, cur_interface->id, NULL, ROUTE_ARO, neigh->lifetime - 2, 0);
-#ifndef NO_MLE
+
         /* We need to know peer is a host before publishing - this needs MLE. Not yet established
          * what to do without MLE - might need special external/non-external prioritisation at root.
          * This "publish for RFD" rule comes from ZigBee IP.
          */
-        mle_neigh_table_entry_t *mle_entry = mle_class_get_by_link_address(cur_interface->id, ipv6_neighbour_eui64(&cur_interface->ipv6_neighbour_cache, neigh), ADDR_802_15_4_LONG);
-        if (mle_entry && ((mle_entry->mode & MLE_DEV_MASK) == MLE_RFD_DEV)) {
+        mac_neighbor_table_entry_t *entry = mac_neighbor_table_address_discover(mac_neighbor_info(cur_interface), ipv6_neighbour_eui64(&cur_interface->ipv6_neighbour_cache, neigh), ADDR_802_15_4_LONG);
+
+        if (entry && !entry->ffd_device) {
             rpl_control_publish_host_address(protocol_6lowpan_rpl_domain, neigh->ip_address, neigh->lifetime);
         }
         protocol_6lowpan_neighbor_address_state_synch(cur_interface, aro->eui64, neigh->ip_address + 8);
-#endif
+
     } else {
         /* Um, no - can't transmit response if we remove NCE now! */
         //ipv6_neighbour_entry_remove(&cur_interface->ipv6_neighbour_cache, neigh);
@@ -863,9 +866,7 @@ static void nd_update_registration(protocol_interface_info_entry_t *cur_interfac
         neigh->lifetime = 2;
         ipv6_neighbour_set_state(&cur_interface->ipv6_neighbour_cache, neigh, IP_NEIGHBOUR_STALE);
         ipv6_route_add(neigh->ip_address, 128, cur_interface->id, NULL, ROUTE_ARO, 4, 0);
-#ifndef NO_MLE
         rpl_control_unpublish_address(protocol_6lowpan_rpl_domain, neigh->ip_address);
-#endif
     }
 }
 
@@ -967,7 +968,13 @@ bool nd_ns_aro_handler(protocol_interface_info_entry_t *cur_interface, const uin
     /* Set the LL address, ensure it's marked STALE */
     ipv6_neighbour_entry_update_unsolicited(&cur_interface->ipv6_neighbour_cache, neigh, ll_addr.addr_type, ll_addr.address);
     ipv6_neighbour_set_state(&cur_interface->ipv6_neighbour_cache, neigh, IP_NEIGHBOUR_STALE);
-
+    if (ws_info(cur_interface)) {
+        aro_out->status = ARO_SUCCESS;
+        aro_out->present = true;
+        // Todo: this might not be needed...
+        nd_update_registration(cur_interface, neigh, aro_out);
+        return true;
+    }
     if (cur_interface->bootsrap_mode == ARM_NWK_BOOTSRAP_MODE_6LoWPAN_BORDER_ROUTER || nd_params.multihop_dad == false) {
         if (cur_interface->bootsrap_mode == ARM_NWK_BOOTSRAP_MODE_6LoWPAN_BORDER_ROUTER) {
             whiteboard_entry_t *wb;
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_address_registration_client.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_address_registration_client.h
index 4713401f69..d43f964d94 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_address_registration_client.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_address_registration_client.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017, Arm Limited and affiliates.
+ * Copyright (c) 2017-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -30,10 +30,23 @@
 #ifndef THREAD_ADDRESS_REGISTRATION_CLIENT_H_
 #define THREAD_ADDRESS_REGISTRATION_CLIENT_H_
 
+
+#ifdef HAVE_THREAD_V2
+
+void thread_address_registration_init(void);
+void thread_address_registration_deinit(void);
+
+void thread_address_registration_timer_set(protocol_interface_info_entry_t *interface, uint16_t dua_delay_seconds, uint16_t mlr_refresh_seconds);
+void thread_address_registration_timer(protocol_interface_info_entry_t *interface, uint16_t seconds);
+#else
+
 #define thread_address_registration_init(void)
 #define thread_address_registration_deinit(void)
 
-#define thread_address_registration_timer_set(interface, seconds);
+#define thread_address_registration_timer_set(interface, dua_delay_seconds, mlr_refresh_seconds);
 #define thread_address_registration_timer(interface, seconds);
 
+#endif
+
+
 #endif /* THREAD_ADDRESS_REGISTRATION_CLIENT_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api.c
index 0e0fc04d11..6c0b09a2c0 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017, Arm Limited and affiliates.
+ * Copyright (c) 2017-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -943,7 +943,7 @@ 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])
+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) {
@@ -955,18 +955,18 @@ static int thread_bbr_na_send(int8_t interface_id, const uint8_t target[static 1
     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;
+
+int thread_bbr_nd_entry_add (int8_t interface_id, const uint8_t *addr_data_ptr,  uint32_t lifetime, void *info)
+{
     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");
+        tr_err("bbr out of resources");
         return -2;
     }
     // send NA
@@ -975,14 +975,44 @@ int thread_bbr_nd_entry_add (int8_t interface_id, const uint8_t *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
+int thread_bbr_dua_entry_add (int8_t interface_id, const uint8_t *addr_data_ptr,  uint32_t lifetime, const uint8_t *mleid_ptr)
+{
+    thread_bbr_t *this = thread_bbr_find_by_interface(interface_id);
+    if (!this || this->backbone_interface_id < 0) {
         return -1;
     }
-    //TODO get information to route to parameters eq mleid, timeout
+    thread_pbbr_dua_info_t *map = ns_dyn_mem_alloc(sizeof(thread_pbbr_dua_info_t));
+    if (!map) {
+        goto error;
+    }
+    memcpy(map->mleid_ptr, mleid_ptr, 8);
+    map->last_contact_time = protocol_core_monotonic_time;
+
+    // We are using route info field to store BBR MLEID map
+    ipv6_route_t *route = ipv6_route_add_with_info(addr_data_ptr, 128, interface_id, NULL, ROUTE_THREAD_PROXIED_DUA_HOST, map, 0, lifetime, 0);
+    if (!route) {
+        // Direct route to host allows ND proxying to work
+        ns_dyn_mem_free(map);
+        goto error;
+    }
+    // Route info autofreed
+    route->info_autofree = true;
+    // send NA
+    thread_bbr_na_send(this->backbone_interface_id, addr_data_ptr);
+
     return 0;
+error:
+    tr_err("out of resources");
+    return -2;
+}
+
+struct ipv6_route *thread_bbr_dua_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_DUA_HOST ) {
+        //Not found
+        return NULL;
+    }
+    return route;
 }
 
 int thread_bbr_proxy_state_update(int8_t caller_interface_id , int8_t handler_interface_id, bool status)
@@ -1090,6 +1120,17 @@ return 0;
 #endif // HAVE_THREAD_BORDER_ROUTER
 }
 
+int thread_bbr_prefix_set(int8_t interface_id, uint8_t *prefix)
+{
+    (void) interface_id;
+    (void) prefix;
+#ifdef HAVE_THREAD_BORDER_ROUTER
+    return thread_extension_bbr_prefix_set(interface_id, prefix);
+#else
+    return -1;
+#endif // HAVE_THREAD_BORDER_ROUTER
+}
+
 int thread_bbr_validation_interface_address_set(int8_t interface_id, const uint8_t *addr_ptr, uint16_t port)
 {
     (void) interface_id;
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api_internal.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api_internal.h
index 8930e0190f..dd05ca7525 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api_internal.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bbr_api_internal.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -36,6 +36,8 @@
 
 #include "net_interface.h"
 #ifdef HAVE_THREAD_ROUTER
+struct ipv6_route;
+
 /**
  * \brief Initialize Thread Commissioner relay for BBR and Routers
  *
@@ -98,23 +100,39 @@ 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
+ * \param interface_id addr_data_ptr lifetime info
  */
-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);
+int thread_bbr_nd_entry_add(int8_t interface_id, const uint8_t *addr_data_ptr, uint32_t lifetime, void *info);
 
 /**
- * \brief Find if bbr has nd entry
+ * \brief Add new dua entry to bbr
+ *
+ * \param interface_id addr_data_ptr lifetime info mleid_ptr
+ */
+int thread_bbr_dua_entry_add (int8_t interface_id, const uint8_t *addr_data_ptr, uint32_t lifetime, const uint8_t *mleid_ptr);
+
+/**
+ * \brief Send na
+ *
+ * \param interface_id addr_data_ptr lifetime info mleid_ptr
+ */
+int thread_bbr_na_send(int8_t interface_id, const uint8_t target[static 16]);
+
+/**
+ * \brief Find if bbr has dua entry
  *
  * \param interface_id addr_data_ptr
  */
-int thread_bbr_nd_entry_find(int8_t interface_id, const uint8_t *addr_data_ptr);
+struct ipv6_route *thread_bbr_dua_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)
+#define thread_bbr_nd_entry_add(interface_id, addr_data_ptr, lifetime, info) (0)
+#define thread_bbr_dua_entry_add(interface_id, addr_data_ptr, lifetime, mleid_ptr) (0)
+#define thread_bbr_dua_entry_find(interface_id, addr_data_ptr) (NULL)
+#define thread_bbr_na_send(interface_id, target) (0)
 #endif //HAVE_THREAD_BORDER_ROUTER
 
 
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bootstrap.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bootstrap.c
index 3f746d20d1..0e26b643ed 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bootstrap.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bootstrap.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -59,6 +59,7 @@
 #include "6LoWPAN/Thread/thread_router_bootstrap.h"
 #include "6LoWPAN/Thread/thread_management_internal.h"
 #include "6LoWPAN/Thread/thread_management_server.h"
+#include "6LoWPAN/Thread/thread_neighbor_class.h"
 #include "6LoWPAN/Thread/thread_network_data_lib.h"
 #include "6LoWPAN/Thread/thread_network_synch.h"
 #include "6LoWPAN/Thread/thread_joiner_application.h"
@@ -90,9 +91,14 @@
 #include "thread_meshcop_lib.h"
 #include "multicast_api.h"
 #include "mlme.h"
+#include "Service_Libs/etx/etx.h"
 #include "Service_Libs/nd_proxy/nd_proxy.h"
 #include "Service_Libs/blacklist/blacklist.h"
+#include "Service_Libs/mle_service/mle_service_api.h"
 #include "6LoWPAN/MAC/mac_data_poll.h"
+#include "6LoWPAN/lowpan_adaptation_interface.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
+#include "platform/topo_trace.h"
 
 #define TRACE_GROUP "thbs"
 
@@ -111,27 +117,46 @@ static void thread_bootstrap_generate_leader_and_link(protocol_interface_info_en
 static int thread_bootstrap_attach_start(int8_t interface_id, thread_bootsrap_state_type_e state);
 static void thread_bootsrap_network_discovery_failure(int8_t interface_id);
 
-static void thread_neighbor_remove(int8_t interface_id, mle_neigh_table_entry_t *cur);
+static void thread_neighbor_remove(mac_neighbor_table_entry_t *entry_ptr, void *user_data);
 static void thread_bootsrap_network_join_start(struct protocol_interface_info_entry *cur_interface, discovery_response_list_t *nwk_info);
 
 
 
-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);
-    if (!cur || !(cur->lowpan_info & INTERFACE_NWK_ACTIVE)) {
+static void thread_neighbor_remove(mac_neighbor_table_entry_t *entry_ptr, void *user_data)
+{
+    protocol_interface_info_entry_t *cur = user_data;
+    lowpan_adaptation_remove_free_indirect_table(cur, entry_ptr);
+
+    thread_reset_neighbour_info(cur, entry_ptr);
+    //Removes ETX neighbor
+    etx_neighbor_remove(cur->id, entry_ptr->index);
+    //Remove MLE frame counter info
+    mle_service_frame_counter_entry_delete(cur->id, entry_ptr->index);
+}
+
+static bool thread_neighbor_entry_nud_notify(mac_neighbor_table_entry_t *entry_ptr, void *user_data)
+{
+
+    // Sleepy host
+    protocol_interface_info_entry_t *cur_interface = user_data;
+
+    if (thread_am_router(cur_interface)) {
+        return false; //Never do Keep alive with any one
+    }
+
+    if (entry_ptr->link_role != PRIORITY_PARENT_NEIGHBOUR) {
+        return false; //Do not never challenge than priority parent
+    }
+
+    if (cur_interface->lowpan_info & INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE) {
+        return false; //Sleepy end device should not never challenge
+    }
+
+    if (entry_ptr->lifetime  > MLE_TABLE_CHALLENGE_TIMER) {
         return false;
     }
 
-    return true;
-}
-
-static void thread_neighbor_remove(int8_t interface_id, mle_neigh_table_entry_t *cur)
-{
-    protocol_interface_info_entry_t *cur_interface = protocol_stack_interface_info_get_by_id(interface_id);
-    if (!cur_interface) {
-        return;
-    }
-    thread_reset_neighbour_info(cur_interface, cur);
+    return thread_host_bootstrap_child_update(cur_interface, entry_ptr->mac64);
 }
 
 int8_t thread_mle_class_init(int8_t interface_id)
@@ -151,11 +176,29 @@ int8_t thread_mle_class_init(int8_t interface_id)
         return -1;
     }
 
-    if (mle_class_init(interface_id, buffer.device_decription_table_size - 1, &thread_neighbor_remove, &thread_host_bootstrap_child_update, &thread_interface_is_active) != 0) {
+    thread_neighbor_class_delete(&cur->thread_info->neighbor_class);
+
+    if (!thread_neighbor_class_create(&cur->thread_info->neighbor_class, buffer.device_decription_table_size - 1)) {
         return -1;
     }
 
-    mle_class_router_challenge(interface_id, NULL);
+    if (!mac_neighbor_info(cur) ) {
+        mac_neighbor_info(cur) = mac_neighbor_table_create(buffer.device_decription_table_size - 1, thread_neighbor_remove
+                                                                            , thread_neighbor_entry_nud_notify, cur);
+        if (!mac_neighbor_info(cur)) {
+            return -1;
+        }
+    }
+
+    if (mle_service_frame_counter_table_allocate(interface_id, buffer.device_decription_table_size - 1)) {
+        return -1;
+    }
+
+    if (!etx_storage_list_allocate(cur->id, buffer.device_decription_table_size - 1)) {
+        return -1;
+    }
+
+    lowpan_adaptation_interface_etx_update_enable(cur->id);
 
     //Defined well know neighbour for discovery
 
@@ -232,7 +275,7 @@ uint8_t thread_calculate_link_margin(int8_t dbm, uint8_t compLinkMarginFromParen
     return newLqi;
 }
 
-bool thread_check_is_this_my_parent(protocol_interface_info_entry_t *cur, mle_neigh_table_entry_t *entry_temp)
+bool thread_check_is_this_my_parent(protocol_interface_info_entry_t *cur, mac_neighbor_table_entry_t *entry_temp)
 {
     if (entry_temp && thread_info(cur)->thread_endnode_parent) {
         if(memcmp(entry_temp->mac64, thread_info(cur)->thread_endnode_parent->mac64, 8) == 0) {
@@ -245,45 +288,31 @@ bool thread_check_is_this_my_parent(protocol_interface_info_entry_t *cur, mle_ne
 bool thread_bootstrap_request_network_data(protocol_interface_info_entry_t *cur, thread_leader_data_t *leaderData, uint16_t short_address)
 {
     bool requestNetworkdata = false;
-    thread_leader_data_t *leadeInfo = thread_info(cur)->thread_leader_data;
+    thread_leader_data_t *leaderInfo = thread_info(cur)->thread_leader_data;
 
     if (thread_info(cur)->thread_endnode_parent->shortAddress != short_address) {
         return false;
     }
 
-    if (thread_info(cur)->thread_leader_data->partitionId != leaderData->partitionId) {
+    if (!thread_partition_match(cur, leaderData)) {
         tr_debug("Learn new Network Data");
         requestNetworkdata = true;
-        thread_info(cur)->thread_leader_data->dataVersion = leaderData->dataVersion - 1;
-        thread_info(cur)->thread_leader_data->stableDataVersion = leaderData->stableDataVersion - 1;
+        thread_partition_info_update(cur, leaderData);
     }
-    else if (common_serial_number_greater_8(leaderData->dataVersion, leadeInfo->dataVersion)) {
+    else if (common_serial_number_greater_8(leaderData->dataVersion, leaderInfo->dataVersion)) {
         requestNetworkdata = true;
 
-    } else if (common_serial_number_greater_8(leaderData->stableDataVersion, leadeInfo->stableDataVersion)) {
+    } else if (common_serial_number_greater_8(leaderData->stableDataVersion, leaderInfo->stableDataVersion)) {
         requestNetworkdata = true;
 
     }
 
-    // Version number is updated when new network data is learned to avoid synchronization problems
-    thread_info(cur)->thread_leader_data->leaderRouterId = leaderData->leaderRouterId;
-    thread_info(cur)->thread_leader_data->partitionId = leaderData->partitionId;
     if (requestNetworkdata) {
         thread_bootstrap_parent_network_data_request(cur, true);
     }
     return true;
 }
 
-bool thread_instance_id_matches(protocol_interface_info_entry_t *cur, thread_leader_data_t *leaderData)
-{
-    if (thread_info(cur)->thread_leader_data) {
-        if (thread_info(cur)->thread_leader_data->partitionId == leaderData->partitionId) {
-            return true;
-        }
-    }
-    return false;
-}
-
 static int thread_router_check_previous_partition_info(protocol_interface_info_entry_t *cur, thread_leader_data_t *leaderData, mle_tlv_info_t *routeTlv)
 {
     if (!routeTlv || !routeTlv->dataPtr || !routeTlv->tlvLen || !leaderData) {
@@ -386,8 +415,7 @@ 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) ||
-        (thread_info(cur)->thread_leader_data->weighting != leaderData->weighting)) {
+    if (!thread_partition_match(cur, leaderData)) {
         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);
@@ -472,11 +500,9 @@ void thread_end_device_mode_set(protocol_interface_info_entry_t *cur, bool sleep
 {
     if (sleepy) {
         cur->lowpan_info |= INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE;
-        mle_class_mode_set(cur->id, MLE_CLASS_SLEEPY_END_DEVICE);
         mac_helper_pib_boolean_set(cur, macRxOnWhenIdle, false);
     } else {
         cur->lowpan_info &= ~INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE;
-        mle_class_mode_set(cur->id, MLE_CLASS_END_DEVICE);
         mac_helper_pib_boolean_set(cur, macRxOnWhenIdle, true);
     }
 }
@@ -537,9 +563,6 @@ void thread_set_link_local_address(protocol_interface_info_entry_t *cur)
 
 static int thread_configuration_security_activate(protocol_interface_info_entry_t *cur, link_configuration_s *linkConfiguration)
 {
-    uint8_t key_material[32];
-    uint8_t key_index;
-
     tr_debug("MAC SET Security Mode");
 
     if (!(cur->lowpan_info & INTERFACE_NWK_ACTIVE) || !(cur->configure_flags & INTERFACE_BOOTSTRAP_DEFINED)) {
@@ -554,16 +577,11 @@ static int thread_configuration_security_activate(protocol_interface_info_entry_
     cur->thread_info->masterSecretMaterial.historyKeyValid = false;
     cur->thread_info->masterSecretMaterial.valid_Info = true;
     // Update the guard timer value
-    thread_calculate_key_guard_timer(cur, linkConfiguration, true);
+    thread_key_guard_timer_calculate(cur, linkConfiguration, true);
     //Define KEY's
-    thread_key_get(linkConfiguration->master_key, key_material, linkConfiguration->key_sequence);
-    key_index = THREAD_KEY_INDEX(linkConfiguration->key_sequence);
-    //Set Keys
-    mac_helper_security_default_key_set(cur, &key_material[16], key_index, MAC_KEY_ID_MODE_IDX);
-    //Add Security to MLE service
-    mle_service_security_set_security_key(cur->id, key_material, key_index, true);
-    //Gen also Next Key
-    thread_security_next_key_generate(cur, linkConfiguration->master_key, linkConfiguration->key_sequence);
+    thread_security_prev_key_generate(cur,linkConfiguration->master_key,linkConfiguration->key_sequence);
+    thread_security_key_generate(cur,linkConfiguration->master_key,linkConfiguration->key_sequence);
+    thread_security_next_key_generate(cur,linkConfiguration->master_key,linkConfiguration->key_sequence);
     return 0;
 }
 
@@ -711,9 +729,9 @@ int thread_configuration_mle_disable(protocol_interface_info_entry_t *cur)
     return 0;
 }
 
-int thread_mle_service_register(int8_t interface_id, uint8_t *mac64 )
+static int thread_mle_service_register(protocol_interface_info_entry_t *cur, uint8_t *mac64 )
 {
-    if (mle_service_interface_register(interface_id,thread_mle_parent_discover_receive_cb, mac64,8) != 0) {
+    if (mle_service_interface_register(cur->id, cur, thread_mle_parent_discover_receive_cb, mac64,8) != 0) {
             tr_error("Mle Service init Fail");
             return -1;
     }
@@ -1052,7 +1070,7 @@ void thread_tasklet(arm_event_s *event)
         case THREAD_CHILD_UPDATE:
             tr_debug_extra("Thread SM THREAD_CHILD_UPDATE");
             if (thread_info(cur)->thread_endnode_parent) {
-                thread_host_bootstrap_child_update(cur->id, cur->thread_info->thread_endnode_parent->mac64);
+                thread_host_bootstrap_child_update(cur, cur->thread_info->thread_endnode_parent->mac64);
             }
             break;
         case THREAD_ANNOUNCE_ACTIVE: {
@@ -1166,12 +1184,12 @@ void thread_bootstrap_ready(protocol_interface_info_entry_t *cur)
 
 void thread_neighbor_list_clean(struct protocol_interface_info_entry *cur)
 {
-    mle_neigh_table_list_t *neig_list = mle_class_active_list_get(cur->id);
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
 
-    ns_list_foreach_safe(mle_neigh_table_entry_t, cur_entry, neig_list) {
-        if (!thread_addr_is_equal_or_child(cur->thread_info->routerShortAddress, cur_entry->short_adr)) {
-            tr_debug("Free ID %x", cur_entry->short_adr);
-            mle_class_remove_entry(cur->id, cur_entry);
+    ns_list_foreach_safe(mac_neighbor_table_entry_t, cur_entry, mac_table_list) {
+        if (!thread_addr_is_equal_or_child(cur->thread_info->routerShortAddress, cur_entry->mac16)) {
+            tr_debug("Free ID %x", cur_entry->mac16);
+            mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), cur_entry);
         }
     }
 }
@@ -1454,9 +1472,7 @@ int thread_bootstrap_reset(protocol_interface_info_entry_t *cur)
 
     neighbor_cache_flush(&cur->neigh_cache);
     thread_bootstrap_stop(cur);
-#ifndef NO_MLE
-    mle_class_list_clean(cur->id);
-#endif
+    mac_neighbor_table_neighbor_list_clean(mac_neighbor_info(cur));
     cur->bootsrap_state_machine_cnt = 0;
     mac_helper_free_scan_confirm(&cur->mac_parameters->nwk_scan_params);
     //tr_debug( "--> idle");
@@ -1600,6 +1616,7 @@ void thread_bootstrap_routing_activate(protocol_interface_info_entry_t *cur)
         // FEDs and routers (REEDs) perform their own address resolution
         thread_nd_service_activate(cur->id);
     } else {
+        thread_nd_client_service_activate(cur->id);
         thread_child_set_default_route(cur);
     }
 }
@@ -2177,7 +2194,7 @@ void thread_bootstrap_start_network_discovery(protocol_interface_info_entry_t *c
     scan_request.channel_mask = cur->mac_parameters->nwk_scan_params.stack_chan_list.channel_mask[0];
     scan_request.filter_tlv_data = NULL;
     scan_request.filter_tlv_length = 0;
-    if (thread_discovery_network_scan(cur->id, &scan_request, discover_ready) != 0 ) {
+    if (thread_discovery_network_scan(cur, &scan_request, discover_ready) != 0 ) {
         tr_error("Discovery scan start fail");
     }
 }
@@ -2198,7 +2215,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,thread_joiner_application_random_mac_get(cur->id)) != 0 ||
+            if (thread_mle_service_register(cur,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);
@@ -2257,6 +2274,7 @@ void thread_bootstrap_stop(protocol_interface_info_entry_t *cur)
     ipv6_route_table_remove_info(cur->id, ROUTE_THREAD, NULL);
     ipv6_route_table_remove_info(cur->id, ROUTE_THREAD_BORDER_ROUTER, NULL);
     ipv6_route_table_remove_info(cur->id, ROUTE_THREAD_PROXIED_HOST, NULL);
+    ipv6_route_table_remove_info(cur->id, ROUTE_THREAD_PROXIED_DUA_HOST, NULL);
     thread_leader_service_leader_data_free(cur->thread_info);
     thread_bootstrap_all_nodes_multicast_unregister(cur);
     thread_data_base_init(cur->thread_info, cur->id);
@@ -2672,7 +2690,7 @@ int thread_bootstrap_network_data_activate(protocol_interface_info_entry_t *cur)
     thread_router_bootstrap_anycast_address_register(cur);
     // Update joiner router status
     thread_management_server_joiner_router_init(cur->id);
-    thread_extension_joiner_router_init(cur->id);
+    thread_extension_service_init(cur);
 
     // Update border router relay
     thread_bbr_commissioner_proxy_service_update(cur->id);
@@ -2870,8 +2888,7 @@ void thread_bootstrap_clear_neighbor_entries(protocol_interface_info_entry_t *cu
             ipv6_neighbour_entry_remove(&cur->ipv6_neighbour_cache, neighbour);
         }
     }
-
-    mle_class_list_clean(cur->id);
+    mac_neighbor_table_neighbor_list_clean(mac_neighbor_info(cur));
 }
 
 void thread_bootstrap_dynamic_configuration_save(protocol_interface_info_entry_t *cur)
@@ -2883,10 +2900,10 @@ void thread_bootstrap_dynamic_configuration_save(protocol_interface_info_entry_t
 
     if (thread_i_am_router(cur)) {
         /* Store information of our children to the dynamic storage */
-        mle_neigh_table_list_t *neig_list = mle_class_active_list_get(cur->id);
-        ns_list_foreach_safe(mle_neigh_table_entry_t, entry, neig_list) {
-            if (thread_addr_is_child(mac_helper_mac16_address_get(cur), entry->short_adr)) {
-                thread_dynamic_storage_child_info_store(cur->id, entry);
+        mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
+        ns_list_foreach_safe(mac_neighbor_table_entry_t, entry, mac_table_list) {
+            if (thread_addr_is_child(mac_helper_mac16_address_get(cur), entry->mac16)) {
+                thread_dynamic_storage_child_info_store(cur, entry);
             }
         }
     }
@@ -2910,7 +2927,7 @@ bool thread_bootstrap_link_create_check(protocol_interface_info_entry_t *interfa
         return false;
     }
 
-    if(mle_class_free_entry_count_get(interface->id) < 1) {
+    if(mle_class_free_entry_count_get(interface) < 1) {
         // We dont have room for any new links
         tr_warn("Link ignore no room for addr:%x", short_address);
         return false;
@@ -2928,7 +2945,7 @@ bool thread_bootstrap_link_create_check(protocol_interface_info_entry_t *interfa
         return false;
     }
 
-    if (mle_class_active_neigh_counter(interface->id) < THREAD_REED_AND_END_DEVICE_NEIGHBOR_LINKS + 1) {
+    if (mle_class_active_neigh_counter(interface) < THREAD_REED_AND_END_DEVICE_NEIGHBOR_LINKS + 1) {
         return true;
     }
 
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bootstrap.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bootstrap.h
index 517321704b..212f1ca044 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bootstrap.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_bootstrap.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -52,7 +52,7 @@ struct thread_info_s;
 struct protocol_interface_info_entry;
 struct thread_leader_data_s;
 struct link_configuration;
-struct mle_neigh_table_entry_t;
+struct mac_neighbor_table_entry;
 struct mle_tlv_info_s;
 
 typedef enum {
@@ -103,12 +103,11 @@ uint8_t thread_mode_get_by_interface_ptr(struct protocol_interface_info_entry *c
 void thread_bootstrap_device_synch_finish(protocol_interface_info_entry_t *cur);
 int8_t thread_mle_class_init(int8_t interface_id);
 void thread_general_mle_receive_cb(int8_t interface_id, mle_message_t *mle_msg, mle_security_header_t *security_headers);
-int thread_bootstrap_reset_child_info(protocol_interface_info_entry_t *cur, struct mle_neigh_table_entry_t *child);
 void thread_bootstrap_ready(struct protocol_interface_info_entry *cur);
 int thread_bootstrap_reset(struct protocol_interface_info_entry *cur);
 void thread_neighbor_list_clean(struct protocol_interface_info_entry *cur);
 bool thread_bootstrap_request_network_data(struct protocol_interface_info_entry *cur, struct thread_leader_data_s *leaderData, uint16_t short_address);
-bool thread_check_is_this_my_parent(struct protocol_interface_info_entry *cur, struct mle_neigh_table_entry_t *entry_temp);
+bool thread_check_is_this_my_parent(struct protocol_interface_info_entry *cur, struct mac_neighbor_table_entry *entry_temp);
 void thread_clean_old_16_bit_address_based_addresses(struct protocol_interface_info_entry *cur);
 int8_t thread_bootsrap_event_trig(thread_bootsrap_event_type_e event_type, int8_t Id, arm_library_event_priority_e priority);
 void thread_interface_init(struct protocol_interface_info_entry *cur);
@@ -126,7 +125,6 @@ int thread_parent_discover_start(int8_t interface_id, uint8_t *mac64 );
 
 bool thread_device_synch_timeout(int8_t interface_id, uint16_t msgId, bool usedAllRetries);
 bool thread_link_request_timeout(int8_t interface_id, uint16_t msgId, bool usedAllRetries);
-bool thread_instance_id_matches(struct protocol_interface_info_entry *cur, struct thread_leader_data_s *leaderData);
 int thread_leader_data_validation(struct protocol_interface_info_entry *cur, struct thread_leader_data_s *leaderData, struct mle_tlv_info_s *routeTlv);
 uint8_t thread_calculate_link_margin(int8_t dbm, uint8_t compLinkMarginFromParent);
 uint8_t thread_compute_link_margin(int8_t rssi);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_border_router_api.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_border_router_api.c
index acad9e474c..8ca402db62 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_border_router_api.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_border_router_api.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -297,19 +297,19 @@ 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)
+static void thread_border_router_child_network_data_clean(protocol_interface_info_entry_t *cur, 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)) {
+    if (mac_neighbor_table_address_discover(mac_neighbor_info(cur), 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);
+    thread_management_client_network_data_unregister(cur->id, child_id);
 }
 
 static void thread_border_router_lost_children_nwk_data_validate(protocol_interface_info_entry_t *cur, uint16_t router_short_addr)
@@ -326,7 +326,7 @@ static void thread_border_router_lost_children_nwk_data_validate(protocol_interf
         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);
+                thread_border_router_child_network_data_clean(cur, curRoute->routerID);
             }
         }
 
@@ -334,7 +334,7 @@ static void thread_border_router_lost_children_nwk_data_validate(protocol_interf
         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);
+                thread_border_router_child_network_data_clean(cur, curBR->routerID);
             }
         }
     }
@@ -344,7 +344,7 @@ static void thread_border_router_lost_children_nwk_data_validate(protocol_interf
         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);
+                thread_border_router_child_network_data_clean(cur, server->router_id);
             }
         }
     }
@@ -626,6 +626,7 @@ int thread_border_router_prefix_add(int8_t interface_id, uint8_t *prefix_ptr, ui
     service.stableData = prefix_info_ptr->stableData;
     service.P_on_mesh = prefix_info_ptr->P_on_mesh;
     service.P_nd_dns = prefix_info_ptr->P_nd_dns;
+    service.P_res1 = prefix_info_ptr->P_res1;
 
     return thread_local_server_list_add_on_mesh_server(&cur->thread_info->localServerDataBase, &prefixTlv, &service);
 #else
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.c
index db166f84ce..0716d79cf7 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, 2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2015, 2017-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -64,6 +64,7 @@
 #include "6LoWPAN/Thread/thread_address_registration_client.h"
 #include "6LoWPAN/Thread/thread_resolution_client.h"
 #include <6LoWPAN/Thread/thread_extension_bootstrap.h>
+#include "6LoWPAN/Thread/thread_neighbor_class.h"
 #include "MLE/mle.h"
 #include "Service_Libs/mle_service/mle_service_security.h"
 #include "Service_Libs/blacklist/blacklist.h"
@@ -79,6 +80,7 @@
 #include "MLE/mle_tlv.h"
 #include "Service_Libs/nd_proxy/nd_proxy.h"
 #include "Service_Libs/mle_service/mle_service_api.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 #include "6LoWPAN/MAC/mac_helper.h"
 #include "6LoWPAN/MAC/mac_pairwise_key.h"
 #include "6LoWPAN/MAC/mac_data_poll.h"
@@ -104,7 +106,7 @@ thread_leader_data_t *thread_leader_data_generate(void);
 thread_parent_info_t *thread_parent_data_allocate(thread_info_t *info);
 static uint8_t * thread_joining_port_tlv_write(uint16_t port, uint8_t *ptr);
 static uint8_t * thread_commissioner_port_tlv_write(uint16_t port, uint8_t *ptr);
-static void thread_tx_failure_handler(int8_t nwk_id, uint8_t accumulated_failures, mle_neigh_table_entry_t *neighbor);
+static void thread_tx_failure_handler(int8_t nwk_id, uint8_t accumulated_failures, uint8_t attribute_index);
 static void thread_address_notification_cb(struct protocol_interface_info_entry *interface, const struct if_address_entry *addr, if_address_callback_t reason);
 
 /* Helper functions*/
@@ -152,33 +154,37 @@ uint8_t *thread_management_key_request_with_sequence(int8_t interface_id, uint8_
     if (!linkConfiguration) {
         return NULL;
     }
+    //tr_debug("MLE key request by sequence id %"PRIu8" seq %"PRIu32, keyId, keySequnce);
 
     cur = protocol_stack_interface_info_get_by_id(interface_id);
-    if (cur && cur->thread_info) {
-        if (cur->thread_info->masterSecretMaterial.valid_Info) {
-            if (keySequnce == linkConfiguration->key_sequence) {
-                if (mle_service_security_default_key_id_get(interface_id) == keyId) {
-                    keyPtr =  mle_service_security_default_key_get(interface_id);
-                }
-            } else if (keySequnce == (linkConfiguration->key_sequence + 1)) {
-                if (mle_service_security_next_key_id_get(interface_id) == keyId) {
-                    keyPtr =  mle_service_security_next_key_get(interface_id);
-                }
-            }
-
-            if (!keyPtr) {
-                tr_debug("Gen temporary key id %"PRIu8" seq %"PRIu32, keyId, keySequnce);
-                thread_key_get(linkConfiguration->master_key, cur->thread_info->masterSecretMaterial.historyKey, keySequnce);
-                cur->thread_info->masterSecretMaterial.historyKeyId = keyId;
-                cur->thread_info->masterSecretMaterial.historyKeyValid = false;
-                keyPtr = cur->thread_info->masterSecretMaterial.historyKey;
-            }
+    if (!cur || !cur->thread_info) {
+        return NULL;
+    }
+    if (!cur->thread_info->masterSecretMaterial.valid_Info) {
+        return NULL;
+    }
+    if (keySequnce == linkConfiguration->key_sequence) {
+        if (mle_service_security_default_key_id_get(interface_id) == keyId) {
+            keyPtr =  mle_service_security_default_key_get(interface_id);
         }
+    } else if (keySequnce == (linkConfiguration->key_sequence + 1)) {
+        if (mle_service_security_next_key_id_get(interface_id) == keyId) {
+            keyPtr =  mle_service_security_next_key_get(interface_id);
+        }
+    }
+
+    if (!keyPtr) {
+        tr_debug("Gen temporary key id %"PRIu8" seq %"PRIu32, keyId, keySequnce);
+        thread_key_get(linkConfiguration->master_key, cur->thread_info->masterSecretMaterial.historyKey, keySequnce);
+        cur->thread_info->masterSecretMaterial.historyKeyId = keyId;
+        cur->thread_info->masterSecretMaterial.historyKeyValid = false;
+        keyPtr = cur->thread_info->masterSecretMaterial.historyKey;
     }
     return keyPtr;
 }
 uint8_t * thread_mle_service_security_notify_cb(int8_t interface_id, mle_security_event_t event, uint8_t keyId)
 {
+    (void)keyId;
     protocol_interface_info_entry_t *interface = protocol_stack_interface_info_get_by_id(interface_id);
     if (!interface) {
         return NULL;
@@ -197,7 +203,7 @@ uint8_t * thread_mle_service_security_notify_cb(int8_t interface_id, mle_securit
             break;
 
         case MLE_SEC_UNKNOWN_KEY:
-            return thread_management_key_request(interface_id,keyId);
+            return NULL;
     }
     return NULL;
 }
@@ -360,23 +366,31 @@ bool thread_connectivity_tlv_parse(uint8_t *ptr, uint16_t dataLength, thread_con
     return false;
 }
 
-void thread_calculate_key_guard_timer(protocol_interface_info_entry_t *cur, link_configuration_s *linkConfiguration, bool is_init)
+void thread_key_guard_timer_calculate(protocol_interface_info_entry_t *cur, link_configuration_s *linkConfiguration, bool is_init)
 {
     uint32_t key_rotation = linkConfiguration ? linkConfiguration->key_rotation : 0;
 
-    if (is_init && key_rotation < 3600) {
+    if (is_init && key_rotation < 1) {
         tr_warn("Attempted to set key rotation time smaller than 1 hour.");
-        key_rotation = 3600;
+        key_rotation = 1;
     }
 
-    cur->thread_info->masterSecretMaterial.keyRotation = key_rotation;
-    cur->thread_info->masterSecretMaterial.keySwitchGuardTimer = is_init ? 0 : (key_rotation * 0.93);
+    cur->thread_info->masterSecretMaterial.keyRotation = key_rotation * 3600; // setting value is hours converting to seconds
+    cur->thread_info->masterSecretMaterial.keySwitchGuardTimer = is_init ? 0 : (key_rotation * 3600 * 0.93);
+}
+
+void thread_key_guard_timer_reset(protocol_interface_info_entry_t *cur)
+{
+    cur->thread_info->masterSecretMaterial.keySwitchGuardTimer = 0;
 }
 
 thread_leader_data_t *thread_leader_data_generate(void)
 {
     thread_leader_data_t *leader_data;
     leader_data = ns_dyn_mem_alloc(sizeof(thread_leader_data_t));
+    if (leader_data) {
+        memset(leader_data,0,sizeof(thread_leader_data_t));
+    }
     return leader_data;
 }
 
@@ -663,16 +677,27 @@ thread_mcast_child_t *thread_child_mcast_entry_find(thread_mcast_children_list_t
     return NULL;
 }
 
+bool thread_stable_context_check(protocol_interface_info_entry_t *cur, buffer_t *buf)
+{
+    mac_neighbor_table_entry_t *entry = mac_neighbor_table_address_discover(mac_neighbor_info(cur),buf->dst_sa.address + 2 , buf->dst_sa.addr_type);
+    if (entry && thread_addr_is_child(mac_helper_mac16_address_get(cur), entry->mac16)) {
+
+        /* Check if the child can handle only stable network data (e.g. sleepy device) */
+        return !(thread_neighbor_class_request_full_data_setup(&cur->thread_info->neighbor_class, entry->index));
+    }
+    return false;
+}
+
 thread_mcast_child_t *thread_child_mcast_entry_get(protocol_interface_info_entry_t *cur, const uint8_t *mcast_addr, const uint8_t *mac64)
 {
-    mle_neigh_table_entry_t *mle_entry = mle_class_get_by_link_address(cur->id, mac64, ADDR_802_15_4_LONG);
+    mac_neighbor_table_entry_t *entry = mac_neighbor_table_address_discover(mac_neighbor_info(cur), mac64, ADDR_802_15_4_LONG);
 
-    if (!mle_entry) {
+    if (!entry) {
         tr_error("No MLE entry.");
         return NULL;
     }
 
-    if (mle_entry->mode & MLE_RX_ON_IDLE) {
+    if (entry->rx_on_idle) {
         /* Not a sleepy child */
         tr_debug("Not a sleepy child");
         return NULL;
@@ -791,8 +816,6 @@ int thread_init(protocol_interface_info_entry_t *cur)
         return -1;
     }
 
-    mle_class_router_challenge(cur->id, NULL);
-
     if (etx_accum_failures_callback_register(cur->nwk_id, cur->id, 1, thread_tx_failure_handler) != 1) {
         return -1;
     }
@@ -803,7 +826,6 @@ int thread_init(protocol_interface_info_entry_t *cur)
     thread_data_base_init(cur->thread_info, cur->id);
     mac_helper_pib_boolean_set(cur,macThreadForceLongAddressForBeacon , true);
     mac_helper_mac16_address_set(cur, 0xffff);
-    mle_class_mode_set(cur->id, MLE_CLASS_END_DEVICE);
     return 0;
 }
 
@@ -817,7 +839,7 @@ int thread_attach_ready(protocol_interface_info_entry_t *cur)
         case THREAD_STATE_CONNECTED:
         case THREAD_STATE_CONNECTED_ROUTER:
             return 0;
-            break;
+            /* break; */
         default:
             break;
     }
@@ -919,7 +941,7 @@ static void thread_key_switch_timer(protocol_interface_info_entry_t *cur, uint16
 
         tr_debug("thrKeyRotation == 0: sync key material by %"PRIu32, linkConfiguration->key_sequence + 1);
         thread_management_key_sets_calc(cur, linkConfiguration, linkConfiguration->key_sequence + 1);
-        thread_calculate_key_guard_timer(cur, linkConfiguration, false);
+        thread_key_guard_timer_calculate(cur, linkConfiguration, false);
     }
 }
 
@@ -1019,14 +1041,15 @@ void thread_timer(protocol_interface_info_entry_t *cur, uint8_t ticks)
         return;
     }
 
-    if (thread_i_am_router(cur)) {
+    if (cur->nwk_bootstrap_state != ER_BOOTSRAP_DONE && cur->nwk_bootstrap_state != ER_MLE_ATTACH_READY) {
+        /* Own attach is ongoing, do not send advertisements */
+        return;
+    }
 
+    if (thread_i_am_router(cur)) {
         if (thread_routing_timer(thread_info, ticks)) {
             thread_router_bootstrap_mle_advertise(cur);
         }
-
-    } else {
-
     }
 }
 
@@ -1121,14 +1144,20 @@ uint8_t thread_beacon_indication(uint8_t *ptr, uint8_t len, protocol_interface_i
 
 static uint8_t *thread_linkquality_write(int8_t interface_id, uint8_t *buffer)
 {
+    protocol_interface_info_entry_t *interface_ptr = protocol_stack_interface_info_get_by_id(interface_id);
+    if (!interface_ptr && !interface_ptr->thread_info) {
+        return buffer;
+    }
+
     uint8_t lqi1 = 0, lqi2 = 0, lqi3 = 0;
     thread_link_quality_e thread_link_quality;
-    mle_neigh_table_list_t *neigh_list = mle_class_active_list_get(interface_id);
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(interface_ptr)->neighbour_list;
 
-    ns_list_foreach(mle_neigh_table_entry_t, cur, neigh_list) {
-        if (thread_is_router_addr(cur->short_adr)) {
+    ns_list_foreach(mac_neighbor_table_entry_t, cur, mac_table_list) {
+        if (thread_is_router_addr(cur->mac16)) {
             // Only count routers to link quality
-            thread_link_quality = thread_link_margin_to_quality(cur->link_margin);
+            uint16_t link_margin = thread_neighbor_entry_linkmargin_get(&interface_ptr->thread_info->neighbor_class, cur->index);
+            thread_link_quality = thread_link_margin_to_quality(link_margin);
             switch (thread_link_quality) {
                 case QUALITY_20dB:
                     lqi3++;
@@ -1193,14 +1222,14 @@ uint8_t *thread_connectivity_tlv_write(uint8_t *ptr, protocol_interface_info_ent
     *ptr++ = 10;
 
     // determine parent priority
-    if ((mode & MLE_DEV_MASK) == MLE_RFD_DEV && (3*mle_class_rfd_entry_count_get(cur->id) > 2*THREAD_MAX_MTD_CHILDREN)) {
+    if ((mode & MLE_DEV_MASK) == MLE_RFD_DEV && (3*mle_class_rfd_entry_count_get(cur) > 2*THREAD_MAX_MTD_CHILDREN)) {
         *ptr++ = CONNECTIVITY_PP_LOW;
-    } else if (!(mode & MLE_RX_ON_IDLE) && (3*mle_class_sleepy_entry_count_get(cur->id) > 2*THREAD_MAX_SED_CHILDREN)) {
+    } else if (!(mode & MLE_RX_ON_IDLE) && (3*mle_class_sleepy_entry_count_get(cur) > 2*THREAD_MAX_SED_CHILDREN)) {
         *ptr++ = CONNECTIVITY_PP_LOW;
     } else if (3*thread_router_bootstrap_child_count_get(cur) > 2*thread->maxChildCount) {
         // 1/3 of the child capacity remaining, PP=low
         *ptr++ = CONNECTIVITY_PP_LOW;
-    } else if (mle_class_free_entry_count_get(cur->id) < THREAD_FREE_MLE_ENTRY_THRESHOLD) {
+    } else if (mle_class_free_entry_count_get(cur) < THREAD_FREE_MLE_ENTRY_THRESHOLD) {
         // If only few entries available in the MLE table, change priority to low
         *ptr++ = CONNECTIVITY_PP_LOW;
     } else {
@@ -1829,7 +1858,7 @@ static uint8_t * thread_commissioner_port_tlv_write(uint16_t port, uint8_t *ptr)
     return common_write_16_bit(port, ptr);
 }
 
-static void thread_tx_failure_handler(int8_t nwk_id, uint8_t accumulated_failures, mle_neigh_table_entry_t *neighbor)
+static void thread_tx_failure_handler(int8_t nwk_id, uint8_t accumulated_failures, uint8_t attribute_index)
 {
     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(nwk_id);
 
@@ -1839,27 +1868,33 @@ static void thread_tx_failure_handler(int8_t nwk_id, uint8_t accumulated_failure
         return;
     }
 
+    mac_neighbor_table_entry_t *neighbor = mac_neighbor_table_attribute_discover(mac_neighbor_info(cur), attribute_index);
+    if (!neighbor) {
+        return;
+    }
+
     if (accumulated_failures >= THREAD_MAC_TRANSMISSIONS*THREAD_FAILED_CHILD_TRANSMISSIONS) {
-        mle_class_remove_entry(cur->id, neighbor);
+        mac_neighbor_table_neighbor_remove(mac_neighbor_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)
+void thread_reset_neighbour_info(protocol_interface_info_entry_t *cur, mac_neighbor_table_entry_t *neighbour)
 {
     thread_parent_info_t *thread_endnode_parent = thread_info(cur)->thread_endnode_parent;
 
-    if (!thread_i_am_router(cur) && thread_endnode_parent && thread_endnode_parent->shortAddress == neighbour->short_adr) {
+    if (!thread_i_am_router(cur) && thread_endnode_parent && thread_endnode_parent->shortAddress == neighbour->mac16) {
         if(cur->nwk_bootstrap_state != ER_CHILD_ID_REQ) {
             tr_warn("End device lost parent, reset!\n");
             thread_bootstrap_connection_error(cur->id, CON_PARENT_CONNECT_DOWN, NULL);
         }
     }
 
-    thread_routing_remove_link(cur, neighbour->short_adr);
+    thread_routing_remove_link(cur, neighbour->mac16);
     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);
+    mac_helper_devicetable_remove(cur->mac_api, neighbour->index);
+    thread_neighbor_class_entry_remove(&cur->thread_info->neighbor_class, neighbour->index);
 }
 
 uint8_t thread_get_router_count_from_route_tlv(mle_tlv_info_t *routeTlv)
@@ -1894,7 +1929,7 @@ static void thread_address_notification_cb(struct protocol_interface_info_entry
         } else {
             if (reason == ADDR_CALLBACK_DAD_COMPLETE) {
                 /* Send address notification (our parent doesn't do that for us) */
-                thread_extension_address_registration(interface, addr->address, NULL);
+                thread_extension_address_registration(interface, addr->address, NULL, false, false);
             }
         }
     }
@@ -1902,9 +1937,6 @@ static void thread_address_notification_cb(struct protocol_interface_info_entry
 
 void thread_mcast_group_change(struct protocol_interface_info_entry *interface, if_group_entry_t *group, bool addr_added)
 {
-    (void) addr_added;
-
-    group->mld_timer = 0;
 
     if (thread_attach_ready(interface) != 0) {
         return;
@@ -1918,11 +1950,13 @@ void thread_mcast_group_change(struct protocol_interface_info_entry *interface,
             interface->thread_info->childUpdateReqTimer = 1;
         }
     } else {
-        thread_extension_mcast_subscrition_change(interface, group, addr_added);
+        if (addr_added) {
+            thread_address_registration_timer_set(interface, 0, 1);
+        }
     }
 }
 
-void thread_old_partition_data_purge(protocol_interface_info_entry_t *cur)
+void thread_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(cur->id);
@@ -1935,5 +1969,33 @@ void thread_old_partition_data_purge(protocol_interface_info_entry_t *cur)
 
 }
 
+bool thread_partition_match(protocol_interface_info_entry_t *cur, thread_leader_data_t *leaderData)
+{
+    if (thread_info(cur)->thread_leader_data) {
+        if ((thread_info(cur)->thread_leader_data->partitionId == leaderData->partitionId) &&
+            (thread_info(cur)->thread_leader_data->weighting == leaderData->weighting)) {
+            return true;
+        }
+    }
+    return false;
+}
+
+void thread_partition_info_update(protocol_interface_info_entry_t *cur, thread_leader_data_t *leaderData)
+{
+    /* Force network data update later when processing network data TLV */
+    thread_info(cur)->thread_leader_data->dataVersion = leaderData->dataVersion - 1;
+    thread_info(cur)->thread_leader_data->stableDataVersion = leaderData->stableDataVersion - 1;
+    thread_info(cur)->thread_leader_data->leaderRouterId = leaderData->leaderRouterId;
+    thread_info(cur)->thread_leader_data->partitionId = leaderData->partitionId;
+    thread_info(cur)->thread_leader_data->weighting = leaderData->weighting;
+    /* New network data learned, get rid of old partition data */
+    thread_partition_data_purge(cur);
+}
+
+void thread_neighbor_communication_update(protocol_interface_info_entry_t *cur, uint8_t neighbor_attribute_index)
+{
+    thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, neighbor_attribute_index);
+}
+
 #endif
 
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.h
index b06b9454e4..09ffce3900 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_common.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -40,6 +40,8 @@
 #include "eventOS_event_timer.h"
 #include "MLE/mle_tlv.h"
 
+struct mac_neighbor_table_entry;
+
 #define MAX_MLE_CHALLENGE_LENGTH 32
 
 /*
@@ -55,8 +57,8 @@
 #define NETWORK_ID_TIMEOUT 120 //seconds
 
 // Values when adverticements are made faster when leader connection is restored
-#define NETWORK_ID_SPEEDUP 60 //seconds
-#define NETWORK_ID_SPEEDUP_MAX 100 //seconds
+#define NETWORK_ID_SPEEDUP 55 //seconds
+#define NETWORK_ID_SPEEDUP_MAX 80 //seconds
 
 #define DHCPV6_ENTERPRISE_THREAD 0x0000AFAA
 #define DHCPV6_OPTION_VENDOR_SPESIFIC_INFO_LEN 0x0011
@@ -92,6 +94,22 @@ typedef enum {
     THREAD_COMMISSIONER_REGISTERED
 } thread_commissioner_register_status_e;
 
+typedef struct thread_neigh_table_entry_s {
+    uint8_t         mlEid[8];
+    uint32_t        last_contact_time;  /*!< monotonic time - hard to define "contact"; used for Thread Leasequery replies */
+    uint16_t        link_margin;
+    bool secured_data_request:1;
+    bool request_full_data_set:1;
+} thread_neigh_table_entry_t ;
+
+/**
+ * Neighbor info data base
+ */
+typedef struct thread_neighbor_class_s {
+    thread_neigh_table_entry_t *neigh_info_list;          /*!< Allocated Neighbour info array*/
+    uint8_t list_size;                                    /*!< List size*/
+} thread_neighbor_class_t;
+
 typedef struct thread_mcast_child {
     uint8_t mac64[8];
     ns_list_link_t link;
@@ -261,6 +279,7 @@ typedef struct thread_previous_partition_info_s {
 
 typedef struct thread_info_s {
     thread_routing_info_t routing;
+    thread_neighbor_class_t neighbor_class;
     thread_master_secret_material_t masterSecretMaterial;
     thread_network_data_cache_entry_t networkDataStorage;
     thread_network_local_data_cache_entry_t localServerDataBase;
@@ -363,7 +382,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_reset_neighbour_info(protocol_interface_info_entry_t *cur, struct mac_neighbor_table_entry *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);
@@ -413,15 +432,19 @@ uint8_t *thread_pending_operational_dataset_write(protocol_interface_info_entry_
 bool thread_pending_operational_dataset_process(protocol_interface_info_entry_t *cur, uint64_t mle_pending_timestamp, uint8_t *ptr, uint16_t len);
 /*Write optional thread leader data TLV if leader data is known*/
 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_key_guard_timer_calculate(protocol_interface_info_entry_t *cur, link_configuration_s *linkConfiguration, bool is_init);
+void thread_key_guard_timer_reset(protocol_interface_info_entry_t *cur);
 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(protocol_interface_info_entry_t *cur);
-
+void thread_partition_data_purge(protocol_interface_info_entry_t *cur);
+bool thread_partition_match(protocol_interface_info_entry_t *cur, thread_leader_data_t *leaderData);
+void thread_partition_info_update(protocol_interface_info_entry_t *cur, thread_leader_data_t *leaderData);
+void thread_neighbor_communication_update(protocol_interface_info_entry_t *cur, uint8_t neighbor_attribute_index);
+bool thread_stable_context_check(protocol_interface_info_entry_t *cur, buffer_t *buf);
 #else // HAVE_THREAD
 
 NS_DUMMY_DEFINITIONS_OK
-
+#define thread_stable_context_check(cur, buf) (false)
 #define thread_info(cur) ((thread_info_t *) NULL)
 #define thread_am_router(cur) (false)
 #define thread_am_host(cur) (false)
@@ -438,6 +461,7 @@ NS_DUMMY_DEFINITIONS_OK
 #define thread_link_reject_send(interface, ll64) 0
 #define thread_addr_is_mesh_local_16(addr, cur) false
 #define thread_mcast_group_change(interface, group, group_added) ((void)0)
+#define thread_neighbor_communication_update(cur, neighbor_attribute_index) ((void)0)
 #endif // HAVE_THREAD
 
 #endif /* LOWPAN_THREAD_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_config.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_config.h
index ca689a004f..0db796cd5d 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_config.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_config.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_constants.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_constants.h
index 978606b360..4150f4cc23 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_constants.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_constants.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_dhcpv6_client.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_dhcpv6_client.c
index 642857bdd7..a49054e3ed 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_dhcpv6_client.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_dhcpv6_client.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2017, Arm Limited and affiliates.
+ * Copyright (c) 2013-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_diagnostic.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_diagnostic.c
index 673aa22df9..fdce83ece8 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_diagnostic.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_diagnostic.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -39,6 +39,7 @@
 #include "thread_joiner_application.h"
 #include "thread_leader_service.h"
 #include "thread_router_bootstrap.h"
+#include "6LoWPAN/Thread/thread_neighbor_class.h"
 #include "MLE/mle.h"
 #include "6LoWPAN/Thread/thread_router_bootstrap.h"
 #include "thread_config.h"
@@ -47,6 +48,7 @@
 #include "thread_diagcop_lib.h"
 #include "common_functions.h"
 #include "6LoWPAN/MAC/mac_helper.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 #include "mac_api.h"
 
 
@@ -93,31 +95,31 @@ static uint8_t *thread_diagnostic_child_table_tlv_build(uint8_t *data_ptr, proto
     uint8_t child_count = 0;
     uint8_t calculated_timeout;
 
-    mle_neigh_table_list_t *mle_table = mle_class_active_list_get(cur->id);
-    if (!mle_table) {
-        return data_ptr;
-    }
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
 
     child_count = thread_router_bootstrap_child_count_get(cur);
 
     *data_ptr++ = DIAGCOP_TLV_CHILD_TABLE;  // Type
     *data_ptr++ = (3 * child_count);        // Length
 
-    ns_list_foreach(mle_neigh_table_entry_t, cur_entry, mle_table) {
-        if (cur_entry->threadNeighbor && thread_router_addr_from_addr(cur_entry->short_adr) == mac_helper_mac16_address_get(cur)){
+    ns_list_foreach(mac_neighbor_table_entry_t, cur_entry, mac_table_list) {
+        if (thread_router_addr_from_addr(cur_entry->mac16) == mac_helper_mac16_address_get(cur)){
             /* |0|1|2|3|4|5|6|7|8|9|0|1|2|3|4|5|6|7|8|9|0|1|2|3| */
             /* |Timeout  |Rsv|  Child ID       |      Mode     | */
-            calculated_timeout = thread_log2_aprx((cur_entry->timeout_rx-1)*MLE_TIMER_TICKS_SECONDS) + 4;
-            tr_debug("Write child table TLV entry: %d - %d - %d", calculated_timeout, cur_entry->short_adr, cur_entry->mode);
+            calculated_timeout = thread_log2_aprx(cur_entry->link_lifetime - 1) + 4;
+            uint8_t mode = 0;
+            mode |= mle_mode_write_from_mac_entry(cur_entry);
+            mode |= thread_neighbor_class_mode_write_from_entry(&cur->thread_info->neighbor_class, cur_entry->index);
+            tr_debug("Write child table TLV entry: %d - %d - %d", calculated_timeout, cur_entry->mac16, mode);
             *data_ptr = 0x00; //reserved bytes to zero
             *data_ptr = calculated_timeout << 3;
 
-            if(cur_entry->short_adr & 0x0100){
+            if(cur_entry->mac16 & 0x0100){
                *data_ptr = *data_ptr | 0x01;
             }
             data_ptr++;
-            *data_ptr++ = (uint8_t)(cur_entry->short_adr & 0x00ff);
-            *data_ptr++ = cur_entry->mode;
+            *data_ptr++ = (uint8_t)(cur_entry->mac16 & 0x00ff);
+            *data_ptr++ = mode;
         }
     }
 
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_discovery.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_discovery.c
index b9e484f341..8597483fa1 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_discovery.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_discovery.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -818,7 +818,7 @@ static void thread_discovery_request_msg_handler(thread_discovery_class_t * disc
     tr_debug("Thread discovery request message RX");
 
     // Check if we have room for new neighbor
-    if (mle_class_free_entry_count_get(discovery_class->interface_id) < 1) {
+    if (mle_class_free_entry_count_get(discovery_class->interface) < 1) {
         tr_debug("MLE table full, skip request");
         return;
     }
@@ -1281,10 +1281,10 @@ static void thread_discovery_normal_receive_cb(int8_t interface_id, mle_message_
 /**
  * Start Thread network discovery
  */
-int thread_discovery_network_scan(int8_t interface_id, thread_discover_reques_t *scan_request, thread_discovery_ready_cb *ready_cb)
+int thread_discovery_network_scan(struct protocol_interface_info_entry *cur_interface, thread_discover_reques_t *scan_request, thread_discovery_ready_cb *ready_cb)
 {
 
-    thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
+    thread_discovery_class_t * discovery_class = thread_discovery_class_get(cur_interface->id);
     if (!discovery_class || !ready_cb || !scan_request) {
         return -1;
     }
@@ -1304,12 +1304,12 @@ int thread_discovery_network_scan(int8_t interface_id, thread_discover_reques_t
         return -3;
     }
 
-    if (mle_service_interface_register(interface_id, thread_discovery_normal_receive_cb, discovery_class->discovery_request->temporary_mac64,8) != 0) {
+    if (mle_service_interface_register(cur_interface->id, cur_interface, thread_discovery_normal_receive_cb, discovery_class->discovery_request->temporary_mac64,8) != 0) {
         thread_discovery_request_free(discovery_class);
         return -1;
     }
 
-    if (mle_service_interface_receiver_bypass_handler_update(interface_id, thread_discovery_message_receiver_cb) != 0) {
+    if (mle_service_interface_receiver_bypass_handler_update(cur_interface->id, thread_discovery_message_receiver_cb) != 0) {
         thread_discovery_request_free(discovery_class);
         return -1;
     }
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_discovery.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_discovery.h
index 35487ebec0..dd08496bf6 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_discovery.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_discovery.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -122,7 +122,7 @@ int thread_discovery_responser_enable(int8_t interface_id, bool enable_service);
 /**
  * Start Thread network discovery
  */
-int thread_discovery_network_scan(int8_t interface_id, thread_discover_reques_t *scan_request, thread_discovery_ready_cb *ready_cb);
+int thread_discovery_network_scan(struct protocol_interface_info_entry *cur_interface, thread_discover_reques_t *scan_request, thread_discovery_ready_cb *ready_cb);
 
 /**
  * Start device orphan scan
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_extension.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_extension.h
index 8e7a555e01..671ea9823c 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_extension.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_extension.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -40,27 +40,66 @@ extern "C" {
 
 struct discovery_response_list;
 
+#ifdef HAVE_THREAD_V2
+
+void thread_extension_allocate(protocol_interface_info_entry_t *cur);
+void thread_extension_free(protocol_interface_info_entry_t *cur);
+void thread_extension_init(int8_t interface_id, int8_t coap_service_id);
+void thread_extension_mtd_service_register(protocol_interface_info_entry_t *cur);
+int thread_extension_network_prefix_get(int8_t interface_id, uint8_t *options_ptr, uint8_t *prefix_ptr, uint8_t *prefix_len);
+void thread_extension_network_data_process(struct protocol_interface_info_entry *cur);
+int thread_extension_primary_bbr_get(struct protocol_interface_info_entry *cur, uint8_t *addr_ptr, uint8_t *seq_ptr, uint32_t *timer1_ptr, uint32_t *timer2_ptr);
+void thread_extension_address_registration(struct protocol_interface_info_entry *interface, const uint8_t *addr, const uint8_t *child_mac64, bool refresh_child_entry, bool duplicate_child_detected);
+void thread_extension_address_generate(protocol_interface_info_entry_t *cur);
+void thread_extension_aloc_generate(struct protocol_interface_info_entry *cur);
+bool thread_extension_aloc_map(protocol_interface_info_entry_t *cur, uint16_t *addr16);
+void thread_extension_mcast_subscrition_change(protocol_interface_info_entry_t *interface);
+void thread_extension_address_registration_trigger(protocol_interface_info_entry_t *interface);
+void thread_extension_route_set(protocol_interface_info_entry_t *cur);
+void thread_extension_activate(protocol_interface_info_entry_t *cur);
+bool thread_extension_enabled(protocol_interface_info_entry_t *cur);
+bool thread_extension_context_can_delete(int8_t id, uint8_t servicesPrefix[16], uint8_t context_prefix_length);
+bool thread_extension_version_check(uint8_t version);
+void thread_extension_discover_response_read(struct discovery_response_list *nwk_info, uint16_t discover_response_tlv, uint8_t *data_ptr, uint16_t data_len);
+void thread_extension_discover_response_tlv_write(uint16_t *data, uint8_t version, uint16_t securityPolicy);
+int thread_extension_service_init(protocol_interface_info_entry_t *cur);
+void thread_extension_addr_ntf_send(struct protocol_interface_info_entry *cur, uint8_t *destination_address, const uint8_t *addr_data_ptr, uint8_t bbr_status);
+#ifdef HAVE_THREAD_ROUTER
+bool thread_extension_joining_enabled(int8_t interface_id);
+uint8_t thread_extension_discover_response_len(protocol_interface_info_entry_t *cur);
+uint8_t *thread_extension_discover_response_write(protocol_interface_info_entry_t *cur, uint8_t *ptr);
+
+#else
+#define thread_extension_joining_enabled(interface_id) false
+#define thread_extension_discover_response_len(cur) 0
+#define thread_extension_discover_response_write(cur, ptr) (ptr)
+#endif //HAVE_THREAD_ROUTER
+#else
+
 #define thread_extension_allocate(cur)
 #define thread_extension_free(cur)
 #define thread_extension_init(interface_id,coap_service_id)
 #define thread_extension_network_prefix_get(interface_id,options_ptr,prefix_ptr,prefix_len) (-1)
 #define thread_extension_network_data_process(cur)
 #define thread_extension_primary_bbr_get(cur,addr_ptr,seq_ptr,timer1_ptr, timer2_ptr) (-1)
-#define thread_extension_address_registration(interface,addr,child_mac64)
+#define thread_extension_address_registration(interface,addr,child_mac64,refresh_child_entry,duplicate_child_detected)
 #define thread_extension_address_generate(cur)
 #define thread_extension_aloc_generate(cur)
 #define thread_extension_aloc_map(cur, addr16) false
-#define thread_extension_mcast_subscrition_change(interface, group, added)
+#define thread_extension_mcast_subscrition_change(interface)
 #define thread_extension_route_set(cur)
 #define thread_extension_activate(cur)
 #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)
-#define thread_extension_joiner_router_init(interface_id)
+#define thread_extension_service_init(cur) 0
 #define thread_extension_joining_enabled(interface_id) false
 #define thread_extension_discover_response_len(cur) 0
 #define thread_extension_discover_response_write(cur, ptr) (ptr)
+#define thread_extension_addr_ntf_send(cur,destination_address,addr_data_ptr,bbr_status)
+#define thread_extension_context_can_delete(id, servicesPrefix, context_prefix_length) false
+#endif
 
 #ifdef __cplusplus
 }
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_extension_bbr.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_extension_bbr.h
index 5e4ac8f8c1..cad08464b0 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_extension_bbr.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_extension_bbr.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017, Arm Limited and affiliates.
+ * Copyright (c) 2017-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -38,6 +38,14 @@
 extern "C" {
 #endif
 
+/*
+ * Thread PBBR ML-EID map structure
+ */
+typedef struct thread_pbbr_dua_info {
+    uint8_t mleid_ptr[8];
+    uint32_t last_contact_time;
+} thread_pbbr_dua_info_t;
+
 #if defined(HAVE_THREAD_V2) && defined(HAVE_THREAD_BORDER_ROUTER)
 
 int8_t thread_extension_bbr_init(int8_t interface_id, int8_t backbone_interface_id);
@@ -47,6 +55,7 @@ void thread_extension_bbr_seconds_timer(int8_t interface_id, uint32_t seconds);
 int thread_extension_bbr_timeout_set(int8_t interface_id, uint32_t timeout_a, uint32_t timeout_b, uint32_t delay);
 int thread_extension_bbr_address_set(int8_t interface_id, const uint8_t *addr_ptr, uint16_t port);
 void thread_extension_bbr_route_update(protocol_interface_info_entry_t *cur);
+int thread_extension_bbr_prefix_set(int8_t interface_id, uint8_t *prefix);
 
 
 #else
@@ -58,6 +67,7 @@ void thread_extension_bbr_route_update(protocol_interface_info_entry_t *cur);
 #define thread_extension_bbr_timeout_set(interface_id, timeout_a, timeout_b, delay)
 #define thread_extension_bbr_address_set(interface_id, addr_ptr, port) (-1)
 #define thread_extension_bbr_route_update(cur)
+#define thread_extension_bbr_prefix_set(interface_id, prefix) 0
 #endif
 
 #ifdef __cplusplus
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_host_bootstrap.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_host_bootstrap.c
index b1cf644147..5036661612 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_host_bootstrap.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_host_bootstrap.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -63,6 +63,7 @@
 #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_neighbor_class.h"
 #include "thread_management_if.h"
 #include "Common_Protocols/ipv6.h"
 #include "MPL/mpl.h"
@@ -73,6 +74,7 @@
 #include "6LoWPAN/MAC/mac_helper.h"
 #include "6LoWPAN/MAC/mac_data_poll.h"
 #include "Core/include/address.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 
 #define TRACE_GROUP "tebs"
 
@@ -122,7 +124,8 @@ static void thread_merge_prepare(protocol_interface_info_entry_t *cur)
     thread_clean_old_16_bit_address_based_addresses(cur);
     mpl_clear_realm_scope_seeds(cur);
     ipv6_route_table_remove_info(cur->id, ROUTE_THREAD_PROXIED_HOST, NULL);
-    thread_old_partition_data_purge(cur);
+    ipv6_route_table_remove_info(cur->id, ROUTE_THREAD_PROXIED_DUA_HOST, NULL);
+    thread_partition_data_purge(cur);
     thread_network_data_clean(cur);
     cur->nwk_mode = ARM_NWK_GP_IP_MODE;
 }
@@ -145,7 +148,7 @@ static bool thread_parent_discover_timeout_cb(int8_t interface_id, uint16_t msgI
         uint8_t ll64[16];
         thread_scanned_parent_t *parent = cur->thread_info->thread_attach_scanned_parent;
         link_configuration_s *linkConfiguration;
-        mle_neigh_table_entry_t *entry_temp;
+        mac_neighbor_table_entry_t *entry_temp;
 
         linkConfiguration = thread_joiner_application_get_config(interface_id);
         if (!linkConfiguration) {
@@ -156,22 +159,25 @@ static bool thread_parent_discover_timeout_cb(int8_t interface_id, uint16_t msgI
         memcpy(ll64, ADDR_LINK_LOCAL_PREFIX , 8);
         memcpy(&ll64[8], parent->mac64 , 8);
         ll64[8] ^= 2;
-
-        entry_temp = mle_class_get_entry_by_ll64(interface_id, parent->linkMarginToParent,ll64, true, &new_entry_created);
+        entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), ll64, true, &new_entry_created);
         if (entry_temp == NULL) {
             return false;
         }
-        entry_temp->threadNeighbor = true;
-        entry_temp->short_adr = parent->shortAddress;
-        entry_temp->priorityFlag = true;
-        entry_temp->holdTime = 90;
-        entry_temp->mle_frame_counter = parent->mleFrameCounter;
+        thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index, parent->linkMarginToParent, new_entry_created);
+        thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
+
+        entry_temp->mac16 = parent->shortAddress;
+        entry_temp->link_role = PRIORITY_PARENT_NEIGHBOUR;
+
+        mle_service_frame_counter_entry_add(interface_id, entry_temp->index, parent->mleFrameCounter);
 
         thread_management_key_sets_calc(cur, linkConfiguration, cur->thread_info->thread_attach_scanned_parent->keySequence);
-        thread_calculate_key_guard_timer(cur, linkConfiguration, true);
+        thread_key_guard_timer_calculate(cur, linkConfiguration, true);
 
-        mac_helper_devicetable_set(entry_temp, cur, parent->linLayerFrameCounter, mac_helper_default_key_index_get(cur), new_entry_created);
-        mle_entry_timeout_update(entry_temp, THREAD_DEFAULT_LINK_LIFETIME);
+        mlme_device_descriptor_t device_desc;
+        mac_helper_device_description_write(cur, &device_desc, entry_temp->mac64, entry_temp->mac16,parent->linLayerFrameCounter, false);
+        mac_helper_devicetable_set(&device_desc, cur,entry_temp->index, mac_helper_default_key_index_get(cur), new_entry_created);
+        mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_temp, THREAD_DEFAULT_LINK_LIFETIME);
 
         if (cur->thread_info->thread_device_mode == THREAD_DEVICE_MODE_SLEEPY_END_DEVICE) {
             nwk_thread_host_control(cur, NET_HOST_FAST_POLL_MODE, 50);
@@ -284,6 +290,8 @@ void thread_network_attach_start(protocol_interface_info_entry_t *cur)
         tr_debug("MLE Parent request");
         cur->nwk_bootstrap_state = ER_MLE_SCAN;
         cur->bootsrap_state_machine_cnt = 0;
+        /* advance trickle timer by 6 (in 100ms ticks) seconds if needed */
+        thread_routing_trickle_advance(&cur->thread_info->routing, 6*10);
     } else {
         cur->bootsrap_state_machine_cnt = 5;
     }
@@ -297,7 +305,7 @@ static int thread_end_device_synch_response_validate(protocol_interface_info_ent
     uint16_t address16;
     uint32_t llFrameCounter;
     thread_leader_data_t leaderData;
-    mle_neigh_table_entry_t *entry_temp;
+    mac_neighbor_table_entry_t *entry_temp;
     bool new_entry_created;
 
     tr_debug("Validate Link Synch Response");
@@ -323,31 +331,36 @@ static int thread_end_device_synch_response_validate(protocol_interface_info_ent
 
     if (securityHeader->KeyIdMode == MAC_KEY_ID_MODE_SRC4_IDX) {
         thread_management_key_synch_req(cur->id, common_read_32_bit(securityHeader->Keysource));
+        // if learning key sequence from link sync actual guard timer value is not known
+        thread_key_guard_timer_reset(cur);
     } else {
         tr_debug("Key ID Mode 2 not used; dropped.");
         return -3;
     }
 
     //Update parent link information
-    entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, src_address, true, &new_entry_created);
+    entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), src_address, true, &new_entry_created);
 
     if (!entry_temp) {
         tr_debug("Neighbor allocate fail");
         return -2;
     }
+    thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index, linkMargin, new_entry_created);
+    thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
 /*
 
 */
-    entry_temp->threadNeighbor = true;
-    entry_temp->short_adr = srcAddress;
-    entry_temp->handshakeReady = 1;
-    entry_temp->holdTime = 90;
-    entry_temp->priorityFlag = true; // Make this our parent
-    common_write_16_bit(entry_temp->short_adr, shortAddress);
+    entry_temp->mac16 = srcAddress;
+    entry_temp->connected_device = 1;
+    entry_temp->link_role = PRIORITY_PARENT_NEIGHBOUR; // Make this our parent
+    common_write_16_bit(entry_temp->mac16, shortAddress);
 
     mac_helper_coordinator_address_set(cur, ADDR_802_15_4_SHORT, shortAddress);
-    mle_entry_timeout_update(entry_temp, thread_info(cur)->host_link_timeout);
-    mac_helper_devicetable_set(entry_temp, cur, llFrameCounter, securityHeader->KeyIndex, new_entry_created);
+    mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_temp, thread_info(cur)->host_link_timeout);
+
+    mlme_device_descriptor_t device_desc;
+    mac_helper_device_description_write(cur, &device_desc, entry_temp->mac64, entry_temp->mac16,llFrameCounter, false);
+    mac_helper_devicetable_set(&device_desc, cur, entry_temp->index, securityHeader->KeyIndex, new_entry_created);
 
     thread_info(cur)->thread_attached_state = THREAD_STATE_CONNECTED;
     thread_bootstrap_update_ml16_address(cur, address16);
@@ -377,12 +390,10 @@ static int thread_end_device_synch_response_validate(protocol_interface_info_ent
 
 static void thread_child_synch_receive_cb(int8_t interface_id, mle_message_t *mle_msg, mle_security_header_t *security_headers)
 {
+    (void) interface_id;
     tr_debug("Thread MLE message child_synch handler");
     //State machine What packet shuold accept in this case
-    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
-    if (!cur) {
-        return;
-    }
+    protocol_interface_info_entry_t *cur = mle_msg->interface_ptr;
 
     /* Check that message is from link-local scope */
     if(!addr_is_ipv6_link_local(mle_msg->packet_src_address)) {
@@ -466,10 +477,8 @@ static bool thread_host_prefer_parent_response(protocol_interface_info_entry_t *
  */
 void thread_mle_parent_discover_receive_cb(int8_t interface_id, mle_message_t *mle_msg, mle_security_header_t *security_headers)
 {
-    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
-    if (!cur) {
-        return;
-    }
+    (void) interface_id;
+    protocol_interface_info_entry_t *cur = mle_msg->interface_ptr;
 
     /* Check that message is from link-local scope */
     if(!addr_is_ipv6_link_local(mle_msg->packet_src_address)) {
@@ -538,8 +547,7 @@ 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) ||
-                        (thread_info(cur)->thread_leader_data->weighting != leaderData.weighting)) {
+                    if (!thread_partition_match(cur, &leaderData)) {
                         //accept only same ID at reattach phase
                         return;
                     }
@@ -557,8 +565,7 @@ 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) &&
-                        (thread_info(cur)->thread_leader_data->weighting == leaderData.weighting)) {
+                    if (thread_partition_match(cur, &leaderData)) {
                         //accept only different ID at anyattach phase
                         tr_debug("Drop old partition");
                         return;
@@ -736,12 +743,9 @@ void thread_mle_parent_discover_receive_cb(int8_t interface_id, mle_message_t *m
 static void thread_mle_child_request_receive_cb(int8_t interface_id, mle_message_t *mle_msg, mle_security_header_t *security_headers)
 {
     thread_leader_data_t leaderData;
-    mle_neigh_table_entry_t *entry_temp;
-    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
+    mac_neighbor_table_entry_t *entry_temp;
+    protocol_interface_info_entry_t *cur = mle_msg->interface_ptr;
     link_configuration_s *link_configuration;
-    if (!cur) {
-        return;
-    }
     link_configuration = thread_joiner_application_get_config(cur->id);
     if (!link_configuration) {
         return;
@@ -790,17 +794,19 @@ static void thread_mle_child_request_receive_cb(int8_t interface_id, mle_message
             thread_merge_prepare(cur);
 
             // Create entry for new parent
-            entry_temp = mle_class_get_entry_by_ll64(cur->id, thread_compute_link_margin(mle_msg->dbm), mle_msg->packet_src_address, true, &new_entry_created);
+            entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, true, &new_entry_created);
             if (entry_temp == NULL) {
                 // todo: what to do here?
                 return;
             }
+            thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index, thread_compute_link_margin(mle_msg->dbm), new_entry_created);
+            thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
 
             //Parse mandatory TLV's
             if (!thread_leader_data_parse(mle_msg->data_ptr, mle_msg->data_length, &leaderData)) {
                 return;
             }
-            if (!mle_tlv_read_16_bit_tlv(MLE_TYPE_SRC_ADDRESS, mle_msg->data_ptr, mle_msg->data_length, &entry_temp->short_adr)) {
+            if (!mle_tlv_read_16_bit_tlv(MLE_TYPE_SRC_ADDRESS, mle_msg->data_ptr, mle_msg->data_length, &entry_temp->mac16)) {
                 return;
             }
 
@@ -836,25 +842,24 @@ static void thread_mle_child_request_receive_cb(int8_t interface_id, mle_message
                 return;
             }
 
-            common_write_16_bit(entry_temp->short_adr, shortAddress);
+            common_write_16_bit(entry_temp->mac16, shortAddress);
             //Update possible reed address by real router address
-            scan_result->shortAddress = entry_temp->short_adr;
+            scan_result->shortAddress = entry_temp->mac16;
 
-            entry_temp->holdTime = 90;
-            entry_temp->handshakeReady = 1;
-            entry_temp->priorityFlag = true;
-            entry_temp->threadNeighbor = true;
+            entry_temp->connected_device = 1;
+            entry_temp->link_role = PRIORITY_PARENT_NEIGHBOUR;
 
             mac_helper_coordinator_address_set(cur, ADDR_802_15_4_SHORT, shortAddress);
-            mle_entry_timeout_update(entry_temp, thread_info(cur)->host_link_timeout);
+            mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_temp, thread_info(cur)->host_link_timeout);
 
             if (scan_result->security_key_index != security_headers->KeyIndex) {
                 // KeyIndex has been changed between parent_response and child_id_response, reset link layer frame counter
                 scan_result->linLayerFrameCounter = 0;
                 scan_result->security_key_index = security_headers->KeyIndex;
             }
-
-            mac_helper_devicetable_set(entry_temp, cur, scan_result->linLayerFrameCounter, security_headers->KeyIndex, new_entry_created);
+            mlme_device_descriptor_t device_desc;
+            mac_helper_device_description_write(cur, &device_desc, entry_temp->mac64, entry_temp->mac16,scan_result->linLayerFrameCounter, false);
+            mac_helper_devicetable_set(&device_desc, cur, entry_temp->index, security_headers->KeyIndex, new_entry_created);
 
             thread_info(cur)->thread_attached_state = THREAD_STATE_CONNECTED;
 
@@ -948,23 +953,26 @@ static int8_t thread_end_device_synch_start(protocol_interface_info_entry_t *cur
 void thread_endevice_synch_start(protocol_interface_info_entry_t *cur)
 {
     if (cur->thread_info->thread_endnode_parent) {
-        mle_neigh_table_entry_t *entry_temp;
         bool new_entry_created;
 
         // Add the parent to the MLE neighbor table
-        entry_temp = mle_class_get_entry_by_mac64(cur->id, 64, cur->thread_info->thread_endnode_parent->mac64, true, &new_entry_created);
+        mac_neighbor_table_entry_t *mac_entry = mac_neighbor_entry_get_by_mac64(mac_neighbor_info(cur), cur->thread_info->thread_endnode_parent->mac64, true, &new_entry_created);
+        if (mac_entry) {
+            //Add link margin 64
+            thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, mac_entry->index,64, new_entry_created);
+            thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, mac_entry->index);
 
-        if (entry_temp) {
-            entry_temp->short_adr = cur->thread_info->thread_endnode_parent->shortAddress;
-            entry_temp->handshakeReady = 1;
-            entry_temp->threadNeighbor = true;
+            mac_entry->mac16 = cur->thread_info->thread_endnode_parent->shortAddress;
+            mac_entry->connected_device = 1;
 
             // In case we don't get response to sync; use temporary timeout here,
             // Child ID Response handler will set correct value later
-            mle_entry_timeout_update(entry_temp, 20);
+            mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), mac_entry, mac_entry->link_lifetime);
 
             // Add the parent to the MAC table (for e.g. secured/fragmented Child Update Response)
-            mac_helper_devicetable_set(entry_temp, cur, 0, cur->mac_parameters->mac_default_key_index, new_entry_created);
+            mlme_device_descriptor_t device_desc;
+            mac_helper_device_description_write(cur, &device_desc, mac_entry->mac64, mac_entry->mac16,0, false);
+            mac_helper_devicetable_set(&device_desc, cur, mac_entry->index, cur->mac_parameters->mac_default_key_index, new_entry_created);
         }
     }
 
@@ -978,10 +986,9 @@ void thread_endevice_synch_start(protocol_interface_info_entry_t *cur)
 
 static bool thread_child_id_req_timeout(int8_t interface_id, uint16_t msgId, bool usedAllRetries)
 {
-    mle_neigh_table_entry_t *entry_temp;
+    mac_neighbor_table_entry_t *entry_temp;
     thread_scanned_parent_t *scanned_parent;
     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
-    uint8_t ll64[16];
 
     (void)msgId;
 
@@ -1017,15 +1024,17 @@ static bool thread_child_id_req_timeout(int8_t interface_id, uint16_t msgId, boo
     tr_debug("Back to old partition");
 
     /* If scanned parent is from other partition, delete from MLE table */
-    if (scanned_parent->leader_data.partitionId != thread_info(cur)->thread_leader_data->partitionId) {
-        memcpy(ll64, ADDR_LINK_LOCAL_PREFIX , 8);
-        memcpy(&ll64[8], scanned_parent->mac64 , 8);
-        ll64[8] ^= 2;
-
-        entry_temp = mle_class_get_entry_by_ll64(cur->id, scanned_parent->linkMarginToParent, ll64, false, NULL);
-        if (entry_temp && !thread_check_is_this_my_parent(cur, entry_temp)) {
-            // remove scanned_parent entry only if it is not my parent
-            mle_class_remove_entry(cur->id, entry_temp);
+    if ((scanned_parent->leader_data.partitionId != thread_info(cur)->thread_leader_data->partitionId) ||
+        (scanned_parent->leader_data.weighting != thread_info(cur)->thread_leader_data->weighting)) {
+        entry_temp = mac_neighbor_table_address_discover(mac_neighbor_info(cur), scanned_parent->mac64, ADDR_802_15_4_LONG);
+        if (entry_temp) {
+            bool my_parent = thread_check_is_this_my_parent(cur, entry_temp);
+            mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), entry_temp);
+            if (my_parent) {
+                tr_debug("No parent resp - any-attach");
+                thread_bootstrap_connection_error(interface_id, CON_ERROR_NETWORK_ATTACH_FAIL, NULL);
+                goto exit;
+            }
         }
     }
 
@@ -1159,22 +1168,22 @@ static bool thread_child_update_timeout_cb(int8_t interface_id, uint16_t msgId,
 
 }
 
-int8_t thread_host_bootstrap_child_update(int8_t interface_id, const uint8_t *mac64)
+bool thread_host_bootstrap_child_update(protocol_interface_info_entry_t *cur, const uint8_t *mac64)
 {
-    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
     mle_message_timeout_params_t timeout;
     uint8_t mode;
     uint32_t keySequence;
 
 
     if (!cur->thread_info->thread_endnode_parent) {
-        return -1;
+        tr_debug("Not end device parent info for NUD");
+        return false;
     }
 
     if (cur->thread_info->thread_endnode_parent->childUpdateProcessActive) {
         //Set Pending if earlier process is already started
         cur->thread_info->thread_endnode_parent->childUpdatePending = true;
-        return -1;
+        return false;
     }
     //Trig event
     cur->thread_info->thread_endnode_parent->childUpdatePending = false;
@@ -1189,7 +1198,7 @@ int8_t thread_host_bootstrap_child_update(int8_t interface_id, const uint8_t *ma
 
     uint16_t bufId = mle_service_msg_allocate(cur->id, 150 + 3 + 6 + 10, false, MLE_COMMAND_CHILD_UPDATE_REQUEST);
     if (bufId == 0) {
-        return -1;
+        return false;
     }
 
     thread_management_get_current_keysequence(cur->id, &keySequence);
@@ -1228,7 +1237,7 @@ int8_t thread_host_bootstrap_child_update(int8_t interface_id, const uint8_t *ma
     mle_service_set_msg_timeout_parameters(bufId, &timeout);
     mle_service_send_message(bufId);
 
-    return 0;
+    return true;
 }
 int thread_host_bootstrap_child_update_negative_response(protocol_interface_info_entry_t *cur, uint8_t *dstAddress, mle_tlv_info_t *challenge)
 {
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_host_bootstrap.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_host_bootstrap.h
index e154208540..b541154527 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_host_bootstrap.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_host_bootstrap.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -42,7 +42,7 @@ struct protocol_interface_info_entry;
 struct mle_security_header;
 
 void thread_network_attach_start(struct protocol_interface_info_entry *cur);
-int8_t thread_host_bootstrap_child_update(int8_t interface_id, const uint8_t *mac64);
+bool thread_host_bootstrap_child_update(struct protocol_interface_info_entry *cur, const uint8_t *mac64);
 int thread_host_bootstrap_child_update_negative_response(protocol_interface_info_entry_t *cur, uint8_t *dstAddress, mle_tlv_info_t *challenge);
 void thread_child_set_default_route(struct protocol_interface_info_entry *cur);
 
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.c
index 91400d7e40..5bc1b78493 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -778,8 +778,8 @@ static bool thread_joiner_application_validate_settings(thread_joiner_t *this)
         new_value_generated = 1;
         tr_info("Generating Random ML-EID");
     }
-    if (this->configuration_ptr->key_rotation < 3600) {
-        this->configuration_ptr->key_rotation = 3600;
+    if (this->configuration_ptr->key_rotation < 1) {
+        this->configuration_ptr->key_rotation = 1;
     }
     return new_value_generated;
 }
@@ -967,17 +967,12 @@ static int thread_joiner_application_nvm_link_config_read(thread_joiner_t *this)
     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
     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);
+    //Add Security to MLE service
+    thread_security_prev_key_generate(cur,this->configuration_ptr->master_key,this->configuration_ptr->key_sequence);
+    thread_security_key_generate(cur,this->configuration_ptr->master_key,this->configuration_ptr->key_sequence);
+    thread_security_next_key_generate(cur,this->configuration_ptr->master_key,this->configuration_ptr->key_sequence);
+
     // 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);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.h
index 21258690ee..2720e429ce 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_joiner_application.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_leader_service.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_leader_service.c
index d1e21e7f6d..d2be5fd3eb 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_leader_service.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_leader_service.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -414,7 +414,7 @@ static int thread_leader_service_active_set_cb(int8_t service_id, uint8_t source
         }
     }
     if (3 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_SECURITY_POLICY, &ptr)) {
-        if (common_read_16_bit(ptr) < 3600) {
+        if (common_read_16_bit(ptr) < 1) {
             tr_warn("invalid security policy");
             ret = -1;
             goto send_response;
@@ -1090,7 +1090,7 @@ static int thread_leader_service_petition_cb(int8_t service_id, uint8_t source_a
         ptr = thread_meshcop_tlv_data_write_uint8(ptr, MESHCOP_TLV_STATE, 0xff);
     }
 
-    tr_debug("Petition req recv id %s, RESP session id: %d ret %d", commissioner_id_ptr, session_id, ret);
+    tr_debug("Petition req recv id %s, RESP session id: %d ret %d", commissioner_id_ptr ? commissioner_id_ptr : "(none)", session_id, ret);
 
 send_error_response:
     coap_service_response_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, request_ptr, response_code, COAP_CT_OCTET_STREAM, payload, ptr - payload);
@@ -1346,7 +1346,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_partition_data_purge(cur);
     // remove any existing rloc mapping in nvm
     thread_nvm_store_mleid_rloc_map_remove();
     cur->lowpan_address_mode = NET_6LOWPAN_GP16_ADDRESS;
@@ -1526,7 +1526,7 @@ void thread_leader_service_timer(protocol_interface_info_entry_t *cur, uint32_t
         cur->thread_info->leader_private_data->leader_id_seq_timer = ID_SEQUENCE_PERIOD;
 
         thread_leader_service_update_id_set(cur);
-        thread_network_data_context_re_use_timer_update(&cur->thread_info->networkDataStorage, 10, &cur->lowpan_contexts);
+        thread_network_data_context_re_use_timer_update(cur->id, &cur->thread_info->networkDataStorage, 10, &cur->lowpan_contexts);
         // Send update to network data if needed
         thread_leader_service_network_data_changed(cur, false, false);
     }
@@ -1621,7 +1621,6 @@ void thread_leader_service_thread_partitition_generate(int8_t interface_id, bool
     //memset(cur->thread_info->lastValidRouteMask, 0, (N_THREAD_ROUTERS+7)/8);
     //SET SHort Address & PAN-ID
     cur->lowpan_info &= ~INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE;
-    mle_class_mode_set(cur->id, MLE_CLASS_ROUTER);
     mac_helper_pib_boolean_set(cur, macRxOnWhenIdle, true);
     cur->interface_mode = INTERFACE_UP;
     cur->nwk_mode = ARM_NWK_GP_IP_MODE;
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_leader_service.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_leader_service.h
index 7eabcb3028..a28bb9202f 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_leader_service.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_leader_service.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_client.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_client.c
index 3d1f7a812f..0ccae5a3ac 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_client.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_client.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -146,6 +146,8 @@ void thread_management_client_init(int8_t interface_id)
 
     if (this) {
         this->network_data_set_cb_ptr = NULL;
+        this->router_id_release_cb_ptr = NULL;
+        this->neighbor_discovery_cb_ptr = NULL;
         this->router_id_cb_ptr = NULL;
         this->interface_id = interface_id;
         this->coap_asd_msg_id = 0;
@@ -333,6 +335,15 @@ static int thread_management_client_neighbor_discovery_data_cb(int8_t service_id
     return 0;
 }
 
+int thread_management_client_get_interface_id_by_service_id(int8_t service_id)
+{
+    thread_management_t *this = thread_management_find_by_service(service_id);
+    if (!this) {
+        return -1;
+    }
+    return this->interface_id;
+}
+
 int thread_management_client_neighbor_discovery_data_request(int8_t interface_id, const uint8_t destination[16],  const uint8_t *options, uint8_t options_len, thread_management_client_network_data_set_cb *set_cb)
 {
     thread_management_t *this = thread_management_find(interface_id);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_client.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_client.h
index d08a95d740..583b6fcec4 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_client.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_client.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -148,6 +148,15 @@ int thread_management_client_network_data_unregister(int8_t interface_id, uint16
  */
 int thread_management_client_neighbor_discovery_data_request(int8_t interface_id, const uint8_t destination[16],  const uint8_t *options, uint8_t options_len, thread_management_client_network_data_set_cb *set_cb);
 
+/** Get interface_id of based on coap service_id
+ *
+ *
+ * /param service_id coap service id.
+ *
+ *return interface_id of thread instance if successful and -1 for failure
+ */
+int thread_management_client_get_interface_id_by_service_id(int8_t service_id);
+
 /** send active configuration dataset get for parent
  *
  * /param interface_id interface id of this thread instance.
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_if.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_if.c
index 58ea54aea3..767bb35538 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_if.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_if.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -62,6 +62,7 @@
 #include "6LoWPAN/Thread/thread_tmfcop_lib.h"
 #include "6LoWPAN/Thread/thread_constants.h"
 #include "6LoWPAN/Thread/thread_extension_bootstrap.h"
+#include "6LoWPAN/Thread/thread_extension.h"
 #include "6LoWPAN/Bootstraps/protocol_6lowpan.h"
 #include "RPL/rpl_control.h" // insanity - bootstraps shouldn't be doing each others' clean-up
 #include "MLE/mle.h"
@@ -77,7 +78,9 @@
 #include "Service_Libs/blacklist/blacklist.h"
 #include "6LoWPAN/MAC/mac_helper.h"
 #include "6LoWPAN/MAC/mac_pairwise_key.h"
+#include "6LoWPAN/MAC/mpx_api.h"
 #include "6LoWPAN/lowpan_adaptation_interface.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 #include "mac_common_defines.h"
 #include "mlme.h"
 #include "mac_api.h"
@@ -307,68 +310,6 @@ void thread_key_get(uint8_t *key, uint8_t *key_material_buf,  uint32_t key_seque
     SHALIB_finish_HMAC(key_material_buf, 8);
 }
 
-/**
- * Increment Thread key sequence counter
- *
- * \param interface_id Network Interface
- *
- * return 0, ADD OK
- * return <0 Add Not OK
- */
-uint8_t *thread_management_key_request(int8_t interface_id, uint8_t keyId)
-{
-    protocol_interface_info_entry_t *cur;
-    uint8_t *keyPtr = NULL;
-    link_configuration_s *linkConfiguration;
-    linkConfiguration = thread_joiner_application_get_config(interface_id);
-    if (!linkConfiguration) {
-        return NULL;
-    }
-    cur = protocol_stack_interface_info_get_by_id(interface_id);
-    if (cur && cur->thread_info) {
-        if (cur->thread_info->masterSecretMaterial.valid_Info) {
-
-            if (cur->thread_info->masterSecretMaterial.historyKeyValid && (cur->thread_info->masterSecretMaterial.historyKeyId == keyId)) {
-
-                keyPtr = cur->thread_info->masterSecretMaterial.historyKey;
-
-            } else {
-                uint32_t thrSequenceCounter;
-                uint8_t compId = 0xff, keyIdDiff;
-                thrSequenceCounter = linkConfiguration->key_sequence + 1;
-                //Calculate Current Next key ID
-                compId = THREAD_KEY_INDEX(thrSequenceCounter);
-
-                if (keyId > compId) {
-                    keyIdDiff = (keyId - compId);
-                } else {
-                    keyIdDiff = (128 - (compId - keyId));
-                }
-
-                if (keyIdDiff > 64) {
-                    //Calc Temp Key
-                    thrSequenceCounter -= (128 - keyIdDiff);
-                    tr_debug("Gen temporary key id %"PRIu8" seq %"PRIu32, keyId, thrSequenceCounter);
-                    thread_key_get(linkConfiguration->master_key, cur->thread_info->masterSecretMaterial.historyKey, thrSequenceCounter);
-                    cur->thread_info->masterSecretMaterial.historyKeyId = keyId;
-                    cur->thread_info->masterSecretMaterial.historyKeyValid = true;
-                    keyPtr = cur->thread_info->masterSecretMaterial.historyKey;
-                } else {
-                    tr_debug("Gen new key id %"PRIu8" %"PRIu8" diff", keyId, keyIdDiff);
-                    thrSequenceCounter += keyIdDiff;
-                    //Generate
-                    tr_debug("Missed n key Update...generated missed key %"PRIu8" update by seq %"PRIu32, keyIdDiff, thrSequenceCounter);
-                    thread_management_key_sets_calc(cur, linkConfiguration, thrSequenceCounter);
-                    //Get Default key
-                    keyPtr =  mle_service_security_default_key_get(interface_id);
-                }
-                thread_nvm_store_seq_counter_write(linkConfiguration->key_sequence);
-            }
-        }
-    }
-    return keyPtr;
-}
-
 /**
  * Thread key sequence & key synch
  *
@@ -381,22 +322,32 @@ void thread_management_key_synch_req(int8_t interface_id, uint32_t keySequnce)
     if (!linkConfiguration) {
         return;
     }
-
     cur = protocol_stack_interface_info_get_by_id(interface_id);
-    if (cur && cur->thread_info) {
-        if (cur->thread_info->masterSecretMaterial.valid_Info) {
-            if (keySequnce != linkConfiguration->key_sequence) {
-                if ((cur->thread_info->masterSecretMaterial.keySwitchGuardTimer == 0 && keySequnce > linkConfiguration->key_sequence)) {
-                    tr_debug("Sync key material by %"PRIu32, keySequnce);
-                    thread_management_key_sets_calc(cur, linkConfiguration, keySequnce);
-                    thread_calculate_key_guard_timer(cur, linkConfiguration, false);
-                }
-            }
-        }
+    if (!cur || !cur->thread_info) {
+        return;
     }
+    if (!cur->thread_info->masterSecretMaterial.valid_Info) {
+        return;
+    }
+    //tr_debug("Sync key material by %"PRIu32, keySequnce);
+
+    if (keySequnce <= linkConfiguration->key_sequence) {
+        // Smaller or equal request ignored
+        //tr_debug("Sync key material no change");
+        return;
+    }
+    if ((cur->thread_info->masterSecretMaterial.keySwitchGuardTimer > 0)) {
+        // Guard time prevent the change
+        //tr_debug("Sync key material guard blocked");
+        return;
+    }
+    // Calculate new keys
+    tr_debug("Sync key material by %"PRIu32, keySequnce);
+    thread_management_key_sets_calc(cur, linkConfiguration, keySequnce);
+    thread_key_guard_timer_calculate(cur, linkConfiguration, false);
 }
 
-void thread_history_key_material_push(thread_info_t *thread_info, uint8_t *mleKeyPtr, uint8_t keyId)
+static void thread_history_key_material_push(thread_info_t *thread_info, uint8_t *mleKeyPtr, uint8_t keyId)
 {
     thread_info->masterSecretMaterial.historyKeyValid = true;
     thread_info->masterSecretMaterial.historyKeyId = keyId;
@@ -424,7 +375,7 @@ void thread_security_update_from_mac(protocol_interface_info_entry_t *cur)
     linkConfiguration->key_sequence++;
     mac_helper_link_frame_counter_set(cur->id, 0);
     thread_security_next_key_generate(cur, linkConfiguration->master_key, linkConfiguration->key_sequence);
-    thread_calculate_key_guard_timer(cur, linkConfiguration, false);
+    thread_key_guard_timer_calculate(cur, linkConfiguration, false);
 }
 
 void thread_security_key_generate(protocol_interface_info_entry_t *cur, uint8_t *masterKey, uint32_t keySequence)
@@ -437,11 +388,33 @@ void thread_security_key_generate(protocol_interface_info_entry_t *cur, uint8_t
     thread_key_get(masterKey, key_material, thrKeySequenceCounter);
     /* Update keys as primary keys */
     key_index = THREAD_KEY_INDEX(thrKeySequenceCounter);
-    tr_debug("Set key Id: %u", key_index);
+    tr_debug("Set current key Id: %u", key_index);
     mac_helper_security_default_key_set(cur, &key_material[16], key_index, MAC_KEY_ID_MODE_IDX);
     mle_service_security_set_security_key(cur->id, key_material, key_index, true);
 }
 
+
+void thread_security_prev_key_generate(protocol_interface_info_entry_t *cur, uint8_t *masterKey, uint32_t keySequence)
+{
+    uint8_t key_material[32];
+    uint8_t key_index;
+    uint32_t thrKeySequenceCounter;
+    if (keySequence == 0) {
+        // in initial value there is no prev available
+        return;
+    }
+    thrKeySequenceCounter = keySequence - 1;
+   /* Produced keys from Thread security material: MAC key | MLE key */
+    thread_key_get(masterKey, key_material, thrKeySequenceCounter);
+    /* Update keys as primary keys */
+    key_index = THREAD_KEY_INDEX(thrKeySequenceCounter);
+    tr_debug("Set previous key Id: %u", key_index);
+    mac_helper_security_prev_key_set(cur, &key_material[16], key_index, MAC_KEY_ID_MODE_IDX);
+    mle_service_security_set_security_key(cur->id, key_material, key_index, false);
+    //copy master secret material to history
+    thread_history_key_material_push(cur->thread_info, key_material, key_index);
+}
+
 void thread_security_next_key_generate(protocol_interface_info_entry_t *cur, uint8_t *masterKey, uint32_t keySequence)
 {
     uint8_t key_material[32];
@@ -471,31 +444,17 @@ int thread_management_key_sets_calc(protocol_interface_info_entry_t *cur, link_c
         ret_val = 0;
     } else {
         /* Generate new key set */
-        uint8_t key_material[32];
-        uint8_t key_index;
-        thread_nvm_fast_data_t fast_data;
-        //SET History key, Current Key & Next Key
-        //Generate History Key
         //Clean All Keys
         mac_helper_security_key_clean(cur);
-        thrKeySequenceCounter--;
-        /* Update keys as non-primary keys */
-        thread_key_get(linkConfiguration->master_key, key_material, thrKeySequenceCounter);
-        key_index = THREAD_KEY_INDEX(thrKeySequenceCounter);
-        //copy master secret material to history
-        memcpy(cur->thread_info->masterSecretMaterial.historyKey, key_material, 32);
-        cur->thread_info->masterSecretMaterial.historyKeyId = key_index;
-        cur->thread_info->masterSecretMaterial.historyKeyValid = true;
 
-        mac_helper_security_prev_key_set(cur, &key_material[16], key_index, MAC_KEY_ID_MODE_IDX);
-        //Set New Active Key
-        thrKeySequenceCounter++;
+        // Update key sequence
         linkConfiguration->key_sequence = thrKeySequenceCounter;
-        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_write(&fast_data);
+        // Zero all frame counters. MLE does it automatically
         mac_helper_link_frame_counter_set(cur->id, 0);
+        // Store all frame counters as zero and update the sequence counter
+        thread_nvm_store_fast_data_write_all(0, 0, thrKeySequenceCounter);
+
+        thread_security_prev_key_generate(cur,linkConfiguration->master_key,linkConfiguration->key_sequence);
         thread_security_key_generate(cur,linkConfiguration->master_key,linkConfiguration->key_sequence);
         thread_security_next_key_generate(cur,linkConfiguration->master_key,linkConfiguration->key_sequence);
         ret_val = 0;
@@ -790,12 +749,23 @@ int thread_dhcpv6_server_delete(int8_t interface_id, uint8_t *prefix_ptr)
 }
 
 #ifdef HAVE_THREAD
+static mac_neighbor_table_entry_t *neighbor_data_poll_referesh(protocol_interface_info_entry_t *cur, const uint8_t *address, addrtype_t type)
+{
+    mac_neighbor_table_entry_t *entry = mac_neighbor_table_address_discover(mac_neighbor_info(cur), address, type);
+    if (!entry || !entry->connected_device) {
+        return NULL;
+    }
+
+    entry->lifetime = entry->link_lifetime;
+    return entry;
+}
 void thread_comm_status_indication_cb(int8_t if_id, const mlme_comm_status_t* status)
 {
     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(if_id);
     if (!cur) {
         return;
     }
+    mac_neighbor_table_entry_t *entry;
 
     switch (status->status) {
 
@@ -808,7 +778,10 @@ void thread_comm_status_indication_cb(int8_t if_id, const mlme_comm_status_t* st
             break;
 
         case MLME_DATA_POLL_NOTIFICATION:
-            mle_refresh_entry_timeout(if_id, status->SrcAddr, (addrtype_t)status->SrcAddrMode, false);
+            entry = neighbor_data_poll_referesh(cur, status->SrcAddr, (addrtype_t)status->SrcAddrMode);
+            if (entry) {
+                thread_neighbor_communication_update(cur, entry->index);
+            }
             break;
         default:
             break;
@@ -908,7 +881,7 @@ int thread_management_node_init(
     scan_params->scan_duration = 5;
 
     cur->thread_info->masterSecretMaterial.valid_Info = false;
-    thread_calculate_key_guard_timer(cur, static_configuration, true);
+    thread_key_guard_timer_calculate(cur, static_configuration, true);
 
     cur->thread_info->maxChildCount = THREAD_MAX_CHILD_COUNT;
     cur->thread_info->rfc6775 = false;
@@ -1439,10 +1412,21 @@ int thread_management_partition_weighting_set(int8_t interface_id, uint8_t parti
         return 0;
     }
 
+    bool trig_network_scan = false;
+    if (cur->thread_info->thread_leader_data) {
+        if (cur->thread_info->thread_leader_data->weighting < partition_weighting) {
+            trig_network_scan = true;
+        }
+    }
+
     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);
+        if (trig_network_scan && thread_extension_enabled(cur)) {
+            thread_nvm_store_link_info_clear();
+            // bootstrap active and weighting has changed
+            thread_bootstrap_reset_restart(interface_id);
+        }
     }
 
     return 0;
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_internal.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_internal.h
index 0d3dc94f40..3e3dac832f 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_internal.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_internal.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -77,15 +77,13 @@ int thread_management_increment_key_sequence_counter(int8_t interface_id);
 
 int thread_management_key_sets_calc(protocol_interface_info_entry_t *cur, link_configuration_s *linkConfiguration, uint32_t thrKeySequenceCounter);
 
-uint8_t *thread_management_key_request(int8_t interface_id, uint8_t keyId);
-
 void thread_management_key_synch_req(int8_t interface_id, uint32_t keySequnce);
 uint8_t *thread_management_key_request_with_sequence(int8_t interface_id, uint8_t keyId, uint32_t keySequnce);
 
 void thread_security_update_from_mac(struct protocol_interface_info_entry *cur);
 
-void thread_history_key_material_push(struct thread_info_s *thread_info, uint8_t *mleKeyPtr, uint8_t keyId);
-
+void thread_security_key_generate(struct protocol_interface_info_entry *cur, uint8_t *masterKey, uint32_t keySequence);
+void thread_security_prev_key_generate(struct protocol_interface_info_entry *cur, uint8_t *masterKey, uint32_t keySequence);
 void thread_security_next_key_generate(struct protocol_interface_info_entry *cur, uint8_t *masterKey, uint32_t keySequence);
 
 void thread_key_get(uint8_t *key, uint8_t *key_material_buf,  uint32_t key_sequence_counter);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_server.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_server.c
index 329c26c14a..8a4ae139fd 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_server.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_management_server.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -1159,7 +1159,6 @@ int thread_management_server_init(int8_t interface_id)
     }
 #endif
     thread_extension_init(interface_id, this->coap_service_id);
-
     // All thread devices
     coap_service_register_uri(this->coap_service_id, THREAD_URI_MANAGEMENT_GET, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_management_server_get_command_cb);
 
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_mle_message_handler.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_mle_message_handler.c
index fee1e22350..2c108ee6e8 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_mle_message_handler.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_mle_message_handler.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -38,6 +38,7 @@
 #include "6LoWPAN/Thread/thread_config.h"
 #include "6LoWPAN/Thread/thread_common.h"
 #include "6LoWPAN/Thread/thread_lowpower_private_api.h"
+#include "6LoWPAN/Thread/thread_neighbor_class.h"
 #include "6LoWPAN/Thread/thread_mle_message_handler.h"
 #include "6LoWPAN/Thread/thread_bootstrap.h"
 #include "6LoWPAN/Thread/thread_management_internal.h"
@@ -48,13 +49,15 @@
 #include "6LoWPAN/Thread/thread_extension.h"
 #include "6LoWPAN/Thread/thread_router_bootstrap.h"
 #include "6LoWPAN/Thread/thread_network_synch.h"
+#include "6LoWPAN/Thread/thread_neighbor_class.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 #include "6LoWPAN/MAC/mac_helper.h"
 #include "6LoWPAN/MAC/mac_data_poll.h"
 #include "MLE/mle.h"
 #include "mac_api.h"
 #define TRACE_GROUP "thmh"
 static int8_t thread_link_request_start(protocol_interface_info_entry_t *cur, uint8_t *router_ll64);
-static bool thread_router_leader_data_process(protocol_interface_info_entry_t *cur, uint8_t *src_address, thread_leader_data_t *leaderData, mle_tlv_info_t *routeTlv, mle_neigh_table_entry_t *neighbor);
+static bool thread_router_leader_data_process(protocol_interface_info_entry_t *cur, uint8_t *src_address, thread_leader_data_t *leaderData, mle_tlv_info_t *routeTlv, mac_neighbor_table_entry_t *neighbor);
 static void thread_parse_advertisement(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, mle_security_header_t *security_headers, uint8_t linkMargin);
 static void thread_save_leader_data(protocol_interface_info_entry_t *cur, thread_leader_data_t *leaderData);
 static void thread_parse_accept(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, mle_security_header_t *security_headers, uint8_t linkMargin);
@@ -72,11 +75,7 @@ void thread_general_mle_receive_cb(int8_t interface_id, mle_message_t *mle_msg,
      *   and MUST be discarded.
      */
 
-    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
-
-    if (!cur) {
-        return;
-    }
+    protocol_interface_info_entry_t *cur = mle_msg->interface_ptr;
 
     /* Check that message is from link-local scope */
     if(!addr_is_ipv6_link_local(mle_msg->packet_src_address)) {
@@ -100,11 +99,11 @@ void thread_general_mle_receive_cb(int8_t interface_id, mle_message_t *mle_msg,
     }
 
     case MLE_COMMAND_REJECT: {
-        mle_neigh_table_entry_t *entry_temp;
+        mac_neighbor_table_entry_t *entry_temp;
         tr_warn("Reject Link");
-        entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, false, NULL);
+        entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, false, NULL);
         if (entry_temp) {
-            mle_class_remove_entry(cur->id, entry_temp);
+            mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), entry_temp);
         }
         break;
     }
@@ -159,16 +158,11 @@ static void thread_save_leader_data(protocol_interface_info_entry_t *cur, thread
             requestNetworkdata = true;
         }
 
-        if (thread_info(cur)->thread_leader_data->partitionId != leaderData->partitionId) {
+        if (!thread_partition_match(cur, leaderData)) {
             requestNetworkdata = true;
-            thread_info(cur)->thread_leader_data->stableDataVersion = leaderData->stableDataVersion - 1;
-            thread_info(cur)->thread_leader_data->dataVersion = leaderData->dataVersion - 1;
+            thread_partition_info_update(cur, leaderData);
         }
 
-        thread_info(cur)->thread_leader_data->partitionId = leaderData->partitionId;
-        thread_info(cur)->thread_leader_data->leaderRouterId = leaderData->leaderRouterId;
-        thread_info(cur)->thread_leader_data->weighting = leaderData->weighting;
-
         if (requestNetworkdata) {
             thread_bootstrap_parent_network_data_request(cur, false);
         } else {
@@ -215,12 +209,12 @@ static int8_t thread_link_request_start(protocol_interface_info_entry_t *cur, ui
     return 0;
 }
 
-static bool thread_router_leader_data_process(protocol_interface_info_entry_t *cur, uint8_t *src_address, thread_leader_data_t *leaderData, mle_tlv_info_t *routeTlv, mle_neigh_table_entry_t *neighbor)
+static bool thread_router_leader_data_process(protocol_interface_info_entry_t *cur, uint8_t *src_address, thread_leader_data_t *leaderData, mle_tlv_info_t *routeTlv, mac_neighbor_table_entry_t *neighbor)
 {
     int leaderDataUpdate = thread_leader_data_validation(cur, leaderData, routeTlv);
 
     if (leaderDataUpdate == 1) {
-        if (neighbor && neighbor->handshakeReady == 1) {
+        if (neighbor && neighbor->connected_device == 1) {
             // Request network data if we have a 2-way link
             tr_debug("Request New Network Data from %s", trace_ipv6(src_address));
             thread_network_data_request_send(cur, src_address, true);
@@ -272,30 +266,32 @@ static bool thread_router_advertiment_tlv_analyze(uint8_t *ptr, uint16_t data_le
     return true;
 }
 
-static void thread_update_mle_entry(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, mle_security_header_t *security_headers, mle_neigh_table_entry_t *entry_temp, uint16_t short_address)
+static void thread_update_mle_entry(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, mle_security_header_t *security_headers, mac_neighbor_table_entry_t *entry_temp, uint16_t short_address)
 {
     if (!entry_temp) {
         return;
     }
 
+    uint8_t linkMargin = thread_compute_link_margin(mle_msg->dbm);
     mle_neigh_entry_frame_counter_update(entry_temp, mle_msg->data_ptr, mle_msg->data_length, cur, security_headers->KeyIndex);
 
+    thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index,linkMargin, false);
+    thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
+
     if (!thread_attach_active_router(cur) && !thread_check_is_this_my_parent(cur, entry_temp)) {
-        mle_entry_timeout_refresh(entry_temp);
-    } else {
-        entry_temp->last_contact_time = protocol_core_monotonic_time;
+        mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_temp, entry_temp->link_lifetime);
     }
 
-    if (short_address != entry_temp->short_adr) {
-        if (thread_router_addr_from_addr(entry_temp->short_adr) == cur->thread_info->routerShortAddress) {
+    if (short_address != entry_temp->mac16) {
+        if (thread_router_addr_from_addr(entry_temp->mac16) == cur->thread_info->routerShortAddress) {
             thread_dynamic_storage_child_info_clear(cur->id, entry_temp);
-            protocol_6lowpan_release_short_link_address_from_neighcache(cur, entry_temp->short_adr);
+            protocol_6lowpan_release_short_link_address_from_neighcache(cur, entry_temp->mac16);
         }
-        entry_temp->short_adr = short_address;
+        entry_temp->mac16 = short_address;
         /* throw MLME_GET request, short address is changed automatically in get request callback */
         mlme_get_t get_req;
         get_req.attr = macDeviceTable;
-        get_req.attr_index = entry_temp->attribute_index;
+        get_req.attr_index = entry_temp->index;
         cur->mac_api->mlme_req(cur->mac_api, MLME_GET, &get_req);
     }
 
@@ -304,11 +300,6 @@ static void thread_update_mle_entry(protocol_interface_info_entry_t *cur, mle_me
 
 static bool thread_parse_advertisement_from_parent(protocol_interface_info_entry_t *cur, thread_leader_data_t *leader_data, uint16_t short_address)
 {
-    if ((thread_info(cur)->thread_leader_data->partitionId != leader_data->partitionId) ||
-        (thread_info(cur)->thread_leader_data->weighting != leader_data->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, leader_data, short_address)) {
         tr_debug("Parent short address changed - re-attach");
@@ -323,7 +314,7 @@ static void thread_parse_advertisement(protocol_interface_info_entry_t *cur, mle
 {
     mle_tlv_info_t routeTlv;
     thread_leader_data_t leaderData;
-    mle_neigh_table_entry_t *entry_temp;
+    mac_neighbor_table_entry_t *entry_temp;
     uint16_t shortAddress;
     bool adv_from_my_partition;
     bool my_parent;
@@ -342,12 +333,16 @@ static void thread_parse_advertisement(protocol_interface_info_entry_t *cur, mle
     }
 
     // Get MLE entry
-    entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, false, NULL);
+    entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, false, NULL);
+    if (entry_temp) {
+        thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index, linkMargin, false);
+        thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
+    }
 
     // Check if this is from my parent
     my_parent = thread_check_is_this_my_parent(cur, entry_temp);
 
-    adv_from_my_partition = thread_instance_id_matches(cur, &leaderData);
+    adv_from_my_partition = thread_partition_match(cur, &leaderData);
 
     if ((security_headers->KeyIdMode == MAC_KEY_ID_MODE_SRC4_IDX) && adv_from_my_partition) {
         thread_management_key_synch_req(cur->id, common_read_32_bit(security_headers->Keysource));
@@ -355,13 +350,14 @@ static void thread_parse_advertisement(protocol_interface_info_entry_t *cur, mle
 
     if (entry_temp && !adv_from_my_partition && !my_parent ) {
         // Remove MLE entry that are located in other partition and is not my parent
-        mle_class_remove_entry(cur->id, entry_temp);
+        mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), entry_temp);
         entry_temp = NULL;
     }
 
     /* Check parent status */
     if (!thread_attach_active_router(cur) && my_parent) {
         if (!thread_parse_advertisement_from_parent(cur, &leaderData, shortAddress)) {
+            mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), entry_temp);
             return;
         }
     }
@@ -374,7 +370,7 @@ static void thread_parse_advertisement(protocol_interface_info_entry_t *cur, mle
         /* REED and FED */
         if (!thread_attach_active_router(cur)) {
             /* Check if advertisement is from same partition */
-            if (thread_info(cur)->thread_leader_data->weighting == leaderData.weighting && thread_info(cur)->thread_leader_data->partitionId == leaderData.partitionId ) {
+            if (thread_partition_match(cur, &leaderData)) {
                 if (!entry_temp && thread_bootstrap_link_create_check(cur, shortAddress) && thread_bootstrap_link_create_allowed(cur, shortAddress, mle_msg->packet_src_address)) {
                     // Create link to new neighbor no other processing allowed
                     thread_link_request_start(cur, mle_msg->packet_src_address);
@@ -404,9 +400,8 @@ static void thread_parse_advertisement(protocol_interface_info_entry_t *cur, mle
     }
 
     // Process route TLV
-    if ((entry_temp && routeTlv.dataPtr && routeTlv.tlvLen) &&
-            (thread_info(cur)->thread_leader_data->partitionId == leaderData.partitionId)){
-        tr_debug("Update Route TLV %x", entry_temp->short_adr);
+    if ((entry_temp && routeTlv.dataPtr && routeTlv.tlvLen) && thread_partition_match(cur, &leaderData)){
+        tr_debug("Update Route TLV %x", entry_temp->mac16);
         thread_router_bootstrap_route_tlv_push(cur, routeTlv.dataPtr, routeTlv.tlvLen , linkMargin, entry_temp);
     }
 }
@@ -417,7 +412,7 @@ static void thread_parse_accept(protocol_interface_info_entry_t *cur, mle_messag
     uint16_t version, shortAddress;
     uint16_t messageId;
     uint8_t linkMarginfronNeigh;
-    mle_neigh_table_entry_t *entry_temp;
+    mac_neighbor_table_entry_t *entry_temp;
     bool createNew, new_entry_created;
 
     tr_info("MLE LINK ACCEPT");
@@ -444,12 +439,14 @@ static void thread_parse_accept(protocol_interface_info_entry_t *cur, mle_messag
     /* Call to determine whether or not we should create a new link */
     createNew = thread_bootstrap_link_create_check(cur, shortAddress);
 
-    entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, createNew, &new_entry_created);
+    entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, createNew, &new_entry_created);
 
     if (!entry_temp) {
         thread_link_reject_send(cur, mle_msg->packet_src_address);
         return;
     }
+    thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index, linkMargin, new_entry_created);
+    thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
 
     if (security_headers->KeyIdMode == MAC_KEY_ID_MODE_SRC4_IDX) {
         thread_management_key_synch_req(cur->id, common_read_32_bit(security_headers->Keysource));
@@ -460,32 +457,35 @@ static void thread_parse_accept(protocol_interface_info_entry_t *cur, mle_messag
         mleFrameCounter = llFrameCounter;
     }
 
-    entry_temp->threadNeighbor = true;
-    entry_temp->short_adr = shortAddress;
-    entry_temp->mle_frame_counter = mleFrameCounter;
+    entry_temp->mac16 = shortAddress;
+    mle_service_frame_counter_entry_add(cur->id, entry_temp->index, mleFrameCounter);
     // Set full data as REED needs full data and SED will not make links
-    entry_temp->mode |= MLE_THREAD_REQ_FULL_DATA_SET;
+    thread_neighbor_class_request_full_data_setup_set(&cur->thread_info->neighbor_class, entry_temp->index, true);
+    mlme_device_descriptor_t device_desc;
+    mac_helper_device_description_write(cur, &device_desc, entry_temp->mac64, entry_temp->mac16,llFrameCounter, false);
+    mac_helper_devicetable_set(&device_desc, cur, entry_temp->index, security_headers->KeyIndex, new_entry_created);
+    uint32_t timeout;
 
-    mac_helper_devicetable_set(entry_temp, cur, llFrameCounter, security_headers->KeyIndex, new_entry_created);
-
-    if (entry_temp->timeout_rx) {
-        mle_entry_timeout_refresh(entry_temp);
+    if (new_entry_created) {
+        timeout = THREAD_DEFAULT_LINK_LIFETIME;
     } else {
-        mle_entry_timeout_update(entry_temp, THREAD_DEFAULT_LINK_LIFETIME);
+        timeout = entry_temp->link_lifetime;
     }
 
-    if (thread_i_am_router(cur) && thread_is_router_addr(entry_temp->short_adr)) {
+    mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_temp, timeout);
+
+    if (thread_i_am_router(cur) && thread_is_router_addr(entry_temp->mac16)) {
         // If we both are routers, mark the link as 2-way
-        entry_temp->handshakeReady = 1;
-        tr_debug("Marked link as 2-way, handshakeReady=%d", entry_temp->handshakeReady);
+        entry_temp->connected_device = 1;
+        tr_debug("Marked link as 2-way, handshakeReady=%d", entry_temp->connected_device);
     } else {
-        tr_debug("Marked link as 1-way, handshakeReady=%d", entry_temp->handshakeReady);
+        tr_debug("Marked link as 1-way, handshakeReady=%d", entry_temp->connected_device);
     }
 
     blacklist_update(mle_msg->packet_src_address, true);
 
     if (mle_tlv_read_8_bit_tlv(MLE_TYPE_RSSI, mle_msg->data_ptr, mle_msg->data_length, &linkMarginfronNeigh)) {
-        thread_routing_update_link_margin(cur, entry_temp->short_adr, linkMargin, linkMarginfronNeigh);
+        thread_routing_update_link_margin(cur, entry_temp->mac16, linkMargin, linkMarginfronNeigh);
     }
 }
 static void thread_parse_annoucement(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg)
@@ -544,7 +544,7 @@ static void thread_parse_data_response(protocol_interface_info_entry_t *cur, mle
     mle_tlv_info_t ConfigurationTlv;
     uint64_t active_timestamp = 0;
     uint64_t pending_timestamp = 0;// means no pending timestamp
-    mle_neigh_table_entry_t *entry_temp;
+    mac_neighbor_table_entry_t *entry_temp;
     bool accept_new_data = false;
     bool leaderDataReceived;
 
@@ -560,7 +560,12 @@ static void thread_parse_data_response(protocol_interface_info_entry_t *cur, mle
         return;
     }
 
-    entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, false, NULL);
+    entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, false, NULL);
+
+    if (entry_temp) {
+        thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index, linkMargin, false);
+        thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
+    }
 
     if(cur->thread_info->thread_device_mode == THREAD_DEVICE_MODE_ROUTER ||
             cur->thread_info->thread_device_mode == THREAD_DEVICE_MODE_FULL_END_DEVICE) {
@@ -570,7 +575,7 @@ static void thread_parse_data_response(protocol_interface_info_entry_t *cur, mle
                 return;
             }
         } else {
-            if (thread_info(cur)->thread_leader_data->partitionId != leaderData.partitionId) {
+            if (!thread_partition_match(cur, &leaderData)) {
                 // if receiving data response from different partition it is dropped
                 return;
             }
@@ -583,10 +588,8 @@ 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);
+    if (!thread_partition_match(cur, &leaderData)) {
+        thread_partition_info_update(cur, &leaderData);
         accept_new_data = true;
     }
 
@@ -705,11 +708,11 @@ static void thread_host_child_update_request_process(protocol_interface_info_ent
     mle_tlv_info_t tlv_req;
     uint64_t active_timestamp = 0;
     uint64_t pending_timestamp = 0;// means no pending timestamp
-    mle_neigh_table_entry_t *entry_temp;
+    mac_neighbor_table_entry_t *entry_temp;
     bool data_request_needed = false;
 
     tr_debug("Child update request");
-    entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, false, NULL);
+    entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, false, NULL);
 
     if (!thread_leader_data_parse(mle_msg->data_ptr, mle_msg->data_length, &leaderData) ||
         !entry_temp ||
@@ -718,14 +721,16 @@ static void thread_host_child_update_request_process(protocol_interface_info_ent
         tr_warn("invalid message");
         return;
     }
+
+    thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index, linkMargin, false);
+    thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
+
     mle_tlv_read_tlv(MLE_TYPE_CHALLENGE, mle_msg->data_ptr, mle_msg->data_length, &challengeTlv);
     mle_tlv_read_tlv(MLE_TYPE_TLV_REQUEST, mle_msg->data_ptr, mle_msg->data_length, &tlv_req);
 
     // Check if partition changed
-    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);
+    if (!thread_partition_match(cur, &leaderData)) {
+        thread_partition_info_update(cur, &leaderData);
     }
     //Check Network Data TLV
     if (mle_tlv_read_tlv(MLE_TYPE_NETWORK_DATA, mle_msg->data_ptr, mle_msg->data_length, &networkDataTlv)) {
@@ -767,7 +772,7 @@ static void thread_parse_child_update_response(protocol_interface_info_entry_t *
 {
     uint8_t mode;
     uint32_t timeout;
-    mle_neigh_table_entry_t *entry_temp;
+    mac_neighbor_table_entry_t *entry_temp;
     thread_leader_data_t leaderData = {0};
     uint8_t status;
     bool leader_data_received;
@@ -780,11 +785,12 @@ static void thread_parse_child_update_response(protocol_interface_info_entry_t *
 
     //mle_service_buffer_find
     leader_data_received = thread_leader_data_parse(mle_msg->data_ptr, mle_msg->data_length, &leaderData);
-    entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, false, NULL);
+    entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, false, NULL);
 
     if (mle_tlv_read_8_bit_tlv(MLE_TYPE_STATUS, mle_msg->data_ptr, mle_msg->data_length, &status) &&
         status == 1 && thread_check_is_this_my_parent(cur, entry_temp)) {
         tr_debug("parent has connection error");
+        mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), entry_temp);
         thread_bootstrap_connection_error(cur->id, CON_PARENT_CONNECT_DOWN, NULL);
         return;
     }
@@ -798,6 +804,8 @@ static void thread_parse_child_update_response(protocol_interface_info_entry_t *
     }
 
     if (security_headers->KeyIdMode == MAC_KEY_ID_MODE_SRC4_IDX) {
+        thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index, linkMargin, false);
+        thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
         thread_management_key_synch_req(cur->id, common_read_32_bit(security_headers->Keysource));
     } else {
         tr_debug("Key ID Mode 2 not used; dropped.");
@@ -811,13 +819,11 @@ static void thread_parse_child_update_response(protocol_interface_info_entry_t *
 
     timeout = cur->thread_info->host_link_timeout;
     if (mle_tlv_read_32_bit_tlv(MLE_TYPE_TIMEOUT, mle_msg->data_ptr, mle_msg->data_length, &timeout)) {
-        entry_temp->holdTime = 90;
         tr_debug("Setting child timeout, value=%"PRIu32, timeout);
-        mle_entry_timeout_update(entry_temp, timeout);
     }
 
     tr_debug("Keep-Alive -->Respond from Parent");
-    mle_entry_timeout_refresh(entry_temp);
+    mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_temp, timeout);
 
     //Save possible new Leader Data
     if (leader_data_received) {
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nd.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nd.c
index 4c9a5af391..717e757bff 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nd.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nd.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -50,6 +50,7 @@
 #include "ns_trace.h"
 #include "6LoWPAN/Thread/thread_common.h"
 #include "6LoWPAN/Thread/thread_routing.h"
+#include "6LoWPAN/Thread/thread_neighbor_class.h"
 #include "6LoWPAN/Thread/thread_nd.h"
 #include "6LoWPAN/Thread/thread_joiner_application.h"
 #include "6LoWPAN/Thread/thread_extension.h"
@@ -57,6 +58,7 @@
 #include "6LoWPAN/Thread/thread_resolution_server.h"
 #include "6LoWPAN/Thread/thread_bbr_api_internal.h"
 #include "6LoWPAN/Thread/thread_extension_bbr.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 #include "6LoWPAN/MAC/mac_helper.h"
 #include "Common_Protocols/icmpv6.h"
 #include "MLE/mle.h"
@@ -196,14 +198,14 @@ bool thread_nd_ns_transmit(protocol_interface_info_entry_t *cur, ipv6_neighbour_
     }
 }
 
-static mle_neigh_table_entry_t *thread_nd_child_mleid_get(int8_t interface_id, uint8_t *childAddress, uint8_t *mlmeid_ptr)
+static mac_neighbor_table_entry_t *thread_nd_child_mleid_get(protocol_interface_info_entry_t *cur, uint8_t *childAddress, uint8_t *mlmeid_ptr)
 {
-    mle_neigh_table_entry_t *entry_temp;
-    entry_temp = mle_class_get_by_link_address(interface_id, childAddress, ADDR_802_15_4_SHORT);
-    if (entry_temp) {
-        if ((entry_temp->mode & MLE_DEV_MASK) == MLE_RFD_DEV) {
-            memcpy(mlmeid_ptr,entry_temp->mlEid,8);
-            return entry_temp;
+    mac_neighbor_table_entry_t *entry = mac_neighbor_table_address_discover(mac_neighbor_info(cur), childAddress, ADDR_802_15_4_SHORT);
+    if (entry && !entry->ffd_device) {
+        uint8_t *ptr = thread_neighbor_class_get_mleid(&cur->thread_info->neighbor_class, entry->index);
+        if (ptr) {
+            memcpy(mlmeid_ptr, ptr, 8);
+            return entry;
         }
     }
     return NULL;
@@ -228,12 +230,13 @@ static int thread_nd_address_query_lookup(int8_t interface_id, const uint8_t tar
     /* Scan IPv6 neighbour cache for registered entries of children */
     ns_list_foreach(ipv6_neighbour_t, n, &cur->ipv6_neighbour_cache.list) {
         if (n->type == IP_NEIGHBOUR_REGISTERED && addr_ipv6_equal(n->ip_address, target_addr)) {
-            mle_neigh_table_entry_t *mle_entry;
+            mac_neighbor_table_entry_t *mle_entry;
             *addr_out = mac16;
-            mle_entry = thread_nd_child_mleid_get(interface_id, &n->ll_address[2], mleid_ptr);
+            mle_entry = thread_nd_child_mleid_get(cur, &n->ll_address[2], mleid_ptr);
             if (mle_entry) {
                 //Get MLEID from Child
-                *last_transaction_time = (protocol_core_monotonic_time - mle_entry->last_contact_time) / 10;  /* Both variables are count of 100ms ticks. */
+                uint32_t last_contact = thread_neighbor_last_communication_time_get(&cur->thread_info->neighbor_class, mle_entry->index);
+                *last_transaction_time = (protocol_core_monotonic_time - last_contact) / 10;  /* Both variables are count of 100ms ticks. */
                 *proxy = true;
                 return 0;
             }
@@ -291,17 +294,18 @@ static void thread_nd_address_error(int8_t interface_id, const uint8_t ip_addr[1
     if_address_entry_t *addr_entry = addr_get_entry(cur, ip_addr);
     if (addr_entry && memcmp(ml_eid, cur->iid_slaac, 8)) {
         addr_duplicate_detected(cur, ip_addr);
+        thread_extension_address_generate(cur);
     }
 
     /* Scan IPv6 neighbour cache for registered entries of children */
     ns_list_foreach_safe(ipv6_neighbour_t, n, &cur->ipv6_neighbour_cache.list) {
         if (n->type == IP_NEIGHBOUR_REGISTERED && addr_ipv6_equal(n->ip_address, ip_addr)) {
             uint8_t child_mleid[8];
-            mle_neigh_table_entry_t *child = thread_nd_child_mleid_get(interface_id, &n->ll_address[2], child_mleid);
+            mac_neighbor_table_entry_t *child = thread_nd_child_mleid_get(cur, &n->ll_address[2], child_mleid);
             /* If this address belongs to an RFD child, with a different ML-EID, we must send it a duplicate message, and remove the EID */
             if (child && memcmp(child_mleid, ml_eid, 8)) {
                 uint8_t child_ml_addr[16];
-                thread_addr_write_mesh_local_16(child_ml_addr, child->short_adr, cur->thread_info);
+                thread_addr_write_mesh_local_16(child_ml_addr, child->mac16, cur->thread_info);
                 tr_warn("Forwarding address error to child %04x", common_read_16_bit(&n->ll_address[2]));
                 thread_resolution_client_address_error(interface_id, child_ml_addr, ip_addr, ml_eid);
                 ipv6_neighbour_entry_remove(&cur->ipv6_neighbour_cache, n);
@@ -367,7 +371,7 @@ buffer_t *thread_nd_snoop(protocol_interface_info_entry_t *cur, buffer_t *buf, c
         return buffer_free(buf);
     }
 
-    if (!mle_class_get_by_link_address(cur->id, &ll_dst->address[2], ADDR_802_15_4_SHORT)) {
+    if (!mac_neighbor_table_address_discover(mac_neighbor_info(cur), &ll_dst->address[2], ADDR_802_15_4_SHORT)) {
         /* We now know this was a packet for a non-existent child */
         goto bounce;
     }
@@ -433,7 +437,7 @@ buffer_t *thread_nd_special_forwarding(protocol_interface_info_entry_t *cur, buf
         return buf;
     }
 
-    mle_neigh_table_entry_t *entry = mle_class_get_by_link_address(cur->id, &ll_src->address[2], ADDR_802_15_4_SHORT);
+    mac_neighbor_table_entry_t *entry = mac_neighbor_table_address_discover(mac_neighbor_info(cur), &ll_src->address[2], ADDR_802_15_4_SHORT);
 
     /* Due to note 1 and 1b above, full-function children / neighbor routers
      * who did resolve to an RLOC16 may have optimised out the mesh header.
@@ -446,7 +450,7 @@ buffer_t *thread_nd_special_forwarding(protocol_interface_info_entry_t *cur, buf
      * is not in our neighbour cache, we need to send the child an error
      * to clear its cache.)
      */
-    if (!(buf->options.lowpan_mesh_rx || (entry && (entry->mode & MLE_DEV_MASK) == MLE_FFD_DEV))) {
+    if (!(buf->options.lowpan_mesh_rx || (entry && entry->ffd_device))) {
         return buf;
     }
 
@@ -530,10 +534,11 @@ buffer_t *thread_nd_icmp_handler(protocol_interface_info_entry_t *cur, buffer_t
     return buf;
 }
 
-int thread_nd_address_registration(protocol_interface_info_entry_t *cur, const uint8_t *ipv6Address, uint16_t mac16, uint16_t panId, const uint8_t *mac64)
+int thread_nd_address_registration(protocol_interface_info_entry_t *cur, const uint8_t *ipv6Address, uint16_t mac16, uint16_t panId, const uint8_t *mac64, bool *new_neighbour_created)
 {
     ipv6_neighbour_t *neigh;
     uint8_t ll_address[4];
+    bool neighbor_created = false;
     common_write_16_bit(panId, ll_address + 0);
     common_write_16_bit(mac16, ll_address + 2);
     neigh = ipv6_neighbour_lookup_or_create(&cur->ipv6_neighbour_cache, ipv6Address);
@@ -542,21 +547,24 @@ int thread_nd_address_registration(protocol_interface_info_entry_t *cur, const u
     }
 
     uint8_t *nce_eui64 = ipv6_neighbour_eui64(&cur->ipv6_neighbour_cache, neigh);
-    /*if (neigh->state != IP_NEIGHBOUR_NEW)
-       {
-              // Worry about this later
-              // Compare mac64 to nce_eui64 to spot duplicates
-       }
-       else*/
+    if (neigh->state != IP_NEIGHBOUR_NEW && memcmp(nce_eui64, mac64, 8) != 0)
     {
-        /* New entry */
-        memcpy(nce_eui64, mac64, 8);
+        return -2;
     }
+
+    /* New entry */
+    if (neigh->state == IP_NEIGHBOUR_NEW) {
+        neighbor_created = true;
+    }
+    memcpy(nce_eui64, mac64, 8);
     /* Set the LL address, ensure it's marked STALE */
     ipv6_neighbour_entry_update_unsolicited(&cur->ipv6_neighbour_cache, neigh, ADDR_802_15_4_SHORT, ll_address);
     neigh->type = IP_NEIGHBOUR_REGISTERED;
     neigh->lifetime = 0xffffffff; //Set Infinite
     ipv6_neighbour_set_state(&cur->ipv6_neighbour_cache, neigh, IP_NEIGHBOUR_STALE);
+    if (new_neighbour_created) {
+        *new_neighbour_created = neighbor_created;
+    }
     return 0;
 }
 
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nd.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nd.h
index 69d35740f3..60277db436 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nd.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nd.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -55,7 +55,7 @@ int thread_nd_map_anycast_address(protocol_interface_info_entry_t *cur, uint16_t
 void thread_nd_address_remove(protocol_interface_info_entry_t *cur_interface, addrtype_t ll_type, const uint8_t *ll_address);
 void thread_nd_flush_neighbour_cache_for_short_addr(struct protocol_interface_info_entry *cur, uint16_t flushee, bool children);
 
-int thread_nd_address_registration(struct protocol_interface_info_entry *cur, const uint8_t *ipv6Address, uint16_t mac16, uint16_t panId, const uint8_t *mac64);
+int thread_nd_address_registration(struct protocol_interface_info_entry *cur, const uint8_t *ipv6Address, uint16_t mac16, uint16_t panId, const uint8_t *mac64, bool *new_neighbour_created);
 
 #else //HAVE_THREAD_NEIGHBOR_DISCOVERY
 
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_neighbor_class.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_neighbor_class.c
new file mode 100644
index 0000000000..5d626c4d84
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_neighbor_class.c
@@ -0,0 +1,209 @@
+/*
+ * Copyright (c) 2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holder nor the
+ *    names of its contributors may be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+#include "nsconfig.h"
+#include <string.h>
+#include "ns_types.h"
+#include "ns_trace.h"
+#include "nsdynmemLIB.h"
+#include "common_functions.h"
+#include "NWK_INTERFACE/Include/protocol.h"
+#include "6LoWPAN/Thread/thread_common.h"
+#include "6LoWPAN/Thread/thread_routing.h"
+#include "6LoWPAN/Thread/thread_neighbor_class.h"
+
+
+static thread_neigh_table_entry_t * thread_neighbor_class_table_entry_get(thread_neighbor_class_t *class_ptr, uint8_t attribute_index)
+{
+    if (!class_ptr->neigh_info_list || attribute_index >= class_ptr->list_size) {
+        return NULL;
+    }
+
+    thread_neigh_table_entry_t *entry = class_ptr->neigh_info_list + attribute_index;
+    return entry;
+}
+
+bool thread_neighbor_class_create(thread_neighbor_class_t *class_ptr, uint8_t neigh_table_size)
+{
+    class_ptr->neigh_info_list = ns_dyn_mem_alloc(sizeof(thread_neigh_table_entry_t) * neigh_table_size);
+    if (!class_ptr->neigh_info_list) {
+        return false;
+    }
+
+    class_ptr->list_size = neigh_table_size;
+    thread_neigh_table_entry_t * list_ptr = class_ptr->neigh_info_list;
+    for (uint8_t i = 0; i< neigh_table_size; i++) {
+        memset(list_ptr, 0, sizeof(thread_neigh_table_entry_t));
+        list_ptr++;
+    }
+    return true;
+}
+
+void thread_neighbor_class_delete(thread_neighbor_class_t *class_ptr)
+{
+    ns_dyn_mem_free(class_ptr->neigh_info_list);
+    class_ptr->neigh_info_list = NULL;
+    class_ptr->list_size = 0;
+}
+
+void thread_neighbor_class_add_mleid(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index, const uint8_t *mleid)
+{
+    thread_neigh_table_entry_t *entry = thread_neighbor_class_table_entry_get(class_ptr,attribute_index);
+    if (!entry) {
+        return;
+    }
+    memcpy(entry->mlEid, mleid, 8);
+}
+
+uint8_t * thread_neighbor_class_get_mleid(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index)
+{
+    thread_neigh_table_entry_t *entry = thread_neighbor_class_table_entry_get(class_ptr,attribute_index);
+    if (!entry) {
+        return NULL;
+    }
+    return entry->mlEid;
+}
+
+void thread_neighbor_last_communication_time_update(thread_neighbor_class_t *class_ptr, uint8_t attribute_index)
+{
+    thread_neigh_table_entry_t *entry = thread_neighbor_class_table_entry_get(class_ptr,attribute_index);
+    if (!entry) {
+        return;
+    }
+    entry->last_contact_time = protocol_core_monotonic_time;
+}
+
+void thread_neighbor_class_update_link(thread_neighbor_class_t *class_ptr, uint8_t attribute_index, uint8_t linkmargin, bool new_link)
+{
+    thread_neigh_table_entry_t *entry = thread_neighbor_class_table_entry_get(class_ptr,attribute_index);
+    if (!entry) {
+        return;
+    }
+
+    if (new_link) {
+        entry->link_margin = entry->link_margin + linkmargin - (entry->link_margin >> THREAD_LINK_MARGIN_SCALING);
+    } else {
+        entry->link_margin = linkmargin << THREAD_LINK_MARGIN_SCALING;
+    }
+}
+
+uint16_t thread_neighbor_entry_linkmargin_get(thread_neighbor_class_t *class_ptr, uint8_t attribute_index)
+{
+    thread_neigh_table_entry_t *entry = thread_neighbor_class_table_entry_get(class_ptr,attribute_index);
+    if (!entry) {
+        return 0;
+    }
+    return entry->link_margin;
+}
+
+uint32_t thread_neighbor_last_communication_time_get(thread_neighbor_class_t *class_ptr, uint8_t attribute_index)
+{
+    thread_neigh_table_entry_t *entry = thread_neighbor_class_table_entry_get(class_ptr,attribute_index);
+    if (!entry) {
+        return 0;
+    }
+    return entry->last_contact_time;
+}
+
+bool thread_neighbor_class_mleid_compare(thread_neighbor_class_t *class_ptr, uint8_t attribute_index, const uint8_t *mleid)
+{
+    thread_neigh_table_entry_t *entry = thread_neighbor_class_table_entry_get(class_ptr,attribute_index);
+    if (!entry || memcmp(entry->mlEid, mleid, 8)) {
+        return false;
+    }
+    return true;
+}
+
+bool thread_neighbor_class_request_full_data_setup(thread_neighbor_class_t *class_ptr, uint8_t attribute_index)
+{
+    thread_neigh_table_entry_t *entry = thread_neighbor_class_table_entry_get(class_ptr,attribute_index);
+    if (!entry) {
+        return false;
+    }
+    return entry->request_full_data_set;
+
+}
+
+bool thread_neighbor_class_secured_data_request(thread_neighbor_class_t *class_ptr, uint8_t attribute_index)
+{
+    thread_neigh_table_entry_t *entry = thread_neighbor_class_table_entry_get(class_ptr,attribute_index);
+    if (!entry) {
+        return false;
+    }
+    return entry->secured_data_request;
+}
+
+void thread_neighbor_class_request_full_data_setup_set(thread_neighbor_class_t *class_ptr, uint8_t attribute_index, bool value)
+{
+    thread_neigh_table_entry_t *entry = thread_neighbor_class_table_entry_get(class_ptr,attribute_index);
+    if (entry) {
+        entry->request_full_data_set = value;
+    }
+}
+
+void thread_neighbor_class_secured_data_request_set(thread_neighbor_class_t *class_ptr, uint8_t attribute_index, bool value)
+{
+    thread_neigh_table_entry_t *entry = thread_neighbor_class_table_entry_get(class_ptr,attribute_index);
+    if (entry) {
+        entry->secured_data_request = value;
+    }
+}
+
+void thread_neighbor_class_mode_parse_to_entry(thread_neighbor_class_t *class_ptr, uint8_t attribute_index, uint8_t mode)
+{
+    thread_neigh_table_entry_t *entry = thread_neighbor_class_table_entry_get(class_ptr,attribute_index);
+    if (entry) {
+        entry->request_full_data_set = mode & MLE_THREAD_REQ_FULL_DATA_SET;
+        entry->secured_data_request = mode & MLE_THREAD_SECURED_DATA_REQUEST;
+    }
+}
+
+uint8_t thread_neighbor_class_mode_write_from_entry(thread_neighbor_class_t *class_ptr, uint8_t attribute_index)
+{
+    uint8_t mode = 0;
+    thread_neigh_table_entry_t *entry = thread_neighbor_class_table_entry_get(class_ptr,attribute_index);
+    if (entry) {
+        if (entry->request_full_data_set) {
+            mode |= MLE_THREAD_REQ_FULL_DATA_SET;
+        }
+
+        if (entry->secured_data_request) {
+            mode |= MLE_THREAD_SECURED_DATA_REQUEST;
+        }
+    }
+    return mode;
+}
+
+
+void thread_neighbor_class_entry_remove(thread_neighbor_class_t *class_ptr, uint8_t attribute_index)
+{
+    thread_neigh_table_entry_t *entry = thread_neighbor_class_table_entry_get(class_ptr,attribute_index);
+    if (entry) {
+        memset(entry, 0, sizeof(thread_neigh_table_entry_t));
+    }
+}
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_neighbor_class.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_neighbor_class.h
new file mode 100644
index 0000000000..ccf5f766e4
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_neighbor_class.h
@@ -0,0 +1,73 @@
+/*
+ * Copyright (c) 2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: BSD-3-Clause
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in the
+ *    documentation and/or other materials provided with the distribution.
+ * 3. Neither the name of the copyright holder nor the
+ *    names of its contributors may be used to endorse or promote products
+ *    derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
+ * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
+ * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
+ * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
+ * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
+ * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
+ * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
+ * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
+ * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
+ * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+#ifndef THREAD_NEIGHBOR_CLASS_H_
+#define THREAD_NEIGHBOR_CLASS_H_
+
+struct thread_neighbor_class_s;
+
+/** Thead Spesific ModeFlags */
+#define MLE_THREAD_SECURED_DATA_REQUEST 0x04
+#define MLE_THREAD_REQ_FULL_DATA_SET 0x01
+
+bool thread_neighbor_class_create(struct thread_neighbor_class_s *class_ptr, uint8_t neigh_table_size);
+
+void thread_neighbor_class_delete(struct thread_neighbor_class_s *class_ptr);
+
+void thread_neighbor_class_add_link(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index, uint8_t linkmargin);
+
+void thread_neighbor_class_add_mleid(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index, const uint8_t *mleid);
+
+uint8_t * thread_neighbor_class_get_mleid(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index);
+
+void thread_neighbor_class_update_link(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index, uint8_t linkmargin, bool new_link);
+
+void thread_neighbor_last_communication_time_update(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index);
+
+uint16_t thread_neighbor_entry_linkmargin_get(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index);
+
+uint32_t thread_neighbor_last_communication_time_get(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index);
+
+bool thread_neighbor_class_mleid_compare(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index, const uint8_t *mleid);
+
+bool thread_neighbor_class_request_full_data_setup(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index);
+
+bool thread_neighbor_class_secured_data_request(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index);
+
+void thread_neighbor_class_mode_parse_to_entry(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index, uint8_t mode);
+
+uint8_t thread_neighbor_class_mode_write_from_entry(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index);
+
+void thread_neighbor_class_request_full_data_setup_set(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index, bool value);
+
+void thread_neighbor_class_secured_data_request_set(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index, bool value);
+
+void thread_neighbor_class_entry_remove(struct thread_neighbor_class_s *class_ptr, uint8_t attribute_index);
+
+#endif /* THREAD_NEIGHBOR_CLASS_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_lib.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_lib.h
index ecc85cfcd1..5c37901111 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_lib.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_lib.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -60,6 +60,7 @@
 #define THREAD_P_DEF_ROUTE_BIT_MOVE     9   /* R-bit */
 #define THREAD_P_ON_MESH_BIT_MOVE       8   /* O-bit */
 #define THREAD_P_ND_DNS_BIT_MOVE        7   /* N-bit */
+#define THREAD_P_ND_RES_BIT_MOVE        6   /* First reserved bit */
 
 /* Bit shift for HasRouteTLV preference bit */
 #define THREAD_HAS_ROUTE_PRF_BIT_MOVE   6   /* Prf-bits */
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.c
index 2ba5226c2d..0947cf4564 100755
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -328,9 +328,8 @@ thread_network_local_data_entry_t *thread_local_service_list_allocate(thread_pre
         newEntry->servicesPrefixLen = prefixTlv->PrefixLen;
         newEntry->domainId = prefixTlv->domainId;
         newEntry->dhcpv6ServerActive = false;
-        newEntry->dhcpv6ServerDataStable = false;
+        newEntry->brDataStable = false;
         newEntry->slaacServerActive = false;
-        newEntry->slaacServerDataStable = false;
         newEntry->slaacPreferred = false;
         newEntry->routeActive = false;
         newEntry->routeDataStable = false;
@@ -339,6 +338,8 @@ thread_network_local_data_entry_t *thread_local_service_list_allocate(thread_pre
         newEntry->defaultRoute = false;
         newEntry->onMesh = false;
         newEntry->ndDns = false;
+        newEntry->brActive = false;
+        newEntry->res1 = false;
     }
     return newEntry;
 }
@@ -373,6 +374,7 @@ thread_network_server_data_entry_t *thread_server_entry_allocate(thread_border_r
         newEntry->Prf = service->Prf;
         newEntry->P_on_mesh = service->P_on_mesh;
         newEntry->P_nd_dns = service->P_nd_dns;
+        newEntry->P_res1 = service->P_res1;
         newEntry->canDelete = false;
 
     }
@@ -868,7 +870,7 @@ static int thread_service_data_delete_mark_by_router_id(thread_network_data_serv
     return retVal;
 }
 
-static int thread_server_context_clean(thread_network_data_cache_entry_t *cachePtr, thread_data_context_list_t *listPtr, thread_network_data_prefix_cache_entry_t *prefixEntry, lowpan_context_list_t *context_list)
+static int thread_server_context_clean(int8_t id, thread_network_data_cache_entry_t *cachePtr, thread_data_context_list_t *listPtr, thread_network_data_prefix_cache_entry_t *prefixEntry, lowpan_context_list_t *context_list)
 {
     int retVal = -1;
     (void) prefixEntry;
@@ -879,7 +881,9 @@ static int thread_server_context_clean(thread_network_data_cache_entry_t *cacheP
                 cachePtr->stableUpdatePushed = true;
             }
             // Set context lifetime to 0 to delete
-            lowpan_context_update(context_list, cur->cid, 0, NULL, 0, true);
+            if (thread_extension_context_can_delete(id, prefixEntry->servicesPrefix, cur->contextPrefixLength)) {
+                lowpan_context_update(context_list, cur->cid, 0, NULL, 0, true);
+            }
             ns_list_remove(listPtr, cur);
             ns_dyn_mem_free(cur);
             retVal = 0;
@@ -1019,7 +1023,7 @@ bool thread_network_data_router_id_free(thread_network_data_cache_entry_t *cache
         }
         thread_server_data_clean_by_router_id(cachePtr, &cur->routeList, cur, true, curInterface);
         if (!is_leader) {
-            thread_server_context_clean(cachePtr, &cur->contextList, cur, &curInterface->lowpan_contexts);
+            thread_server_context_clean(curInterface->id, cachePtr, &cur->contextList, cur, &curInterface->lowpan_contexts);
         }
 
         if (ns_list_is_empty(&cur->borderRouterList)) {
@@ -1071,7 +1075,7 @@ bool thread_network_data_router_id_free(thread_network_data_cache_entry_t *cache
     return address_removed;
 }
 
-void thread_network_data_context_re_use_timer_update(thread_network_data_cache_entry_t *cachePtr, uint32_t ticks, lowpan_context_list_t *context_list)
+void thread_network_data_context_re_use_timer_update(int8_t id, thread_network_data_cache_entry_t *cachePtr, uint32_t ticks, lowpan_context_list_t *context_list)
 {
     ns_list_foreach_safe(thread_network_data_prefix_cache_entry_t, cur, &cachePtr->localPrefixList) {
         ns_list_foreach_safe(thread_network_data_context_entry_t, curContext, &cur->contextList) {
@@ -1086,7 +1090,9 @@ void thread_network_data_context_re_use_timer_update(thread_network_data_cache_e
                         cachePtr->temporaryUpdatePushed = true;
                     }
                     // Set context lifetime to 0 to delete
-                    lowpan_context_update(context_list, curContext->cid, 0, NULL, 0, true);
+                    if (thread_extension_context_can_delete(id, cur->servicesPrefix,curContext->contextPrefixLength)) {
+                        lowpan_context_update(context_list, curContext->cid, 0, NULL, 0, true);
+                    }
                     ns_dyn_mem_free(curContext);
                 }
             }
@@ -1708,21 +1714,21 @@ int thread_local_server_list_add_on_mesh_server(thread_network_local_data_cache_
     if (networkDataList) {
         thread_network_local_data_entry_t *prefix_entry = thread_local_prefix_entry_get(&networkDataList->prefix_list, prefixTlv);
         if (prefix_entry) {
+            prefix_entry->brDataStable = service->stableData;
             prefix_entry->preference = service->Prf;
             prefix_entry->configure = service->P_configure;
             prefix_entry->defaultRoute = service->P_default_route;
             prefix_entry->onMesh = service->P_on_mesh;
             prefix_entry->ndDns = service->P_nd_dns;
+            prefix_entry->res1 = service->P_res1;
+            prefix_entry->dhcpv6ServerActive = service->P_dhcp;
+            prefix_entry->slaacServerActive = service->P_slaac;
+            prefix_entry->slaacPreferred = service->P_preferred;
 
-            if (service->P_dhcp) {
-                prefix_entry->dhcpv6ServerActive = true;
-                prefix_entry->dhcpv6ServerDataStable = service->stableData;
-            }
-
-            if (service->P_slaac) {
-                prefix_entry->slaacServerActive = true;
-                prefix_entry->slaacServerDataStable = service->stableData;
-                prefix_entry->slaacPreferred = service->P_preferred;
+            if (service->P_dhcp ||
+                    service->P_slaac ||
+                    service->P_res1) {
+                prefix_entry->brActive = true;
             }
 
             if (prefixTlv->PrefixLen == 0) {
@@ -1891,9 +1897,7 @@ int thread_local_server_del_route(thread_network_local_data_cache_entry_t *netwo
     }
 
     prefix_entry->routeActive = false;
-    if (prefix_entry->dhcpv6ServerActive) {
-        return 0;
-    } else if (prefix_entry->slaacServerActive) {
+    if (prefix_entry->brActive) {
         return 0;
     }
 
@@ -2023,17 +2027,13 @@ uint8_t thread_server_prefix_length(thread_network_local_data_entry_t *cur)
 
 
     if (cur->routeActive) {
-        if (!((cur->slaacServerActive || cur->dhcpv6ServerActive) && cur->defaultRoute)) {
+        if (!(cur->brActive && cur->defaultRoute)) {
             // HasRoute is added if BorderRouter TLV does not have default route bit
             tempLength += 5;
         }
     }
 
-    if (cur->dhcpv6ServerActive) {
-        tempLength += 6;
-    }
-
-    if (cur->slaacServerActive) {
+    if (cur->brActive) {
         tempLength += 6;
     }
 
@@ -2091,14 +2091,9 @@ uint16_t thread_nd_own_service_list_data_size(thread_network_local_data_cache_en
 
 static bool thread_check_local_data_prefix_stable_boolean(thread_network_local_data_entry_t *dataList)
 {
-    if (dataList->dhcpv6ServerActive && dataList->dhcpv6ServerDataStable) {
+    if (dataList->brActive && dataList->brDataStable) {
         return true;
     }
-
-    if (dataList->slaacServerActive && dataList->slaacServerDataStable) {
-        return true;
-    }
-
     return false;
 }
 
@@ -2269,7 +2264,7 @@ uint8_t * thread_nd_own_service_list_data_write(thread_network_local_data_cache_
         if (servicesLen) {
             ptr = thread_nd_hosted_prefix_header_write(ptr,servicesLen, cur);
             if (cur->routeActive) {
-                if (!((cur->slaacServerActive || cur->dhcpv6ServerActive) && cur->defaultRoute)) {
+                if (!(cur->brActive && cur->defaultRoute)) {
                     // HasRoute is added if BorderRouter TLV does not have default route bit
                     uint8_t preference = 0;
                     tlvType = THREAD_NWK_DATA_TYPE_ROUTE;
@@ -2283,10 +2278,10 @@ uint8_t * thread_nd_own_service_list_data_write(thread_network_local_data_cache_
                 }
             }
 
-            if (cur->slaacServerActive || cur->dhcpv6ServerActive) {
+            if (cur->brActive) {
                 uint16_t flags = 0;
                 tlvType = THREAD_NWK_DATA_TYPE_BORDER_ROUTER;
-                if (cur->slaacServerDataStable || cur->dhcpv6ServerDataStable) {
+                if (cur->brDataStable) {
                     tlvType |= THREAD_NWK_STABLE_DATA;
                 }
                 if (cur->slaacServerActive) {
@@ -2313,6 +2308,9 @@ uint8_t * thread_nd_own_service_list_data_write(thread_network_local_data_cache_
                 if (cur->ndDns) {
                     flags |= 1 << THREAD_P_ND_DNS_BIT_MOVE;
                 }
+                if (cur->res1) {
+                    flags |= 1 << THREAD_P_ND_RES_BIT_MOVE;
+                }
                 ptr = thread_service_border_router_tlv_write(ptr, tlvType, routerID, flags);
             } // slaac or dhcp
         }
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.h
index 33671a74cf..10dbd298f1 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_data_storage.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -72,6 +72,7 @@ typedef struct thread_network_data_temporary_route_or_dhcpv6_server_entry_s {
     bool                stableData: 1;
     bool                P_on_mesh: 1;
     bool                P_nd_dns: 1;
+    bool                P_res1: 1;
     bool                canDelete: 1;
     ns_list_link_t      link;               /*!< List link entry */
 } thread_network_server_data_entry_t;
@@ -87,6 +88,7 @@ typedef struct thread_border_router_tlv_entry_t {
     bool                stableData: 1;      /* P_stable */
     bool                P_on_mesh: 1;       /* P_on_mesh */
     bool                P_nd_dns: 1;        /* P_nd_dns */
+    bool                P_res1: 1;        /* P_res1 */
 } thread_border_router_tlv_entry_t;
 
 typedef struct thread_prefix_tlv {
@@ -136,16 +138,17 @@ typedef struct thread_network_local_data_entry_s {
     uint8_t           servicesPrefixLen;    /*!< Prefix length in bits This Can Be 1-128 */
     bool                routeActive: 1;
     bool                routeDataStable: 1;
+    bool                brActive: 1;
     bool                dhcpv6ServerActive: 1;
-    bool                dhcpv6ServerDataStable: 1;
+    bool                brDataStable: 1;
     bool                slaacServerActive: 1;
-    bool                slaacServerDataStable: 1;
     bool                slaacPreferred: 1;
     unsigned            preference: 2;
     bool                configure: 1;
     bool                defaultRoute: 1;
     bool                onMesh: 1;
     bool                ndDns: 1;
+    bool                res1:1;
     ns_list_link_t      link;               /*!< List link entry */
 } thread_network_local_data_entry_t;
 
@@ -207,7 +210,7 @@ bool thread_network_data_router_id_free(thread_network_data_cache_entry_t *cache
 
 void thread_network_local_data_free_and_clean(thread_network_local_data_cache_entry_t *cachePtr, int8_t interface_id);
 
-void thread_network_data_context_re_use_timer_update(thread_network_data_cache_entry_t *cachePtr, uint32_t ticks, lowpan_context_list_t *context_list);
+void thread_network_data_context_re_use_timer_update(int8_t id, thread_network_data_cache_entry_t *cachePtr, uint32_t ticks, lowpan_context_list_t *context_list);
 
 /**
  * Add new route information to route List
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.c
index 8528d0c8b8..22d6e212bc 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -56,12 +56,14 @@
 #include "6LoWPAN/Thread/thread_joiner_application.h"
 #include "6LoWPAN/Thread/thread_management_client.h"
 #include "6LoWPAN/Thread/thread_network_synch.h"
+#include "6LoWPAN/Thread/thread_neighbor_class.h"
 #include "thread_management_if.h"
 #include "thread_config.h"
 #include "Common_Protocols/ipv6.h"
 #include "Common_Protocols/icmpv6.h"
 #include "Common_Protocols/icmpv6_radv.h"
 #include "MLE/mle.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 #include "6LoWPAN/MAC/mac_helper.h"
 
 #define TRACE_GROUP "tsyn"
@@ -90,7 +92,7 @@ typedef struct thread_network_dynamic_data_entry {
 } thread_network_dynamic_data_entry_t;
 
 static thread_sync_child_info_t *thread_dynamic_storage_free_child_find(int8_t interface_id);
-static thread_sync_child_info_t *thread_dynamic_storage_child_info_find(int8_t interface_id, mle_neigh_table_entry_t *child);
+static thread_sync_child_info_t *thread_dynamic_storage_child_info_find(int8_t interface_id, mac_neighbor_table_entry_t *child);
 static NS_LIST_DEFINE(thread_network_dynamic_data_info, thread_network_dynamic_data_entry_t, link);
 
 thread_network_dynamic_data_entry_t *thread_network_synch_create(int8_t interfaceId)
@@ -155,54 +157,53 @@ int thread_network_synch_data_free(int8_t interface_id)
  * Dynamic network data storage.
  */
 
-void thread_dynamic_storage_child_info_store(int8_t interface_id, mle_neigh_table_entry_t *child)
+void thread_dynamic_storage_child_info_store(protocol_interface_info_entry_t *cur_interface, mac_neighbor_table_entry_t *child)
 {
-    thread_network_dynamic_data_entry_t *storeEntry = thread_network_synch_find(interface_id);
+    thread_network_dynamic_data_entry_t *storeEntry = thread_network_synch_find(cur_interface->id);
 
     if (!storeEntry) {
-        storeEntry = thread_network_synch_create(interface_id);
+        storeEntry = thread_network_synch_create(cur_interface->id);
     }
 
     if (!storeEntry) {
         return;
     }
 
-    thread_sync_child_info_t *child_info = thread_dynamic_storage_child_info_find(interface_id, child);
+    uint32_t mle_frame_counter = mle_service_neighbor_frame_counter_get(cur_interface->id, child->index);
+
+    thread_sync_child_info_t *child_info = thread_dynamic_storage_child_info_find(cur_interface->id, child);
+    if (!child_info) {
+        child_info = thread_dynamic_storage_free_child_find(cur_interface->id);
+    }
+
     if (child_info) {
-        child_info->mode = child->mode;
-        child_info->short_addr = child->short_adr;
-        child_info->mle_frame_counter = child->mle_frame_counter;
+        uint8_t mode = mle_mode_write_from_mac_entry(child);
+        mode |= thread_neighbor_class_mode_write_from_entry(&cur_interface->thread_info->neighbor_class, child->index);
+
+        child_info->mode = mode;
+        child_info->short_addr = child->mac16;
+        child_info->mle_frame_counter = mle_frame_counter;
         child_info->mac_frame_counter = 0;
         memcpy(child_info->long_addr, child->mac64, 8);
         return;
     }
-
-    child_info = thread_dynamic_storage_free_child_find(interface_id);
-
-    if (child_info) {
-        child_info->mode = child->mode;
-        child_info->short_addr = child->short_adr;
-        child_info->mle_frame_counter = child->mle_frame_counter;
-        child_info->mac_frame_counter = 0;
-        memcpy(child_info->long_addr, child->mac64, 8);
-    }
     return;
 }
 
-void thread_dynamic_storage_child_info_clear(int8_t interface_id, mle_neigh_table_entry_t *child)
+void thread_dynamic_storage_child_info_clear(int8_t interface_id, struct mac_neighbor_table_entry *child)
 {
     thread_sync_child_info_t *child_info = thread_dynamic_storage_child_info_find(interface_id, child);
 
     if (child_info){
         // Clear child information
         memset (child_info,0,sizeof(thread_sync_child_info_t));
-        tr_debug("Dynamic storage: cleared child; mac16=%04x", child->short_adr);
+        tr_debug("Dynamic storage: cleared child; mac16=%04x", child->mac16);
         return;
     }
     return;
 }
 
-static thread_sync_child_info_t *thread_dynamic_storage_child_info_find(int8_t interface_id, mle_neigh_table_entry_t *child)
+static thread_sync_child_info_t *thread_dynamic_storage_child_info_find(int8_t interface_id, mac_neighbor_table_entry_t *child)
 {
     thread_network_dynamic_data_entry_t *storeEntry = thread_network_synch_find(interface_id);
 
@@ -236,6 +237,10 @@ static thread_sync_child_info_t *thread_dynamic_storage_free_child_find(int8_t i
 void thread_dynamic_storage_build_mle_table(int8_t interface_id)
 {
     tr_debug("Dynamic storage: building MLE table.");
+    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
+    if (!cur || !cur->mac_parameters) {
+        return;
+    }
 
     thread_network_dynamic_data_entry_t *storeEntry = thread_network_synch_find(interface_id);
     bool new_entry_created;
@@ -248,9 +253,9 @@ void thread_dynamic_storage_build_mle_table(int8_t interface_id)
         return;
     }
 
-    mle_neigh_table_list_t *neig_list = mle_class_active_list_get(interface_id);
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
 
-    if (!neig_list) {
+    if (!mac_table_list) {
         return;
     }
 
@@ -262,19 +267,21 @@ void thread_dynamic_storage_build_mle_table(int8_t interface_id)
         }
         uint8_t *mac64 = storeEntry->networ_dynamic_data_parameters.children[i].long_addr;
         tr_debug("Child: %04x, %s", storeEntry->networ_dynamic_data_parameters.children[i].short_addr, trace_array(mac64, 8));
-        mle_neigh_table_entry_t *entry = mle_class_get_entry_by_mac64(interface_id, 64, mac64, true, &new_entry_created);
-        if (entry) {
-            entry->short_adr = storeEntry->networ_dynamic_data_parameters.children[i].short_addr;
-            entry->mle_frame_counter = storeEntry->networ_dynamic_data_parameters.children[i].mle_frame_counter;
-            entry->mode = storeEntry->networ_dynamic_data_parameters.children[i].mode;
-            entry->threadNeighbor = true;
 
-            protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
+        mac_neighbor_table_entry_t *mac_entry = mac_neighbor_entry_get_by_mac64(mac_neighbor_info(cur), mac64, true, &new_entry_created);
+        if (mac_entry) {
 
-            if (cur && cur->mac_parameters) {
-                // Set MAC layer frame counter for the child
-                mac_helper_devicetable_set(entry, cur, storeEntry->networ_dynamic_data_parameters.children[i].mac_frame_counter, cur->mac_parameters->mac_default_key_index, new_entry_created);
-            }
+            mac_entry->mac16 = storeEntry->networ_dynamic_data_parameters.children[i].short_addr;
+            mle_service_frame_counter_entry_add(interface_id, mac_entry->index, storeEntry->networ_dynamic_data_parameters.children[i].mle_frame_counter);
+            mle_mode_parse_to_mac_entry(mac_entry, storeEntry->networ_dynamic_data_parameters.children[i].mode);
+
+            // Set MAC layer frame counter for the child
+            mlme_device_descriptor_t device_desc;
+            thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, mac_entry->index,64, new_entry_created);
+            thread_neighbor_class_mode_parse_to_entry(&cur->thread_info->neighbor_class, mac_entry->index,storeEntry->networ_dynamic_data_parameters.children[i].mode);
+            thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, mac_entry->index);
+            mac_helper_device_description_write(cur, &device_desc, mac_entry->mac64, mac_entry->mac16,storeEntry->networ_dynamic_data_parameters.children[i].mac_frame_counter, false);
+            mac_helper_devicetable_set(&device_desc, cur, mac_entry->index, cur->mac_parameters->mac_default_key_index, new_entry_created);
         }
     }
 }
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.h
index eae9f1807c..eb85ae0a03 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_network_synch.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -35,6 +35,9 @@
 #ifndef THREAD_NETWORK_SYNCH_H_
 #define THREAD_NETWORK_SYNCH_H_
 
+struct protocol_interface_info_entry;
+struct mac_neighbor_table_entry;
+
 //Call This when clean synched networkdata
 int thread_network_synch_data_free(int8_t interface_id);
 //Call This when want synch last network setup
@@ -46,8 +49,8 @@ bool thread_dynamic_storage_pending_configuration_exists(int8_t interface_id);
 void thread_dynamic_storage_pending_configuration_read(int8_t interface_id, void *data, uint16_t size);
 void thread_dynamic_storage_pending_configuration_store(int8_t interface_id, void *data, uint16_t size);
 
-void thread_dynamic_storage_child_info_store(int8_t interface_id, mle_neigh_table_entry_t *child);
-void thread_dynamic_storage_child_info_clear(int8_t interface_id, mle_neigh_table_entry_t *child);
+void thread_dynamic_storage_child_info_store(struct protocol_interface_info_entry *cur_interface, struct mac_neighbor_table_entry *child);
+void thread_dynamic_storage_child_info_clear(int8_t interface_id, struct mac_neighbor_table_entry *child);
 void thread_dynamic_storage_build_mle_table(int8_t interface_id);
 void thread_dynamic_storage_frame_counter_store(int8_t interface_id, uint32_t mle_frame_counter, uint32_t mac_frame_counter);
 uint32_t thread_dynamic_storage_mle_frame_counter_get(int8_t interfaceId);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.c
index e0c7c9224c..0aa4657f68 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017, Arm Limited and affiliates.
+ * Copyright (c) 2017-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -84,7 +84,7 @@ static int thread_nvm_store_fast_data_save(thread_nvm_fast_data_t* fast_data_to_
 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 150
+#define MAX_ROOT_PATH_LEN 200
 
 #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)
@@ -330,6 +330,15 @@ int thread_nvm_store_fast_data_check_and_write(uint32_t mac_frame_counter, uint3
     return ret;
 }
 
+int thread_nvm_store_fast_data_write_all(uint32_t mac_frame_counter, uint32_t mle_frame_counter, uint32_t network_seq_counter)
+{
+    int ret;
+    ret = thread_nvm_store_all_counters_store(mac_frame_counter, mle_frame_counter, network_seq_counter);
+    cached_fast_data.mac_frame_counter = mac_frame_counter;
+    cached_fast_data.mle_frame_counter = mle_frame_counter;
+    cached_fast_data.seq_counter=network_seq_counter;
+    return ret;
+}
 
 int thread_nvm_store_frame_counters_check_and_write(uint32_t mac_frame_counter, uint32_t mle_frame_counter)
 {
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.h
index cd160590ad..081c7cf6ed 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_nvm_store.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017, Arm Limited and affiliates.
+ * Copyright (c) 2017-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -84,6 +84,9 @@ int thread_nvm_store_fast_data_write(thread_nvm_fast_data_t* fast_data);
 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_write(uint32_t mac_frame_counter, uint32_t mle_frame_counter, uint32_t network_seq_counter);
+/*Store all fast data values unconditionally*/
+int thread_nvm_store_fast_data_write_all(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);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_resolution_server.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_resolution_server.c
index 292b7ca843..362f96aa7c 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_resolution_server.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_resolution_server.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_resolution_server.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_resolution_server.h
index a3795a1867..77fa611045 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_resolution_server.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_resolution_server.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.c
index ced6b84281..0c00650f7f 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -66,6 +66,7 @@
 #include "6LoWPAN/Thread/thread_lowpower_private_api.h"
 #include "6LoWPAN/Thread/thread_tmfcop_lib.h"
 #include "6LoWPAN/Thread/thread_nvm_store.h"
+#include "6LoWPAN/Thread/thread_neighbor_class.h"
 #include "thread_management_if.h"
 #include "Common_Protocols/ipv6.h"
 #include "Common_Protocols/icmpv6.h"
@@ -83,12 +84,12 @@
 #include "6LoWPAN/MAC/mac_data_poll.h"
 #include "thread_border_router_api.h"
 #include "Core/include/address.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 
 #ifdef HAVE_THREAD_ROUTER
 
 #define TRACE_GROUP "trbs"
 
-static bool thread_rfd_device(uint8_t mode);
 static uint8_t *thread_tlv_add(protocol_interface_info_entry_t *cur, uint8_t *ptr, uint8_t tlv_type, uint8_t mode);
 
 static void mle_multicast_push_to_sleep_child(protocol_interface_info_entry_t *cur, buffer_t *buf);
@@ -103,18 +104,10 @@ static int thread_router_accept_request_build(protocol_interface_info_entry_t *c
 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);
+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, struct mac_neighbor_table_entry *neigh_info);
 
 static int8_t thread_router_bootstrap_synch_request_send(protocol_interface_info_entry_t *cur);
-static bool thread_child_id_request(protocol_interface_info_entry_t *cur, mle_neigh_table_entry_t *entry_temp);
-
-static bool thread_rfd_device(uint8_t mode)
-{
-    if ((mode & MLE_DEV_MASK) != MLE_RFD_DEV) {
-        return false;
-    }
-    return true;
-}
+static bool thread_child_id_request(protocol_interface_info_entry_t *cur, struct mac_neighbor_table_entry *entry_temp);
 
 static bool thread_router_parent_address_check(protocol_interface_info_entry_t *cur, uint8_t *source_addr)
 {
@@ -209,7 +202,7 @@ int thread_router_bootstrap_mle_advertise(protocol_interface_info_entry_t *cur)
 
 }
 
-static void clone_multicast_to_unicast(protocol_interface_info_entry_t *cur, mle_neigh_table_entry_t *mle_entry, buffer_t *buf)
+static void clone_multicast_to_unicast(protocol_interface_info_entry_t *cur, mac_neighbor_table_entry_t *entry, buffer_t *buf)
 {
     link_configuration_s *linkConfiguration = thread_joiner_application_get_config(cur->id);
     if (!linkConfiguration) {
@@ -224,7 +217,7 @@ static void clone_multicast_to_unicast(protocol_interface_info_entry_t *cur, mle
     new_buf->info = (buffer_info_t) (B_DIR_DOWN | B_TO_IPV6_TXRX | B_FROM_IPV6_FWD);
     uint8_t next_hop[16];
     memcpy(next_hop, ADDR_LINK_LOCAL_PREFIX, 8);
-    memcpy(&next_hop[8], mle_entry->mac64, 8);
+    memcpy(&next_hop[8], entry->mac64, 8);
     next_hop[8] ^= 2;
 
     ipv6_buffer_route_to(new_buf, next_hop, cur);
@@ -234,15 +227,11 @@ static void clone_multicast_to_unicast(protocol_interface_info_entry_t *cur, mle
 
 static void mle_multicast_push_to_sleep_child(protocol_interface_info_entry_t *cur, buffer_t *buf)
 {
-    mle_neigh_table_list_t *mle_neigh_list = mle_class_active_list_get(cur->id);
-    if (!mle_neigh_list) {
-        return;
-    }
-    ns_list_foreach(mle_neigh_table_entry_t, cur_entry, mle_neigh_list) {
-        if (cur_entry->threadNeighbor) {
-            if (!(cur_entry->mode & MLE_RX_ON_IDLE)) {
-                clone_multicast_to_unicast(cur, cur_entry, buf);
-            }
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
+
+    ns_list_foreach(mac_neighbor_table_entry_t, cur_entry, mac_table_list) {
+        if (!cur_entry->rx_on_idle) {
+            clone_multicast_to_unicast(cur, cur_entry, buf);
         }
     }
 }
@@ -260,9 +249,9 @@ static void thread_registered_mcast_forward_to_child(protocol_interface_info_ent
         tr_debug("Forwarding to registered multicast address: %s", trace_ipv6(addr->address));
 
         ns_list_foreach(thread_mcast_child_t, child, &addr->children) {
-            mle_neigh_table_entry_t *mle_entry = mle_class_get_by_link_address(cur->id, child->mac64, ADDR_802_15_4_LONG);
-            if (mle_entry && mle_entry->threadNeighbor && !(mle_entry->mode & MLE_RX_ON_IDLE)) {
-                clone_multicast_to_unicast(cur, mle_entry, buffer);
+            mac_neighbor_table_entry_t *entry = mac_neighbor_table_address_discover(mac_neighbor_info(cur), child->mac64, ADDR_802_15_4_LONG);
+            if (entry && !entry->rx_on_idle) {
+                clone_multicast_to_unicast(cur, entry, buffer);
             }
         }
     }
@@ -304,13 +293,10 @@ static void thread_router_synch_receive_cb(int8_t interface_id, mle_message_t *m
     uint16_t version, shortAddress, address16;
     uint32_t llFrameCounter;
     mle_tlv_info_t routing;
-    mle_neigh_table_entry_t *entry_temp;
+    mac_neighbor_table_entry_t *entry_temp;
     tr_debug("Thread MLE message router sync");
     //State machine What packet shuold accept in this case
-    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
-    if (!cur) {
-        return;
-    }
+    protocol_interface_info_entry_t *cur = mle_msg->interface_ptr;
 
     /* Check that message is from link-local scope */
     if(!addr_is_ipv6_link_local(mle_msg->packet_src_address)) {
@@ -353,42 +339,56 @@ static void thread_router_synch_receive_cb(int8_t interface_id, mle_message_t *m
                 return;
             }
             //Allocate neighbor entry
-
-            entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, true, &new_neigbour);
+            entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, true, &new_neigbour);
             if (!entry_temp) {
                 return;
             }
 
+            if (security_headers->KeyIdMode == MAC_KEY_ID_MODE_SRC4_IDX) {
+                thread_management_key_synch_req(cur->id, common_read_32_bit(security_headers->Keysource));
+                thread_key_guard_timer_reset(cur);
+            } else {
+                tr_error("Key ID Mode 2 not used; dropped.");
+                return;
+            }
+
+            thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index, linkMargin, new_neigbour);
+            thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
+
             //Free Response
             mle_service_msg_free(messageId);
-            entry_temp->threadNeighbor = true;
-            entry_temp->short_adr = shortAddress;
+            entry_temp->mac16 = shortAddress;
+
             //when allocating neighbour entry, use MLE Frame counter if present to validate further advertisements from the neighbour
-            entry_temp->mle_frame_counter = mleFrameCounter;
-            if (entry_temp->timeout_rx) {
-                mle_entry_timeout_refresh(entry_temp);
+            mle_service_frame_counter_entry_add(cur->id, entry_temp->index, mleFrameCounter);
+            uint32_t timeout_tlv;
+
+            mle_tlv_info_t mle_tlv_info;
+
+            if (mle_tlv_option_discover(mle_msg->data_ptr, mle_msg->data_length, MLE_TYPE_TIMEOUT, &mle_tlv_info) > 0) {
+                timeout_tlv = common_read_32_bit(mle_tlv_info.dataPtr);
             } else {
-                mle_tlv_info_t mle_tlv_info;
-                uint32_t timeout_tlv;
-                if (mle_tlv_option_discover(mle_msg->data_ptr, mle_msg->data_length, MLE_TYPE_TIMEOUT, &mle_tlv_info) > 0) {
-                    timeout_tlv = common_read_32_bit(mle_tlv_info.dataPtr);
-                    mle_entry_timeout_update(entry_temp, timeout_tlv);
+                if (new_neigbour) {
+                    timeout_tlv = THREAD_DEFAULT_LINK_LIFETIME;
                 } else {
-                    mle_entry_timeout_update(entry_temp, THREAD_DEFAULT_LINK_LIFETIME);
+                    timeout_tlv = entry_temp->link_lifetime;
                 }
             }
 
-            if (thread_is_router_addr(shortAddress)) {
-                entry_temp->handshakeReady = 1;
-            }
+            mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_temp, timeout_tlv);
 
-            entry_temp->mode |= MLE_THREAD_REQ_FULL_DATA_SET;
+            if (thread_is_router_addr(shortAddress)) {
+                entry_temp->connected_device = 1;
+            }
+            thread_neighbor_class_request_full_data_setup_set(&cur->thread_info->neighbor_class, entry_temp->index, true);
 
             if (mle_tlv_read_8_bit_tlv(MLE_TYPE_RSSI, mle_msg->data_ptr, mle_msg->data_length, &linkMarginfronNeigh)) {
-                thread_routing_update_link_margin(cur, entry_temp->short_adr, linkMargin, linkMarginfronNeigh);
+                thread_routing_update_link_margin(cur, entry_temp->mac16, linkMargin, linkMarginfronNeigh);
             }
 
-            mac_helper_devicetable_set(entry_temp, cur, llFrameCounter, security_headers->KeyIndex, new_neigbour);
+            mlme_device_descriptor_t device_desc;
+            mac_helper_device_description_write(cur, &device_desc, entry_temp->mac64, entry_temp->mac16,llFrameCounter, false);
+            mac_helper_devicetable_set(&device_desc, cur, entry_temp->index, security_headers->KeyIndex, new_neigbour);
 
             //Copy Leader Data
             *cur->thread_info->thread_leader_data = leaderData;
@@ -949,17 +949,17 @@ 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)
+int thread_router_bootstrap_reset_child_info(protocol_interface_info_entry_t *cur, struct mac_neighbor_table_entry *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) {
+    if (thread_is_router_addr(child->mac16) || child->mac16 >= 0xfffe) {
         return -1;
     }
-    tr_debug("Child free %x", child->short_adr);
+    tr_debug("Child free %x", child->mac16);
     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
@@ -967,12 +967,12 @@ int thread_router_bootstrap_reset_child_info(protocol_interface_info_entry_t *cu
      * 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);
+    protocol_6lowpan_release_short_link_address_from_neighcache(cur, child->mac16);
 
     // 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)) {
+    if (thread_network_data_services_registered(&cur->thread_info->networkDataStorage, child->mac16)) {
         tr_debug("Remove references to Child's RLOC16 from the Network Data");
-        thread_management_client_network_data_unregister(cur->id, child->short_adr);
+        thread_management_client_network_data_unregister(cur->id, child->mac16);
     }
 
     // Clear all (sleepy) child registrations to multicast groups
@@ -991,13 +991,11 @@ void thread_router_bootstrap_child_information_clear(protocol_interface_info_ent
     }
 
     // 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 (table_entry->short_adr < 0xfffe && !thread_is_router_addr(table_entry->short_adr)) {
-            mle_class_remove_entry(cur->id, table_entry);
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
+
+    ns_list_foreach_safe(mac_neighbor_table_entry_t, table_entry, mac_table_list) {
+        if (table_entry->mac16 < 0xfffe && !thread_is_router_addr(table_entry->mac16)) {
+            mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), table_entry);
         }
     }
 }
@@ -1006,17 +1004,14 @@ static void thread_router_bootstrap_invalid_child_information_clear(protocol_int
 
     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;
-    }
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
 
     // 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);
+    ns_list_foreach_safe(mac_neighbor_table_entry_t, table_entry, mac_table_list) {
+        if (table_entry->mac16 < 0xfffe &&
+                !thread_is_router_addr(table_entry->mac16) &&
+                thread_router_addr_from_addr(table_entry->mac16) != router_rloc) {
+            mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), table_entry);
         }
     }
 }
@@ -1105,7 +1100,7 @@ void thread_router_bootstrap_router_id_request(protocol_interface_info_entry_t *
     }
 }
 
-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)
+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, struct mac_neighbor_table_entry *neigh_info)
 {
     uint16_t len = 12 + 4; //Leader data and short address
     uint8_t *ptr;
@@ -1117,7 +1112,7 @@ static int mle_attach_child_id_response_build(protocol_interface_info_entry_t *c
         return -1;
     }
 
-    if (neigh_info->mode & MLE_THREAD_REQ_FULL_DATA_SET) {
+    if (thread_neighbor_class_request_full_data_setup(&cur->thread_info->neighbor_class, neigh_info->index)) {
         fullList = true;
     }
 
@@ -1163,7 +1158,7 @@ static int mle_attach_child_id_response_build(protocol_interface_info_entry_t *c
     ptr = mle_general_write_source_address(ptr, cur);
 
     if (child_req->shortAddressReq) {
-        ptr = mle_tlv_write_short_address(ptr, neigh_info->short_adr);
+        ptr = mle_tlv_write_short_address(ptr, neigh_info->mac16);
     }
 
     if (child_req->routeReq) {
@@ -1214,12 +1209,10 @@ uint16_t thread_router_bootstrap_child_count_get(protocol_interface_info_entry_t
     if (router_address >= 0xfffe) {
         return 0;
     }
-    mle_neigh_table_list_t *mle_table = mle_class_active_list_get(cur->id);
-    if (!mle_table) {
-        return -1;
-    }
-    ns_list_foreach(mle_neigh_table_entry_t, cur_entry, mle_table) {
-        if (thread_router_addr_from_addr(cur_entry->short_adr) == router_address) {
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
+
+    ns_list_foreach(mac_neighbor_table_entry_t, cur_entry, mac_table_list) {
+        if (thread_router_addr_from_addr(cur_entry->mac16) == router_address) {
             child_count++;
         }
     }
@@ -1228,22 +1221,19 @@ uint16_t thread_router_bootstrap_child_count_get(protocol_interface_info_entry_t
 
 static uint16_t thread_router_bootstrap_child_address_generate(protocol_interface_info_entry_t *cur)
 {
-    mle_neigh_table_list_t *mle_table = mle_class_active_list_get(cur->id);
-    if (!mle_table) {
-        return -1;
-    }
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
 
     if (thread_router_bootstrap_child_count_get(cur) >= cur->thread_info->maxChildCount) {
         tr_info("Maximum count %d reached", cur->thread_info->maxChildCount);
-        return 0xffff;
+        return 0xfffe;
     }
 
     bool address_allocated = false;
     for (uint16_t i = 0; i < cur->thread_info->maxChildCount; i++) {
         address_allocated = false;
         cur->thread_info->lastAllocatedChildAddress = (cur->thread_info->lastAllocatedChildAddress % THREAD_MAX_CHILD_ID_COUNT) + 1;
-        ns_list_foreach(mle_neigh_table_entry_t, cur_entry, mle_table) {
-            if ( (cur_entry->short_adr & THREAD_CHILD_MASK) == cur->thread_info->lastAllocatedChildAddress) {
+        ns_list_foreach(mac_neighbor_table_entry_t, cur_entry, mac_table_list) {
+            if ( (cur_entry->mac16 & THREAD_CHILD_MASK) == cur->thread_info->lastAllocatedChildAddress) {
                 address_allocated = true;
                 break;
             }
@@ -1254,29 +1244,29 @@ static uint16_t thread_router_bootstrap_child_address_generate(protocol_interfac
     }
     if (address_allocated){
         // all possible addresses already allocated
-        return 0xffff;
+        return 0xfffe;
     }
     return ((mac_helper_mac16_address_get(cur) & THREAD_ROUTER_MASK) | cur->thread_info->lastAllocatedChildAddress);
 }
 
-static bool thread_child_id_request(protocol_interface_info_entry_t *cur, mle_neigh_table_entry_t *entry_temp)
+static bool thread_child_id_request(protocol_interface_info_entry_t *cur, struct mac_neighbor_table_entry *entry_temp)
 {
         //Remove All Short address links from router
-        if (entry_temp->short_adr != 0xffff) {
-            protocol_6lowpan_release_short_link_address_from_neighcache(cur, entry_temp->short_adr);
+        if (entry_temp->mac16 < 0xfffe) {
+            protocol_6lowpan_release_short_link_address_from_neighcache(cur, entry_temp->mac16);
         }
 
         //allocate child address if current is router, 0xffff or not our child
-        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);
+        if (!thread_addr_is_child(mac_helper_mac16_address_get(cur), entry_temp->mac16)) {
+            entry_temp->mac16 = thread_router_bootstrap_child_address_generate(cur);
         }
 
-        if (entry_temp->short_adr == 0xffff) {
+        if (entry_temp->mac16 >= 0xfffe) {
             return false;
         }
 
         // Store this child info to the NVM
-        thread_dynamic_storage_child_info_store(cur->id, entry_temp);
+        thread_dynamic_storage_child_info_store(cur, entry_temp);
     //}
     return true;
 }
@@ -1311,7 +1301,7 @@ void thread_router_bootstrap_child_id_handler(protocol_interface_info_entry_t *c
     memcpy(&ll64[8], req->euid64 , 8);
     ll64[8] ^= 2;
     //Allocate entry
-    mle_neigh_table_entry_t *entry_temp = mle_class_get_entry_by_ll64(cur->id, req->linkMargin, ll64, true, &new_neigbour);
+    mac_neighbor_table_entry_t *entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), ll64, true, &new_neigbour);
 
     if (!entry_temp) {
         //Send link reject
@@ -1319,44 +1309,48 @@ void thread_router_bootstrap_child_id_handler(protocol_interface_info_entry_t *c
         goto free_request;
     }
 
+    thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index, req->linkMargin, new_neigbour);
+    thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
+
     // If mac frame couter is less than previous we need to leave the old one
 
     //Update or set neigh info
-    entry_temp->holdTime = 90;
-
-    mle_entry_timeout_update(entry_temp, req->timeout);
-    entry_temp->mode = req->mode;
-    entry_temp->threadNeighbor = true;
-    entry_temp->handshakeReady = 1;
-    entry_temp->mle_frame_counter = req->mleFrameCounter;
+    mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_temp, req->timeout);
+    mle_mode_parse_to_mac_entry(entry_temp, req->mode);
+    thread_neighbor_class_mode_parse_to_entry(&cur->thread_info->neighbor_class, entry_temp->index, req->mode);
+    entry_temp->connected_device = 1;
+    mle_service_frame_counter_entry_add(cur->id, entry_temp->index, req->mleFrameCounter);
 
     if (req->shortAddressReq) {
         if (!thread_child_id_request(cur, entry_temp)) {
-            mle_class_remove_entry(cur->id, entry_temp);
+            mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), entry_temp);
             thread_link_reject_send(cur, ll64);
             goto free_request;
         }
     }
     if (new_neigbour) {
-        mac_helper_devicetable_set(entry_temp, cur, req->frameCounter, req->keyId, new_neigbour);
+        mlme_device_descriptor_t device_desc;
+        mac_helper_device_description_write(cur, &device_desc, entry_temp->mac64, entry_temp->mac16,req->frameCounter, false);
+        mac_helper_devicetable_set(&device_desc, cur, entry_temp->index, req->keyId, new_neigbour);
     } else {
         // in get response handler this will update the short address from MLE table
         mlme_get_t get_req;
         get_req.attr = macDeviceTable;
-        get_req.attr_index = entry_temp->attribute_index;
+        get_req.attr_index = entry_temp->index;
         cur->mac_api->mlme_req(cur->mac_api, MLME_GET, &get_req);
     }
 
     //Register MLEID if RRFD device
-    if ((entry_temp->mode & MLE_DEV_MASK) == MLE_RFD_DEV) {
+    if (!entry_temp->ffd_device) {
         uint8_t tempIPv6Address[16];
         memcpy(tempIPv6Address, thread_info(cur)->threadPrivatePrefixInfo.ulaPrefix, 8);
         memcpy(&tempIPv6Address[8], req->eiid, 8);
-        memcpy(&entry_temp->mlEid, req->eiid, 8);
+
+        thread_neighbor_class_add_mleid(&cur->thread_info->neighbor_class, entry_temp->index, req->eiid);
 
         tr_debug("Register %s", trace_ipv6(tempIPv6Address));
         //Register GP --> 16
-        thread_nd_address_registration(cur, tempIPv6Address, entry_temp->short_adr, cur->mac_parameters->pan_id, entry_temp->mac64);
+        thread_nd_address_registration(cur, tempIPv6Address, entry_temp->mac16, cur->mac_parameters->pan_id, entry_temp->mac64, NULL);
     }
 
     mle_attach_child_id_response_build(cur,ll64,req, entry_temp);
@@ -1371,6 +1365,7 @@ static void thread_address_registration_tlv_parse(uint8_t *ptr, uint16_t data_le
     lowpan_context_t *ctx;
     uint8_t tempIPv6Address[16];
     uint8_t ctxId;
+    bool new_neighbour_created;
 
     while (data_length) {
         //Read
@@ -1383,8 +1378,8 @@ static void thread_address_registration_tlv_parse(uint8_t *ptr, uint16_t data_le
                 memcpy(&tempIPv6Address[8], ptr, 8);
                 tr_debug("Register %s", trace_ipv6(tempIPv6Address));
                 //Register GP --> 16
-                thread_nd_address_registration(cur, tempIPv6Address, mac16, cur->mac_parameters->pan_id, mac64);
-                thread_extension_address_registration(cur, tempIPv6Address, mac64);
+                int retVal = thread_nd_address_registration(cur, tempIPv6Address, mac16, cur->mac_parameters->pan_id, mac64, &new_neighbour_created);
+                thread_extension_address_registration(cur, tempIPv6Address, mac64, new_neighbour_created, retVal == -2);
             } else {
                 tr_debug("No Context %u", ctxId);
             }
@@ -1403,8 +1398,8 @@ static void thread_address_registration_tlv_parse(uint8_t *ptr, uint16_t data_le
                 }
             } else {
                 //Register GP --> 16
-                thread_nd_address_registration(cur, ptr, mac16, cur->mac_parameters->pan_id, mac64);
-                thread_extension_address_registration(cur, ptr, mac64);
+                int retVal = thread_nd_address_registration(cur, ptr, mac16, cur->mac_parameters->pan_id, mac64, &new_neighbour_created);
+                thread_extension_address_registration(cur, ptr, mac64, new_neighbour_created, retVal == -2);
             }
 
             ptr += 16;
@@ -1415,14 +1410,12 @@ static void thread_address_registration_tlv_parse(uint8_t *ptr, uint16_t data_le
 
 void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *mle_msg, mle_security_header_t *security_headers)
 {
+    (void) interface_id;
     thread_leader_data_t leaderData;
-    mle_neigh_table_entry_t *entry_temp;
+    mac_neighbor_table_entry_t *entry_temp;
     bool rssiTLV;
     bool leaderDataReceived;
-    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
-    if (!cur) {
-        return;
-    }
+    protocol_interface_info_entry_t *cur = mle_msg->interface_ptr;
 
     //TLV REQUSTED
     if (mle_tlv_type_requested(MLE_TYPE_RSSI, mle_msg->data_ptr, mle_msg->data_length)) {
@@ -1471,9 +1464,13 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
                 }
 
                 // parent request received
-                entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, false, NULL);
+                entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, false, NULL);
                 if (entry_temp) {
-                    entry_temp->mode = (MLE_FFD_DEV | MLE_RX_ON_IDLE | MLE_THREAD_REQ_FULL_DATA_SET);
+                    //Set MAC modes
+                    mle_mode_parse_to_mac_entry(entry_temp, (MLE_FFD_DEV | MLE_RX_ON_IDLE));
+                    thread_neighbor_class_mode_parse_to_entry(&cur->thread_info->neighbor_class, entry_temp->index, MLE_THREAD_REQ_FULL_DATA_SET);
+                    thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index, linkMargin, false);
+                    thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
                 }
                 if(!entry_temp) {
                     if (!mle_tlv_read_8_bit_tlv(MLE_TYPE_MODE, mle_msg->data_ptr, mle_msg->data_length, &mode)) {
@@ -1487,17 +1484,17 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
                         return;
                     }
 
-                    if (mle_class_free_entry_count_get(cur->id) < 1) {
+                    if (mle_class_free_entry_count_get(cur) < 1) {
                         tr_info("Drop MLE message: no space left in the MLE table");
                         return;
                     }
 
-                    if ((mode & MLE_DEV_MASK) == MLE_RFD_DEV && mle_class_rfd_entry_count_get(cur->id) >= THREAD_MAX_MTD_CHILDREN) {
+                    if ((mode & MLE_DEV_MASK) == MLE_RFD_DEV && mle_class_rfd_entry_count_get(cur) >= THREAD_MAX_MTD_CHILDREN) {
                         tr_info("Drop MLE message: maximum MTD child count reached.");
                         return;
                     }
 
-                    if (!(mode & MLE_RX_ON_IDLE) && mle_class_sleepy_entry_count_get(cur->id) >= THREAD_MAX_SED_CHILDREN) {
+                    if (!(mode & MLE_RX_ON_IDLE) && mle_class_sleepy_entry_count_get(cur) >= THREAD_MAX_SED_CHILDREN) {
                         tr_info("Drop MLE message: maximum SED child count reached.");
                         return;
                     }
@@ -1571,10 +1568,14 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
                     return;
                 }
 
-                // If we are in REED mode and receive child IR request from our parent, call connection error.
+                // If we are in REED mode and receive child ID request from our parent, call connection error.
                 if (thread_am_reed(cur)) {
                     if (thread_router_parent_address_check(cur, mle_msg->packet_src_address)) {
                         tr_debug("Child ID req from own parent -> connection error");
+                        entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, false, NULL);
+                        if (entry_temp) {
+                            mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), entry_temp);
+                        }
                         thread_bootstrap_connection_error(cur->id, CON_PARENT_CONNECT_DOWN, NULL);
                         return;
                     }
@@ -1714,7 +1715,7 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
                     if (mle_tlv_read_16_bit_tlv(MLE_TYPE_SRC_ADDRESS, mle_msg->data_ptr, mle_msg->data_length, &shortAddress)) {
                         //Correct TLV's lets response
                         if (!thread_is_router_addr(shortAddress)) {
-                            if (leaderDataReceived && thread_info(cur)->thread_leader_data->partitionId == leaderData.partitionId) {
+                            if (leaderDataReceived && thread_partition_match(cur, &leaderData)) {
                                 //REED or end device send response
                                 thread_router_accept_to_endevice(cur, mle_msg, challengeTlv.dataPtr, challengeTlv.tlvLen);
                             } else {
@@ -1726,7 +1727,7 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
                     return;
                 }
 
-                entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, false, NULL);
+                entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, false, NULL);
 
                 if (!mle_tlv_read_16_bit_tlv(MLE_TYPE_SRC_ADDRESS, mle_msg->data_ptr, mle_msg->data_length, &shortAddress)) {
 
@@ -1738,9 +1739,11 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
                      *
                      */
 
-                    if (entry_temp && entry_temp->handshakeReady) {
-                        mle_entry_timeout_refresh(entry_temp);
-                        thread_router_synch_accept_request_build(cur, mle_msg, entry_temp->short_adr, challengeTlv.dataPtr, challengeTlv.tlvLen, requestTlv.dataPtr, requestTlv.tlvLen);
+                    if (entry_temp && entry_temp->connected_device) {
+                        mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_temp, entry_temp->link_lifetime);
+                        thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index,linkMargin, false);
+                        thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
+                        thread_router_synch_accept_request_build(cur, mle_msg, entry_temp->mac16, challengeTlv.dataPtr, challengeTlv.tlvLen, requestTlv.dataPtr, requestTlv.tlvLen);
                     }
                 }
                 bool update_mac_mib = false;
@@ -1759,7 +1762,7 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
                 }
 
                 //validate partition id
-                if (thread_info(cur)->thread_leader_data->partitionId != leaderData.partitionId) {
+                if (!thread_partition_match(cur, &leaderData)) {
                     tr_debug("Drop link request from wrong partition");
                     return;
                 }
@@ -1767,26 +1770,28 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
                 if (entry_temp) {
                     /*remove from child list when becoming router*/
                     // Was this previously our child? If yes, update.
-                    if ((entry_temp->short_adr & THREAD_CHILD_MASK) && thread_router_addr_from_addr(entry_temp->short_adr) == cur->thread_info->routerShortAddress) {
+                    if ((entry_temp->mac16 & THREAD_CHILD_MASK) && thread_router_addr_from_addr(entry_temp->mac16) == cur->thread_info->routerShortAddress) {
                         thread_dynamic_storage_child_info_clear(cur->id, entry_temp);
-                        protocol_6lowpan_release_short_link_address_from_neighcache(cur, entry_temp->short_adr);
+                        protocol_6lowpan_release_short_link_address_from_neighcache(cur, entry_temp->mac16);
                     }
                     update_mac_mib = true;
-                    entry_temp->short_adr = shortAddress; // short address refreshed
+                    entry_temp->mac16 = shortAddress; // short address refreshed
 
-                    if (entry_temp->handshakeReady) {
+                    if (entry_temp->connected_device) {
                         if (mle_tlv_read_tlv(MLE_TYPE_ADDRESS_REGISTRATION, mle_msg->data_ptr, mle_msg->data_length, &addressRegisteredTlv)) {
-                            if (thread_rfd_device(entry_temp->mode)) {
-                                thread_address_registration_tlv_parse(addressRegisteredTlv.dataPtr, addressRegisteredTlv.tlvLen, cur, entry_temp->short_adr, entry_temp->mac64);
+                            if (!entry_temp->ffd_device) {
+                                thread_address_registration_tlv_parse(addressRegisteredTlv.dataPtr, addressRegisteredTlv.tlvLen, cur, entry_temp->mac16, entry_temp->mac64);
                             }
                         }
 
-                        mle_entry_timeout_refresh(entry_temp);
+                        mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_temp, entry_temp->link_lifetime);
+                        thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index,linkMargin, false);
+                        thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
                         if (!mle_neigh_entry_frame_counter_update(entry_temp, mle_msg->data_ptr, mle_msg->data_length, cur, security_headers->KeyIndex) &&  update_mac_mib) {
                             //GET
                             mlme_get_t get_req;
                             get_req.attr = macDeviceTable;
-                            get_req.attr_index = entry_temp->attribute_index;
+                            get_req.attr_index = entry_temp->index;
                             cur->mac_api->mlme_req(cur->mac_api, MLME_GET, &get_req);
                         }
                         response_type = MLE_COMMAND_ACCEPT;
@@ -1853,32 +1858,41 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
                 createNew = thread_bootstrap_link_create_check(cur, shortAddress);
 
                 //Send Response
-                entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, createNew, &new_entry);
+
+                entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, createNew, &new_entry);
                 if (entry_temp) {
 
+                    thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index,linkMargin, new_entry);
+                    thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
                     if (security_headers->KeyIdMode == MAC_KEY_ID_MODE_SRC4_IDX) {
                         thread_management_key_synch_req(cur->id, common_read_32_bit(security_headers->Keysource));
                     }
 
-                    entry_temp->threadNeighbor = true;
-                    entry_temp->short_adr = shortAddress;
-                    mac_helper_devicetable_set(entry_temp, cur, llFrameCounter, security_headers->KeyIndex, new_entry);
-                    if (entry_temp->timeout_rx) {
-                        mle_entry_timeout_refresh(entry_temp);
+                    entry_temp->mac16 = shortAddress;
+                    mlme_device_descriptor_t device_desc;
+                    mac_helper_device_description_write(cur, &device_desc, entry_temp->mac64, entry_temp->mac16,llFrameCounter, false);
+                    mac_helper_devicetable_set(&device_desc, cur, entry_temp->index, security_headers->KeyIndex, new_entry);
+
+                    uint32_t link_lifetime;
+                    if (new_entry) {
+                        link_lifetime = THREAD_DEFAULT_LINK_LIFETIME;
+
                     } else {
-                        mle_entry_timeout_update(entry_temp, THREAD_DEFAULT_LINK_LIFETIME);
+                        link_lifetime = entry_temp->link_lifetime;
                     }
 
+                    mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_temp, link_lifetime);
+
                     if (thread_is_router_addr(shortAddress)) {
-                        entry_temp->handshakeReady = 1;
+                        entry_temp->connected_device = 1;
                     }
 
-                    entry_temp->mode |= MLE_THREAD_REQ_FULL_DATA_SET;
+                    thread_neighbor_class_request_full_data_setup_set(&cur->thread_info->neighbor_class, entry_temp->index, true);
 
-                    thread_routing_update_link_margin(cur, entry_temp->short_adr, linkMargin, linkMarginfronNeigh);
+                    thread_routing_update_link_margin(cur, entry_temp->mac16, linkMargin, linkMarginfronNeigh);
                     //Read Source address and Challenge
                     mac_data_poll_protocol_poll_mode_decrement(cur);
-                    thread_router_accept_request_build(cur, mle_msg, entry_temp->short_adr, challengeTlv.dataPtr, challengeTlv.tlvLen, MLE_COMMAND_ACCEPT, rssiTLV, linkMargin);
+                    thread_router_accept_request_build(cur, mle_msg, entry_temp->mac16, challengeTlv.dataPtr, challengeTlv.tlvLen, MLE_COMMAND_ACCEPT, rssiTLV, linkMargin);
 
                     blacklist_update(mle_msg->packet_src_address, true);
                 } else {
@@ -1899,11 +1913,12 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
                     mle_tlv_info_t addressRegisterTlv = {0};
                     mle_tlv_info_t challengeTlv = {0};
                     mle_tlv_info_t tlv_req = {0};
-                    entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, false, NULL);
+                    entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, false, NULL);
 
                     if (mle_tlv_read_8_bit_tlv(MLE_TYPE_STATUS, mle_msg->data_ptr, mle_msg->data_length, &status)) {
                         if (1 == status && thread_check_is_this_my_parent(cur, entry_temp)) {
-                            tr_debug("parent has removed REED");
+                            tr_debug("Parent has removed REED");
+                            mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), entry_temp);
                             thread_bootstrap_connection_error(cur->id, CON_PARENT_CONNECT_DOWN, NULL);
                         }
                         return;
@@ -1923,9 +1938,10 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
                     }
 
                     //Keep alive updated
-                    entry_temp->ttl = entry_temp->timeout_rx;
-                    entry_temp->last_contact_time = protocol_core_monotonic_time;
-                    entry_temp->mode = mode;
+                    thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index,linkMargin, false);
+                    thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
+                    mle_mode_parse_to_mac_entry(entry_temp, mode);
+                    thread_neighbor_class_mode_parse_to_entry(&cur->thread_info->neighbor_class, entry_temp->index, mode);
 
                     addressRegisterTlv.tlvType = MLE_TYPE_UNASSIGNED;
                     mle_tlv_read_tlv(MLE_TYPE_ADDRESS_REGISTRATION, mle_msg->data_ptr, mle_msg->data_length, &addressRegisterTlv);
@@ -1935,25 +1951,26 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
                     mle_tlv_read_tlv(MLE_TYPE_TLV_REQUEST, mle_msg->data_ptr, mle_msg->data_length, &tlv_req);
 
                     if (addressRegisterTlv.tlvType == MLE_TYPE_ADDRESS_REGISTRATION &&
-                        thread_rfd_device(entry_temp->mode)) {
-
+                        !entry_temp->ffd_device) {
                         tr_debug("Register child address");
-                        thread_address_registration_tlv_parse(addressRegisterTlv.dataPtr, addressRegisterTlv.tlvLen, cur, entry_temp->short_adr, entry_temp->mac64);
+                        thread_address_registration_tlv_parse(addressRegisterTlv.dataPtr, addressRegisterTlv.tlvLen, cur, entry_temp->mac16, entry_temp->mac64);
                     }
 
                     if (mle_tlv_read_32_bit_tlv(MLE_TYPE_TIMEOUT, mle_msg->data_ptr, mle_msg->data_length, &timeout)) {
 
                         tr_debug("Setting child timeout, value=%"PRIu32, timeout);
-                        entry_temp->holdTime = 90;
-                        mle_entry_timeout_update(entry_temp, timeout);
+                        mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_temp, timeout);
+                    } else {
+                        mac_neighbor_table_neighbor_refresh(mac_neighbor_info(cur), entry_temp, entry_temp->link_lifetime);
                     }
+
                     if (!leaderDataReceived) {
                         tr_debug("Child synch req");
                     }
 
                     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, active_timestamp, pending_timestamp);
+                    thread_child_update_response(cur, mle_msg->packet_src_address, mode, entry_temp->mac16, timeout, &addressRegisterTlv, &tlv_req, &challengeTlv, active_timestamp, pending_timestamp);
 
                 }
                 break;
@@ -1969,11 +1986,16 @@ void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *
             case MLE_COMMAND_DATA_REQUEST: {
                 mle_tlv_info_t requestTlv;
                 tr_info("Recv Router Data Request");
-                entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, false, NULL);
+
+                entry_temp = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur), mle_msg->packet_src_address, false, NULL);
                 if (!entry_temp || !mle_tlv_read_tlv(MLE_TYPE_TLV_REQUEST, mle_msg->data_ptr, mle_msg->data_length, &requestTlv)) {
                     return;
                 }
-                mle_build_and_send_data_response_msg(cur, mle_msg->packet_src_address, mle_msg->data_ptr, mle_msg->data_length, &requestTlv, entry_temp->mode);
+                thread_neighbor_class_update_link(&cur->thread_info->neighbor_class, entry_temp->index,linkMargin, false);
+                thread_neighbor_last_communication_time_update(&cur->thread_info->neighbor_class, entry_temp->index);
+                uint8_t mode = mle_mode_write_from_mac_entry(entry_temp);
+                mode |= thread_neighbor_class_mode_write_from_entry(&cur->thread_info->neighbor_class, entry_temp->index);
+                mle_build_and_send_data_response_msg(cur, mle_msg->packet_src_address, mle_msg->data_ptr, mle_msg->data_length, &requestTlv, mode);
             }
             break;
             default:
@@ -2184,7 +2206,10 @@ void thread_router_bootstrap_child_id_reject(protocol_interface_info_entry_t *cu
     while (req) {
         tr_debug("Remove entry from list");
         //Remove entry from list
-        mle_class_remove_neighbour(cur->id, req->euid64, ADDR_802_15_4_LONG);
+        mac_neighbor_table_entry_t *neighbor = mac_neighbor_table_address_discover(mac_neighbor_info(cur), req->euid64, ADDR_802_15_4_LONG);
+        if (neighbor) {
+            mac_neighbor_table_neighbor_remove(mac_neighbor_info(cur), neighbor);
+        }
 
         ns_dyn_mem_free(req);
         req = thread_child_id_request_entry_get_from_the_list(cur);
@@ -2206,7 +2231,6 @@ void thread_router_bootstrap_active_router_attach(protocol_interface_info_entry_
     cur->lowpan_info |= INTERFACE_NWK_ROUTER_DEVICE;
     thread_routing_activate(&cur->thread_info->routing);
     thread_router_synch_new_router(cur, ADDR_LINK_LOCAL_ALL_ROUTERS);
-    mle_class_mode_set(cur->id, MLE_CLASS_ROUTER);
     thread_bootstrap_ready(cur);
     thread_bootstrap_network_prefixes_process(cur);
     thread_nd_service_activate(cur->id);
@@ -2226,7 +2250,7 @@ static int thread_validate_own_routeid_from_new_mask(const uint8_t *master_route
     return ret_val;
 }
 
-int thread_router_bootstrap_route_tlv_push(protocol_interface_info_entry_t *cur, uint8_t *route_tlv, uint8_t route_len, uint8_t linkMargin, mle_neigh_table_entry_t *entry)
+int thread_router_bootstrap_route_tlv_push(protocol_interface_info_entry_t *cur, uint8_t *route_tlv, uint8_t route_len, uint8_t linkMargin, struct mac_neighbor_table_entry *entry)
 {
     (void) route_len;
 
@@ -2239,9 +2263,9 @@ int thread_router_bootstrap_route_tlv_push(protocol_interface_info_entry_t *cur,
     route_data = route_tlv;
     uint16_t mac16 = mac_helper_mac16_address_get(cur);
 
-    if (!thread_is_router_addr(entry->short_adr)) {
+    if (!thread_is_router_addr(entry->mac16)) {
         // Received route tlv from non router ignore
-        tr_info("drop route Processing from end device %x", entry->short_adr);
+        tr_info("drop route Processing from end device %x", entry->mac16);
         return 0;
     }
 
@@ -2251,21 +2275,20 @@ int thread_router_bootstrap_route_tlv_push(protocol_interface_info_entry_t *cur,
             thread_routing_leader_connection_validate(cur->thread_info,routing->networkFragmentationTimer);
             routing->networkFragmentationTimer = 0;
             if (thread_validate_own_routeid_from_new_mask(router_id_mask, thread_router_id_from_addr(mac16)) != 0) {
-
                 tr_debug("RouterID not valid any More");
                 thread_bootstrap_connection_error(cur->id, CON_ERROR_NETWORK_KICK, NULL);
                 return 0;
             }
         }
     } else if (!thread_info(cur)->thread_endnode_parent ||
-               thread_info(cur)->thread_endnode_parent->shortAddress != entry->short_adr ) {
+               thread_info(cur)->thread_endnode_parent->shortAddress != entry->mac16 ) {
         return 0;
     }
 
     /* XXX Is short_src_adr ever reset? Is it undefined if info not in msg? */
     /* Don't add routing link if MLE link is NOT bi-directional (i.e. we can only hear) */
-    if (entry->handshakeReady) {
-        thread_routing_add_link(cur, entry->short_adr, linkMargin, route_id_seq, router_id_mask, route_data, false);
+    if (entry->connected_device) {
+        thread_routing_add_link(cur, entry->mac16, linkMargin, route_id_seq, router_id_mask, route_data, false);
     }
 
     return 0;
@@ -2342,6 +2365,13 @@ static void thread_reed_advertisements_cb(void* arg)
     protocol_interface_info_entry_t *cur = arg;
 
     cur->thread_info->routerSelectParameters.reedAdvertisementTimeout = NULL;
+
+    if (cur->nwk_bootstrap_state != ER_BOOTSRAP_DONE && cur->nwk_bootstrap_state != ER_MLE_ATTACH_READY) {
+        /* Own attach is ongoing, try to send advertisement after few seconds */
+        cur->thread_info->routerSelectParameters.reedAdvertisementTimeout = eventOS_timeout_ms(thread_reed_advertisements_cb, 2 * 1000, cur);
+        return;
+    }
+
     if (cur->thread_info->thread_attached_state == THREAD_STATE_CONNECTED &&
             cur->thread_info->thread_device_mode == THREAD_DEVICE_MODE_ROUTER){
         thread_reed_advertise(cur);
@@ -2461,23 +2491,22 @@ void thread_router_bootstrap_timer(protocol_interface_info_entry_t *cur, uint32_
     return;
 }
 
-void thread_router_bootstrap_advertiment_analyze(protocol_interface_info_entry_t *cur, uint8_t *src_address, mle_neigh_table_entry_t *entry_temp, uint16_t shortAddress)
+void thread_router_bootstrap_advertiment_analyze(protocol_interface_info_entry_t *cur, uint8_t *src_address, struct mac_neighbor_table_entry *entry_temp, uint16_t shortAddress)
 {
     if (entry_temp) {
-        entry_temp->threadNeighbor = true;
 
-        if (entry_temp->timeout_rx == 0 || thread_is_router_addr(shortAddress)) {
-            entry_temp->timeout_rx = THREAD_DEFAULT_LINK_LIFETIME / MLE_TIMER_TICKS_SECONDS;
-            entry_temp->timeout_rx++;
+        if (thread_is_router_addr(shortAddress)) {
+            entry_temp->link_lifetime = THREAD_DEFAULT_LINK_LIFETIME;
+            entry_temp->link_lifetime++;
         }
 
         if (thread_is_router_addr(shortAddress)) {
             //Update MAC Security PIB table by get & set Operation
             mlme_get_t get_req;
             get_req.attr = macDeviceTable;
-            get_req.attr_index = entry_temp->attribute_index;
+            get_req.attr_index = entry_temp->index;
             cur->mac_api->mlme_req(cur->mac_api, MLME_GET, &get_req);
-            entry_temp->ttl = entry_temp->timeout_rx;
+            entry_temp->lifetime = entry_temp->link_lifetime;
         }
     } else {
         //
@@ -2661,19 +2690,18 @@ static int thread_router_bootstrap_network_data_propagation(protocol_interface_i
 static void thread_router_bootstrap_network_data_push_to_sleep_child(protocol_interface_info_entry_t *cur, bool stableDataUpdate)
 {
     uint8_t childLinkLocalAddress[16];
-    mle_neigh_table_list_t *mle_table = mle_class_active_list_get(cur->id);
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
+
     memcpy(childLinkLocalAddress, ADDR_LINK_LOCAL_PREFIX, 8);
-    ns_list_foreach(mle_neigh_table_entry_t, cur_entry, mle_table) {
-        if (cur_entry->threadNeighbor) {
-            if (!(cur_entry->mode & MLE_RX_ON_IDLE)) {
-                memcpy(&childLinkLocalAddress[8], cur_entry->mac64, 8);
-                childLinkLocalAddress[8] ^= 2;
-                if (cur_entry->mode & MLE_THREAD_REQ_FULL_DATA_SET) {
-                    thread_router_bootstrap_network_data_propagation(cur, childLinkLocalAddress, true);
-                } else {
-                    if (stableDataUpdate) {
-                        thread_router_bootstrap_network_data_propagation(cur, childLinkLocalAddress, false);
-                    }
+    ns_list_foreach(mac_neighbor_table_entry_t, cur_entry, mac_table_list) {
+        if (!cur_entry->rx_on_idle) {
+            memcpy(&childLinkLocalAddress[8], cur_entry->mac64, 8);
+            childLinkLocalAddress[8] ^= 2;
+            if (thread_neighbor_class_request_full_data_setup(&cur->thread_info->neighbor_class, cur_entry->index)) {
+                thread_router_bootstrap_network_data_propagation(cur, childLinkLocalAddress, true);
+            } else {
+                if (stableDataUpdate) {
+                    thread_router_bootstrap_network_data_propagation(cur, childLinkLocalAddress, false);
                 }
             }
         }
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.h
index faf2f7db0d..0143ce1e30 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_router_bootstrap.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -43,13 +43,14 @@ struct protocol_interface_info_entry;
 struct thread_info_s;
 struct mle_security_header;
 struct buffer;
+struct mac_neighbor_table_entry;
 
 void thread_router_bootstrap_reed_advertisements_start(protocol_interface_info_entry_t *cur);
 void thread_router_bootstrap_reed_merge_advertisement(protocol_interface_info_entry_t *cur);
 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);
+int thread_router_bootstrap_reset_child_info(protocol_interface_info_entry_t *cur, struct mac_neighbor_table_entry *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);
@@ -61,11 +62,11 @@ int thread_router_bootstrap_link_synch_start(struct protocol_interface_info_entr
 bool thread_router_bootstrap_router_downgrade(struct protocol_interface_info_entry *cur);
 bool thread_router_bootstrap_reed_upgrade(struct protocol_interface_info_entry *cur);
 void thread_router_bootstrap_active_router_attach(struct protocol_interface_info_entry *cur);
-int thread_router_bootstrap_route_tlv_push(protocol_interface_info_entry_t *cur, uint8_t *route_tlv, uint8_t route_len, uint8_t linkMargin, mle_neigh_table_entry_t *entry);
+int thread_router_bootstrap_route_tlv_push(protocol_interface_info_entry_t *cur, uint8_t *route_tlv, uint8_t route_len, uint8_t linkMargin, struct mac_neighbor_table_entry *entry);
 void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *mle_msg, struct mle_security_header *security_headers);
 void thread_router_bootstrap_timer(protocol_interface_info_entry_t *cur, uint32_t ticks);
-uint32_t thread_router_bootstrap_random_upgrade_jitter();
-void thread_router_bootstrap_advertiment_analyze(protocol_interface_info_entry_t *cur, uint8_t *src_address, mle_neigh_table_entry_t *entry_temp, uint16_t shortAddress);
+uint32_t thread_router_bootstrap_random_upgrade_jitter(void);
+void thread_router_bootstrap_advertiment_analyze(protocol_interface_info_entry_t *cur, uint8_t *src_address, struct mac_neighbor_table_entry *entry_temp, uint16_t shortAddress);
 
 void thread_router_bootstrap_multicast_forwarder_enable(protocol_interface_info_entry_t *cur, buffer_t *buf);
 void thread_router_bootstrap_anycast_address_register(protocol_interface_info_entry_t *cur);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.c
index 8fdf01da7b..43042de94e 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -52,6 +52,7 @@
 #include "6LoWPAN/Thread/thread_routing.h"
 #include "6LoWPAN/Thread/thread_leader_service.h"
 #include "6LoWPAN/MAC/mac_helper.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 
 #define TRACE_GROUP "trou"
 
@@ -183,7 +184,7 @@ static inline thread_link_quality_e thread_quality_combine(thread_link_quality_e
 /* Return the quality (worse of incoming and outgoing quality) for a neighbour router */
 static inline thread_link_quality_e thread_neighbour_router_quality(const thread_router_link_t *neighbour)
 {
-    return thread_quality_combine(neighbour->incoming_quality, neighbour->outgoing_quality);
+    return thread_quality_combine((thread_link_quality_e) neighbour->incoming_quality, (thread_link_quality_e) neighbour->outgoing_quality);
 }
 
 
@@ -265,8 +266,8 @@ static int_fast8_t thread_route_fn(
     uint16_t dest_router_addr = thread_router_addr_from_addr(dest);
     if (dest_router_addr == mac16) {
         /* We're this device's parent - transmit direct to it */
-        mle_neigh_table_entry_t *entry = mle_class_get_by_link_address(cur->id, dest_addr, ADDR_802_15_4_SHORT);
-        if (!entry || (entry->mode & MLE_DEV_MASK) == MLE_RFD_DEV) {
+        mac_neighbor_table_entry_t *entry = mac_neighbor_table_address_discover(mac_neighbor_info(cur), dest_addr, ADDR_802_15_4_SHORT);
+        if (!entry || !entry->ffd_device) {
             /* To cover some of draft-kelsey-thread-network-data-00, we send the
              * packet up to our own IP layer in the case where it's addressed to
              * an unrecognised child. The special IP forwarding rules can then
@@ -690,8 +691,8 @@ int_fast8_t thread_routing_add_link(protocol_interface_info_entry_t *cur,
         if (our_quality_to_other_neighbour < QUALITY_10dB) {
             continue;
         }
-        thread_link_quality_e neighbours_incoming_quality_to_other_neighbour = (byte & ROUTE_DATA_IN_MASK) >> ROUTE_DATA_IN_SHIFT;
-        thread_link_quality_e neighbours_outgoing_quality_to_other_neighbour = (byte & ROUTE_DATA_OUT_MASK) >> ROUTE_DATA_OUT_SHIFT;
+        thread_link_quality_e neighbours_incoming_quality_to_other_neighbour = (thread_link_quality_e) ((byte & ROUTE_DATA_IN_MASK) >> ROUTE_DATA_IN_SHIFT);
+        thread_link_quality_e neighbours_outgoing_quality_to_other_neighbour = (thread_link_quality_e) ((byte & ROUTE_DATA_OUT_MASK) >> ROUTE_DATA_OUT_SHIFT);
         thread_link_quality_e neighbours_quality_to_other_neighbour = thread_quality_combine(neighbours_incoming_quality_to_other_neighbour,
                                                                                              neighbours_outgoing_quality_to_other_neighbour);
         if (neighbours_quality_to_other_neighbour < our_quality_to_other_neighbour) {
@@ -975,6 +976,21 @@ static void thread_trickle_accelerate(trickle_t *t, const trickle_params_t *para
     }
 }
 
+void thread_routing_trickle_advance(thread_routing_info_t *routing, uint16_t ticks)
+{
+    trickle_t *t =&routing->mle_advert_timer;
+
+    if (!trickle_running(t, &thread_mle_advert_trickle_params)) {
+        return;
+    }
+
+    if ((t->t > t->now) && (t->t - t->now < ticks)) {
+        /* advance trickle elapsing time by number of ticks */
+        t->t = t->t + ticks - (t->t - t->now);
+        tr_debug("trickle advanced to %d, now %d ", t->t, t->now);
+    }
+}
+
 // This functions speeds up next advertisement depending on the disconnect period to leader
 void thread_routing_leader_connection_validate(thread_info_t *thread, uint16_t disconnect_period)
 {
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.h
index 9896e982c0..e895bb03e5 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_routing.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -187,6 +187,7 @@ void thread_routing_free(thread_routing_info_t *routing);
 void thread_routing_activate(thread_routing_info_t *routing);
 void thread_routing_deactivate(thread_routing_info_t *routing);
 bool thread_routing_timer(struct thread_info_s *thread, uint8_t ticks);
+void thread_routing_trickle_advance(thread_routing_info_t *routing, uint16_t ticks);
 void thread_routing_leader_connection_validate(struct thread_info_s *thread, uint16_t disconnect_period);
 void thread_routing_set_mesh_callbacks(protocol_interface_info_entry_t *cur);
 
@@ -233,6 +234,7 @@ int_fast8_t thread_routing_get_route_data(protocol_interface_info_entry_t *cur,
 #define thread_routing_activate(routing)
 #define thread_routing_deactivate(routing)
 #define thread_routing_timer(thread, ticks) false
+#define thread_routing_trickle_advance(routing, ticks)
 #define thread_routing_leader_connection_validate(thread, disconnect_period)
 #define thread_routing_set_mesh_callbacks(cur)
 #define thread_routing_cost_get_by_router_id(routing, routerId) (0)
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_test_api.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_test_api.c
index c6504beee8..d2da48561e 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_test_api.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/Thread/thread_test_api.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: BSD-3-Clause
  *
  * Redistribution and use in source and binary forms, with or without
@@ -49,11 +49,13 @@
 #include "6LoWPAN/Thread/thread_discovery.h"
 #include "6LoWPAN/Thread/thread_nvm_store.h"
 #include "6LoWPAN/Thread/thread_extension_bootstrap.h"
+#include "6LoWPAN/Thread/thread_neighbor_class.h"
 #include "MLE/mle.h"
 #include "thread_meshcop_lib.h"
 #include "thread_diagcop_lib.h"
 #include "coap_service_api.h"
 #include "Service_Libs/mle_service/mle_service_api.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 #include "6LoWPAN/MAC/mac_helper.h"
 
 #define TRACE_GROUP "tapi"
@@ -452,7 +454,7 @@ int thread_test_key_rotation_update(int8_t interface_id, uint32_t thrKeyRotation
         if (cur->lowpan_info & INTERFACE_NWK_ACTIVE) {
             if (cur->configure_flags & INTERFACE_BOOTSTRAP_DEFINED) {
                 linkConfiguration->key_rotation = thrKeyRotation;
-                thread_calculate_key_guard_timer(cur, linkConfiguration, false);
+                thread_key_guard_timer_calculate(cur, linkConfiguration, false);
                 ret_val = 0;
             }
         }
@@ -593,7 +595,7 @@ int thread_test_security_material_set(int8_t interface_id, bool enableSecurity,
                     mle_service_security_set_security_key(cur->id, key_material, key_index, true);
                     //Gen also Next Key
                     thread_security_next_key_generate(cur, linkConfiguration->master_key, thrKeySequenceCounter);
-                    thread_calculate_key_guard_timer(cur, linkConfiguration, false);
+                    thread_key_guard_timer_calculate(cur, linkConfiguration, false);
                 }
             } else {
                 ret_val = 0;
@@ -625,7 +627,8 @@ int thread_test_version_set(int8_t interface_id, uint8_t version)
     thread_version = version;
     cur = protocol_stack_interface_info_get_by_id(interface_id);
     if (!cur) {
-        return -1;
+        /*We already stored the new Thread version above, so even if cur is NULL the version is updated.*/
+        return 0;
     }
     cur->thread_info->version = version;
     return 0;
@@ -1114,22 +1117,22 @@ int8_t thread_test_child_info_get(int8_t interface_id, uint8_t index, uint16_t *
     protocol_interface_info_entry_t *cur;
     uint8_t n= 0;
     cur = protocol_stack_interface_info_get_by_id(interface_id);
-    if (!cur || !cur->thread_info || cur->thread_info->thread_device_mode != THREAD_DEVICE_MODE_ROUTER) {
+    if (!cur || !cur->mac_parameters || !cur->thread_info || cur->thread_info->thread_device_mode != THREAD_DEVICE_MODE_ROUTER) {
         return -1;
     }
     uint16_t mac16 = mac_helper_mac16_address_get(cur);
-    mle_neigh_table_list_t *mle_table = mle_class_active_list_get(interface_id);
-    if (!mle_table) {
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
+    if (!mac_table_list) {
         return -1;
     }
 
-    ns_list_foreach(mle_neigh_table_entry_t, cur_entry, mle_table) {
-        if (cur_entry->threadNeighbor && thread_router_addr_from_addr(cur_entry->short_adr) == thread_router_addr_from_addr(mac16)) {
+    ns_list_foreach(mac_neighbor_table_entry_t, cur_entry, mac_table_list) {
+        if (thread_router_addr_from_addr(cur_entry->mac16) == thread_router_addr_from_addr(mac16)) {
             if (n == index) {
-                *short_addr = cur_entry->short_adr;
+                *short_addr = cur_entry->mac16;
                 memcpy(mac64,cur_entry->mac64, 8);
-                *sleepy = (cur_entry->mode & MLE_RX_ON_IDLE) != MLE_RX_ON_IDLE;
-                *margin = cur_entry->link_margin;
+                *sleepy = cur_entry->rx_on_idle != true;
+                *margin = thread_neighbor_entry_linkmargin_get(&cur->thread_info->neighbor_class, cur_entry->index);
                 return 0;
             }
          n++;
@@ -1156,17 +1159,17 @@ int8_t thread_test_neighbour_info_get(int8_t interface_id, uint8_t index, uint16
         return -1;
     }
     uint16_t mac16 = mac_helper_mac16_address_get(cur);
-    mle_neigh_table_list_t *mle_table = mle_class_active_list_get(interface_id);
-    if (!mle_table) {
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
+    if (!mac_table_list) {
         return -1;
     }
 
-    ns_list_foreach(mle_neigh_table_entry_t, cur_entry, mle_table) {
-        if (cur_entry->threadNeighbor && thread_router_addr_from_addr(cur_entry->short_adr) != thread_router_addr_from_addr(mac16)) {
+    ns_list_foreach(mac_neighbor_table_entry_t, cur_entry, mac_table_list) {
+        if (thread_router_addr_from_addr(cur_entry->mac16) != thread_router_addr_from_addr(mac16)) {
             if (n == index) {
-                *short_addr = cur_entry->short_adr;
+                *short_addr = cur_entry->mac16;
                 memcpy(mac64,cur_entry->mac64, 8);
-                *margin = cur_entry->link_margin;
+                *margin = thread_neighbor_entry_linkmargin_get(&cur->thread_info->neighbor_class, cur_entry->index);
                 return 0;
             }
             n++;
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/adaptation_interface.c b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/adaptation_interface.c
index 230a21893c..226ed6f964 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/adaptation_interface.c
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/adaptation_interface.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -35,6 +35,7 @@
 #include "NWK_INTERFACE/Include/protocol_timer.h"
 #include "Service_Libs/etx/etx.h"
 #include "6LoWPAN/MAC/mac_helper.h"
+#include "6LoWPAN/MAC/mpx_api.h"
 #include "6LoWPAN/Mesh/mesh.h"
 #include "6LoWPAN/IPHC_Decode/iphc_decompress.h"
 #include "lowpan_adaptation_interface.h"
@@ -42,9 +43,15 @@
 #ifdef HAVE_RPL
 #include "RPL/rpl_data.h"
 #endif
+#include "6LoWPAN/ws/ws_common.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
+#include "6LoWPAN/Thread/thread_common.h"
+#include "6LoWPAN/ws/ws_common.h"
 
 #define TRACE_GROUP "6lAd"
 
+typedef void (adaptation_etx_update_cb)(protocol_interface_info_entry_t *cur, buffer_t *buf, const mcps_data_conf_t *confirm);
+
 // #define EXTRA_DEBUG_EXTRA
 #ifdef EXTRA_DEBUG_EXTRA
 #define tr_debug_extra(...) tr_debug(__VA_ARGS__)
@@ -88,6 +95,9 @@ typedef struct {
     uint16_t max_indirect_big_packets_total;
     uint16_t max_indirect_small_packets_per_child;
     bool fragmenter_active; /*!< Fragmenter state */
+    adaptation_etx_update_cb *etx_update_cb;
+    mpx_api_t *mpx_api;
+    uint16_t mpx_user_id;
     ns_list_link_t      link; /*!< List link entry */
 } fragmenter_interface_t;
 
@@ -101,8 +111,8 @@ static void lowpan_adaptation_tx_queue_write(fragmenter_interface_t *interface_p
 static buffer_t * lowpan_adaptation_tx_queue_read(fragmenter_interface_t *interface_ptr, protocol_interface_info_entry_t *cur);
 
 /* Data direction and message length validation */
-static bool lowpan_adaptation_indirect_data_request(mle_neigh_table_entry_t *mle_entry);
-static bool lowpan_adaptation_request_longer_than_mtu(protocol_interface_info_entry_t *cur, buffer_t *buf);
+static bool lowpan_adaptation_indirect_data_request(mac_neighbor_table_entry_t *mle_entry);
+static bool lowpan_adaptation_request_longer_than_mtu(protocol_interface_info_entry_t *cur, buffer_t *buf, fragmenter_interface_t *interface_ptr);
 
 /* Common data tx request process functions */
 static void lowpan_active_buffer_state_reset(fragmenter_tx_entry_t *tx_buffer);
@@ -110,7 +120,7 @@ static uint8_t lowpan_data_request_unique_handle_get(fragmenter_interface_t *int
 static fragmenter_tx_entry_t *lowpan_indirect_entry_allocate(uint16_t fragment_buffer_size);
 static fragmenter_tx_entry_t * lowpan_adaptation_tx_process_init(fragmenter_interface_t *interface_ptr, bool indirect, bool fragmented, bool is_unicast);
 static void lowpan_adaptation_data_request_primitiv_set(const buffer_t *buf, mcps_data_req_t *dataReq, protocol_interface_info_entry_t *cur);
-static void lowpan_data_request_to_mac(protocol_interface_info_entry_t *cur, buffer_t *buf, fragmenter_tx_entry_t *tx_ptr);
+static void lowpan_data_request_to_mac(protocol_interface_info_entry_t *cur, buffer_t *buf, fragmenter_tx_entry_t *tx_ptr, fragmenter_interface_t *interface_ptr);
 
 /* Tx confirmation local functions */
 static bool lowpan_active_tx_handle_verify(uint8_t handle, buffer_t *buf);
@@ -120,12 +130,49 @@ static uint8_t map_mlme_status_to_socket_event(uint8_t mlme_status);
 static bool lowpan_adaptation_tx_process_ready(fragmenter_tx_entry_t *tx_ptr);
 
 /* Fragmentation local functions */
-static int8_t lowpan_message_fragmentation_init(buffer_t *buf, fragmenter_tx_entry_t *frag_entry, protocol_interface_info_entry_t *cur);
+static int8_t lowpan_message_fragmentation_init(buffer_t *buf, fragmenter_tx_entry_t *frag_entry, protocol_interface_info_entry_t *cur, fragmenter_interface_t *interface_ptr);
 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);
 
+static void lowpan_adaptation_etx_update_cb(protocol_interface_info_entry_t *cur, buffer_t *buf, const mcps_data_conf_t *confirm)
+{
+    switch (confirm->status) {
+        case MLME_TX_NO_ACK:
+        case MLME_NO_DATA:
+        case MLME_SUCCESS:
+            if (buf->link_specific.ieee802_15_4.requestAck) {
+                if (cur->lowpan_info & INTERFACE_NWK_BOOTSRAP_MLE) {
+                    bool success = false;
+                    if (confirm->status == MLME_SUCCESS) {
+                        success = true;
+                    }
+                    // Gets table entry
+                    mac_neighbor_table_entry_t *neigh_table_ptr = mac_neighbor_table_address_discover(mac_neighbor_info(cur), buf->dst_sa.address + PAN_ID_LEN, buf->dst_sa.addr_type);
+                    if (neigh_table_ptr) {
+                        etx_transm_attempts_update(cur->id, 1 + confirm->tx_retries , success, neigh_table_ptr->index);
+                        // Updates ETX statistics
+                        etx_storage_t * etx_entry = etx_storage_entry_get(cur->id, neigh_table_ptr->index);
+                        if (etx_entry) {
+                            if (neigh_table_ptr->link_role == PRIORITY_PARENT_NEIGHBOUR) {
+                                protocol_stats_update(STATS_ETX_1ST_PARENT, etx_entry->etx >> 4);
+                            } else if (neigh_table_ptr->link_role == SECONDARY_PARENT_NEIGHBOUR) {
+                                protocol_stats_update(STATS_ETX_2ND_PARENT, etx_entry->etx >> 4);
+                            }
+                        }
+                    }
+                }
+            }
+            break;
+        default:
+
+            break;
+
+    }
+}
+
+
 //Discover
 static fragmenter_interface_t *lowpan_adaptation_interface_discover(int8_t interfaceId)
 {
@@ -139,6 +186,18 @@ static fragmenter_interface_t *lowpan_adaptation_interface_discover(int8_t inter
     return NULL;
 }
 
+static struct protocol_interface_info_entry *lowpan_adaptation_network_interface_discover(const mpx_api_t* api)
+{
+
+    ns_list_foreach(fragmenter_interface_t, interface_ptr, &fragmenter_interface_list) {
+        if (api == interface_ptr->mpx_api) {
+            return protocol_stack_interface_info_get_by_id(interface_ptr->interface_id);
+        }
+    }
+
+    return NULL;
+}
+
 
 static void lowpan_adaptation_tx_queue_write(fragmenter_interface_t *interface_ptr , buffer_t *buf)
 {
@@ -164,7 +223,7 @@ static buffer_t * lowpan_adaptation_tx_queue_read(fragmenter_interface_t *interf
      * Data confirm has freed the corresponding "active buffer" and this function will look for new buffer to be set as active buffer.
      */
     ns_list_foreach_safe(buffer_t, buf, &interface_ptr->directTxQueue) {
-        bool fragmented_needed = lowpan_adaptation_request_longer_than_mtu(cur, buf);
+        bool fragmented_needed = lowpan_adaptation_request_longer_than_mtu(cur, buf, interface_ptr);
         //Check that we not trig second active fragmentation process
         if (fragmented_needed && interface_ptr->fragmenter_active) {
             tr_debug("Do not trig Second active fragmentation");
@@ -179,9 +238,13 @@ static buffer_t * lowpan_adaptation_tx_queue_read(fragmenter_interface_t *interf
 
 //fragmentation needed
 
-static bool lowpan_adaptation_request_longer_than_mtu(protocol_interface_info_entry_t *cur, buffer_t *buf)
+static bool lowpan_adaptation_request_longer_than_mtu(protocol_interface_info_entry_t *cur, buffer_t *buf, fragmenter_interface_t *interface_ptr)
 {
-    uint_fast8_t overhead = mac_helper_frame_overhead(cur, buf);
+    uint_fast16_t overhead = mac_helper_frame_overhead(cur, buf);
+
+    if (interface_ptr->mpx_api) {
+        overhead += interface_ptr->mpx_api->mpx_headroom_size_get(interface_ptr->mpx_api, interface_ptr->mpx_user_id);
+    }
 
 
     if (buffer_data_length(buf) > (int16_t)mac_helper_max_payload_size(cur, overhead)) {
@@ -191,9 +254,9 @@ static bool lowpan_adaptation_request_longer_than_mtu(protocol_interface_info_en
     }
 }
 
-static bool lowpan_adaptation_indirect_data_request(mle_neigh_table_entry_t *mle_entry)
+static bool lowpan_adaptation_indirect_data_request(mac_neighbor_table_entry_t *entry_ptr)
 {
-    if (mle_entry && !(mle_entry->mode & MLE_RX_ON_IDLE)) {
+    if (entry_ptr && !(entry_ptr->rx_on_idle)) {
         return true;
     }
     return false;
@@ -302,6 +365,14 @@ int8_t lowpan_adaptation_interface_init(int8_t interface_id, uint16_t mac_mtu_si
     return 0;
 }
 
+void lowpan_adaptation_interface_etx_update_enable(int8_t interface_id)
+{
+    fragmenter_interface_t *interface_ptr = lowpan_adaptation_interface_discover(interface_id);
+    if (interface_ptr) {
+        interface_ptr->etx_update_cb = lowpan_adaptation_etx_update_cb;
+    }
+}
+
 int8_t lowpan_adaptation_interface_free(int8_t interface_id)
 {
     //Discover
@@ -350,6 +421,44 @@ int8_t lowpan_adaptation_interface_reset(int8_t interface_id)
     return 0;
 }
 
+static void lowpan_adaptation_mpx_data_confirm(const mpx_api_t* api, const struct mcps_data_conf_s *data)
+{
+    protocol_interface_info_entry_t * interface = lowpan_adaptation_network_interface_discover(api);
+
+    lowpan_adaptation_interface_tx_confirm(interface, data);
+}
+
+static void lowpan_adaptation_mpx_data_indication(const mpx_api_t* api, const struct mcps_data_ind_s *data)
+{
+    protocol_interface_info_entry_t * interface = lowpan_adaptation_network_interface_discover(api);
+    lowpan_adaptation_interface_data_ind(interface, data);
+}
+
+
+
+
+int8_t lowpan_adaptation_interface_mpx_register(int8_t interface_id, struct mpx_api_s *mpx_api, uint16_t mpx_user_id)
+{
+    //Discover
+    fragmenter_interface_t *interface_ptr = lowpan_adaptation_interface_discover(interface_id);
+    if (!interface_ptr) {
+        return -1;
+    }
+    if (!mpx_api && interface_ptr->mpx_api) {
+        //Disable Data Callbacks from MPX Class
+        interface_ptr->mpx_api->mpx_user_registration(interface_ptr->mpx_api, NULL, NULL, interface_ptr->mpx_user_id);
+    }
+
+    interface_ptr->mpx_api = mpx_api;
+    interface_ptr->mpx_user_id = mpx_user_id;
+
+    if (interface_ptr->mpx_api) {
+        //Register MPX callbacks: confirmation and indication
+        interface_ptr->mpx_api->mpx_user_registration(interface_ptr->mpx_api, lowpan_adaptation_mpx_data_confirm, lowpan_adaptation_mpx_data_indication, interface_ptr->mpx_user_id);
+    }
+    return 0;
+}
+
 
 static fragmenter_tx_entry_t *lowpan_indirect_entry_allocate(uint16_t fragment_buffer_size)
 {
@@ -377,7 +486,7 @@ static fragmenter_tx_entry_t *lowpan_indirect_entry_allocate(uint16_t fragment_b
     return indirec_entry;
 }
 
-static int8_t lowpan_message_fragmentation_init(buffer_t *buf, fragmenter_tx_entry_t *frag_entry, protocol_interface_info_entry_t *cur)
+static int8_t lowpan_message_fragmentation_init(buffer_t *buf, fragmenter_tx_entry_t *frag_entry, protocol_interface_info_entry_t *cur, fragmenter_interface_t *interface_ptr)
 {
     uint8_t *ptr;
     uint16_t uncompressed_size;
@@ -405,7 +514,11 @@ static int8_t lowpan_message_fragmentation_init(buffer_t *buf, fragmenter_tx_ent
     frag_entry->orig_size = frag_entry->size;
     frag_entry->size += (uncompressed_size - frag_entry->pattern);
 
-    uint_fast8_t overhead = mac_helper_frame_overhead(cur, buf);
+    uint_fast16_t overhead = mac_helper_frame_overhead(cur, buf);
+    if (interface_ptr->mpx_api) {
+        overhead += interface_ptr->mpx_api->mpx_headroom_size_get(interface_ptr->mpx_api, interface_ptr->mpx_user_id);
+    }
+
     frag_entry->frag_max = mac_helper_max_payload_size(cur, overhead);
 
 
@@ -498,24 +611,25 @@ static fragmenter_tx_entry_t * lowpan_adaptation_tx_process_init(fragmenter_inte
 
 buffer_t * lowpan_adaptation_data_process_tx_preprocess(protocol_interface_info_entry_t *cur, buffer_t *buf)
 {
+    mac_neighbor_table_entry_t *neigh_entry_ptr = NULL;
+
     //Validate is link known and set indirect, datareq and security key id mode
     if (buf->dst_sa.addr_type == ADDR_NONE) {
         goto tx_error_handler;
     }
 
-    mle_neigh_table_entry_t *mle_entry = NULL;
     /* If MLE is enabled, we will talk if we have an MLE association */
     if (buf->dst_sa.addr_type == ADDR_802_15_4_LONG ) {
-        mle_entry = mle_class_get_by_link_address(cur->id, buf->dst_sa.address + 2, buf->dst_sa.addr_type);
+        neigh_entry_ptr = mac_neighbor_table_address_discover(mac_neighbor_info(cur), buf->dst_sa.address + 2, buf->dst_sa.addr_type);
 
     } else if(buf->dst_sa.addr_type == ADDR_802_15_4_SHORT && (common_read_16_bit(buf->dst_sa.address + 2)) != 0xffff) {
-        mle_entry = mle_class_get_by_link_address(cur->id, buf->dst_sa.address + 2, buf->dst_sa.addr_type);
+        neigh_entry_ptr = mac_neighbor_table_address_discover(mac_neighbor_info(cur), buf->dst_sa.address + 2, buf->dst_sa.addr_type);
     }
 
     //Validate neighbour
-    if (!buf->options.ll_security_bypass_tx && mle_entry) {
+    if (!buf->options.ll_security_bypass_tx && neigh_entry_ptr) {
 
-        if (mle_entry->handshakeReady ||  mle_entry->thread_commission) {
+        if (neigh_entry_ptr->connected_device ||  neigh_entry_ptr->trusted_device) {
 
         } else {
             //tr_warn("Drop TX to unassociated %s", trace_sockaddr(&buf->dst_sa, true));
@@ -534,14 +648,14 @@ buffer_t * lowpan_adaptation_data_process_tx_preprocess(protocol_interface_info_
         buf->link_specific.ieee802_15_4.requestAck = false;
     } else {
         buf->link_specific.ieee802_15_4.requestAck = true;
-        buf->link_specific.ieee802_15_4.indirectTxProcess = lowpan_adaptation_indirect_data_request(mle_entry);
+        buf->link_specific.ieee802_15_4.indirectTxProcess = lowpan_adaptation_indirect_data_request(neigh_entry_ptr);
     }
 
     if (buf->link_specific.ieee802_15_4.key_id_mode != B_SECURITY_KEY_ID_2) {
 
         if (!buf->link_specific.ieee802_15_4.requestAck ) {
             buf->link_specific.ieee802_15_4.key_id_mode = B_SECURITY_KEY_ID_MODE_DEFAULT;
-        } else if (mle_entry && !mle_entry->thread_commission) {
+        } else if (ws_info(cur) || (neigh_entry_ptr && !neigh_entry_ptr->trusted_device)) {
             buf->link_specific.ieee802_15_4.key_id_mode  = B_SECURITY_KEY_ID_MODE_DEFAULT;
         } else {
             buf->link_specific.ieee802_15_4.key_id_mode  = B_SECURITY_KEY_ID_IMPLICIT;
@@ -551,6 +665,11 @@ buffer_t * lowpan_adaptation_data_process_tx_preprocess(protocol_interface_info_
     return buf;
 
     tx_error_handler:
+    if (neigh_entry_ptr && neigh_entry_ptr->nud_active) {
+        mac_neighbor_info(cur)->active_nud_process--;
+        neigh_entry_ptr->nud_active = false;
+
+    }
     socket_tx_buffer_event_and_free(buf, SOCKET_TX_FAIL);
     return NULL;
 
@@ -616,7 +735,7 @@ static bool lowpan_adaptation_indirect_cache_sanity_check(protocol_interface_inf
             // 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);
+            lowpan_data_request_to_mac(cur, fragmenter_tx_entry->buf, fragmenter_tx_entry, interface_ptr);
             return true;
         }
     }
@@ -638,7 +757,7 @@ static bool lowpan_adaptation_indirect_cache_trigger(protocol_interface_info_ent
             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);
+                lowpan_data_request_to_mac(cur, fragmenter_tx_entry->buf, fragmenter_tx_entry, interface_ptr);
                 return true;
             }
         }
@@ -674,7 +793,7 @@ static fragmenter_tx_entry_t* lowpan_adaptation_indirect_first_cached_request_ge
     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)
+static void lowpan_adaptation_make_room_for_small_packet(protocol_interface_info_entry_t *cur, fragmenter_interface_t *interface_ptr, mac_neighbor_table_entry_t *neighbour_to_count)
 {
     if (interface_ptr->max_indirect_small_packets_per_child == 0) {
         return;
@@ -683,7 +802,7 @@ static void lowpan_adaptation_make_room_for_small_packet(protocol_interface_info
     uint_fast16_t count = 0;
 
     ns_list_foreach_reverse_safe(fragmenter_tx_entry_t, tx_entry, &interface_ptr->indirect_tx_queue) {
-        mle_neigh_table_entry_t *tx_neighbour = mle_class_get_by_link_address(cur->id, tx_entry->buf->dst_sa.address + 2, tx_entry->buf->dst_sa.addr_type);
+        mac_neighbor_table_entry_t *tx_neighbour = mac_neighbor_table_address_discover(mac_neighbor_info(cur), tx_entry->buf->dst_sa.address + 2, tx_entry->buf->dst_sa.addr_type);
         if (tx_neighbour == neighbour_to_count && buffer_data_length(tx_entry->buf) <= interface_ptr->indirect_big_packet_threshold) {
             if (++count >= interface_ptr->max_indirect_small_packets_per_child) {
                 lowpan_adaptation_indirect_queue_free_message(cur, interface_ptr, tx_entry);
@@ -710,7 +829,7 @@ static void lowpan_adaptation_make_room_for_big_packet(struct protocol_interface
     }
 }
 
-static void lowpan_data_request_to_mac(protocol_interface_info_entry_t *cur, buffer_t *buf, fragmenter_tx_entry_t *tx_ptr)
+static void lowpan_data_request_to_mac(protocol_interface_info_entry_t *cur, buffer_t *buf, fragmenter_tx_entry_t *tx_ptr, fragmenter_interface_t *interface_ptr)
 {
     mcps_data_req_t dataReq;
 
@@ -737,7 +856,11 @@ static void lowpan_data_request_to_mac(protocol_interface_info_entry_t *cur, buf
         }
     }
 
-    cur->mac_api->mcps_data_req(cur->mac_api, &dataReq);
+    if (interface_ptr->mpx_api) {
+        interface_ptr->mpx_api->mpx_data_request(interface_ptr->mpx_api, &dataReq, interface_ptr->mpx_user_id);
+    } else {
+        cur->mac_api->mcps_data_req(cur->mac_api, &dataReq);
+    }
 }
 
 int8_t lowpan_adaptation_interface_tx(protocol_interface_info_entry_t *cur, buffer_t *buf)
@@ -756,7 +879,7 @@ int8_t lowpan_adaptation_interface_tx(protocol_interface_info_entry_t *cur, buff
     }
 
     //Check packet size
-    bool fragmented_needed = lowpan_adaptation_request_longer_than_mtu(cur, buf);
+    bool fragmented_needed = lowpan_adaptation_request_longer_than_mtu(cur, buf, interface_ptr);
     bool is_unicast = buf->link_specific.ieee802_15_4.requestAck;
     bool indirect = buf->link_specific.ieee802_15_4.indirectTxProcess;
     if (!indirect) {
@@ -783,7 +906,7 @@ int8_t lowpan_adaptation_interface_tx(protocol_interface_info_entry_t *cur, buff
 
     if (fragmented_needed) {
         //Fragmentation init
-        if (lowpan_message_fragmentation_init(buf, tx_ptr, cur) ) {
+        if (lowpan_message_fragmentation_init(buf, tx_ptr, cur, interface_ptr) ) {
             tr_error("Fragment init fail");
             if (indirect) {
                 ns_dyn_mem_free(tx_ptr->fragmenter_buf);
@@ -801,9 +924,9 @@ 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 (mle_entry) {
-            buf->link_specific.ieee802_15_4.indirectTTL = (uint32_t) mle_entry->timeout_rx * MLE_TIMER_TICKS_MS;
+        mac_neighbor_table_entry_t *neigh_entry_ptr = mac_neighbor_table_address_discover(mac_neighbor_info(cur), buf->dst_sa.address + PAN_ID_LEN, buf->dst_sa.addr_type);
+        if (neigh_entry_ptr) {
+            buf->link_specific.ieee802_15_4.indirectTTL = (uint32_t) neigh_entry_ptr->link_lifetime * 1000;
         } else {
             buf->link_specific.ieee802_15_4.indirectTTL = cur->mac_parameters->mac_in_direct_entry_timeout;
         }
@@ -812,7 +935,7 @@ int8_t lowpan_adaptation_interface_tx(protocol_interface_info_entry_t *cur, buff
 
         // 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);
+            lowpan_adaptation_make_room_for_small_packet(cur, interface_ptr, neigh_entry_ptr);
         } else {
             lowpan_adaptation_make_room_for_big_packet(cur, interface_ptr);
         }
@@ -841,7 +964,7 @@ int8_t lowpan_adaptation_interface_tx(protocol_interface_info_entry_t *cur, buff
         }
     }
 
-    lowpan_data_request_to_mac(cur, buf, tx_ptr);
+    lowpan_data_request_to_mac(cur, buf, tx_ptr, interface_ptr);
     return 0;
 
 
@@ -952,28 +1075,15 @@ int8_t lowpan_adaptation_interface_tx_confirm(protocol_interface_info_entry_t *c
         {
             buf->link_specific.ieee802_15_4.indirectTTL -= 7000;
             //Push Back to MAC
-            lowpan_data_request_to_mac(cur, buf, tx_ptr);
+            lowpan_data_request_to_mac(cur, buf, tx_ptr, interface_ptr);
             return 0;
         }
     }
 
-    switch (confirm->status) {
-        case MLME_TX_NO_ACK:
-        case MLME_NO_DATA:
-        case MLME_SUCCESS:
-            if (buf->link_specific.ieee802_15_4.requestAck) {
-                bool success = false;
-                if (confirm->status == MLME_SUCCESS) {
-                    success = true;
-                }
-                etx_transm_attempts_update(cur->id, 1 + confirm->tx_retries , success, buf->dst_sa.addr_type, buf->dst_sa.address);
-            }
-            break;
-        default:
-
-            break;
-
+    if (interface_ptr->etx_update_cb) {
+        interface_ptr->etx_update_cb(cur, buf, confirm);
     }
+
     //Switch original channel back
     if (buf->link_specific.ieee802_15_4.rf_channel_switch) {
         mac_helper_mac_channel_set(cur, buf->link_specific.ieee802_15_4.selected_channel);
@@ -983,7 +1093,7 @@ int8_t lowpan_adaptation_interface_tx_confirm(protocol_interface_info_entry_t *c
     switch (confirm->status) {
 
         case MLME_BUSY_CHAN:
-            lowpan_data_request_to_mac(cur, buf, tx_ptr);
+            lowpan_data_request_to_mac(cur, buf, tx_ptr, interface_ptr);
             break;
         case MLME_SUCCESS:
 
@@ -1005,7 +1115,7 @@ int8_t lowpan_adaptation_interface_tx_confirm(protocol_interface_info_entry_t *c
                     return 0;
                 }
             } else {
-                lowpan_data_request_to_mac(cur, buf, tx_ptr);
+                lowpan_data_request_to_mac(cur, buf, tx_ptr, interface_ptr);
             }
 
             break;
@@ -1049,6 +1159,73 @@ int8_t lowpan_adaptation_interface_tx_confirm(protocol_interface_info_entry_t *c
 
 }
 
+static bool mac_data_is_broadcast_addr(const sockaddr_t *addr)
+{
+    return (addr->addr_type == ADDR_802_15_4_SHORT) &&
+           (addr->address[2] == 0xFF && addr->address[3] == 0xFF);
+}
+
+static bool mcps_data_indication_neighbor_validate(protocol_interface_info_entry_t *cur, const sockaddr_t *addr)
+{
+    if (thread_info(cur) || ws_info(cur) || (cur->lowpan_info & INTERFACE_NWK_BOOTSRAP_MLE)) {
+        mac_neighbor_table_entry_t * neighbor = mac_neighbor_table_address_discover(mac_neighbor_info(cur), addr->address + 2, addr->addr_type);
+        if (neighbor && (neighbor->connected_device ||  neighbor->trusted_device)) {
+            return true;
+        }
+
+        /* Otherwise, we don't know them */
+        return false;
+    } else {
+        //6lowpan without MLE don't can't do validation
+        return true;
+    }
+
+}
+
+void lowpan_adaptation_interface_data_ind(protocol_interface_info_entry_t *cur, const mcps_data_ind_t *data_ind)
+{
+    buffer_t *buf = buffer_get(data_ind->msduLength);
+    if (!buf || !cur) {
+        return;
+    }
+    uint8_t *ptr;
+    buffer_data_add(buf, data_ind->msdu_ptr, data_ind->msduLength);
+    //tr_debug("MAC Paylod size %u %s",data_ind->msduLength, trace_array(data_ind->msdu_ptr, 8));
+    buf->options.lqi = data_ind->mpduLinkQuality;
+    buf->options.dbm = data_ind->signal_dbm;
+    buf->src_sa.addr_type = (addrtype_t)data_ind->SrcAddrMode;
+    ptr = common_write_16_bit(data_ind->SrcPANId, buf->src_sa.address);
+    memcpy(ptr, data_ind->SrcAddr, 8);
+    buf->dst_sa.addr_type = (addrtype_t)data_ind->DstAddrMode;
+    ptr = common_write_16_bit(data_ind->DstPANId, buf->dst_sa.address);
+    memcpy(ptr, data_ind->DstAddr, 8);
+    //Set Link spesific stuff to seperately
+    buf->link_specific.ieee802_15_4.srcPanId = data_ind->SrcPANId;
+    buf->link_specific.ieee802_15_4.dstPanId = data_ind->DstPANId;
+
+    if (mac_data_is_broadcast_addr(&buf->dst_sa)) {
+        buf->options.ll_broadcast_rx = true;
+    }
+    buf->interface = cur;
+    if (data_ind->Key.SecurityLevel) {
+        buf->link_specific.ieee802_15_4.fc_security = true;
+
+        if (cur->mac_security_key_usage_update_cb) {
+            cur->mac_security_key_usage_update_cb(cur, &data_ind->Key);
+        }
+    } else {
+        buf->link_specific.ieee802_15_4.fc_security = false;
+        if (mac_helper_default_security_level_get(cur) ||
+                !mcps_data_indication_neighbor_validate(cur, &buf->src_sa)) {
+            //SET By Pass
+            buf->options.ll_security_bypass_rx = true;
+        }
+    }
+
+    buf->info = (buffer_info_t)(B_TO_IPV6_TXRX | B_FROM_MAC | B_DIR_UP);
+    protocol_push(buf);
+}
+
 static uint8_t map_mlme_status_to_socket_event(uint8_t mlme_status)
 {
     uint8_t socket_event;
@@ -1104,22 +1281,38 @@ static bool lowpan_tx_buffer_address_compare(sockaddr_t *dst_sa, uint8_t *addres
     return true;
 }
 
-static void lowpan_adaptation_purge_from_mac(struct protocol_interface_info_entry *cur, uint8_t msduhandle)
+static void lowpan_adaptation_purge_from_mac(struct protocol_interface_info_entry *cur, fragmenter_interface_t *interface_ptr,  uint8_t msduhandle)
 {
     mcps_purge_t purge_req;
     purge_req.msduHandle = msduhandle;
-    cur->mac_api->mcps_purge_req(cur->mac_api, &purge_req);
+    if (interface_ptr->mpx_api) {
+        interface_ptr->mpx_api->mpx_data_purge(interface_ptr->mpx_api, &purge_req, interface_ptr->mpx_user_id);
+    } else {
+        if (cur->mac_api->mcps_purge_req) {
+            cur->mac_api->mcps_purge_req(cur->mac_api, &purge_req);
+        }
+    }
 }
 
 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)
 {
     tr_debug("Purge from indirect handle %u", tx_ptr->buf->seq);
-    if (cur->mac_api->mcps_purge_req) {
-        lowpan_adaptation_purge_from_mac(cur, tx_ptr->buf->seq);
-    }
+    lowpan_adaptation_purge_from_mac(cur, interface_ptr, tx_ptr->buf->seq);
     lowpan_adaptation_data_process_clean(interface_ptr, tx_ptr, SOCKET_TX_FAIL);
 }
 
+void lowpan_adaptation_remove_free_indirect_table(protocol_interface_info_entry_t *cur_interface, mac_neighbor_table_entry_t *entry_ptr)
+{
+    //Free firts by defined short address
+    if (entry_ptr->mac16 < 0xfffe) {
+        uint8_t temp_address[2];
+        common_write_16_bit(entry_ptr->mac16, temp_address);
+        lowpan_adaptation_indirect_free_messages_from_queues_by_address(cur_interface, temp_address, ADDR_802_15_4_SHORT);
+    }
+    lowpan_adaptation_indirect_free_messages_from_queues_by_address(cur_interface, entry_ptr->mac64, ADDR_802_15_4_LONG);
+}
+
+
 int8_t lowpan_adaptation_indirect_free_messages_from_queues_by_address(struct protocol_interface_info_entry *cur, uint8_t *address_ptr, addrtype_t adr_type)
 {
     fragmenter_interface_t *interface_ptr = lowpan_adaptation_interface_discover(cur->id);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/lowpan_adaptation_interface.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/lowpan_adaptation_interface.h
index 896c4f6148..6f823b1fa0 100644
--- a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/lowpan_adaptation_interface.h
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/lowpan_adaptation_interface.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -21,13 +21,21 @@
 
 struct protocol_interface_info_entry;
 struct mcps_data_conf_s;
+struct mcps_data_ind_s;
 struct buffer;
+struct mpx_api_s;
+struct mac_neighbor_table_entry;
+
 int8_t lowpan_adaptation_interface_init(int8_t interface_id, uint16_t mac_mtu_size);
 
+void lowpan_adaptation_interface_etx_update_enable(int8_t interface_id);
+
 int8_t lowpan_adaptation_interface_free(int8_t interface_id);
 
 int8_t lowpan_adaptation_interface_reset(int8_t interface_id);
 
+int8_t lowpan_adaptation_interface_mpx_register(int8_t interface_id, struct mpx_api_s *mpx_api, uint16_t mpx_user_id);
+
 /**
  * \brief call this before normatl TX. This function prepare buffer link spesific metadata and verify packet destination
  */
@@ -37,10 +45,14 @@ int8_t lowpan_adaptation_interface_tx(struct protocol_interface_info_entry *cur,
 
 int8_t lowpan_adaptation_interface_tx_confirm(struct protocol_interface_info_entry *cur, const struct mcps_data_conf_s *confirm);
 
+void lowpan_adaptation_interface_data_ind(struct protocol_interface_info_entry *cur, const struct mcps_data_ind_s *data_ind);
+
 struct buffer *lowpan_adaptation_reassembly(struct protocol_interface_info_entry *cur, struct buffer *buf);
 
 bool lowpan_adaptation_tx_active(int8_t interface_id);
 
+void lowpan_adaptation_remove_free_indirect_table(struct protocol_interface_info_entry *cur_interface, struct mac_neighbor_table_entry *entry_ptr);
+
 int8_t lowpan_adaptation_indirect_free_messages_from_queues_by_address(struct protocol_interface_info_entry *cur, uint8_t *address_ptr, addrtype_t adr_type);
 
 int8_t lowpan_adaptation_indirect_queue_params_set(struct protocol_interface_info_entry *cur, uint16_t indirect_big_packet_threshold, uint16_t max_indirect_big_packets_total, uint16_t max_indirect_small_packets_per_child);
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_bootstrap.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_bootstrap.h
new file mode 100644
index 0000000000..4cd3316e5a
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_bootstrap.h
@@ -0,0 +1,62 @@
+/*
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef WS_BOOTSTRAP_H_
+#define WS_BOOTSTRAP_H_
+
+
+typedef enum {
+    WS_INIT_EVENT = 0,       /**< tasklet initializion event*/
+    WS_DISCOVERY_START,      /**< discovery start*/
+    WS_CONFIGURATION_START,  /**< configuration learn start*/
+    WS_AUTHENTICATION_START, /**< authentication start*/
+    WS_OPERATION_START,      /**< active operation start*/
+    WS_ROUTING_READY,        /**< RPL routing connected to BR*/
+    WS_ADDRESS_ADDED         /**< Address added to IF*/
+} ws_bootsrap_event_type_e;
+
+#ifdef HAVE_WS
+
+int ws_bootstrap_init(int8_t interface_id, net_6lowpan_mode_e bootstrap_mode);
+
+void ws_bootstrap_state_machine(protocol_interface_info_entry_t *cur);
+
+/*State machine transactions*/
+void ws_bootstrap_event_discovery_start(protocol_interface_info_entry_t *cur);
+
+void ws_bootstrap_event_configuration_start(protocol_interface_info_entry_t *cur);
+
+void ws_bootstrap_event_authentication_start(protocol_interface_info_entry_t *cur);
+
+void ws_bootstrap_event_operation_start(protocol_interface_info_entry_t *cur);
+
+void ws_bootstrap_event_routing_ready(protocol_interface_info_entry_t *cur);
+
+void ws_bootstrap_configuration_trickle_reset(protocol_interface_info_entry_t *cur);
+
+void ws_bootstrap_seconds_timer(protocol_interface_info_entry_t *cur, uint32_t seconds);
+
+void ws_bootstrap_trigle_timer(protocol_interface_info_entry_t *cur, uint16_t ticks);
+
+#else
+
+#define ws_bootstrap_init(interface_id, bootstrap_mode) (-1)
+#define ws_bootstrap_state_machine(cur)
+
+#endif //HAVE_WS
+
+#endif /* WS_BOOTSTRAP_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common.h
new file mode 100644
index 0000000000..dc02f6938a
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common.h
@@ -0,0 +1,87 @@
+/*
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef WS_COMMON_H_
+#define WS_COMMON_H_
+
+
+#include "ns_types.h"
+#include "fhss_api.h"
+#include "fhss_config.h"
+#include "net_fhss.h"
+#include "6LoWPAN/ws/ws_common_defines.h"
+#include "6LoWPAN/ws/ws_neighbor_class.h"
+
+struct ws_pan_information_s;
+struct ws_neighbor_class_s;
+
+typedef struct parent_info_s {
+        uint16_t pan_id;            /**< PAN ID */
+        uint8_t addr[8];            /**< address */
+        uint8_t link_quality;       /**< LQI value measured during reception of the MPDU */
+        int8_t signal_dbm;          /**< This extension for normal IEEE 802.15.4 Data indication */
+        ws_pan_information_t pan_information;
+        ws_utt_ie_t ws_utt;
+        ws_us_ie_t ws_us;
+        uint32_t timestamp;            /**< Timestamp when packet was received */
+}parent_info_t;
+
+typedef struct ws_info_s {
+    char network_name[33]; // Network name max 32 octets + terminating 0.
+    uint16_t network_pan_id;
+
+    trickle_t trickle_pan_config_solicit;
+    trickle_t trickle_pan_config;
+    trickle_t trickle_pan_advertisement_solicit;
+    trickle_t trickle_pan_advertisement;
+    uint8_t rpl_state; // state from rpl_event_t
+    uint8_t pas_requests; // Amount of PAN solicits sent
+    parent_info_t parent_info;
+    uint32_t pan_version_timer;            /**< border router version udate timeout */
+    uint32_t pan_version_timeout_timer;    /**< routers will fallback to previous state after this */
+    uint8_t gtkhash[32];
+    bool configuration_learned:1;
+
+    struct ws_pan_information_s pan_information;
+    ws_hopping_schedule_t hopping_schdule;
+    struct ws_neighbor_class_s neighbor_storage;
+    struct fhss_timer *fhss_timer_ptr; // Platform adaptation for FHSS timers.
+    struct fhss_api *fhss_api;
+} ws_info_t;
+
+#ifdef HAVE_WS
+
+int8_t ws_common_regulatory_domain_config(protocol_interface_info_entry_t *cur);
+
+int8_t ws_common_allocate_and_init(protocol_interface_info_entry_t *cur);
+
+void ws_common_seconds_timer(protocol_interface_info_entry_t *cur, uint32_t seconds);
+
+void ws_common_fast_timer(protocol_interface_info_entry_t *cur, uint16_t ticks);
+
+void ws_common_neighbor_update(protocol_interface_info_entry_t *cur, const uint8_t *ll_address);
+
+#define ws_info(cur) ((cur)->ws_info)
+#else
+#define ws_info(cur) ((ws_info_t *) NULL)
+#define ws_common_seconds_timer(cur, seconds)
+#define ws_common_neighbor_update(cur, ll_address) ((void) 0)
+#define ws_common_fast_timer(cur, ticks) ((void) 0)
+
+
+#endif //HAVE_WS
+#endif //WS_COMMON_H_
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common_defines.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common_defines.h
new file mode 100644
index 0000000000..4b4c067f70
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_common_defines.h
@@ -0,0 +1,206 @@
+/*
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef WS_COMMON_DEFINES_H_
+#define WS_COMMON_DEFINES_H_
+
+#define WH_IE_ELEMENT_HEADER_LENGTH 3
+
+/* Header IE Sub elements */
+#define WH_IE_UTT_TYPE              1   /**< Unicast Timing and Frame type information */
+#define WH_IE_BT_TYPE               2   /**< Broadcast timing information */
+#define WH_IE_FC_TYPE               3   /**< Flow Control for Extended Direct Frame Exchange */
+#define WH_IE_RSL_TYPE              4   /**< Received Signal Level information */
+#define WH_IE_MHDS_TYPE             5   /**< MHDS information for mesh routing */
+#define WH_IE_VH_TYPE               6   /**< Vendor header information */
+
+#define WS_WP_NESTED_IE             4 /**< WS nested Payload IE element'selement could include mltiple sub payload IE */
+
+#define WS_WP_SUB_IE_ELEMENT_HEADER_LENGTH 2
+
+/* Payload IE sub elements in side WS_WP_NESTED_IE */
+#define WP_PAYLOAD_IE_US_TYPE       1   /**< Unicast Schedule information */
+#define WP_PAYLOAD_IE_BS_TYPE       2   /**< Broadcast Schedule information */
+#define WP_PAYLOAD_IE_VP_TYPE       3   /**< Vendor Payload information */
+#define WP_PAYLOAD_IE_PAN_TYPE      4   /**< PAN Information */
+#define WP_PAYLOAD_IE_NETNAME_TYPE  5   /**< Network Name information */
+#define WP_PAYLOAD_IE_PAN_VER_TYPE  6   /**< Pan configuration version */
+#define WP_PAYLOAD_IE_GTKHASH_TYPE  7   /**< GTK Hash information */
+
+/* WS frame types to WH_IE_UTT_TYPE */
+#define WS_FT_PAN_ADVERT        0          /**< PAN Advert */
+#define WS_FT_PAN_ADVERT_SOL    1          /**< PAN Advert Solicit */
+#define WS_FT_PAN_CONF          2          /**< PAN Config */
+#define WS_FT_PAN_CONF_SOL      3          /**< PAN Config Solicit */
+#define WS_FT_DATA              4          /**< data type inside MPX */
+#define WS_FT_ACK               5          /**< Enhanced ACK */
+#define WS_FT_EAPOL             6          /**< EAPOL message inside MPX */
+
+
+/**
+ * @brief ws_pan_information_t PAN information
+ */
+typedef struct ws_pan_information_s {
+    uint16_t pan_size;          /**< Number devices connected to Border Router. */
+    uint16_t routing_cost;      /**< ETX to border Router. */
+    uint16_t pan_version;       /**< Pan configuration version will be updatd by Border router at PAN. */
+    bool use_parent_bs:1;       /**< 1 for force to follow parent broadcast schedule. 0 node may define own schedule. */
+    bool rpl_routing_method:1;  /**< 1 when RPL routing is selected and 0 when L2 routing. */
+    unsigned version:3;         /**< Pan version support. */
+} ws_pan_information_t;
+
+/**
+ * @brief ws_hopping_schedule_t Chanel hopping schedule information
+ */
+typedef struct ws_hopping_schedule_s {
+    uint8_t fhss_uc_dwell_interval;
+    uint8_t fhss_bc_dwell_interval;
+    uint8_t regulatory_domain;          /**< PHY regulatory domain default to "KR" 0x09 */
+    uint8_t operating_class;            /**< PHY operating class default to 1 */
+    uint8_t operating_mode;             /**< PHY operating mode default to "1b" symbol rate 50, modulation index 1 */
+    uint8_t channel_plan;               /**< 0: use regulatory domain values 1: application defined plan */
+    uint8_t channel_function;           /**< 0: Fixed channel, 1:TR51CF, 2: Direct Hash, 3: Vendor defined */
+    uint8_t channel_spacing;            /**< derived from regulatory domain. 0:200k, 1:400k, 2:600k, 3:100k */
+    uint8_t number_of_channels;         /**< derived from regulatory domain */
+    uint8_t clock_drift;
+    uint8_t timing_accurancy;
+    uint16_t fixed_channel;
+    uint16_t fhss_bsi;
+    uint32_t fhss_broadcast_interval;
+    uint32_t channel_mask[8];
+    uint_fast24_t ch0_freq; // Default should be derived from regulatory domain
+} ws_hopping_schedule_t;
+
+/**
+ * @brief ws_utt_ie_t WS UTT-IE
+ */
+typedef struct ws_utt_ie {
+    uint8_t message_type;
+    uint_fast24_t ufsi;
+} ws_utt_ie_t;
+
+/**
+ * @brief ws_bt_ie_t WS BT-IE read
+ */
+typedef struct ws_bt_ie {
+    uint16_t broadcast_slot_number;
+    uint_fast24_t broadcast_interval_offset;
+} ws_bt_ie_t;
+
+
+/**
+ * @brief ws_channel_plan_zero_t WS channel plan 0 define domain and class
+ */
+typedef struct ws_channel_plan_zero {
+    uint8_t regulator_domain;
+    uint8_t operation_class;
+} ws_channel_plan_zero_t;
+
+/**
+ * @brief ws_channel_plan_one_t WS channel plan 1 define ch0, spasing and channel count
+ */
+typedef struct ws_channel_plan_one {
+    uint_fast24_t ch0;
+    unsigned channel_spacing:4;
+    uint16_t number_of_channel;
+} ws_channel_plan_one_t;
+
+/**
+ * @brief ws_channel_function_zero_t WS function 0 fixed channel
+ */
+typedef struct ws_channel_function_zero {
+    uint16_t fixed_channel;
+} ws_channel_function_zero_t;
+
+/**
+ * @brief ws_channel_function_three_t WS function 3 vendor spesific channel hop
+ */
+typedef struct ws_channel_function_three {
+    uint8_t channel_hop_count;
+    uint8_t *channel_list;
+} ws_channel_function_three_t;
+
+/**
+ * @brief ws_us_ie_t WS US-IE read
+ */
+typedef struct ws_us_ie {
+    uint8_t dwell_interval;
+    uint8_t clock_drift;
+    uint8_t timing_accurancy;
+    unsigned channel_plan:3;
+    unsigned channel_function:3;
+    unsigned excluded_channel_ctrl:2;
+    union {
+        ws_channel_plan_zero_t zero;
+        ws_channel_plan_one_t one;
+    } plan;
+    union {
+        ws_channel_function_zero_t zero;
+        ws_channel_function_three_t three;
+    } function;
+} ws_us_ie_t;
+
+/**
+ * @brief ws_bs_ie_t WS BS-IE read
+ */
+typedef struct ws_bs_ie {
+    uint32_t broadcast_interval;
+    uint16_t broadcast_schedule_identifier;
+    uint8_t dwell_interval;
+    uint8_t clock_drift;
+    uint8_t timing_accurancy;
+    unsigned channel_plan:3;
+    unsigned channel_function:3;
+    unsigned excluded_channel_ctrl:2;
+    union {
+        ws_channel_plan_zero_t zero;
+        ws_channel_plan_one_t one;
+    } plan;
+    union {
+        ws_channel_function_zero_t zero;
+        ws_channel_function_three_t three;
+    } function;
+} ws_bs_ie_t;
+
+
+#define MPX_KEY_MANAGEMENT_ENC_USER_ID 0x0001   /**< MPX Key management user ID */
+#define MPX_LOWPAN_ENC_USER_ID 0xA0ED           /**< MPX Lowpan User Id */
+
+#define WS_FAN_VERSION_1_0 1
+
+#define WS_NEIGHBOR_LINK_TIMEOUT 240
+#define WS_NEIGHBOR_NUD_TIMEOUT WS_NEIGHBOR_LINK_TIMEOUT / 2
+
+/*
+ * Threshold (referenced to DEVICE_MIN_SENS) above which a neighbor node may be considered for inclusion into candidate parent set
+ */
+#define CAND_PARENT_THRESHOLD 10
+/*
+ * Hysteresis factor to be applied to CAND_PARENT_THRESHOLD when admitting or dropping nodes from the candidate parent set.
+ */
+#define CAND_PARENT_HYSTERISIS 3
+
+/*
+ * value when send the first RPL DIS in 100ms ticks. Value is randomized between timeout/2 - timeout
+ */
+#define WS_RPL_DIS_INITIAL_TIMEOUT 600
+/*
+ * value when send subsequent RPL DIS in 100 ms tics. Value is randomized between timeout/2 - timeout
+ */
+#define WS_RPL_DIS_TIMEOUT 1800
+
+#endif /* WS_COMMON_DEFINES_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_neighbor_class.h b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_neighbor_class.h
new file mode 100644
index 0000000000..c05964f00b
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/6LoWPAN/ws/ws_neighbor_class.h
@@ -0,0 +1,164 @@
+/*
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef WS_NEIGHBOR_CLASS_H_
+#define WS_NEIGHBOR_CLASS_H_
+
+#include "fhss_ws_extension.h"
+#include "6LoWPAN/ws/ws_common_defines.h"
+
+typedef struct ws_neighbor_class_entry {
+    fhss_ws_neighbor_timing_info_t   fhss_data;
+    uint16_t rsl_in;                                       /*!< RSL EWMA heard from neighbour*/
+    uint16_t rsl_out;                                      /*!< RSL EWMA heard by neighbour*/
+    bool candidate_parent:1;
+} ws_neighbor_class_entry_t;
+
+/**
+ * Neighbor hopping info data base
+ */
+typedef struct ws_neighbor_class_s {
+    ws_neighbor_class_entry_t *neigh_info_list;           /*!< Allocated hopping info array*/
+    uint8_t list_size;                                    /*!< List size*/
+} ws_neighbor_class_t;
+
+/**
+ * ws_neighbor_class_alloc a function for allocate giving list size
+ *
+ * \param class_data pointer to structure which will be initialized by this function
+ * \param list_size define list size
+ *
+ * \return true Allocate Ok
+ * \return false Allocate Fail
+ *
+ */
+bool ws_neighbor_class_alloc(ws_neighbor_class_t *class_data, uint8_t list_size);
+
+/**
+ * ws_neighbor_class_dealloc a function for free allocated neighbor hopping info
+ *
+ * \param class_data pointer to structure which will be initialized by this function
+ *
+ */
+void ws_neighbor_class_dealloc(ws_neighbor_class_t *class_data);
+
+/**
+ * ws_neighbor_class_entry_t a function for search hopping info for giving neighbor attribut
+ *
+ * \param class_data pointer to structure which will be initialized by this function
+ * \param attribute_index define pointer to storage info
+ *
+ * \return NULL when Attribute is not correct
+ * \return Pointer to neighbor hopping info
+ *
+ */
+ws_neighbor_class_entry_t * ws_neighbor_class_entry_get(ws_neighbor_class_t *class_data, uint8_t attribute_index);
+
+/**
+ * ws_neighbor_class_entry_t a function for search hopping info for giving neighbor attribute index
+ *
+ * \param class_data pointer to structure which will be initialized by this function
+ * \param entry which attribute index is counted.
+ *
+ * \return Attribute index of entry
+ *
+ */
+uint8_t ws_neighbor_class_entry_index_get(ws_neighbor_class_t *class_data, ws_neighbor_class_entry_t *entry);
+
+/**
+ * ws_neighbor_class_entry_remove a function for clean information should be call when neighbor is removed
+ *
+ * \param class_data pointer to structure which will be initialized by this function
+ * \param attribute_index define pointer to storage info
+ *
+ */
+void ws_neighbor_class_entry_remove(ws_neighbor_class_t *class_data, uint8_t attribute_index);
+
+/**
+ * ws_neighbor_class_neighbor_unicast_time_info_update a function for update neighbor unicast time information
+ *
+ * \param ws_neighbor pointer to neighbor
+ * \param ws_utt Unicast time IE data
+ * \param timestamp timestamp for received data
+ *
+ */
+void ws_neighbor_class_neighbor_unicast_time_info_update(ws_neighbor_class_entry_t *ws_neighbor, ws_utt_ie_t *ws_utt, uint32_t timestamp);
+
+/**
+ * ws_neighbor_class_neighbor_unicast_schedule_set a function for update neighbor unicast shedule information
+ *
+ * \param ws_neighbor pointer to neighbor
+ * \param ws_us Unicast schedule IE data
+ *
+ */
+void ws_neighbor_class_neighbor_unicast_schedule_set(ws_neighbor_class_entry_t *ws_neighbor, ws_us_ie_t *ws_us);
+
+
+/**
+ * ws_neighbor_class_neighbor_broadcast_time_info_update a function for update neighbor broadcast time information
+ *
+ * \param ws_neighbor pointer to neighbor
+ * \param ws_bt_ie Broadcast time IE data
+ * \param timestamp timestamp for received data
+ *
+ */
+void ws_neighbor_class_neighbor_broadcast_time_info_update(ws_neighbor_class_entry_t *ws_neighbor, ws_bt_ie_t *ws_bt_ie, uint32_t timestamp);
+
+/**
+ * ws_neighbor_class_neighbor_broadcast_schedule_set a function for update neighbor broadcast shedule information
+ *
+ * \param ws_neighbor pointer to neighbor
+ * \param ws_bs_ie Broadcast schedule IE data
+ *
+ */
+void ws_neighbor_class_neighbor_broadcast_schedule_set(ws_neighbor_class_entry_t *ws_neighbor, ws_bs_ie_t *ws_bs_ie);
+
+/**
+ * ws_neighbor_class_rssi_from_dbm_calculate
+ *
+ * Calculates rssi value from dbm heard taking into account min sensitivity of radio
+ * dynamically adjusts min sensitivity if value is not properly set
+ *
+ * \param dbm_heard; dbm heard from the neighbour
+ *
+ */
+uint8_t ws_neighbor_class_rssi_from_dbm_calculate(int8_t dbm_heard);
+
+/** Helper macros to read RSL values from neighbour class.
+ *
+ */
+#define ws_neighbor_class_rsl_in_get(ws_neighbour) (ws_neighbour->rsl_in >> WS_RSL_SCALING)
+#define ws_neighbor_class_rsl_out_get(ws_neighbour) (ws_neighbour->rsl_in >> WS_RSL_SCALING)
+
+/**
+ * ws_neighbor_class_neighbor_broadcast_schedule_set a function for update neighbor broadcast shedule information
+ *
+ * \param ws_neighbor pointer to neighbor
+ * \param dbm_heard; dbm heard from the neighbour
+ *
+ */
+void ws_neighbor_class_rsl_in_calculate(ws_neighbor_class_entry_t *ws_neighbor, int8_t dbm_heard);
+/**
+ * ws_neighbor_class_neighbor_broadcast_schedule_set a function for update neighbor broadcast shedule information
+ *
+ * \param ws_neighbor pointer to neighbor
+ * \param rsl_reported; rsl value reported by neighbour in packet from RSL-IE
+ *
+ */
+void ws_neighbor_class_rsl_out_calculate(ws_neighbor_class_entry_t *ws_neighbor, uint8_t rsl_reported);
+
+#endif /* WS_NEIGHBOR_CLASS_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/BorderRouter/border_router.c b/features/nanostack/sal-stack-nanostack/source/BorderRouter/border_router.c
index e090e6ef57..08d7b13a05 100644
--- a/features/nanostack/sal-stack-nanostack/source/BorderRouter/border_router.c
+++ b/features/nanostack/sal-stack-nanostack/source/BorderRouter/border_router.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012-2017, Arm Limited and affiliates.
+ * Copyright (c) 2012-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -506,9 +506,8 @@ void border_router_start(protocol_interface_info_entry_t *cur, bool warm_link_re
     if (warm_link_restart) {
         return;
     }
-
+    mac_neighbor_table_neighbor_list_clean(mac_neighbor_info(cur));
 #ifndef NO_MLE
-    mle_class_list_clean(cur->id);
     blacklist_clear();
 #endif
 
@@ -708,8 +707,8 @@ static int8_t arm_border_router_interface_down(protocol_interface_info_entry_t *
         cur->nwk_wpan_nvm_api->nvm_params_update_cb(cur->nwk_wpan_nvm_api, true);
     }
     cur->if_lowpan_security_params->mle_security_frame_counter = mle_service_security_get_frame_counter(cur->id);
+    mac_neighbor_table_neighbor_list_clean(mac_neighbor_info(cur));
 #ifndef NO_MLE
-    mle_class_list_clean(cur->id);
     blacklist_clear();
 #endif
     if (nd_nwk) {
diff --git a/features/nanostack/sal-stack-nanostack/source/Common_Protocols/icmpv6.c b/features/nanostack/sal-stack-nanostack/source/Common_Protocols/icmpv6.c
index 6c4240b65d..647cb200cb 100644
--- a/features/nanostack/sal-stack-nanostack/source/Common_Protocols/icmpv6.c
+++ b/features/nanostack/sal-stack-nanostack/source/Common_Protocols/icmpv6.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2017, Arm Limited and affiliates.
+ * Copyright (c) 2013-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -20,8 +20,8 @@
 #include "ns_trace.h"
 #include "randLIB.h"
 #include "NWK_INTERFACE/Include/protocol.h"
-#ifdef HAVE_RPL
 #include "RPL/rpl_control.h"
+#ifdef HAVE_RPL
 #include "RPL/rpl_data.h"
 #endif
 #include "RPL/rpl_protocol.h"
@@ -44,6 +44,8 @@
 #include "common_functions.h"
 #include "6LoWPAN/ND/nd_router_object.h"
 #include "6LoWPAN/Bootstraps/protocol_6lowpan.h"
+#include "6LoWPAN/ws/ws_common_defines.h"
+#include "6LoWPAN/ws/ws_common.h"
 
 #define TRACE_GROUP "icmp"
 
@@ -420,6 +422,7 @@ static buffer_t *icmpv6_ns_handler(buffer_t *buf)
 {
     protocol_interface_info_entry_t *cur;
     uint8_t target[16];
+    uint8_t dummy_sllao[16];
     bool proxy = false;
     const uint8_t *sllao;
     const uint8_t *aro;
@@ -439,7 +442,9 @@ static buffer_t *icmpv6_ns_handler(buffer_t *buf)
     sllao = icmpv6_find_option_in_buffer(buf, 20, ICMPV6_OPT_SRC_LL_ADDR, 0);
 
     /* If no SLLAO, ignore ARO (RFC 6775 6.5) */
-    if (sllao && cur->ipv6_neighbour_cache.recv_addr_reg) {
+    /* This rule can be bypassed by setting flag "use_eui64_as_slla_in_aro" to true */
+    if (cur->ipv6_neighbour_cache.recv_addr_reg &&
+            (cur->ipv6_neighbour_cache.use_eui64_as_slla_in_aro || sllao)) {
         aro = icmpv6_find_option_in_buffer(buf, 20, ICMPV6_OPT_ADDR_REGISTRATION, 0);
     } else {
         aro = NULL;
@@ -450,6 +455,15 @@ static buffer_t *icmpv6_ns_handler(buffer_t *buf)
         goto drop;
     }
 
+    /* If there was no SLLAO on ARO, use mac address to create dummy one... */
+    if (aro && !sllao && cur->ipv6_neighbour_cache.use_eui64_as_slla_in_aro) {
+        dummy_sllao[0] = ICMPV6_OPT_SRC_LL_ADDR;    // Type
+        dummy_sllao[1] = 2;                         // Length = 2x8 bytes
+        memcpy(dummy_sllao + 2, aro + 8, 8);        // EUI-64
+        memset(dummy_sllao + 10, 0, 6);             // Padding
+
+        sllao = dummy_sllao;
+    }
     // Skip the 4 reserved bytes
     dptr += 4;
 
@@ -529,7 +543,7 @@ drop:
 
 }
 
-int icmpv6_slaac_prefix_update(struct protocol_interface_info_entry *cur, uint8_t *prefix_ptr, uint8_t prefix_len, uint32_t valid_lifetime, uint32_t preferred_lifetime)
+int icmpv6_slaac_prefix_update(struct protocol_interface_info_entry *cur, const uint8_t *prefix_ptr, uint8_t prefix_len, uint32_t valid_lifetime, uint32_t preferred_lifetime)
 {
     int ret_val = -1;
 
@@ -564,7 +578,7 @@ void icmpv6_slaac_prefix_register_trig(struct protocol_interface_info_entry *cur
 }
 #endif // HAVE_IPV6_ND
 
-if_address_entry_t *icmpv6_slaac_address_add(protocol_interface_info_entry_t *cur, uint8_t *prefix_ptr, uint8_t prefix_len, uint32_t valid_lifetime, uint32_t preferred_lifetime, bool skip_dad, slaac_src_e slaac_src)
+if_address_entry_t *icmpv6_slaac_address_add(protocol_interface_info_entry_t *cur, const uint8_t *prefix_ptr, uint8_t prefix_len, uint32_t valid_lifetime, uint32_t preferred_lifetime, bool skip_dad, slaac_src_e slaac_src)
 {
     if_address_entry_t *address_entry;
     uint8_t ipv6_address[16];
@@ -774,7 +788,7 @@ static buffer_t *icmpv6_ra_handler(buffer_t *buf)
             ptr += 4;
             uint32_t preferred_lifetime = common_read_32_bit(ptr);
             ptr += 8; //Update 32-bit time and reserved 32-bit
-            uint8_t *prefix_ptr = ptr;
+            const uint8_t *prefix_ptr = ptr;
 
             //Check is L Flag active
             if (prefix_flags & PIO_L) {
@@ -990,6 +1004,10 @@ static buffer_t *icmpv6_na_handler(buffer_t *buf)
     }
 
     ipv6_neighbour_update_from_na(&cur->ipv6_neighbour_cache, neighbour_entry, flags, buf->dst_sa.addr_type, buf->dst_sa.address);
+    if (ws_info(cur) && neighbour_entry->state == IP_NEIGHBOUR_REACHABLE) {
+        tr_debug("NA neigh update");
+        ws_common_neighbor_update(cur, target);
+    }
 
 drop:
     return buffer_free(buf);
@@ -1323,7 +1341,7 @@ buffer_t *icmpv6_build_ns(protocol_interface_info_entry_t *cur, const uint8_t ta
     } else {
         /* RFC 4861 7.2.2. says we should use the source of traffic prompting the NS, if possible */
         /* This is also used to specify the address for ARO messages */
-        if (prompting_src_addr && addr_is_assigned_to_interface(cur, prompting_src_addr)) {
+        if (aro || (prompting_src_addr && addr_is_assigned_to_interface(cur, prompting_src_addr))) {
             memcpy(buf->src_sa.address, prompting_src_addr, 16);
         } else {
             /* Otherwise, according to RFC 4861, we could use any address.
@@ -1347,7 +1365,15 @@ buffer_t *icmpv6_build_ns(protocol_interface_info_entry_t *cur, const uint8_t ta
             }
         }
         /* SLLAO is required if we're sending an ARO */
-        ptr = icmpv6_write_icmp_lla(cur, ptr, ICMPV6_OPT_SRC_LL_ADDR, aro, buf->src_sa.address);
+        /* This rule can be bypassed with flag use_eui64_as_slla_in_aro */
+        if (!cur->ipv6_neighbour_cache.use_eui64_as_slla_in_aro) {
+            ptr = icmpv6_write_icmp_lla(cur, ptr, ICMPV6_OPT_SRC_LL_ADDR, aro, buf->src_sa.address);
+        }
+        /* If ARO Success sending is omitted, MAC ACK is used instead */
+        /* Setting callback for receiving ACK from adaptation layer */
+        if (aro && cur->ipv6_neighbour_cache.omit_aro_success) {
+            buf->ack_receive_cb = rpl_control_address_register_done;
+        }
     }
     buf->src_sa.addr_type = ADDR_IPV6;
 
@@ -1481,6 +1507,12 @@ buffer_t *icmpv6_build_na(protocol_interface_info_entry_t *cur, bool solicited,
 
     tr_debug("Build NA");
 
+    /* Check if ARO status == success, then sending can be omitted with flag */
+    if (aro && cur->ipv6_neighbour_cache.omit_aro_success && aro->status == ARO_SUCCESS) {
+        tr_debug("Omit success reply");
+        return NULL;
+    }
+
     buffer_t *buf = buffer_get(8 + 16 + 16 + 16); /* fixed, target addr, target ll addr, aro */
     if (!buf) {
         return NULL;
diff --git a/features/nanostack/sal-stack-nanostack/source/Common_Protocols/icmpv6.h b/features/nanostack/sal-stack-nanostack/source/Common_Protocols/icmpv6.h
index 7dfd83231a..c209e2af42 100644
--- a/features/nanostack/sal-stack-nanostack/source/Common_Protocols/icmpv6.h
+++ b/features/nanostack/sal-stack-nanostack/source/Common_Protocols/icmpv6.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2017, Arm Limited and affiliates.
+ * Copyright (c) 2013-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -140,8 +140,8 @@ extern void icmpv6_recv_ra_routes(struct protocol_interface_info_entry *cur, boo
 extern void icmpv6_recv_ra_prefixes(struct protocol_interface_info_entry *cur, bool enable);
 
 extern void icmpv6_slaac_prefix_register_trig(struct protocol_interface_info_entry *cur, uint8_t *prefix_ptr, uint8_t prefix_len);
-extern int icmpv6_slaac_prefix_update(struct protocol_interface_info_entry *cur, uint8_t *prefix_ptr, uint8_t prefix_len, uint32_t valid_lifetime, uint32_t preferred_lifetime);
-extern struct if_address_entry *icmpv6_slaac_address_add(struct protocol_interface_info_entry *cur, uint8_t *prefix_ptr, uint8_t prefix_len, uint32_t valid_lifetime, uint32_t preferred_lifetime, bool skip_dad, slaac_src_e slaac_src);
+extern int icmpv6_slaac_prefix_update(struct protocol_interface_info_entry *cur, const uint8_t *prefix_ptr, uint8_t prefix_len, uint32_t valid_lifetime, uint32_t preferred_lifetime);
+extern struct if_address_entry *icmpv6_slaac_address_add(struct protocol_interface_info_entry *cur, const uint8_t *prefix_ptr, uint8_t prefix_len, uint32_t valid_lifetime, uint32_t preferred_lifetime, bool skip_dad, slaac_src_e slaac_src);
 
 
 /*
diff --git a/features/nanostack/sal-stack-nanostack/source/Common_Protocols/ipv6.c b/features/nanostack/sal-stack-nanostack/source/Common_Protocols/ipv6.c
index bf19d1a568..af3ca814f4 100644
--- a/features/nanostack/sal-stack-nanostack/source/Common_Protocols/ipv6.c
+++ b/features/nanostack/sal-stack-nanostack/source/Common_Protocols/ipv6.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2017, Arm Limited and affiliates.
+ * Copyright (c) 2013-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/features/nanostack/sal-stack-nanostack/source/Common_Protocols/ipv6_fragmentation.c b/features/nanostack/sal-stack-nanostack/source/Common_Protocols/ipv6_fragmentation.c
index 13169a0c19..0c770e26eb 100644
--- a/features/nanostack/sal-stack-nanostack/source/Common_Protocols/ipv6_fragmentation.c
+++ b/features/nanostack/sal-stack-nanostack/source/Common_Protocols/ipv6_fragmentation.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/features/nanostack/sal-stack-nanostack/source/Common_Protocols/tcp.c b/features/nanostack/sal-stack-nanostack/source/Common_Protocols/tcp.c
index 5fb61554ed..526196eb8b 100644
--- a/features/nanostack/sal-stack-nanostack/source/Common_Protocols/tcp.c
+++ b/features/nanostack/sal-stack-nanostack/source/Common_Protocols/tcp.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2017, Arm Limited and affiliates.
+ * Copyright (c) 2013-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/features/nanostack/sal-stack-nanostack/source/Core/address.c b/features/nanostack/sal-stack-nanostack/source/Core/address.c
index 31b4434e75..5bf5bd4ed0 100644
--- a/features/nanostack/sal-stack-nanostack/source/Core/address.c
+++ b/features/nanostack/sal-stack-nanostack/source/Core/address.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2008, 2010-2017, Arm Limited and affiliates.
+ * Copyright (c) 2008, 2010-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/features/nanostack/sal-stack-nanostack/source/Core/buffer_dyn.c b/features/nanostack/sal-stack-nanostack/source/Core/buffer_dyn.c
index 1466853689..b83d7164e0 100644
--- a/features/nanostack/sal-stack-nanostack/source/Core/buffer_dyn.c
+++ b/features/nanostack/sal-stack-nanostack/source/Core/buffer_dyn.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2011-2017, Arm Limited and affiliates.
+ * Copyright (c) 2011-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -110,7 +110,7 @@ buffer_t *buffer_get_specific(uint16_t headroom, uint16_t size, uint16_t minspac
 #endif
         buf->size = total_size;
     } else {
-        tr_error("buffer_get failed: alloc(%zd)", sizeof(buffer_t) + total_size);
+        tr_error("buffer_get failed: alloc(%d)", (int) sizeof(buffer_t) + total_size);
     }
 
     protocol_stats_update(STATS_BUFFER_ALLOC, 1);
diff --git a/features/nanostack/sal-stack-nanostack/source/Core/include/address.h b/features/nanostack/sal-stack-nanostack/source/Core/include/address.h
index 9f3c6b24d8..09f7e6a8cf 100644
--- a/features/nanostack/sal-stack-nanostack/source/Core/include/address.h
+++ b/features/nanostack/sal-stack-nanostack/source/Core/include/address.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2008, 2010-2017, Arm Limited and affiliates.
+ * Copyright (c) 2008, 2010-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -96,6 +96,8 @@ typedef struct if_address_entry {
     bool temporary:1;           // RFC 4941 temporary address
     bool tentative:1;           // Tentative address (Duplicate Address Detection running)
     bool group_added:1;         // Solicited-Node group added
+    uint8_t addr_reg_pend;      // Bitmask for pending address registrations. Based on RPL path control bits
+    uint8_t addr_reg_done;      // Bitmask for address registration done. Based on RPL path control bits
     if_address_source_t source; //
     if_address_callback_fn *cb; // Address protocol callback function
     void *data;                 // Address protocol data
diff --git a/features/nanostack/sal-stack-nanostack/source/Core/include/ns_buffer.h b/features/nanostack/sal-stack-nanostack/source/Core/include/ns_buffer.h
index 60e5f22bff..b89ab3f777 100644
--- a/features/nanostack/sal-stack-nanostack/source/Core/include/ns_buffer.h
+++ b/features/nanostack/sal-stack-nanostack/source/Core/include/ns_buffer.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2008-2017, Arm Limited and affiliates.
+ * Copyright (c) 2008-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -228,6 +228,7 @@ typedef struct buffer {
     uint8_t             trickle_data_field[4];
     buffer_options_t    options;                /*!< Additional signal info etc */
     buffer_routing_info_t *route;               /* A pointer last to try to get neat alignment for data */
+    void (*ack_receive_cb)(struct buffer *buffer_ptr, uint8_t status); /*!< ACK receive callback. If set, will be called when TX is done */
     uint8_t             buf[];                  /*!< Trailing buffer data */
  } buffer_t;
 
diff --git a/features/nanostack/sal-stack-nanostack/source/Core/ns_socket.c b/features/nanostack/sal-stack-nanostack/source/Core/ns_socket.c
index 01f7fe2360..ad9135ca90 100644
--- a/features/nanostack/sal-stack-nanostack/source/Core/ns_socket.c
+++ b/features/nanostack/sal-stack-nanostack/source/Core/ns_socket.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2008-2015, 2017, Arm Limited and affiliates.
+ * Copyright (c) 2008-2015, 2017-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -1396,6 +1396,10 @@ buffer_t *socket_tx_buffer_event(buffer_t *buf, uint8_t status)
      * and we mapped straight to MAC address).
      */
 
+    if (buf->ack_receive_cb) {
+        buf->ack_receive_cb(buf, status);
+    }
+
     /* Suppress events once socket orphaned */
     if (!buf->socket || (buf->socket->flags & (SOCKET_FLAG_PENDING|SOCKET_FLAG_CLOSED))) {
         return buf;
diff --git a/features/nanostack/sal-stack-nanostack/source/DHCPv6_Server/DHCPv6_Server_service.c b/features/nanostack/sal-stack-nanostack/source/DHCPv6_Server/DHCPv6_Server_service.c
index e33f3e361e..26b7b3a796 100644
--- a/features/nanostack/sal-stack-nanostack/source/DHCPv6_Server/DHCPv6_Server_service.c
+++ b/features/nanostack/sal-stack-nanostack/source/DHCPv6_Server/DHCPv6_Server_service.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -106,7 +106,7 @@ 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) {
+        if (thread_bbr_nd_entry_add(serverBase->interfaceId,dhcp_allocated_address->nonTemporalAddress, nonTemporalAddress.validLifeTime, serverBase->guaPrefix) == -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) {
@@ -114,7 +114,6 @@ int DHCPv6_server_respond_client(dhcpv6_gua_server_entry_s *serverBase, dhcpv6_r
                 libdhcpv6_address_rm_from_allocated_list(serverBase,dhcp_allocated_address->nonTemporalAddress);
             }
 
-
         }
     }
 
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_defines.h b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_defines.h
index cd7a6fa20d..9315d36e95 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_defines.h
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_defines.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -139,6 +139,9 @@ typedef struct protocol_interface_rf_mac_setup {
     bool macUpState;
     bool shortAdressValid: 1;   //Define Dynamic src address to mac16 when it is true
     bool beaconSrcAddressModeLong: 1; //This force beacon src to mac64 otherwise shortAdressValid will define type
+    bool mac_extension_enabled:1;
+    bool mac_ack_tx_active:1;
+    bool mac_frame_pending:1;
     uint16_t mac_short_address;
     uint16_t pan_id;
     uint8_t mac64[8];
@@ -154,6 +157,8 @@ typedef struct protocol_interface_rf_mac_setup {
     bool macProminousMode:1;
     bool macGTSPermit:1;
     bool mac_security_enabled:1;
+    /* Let trough packet which is secured properly (MIC authenticated group key)  and src address is 64-bit*/
+    bool mac_security_bypass_unknow_device:1;
     /* Load balancing need this feature */
     bool macAcceptAnyBeacon:1;
 
@@ -169,6 +174,7 @@ typedef struct protocol_interface_rf_mac_setup {
     bool macRfRadioTxActive:1;
     bool macBroadcastDisabled:1;
     bool scan_active:1;
+    bool rf_csma_extension_supported:1;
     /* CSMA Params */
     unsigned macMinBE:4;
     unsigned macMaxBE:4;
@@ -201,6 +207,7 @@ typedef struct protocol_interface_rf_mac_setup {
     uint8_t mac_cca_retry;
     uint8_t mac_ack_wait_duration;
     uint8_t mac_mlme_retry_max;
+    uint8_t aUnitBackoffPeriod;
     /* Indirect queue parameters */
     struct mac_pre_build_frame *indirect_pd_data_request_queue;
     arm_event_t mac_mcps_timer_event;
@@ -217,6 +224,8 @@ typedef struct protocol_interface_rf_mac_setup {
     int8_t cca_timer_id;
     int8_t bc_timer_id;
     uint32_t mlme_tick_count;
+    uint32_t symbol_rate;
+    uint32_t symbol_time_us;
     uint8_t max_ED;
     uint16_t mlme_ED_counter;
     mac_tx_status_t mac_tx_status;
@@ -250,22 +259,26 @@ typedef struct protocol_interface_rf_mac_setup {
 } protocol_interface_rf_mac_setup_s;
 
 
-#define MAC_FCF_FRAME_TYPE_MASK         0x0007
-#define MAC_FCF_FRAME_TYPE_SHIFT        0
-#define MAC_FCF_SECURITY_BIT_MASK       0x0008
-#define MAC_FCF_SECURITY_BIT_SHIFT      3
-#define MAC_FCF_PENDING_BIT_MASK        0x0010
-#define MAC_FCF_PENDING_BIT_SHIFT       4
-#define MAC_FCF_ACK_REQ_BIT_MASK        0x0020
-#define MAC_FCF_ACK_REQ_BIT_SHIFT       5
-#define MAC_FCF_INTRA_PANID_MASK        0x0040
-#define MAC_FCF_INTRA_PANID_SHIFT       6
-#define MAC_FCF_DST_ADDR_MASK           0x0c00
-#define MAC_FCF_DST_ADDR_SHIFT          10
-#define MAC_FCF_VERSION_MASK            0x3000
-#define MAC_FCF_VERSION_SHIFT           12
-#define MAC_FCF_SRC_ADDR_MASK           0xc000
-#define MAC_FCF_SRC_ADDR_SHIFT          14
+#define MAC_FCF_FRAME_TYPE_MASK             0x0007
+#define MAC_FCF_FRAME_TYPE_SHIFT            0
+#define MAC_FCF_SECURITY_BIT_MASK           0x0008
+#define MAC_FCF_SECURITY_BIT_SHIFT          3
+#define MAC_FCF_PENDING_BIT_MASK            0x0010
+#define MAC_FCF_PENDING_BIT_SHIFT           4
+#define MAC_FCF_ACK_REQ_BIT_MASK            0x0020
+#define MAC_FCF_ACK_REQ_BIT_SHIFT           5
+#define MAC_FCF_INTRA_PANID_MASK            0x0040
+#define MAC_FCF_INTRA_PANID_SHIFT           6
+#define MAC_FCF_SEQ_NUM_SUPPRESS_MASK       0x0100
+#define MAC_FCF_SEQ_NUM_SUPPRESS_SHIFT      8
+#define MAC_FCF_IE_PRESENTS_MASK            0x0200
+#define MAC_FCF_IE_PRESENTS_SHIFT           9
+#define MAC_FCF_DST_ADDR_MASK               0x0c00
+#define MAC_FCF_DST_ADDR_SHIFT              10
+#define MAC_FCF_VERSION_MASK                0x3000
+#define MAC_FCF_VERSION_SHIFT               12
+#define MAC_FCF_SRC_ADDR_MASK               0xc000
+#define MAC_FCF_SRC_ADDR_SHIFT              14
 
 /* MAC supported frame types */
 #define FC_BEACON_FRAME         0x00
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_fhss_callbacks.c b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_fhss_callbacks.c
index 24e8d7eadd..d326aaa078 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_fhss_callbacks.c
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_fhss_callbacks.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -57,7 +57,12 @@ uint32_t mac_read_phy_datarate(const fhss_api_t *fhss_api)
     if (!mac_setup) {
         return 0;
     }
-    return dev_get_phy_datarate(mac_setup->dev_driver->phy_driver, mac_setup->mac_channel_list.channel_page);
+    uint32_t datarate = dev_get_phy_datarate(mac_setup->dev_driver->phy_driver, mac_setup->mac_channel_list.channel_page);
+    // If datarate is not set, use default 250kbit/s.
+    if (!datarate) {
+        datarate = 250000;
+    }
+    return datarate;
 }
 
 int mac_set_channel(const fhss_api_t *fhss_api, uint8_t channel_number)
@@ -66,9 +71,8 @@ int mac_set_channel(const fhss_api_t *fhss_api, uint8_t channel_number)
     if (!mac_setup) {
         return -1;
     }
-    // If TX is active, send internal CCA fail event. MAC state machine would crash without TX done event.
-    if (mac_setup->macRfRadioTxActive == true) {
-        mac_setup->dev_driver->phy_driver->phy_tx_done_cb(mac_setup->dev_driver->id, 1, PHY_LINK_TX_FAIL, 0, 0);
+    if (mac_setup->mac_ack_tx_active || (mac_setup->active_pd_data_request && mac_setup->active_pd_data_request->asynch_request)) {
+        return -1;
     }
     return mac_mlme_rf_channel_change(mac_setup, channel_number);
 }
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_filter.c b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_filter.c
index 6bcaee9d59..5701a94997 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_filter.c
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_filter.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -26,6 +26,7 @@
 #include "mac_filter.h"
 #include "mac_common_defines.h"
 
+#include "mac_mcps.h"
 #include "MAC/IEEE802_15_4/mac_mcps_sap.h"
 #include "MAC/IEEE802_15_4/mac_header_helper_functions.h"
 
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_header_helper_functions.c b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_header_helper_functions.c
index e5ed44e63e..2867513170 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_header_helper_functions.c
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_header_helper_functions.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -14,16 +14,139 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
+#include "nsconfig.h"
 #include "ns_types.h"
 #include "string.h"
+#include "ns_trace.h"
 #include "mlme.h"
 #include "mac_api.h"
+#include "fhss_api.h"
 #include "common_functions.h"
 #include "mac_common_defines.h"
 #include "MAC/IEEE802_15_4/mac_defines.h"
 #include "MAC/IEEE802_15_4/mac_mcps_sap.h"
 #include "MAC/IEEE802_15_4/mac_header_helper_functions.h"
 
+static uint8_t *mcps_mac_security_aux_header_start_pointer_get(const mac_pre_parsed_frame_t *buffer);
+static uint8_t * mac_header_information_elements_write(const mac_pre_build_frame_t *buffer, uint8_t *ptr);
+
+
+static uint8_t mac_fcf_lenght(const mac_fcf_sequence_t *header)
+{
+    uint8_t length;
+    if (header->frameVersion == MAC_FRAME_VERSION_2015) {
+        if (header->sequenceNumberSuppress) {
+            length = 2; //Skip FCF
+        } else {
+            length = 3; //Skip FCF + DSN
+        }
+    } else {
+        length= 3; //Skip FCF + DSN
+    }
+    return length;
+}
+
+bool mac_dst_panid_present(const mac_fcf_sequence_t *header)
+{
+    bool presents = false;
+    if (header->DstAddrMode && header->SrcAddrMode) {
+        if (header->frameVersion == MAC_FRAME_VERSION_2015) {
+            if (header->DstAddrMode == MAC_ADDR_MODE_64_BIT && header->SrcAddrMode == MAC_ADDR_MODE_64_BIT &&  header->intraPan) {
+
+            } else {
+                presents = true;
+            }
+        } else {
+            presents = true;
+        }
+
+    } else if (header->DstAddrMode && !header->SrcAddrMode) {
+        if (header->frameVersion == MAC_FRAME_VERSION_2015) {
+            if (!header->intraPan) {
+                presents = true;
+            }
+        } else {
+            presents = true;
+        }
+
+    } else if (!header->DstAddrMode && !header->SrcAddrMode) {
+        if (header->frameVersion == MAC_FRAME_VERSION_2015) {
+            if (header->intraPan) {
+                presents = true;
+            }
+        }
+    }
+
+    return presents;
+}
+
+bool mac_src_panid_present(const mac_fcf_sequence_t *header)
+{
+    bool presents = false;
+    if (header->DstAddrMode && header->SrcAddrMode) {
+        if (header->frameVersion == MAC_FRAME_VERSION_2015) {
+            if (header->DstAddrMode == MAC_ADDR_MODE_64_BIT && header->SrcAddrMode == MAC_ADDR_MODE_64_BIT) {
+
+            } else if (!header->intraPan) {
+                presents = true;
+            }
+        } else if (!header->intraPan) {
+            presents = true;
+        }
+
+    } else if (header->SrcAddrMode) {
+        if (header->frameVersion == MAC_FRAME_VERSION_2015) {
+            if (!header->intraPan) {
+                presents = true;
+            }
+        } else {
+            presents = true;
+        }
+    }
+    return presents;
+}
+
+uint8_t mac_address_length(uint8_t address_mode)
+{
+    uint8_t length = 0;
+    switch (address_mode) {
+            case MAC_ADDR_MODE_NONE:
+                break;
+            case MAC_ADDR_MODE_16_BIT:
+                length = 2;
+                break;
+            case MAC_ADDR_MODE_64_BIT:
+                length = 8;
+                break;
+        }
+    return length;
+}
+
+static uint8_t mac_dst_address_length_with_panid(const mac_fcf_sequence_t *header)
+{
+    uint8_t length = 0;
+
+    if (header->DstPanPresents) {
+        length += 2;
+    }
+
+    length += mac_address_length(header->DstAddrMode);
+
+    return length;
+}
+
+static uint8_t mac_src_address_length_with_panid(const mac_fcf_sequence_t *header)
+{
+    uint8_t length = 0;
+
+    if (header->SrcPanPresents) {
+        length += 2;
+    }
+
+    length += mac_address_length(header->SrcAddrMode);
+
+    return length;
+}
 
 
 uint8_t mac_security_mic_length_get(uint8_t security_level)
@@ -75,48 +198,16 @@ uint8_t mac_header_security_aux_header_length(uint8_t security_level, uint8_t ke
 uint8_t mac_header_address_length(const mac_fcf_sequence_t *fcf)
 {
     uint8_t address_length = 0;
-    if( !fcf ){
-        return address_length;
-    }
 
-    if(fcf->DstAddrMode && fcf->SrcAddrMode) {
-        if (fcf->DstAddrMode == MAC_ADDR_MODE_16_BIT) {
-            address_length = 4;
-        } else {
-            address_length = 10;
-        }
+    address_length += mac_dst_address_length_with_panid(fcf);
+    address_length += mac_src_address_length_with_panid(fcf);
 
-        if (fcf->SrcAddrMode == MAC_ADDR_MODE_16_BIT) {
-            address_length += 2;
-        } else {
-            address_length += 8;
-        }
-
-        if (!fcf->intraPan) {
-            address_length += 2;
-        }
-    } else if (fcf->DstAddrMode) {
-        if (fcf->DstAddrMode == MAC_ADDR_MODE_16_BIT) {
-            address_length = 4;
-        } else {
-            address_length = 10;
-        }
-    } else if (fcf->SrcAddrMode){
-        if (fcf->SrcAddrMode == MAC_ADDR_MODE_16_BIT) {
-            address_length = 4;
-        } else {
-            address_length = 10;
-        }
-    }
     return address_length;
 
 }
 
 void mac_header_security_parameter_set(mac_aux_security_header_t *header, const mlme_security_t *frame_setup)
 {
-    if( !header || !frame_setup ){
-        return;
-    }
     header->securityLevel = frame_setup->SecurityLevel;
 
     if (header->securityLevel) {
@@ -145,14 +236,10 @@ void mac_header_security_parameter_set(mac_aux_security_header_t *header, const
     }
 }
 
-void mac_header_parse_fcf_dsn(mac_fcf_sequence_t *header, const uint8_t *ptr)
+const uint8_t * mac_header_parse_fcf_dsn(mac_fcf_sequence_t *header, const uint8_t *ptr)
 {
-    if( !header || !ptr ){
-        return;
-    }
     uint16_t fcf = common_read_16_bit_inverse(ptr);
     ptr += 2;
-    header->DSN = *ptr;
 
     //Read Frame Type
     header->frametype = ((fcf & MAC_FCF_FRAME_TYPE_MASK) >> MAC_FCF_FRAME_TYPE_SHIFT);
@@ -160,9 +247,27 @@ void mac_header_parse_fcf_dsn(mac_fcf_sequence_t *header, const uint8_t *ptr)
     header->framePending = ((fcf & MAC_FCF_PENDING_BIT_MASK) >> MAC_FCF_PENDING_BIT_SHIFT);
     header->ackRequested = ((fcf & MAC_FCF_ACK_REQ_BIT_MASK) >> MAC_FCF_ACK_REQ_BIT_SHIFT);
     header->intraPan = ((fcf & MAC_FCF_INTRA_PANID_MASK ) >> MAC_FCF_INTRA_PANID_SHIFT);
+
     header->DstAddrMode = ((fcf & MAC_FCF_DST_ADDR_MASK ) >> MAC_FCF_DST_ADDR_SHIFT);
     header->frameVersion = ((fcf & MAC_FCF_VERSION_MASK) >> MAC_FCF_VERSION_SHIFT);
     header->SrcAddrMode = ((fcf & MAC_FCF_SRC_ADDR_MASK ) >> MAC_FCF_SRC_ADDR_SHIFT);
+
+    if (header->frameVersion == MAC_FRAME_VERSION_2015 ) {
+        header->sequenceNumberSuppress = ((fcf & MAC_FCF_SEQ_NUM_SUPPRESS_MASK ) >> MAC_FCF_SEQ_NUM_SUPPRESS_SHIFT);
+        header->informationElementsPresets = ((fcf & MAC_FCF_IE_PRESENTS_MASK ) >> MAC_FCF_IE_PRESENTS_SHIFT);
+    } else {
+        //SET False to ALL 2015 Extension's by default
+        header->sequenceNumberSuppress = false;
+        header->informationElementsPresets = false;
+    }
+
+    if (header->frameVersion < MAC_FRAME_VERSION_2015 || (header->frameVersion ==  MAC_FRAME_VERSION_2015 &&  !header->sequenceNumberSuppress)) {
+        header->DSN = *ptr++;
+    } else {
+        header->DSN = 0;
+    }
+    return ptr;
+
 }
 
 static uint8_t * mac_header_write_fcf_dsn(const mac_fcf_sequence_t *header, uint8_t *ptr)
@@ -174,29 +279,36 @@ static uint8_t * mac_header_write_fcf_dsn(const mac_fcf_sequence_t *header, uint
     fcf |= (header->framePending << MAC_FCF_PENDING_BIT_SHIFT);
     fcf |= (header->ackRequested << MAC_FCF_ACK_REQ_BIT_SHIFT);
     fcf |= (header->intraPan << MAC_FCF_INTRA_PANID_SHIFT);
+    fcf |= (header->sequenceNumberSuppress << MAC_FCF_SEQ_NUM_SUPPRESS_SHIFT);
+    fcf |= (header->informationElementsPresets << MAC_FCF_IE_PRESENTS_SHIFT);
     fcf |= (header->DstAddrMode << MAC_FCF_DST_ADDR_SHIFT);
     fcf |= (header->frameVersion << MAC_FCF_VERSION_SHIFT);
     fcf |= (header->SrcAddrMode << MAC_FCF_SRC_ADDR_SHIFT);
     ptr = common_write_16_bit_inverse(fcf,ptr);
-    *ptr++ = header->DSN;
+    if (header->frameVersion < MAC_FRAME_VERSION_2015 || (header->frameVersion ==  MAC_FRAME_VERSION_2015 &&  !header->sequenceNumberSuppress)) {
+        *ptr++ = header->DSN;
+    }
     return ptr;
 }
 
-void mac_header_security_components_read(mac_pre_parsed_frame_t *buffer, mlme_security_t *security_params)
+uint16_t mac_header_off_set_to_aux_header(const mac_fcf_sequence_t *fcf)
 {
-    if( !buffer || !security_params ){
-        return;
-    }
-    uint8_t *ptr = mcps_mac_security_aux_header_start_pointer_get(buffer);
-    memset(security_params, 0, sizeof(mlme_security_t));
-    uint8_t key_source_len = 0;
-    if (!buffer->fcf_dsn.securityEnabled) {
-        return;
+    //Skip first FCF & address field
+    uint16_t offset = mac_fcf_lenght(fcf);//Skip FCF + DSN
+    offset += mac_dst_address_length_with_panid(fcf);
+    offset += mac_address_length(fcf->SrcAddrMode);
+    if (fcf->SrcPanPresents) {
+        offset += 2; //Skip PanId
     }
+    return offset;
+}
 
+void mac_header_security_aux_header_parse(const uint8_t *ptr, mlme_security_t *security_params)
+{
+    uint8_t key_source_len = 0;
     security_params->KeyIdMode = (*ptr >> 3);
     security_params->SecurityLevel = *ptr++;
-    ptr += 4;
+    ptr += 4; //Skip Frame counter
     switch (security_params->KeyIdMode) {
         case MAC_KEY_ID_MODE_IMPLICIT:
             break;
@@ -214,84 +326,65 @@ void mac_header_security_components_read(mac_pre_parsed_frame_t *buffer, mlme_se
     }
 }
 
+void mac_header_security_components_read(mac_pre_parsed_frame_t *buffer, mlme_security_t *security_params)
+{
+    memset(security_params, 0, sizeof(mlme_security_t));
+    if (!buffer->fcf_dsn.securityEnabled) {
+        return;
+    }
+
+    mac_header_security_aux_header_parse(mcps_mac_security_aux_header_start_pointer_get(buffer), security_params);
+
+}
+
 uint16_t mac_header_get_src_panid(const mac_fcf_sequence_t *header, const uint8_t *ptr)
 {
-    if( !header || !ptr ){
-        return 0;
-    }
-    ptr += 3; //Skip FCF + DSN
 
-    if (!header->intraPan) {
-        switch (header->DstAddrMode) {
-            case MAC_ADDR_MODE_NONE:
-                break;
-            case MAC_ADDR_MODE_16_BIT:
-                ptr += 4;
-                break;
-            case MAC_ADDR_MODE_64_BIT:
-                ptr += 10;
-                break;
+    if (!header->SrcPanPresents) {
+        if (!header->DstPanPresents) {
+            return 0xffff;
         }
+        return mac_header_get_dst_panid(header, ptr);
     }
-    uint16_t panid = 0;
-    panid += *ptr++;
-    panid += (uint16_t)(*ptr++) << 8;
 
-    return panid;
+    ptr += mac_fcf_lenght(header);//Skip FCF + DSN
+
+    ptr += mac_dst_address_length_with_panid(header); //Skip Dst panID & Address
+
+    return common_read_16_bit_inverse(ptr);
 }
 
 uint16_t mac_header_get_dst_panid(const mac_fcf_sequence_t *header, const uint8_t *ptr)
 {
-    if( !header || !ptr ){
-        return 0;
-    }
-    if (header->DstAddrMode == MAC_ADDR_MODE_NONE) {
+    if (!header->DstPanPresents) {
         return 0xffff;
     }
-    ptr += 3;
-    uint16_t panid = 0;
-    panid += *ptr++;
-    panid += (uint16_t)(*ptr++) << 8;
 
-    return panid;
+    ptr += mac_fcf_lenght(header);//Skip FCF + DSN
+
+    return common_read_16_bit_inverse(ptr);
 }
 
 void mac_header_get_src_address(const mac_fcf_sequence_t *header, const uint8_t *ptr, uint8_t *address_ptr)
 {
-    if( !header || !ptr || !address_ptr ){
-        return;
-    }
-    ptr += 3; //Skip FCF + DSN
-    switch (header->DstAddrMode) {
-        case MAC_ADDR_MODE_NONE:
-            ptr += 2;
-            break;
-        case MAC_ADDR_MODE_16_BIT:
-            ptr += 4;
-            if (!header->intraPan) {
-                ptr += 2;
-            }
-            break;
-        case MAC_ADDR_MODE_64_BIT:
-            ptr += 10;
-            if (!header->intraPan) {
-                ptr += 2;
-            }
-            break;
-    }
-
 
     if (header->SrcAddrMode == MAC_ADDR_MODE_NONE) {
         return;
     }
+
+    ptr += mac_fcf_lenght(header);//Skip FCF + DSN
+
+    ptr += mac_dst_address_length_with_panid(header);
+
     uint8_t address_len, address_index, i;
-    if (header->SrcAddrMode == MAC_ADDR_MODE_16_BIT) {
-        address_len = 2;
-        address_index = 1;
-    } else  {
-        address_len = 8;
-        address_index = 7;
+
+    address_len = mac_address_length(header->SrcAddrMode);
+
+    if (header->SrcPanPresents) {
+        ptr += 2; //Skip PanId
     }
+    address_index = address_len - 1;
+
 
     for (i = 0; i < address_len; i++) {
         address_ptr[address_index - i] = *ptr++;
@@ -300,67 +393,59 @@ void mac_header_get_src_address(const mac_fcf_sequence_t *header, const uint8_t
 
 void mac_header_get_dst_address(const mac_fcf_sequence_t *header, const uint8_t *ptr, uint8_t *address_ptr)
 {
-    if( !header || !ptr || !address_ptr ){
-        return;
-    }
 
     if (header->DstAddrMode == MAC_ADDR_MODE_NONE) {
         return;
     }
     uint8_t address_len, address_index, i;
-    if (header->DstAddrMode == MAC_ADDR_MODE_16_BIT) {
 
-        address_len = 2;
-        address_index = 1;
-    } else  {
-        address_len = 8;
-        address_index = 7;
+    ptr += mac_fcf_lenght(header);//Skip FCF + DSN
+
+    address_len = mac_address_length(header->DstAddrMode);
+
+    if (header->DstPanPresents) {
+        ptr += 2; //Skip PanId
     }
-    ptr += 5; //Skip fcf, dsn & PANID
+    address_index = address_len - 1;
 
     for (i = 0; i < address_len; i++) {
         address_ptr[address_index - i] = *ptr++;
     }
 }
 
-uint16_t mcps_payload_length_from_received_frame(const mac_pre_parsed_frame_t *buffer)
+static uint16_t mac_payload_length_calc_with_ie(uint16_t payload_length, uint16_t payload_ie_length)
 {
-    if( !buffer ){
-        return 0;
+    uint16_t length = payload_length;
+    if (payload_ie_length) {
+        if (length) {
+            length += 2;
+        }
+        length += payload_ie_length;
     }
-    return buffer->mac_payload_length;
+    return length;
 }
 
 uint8_t mcps_mac_header_length_from_received_frame(const mac_pre_parsed_frame_t *buffer)
 {
-    if( !buffer ){
-        return 0;
-    }
-    return (buffer->mac_header_length + buffer->security_aux_header_length);
+    return (buffer->mac_header_length + buffer->security_aux_header_length + buffer->header_ie_length);
 }
 
 uint8_t *mcps_mac_payload_pointer_get(const mac_pre_parsed_frame_t *buffer)
 {
-    if( !buffer ){
-        return NULL;
-    }
     uint8_t *ptr = (uint8_t *) mac_header_message_start_pointer(buffer);
-    ptr += (buffer->mac_header_length + buffer->security_aux_header_length);
+    ptr += mcps_mac_header_length_from_received_frame(buffer);
     return ptr;
 }
 
 uint8_t *mcps_security_mic_pointer_get(const mac_pre_parsed_frame_t *buffer)
 {
-    if (!buffer) {
-        return NULL;
-    }
-    uint8_t *ptr = mcps_mac_payload_pointer_get(buffer);
-    return (ptr + buffer->mac_payload_length);
+    uint8_t *ptr = mcps_mac_payload_pointer_get(buffer) + buffer->mac_payload_length;
+    return ptr;
 }
 
-uint8_t *mcps_mac_security_aux_header_start_pointer_get(const mac_pre_parsed_frame_t *buffer)
+static uint8_t *mcps_mac_security_aux_header_start_pointer_get(const mac_pre_parsed_frame_t *buffer)
 {
-    if (!buffer || !buffer->fcf_dsn.securityEnabled) {
+    if (!buffer->fcf_dsn.securityEnabled) {
         return NULL;
     }
     return (uint8_t *) (mac_header_message_start_pointer(buffer) + buffer->mac_header_length);
@@ -368,16 +453,13 @@ uint8_t *mcps_mac_security_aux_header_start_pointer_get(const mac_pre_parsed_fra
 
 uint8_t mcps_mac_command_frame_id_get(const mac_pre_parsed_frame_t *buffer)
 {
-    if ( !buffer ) {
-        return 0;
-    }
     const uint8_t *ptr = mcps_mac_payload_pointer_get(buffer);
     return *ptr;
 }
 
 uint32_t mcps_mac_security_frame_counter_read(const mac_pre_parsed_frame_t *buffer)
 {
-    if (!buffer || !buffer->fcf_dsn.securityEnabled) {
+    if (!buffer->fcf_dsn.securityEnabled) {
         return 0xffffffff;
     }
 
@@ -386,7 +468,6 @@ uint32_t mcps_mac_security_frame_counter_read(const mac_pre_parsed_frame_t *buff
 
 }
 
-
 static uint8_t *mcps_mac_frame_address_write(uint8_t *ptr, uint8_t addressType, const uint8_t *addressPtr)
 {
     if (addressType == MAC_ADDR_MODE_16_BIT) {
@@ -401,21 +482,266 @@ static uint8_t *mcps_mac_frame_address_write(uint8_t *ptr, uint8_t addressType,
     return ptr;
 }
 
-uint8_t * mcps_generic_header_write(uint8_t *ptr, const mac_pre_build_frame_t *buffer)
+static uint8_t *mac_security_interface_aux_security_header_write(uint8_t *ptr, const mac_aux_security_header_t *auxHeader)
 {
-    ptr = mac_header_write_fcf_dsn(&buffer->fcf_dsn, ptr);
+    uint8_t auxBaseHeader;
+    auxBaseHeader = auxHeader->securityLevel;
+    auxBaseHeader |= (auxHeader->KeyIdMode << 3);
+    *ptr++ = auxBaseHeader;
+    ptr = common_write_32_bit_inverse(auxHeader->frameCounter, ptr);
 
-    if (buffer->fcf_dsn.DstAddrMode) {
-        ptr = common_write_16_bit_inverse(buffer->DstPANId, ptr);
-        //Write DST
-        ptr = mcps_mac_frame_address_write(ptr, buffer->fcf_dsn.DstAddrMode,  buffer->DstAddr);
-    }
-
-    if (buffer->fcf_dsn.SrcAddrMode ){
-        if (!(buffer->fcf_dsn.intraPan)) {
-            ptr = common_write_16_bit_inverse(buffer->SrcPANId, ptr);
-        }
-        ptr = mcps_mac_frame_address_write(ptr, buffer->fcf_dsn.SrcAddrMode, buffer->SrcAddr);
+    switch (auxHeader->KeyIdMode) {
+        case MAC_KEY_ID_MODE_SRC8_IDX:
+            memcpy(ptr, auxHeader->Keysource, 8);
+            ptr += 8;
+            *ptr++ = auxHeader->KeyIndex;
+            break;
+        case MAC_KEY_ID_MODE_SRC4_IDX:
+            memcpy(ptr, auxHeader->Keysource, 4);
+            ptr += 4;
+            *ptr++ = auxHeader->KeyIndex;
+            break;
+        case MAC_KEY_ID_MODE_IDX:
+            *ptr++ = auxHeader->KeyIndex;
+            break;
+        default:
+            break;
+    }
+    return ptr;
+}
+
+uint8_t * mac_generic_packet_write(struct protocol_interface_rf_mac_setup *rf_ptr, uint8_t *ptr, const mac_pre_build_frame_t *buffer)
+{
+    ptr = mac_header_write_fcf_dsn(&buffer->fcf_dsn, ptr);
+
+    if (buffer->fcf_dsn.DstPanPresents) {
+        ptr = common_write_16_bit_inverse(buffer->DstPANId, ptr);
+    }
+
+    if (buffer->fcf_dsn.DstAddrMode) {
+        //Write DST
+        ptr = mcps_mac_frame_address_write(ptr, buffer->fcf_dsn.DstAddrMode,  buffer->DstAddr);
+    }
+
+    if (buffer->fcf_dsn.SrcPanPresents) {
+        ptr = common_write_16_bit_inverse(buffer->SrcPANId, ptr);
+    }
+
+    if (buffer->fcf_dsn.SrcAddrMode ) {
+        ptr = mcps_mac_frame_address_write(ptr, buffer->fcf_dsn.SrcAddrMode, buffer->SrcAddr);
+    }
+
+    if (buffer->fcf_dsn.securityEnabled) {
+        ptr = mac_security_interface_aux_security_header_write(ptr, &buffer->aux_header);
+    }
+    uint8_t *ie_start = ptr;
+    //Copy Payload and set IE Elemets
+    ptr = mac_header_information_elements_write(buffer, ptr);
+    if (buffer->mac_payload_length) {
+        memcpy(ptr, buffer->mac_payload, buffer->mac_payload_length );
+        ptr += buffer->mac_payload_length;
+    }
+    if (rf_ptr->fhss_api) {
+        rf_ptr->fhss_api->write_synch_info(rf_ptr->fhss_api, ie_start, buffer->headerIeLength, FHSS_DATA_FRAME, buffer->tx_time);
+    }
+    return ptr;
+}
+
+static uint8_t *mac_write_ie_vector_list(ns_ie_iovec_t *list, uint16_t length, uint8_t *ptr)
+{
+    const ns_ie_iovec_t *msg_iov = list;
+    for (uint_fast16_t i = 0; i < length; i++, msg_iov++) {
+        memcpy(ptr, msg_iov->ieBase, msg_iov->iovLen);
+        ptr += msg_iov->iovLen;
+    }
+    return ptr;
+}
+
+static bool mac_parse_header_ie(mac_header_IE_t *header_element, uint8_t *ptr)
+{
+    uint16_t ie_dummy = common_read_16_bit_inverse(ptr);
+    if (ie_dummy & 0x8000) {
+        return false;
+    }
+    header_element->length = (ie_dummy & 0x007f);
+    header_element->id = ((ie_dummy & 0x7f80  ) >> 7 );
+    header_element->content_ptr = ptr + 2;
+    return true;
+}
+
+static bool mac_parse_payload_ie(mac_payload_IE_t *payload_element, uint8_t *ptr)
+{
+    uint16_t ie_dummy = common_read_16_bit_inverse(ptr);
+    if (!(ie_dummy & 0x8000)) {
+        return false;
+    }
+    payload_element->length = (ie_dummy & 0x07ff);
+    payload_element->id = ((ie_dummy & 0x7800  ) >> 11);
+    payload_element->content_ptr = ptr + 2;
+    return true;
+}
+
+
+bool mac_header_information_elements_parse(mac_pre_parsed_frame_t *buffer)
+{
+    uint8_t *ptr = (mac_header_message_start_pointer(buffer) + buffer->mac_header_length + buffer->security_aux_header_length);
+
+    buffer->headerIePtr = NULL;
+    buffer->headerIeLength = 0;
+    buffer->payloadsIePtr = NULL;
+    buffer->payloadsIeLength = 0;
+    buffer->header_ie_length = 0;
+    if (!buffer->fcf_dsn.informationElementsPresets) {
+        buffer->macPayloadPtr = ptr;
+        return true;
+    }
+
+    if (buffer->mac_payload_length < 2) {
+        return false;
+    }
+
+    mac_header_IE_t header_ie;
+    buffer->headerIePtr = ptr;
+
+    while (buffer->mac_payload_length >= 2) {
+
+        if (!mac_parse_header_ie(&header_ie, ptr)) {
+            return false;
+        }
+
+        buffer->mac_payload_length -= 2;
+        if (header_ie.length > buffer->mac_payload_length) {
+            return false;
+        }
+
+        buffer->mac_payload_length -= header_ie.length;
+
+        buffer->header_ie_length  += header_ie.length +2;
+        ptr += (2 + header_ie.length);
+
+        if (header_ie.id == MAC_HEADER_TERMINATION1_IE_ID) {
+            break;
+        } else if (header_ie.id == MAC_HEADER_TERMINATION2_IE_ID) {
+            buffer->macPayloadPtr = ptr;
+            return true;
+        }
+        buffer->headerIeLength += header_ie.length +2;
+    }
+
+    return true;
+}
+
+
+bool mac_payload_information_elements_parse(mac_pre_parsed_frame_t *buffer)
+{
+    uint8_t *ptr = (mac_header_message_start_pointer(buffer) + buffer->mac_header_length + buffer->security_aux_header_length + buffer->header_ie_length);
+    if (!buffer->fcf_dsn.informationElementsPresets) {
+        return true;
+    }
+
+    if (buffer->mac_payload_length < 2) {
+        return false;
+    }
+
+    //Parse Payload IE
+    buffer->payloadsIePtr = ptr;
+    mac_payload_IE_t payload_ie;
+    while (buffer->mac_payload_length >= 2) {
+
+        if (!mac_parse_payload_ie(&payload_ie, ptr)) {
+            return false;
+        }
+        buffer->mac_payload_length -= 2;
+        if (payload_ie.length > buffer->mac_payload_length) {
+            return false;
+        }
+        buffer->mac_payload_length -= payload_ie.length;
+
+        if (payload_ie.id == MAC_PAYLOAD_TERMINATION_IE_GROUP_ID) {
+            break;
+        }
+        buffer->payloadsIeLength += payload_ie.length + 2;
+        buffer->macPayloadPtr += payload_ie.length + 2;
+        ptr += (2 + payload_ie.length);
+
+    }
+    buffer->macPayloadPtr = ptr;
+    return true;
+}
+
+
+static uint8_t *mac_header_ie_terimate(uint8_t *ptr, uint8_t type)
+{
+    uint16_t ie_dummy = 0;
+    ie_dummy |= (type << 7 );
+    return common_write_16_bit_inverse(ie_dummy, ptr);
+
+
+}
+
+static uint8_t *mac_payload_ie_terimate(uint8_t *ptr)
+{
+    uint16_t ie_dummy = 0;
+    ie_dummy |= (MAC_PAYLOAD_TERMINATION_IE_GROUP_ID << 11 );
+    ie_dummy |= (1 << 15);
+    return common_write_16_bit_inverse(ie_dummy, ptr);
+}
+
+
+void mac_header_information_elements_preparation(mac_pre_build_frame_t *buffer)
+{
+    if (buffer->message_builded) {
+        return;
+    }
+
+    if (buffer->headerIeLength ||  buffer->payloadsIeLength) {
+        buffer->fcf_dsn.frameVersion = MAC_FRAME_VERSION_2015;
+        buffer->fcf_dsn.informationElementsPresets = true;
+        buffer->message_builded = true;
+        //Write Header elements
+        if (buffer->headerIeLength) {
+            buffer->mac_header_length_with_security += buffer->headerIeLength;
+            if (!buffer->payloadsIeLength && !buffer->mac_payload_length) {
+                //No termination needed
+                return;
+            }
+            //Terminate
+            buffer->mac_header_length_with_security += 2;
+        } else {
+            buffer->mac_header_length_with_security += 2;
+        }
+    }
+}
+
+uint16_t mac_buffer_total_payload_length(mac_pre_build_frame_t *buffer)
+{
+    return mac_payload_length_calc_with_ie(buffer->mac_payload_length, buffer->payloadsIeLength);
+}
+
+
+static uint8_t * mac_header_information_elements_write(const mac_pre_build_frame_t *buffer, uint8_t *ptr)
+{
+    if (buffer->fcf_dsn.frameVersion == MAC_FRAME_VERSION_2015 && buffer->fcf_dsn.informationElementsPresets) {
+        //Write Header elements
+        if (buffer->headerIeLength) {
+            ptr = mac_write_ie_vector_list(buffer->ie_elements.headerIeVectorList, buffer->ie_elements.headerIovLength, ptr);
+
+            if (!buffer->payloadsIeLength && !buffer->mac_payload_length) {
+                //No termination needed
+                return ptr;
+            } else if (!buffer->payloadsIeLength && buffer->mac_payload_length) {
+                return mac_header_ie_terimate(ptr, MAC_HEADER_TERMINATION2_IE_ID);
+            }
+        }
+        //Add Header Termination
+        ptr = mac_header_ie_terimate(ptr, MAC_HEADER_TERMINATION1_IE_ID);
+
+        if (buffer->payloadsIeLength) {
+            ptr = mac_write_ie_vector_list(buffer->ie_elements.payloadIeVectorList, buffer->ie_elements.payloadIovLength, ptr);
+            if (buffer->mac_payload_length) {
+                ptr = mac_payload_ie_terimate(ptr);
+            }
+        }
     }
     return ptr;
 }
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_header_helper_functions.h b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_header_helper_functions.h
index 5b71c64849..4231f39820 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_header_helper_functions.h
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_header_helper_functions.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -24,9 +24,13 @@ struct mlme_security_s;
 struct mac_pre_parsed_frame_s;
 struct mac_pre_build_frame;
 struct mac_aux_security_header_s;
+struct ns_ie_iovec;
+struct protocol_interface_rf_mac_setup;
 
 uint8_t mac_security_mic_length_get(uint8_t security_level);
 uint8_t mac_header_security_aux_header_length(uint8_t security_level, uint8_t keyIdmode);
+bool mac_dst_panid_present(const struct mac_fcf_sequence_s *header);
+bool mac_src_panid_present(const struct mac_fcf_sequence_s *header);
 uint8_t mac_header_address_length(const struct mac_fcf_sequence_s *fcf);
 
 void mac_header_security_parameter_set(struct mac_aux_security_header_s *header, const struct mlme_security_s *frame_setup);
@@ -34,10 +38,24 @@ void mac_header_security_parameter_set(struct mac_aux_security_header_s *header,
 /**
  * MAC layer FCF and data sequence parse from data
  */
-void mac_header_parse_fcf_dsn(struct mac_fcf_sequence_s *header, const uint8_t *ptr);
+const uint8_t * mac_header_parse_fcf_dsn(struct mac_fcf_sequence_s *header, const uint8_t *ptr);
+
+uint16_t mac_header_off_set_to_aux_header(const struct mac_fcf_sequence_s *fcf);
+
+void mac_header_security_aux_header_parse(const uint8_t *ptr, struct mlme_security_s *security_params);
 
 void mac_header_security_components_read(struct mac_pre_parsed_frame_s *buffer, struct mlme_security_s *security_params);
 
+bool mac_header_information_elements_parse(struct mac_pre_parsed_frame_s *buffer);
+
+bool mac_payload_information_elements_parse(struct mac_pre_parsed_frame_s *buffer);
+
+uint16_t mac_header_information_elements_length(struct mac_pre_build_frame *buffer);
+
+void mac_header_information_elements_preparation(struct mac_pre_build_frame *buffer);
+
+uint16_t mac_buffer_total_payload_length(struct mac_pre_build_frame *buffer);
+
 /**
  *  Next function should only call when address mode is not MAC_ADDR_MODE_NONE and parsed proper header!
  */
@@ -45,16 +63,14 @@ uint16_t mac_header_get_src_panid(const struct mac_fcf_sequence_s *header, const
 uint16_t mac_header_get_dst_panid(const struct mac_fcf_sequence_s *header, const uint8_t *ptr);
 void mac_header_get_src_address(const struct mac_fcf_sequence_s *header, const uint8_t *ptr, uint8_t *address_ptr);
 void mac_header_get_dst_address(const struct mac_fcf_sequence_s *header, const uint8_t *ptr, uint8_t *address_ptr);
-
-uint16_t mcps_payload_length_from_received_frame(const struct mac_pre_parsed_frame_s *buffer);
+uint8_t mac_address_length(uint8_t address_mode);
 uint8_t mcps_mac_header_length_from_received_frame(const struct mac_pre_parsed_frame_s *buffer);
 uint32_t mcps_mac_security_frame_counter_read(const struct mac_pre_parsed_frame_s *buffer);
 uint8_t mcps_mac_command_frame_id_get(const struct mac_pre_parsed_frame_s *buffer);
 uint8_t *mcps_mac_payload_pointer_get(const struct mac_pre_parsed_frame_s *buffer);
 uint8_t *mcps_security_mic_pointer_get(const struct mac_pre_parsed_frame_s *buffer);
-uint8_t *mcps_mac_security_aux_header_start_pointer_get(const struct mac_pre_parsed_frame_s *buffer);
-
-uint8_t * mcps_generic_header_write(uint8_t *ptr, const struct mac_pre_build_frame *buffer);
+/* Write Mac Header and payload */
+uint8_t * mac_generic_packet_write(struct protocol_interface_rf_mac_setup *rf_ptr, uint8_t *ptr, const struct mac_pre_build_frame *buffer);
 
 
 /** get pointer to Mac header start point*/
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_indirect_data.c b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_indirect_data.c
index 153c68ee09..3820c16e8d 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_indirect_data.c
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_indirect_data.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -55,6 +55,7 @@ void mac_indirect_data_ttl_handle(protocol_interface_rf_mac_setup_s *cur, uint16
         if( dev_driver && dev_driver->extension ){
             dev_driver->extension(PHY_EXTENSION_CTRL_PENDING_BIT, &value);
         }
+        cur->mac_frame_pending = false;
         return;
     }
     mac_pre_build_frame_t *buf, *buf_prev = 0, *buf_temp = 0;
@@ -93,7 +94,13 @@ void mac_indirect_data_ttl_handle(protocol_interface_rf_mac_setup_s *cur, uint16
             confirm.status = MLME_TRANSACTION_EXPIRED;
 
             mcps_sap_prebuild_frame_buffer_free(buf_temp);
-            if( api->data_conf_cb ) {
+
+            if (cur->mac_extension_enabled) {
+                mcps_data_conf_payload_t data_conf;
+                memset(&data_conf, 0, sizeof(mcps_data_conf_payload_t));
+                //Check Payload Here
+                api->data_conf_ext_cb(api, &confirm, &data_conf);
+            } else {
                 api->data_conf_cb(api, &confirm);
             }
         }
@@ -216,6 +223,7 @@ void mac_indirect_queue_write(protocol_interface_rf_mac_setup_s *rf_mac_setup, m
         //Trig timer and set pending flag to radio
         eventOS_callback_timer_stop(rf_mac_setup->mac_mcps_timer);
         eventOS_callback_timer_start(rf_mac_setup->mac_mcps_timer, MAC_INDIRECT_TICK_IN_MS * 20);
+        rf_mac_setup->mac_frame_pending = true;
         if (rf_mac_setup->dev_driver->phy_driver->extension)
         {
             uint8_t value = 1;
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.c b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.c
index eafdeb6b25..cfe5683760 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.c
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -51,6 +51,9 @@
 
 #define TRACE_GROUP "mMCp"
 
+// Used to set TX time (us) with FHSS. Must be <= 65ms.
+#define MAC_TX_PROCESSING_DELAY_INITIAL 2000
+
 typedef struct {
     uint8_t address[8];
     unsigned addr_type:2;
@@ -62,13 +65,72 @@ typedef struct {
 void mcps_sap_pd_req_queue_write(protocol_interface_rf_mac_setup_s *rf_mac_setup, mac_pre_build_frame_t *buffer);
 static mac_pre_build_frame_t * mcps_sap_pd_req_queue_read(protocol_interface_rf_mac_setup_s *rf_mac_setup, bool is_bc_queue, bool flush);
 static int8_t mcps_pd_data_request(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer);
-static void mcps_data_confirm_handle(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer);
+static void mcps_data_confirm_handle(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer, mac_pre_parsed_frame_t *ack_buf);
 static void mac_set_active_event(protocol_interface_rf_mac_setup_s *rf_mac_setup, uint8_t event_type);
 static void mac_clear_active_event(protocol_interface_rf_mac_setup_s *rf_mac_setup, uint8_t event_type);
 static bool mac_read_active_event(protocol_interface_rf_mac_setup_s *rf_mac_setup, uint8_t event_type);
+static int8_t mcps_pd_data_cca_trig(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer);
 
 static int8_t mac_tasklet_event_handler = -1;
 
+/**
+ * Get PHY time stamp.
+ *
+ * \param rf_mac_setup pointer to MAC
+ * \return Timestamp from PHY
+ *
+ */
+static uint32_t mac_mcps_sap_get_phy_timestamp(protocol_interface_rf_mac_setup_s *rf_mac_setup)
+{
+    uint32_t timestamp;
+    rf_mac_setup->dev_driver->phy_driver->extension(PHY_EXTENSION_GET_TIMESTAMP, (uint8_t *)&timestamp);
+    return timestamp;
+}
+
+static void mac_data_request_init(protocol_interface_rf_mac_setup_s *rf_mac_setup, mac_pre_build_frame_t *buffer)
+{
+    rf_mac_setup->active_pd_data_request = buffer;
+    if (buffer->asynch_request) {
+        buffer->asynch_channel = rf_mac_setup->mac_channel; //Store Original channel
+        uint16_t channel = mlme_scan_analyze_next_channel(&buffer->asynch_channel_list);
+        if (channel <= 0xff) {
+            uint8_t switch_channel = channel;
+            if (rf_mac_setup->mac_channel != switch_channel) {
+                mac_mlme_mac_radio_disabled(rf_mac_setup);
+                rf_mac_setup->mac_channel = channel;
+                mac_mlme_mac_radio_enable(rf_mac_setup);
+            }
+        }
+    }
+}
+
+static bool mac_data_request_confirmation_finnish(protocol_interface_rf_mac_setup_s *rf_mac_setup, mac_pre_build_frame_t *buffer)
+{
+    if (!buffer->asynch_request) {
+        return true;
+    }
+
+    uint16_t channel = mlme_scan_analyze_next_channel(&buffer->asynch_channel_list);
+    uint8_t switch_channel;
+    bool return_value;
+
+    if (channel > 0x00ff) {
+        return_value =  true;
+        switch_channel = buffer->asynch_channel;
+    } else {
+        switch_channel = channel;
+        return_value =  false;
+    }
+    //Set original Channel back if channel is switched
+    if (rf_mac_setup->mac_channel != switch_channel) {
+        mac_mlme_mac_radio_disabled(rf_mac_setup);
+        rf_mac_setup->mac_channel = switch_channel;
+        mac_mlme_mac_radio_enable(rf_mac_setup);
+    }
+    return return_value;
+
+}
+
 
 static void mac_data_poll_radio_disable_check(protocol_interface_rf_mac_setup_s *rf_mac_setup)
 {
@@ -81,20 +143,79 @@ static void mac_data_poll_radio_disable_check(protocol_interface_rf_mac_setup_s
     }
 }
 
-static void mcps_data_confirm_cb(protocol_interface_rf_mac_setup_s *rf_mac_setup, mcps_data_conf_t *confirm)
+static void mcps_data_confirm_cb(protocol_interface_rf_mac_setup_s *rf_mac_setup, mcps_data_conf_t *confirm, mac_pre_parsed_frame_t *ack_buf)
 {
 
     mac_data_poll_radio_disable_check(rf_mac_setup);
 
     if( get_sw_mac_api(rf_mac_setup) ) {
-        get_sw_mac_api(rf_mac_setup)->data_conf_cb(get_sw_mac_api(rf_mac_setup), confirm);
+        if (rf_mac_setup->mac_extension_enabled) {
+            mcps_data_conf_payload_t data_conf;
+            memset(&data_conf, 0, sizeof(mcps_data_conf_payload_t));
+            if (ack_buf) {
+                data_conf.payloadIeList = ack_buf->payloadsIePtr;
+                data_conf.payloadIeListLength = ack_buf->payloadsIeLength;
+                data_conf.headerIeList = ack_buf->headerIePtr;
+                data_conf.headerIeListLength = ack_buf->headerIeLength;
+                data_conf.payloadLength = ack_buf->mac_payload_length;
+                data_conf.payloadPtr = ack_buf->macPayloadPtr;
+            }
+            //Check Payload Here
+            get_sw_mac_api(rf_mac_setup)->data_conf_ext_cb(get_sw_mac_api(rf_mac_setup), confirm, &data_conf);
+        } else {
+            get_sw_mac_api(rf_mac_setup)->data_conf_cb(get_sw_mac_api(rf_mac_setup), confirm);
+        }
     }
 }
 
-void mcps_sap_data_req_handler(protocol_interface_rf_mac_setup_s *rf_mac_setup , const mcps_data_req_t *data_req ) {
+void mcps_sap_data_req_handler(protocol_interface_rf_mac_setup_s *rf_mac_setup , const mcps_data_req_t *data_req)
+{
+    mcps_data_req_ie_list_t ie_list;
+    memset(&ie_list, 0 , sizeof(mcps_data_req_ie_list_t));
+    mcps_sap_data_req_handler_ext(rf_mac_setup, data_req, &ie_list, NULL);
+}
+
+static bool mac_ie_vector_length_validate(ns_ie_iovec_t *ie_vector, uint16_t iov_length,  uint16_t *length_out)
+{
+    if (length_out) {
+        *length_out = 0;
+    }
+
+    if (!iov_length) {
+        return true;
+    }
+
+    if (iov_length != 0 && !ie_vector) {
+        return false;
+    }
+
+    uint16_t msg_length = 0;
+    ns_ie_iovec_t *msg_iov = ie_vector;
+    for (uint_fast16_t i = 0; i < iov_length; i++) {
+        if (msg_iov->iovLen != 0 && !msg_iov->ieBase) {
+            return false;
+        }
+        msg_length += msg_iov->iovLen;
+        if (msg_length < msg_iov->iovLen) {
+            return false;
+        }
+        msg_iov++;
+    }
+
+    if (length_out) {
+        *length_out = msg_length;
+    }
+
+    return true;
+
+}
+
+
+void mcps_sap_data_req_handler_ext(protocol_interface_rf_mac_setup_s *rf_mac_setup , const mcps_data_req_t *data_req , const mcps_data_req_ie_list_t *ie_list, const channel_list_s *asynch_channel_list) {
     uint8_t status = MLME_SUCCESS;
     mac_pre_build_frame_t *buffer = NULL;
 
+
     if (!rf_mac_setup->mac_security_enabled) {
         if (data_req->Key.SecurityLevel) {
             status = MLME_UNSUPPORTED_SECURITY;
@@ -102,12 +223,34 @@ void mcps_sap_data_req_handler(protocol_interface_rf_mac_setup_s *rf_mac_setup ,
         }
     }
 
-    if (data_req->msduLength > rf_mac_setup->dev_driver->phy_driver->phy_MTU - MAC_DATA_PACKET_MIN_HEADER_LENGTH) {
+    uint16_t ie_header_length = 0;
+    uint16_t ie_payload_length = 0;
+
+    if (!mac_ie_vector_length_validate(ie_list->headerIeVectorList, ie_list->headerIovLength, &ie_header_length)) {
+        status = MLME_INVALID_PARAMETER;
+        goto verify_status;
+    }
+
+    if (!mac_ie_vector_length_validate(ie_list->payloadIeVectorList, ie_list->payloadIovLength, &ie_payload_length)) {
+        status = MLME_INVALID_PARAMETER;
+        goto verify_status;
+    }
+
+    if ((ie_header_length || ie_payload_length || asynch_channel_list) && !rf_mac_setup->mac_extension_enabled ) {
+        //Report error when feature is not enaled yet
+        status = MLME_INVALID_PARAMETER;
+        goto verify_status;
+    } else if (asynch_channel_list && data_req->TxAckReq) {
+        //Report Asynch Message is not allowed to call with ACK requested.
+        status = MLME_INVALID_PARAMETER;
+        goto verify_status;
+    }
+
+    if ((data_req->msduLength + ie_header_length + ie_payload_length) > rf_mac_setup->dev_driver->phy_driver->phy_MTU - MAC_DATA_PACKET_MIN_HEADER_LENGTH) {
         tr_debug("packet %u, %u",data_req->msduLength, rf_mac_setup->dev_driver->phy_driver->phy_MTU);
         status = MLME_FRAME_TOO_LONG;
         goto verify_status;
     }
-    //protocol_interface_rf_mac_setup_s *rf_mac_setup = (protocol_interface_rf_mac_setup_s*)api;
     buffer = mcps_sap_prebuild_frame_buffer_get(0);
     //tr_debug("Data Req");
     if (!buffer) {
@@ -121,6 +264,13 @@ void mcps_sap_data_req_handler(protocol_interface_rf_mac_setup_s *rf_mac_setup ,
         tr_debug("Drop MAC tx packet when mac disabled");
         goto verify_status;
     }
+
+    if (asynch_channel_list ) {
+        //Copy Asynch data list
+        buffer->asynch_channel_list = *asynch_channel_list;
+        buffer->asynch_request = true;
+    }
+
     buffer->upper_layer_request = true;
     buffer->fcf_dsn.frametype = FC_DATA_FRAME;
     buffer->fcf_dsn.ackRequested = data_req->TxAckReq;
@@ -143,8 +293,8 @@ void mcps_sap_data_req_handler(protocol_interface_rf_mac_setup_s *rf_mac_setup ,
     buffer->fcf_dsn.SrcAddrMode = data_req->SrcAddrMode;
     buffer->fcf_dsn.framePending = data_req->PendingBit;
 
-    if (buffer->fcf_dsn.SrcAddrMode == MAC_ADDR_MODE_NONE) {
-        if (buffer->fcf_dsn.DstAddrMode == MAC_ADDR_MODE_NONE) {
+    if (buffer->fcf_dsn.SrcAddrMode == MAC_ADDR_MODE_NONE && !rf_mac_setup->mac_extension_enabled) {
+        if (buffer->fcf_dsn.DstAddrMode == MAC_ADDR_MODE_NONE ) {
             status = MLME_INVALID_ADDRESS;
             goto verify_status;
         }
@@ -158,10 +308,48 @@ void mcps_sap_data_req_handler(protocol_interface_rf_mac_setup_s *rf_mac_setup ,
 
     mac_frame_src_address_set_from_interface(buffer->fcf_dsn.SrcAddrMode, rf_mac_setup, buffer->SrcAddr);
 
-    if (buffer->DstPANId == buffer->SrcPANId) {
-        buffer->fcf_dsn.intraPan = true;
+    buffer->ie_elements.headerIeVectorList = ie_list->headerIeVectorList;
+    buffer->ie_elements.headerIovLength = ie_list->headerIovLength;
+    buffer->ie_elements.payloadIeVectorList = ie_list->payloadIeVectorList;
+    buffer->ie_elements.payloadIovLength = ie_list->payloadIovLength;
+    buffer->headerIeLength = ie_header_length;
+    buffer->payloadsIeLength = ie_payload_length;
+
+
+    if (rf_mac_setup->mac_extension_enabled) {
+        //Handle mac extension's
+        buffer->fcf_dsn.frameVersion = MAC_FRAME_VERSION_2015;
+        if (ie_header_length || ie_payload_length) {
+            buffer->fcf_dsn.informationElementsPresets = true;
+        }
+
+        buffer->fcf_dsn.sequenceNumberSuppress = data_req->SeqNumSuppressed;
+        if (buffer->fcf_dsn.sequenceNumberSuppress) {
+            buffer->mac_header_length_with_security--;
+        }
+        /* PAN-ID compression bit enable when necessary */
+        if (buffer->fcf_dsn.SrcAddrMode == MAC_ADDR_MODE_NONE && buffer->fcf_dsn.DstAddrMode == MAC_ADDR_MODE_NONE) {
+            buffer->fcf_dsn.intraPan = !data_req->PanIdSuppressed;
+        } else if (buffer->fcf_dsn.DstAddrMode == MAC_ADDR_MODE_NONE) {
+            buffer->fcf_dsn.intraPan = data_req->PanIdSuppressed;
+        } else if (buffer->fcf_dsn.SrcAddrMode == MAC_ADDR_MODE_NONE || (buffer->fcf_dsn.SrcAddrMode == MAC_ADDR_MODE_64_BIT && buffer->fcf_dsn.DstAddrMode == MAC_ADDR_MODE_64_BIT)) {
+            buffer->fcf_dsn.intraPan = data_req->PanIdSuppressed;
+        } else /* two addresses, at least one address short */ {
+            // ignore or fault panidsuppressed
+            if (buffer->DstPANId == buffer->SrcPANId ) {
+                buffer->fcf_dsn.intraPan = true;
+            }
+        }
+    } else {
+        /* PAN-ID compression bit enable when necessary */
+        if ((buffer->fcf_dsn.DstAddrMode && buffer->fcf_dsn.SrcAddrMode) && (buffer->DstPANId == buffer->SrcPANId)) {
+            buffer->fcf_dsn.intraPan = true;
+        }
     }
 
+    //Check PanID presents at header
+    buffer->fcf_dsn.DstPanPresents = mac_dst_panid_present(&buffer->fcf_dsn);
+    buffer->fcf_dsn.SrcPanPresents = mac_src_panid_present(&buffer->fcf_dsn);
     //Calculate address length
     buffer->mac_header_length_with_security += mac_header_address_length(&buffer->fcf_dsn);
     buffer->mac_payload = data_req->msdu;
@@ -181,7 +369,7 @@ void mcps_sap_data_req_handler(protocol_interface_rf_mac_setup_s *rf_mac_setup ,
         confirm.status = status;
         tr_debug("DATA REQ Fail %u", status);
         mcps_sap_prebuild_frame_buffer_free(buffer);
-        mcps_data_confirm_cb(rf_mac_setup, &confirm);
+        mcps_data_confirm_cb(rf_mac_setup, &confirm, NULL);
     }
 }
 
@@ -200,8 +388,10 @@ static int8_t mac_virtual_data_req_handler(protocol_interface_rf_mac_setup_s *rf
     }
 
     mac_header_parse_fcf_dsn(&buffer->fcf_dsn, data_ptr);
+    buffer->fcf_dsn.DstPanPresents = mac_dst_panid_present(&buffer->fcf_dsn);
+    buffer->fcf_dsn.SrcPanPresents = mac_src_panid_present(&buffer->fcf_dsn);
     // Use MAC sequence as handle
-    buffer->msduHandle = data_ptr[2];
+    buffer->msduHandle = buffer->fcf_dsn.DSN;
     memcpy(buffer->mac_payload,data_ptr,  data_length);
     buffer->mac_payload_length = data_length;
 
@@ -227,8 +417,8 @@ static int8_t mac_virtual_mlme_nap_req_handler(protocol_interface_rf_mac_setup_s
             }
 
             mlme_scan_t mlme_scan_req;
-            mlme_scan_req.ScanType = *ptr++;
-            mlme_scan_req.ScanChannels.channel_page = *ptr++;
+            mlme_scan_req.ScanType = (mac_scan_type_t) *ptr++;
+            mlme_scan_req.ScanChannels.channel_page = (channel_page_e) *ptr++;
             memcpy(mlme_scan_req.ScanChannels.channel_mask, ptr, 32);
             ptr += 32;
             mlme_scan_req.ScanDuration = *ptr++;
@@ -239,14 +429,13 @@ static int8_t mac_virtual_mlme_nap_req_handler(protocol_interface_rf_mac_setup_s
             memcpy(mlme_scan_req.Key.Keysource, ptr, 8);
             mac_mlme_scan_request(&mlme_scan_req, rf_mac_setup);
             return 0;
-            break;
         }
         case MLME_SET:{
             if (mlme_req->ptr_length < 3) {
                 return -1;
             }
             mlme_set_t mlme_set_req;
-            mlme_set_req.attr = *ptr++;
+            mlme_set_req.attr = (mlme_attr_t) *ptr++;
             mlme_set_req.attr_index = *ptr++;
             mlme_set_req.value_pointer = ptr;
             mlme_set_req.value_size = mlme_req->ptr_length - 2;
@@ -338,6 +527,7 @@ static uint8_t mac_data_interface_decrypt_packet(mac_pre_parsed_frame_t *b, mlme
     mlme_key_device_descriptor_t *key_device_description = NULL;
     uint8_t device_descriptor_handle;
     uint8_t openPayloadLength = 0;
+    bool security_by_pass = false;
     protocol_interface_rf_mac_setup_s *rf_mac_setup = (protocol_interface_rf_mac_setup_s*)b->mac_class_ptr;
 //    mlme_security_level_descriptor_t security_level_compare;
 
@@ -401,7 +591,12 @@ static uint8_t mac_data_interface_decrypt_packet(mac_pre_parsed_frame_t *b, mlme
     } else {
 
         if (!b->neigh_info) {
-            return MLME_UNSUPPORTED_SECURITY;
+            if (rf_mac_setup->mac_security_bypass_unknow_device && (b->fcf_dsn.SrcAddrMode == MAC_ADDR_MODE_64_BIT
+                    && security_params->SecurityLevel > AES_SECURITY_LEVEL_ENC)) {
+                security_by_pass = true;
+            } else {
+                return MLME_UNSUPPORTED_SECURITY;
+            }
         }
 
         device_descriptor_handle = mac_mib_device_descption_attribute_get_by_descriptor(rf_mac_setup, b->neigh_info);
@@ -415,52 +610,57 @@ static uint8_t mac_data_interface_decrypt_packet(mac_pre_parsed_frame_t *b, mlme
             return MLME_UNAVAILABLE_KEY;
         }
 
-        if (neighbour_validation.frameCounter < b->neigh_info->FrameCounter ) {
+        if (b->neigh_info && neighbour_validation.frameCounter < b->neigh_info->FrameCounter ) {
             tr_debug("MLME_COUNTER_ERROR");
             return MLME_COUNTER_ERROR;
         }
     }
 
     key = key_description->Key;
-    memcpy(neighbour_validation.nonce_ptr, b->neigh_info->ExtAddress, 8);
+    if (security_by_pass) {
+        memcpy(neighbour_validation.nonce_ptr, neighbour_validation.address, 8);
+    } else {
+        memcpy(neighbour_validation.nonce_ptr, b->neigh_info->ExtAddress, 8);
+    }
 
-    ccm_globals_t *ccm_ptr = ccm_sec_init(security_params->SecurityLevel, key, AES_CCM_DECRYPT, 2);
+    ccm_globals_t ccm_ptr;
 
-    if (!ccm_ptr) {
+    if (!ccm_sec_init(&ccm_ptr, security_params->SecurityLevel, key, AES_CCM_DECRYPT, 2)) {
         return MLME_UNSUPPORTED_SECURITY;
     }
 
-    mac_security_interface_aux_ccm_nonce_set(ccm_ptr->exp_nonce, neighbour_validation.nonce_ptr, neighbour_validation.frameCounter, security_params->SecurityLevel);
+    mac_security_interface_aux_ccm_nonce_set(ccm_ptr.exp_nonce, neighbour_validation.nonce_ptr, neighbour_validation.frameCounter, security_params->SecurityLevel);
 
-    if (ccm_ptr->mic_len) {
+    if (ccm_ptr.mic_len) {
      // this is asuming that there is no headroom for buffers.
-     ccm_ptr->adata_len = mcps_mac_header_length_from_received_frame(b) + openPayloadLength;
+     ccm_ptr.adata_len = mcps_mac_header_length_from_received_frame(b) + openPayloadLength;
      //SET MIC PTR
-     ccm_ptr->mic = mcps_security_mic_pointer_get(b);
-     ccm_ptr->adata_ptr = mac_header_message_start_pointer(b);
+     ccm_ptr.mic = mcps_security_mic_pointer_get(b);
+     ccm_ptr.adata_ptr = mac_header_message_start_pointer(b);
     }
 
-    ccm_ptr->data_ptr = (mcps_mac_payload_pointer_get(b) + openPayloadLength);
-    ccm_ptr->data_len = mcps_payload_length_from_received_frame(b) - openPayloadLength;
-    if (ccm_process_run(ccm_ptr) != 0) {
-        tr_warning("MIC Fail adata %s", trace_array(ccm_ptr->adata_ptr, ccm_ptr->adata_len));
-        tr_warning("Nonce %s", trace_array(ccm_ptr->exp_nonce, 13));
+    ccm_ptr.data_ptr = (mcps_mac_payload_pointer_get(b) + openPayloadLength);
+    ccm_ptr.data_len = b->mac_payload_length - openPayloadLength;
+    if (ccm_process_run(&ccm_ptr) != 0) {
+        tr_warning("MIC Fail adata %s", trace_array(ccm_ptr.adata_ptr, ccm_ptr.adata_len));
+        tr_warning("Nonce %s", trace_array(ccm_ptr.exp_nonce, 13));
         if (openPayloadLength) {
-            tr_warning("%s", tr_array(ccm_ptr->data_ptr,  ccm_ptr->data_len));
+            tr_warning("%s", tr_array(ccm_ptr.data_ptr,  ccm_ptr.data_len));
         }
         return MLME_SECURITY_FAIL;
     }
 
     //Update key device and key description tables
-
-    b->neigh_info->FrameCounter = neighbour_validation.frameCounter + 1;
-    if (!key_device_description) {
-        // Black list old used keys by this device
-        mac_sec_mib_device_description_blacklist(rf_mac_setup, device_descriptor_handle);
-        key_device_description =  mac_sec_mib_key_device_description_list_update(key_description);
-        if (key_device_description) {
-            tr_debug("Set new device user %u for key", device_descriptor_handle);
-            key_device_description->DeviceDescriptorHandle = device_descriptor_handle;
+    if (!security_by_pass) {
+        b->neigh_info->FrameCounter = neighbour_validation.frameCounter + 1;
+        if (!key_device_description) {
+            // Black list old used keys by this device
+            mac_sec_mib_device_description_blacklist(rf_mac_setup, device_descriptor_handle);
+            key_device_description =  mac_sec_mib_key_device_description_list_update(key_description);
+            if (key_device_description) {
+                tr_debug("Set new device user %u for key", device_descriptor_handle);
+                key_device_description->DeviceDescriptorHandle = device_descriptor_handle;
+            }
         }
     }
 
@@ -531,22 +731,27 @@ static int8_t mac_data_sap_rx_handler(mac_pre_parsed_frame_t *buf, protocol_inte
 
     data_ind->mpduLinkQuality = buf->LQI;
     data_ind->signal_dbm = buf->dbm;
+    data_ind->timestamp = buf->timestamp;
     /* Parse security part */
     mac_header_security_components_read(buf, &data_ind->Key);
-    data_ind->msduLength = buf->mac_payload_length;
-    data_ind->msdu_ptr = mcps_mac_payload_pointer_get(buf);
-
-
 
     buf->neigh_info = mac_sec_mib_device_description_get(rf_mac_setup, data_ind->SrcAddr, data_ind->SrcAddrMode);
     if (buf->fcf_dsn.securityEnabled) {
         status = mac_data_interface_decrypt_packet(buf, &data_ind->Key);
         if (status != MLME_SUCCESS) {
+            tr_debug("Decrypt fail, %d", status);
             mcps_comm_status_indication_generate(status, buf, mac);
             goto DROP_PACKET;
         }
     }
 
+    if (!mac_payload_information_elements_parse(buf)) {
+        tr_debug("Drop by Paylod IE");
+        goto DROP_PACKET;
+    }
+    data_ind->msduLength = buf->mac_payload_length;
+    data_ind->msdu_ptr = buf->macPayloadPtr;
+
     /* Validate Polling device */
     if (!rf_mac_setup->macCapRxOnIdle) {
         if (mac_data_interface_host_accept_data(data_ind, rf_mac_setup) != 0) {
@@ -556,7 +761,22 @@ static int8_t mac_data_sap_rx_handler(mac_pre_parsed_frame_t *buf, protocol_inte
     }
 
     if (mac) {
-        mac->data_ind_cb(mac, data_ind);
+
+        if (buf->fcf_dsn.frameVersion == MAC_FRAME_VERSION_2015) {
+            if (!rf_mac_setup->mac_extension_enabled) {
+                tr_debug("No Ext reg");
+                goto DROP_PACKET;
+            }
+            mcps_data_ie_list_t ie_list;
+            ie_list.payloadIeList = buf->payloadsIePtr;
+            ie_list.payloadIeListLength = buf->payloadsIeLength;
+            ie_list.headerIeList = buf->headerIePtr;
+            ie_list.headerIeListLength = buf->headerIeLength;
+            mac->data_ind_ext_cb(mac, data_ind, &ie_list);
+
+        } else {
+            mac->data_ind_cb(mac, data_ind);
+        }
         retval = 0;
     }
 
@@ -586,6 +806,10 @@ static void mac_lib_res_no_data_to_req(mac_pre_parsed_frame_t *buffer, protocol_
     buf->fcf_dsn.intraPan = true;
     buf->fcf_dsn.ackRequested = true;
     buf->mac_header_length_with_security = 3;
+    //Check PanID presents at header
+    buf->fcf_dsn.DstPanPresents = mac_dst_panid_present(&buf->fcf_dsn);
+    buf->fcf_dsn.SrcPanPresents = mac_src_panid_present(&buf->fcf_dsn);
+
     if (buffer->fcf_dsn.securityEnabled) {
         buf->fcf_dsn.frameVersion = MAC_FRAME_VERSION_2006;
         buf->aux_header.securityLevel = rf_mac_setup->mac_auto_request.SecurityLevel;
@@ -649,7 +873,6 @@ static int8_t mac_command_sap_rx_handler(mac_pre_parsed_frame_t *buf, protocol_i
         case MAC_DATA_REQ:
             //Here 2 check
             if (mac_indirect_data_req_handle(buf, rf_mac_setup) == 0) {
-                tr_debug("Trig Dummy packet");
                 mac_lib_res_no_data_to_req(buf, rf_mac_setup);
             }
             retval = 0;
@@ -695,11 +918,16 @@ static void mac_data_interface_parse_beacon(mac_pre_parsed_frame_t *buf, protoco
         return;
     }
 
+    if (!mac_payload_information_elements_parse(buf)) {
+        tr_debug("Drop by Paylod IE");
+        return;
+    }
+
     /*Add received bytes in statistics*/
     tr_debug("mac_parse_beacon");
 
-    ptr = mcps_mac_payload_pointer_get(buf);
-    len = mcps_payload_length_from_received_frame(buf);
+    ptr = buf->macPayloadPtr;
+    len = buf->mac_payload_length;
     SuperframeSpec[0] = *ptr++;
     SuperframeSpec[1] = *ptr++;
     gts_spec.description_count = (*ptr & 7);
@@ -836,10 +1064,10 @@ void mac_mcps_trig_buffer_from_queue(protocol_interface_rf_mac_setup_s *rf_mac_s
         buffer = mcps_sap_pd_req_queue_read(rf_mac_setup, is_bc_queue, false);
 
         if (buffer) {
-            rf_mac_setup->active_pd_data_request = buffer;
+            mac_data_request_init(rf_mac_setup, buffer);
             if (mcps_pd_data_request(rf_mac_setup, buffer) != 0) {
                 rf_mac_setup->active_pd_data_request = NULL;
-                mcps_data_confirm_handle(rf_mac_setup, buffer);
+                mcps_data_confirm_handle(rf_mac_setup, buffer, NULL);
             } else {
                 return;
             }
@@ -849,19 +1077,89 @@ void mac_mcps_trig_buffer_from_queue(protocol_interface_rf_mac_setup_s *rf_mac_s
     }
 }
 
+
+static int8_t mac_ack_sap_rx_handler(mac_pre_parsed_frame_t *buf, protocol_interface_rf_mac_setup_s *rf_mac_setup)
+{
+    //allocate Data ind primitiv and parse packet to that
+    mlme_security_t key;
+    uint8_t SrcAddr[8];         /**< Source address */
+    memset(SrcAddr, 0, 8);
+    memset(&key, 0, sizeof(mlme_security_t));
+    mac_header_get_src_address(&buf->fcf_dsn, mac_header_message_start_pointer(buf), SrcAddr);
+    /* Parse security part */
+    mac_header_security_components_read(buf, &key);
+
+    buf->neigh_info = mac_sec_mib_device_description_get(rf_mac_setup, SrcAddr, buf->fcf_dsn.SrcAddrMode);
+    if (buf->fcf_dsn.securityEnabled) {
+        uint8_t status = mac_data_interface_decrypt_packet(buf, &key);
+        if (status != MLME_SUCCESS) {
+            tr_debug("ACK Decrypt fail");
+            return -1;
+        }
+    }
+
+    if (buf->mac_payload_length && !mac_payload_information_elements_parse(buf)) {
+        tr_debug("Drop ACK by Paylod IE");
+        return -1;
+    }
+
+    return 0;
+}
+
 static void mac_pd_data_confirm_handle(protocol_interface_rf_mac_setup_s *rf_mac_setup) {
 
-    if (!rf_mac_setup->active_pd_data_request) {
-        tr_debug("No pending TX process anymore");
-    } else {
+    if (rf_mac_setup->active_pd_data_request) {
         mac_pre_build_frame_t *buffer = rf_mac_setup->active_pd_data_request;
-        rf_mac_setup->active_pd_data_request = NULL;
-        mcps_data_confirm_handle(rf_mac_setup, buffer);
+        if (mac_data_request_confirmation_finnish(rf_mac_setup, buffer) ) {
+            rf_mac_setup->active_pd_data_request = NULL;
+            if (buffer->asynch_request && rf_mac_setup->fhss_api) {
+                // Must return to scheduled channel after asynch process by calling TX done
+                rf_mac_setup->fhss_api->data_tx_done(rf_mac_setup->fhss_api, false, true, buffer->msduHandle);
+            }
+            mcps_data_confirm_handle(rf_mac_setup, buffer, NULL);
+        } else {
+            if (mcps_pd_data_request(rf_mac_setup, buffer) != 0) {
+                rf_mac_setup->active_pd_data_request = NULL;
+                mcps_data_confirm_handle(rf_mac_setup, buffer, NULL);
+            } else {
+                return;
+            }
+        }
     }
 
     mac_mcps_trig_buffer_from_queue(rf_mac_setup);
 }
 
+
+static void mac_pd_data_ack_handler(mac_pre_parsed_frame_t *buf) {
+
+    protocol_interface_rf_mac_setup_s *rf_mac_setup = buf->mac_class_ptr;
+
+    if (!rf_mac_setup->active_pd_data_request) {
+        mcps_sap_pre_parsed_frame_buffer_free(buf);
+    } else {
+        mac_pre_build_frame_t *buffer = rf_mac_setup->active_pd_data_request;
+
+        if (mac_ack_sap_rx_handler(buf, rf_mac_setup)) {
+            //Do not forward ACK payload but Accept ACK
+            mcps_sap_pre_parsed_frame_buffer_free(buf);
+            buf = NULL;
+        }
+
+        rf_mac_setup->active_pd_data_request = NULL;
+        if (buffer->asynch_request && rf_mac_setup->fhss_api) {
+            // Must return to scheduled channel after asynch process by calling TX done
+            rf_mac_setup->fhss_api->data_tx_done(rf_mac_setup->fhss_api, false, true, buffer->msduHandle);
+        }
+        mcps_data_confirm_handle(rf_mac_setup, buffer, buf);
+        mcps_sap_pre_parsed_frame_buffer_free(buf);
+
+    }
+
+    mac_mcps_trig_buffer_from_queue(rf_mac_setup);
+}
+
+
 static void mac_mcps_sap_data_tasklet(arm_event_s *event)
 {
     uint8_t event_type = event->event_type;
@@ -879,6 +1177,10 @@ static void mac_mcps_sap_data_tasklet(arm_event_s *event)
             mac_pd_data_confirm_handle((protocol_interface_rf_mac_setup_s*)event->data_ptr);
             break;
 
+        case MCPS_SAP_DATA_ACK_CNF_EVENT:
+            mac_pd_data_ack_handler((mac_pre_parsed_frame_t*)event->data_ptr);
+            break;
+
         case MAC_MLME_EVENT_HANDLER:
             mac_mlme_event_cb(event->data_ptr);
             break;
@@ -944,7 +1246,7 @@ void mcps_sap_prebuild_frame_buffer_free(mac_pre_build_frame_t *buffer)
 
 }
 
-static ccm_globals_t * mac_frame_security_parameters_init(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer)
+static bool mac_frame_security_parameters_init(ccm_globals_t *ccm_ptr, protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer)
 {
     /* Encrypt the packet payload if AES encyption bit is set */
     mlme_security_t key_source;
@@ -956,7 +1258,7 @@ static ccm_globals_t * mac_frame_security_parameters_init(protocol_interface_rf_
 
     if (!key_description ) {
         buffer->status = MLME_UNAVAILABLE_KEY;
-        return NULL;
+        return false;
 
     }
     mlme_device_descriptor_t *device_description;
@@ -967,7 +1269,7 @@ static ccm_globals_t * mac_frame_security_parameters_init(protocol_interface_rf_
         if (!device_description) {
 
             buffer->status = MLME_UNAVAILABLE_KEY;
-            return NULL;
+            return false;
         }
         nonce_ext_64_ptr = device_description->ExtAddress;
     } else {
@@ -976,8 +1278,13 @@ static ccm_globals_t * mac_frame_security_parameters_init(protocol_interface_rf_
             device_description =  mac_sec_mib_device_description_get(rf_ptr, buffer->DstAddr, buffer->fcf_dsn.DstAddrMode);
             if (!device_description) {
 
-                buffer->status = MLME_UNAVAILABLE_KEY;
-                return NULL;
+                if (rf_ptr->mac_security_bypass_unknow_device && (buffer->fcf_dsn.SrcAddrMode == MAC_ADDR_MODE_64_BIT
+                        && buffer->aux_header.securityLevel > AES_SECURITY_LEVEL_ENC)) {
+
+                } else {
+                    buffer->status = MLME_UNAVAILABLE_KEY;
+                    return false;
+                }
             }
         }
         nonce_ext_64_ptr = rf_ptr->mac64;
@@ -985,26 +1292,20 @@ static ccm_globals_t * mac_frame_security_parameters_init(protocol_interface_rf_
 
     uint8_t *key_ptr = key_description->Key;
 
-    //Remember to update security counter here!
-    buffer->aux_header.frameCounter = rf_ptr->security_frame_counter;
-
     //Check If frame counter overflow is coming
     if (buffer->aux_header.frameCounter == 0xffffffff) {
         buffer->status = MLME_COUNTER_ERROR;
-        return NULL;
+        return false;
     }
 
-    ccm_globals_t *ccm_ptr = ccm_sec_init(buffer->aux_header.securityLevel, key_ptr, AES_CCM_ENCRYPT, 2);
-    if (!ccm_ptr) {
+    if (!ccm_sec_init(ccm_ptr, buffer->aux_header.securityLevel, key_ptr, AES_CCM_ENCRYPT, 2)) {
         buffer->status = MLME_SECURITY_FAIL;
-        return NULL;
+        return false;
     }
 
     mac_security_interface_aux_ccm_nonce_set(ccm_ptr->exp_nonce, nonce_ext_64_ptr, buffer->aux_header.frameCounter,
                                              buffer->aux_header.securityLevel);
-    //Increment security counter
-    rf_ptr->security_frame_counter++;
-    return ccm_ptr;
+    return true;
 
 }
 
@@ -1113,7 +1414,7 @@ static void mac_data_interface_internal_tx_confirm_handle(protocol_interface_rf_
 
 }
 
-static void mcps_data_confirm_handle(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer) {
+static void mcps_data_confirm_handle(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer, mac_pre_parsed_frame_t *ack_buf) {
 
     sw_mac_stats_update(rf_ptr, STAT_MAC_TX_CCA_ATT, rf_ptr->mac_tx_status.cca_cnt);
     sw_mac_stats_update(rf_ptr, STAT_MAC_TX_RETRY, rf_ptr->mac_tx_status.retry);
@@ -1123,7 +1424,6 @@ static void mcps_data_confirm_handle(protocol_interface_rf_mac_setup_s *rf_ptr,
         if ((rf_ptr->mac_tx_result == MAC_TX_FAIL) || (rf_ptr->mac_tx_result == MAC_CCA_FAIL)) {
             if (rf_ptr->fhss_api->data_tx_fail(rf_ptr->fhss_api, buffer->msduHandle, mac_convert_frame_type_to_fhss(buffer->fcf_dsn.frametype)) == true) {
                 mcps_sap_pd_req_queue_write(rf_ptr, buffer);
-                tr_debug("Channel retry, TX result: %u, handle: %u", rf_ptr->mac_tx_result, buffer->msduHandle);
                 return;
             }
         }
@@ -1133,12 +1433,16 @@ static void mcps_data_confirm_handle(protocol_interface_rf_mac_setup_s *rf_ptr,
     mac_common_data_confirmation_handle(rf_ptr, buffer);
     confirm.msduHandle = buffer->msduHandle;
     confirm.status = buffer->status;
-    confirm.timestamp = 0;
+    if (ack_buf) {
+        confirm.timestamp = ack_buf->timestamp;
+    } else {
+        confirm.timestamp = 0;
+    }
 
     if (buffer->upper_layer_request) {
         //Check tunnel
         mcps_sap_prebuild_frame_buffer_free(buffer);
-        mcps_data_confirm_cb(rf_ptr, &confirm);
+        mcps_data_confirm_cb(rf_ptr, &confirm, ack_buf);
     } else {
         mac_data_interface_internal_tx_confirm_handle(rf_ptr, buffer);
     }
@@ -1163,127 +1467,395 @@ static void mac_security_authentication_data_params_set(ccm_globals_t *ccm_ptr,
     }
 }
 
-static uint8_t *mac_security_interface_aux_security_header_write(uint8_t *ptr, const mac_aux_security_header_t *auxHeader)
+static uint32_t mcps_calculate_tx_time(protocol_interface_rf_mac_setup_s *rf_mac_setup, uint32_t time_to_tx)
 {
-    uint8_t auxBaseHeader;
-    auxBaseHeader = auxHeader->securityLevel;
-    auxBaseHeader |= (auxHeader->KeyIdMode << 3);
-    *ptr++ = auxBaseHeader;
-    ptr = common_write_32_bit_inverse(auxHeader->frameCounter, ptr);
-
-    switch (auxHeader->KeyIdMode) {
-        case MAC_KEY_ID_MODE_SRC8_IDX:
-            memcpy(ptr, auxHeader->Keysource, 8);
-            ptr += 8;
-            *ptr++ = auxHeader->KeyIndex;
-            break;
-        case MAC_KEY_ID_MODE_SRC4_IDX:
-            memcpy(ptr, auxHeader->Keysource, 4);
-            ptr += 4;
-            *ptr++ = auxHeader->KeyIndex;
-            break;
-        case MAC_KEY_ID_MODE_IDX:
-            *ptr++ = auxHeader->KeyIndex;
-            break;
-        default:
-            break;
+    // Max. time to TX is 65ms
+    if (time_to_tx > 65000) {
+        time_to_tx = 65000;
+    }
+    return mac_mcps_sap_get_phy_timestamp(rf_mac_setup) + time_to_tx;
+}
+
+static void mcps_generic_sequence_number_allocate(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer)
+{
+    if (buffer->fcf_dsn.frameVersion < MAC_FRAME_VERSION_2015 || (buffer->fcf_dsn.frameVersion ==  MAC_FRAME_VERSION_2015 &&  !buffer->fcf_dsn.sequenceNumberSuppress)) {
+        /* Allocate SQN */
+        switch (buffer->fcf_dsn.frametype) {
+            case MAC_FRAME_CMD:
+            case MAC_FRAME_DATA:
+                buffer->fcf_dsn.DSN = mac_mlme_set_new_sqn(rf_ptr);
+                break;
+            case MAC_FRAME_BEACON:
+                buffer->fcf_dsn.DSN = mac_mlme_set_new_beacon_sqn(rf_ptr);
+                break;
+            default:
+                break;
+        }
     }
-    return ptr;
 }
 
 
-static int8_t mcps_generic_packet_build(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer) {
+static uint32_t mcps_generic_backoff_calc(protocol_interface_rf_mac_setup_s *rf_ptr)
+{
+    uint32_t random_period = mac_csma_backoff_get(rf_ptr);
+    if (rf_ptr->rf_csma_extension_supported) {
+        return mcps_calculate_tx_time(rf_ptr, random_period);
+    }
+    return random_period;
+}
+
+static int8_t mcps_generic_packet_build(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer)
+{
     phy_device_driver_s *dev_driver = rf_ptr->dev_driver->phy_driver;
     dev_driver_tx_buffer_s *tx_buf = &rf_ptr->dev_driver_tx_buffer;
-    uint8_t open_payload = 0;
-    uint8_t *ptr;
-    ccm_globals_t *ccm_ptr = NULL;
+
+    ccm_globals_t ccm_ptr;
 
     if (buffer->mac_header_length_with_security == 0) {
         rf_ptr->mac_tx_status.length = buffer->mac_payload_length;
-        ptr = tx_buf->buf;
+        uint8_t *ptr = tx_buf->buf;
         if (dev_driver->phy_header_length) {
             ptr += dev_driver->phy_header_length;
         }
         tx_buf->len = buffer->mac_payload_length;
 
         memcpy(ptr, buffer->mac_payload, buffer->mac_payload_length );
+        buffer->tx_time = mcps_generic_backoff_calc(rf_ptr);
         return 0;
     }
 
-    if (buffer->fcf_dsn.frametype != FC_DATA_FRAME) {
-        open_payload = 1;
-    }
+    //This will prepare MHR length with Header IE
+    mac_header_information_elements_preparation(buffer);
 
-    if (buffer->fcf_dsn.frametype == MAC_FRAME_BEACON) {
-        buffer->fcf_dsn.DSN = mac_mlme_set_new_beacon_sqn(rf_ptr);
-    } else {
-        buffer->fcf_dsn.DSN = mac_mlme_set_new_sqn(rf_ptr);
-    }
+    mcps_generic_sequence_number_allocate(rf_ptr, buffer);
 
     if (buffer->fcf_dsn.securityEnabled) {
-        ccm_ptr = mac_frame_security_parameters_init(rf_ptr, buffer);
-        if ( !ccm_ptr) {
+        //Remember to update security counter here!
+        buffer->aux_header.frameCounter = mac_mlme_framecounter_get(rf_ptr);
+        if (!mac_frame_security_parameters_init(&ccm_ptr, rf_ptr, buffer)) {
             return -2;
         }
+        //Increment security counter
+        mac_mlme_framecounter_increment(rf_ptr);
+
     }
 
-    if (buffer->mac_payload_length > MAC_IEEE_802_15_4_MAX_MAC_SAFE_PAYLOAD_SIZE &&
+    //Calculate Payload length here with IE extension
+    uint16_t frame_length = mac_buffer_total_payload_length(buffer);
+    //Storage Mac Payload length here
+    uint16_t mac_payload_length = frame_length;
+
+    if (mac_payload_length > MAC_IEEE_802_15_4_MAX_MAC_SAFE_PAYLOAD_SIZE &&
             dev_driver->phy_MTU == MAC_IEEE_802_15_4_MAX_PHY_PACKET_SIZE) {
         /* IEEE 802.15.4-2003 only allowed unsecured payloads up to 102 bytes
         * (always leaving room for maximum MAC overhead).
         * IEEE 802.15.4-2006 allows bigger if MAC header is small enough, but
         * we have to set the version field.
         */
-        buffer->fcf_dsn.frameVersion = MAC_FRAME_VERSION_2006;
+        if (buffer->fcf_dsn.frameVersion < MAC_FRAME_VERSION_2006) {
+            buffer->fcf_dsn.frameVersion = MAC_FRAME_VERSION_2006;
+        }
     }
-    //Frame length
-    uint16_t frame_length = buffer->mac_header_length_with_security + buffer->mac_payload_length + buffer->security_mic_len;
+
+    if (rf_ptr->mac_ack_tx_active) {
+        if (buffer->fcf_dsn.securityEnabled) {
+            ccm_free(&ccm_ptr);
+        }
+        return 0;
+    }
+
+    //Add MHR length to total length
+    frame_length += buffer->mac_header_length_with_security + buffer->security_mic_len;
     if ((frame_length) > dev_driver->phy_MTU - 2) {
-        tr_debug("Too Long %u, %u pa %u header %u mic",frame_length, buffer->mac_payload_length, buffer->mac_header_length_with_security,  buffer->security_mic_len);
+        tr_debug("Too Long %u, %u pa %u header %u mic %u",frame_length, mac_payload_length, buffer->mac_header_length_with_security,  buffer->security_mic_len, dev_driver->phy_MTU);
         buffer->status = MLME_FRAME_TOO_LONG;
+        //decrement security counter
+        mac_mlme_framecounter_decrement(rf_ptr);
         return -1;
     }
 
     rf_ptr->mac_tx_status.length = frame_length;
-    ptr = tx_buf->buf;
+    uint8_t *ptr = tx_buf->buf;
     if (dev_driver->phy_header_length) {
         ptr += dev_driver->phy_header_length;
     }
-    tx_buf->len = frame_length;
 
-    ptr = mcps_generic_header_write(ptr, buffer);
+    tx_buf->len = frame_length;
+    uint8_t *mhr_start = ptr;
+    buffer->tx_time = mcps_generic_backoff_calc(rf_ptr);
+
+    ptr = mac_generic_packet_write(rf_ptr, ptr, buffer);
+
 
     if (buffer->fcf_dsn.securityEnabled) {
-        ptr = mac_security_interface_aux_security_header_write(ptr, &buffer->aux_header);
-        ccm_ptr->data_len = (buffer->mac_payload_length - open_payload);
-            ccm_ptr->data_ptr = ptr + open_payload;
-        mac_security_data_params_set(ccm_ptr, (ptr + open_payload), (buffer->mac_payload_length - open_payload));
-        mac_security_authentication_data_params_set(ccm_ptr, (ptr  - buffer->mac_header_length_with_security), (buffer->mac_header_length_with_security +open_payload));
-        //Copy Payload
-        memcpy(ptr, buffer->mac_payload, buffer->mac_payload_length);
-        ccm_process_run(ccm_ptr);
-    } else {
-        //Copy Payload
-        memcpy(ptr, buffer->mac_payload, buffer->mac_payload_length );
+        uint8_t open_payload = 0;
+        if (buffer->fcf_dsn.frametype == MAC_FRAME_CMD) {
+            open_payload = 1;
+        }
+        mac_security_data_params_set(&ccm_ptr, (mhr_start + (buffer->mac_header_length_with_security + open_payload)), (mac_payload_length - open_payload));
+        mac_security_authentication_data_params_set(&ccm_ptr, mhr_start, (buffer->mac_header_length_with_security +open_payload));
+        ccm_process_run(&ccm_ptr);
     }
+
+    return 0;
+}
+
+
+int8_t mcps_generic_ack_build(protocol_interface_rf_mac_setup_s *rf_ptr, const mac_fcf_sequence_t *fcf, const uint8_t *data_ptr, const mcps_ack_data_payload_t *ack_payload, uint32_t rx_time)
+{
+    phy_device_driver_s *dev_driver = rf_ptr->dev_driver->phy_driver;
+    dev_driver_tx_buffer_s *tx_buf = &rf_ptr->dev_driver_tx_buffer;
+
+    ccm_globals_t ccm_ptr;
+
+    mac_pre_build_frame_t pd_act_buf;
+    mac_pre_build_frame_t *buffer = &pd_act_buf;
+    memset(buffer, 0, sizeof(mac_pre_build_frame_t));
+    buffer->fcf_dsn.frametype = FC_ACK_FRAME;
+    buffer->fcf_dsn.frameVersion = fcf->frameVersion;
+    buffer->fcf_dsn.framePending = rf_ptr->mac_frame_pending;
+    buffer->fcf_dsn.DSN = fcf->DSN;
+    buffer->fcf_dsn.sequenceNumberSuppress = fcf->sequenceNumberSuppress;
+    buffer->fcf_dsn.DstPanPresents = fcf->DstPanPresents;
+    buffer->fcf_dsn.SrcAddrMode = fcf->DstAddrMode;
+    buffer->fcf_dsn.SrcPanPresents = fcf->SrcPanPresents;
+    buffer->fcf_dsn.DstAddrMode = fcf->SrcAddrMode;
+    if (buffer->fcf_dsn.sequenceNumberSuppress) {
+        buffer->mac_header_length_with_security = 2;
+    } else {
+        buffer->mac_header_length_with_security = 3;
+    }
+
+    buffer->mac_header_length_with_security += mac_header_address_length(&buffer->fcf_dsn);
+
+    buffer->DstPANId = mac_header_get_src_panid(fcf, data_ptr);
+    buffer->SrcPANId = mac_header_get_dst_panid(fcf, data_ptr);
+
+    mac_header_get_src_address(fcf, data_ptr, buffer->DstAddr);
+    mac_header_get_dst_address(fcf, data_ptr, buffer->SrcAddr);
+
+    //Security
+    buffer->fcf_dsn.securityEnabled = fcf->securityEnabled;
+    if (buffer->fcf_dsn.securityEnabled ) {
+        //Read Security AUX headers
+        const uint8_t *ptr = data_ptr;
+        ptr += mac_header_off_set_to_aux_header(fcf);
+        //Start parsing AUX header
+        mlme_security_t aux_parse;
+        mac_header_security_aux_header_parse(ptr, &aux_parse);
+        buffer->aux_header.KeyIdMode = aux_parse.KeyIdMode;
+        buffer->aux_header.KeyIndex = aux_parse.KeyIndex;
+        buffer->aux_header.securityLevel = aux_parse.SecurityLevel;
+        memcpy(buffer->aux_header.Keysource, aux_parse.Keysource, 8);
+
+        buffer->security_mic_len = mac_security_mic_length_get(buffer->aux_header.securityLevel);
+        buffer->fcf_dsn.frameVersion = MAC_FRAME_VERSION_2006;
+        buffer->mac_header_length_with_security += mac_header_security_aux_header_length(buffer->aux_header.securityLevel, buffer->aux_header.KeyIdMode);
+
+    }
+
+
+    //TODO Request Application data to ACK
+    uint16_t ie_header_length = 0;
+    uint16_t ie_payload_length = 0;
+
+    if (!mac_ie_vector_length_validate(ack_payload->ie_elements.headerIeVectorList, ack_payload->ie_elements.headerIovLength, &ie_header_length)) {
+        return -1;
+    }
+
+    if (!mac_ie_vector_length_validate(ack_payload->ie_elements.payloadIeVectorList, ack_payload->ie_elements.payloadIovLength, &ie_payload_length)) {
+        return -1;
+    }
+
+    buffer->ie_elements.headerIeVectorList = ack_payload->ie_elements.headerIeVectorList;
+    buffer->ie_elements.headerIovLength = ack_payload->ie_elements.headerIovLength;
+    buffer->ie_elements.payloadIeVectorList = ack_payload->ie_elements.payloadIeVectorList;
+    buffer->ie_elements.payloadIovLength = ack_payload->ie_elements.payloadIovLength;
+    buffer->headerIeLength = ie_header_length;
+    buffer->payloadsIeLength = ie_payload_length;
+    buffer->mac_payload = ack_payload->payloadPtr;
+    buffer->mac_payload_length = ack_payload->payloadLength;
+
+    //This will prepare MHR length with Header IE
+    mac_header_information_elements_preparation(buffer);
+
+    if (buffer->fcf_dsn.securityEnabled) {
+        //Remember to update security counter here!
+        buffer->aux_header.frameCounter = mac_mlme_framecounter_get(rf_ptr);
+        if ( !mac_frame_security_parameters_init(&ccm_ptr, rf_ptr, buffer)) {
+            return -2;
+        }
+        //Increment security counter
+        mac_mlme_framecounter_increment(rf_ptr);
+    }
+
+    //Calculate Payload length here with IE extension
+    uint16_t frame_length = mac_buffer_total_payload_length(buffer);
+    //Storage Mac Payload length here
+    uint16_t mac_payload_length = frame_length;
+
+    //Add MHR length to total length
+    frame_length += buffer->mac_header_length_with_security + buffer->security_mic_len;
+    if ((frame_length) > dev_driver->phy_MTU - 2) {
+        buffer->status = MLME_FRAME_TOO_LONG;
+
+        if (buffer->fcf_dsn.securityEnabled) {
+            //decrement security counter
+            mac_mlme_framecounter_decrement(rf_ptr);
+            ccm_free(&ccm_ptr);
+        }
+        return -1;
+    }
+
+    rf_ptr->mac_tx_status.length = frame_length;
+    uint8_t *ptr = tx_buf->buf;
+    if (dev_driver->phy_header_length) {
+        ptr += dev_driver->phy_header_length;
+    }
+
+    tx_buf->len = frame_length;
+    uint8_t *mhr_start = ptr;
+    buffer->tx_time = rx_time + 196; //Send 196 us later
+
+    ptr = mac_generic_packet_write(rf_ptr, ptr, buffer);
+
+
+    if (buffer->fcf_dsn.securityEnabled) {
+        mac_security_data_params_set(&ccm_ptr, (mhr_start + (buffer->mac_header_length_with_security )), (mac_payload_length ));
+        mac_security_authentication_data_params_set(&ccm_ptr, mhr_start, (buffer->mac_header_length_with_security));
+        ccm_process_run(&ccm_ptr);
+    }
+    //Disable TX Time
+    phy_csma_params_t csma_params;
+    csma_params.backoff_time = 0;
+    csma_params.cca_enabled = false;
+    rf_ptr->dev_driver->phy_driver->extension(PHY_EXTENSION_SET_CSMA_PARAMETERS, (uint8_t*) &csma_params);
+    if (rf_ptr->active_pd_data_request) {
+        mac_pd_sap_set_phy_tx_time(rf_ptr, 0, false);
+    }
+
+    return mcps_pd_data_cca_trig(rf_ptr, buffer);
+}
+
+
+static int8_t mcps_generic_packet_rebuild(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer)
+{
+    phy_device_driver_s *dev_driver = rf_ptr->dev_driver->phy_driver;
+    dev_driver_tx_buffer_s *tx_buf = &rf_ptr->dev_driver_tx_buffer;
+    ccm_globals_t ccm_ptr;
+
+    if (!buffer) {
+        return -1;
+    }
+
+    if (buffer->mac_header_length_with_security == 0) {
+        rf_ptr->mac_tx_status.length = buffer->mac_payload_length;
+        uint8_t *ptr = tx_buf->buf;
+        if (dev_driver->phy_header_length) {
+            ptr += dev_driver->phy_header_length;
+        }
+        tx_buf->len = buffer->mac_payload_length;
+
+        memcpy(ptr, buffer->mac_payload, buffer->mac_payload_length );
+        buffer->tx_time = mcps_generic_backoff_calc(rf_ptr);
+        return 0;
+    }
+
+    if (buffer->fcf_dsn.securityEnabled) {
+        if ( !mac_frame_security_parameters_init(&ccm_ptr, rf_ptr, buffer)) {
+            return -2;
+        }
+    }
+
+    //Calculate Payload length here with IE extension
+    uint16_t frame_length = mac_buffer_total_payload_length(buffer);
+    //Storage Mac Payload length here
+    uint16_t mac_payload_length = frame_length;
+
+    //Add MHR length to total length
+    frame_length += buffer->mac_header_length_with_security + buffer->security_mic_len;
+
+    rf_ptr->mac_tx_status.length = frame_length;
+    uint8_t *ptr = tx_buf->buf;
+    if (dev_driver->phy_header_length) {
+        ptr += dev_driver->phy_header_length;
+    }
+
+    tx_buf->len = frame_length;
+    uint8_t *mhr_start = ptr;
+
+    buffer->tx_time = mcps_generic_backoff_calc(rf_ptr);
+
+    ptr = mac_generic_packet_write(rf_ptr, ptr, buffer);
+
+    if (buffer->fcf_dsn.securityEnabled) {
+        uint8_t open_payload = 0;
+        if (buffer->fcf_dsn.frametype == MAC_FRAME_CMD) {
+            open_payload = 1;
+        }
+        mac_security_data_params_set(&ccm_ptr, (mhr_start + (buffer->mac_header_length_with_security + open_payload)), (mac_payload_length - open_payload));
+        mac_security_authentication_data_params_set(&ccm_ptr, mhr_start, (buffer->mac_header_length_with_security +open_payload));
+        ccm_process_run(&ccm_ptr);
+    }
+
+    return 0;
+}
+
+static int8_t mcps_pd_data_cca_trig(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer)
+{
+    mac_mlme_mac_radio_enable(rf_ptr);
+    rf_ptr->macTxProcessActive = true;
+    if (rf_ptr->rf_csma_extension_supported) {
+        //Write TX time
+        bool cca_enabled;
+        if (buffer->fcf_dsn.frametype == MAC_FRAME_ACK) {
+            cca_enabled = false;
+        } else {
+            cca_enabled = true;
+        }
+        mac_pd_sap_set_phy_tx_time(rf_ptr, buffer->tx_time, cca_enabled);
+        if (mac_plme_cca_req(rf_ptr) != 0) {
+            rf_ptr->macTxProcessActive = false;
+            return -1;
+        }
+    } else {
+        timer_mac_start(rf_ptr, MAC_TIMER_CCA, (uint16_t)(buffer->tx_time / 50));
+    }
+
     return 0;
 }
 
 static int8_t mcps_pd_data_request(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer)
 {
     rf_ptr->macTxRequestAck = false;
+
+
     memset(&(rf_ptr->mac_tx_status), 0, sizeof(mac_tx_status_t));
+    rf_ptr->mac_cca_retry = 0;
+    rf_ptr->mac_tx_retry = 0;
+    mac_csma_param_init(rf_ptr);
     if (mcps_generic_packet_build(rf_ptr, buffer) != 0) {
         return -1;
     }
     rf_ptr->macTxRequestAck = buffer->fcf_dsn.ackRequested;
+    if (!rf_ptr->mac_ack_tx_active) {
+        return mcps_pd_data_cca_trig(rf_ptr, buffer);
+    } else {
+        return 0;
+    }
 
-    mac_mlme_mac_radio_enable(rf_ptr);
-    /* Trig MAC TX */
-    return mac_pd_sap_req(rf_ptr);
 }
 
+int8_t mcps_pd_data_rebuild(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_build_frame_t *buffer)
+{
+    if (mcps_generic_packet_rebuild(rf_ptr, buffer) != 0) {
+        return -1;
+    }
+
+    return mcps_pd_data_cca_trig(rf_ptr, buffer);
+}
+
+
 bool mac_is_ack_request_set(mac_pre_build_frame_t *buffer)
 {
     return buffer->fcf_dsn.ackRequested;
@@ -1310,15 +1882,16 @@ void mcps_sap_pd_req_queue_write(protocol_interface_rf_mac_setup_s *rf_mac_setup
         if ((rf_mac_setup->macBroadcastDisabled == true) && !mac_is_ack_request_set(buffer)) {
             goto push_to_queue;
         }
-        if (rf_mac_setup->fhss_api) {
+        if (rf_mac_setup->fhss_api && (buffer->asynch_request == false)) {
+            uint16_t frame_length = buffer->mac_payload_length + buffer->headerIeLength + buffer->payloadsIeLength;
             if (rf_mac_setup->fhss_api->check_tx_conditions(rf_mac_setup->fhss_api, !mac_is_ack_request_set(buffer),
-            	buffer->msduHandle, mac_convert_frame_type_to_fhss(buffer->fcf_dsn.frametype), buffer->mac_payload_length,
-            	rf_mac_setup->dev_driver->phy_driver->phy_header_length, rf_mac_setup->dev_driver->phy_driver->phy_tail_length) == false) {
+                buffer->msduHandle, mac_convert_frame_type_to_fhss(buffer->fcf_dsn.frametype), frame_length,
+                rf_mac_setup->dev_driver->phy_driver->phy_header_length, rf_mac_setup->dev_driver->phy_driver->phy_tail_length) == false) {
                 goto push_to_queue;
             }
         }
         //Start TX process immediately
-        rf_mac_setup->active_pd_data_request = buffer;
+        mac_data_request_init(rf_mac_setup, buffer);
         if (mcps_pd_data_request(rf_mac_setup, buffer) != 0) {
             rf_mac_setup->active_pd_data_request = NULL;
             mcps_data_conf_t confirm;
@@ -1328,7 +1901,7 @@ void mcps_sap_pd_req_queue_write(protocol_interface_rf_mac_setup_s *rf_mac_setup
             bool requested_from_up = buffer->upper_layer_request;
             mcps_sap_prebuild_frame_buffer_free(buffer);
             if (requested_from_up) {
-                mcps_data_confirm_cb(rf_mac_setup, &confirm);
+                mcps_data_confirm_cb(rf_mac_setup, &confirm, NULL);
             }
             //Call
         }
@@ -1342,7 +1915,7 @@ push_to_queue:
     bool use_bc_queue = false;
 
     // When FHSS is enabled, broadcast buffers are pushed to own queue
-    if (rf_mac_setup->fhss_api) {
+    if (rf_mac_setup->fhss_api && (buffer->asynch_request == false)) {
         if (rf_mac_setup->fhss_api->use_broadcast_queue(rf_mac_setup->fhss_api, !mac_is_ack_request_set(buffer),
                 mac_convert_frame_type_to_fhss(buffer->fcf_dsn.frametype)) == true) {
             cur = rf_mac_setup->pd_data_request_bc_queue_to_go;
@@ -1524,9 +2097,25 @@ void mcps_sap_pd_confirm(void *mac_ptr)
         .priority = ARM_LIB_HIGH_PRIORITY_EVENT,
     };
 
-    if (eventOS_event_send(&event) != 0) {
-        tr_error("mcps_sap_pd_confirm(): event send failed");
+    eventOS_event_send(&event);
+
+}
+
+void mcps_sap_pd_ack(void *ack_ptr)
+{
+    if (mac_tasklet_event_handler < 0  || !ack_ptr) {
+        return;
     }
+    arm_event_s event = {
+        .receiver = mac_tasklet_event_handler,
+        .sender = 0,
+        .event_id = 0,
+        .data_ptr = ack_ptr,
+        .event_type = MCPS_SAP_DATA_ACK_CNF_EVENT,
+        .priority = ARM_LIB_HIGH_PRIORITY_EVENT,
+    };
+
+    eventOS_event_send(&event);
 }
 
 void mcps_sap_trig_tx(void *mac_ptr)
@@ -1546,9 +2135,7 @@ void mcps_sap_trig_tx(void *mac_ptr)
         .priority = ARM_LIB_MED_PRIORITY_EVENT,
     };
 
-    if (eventOS_event_send(&event) != 0) {
-        tr_error("mcps_sap_trig_tx(): event send failed");
-    } else {
+    if (eventOS_event_send(&event) == 0) {
         mac_set_active_event(mac_ptr, MAC_SAP_TRIG_TX);
     }
 }
@@ -1571,9 +2158,7 @@ void mac_generic_event_trig(uint8_t event_type, void *mac_ptr, bool low_latency)
         .priority = priority,
     };
 
-    if (eventOS_event_send(&event) != 0) {
-        tr_error("mac_generic_event_trig(): event send failed");
-    }
+    eventOS_event_send(&event);
 }
 
 void mac_mcps_buffer_queue_free(protocol_interface_rf_mac_setup_s *rf_mac_setup) {
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.h b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.h
index 2da856fdf4..59d14cdbf7 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.h
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mcps_sap.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -32,6 +32,9 @@ struct protocol_interface_rf_mac_setup;
 struct mcps_data_req_s;
 struct arm_phy_sap_msg_s;
 struct mcps_purge_s;
+struct mcps_data_ie_list;
+struct mcps_data_req_ie_list;
+struct channel_list_s;
 
 /** Address types */
 typedef enum {
@@ -51,6 +54,7 @@ typedef enum {
 #define MAC_MCPS_INDIRECT_TIMER_CB 4
 #define MAC_MLME_SCAN_CONFIRM_HANDLER 5
 #define MAC_SAP_TRIG_TX 6
+#define MCPS_SAP_DATA_ACK_CNF_EVENT 7
 
 
 /**
@@ -71,6 +75,10 @@ typedef struct mac_fcf_sequence_s{
     bool framePending :1;
     bool ackRequested:1;
     bool intraPan:1;
+    bool sequenceNumberSuppress:1;
+    bool informationElementsPresets:1;
+    bool DstPanPresents:1;
+    bool SrcPanPresents:1;
     unsigned DstAddrMode:2; /*0x00 = no address 0x01 = reserved 0x02 = 16-bit short address 0x03 = 64-bit extended address */
     unsigned frameVersion:2;
     unsigned SrcAddrMode:2; /*0x00 = no address 0x01 = reserved 0x02 = 16-bit short address 0x03 = 64-bit extended address */
@@ -79,16 +87,23 @@ typedef struct mac_fcf_sequence_s{
 
 typedef struct mac_pre_parsed_frame_s{
     void *mac_class_ptr;
+    uint8_t *payloadsIePtr;
+    uint8_t *headerIePtr;
+    uint8_t *macPayloadPtr;
     mlme_device_descriptor_t *neigh_info;
     uint8_t LQI;
     int8_t dbm;
     mac_fcf_sequence_t fcf_dsn;
     uint16_t frameLength; //Encoded or open payload length
-    uint8_t mac_header_length;
+    uint16_t payloadsIeLength;
+    uint16_t headerIeLength;
+    uint16_t mac_header_length;
+    uint16_t header_ie_length;
     uint8_t security_aux_header_length;
     uint16_t mac_payload_length;
     uint32_t timestamp;
     bool ack_pendinfg_status;
+
     uint8_t buf[]; /*!< Trailing buffer data */
 } mac_pre_parsed_frame_t;
 
@@ -100,14 +115,22 @@ typedef struct mac_pre_build_frame{
     uint8_t SrcAddr[8];
     mac_aux_security_header_t aux_header;
     uint8_t mac_command_id; //For MLME
+    uint16_t payloadsIeLength;
+    uint16_t headerIeLength;
     uint16_t mac_payload_length;
+    uint16_t mac_header_length_with_security;
     uint8_t msduHandle;
     uint16_t buffer_ttl;
+    struct mcps_data_req_ie_list ie_elements;
+    struct channel_list_s asynch_channel_list;
     uint8_t *mac_payload;
     uint8_t status;
+    uint8_t asynch_channel;
+    uint32_t tx_time;
     bool upper_layer_request;
     bool mac_allocated_payload_ptr:1;
-    unsigned mac_header_length_with_security : 6; //Total max is 37
+    bool asynch_request:1;
+    bool message_builded:1;
     unsigned security_mic_len:5;    //Max possible lengths 0, 4, 8, 16 bytes
     unsigned priority:2;
     struct mac_pre_build_frame *next; //Pointer for queue purpose
@@ -159,10 +182,14 @@ int8_t mcps_sap_pd_ind(mac_pre_parsed_frame_t *buffer);
  */
 void mcps_sap_pd_confirm(void *mac_ptr);
 
+void mcps_sap_pd_ack(void *ack_ptr);
+
 int8_t mac_virtual_sap_data_cb(void *identifier, struct arm_phy_sap_msg_s *message);
 
 void mcps_sap_data_req_handler(struct protocol_interface_rf_mac_setup *rf_mac_setup , const struct mcps_data_req_s *data_req );
 
+void mcps_sap_data_req_handler_ext(struct protocol_interface_rf_mac_setup *rf_mac_setup , const struct mcps_data_req_s *data_req , const struct mcps_data_req_ie_list *ie_list, const channel_list_s *asynch_channel_list);
+
 void mac_mcps_trig_buffer_from_queue(struct protocol_interface_rf_mac_setup *rf_mac_setup);
 
 void mac_mcps_buffer_queue_free(struct protocol_interface_rf_mac_setup *rf_mac_setup);
@@ -175,4 +202,8 @@ void mcps_sap_trig_tx(void *mac_ptr);
 
 void mcps_sap_purge_reg_handler(struct protocol_interface_rf_mac_setup *rf_mac_setup, const struct mcps_purge_s *purge_req);
 
+int8_t mcps_pd_data_rebuild(struct protocol_interface_rf_mac_setup *rf_ptr, mac_pre_build_frame_t *buffer);
+
+int8_t mcps_generic_ack_build(struct protocol_interface_rf_mac_setup *rf_ptr, const mac_fcf_sequence_t *fcf, const uint8_t *data_ptr, const mcps_ack_data_payload_t *ack_payload, uint32_t rx_time);
+
 #endif /* MAC_IEEE802_15_4_MAC_MCPS_SAP_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mlme.c b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mlme.c
index 5643169154..bdf1c04969 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mlme.c
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mlme.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2017, Arm Limited and affiliates.
+ * Copyright (c) 2013-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -85,14 +85,14 @@ static void mac_mlme_energy_scan_start(protocol_interface_rf_mac_setup_s *rf_mac
     rf_mac_setup->macRfRadioTxActive = false;
 }
 
-static uint16_t mlme_scan_analyze_next_channel(protocol_interface_rf_mac_setup_s *rf_mac_setup)
+uint16_t mlme_scan_analyze_next_channel(channel_list_s *mac_channel_list)
 {
     uint8_t i, j = 0, k = 1;
     uint32_t mask = 1;
-    uint32_t *channel_mask = rf_mac_setup->mac_channel_list.channel_mask;
+    uint32_t *channel_mask = mac_channel_list->channel_mask;
 
-    if (rf_mac_setup->mac_channel_list.channel_page == CHANNEL_PAGE_9 ||
-        rf_mac_setup->mac_channel_list.channel_page == CHANNEL_PAGE_10) {
+    if (mac_channel_list->channel_page == CHANNEL_PAGE_9 ||
+            mac_channel_list->channel_page == CHANNEL_PAGE_10) {
         k=8;
     }
     for(j=0; j<k; j++)
@@ -228,6 +228,8 @@ uint8_t mac_mlme_beacon_req_tx(protocol_interface_rf_mac_setup_s *rf_ptr)
         buf->mac_payload = &buf->mac_command_id;
         buf->mac_payload_length = 1;
         buf->priority = MAC_PD_DATA_MEDIUM_PRIORITY;
+        buf->fcf_dsn.DstPanPresents = true;
+        buf->fcf_dsn.SrcPanPresents = false;
 
         tr_debug("BEA REQ tx");
         mcps_sap_pd_req_queue_write(rf_ptr, buf);
@@ -278,7 +280,7 @@ static void mac_mlme_scan_start(protocol_interface_rf_mac_setup_s *rf_mac_setup)
 {
     uint8_t channel;
 
-    channel = (uint8_t) mlme_scan_analyze_next_channel(rf_mac_setup);
+    channel = (uint8_t) mlme_scan_analyze_next_channel(&rf_mac_setup->mac_channel_list);
     mac_mlme_scan_init(channel, rf_mac_setup);
 }
 
@@ -508,6 +510,9 @@ static int8_t mac_mlme_boolean_set(protocol_interface_rf_mac_setup_s *rf_mac_set
                 rf_mac_setup->dev_driver->phy_driver->extension(PHY_EXTENSION_ACCEPT_ANY_BEACON, (uint8_t*)&value);
             }
             break;
+        case macAcceptByPassUnknowDevice:
+        rf_mac_setup->mac_security_bypass_unknow_device = value;
+        break;
 
         default:
             return -1;
@@ -607,7 +612,9 @@ static int8_t mac_mlme_32bit_set(protocol_interface_rf_mac_setup_s *rf_mac_setup
     (void) value;
     switch (attribute) {
         case macFrameCounter:
+            platform_enter_critical();
             rf_mac_setup->security_frame_counter = value;
+            platform_exit_critical();
             break;
 
         default:
@@ -718,6 +725,30 @@ int8_t mac_mlme_set_req(protocol_interface_rf_mac_setup_s *rf_mac_setup,const ml
     }
 }
 
+uint32_t mac_mlme_framecounter_get(struct protocol_interface_rf_mac_setup *rf_mac_setup)
+{
+    uint32_t value;
+    platform_enter_critical();
+    value = rf_mac_setup->security_frame_counter;
+    platform_exit_critical();
+    return value;
+}
+
+void mac_mlme_framecounter_increment(struct protocol_interface_rf_mac_setup *rf_mac_setup)
+{
+    platform_enter_critical();
+    rf_mac_setup->security_frame_counter++;
+    platform_exit_critical();
+}
+
+void mac_mlme_framecounter_decrement(struct protocol_interface_rf_mac_setup *rf_mac_setup)
+{
+    platform_enter_critical();
+    rf_mac_setup->security_frame_counter--;
+    platform_exit_critical();
+}
+
+
 int8_t mac_mlme_get_req(struct protocol_interface_rf_mac_setup *rf_mac_setup, mlme_get_conf_t *get_req)
 {
     if (!get_req || !rf_mac_setup ) {
@@ -740,7 +771,9 @@ int8_t mac_mlme_get_req(struct protocol_interface_rf_mac_setup *rf_mac_setup, ml
             break;
 
         case macFrameCounter:
+            platform_enter_critical();
             get_req->value_pointer = &rf_mac_setup->security_frame_counter;
+            platform_exit_critical();
             get_req->value_size = 4;
             break;
 
@@ -762,7 +795,7 @@ static void mlme_scan_operation(protocol_interface_rf_mac_setup_s *rf_mac_setup)
         resp->ResultListSize++;
     }
 
-    channel = mlme_scan_analyze_next_channel(rf_mac_setup);
+    channel = mlme_scan_analyze_next_channel(&rf_mac_setup->mac_channel_list);
     if (channel > 0xff || rf_mac_setup->mac_mlme_scan_resp->ResultListSize == MLME_MAC_RES_SIZE_MAX) {
         resp->status = MLME_SUCCESS;
         tr_debug("Scan Complete..Halt MAC");
@@ -993,6 +1026,7 @@ protocol_interface_rf_mac_setup_s * mac_mlme_data_base_allocate(uint8_t *mac64,
     entry->bc_timer_id = -1;
     entry->mac_interface_id = -1;
     entry->dev_driver = dev_driver;
+    entry->aUnitBackoffPeriod = 20; //This can be different in some Platform 20 comes from 12-symbol turnaround and 8 symbol CCA read
 
     if (mac_sec_mib_init(entry, storage_sizes) != 0) {
         mac_mlme_data_base_deallocate(entry);
@@ -1058,6 +1092,15 @@ protocol_interface_rf_mac_setup_s * mac_mlme_data_base_allocate(uint8_t *mac64,
     entry->mac_mcps_timer_event.receiver = entry->mac_tasklet_id;
     entry->mac_mcps_timer_event.sender = 0;
     entry->mac_mcps_timer_event.event_id = 0;
+    bool rf_support = false;
+    dev_driver->phy_driver->extension(PHY_EXTENSION_DYNAMIC_RF_SUPPORTED, (uint8_t*)&rf_support);
+    entry->rf_csma_extension_supported = rf_support;
+    if (entry->rf_csma_extension_supported) {
+        entry->dev_driver->phy_driver->extension(PHY_EXTENSION_GET_SYMBOLS_PER_SECOND, (uint8_t*) &entry->symbol_rate);
+        entry->symbol_time_us = 1000000 / entry->symbol_rate;
+        tr_debug("SW-MAC driver support rf extension %"PRIu32" symbol/seconds  %"PRIu32" us symbol time length", entry->symbol_rate, entry->symbol_time_us);
+    }
+
     //How many 10us ticks backoff period is for waiting 20symbols which is typically 10 bytes time
     entry->backoff_period_in_10us = mac_backoff_ticks_calc(dev_driver->phy_driver);
     return entry;
@@ -1255,7 +1298,7 @@ int8_t mac_mlme_virtual_confirmation_handle(int8_t driver_id, const uint8_t *dat
     if (!mac_setup) {
         return -1;
     }
-    mlme_primitive primitive = *data_ptr;
+    mlme_primitive primitive = (mlme_primitive) *data_ptr;
     if (primitive == MLME_SCAN) {
         mlme_scan_conf_t *resp = ns_dyn_mem_temporary_alloc(sizeof(mlme_scan_conf_t));
         if (!resp) {
@@ -1560,6 +1603,9 @@ void mac_mlme_poll_req(protocol_interface_rf_mac_setup_s *cur, const mlme_poll_t
         buf->fcf_dsn.SrcAddrMode = MAC_ADDR_MODE_16_BIT;
     }
     mac_frame_src_address_set_from_interface(buf->fcf_dsn.SrcAddrMode, cur, buf->SrcAddr);
+    //Check PanID presents at header
+    buf->fcf_dsn.DstPanPresents = mac_dst_panid_present(&buf->fcf_dsn);
+    buf->fcf_dsn.SrcPanPresents = mac_src_panid_present(&buf->fcf_dsn);
     buf->mac_header_length_with_security += mac_header_address_length(&buf->fcf_dsn);
     buf->priority = MAC_PD_DATA_MEDIUM_PRIORITY;
     mcps_sap_pd_req_queue_write(cur, buf);
@@ -1626,6 +1672,9 @@ int8_t mac_mlme_beacon_tx(protocol_interface_rf_mac_setup_s *rf_ptr)
     }
     buf->SrcPANId = mac_mlme_get_panid(rf_ptr);
     mac_frame_src_address_set_from_interface(buf->fcf_dsn.SrcAddrMode, rf_ptr, buf->SrcAddr);
+    buf->fcf_dsn.DstPanPresents = false;
+    buf->fcf_dsn.SrcPanPresents = true;
+
     uint8_t *ptr = buf->mac_payload;
 
     *ptr++ = 0xff;//Superframe disabled
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mlme.h b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mlme.h
index 70d13c4480..41f2960159 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mlme.h
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_mlme.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -39,6 +39,7 @@ struct mlme_scan_s;
 struct mlme_start_s;
 struct mlme_get_conf_s;
 struct mlme_set_s;
+struct channel_list_s;
 
 void mac_mlme_scan_confirmation_handle(struct protocol_interface_rf_mac_setup *rf_ptr);
 
@@ -64,6 +65,11 @@ int8_t mac_mlme_get_req(struct protocol_interface_rf_mac_setup *rf_mac_setup, st
 
 void mac_extended_mac_set(struct protocol_interface_rf_mac_setup *rf_mac_setup, const uint8_t *mac64);
 
+uint32_t mac_mlme_framecounter_get(struct protocol_interface_rf_mac_setup *rf_mac_setup);
+
+void mac_mlme_framecounter_increment(struct protocol_interface_rf_mac_setup *rf_mac_setup);
+void mac_mlme_framecounter_decrement(struct protocol_interface_rf_mac_setup *rf_mac_setup);
+
 /**
  * MLME Poll Request
  *
@@ -119,4 +125,6 @@ int8_t mac_mlme_beacon_tx(struct protocol_interface_rf_mac_setup *rf_ptr);
 uint8_t mac_mlme_beacon_req_tx(struct protocol_interface_rf_mac_setup *rf_ptr);
 int8_t mac_mlme_virtual_confirmation_handle(int8_t driver_id, const uint8_t *data_ptr, uint16_t length);
 
+uint16_t mlme_scan_analyze_next_channel(struct channel_list_s *mac_channel_list);
+
 #endif /* MAC_MLME_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_pd_sap.c b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_pd_sap.c
index 289fb15337..9a736779c1 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_pd_sap.c
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_pd_sap.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -18,7 +18,6 @@
 #include "ns_types.h"
 #include "eventOS_event.h"
 #include "string.h"
-#include "ns_trace.h"
 #include "nsdynmemLIB.h"
 #include "randLIB.h"
 #include "ccmLIB.h"
@@ -36,16 +35,13 @@
 #include "MAC/IEEE802_15_4/mac_mcps_sap.h"
 #include "MAC/rf_driver_storage.h"
 
-
-#define TRACE_GROUP "mPDs"
-
 /* Define TX Timeot Period */
 // Hardcoded to 1200ms. Should be changed dynamic: (FHSS) channel retries needs longer timeout
 #define NWKTX_TIMEOUT_PERIOD (1200*20)
 
 static int8_t mac_data_interface_tx_done_cb(protocol_interface_rf_mac_setup_s *rf_ptr, phy_link_tx_status_e status, uint8_t cca_retry, uint8_t tx_retry);
 
-static void mac_csma_param_init(protocol_interface_rf_mac_setup_s *rf_mac_setup)
+void mac_csma_param_init(protocol_interface_rf_mac_setup_s *rf_mac_setup)
 {
     rf_mac_setup->macCurrentBE = rf_mac_setup->macMinBE;
 }
@@ -79,6 +75,25 @@ static void mac_csma_backoff_start(protocol_interface_rf_mac_setup_s *rf_mac_set
     timer_mac_start(rf_mac_setup, MAC_TIMER_CCA, backoff_slots);
 }
 
+
+uint32_t mac_csma_backoff_get(protocol_interface_rf_mac_setup_s *rf_mac_setup)
+{
+    uint8_t backoff = mac_csma_random_backoff_get(rf_mac_setup);
+    uint32_t backoff_in_us;
+    //Multiple aUnitBackoffPeriod symbol time
+    if (rf_mac_setup->rf_csma_extension_supported) {
+        backoff_in_us = backoff * rf_mac_setup->aUnitBackoffPeriod * rf_mac_setup->symbol_time_us;
+    } else {
+        backoff_in_us = backoff * rf_mac_setup->backoff_period_in_10us * 10;
+    }
+
+    if (backoff_in_us == 0) {
+        backoff_in_us = 1;
+    }
+    return backoff_in_us;
+}
+
+
 /*
  * \file mac_pd_sap.c
  * \brief Add short description about this file!!!
@@ -117,23 +132,23 @@ static void mac_data_interface_tx_to_cb(protocol_interface_rf_mac_setup_s *rf_pt
     mac_tx_done_state_set(rf_ptr, MAC_TX_TIMEOUT);
 }
 
-static int8_t mac_plme_cca_req(protocol_interface_rf_mac_setup_s *rf_ptr) {
-    dev_driver_tx_buffer_s *tx_buf = &rf_ptr->dev_driver_tx_buffer;
-    phy_device_driver_s *dev_driver = rf_ptr->dev_driver->phy_driver;
+int8_t mac_plme_cca_req(protocol_interface_rf_mac_setup_s *rf_mac_setup) {
+    dev_driver_tx_buffer_s *tx_buf = &rf_mac_setup->dev_driver_tx_buffer;
+    phy_device_driver_s *dev_driver = rf_mac_setup->dev_driver->phy_driver;
 
 
     if (dev_driver->arm_net_virtual_tx_cb) {
         if (dev_driver->tx(tx_buf->buf, tx_buf->len, 1, PHY_LAYER_PAYLOAD) == 0) {
-            rf_ptr->macRfRadioTxActive = true;
-            timer_mac_start(rf_ptr, MAC_TX_TIMEOUT, NWKTX_TIMEOUT_PERIOD);  /*Start Timeout timer for virtual packet loss*/
+            rf_mac_setup->macRfRadioTxActive = true;
+            timer_mac_start(rf_mac_setup, MAC_TX_TIMEOUT, NWKTX_TIMEOUT_PERIOD);  /*Start Timeout timer for virtual packet loss*/
         } else {
-            mac_data_interface_tx_to_cb(rf_ptr);
+            mac_data_interface_tx_to_cb(rf_mac_setup);
         }
         return 0;
     }
 
     if (dev_driver->tx(tx_buf->buf, tx_buf->len, 1, PHY_LAYER_PAYLOAD) == 0) {
-        rf_ptr->macRfRadioTxActive = true;
+        rf_mac_setup->macRfRadioTxActive = true;
         return 0;
     }
     return -1;
@@ -160,12 +175,47 @@ int8_t mac_pd_sap_req(protocol_interface_rf_mac_setup_s *rf_mac_setup)
     return 0;
 }
 
+
+/**
+ * Set PHY TX time.
+ *
+ * \param rf_mac_setup pointer to MAC.
+ * \param tx_time TX timestamp to be set.
+ *
+ */
+void mac_pd_sap_set_phy_tx_time(protocol_interface_rf_mac_setup_s *rf_mac_setup, uint32_t tx_time, bool cca_enabled)
+{
+    // With TX time set to zero, PHY sends immediately
+    if (!tx_time) {
+        tx_time++;
+    }
+    phy_csma_params_t csma_params;
+    csma_params.backoff_time = tx_time;
+    csma_params.cca_enabled = cca_enabled;
+    rf_mac_setup->dev_driver->phy_driver->extension(PHY_EXTENSION_SET_CSMA_PARAMETERS, (uint8_t*) &csma_params);
+}
+
+/**
+ * Get PHY RX time.
+ *
+ * \param rf_mac_setup pointer to MAC
+ * \return Timestamp of last PHY reception
+ *
+ */
+static uint32_t mac_pd_sap_get_phy_rx_time(protocol_interface_rf_mac_setup_s *rf_mac_setup)
+{
+    uint8_t rx_time_buffer[4];
+    rf_mac_setup->dev_driver->phy_driver->extension(PHY_EXTENSION_READ_RX_TIME, rx_time_buffer);
+    return common_read_32_bit(rx_time_buffer);
+}
+
 /**
  * Run Mac data interface state Machine for mac timer.
  *
  */
 void mac_pd_sap_state_machine(protocol_interface_rf_mac_setup_s *rf_mac_setup)
 {
+
     if (rf_mac_setup->macUpState && rf_mac_setup->macTxProcessActive) {
 
         if (rf_mac_setup->mac_tx_result == MAC_TIMER_CCA) {
@@ -175,16 +225,25 @@ void mac_pd_sap_state_machine(protocol_interface_rf_mac_setup_s *rf_mac_setup)
                 if (!active_buf) {
                     return;
                 }
+                bool cca_enabled;
+                if (active_buf->fcf_dsn.frametype == MAC_FRAME_ACK) {
+                    cca_enabled = false;
+                } else {
+                    cca_enabled = true;
+                }
+
+                mac_pd_sap_set_phy_tx_time(rf_mac_setup, active_buf->tx_time, cca_enabled);
                 if (active_buf->fcf_dsn.frametype == FC_BEACON_FRAME) {
                     // FHSS synchronization info is written in the end of transmitted (Beacon) buffer
                     dev_driver_tx_buffer_s *tx_buf = &rf_mac_setup->dev_driver_tx_buffer;
                     synch_info = tx_buf->buf + rf_mac_setup->dev_driver->phy_driver->phy_header_length + tx_buf->len - FHSS_SYNCH_INFO_LENGTH;
+                    rf_mac_setup->fhss_api->write_synch_info(rf_mac_setup->fhss_api, synch_info, 0, FHSS_SYNCH_FRAME, 0);
                 }
                 // Change to destination channel and write synchronization info to Beacon frames here
                 int tx_handle_retval = rf_mac_setup->fhss_api->tx_handle(rf_mac_setup->fhss_api, !mac_is_ack_request_set(active_buf),
                         active_buf->DstAddr, mac_convert_frame_type_to_fhss(active_buf->fcf_dsn.frametype),
-                        synch_info, active_buf->mac_payload_length, rf_mac_setup->dev_driver->phy_driver->phy_header_length,
-                        rf_mac_setup->dev_driver->phy_driver->phy_tail_length);
+                        active_buf->mac_payload_length, rf_mac_setup->dev_driver->phy_driver->phy_header_length,
+                        rf_mac_setup->dev_driver->phy_driver->phy_tail_length, active_buf->tx_time);
                 // When FHSS TX handle returns -1, transmission of the packet is currently not allowed -> restart CCA timer
                 if (tx_handle_retval == -1) {
                     timer_mac_start(rf_mac_setup, MAC_TIMER_CCA, randLIB_get_random_in_range(20, 400) + 1);
@@ -210,28 +269,30 @@ void mac_pd_sap_state_machine(protocol_interface_rf_mac_setup_s *rf_mac_setup)
 
 static void  mac_sap_cca_fail_cb(protocol_interface_rf_mac_setup_s *rf_ptr) {
     rf_ptr->macRfRadioTxActive = false;
-    if (rf_ptr->mac_cca_retry > rf_ptr->macMaxCSMABackoffs) {
+    if (rf_ptr->mac_cca_retry > rf_ptr->macMaxCSMABackoffs || (rf_ptr->active_pd_data_request && rf_ptr->active_pd_data_request->asynch_request)) {
         //Send MAC_CCA_FAIL
         mac_tx_done_state_set(rf_ptr, MAC_CCA_FAIL);
     } else {
         timer_mac_stop(rf_ptr);
         mac_csma_BE_update(rf_ptr);
-        mac_csma_backoff_start(rf_ptr);
+        if (mcps_pd_data_rebuild(rf_ptr, rf_ptr->active_pd_data_request) ) {
+            mac_tx_done_state_set(rf_ptr, MAC_CCA_FAIL);
+        }
     }
 }
 
-static uint16_t mac_get_retry_period(protocol_interface_rf_mac_setup_s *rf_ptr)
-{
-    if (rf_ptr->fhss_api) {
-        return rf_ptr->fhss_api->get_retry_period(rf_ptr->fhss_api, rf_ptr->active_pd_data_request->DstAddr, rf_ptr->dev_driver->phy_driver->phy_MTU);
-    }
-    uint8_t backoff_length = mac_csma_random_backoff_get(rf_ptr);
-    uint16_t backoff_slots = mac_csma_backoff_period_convert_to50us(backoff_length, rf_ptr->backoff_period_in_10us);
-    if (backoff_slots == 0) {
-        backoff_slots = 1;
-    }
-    return backoff_slots;
-}
+//static uint16_t mac_get_retry_period(protocol_interface_rf_mac_setup_s *rf_ptr)
+//{
+//    if (rf_ptr->fhss_api && rf_ptr->fhss_api->get_retry_period) {
+//        return rf_ptr->fhss_api->get_retry_period(rf_ptr->fhss_api, rf_ptr->active_pd_data_request->DstAddr, rf_ptr->dev_driver->phy_driver->phy_MTU);
+//    }
+//    uint8_t backoff_length = mac_csma_random_backoff_get(rf_ptr);
+//    uint16_t backoff_slots = mac_csma_backoff_period_convert_to50us(backoff_length, rf_ptr->backoff_period_in_10us);
+//    if (backoff_slots == 0) {
+//        backoff_slots = 1;
+//    }
+//    return backoff_slots;
+//}
 
 static void mac_sap_no_ack_cb(protocol_interface_rf_mac_setup_s *rf_ptr) {
     rf_ptr->macRfRadioTxActive = false;
@@ -241,7 +302,10 @@ static void mac_sap_no_ack_cb(protocol_interface_rf_mac_setup_s *rf_ptr) {
         mac_csma_param_init(rf_ptr);
         rf_ptr->mac_tx_status.retry++;
         /*Send retry using random interval*/
-        timer_mac_start(rf_ptr, MAC_TIMER_CCA, mac_get_retry_period(rf_ptr));
+        if (mcps_pd_data_rebuild(rf_ptr, rf_ptr->active_pd_data_request) ) {
+            mac_tx_done_state_set(rf_ptr, MAC_CCA_FAIL);
+        }
+      
     } else {
         //Send TX Fail event
         // rf_mac_setup->ip_tx_active->bad_channel = rf_mac_setup->mac_channel;
@@ -249,6 +313,14 @@ static void mac_sap_no_ack_cb(protocol_interface_rf_mac_setup_s *rf_ptr) {
     }
 }
 
+static bool mac_data_counter_too_small(uint32_t current_counter, uint32_t packet_counter)
+{
+   if((current_counter - packet_counter) >= 2) {
+       return true;
+   }
+   return false;
+}
+
 
 static int8_t mac_data_interface_tx_done_cb(protocol_interface_rf_mac_setup_s *rf_ptr, phy_link_tx_status_e status, uint8_t cca_retry, uint8_t tx_retry)
 {
@@ -256,21 +328,80 @@ static int8_t mac_data_interface_tx_done_cb(protocol_interface_rf_mac_setup_s *r
     if (!rf_ptr->macRfRadioTxActive) {
         return -1;
     }
-    bool waiting_ack = false;
-    // Do not update CCA count when Ack is received, it was already updated with PHY_LINK_TX_SUCCESS event
-    if ((status != PHY_LINK_TX_DONE) && (status != PHY_LINK_TX_DONE_PENDING)) {
-        /* For PHY_LINK_TX_SUCCESS and PHY_LINK_CCA_FAIL cca_retry must always be > 0.
-         * PHY_LINK_TX_FAIL either happened during transmission or when waiting Ack -> we must use the CCA count given by PHY.
-         */
-        if ((cca_retry == 0) && (status != PHY_LINK_TX_FAIL)) {
-            cca_retry = 1;
+
+    if (status == PHY_LINK_CCA_PREPARE) {
+        if (rf_ptr->fhss_api) {
+            if (rf_ptr->mac_ack_tx_active) {
+                return 0;
+            }
+            mac_pre_build_frame_t *active_buf = rf_ptr->active_pd_data_request;
+            if (!active_buf) {
+                return -1;
+            }
+
+            if (active_buf->fcf_dsn.frametype == FC_BEACON_FRAME) {
+                // FHSS synchronization info is written in the end of transmitted (Beacon) buffer
+                dev_driver_tx_buffer_s *tx_buf = &rf_ptr->dev_driver_tx_buffer;
+                uint8_t *synch_info = tx_buf->buf + rf_ptr->dev_driver->phy_driver->phy_header_length + tx_buf->len - FHSS_SYNCH_INFO_LENGTH;
+                rf_ptr->fhss_api->write_synch_info(rf_ptr->fhss_api, synch_info, 0, FHSS_SYNCH_FRAME, 0);
+            }
+            if (active_buf->asynch_request == false) {
+                // Change to destination channel and write synchronization info to Beacon frames here
+                int tx_handle_retval = rf_ptr->fhss_api->tx_handle(rf_ptr->fhss_api, !mac_is_ack_request_set(active_buf),
+                                                                     active_buf->DstAddr, mac_convert_frame_type_to_fhss(active_buf->fcf_dsn.frametype),
+                                                                     active_buf->mac_payload_length, rf_ptr->dev_driver->phy_driver->phy_header_length,
+                                                                     rf_ptr->dev_driver->phy_driver->phy_tail_length, active_buf->tx_time);
+                // When FHSS TX handle returns -1, transmission of the packet is currently not allowed -> restart CCA timer
+                if (tx_handle_retval == -1) {
+                    mac_sap_cca_fail_cb(rf_ptr);
+                    return -2;
+                }
+                // When FHSS TX handle returns -3, we are trying to transmit broadcast packet on unicast channel -> push back
+                // to queue by using CCA fail event
+                if (tx_handle_retval == -3) {
+                    mac_tx_done_state_set(rf_ptr, MAC_CCA_FAIL);
+                    return -3;
+                }
+            }
         }
-        rf_ptr->mac_tx_status.cca_cnt += cca_retry;
-        rf_ptr->mac_cca_retry += cca_retry;
+        return 0;
+    }
+
+    //
+    bool waiting_ack = false;
+
+
+    if (rf_ptr->mac_ack_tx_active) {
+        rf_ptr->mac_ack_tx_active = false;
+        if (rf_ptr->active_pd_data_request) {
+
+            if (rf_ptr->active_pd_data_request->fcf_dsn.securityEnabled) {
+                uint32_t current_counter = mac_mlme_framecounter_get(rf_ptr);
+                if (mac_data_counter_too_small(current_counter, rf_ptr->active_pd_data_request->aux_header.frameCounter)) {
+                    rf_ptr->active_pd_data_request->aux_header.frameCounter = current_counter;
+                    mac_mlme_framecounter_increment(rf_ptr);
+                }
+            }
+            //GEN TX failure
+            mac_sap_cca_fail_cb(rf_ptr);
+        }
+        return 0;
+    } else {
+        // Do not update CCA count when Ack is received, it was already updated with PHY_LINK_TX_SUCCESS event
+        if ((status != PHY_LINK_TX_DONE) && (status != PHY_LINK_TX_DONE_PENDING)) {
+            /* For PHY_LINK_TX_SUCCESS and PHY_LINK_CCA_FAIL cca_retry must always be > 0.
+             * PHY_LINK_TX_FAIL either happened during transmission or when waiting Ack -> we must use the CCA count given by PHY.
+             */
+            if ((cca_retry == 0) && (status != PHY_LINK_TX_FAIL)) {
+                cca_retry = 1;
+            }
+            rf_ptr->mac_tx_status.cca_cnt += cca_retry;
+            rf_ptr->mac_cca_retry += cca_retry;
+        }
+        rf_ptr->mac_tx_status.retry += tx_retry;
+        rf_ptr->mac_tx_retry += tx_retry;
+        timer_mac_stop(rf_ptr);
     }
-    rf_ptr->mac_tx_status.retry += tx_retry;
-    rf_ptr->mac_tx_retry += tx_retry;
-    timer_mac_stop(rf_ptr);
 
     switch (status) {
         case PHY_LINK_TX_SUCCESS:
@@ -298,7 +429,9 @@ static int8_t mac_data_interface_tx_done_cb(protocol_interface_rf_mac_setup_s *r
 
         case PHY_LINK_TX_DONE_PENDING:
             mac_tx_done_state_set(rf_ptr, MAC_TX_DONE_PENDING);
+            break;
 
+        default:
             break;
     }
     if (rf_ptr->fhss_api) {
@@ -306,14 +439,99 @@ static int8_t mac_data_interface_tx_done_cb(protocol_interface_rf_mac_setup_s *r
         if (rf_ptr->mac_tx_result == MAC_TX_DONE) {
             tx_is_done = true;
         }
-        rf_ptr->fhss_api->data_tx_done(rf_ptr->fhss_api, waiting_ack, tx_is_done, rf_ptr->active_pd_data_request->msduHandle);
+        if (rf_ptr->active_pd_data_request->asynch_request == false) {
+            rf_ptr->fhss_api->data_tx_done(rf_ptr->fhss_api, waiting_ack, tx_is_done, rf_ptr->active_pd_data_request->msduHandle);
+        }
     }
     return 0;
 }
 
+
+static int8_t mac_data_interface_tx_done_by_ack_cb(protocol_interface_rf_mac_setup_s *rf_ptr, mac_pre_parsed_frame_t *buf)
+{
+
+    if (!rf_ptr->macRfRadioTxActive) {
+        return -1;
+    }
+
+    timer_mac_stop(rf_ptr);
+    if (buf->fcf_dsn.framePending) {
+        rf_ptr->mac_tx_result = MAC_TX_DONE_PENDING;
+    } else {
+        rf_ptr->mac_tx_result = MAC_TX_DONE;
+    }
+    rf_ptr->macRfRadioTxActive = false;
+    rf_ptr->macTxProcessActive = false;
+    mcps_sap_pd_ack(buf);
+
+    if (rf_ptr->fhss_api) {
+        if (rf_ptr->active_pd_data_request->asynch_request == false) {
+            rf_ptr->fhss_api->data_tx_done(rf_ptr->fhss_api, false, true, rf_ptr->active_pd_data_request->msduHandle);
+        }
+    }
+    return 0;
+}
+
+static bool mac_pd_sap_ack_validation(protocol_interface_rf_mac_setup_s *rf_ptr, mac_fcf_sequence_t *fcf_dsn, const uint8_t *data_ptr)
+{
+    if (!rf_ptr->active_pd_data_request || !rf_ptr->active_pd_data_request->fcf_dsn.ackRequested) {
+        return false; //No active Data request anymore or no ACK request for current TX
+    }
+
+    if (fcf_dsn->frameVersion != rf_ptr->active_pd_data_request->fcf_dsn.frameVersion) {
+        return false;
+    }
+
+    if (fcf_dsn->frameVersion == MAC_FRAME_VERSION_2015) {
+
+        //Validate ACK SRC address mode and address to Active TX dst address
+        if (rf_ptr->active_pd_data_request->fcf_dsn.DstAddrMode != fcf_dsn->SrcAddrMode) {
+            return false;
+        }
+
+        if (fcf_dsn->SrcAddrMode) {
+            uint8_t srcAddress[8];
+            uint8_t address_length = mac_address_length(fcf_dsn->SrcAddrMode);
+            mac_header_get_src_address(fcf_dsn, data_ptr, srcAddress);
+            if (memcmp(srcAddress, rf_ptr->active_pd_data_request->DstAddr, address_length)) {
+                return false;
+            }
+        }
+
+        //Validate ACK DST address mode and address to Active TX src address
+        if (rf_ptr->active_pd_data_request->fcf_dsn.SrcAddrMode != fcf_dsn->DstAddrMode) {
+            return false;
+        }
+
+        if (fcf_dsn->DstAddrMode) {
+            uint8_t dstAddress[8];
+            uint8_t address_length = mac_address_length(fcf_dsn->DstAddrMode);
+            mac_header_get_dst_address(fcf_dsn, data_ptr, dstAddress);
+            if (memcmp(dstAddress, rf_ptr->active_pd_data_request->SrcAddr, address_length)) {
+                return false;
+            }
+        }
+
+        if (rf_ptr->active_pd_data_request->fcf_dsn.sequenceNumberSuppress != fcf_dsn->sequenceNumberSuppress) {
+            return false; //sequence number validation not correct
+        }
+
+        if (!fcf_dsn->sequenceNumberSuppress && (rf_ptr->active_pd_data_request->fcf_dsn.DSN != fcf_dsn->DSN)) {
+            return false;
+        }
+        return true;
+    }
+
+    if (rf_ptr->active_pd_data_request->fcf_dsn.DSN != fcf_dsn->DSN) {
+        return false;
+    }
+    return true;
+}
+
 int8_t mac_pd_sap_data_cb(void *identifier, arm_phy_sap_msg_t *message)
 {
     protocol_interface_rf_mac_setup_s *rf_ptr = (protocol_interface_rf_mac_setup_s*)identifier;
+    mac_pre_parsed_frame_t *buffer = NULL;
 
     if (!rf_ptr || !message ) {
         return -1;
@@ -325,7 +543,6 @@ int8_t mac_pd_sap_data_cb(void *identifier, arm_phy_sap_msg_t *message)
 
     if (message->id == MAC15_4_PD_SAP_DATA_IND) {
         const uint8_t *ptr;
-
         arm_pd_sap_generic_ind_t *pd_data_ind = &(message->message.generic_data_ind);
 
         if (pd_data_ind->data_len < 3 ) {
@@ -333,21 +550,86 @@ int8_t mac_pd_sap_data_cb(void *identifier, arm_phy_sap_msg_t *message)
         }
         ptr = pd_data_ind->data_ptr;
 
-        mac_pre_parsed_frame_t *buffer = mcps_sap_pre_parsed_frame_buffer_get(pd_data_ind->data_ptr, pd_data_ind->data_len);
+        uint32_t time_stamp = 0;
+        if (rf_ptr->rf_csma_extension_supported) {
+            time_stamp = mac_pd_sap_get_phy_rx_time(rf_ptr);
+        }
+        mac_fcf_sequence_t fcf_read;
+        ptr = mac_header_parse_fcf_dsn(&fcf_read, ptr);
+        //Check PanID presents at header
+        fcf_read.DstPanPresents = mac_dst_panid_present(&fcf_read);
+        fcf_read.SrcPanPresents = mac_src_panid_present(&fcf_read);
+        int16_t length = pd_data_ind->data_len;
+        if (!rf_ptr->macProminousMode) {
+
+            //Unsupported Frame
+            if (fcf_read.frametype > FC_CMD_FRAME || (fcf_read.frametype == FC_ACK_FRAME && fcf_read.frameVersion != MAC_FRAME_VERSION_2015)) {
+                goto ERROR_HANDLER;
+            }
+
+            switch (fcf_read.frametype) {
+                case FC_DATA_FRAME:
+                    if (fcf_read.SrcAddrMode == MAC_ADDR_MODE_NONE) {
+                        return -1;
+                    } else if (fcf_read.DstAddrMode == MAC_ADDR_MODE_NONE && fcf_read.frameVersion != MAC_FRAME_VERSION_2015) {
+                        return -1;
+                    }
+                    break;
+                case FC_BEACON_FRAME:
+                    if (fcf_read.SrcAddrMode == MAC_ADDR_MODE_NONE || fcf_read.DstAddrMode != MAC_ADDR_MODE_NONE) {
+                        return -1;
+                    }
+                    break;
+                case FC_ACK_FRAME:
+                    //Validate here that we are waiting ack
+                    if (fcf_read.ackRequested) {
+                        return -1;
+                    }
+
+                    //Validate ACK
+                    if (!mac_pd_sap_ack_validation(rf_ptr, &fcf_read, pd_data_ind->data_ptr)) {
+                        return -1;
+                    }
+                    break;
+
+                default:
+                    break;
+            }
+
+            //Generate ACK when Extension is enabled and ACK is requested
+            if (rf_ptr->mac_extension_enabled && fcf_read.ackRequested && fcf_read.frameVersion == MAC_FRAME_VERSION_2015) {
+                //SEND ACK here
+                if (rf_ptr->mac_ack_tx_active) {
+                    return -1;
+                }
+
+                mcps_ack_data_payload_t ack_payload;
+                mac_api_t *mac_api = get_sw_mac_api(rf_ptr);
+                mac_api->enhanced_ack_data_req_cb(mac_api, &ack_payload, pd_data_ind->dbm, pd_data_ind->link_quality);
+                //Calculate Delta time
+
+                if (mcps_generic_ack_build(rf_ptr, &fcf_read, pd_data_ind->data_ptr, &ack_payload, time_stamp) !=0) {
+                    return -1;
+                }
+
+                rf_ptr->mac_ack_tx_active = true;
+            }
+        }
+
+        buffer = mcps_sap_pre_parsed_frame_buffer_get(pd_data_ind->data_ptr, pd_data_ind->data_len);
 
         if (!buffer) {
-            tr_error("pd_ind buffer get fail %u", pd_data_ind->data_len);
             sw_mac_stats_update(rf_ptr, STAT_MAC_RX_DROP, 0);
             return -3;
         }
-        if (rf_ptr->fhss_api) {
-            buffer->timestamp = rf_ptr->fhss_api->read_timestamp(rf_ptr->fhss_api);
-        }
-        buffer->ack_pendinfg_status = mac_data_interface_read_last_ack_pending_status(rf_ptr);
-        mac_header_parse_fcf_dsn(&buffer->fcf_dsn, ptr);
 
-        int16_t length = pd_data_ind->data_len;
-        ptr += 3;
+        //Copy Pre Parsed values
+        buffer->fcf_dsn = fcf_read;
+        buffer->timestamp = time_stamp;
+
+        buffer->ack_pendinfg_status = mac_data_interface_read_last_ack_pending_status(rf_ptr);
+
+
         // Upward direction functions assume no headroom and are trusting that removed bytes are still valid.
         // see mac.c:655
 
@@ -355,41 +637,16 @@ int8_t mac_pd_sap_data_cb(void *identifier, arm_phy_sap_msg_t *message)
         buffer->dbm = pd_data_ind->dbm;
         buffer->LQI = pd_data_ind->link_quality;
         buffer->mac_class_ptr = rf_ptr;
-        buffer->mac_header_length = 3;
+        //Dnamic calculation for FCF + SEQ parse
+        buffer->mac_header_length = ptr - pd_data_ind->data_ptr;
 
         if (!rf_ptr->macProminousMode) {
 
-            if (buffer->fcf_dsn.frametype > FC_CMD_FRAME || buffer->fcf_dsn.frametype == FC_ACK_FRAME) {
+            if (buffer->fcf_dsn.frametype > FC_CMD_FRAME) {
                 goto ERROR_HANDLER;
             }
-            //Verify Length after address field
-            switch (buffer->fcf_dsn.DstAddrMode) {
-                case MAC_ADDR_MODE_64_BIT:
-                    buffer->mac_header_length += 10;
-                    break;
-                case MAC_ADDR_MODE_16_BIT:
-                    buffer->mac_header_length += 4;
-                    break;
-                case MAC_ADDR_MODE_NONE:
-                    break;
-            }
 
-            switch (buffer->fcf_dsn.SrcAddrMode) {
-                case MAC_ADDR_MODE_64_BIT:
-                    buffer->mac_header_length += 8;
-                    if (!buffer->fcf_dsn.intraPan) {
-                        buffer->mac_header_length += 2;
-                    }
-                    break;
-                case MAC_ADDR_MODE_16_BIT:
-                    buffer->mac_header_length += 2;
-                    if (!buffer->fcf_dsn.intraPan) {
-                        buffer->mac_header_length += 2;
-                    }
-                    break;
-                case MAC_ADDR_MODE_NONE:
-                    break;
-            }
+            buffer->mac_header_length += mac_header_address_length(&buffer->fcf_dsn);
 
             length -= buffer->mac_header_length;
 
@@ -406,6 +663,7 @@ int8_t mac_pd_sap_data_cb(void *identifier, arm_phy_sap_msg_t *message)
                 uint8_t auxBaseHeader = *security_ptr;
                 key_id_mode = (auxBaseHeader >> 3) & 3;
                 security_level = auxBaseHeader & 7;
+
                 switch (key_id_mode) {
                     case MAC_KEY_ID_MODE_IMPLICIT:
                         if (security_level) {
@@ -426,24 +684,7 @@ int8_t mac_pd_sap_data_cb(void *identifier, arm_phy_sap_msg_t *message)
                 }
 
                 length -= buffer->security_aux_header_length;
-
-                switch (security_level) {
-                    case 1:
-                    case 5:
-                        mic_len = 4;
-                        break;
-                    case 2:
-                    case 6:
-                        mic_len = 8;
-                        break;
-                    case 3:
-                    case 7:
-                        mic_len = 16;
-                        break;
-                    default:
-                        mic_len = 0;
-                        break;
-                }
+                mic_len = mac_security_mic_length_get(security_level);
 
                 length -= mic_len;
 
@@ -455,28 +696,29 @@ int8_t mac_pd_sap_data_cb(void *identifier, arm_phy_sap_msg_t *message)
                 buffer->mac_payload_length -= (buffer->security_aux_header_length + mic_len);
             }
 
-            switch (buffer->fcf_dsn.frametype) {
-                case FC_DATA_FRAME:
-                    if (buffer->fcf_dsn.SrcAddrMode == MAC_ADDR_MODE_NONE || buffer->fcf_dsn.DstAddrMode == MAC_ADDR_MODE_NONE) {
-                        goto ERROR_HANDLER;
-                    }
-                    break;
-                case FC_BEACON_FRAME:
-                    if (buffer->fcf_dsn.SrcAddrMode == MAC_ADDR_MODE_NONE || buffer->fcf_dsn.DstAddrMode != MAC_ADDR_MODE_NONE) {
-                        goto ERROR_HANDLER;
-                    }
-                    break;
-                default:
-                    if (length == 0) {
-                        goto ERROR_HANDLER;
-                    }
-                    break;
-            }
 
         }
+        //Do not accept commend frame with length 0
+        if (fcf_read.frametype == FC_CMD_FRAME && length == 0) {
+            return -1;
+        }
 
-        if (mcps_sap_pd_ind(buffer) == 0) {
+        //Parse IE Elements
+        if (!mac_header_information_elements_parse(buffer)) {
+            goto ERROR_HANDLER;
+        }
+
+        if (!rf_ptr->macProminousMode && buffer->fcf_dsn.frametype == FC_ACK_FRAME) {
+            if (mac_data_interface_tx_done_by_ack_cb(rf_ptr, buffer)) {
+                mcps_sap_pre_parsed_frame_buffer_free(buffer);
+            }
             return 0;
+
+        } else {
+
+            if (mcps_sap_pd_ind(buffer) == 0) {
+                return 0;
+            }
         }
         ERROR_HANDLER:
         mcps_sap_pre_parsed_frame_buffer_free(buffer);
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_pd_sap.h b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_pd_sap.h
index b476ea1af8..f975f03952 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_pd_sap.h
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_pd_sap.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -37,9 +37,17 @@ struct arm_phy_sap_msg_s;
  */
 int8_t mac_pd_sap_req(struct protocol_interface_rf_mac_setup *rf_mac_setup);
 
+int8_t mac_plme_cca_req(struct protocol_interface_rf_mac_setup *rf_mac_setup);
+
+void mac_pd_sap_set_phy_tx_time(struct protocol_interface_rf_mac_setup *rf_mac_setup, uint32_t tx_time, bool cca_enabled);
+
 void mac_pd_sap_rf_low_level_function_set(void *mac_ptr, void *driver);
 
 int8_t mac_pd_sap_data_cb(void *identifier, struct arm_phy_sap_msg_s *message);
+
+void mac_csma_param_init(struct protocol_interface_rf_mac_setup *rf_mac_setup);
+
+uint32_t mac_csma_backoff_get(struct protocol_interface_rf_mac_setup *rf_mac_setup);
 /**
  * Run Mac data interface state Machine.
  *
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_security_mib.c b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_security_mib.c
index 02a5918c29..3b64707722 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_security_mib.c
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/mac_security_mib.c
@@ -395,7 +395,7 @@ mlme_key_descriptor_t * mac_sec_key_description_get(protocol_interface_rf_mac_se
         key_description++;
     }
 
-    tr_debug("LookuPdata search fail %s", trace_array(lookup_data, 9));
+    //tr_debug("LookuPdata search fail %s", trace_array(lookup_data, 9));
 
     return NULL;
 }
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/sw_mac.c b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/sw_mac.c
index a0be817f95..45fbfc2ca0 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/sw_mac.c
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/IEEE802_15_4/sw_mac.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -49,9 +49,11 @@ static mac_internal_t mac_store; //Hack only at this point, later put into linke
 static int8_t ns_sw_mac_initialize(mac_api_t *api, mcps_data_confirm *mcps_data_conf_cb,
                                    mcps_data_indication *mcps_data_ind_cb, mcps_purge_confirm *purge_conf_cb,
                                    mlme_confirm *mlme_conf_callback, mlme_indication *mlme_ind_callback, int8_t parent_id);
+static int8_t ns_sw_mac_api_enable_mcps_ext(mac_api_t *api, mcps_data_indication_ext *data_ind_cb, mcps_data_confirm_ext *data_cnf_cb, mcps_ack_data_req_ext *ack_data_req_cb);
 
 static void mlme_req(const mac_api_t* api, mlme_primitive id, const void *data);
 static void mcps_req(const mac_api_t* api, const mcps_data_req_t *data);
+static void mcps_req_ext(const mac_api_t* api, const mcps_data_req_t *data, const mcps_data_req_ie_list_t *ie_ext, const channel_list_s *asynch_channel_list);
 static void purge_req(const mac_api_t* api, const mcps_purge_t *data);
 static int8_t macext_mac64_address_set( const mac_api_t* api, const uint8_t *mac64);
 static int8_t macext_mac64_address_get( const mac_api_t* api, mac_extended_address_type type, uint8_t *mac64_buf);
@@ -105,8 +107,10 @@ mac_api_t *ns_sw_mac_create(int8_t rf_driver_id, mac_description_storage_size_t
     arm_net_virtual_confirmation_rx_cb_set(driver->phy_driver, &mac_mlme_virtual_confirmation_handle);
 
     this->mac_initialize = &ns_sw_mac_initialize;
+    this->mac_mcps_extension_enable = &ns_sw_mac_api_enable_mcps_ext;
     this->mlme_req = &mlme_req;
     this->mcps_data_req = &mcps_req;
+    this->mcps_data_req_ext = &mcps_req_ext;
     this->mcps_purge_req = &purge_req;
     this->mac64_get = &macext_mac64_address_get;
     this->mac64_set = &macext_mac64_address_set;
@@ -161,6 +165,11 @@ int ns_sw_mac_fhss_register(mac_api_t *mac_api, fhss_api_t *fhss_api)
     if (!mac_setup) {
         return -2;
     }
+
+    if (!mac_setup->rf_csma_extension_supported) {
+        return -2;
+    }
+
     // Assign FHSS API
     mac_setup->fhss_api = fhss_api;
     // Pass MAC functions to FHSS
@@ -178,6 +187,18 @@ int ns_sw_mac_fhss_register(mac_api_t *mac_api, fhss_api_t *fhss_api)
     return 0;
 }
 
+struct fhss_api *ns_sw_mac_get_fhss_api(struct mac_api_s *mac_api)
+{
+    if (!mac_api) {
+        return NULL;
+    }
+    protocol_interface_rf_mac_setup_s *mac_setup = get_sw_mac_ptr_by_mac_api(mac_api);
+    if (!mac_setup) {
+        return NULL;
+    }
+    return mac_setup->fhss_api;
+}
+
 int ns_sw_mac_statistics_start(struct mac_api_s *mac_api, struct mac_statistics_s *mac_statistics)
 {
     if (!mac_api || !mac_statistics) {
@@ -213,6 +234,30 @@ static int8_t ns_sw_mac_initialize(mac_api_t *api, mcps_data_confirm *mcps_data_
     return 0;
 }
 
+static int8_t ns_sw_mac_api_enable_mcps_ext(mac_api_t *api, mcps_data_indication_ext *data_ind_cb, mcps_data_confirm_ext *data_cnf_cb, mcps_ack_data_req_ext *ack_data_req_cb)
+{
+    //TODO: Find from linked list instead
+    if(api != mac_store.mac_api ){
+        return -1;
+    }
+
+    mac_api_t *cur = mac_store.mac_api;
+
+    if (!mac_store.setup->rf_csma_extension_supported) {
+        return -1;
+    }
+
+    cur->data_conf_ext_cb = data_cnf_cb;
+    cur->data_ind_ext_cb = data_ind_cb;
+    cur->enhanced_ack_data_req_cb = ack_data_req_cb;
+    if (data_cnf_cb && data_ind_cb && ack_data_req_cb) {
+        mac_store.setup->mac_extension_enabled = true;
+    } else {
+        mac_store.setup->mac_extension_enabled = false;
+    }
+    return 0;
+}
+
 mac_api_t *get_sw_mac_api(protocol_interface_rf_mac_setup_s *setup)
 {
     if (!mac_store.mac_api || mac_store.mac_api->parent_id == -1 || mac_store.setup != setup) {
@@ -448,10 +493,22 @@ void mcps_req(const mac_api_t* api, const mcps_data_req_t *data)
 {
     //TODO: Populate linked list when present
     if (mac_store.mac_api == api) {
-        mcps_sap_data_req_handler(mac_store.setup , data );
+        /* Call direct new API but without IE extensions */
+        mcps_data_req_ie_list_t ie_list;
+        memset(&ie_list, 0 , sizeof(mcps_data_req_ie_list_t));
+        mcps_sap_data_req_handler_ext(mac_store.setup , data , &ie_list, NULL);
     }
 }
 
+void mcps_req_ext(const mac_api_t* api, const mcps_data_req_t *data, const mcps_data_req_ie_list_t *ie_ext, const channel_list_s *asynch_channel_list)
+{
+//TODO: Populate linked list when present
+    if (mac_store.mac_api == api) {
+        mcps_sap_data_req_handler_ext(mac_store.setup , data , ie_ext, asynch_channel_list);
+    }
+}
+
+
 static void purge_req(const mac_api_t* api, const mcps_purge_t *data)
 {
     if (mac_store.mac_api == api) {
diff --git a/features/nanostack/sal-stack-nanostack/source/MAC/virtual_rf/virtual_rf_client.c b/features/nanostack/sal-stack-nanostack/source/MAC/virtual_rf/virtual_rf_client.c
index b81b2564dc..651e21e05d 100644
--- a/features/nanostack/sal-stack-nanostack/source/MAC/virtual_rf/virtual_rf_client.c
+++ b/features/nanostack/sal-stack-nanostack/source/MAC/virtual_rf/virtual_rf_client.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -138,7 +138,7 @@ static int8_t phy_rf_virtual_rx(const uint8_t *data_ptr, uint16_t data_len,int8_
                 return -1;
             }
             phy_msg.id = MACTUN_MLME_NAP_EXTENSION;
-            phy_msg.message.mlme_request.primitive = *data_ptr++;
+            phy_msg.message.mlme_request.primitive = (mlme_primitive) *data_ptr++;
             phy_msg.message.mlme_request.mlme_ptr = data_ptr;
             phy_msg.message.mlme_request.ptr_length = (data_len - 2);
 
diff --git a/features/nanostack/sal-stack-nanostack/source/MLE/mle.c b/features/nanostack/sal-stack-nanostack/source/MLE/mle.c
index 39177b1537..5168fce5c2 100644
--- a/features/nanostack/sal-stack-nanostack/source/MLE/mle.c
+++ b/features/nanostack/sal-stack-nanostack/source/MLE/mle.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2017, Arm Limited and affiliates.
+ * Copyright (c) 2013-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -18,7 +18,7 @@
 #include "nsconfig.h"
 #include "MLE/mle.h"
 
-#ifndef NO_MLE
+
 #include "ns_types.h"
 #include "eventOS_event.h"
 #include "eventOS_event_timer.h"
@@ -46,320 +46,25 @@
 #include "6LoWPAN/MAC/mac_data_poll.h"
 #include "6LoWPAN/lowpan_adaptation_interface.h"
 
-#define MLE_UNICAST_CHALLENGE_TIMEOUT 20
-
 #define TRACE_GROUP "mle"
 
-typedef enum {
-    ARM_MLE_INIT = 0,
-    ARM_MLE_TTL_TIMER
-} arm_mle_event_id_e;
-
-//MLE class structure
-typedef struct mle_table_class {
-    int8_t interfaceId;
-    mle_class_user_mode mode;
-    mle_neigh_table_list_t mle_table;       //Active Entry
-    mle_neigh_table_list_t free_enty_list;
-    mle_neigh_table_entry_t *allocated_buffer;
-    uint8_t buffer_size;
-    mle_entry_user_entry_remove_notify *remove_cb;
-    mle_entry_link_keep_alive *keep_alive_cb;
-    mle_entry_link_keep_alive *challenge_cb;
-    mle_entry_interface_activate *interface_is_active;
-    ns_list_link_t link; /*!< List link entry */
-} mle_table_class_t;
-
-static NS_LIST_DEFINE(mle_table_calss_list, mle_table_class_t, link);
-
-static int8_t mle_tasklet_id = -1;
-
-static arm_event_storage_t *mle_class_timer_storage = NULL;
-
-static mle_neigh_table_entry_t *mle_class_neighbor_get(mle_neigh_table_list_t *mle_table, const uint8_t *address, addrtype_t type);
-static mle_neigh_table_entry_t *mle_class_get_free_entry(mle_neigh_table_list_t *mle_table);
-static bool mle_class_neighbor_validate(mle_neigh_table_list_t *mle_table, const mle_neigh_table_entry_t *entry);
-static void mle_event_handler(arm_event_s *event);
 
 
-static bool mle_table_timer_start(void)
+int16_t mle_class_free_entry_count_get(struct protocol_interface_info_entry *cur)
 {
-    if (!mle_class_timer_storage) {
+    mac_neighbor_table_list_t *mac_table_free_list = &mac_neighbor_info(cur)->free_list;
+    return ns_list_count(mac_table_free_list);
 
-        arm_event_s event = {
-                .receiver = mle_tasklet_id,
-                .sender = 0,
-                .event_id = 0,
-                .data_ptr = NULL,
-                .event_type = ARM_MLE_TTL_TIMER,
-                .priority = ARM_LIB_LOW_PRIORITY_EVENT,
-        };
-
-        mle_class_timer_storage  = eventOS_event_timer_request_every(&event, eventOS_event_timer_ms_to_ticks(MLE_TIMER_TICKS_MS));
-        if (!mle_class_timer_storage) {
-            tr_error("Mle timer start fail");
-            return false;
-        }
-    }
-
-    return true;
 }
 
-static void mle_table_timer_stop(void)
+int16_t mle_class_sleepy_entry_count_get(struct protocol_interface_info_entry *cur)
 {
-    if (mle_class_timer_storage && ns_list_is_empty(&mle_table_calss_list)) {
-        eventOS_cancel(mle_class_timer_storage);
-        mle_class_timer_storage = NULL;
-    }
-}
-
-
-static void mle_table_remove_free_indirect_table(int8_t interface_id, mle_neigh_table_entry_t *entry_ptr)
-{
-    protocol_interface_info_entry_t *cur_interface = protocol_stack_interface_info_get_by_id(interface_id);
-    if (!cur_interface) {
-        return;
-    }
-    //Free firts by defined short address
-    if (entry_ptr->short_adr < 0xfffe) {
-        uint8_t temp_address[2];
-        common_write_16_bit(entry_ptr->short_adr, temp_address);
-        lowpan_adaptation_indirect_free_messages_from_queues_by_address(cur_interface, temp_address, ADDR_802_15_4_SHORT);
-    }
-    lowpan_adaptation_indirect_free_messages_from_queues_by_address(cur_interface, entry_ptr->mac64, ADDR_802_15_4_LONG);
-}
-
-static void mle_table_class_list_free(mle_table_class_t *mle_table_class) {
-
-    ns_list_foreach_safe(mle_neigh_table_entry_t, cur, &mle_table_class->mle_table) {
-        ns_list_remove(&mle_table_class->mle_table, cur);
-        //Clean Indirect queue
-        mle_table_remove_free_indirect_table(mle_table_class->interfaceId, cur);
-        //Call Remove callback
-        mle_table_class->remove_cb(mle_table_class->interfaceId, cur);
-        //Removes ETX neighbor
-        etx_neighbor_remove(mle_table_class->interfaceId, cur);
-        ns_list_add_to_start(&mle_table_class->free_enty_list, cur);
-    }
-    topo_trace(TOPOLOGY_MLE, NULL, TOPO_CLEAR);
-}
-
-static void mle_table_class_free(mle_table_class_t *main_list)
-{
-    ns_list_remove(&mle_table_calss_list, main_list);
-    mle_table_class_list_free(main_list);
-    //Free list buffer
-    ns_dyn_mem_free(main_list->allocated_buffer);
-    ns_dyn_mem_free(main_list);
-    mle_table_timer_stop();
-}
-
-
-
-static int8_t mle_table_class_table_buffer_allocate(mle_table_class_t *mle_class, uint8_t list_size)
-{
-    mle_neigh_table_entry_t *list_buffer = ns_dyn_mem_alloc(sizeof(mle_neigh_table_entry_t) * list_size);
-    if (!list_buffer) {
-        return -1;
-    }
-
-    mle_class->allocated_buffer = list_buffer; //Save storaged
-    ns_list_init(&mle_class->free_enty_list);
-    ns_list_init(&mle_class->mle_table);
-    for (uint8_t i = 0; i< list_size; i++) {
-        memset(list_buffer, 0, sizeof(mle_neigh_table_entry_t));
-
-        list_buffer->attribute_index = i;
-        //Add to list
-        ns_list_add_to_end(&mle_class->free_enty_list,list_buffer);
-        list_buffer++;
-    }
-    return 0;
-}
-
-
-static mle_table_class_t * mle_table_class_allocate(int8_t interfaceId, uint8_t list_size)
-{
-    mle_table_class_t *newClass = ns_dyn_mem_alloc(sizeof(mle_table_class_t));
-
-    if (newClass) {
-        memset(newClass, 0, sizeof(mle_table_class_t));
-        if (mle_table_class_table_buffer_allocate(newClass,list_size) != 0) {
-            ns_dyn_mem_free(newClass);
-            return NULL;
-        }
-        newClass->interfaceId = interfaceId;
-        newClass->buffer_size = list_size;
-    }
-    return newClass;
-}
-
-static mle_table_class_t * mle_table_class_discover(int8_t interface_id) {
-    ns_list_foreach(mle_table_class_t, cur_mle_class, &mle_table_calss_list) {
-        if (cur_mle_class->interfaceId == interface_id) {
-            return cur_mle_class;
-        }
-    }
-    return NULL;
-}
-
-static int8_t mle_class_event_handler_init(void) {
-    if (mle_tasklet_id == -1) {
-        //GENERATE TASKLET
-        mle_tasklet_id = eventOS_event_handler_create(&mle_event_handler, ARM_MLE_INIT);
-
-    }
-
-    return mle_tasklet_id;
-}
-
-int8_t mle_class_init(int8_t interface_id, uint8_t table_size, mle_entry_user_entry_remove_notify *remove_cb, mle_entry_link_keep_alive *keep_alive_cb, mle_entry_interface_activate *interface_is_active)
-{
-    //Discover from the list
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-    //Clean list and set function pointer call backs
-    if (mle_class_ptr) {
-        mle_class_ptr->remove_cb = remove_cb;
-        mle_class_ptr->keep_alive_cb = keep_alive_cb;
-        mle_class_ptr->challenge_cb = NULL;
-        mle_class_ptr->interface_is_active = interface_is_active;
-        if (mle_class_ptr->buffer_size != table_size) { //Clean tabs only when table size is different
-            mle_table_class_list_free(mle_class_ptr);
-            ns_dyn_mem_free(mle_class_ptr->allocated_buffer);
-            mle_class_ptr->allocated_buffer = NULL;
-            //Reallocate
-            if (mle_table_class_table_buffer_allocate(mle_class_ptr,table_size) != 0) {
-                ns_list_remove(&mle_table_calss_list, mle_class_ptr);
-                ns_dyn_mem_free(mle_class_ptr);
-                return -2;
-            }
-            mle_class_ptr->buffer_size = table_size;
-        }
-        return 0;
-    }
-
-    if (mle_class_event_handler_init() < 0) {
-        return -2;
-    }
-
-    if (!mle_table_timer_start()) {
-        return -2;
-    }
-
-    mle_class_ptr  = mle_table_class_allocate(interface_id, table_size);
-    //Allocate new
-    if (!mle_class_ptr) {
-        return -2;
-    }
-
-    tr_debug("MLE service init size %d", table_size);
-    mle_class_ptr->remove_cb = remove_cb;
-    mle_class_ptr->keep_alive_cb = keep_alive_cb;
-    mle_class_ptr->challenge_cb = NULL;
-    mle_class_ptr->interface_is_active = interface_is_active;
-    ns_list_add_to_end(&mle_table_calss_list, mle_class_ptr);
-    return 0;
-
-}
-
-int8_t mle_class_router_challenge(int8_t interface_id,mle_entry_link_keep_alive *challenge_cb)
-{
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-    //Clean list and set function pointer call backs
-    if (!mle_class_ptr) {
-        return -1;
-    }
-    mle_class_ptr->challenge_cb = challenge_cb;
-    return 0;
-}
-
-bool mle_class_exists_for_interface(int8_t interface_id) {
-
-    if (mle_table_class_discover(interface_id)) {
-        return true;
-    }
-
-    return false;
-}
-
-int8_t mle_class_deallocate(int8_t interface_id)
-{
-    //Discover from the list
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-    //Clean list and set function pointer call backs
-    if (!mle_class_ptr) {
-        return -1;
-    }
-
-    mle_table_class_free(mle_class_ptr);
-    return 0;
-
-}
-
-int8_t mle_class_list_clean(int8_t interface_id)
-{
-    //Discover from the list
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-    //Clean list and set function pointer call backs
-    if (!mle_class_ptr) {
-        return -1;
-    }
-
-    mle_table_class_list_free(mle_class_ptr);
-    return 0;
-
-}
-
-int8_t mle_class_mode_set(int8_t interface_id,mle_class_user_mode mode)
-{
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-    //Clean list and set function pointer call backs
-    if (!mle_class_ptr) {
-        return -1;
-    }
-    mle_class_ptr->mode = mode;
-
-    return 0;
-}
-
-int8_t mle_class_set_new_key_pending(int8_t interface_id)
-{
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-
-    if (!mle_class_ptr) {
-        return -1;
-    }
-
-    ns_list_foreach_safe(mle_neigh_table_entry_t, cur, &mle_class_ptr->mle_table) {
-        cur->new_key_pending = true;
-    }
-
-    return 0;
-}
-
-int16_t mle_class_free_entry_count_get(int8_t interface_id)
-{
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-    //Clean list and set function pointer call backs
-    if (!mle_class_ptr) {
-        return 0;
-    }
-    return ns_list_count(&(mle_class_ptr->free_enty_list));
-
-}
-
-int16_t mle_class_sleepy_entry_count_get(int8_t interface_id)
-{
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-
-    if (!mle_class_ptr) {
-        return 0;
-    }
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
 
     uint16_t count = 0;
 
-    ns_list_foreach(mle_neigh_table_entry_t, entry, &mle_class_ptr->mle_table) {
-        if (!(entry->mode & MLE_RX_ON_IDLE)) {
+    ns_list_foreach(mac_neighbor_table_entry_t, entry, mac_table_list) {
+        if (!entry->rx_on_idle) {
             count++;
         }
     }
@@ -367,18 +72,14 @@ int16_t mle_class_sleepy_entry_count_get(int8_t interface_id)
     return count;
 }
 
-int16_t mle_class_rfd_entry_count_get(int8_t interface_id)
+int16_t mle_class_rfd_entry_count_get(struct protocol_interface_info_entry *cur)
 {
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-
-    if (!mle_class_ptr) {
-        return 0;
-    }
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
 
     uint16_t count = 0;
 
-    ns_list_foreach(mle_neigh_table_entry_t, entry, &mle_class_ptr->mle_table) {
-        if ((entry->mode & MLE_DEV_MASK) == MLE_RFD_DEV) {
+    ns_list_foreach(mac_neighbor_table_entry_t, entry, mac_table_list) {
+        if (!entry->ffd_device ) {
             count++;
         }
     }
@@ -386,360 +87,24 @@ int16_t mle_class_rfd_entry_count_get(int8_t interface_id)
     return count;
 }
 
-mle_neigh_table_entry_t *mle_class_get_entry_by_ll64(int8_t interface_id, uint8_t linkMargin, const uint8_t *ipv6Address, bool allocateNew, bool *new_entry_allocated)
+uint16_t mle_class_active_neigh_counter(protocol_interface_info_entry_t *cur)
 {
-    // Check it really is LL64 (not LL16)
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
 
-    if (memcmp(ipv6Address, ADDR_LINK_LOCAL_PREFIX , 8) != 0) {
-        return NULL;    //Mot Link Local Address
-    }
-
-    if (memcmp((ipv6Address + 8), ADDR_SHORT_ADR_SUFFIC , 6) == 0) {
-        return NULL;
-    }
-    // map
-    uint8_t temporary_mac64[8];
-    memcpy(temporary_mac64, (ipv6Address + 8), 8);
-    temporary_mac64[0] ^= 2;
-
-    return mle_class_get_entry_by_mac64(interface_id, linkMargin, temporary_mac64, allocateNew, new_entry_allocated);
+    return ns_list_count(mac_table_list);
 }
 
-mle_neigh_table_entry_t *mle_class_discover_entry_by_ll64(int8_t interface_id, const uint8_t *ipv6Address)
+#ifndef NO_MLE
+
+int8_t mle_class_set_new_key_pending(struct protocol_interface_info_entry *cur)
 {
+    mac_neighbor_table_list_t *mac_table_list = &mac_neighbor_info(cur)->neighbour_list;
 
-    // Check it really is LL64 (not LL16)
-
-    if (memcmp(ipv6Address, ADDR_LINK_LOCAL_PREFIX , 8) != 0) {
-        return NULL;    //Mot Link Local Address
+    ns_list_foreach_safe(mac_neighbor_table_entry_t, cur_entry, mac_table_list) {
+        mle_service_frame_counter_entry_new_key_pending_set(cur->id, cur_entry->index);
     }
 
-    if (memcmp((ipv6Address + 8), ADDR_SHORT_ADR_SUFFIC , 6) == 0) {
-        return NULL;
-    }
-    // map
-    uint8_t temporary_mac64[8];
-    memcpy(temporary_mac64, (ipv6Address + 8), 8);
-    temporary_mac64[0] ^= 2;
-
-    return mle_class_get_by_link_address(interface_id, temporary_mac64, ADDR_802_15_4_LONG);
-}
-
-
-mle_neigh_table_entry_t *mle_class_get_entry_by_mac64(int8_t interface_id, uint8_t linkMargin, const uint8_t *mac64, bool allocateNew, bool *new_entry_allocated)
-{
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-    //Clean list and set function pointer call backs
-    if (!mle_class_ptr) {
-        return NULL;
-    }
-
-    if (new_entry_allocated) {
-        *new_entry_allocated = false;
-    }
-
-    mle_neigh_table_entry_t *ret_val = mle_class_neighbor_get(&mle_class_ptr->mle_table, mac64, ADDR_802_15_4_LONG);
-
-
-    /* Look for existing entry */
-    if (ret_val) {
-        ret_val->link_margin = ret_val->link_margin + linkMargin - (ret_val->link_margin >> THREAD_LINK_MARGIN_SCALING);
-        return ret_val;
-    }
-
-    if (allocateNew) {
-        ret_val = mle_class_get_free_entry(&mle_class_ptr->free_enty_list);
-
-        if (ret_val) {
-            //Add to active list
-            ns_list_add_to_start(&mle_class_ptr->mle_table, ret_val);
-            topo_trace(TOPOLOGY_MLE, mac64, TOPO_ADD);
-            ret_val->link_margin = linkMargin << THREAD_LINK_MARGIN_SCALING;
-            memcpy(ret_val->mac64, mac64, 8);
-            if (new_entry_allocated) {
-                *new_entry_allocated = true;
-            }
-        }
-    }
-    return ret_val;
-}
-
-static mle_neigh_table_entry_t *mle_class_neighbor_get_by_attribute_index(mle_neigh_table_list_t *mle_table, uint8_t attribute_index)
-{
-
-    ns_list_foreach(mle_neigh_table_entry_t, cur, mle_table) {
-        if (cur->attribute_index == attribute_index) {
-            return cur;
-        }
-    }
-    return NULL;
-}
-
-static bool mle_class_neighbor_validate(mle_neigh_table_list_t *mle_table, const mle_neigh_table_entry_t *entry)
-{
-
-    ns_list_foreach(mle_neigh_table_entry_t, cur, mle_table) {
-        if (cur == entry) {
-            return true;
-        }
-    }
-    return false;
-}
-
-static mle_neigh_table_entry_t *mle_class_neighbor_get(mle_neigh_table_list_t *mle_table, const uint8_t *address, addrtype_t type)
-{
-    uint16_t short_id;
-    if (type == ADDR_802_15_4_SHORT) {
-        short_id = common_read_16_bit(address);
-    }
-    ns_list_foreach(mle_neigh_table_entry_t, cur, mle_table) {
-
-        if (type == ADDR_802_15_4_SHORT) {
-            if (cur->short_adr == short_id) {
-                return cur;
-            }
-        } else  {
-            if (memcmp(cur->mac64, address, 8) == 0) {
-                return cur;
-            }
-        }
-
-    }
-    return NULL;
-}
-
-static mle_neigh_table_entry_t *mle_class_get_free_entry(mle_neigh_table_list_t *mle_table)
-{
-    mle_neigh_table_entry_t *mle_entry = ns_list_get_first(mle_table);
-    if (mle_entry) {
-        //Remove from the list
-        ns_list_remove(mle_table, mle_entry);
-        uint8_t attribute_id = mle_entry->attribute_index;
-        memset(mle_entry, 0, sizeof(mle_neigh_table_entry_t));
-        mle_entry->attribute_index = attribute_id;
-        mle_entry->short_adr = 0xffff;
-        mle_entry->mode = MLE_FFD_DEV | MLE_RX_ON_IDLE;
-        mle_entry->holdTime = 7;
-        mle_entry->last_contact_time = protocol_core_monotonic_time;
-        mle_entry->mle_frame_counter = 0;
-        mle_entry->last_key_sequence = 0;
-        mle_entry->new_key_pending = false;
-        mle_entry->medium_ttl_challenge = false;
-    }
-
-    return mle_entry;
-}
-
-
-
-mle_neigh_table_entry_t *mle_class_get_by_link_address(int8_t interface_id, const uint8_t *address, addrtype_t type)
-{
-    switch (type) {
-        case ADDR_802_15_4_SHORT:
-        case ADDR_802_15_4_LONG:
-
-            break;
-        default:
-            return NULL;
-    }
-
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-    //Clean list and set function pointer call backs
-    if (!mle_class_ptr) {
-        return NULL;
-    }
-
-    return mle_class_neighbor_get(&mle_class_ptr->mle_table, address, type);
-
-}
-
-mle_neigh_table_entry_t *mle_class_get_by_device_attribute_id(int8_t interface_id, uint8_t attribute_index)
-{
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-    //Clean list and set function pointer call backs
-    if (!mle_class_ptr) {
-        return NULL;
-    }
-
-    return mle_class_neighbor_get_by_attribute_index(&mle_class_ptr->mle_table, attribute_index);
-
-}
-
-int8_t mle_class_remove_entry(int8_t interface_id, mle_neigh_table_entry_t *entry)
-{
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-    //Clean list and set function pointer call backs
-    if (!mle_class_ptr) {
-        return -1;
-    }
-
-    //Validate Pointer
-    if (!mle_class_neighbor_validate(&mle_class_ptr->mle_table, entry)) {
-        return -2;
-    }
-    //Remove from list
-    ns_list_remove(&mle_class_ptr->mle_table, entry);
-    //Free Indirect Queue
-    mle_table_remove_free_indirect_table(mle_class_ptr->interfaceId, entry);
-    //Call Remove callback
-    mle_class_ptr->remove_cb(mle_class_ptr->interfaceId, entry);
-    topo_trace(TOPOLOGY_MLE, entry->ext64, TOPO_REMOVE);
-
-    //Removes ETX neighbor
-    etx_neighbor_remove(interface_id, entry);
-    //Add to free list
-    ns_list_add_to_start(&mle_class_ptr->free_enty_list, entry);
-    return 0;
-}
-
-int8_t mle_class_remove_neighbour(int8_t interface_id, const uint8_t *address, addrtype_t type)
-{
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-    //Clean list and set function pointer call backs
-    if (!mle_class_ptr) {
-        return -1;
-    }
-
-    mle_neigh_table_entry_t * entry = mle_class_get_by_link_address(interface_id, address, type);
-    if (!entry) {
-        return -2;
-    }
-    //Remove from list
-    ns_list_remove(&mle_class_ptr->mle_table, entry);
-    //Free Indirect Queue
-    mle_table_remove_free_indirect_table(mle_class_ptr->interfaceId, entry);
-    //Call Remove callback
-    mle_class_ptr->remove_cb(mle_class_ptr->interfaceId, entry);
-
-    topo_trace(TOPOLOGY_MLE, entry->ext64, TOPO_REMOVE);
-
-     //Removes ETX neighbor
-     etx_neighbor_remove(interface_id, entry);
-    //Add to free list
-    ns_list_add_to_start(&mle_class_ptr->free_enty_list, entry);
-
     return 0;
-
-}
-
-
-
-static void mle_class_table_ttl(uint16_t ticks, mle_table_class_t *mle_class_ptr)
-{
-    uint16_t new_ttl;
-    bool challengeCheck;
-    bool remove_entry;
-
-    //validate that interface is still active
-    if (!mle_class_ptr->interface_is_active(mle_class_ptr->interfaceId)) {
-        return;
-    }
-
-    ns_list_foreach_safe(mle_neigh_table_entry_t, cur, &mle_class_ptr->mle_table) {
-        new_ttl = 0;
-        remove_entry = false;
-        switch (mle_class_ptr->mode) {
-            case MLE_CLASS_END_DEVICE:
-                if (cur->priorityFlag) {
-                    challengeCheck = true;
-                } else {
-                    challengeCheck = false;
-                }
-                break;
-            case MLE_CLASS_ROUTER: //Router and sleepy end device never do challenge
-            default:
-                challengeCheck = false;
-                break;
-        }
-
-        if (challengeCheck) {
-            if (cur->ttl > MLE_TABLE_CHALLENGE_TIMER || cur->ttl < MLE_TABLE_CHALLENGE_TIMER) {
-                new_ttl = ticks + MLE_TABLE_CHALLENGE_TIMER;
-            }
-
-            if (cur->ttl <= new_ttl) {
-                if (cur->ttl != 1) {
-                    if (mle_class_ptr->keep_alive_cb(mle_class_ptr->interfaceId, cur->mac64) != 0) {
-                        cur->ttl--;
-                        if (cur->ttl == 1) {
-                            remove_entry = true;
-                        }
-                    } else {
-                        cur->ttl = 1; //Lock retries here
-                    }
-                }
-            } else {
-                cur->ttl -= ticks;
-            }
-        } else {
-            if (ticks >= cur->ttl) {
-
-                remove_entry = true;
-            } else {
-                cur->ttl -= ticks;
-            }
-        }
-
-        if (remove_entry) {
-            //Silence delete
-            //Remove from list
-            ns_list_remove(&mle_class_ptr->mle_table, cur);
-            //Free Indirect Queue
-            mle_table_remove_free_indirect_table(mle_class_ptr->interfaceId, cur);
-            mle_class_ptr->remove_cb(mle_class_ptr->interfaceId, cur);
-            topo_trace(TOPOLOGY_MLE, cur->ext64, TOPO_REMOVE);
-            //Removes ETX neighbor
-            etx_neighbor_remove(mle_class_ptr->interfaceId, cur);
-            //Add to free list
-            ns_list_add_to_start(&mle_class_ptr->free_enty_list, cur);
-
-        }
-    } // for each entry
-
-    //Router to Router Challenge timeout for big network and FHSS systems which could loose broadcast messages.
-    if (mle_class_ptr->challenge_cb && mle_class_ptr->mode == MLE_CLASS_ROUTER) {
-        //Calculate timeout trigger
-        uint8_t challenge_count = 0;
-        ns_list_foreach_safe(mle_neigh_table_entry_t, cur, &mle_class_ptr->mle_table) {
-            if (((cur->mode & MLE_DEV_MASK) == MLE_FFD_DEV)  && !cur->medium_ttl_challenge) {
-                //Challenge Neighbour
-                if (cur->ttl < (cur->timeout_rx / 2)) {
-                    if (mle_class_ptr->challenge_cb(mle_class_ptr->interfaceId, cur->mac64) != 0) {
-                        tr_error("Router2Router challenge start fail");
-                        return;
-                    }
-                    cur->medium_ttl_challenge = true;
-                    if (++challenge_count == 2) {
-                        //trig only 2 active / 4 second period.
-                        return;
-                    }
-                }
-            }
-        }
-    }
-}
-
-
-mle_neigh_table_list_t *mle_class_active_list_get(int8_t interface_id)
-{
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-    //Clean list and set function pointer call backs
-    if (!mle_class_ptr) {
-        return NULL;
-    }
-    return &mle_class_ptr->mle_table;
-}
-
-uint16_t mle_class_active_neigh_counter(int8_t interface_id)
-{
-    mle_table_class_t *mle_class_ptr = mle_table_class_discover(interface_id);
-    //Clean list and set function pointer call backs
-    if (!mle_class_ptr) {
-        return 0xffff;
-    }
-
-
-    return ns_list_count(&mle_class_ptr->mle_table);
 }
 
 uint8_t *mle_general_write_source_address(uint8_t *ptr, protocol_interface_info_entry_t *cur)
@@ -760,26 +125,7 @@ uint8_t *mle_general_write_link_layer_framecounter(uint8_t *ptr, protocol_interf
     return mle_tlv_write_link_layer_framecount(ptr, temp_counter);
 }
 
-static void mle_event_handler(arm_event_s *event)
-{
-    switch (event->event_type) {
-        case ARM_MLE_INIT:
-            tr_debug("MLE Tasklet Generated");
-            break;
-
-        case ARM_MLE_TTL_TIMER:
-            //Do list in future for each of mle user
-            //Set here mle class ttl update
-            ns_list_foreach_safe(mle_table_class_t, mle_clas_entry, &mle_table_calss_list) {
-                mle_class_table_ttl(1, mle_clas_entry);
-            }
-
-
-            break;
-    }
-}
-
-bool mle_neigh_entry_frame_counter_update(mle_neigh_table_entry_t *entry_temp, uint8_t *tlv_ptr, uint16_t tlv_length, protocol_interface_info_entry_t *cur, uint8_t key_id)
+bool mle_neigh_entry_frame_counter_update(struct mac_neighbor_table_entry *entry_temp, uint8_t *tlv_ptr, uint16_t tlv_length, protocol_interface_info_entry_t *cur, uint8_t key_id)
 {
     mle_tlv_info_t mle_tlv_info;
     uint32_t frame_counter;
@@ -792,58 +138,31 @@ bool mle_neigh_entry_frame_counter_update(mle_neigh_table_entry_t *entry_temp, u
     } else {
         frame_counter = common_read_32_bit(mle_tlv_info.dataPtr);
     }
-
-    mac_helper_devicetable_set(entry_temp, cur, frame_counter, key_id, false);
+    mlme_device_descriptor_t device_desc;
+    mac_helper_device_description_write(cur, &device_desc, entry_temp->mac64, entry_temp->mac16,frame_counter, false);
+    mac_helper_devicetable_set(&device_desc, cur, entry_temp->index, key_id, false);
     return true;
 }
 
-void mle_entry_timeout_update(mle_neigh_table_entry_t *entry_temp, uint32_t timeout_tlv)
+
+void mle_mode_parse_to_mac_entry(mac_neighbor_table_entry_t *mac_entry, uint8_t mode)
 {
-    if (timeout_tlv > 86400) {
-        timeout_tlv = 86400;
-    } else if (timeout_tlv == 0) {
-        timeout_tlv = 500;
-    }
-    timeout_tlv /= MLE_TIMER_TICKS_SECONDS;
-    timeout_tlv++;
-    entry_temp->timeout_rx = timeout_tlv;
-    mle_entry_timeout_refresh(entry_temp);
+    mac_entry->rx_on_idle = mode & MLE_RX_ON_IDLE;
+    mac_entry->ffd_device = mode & MLE_FFD_DEV;
 }
 
-void mle_entry_timeout_refresh(mle_neigh_table_entry_t *entry_temp)
+uint8_t  mle_mode_write_from_mac_entry(mac_neighbor_table_entry_t *mac_entry)
 {
-    entry_temp->ttl = entry_temp->timeout_rx;
-    entry_temp->last_contact_time = protocol_core_monotonic_time;
-    entry_temp->medium_ttl_challenge = false;
-}
-
-static void mle_refresh_entry(mle_neigh_table_entry_t *neig_info, bool dataPollConfirmation)
-{
-    if (!neig_info) {
-        return;
-    }
-    if (!neig_info->handshakeReady) {
-        tr_debug("refresh:Link Handshake not ready yet");
-        return;
+    uint8_t mode = 0;
+    if (mac_entry->rx_on_idle) {
+        mode |= MLE_RX_ON_IDLE;
     }
 
-    neig_info->last_contact_time = protocol_core_monotonic_time;
-    neig_info->medium_ttl_challenge = false;
-
-    if (dataPollConfirmation) {
-        if (neig_info->ttl > MLE_TABLE_CHALLENGE_TIMER) {
-            neig_info->ttl = neig_info->timeout_rx;
-        }
-    } else {
-        neig_info->ttl = neig_info->timeout_rx;
+    if (mac_entry->ffd_device) {
+        mode |= MLE_FFD_DEV;
     }
+    return mode;
 }
 
-mle_neigh_table_entry_t *mle_refresh_entry_timeout(int8_t interfaceId, const uint8_t *addressPtr, addrtype_t addressType, bool dataPollConfirmation)
-{
-    mle_neigh_table_entry_t * neigh_info =  mle_class_get_by_link_address(interfaceId, addressPtr, addressType);
-    mle_refresh_entry(neigh_info, dataPollConfirmation);
-    return neigh_info;
-}
 
 #endif /* NO_MLE */
diff --git a/features/nanostack/sal-stack-nanostack/source/MLE/mle.h b/features/nanostack/sal-stack-nanostack/source/MLE/mle.h
index 3d16cdec40..6cd67cd5f1 100644
--- a/features/nanostack/sal-stack-nanostack/source/MLE/mle.h
+++ b/features/nanostack/sal-stack-nanostack/source/MLE/mle.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2017, Arm Limited and affiliates.
+ * Copyright (c) 2013-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -23,14 +23,7 @@
 #include "ns_list.h"
 
 struct buffer;
-
-#ifndef MAX_MLE_INFO_CNT
-#ifdef CORTEXM3_STM32W108CC
-#define MAX_MLE_INFO_CNT 40
-#else
-#define MAX_MLE_INFO_CNT 256
-#endif
-#endif
+struct mac_neighbor_table_entry;
 
 #define MLE_MAX_ROUTERS 64
 /* Route option layout: 1 Sequence byte, ID mask (bit per router), Data (byte per valid ID) */
@@ -131,118 +124,25 @@ typedef enum mle_tlv_type_t_ {
 #define MLE_FFD_DEV 2
 #define MLE_RFD_DEV 0
 #define MLE_RX_ON_IDLE 8
-/** Thead Spesific ModeFlags */
-#define MLE_THREAD_SECURED_DATA_REQUEST 0x04
-#define MLE_THREAD_REQ_FULL_DATA_SET 0x01
 
-#define MLE_TIMER_TICKS_SECONDS     4
-#define MLE_TIMER_TICKS_MS (MLE_TIMER_TICKS_SECONDS*1000)
-
-#define MLE_TABLE_CHALLENGE_TIMER       3
+#define MLE_TABLE_CHALLENGE_TIMER       12
 
 #define MLE_NEIGHBOR_PRIORITY_LINK (1 << 5)
 #define MLE_NEIGHBOR_OUTGOING_LINK (1 << 6)
 #define MLE_NEIGHBOR_INCOMING_LINK (1 << 7)
 
-typedef struct mle_neigh_table_entry_t {
-    uint8_t                             attribute_index;
-    uint16_t                           ttl;                       /*!< destination TTL * 4 seconds */
-    uint32_t                            last_contact_time;        /*!< monotonic time - hard to define "contact"; used for Thread Leasequery replies */
-    uint32_t                           mle_frame_counter;
-    uint16_t                           timeout_rx;
-    uint16_t                            holdTime;
-    uint16_t                            link_margin;
-    uint8_t                            mac64[8];                  /*!< MAC64 */
-    uint8_t                            mlEid[8];
-    uint16_t                           short_adr;
-    uint8_t                            mode;
-    uint16_t                           etx;                       /*!< 12 bits fraction */
-    uint16_t                           stored_diff_etx;           /*!< 12 bits fraction */
-    uint8_t                            remote_incoming_idr;       /*!< 5 bits fraction */
-    uint32_t                           last_key_sequence;
-    unsigned                           accumulated_failures: 5;
-    unsigned                           new_key_pending:1;
-    unsigned                           link_q_adv_sent: 1;
-    unsigned                           tmp_etx: 1;
-    unsigned                           priority_child_flag: 1;    /* Is using our node as preferred parent */
-    unsigned                           second_priority_flag: 1;   /* Is secondary parent */
-    unsigned                            thread_commission: 1;
-    unsigned                           threadNeighbor: 1;
-    unsigned                            priorityFlag: 1;
-    unsigned                            handshakeReady: 1;
-    unsigned                            medium_ttl_challenge: 1;
-    unsigned                            linkIdr: 4;
-    ns_list_link_t                     link;
-} mle_neigh_table_entry_t ;
-
-typedef NS_LIST_HEAD(mle_neigh_table_entry_t, link) mle_neigh_table_list_t;
-
-
-/* MLE TLV types */
-typedef enum mle_class_user_mode {
-    MLE_CLASS_ROUTER = 0,
-    MLE_CLASS_END_DEVICE,
-    MLE_CLASS_SLEEPY_END_DEVICE,
-} mle_class_user_mode;
-
-//MLE table class function pointer types
-/**
- * Remove entry notify
- */
-typedef void mle_entry_user_entry_remove_notify(int8_t interface_id, mle_neigh_table_entry_t *entry_ptr);
-
-typedef int8_t mle_entry_link_keep_alive(int8_t interface_id, const uint8_t *mac64);
-
-typedef bool mle_entry_interface_activate(int8_t interface_id);
-
-int8_t mle_class_init(int8_t interface_id, uint8_t table_size, mle_entry_user_entry_remove_notify *remove_cb, mle_entry_link_keep_alive *keep_alive_cb, mle_entry_interface_activate *interface_is_active);
-
-int8_t mle_class_router_challenge(int8_t interface_id,mle_entry_link_keep_alive *challenge_cb);
-
-bool mle_class_exists_for_interface(int8_t interface_id);
-
-int8_t mle_class_deallocate(int8_t interface_id);
-
-int8_t mle_class_list_clean(int8_t interface_id);
-
-int8_t mle_class_mode_set(int8_t interface_id,mle_class_user_mode mode);
-
-int8_t mle_class_set_new_key_pending(int8_t interface_id);
-
-int16_t mle_class_free_entry_count_get(int8_t interface_id);
-
-mle_neigh_table_entry_t *mle_class_get_entry_by_ll64(int8_t interface_id, uint8_t linkMargin, const uint8_t *ipv6Address, bool allocateNew, bool *new_entry_allocated);
-
-mle_neigh_table_entry_t *mle_class_discover_entry_by_ll64(int8_t interface_id, const uint8_t *ipv6Address);
-
-mle_neigh_table_entry_t *mle_class_get_entry_by_mac64(int8_t interface_id, uint8_t linkMargin, const uint8_t *mac64, bool allocateNew, bool *new_entry_allocated);
-
-mle_neigh_table_entry_t *mle_class_get_by_link_address(int8_t interface_id, const uint8_t *address, addrtype_t type);
-
-mle_neigh_table_entry_t *mle_class_get_by_device_attribute_id(int8_t interface_id, uint8_t attribute_index);
-
-int8_t mle_class_remove_entry(int8_t interface_id, mle_neigh_table_entry_t *entry);
-
-int8_t mle_class_remove_neighbour(int8_t interface_id, const uint8_t *address, addrtype_t type);
-
-mle_neigh_table_list_t *mle_class_active_list_get(int8_t interface_id);
-
-int16_t mle_class_sleepy_entry_count_get(int8_t interface_id);
-int16_t mle_class_rfd_entry_count_get(int8_t interface_id);
-
-uint16_t mle_class_active_neigh_counter(int8_t interface_id);
-
-/**
- *  Function to refresh Neigh Entry when get some trusted response or spesified place
- */
-mle_neigh_table_entry_t *mle_refresh_entry_timeout(int8_t interfaceId, const uint8_t *addressPtr, addrtype_t addressType, bool dataPollConfirmation);
-
-bool mle_neigh_entry_frame_counter_update(mle_neigh_table_entry_t *entry_temp, uint8_t *tlv_ptr, uint16_t tlv_length, struct protocol_interface_info_entry *cur, uint8_t key_id);
 
+int8_t mle_class_set_new_key_pending(struct protocol_interface_info_entry *cur);
+int16_t mle_class_free_entry_count_get(struct protocol_interface_info_entry *cur);
+int16_t mle_class_sleepy_entry_count_get(struct protocol_interface_info_entry *cur);
+int16_t mle_class_rfd_entry_count_get(struct protocol_interface_info_entry *cur);
+uint16_t mle_class_active_neigh_counter(struct protocol_interface_info_entry *cur);
+bool mle_neigh_entry_frame_counter_update(struct mac_neighbor_table_entry *entry_temp, uint8_t *tlv_ptr, uint16_t tlv_length, struct protocol_interface_info_entry *cur, uint8_t key_id);
 uint8_t *mle_general_write_source_address(uint8_t *ptr, struct protocol_interface_info_entry *cur);
 
 uint8_t *mle_general_write_link_layer_framecounter(uint8_t *ptr, struct protocol_interface_info_entry *cur);
-void mle_entry_timeout_update(mle_neigh_table_entry_t *entry_temp, uint32_t timeout_tlv);
-void mle_entry_timeout_refresh(mle_neigh_table_entry_t *entry_temp);
+
+void mle_mode_parse_to_mac_entry(struct mac_neighbor_table_entry *mac_entry, uint8_t mode);
+uint8_t  mle_mode_write_from_mac_entry(struct mac_neighbor_table_entry *mac_entry);
 
 #endif /* MLE_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/MPL/mpl.c b/features/nanostack/sal-stack-nanostack/source/MPL/mpl.c
index 9ad06d49d3..13ad35e10e 100644
--- a/features/nanostack/sal-stack-nanostack/source/MPL/mpl.c
+++ b/features/nanostack/sal-stack-nanostack/source/MPL/mpl.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol.h b/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol.h
index 1e7290c118..de582791d7 100644
--- a/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol.h
+++ b/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -45,6 +45,7 @@
 #include "Service_Libs/Neighbor_cache/neighbor_table_definition.h"
 #include "Service_Libs/Trickle/trickle.h"
 #include "Service_Libs/pan_blacklist/pan_blacklist_api.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 #include "net_polling_api.h"
 #include "ipv6_stack/ipv6_routing_table.h"
 
@@ -246,6 +247,7 @@ typedef struct arm_15_4_mac_parameters_t {
     beacon_compare_rx_cb *beacon_compare_rx_cb_ptr;
     beacon_join_priority_tx_cb *beacon_join_priority_tx_cb_ptr;
     uint8_t (*beacon_ind)(uint8_t *ptr, uint8_t len, protocol_interface_info_entry_t *cur);
+    mac_neighbor_table_t *mac_neighbor_table;
 }arm_15_4_mac_parameters_t;
 
 typedef void mac_poll_fail_cb(int8_t nwk_interface_id);
@@ -316,6 +318,7 @@ typedef struct {
 } ipv6_interface_info_t;
 
 struct thread_info_s;
+struct ws_info_s;
 struct mesh_callbacks_s;
 struct auth_info;
 struct rpl_domain;
@@ -339,7 +342,6 @@ typedef void beacon_indication_cb(int8_t if_id, const mlme_beacon_ind_t* conf);
 typedef void comm_status_indication_cb(int8_t if_id, const mlme_comm_status_t* status);
 
 
-
 struct protocol_interface_info_entry {
     beacon_indication_cb *beacon_cb;
     scan_confirm_cb *scan_cb;
@@ -444,6 +446,9 @@ struct protocol_interface_info_entry {
     pan_coordinator_blaclist_cache_s pan_cordinator_black_list;
 #ifdef HAVE_THREAD
     struct thread_info_s *thread_info;
+#endif
+#ifdef HAVE_WS
+    struct ws_info_s *ws_info;
 #endif
     struct rpl_domain *rpl_domain;
     struct mesh_callbacks_s *mesh_callbacks;
@@ -470,6 +475,7 @@ struct protocol_interface_info_entry {
     uint8_t (*if_llao_parse)(struct protocol_interface_info_entry *cur, const uint8_t *opt_in, sockaddr_t *ll_addr_out);
     uint8_t (*if_llao_write)(struct protocol_interface_info_entry *cur, uint8_t *opt_out, uint8_t opt_type, bool must, const uint8_t *ip_addr);
     void (*mac_security_key_usage_update_cb)(struct protocol_interface_info_entry *cur, const struct mlme_security_s *security_params);
+    uint16_t (*etx_read_override)(struct protocol_interface_info_entry *cur, addrtype_t addr_type, const uint8_t *addr_ptr);
 };
 
 typedef NS_LIST_HEAD(protocol_interface_info_entry_t, link) protocol_interface_list_t;
diff --git a/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol_abstract.h b/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol_abstract.h
index 3c9100caa7..78aa552c38 100644
--- a/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol_abstract.h
+++ b/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/Include/protocol_abstract.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -21,6 +21,7 @@
 #include "ns_types.h"
 
 struct rpl_domain;
+struct fhss_api;
 
 /*!
  * \enum nwk_interface_id
@@ -43,5 +44,6 @@ extern int protocol_core_buffers_in_event_queue;
 protocol_interface_info_entry_t *protocol_stack_interface_info_get_by_id(int8_t nwk_id);
 protocol_interface_info_entry_t *protocol_stack_interface_info_get_by_bootstrap_id(int8_t id);
 protocol_interface_info_entry_t *protocol_stack_interface_info_get_by_rpl_domain(const struct rpl_domain *domain, int8_t last_id);
+protocol_interface_info_entry_t *protocol_stack_interface_info_get_by_fhss_api(const struct fhss_api *fhss_api);
 
 #endif /* NWK_INTERFACE_INCLUDE_PROTOCOL_ABSTRACT_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/protocol_core.c b/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/protocol_core.c
index 2950b79964..5cb03ce1a3 100644
--- a/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/protocol_core.c
+++ b/features/nanostack/sal-stack-nanostack/source/NWK_INTERFACE/protocol_core.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -64,6 +64,8 @@
 #include "6LoWPAN/Thread/thread_bootstrap.h"
 #include "6LoWPAN/Thread/thread_routing.h"
 #include "6LoWPAN/Thread/thread_management_internal.h"
+#include "6LoWPAN/ws/ws_bootstrap.h"
+#include "6LoWPAN/ws/ws_common.h"
 #include "ipv6_stack/protocol_ipv6.h"
 #include "Service_Libs/whiteboard/whiteboard.h"
 
@@ -242,10 +244,16 @@ void core_timer_event_handle(uint16_t ticksUpdate)
                 if (cur->lowpan_info & INTERFACE_NWK_ACTIVE) {
                     if (thread_info(cur)) {
                         thread_seconds_timer(cur, seconds);
+                    } else if (ws_info(cur)) {
+                        ws_common_seconds_timer(cur, seconds);
                     } else if (cur->lowpan_info & INTERFACE_NWK_ROUTER_DEVICE) {
                         beacon_join_priority_update(cur->id);
                     }
 
+                    if (cur->mac_parameters) {
+                        mac_neighbor_table_neighbor_timeout_update(mac_neighbor_info(cur), seconds);
+                    }
+
                     if (cur->nwk_wpan_nvm_api) {
                         cur->nwk_wpan_nvm_api->nvm_params_update_cb(cur->nwk_wpan_nvm_api, false);
                     }
@@ -304,6 +312,8 @@ void core_timer_event_handle(uint16_t ticksUpdate)
                 nd_object_timer(cur,ticksUpdate);
                 if (thread_info(cur)) {
                     thread_timer(cur, ticksUpdate);
+                } else if (ws_info(cur)) {
+                    ws_common_fast_timer(cur, ticksUpdate);
                 }
                 lowpan_context_timer(&cur->lowpan_contexts, ticksUpdate);
             }
@@ -383,6 +393,11 @@ void protocol_core_interface_info_reset(protocol_interface_info_entry_t *entry)
         ns_list_foreach_safe(if_address_entry_t, addr, &entry->ip_addresses) {
             addr_delete_entry(entry, addr);
         }
+#ifdef MULTICAST_FORWARDING
+        ns_list_foreach_safe(if_group_fwd_entry_t, group, &entry->ip_groups_fwd) {
+            addr_multicast_fwd_remove(entry, group->group);
+        }
+#endif
 #ifdef HAVE_RPL
         /* This is done after address deletion, so RPL can act on them */
         rpl_control_remove_domain_from_interface(entry);
@@ -759,6 +774,20 @@ protocol_interface_info_entry_t *protocol_stack_interface_info_get_by_rpl_domain
     return NULL;
 }
 
+protocol_interface_info_entry_t *protocol_stack_interface_info_get_by_fhss_api(const struct fhss_api *fhss_api)
+{
+#ifdef HAVE_WS
+    ns_list_foreach(protocol_interface_info_entry_t, cur, &protocol_interface_info_list) {
+        if (cur->ws_info->fhss_api == fhss_api) {
+            return cur;
+        }
+    }
+#else
+    (void)fhss_api;
+#endif //HAVE_WS
+    return NULL;
+}
+
 protocol_interface_info_entry_t *protocol_stack_interface_sleep_possibility(void)
 {
     ns_list_foreach(protocol_interface_info_entry_t, cur, &protocol_interface_info_list) {
@@ -1059,6 +1088,8 @@ void net_bootsrap_cb_run(uint8_t event)
             //eventOS_scheduler_set_active_tasklet(protocol_read_tasklet_id());
             if (thread_info(cur)) {
                 thread_bootstrap_state_machine(cur);
+            } else if (ws_info(cur)) {
+                ws_bootstrap_state_machine(cur);
             } else {
                 protocol_6lowpan_bootstrap(cur);
             }
diff --git a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.c b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.c
index 703f81c261..e95865bfad 100644
--- a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.c
+++ b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -45,7 +45,7 @@
 #include "NWK_INTERFACE/Include/protocol_stats.h"
 #include "Common_Protocols/ipv6_constants.h"
 #include "Common_Protocols/icmpv6.h"
-
+#include "ipv6_stack/protocol_ipv6.h"
 #include "Service_Libs/etx/etx.h" /* slight ick */
 
 #include "net_rpl.h"
@@ -182,6 +182,51 @@ void rpl_control_unpublish_address(rpl_domain_t *domain, const uint8_t addr[16])
     }
 }
 
+static if_address_entry_t *rpl_instance_reg_addr_get(protocol_interface_info_entry_t *interface)
+{
+    ns_list_foreach(if_address_entry_t, address, &interface->ip_addresses) {
+        if (!address->addr_reg_done && !addr_is_ipv6_link_local(address->address)) {
+            return address;
+        }
+    }
+
+    return NULL;
+}
+
+/* Send address registration to either specified address, or to non-registered address */
+void rpl_control_register_address(protocol_interface_info_entry_t *interface, if_address_entry_t *addr)
+{
+    if_address_entry_t *reg_addr = addr;
+
+    if (!reg_addr) {
+        reg_addr = rpl_instance_reg_addr_get(interface);
+
+        if (!reg_addr) {
+            return;
+        }
+    }
+    ns_list_foreach(struct rpl_instance, instance, &interface->rpl_domain->instances) {
+        rpl_instance_send_address_registration(interface, instance, reg_addr);
+    }
+}
+
+void rpl_control_address_register_done(struct buffer *buf, uint8_t status)
+{
+    ns_list_foreach(if_address_entry_t, addr, &buf->interface->ip_addresses) {
+
+        /* Optimize, ll addresses are not registered anyway.. */
+        if (addr_is_ipv6_link_local(addr->address) || !addr->addr_reg_pend) {
+            continue;
+        }
+
+        ns_list_foreach(struct rpl_instance, instance, &buf->interface->rpl_domain->instances) {
+            if (rpl_instance_address_registration_done(buf->interface, instance, addr, status)) {
+                return;
+            }
+        }
+    }
+}
+
 /* Address changes need to trigger DAO target re-evaluation */
 static void rpl_control_addr_notifier(struct protocol_interface_info_entry *interface, const if_address_entry_t *addr, if_address_callback_t reason)
 {
@@ -209,11 +254,10 @@ static void rpl_control_addr_notifier(struct protocol_interface_info_entry *inte
     }
 }
 
-static void rpl_control_etx_change_callback(int8_t  nwk_id, uint16_t previous_etx, uint16_t current_etx, const uint8_t *mac64_addr_ptr, uint16_t mac16_addr)
+static void rpl_control_etx_change_callback(int8_t  nwk_id, uint16_t previous_etx, uint16_t current_etx, uint8_t attribute_index)
 {
     (void)previous_etx;
     (void)current_etx;
-    (void)mac16_addr;
 
     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(nwk_id);
     if (!cur || !cur->rpl_domain) {
@@ -221,7 +265,7 @@ static void rpl_control_etx_change_callback(int8_t  nwk_id, uint16_t previous_et
     }
     rpl_domain_t *domain = cur->rpl_domain;
     uint16_t delay = rpl_policy_etx_change_parent_selection_delay(domain);
-    tr_debug("Triggering parent selection due to ETX change on %s", trace_array(mac64_addr_ptr, 8));
+    tr_debug("Triggering parent selection due to ETX change on neigh index %u, etx %u", attribute_index, current_etx);
 
     ns_list_foreach(rpl_instance_t, instance, &domain->instances) {
         rpl_instance_trigger_parent_selection(instance, delay);
@@ -411,7 +455,7 @@ void rpl_control_delete_dodag_root(rpl_domain_t *domain, rpl_dodag_t *dodag)
 {
     (void)domain;
 
-    rpl_delete_dodag(dodag);
+    rpl_delete_dodag_root(dodag);
 }
 
 void rpl_control_update_dodag_route(rpl_dodag_t *dodag, const uint8_t *prefix, uint8_t prefix_len, uint8_t flags, uint32_t lifetime, bool age)
@@ -628,15 +672,26 @@ static void rpl_control_process_prefix_options(protocol_interface_info_entry_t *
         }
         uint8_t prefix_len = ptr[2];
         uint8_t flags = ptr[3];
-        uint32_t preferred = common_read_32_bit(ptr + 4);
-        uint32_t valid = common_read_32_bit(ptr + 8);
+        uint32_t valid = common_read_32_bit(ptr + 4);
+        uint32_t preferred = common_read_32_bit(ptr + 8);
         const uint8_t *prefix = ptr + 16;
 
         if (!pref_parent || neighbour == pref_parent) {
-            /* XXX We don't yet locally handle A and L flags. Presumably should
-             * only locally process for DODAG's we're a member of? Should we
-             * process now, or later?
+            //Check is L Flag active
+            if (flags & PIO_L) {
+                //define ONLink Route Information
+                //tr_debug("Register On Link Prefix to routing table");
+                ipv6_route_add(prefix, prefix_len, cur->id, NULL, ROUTE_RADV, valid, 0);
+            }
+            /* Check if A-Flag.
+             * A RPL node may use this option for the purpose of Stateless Address Autoconfiguration (SLAAC)
+             * from a prefix advertised by a parent.
              */
+            if (pref_parent && (flags & PIO_A)) {
+                if (icmpv6_slaac_prefix_update(cur, prefix, prefix_len, valid, preferred) != 0) {
+                    ipv6_interface_slaac_handler(cur, prefix, prefix_len, valid, preferred);
+                }
+            }
 
             /* Store prefixes for possible forwarding */
             /* XXX if leaf - don't bother? Or do we want to remember them for
@@ -1602,6 +1657,12 @@ const uint8_t *rpl_control_preferred_parent_addr(const rpl_instance_t *instance,
     }
 }
 
+uint16_t rpl_control_current_rank(const struct rpl_instance *instance)
+{
+    return rpl_instance_current_rank(instance);
+}
+
+
 static void rpl_domain_print(const rpl_domain_t *domain, route_print_fn_t *print_fn)
 {
     print_fn("RPL Domain %p", (void *) domain);
diff --git a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.h b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.h
index 6b6ff40f82..72f7788551 100644
--- a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.h
+++ b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_control.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -143,6 +143,8 @@ void rpl_control_set_callback(rpl_domain_t *domain, rpl_domain_callback_t callba
 /* Target publishing */
 void rpl_control_publish_host_address(rpl_domain_t *domain, const uint8_t addr[16], uint32_t lifetime);
 void rpl_control_unpublish_address(rpl_domain_t *domain, const uint8_t addr[16]);
+void rpl_control_register_address(struct protocol_interface_info_entry *interface, if_address_entry_t *addr);
+void rpl_control_address_register_done(struct buffer *buf, uint8_t status);
 
 /* Configure and return the routing lookup predicate for a specified RPL instance ID */
 ipv6_route_predicate_fn_t *rpl_control_get_route_predicate(rpl_domain_t *domain, uint8_t instance_id, const uint8_t src[16], const uint8_t dst[16]);
@@ -156,12 +158,16 @@ bool rpl_control_get_instance_dao_target_count(rpl_domain_t *domain, uint8_t ins
 bool rpl_control_read_dodag_info(const struct rpl_instance *instance, struct rpl_dodag_info_t *dodag_info);
 const rpl_dodag_conf_t *rpl_control_get_dodag_config(const struct rpl_instance *instance);
 const uint8_t *rpl_control_preferred_parent_addr(const struct rpl_instance *instance, bool global);
+uint16_t rpl_control_current_rank(const struct rpl_instance *instance);
+
 
 #else /* HAVE_RPL */
 
 #define rpl_control_fast_timer(ticks) ((void) 0)
 #define rpl_control_slow_timer(seconds) ((void) 0)
 #define rpl_control_remove_domain_from_interface(cur) ((void) 0)
+#define rpl_control_register_address(interface, addr) ((void) 0)
+#define rpl_control_address_register_done NULL
 
 #endif /* HAVE_RPL */
 
diff --git a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_data.c b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_data.c
index 4c3a84a57e..d599133e7e 100644
--- a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_data.c
+++ b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_data.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_downward.c b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_downward.c
index d9c98dc63d..8e040e5c67 100644
--- a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_downward.c
+++ b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_downward.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -88,6 +88,7 @@
 #include "randLIB.h"
 #include "ip6string.h"
 
+#include "Common_Protocols/icmpv6.h"
 #include "NWK_INTERFACE/Include/protocol.h"
 #include "ipv6_stack/ipv6_routing_table.h"
 
@@ -580,6 +581,61 @@ static void rpl_downward_reset_assigning(rpl_instance_t *instance, uint8_t pcs_m
 }
 
 
+void rpl_instance_send_address_registration(protocol_interface_info_entry_t *interface, rpl_instance_t *instance, if_address_entry_t *addr)
+{
+    aro_t aro;
+    buffer_t *buf;
+
+    aro.status = ARO_SUCCESS;
+    aro.present = true;
+    aro.lifetime = addr->preferred_lifetime;
+    memcpy(aro.eui64, interface->mac, 8);
+
+    // go through neighbour list, and send to all assigned parents.
+    ns_list_foreach(rpl_neighbour_t, neighbour, &instance->candidate_neighbours) {
+        if (neighbour->dao_path_control) {
+            tr_debug("Send ARO %s to %s", trace_ipv6(addr->address), trace_ipv6(neighbour->ll_address));
+            buf = icmpv6_build_ns(interface, neighbour->ll_address, addr->address, true, false, &aro);
+            addr->addr_reg_pend |= neighbour->dao_path_control;
+            protocol_push(buf);
+        } else {
+            tr_debug("Skip ARO to %s - no pc", trace_ipv6(neighbour->ll_address));
+        }
+    }
+}
+
+bool rpl_instance_address_registration_done(protocol_interface_info_entry_t *interface, rpl_instance_t *instance, if_address_entry_t *addr, uint8_t status)
+{
+    ns_list_foreach(rpl_neighbour_t, neighbour, &instance->candidate_neighbours) {
+        // Check path control mask
+        if (!(addr->addr_reg_pend & neighbour->dao_path_control)) {
+            continue;
+        }
+
+        tr_debug("Address %s register to %s", trace_ipv6(addr->address), trace_ipv6(neighbour->ll_address));
+
+        /* Clear pending flag */
+        addr->addr_reg_pend &= ~neighbour->dao_path_control;
+
+        if (status == SOCKET_TX_DONE) {
+            addr->addr_reg_done |= neighbour->dao_path_control;
+            /* State_timer is 1/10 s. Set renewal to 75-85% of lifetime */
+            addr->state_timer = (addr->preferred_lifetime * randLIB_get_random_in_range(75, 85) / 10);
+        } else {
+            tr_error("Address registration failed");
+        }
+
+        /* If that was last one to reply, send next one. */
+        if (!addr->addr_reg_pend) {
+            rpl_control_register_address(interface, NULL);
+        }
+
+        return true;
+    }
+
+    return false;
+}
+
 /* We are optimised for sending updates to existing targets to current parents;
  * we track the state of what information DAO parents have, and manage the
  * updates together with message coalescing and ack tracking.
diff --git a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_downward.h b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_downward.h
index 8dcfa893b6..15d40bae7a 100644
--- a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_downward.h
+++ b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_downward.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -41,6 +41,9 @@ void rpl_instance_dao_request(struct rpl_instance *instance, struct rpl_neighbou
 void rpl_instance_dao_trigger(struct rpl_instance *instance, uint16_t delay);
 void rpl_instance_dao_acked(struct rpl_instance *instance, const uint8_t src[16], int8_t interface_id, uint8_t dao_sequence, uint8_t status);
 
+void rpl_instance_send_address_registration(protocol_interface_info_entry_t *interface, rpl_instance_t *instance, if_address_entry_t *addr);
+bool rpl_instance_address_registration_done(protocol_interface_info_entry_t *interface, rpl_instance_t *instance, if_address_entry_t *addr, uint8_t status);
+
 #ifdef HAVE_RPL_DAO_HANDLING
 bool rpl_instance_dao_received(struct rpl_instance *instance, const uint8_t src[16], int8_t interface_id, bool multicast, const uint8_t *opts, uint16_t opts_len, uint8_t *status_out);
 #endif
diff --git a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_upward.c b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_upward.c
index d364c01ab3..126e195312 100644
--- a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_upward.c
+++ b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_upward.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -542,6 +542,14 @@ void rpl_delete_dodag_version(rpl_dodag_version_t *version)
     rpl_dodag_t *dodag = version->dodag;
     rpl_instance_t *instance = dodag->instance;
 
+    if (instance->current_dodag_version == version) {
+        // Don't call rpl_instance_set_dodag_version(NULL) - that would pre-empt parent reselection,
+        // triggering poison immediately.
+        // Give parent selection a chance to select another version (but will it have any info on-hand?)
+        instance->current_dodag_version = NULL;
+        rpl_instance_trigger_parent_selection(instance, 5);
+    }
+
     ns_list_foreach_safe(rpl_neighbour_t, neighbour, &instance->candidate_neighbours) {
         if (neighbour->dodag_version == version) {
             rpl_delete_neighbour(instance, neighbour);
@@ -649,6 +657,21 @@ void rpl_delete_dodag(rpl_dodag_t *dodag)
     rpl_free(dodag, sizeof(*dodag));
 }
 
+void rpl_delete_dodag_root(rpl_dodag_t *dodag)
+{
+    // This should trigger immediate poison
+    rpl_instance_set_dodag_version(dodag->instance, NULL, RPL_RANK_INFINITE);
+    // Deleting DODAG is not ideal - we will just pick up adverts from our
+    // former children, and recreate, possibly violating the MaxRankIncrease.
+    // Should retain DODAG version info and just unset root flag, which will
+    // limit what happens when we hear adverts.
+    // Problem is rpl_control_create_dodag_root which can't handle the
+    // case where DODAG already exists. This would always be a problem if
+    // we'd heard adverts in between delete and create, but would be an instant
+    // problem without this delete. Need to fix.
+    rpl_delete_dodag(dodag);
+}
+
 /* Convert RPL configuration to generic trickle parameters. Returns true if
  * the value in the generic object has changed.
  */
@@ -1008,31 +1031,15 @@ rpl_instance_t *rpl_create_instance(rpl_domain_t *domain, uint8_t instance_id)
     if (!instance) {
         return NULL;
     }
+    memset(instance, 0, sizeof(rpl_instance_t));
     ns_list_init(&instance->dodags);
     ns_list_init(&instance->candidate_neighbours);
     ns_list_init(&instance->dao_targets);
     instance->dtsn = rpl_seq_init();
-    instance->srh_error_count = 0;
-    instance->poison_count = 0;
-    instance->repair_dis_timer = 0;
-    instance->repair_dis_count = 0;
     instance->last_dao_trigger_time = protocol_core_monotonic_time;
-    instance->root_paths_valid = false;
-    instance->root_topo_sort_valid = false;
     instance->dao_sequence = rpl_seq_init();
-    instance->dao_sequence_in_transit = 0;
-    instance->dao_in_transit = false;
-    instance->dao_retry_timer = 0;
-    instance->dao_attempt = 0;
-    instance->delay_dao_timer = 0;
-    instance->parent_selection_timer = 0;
-    instance->neighbours_changed = false;
-    instance->local_repair = false;
     instance->id = instance_id;
     instance->domain = domain;
-    instance->current_dodag_version = NULL;
-    instance->dio_not_consistent = false;
-    instance->of = NULL;
 
     ns_list_add_to_start(&domain->instances, instance);
     return instance;
@@ -1345,7 +1352,6 @@ void rpl_instance_run_parent_selection(rpl_instance_t *instance)
         protocol_stats_update(STATS_RPL_PARENT_CHANGE, 1);
     }
 
-#ifndef NO_MLE
     // Sets new preferred parent
     if (preferred_parent) {
         ipv6_map_ip_to_ll_and_call_ll_addr_handler(NULL, preferred_parent->interface_id, NULL, preferred_parent->ll_address,
@@ -1359,7 +1365,6 @@ void rpl_instance_run_parent_selection(rpl_instance_t *instance)
             protocol_6lowpan_neighbor_priority_clear_all(preferred_parent->interface_id, PRIORITY_2ND);
         }
     }
-#endif
 
     rpl_instance_set_local_repair(instance, preferred_parent == NULL);
 
diff --git a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_upward.h b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_upward.h
index b269f6e55d..1718d75373 100644
--- a/features/nanostack/sal-stack-nanostack/source/RPL/rpl_upward.h
+++ b/features/nanostack/sal-stack-nanostack/source/RPL/rpl_upward.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -88,6 +88,7 @@ void rpl_instance_slow_timer(rpl_instance_t *instance, uint16_t seconds);
 rpl_dodag_t *rpl_lookup_dodag(const rpl_instance_t *instance, const uint8_t *dodagid);
 rpl_dodag_t *rpl_create_dodag(rpl_instance_t *instance, const uint8_t *dodagid, uint8_t g_mop_prf);
 void rpl_delete_dodag(rpl_dodag_t *dodag);
+void rpl_delete_dodag_root(rpl_dodag_t *dodag);
 uint8_t rpl_dodag_mop(const rpl_dodag_t *dodag);
 void rpl_dodag_set_root(rpl_dodag_t *dodag, bool root);
 #ifdef HAVE_RPL_ROOT
diff --git a/features/nanostack/sal-stack-nanostack/source/Security/PANA/eap_protocol.c b/features/nanostack/sal-stack-nanostack/source/Security/PANA/eap_protocol.c
index d28dfba0c2..7a2e8c6199 100644
--- a/features/nanostack/sal-stack-nanostack/source/Security/PANA/eap_protocol.c
+++ b/features/nanostack/sal-stack-nanostack/source/Security/PANA/eap_protocol.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017, Arm Limited and affiliates.
+ * Copyright (c) 2017-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/features/nanostack/sal-stack-nanostack/source/Security/PANA/pana.c b/features/nanostack/sal-stack-nanostack/source/Security/PANA/pana.c
index 23a9cc5d87..b760f10f06 100644
--- a/features/nanostack/sal-stack-nanostack/source/Security/PANA/pana.c
+++ b/features/nanostack/sal-stack-nanostack/source/Security/PANA/pana.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2017, Arm Limited and affiliates.
+ * Copyright (c) 2013-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -750,24 +750,23 @@ int8_t pana_ccm_data_crypt(uint8_t *ptr, uint16_t len, uint8_t operation_type, u
 {
     uint8_t *explict_ptr;
     uint8_t *key_ptr = 0;
-    ccm_globals_t *ccm_ptr = 0;
+    ccm_globals_t ccm_ptr;
     key_ptr = suite->pana_session.pana_PAA_enc_key;
 
     //Here Comes AES Decrypt
-    ccm_ptr = ccm_sec_init(AES_SECURITY_LEVEL_ENC, key_ptr, operation_type , 3);
-    if (!ccm_ptr) {
+    if (!ccm_sec_init(&ccm_ptr, AES_SECURITY_LEVEL_ENC, key_ptr, operation_type , 3)) {
         return -1;
     }
 
-    explict_ptr = ccm_ptr->exp_nonce;
+    explict_ptr = ccm_ptr.exp_nonce;
     //Set IV
     explict_ptr = common_write_32_bit(suite->pana_session.pana_key_id, explict_ptr);
     //SET EXP 4 octest Session ID, 4 Octet Pana SQN number
     explict_ptr = common_write_32_bit(suite->pana_session.session_id, explict_ptr);
     explict_ptr = common_write_32_bit(message_seq, explict_ptr);
-    ccm_ptr->data_len = len;
-    ccm_ptr->data_ptr  = ptr;
-    return ccm_process_run(ccm_ptr);
+    ccm_ptr.data_len = len;
+    ccm_ptr.data_ptr  = ptr;
+    return ccm_process_run(&ccm_ptr);
 }
 
 buffer_t *pana_relay_parse(buffer_t *buf)
diff --git a/features/nanostack/sal-stack-nanostack/source/Security/TLS/tls_ccm_crypt.c b/features/nanostack/sal-stack-nanostack/source/Security/TLS/tls_ccm_crypt.c
index 6627a49d19..45693639af 100644
--- a/features/nanostack/sal-stack-nanostack/source/Security/TLS/tls_ccm_crypt.c
+++ b/features/nanostack/sal-stack-nanostack/source/Security/TLS/tls_ccm_crypt.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017, Arm Limited and affiliates.
+ * Copyright (c) 2017-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -30,9 +30,8 @@
 #define TRACE_GROUP "cryp"
 #define TLS_1_2_VER                 0x0303
 
-static ccm_globals_t * tls_ccm_init(const uint8_t *key_expansion, const uint8_t *nonce, bool server, uint8_t crypt_process)
+static bool tls_ccm_init(ccm_globals_t *ccm_ptr, const uint8_t *key_expansion, const uint8_t *nonce, bool server, uint8_t crypt_process)
 {
-    ccm_globals_t *ccm_ptr;
     const uint8_t *key, *iv_ptr;
     if (server) {
         key = key_expansion + SERVER_WRITE_KEY;
@@ -41,12 +40,14 @@ static ccm_globals_t * tls_ccm_init(const uint8_t *key_expansion, const uint8_t
         key = key_expansion + CLIENT_WRITE_KEY;
         iv_ptr = key_expansion + CLIENT_IV;
     }
-    ccm_ptr = ccm_sec_init(AES_SECURITY_LEVEL_ENC_MIC64, key, crypt_process , 3);
-    if (ccm_ptr) {
-        memcpy(ccm_ptr->exp_nonce, iv_ptr, 4);
-        memcpy(&ccm_ptr->exp_nonce[4], nonce, 8);
+
+    if (!ccm_sec_init(ccm_ptr, AES_SECURITY_LEVEL_ENC_MIC64, key, crypt_process , 3) ) {
+        return false;
     }
-    return ccm_ptr;
+
+    memcpy(ccm_ptr->exp_nonce, iv_ptr, 4);
+    memcpy(&ccm_ptr->exp_nonce[4], nonce, 8);
+    return true;
 }
 
 
@@ -64,16 +65,16 @@ static void tls_set_adata(ccm_globals_t *ccm_ptr, uint8_t *a_data, const uint8_t
 
 int8_t tls_ccm_data_encrypt(uint8_t *data_ptr, uint16_t data_length, const uint8_t *key_expansion, const uint8_t *nonce, uint8_t type, bool server)
 {
-    ccm_globals_t * ccm_ptr = tls_ccm_init(key_expansion, nonce, server, AES_CCM_ENCRYPT);
-    if (!ccm_ptr) {
+    ccm_globals_t ccm_ptr;
+    if (!tls_ccm_init(&ccm_ptr, key_expansion, nonce, server, AES_CCM_ENCRYPT)) {
         return -1;
     }
     uint8_t adata[13];
-    ccm_ptr->data_len = data_length;
-    ccm_ptr->data_ptr  = data_ptr;
-    ccm_ptr->mic = (ccm_ptr->data_ptr + ccm_ptr->data_len);
-    tls_set_adata(ccm_ptr,adata, nonce, type);
-    return ccm_process_run(ccm_ptr);
+    ccm_ptr.data_len = data_length;
+    ccm_ptr.data_ptr  = data_ptr;
+    ccm_ptr.mic = (ccm_ptr.data_ptr + ccm_ptr.data_len);
+    tls_set_adata(&ccm_ptr,adata, nonce, type);
+    return ccm_process_run(&ccm_ptr);
 }
 
 int8_t tls_ccm_data_decrypt(uint8_t *data_ptr, uint16_t data_length, const uint8_t *key_expansion, uint8_t type, bool server)
@@ -82,18 +83,19 @@ int8_t tls_ccm_data_decrypt(uint8_t *data_ptr, uint16_t data_length, const uint8
     if (data_length <= 16) {
         return -1;
     }
-    ccm_globals_t *ccm_ptr = tls_ccm_init(key_expansion, data_ptr, server, AES_CCM_DECRYPT);
-    if (!ccm_ptr) {
+    ccm_globals_t ccm_ptr;
+
+    if (!tls_ccm_init(&ccm_ptr, key_expansion, data_ptr, server, AES_CCM_DECRYPT)) {
         return -1;
     }
 
     uint8_t adata[13];
-    ccm_ptr->data_len = data_length - 16;
+    ccm_ptr.data_len = data_length - 16;
 
-    tls_set_adata(ccm_ptr, adata, data_ptr, type);
-    ccm_ptr->data_ptr  = data_ptr + 8;
-    ccm_ptr->mic = (ccm_ptr->data_ptr + ccm_ptr->data_len);
+    tls_set_adata(&ccm_ptr, adata, data_ptr, type);
+    ccm_ptr.data_ptr  = data_ptr + 8;
+    ccm_ptr.mic = (ccm_ptr.data_ptr + ccm_ptr.data_len);
 
-    return ccm_process_run(ccm_ptr);
+    return ccm_process_run(&ccm_ptr);
 }
 #endif
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/CCM_lib/ccm_security.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/CCM_lib/ccm_security.c
index 9561075745..14deaa5226 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/CCM_lib/ccm_security.c
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/CCM_lib/ccm_security.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2015, 2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2015, 2017-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -43,25 +43,12 @@
 #include "ccmLIB.h"
 #include "platform/arm_hal_aes.h"
 
-#ifndef CCM_USE_MUTEX
-#define arm_ccm_mutex_lock()
-#define arm_ccm_mutex_unlock()
-#endif
-
-static ccm_globals_t ccm_globals;
-
-/* CCM Library Parameters */
-static uint8_t CCM_L_PARAM = 2;
-static uint8_t ccm_sec_level;
-static uint8_t CCM_ENCODE_MODE;
-static const uint8_t *ccm_key_ptr;
-
-static void ccm_generate_A0(uint8_t *ptr);
-static void ccm_auth_generate_B0(uint8_t *ptr, uint16_t len);
-static void ccm_auth_calc_Xi(uint8_t X[static 16], uint8_t Blen, const uint8_t B[static Blen]);
+static void ccm_generate_A0(uint8_t *ptr, ccm_globals_t *ccm_pramters);
+static void ccm_auth_generate_B0(uint8_t *ptr, ccm_globals_t *ccm_params);
+static void ccm_auth_calc_Xi(void *aes_context, uint8_t X[static 16], uint8_t Blen, const uint8_t B[static Blen]);
 static uint8_t ccm_mic_len_calc(uint8_t sec_level);
-static void ccm_encode(uint16_t len , uint8_t *ptr);
-static int8_t ccm_calc_auth_MIC(const uint8_t *data_ptr, uint16_t data_len, const uint8_t *adata_ptr, uint16_t adata_len);
+static void ccm_encode(ccm_globals_t *ccm_params);
+static int8_t ccm_calc_auth_MIC(ccm_globals_t *ccm_params);
 
 /**
  * \brief A function to init CCM library.
@@ -73,24 +60,24 @@ static int8_t ccm_calc_auth_MIC(const uint8_t *data_ptr, uint16_t data_len, cons
  * \return Pointer to Global CCM paramameter buffer.
  * \return 0 When parameter fail or CCM is Busy.
  */
-ccm_globals_t   *ccm_sec_init(uint8_t sec_level, const uint8_t *ccm_key, uint8_t mode, uint8_t ccm_l)
+bool ccm_sec_init(ccm_globals_t *ccm_context, uint8_t sec_level, const uint8_t *ccm_key, uint8_t mode, uint8_t ccm_l)
 {
-    ccm_globals_t *ret_val = 0;
+    memset(ccm_context, 0, sizeof(ccm_globals_t));
+
     if ((ccm_l == 2 || ccm_l == 3) && (sec_level < 8)) {
-        arm_ccm_mutex_lock();
-        memset(&ccm_globals, 0, sizeof(ccm_globals_t));
-        CCM_ENCODE_MODE = mode;
-        ccm_sec_level = sec_level;
-        CCM_L_PARAM = ccm_l;
-        ccm_key_ptr = ccm_key;
-        arm_aes_start(ccm_key);
-        ccm_globals.mic_len = ccm_mic_len_calc(ccm_sec_level);
-        ccm_globals.mic = 0;
-        ret_val = &ccm_globals;
-    } else {
-        ccm_key_ptr = 0;
+        void *aes_context = arm_aes_start(ccm_key);
+        if (!aes_context) {
+            return false;
+        }
+        ccm_context->aes_context = aes_context;
+        ccm_context->ccm_encode_mode = mode;
+        ccm_context->ccm_sec_level = sec_level;
+        ccm_context->ccm_l_param = ccm_l;
+        ccm_context->key_ptr = ccm_key;
+        ccm_context->mic_len = ccm_mic_len_calc(sec_level);
+        return true;
     }
-    return ret_val;
+    return false;
 }
 
 /**
@@ -128,23 +115,23 @@ int8_t ccm_process_run(ccm_globals_t *ccm_params)
         goto END;
     }
 
-    if (CCM_ENCODE_MODE == AES_CCM_ENCRYPT) {
+    if (ccm_params->ccm_encode_mode == AES_CCM_ENCRYPT) {
         if (ccm_params->mic_len) {
             //Calc
-            if (ccm_calc_auth_MIC(ccm_params->data_ptr, ccm_params->data_len, ccm_params->adata_ptr, ccm_params->adata_len)) {
+            if (ccm_calc_auth_MIC(ccm_params)) {
                 goto END;
             }
         }
         if (ccm_params->data_len) {
-            ccm_encode(ccm_params->data_len, ccm_params->data_ptr);
+            ccm_encode(ccm_params);
         }
         ret_val = 0;
     } else {
         if (ccm_params->data_len) {
-            ccm_encode(ccm_params->data_len, ccm_params->data_ptr);
+            ccm_encode(ccm_params);
         }
         if (ccm_params->mic_len) {
-            if (ccm_calc_auth_MIC(ccm_params->data_ptr, ccm_params->data_len, ccm_params->adata_ptr, ccm_params->adata_len) == 0) {
+            if (ccm_calc_auth_MIC(ccm_params) == 0) {
                 ret_val = 0;
             }
         } else {
@@ -153,26 +140,32 @@ int8_t ccm_process_run(ccm_globals_t *ccm_params)
     }
 
 END:
-    ccm_key_ptr = 0;
-    arm_aes_finish();
-    arm_ccm_mutex_unlock();
+    ccm_free(ccm_params);
     return ret_val;
 }
 
+void ccm_free(ccm_globals_t *ccm_params)
+{
+    if (ccm_params && ccm_params->aes_context) {
+        arm_aes_finish(ccm_params->aes_context);
+    }
+}
+
 
 /* Counter-mode encryption/decryption
  *  Ci := E(Key, Ai) ^ Mi
  */
-static void ccm_encode(uint16_t len , uint8_t *ptr)
+static void ccm_encode(ccm_globals_t *ccm_params)
 {
-    if (!ccm_key_ptr || ccm_sec_level < AES_SECURITY_LEVEL_ENC) {
+    if (!ccm_params->key_ptr || ccm_params->ccm_sec_level < AES_SECURITY_LEVEL_ENC) {
         return;
     }
-
+    uint16_t len = ccm_params->data_len;
+    uint8_t *ptr = ccm_params->data_ptr;
     uint8_t Ai[16], Si[16];
 
     //first, generate A0
-    ccm_generate_A0(Ai);
+    ccm_generate_A0(Ai, ccm_params);
 
     while (len) {
         //increment counter in Ai - 16-bit increment enough; len is 16-bit
@@ -181,7 +174,7 @@ static void ccm_encode(uint16_t len , uint8_t *ptr)
         }
 
         // Si := E(Key, Ai)
-        arm_aes_encrypt(Ai, Si);
+        arm_aes_encrypt(ccm_params->aes_context, Ai, Si);
 
         // output := Si ^ input
         for (int_fast8_t i = 0; i < 16 && len; i++, len--) {
@@ -191,15 +184,19 @@ static void ccm_encode(uint16_t len , uint8_t *ptr)
 }
 
 
-static int8_t ccm_calc_auth_MIC(const uint8_t *data_ptr, uint16_t data_len, const uint8_t *adata_ptr, uint16_t adata_len)
+static int8_t ccm_calc_auth_MIC(ccm_globals_t *ccm_params)
 {
+    const uint8_t *data_ptr = ccm_params->data_ptr;
+    uint16_t data_len = ccm_params->data_len;
+    const uint8_t *adata_ptr = ccm_params->adata_ptr;
+    uint16_t adata_len = ccm_params->adata_len;
     uint8_t Xi[16];
 
     // As a convenience, treat "data" as "adata", reflecting that "Private
     // Payload" is part of "a data" not "m data" for unencrypted modes.
     // The distinction matters because there's an "align to block" between
     // "a" and "m", which we don't do when it's all in "a".
-    if (ccm_sec_level < AES_SECURITY_LEVEL_ENC && data_len != 0) {
+    if (ccm_params->ccm_sec_level < AES_SECURITY_LEVEL_ENC && data_len != 0) {
         // This trick only works if data follows adata
         if (data_ptr == adata_ptr + adata_len) {
             adata_len += data_len;
@@ -209,11 +206,11 @@ static int8_t ccm_calc_auth_MIC(const uint8_t *data_ptr, uint16_t data_len, cons
         }
     }
 
-    ccm_auth_generate_B0(Xi, data_len);  //Set B0
+    ccm_auth_generate_B0(Xi, ccm_params);  //Set B0
 
     // Calculate X1: E(key, B0)
     // [Could use ccm_auth_calc_Xi - it's formally X1 := E(key, B0 ^ X0), where X0 = 0]
-    arm_aes_encrypt(Xi, Xi);
+    arm_aes_encrypt(ccm_params->aes_context, Xi, Xi);
 
     //First authentication block has 2-byte length field concatenated
     if (adata_len) {
@@ -224,7 +221,7 @@ static int8_t ccm_calc_auth_MIC(const uint8_t *data_ptr, uint16_t data_len, cons
         B1[1] = adata_len;
         memcpy(&B1[2], adata_ptr, t_len);
 
-        ccm_auth_calc_Xi(Xi, 2 + t_len, B1);
+        ccm_auth_calc_Xi(ccm_params->aes_context, Xi, 2 + t_len, B1);
         adata_ptr += t_len;
         adata_len -= t_len;
     }
@@ -232,7 +229,7 @@ static int8_t ccm_calc_auth_MIC(const uint8_t *data_ptr, uint16_t data_len, cons
     while (adata_len) {
         uint_fast8_t t_len = adata_len > 16 ? 16 : adata_len;
 
-        ccm_auth_calc_Xi(Xi, t_len, adata_ptr);
+        ccm_auth_calc_Xi(ccm_params->aes_context, Xi, t_len, adata_ptr);
         adata_ptr += t_len;
         adata_len -= t_len;
     }
@@ -240,7 +237,7 @@ static int8_t ccm_calc_auth_MIC(const uint8_t *data_ptr, uint16_t data_len, cons
     while (data_len) {
         uint_fast8_t t_len = data_len > 16 ? 16 : data_len;
 
-        ccm_auth_calc_Xi(Xi, t_len, data_ptr);
+        ccm_auth_calc_Xi(ccm_params->aes_context, Xi, t_len, data_ptr);
         data_ptr += t_len;
         data_len -= t_len;
     }
@@ -249,17 +246,17 @@ static int8_t ccm_calc_auth_MIC(const uint8_t *data_ptr, uint16_t data_len, cons
 
     // Encryption block S0 is E(Key, A0)
     uint8_t S0[16];
-    ccm_generate_A0(S0);
-    arm_aes_encrypt(S0, S0);
+    ccm_generate_A0(S0, ccm_params);
+    arm_aes_encrypt(ccm_params->aes_context, S0, S0);
 
     // Encrypted authentication tag U is S0^T (leftmost M octets)
-    if (CCM_ENCODE_MODE == AES_CCM_ENCRYPT) {
-        for (uint_fast8_t i = 0; i < ccm_globals.mic_len; i++) {
-            ccm_globals.mic[i] = Xi[i] ^ S0[i];
+    if (ccm_params->ccm_encode_mode == AES_CCM_ENCRYPT) {
+        for (uint_fast8_t i = 0; i < ccm_params->mic_len; i++) {
+            ccm_params->mic[i] = Xi[i] ^ S0[i];
         }
     } else {
-        for (uint_fast8_t i = 0; i < ccm_globals.mic_len; i++)
-            if (ccm_globals.mic[i] != (Xi[i] ^ S0[i])) {
+        for (uint_fast8_t i = 0; i < ccm_params->mic_len; i++)
+            if (ccm_params->mic[i] != (Xi[i] ^ S0[i])) {
                 return -1;
             }
     }
@@ -273,27 +270,27 @@ static int8_t ccm_calc_auth_MIC(const uint8_t *data_ptr, uint16_t data_len, cons
  *
  * \return none.
  */
-static void ccm_generate_A0(uint8_t *ptr)
+static void ccm_generate_A0(uint8_t *ptr, ccm_globals_t *ccm_pramters)
 {
     uint8_t n_len, flags;
-    flags = CCM_L_PARAM - 1;
-    n_len = 15 - CCM_L_PARAM;
+    flags = ccm_pramters->ccm_l_param - 1;
+    n_len = 15 - ccm_pramters->ccm_l_param;
 
     //FLAGS = L' = L - 1;
     *ptr++ = flags;
-    memcpy(ptr, ccm_globals.exp_nonce, n_len);
+    memcpy(ptr, ccm_pramters->exp_nonce, n_len);
     ptr += n_len;
-    memset(ptr, 0, CCM_L_PARAM);
+    memset(ptr, 0, ccm_pramters->ccm_l_param);
 }
 
 /* Calculate X[i+1]: X[i+1] := E(Key, X[i] ^ B[i]) */
 /* Blen is <= 16; this handles zero-padding B when it is < 16 */
-static void ccm_auth_calc_Xi(uint8_t X[static 16], uint8_t Blen, const uint8_t B[static Blen])
+static void ccm_auth_calc_Xi(void *aes_context, uint8_t X[static 16], uint8_t Blen, const uint8_t B[static Blen])
 {
     for (uint_fast8_t i = 0; i < Blen; i++) {
         X[i] ^= B[i];
     }
-    arm_aes_encrypt(X, X);
+    arm_aes_encrypt(aes_context, X, X);
 }
 
 /* flags = reserved(1) || Adata(1) || M (3) || L (3)
@@ -301,27 +298,27 @@ static void ccm_auth_calc_Xi(uint8_t X[static 16], uint8_t Blen, const uint8_t B
  *               L = CCM_L_PARAM - 1
  */
 /* B0 := flags(1)|| Nonce(15-L) || length of message(L) */
-static void ccm_auth_generate_B0(uint8_t *ptr, uint16_t len)
+static void ccm_auth_generate_B0(uint8_t *ptr, ccm_globals_t *ccm_params)
 {
     uint8_t flags = 0;
     uint8_t n_len;
 
-    n_len = 15 - CCM_L_PARAM;
+    n_len = 15 - ccm_params->ccm_l_param;
 
-    if (ccm_globals.mic_len) {
-        flags = ccm_globals.mic_len - 2;
+    if (ccm_params->mic_len) {
+        flags = ccm_params->mic_len - 2;
         flags <<= 2;
     }
     flags |= 0x40;
-    flags |= (CCM_L_PARAM - 1);
+    flags |= (ccm_params->ccm_l_param - 1);
     *ptr++ = flags;
-    memcpy(ptr, ccm_globals.exp_nonce, n_len);
+    memcpy(ptr, ccm_params->exp_nonce, n_len);
     ptr += n_len;
-    if (CCM_L_PARAM == 3) {
+    if (ccm_params->ccm_l_param == 3) {
         *ptr++ = 0;
     }
-    *ptr++ = len >> 8;
-    *ptr = len;
+    *ptr++ = ccm_params->data_len >> 8;
+    *ptr = ccm_params->data_len;
 }
 
 
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/CCM_lib/mbedOS/aes_mbedtls_adapter.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/CCM_lib/mbedOS/aes_mbedtls_adapter.c
index 294e7e2233..801946fb91 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/CCM_lib/mbedOS/aes_mbedtls_adapter.c
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/CCM_lib/mbedOS/aes_mbedtls_adapter.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -25,7 +25,9 @@
  *
  * 2) Platform with hardware AES assist, mbed TLS not in use:
  *     Do not use this source file - implement arm_hal_aes.h yourself using
- *     your AES hardware.
+ *     your AES hardware. Note that you must be able to provide
+ *     ARM_AES_MBEDTLS_CONTEXT_MIN contexts. This may or may not be 1, depending
+ *     on Nanostack config.
  *
  * 3) Platform without hardware assist, already using (or wanting to use) mbed TLS:
  *     Use this source file, and define NS_USE_EXTERNAL_MBED_TLS so that
@@ -38,18 +40,11 @@
  *     MBEDTLS_AES_C enabled. Attach your hardware-accelerated AES to mbed TLS
  *     by defining MBEDTLS_AES_ALT; it will then be used both by users
  *     of arm_hal_aes.h, and other users of mbed TLS.
- *
- * 5) Platform with non-context-capable hardware assist, already using mbed TLS:
- *     If it's not possible, or too complex, to handle multiple contexts for the
- *     AES decode, then you will not be able to accelerate all mbed TLS users.
- *     Instead you can reserve the AES hardware for providing arm_hal_aes.h, so
- *     this becomes the same as case 2. Don't use this source file - implement
- *     arm_hal_aes.h yourself using your AES hardware. The external mbed TLS
- *     will use its software implementation.
  */
 
 /* Get the API we are implementing from libService */
 #include "platform/arm_hal_aes.h"
+#include "platform/arm_hal_interrupt.h"
 
 /* Either pull in the external mbed TLS header for its AES functions, or
  * pull in our own local cut-down copy of the mbed TLS code.
@@ -60,20 +55,48 @@
 #include "aes_mbedtls.c"
 #endif /* NS_USE_EXTERNAL_MBED_TLS */
 
-static mbedtls_aes_context context;
+struct arm_aes_context {
+    mbedtls_aes_context ctx;
+    bool reserved;
+};
 
-void arm_aes_start(const uint8_t key[static 16])
+static arm_aes_context_t context_list[ARM_AES_MBEDTLS_CONTEXT_MIN];
+
+static arm_aes_context_t * mbed_tls_context_get(void)
 {
-    mbedtls_aes_init(&context);
-    mbedtls_aes_setkey_enc(&context, key, 128);
+    platform_enter_critical();
+    for (int i = 0; i < ARM_AES_MBEDTLS_CONTEXT_MIN; i++) {
+        if (!context_list[i].reserved) {
+            //Reserve context
+            context_list[i].reserved = true;
+            platform_exit_critical();
+            return &context_list[i];
+        }
+    }
+
+    platform_exit_critical();
+    return NULL;
 }
 
-void arm_aes_encrypt(const uint8_t src[static 16], uint8_t dst[static 16])
+arm_aes_context_t *arm_aes_start(const uint8_t key[static 16])
 {
-    mbedtls_aes_crypt_ecb(&context, MBEDTLS_AES_ENCRYPT, src, dst);
+    arm_aes_context_t *context = mbed_tls_context_get();
+    if (context) {
+        mbedtls_aes_init(&context->ctx);
+        mbedtls_aes_setkey_enc(&context->ctx, key, 128);
+    }
+    return context;
 }
 
-void arm_aes_finish(void)
+void arm_aes_encrypt(arm_aes_context_t *aes_context, const uint8_t src[static 16], uint8_t dst[static 16])
 {
-    mbedtls_aes_free(&context);
+    mbedtls_aes_crypt_ecb(&aes_context->ctx, MBEDTLS_AES_ENCRYPT, src, dst);
+}
+
+void arm_aes_finish(arm_aes_context_t *aes_context)
+{
+    mbedtls_aes_free(&aes_context->ctx);
+    platform_enter_critical();
+    aes_context->reserved = false;
+    platform_exit_critical();
 }
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/etx/etx.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/etx/etx.c
index 4047a8688c..981ab71ad8 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/etx/etx.c
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/etx/etx.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -28,6 +28,7 @@
 #include "NWK_INTERFACE/Include/protocol.h"
 #include "NWK_INTERFACE/Include/protocol_stats.h"
 #include "Service_Libs/etx/etx.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
 #include "Service_Libs/utils/isqrt.h"
 
 //TODO: Refactor this away!
@@ -37,14 +38,16 @@
 
 static uint16_t etx_current_calc(uint16_t etx, uint8_t accumulated_failures);
 static uint16_t etx_dbm_lqi_calc(uint8_t lqi, int8_t dbm);
-static void etx_value_change_callback_needed_check(uint16_t etx, uint16_t *stored_diff_etx, uint8_t accumulated_failures, const uint8_t *mac64_addr_ptr, uint16_t mac16_addr);
-static void etx_accum_failures_callback_needed_check(struct mle_neigh_table_entry_t *neigh_table_ptr);
+static void etx_value_change_callback_needed_check(uint16_t etx, uint16_t *stored_diff_etx, uint8_t accumulated_failures, uint8_t attribute_index);
+static void etx_accum_failures_callback_needed_check(etx_storage_t * entry, uint8_t attribute_index);
 
 typedef struct {
     uint16_t hysteresis;                            // 12 bit fraction
     uint8_t accum_threshold;
     etx_value_change_handler_t *callback_ptr;
     etx_accum_failures_handler_t *accum_cb_ptr;
+    etx_storage_t *etx_storage_list;
+    uint8_t ext_storage_list_size;
     int8_t interface_id;
 } ext_info_t;
 
@@ -53,6 +56,8 @@ static ext_info_t etx_info = {
     .accum_threshold = 0,
     .callback_ptr = NULL,
     .accum_cb_ptr = NULL,
+    .etx_storage_list = NULL,
+    .ext_storage_list_size = 0,
     .interface_id = -1
 };
 
@@ -67,71 +72,57 @@ static ext_info_t etx_info = {
  * \param addr_type address type, ADDR_802_15_4_SHORT or ADDR_802_15_4_LONG
  * \param addr_ptr PAN ID with 802.15.4 address
  */
-void etx_transm_attempts_update(int8_t interface_id, uint8_t attempts, bool success, addrtype_t addr_type, const uint8_t *addr_ptr)
+void etx_transm_attempts_update(int8_t interface_id, uint8_t attempts, bool success, uint8_t attribute_index)
 {
-    mle_neigh_table_entry_t *neigh_table_ptr = NULL;
     uint32_t etx;
     uint8_t accumulated_failures;
-
-    if (!addr_ptr) {
-        return;
-    }
-
     // Gets table entry
-    neigh_table_ptr = mle_class_get_by_link_address(interface_id, addr_ptr + PAN_ID_LEN, addr_type);
-
-    if (neigh_table_ptr == NULL) {
+    etx_storage_t * entry = etx_storage_entry_get(interface_id, attribute_index);
+    if (!entry) {
         return;
     }
 
-    accumulated_failures = neigh_table_ptr->accumulated_failures;
+    accumulated_failures = entry->accumulated_failures;
 
     if (!success) {
         /* Stores failed attempts to estimate ETX and to calculate
            new ETX after successful sending */
         if (accumulated_failures + attempts < 32) {
-            neigh_table_ptr->accumulated_failures += attempts;
+            entry->accumulated_failures += attempts;
         } else {
             success = true;
         }
     }
 
     if (success) {
-        neigh_table_ptr->accumulated_failures = 0;
+        entry->accumulated_failures = 0;
     } else {
-        etx_accum_failures_callback_needed_check(neigh_table_ptr);
+        etx_accum_failures_callback_needed_check(entry, attribute_index);
     }
 
-    if (neigh_table_ptr->etx) {
+    if (entry->etx) {
         // If hysteresis is set stores ETX value for comparison
-        if (etx_info.hysteresis && !neigh_table_ptr->stored_diff_etx) {
-            neigh_table_ptr->stored_diff_etx = neigh_table_ptr->etx;
+        if (etx_info.hysteresis && !entry->stored_diff_etx) {
+            entry->stored_diff_etx = entry->etx;
         }
 
         if (success) {
             // ETX = 7/8 * current ETX + 1/8 * ((attempts + failed attempts) << 12)
-            etx = neigh_table_ptr->etx - (neigh_table_ptr->etx >> ETX_MOVING_AVERAGE_FRACTION);
+            etx = entry->etx - (entry->etx >> ETX_MOVING_AVERAGE_FRACTION);
             etx += (attempts + accumulated_failures) << (12 - ETX_MOVING_AVERAGE_FRACTION);
 
             if (etx > 0xffff) {
-                neigh_table_ptr->etx = 0xffff;
+                entry->etx = 0xffff;
             } else {
-                neigh_table_ptr->etx = etx;
+                entry->etx = etx;
             }
         }
 
         // If real ETX value has been received do not update based on LQI or dBm
-        neigh_table_ptr->tmp_etx = false;
+        entry->tmp_etx = false;
 
         // Checks if ETX value change callback is needed
-        etx_value_change_callback_needed_check(neigh_table_ptr->etx, &(neigh_table_ptr->stored_diff_etx), neigh_table_ptr->accumulated_failures, &(neigh_table_ptr->mac64[0]), neigh_table_ptr->short_adr);
-
-        // Updates ETX statistics
-        if (neigh_table_ptr->priorityFlag) {
-            protocol_stats_update(STATS_ETX_1ST_PARENT, neigh_table_ptr->etx >> 4);
-        } else if (neigh_table_ptr->second_priority_flag) {
-            protocol_stats_update(STATS_ETX_2ND_PARENT, neigh_table_ptr->etx >> 4);
-        }
+        etx_value_change_callback_needed_check(entry->etx, &(entry->stored_diff_etx), entry->accumulated_failures, attribute_index);
     }
 }
 
@@ -144,34 +135,34 @@ void etx_transm_attempts_update(int8_t interface_id, uint8_t attempts, bool succ
  * \param remote_incoming_idr Remote incoming IDR
  * \param mac64_addr_ptr long MAC address
  */
-void etx_remote_incoming_idr_update(int8_t interface_id, uint8_t remote_incoming_idr, mle_neigh_table_entry_t *neigh_table_ptr)
+void etx_remote_incoming_idr_update(int8_t interface_id, uint8_t remote_incoming_idr, uint8_t attribute_index)
 {
-    (void) interface_id;
+    etx_storage_t * entry = etx_storage_entry_get(interface_id, attribute_index);
 
-    if (neigh_table_ptr) {
+    if (entry) {
         // If ETX has been set
-        if (neigh_table_ptr->etx) {
+        if (entry->etx) {
             // If hysteresis is set stores ETX value to enable comparison
-            if (etx_info.hysteresis && !neigh_table_ptr->stored_diff_etx) {
-                neigh_table_ptr->stored_diff_etx = neigh_table_ptr->etx;
+            if (etx_info.hysteresis && !entry->stored_diff_etx) {
+                entry->stored_diff_etx = entry->etx;
             }
             // remote EXT = remote incoming IDR^2 (12 bit fraction)
             uint32_t remote_ext = ((uint32_t)remote_incoming_idr * remote_incoming_idr) << 2;
 
             // ETX = 7/8 * current ETX + 1/8 * remote ETX */
-            uint32_t etx = neigh_table_ptr->etx - (neigh_table_ptr->etx >> ETX_MOVING_AVERAGE_FRACTION);
+            uint32_t etx = entry->etx - (entry->etx >> ETX_MOVING_AVERAGE_FRACTION);
             etx += remote_ext >> ETX_MOVING_AVERAGE_FRACTION;
 
             if (etx > 0xffff) {
-                neigh_table_ptr->etx = 0xffff;
+                entry->etx = 0xffff;
             } else {
-                neigh_table_ptr->etx = etx;
+                entry->etx = etx;
             }
 
             // Checks if ETX value change callback is needed
-            etx_value_change_callback_needed_check(neigh_table_ptr->etx, &(neigh_table_ptr->stored_diff_etx), neigh_table_ptr->accumulated_failures, &(neigh_table_ptr->mac64[0]), neigh_table_ptr->short_adr);
+            etx_value_change_callback_needed_check(entry->etx, &(entry->stored_diff_etx), entry->accumulated_failures, attribute_index);
         }
-        neigh_table_ptr->remote_incoming_idr = remote_incoming_idr;
+        entry->remote_incoming_idr = remote_incoming_idr;
     }
 }
 
@@ -191,25 +182,42 @@ void etx_remote_incoming_idr_update(int8_t interface_id, uint8_t remote_incoming
  */
 uint16_t etx_read(int8_t interface_id, addrtype_t addr_type, const uint8_t *addr_ptr)
 {
-    mle_neigh_table_entry_t *neigh_table_ptr;
-    uint16_t etx = 0;
+    protocol_interface_info_entry_t *interface = protocol_stack_interface_info_get_by_id(interface_id);
 
-    if (!addr_ptr) {
+    if (!addr_ptr || !interface) {
         return 0;
     }
 
-    if (!mle_class_exists_for_interface(interface_id)) {
+    if (interface->etx_read_override) {
+        // Interface has modified ETX calculation
+        return interface->etx_read_override(interface, addr_type, addr_ptr);
+    }
+
+    uint8_t attribute_index;
+    if (interface->nwk_id == IF_IPV6) {
         return 1;
     }
 
-    neigh_table_ptr = mle_class_get_by_link_address(interface_id, addr_ptr + PAN_ID_LEN, addr_type);
-
-    if (neigh_table_ptr) {
-        etx = etx_current_calc(neigh_table_ptr->etx, neigh_table_ptr->accumulated_failures);
-        etx >>= 4;
-    } else {
-        etx = 0xffff;
+    //Must Support old MLE table and new still same time
+    mac_neighbor_table_entry_t *mac_neighbor = mac_neighbor_table_address_discover(mac_neighbor_info(interface), addr_ptr + PAN_ID_LEN, addr_type);
+    if (!mac_neighbor) {
+        return 0xffff;
     }
+    attribute_index = mac_neighbor->index;
+
+
+    //tr_debug("Etx Read from atribute %u", attribute_index);
+
+    etx_storage_t * entry = etx_storage_entry_get(interface_id, attribute_index);
+
+    if (!entry) {
+        return 0xffff;
+    }
+
+    uint16_t etx  = etx_current_calc(entry->etx, entry->accumulated_failures);
+    etx >>= 4;
+
+    //tr_debug("Etx value %u", etx);
 
     return etx;
 }
@@ -224,12 +232,12 @@ uint16_t etx_read(int8_t interface_id, addrtype_t addr_type, const uint8_t *addr
  * \return 0x0100 to 0xFFFF incoming IDR value (8 bit fraction)
  * \return 0x0000 address unknown
  */
-uint16_t etx_local_incoming_idr_read(int8_t interface_id, mle_neigh_table_entry_t *neigh_table_ptr)
+uint16_t etx_local_incoming_idr_read(int8_t interface_id, uint8_t attribute_index)
 {
     uint32_t local_incoming_idr = 0;
-    (void) interface_id;
-    if (neigh_table_ptr) {
-        uint16_t local_etx = etx_current_calc(neigh_table_ptr->etx, neigh_table_ptr->accumulated_failures);
+    etx_storage_t * entry = etx_storage_entry_get(interface_id, attribute_index);
+    if (entry) {
+        uint16_t local_etx = etx_current_calc(entry->etx, entry->accumulated_failures);
 
         local_incoming_idr = isqrt32((uint32_t)local_etx << 16);
         // divide by sqrt(2^12)
@@ -239,6 +247,25 @@ uint16_t etx_local_incoming_idr_read(int8_t interface_id, mle_neigh_table_entry_
     return local_incoming_idr;
 }
 
+/**
+ * \brief A function to read local incoming IDR value
+ *
+ *  Returns local incoming IDR value for an address
+ *
+ * \param mac64_addr_ptr long MAC address
+ *
+ * \return 0x0100 to 0xFFFF incoming IDR value (8 bit fraction)
+ * \return 0x0000 address unknown
+ */
+uint16_t etx_local_etx_read(int8_t interface_id, uint8_t attribute_index)
+{
+    etx_storage_t * entry = etx_storage_entry_get(interface_id, attribute_index);
+    if (!entry) {
+        return 0;
+    }
+    return etx_current_calc(entry->etx, entry->accumulated_failures) >> 4;
+}
+
 /**
  * \brief A function to calculate current ETX
  *
@@ -280,37 +307,38 @@ static uint16_t etx_current_calc(uint16_t etx, uint8_t accumulated_failures)
  *
  * \return 0x0100 to 0xFFFF local incoming IDR value (8 bit fraction)
  */
-uint16_t etx_lqi_dbm_update(int8_t interface_id, uint8_t lqi, int8_t dbm, mle_neigh_table_entry_t *neigh_table_ptr)
+uint16_t etx_lqi_dbm_update(int8_t interface_id, uint8_t lqi, int8_t dbm, uint8_t attribute_index)
 {
     uint32_t local_incoming_idr = 0;
     uint32_t etx = 0;
-    (void) interface_id;
 
-    if (neigh_table_ptr) {
+    etx_storage_t *entry = etx_storage_entry_get(interface_id, attribute_index);
+
+    if (entry) {
         // If local ETX is not set calculate it based on LQI and dBm
-        if (!neigh_table_ptr->etx) {
+        if (!entry->etx) {
             etx = etx_dbm_lqi_calc(lqi, dbm);
-            neigh_table_ptr->etx = etx;
-            neigh_table_ptr->tmp_etx = true;
+            entry->etx = etx;
+            entry->tmp_etx = true;
         }
         // If local ETX has been calculated without remote incoming IDR and
         // remote incoming IDR is available update it by remote incoming IDR value
-        if (neigh_table_ptr->remote_incoming_idr && neigh_table_ptr->tmp_etx) {
-            neigh_table_ptr->tmp_etx = false;
+        if (entry->remote_incoming_idr && entry->tmp_etx) {
+            entry->tmp_etx = false;
 
-            local_incoming_idr = isqrt32((uint32_t)neigh_table_ptr->etx << 16);
+            local_incoming_idr = isqrt32((uint32_t)entry->etx << 16);
             // divide by sqrt(2^12) and scale to 12 bit fraction
             local_incoming_idr = local_incoming_idr >> 2;
 
-            etx = local_incoming_idr * (((uint16_t)neigh_table_ptr->remote_incoming_idr) << 7);
-            neigh_table_ptr->etx = etx >> 12;
+            etx = local_incoming_idr * (((uint16_t)entry->remote_incoming_idr) << 7);
+            entry->etx = etx >> 12;
 
             local_incoming_idr >>= 4;
         }
 
         // If local ETX has been calculated indicates new neighbor
         if (etx) {
-            etx_neighbor_add(interface_id, neigh_table_ptr);
+            etx_neighbor_add(interface_id, attribute_index);
         }
     }
 
@@ -402,6 +430,50 @@ uint8_t etx_value_change_callback_register(nwk_interface_id nwk_id, int8_t inter
     }
 }
 
+bool etx_storage_list_allocate(int8_t interface_id, uint8_t etx_storage_size)
+{
+    if (!etx_storage_size) {
+        ns_dyn_mem_free(etx_info.etx_storage_list);
+        etx_info.etx_storage_list = NULL;
+        etx_info.ext_storage_list_size = 0;
+        return true;
+    }
+
+    if (etx_info.ext_storage_list_size == etx_storage_size) {
+        return true;
+    }
+
+    ns_dyn_mem_free(etx_info.etx_storage_list);
+    etx_info.ext_storage_list_size = 0;
+    etx_info.etx_storage_list = ns_dyn_mem_alloc(sizeof(etx_storage_t) * etx_storage_size);
+    if (!etx_info.etx_storage_list) {
+        return false;
+    }
+
+    etx_info.ext_storage_list_size = etx_storage_size;
+    etx_info.interface_id = interface_id;
+    etx_storage_t * list_ptr = etx_info.etx_storage_list;
+    for (uint8_t i = 0; i< etx_storage_size; i++) {
+        memset(list_ptr, 0, sizeof(etx_storage_t));
+
+        list_ptr++;
+    }
+    return true;
+
+}
+
+etx_storage_t *etx_storage_entry_get(int8_t interface_id, uint8_t attribute_index)
+{
+    if (etx_info.interface_id != interface_id || !etx_info.etx_storage_list || attribute_index >= etx_info.ext_storage_list_size) {
+        tr_debug("Unknow ID or un initilized ETX %u", attribute_index);
+        return NULL;
+    }
+
+    etx_storage_t *entry = etx_info.etx_storage_list + attribute_index;
+    return entry;
+}
+
+
 /**
  * \brief A function to register accumulated failures callback
  *
@@ -444,11 +516,10 @@ uint8_t etx_accum_failures_callback_register(nwk_interface_id nwk_id, int8_t int
  *
  * \return ETX value (12 bit fraction)
  */
-static void etx_value_change_callback_needed_check(uint16_t etx, uint16_t *stored_diff_etx, uint8_t accumulated_failures, const uint8_t *mac64_addr_ptr, uint16_t mac16_addr)
+static void etx_value_change_callback_needed_check(uint16_t etx, uint16_t *stored_diff_etx, uint8_t accumulated_failures, uint8_t attribute_index)
 {
     uint16_t current_etx;
     bool callback = false;
-
     if (!etx_info.hysteresis) {
         return;
     }
@@ -469,7 +540,7 @@ static void etx_value_change_callback_needed_check(uint16_t etx, uint16_t *store
 
     // Calls callback function
     if (callback) {
-        etx_info.callback_ptr(etx_info.interface_id, (*stored_diff_etx) >> 4, current_etx >> 4, mac64_addr_ptr, mac16_addr);
+        etx_info.callback_ptr(etx_info.interface_id, (*stored_diff_etx) >> 4, current_etx >> 4, attribute_index);
         *stored_diff_etx = current_etx;
     }
 }
@@ -482,17 +553,17 @@ static void etx_value_change_callback_needed_check(uint16_t etx, uint16_t *store
  *
  * \param neigh_table_ptr the neighbor node in question
  */
-static void etx_accum_failures_callback_needed_check(struct mle_neigh_table_entry_t *neigh_table_ptr)
+static void etx_accum_failures_callback_needed_check(etx_storage_t * entry, uint8_t attribute_index)
 {
     if (!etx_info.accum_threshold) {
         return;
     }
 
-    if (neigh_table_ptr->accumulated_failures < etx_info.accum_threshold) {
+    if (entry->accumulated_failures < etx_info.accum_threshold) {
         return;
     }
 
-    etx_info.accum_cb_ptr(etx_info.interface_id, neigh_table_ptr->accumulated_failures, neigh_table_ptr);
+    etx_info.accum_cb_ptr(etx_info.interface_id, entry->accumulated_failures, attribute_index);
 }
 
 /**
@@ -504,19 +575,22 @@ static void etx_accum_failures_callback_needed_check(struct mle_neigh_table_entr
  * \param mac64_addr_ptr long MAC address
  *
  */
-void etx_neighbor_remove(int8_t interface_id, mle_neigh_table_entry_t *neigh_table_ptr) {
+void etx_neighbor_remove(int8_t interface_id, uint8_t attribute_index) {
 
+    tr_debug("Remove attribute %u", attribute_index);
     uint16_t stored_diff_etx;
-    (void) interface_id;
-    if (neigh_table_ptr && etx_info.callback_ptr) {
-        if (neigh_table_ptr->etx) {
-            stored_diff_etx = neigh_table_ptr->stored_diff_etx >> 4;
+    etx_storage_t *entry = etx_storage_entry_get(interface_id, attribute_index);
+    if (entry && etx_info.callback_ptr) {
+
+        if (entry->etx) {
+            stored_diff_etx = entry->stored_diff_etx >> 4;
             if (!stored_diff_etx) {
                 stored_diff_etx = 0xffff;
             }
-            etx_info.callback_ptr(etx_info.interface_id, stored_diff_etx, 0xffff, neigh_table_ptr->mac64,
-                neigh_table_ptr->short_adr);
+            etx_info.callback_ptr(etx_info.interface_id, stored_diff_etx, 0xffff, attribute_index);
         }
+        //Clear all data base back to zero for new user
+        memset(entry, 0, sizeof(etx_storage_t));
     }
 }
 
@@ -529,20 +603,21 @@ void etx_neighbor_remove(int8_t interface_id, mle_neigh_table_entry_t *neigh_tab
  * \param mac64_addr_ptr long MAC address
  *
  */
-void etx_neighbor_add(int8_t interface_id, mle_neigh_table_entry_t *neigh_table_ptr) {
+void etx_neighbor_add(int8_t interface_id, uint8_t attribute_index) {
 
+    tr_debug("Add attribute %u", attribute_index);
     uint16_t stored_diff_etx;
-    (void) interface_id;
-    if (neigh_table_ptr && etx_info.callback_ptr) {
+    etx_storage_t *entry = etx_storage_entry_get(interface_id, attribute_index);
+    if (entry && etx_info.callback_ptr) {
         // Gets table entry
 
-        if (neigh_table_ptr->etx) {
-            stored_diff_etx = neigh_table_ptr->stored_diff_etx;
+        if (entry->etx) {
+            stored_diff_etx = entry->stored_diff_etx;
             if (!stored_diff_etx) {
-                stored_diff_etx = neigh_table_ptr->etx;
+                stored_diff_etx = entry->etx;
             }
-            etx_info.callback_ptr(etx_info.interface_id, stored_diff_etx >> 4, neigh_table_ptr->etx >> 4,
-                neigh_table_ptr->mac64, neigh_table_ptr->short_adr);
+            etx_info.callback_ptr(etx_info.interface_id, stored_diff_etx >> 4, entry->etx >> 4,
+                                  attribute_index);
         }
     }
 }
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/etx/etx.h b/features/nanostack/sal-stack-nanostack/source/Service_Libs/etx/etx.h
index f234b84868..12b1ee6117 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/etx/etx.h
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/etx/etx.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -26,12 +26,20 @@
 
 #include "NWK_INTERFACE/Include/protocol_abstract.h"
 
-struct mle_neigh_table_entry_t;
 /* Fraction that is used when calculating moving average
    e.g. ETX = 7/8 * current ETX + 1/8 * new ETX sample
    Range for value can be from 1 to 11 */
 #define ETX_MOVING_AVERAGE_FRACTION      3     // n >> 3, 1/8
 
+typedef struct etx_storage_s {
+    uint16_t        etx;                       /*!< 12 bits fraction */
+    uint16_t        stored_diff_etx;           /*!< 12 bits fraction */
+    uint8_t         remote_incoming_idr;       /*!< 5 bits fraction */
+    unsigned        accumulated_failures: 5;
+    unsigned        tmp_etx: 1;
+    unsigned        linkIdr: 4;
+} etx_storage_t;
+
 /**
  * \brief A function to update ETX value based on transmission attempts
  *
@@ -41,10 +49,9 @@ struct mle_neigh_table_entry_t;
  * \param interface_id Interface identifier
  * \param attempts number of attempts to send message
  * \param success was message sending successful
- * \param addr_type address type, ADDR_802_15_4_SHORT or ADDR_802_15_4_LONG
- * \param addr_ptr PAN ID with 802.15.4 address
+ * \param attribute_index Neighbour attribute index
  */
-void etx_transm_attempts_update(int8_t interface_id, uint8_t attempts, bool success, addrtype_t addr_type, const uint8_t *addr_ptr);
+void etx_transm_attempts_update(int8_t interface_id, uint8_t attempts, bool success, uint8_t attribute_index);
 
 /**
  * \brief A function to update ETX value based on remote incoming IDR
@@ -54,9 +61,9 @@ void etx_transm_attempts_update(int8_t interface_id, uint8_t attempts, bool succ
  *
  * \param interface_id Interface identifier
  * \param remote_incoming_idr Remote incoming IDR
- * \param mac64_addr_ptr long MAC address
+ * \param attribute_index Neighbour attribute index
  */
-void etx_remote_incoming_idr_update(int8_t interface_id, uint8_t remote_incoming_idr, struct mle_neigh_table_entry_t *neigh_table_ptr);
+void etx_remote_incoming_idr_update(int8_t interface_id, uint8_t remote_incoming_idr, uint8_t attribute_index);
 
 /**
  * \brief A function to read ETX value
@@ -77,14 +84,26 @@ uint16_t etx_read(int8_t interface_id, addrtype_t addr_type, const uint8_t *addr
 /**
  * \brief A function to read local incoming IDR value
  *
- *  Returns local incoming IDR value for an address
+ *  Returns local incoming IDR value for an neighbour
  *
- * \param mac64_addr_ptr long MAC address
+ * \param attribute_index Neighbour attribute index
  *
  * \return 0x0100 to 0xFFFF incoming IDR value (8 bit fraction)
  * \return 0x0000 address unknown
  */
-uint16_t etx_local_incoming_idr_read(int8_t interface_id, struct mle_neigh_table_entry_t *neigh_table_ptr);
+uint16_t etx_local_incoming_idr_read(int8_t interface_id, uint8_t attribute_index);
+
+/**
+ * \brief A function to read local ETXvalue
+ *
+ *  Returns local ETX value for an address
+ *
+ * \param mac64_addr_ptr long MAC address
+ *
+ * \return 0x0100 to 0xFFFF ETX value (8 bit fraction)
+ * \return 0x0000 address unknown
+ */
+uint16_t etx_local_etx_read(int8_t interface_id, uint8_t attribute_index);
 
 /**
  * \brief A function to update ETX value based on LQI and dBm
@@ -93,11 +112,11 @@ uint16_t etx_local_incoming_idr_read(int8_t interface_id, struct mle_neigh_table
  *
  * \param lqi link quality indicator
  * \param dbm measured dBm
- * \param mac64_addr_ptr long MAC address
+ * \param attribute_index Neighbour attribute index
  *
  * \return 0x0100 to 0xFFFF local incoming IDR value (8 bit fraction)
  */
-uint16_t etx_lqi_dbm_update(int8_t interface_id, uint8_t lqi, int8_t dbm, struct mle_neigh_table_entry_t *neigh_table_ptr);
+uint16_t etx_lqi_dbm_update(int8_t interface_id, uint8_t lqi, int8_t dbm, uint8_t attribute_index);
 
 /**
  * \brief A function callback that indicates ETX value change
@@ -108,11 +127,10 @@ uint16_t etx_lqi_dbm_update(int8_t interface_id, uint8_t lqi, int8_t dbm, struct
  * \param nwk_interface_id network interface id
  * \param previous_etx ETX value to what the current ETX was compared (8 bit fraction)
  * \param current_etx current ETX value (8 bit fraction)
- * \param mac64_addr_ptr long MAC address
- * \param mac16_addr short MAC address or 0xffff address is not set
+ * \param attribute_index Neighbour attribute index
  *
  */
-typedef void (etx_value_change_handler_t)(int8_t nwk_id, uint16_t previous_etx, uint16_t current_etx, const uint8_t *mac64_addr_ptr, uint16_t mac16_addr);
+typedef void (etx_value_change_handler_t)(int8_t nwk_id, uint16_t previous_etx, uint16_t current_etx, uint8_t attribute_index);
 
 /**
  * \brief A function callback that indicates the number of accumulated TX failures
@@ -121,10 +139,10 @@ typedef void (etx_value_change_handler_t)(int8_t nwk_id, uint16_t previous_etx,
  *
  * \param interface_id interface ID
  * \param accumulated_failures number of accumulated failures
- * \param neigh_table_ptr the neighbor node in question
+ * \param attribute_index Neighbour attribute index
  *
  */
-typedef void (etx_accum_failures_handler_t)(int8_t interface_id, uint8_t accumulated_failures, struct mle_neigh_table_entry_t *neigh_table_ptr);
+typedef void (etx_accum_failures_handler_t)(int8_t interface_id, uint8_t accumulated_failures, uint8_t attribute_index);
 
 /**
  * \brief A function to register ETX value change callback
@@ -141,6 +159,29 @@ typedef void (etx_accum_failures_handler_t)(int8_t interface_id, uint8_t accumul
  */
 uint8_t etx_value_change_callback_register(nwk_interface_id nwk_id,int8_t interface_id, uint16_t hysteresis, etx_value_change_handler_t *callback_ptr);
 
+/**
+ * \brief A function to allocte ETX storage list
+ *
+ * \param interface_id interface id
+ * \param etx_storage_size Size of storage. 0 will free allocate data
+ *
+ * \return false Allocate fail
+ * \return true Allocate OK
+ */
+bool etx_storage_list_allocate(int8_t interface_id, uint8_t etx_storage_size);
+
+/**
+ * \brief A function to read ETX storage for defined neighbour
+ *
+ * \param interface_id interface id
+ * \param attribute_index Neighbour attribute index
+ *
+ * \return Pointer to ETX storage
+ * \return NULL When unknow interface or attribute
+ */
+etx_storage_t *etx_storage_entry_get(int8_t interface_id, uint8_t attribute_index);
+
+
 /**
  * \brief A function to register accumulated failures callback
  *
@@ -164,10 +205,10 @@ uint8_t etx_accum_failures_callback_register(nwk_interface_id nwk_id, int8_t int
  *  Notifies ETX module that neighbor has been removed. Calls ETX value change callback
  *  if that is set.
  *
- * \param mac64_addr_ptr long MAC address
+ * \param attribute_index Neighbour attribute index
  *
  */
-void etx_neighbor_remove(int8_t interface_id, struct mle_neigh_table_entry_t *neigh_table_ptr);
+void etx_neighbor_remove(int8_t interface_id, uint8_t attribute_index);
 
 /**
  * \brief A function to add ETX neighbor
@@ -175,9 +216,9 @@ void etx_neighbor_remove(int8_t interface_id, struct mle_neigh_table_entry_t *ne
  *  Notifies ETX module that neighbor has been added. Calls ETX value change callback
  *  if that is set.
  *
- * \param mac64_addr_ptr long MAC address
+ * \param attribute_index Neighbour attribute index
  *
  */
-void etx_neighbor_add(int8_t interface_id, struct mle_neigh_table_entry_t *neigh_table_ptr);
+void etx_neighbor_add(int8_t interface_id, uint8_t attribute_index);
 
 #endif /* ETX_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/channel_functions.h b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/channel_functions.h
new file mode 100644
index 0000000000..990a7ac90e
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/channel_functions.h
@@ -0,0 +1,58 @@
+/*
+ * Copyright (c) 2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef CHANNEL_FUNC_H_
+#define CHANNEL_FUNC_H_
+
+/**
+ * @brief Compute the unicast schedule channel index using tr51 channel function.
+ * @param slot_number Current slot number.
+ * @param mac MAC address of the node for which the index is calculated.
+ * @param number_of_channels Number of channels.
+ * @param excluded_channels Excluded channels.
+ * @return Channel index.
+ */
+int32_t tr51_get_uc_channel_index(uint16_t slot_number, uint8_t *mac, int16_t number_of_channels, uint32_t *excluded_channels);
+
+/**
+ * @brief Compute the broadcast schedule channel index using tr51 channel function.
+ * @param slot_number Current slot number.
+ * @param bsi Broadcast schedule identifier of the node for which the index is calculated.
+ * @param number_of_channels Number of channels.
+ * @param excluded_channels Excluded channels.
+ * @return Channel index.
+ */
+int32_t tr51_get_bc_channel_index(uint16_t slot_number, uint16_t bsi, int16_t number_of_channels, uint32_t *excluded_channels);
+
+/**
+ * @brief Compute the unicast schedule channel index using direct hash channel function.
+ * @param slot_number Current slot number.
+ * @param mac MAC address of the node for which the index is calculated.
+ * @param number_of_channels Number of channels.
+ * @return Channel index.
+ */
+int32_t dh1cf_get_uc_channel_index(uint16_t slot_number, uint8_t *mac, int16_t number_of_channels);
+
+/**
+ * @brief Compute the broadcast schedule channel index using direct hash channel function.
+ * @param slot_number Current slot number.
+ * @param bsi Broadcast schedule identifier of the node for which the index is calculated.
+ * @param number_of_channels Number of channels.
+ * @return Channel index.
+ */
+int32_t dh1cf_get_bc_channel_index(uint16_t slot_number, uint16_t bsi, int16_t number_of_channels);
+
+#endif /*CHANNEL_FUNC_H_*/
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/channel_list.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/channel_list.c
index baabf9e734..7d22696bd9 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/channel_list.c
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/channel_list.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -30,11 +30,6 @@ const int CHANNEL_LIST_SIZE_IN_BITS = 8*32;
 static bool channel_list_bit_test32(uint32_t word, int_fast8_t bit_number);
 static bool channel_list_bit_test(const uint32_t* list, int bit_number);
 
-#if 0
-static void channel_list_bit_set32(uint32_t* word, int_fast8_t bit_number);
-static void channel_list_bit_set(uint32_t* list, int bit_number);
-#endif
-
 // test bit by number
 static bool channel_list_bit_test32(uint32_t word, int_fast8_t bit_number)
 {
@@ -55,78 +50,7 @@ static bool channel_list_bit_test(const uint32_t* list, int bit_number)
 
     return channel_list_bit_test32(list[word_index], bit_index);
 }
-#if 0
-// set bit by number
-static void channel_list_bit_set32(uint32_t* word, int_fast8_t bit_number)
-{
-    *word |= ((uint32_t) 1 << bit_number);
-}
 
-static void channel_list_bit_set(uint32_t* list, int bit_number)
-{
-    const int_fast8_t word_index = bit_number / 32;
-    const int_fast8_t bit_index = bit_number % 32;
-
-    channel_list_bit_set32(&(list[word_index]), bit_index);
-}
-
-
-void channel_list_print(uint8_t dlevel, const char *grp, const uint32_t* list)
-{
-
-    int temp_channel = 0;
-
-#define CHANNELS_PER_LINE 32
-
-    uint8_t channels[CHANNELS_PER_LINE];
-
-    for (int line_index = 0; line_index < (CHANNEL_LIST_SIZE_IN_BITS/CHANNELS_PER_LINE); line_index++) {
-
-        int channels_found = 0;
-
-        for (int row_index = 0; row_index < CHANNELS_PER_LINE; row_index++) {
-
-            if (channel_list_bit_test(list, temp_channel)) {
-
-                channels[channels_found] = temp_channel;
-                channels_found++;
-            }
-
-            temp_channel++;
-        }
-
-        tr_info("arr[%d]: %s", line_index, trace_array(channels, channels_found));
-    }
-}
-
-// this just avoids mistakes/copypaste on client side
-void channel_list_clear_mask(uint32_t* list)
-{
-    const int mask_size = (sizeof(list));
-
-    memset(list, 0, mask_size);
-}
-
-static int channel_list_search_in_range(const uint32_t* list, int start_index, int end_index)
-{
-    int found_index = -1;
-    for (int index = start_index; index <= end_index; index++) {
-
-        if (channel_list_bit_test(list, index)) {
-            found_index = index;
-            break;
-        }
-    }
-    return found_index;
-}
-
-// utility for getting the first channel
-int channel_list_get_first(const uint32_t* list)
-{
-
-    return channel_list_get_next(list, 0xff);
-}
-#endif
 static uint8_t channel_list_search(const uint32_t* list, int index)
 {
     uint8_t channel = 0;
@@ -163,58 +87,7 @@ uint8_t channel_list_get_channel(const uint32_t* list, int current_index)
 
     return found_index;
 }
-#if 0
-int channel_list_get_next(const uint32_t* list, int current_index)
-{
-    int found_index;
 
-    current_index++;
-
-    if (current_index >= CHANNEL_LIST_SIZE_IN_BITS) {
-
-        current_index = 0;
-    }
-
-    // One could use a optimization here to avoid looping through masks 1..7
-    // if page is not 9 or 10. But is it really worth it?
-    found_index = channel_list_search_in_range(list, current_index, CHANNEL_LIST_SIZE_IN_BITS-1);
-
-    if ((found_index < 0) && (current_index > 0)) {
-
-        found_index = channel_list_search_in_range(list, 0, current_index-1);
-    }
-
-    return found_index;
-}
-
-int channel_list_get_next_broadcast(const uint32_t* list, int broadcast_channel_count, int current_index)
-{
-
-    // XXX: all these could/should be pre-calculated on configuration time.
-    const int first_broadcast = channel_list_get_first(list);
-    const int total_channel_count = channel_list_count_channels(list);
-    int channels_to_loop = total_channel_count / broadcast_channel_count; // crash if broadcast is configured to zero
-
-    int next_broadcast = -1;
-
-    // If there are more than one broadcast channels and we're not yet at the last channel,
-    // iteratively search for next broadcast channel.
-    if ((channels_to_loop > 1) && (current_index < (CHANNEL_LIST_SIZE_IN_BITS - 1))) {
-
-        while (channels_to_loop > 0) {
-
-            current_index = channel_list_get_next(list, current_index);
-            channels_to_loop--;
-        }
-        next_broadcast = current_index;
-    }
-    else {
-        next_broadcast = first_broadcast;
-    }
-
-    return next_broadcast;
-}
-#endif
 // count the amount of channels enabled in a list
 int channel_list_count_channels(const uint32_t* list)
 {
@@ -230,30 +103,3 @@ int channel_list_count_channels(const uint32_t* list)
 
     return channel_count;
 }
-#if 0
-int channel_list_enable_channel(uint32_t* list, int channel_number)
-{
-    int ret_val = -1;
-
-    if ((channel_number >= 0) && (channel_number < CHANNEL_LIST_SIZE_IN_BITS)) {
-
-        channel_list_bit_set(list, channel_number);
-
-        ret_val = 0;
-    }
-
-    return ret_val;
-}
-
-bool channel_list_is_channel_enabled(const uint32_t* list, int channel_number) {
-
-    int ret_val = false;
-
-    if ((channel_number >= 0) && (channel_number < CHANNEL_LIST_SIZE_IN_BITS)) {
-
-        ret_val = channel_list_bit_test(list, channel_number);
-    }
-
-    return ret_val;
-}
-#endif
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/channel_list.h b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/channel_list.h
index 86ec6bf03d..5e3a2139e0 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/channel_list.h
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/channel_list.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -22,11 +22,6 @@
 extern "C" {
 #endif
 
-/**
- * Dump the channel list object data to ns_trace using given trace level and group.
- */
-void channel_list_print(uint8_t dlevel, const char* grp, const uint32_t* list);
-
 /**
  * Get channel number using channel index.
  *
@@ -37,34 +32,6 @@ void channel_list_print(uint8_t dlevel, const char* grp, const uint32_t* list);
  */
 uint8_t channel_list_get_channel(const uint32_t* list, int current_index);
 
-/**
- * Clear the channel mask bitmap, does not change channel page.
- *
- * @param list list which mask is to be cleared
- */
-void channel_list_clear_mask(uint32_t* list);
-
-/**
- * Get next enabled channel number from given list. Channels are now taken sequentially,
- * starting from the index given.
- *
- * @param list to scan
- * @param the currently used channel index, ie. the place where search for next channel
- * is started
- * @return channel number of next channel
- */
-int channel_list_get_next(const uint32_t* list, int current_index);
-
-int channel_list_get_next_broadcast(const uint32_t* list, int broadcast_channel_count, int current_index);
-
-/**
- * Get the first channel enabled in a list.
- *
- * @param list to scan
- * @return index of the first channel enabled
- */
-int channel_list_get_first(const uint32_t* list);
-
 /**
  * Count the amount of channels enabled in a list.
  *
@@ -73,31 +40,6 @@ int channel_list_get_first(const uint32_t* list);
  */
 int channel_list_count_channels(const uint32_t* list);
 
-/**
- * Enable channel by given channel number. This is likely to be used
- * from the test/application configuration side.
- *
- * Note: the channel number validity is not (yet?) verified, so one
- * can enable invalid channels which should not be according to channel page.
- *
- * @param list to modify
- * @param channel number
- * @return 0 on success, negative on failure (out of bounds channel)
- */
-int channel_list_enable_channel(uint32_t* list, int channel_number);
-
-/**
- * Check, if given channel is enabled. This is likely to be used
- * from the test/application configuration side.
- *
- * Note: the channel number validity is not (yet?) verified, so one
- * can enable invalid channels which should not be according to channel page.
- *
- * @param list to test
- * @param channel number
- * @return true, if channel is enabled on mask, false if not
- */
-bool channel_list_is_channel_enabled(const uint32_t* list, int channel_number);
 
 #ifdef __cplusplus
 }
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss.c
index 9294007c6f..5e898153a4 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss.c
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -19,133 +19,76 @@
 #include "fhss_api.h"
 #include "fhss_config.h"
 #include "fhss.h"
+#include "fhss_common.h"
 #include "fhss_channel.h"
 #include "channel_list.h"
 #include "nsdynmemLIB.h"
-#include "fhss_beacon.h"
 #include "fhss_statistics.h"
 #include "ns_trace.h"
 #include "eventOS_event.h"
 #include "eventOS_callback_timer.h"
+#include "platform/arm_hal_interrupt.h"
 #include "randLIB.h"
+#include "common_functions.h"
 #include <string.h>
 
 #define TRACE_GROUP "fhss"
-// Uncomment this to get timestamped output on superframe events.
-// Note: the result may be massive, as there will be dozens or hundreds of lines of trace per second.
-// #define FHSS_MASSIVE_TRACE
 
-// TODO: create linked list
-// FHSS object pointer
-fhss_structure_t *fhss_struct = 0;
-
-static bool fhss_check_remaining_tx_time(fhss_structure_t *fhss_structure, uint16_t tx_length, uint8_t phy_header_length, uint8_t phy_tail_length);
-static void fhss_event_timer_cb(int8_t timer_id, uint16_t slots);
-static fhss_structure_t *fhss_get_object_with_timer_id(const int8_t timer_id);
-static int fhss_generate_scramble_table(fhss_structure_t *fhss_structure);
-static bool fhss_is_there_common_divisor(uint16_t i, uint8_t j);
-static void fhss_update_channel(fhss_structure_t *fhss_structure);
-static int fhss_reset_synch_monitor(fhss_synch_monitor_s *synch_monitor, bool reset_compensation);
 static int fhss_reset(fhss_structure_t *fhss_structure);
-static void fhss_failed_list_free(fhss_structure_t *fhss_structure);
+static bool fhss_is_bc_sending_superframe(fhss_structure_t *fhss_structure);
+static bool fhss_check_remaining_tx_time(fhss_structure_t *fhss_structure, uint16_t tx_length, uint8_t phy_header_length, uint8_t phy_tail_length);
+static bool fhss_is_there_common_divisor(uint16_t i, uint8_t j);
+static void fhss_beacon_received(fhss_structure_t *fhss_structure, const uint8_t *synch_info, const uint32_t elapsed_time);
+static void fhss_superframe_handler(const fhss_api_t *fhss_api, uint16_t delay);
+static int8_t fhss_beacon_create_tasklet(fhss_structure_t *fhss_structure);
+static void fhss_beacon_tasklet_func(arm_event_s* event);
+static int fhss_beacon_periodic_start(fhss_structure_t *fhss_structure, uint32_t time_to_first_beacon);
+static void fhss_beacon_periodic_stop(fhss_structure_t *fhss_structure);
 
-
-int8_t fhss_enable(fhss_api_t *fhss_api, const fhss_configuration_t *fhss_configuration, const fhss_timer_t *fhss_timer, fhss_statistics_t *fhss_statistics)
+fhss_structure_t *fhss_enable(fhss_api_t *fhss_api, const fhss_configuration_t *fhss_configuration, const fhss_timer_t *fhss_timer, fhss_statistics_t *fhss_statistics)
 {
-    if (!fhss_api || !fhss_configuration || !fhss_timer || fhss_struct) {
+    if (!fhss_api || !fhss_configuration || !fhss_timer) {
         tr_err("Invalid FHSS enable configuration");
-        return -1;
+        return NULL;
     }
     int channel_count = channel_list_count_channels(fhss_configuration->channel_mask);
     if (channel_count <= 0) {
         // There must be at least one configured channel in channel list
-        return -2;
+        return NULL;
     }
-    fhss_struct = ns_dyn_mem_alloc(sizeof(fhss_structure_t));
+    fhss_structure_t *fhss_struct = fhss_allocate_instance(fhss_api, fhss_timer);
     if (!fhss_struct) {
-        return -3;
+        return NULL;
     }
-    fhss_struct->fhss_api = fhss_api;
-    fhss_struct->fhss_configuration = *fhss_configuration;
-    fhss_struct->platform_functions = *fhss_timer;
-    fhss_struct->fhss_stats_ptr = fhss_statistics;
+    fhss_struct->bs = ns_dyn_mem_alloc(sizeof(fhss_bs_t));
+    if (!fhss_struct->bs) {
+        fhss_free_instance(fhss_api);
+        return NULL;
+    }
+    memset(fhss_struct->bs, 0, sizeof(fhss_bs_t));
+
+    fhss_struct->bs->fhss_configuration = *fhss_configuration;
+    fhss_struct->bs->fhss_stats_ptr = fhss_statistics;
     fhss_struct->number_of_channels = channel_count;
 
     // set a invalid id to tasklet_id, so we know that one is not started yet
     fhss_struct->beacon_tasklet_id = -1;
 
-    if (!fhss_struct->platform_functions.fhss_resolution_divider) {
-        fhss_struct->platform_functions.fhss_resolution_divider = 1;
-    }
     // Default synch interval is 240 seconds
-    if (!fhss_struct->fhss_configuration.fhss_max_synch_interval) {
-        fhss_struct->fhss_configuration.fhss_max_synch_interval = 240;
+    if (!fhss_struct->bs->fhss_configuration.fhss_max_synch_interval) {
+        fhss_struct->bs->fhss_configuration.fhss_max_synch_interval = 240;
     }
     ns_list_init(&fhss_struct->fhss_failed_tx_list);
     fhss_struct->own_hop = 0xff;
     fhss_reset(fhss_struct);
-    fhss_reset_synch_monitor(&fhss_struct->synch_monitor, true);
-    fhss_struct->active_fhss_events = 0;
-    fhss_struct->fhss_beacon_info_store = NULL;
-    fhss_struct->fhss_event_timer = eventOS_callback_timer_register(fhss_event_timer_cb);
-
-    fhss_generate_scramble_table(fhss_struct);
 
     if (fhss_beacon_create_tasklet(fhss_struct) < 0) {
-        // XXX: should we free the fhss_structure here?
-        return -5;
-    }
-
-    return 0;
-}
-
-int8_t fhss_set_datarate(fhss_structure_t *fhss_structure, uint32_t datarate)
-{
-    if (!fhss_structure) {
-        return -1;
-    }
-    // If datarate is not set, use default 250kbit/s. Datarate is used as divider later.
-    if (!datarate) {
-        datarate = 250000;
-    }
-    fhss_structure->datarate = datarate;
-    return 0;
-}
-
-int8_t fhss_set_synch_configuration(fhss_structure_t *fhss_structure, const fhss_synch_configuration_t *fhss_synch_configuration)
-{
-    if (!fhss_structure) {
-        return -1;
-    }
-    if (!fhss_synch_configuration) {
-        return -2;
-    }
-    // None of the configurations can be set zero
-    if( fhss_synch_configuration->fhss_number_of_bc_channels == 0 || fhss_synch_configuration->fhss_number_of_tx_slots == 0
-            || fhss_synch_configuration->fhss_number_of_superframes == 0 || fhss_synch_configuration->fhss_superframe_length == 0) {
-        return -3;
-    }
-    // Number of channels must be divisible with the number of broadcast channels.
-    // Number of superframes must be divisible with the number of TX slots
-    if (((fhss_structure->number_of_channels % fhss_synch_configuration->fhss_number_of_bc_channels) != 0) ||
-            ((fhss_synch_configuration->fhss_number_of_superframes % fhss_synch_configuration->fhss_number_of_tx_slots) != 0) ||
-            (fhss_synch_configuration->fhss_number_of_superframes <= fhss_synch_configuration->fhss_number_of_tx_slots)) {
-        return -4;
-    }
-    fhss_structure->synch_configuration = *fhss_synch_configuration;
-    fhss_structure->own_hop = 0;
-    return 0;
-}
-
-fhss_structure_t *fhss_get_object_with_api(const fhss_api_t *fhss_api)
-{
-    if (!fhss_api || !fhss_struct) {
+        ns_dyn_mem_free(fhss_struct->bs);
+        fhss_free_instance(fhss_api);
         return NULL;
     }
-    if (fhss_struct->fhss_api == fhss_api) {
-        return fhss_struct;
-    }
-    return NULL;
+
+    return fhss_struct;
 }
 
 bool fhss_is_synch_root(fhss_structure_t *fhss_structure)
@@ -156,45 +99,16 @@ bool fhss_is_synch_root(fhss_structure_t *fhss_structure)
     return true;
 }
 
-void fhss_set_active_event(fhss_structure_t *fhss_structure, uint8_t event_type)
-{
-    fhss_structure->active_fhss_events |= (1 << event_type);
-}
-
-void fhss_clear_active_event(fhss_structure_t *fhss_structure, uint8_t event_type)
-{
-    fhss_structure->active_fhss_events &= ~(1 << event_type);
-}
-
-bool fhss_read_active_event(fhss_structure_t *fhss_structure, uint8_t event_type)
-{
-    if (fhss_structure->active_fhss_events & (1 << event_type)) {
-        return true;
-    }
-    return false;
-}
-
-static fhss_structure_t *fhss_get_object_with_timer_id(const int8_t timer_id)
-{
-    if (timer_id <0 || !fhss_struct) {
-        return NULL;
-    }
-    if (fhss_struct->fhss_event_timer == timer_id) {
-        return fhss_struct;
-    }
-    return NULL;
-}
-
 static bool fhss_is_bc_sending_superframe(fhss_structure_t *fhss_structure)
 {
-    if (fhss_structure->current_superframe >= fhss_structure->broadcast_start_superframe) {
+    if (fhss_structure->bs->current_superframe >= fhss_structure->bs->broadcast_start_superframe) {
         return true;
     }
 
     return false;
 }
 
-bool fhss_check_bad_channel(fhss_structure_t *fhss_structure, uint8_t handle)
+static bool fhss_check_bad_channel(fhss_structure_t *fhss_structure, uint8_t handle)
 {
     if (!fhss_structure) {
         return false;
@@ -209,7 +123,7 @@ bool fhss_check_bad_channel(fhss_structure_t *fhss_structure, uint8_t handle)
     return true;
 }
 
-bool fhss_check_channel_type(fhss_structure_t *fhss_structure, bool is_bc, int frame_type)
+static bool fhss_check_channel_type(fhss_structure_t *fhss_structure, bool is_bc, int frame_type)
 {
     if (!fhss_structure) {
         return false;
@@ -228,7 +142,7 @@ bool fhss_check_channel_type(fhss_structure_t *fhss_structure, bool is_bc, int f
             }
         } else if (fhss_is_current_channel_broadcast(fhss_structure) == true) {
             // Drop: If waiting synchronization Beacon and packet has unicast destination
-            if (fhss_structure->beacons_received_timer) {
+            if (fhss_structure->bs->beacons_received_timer) {
                 return false;
             }
         }
@@ -238,26 +152,26 @@ bool fhss_check_channel_type(fhss_structure_t *fhss_structure, bool is_bc, int f
     }
     if (frame_type == FHSS_SYNCH_REQUEST_FRAME) {
         // Drop: If we have unicast channels in our configuration and current channel is broadcast channel
-        if ((fhss_structure->number_of_channels > fhss_structure->synch_configuration.fhss_number_of_bc_channels) && (fhss_is_current_channel_broadcast(fhss_structure) == true)) {
+        if ((fhss_structure->number_of_channels > fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels) && (fhss_is_current_channel_broadcast(fhss_structure) == true)) {
             return false;
         }
 
-        uint8_t current_superframe = fhss_structure->current_superframe;
-        uint8_t synch_attempt = fhss_structure->beacons_received_timer;
-        if (fhss_structure->synch_configuration.fhss_number_of_tx_slots > 1) {
+        uint8_t current_superframe = fhss_structure->bs->current_superframe;
+        uint8_t synch_attempt = fhss_structure->bs->beacons_received_timer;
+        if (fhss_structure->bs->synch_configuration.fhss_number_of_tx_slots > 1) {
             // Send synch request either before or after the middle of the channel depending on attempt number.
-            uint8_t middle_of_superframes = fhss_structure->synch_configuration.fhss_number_of_superframes / 2;
+            uint8_t middle_of_superframes = fhss_structure->bs->synch_configuration.fhss_number_of_superframes / 2;
 
             if (synch_attempt & 1) {
-                if (fhss_structure->current_superframe < middle_of_superframes) {
+                if (fhss_structure->bs->current_superframe < middle_of_superframes) {
                     return false;
                 }
             } else {
-                if (fhss_structure->current_superframe >= middle_of_superframes) {
+                if (fhss_structure->bs->current_superframe >= middle_of_superframes) {
                     return false;
                 }
             }
-        } else if ((current_superframe == 0) || (current_superframe == (fhss_structure->synch_configuration.fhss_number_of_superframes - 1))){
+        } else if ((current_superframe == 0) || (current_superframe == (fhss_structure->bs->synch_configuration.fhss_number_of_superframes - 1))){
             return false;
         }
     }
@@ -265,7 +179,7 @@ bool fhss_check_channel_type(fhss_structure_t *fhss_structure, bool is_bc, int f
 
 }
 
-bool fhss_check_tx_allowed(fhss_structure_t *fhss_structure, bool is_bc, uint16_t frame_length, int frame_type, uint8_t phy_header_length, uint8_t phy_tail_length)
+static bool fhss_check_tx_allowed(fhss_structure_t *fhss_structure, bool is_bc, uint16_t frame_length, int frame_type, uint8_t phy_header_length, uint8_t phy_tail_length)
 {
     if (!fhss_structure) {
         return false;
@@ -283,7 +197,7 @@ bool fhss_check_tx_allowed(fhss_structure_t *fhss_structure, bool is_bc, uint16_
         return true;
     }
     // Deny: If FHSS is not on TX slot
-    if (fhss_structure->tx_allowed == false) {
+    if (fhss_structure->bs->tx_allowed == false) {
         return false;
     }
     // Deny: If not enough time before TX slot end
@@ -293,205 +207,23 @@ bool fhss_check_tx_allowed(fhss_structure_t *fhss_structure, bool is_bc, uint16_
     return true;
 }
 
-static int fhss_reset_synch_monitor(fhss_synch_monitor_s *synch_monitor, bool reset_compensation)
+static int fhss_reset_synch_monitor(fhss_synch_monitor_s *synch_monitor)
 {
 	if (synch_monitor) {
 	    synch_monitor->avg_synch_fix = 0;
 	    // Initialize to -1 instead of 0 to drop the first beacon after network scan (from synch monitoring)
 	    synch_monitor->avg_synch_fix_counter = -1;
-	    if (reset_compensation == true) {
-	        synch_monitor->drift_compensation = 0;
-	    }
 	    synch_monitor->channel_counter = 0;
 		return 0;
 	}
 	return -1;
 }
 
-static int fhss_reset(fhss_structure_t *fhss_structure)
+static int fhss_update_txrx_slots(fhss_structure_t *fhss_structure)
 {
-    if (fhss_structure) {
-        fhss_structure->platform_functions.fhss_timer_stop(fhss_structure->fhss_api);
-        fhss_struct->synch_panid = 0xffff;
-        fhss_beacon_periodic_stop(fhss_structure);
-        fhss_struct->current_superframe = 0;
-        fhss_struct->current_channel_index = 0;
-        fhss_struct->channel_list_counter = 0;
-        if (fhss_is_synch_root(fhss_structure) == false) {
-            fhss_struct->own_hop = 0xff;
-        }
-        fhss_struct->tx_allowed = false;
-        fhss_struct->synch_interval = (uint32_t) (fhss_struct->fhss_configuration.fhss_max_synch_interval/BEACON_INTERVAL_INIT_DIVIDER) * 1000;
-        fhss_struct->rx_channel = 0;
-        fhss_struct->beacons_received_timer = 0;
-        memset(fhss_struct->synch_parent, 0xff, 8);
-        fhss_struct->send_synch_info_on_next_broadcast_channel = false;
-        memset(&fhss_struct->synch_configuration, 0, sizeof(fhss_synch_configuration_t));
-        fhss_struct->synch_infos_sent_counter = 0;
-        fhss_struct->broadcast_start_superframe = 0;
-        fhss_failed_list_free(fhss_structure);
-        fhss_struct->fhss_state = FHSS_UNSYNCHRONIZED;
-        return 0;
-    }
-    return -1;
-}
-
-int fhss_down(fhss_structure_t *fhss_structure)
-{
-    if (fhss_structure) {
-        fhss_reset(fhss_structure);
-        fhss_reset_synch_monitor(&fhss_struct->synch_monitor, false);
-        fhss_stats_update(fhss_structure, STATS_FHSS_DRIFT_COMP, fhss_structure->synch_monitor.drift_compensation);
-        fhss_stats_update(fhss_structure, STATS_FHSS_AVG_SYNCH_FIX, fhss_structure->synch_monitor.avg_synch_fix);
-        fhss_stats_update(fhss_structure, STATS_FHSS_SYNCH_INTERVAL, fhss_structure->synch_interval / 1000);
-        return 0;
-    }
-    return -1;
-}
-
-
-int8_t fhss_disable(fhss_structure_t *fhss_structure)
-{
-    if (!fhss_structure) {
-        return -1;
-    }
-    ns_dyn_mem_free(fhss_structure);
-    fhss_structure = 0;
-    fhss_struct = 0;
-    return 0;
-}
-
-void fhss_start_timer(fhss_structure_t *fhss_structure, uint32_t time, void (*callback)(const fhss_api_t *fhss_api, uint16_t))
-{
-    if (callback){
-        // Don't allow starting with zero slots
-        if (time < fhss_structure->platform_functions.fhss_resolution_divider) {
-            time = fhss_structure->platform_functions.fhss_resolution_divider;
-        }
-        fhss_structure->platform_functions.fhss_timer_start(time / fhss_structure->platform_functions.fhss_resolution_divider, callback, fhss_structure->fhss_api);
-    }
-}
-
-uint32_t fhss_get_remaining_time_to_next_superframe(const fhss_structure_t *fhss_structure)
-{
-    const uint32_t slots = fhss_structure->platform_functions.fhss_get_remaining_slots(fhss_structure->fhss_api);
-    const uint32_t time = slots * fhss_structure->platform_functions.fhss_resolution_divider;
-    return time;
-}
-
-void fhss_superframe_handler(const fhss_api_t *fhss_api, uint16_t delay)
-{
-    int compensation = 0;
-    fhss_structure_t *fhss_structure = fhss_get_object_with_api(fhss_api);
-    if (!fhss_structure) {
-        return;
-    }
-/* Drift compensation doesn't work with Linux platform */
-#ifndef __linux__
-    // Drift compensation on first superframe
-    if (fhss_structure->current_superframe == 0) {
-        /* Idea is to compensate number of drift_compensation (microseconds) on each channel.
-         * However, fhss_resolution_divider defines the minimum timer resolution.
-         * E.g. if fhss_resolution_divider = 64, compensate (drift_compensation * 64) on each 64th channel.
-         */
-        if (++fhss_structure->synch_monitor.channel_counter == fhss_structure->platform_functions.fhss_resolution_divider) {
-            compensation = fhss_structure->synch_monitor.drift_compensation;
-            fhss_structure->synch_monitor.channel_counter = 0;
-        }
-    }
-#endif //__linux__
-    // Restart timer asap to minimize the effect of dynamic execution time of
-    // the rest of function.
-    fhss_start_timer(fhss_structure, (fhss_structure->synch_configuration.fhss_superframe_length) - (delay * fhss_structure->platform_functions.fhss_resolution_divider) + (compensation * fhss_structure->platform_functions.fhss_resolution_divider), fhss_superframe_handler);
-
-    // check, if the current frame was the last one
-    if (fhss_structure->current_superframe >= (fhss_structure->synch_configuration.fhss_number_of_superframes - 1)) {
-
-        // last superframe has passed, change channel
-        fhss_structure->current_superframe = 0;
-        fhss_structure->current_channel_index++;
-        if (fhss_structure->current_channel_index >= fhss_structure->number_of_channels) {
-            fhss_structure->synch_infos_sent_counter = 0;
-            fhss_structure->current_channel_index = 0;
-            fhss_structure->channel_list_counter++;
-            // Repeated cycle is started from beginning, reset counter. Don't let the channel_list_counter overflow.
-            if (fhss_structure->channel_list_counter >= ((uint16_t) fhss_structure->number_of_channels * MAX_SCRAMBLE_TABLE_INDEXES)) {
-                fhss_structure->channel_list_counter = 0;
-            }
-            // Hop 0 don't have parent
-            if (fhss_is_synch_root(fhss_structure) == false) {
-                fhss_trig_event(fhss_structure, FHSS_COMPARE_SYNCH_PARENT);
-            }
-            fhss_trig_event(fhss_structure, FHSS_UPDATE_SYNCH_INFO_STORAGE);
-        }
-        fhss_update_channel(fhss_structure);
-    } else  {
-        // bump up the superframe counter
-        fhss_structure->current_superframe++;
-
-#ifdef FHSS_MASSIVE_TRACE
-        tr_debug("%"PRIu32": handler, super: %"PRIu8,
-                fhss_structure->fhss_api->read_timestamp(fhss_structure->fhss_api), fhss_structure->current_superframe);
-#endif
-    }
-    if ((fhss_structure->send_synch_info_on_next_broadcast_channel == true) && (fhss_is_current_channel_broadcast(fhss_structure) == true)) {
-        /* Randomize sending superframe of synchronization frame:
-         * on first superframe probability is 1/number of superframes
-         * on second superframe probability is 1/(number of superframes-1)
-         * on third superframe probability is 1/(number of superframes-2)
-         * on last superframe probability is 1/1
-         */
-        if (randLIB_get_random_in_range(1, fhss_structure->synch_configuration.fhss_number_of_superframes - fhss_structure->current_superframe) == 1) {
-            fhss_structure->send_synch_info_on_next_broadcast_channel = false;
-            fhss_structure->synch_infos_sent_counter++;
-            fhss_structure->callbacks.send_fhss_frame(fhss_structure->fhss_api, FHSS_SYNCH_FRAME);
-        }
-    }
-    fhss_update_txrx_slots(fhss_structure);
-    uint16_t queue_size = fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, false) + fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, true);
-    if ((fhss_structure->tx_allowed == true || fhss_is_current_channel_broadcast(fhss_structure) == true) && queue_size) {
-        /* Start timer with random timeout to trigger TX queue poll event.
-         * Max random is half of the superframe length. Event timer resolution is 50us.
-         * Divide Max random with TX queue size to transmit faster when TX queue is growing
-         */
-        uint16_t max_random = ((fhss_structure->synch_configuration.fhss_superframe_length / 2) / 50) / queue_size;
-        eventOS_callback_timer_start(fhss_structure->fhss_event_timer, randLIB_get_random_in_range(1, max_random));
-    }
-    if (fhss_structure->fhss_timeout) {
-        fhss_structure->fhss_timer += fhss_structure->synch_configuration.fhss_superframe_length;
-        if (fhss_structure->fhss_timer >= fhss_structure->fhss_timeout) {
-            fhss_trig_event(fhss_structure, FHSS_TIMER_EVENT);
-            fhss_structure->fhss_timeout = 0;
-            fhss_structure->fhss_timer = 0;
-        }
-    }
-}
-
-int fhss_timeout_start(fhss_structure_t *fhss_structure, uint32_t time)
-{
-    if (!fhss_structure) {
-        return -1;
-    }
-    fhss_structure->fhss_timeout = time;
-    fhss_structure->fhss_timer = 0;
-    return 0;
-}
-
-int fhss_timeout_stop(fhss_structure_t *fhss_structure)
-{
-    if (!fhss_structure) {
-        return -1;
-    }
-    fhss_structure->fhss_timeout = 0;
-    fhss_structure->fhss_timer = 0;
-    return 0;
-}
-
-int fhss_update_txrx_slots(fhss_structure_t *fhss_structure)
-{
-    uint8_t cur_superframe = fhss_structure->current_superframe;
-    uint8_t number_of_tx_slots = fhss_structure->synch_configuration.fhss_number_of_tx_slots;
-    uint8_t number_of_superframes = fhss_structure->synch_configuration.fhss_number_of_superframes;
+    uint8_t cur_superframe = fhss_structure->bs->current_superframe;
+    uint8_t number_of_tx_slots = fhss_structure->bs->synch_configuration.fhss_number_of_tx_slots;
+    uint8_t number_of_superframes = fhss_structure->bs->synch_configuration.fhss_number_of_superframes;
     uint8_t tx_slot_length = ((number_of_superframes / 2) / number_of_tx_slots);
     uint8_t tx_slot_up_limit = tx_slot_length;
     bool tx_allowed = false;
@@ -521,7 +253,7 @@ int fhss_update_txrx_slots(fhss_structure_t *fhss_structure)
             tx_slot_up_limit += (tx_slot_length * 2);
         }
     }
-    fhss_structure->tx_allowed = tx_allowed;
+    fhss_structure->bs->tx_allowed = tx_allowed;
     return 0;
 }
 
@@ -536,22 +268,22 @@ static int fhss_update_drift_compensation(fhss_structure_t *fhss_structure)
     if (!fhss_structure) {
         return 0;
     }
-    bc_density = (fhss_structure->number_of_channels / fhss_structure->synch_configuration.fhss_number_of_bc_channels);
-    channel_dwell_time = ((uint32_t)fhss_structure->synch_configuration.fhss_superframe_length * fhss_structure->synch_configuration.fhss_number_of_superframes) / 1000;
+    bc_density = (fhss_structure->number_of_channels / fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels);
+    channel_dwell_time = ((uint32_t)fhss_structure->bs->synch_configuration.fhss_superframe_length * fhss_structure->bs->synch_configuration.fhss_number_of_superframes) / 1000;
     // Calculate last synchronisation period
-    if (fhss_structure->synch_interval != ((uint32_t)fhss_structure->fhss_configuration.fhss_max_synch_interval * 1000)) {
+    if (fhss_structure->bs->synch_interval != ((uint32_t)fhss_structure->bs->fhss_configuration.fhss_max_synch_interval * 1000)) {
         // Last period was half of the current started period and max random period is shorter
-        synch_period = (fhss_structure->synch_interval / 2) + (bc_density * channel_dwell_time) * (fhss_structure->synch_configuration.fhss_number_of_bc_channels / 2);
+        synch_period = (fhss_structure->bs->synch_interval / 2) + (bc_density * channel_dwell_time) * (fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels / 2);
     } else {
-        synch_period = fhss_structure->synch_interval + (bc_density * channel_dwell_time) * fhss_structure->synch_configuration.fhss_number_of_bc_channels;
+        synch_period = fhss_structure->bs->synch_interval + (bc_density * channel_dwell_time) * fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels;
     }
 
     // E.g. 240000ms / (50000us * 8) = 600 channels per fhss_beacon_send_interval
-    channels_per_synch_period = (synch_period * 1000) / ((uint32_t)fhss_structure->synch_configuration.fhss_superframe_length * fhss_structure->synch_configuration.fhss_number_of_superframes);
+    channels_per_synch_period = (synch_period * 1000) / ((uint32_t)fhss_structure->bs->synch_configuration.fhss_superframe_length * fhss_structure->bs->synch_configuration.fhss_number_of_superframes);
     // Calculate compensation value: how much to compensate on each channel. E.g. 6000us / 600channels = 10us/channel
-    fhss_structure->synch_monitor.drift_compensation += (fhss_structure->synch_monitor.avg_synch_fix / channels_per_synch_period);
-    fhss_stats_update(fhss_structure, STATS_FHSS_DRIFT_COMP, fhss_structure->synch_monitor.drift_compensation);
-    if ((fhss_structure->synch_monitor.avg_synch_fix > FHSS_SYNCH_DRIFT_TOO_HIGH_LIMIT) || (fhss_structure->synch_monitor.avg_synch_fix < -FHSS_SYNCH_DRIFT_TOO_HIGH_LIMIT)) {
+    fhss_structure->bs->synch_monitor.drift_compensation += (fhss_structure->bs->synch_monitor.avg_synch_fix / channels_per_synch_period);
+    fhss_stats_update(fhss_structure, STATS_FHSS_DRIFT_COMP, fhss_structure->bs->synch_monitor.drift_compensation);
+    if ((fhss_structure->bs->synch_monitor.avg_synch_fix > FHSS_SYNCH_DRIFT_TOO_HIGH_LIMIT) || (fhss_structure->bs->synch_monitor.avg_synch_fix < -FHSS_SYNCH_DRIFT_TOO_HIGH_LIMIT)) {
         // Indicates that more frequent synchronization is needed
         retval = -1;
     }
@@ -560,7 +292,7 @@ static int fhss_update_drift_compensation(fhss_structure_t *fhss_structure)
 
 static int fhss_update_synch_monitor(fhss_structure_t *fhss_structure, const fhss_synchronization_beacon_payload_s *payload, uint8_t superframe_own, int32_t remaining_time_own, const int32_t time_to_next_superframe)
 {
-    fhss_synch_configuration_t *configuration = &fhss_structure->synch_configuration;
+    fhss_synch_configuration_t *configuration = &fhss_structure->bs->synch_configuration;
     int super_frame_changed = payload->current_superframe - superframe_own;
     int retval = 0;
     if (((super_frame_changed < 0) && ((super_frame_changed * -1) < (payload->number_of_superframes_per_channel / 2))) || (super_frame_changed > payload->number_of_superframes_per_channel / 2)) {
@@ -591,22 +323,22 @@ static int fhss_update_synch_monitor(fhss_structure_t *fhss_structure, const fhs
 
         int32_t prev_synch_fix = (time_to_next_superframe - remaining_time_own);
         // After network scan counter was initialized to -1 to drop this fix from monitoring
-        if (fhss_structure->synch_monitor.avg_synch_fix_counter >= 0) {
-            fhss_structure->synch_monitor.avg_synch_fix += prev_synch_fix;
+        if (fhss_structure->bs->synch_monitor.avg_synch_fix_counter >= 0) {
+            fhss_structure->bs->synch_monitor.avg_synch_fix += prev_synch_fix;
         }
-        fhss_structure->synch_monitor.avg_synch_fix_counter++;
-        if (fhss_structure->synch_monitor.avg_synch_fix_counter == SYNCH_MONITOR_AVG_SAMPLES) {
-            fhss_structure->synch_monitor.avg_synch_fix /= SYNCH_MONITOR_AVG_SAMPLES;
-            fhss_stats_update(fhss_structure, STATS_FHSS_AVG_SYNCH_FIX, fhss_structure->synch_monitor.avg_synch_fix);
+        fhss_structure->bs->synch_monitor.avg_synch_fix_counter++;
+        if (fhss_structure->bs->synch_monitor.avg_synch_fix_counter == SYNCH_MONITOR_AVG_SAMPLES) {
+            fhss_structure->bs->synch_monitor.avg_synch_fix /= SYNCH_MONITOR_AVG_SAMPLES;
+            fhss_stats_update(fhss_structure, STATS_FHSS_AVG_SYNCH_FIX, fhss_structure->bs->synch_monitor.avg_synch_fix);
             retval = fhss_update_drift_compensation(fhss_structure);
-            fhss_structure->synch_monitor.avg_synch_fix_counter = 0;
-            fhss_structure->synch_monitor.avg_synch_fix = 0;
+            fhss_structure->bs->synch_monitor.avg_synch_fix_counter = 0;
+            fhss_structure->bs->synch_monitor.avg_synch_fix = 0;
         }
     }
     return retval;
 }
 
-int fhss_sync_with_beacon(fhss_structure_t *fhss_structure,
+static int fhss_sync_with_beacon(fhss_structure_t *fhss_structure,
         const fhss_synchronization_beacon_payload_s *payload)
 {
     int ret_val = -1;
@@ -615,19 +347,19 @@ int fhss_sync_with_beacon(fhss_structure_t *fhss_structure,
 
     if (fhss_structure) {
         // Do not allow synchronising devices above 253 hops.
-        if (payload->hop_count > 253) {
+        if (payload->hop_count > (FHSS_MAX_ALLOWED_HOPS-1)) {
             return 0;
         }
         // To make synchronization monitoring more effective, drop extra Beacons.
-        if (fhss_structure->fhss_state == FHSS_SYNCHRONIZED && (fhss_is_current_channel_broadcast(fhss_structure) == false || (fhss_structure->beacon_received_on_this_bc_channel == true))) {
+        if (fhss_structure->fhss_state == FHSS_SYNCHRONIZED && (fhss_is_current_channel_broadcast(fhss_structure) == false || (fhss_structure->bs->beacon_received_on_this_bc_channel == true))) {
             return 0;
         }
 
-        fhss_synch_configuration_t *configuration = &fhss_structure->synch_configuration;
-        fhss_structure->beacon_received_on_this_bc_channel = true;
+        fhss_synch_configuration_t *configuration = &fhss_structure->bs->synch_configuration;
+        fhss_structure->bs->beacon_received_on_this_bc_channel = true;
 
-        superframe_own = fhss_structure->current_superframe;
-        fhss_structure->current_superframe = payload->current_superframe;
+        superframe_own = fhss_structure->bs->current_superframe;
+        fhss_structure->bs->current_superframe = payload->current_superframe;
         // Clone the static config values from parent which has the authority.
         configuration->fhss_number_of_bc_channels = payload->number_of_broadcast_channels;
         configuration->fhss_number_of_tx_slots = payload->number_of_tx_slots;
@@ -641,36 +373,36 @@ int fhss_sync_with_beacon(fhss_structure_t *fhss_structure,
             fhss_structure->own_hop = payload->hop_count + 1;
         }
         fhss_stats_update(fhss_structure, STATS_FHSS_HOP_COUNT, fhss_structure->own_hop);
-        fhss_structure->channel_list_counter = payload->channel_list_counter;
-        fhss_structure->current_channel_index = payload->channel_index;
+        fhss_structure->bs->channel_list_counter = payload->channel_list_counter;
+        fhss_structure->bs->current_channel_index = payload->channel_index;
         uint8_t mac_address[8];
         fhss_structure->callbacks.read_mac_address(fhss_structure->fhss_api, mac_address);
 
-        fhss_structure->uc_channel_index = fhss_calculate_uc_index(fhss_structure->current_channel_index, fhss_structure->number_of_channels,
+        fhss_structure->bs->uc_channel_index = fhss_calculate_uc_index(fhss_structure->bs->current_channel_index, fhss_structure->number_of_channels,
                 payload->number_of_broadcast_channels) + fhss_get_offset(fhss_structure, mac_address);
 
         // If current channel is not broadcast, fhss_update_channel will increase UC channel index, otherwise do it here
         if (fhss_is_current_channel_broadcast(fhss_structure) == true || (fhss_structure->fhss_state == FHSS_SYNCHRONIZED)) {
-            fhss_structure->uc_channel_index += 1;
+            fhss_structure->bs->uc_channel_index += 1;
         }
-        if (fhss_structure->uc_channel_index >= (fhss_structure->number_of_channels - payload->number_of_broadcast_channels)) {
-            fhss_structure->uc_channel_index -= (fhss_structure->number_of_channels - payload->number_of_broadcast_channels);
+        if (fhss_structure->bs->uc_channel_index >= (fhss_structure->number_of_channels - payload->number_of_broadcast_channels)) {
+            fhss_structure->bs->uc_channel_index -= (fhss_structure->number_of_channels - payload->number_of_broadcast_channels);
         }
 
-        fhss_structure->platform_functions.fhss_timer_stop(fhss_structure->fhss_api);
+        fhss_structure->platform_functions.fhss_timer_stop(fhss_superframe_handler, fhss_structure->fhss_api);
         // start timer to elapse at approximately same time as the parent will.
         const int32_t time_to_next_superframe = payload->remaining_slots;
         remaining_time_own = fhss_get_remaining_time_to_next_superframe(fhss_structure);
         fhss_start_timer(fhss_structure, time_to_next_superframe, fhss_superframe_handler);
         // Reset beacon received timer when FHSS synchronization is updated
-        fhss_structure->beacons_received_timer = 0;
-        uint16_t bc_density = (fhss_structure->number_of_channels / fhss_structure->synch_configuration.fhss_number_of_bc_channels);
-        uint8_t fhss_number_of_bc_channels = fhss_structure->synch_configuration.fhss_number_of_bc_channels;
-        uint16_t channel_dwell_time = ((uint32_t)fhss_structure->synch_configuration.fhss_superframe_length * fhss_structure->synch_configuration.fhss_number_of_superframes) / 1000;
+        fhss_structure->bs->beacons_received_timer = 0;
+        uint16_t bc_density = (fhss_structure->number_of_channels / fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels);
+        uint8_t fhss_number_of_bc_channels = fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels;
+        uint16_t channel_dwell_time = ((uint32_t)fhss_structure->bs->synch_configuration.fhss_superframe_length * fhss_structure->bs->synch_configuration.fhss_number_of_superframes) / 1000;
 
         if (fhss_structure->fhss_state == FHSS_UNSYNCHRONIZED) {
             fhss_structure->fhss_state = FHSS_SYNCHRONIZED;
-            fhss_update_channel(fhss_structure);
+            fhss_change_to_next_channel(fhss_structure);
         }
         if (fhss_is_synch_root(fhss_structure) == false) {
             // Initially synch drift might be massive. Request first few Beacons more frequently until compensation starts fixing the error.
@@ -678,31 +410,25 @@ int fhss_sync_with_beacon(fhss_structure_t *fhss_structure,
 
             if (fhss_structure->fhss_state == FHSS_SYNCHRONIZED) {
                 if (fhss_update_synch_monitor(fhss_structure, payload, superframe_own, remaining_time_own, time_to_next_superframe)) {
-                    fhss_structure->synch_interval = (uint32_t) (fhss_structure->fhss_configuration.fhss_max_synch_interval/BEACON_INTERVAL_INIT_DIVIDER) * 1000;
+                    fhss_structure->bs->synch_interval = (uint32_t) (fhss_structure->bs->fhss_configuration.fhss_max_synch_interval/BEACON_INTERVAL_INIT_DIVIDER) * 1000;
                 }
             }
 
-            if (fhss_structure->synch_interval != ((uint32_t)fhss_structure->fhss_configuration.fhss_max_synch_interval * 1000)) {
-                fhss_structure->synch_interval *= 2;
-                if (fhss_structure->synch_interval > ((uint32_t)fhss_structure->fhss_configuration.fhss_max_synch_interval * 1000)) {
-                    fhss_structure->synch_interval = ((uint32_t)fhss_structure->fhss_configuration.fhss_max_synch_interval * 1000);
+            if (fhss_structure->bs->synch_interval != ((uint32_t)fhss_structure->bs->fhss_configuration.fhss_max_synch_interval * 1000)) {
+                fhss_structure->bs->synch_interval *= 2;
+                if (fhss_structure->bs->synch_interval > ((uint32_t)fhss_structure->bs->fhss_configuration.fhss_max_synch_interval * 1000)) {
+                    fhss_structure->bs->synch_interval = ((uint32_t)fhss_structure->bs->fhss_configuration.fhss_max_synch_interval * 1000);
                 }
                 beacon_interval_random = (bc_density * channel_dwell_time) * randLIB_get_random_in_range(0, fhss_number_of_bc_channels/2);
             } else {
                 beacon_interval_random = (bc_density * channel_dwell_time) * randLIB_get_random_in_range(0, fhss_number_of_bc_channels);
             }
-            fhss_stats_update(fhss_structure, STATS_FHSS_SYNCH_INTERVAL, fhss_structure->synch_interval / 1000);
-            fhss_beacon_periodic_start(fhss_structure, fhss_structure->synch_interval + beacon_interval_random);
-
-#ifdef FHSS_MASSIVE_TRACE
-            tr_debug("start timer, time now: %"PRIu32", sfr: %"PRIu8", ch-ind: %"PRIu8", time after: %"PRIu32,
-                    fhss_structure->fhss_api->read_timestamp(fhss_structure->fhss_api), fhss_structure->current_superframe,
-                    payload->channel_index, time_to_next_superframe);
-#endif
+            fhss_stats_update(fhss_structure, STATS_FHSS_SYNCH_INTERVAL, fhss_structure->bs->synch_interval / 1000);
+            fhss_beacon_periodic_start(fhss_structure, fhss_structure->bs->synch_interval + beacon_interval_random);
         }
         // Our hop has changed, needs to inform possible children by sending Beacon
         if ((own_hop_tmp != 0) && (own_hop_tmp != fhss_structure->own_hop)) {
-            fhss_structure->send_synch_info_on_next_broadcast_channel = true;
+            fhss_structure->bs->send_synch_info_on_next_broadcast_channel = true;
         }
         ret_val = 0;
     }
@@ -725,12 +451,12 @@ static uint32_t fhss_get_remaining_tx_time(fhss_structure_t *fhss_structure)
     uint32_t remaining_tx_time = 0;
 
     if (fhss_structure) {
-        uint8_t cur_superframe = fhss_structure->current_superframe;
-        uint8_t number_of_tx_slots = fhss_structure->synch_configuration.fhss_number_of_tx_slots;
-        uint8_t number_of_superframes = fhss_structure->synch_configuration.fhss_number_of_superframes;
+        uint8_t cur_superframe = fhss_structure->bs->current_superframe;
+        uint8_t number_of_tx_slots = fhss_structure->bs->synch_configuration.fhss_number_of_tx_slots;
+        uint8_t number_of_superframes = fhss_structure->bs->synch_configuration.fhss_number_of_superframes;
         uint8_t tx_slot_length = ((number_of_superframes / 2) / number_of_tx_slots);
         uint8_t tx_slot_up_limit = tx_slot_length;
-        uint16_t superframe_length = fhss_structure->synch_configuration.fhss_superframe_length;
+        uint16_t superframe_length = fhss_structure->bs->synch_configuration.fhss_superframe_length;
 
         if ((fhss_structure->own_hop % 2)) {
             tx_slot_up_limit += tx_slot_length;
@@ -748,11 +474,6 @@ static uint32_t fhss_get_remaining_tx_time(fhss_structure_t *fhss_structure)
     return remaining_tx_time;
 }
 
-uint32_t fhss_get_tx_time(fhss_structure_t *fhss_structure, uint16_t bytes_to_send, uint8_t phy_header_length, uint8_t phy_tail_length)
-{
-    return ((1000000 / (fhss_structure->datarate / 8)) * (bytes_to_send + phy_header_length + phy_tail_length));
-}
-
 // CCA adds extra 2ms with FHSS
 #define CCA_FHSS_PERIOD 2000
 // Ack frame length
@@ -774,8 +495,8 @@ static bool fhss_check_remaining_tx_time(fhss_structure_t *fhss_structure, uint1
     if (fhss_structure->fhss_state == FHSS_UNSYNCHRONIZED) {
         retval = true;
     } else {
-        tx_processing_delay = fhss_structure->fhss_configuration.fhss_tuning_parameters.tx_processing_delay;
-        ack_processing_delay = fhss_structure->fhss_configuration.fhss_tuning_parameters.ack_processing_delay;
+        tx_processing_delay = fhss_structure->bs->fhss_configuration.fhss_tuning_parameters.tx_processing_delay;
+        ack_processing_delay = fhss_structure->bs->fhss_configuration.fhss_tuning_parameters.ack_processing_delay;
         // Calculate needed TX time (us): CCA static period + TX processing delays + transmission time + Ack processing delays + Ack transmission time
         needed_tx_time = CCA_FHSS_PERIOD + tx_processing_delay + fhss_get_tx_time(fhss_structure, tx_length, phy_header_length, phy_tail_length)
                         + ack_processing_delay + fhss_get_tx_time(fhss_structure, ACK_LENGTH, phy_header_length, phy_tail_length);
@@ -787,81 +508,6 @@ static bool fhss_check_remaining_tx_time(fhss_structure_t *fhss_structure, uint1
     return retval;
 }
 
-int fhss_update_synch_parent_address(fhss_structure_t *fhss_structure)
-{
-    uint8_t parent_address[8];
-
-    if (!fhss_get_parent_address(fhss_structure, parent_address)) {
-        memcpy(fhss_structure->synch_parent, parent_address, 8);
-        return 0;
-    }
-    return -1;
-}
-
-void fhss_trig_event(fhss_structure_t *fhss_structure, uint8_t event_type)
-{
-    if (fhss_read_active_event(fhss_structure, event_type) == true) {
-        return;
-    }
-    arm_event_s event;
-    event.receiver = fhss_structure->beacon_tasklet_id;
-    event.sender = 0;
-    event.event_type = event_type;
-    event.event_id = 0;
-    event.data_ptr = fhss_structure;
-    event.priority = ARM_LIB_HIGH_PRIORITY_EVENT;
-    event.event_data = 0;
-    if (eventOS_event_send(&event) != 0) {
-        tr_error("Event trigger failed: eventOS_event_send() failed");
-    } else {
-        fhss_set_active_event(fhss_structure, event_type);
-    }
-}
-
-int fhss_get_parent_address(fhss_structure_t *fhss_structure, uint8_t *p_addr)
-{
-    int ret_val = -1;
-    if (!fhss_structure || !p_addr) {
-        return -1;
-    }
-
-    ret_val = fhss_structure->callbacks.read_coord_mac_address(fhss_structure->fhss_api, p_addr);
-
-    if (ret_val) {
-        // Use default synchronization parent when RPL parent not found
-        memcpy(p_addr, fhss_structure->synch_parent, 8);
-        ret_val = 0;
-    }
-    return ret_val;
-}
-
-int fhss_compare_with_synch_parent_address(fhss_structure_t *fhss_structure, const uint8_t *source_addr)
-{
-    int ret_val = -1;
-    if (!fhss_structure || !source_addr) {
-        return ret_val;
-    }
-    uint8_t parent_address[8];
-
-    if (fhss_is_synch_root(fhss_structure) == false) {
-        if (!fhss_get_parent_address(fhss_structure, parent_address)) {
-            ret_val = memcmp(source_addr, parent_address, 8);
-        }
-    }
-    return ret_val;
-}
-
-static void fhss_update_channel(fhss_structure_t *fhss_structure)
-{
-    // If channel is broadcast channel (true), send event
-    if (fhss_change_to_next_channel(fhss_structure) == true) {
-        // Only if device is border router
-        if (fhss_structure->own_hop == 0) {
-            fhss_trig_event(fhss_structure, FHSS_BROADCAST_CHANNEL);
-        }
-    }
-}
-
 static bool fhss_is_there_common_divisor(uint16_t i, uint8_t j)
 {
     if (i < j) {
@@ -884,7 +530,7 @@ static int fhss_generate_scramble_table(fhss_structure_t *fhss_structure)
     {
         // Common divisors are skipped
         if (fhss_is_there_common_divisor(fhss_structure->number_of_channels, j) == false) {
-            fhss_structure->fhss_scramble_table[i] = j;
+            fhss_structure->bs->fhss_scramble_table[i] = j;
             i++;
         }
         j++;
@@ -892,19 +538,10 @@ static int fhss_generate_scramble_table(fhss_structure_t *fhss_structure)
     return 0;
 }
 
-static void fhss_event_timer_cb(int8_t timer_id, uint16_t slots)
-{
-    (void) slots;
-    fhss_structure_t *fhss_structure = fhss_get_object_with_timer_id(timer_id);
-    if (fhss_structure) {
-        fhss_structure->callbacks.tx_poll(fhss_structure->fhss_api);
-    }
-}
-
-fhss_beacon_info_t *fhss_get_beacon_info(fhss_structure_t *fhss_structure, uint16_t pan_id)
+static fhss_beacon_info_t *fhss_get_beacon_info(fhss_structure_t *fhss_structure, uint16_t pan_id)
 {
     fhss_beacon_info_t *beacon_info;
-    beacon_info = fhss_structure->fhss_beacon_info_store;
+    beacon_info = fhss_structure->bs->fhss_beacon_info_store;
     while (beacon_info) {
         if (beacon_info->pan_id == pan_id) {
             break;
@@ -918,11 +555,11 @@ static void fhss_store_beacon_info(fhss_structure_t *fhss_structure, fhss_beacon
 {
     fhss_beacon_info_t *beacon_info_cur;
     beacon_info->next = NULL;
-    if (!fhss_structure->fhss_beacon_info_store) {
-        fhss_structure->fhss_beacon_info_store = beacon_info;
+    if (!fhss_structure->bs->fhss_beacon_info_store) {
+        fhss_structure->bs->fhss_beacon_info_store = beacon_info;
         return;
     }
-    beacon_info_cur = fhss_structure->fhss_beacon_info_store;
+    beacon_info_cur = fhss_structure->bs->fhss_beacon_info_store;
     while (beacon_info_cur->next != NULL) {
         beacon_info_cur = beacon_info_cur->next;
     }
@@ -949,19 +586,19 @@ static fhss_beacon_info_t *fhss_create_beacon_info(fhss_structure_t *fhss_struct
 
 static int fhss_remove_beacon_info(fhss_structure_t *fhss_structure, uint16_t pan_id)
 {
-    if (!fhss_structure || !fhss_structure->fhss_beacon_info_store) {
+    if (!fhss_structure || !fhss_structure->bs->fhss_beacon_info_store) {
         return -1;
     }
 
-    if (fhss_structure->fhss_beacon_info_store->pan_id == pan_id) {
-        fhss_beacon_info_t *next = fhss_structure->fhss_beacon_info_store->next;
-        ns_dyn_mem_free(fhss_structure->fhss_beacon_info_store);
-        fhss_structure->fhss_beacon_info_store = next;
+    if (fhss_structure->bs->fhss_beacon_info_store->pan_id == pan_id) {
+        fhss_beacon_info_t *next = fhss_structure->bs->fhss_beacon_info_store->next;
+        ns_dyn_mem_free(fhss_structure->bs->fhss_beacon_info_store);
+        fhss_structure->bs->fhss_beacon_info_store = next;
         return 0;
     }
 
-    fhss_beacon_info_t *removed_beacon_info = fhss_structure->fhss_beacon_info_store->next;
-    fhss_beacon_info_t *prev_beacon_info = fhss_structure->fhss_beacon_info_store;
+    fhss_beacon_info_t *removed_beacon_info = fhss_structure->bs->fhss_beacon_info_store->next;
+    fhss_beacon_info_t *prev_beacon_info = fhss_structure->bs->fhss_beacon_info_store;
 
     while (removed_beacon_info) {
         if (removed_beacon_info->pan_id == pan_id) {
@@ -976,22 +613,50 @@ static int fhss_remove_beacon_info(fhss_structure_t *fhss_structure, uint16_t pa
     return -1;
 }
 
-int fhss_flush_beacon_info_storage(fhss_structure_t *fhss_structure)
+static int fhss_flush_beacon_info_storage(fhss_structure_t *fhss_structure)
 {
     if (!fhss_structure) {
         return -1;
     }
-    fhss_beacon_info_t *beacon_info = fhss_structure->fhss_beacon_info_store;
+    fhss_beacon_info_t *beacon_info = fhss_structure->bs->fhss_beacon_info_store;
     while (beacon_info) {
         fhss_beacon_info_t *next = beacon_info->next;
         ns_dyn_mem_free(beacon_info);
         beacon_info = next;
     }
-    fhss_structure->fhss_beacon_info_store = NULL;
+    fhss_structure->bs->fhss_beacon_info_store = NULL;
     return 0;
 }
 
-int fhss_add_beacon_info(fhss_structure_t *fhss_structure, uint16_t pan_id, uint8_t *source_address, uint32_t timestamp, uint8_t *synch_info)
+static int fhss_reset(fhss_structure_t *fhss_structure)
+{
+    if (!fhss_structure) {
+        return -1;
+    }
+    fhss_structure->platform_functions.fhss_timer_stop(fhss_superframe_handler, fhss_structure->fhss_api);
+    fhss_structure->bs->synch_panid = 0xffff;
+    fhss_beacon_periodic_stop(fhss_structure);
+    fhss_structure->bs->current_superframe = 0;
+    fhss_structure->bs->current_channel_index = 0;
+    fhss_structure->bs->channel_list_counter = 0;
+    if (fhss_is_synch_root(fhss_structure) == false) {
+        fhss_structure->own_hop = 0xff;
+    }
+    fhss_structure->bs->tx_allowed = false;
+    fhss_structure->bs->synch_interval = (uint32_t) (fhss_structure->bs->fhss_configuration.fhss_max_synch_interval/BEACON_INTERVAL_INIT_DIVIDER) * 1000;
+    fhss_structure->rx_channel = 0;
+    fhss_structure->bs->beacons_received_timer = 0;
+    memset(fhss_structure->synch_parent, 0xff, 8);
+    fhss_structure->bs->send_synch_info_on_next_broadcast_channel = false;
+    memset(&fhss_structure->bs->synch_configuration, 0, sizeof(fhss_synch_configuration_t));
+    fhss_structure->bs->synch_infos_sent_counter = 0;
+    fhss_structure->bs->broadcast_start_superframe = 0;
+    fhss_failed_list_free(fhss_structure);
+    fhss_structure->fhss_state = FHSS_UNSYNCHRONIZED;
+    return 0;
+}
+
+static int fhss_add_beacon_info(fhss_structure_t *fhss_structure, uint16_t pan_id, uint8_t *source_address, uint32_t timestamp, uint8_t *synch_info)
 {
     if (!fhss_structure || !source_address || !synch_info) {
         return -1;
@@ -1008,10 +673,10 @@ int fhss_add_beacon_info(fhss_structure_t *fhss_structure, uint16_t pan_id, uint
     return 0;
 }
 
-void fhss_update_beacon_info_lifetimes(fhss_structure_t *fhss_structure, uint32_t timestamp)
+static void fhss_update_beacon_info_lifetimes(fhss_structure_t *fhss_structure, uint32_t timestamp)
 {
     fhss_beacon_info_t *beacon_info;
-    beacon_info = fhss_structure->fhss_beacon_info_store;
+    beacon_info = fhss_structure->bs->fhss_beacon_info_store;
     while (beacon_info) {
         uint32_t time_since_added = timestamp - beacon_info->timestamp;
         // timestamp is microseconds, lifetime is seconds
@@ -1024,43 +689,733 @@ void fhss_update_beacon_info_lifetimes(fhss_structure_t *fhss_structure, uint32_
     }
 }
 
-fhss_failed_tx_t *fhss_failed_handle_find(fhss_structure_t *fhss_structure, uint8_t handle)
+static void fhss_synch_state_set_callback(const fhss_api_t *api, fhss_states fhss_state, uint16_t pan_id)
 {
-    ns_list_foreach(fhss_failed_tx_t, cur, &fhss_structure->fhss_failed_tx_list) {
-        if (cur->handle == handle) {
-            return cur;
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
+    if (!fhss_structure) {
+        return;
+    }
+    // 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_reset(fhss_structure);
+        fhss_reset_synch_monitor(&fhss_structure->bs->synch_monitor);
+        fhss_stats_update(fhss_structure, STATS_FHSS_DRIFT_COMP, fhss_structure->bs->synch_monitor.drift_compensation);
+        fhss_stats_update(fhss_structure, STATS_FHSS_AVG_SYNCH_FIX, fhss_structure->bs->synch_monitor.avg_synch_fix);
+        fhss_stats_update(fhss_structure, STATS_FHSS_SYNCH_INTERVAL, fhss_structure->bs->synch_interval / 1000);
+    } else {
+        // Do not synchronize to current pan
+        if (fhss_structure->bs->synch_panid == pan_id) {
+            tr_debug("Synch same panid %u", pan_id);
+            return;
+        }
+        fhss_generate_scramble_table(fhss_structure);
+
+        uint8_t mac_address[8];
+        fhss_structure->callbacks.read_mac_address(fhss_structure->fhss_api, mac_address);
+        fhss_structure->bs->uc_channel_index = fhss_get_offset(fhss_structure, mac_address);
+        // Get Beacon info from storage
+        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 = fhss_structure->fhss_api->read_timestamp(fhss_structure->fhss_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->bs->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->bs->synch_configuration.fhss_superframe_length, fhss_superframe_handler);
+        } else {
+            tr_error("Synch info not found");
+            return;
         }
     }
-    return NULL;
+    fhss_structure->fhss_state = fhss_state;
+    return;
 }
 
-int fhss_failed_handle_add(fhss_structure_t *fhss_structure, uint8_t handle)
+static void fhss_beacon_decode_raw(fhss_synchronization_beacon_payload_s* dest, const uint8_t* buffer)
 {
-    fhss_failed_tx_t *failed_tx = ns_dyn_mem_alloc(sizeof(fhss_failed_tx_t));
-    if (!failed_tx) {
-        return -2;
+    dest->data_start_delimeter = *buffer++;
+    dest->channel_index = *buffer++;
+    dest->sender_unicast_channel = *buffer++;
+    dest->current_superframe = common_read_16_bit(buffer);
+    buffer += BEACON_FIELD_SIZE(current_superframe);
+    dest->remaining_slots = common_read_16_bit(buffer);
+    buffer += BEACON_FIELD_SIZE(remaining_slots);
+    dest->channel_list_counter = common_read_16_bit(buffer);
+    buffer += BEACON_FIELD_SIZE(channel_list_counter);
+    dest->hop_count = *buffer++;
+    dest->number_of_broadcast_channels = *buffer++;
+    dest->number_of_tx_slots = *buffer++;
+    dest->time_since_last_beacon = common_read_32_bit(buffer);
+    buffer += BEACON_FIELD_SIZE(time_since_last_beacon);
+    dest->processing_delay += common_read_16_bit(buffer);
+    buffer += BEACON_FIELD_SIZE(processing_delay);
+    dest->superframe_length = common_read_16_bit(buffer);
+    buffer += BEACON_FIELD_SIZE(superframe_length);
+    dest->number_of_superframes_per_channel = *buffer;
+}
+
+static uint32_t fhss_get_time_to_next_channel_change(uint16_t remaining_slots_to_next_superframe, uint8_t number_of_superframes, uint8_t current_superframe, uint16_t superframe_length)
+{
+    return remaining_slots_to_next_superframe + ((uint32_t)((number_of_superframes - 1) - current_superframe) * superframe_length);
+}
+
+// Decode the given raw byte buffer into a struct into dest struct and calculate
+// the new values for elapsed_time, channel_index, current_superframe and remaining_slots
+// from current state and given data.
+static void fhss_beacon_decode(fhss_synchronization_beacon_payload_s* dest, const uint8_t* buffer, uint32_t elapsed_time, uint16_t number_of_channels)
+{
+    fhss_beacon_decode_raw(dest, buffer);
+
+    elapsed_time += dest->processing_delay;
+
+    /* To calculate channel index after beacon scan, following calculation is performed
+     *
+     *                           rem. slots to channel change(X)    Channel length (V)
+     *                              |---------------------|     |-----------------------------------------------|
+     *    |    RX'd channel index (Y)                     | ... |                      Y+n                      |
+     * ...|    sf1    |    sf2    |    sf3    |    sf4    | ... |    sf1    |    sf2    |    sf3    |    sf4    |...
+     *                              ^                                     ^
+     *                              |beacon received                      |beacon scan done
+     *                              |-------------------------------------|
+     *                               measured time after beacon RX'd(Z)
+     * V = superframe length * number of superframes
+     * X = remaining slots to superframe change + length of the remaining full superframes to channel change
+     *
+     * Y+n = Y + ((Z - X) / V) + 1
+     *
+     * Or if (Z < X)
+     *       Y+n = Y
+     */
+
+    uint32_t remaining_slots_to_next_channel = fhss_get_time_to_next_channel_change(dest->remaining_slots, dest->number_of_superframes_per_channel, dest->current_superframe, dest->superframe_length);
+    uint16_t temp_channel_index = dest->channel_index;
+    if (elapsed_time >= remaining_slots_to_next_channel) {
+        uint32_t channel_length = (uint32_t) dest->number_of_superframes_per_channel * dest->superframe_length;
+        temp_channel_index = dest->channel_index + ((elapsed_time - remaining_slots_to_next_channel) / channel_length) + 1;
+    }
+    while (temp_channel_index >= number_of_channels) {
+        temp_channel_index -= number_of_channels;
+        dest->channel_list_counter++;
+    }
+    dest->channel_index = temp_channel_index;
+    while (dest->channel_list_counter >= (number_of_channels * MAX_SCRAMBLE_TABLE_INDEXES)) {
+        dest->channel_list_counter -= (number_of_channels * MAX_SCRAMBLE_TABLE_INDEXES);
+    }
+
+    /* To calculate superframe after beacon scan, following calculation is performed
+     *
+     *           rem. slots(X)         sf. length(V)
+     *        |---------------|     |-----------------|
+     *...| RX'd superframe (Y)| ... |      Y+n        |      Y+n+1       |....
+     *        ^                                     ^
+     *        |beacon received                      |beacon scan done
+     *        |-------------------------------------|
+     *          measured time after beacon RX'd(Z)
+     *
+     * Y+n = Y + ((Z - X) / V) + 1
+     *
+     * Or if (Z < X)
+     *       Y+n = Y
+     */
+
+    if (elapsed_time >= dest->remaining_slots) {
+        dest->current_superframe = dest->current_superframe + ((elapsed_time - dest->remaining_slots) / dest->superframe_length) + 1;
+    }
+    while (dest->current_superframe >= dest->number_of_superframes_per_channel) {
+        dest->current_superframe -= dest->number_of_superframes_per_channel;
+    }
+
+    /* To get the remaining slots after beacon scan, following calculation is performed
+     *
+     *         rem. slots(Y)               sf. length(V)    new rem. slots(X)
+     *        |----------|               |---------------| |-------------|
+     *...| superframe 1  | superframe 2  | superframe 3  | superframe 4  |...
+     *        ^                                            ^
+     *        |beacon received                             |beacon scan done
+     *        |--------------------------------------------|
+     *          measured time after beacon RX'd(Z)
+     *
+     * X = V - ((Z - Y) % V)
+     *
+     * Or if (Z < Y)
+     *        X = Y - Z
+     */
+
+    if (elapsed_time < dest->remaining_slots) {
+        dest->remaining_slots = dest->remaining_slots - elapsed_time;
+    } else {
+        dest->remaining_slots = dest->superframe_length - ((elapsed_time - dest->remaining_slots) % dest->superframe_length);
     }
-    failed_tx->bad_channel = fhss_structure->rx_channel;
-    failed_tx->retries_done = 0;
-    failed_tx->handle = handle;
-    ns_list_add_to_end(&fhss_structure->fhss_failed_tx_list, failed_tx);
-    return 0;
 }
 
-int fhss_failed_handle_remove(fhss_structure_t *fhss_structure, uint8_t handle)
+static int fhss_synch_info_validate(fhss_synchronization_beacon_payload_s *payload)
 {
-    fhss_failed_tx_t *failed_tx = fhss_failed_handle_find(fhss_structure, handle);
-    if (!failed_tx) {
+    if (!payload) {
+        return -1;
+    }
+    if (payload->data_start_delimeter != 0) {
+        return -1;
+    }
+    if (payload->current_superframe >= payload->number_of_superframes_per_channel) {
+        return -1;
+    }
+    if (payload->remaining_slots >= payload->superframe_length) {
+        return -1;
+    }
+    if (payload->hop_count > FHSS_MAX_ALLOWED_HOPS-1) {
+        return -1;
+    }
+    if (payload->number_of_broadcast_channels == 0) {
+        return -1;
+    }
+    if (payload->number_of_tx_slots == 0) {
+        return -1;
+    }
+    if (payload->number_of_superframes_per_channel == 0) {
         return -1;
     }
-    ns_list_remove(&fhss_structure->fhss_failed_tx_list, failed_tx);
-    ns_dyn_mem_free(failed_tx); // Free entry
     return 0;
 }
 
-static void fhss_failed_list_free(fhss_structure_t *fhss_structure)
+static void fhss_beacon_received(fhss_structure_t *fhss_structure, const uint8_t *synch_info, const uint32_t elapsed_time)
 {
-    for (uint16_t i = 0; i<256; i++) {
-        fhss_failed_handle_remove(fhss_structure, i);
+
+    if (fhss_structure) {
+
+        if (synch_info) {
+            fhss_synchronization_beacon_payload_s temp_payload;
+            temp_payload.processing_delay = fhss_structure->bs->fhss_configuration.fhss_tuning_parameters.rx_processing_delay;
+            fhss_beacon_decode(&temp_payload, synch_info, elapsed_time, fhss_structure->number_of_channels);
+            if (!fhss_synch_info_validate(&temp_payload)) {
+                fhss_sync_with_beacon(fhss_structure, &temp_payload);
+            } else {
+                tr_err("Invalid synch info received");
+            }
+        }
+    }
+}
+
+static uint32_t fhss_get_sf_timeout_callback(fhss_structure_t *fhss_structure)
+{
+    uint32_t compensation = 0;
+    /* Drift compensation doesn't work with Linux platform */
+#ifndef __linux__
+    // Drift compensation on first superframe
+    if (fhss_structure->bs->current_superframe == 0) {
+        /* Idea is to compensate number of drift_compensation (microseconds) on each channel.
+         * However, fhss_resolution_divider defines the minimum timer resolution.
+         * E.g. if fhss_resolution_divider = 64, compensate (drift_compensation * 64) on each 64th channel.
+         */
+        if (++fhss_structure->bs->synch_monitor.channel_counter == fhss_structure->platform_functions.fhss_resolution_divider) {
+            compensation = fhss_structure->bs->synch_monitor.drift_compensation;
+            fhss_structure->bs->synch_monitor.channel_counter = 0;
+        }
+    }
+#else
+    (void) fhss_structure;
+#endif //__linux__
+    return (fhss_structure->bs->synch_configuration.fhss_superframe_length) + (compensation * fhss_structure->platform_functions.fhss_resolution_divider);
+}
+
+static void fhss_superframe_callback(fhss_structure_t *fhss_structure)
+{
+    if ((fhss_structure->bs->send_synch_info_on_next_broadcast_channel == true) && (fhss_is_current_channel_broadcast(fhss_structure) == true)) {
+        /* Randomize sending superframe of synchronization frame:
+         * on first superframe probability is 1/number of superframes
+         * on second superframe probability is 1/(number of superframes-1)
+         * on third superframe probability is 1/(number of superframes-2)
+         * on last superframe probability is 1/1
+         */
+        if (randLIB_get_random_in_range(1, fhss_structure->bs->synch_configuration.fhss_number_of_superframes - fhss_structure->bs->current_superframe) == 1) {
+            fhss_structure->bs->send_synch_info_on_next_broadcast_channel = false;
+            fhss_structure->bs->synch_infos_sent_counter++;
+            fhss_structure->callbacks.send_fhss_frame(fhss_structure->fhss_api, FHSS_SYNCH_FRAME);
+        }
+    }
+    fhss_update_txrx_slots(fhss_structure);
+    uint16_t queue_size = fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, false) + fhss_structure->callbacks.read_tx_queue_size(fhss_structure->fhss_api, true);
+    if ((fhss_structure->bs->tx_allowed == true || fhss_is_current_channel_broadcast(fhss_structure) == true) && queue_size) {
+        /* Start timer with random timeout to trigger TX queue poll event.
+         * Max random is half of the superframe length. Event timer resolution is 50us.
+         * Divide Max random with TX queue size to transmit faster when TX queue is growing
+         */
+        uint16_t max_random = ((fhss_structure->bs->synch_configuration.fhss_superframe_length / 2) / 50) / queue_size;
+        eventOS_callback_timer_start(fhss_structure->fhss_event_timer, randLIB_get_random_in_range(1, max_random));
+    }
+}
+
+static int fhss_tx_handle_callback(const fhss_api_t *api, bool is_broadcast_addr, uint8_t *destination_address, int frame_type, uint16_t frame_length, uint8_t phy_header_length, uint8_t phy_tail_length, uint32_t tx_time)
+{
+    (void) tx_time;
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
+    if (!fhss_structure) {
+        return -2;
+    }
+    // TODO: needs some more logic to push buffer back to queue
+    if (frame_type == FHSS_DATA_FRAME) {
+        if (is_broadcast_addr == true) {
+            if (fhss_is_current_channel_broadcast(fhss_structure) == false) {
+                tr_info("Broadcast on UC channel -> Back to queue");
+                return -3;
+            }
+        }
+    }
+    if (fhss_check_tx_allowed(fhss_structure, is_broadcast_addr, frame_length, frame_type, phy_header_length, phy_tail_length) == false) {
+        return -1;
+    }
+    // If sending Beacon request on parents Unicast channel
+    if (frame_type == FHSS_SYNCH_REQUEST_FRAME && fhss_structure->fhss_state == FHSS_SYNCHRONIZED) {
+        fhss_change_to_parent_channel(fhss_structure);
+    } else if (frame_type == FHSS_DATA_FRAME) {
+        fhss_change_to_tx_channel(fhss_structure, destination_address);
+    }
+    return 0;
+}
+
+static bool fhss_check_tx_conditions_callback(const fhss_api_t *api, bool is_broadcast_addr, uint8_t handle, int frame_type, uint16_t frame_length, uint8_t phy_header_length, uint8_t phy_tail_length)
+{
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
+    if (!fhss_structure) {
+        return true;
+    }
+    // This condition will check that message is not sent on bad channel
+    if (fhss_check_bad_channel(fhss_structure, handle) == false) {
+        return false;
+    }
+
+    // This condition will check that broadcast messages are sent only broadcast channels
+    if (fhss_check_channel_type(fhss_structure, is_broadcast_addr, frame_type) == false) {
+        return false;
+    }
+
+    // This condition will check that FHSS is on TX slot and there is enough time to transmit before channel or slot change
+    if (fhss_check_tx_allowed(fhss_structure, is_broadcast_addr, frame_length, frame_type, phy_header_length, phy_tail_length) == false) {
+        return false;
+    }
+
+    return true;
+}
+
+static void fhss_update_channel_callback(fhss_structure_t *fhss_structure)
+{
+    if (fhss_structure->bs->current_channel_index == 0) {
+        fhss_structure->bs->synch_infos_sent_counter = 0;
+        if (++fhss_structure->bs->channel_list_counter >= ((uint16_t) fhss_structure->number_of_channels * MAX_SCRAMBLE_TABLE_INDEXES)) {
+            fhss_structure->bs->channel_list_counter = 0;
+        }
+        if (fhss_is_synch_root(fhss_structure) == false) {
+            fhss_trig_event(fhss_structure, FHSS_COMPARE_SYNCH_PARENT);
+        }
+        fhss_trig_event(fhss_structure, FHSS_UPDATE_SYNCH_INFO_STORAGE);
+    }
+    // If channel is broadcast channel (true), send event
+    if (fhss_change_to_next_channel(fhss_structure) == true) {
+        // Only if device is border router
+        if (fhss_structure->own_hop == 0) {
+            fhss_trig_event(fhss_structure, FHSS_BROADCAST_CHANNEL);
+        }
+    }
+}
+
+static uint8_t* fhss_beacon_encode_raw(uint8_t* buffer, const fhss_synchronization_beacon_payload_s* source)
+{
+    *buffer++ = FHSS_DATA_START_DELIMETER;
+    *buffer++ = source->channel_index;
+    *buffer++ = source->sender_unicast_channel;
+    buffer = common_write_16_bit(source->current_superframe, buffer);
+    buffer = common_write_16_bit(source->remaining_slots, buffer);
+    buffer = common_write_16_bit(source->channel_list_counter, buffer);
+    *buffer++ = source->hop_count;
+    *buffer++ = source->number_of_broadcast_channels;
+    *buffer++ = source->number_of_tx_slots;
+    buffer = common_write_32_bit(source->time_since_last_beacon, buffer);
+    buffer = common_write_16_bit(source->processing_delay, buffer);
+    buffer = common_write_16_bit(source->superframe_length, buffer);
+    *buffer++ = source->number_of_superframes_per_channel;
+
+    return buffer;
+}
+
+static void fhss_beacon_build(fhss_structure_t *fhss_structure, uint8_t* dest)
+{
+    fhss_synchronization_beacon_payload_s temp_payload;
+    platform_enter_critical();
+    const fhss_synch_configuration_t *config = &fhss_structure->bs->synch_configuration;
+    temp_payload.channel_index = fhss_structure->bs->current_channel_index;
+    temp_payload.sender_unicast_channel = 0;
+    temp_payload.current_superframe = fhss_structure->bs->current_superframe;
+    // This assumes that the time is always in the range of 0..2**16, which
+    // should be the case as the superframe length field is also in that range.
+    temp_payload.remaining_slots = (uint16_t) fhss_get_remaining_time_to_next_superframe(fhss_structure);
+    temp_payload.channel_list_counter = fhss_structure->bs->channel_list_counter;
+    temp_payload.hop_count = fhss_structure->own_hop;
+    temp_payload.number_of_broadcast_channels = config->fhss_number_of_bc_channels;
+    temp_payload.number_of_tx_slots = config->fhss_number_of_tx_slots;
+    temp_payload.time_since_last_beacon = 0; // XXX not available yet
+    uint32_t tx_time = fhss_get_tx_time(fhss_structure, 71, 0, 0);
+    temp_payload.processing_delay = fhss_structure->bs->fhss_configuration.fhss_tuning_parameters.tx_processing_delay + tx_time;
+    temp_payload.superframe_length = config->fhss_superframe_length;
+    temp_payload.number_of_superframes_per_channel = config->fhss_number_of_superframes;
+    platform_exit_critical();
+    fhss_beacon_encode_raw(dest, &temp_payload);
+}
+
+static int16_t fhss_write_synch_info_callback(const fhss_api_t *api, uint8_t *ptr, uint8_t length, int frame_type, uint32_t tx_time)
+{
+    (void) length;
+    (void) tx_time;
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
+    if (!fhss_structure || !ptr || (frame_type != FHSS_SYNCH_FRAME)) {
+        return -1;
+    }
+    fhss_beacon_build(fhss_structure, ptr);
+    return FHSS_SYNCH_INFO_LENGTH;
+}
+
+static void fhss_data_tx_done_callback(const fhss_api_t *api, bool waiting_ack, bool tx_completed, uint8_t handle)
+{
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
+    if (!fhss_structure) {
+        return;
+    }
+    if (waiting_ack == false) {
+        fhss_change_to_rx_channel(fhss_structure);
+    }
+    // Buffer was successfully transmitted. Remove stored failure handle if exists.
+    if (tx_completed == true) {
+        fhss_failed_tx_t *fhss_failed_tx = fhss_failed_handle_find(fhss_structure, handle);
+        if (fhss_failed_tx) {
+            fhss_failed_handle_remove(fhss_structure, handle);
+        }
+    }
+}
+
+static bool fhss_data_tx_fail_callback(const fhss_api_t *api, uint8_t handle, int frame_type)
+{
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
+    if (!fhss_structure) {
+        return false;
+    }
+    // Only use channel retries when device is synchronized
+    if (fhss_structure->fhss_state == FHSS_UNSYNCHRONIZED) {
+        return false;
+    }
+    // Channel retries are disabled -> return
+    if (fhss_structure->bs->fhss_configuration.fhss_number_of_channel_retries == 0) {
+        return false;
+    }
+    // Use channel retries only for data frames
+    if (FHSS_DATA_FRAME != frame_type) {
+        return false;
+    }
+
+    fhss_failed_tx_t *fhss_failed_tx = fhss_failed_handle_find(fhss_structure, handle);
+    if (fhss_failed_tx) {
+        fhss_failed_tx->retries_done++;
+        if (fhss_failed_tx->retries_done >= fhss_structure->bs->fhss_configuration.fhss_number_of_channel_retries) {
+            // No more retries. Return false to stop retransmitting.
+            fhss_failed_handle_remove(fhss_structure, handle);
+            return false;
+        }
+    } else {
+        // Create new failure handle and return true to retransmit
+        fhss_failed_handle_add(fhss_structure, handle, fhss_structure->rx_channel);
+    }
+    return true;
+}
+
+static void fhss_receive_frame_callback(const fhss_api_t *api, uint16_t pan_id, uint8_t *source_address, uint32_t timestamp, uint8_t *synch_info, int frame_type)
+{
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
+    if (!fhss_structure) {
+        return;
+    }
+    if (FHSS_SYNCH_FRAME == frame_type) {
+        if ((fhss_structure->fhss_state == FHSS_UNSYNCHRONIZED) || fhss_structure->bs->synch_panid != pan_id) {
+            fhss_add_beacon_info(fhss_structure, pan_id, source_address, timestamp, synch_info);
+        } else {
+            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) {
+        // If current channel is broadcast, we don't need to send another synch info on next broadcast channel.
+        // Only send number of MAX_SYNCH_INFOS_PER_CHANNEL_LIST synch infos per one channel list cycle
+        if ((fhss_structure->fhss_state == FHSS_SYNCHRONIZED) && (fhss_is_current_channel_broadcast(fhss_structure) == false)
+                && (fhss_structure->bs->synch_infos_sent_counter < MAX_SYNCH_INFOS_PER_CHANNEL_LIST)) {
+            fhss_structure->bs->send_synch_info_on_next_broadcast_channel = true;
+        }
+    }
+}
+
+static uint16_t fhss_get_retry_period_callback(const fhss_api_t *api, uint8_t *destination_address, uint16_t phy_mtu)
+{
+    uint16_t retry_period = 0;
+    uint16_t random_number = randLIB_get_16bit();
+    uint16_t rnd_mask;
+
+    /* Generate retry back-off period. FHSS is using the known synchronization parent info to delay retransmissions upstream.
+     *
+     */
+    if (phy_mtu < 128) {
+        // Max. random when PHY MTU below 128 is 6.4ms
+        rnd_mask = 0x7f;
+    } else if (phy_mtu < 256) {
+        // Max. random when PHY MTU below 256 is 12.8ms
+        rnd_mask = 0xff;
+    } else {
+        // Max. random when PHY MTU above 255 is 25.6ms
+        rnd_mask = 0x1ff;
+    }
+
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
+    if (fhss_structure) {
+        uint32_t datarate = fhss_structure->callbacks.read_datarate(fhss_structure->fhss_api);
+        uint16_t max_tx_length;
+
+        if (datarate && phy_mtu) {
+            if (fhss_compare_with_synch_parent_address(fhss_structure, destination_address) == 0) {
+                // E.g. (1000000 / (250000bit/s / 8 bits)) * 255 bytes = 8160us
+                max_tx_length = ((1000000 / (datarate / 8)) * phy_mtu);
+                /* Retrying upstream: delay the transmission until assumed hidden node has retried downstream:
+                 * Static period: max random + max tx length
+                 * 50 comes from MAC timer resolution (50us)
+                 */
+                retry_period = (rnd_mask + (max_tx_length / 50));
+            }
+        }
+    }
+
+    // Add 1 to not to ever return zero value.
+    retry_period += ((random_number & rnd_mask) + 1);
+    return retry_period;
+}
+
+static bool fhss_is_broadcast_channel_callback(const fhss_api_t *api)
+{
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
+    if (!fhss_structure) {
+        return true;
+    }
+    // FHSS is unsynchronized, broadcasts allowed
+    if (fhss_structure->fhss_state == FHSS_UNSYNCHRONIZED) {
+        return true;
+    }
+    return fhss_is_current_channel_broadcast(fhss_structure);
+}
+
+static bool fhss_use_broadcast_queue_cb(const fhss_api_t *api, bool is_broadcast_addr, int frame_type)
+{
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
+    if (!fhss_structure) {
+        return false;
+    }
+    // Synch requests are always stored in unicast queue
+    if (frame_type == FHSS_SYNCH_REQUEST_FRAME) {
+        return false;
+    }
+    // Broadcast packets are stored in broadcast queue
+    return is_broadcast_addr;
+}
+
+static void fhss_superframe_handler(const fhss_api_t *fhss_api, uint16_t delay)
+{
+    uint32_t timeout = 0;
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(fhss_api);
+    if (!fhss_structure) {
+        return;
+    }
+
+    timeout = fhss_get_sf_timeout_callback(fhss_structure);
+
+    fhss_start_timer(fhss_structure, timeout - (delay * fhss_structure->platform_functions.fhss_resolution_divider), fhss_superframe_handler);
+
+    if (fhss_structure->bs->current_superframe++ >= (fhss_structure->bs->synch_configuration.fhss_number_of_superframes - 1)) {
+        fhss_structure->bs->current_superframe = 0;
+        if (++fhss_structure->bs->current_channel_index >= fhss_structure->number_of_channels) {
+            fhss_structure->bs->current_channel_index = 0;
+        }
+        fhss_update_channel_callback(fhss_structure);
+    }
+    fhss_superframe_callback(fhss_structure);
+
+    if (fhss_structure->fhss_timeout) {
+        fhss_structure->fhss_timer += fhss_structure->bs->synch_configuration.fhss_superframe_length;
+        if (fhss_structure->fhss_timer >= fhss_structure->fhss_timeout) {
+            fhss_trig_event(fhss_structure, FHSS_TIMER_EVENT);
+            fhss_structure->fhss_timeout = 0;
+            fhss_structure->fhss_timer = 0;
+        }
+    }
+}
+
+uint32_t fhss_get_remaining_time_to_next_superframe(const fhss_structure_t *fhss_structure)
+{
+    const uint32_t slots = fhss_structure->platform_functions.fhss_get_remaining_slots(fhss_superframe_handler, fhss_structure->fhss_api);
+    return (slots * fhss_structure->platform_functions.fhss_resolution_divider);
+}
+
+int8_t fhss_set_synch_configuration(fhss_structure_t *fhss_structure, const fhss_synch_configuration_t *fhss_synch_configuration)
+{
+    if (!fhss_structure) {
+        return -1;
+    }
+    if (!fhss_synch_configuration) {
+        return -2;
+    }
+    // None of the configurations can be set zero
+    if( fhss_synch_configuration->fhss_number_of_bc_channels == 0 || fhss_synch_configuration->fhss_number_of_tx_slots == 0
+            || fhss_synch_configuration->fhss_number_of_superframes == 0 || fhss_synch_configuration->fhss_superframe_length == 0) {
+        return -3;
+    }
+    // Number of channels must be divisible with the number of broadcast channels.
+    // Number of superframes must be divisible with the number of TX slots
+    if (((fhss_structure->number_of_channels % fhss_synch_configuration->fhss_number_of_bc_channels) != 0) ||
+            ((fhss_synch_configuration->fhss_number_of_superframes % fhss_synch_configuration->fhss_number_of_tx_slots) != 0) ||
+            (fhss_synch_configuration->fhss_number_of_superframes <= fhss_synch_configuration->fhss_number_of_tx_slots)) {
+        return -4;
+    }
+    fhss_structure->bs->synch_configuration = *fhss_synch_configuration;
+    fhss_structure->own_hop = 0;
+    return 0;
+}
+
+uint8_t fhss_calculate_uc_index(uint8_t channel_index, uint16_t number_of_channels, uint8_t number_of_broadcast_channels)
+{
+    // When channel index is 0, return last unicast index
+    if (channel_index == 0) {
+        return (number_of_channels - number_of_broadcast_channels - 1);
+    }
+    uint16_t bc_channel_density = (number_of_channels/number_of_broadcast_channels);
+    return channel_index - (channel_index/bc_channel_density) - 1;
+}
+
+int fhss_set_callbacks(fhss_structure_t *fhss_structure)
+{
+    // Set external API
+    fhss_structure->fhss_api->is_broadcast_channel = &fhss_is_broadcast_channel_callback;
+    fhss_structure->fhss_api->use_broadcast_queue = &fhss_use_broadcast_queue_cb;
+    fhss_structure->fhss_api->tx_handle = &fhss_tx_handle_callback;
+    fhss_structure->fhss_api->check_tx_conditions = &fhss_check_tx_conditions_callback;
+    fhss_structure->fhss_api->receive_frame = &fhss_receive_frame_callback;
+    fhss_structure->fhss_api->data_tx_done = &fhss_data_tx_done_callback;
+    fhss_structure->fhss_api->data_tx_fail = &fhss_data_tx_fail_callback;
+    fhss_structure->fhss_api->synch_state_set = &fhss_synch_state_set_callback;
+    fhss_structure->fhss_api->read_timestamp = &fhss_read_timestamp_cb;
+    fhss_structure->fhss_api->get_retry_period = &fhss_get_retry_period_callback;
+    fhss_structure->fhss_api->write_synch_info = &fhss_write_synch_info_callback;
+    fhss_structure->fhss_api->init_callbacks = &fhss_init_callbacks_cb;
+
+    return 0;
+}
+
+static int8_t fhss_beacon_create_tasklet(fhss_structure_t *fhss_structure)
+{
+    if (fhss_structure->beacon_tasklet_id < 0) {
+        fhss_structure->beacon_tasklet_id = eventOS_event_handler_create(fhss_beacon_tasklet_func, FHSS_TASKLET_INIT_EVENT);
+    }
+    return fhss_structure->beacon_tasklet_id;
+}
+
+static int fhss_beacon_periodic_start(fhss_structure_t *fhss_structure, uint32_t time_to_first_beacon)
+{
+    int ret_val = -1;
+
+    if (fhss_structure) {
+        fhss_beacon_periodic_stop(fhss_structure);
+        ret_val = fhss_timeout_start(fhss_structure, time_to_first_beacon * 1000);
+    }
+    return ret_val;
+}
+
+static void fhss_beacon_periodic_stop(fhss_structure_t *fhss_structure)
+{
+    if (fhss_structure) {
+        fhss_timeout_stop(fhss_structure);
+    }
+}
+
+static void fhss_beacon_tasklet_func(arm_event_s* event)
+{
+    fhss_structure_t *fhss_structure = (fhss_structure_t *)event->data_ptr;
+    if (!fhss_structure) {
+        return;
+    }
+    uint8_t parent_address[8];
+    fhss_clear_active_event(fhss_structure, event->event_type);
+    // skip the init event as there will be a timer event after
+    if (event->event_type == FHSS_TIMER_EVENT) {
+        // Stop network when lost number of FHSS_SYNCHRONIZATION_LOST synchronization beacons from parent in a row.
+        if (fhss_structure->bs->beacons_received_timer >= FHSS_SYNCHRONIZATION_LOST) {
+            fhss_structure->callbacks.synch_lost_notification(fhss_structure->fhss_api);
+            fhss_stats_update(fhss_structure, STATS_FHSS_SYNCH_LOST, 1);
+            tr_err("FHSS synchronization lost");
+        } else {
+            uint16_t bc_density = (fhss_structure->number_of_channels / fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels);
+            uint16_t channel_dwell_time = ((uint32_t)fhss_structure->bs->synch_configuration.fhss_superframe_length * fhss_structure->bs->synch_configuration.fhss_number_of_superframes) / 1000;
+
+            fhss_beacon_periodic_start(fhss_structure, (bc_density * channel_dwell_time) * 2);
+            // Send synchronization request
+            fhss_structure->callbacks.send_fhss_frame(fhss_structure->fhss_api, FHSS_SYNCH_REQUEST_FRAME);
+            fhss_structure->bs->beacons_received_timer++;
+#ifdef FEA_TRACE_SUPPORT
+            if (!fhss_get_parent_address(fhss_structure, parent_address)) {
+                tr_debug("Update synch, attempt: %u, %s", fhss_structure->bs->beacons_received_timer, trace_array(parent_address, 8));
+            } else {
+                tr_err("No synch parent found");
+            }
+#endif /*FEA_TRACE_SUPPORT*/
+        }
+    }
+    // Compare if synchronization parent has changed and request beacon if needed
+    else if(event->event_type == FHSS_COMPARE_SYNCH_PARENT)
+    {
+        if (fhss_compare_with_synch_parent_address(fhss_structure, fhss_structure->synch_parent)) {
+            fhss_structure->bs->synch_monitor.avg_synch_fix = 0;
+            if(fhss_structure->bs->synch_monitor.avg_synch_fix_counter > 0) {
+                fhss_structure->bs->synch_monitor.avg_synch_fix_counter = 0;
+            }
+            // Send synchronization request
+            fhss_structure->callbacks.send_fhss_frame(fhss_structure->fhss_api, FHSS_SYNCH_REQUEST_FRAME);
+#ifdef FEA_TRACE_SUPPORT
+            if (!fhss_get_parent_address(fhss_structure, parent_address)) {
+                tr_debug("Synch parent changed, New: %s, Old: %s\n", trace_array(parent_address, 8), trace_array(fhss_structure->synch_parent, 8));
+            } else {
+                tr_err("Synch parent changed : No parent found");
+            }
+#endif /*FEA_TRACE_SUPPORT*/
+        }
+    }
+    else if(event->event_type == FHSS_BROADCAST_CHANNEL)
+    {
+        uint16_t superframe_length = fhss_structure->bs->synch_configuration.fhss_superframe_length;
+        uint8_t number_of_superframes = fhss_structure->bs->synch_configuration.fhss_number_of_superframes;
+        // Given broadcast time is channel length minus 1 superframe
+        fhss_structure->callbacks.broadcast_notify(fhss_structure->fhss_api, (uint32_t)superframe_length * (number_of_superframes - 1));
+    }
+    // Update Beacon info lifetimes
+    else if(event->event_type == FHSS_UPDATE_SYNCH_INFO_STORAGE)
+    {
+        fhss_update_beacon_info_lifetimes(fhss_structure, fhss_read_timestamp_cb(fhss_structure->fhss_api));
     }
 }
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss.h b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss.h
index ed38820cb7..e78073f963 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss.h
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -16,164 +16,110 @@
  */
 #ifndef FHSS_H_
 #define FHSS_H_
-#include "ns_list.h"
 #define MAX_SCRAMBLE_TABLE_INDEXES  20
-
 // Lifetime is given as seconds
 #define BEACON_INFO_LIFETIME        600
 // Limits the number of synchronization info messages sent on broadcast channels
 #define MAX_SYNCH_INFOS_PER_CHANNEL_LIST	2
 // FHSS randomly selects the starting superframe for broadcast channel. This defines how many superframes are used for randomization.
 #define NUMBER_OF_BC_START_SUPERFRAMES  3
+#define FHSS_MAX_ALLOWED_HOPS   254
+#define MAX_FHSS_TIMER_DIVIDER  100
+#define SYNCH_MONITOR_AVG_SAMPLES   5
+#define FHSS_DATA_START_DELIMETER   0x00
+#define FHSS_SYNCHRONIZATION_LOST   10
+#define BEACON_INTERVAL_INIT_DIVIDER 100
+#define FHSS_SYNCH_DRIFT_TOO_HIGH_LIMIT 20000
+#define CLOSE_TO_SUPERFRAME_LENGTH  2000
+#define BEACON_FIELD_SIZE(field) (sizeof((fhss_synchronization_beacon_payload_s*)0)->field)
 
-#define FHSS_TASKLET_INIT_EVENT             0
-#define FHSS_TIMER_EVENT                    1
-#define FHSS_COMPARE_SYNCH_PARENT           2
-#define FHSS_BROADCAST_CHANNEL              3
-#define FHSS_UPDATE_SYNCH_INFO_STORAGE      4
+typedef struct fhss_structure fhss_structure_t;
+typedef struct fhss_beacon_info fhss_beacon_info_t;
+typedef struct fhss_synch_monitor fhss_synch_monitor_s;
+typedef struct fhss_failed_tx fhss_failed_tx_t;
+typedef struct fhss_bs fhss_bs_t;
+typedef struct fhss_synchronization_beacon_payload fhss_synchronization_beacon_payload_s;
 
-struct fhss_callback;
+struct fhss_synchronization_beacon_payload
+{
+    /** Start delimeter */
+    uint8_t data_start_delimeter;
+    /** Channel index */
+    uint8_t channel_index;
+    /** Sender unicast channel index */
+    uint8_t sender_unicast_channel;
+    /** Current superframe */
+    uint16_t current_superframe;
+    /** Remaining time (us) to next superframe */
+    uint16_t remaining_slots;
+    /** Channel list counter */
+    uint16_t channel_list_counter;
+    /** Hop count */
+    uint8_t hop_count;
+    /** Number of broadcast channels */
+    uint8_t number_of_broadcast_channels;
+    /** Number of TX slots per channel */
+    uint8_t number_of_tx_slots;
+    /** Time since last beacon (us) */
+    uint32_t time_since_last_beacon;
+    /** Processing delay (us) */
+    uint16_t processing_delay;
+    /** Superframe length */
+    uint16_t superframe_length;
+    /** Number of superframes per channel */
+    uint8_t number_of_superframes_per_channel;
+};
 
-typedef struct fhss_beacon_info
+struct fhss_beacon_info
 {
     uint16_t pan_id;
     uint8_t source_address[8];
     uint32_t timestamp;
     uint8_t synch_info[FHSS_SYNCH_INFO_LENGTH];
     struct fhss_beacon_info *next;
-}fhss_beacon_info_t;
+};
 
-typedef struct fhss_failed_tx
-{
-    uint8_t handle;
-    uint8_t bad_channel;
-    uint8_t retries_done;
-    ns_list_link_t link;
-}fhss_failed_tx_t;
-
-typedef NS_LIST_HEAD(fhss_failed_tx_t, link) fhss_failed_tx_list_t;
-
-typedef struct
+struct fhss_synch_monitor
 {
     int32_t avg_synch_fix;
     int avg_synch_fix_counter;
     int drift_compensation;
     int channel_counter;
-} fhss_synch_monitor_s;
+};
 
-typedef struct
+struct fhss_bs
 {
-    fhss_api_t *fhss_api;
-    uint32_t datarate;
-    fhss_states fhss_state;
-    fhss_timer_t platform_functions;
-    fhss_configuration_t fhss_configuration;
-    fhss_synch_configuration_t synch_configuration;
-
-    uint8_t fhss_resolution_divider;
-    /** Unicast channel index, [0..(number_of_channels-number_of_beacon_channels-1)] */
     uint8_t uc_channel_index;
-    /** Current superframe number, [0..(fhss_number_of_superframes-1)] */
     uint8_t current_superframe;
-    /** Current channel index, [0..(number_of channels-1)] */
     uint8_t current_channel_index;
-    /** Current broadcast index, [0..(number_of_bc_channels-1)] */
-    uint8_t broadcast_index;
-    /** Number of channels (unicast and broadcast) */
-    uint16_t number_of_channels;
-    /** Channel list counter is increased every time channel list is gone through*/
-    uint16_t channel_list_counter;
-    /** This is used to store current RX channel*/
-    uint8_t rx_channel;
-    /** Own hop count*/
-    uint8_t own_hop;
-    /** Holds the information: transmission is allowed or not on this superframe*/
-    bool tx_allowed;
-    /** The tasklet_id of periodic beacon sender, -1 if none is started. */
-    int8_t beacon_tasklet_id;
-    /** When synchronization is lost, this counter is used to detect the situation and stop network*/
     uint8_t beacons_received_timer;
-    /** Holds the current synchronization parent MAC address*/
-    uint8_t synch_parent[8];
-    // Synch info needs to be sent on next broadcast channel
-    bool send_synch_info_on_next_broadcast_channel;
-    // Used for randomizing broadcast sending. Device is not allowed to start broadcasting before the given superframe.
     uint8_t broadcast_start_superframe;
-    /*Indexes in this table will be used to extend the repeated channel sequence*/
-    uint8_t fhss_scramble_table[MAX_SCRAMBLE_TABLE_INDEXES];
-    /** Used to monitor and fix synchronization drift*/
-    fhss_synch_monitor_s synch_monitor;
-    /** Used to drop multiple synch info messages on same broadcast channel*/
-    bool beacon_received_on_this_bc_channel;
-    /** Timer used for events other than synchronization.*/
-    int8_t fhss_event_timer;
-    /* Counts the number of sent synch Beacons (Beacon on broadcast channel). Used to limit Beacon traffic when several nodes are scanning network*/
     uint8_t synch_infos_sent_counter;
-    uint32_t synch_interval;
-    /* This timer/timeout is used to trigger periodically Beacon requests. Resolution is superframe length. Note that FHSS must be synchronized to use this*/
-    uint32_t fhss_timeout;
-    uint32_t fhss_timer;
+    bool tx_allowed:1;
+    bool send_synch_info_on_next_broadcast_channel:1;
+    bool beacon_received_on_this_bc_channel:1;
+    uint16_t channel_list_counter;
     uint16_t synch_panid;
-    /* Bit mask for FHSS events pushed to event queue. Prevents pushing same event to queue multiple times*/
-    uint8_t active_fhss_events;
+    uint32_t synch_interval;
+    struct fhss_statistics *fhss_stats_ptr;
+    struct fhss_beacon_info *fhss_beacon_info_store;
+    struct fhss_configuration fhss_configuration;
+    struct fhss_synch_configuration synch_configuration;
+    struct fhss_synch_monitor synch_monitor;
+    uint8_t fhss_scramble_table[MAX_SCRAMBLE_TABLE_INDEXES];
+};
 
-    struct fhss_callback callbacks;
-    fhss_beacon_info_t *fhss_beacon_info_store;
-    fhss_failed_tx_list_t fhss_failed_tx_list;
-    fhss_statistics_t *fhss_stats_ptr;
-} fhss_structure_t;
-
-int8_t fhss_enable(fhss_api_t *fhss_api, const fhss_configuration_t *fhss_configuration, const fhss_timer_t *fhss_timer, fhss_statistics_t *fhss_statistics);
-int8_t fhss_disable(fhss_structure_t *fhss_structure);
-int8_t fhss_set_datarate(fhss_structure_t *fhss_structure, uint32_t datarate);
+fhss_structure_t *fhss_enable(fhss_api_t *fhss_api, const fhss_configuration_t *fhss_configuration, const fhss_timer_t *fhss_timer, fhss_statistics_t *fhss_statistics);
 bool fhss_is_synch_root(fhss_structure_t *fhss_structure);
-int8_t fhss_set_synch_configuration(fhss_structure_t *fhss_structure, const fhss_synch_configuration_t *fhss_synch_configuration);
-bool fhss_check_bad_channel(fhss_structure_t *fhss_structure, uint8_t handle);
-bool fhss_check_channel_type(fhss_structure_t *fhss_structure, bool is_bc, int frame_type);
-bool fhss_check_tx_allowed(fhss_structure_t *fhss_structure, bool is_bc, uint16_t frame_length, int frame_type, uint8_t phy_header_length, uint8_t phy_tail_length);
-fhss_structure_t *fhss_get_object_with_api(const fhss_api_t *fhss_api);
-fhss_beacon_info_t *fhss_get_beacon_info(fhss_structure_t *fhss_structure, uint16_t pan_id);
-int fhss_flush_beacon_info_storage(fhss_structure_t *fhss_structure);
-int fhss_add_beacon_info(fhss_structure_t *fhss_structure, uint16_t pan_id, uint8_t *source_address, uint32_t timestamp, uint8_t *synch_info);
-void fhss_update_beacon_info_lifetimes(fhss_structure_t *fhss_structure, uint32_t timestamp);
-void fhss_trig_event(fhss_structure_t *fhss_structure, uint8_t event_type);
-int fhss_timeout_start(fhss_structure_t *fhss_structure, uint32_t time);
-int fhss_timeout_stop(fhss_structure_t *fhss_structure);
-int fhss_compare_with_synch_parent_address(fhss_structure_t *fhss_structure, const uint8_t *source_addr);
-uint32_t fhss_get_tx_time(fhss_structure_t *fhss_structure, uint16_t bytes_to_send, uint8_t phy_header_length, uint8_t phy_tail_length);
-int fhss_get_parent_address(fhss_structure_t *fhss_structure, uint8_t *p_addr);
-int fhss_down(fhss_structure_t *fhss_structure);
-int fhss_update_synch_parent_address(fhss_structure_t *fhss_structure);
-void fhss_superframe_handler(const fhss_api_t *fhss_api, uint16_t delay);
-fhss_failed_tx_t *fhss_failed_handle_find(fhss_structure_t *fhss_structure, uint8_t handle);
-int fhss_failed_handle_add(fhss_structure_t *fhss_structure, uint8_t handle);
-int fhss_failed_handle_remove(fhss_structure_t *fhss_structure, uint8_t handle);
-void fhss_set_active_event(fhss_structure_t *fhss_structure, uint8_t event_type);
-void fhss_clear_active_event(fhss_structure_t *fhss_structure, uint8_t event_type);
-bool fhss_read_active_event(fhss_structure_t *fhss_structure, uint8_t event_type);
-#define MAX_FHSS_TIMER_DIVIDER  100
-#define SYNCH_MONITOR_AVG_SAMPLES   5
-
-// TX/RX slot management interface
-/**
- * Update slots
- *
- * Every superframe handler interrupt calls this function to update the TX/RX slot status.
- *
- * @param fhss_structure Pointer to FHSS structure
- * @return 0 for success, -1 for fail
- */
-int fhss_update_txrx_slots(fhss_structure_t *fhss_structure);
-
-void fhss_start_timer(fhss_structure_t *fhss_structure, uint32_t time, void (*callback)(const fhss_api_t *fhss_api, uint16_t));
-
-
 /**
  * Calculate time in microseconds to start of next superframe.
  *
- * @param fhss_struct FHSS state
+ * @param fhss_struct FHSS structure
  * @return microseconds left to start of next superframe
  */
 uint32_t fhss_get_remaining_time_to_next_superframe(const fhss_structure_t *fhss_struct);
+int8_t fhss_set_synch_configuration(fhss_structure_t *fhss_structure, const fhss_synch_configuration_t *fhss_synch_configuration);
+int fhss_set_callbacks(fhss_structure_t *fhss_structure);
+uint8_t fhss_calculate_uc_index(uint8_t channel_index, uint16_t number_of_channels, uint8_t number_of_broadcast_channels);
 
 #endif /* FHSS_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_beacon.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_beacon.c
deleted file mode 100644
index fccec57b26..0000000000
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_beacon.c
+++ /dev/null
@@ -1,227 +0,0 @@
-/*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
- * SPDX-License-Identifier: Apache-2.0
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-
-#include "nsconfig.h"
-#include "fhss_api.h"
-#include "fhss_config.h"
-#include "fhss.h"
-#include "fhss_beacon.h"
-
-#include "common_functions.h"
-#include <string.h> // memcpy
-#include "ns_trace.h"
-
-#define TRACE_GROUP "fhss"
-
-// This function should be called just prior actually sending a beacon packet
-// to get precise information. Especially the time variables are essential.
-int fhss_beacon_update_payload(fhss_structure_t *fhss_structure,
-        fhss_synchronization_beacon_payload_s *payload)
-{
-
-    int ret_val = 0;
-    if (!fhss_structure || !payload) {
-        return -1;
-    }
-
-    const fhss_synch_configuration_t *config = &fhss_structure->synch_configuration;
-
-    payload->channel_index = fhss_structure->current_channel_index;
-
-    payload->sender_unicast_channel = 0;
-
-    payload->current_superframe = fhss_structure->current_superframe;
-
-    // This assumes that the time is always in the range of 0..2**16, which
-    // should be the case as the superframe length field is also in that range.
-    payload->remaining_slots = (uint16_t) fhss_get_remaining_time_to_next_superframe(fhss_structure);
-    payload->channel_list_counter = fhss_structure->channel_list_counter;
-
-    payload->hop_count = fhss_structure->own_hop;
-    payload->number_of_broadcast_channels = config->fhss_number_of_bc_channels;
-    payload->number_of_tx_slots = config->fhss_number_of_tx_slots;
-    payload->time_since_last_beacon = 0; // XXX not available yet
-    // TODO: Get Beacon length from MAC
-    uint32_t tx_time = fhss_get_tx_time(fhss_structure, 71, 0, 0);
-    payload->processing_delay = fhss_structure->fhss_configuration.fhss_tuning_parameters.tx_processing_delay + tx_time;
-
-    payload->superframe_length = config->fhss_superframe_length;
-
-    payload->number_of_superframes_per_channel = config->fhss_number_of_superframes;
-
-    return ret_val;
-}
-
-uint8_t* fhss_beacon_encode_raw(uint8_t* buffer, const fhss_synchronization_beacon_payload_s* source)
-{
-    *buffer++ = FHSS_DATA_START_DELIMETER;
-    *buffer++ = source->channel_index;
-    *buffer++ = source->sender_unicast_channel;
-    buffer = common_write_16_bit(source->current_superframe, buffer);
-    buffer = common_write_16_bit(source->remaining_slots, buffer);
-    buffer = common_write_16_bit(source->channel_list_counter, buffer);
-    *buffer++ = source->hop_count;
-    *buffer++ = source->number_of_broadcast_channels;
-    *buffer++ = source->number_of_tx_slots;
-    buffer = common_write_32_bit(source->time_since_last_beacon, buffer);
-    buffer = common_write_16_bit(source->processing_delay, buffer);
-    buffer = common_write_16_bit(source->superframe_length, buffer);
-    *buffer++ = source->number_of_superframes_per_channel;
-
-    return buffer;
-}
-
-uint8_t fhss_calculate_uc_index(uint8_t channel_index, uint16_t number_of_channels, uint8_t number_of_broadcast_channels)
-{
-    // When channel index is 0, return last unicast index
-    if (channel_index == 0) {
-        return (number_of_channels - number_of_broadcast_channels - 1);
-    }
-    uint16_t bc_channel_density = (number_of_channels/number_of_broadcast_channels);
-    return channel_index - (channel_index/bc_channel_density) - 1;
-}
-
-
-uint32_t fhss_get_time_to_next_channel_change(uint16_t remaining_slots_to_next_superframe, uint8_t number_of_superframes,
-        uint8_t current_superframe, uint16_t superframe_length)
-{
-    return remaining_slots_to_next_superframe + ((uint32_t)((number_of_superframes - 1) - current_superframe) * superframe_length);
-}
-
-void fhss_beacon_decode_raw(fhss_synchronization_beacon_payload_s* dest, const uint8_t* buffer)
-{
-    dest->data_start_delimeter = *buffer++;
-
-    dest->channel_index = *buffer++;
-    dest->sender_unicast_channel = *buffer++;
-
-    dest->current_superframe = common_read_16_bit(buffer);
-    buffer += BEACON_FIELD_SIZE(current_superframe);
-
-    dest->remaining_slots = common_read_16_bit(buffer);
-    buffer += BEACON_FIELD_SIZE(remaining_slots);
-
-    dest->channel_list_counter = common_read_16_bit(buffer);
-    buffer += BEACON_FIELD_SIZE(channel_list_counter);
-
-    dest->hop_count = *buffer++;
-    dest->number_of_broadcast_channels = *buffer++;
-    dest->number_of_tx_slots = *buffer++;
-
-    dest->time_since_last_beacon = common_read_32_bit(buffer);
-    buffer += BEACON_FIELD_SIZE(time_since_last_beacon);
-
-    dest->processing_delay += common_read_16_bit(buffer);
-
-    buffer += BEACON_FIELD_SIZE(processing_delay);
-
-    dest->superframe_length = common_read_16_bit(buffer);
-    buffer += BEACON_FIELD_SIZE(superframe_length);
-
-    dest->number_of_superframes_per_channel = *buffer;
-}
-
-// Decode the given raw byte buffer into a struct into dest struct and calculate
-// the new values for elapsed_time, channel_index, current_superframe and remaining_slots 
-// from current state and given data.
-void fhss_beacon_decode(fhss_synchronization_beacon_payload_s* dest, const uint8_t* buffer, uint32_t elapsed_time, uint16_t number_of_channels)
-{
-    fhss_beacon_decode_raw(dest, buffer);
-
-    elapsed_time += dest->processing_delay;
-
-    /* To calculate channel index after beacon scan, following calculation is performed
-     *
-     *                           rem. slots to channel change(X)    Channel length (V)
-     *                              |---------------------|     |-----------------------------------------------|
-     *    |    RX'd channel index (Y)                     | ... |                      Y+n                      |
-     * ...|    sf1    |    sf2    |    sf3    |    sf4    | ... |    sf1    |    sf2    |    sf3    |    sf4    |...
-     *                              ^                                     ^
-     *                              |beacon received                      |beacon scan done
-     *                              |-------------------------------------|
-     *                               measured time after beacon RX'd(Z)
-     * V = superframe length * number of superframes
-     * X = remaining slots to superframe change + length of the remaining full superframes to channel change
-     *
-     * Y+n = Y + ((Z - X) / V) + 1
-     *
-     * Or if (Z < X)
-     *       Y+n = Y
-     */
-
-    uint32_t remaining_slots_to_next_channel = fhss_get_time_to_next_channel_change(dest->remaining_slots, dest->number_of_superframes_per_channel, dest->current_superframe, dest->superframe_length);
-    uint16_t temp_channel_index = dest->channel_index;
-    if (elapsed_time >= remaining_slots_to_next_channel) {
-        uint32_t channel_length = (uint32_t) dest->number_of_superframes_per_channel * dest->superframe_length;
-        temp_channel_index = dest->channel_index + ((elapsed_time - remaining_slots_to_next_channel) / channel_length) + 1;
-    }
-    while (temp_channel_index >= number_of_channels) {
-        temp_channel_index -= number_of_channels;
-        dest->channel_list_counter++;
-    }
-    dest->channel_index = temp_channel_index;
-    while (dest->channel_list_counter >= (number_of_channels * MAX_SCRAMBLE_TABLE_INDEXES)) {
-        dest->channel_list_counter -= (number_of_channels * MAX_SCRAMBLE_TABLE_INDEXES);
-    }
-
-    /* To calculate superframe after beacon scan, following calculation is performed
-     *
-     *           rem. slots(X)         sf. length(V)
-     *        |---------------|     |-----------------|
-     *...| RX'd superframe (Y)| ... |      Y+n        |      Y+n+1       |....
-     *        ^                                     ^
-     *        |beacon received                      |beacon scan done
-     *        |-------------------------------------|
-     *          measured time after beacon RX'd(Z)
-     *
-     * Y+n = Y + ((Z - X) / V) + 1
-     *
-     * Or if (Z < X)
-     *       Y+n = Y
-     */
-
-    if (elapsed_time >= dest->remaining_slots) {
-        dest->current_superframe = dest->current_superframe + ((elapsed_time - dest->remaining_slots) / dest->superframe_length) + 1;
-    }
-    while (dest->current_superframe >= dest->number_of_superframes_per_channel) {
-        dest->current_superframe -= dest->number_of_superframes_per_channel;
-    }
-
-    /* To get the remaining slots after beacon scan, following calculation is performed
-     *
-     *         rem. slots(Y)               sf. length(V)    new rem. slots(X)
-     *        |----------|               |---------------| |-------------|
-     *...| superframe 1  | superframe 2  | superframe 3  | superframe 4  |...
-     *        ^                                            ^
-     *        |beacon received                             |beacon scan done
-     *        |--------------------------------------------|
-     *          measured time after beacon RX'd(Z)
-     *
-     * X = V - ((Z - Y) % V)
-     *
-     * Or if (Z < Y)
-     *        X = Y - Z
-     */
-
-    if (elapsed_time < dest->remaining_slots) {
-        dest->remaining_slots = dest->remaining_slots - elapsed_time;
-    } else {
-        dest->remaining_slots = dest->superframe_length - ((elapsed_time - dest->remaining_slots) % dest->superframe_length);
-    }
-
-}
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_beacon.h b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_beacon.h
deleted file mode 100644
index 7684b1c168..0000000000
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_beacon.h
+++ /dev/null
@@ -1,177 +0,0 @@
-/*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
- * SPDX-License-Identifier: Apache-2.0
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-#ifndef __FHSS_BEACON_H__
-#define __FHSS_BEACON_H__
-
-#ifdef __cplusplus
-extern "C" {
-#endif
-
-#include <stdint.h>
-
-#define FHSS_DATA_START_DELIMETER   0x00
-
-#define FHSS_SYNCH_BEACON_LIMIT     2
-#define FHSS_STOP_SENDING_BEACONS   8
-#define FHSS_SYNCHRONIZATION_LOST   10
-
-// Based on datarate and Beacon frame packet length 71 bytes
-#define BEACON_TX_TIME              (1000000 / (dev_get_phy_datarate(cur) / 8) * 71)
-
-#define BEACON_INTERVAL_INIT_DIVIDER 100
-#define FHSS_SYNCH_DRIFT_TOO_HIGH_LIMIT 20000
-#define CLOSE_TO_SUPERFRAME_LENGTH  2000
-
-/** beacon synchronization info */
-typedef struct {
-    /** Start delimeter */
-    uint8_t data_start_delimeter;
-
-    /** Channel index */
-    uint8_t channel_index;
-
-    /** Sender unicast channel index */
-    uint8_t sender_unicast_channel;
-
-    /** Current superframe */
-    uint16_t current_superframe;
-
-    /** Remaining time (us) to next superframe */
-    uint16_t remaining_slots;
-
-    /** Channel list counter */
-    uint16_t channel_list_counter;
-
-    /** Hop count */
-    uint8_t hop_count;
-
-    /** Number of broadcast channels */
-    uint8_t number_of_broadcast_channels;
-
-    /** Number of TX slots per channel */
-    uint8_t number_of_tx_slots;
-
-    /** Time since last beacon (us) */
-    uint32_t time_since_last_beacon;
-
-    /** Processing delay (us) */
-    uint16_t processing_delay;
-
-    /** Superframe length */
-    uint16_t superframe_length;
-
-    /** Number of superframes per channel */
-    uint8_t number_of_superframes_per_channel;
-
-} fhss_synchronization_beacon_payload_s;
-
-
-#define BEACON_FIELD_SIZE(field) (sizeof((fhss_synchronization_beacon_payload_s*)0)->field)
-
-// Calculate the size of data encoded from fhss_synchronization_beacon_payload_s,
-// please add new items from that struct to this macro to keep code working.
-#define FHSS_SYNC_BEACON_PAYLOAD_SYNC_SIZE \
-    (BEACON_FIELD_SIZE(data_start_delimeter) + \
-    BEACON_FIELD_SIZE(channel_index) + \
-    BEACON_FIELD_SIZE(sender_unicast_channel) + \
-    BEACON_FIELD_SIZE(current_superframe) + \
-    BEACON_FIELD_SIZE(remaining_slots) + \
-    BEACON_FIELD_SIZE(channel_list_counter) + \
-    BEACON_FIELD_SIZE(hop_count) + \
-    BEACON_FIELD_SIZE(number_of_broadcast_channels) + \
-    BEACON_FIELD_SIZE(number_of_tx_slots) + \
-    BEACON_FIELD_SIZE(time_since_last_beacon) + \
-    BEACON_FIELD_SIZE(processing_delay) + \
-    BEACON_FIELD_SIZE(superframe_length) + \
-    BEACON_FIELD_SIZE(number_of_superframes_per_channel))
-
-// 1 byte for protocol id,
-// 1 byte for accept join
-// 16 bytes for network id, TODO: find proper define to replace this magic value
-#define FHSS_SYNC_BEACON_PAYLOAD_PREFIX_SIZE (1 + 1 + 16)
-
-// this counts the common prefix and the fhss specific sync payload
-#define FHSS_SYNC_BEACON_PAYLOAD_COMPLETE_SIZE \
-    (FHSS_SYNC_BEACON_PAYLOAD_PREFIX_SIZE + FHSS_SYNC_BEACON_PAYLOAD_SYNC_SIZE)
-
-
-int fhss_beacon_periodic_start(fhss_structure_t *fhss_structure, uint32_t time_to_first_beacon);
-void fhss_beacon_periodic_stop(fhss_structure_t *fhss_structure);
-
-void fhss_beacon_build(fhss_structure_t *fhss_structure, uint8_t *dest);
-
-/**
- * Encode the given beacon syncronization structure into given buffer.
- *
- * Note: this information will not include protocol-id, accept_join or network-id
- *
- * @param buffer target buffer, must be at least FHSS_SYNC_BEACON_PAYLOAD_SIZE in size
- * @param source source synchronization data to be encoded
- * @return pointer to one byte after the encoded data
- */
-uint8_t* fhss_beacon_encode_raw(uint8_t *buffer, const fhss_synchronization_beacon_payload_s *source);
-
-/**
- * Decode the given beacon syncronization structure from given buffer and process the data.
- *
- * @param protocol_id protocol id from beacon payload
- * @param accept_join accept join from beacon payload
- * @param network_id network id from beacon payload, 16 bytes in size
- * @param dest decoded data
- * @param buffer source buffer, must be at least FHSS_SYNC_BEACON_PAYLOAD_SIZE in size
- * @param elapsed_time time since previous beacon
- * @param number_of_channels number of channels
- */
-void fhss_beacon_decode(fhss_synchronization_beacon_payload_s *dest, const uint8_t *buffer, uint32_t elapsed_time, uint16_t number_of_channels);
-
-/**
- * Decode the beacon data as-is to the given struct.
- * 
- * @param dest decoded data
- * @param buffer source buffer, must be at least FHSS_SYNC_BEACON_PAYLOAD_SIZE in size
- */
-void fhss_beacon_decode_raw(fhss_synchronization_beacon_payload_s* dest, const uint8_t* buffer);
-
-int fhss_beacon_update_payload(fhss_structure_t *fhss_structure,
-                                fhss_synchronization_beacon_payload_s *payload);
-
-void fhss_beacon_received(fhss_structure_t *fhss_structure, const uint8_t *synch_info, const uint32_t elapsed_time);
-
-/**
- * This function is called whenever a node receives a beacon in the "proper state".
- * The implmentation of it stores the data received and eventually synchronizes
- * itself.
- *
- * Note: this is in completely illogical header file, but we have a
- * circular dependency with net_fhss.h and protocol.h.
- *
- * @param cur the network interface which received beacon
- * @param payload decoded beacon payload information
- *
- * @return 0 on success
- */
-int fhss_sync_with_beacon(fhss_structure_t *fhss_structure,
-                          const fhss_synchronization_beacon_payload_s *payload);
-
-uint8_t fhss_calculate_uc_index(uint8_t channel_index, uint16_t number_of_channels, uint8_t number_of_broadcast_channels);
-int8_t fhss_beacon_create_tasklet(fhss_structure_t *fhss_structure);
-
-#ifdef __cplusplus
-}
-#endif
-
-#endif // !__FHSS_BEACON_H__
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_beacon_tasklet.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_beacon_tasklet.c
deleted file mode 100644
index 7c54b6d2b0..0000000000
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_beacon_tasklet.c
+++ /dev/null
@@ -1,157 +0,0 @@
-/*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
- * SPDX-License-Identifier: Apache-2.0
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-
-#include "nsconfig.h"
-#include "common_functions.h"
-#include "eventOS_event.h"
-#include "eventOS_event_timer.h"
-#include "ns_trace.h"
-#include "fhss_api.h"
-#include "fhss_config.h"
-#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
-
-#define TRACE_GROUP "fhss"
-
-static void fhss_beacon_tasklet_func(arm_event_s* event);
-
-int8_t fhss_beacon_create_tasklet(fhss_structure_t *fhss_structure)
-{
-    if (fhss_structure->beacon_tasklet_id < 0) {
-        fhss_structure->beacon_tasklet_id = eventOS_event_handler_create(fhss_beacon_tasklet_func, FHSS_TASKLET_INIT_EVENT);
-    }
-    return fhss_structure->beacon_tasklet_id;
-}
-
-int fhss_beacon_periodic_start(fhss_structure_t *fhss_structure,
-                                uint32_t time_to_first_beacon)
-{
-    int ret_val = -1;
-
-    if (fhss_structure) {
-        fhss_beacon_periodic_stop(fhss_structure);
-        ret_val = fhss_timeout_start(fhss_structure, time_to_first_beacon * 1000);
-    }
-    return ret_val;
-}
-
-void fhss_beacon_periodic_stop(fhss_structure_t *fhss_structure)
-{
-    if (fhss_structure) {
-        fhss_timeout_stop(fhss_structure);
-    }
-}
-
-static void fhss_beacon_tasklet_func(arm_event_s* event)
-{
-    fhss_structure_t *fhss_structure = (fhss_structure_t *)event->data_ptr;
-    if (!fhss_structure) {
-        return;
-    }
-#ifdef FEA_TRACE_SUPPORT
-    uint8_t parent_address[8];
-#endif
-    fhss_clear_active_event(fhss_structure, event->event_type);
-    // skip the init event as there will be a timer event after
-    if (event->event_type == FHSS_TIMER_EVENT) {
-        // Stop network when lost number of FHSS_SYNCHRONIZATION_LOST synchronization beacons from parent in a row.
-        if (fhss_structure->beacons_received_timer >= FHSS_SYNCHRONIZATION_LOST) {
-            fhss_structure->callbacks.synch_lost_notification(fhss_structure->fhss_api);
-            fhss_stats_update(fhss_structure, STATS_FHSS_SYNCH_LOST, 1);
-            tr_err("FHSS synchronization lost");
-        } else {
-            uint16_t bc_density = (fhss_structure->number_of_channels / fhss_structure->synch_configuration.fhss_number_of_bc_channels);
-            uint16_t channel_dwell_time = ((uint32_t)fhss_structure->synch_configuration.fhss_superframe_length * fhss_structure->synch_configuration.fhss_number_of_superframes) / 1000;
-
-            fhss_beacon_periodic_start(fhss_structure, (bc_density * channel_dwell_time) * 2);
-            // Send synchronization request
-            fhss_structure->callbacks.send_fhss_frame(fhss_structure->fhss_api, FHSS_SYNCH_REQUEST_FRAME);
-            fhss_structure->beacons_received_timer++;
-#ifdef FEA_TRACE_SUPPORT
-            if (!fhss_get_parent_address(fhss_structure, parent_address)) {
-                tr_debug("Update synch, attempt: %u, %s", fhss_structure->beacons_received_timer, trace_array(parent_address, 8));
-            } else {
-                tr_err("No synch parent found");
-            }
-#endif /*FEA_TRACE_SUPPORT*/
-        }
-    }
-    // Compare if synchronization parent has changed and request beacon if needed
-    else if(event->event_type == FHSS_COMPARE_SYNCH_PARENT)
-    {
-        if (fhss_compare_with_synch_parent_address(fhss_structure, fhss_structure->synch_parent)) {
-            fhss_structure->synch_monitor.avg_synch_fix = 0;
-            if(fhss_structure->synch_monitor.avg_synch_fix_counter > 0) {
-                fhss_structure->synch_monitor.avg_synch_fix_counter = 0;
-            }
-            // Send synchronization request
-            fhss_structure->callbacks.send_fhss_frame(fhss_structure->fhss_api, FHSS_SYNCH_REQUEST_FRAME);
-#ifdef FEA_TRACE_SUPPORT
-            if (!fhss_get_parent_address(fhss_structure, parent_address)) {
-                tr_debug("Synch parent changed, New: %s, Old: %s\n", trace_array(parent_address, 8), trace_array(fhss_structure->synch_parent, 8));
-            } else {
-                tr_err("Synch parent changed : No parent found");
-            }
-#endif /*FEA_TRACE_SUPPORT*/
-        }
-    }
-    else if(event->event_type == FHSS_BROADCAST_CHANNEL)
-    {
-        uint16_t superframe_length = fhss_structure->synch_configuration.fhss_superframe_length;
-        uint8_t number_of_superframes = fhss_structure->synch_configuration.fhss_number_of_superframes;
-        // Given broadcast time is channel length minus 1 superframe
-        fhss_structure->callbacks.broadcast_notify(fhss_structure->fhss_api, (uint32_t)superframe_length * (number_of_superframes - 1));
-    }
-    // Update Beacon info lifetimes
-    else if(event->event_type == FHSS_UPDATE_SYNCH_INFO_STORAGE)
-    {
-        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);
-}
-
-// this assumes that the buffer's data pointer is seeked to the beacon payload
-void fhss_beacon_received(fhss_structure_t *fhss_structure, const uint8_t *synch_info, const uint32_t elapsed_time) {
-
-    if (fhss_structure) {
-
-        if (synch_info) {
-            fhss_synchronization_beacon_payload_s temp_payload;
-            temp_payload.processing_delay = fhss_structure->fhss_configuration.fhss_tuning_parameters.rx_processing_delay;
-            fhss_beacon_decode(&temp_payload, synch_info, elapsed_time, fhss_structure->number_of_channels);
-
-            // use the received information
-            fhss_sync_with_beacon(fhss_structure, &temp_payload);
-        }
-    }
-}
-
-
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_channel.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_channel.c
index 8f28991f2a..7bbf622304 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_channel.c
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_channel.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -19,8 +19,8 @@
 #include "fhss_api.h"
 #include "fhss_config.h"
 #include "fhss.h"
+#include "fhss_common.h"
 #include "fhss_channel.h"
-#include "fhss_beacon.h"
 #include "channel_list.h"
 #include "randLIB.h"
 #include "ns_trace.h"
@@ -28,7 +28,7 @@
 #define TRACE_GROUP "fhss"
 
 // Enable this flag to use channel traces
-//#define FHSS_CHANNEL_DEBUG
+// #define FHSS_CHANNEL_DEBUG
 
 static uint8_t fhss_get_bc_index(const fhss_structure_t *fhss_structure);
 
@@ -90,10 +90,10 @@ uint8_t fhss_add_channel_list_counter(uint8_t index, uint16_t number_of_channels
 static void fhss_generate_broadcast_start_superframe(fhss_structure_t *fhss_structure)
 {
     // If the number of superframes is low, allow broadcast on any superframe
-    if (fhss_structure->synch_configuration.fhss_number_of_superframes < 8) {
-        fhss_structure->broadcast_start_superframe = 0;
+    if (fhss_structure->bs->synch_configuration.fhss_number_of_superframes < 8) {
+        fhss_structure->bs->broadcast_start_superframe = 0;
     } else {
-        fhss_structure->broadcast_start_superframe = randLIB_get_random_in_range(0, NUMBER_OF_BC_START_SUPERFRAMES - 1);
+        fhss_structure->bs->broadcast_start_superframe = randLIB_get_random_in_range(0, NUMBER_OF_BC_START_SUPERFRAMES - 1);
     }
 }
 
@@ -112,8 +112,8 @@ bool fhss_change_to_next_channel(fhss_structure_t *fhss_structure)
     bool broadcast_channel = false;
 
     uint16_t number_of_channels = fhss_structure->number_of_channels;
-    uint8_t number_of_broadcast_channels = fhss_structure->synch_configuration.fhss_number_of_bc_channels;
-    uint8_t unicast_channel_index = fhss_structure->uc_channel_index;
+    uint8_t number_of_broadcast_channels = fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels;
+    uint8_t unicast_channel_index = fhss_structure->bs->uc_channel_index;
     uint8_t channel_index_tmp;
 
     /* Get the channel number using channel index. Latter (number_of_broadcast_channels) indexes in channel table are broadcast channels and
@@ -123,26 +123,20 @@ bool fhss_change_to_next_channel(fhss_structure_t *fhss_structure)
      */
     /* Get broadcast channel */
     if (fhss_is_current_channel_broadcast(fhss_structure) == true) {
-        channel_index_tmp = fhss_calc_channel_shuffle((number_of_channels - number_of_broadcast_channels) + fhss_get_bc_index(fhss_structure), fhss_structure->number_of_channels, fhss_structure->synch_configuration.fhss_number_of_bc_channels);
+        channel_index_tmp = fhss_calc_channel_shuffle((number_of_channels - number_of_broadcast_channels) + fhss_get_bc_index(fhss_structure), fhss_structure->number_of_channels, fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels);
         fhss_generate_broadcast_start_superframe(fhss_structure);
         broadcast_channel = true;
     } else { /* Get unicast channel */
-        channel_index_tmp = fhss_calc_channel_shuffle(unicast_channel_index, fhss_structure->number_of_channels, fhss_structure->synch_configuration.fhss_number_of_bc_channels);
-        fhss_structure->uc_channel_index++;
-        if (fhss_structure->uc_channel_index >= number_of_channels - number_of_broadcast_channels) {
-            fhss_structure->uc_channel_index = 0;
+        channel_index_tmp = fhss_calc_channel_shuffle(unicast_channel_index, fhss_structure->number_of_channels, fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels);
+        if (++fhss_structure->bs->uc_channel_index >= number_of_channels - number_of_broadcast_channels) {
+            fhss_structure->bs->uc_channel_index = 0;
         }
     }
     // Reset Beacon received flag when channel has changed
-    fhss_structure->beacon_received_on_this_bc_channel = false;
-    channel_index_tmp = fhss_add_channel_list_counter(channel_index_tmp, fhss_structure->number_of_channels, fhss_structure->channel_list_counter, fhss_structure->fhss_scramble_table);
-    next_channel = channel_list_get_channel(fhss_structure->fhss_configuration.channel_mask, channel_index_tmp);
+    fhss_structure->bs->beacon_received_on_this_bc_channel = false;
+    channel_index_tmp = fhss_add_channel_list_counter(channel_index_tmp, fhss_structure->number_of_channels, fhss_structure->bs->channel_list_counter, fhss_structure->bs->fhss_scramble_table);
+    next_channel = channel_list_get_channel(fhss_structure->bs->fhss_configuration.channel_mask, channel_index_tmp);
 
-#ifdef FHSS_MASSIVE_TRACE
-    tr_debug("%"PRIu32": update, frame: %"PRIu8", channel: %d",
-            fhss_structure->platform_functions.fhss_get_timestamp(fhss_structure->fhss_api), fhss_structure->current_superframe,
-            next_channel);
-#endif
     fhss_structure->rx_channel = next_channel;
 #ifdef FHSS_CHANNEL_DEBUG
     if (fhss_is_current_channel_broadcast(fhss_structure) == true) {
@@ -158,8 +152,8 @@ bool fhss_change_to_next_channel(fhss_structure_t *fhss_structure)
 static uint8_t fhss_get_bc_index(const fhss_structure_t *fhss_structure)
 {
     uint16_t number_of_channels = fhss_structure->number_of_channels;
-    uint8_t number_of_bc_channels = fhss_structure->synch_configuration.fhss_number_of_bc_channels;
-    uint8_t cur_channel_index = fhss_structure->current_channel_index;
+    uint8_t number_of_bc_channels = fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels;
+    uint8_t cur_channel_index = fhss_structure->bs->current_channel_index;
 
     return cur_channel_index / (number_of_channels/number_of_bc_channels);
 }
@@ -169,7 +163,7 @@ uint8_t fhss_get_offset(fhss_structure_t *fhss_structure, const uint8_t *ptr)
     uint8_t i;
     uint8_t index = *ptr++;
 
-    if (fhss_structure->number_of_channels == fhss_structure->synch_configuration.fhss_number_of_bc_channels) {
+    if (fhss_structure->number_of_channels == fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels) {
         // If all channels are defined as broadcast channels then return 0 to avoid division by 0.
         // This could happen e.g. in OTA case when fast download is needed.
         return 0;
@@ -181,7 +175,7 @@ uint8_t fhss_get_offset(fhss_structure_t *fhss_structure, const uint8_t *ptr)
         index ^= *ptr++;
     }
     // Offset must be < number of unicast channels
-    index %= (fhss_structure->number_of_channels - fhss_structure->synch_configuration.fhss_number_of_bc_channels);
+    index %= (fhss_structure->number_of_channels - fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels);
 
     return index;
 }
@@ -194,13 +188,13 @@ bool fhss_is_current_channel_broadcast(fhss_structure_t *fhss_structure)
     }
 
     // Should always have broadcast channels with FHSS
-    if (!fhss_structure->synch_configuration.fhss_number_of_bc_channels) {
+    if (!fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels) {
         return true;
     }
 
-    uint8_t channel_index = fhss_structure->current_channel_index;
+    uint8_t channel_index = fhss_structure->bs->current_channel_index;
     uint16_t number_of_channels = fhss_structure->number_of_channels;
-    uint8_t number_of_broadcast_channels = fhss_structure->synch_configuration.fhss_number_of_bc_channels;
+    uint8_t number_of_broadcast_channels = fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels;
 
     if (!(channel_index % (number_of_channels / number_of_broadcast_channels))) {
         return true;
@@ -216,15 +210,15 @@ static uint8_t fhss_get_destination_channel(fhss_structure_t *fhss_structure, ui
     if (fhss_structure) {
         if (fhss_is_current_channel_broadcast(fhss_structure) == false) {
             destination_offset = fhss_get_offset(fhss_structure, destination_address);
-            uc_index = fhss_calculate_uc_index(fhss_structure->current_channel_index, fhss_structure->number_of_channels,
-                    fhss_structure->synch_configuration.fhss_number_of_bc_channels) + destination_offset;
-            if (uc_index >= (fhss_structure->number_of_channels - fhss_structure->synch_configuration.fhss_number_of_bc_channels)) {
-                uc_index -= (fhss_structure->number_of_channels - fhss_structure->synch_configuration.fhss_number_of_bc_channels);
+            uc_index = fhss_calculate_uc_index(fhss_structure->bs->current_channel_index, fhss_structure->number_of_channels,
+                    fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels) + destination_offset;
+            if (uc_index >= (fhss_structure->number_of_channels - fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels)) {
+                uc_index -= (fhss_structure->number_of_channels - fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels);
             }
 
-            uc_index = fhss_calc_channel_shuffle(uc_index, fhss_structure->number_of_channels, fhss_structure->synch_configuration.fhss_number_of_bc_channels);
-            uc_index = fhss_add_channel_list_counter(uc_index, fhss_structure->number_of_channels, fhss_structure->channel_list_counter, fhss_structure->fhss_scramble_table);
-            return channel_list_get_channel(fhss_structure->fhss_configuration.channel_mask, uc_index);
+            uc_index = fhss_calc_channel_shuffle(uc_index, fhss_structure->number_of_channels, fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels);
+            uc_index = fhss_add_channel_list_counter(uc_index, fhss_structure->number_of_channels, fhss_structure->bs->channel_list_counter, fhss_structure->bs->fhss_scramble_table);
+            return channel_list_get_channel(fhss_structure->bs->fhss_configuration.channel_mask, uc_index);
         }
         return fhss_structure->rx_channel;
     }
@@ -251,7 +245,7 @@ int fhss_change_to_parent_channel(fhss_structure_t *fhss_structure)
     uint8_t destination_channel;
     uint8_t destination_offset;
     if (fhss_structure) {
-        if (fhss_structure->number_of_channels != fhss_structure->synch_configuration.fhss_number_of_bc_channels) {
+        if (fhss_structure->number_of_channels != fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels) {
             uint8_t parent_address[8];
             if (fhss_get_parent_address(fhss_structure, parent_address)) {
                 return -1;
@@ -259,14 +253,14 @@ int fhss_change_to_parent_channel(fhss_structure_t *fhss_structure)
 
             destination_offset = fhss_get_offset(fhss_structure, parent_address);
 
-            uc_index = fhss_calculate_uc_index(fhss_structure->current_channel_index, fhss_structure->number_of_channels,
-                    fhss_structure->synch_configuration.fhss_number_of_bc_channels) + destination_offset;
-            if (uc_index >= (fhss_structure->number_of_channels - fhss_structure->synch_configuration.fhss_number_of_bc_channels)) {
-                uc_index -= (fhss_structure->number_of_channels - fhss_structure->synch_configuration.fhss_number_of_bc_channels);
+            uc_index = fhss_calculate_uc_index(fhss_structure->bs->current_channel_index, fhss_structure->number_of_channels,
+                    fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels) + destination_offset;
+            if (uc_index >= (fhss_structure->number_of_channels - fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels)) {
+                uc_index -= (fhss_structure->number_of_channels - fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels);
             }
-            uc_index = fhss_calc_channel_shuffle(uc_index, fhss_structure->number_of_channels, fhss_structure->synch_configuration.fhss_number_of_bc_channels);
-            uc_index = fhss_add_channel_list_counter(uc_index, fhss_structure->number_of_channels, fhss_structure->channel_list_counter, fhss_structure->fhss_scramble_table);
-            destination_channel = channel_list_get_channel(fhss_structure->fhss_configuration.channel_mask, uc_index);
+            uc_index = fhss_calc_channel_shuffle(uc_index, fhss_structure->number_of_channels, fhss_structure->bs->synch_configuration.fhss_number_of_bc_channels);
+            uc_index = fhss_add_channel_list_counter(uc_index, fhss_structure->number_of_channels, fhss_structure->bs->channel_list_counter, fhss_structure->bs->fhss_scramble_table);
+            destination_channel = channel_list_get_channel(fhss_structure->bs->fhss_configuration.channel_mask, uc_index);
             fhss_structure->callbacks.change_channel(fhss_structure->fhss_api, destination_channel);
 #ifdef FHSS_CHANNEL_DEBUG
             tr_info("Parent channel: %u", destination_channel);
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_common.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_common.c
new file mode 100644
index 0000000000..592a0b557d
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_common.c
@@ -0,0 +1,293 @@
+/*
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "nsconfig.h"
+#include "ns_types.h"
+#include "ns_trace.h"
+#include "fhss_api.h"
+#include "fhss_config.h"
+#include "fhss.h"
+#include "fhss_common.h"
+#include "fhss_ws.h"
+#include "fhss_statistics.h"
+#include "fhss_channel.h"
+#include "channel_list.h"
+#include "nsdynmemLIB.h"
+#include "eventOS_event.h"
+#include "eventOS_callback_timer.h"
+#include <string.h>
+
+#define TRACE_GROUP "fhssc"
+
+static fhss_structure_t *fhss_struct = NULL;
+
+static void fhss_event_timer_cb(int8_t timer_id, uint16_t slots);
+static fhss_structure_t *fhss_get_object_with_timer_id(const int8_t timer_id);
+static void fhss_set_active_event(fhss_structure_t *fhss_structure, uint8_t event_type);
+static bool fhss_read_active_event(fhss_structure_t *fhss_structure, uint8_t event_type);
+
+
+fhss_structure_t *fhss_allocate_instance(fhss_api_t *fhss_api, const fhss_timer_t *fhss_timer)
+{
+    if (fhss_struct || !fhss_api || !fhss_timer) {
+        return NULL;
+    }
+    fhss_struct = ns_dyn_mem_alloc(sizeof(fhss_structure_t));
+    if (!fhss_struct) {
+        return NULL;
+    }
+    memset(fhss_struct, 0, sizeof(fhss_structure_t));
+    fhss_struct->fhss_api = fhss_api;
+    fhss_struct->platform_functions = *fhss_timer;
+    fhss_struct->fhss_event_timer = eventOS_callback_timer_register(fhss_event_timer_cb);
+    if (!fhss_struct->platform_functions.fhss_resolution_divider) {
+        fhss_struct->platform_functions.fhss_resolution_divider = 1;
+    }
+    return fhss_struct;
+}
+
+int8_t fhss_free_instance(fhss_api_t *fhss_api)
+{
+    if (!fhss_struct || fhss_struct->fhss_api != fhss_api) {
+        return -1;
+    }
+    ns_dyn_mem_free(fhss_struct);
+    fhss_struct = NULL;
+    return 0;
+}
+
+static void fhss_event_timer_cb(int8_t timer_id, uint16_t slots)
+{
+    (void) slots;
+    fhss_structure_t *fhss_structure = fhss_get_object_with_timer_id(timer_id);
+    if (fhss_structure) {
+        fhss_structure->callbacks.tx_poll(fhss_structure->fhss_api);
+    }
+}
+
+static fhss_structure_t *fhss_get_object_with_timer_id(const int8_t timer_id)
+{
+    if (timer_id < 0 || !fhss_struct) {
+        return NULL;
+    }
+    if (fhss_struct->fhss_event_timer == timer_id) {
+        return fhss_struct;
+    }
+    return NULL;
+}
+
+fhss_structure_t *fhss_get_object_with_api(const fhss_api_t *fhss_api)
+{
+    if (!fhss_api || !fhss_struct) {
+        return NULL;
+    }
+    if (fhss_struct->fhss_api == fhss_api) {
+        return fhss_struct;
+    }
+    return NULL;
+}
+
+static void fhss_set_active_event(fhss_structure_t *fhss_structure, uint8_t event_type)
+{
+    fhss_structure->active_fhss_events |= (1 << event_type);
+}
+
+void fhss_clear_active_event(fhss_structure_t *fhss_structure, uint8_t event_type)
+{
+    fhss_structure->active_fhss_events &= ~(1 << event_type);
+}
+
+static bool fhss_read_active_event(fhss_structure_t *fhss_structure, uint8_t event_type)
+{
+    if (fhss_structure->active_fhss_events & (1 << event_type)) {
+        return true;
+    }
+    return false;
+}
+
+int8_t fhss_disable(fhss_structure_t *fhss_structure)
+{
+    if (!fhss_structure) {
+        return -1;
+    }
+    fhss_structure->fhss_api->synch_state_set(fhss_structure->fhss_api, FHSS_UNSYNCHRONIZED, 0);
+    ns_dyn_mem_free(fhss_structure->bs);
+    ns_dyn_mem_free(fhss_structure->ws);
+    ns_dyn_mem_free(fhss_structure);
+    fhss_struct = 0;
+    return 0;
+}
+
+void fhss_start_timer(fhss_structure_t *fhss_structure, uint32_t time, void (*callback)(const fhss_api_t *fhss_api, uint16_t))
+{
+    if (callback){
+        // Don't allow starting with zero slots
+        if (time < fhss_structure->platform_functions.fhss_resolution_divider) {
+            time = fhss_structure->platform_functions.fhss_resolution_divider;
+        }
+        fhss_structure->platform_functions.fhss_timer_start(time / fhss_structure->platform_functions.fhss_resolution_divider, callback, fhss_structure->fhss_api);
+    }
+}
+
+int fhss_timeout_start(fhss_structure_t *fhss_structure, uint32_t time)
+{
+    if (!fhss_structure) {
+        return -1;
+    }
+    fhss_structure->fhss_timeout = time;
+    fhss_structure->fhss_timer = 0;
+    return 0;
+}
+
+int fhss_timeout_stop(fhss_structure_t *fhss_structure)
+{
+    if (!fhss_structure) {
+        return -1;
+    }
+    fhss_structure->fhss_timeout = 0;
+    fhss_structure->fhss_timer = 0;
+    return 0;
+}
+
+int fhss_update_synch_parent_address(fhss_structure_t *fhss_structure)
+{
+    uint8_t parent_address[8];
+
+    if (!fhss_get_parent_address(fhss_structure, parent_address)) {
+        memcpy(fhss_structure->synch_parent, parent_address, 8);
+        return 0;
+    }
+    return -1;
+}
+
+void fhss_trig_event(fhss_structure_t *fhss_structure, uint8_t event_type)
+{
+    if (!fhss_structure || fhss_read_active_event(fhss_structure, event_type) == true) {
+        return;
+    }
+    arm_event_s event;
+    event.receiver = fhss_structure->beacon_tasklet_id;
+    event.sender = 0;
+    event.event_type = event_type;
+    event.event_id = 0;
+    event.data_ptr = fhss_structure;
+    event.priority = ARM_LIB_HIGH_PRIORITY_EVENT;
+    event.event_data = 0;
+    if (eventOS_event_send(&event) != 0) {
+        tr_error("Event trigger failed: eventOS_event_send() failed");
+    } else {
+        fhss_set_active_event(fhss_structure, event_type);
+    }
+}
+
+int fhss_get_parent_address(fhss_structure_t *fhss_structure, uint8_t *p_addr)
+{
+    int ret_val = -1;
+    if (!fhss_structure || !p_addr) {
+        return -1;
+    }
+
+    ret_val = fhss_structure->callbacks.read_coord_mac_address(fhss_structure->fhss_api, p_addr);
+
+    if (ret_val) {
+        // Use default synchronization parent when RPL parent not found
+        memcpy(p_addr, fhss_structure->synch_parent, 8);
+        ret_val = 0;
+    }
+    return ret_val;
+}
+
+int fhss_compare_with_synch_parent_address(fhss_structure_t *fhss_structure, const uint8_t *source_addr)
+{
+    int ret_val = -1;
+    if (!fhss_structure || !source_addr) {
+        return ret_val;
+    }
+    uint8_t parent_address[8];
+
+    if (fhss_is_synch_root(fhss_structure) == false) {
+        if (!fhss_get_parent_address(fhss_structure, parent_address)) {
+            ret_val = memcmp(source_addr, parent_address, 8);
+        }
+    }
+    return ret_val;
+}
+
+uint32_t fhss_read_timestamp_cb(const fhss_api_t *api)
+{
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
+    if (!fhss_structure) {
+        return 0;
+    }
+    return (fhss_structure->platform_functions.fhss_get_timestamp(api) * fhss_structure->platform_functions.fhss_resolution_divider);
+}
+
+int fhss_init_callbacks_cb(const fhss_api_t *api, fhss_callback_t *callbacks)
+{
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
+    if (!fhss_structure || !callbacks) {
+        return -1;
+    }
+    fhss_structure->callbacks = *callbacks;
+    return 0;
+}
+
+
+fhss_failed_tx_t *fhss_failed_handle_find(fhss_structure_t *fhss_structure, uint8_t handle)
+{
+    ns_list_foreach(fhss_failed_tx_t, cur, &fhss_structure->fhss_failed_tx_list) {
+        if (cur->handle == handle) {
+            return cur;
+        }
+    }
+    return NULL;
+}
+
+int fhss_failed_handle_add(fhss_structure_t *fhss_structure, uint8_t handle, uint8_t bad_channel)
+{
+    fhss_failed_tx_t *failed_tx = ns_dyn_mem_alloc(sizeof(fhss_failed_tx_t));
+    if (!failed_tx) {
+        return -1;
+    }
+    failed_tx->bad_channel = bad_channel;
+    failed_tx->retries_done = 0;
+    failed_tx->handle = handle;
+    ns_list_add_to_end(&fhss_structure->fhss_failed_tx_list, failed_tx);
+    return 0;
+}
+
+int fhss_failed_handle_remove(fhss_structure_t *fhss_structure, uint8_t handle)
+{
+    fhss_failed_tx_t *failed_tx = fhss_failed_handle_find(fhss_structure, handle);
+    if (!failed_tx) {
+        return -1;
+    }
+    ns_list_remove(&fhss_structure->fhss_failed_tx_list, failed_tx);
+    ns_dyn_mem_free(failed_tx); // Free entry
+    return 0;
+}
+
+void fhss_failed_list_free(fhss_structure_t *fhss_structure)
+{
+    for (uint16_t i = 0; i<256; i++) {
+        fhss_failed_handle_remove(fhss_structure, i);
+    }
+}
+
+uint32_t fhss_get_tx_time(fhss_structure_t *fhss_structure, uint16_t bytes_to_send, uint8_t phy_header_length, uint8_t phy_tail_length)
+{
+    return ((1000000 / (fhss_structure->callbacks.read_datarate(fhss_structure->fhss_api) / 8)) * (bytes_to_send + phy_header_length + phy_tail_length));
+}
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_common.h b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_common.h
new file mode 100644
index 0000000000..144e089019
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_common.h
@@ -0,0 +1,77 @@
+/*
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef FHSS_COMMON_H_
+#define FHSS_COMMON_H_
+#include "ns_list.h"
+
+#define FHSS_TASKLET_INIT_EVENT             0
+#define FHSS_TIMER_EVENT                    1
+#define FHSS_COMPARE_SYNCH_PARENT           2
+#define FHSS_BROADCAST_CHANNEL              3
+#define FHSS_UPDATE_SYNCH_INFO_STORAGE      4
+
+struct fhss_failed_tx
+{
+    uint8_t handle;
+    uint8_t bad_channel;
+    uint8_t retries_done;
+    ns_list_link_t link;
+};
+typedef NS_LIST_HEAD(fhss_failed_tx_t, link) fhss_failed_tx_list_t;
+
+struct fhss_structure
+{
+    uint8_t own_hop;
+    uint8_t fhss_resolution_divider;
+    uint8_t rx_channel;
+    int8_t beacon_tasklet_id;
+    int8_t fhss_event_timer;
+    uint8_t active_fhss_events;
+    uint16_t number_of_channels;
+    fhss_states fhss_state;
+    uint32_t fhss_timeout;
+    uint32_t fhss_timer;
+    struct fhss_api *fhss_api;
+    struct fhss_bs *bs;
+    struct fhss_ws *ws;
+    struct fhss_timer platform_functions;
+    struct fhss_callback callbacks;
+    fhss_failed_tx_list_t fhss_failed_tx_list;
+    uint8_t synch_parent[8];
+};
+
+fhss_structure_t *fhss_allocate_instance(fhss_api_t *fhss_api, const fhss_timer_t *fhss_timer);
+int8_t fhss_free_instance(fhss_api_t *fhss_api);
+int8_t fhss_set_datarate(fhss_structure_t *fhss_structure, uint32_t datarate);
+fhss_structure_t *fhss_get_object_with_api(const fhss_api_t *fhss_api);
+void fhss_clear_active_event(fhss_structure_t *fhss_structure, uint8_t event_type);
+int8_t fhss_disable(fhss_structure_t *fhss_structure);
+void fhss_start_timer(fhss_structure_t *fhss_structure, uint32_t time, void (*callback)(const fhss_api_t *fhss_api, uint16_t));
+int fhss_timeout_start(fhss_structure_t *fhss_structure, uint32_t time);
+int fhss_timeout_stop(fhss_structure_t *fhss_structure);
+int fhss_update_synch_parent_address(fhss_structure_t *fhss_structure);
+void fhss_trig_event(fhss_structure_t *fhss_structure, uint8_t event_type);
+int fhss_get_parent_address(fhss_structure_t *fhss_structure, uint8_t *p_addr);
+int fhss_compare_with_synch_parent_address(fhss_structure_t *fhss_structure, const uint8_t *source_addr);
+uint32_t fhss_read_timestamp_cb(const fhss_api_t *api);
+int fhss_init_callbacks_cb(const fhss_api_t *api, fhss_callback_t *callbacks);
+int fhss_failed_handle_remove(fhss_structure_t *fhss_structure, uint8_t handle);
+int fhss_failed_handle_add(fhss_structure_t *fhss_structure, uint8_t handle, uint8_t bad_channel);
+fhss_failed_tx_t *fhss_failed_handle_find(fhss_structure_t *fhss_structure, uint8_t handle);
+void fhss_failed_list_free(fhss_structure_t *fhss_structure);
+uint32_t fhss_get_tx_time(fhss_structure_t *fhss_structure, uint16_t bytes_to_send, uint8_t phy_header_length, uint8_t phy_tail_length);
+#endif /*FHSS_COMMON_H_*/
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_configuration_interface.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_configuration_interface.c
index 0f6ada0100..0c7567e516 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_configuration_interface.c
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_configuration_interface.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -22,12 +22,12 @@
 #include "net_fhss.h"
 #include "nsdynmemLIB.h"
 #include "Service_Libs/fhss/fhss.h"
-#include "Service_Libs/fhss/fhss_mac_interface.h"
+#include "Service_Libs/fhss/fhss_common.h"
+#include "Service_Libs/fhss/fhss_ws.h"
 #include "ns_trace.h"
 
 #define TRACE_GROUP "fhss"
 
-
 fhss_api_t *ns_fhss_create(const fhss_configuration_t *fhss_configuration, const fhss_timer_t *fhss_timer, fhss_statistics_t *fhss_statistics)
 {
     fhss_api_t *this = ns_dyn_mem_alloc(sizeof(fhss_api_t));
@@ -35,26 +35,51 @@ fhss_api_t *ns_fhss_create(const fhss_configuration_t *fhss_configuration, const
         return NULL;
     }
     // Create FHSS object
-    int8_t retval = fhss_enable(this, fhss_configuration, fhss_timer, fhss_statistics);
-    if (retval) {
-        tr_err("Failed to enable FHSS, return code: %i", retval);
+    fhss_structure_t *fhss_struct = fhss_enable(this, fhss_configuration, fhss_timer, fhss_statistics);
+    if (!fhss_struct) {
+        tr_err("Failed to enable FHSS");
         ns_dyn_mem_free(this);
         return NULL;
     }
-    this->is_broadcast_channel = &fhss_is_broadcast_channel_cb;
-    this->use_broadcast_queue = &fhss_use_broadcast_queue_cb;
-    this->tx_handle = &fhss_tx_handle_cb;
-    this->check_tx_conditions = &fhss_check_tx_conditions_cb;
-    this->receive_frame = &fhss_receive_frame_cb;
-    this->data_tx_done = &fhss_data_tx_done_cb;
-    this->data_tx_fail = &fhss_data_tx_fail_cb;
-    this->synch_state_set = &fhss_synch_state_set_cb;
-    this->read_timestamp = &fhss_read_timestamp_cb;
-    this->get_retry_period = &fhss_get_retry_period_cb;
-    this->init_callbacks = &fhss_init_callbacks_cb;
+    fhss_set_callbacks(fhss_struct);
     return this;
 }
 
+fhss_api_t *ns_fhss_ws_create(const fhss_ws_configuration_t *fhss_configuration, const fhss_timer_t *fhss_timer)
+{
+    fhss_api_t *this = ns_dyn_mem_alloc(sizeof(fhss_api_t));
+    if (!this) {
+        return NULL;
+    }
+    // Create FHSS object
+    fhss_structure_t *fhss_struct = fhss_ws_enable(this, fhss_configuration, fhss_timer);
+    if (!fhss_struct) {
+        tr_err("Failed to enable FHSS");
+        ns_dyn_mem_free(this);
+        return NULL;
+    }
+    fhss_ws_set_callbacks(fhss_struct);
+    return this;
+}
+
+int ns_fhss_ws_set_parent(const fhss_api_t *fhss_api, const uint8_t eui64[8], const broadcast_timing_info_t *bc_timing_info)
+{
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(fhss_api);
+    if (!fhss_structure || !eui64 || !bc_timing_info) {
+        return -1;
+    }
+    return fhss_ws_set_parent(fhss_structure, eui64, bc_timing_info);
+}
+
+int ns_fhss_ws_remove_parent(const fhss_api_t *fhss_api, const uint8_t eui64[8])
+{
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(fhss_api);
+    if (!fhss_structure || !eui64) {
+        return -1;
+    }
+    return fhss_ws_remove_parent(fhss_structure, eui64);
+}
+
 int ns_fhss_delete(fhss_api_t *fhss_api)
 {
     fhss_structure_t *fhss_structure = fhss_get_object_with_api(fhss_api);
@@ -65,7 +90,6 @@ int ns_fhss_delete(fhss_api_t *fhss_api)
         return -1;
     }
     ns_dyn_mem_free(fhss_api);
-    fhss_api = NULL;
     return 0;
 }
 
@@ -77,3 +101,40 @@ int ns_fhss_configuration_set(fhss_api_t *fhss_api, const fhss_synch_configurati
     }
     return fhss_set_synch_configuration(fhss_structure, fhss_synch_configuration);
 }
+
+int ns_fhss_set_neighbor_info_fp(const fhss_api_t *fhss_api, fhss_get_neighbor_info *get_neighbor_info)
+{
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(fhss_api);
+    if (!fhss_structure || !fhss_structure->ws) {
+        return -1;
+    }
+    fhss_structure->ws->get_neighbor_info = get_neighbor_info;
+    return 0;
+}
+
+const fhss_ws_configuration_t *ns_fhss_ws_configuration_get(const fhss_api_t *fhss_api)
+{
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(fhss_api);
+    if (!fhss_structure || !fhss_structure->ws) {
+        return NULL;
+    }
+    return &fhss_structure->ws->fhss_configuration;
+}
+
+int ns_fhss_ws_configuration_set(const fhss_api_t *fhss_api, const fhss_ws_configuration_t *fhss_configuration)
+{
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(fhss_api);
+    if (!fhss_structure || !fhss_structure->ws) {
+        return -1;
+    }
+    return fhss_ws_configuration_set(fhss_structure, fhss_configuration);
+}
+
+int ns_fhss_ws_set_hop_count(const fhss_api_t *fhss_api, const uint8_t hop_count)
+{
+    fhss_structure_t *fhss_structure = fhss_get_object_with_api(fhss_api);
+    if (!fhss_structure || !fhss_structure->ws) {
+        return -1;
+    }
+    return fhss_ws_set_hop_count(fhss_structure, hop_count);
+}
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_mac_interface.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_mac_interface.c
deleted file mode 100644
index ae839f3eda..0000000000
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_mac_interface.c
+++ /dev/null
@@ -1,308 +0,0 @@
-/*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
- * SPDX-License-Identifier: Apache-2.0
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-#include "nsconfig.h"
-#include <string.h>
-#include "ns_types.h"
-#include "fhss_api.h"
-#include "fhss_config.h"
-#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"
-
-#define TRACE_GROUP "fhss"
-
-
-bool fhss_is_broadcast_channel_cb(const fhss_api_t *api)
-{
-    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
-    if (!fhss_structure) {
-        return false;
-    }
-    // FHSS is unsynchronized, broadcasts allowed
-    if (fhss_structure->fhss_state == FHSS_UNSYNCHRONIZED) {
-        return true;
-    }
-    return fhss_is_current_channel_broadcast(fhss_structure);
-}
-
-bool fhss_use_broadcast_queue_cb(const fhss_api_t *api, bool is_broadcast_addr, int frame_type)
-{
-    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
-    if (!fhss_structure) {
-        return false;
-    }
-    // Synch requests are always stored in unicast queue
-    if (frame_type == FHSS_SYNCH_REQUEST_FRAME) {
-        return false;
-    }
-    // Broadcast packets are stored in broadcast queue
-    return is_broadcast_addr;
-}
-
-int fhss_tx_handle_cb(const fhss_api_t *api, bool is_broadcast_addr, uint8_t *destination_address, int frame_type, uint8_t *synch_info, uint16_t frame_length, uint8_t phy_header_length, uint8_t phy_tail_length)
-{
-    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
-    if (!fhss_structure) {
-        return -2;
-    }
-    // TODO: needs some more logic to push buffer back to queue
-    if (frame_type == FHSS_DATA_FRAME) {
-        if (is_broadcast_addr == true) {
-            if (fhss_is_current_channel_broadcast(fhss_structure) == false) {
-                tr_info("Broadcast on UC channel -> Back to queue");
-                return -3;
-            }
-        }
-    }
-    if (frame_type == FHSS_SYNCH_FRAME) {
-        if (!synch_info) {
-            return -4;
-        }
-        fhss_beacon_build(fhss_structure, synch_info);
-    } else if (fhss_check_tx_allowed(fhss_structure, is_broadcast_addr, frame_length, frame_type, phy_header_length, phy_tail_length) == false) {
-        return -1;
-    }
-    // If sending Beacon request on parents Unicast channel
-    if (frame_type == FHSS_SYNCH_REQUEST_FRAME && fhss_structure->fhss_state == FHSS_SYNCHRONIZED) {
-        fhss_change_to_parent_channel(fhss_structure);
-    } else if (frame_type == FHSS_DATA_FRAME) {
-        fhss_change_to_tx_channel(fhss_structure, destination_address);
-    }
-    return 0;
-}
-
-bool fhss_check_tx_conditions_cb(const fhss_api_t *api, bool is_broadcast_addr, uint8_t handle, int frame_type, uint16_t frame_length, uint8_t phy_header_length, uint8_t phy_tail_length)
-{
-    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
-    if (!fhss_structure) {
-        return false;
-    }
-    // This condition will check that message is not sent on bad channel
-    if (fhss_check_bad_channel(fhss_structure, handle) == false) {
-        return false;
-    }
-
-    // This condition will check that broadcast messages are sent only broadcast channels
-    if (fhss_check_channel_type(fhss_structure, is_broadcast_addr, frame_type) == false) {
-        return false;
-    }
-
-    // This condition will check that FHSS is on TX slot and there is enough time to transmit before channel or slot change
-    if (fhss_check_tx_allowed(fhss_structure, is_broadcast_addr, frame_length, frame_type, phy_header_length, phy_tail_length) == false) {
-        return false;
-    }
-
-    return true;
-}
-
-void fhss_receive_frame_cb(const fhss_api_t *api, uint16_t pan_id, uint8_t *source_address, uint32_t timestamp, uint8_t *synch_info, int frame_type)
-{
-    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
-    if (!fhss_structure) {
-        return;
-    }
-    if (FHSS_SYNCH_FRAME == frame_type) {
-        if ((fhss_structure->fhss_state == FHSS_UNSYNCHRONIZED) || fhss_structure->synch_panid != pan_id) {
-            fhss_add_beacon_info(fhss_structure, pan_id, source_address, timestamp, synch_info);
-        } else {
-            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) {
-        // If current channel is broadcast, we don't need to send another synch info on next broadcast channel.
-        // Only send number of MAX_SYNCH_INFOS_PER_CHANNEL_LIST synch infos per one channel list cycle
-        if ((fhss_structure->fhss_state == FHSS_SYNCHRONIZED) && (fhss_is_current_channel_broadcast(fhss_structure) == false)
-                && (fhss_structure->synch_infos_sent_counter < MAX_SYNCH_INFOS_PER_CHANNEL_LIST)) {
-            fhss_structure->send_synch_info_on_next_broadcast_channel = true;
-        }
-    }
-}
-
-void fhss_data_tx_done_cb(const fhss_api_t *api, bool waiting_ack, bool tx_completed, uint8_t handle)
-{
-    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
-    if (!fhss_structure) {
-        return;
-    }
-    if (waiting_ack == false) {
-        fhss_change_to_rx_channel(fhss_structure);
-    }
-    // Buffer was successfully transmitted. Remove stored failure handle if exists.
-    if (tx_completed == true) {
-        fhss_failed_tx_t *fhss_failed_tx = fhss_failed_handle_find(fhss_structure, handle);
-        if (fhss_failed_tx) {
-            fhss_failed_handle_remove(fhss_structure, handle);
-        }
-    }
-}
-
-bool fhss_data_tx_fail_cb(const fhss_api_t *api, uint8_t handle, int frame_type)
-{
-    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
-    if (!fhss_structure) {
-        return false;
-    }
-    // Only use channel retries when device is synchronized
-    if (fhss_structure->fhss_state == FHSS_UNSYNCHRONIZED) {
-        return false;
-    }
-    // Channel retries are disabled -> return
-    if (fhss_structure->fhss_configuration.fhss_number_of_channel_retries == 0) {
-        return false;
-    }
-    // Use channel retries only for data frames
-    if (FHSS_DATA_FRAME != frame_type) {
-        return false;
-    }
-
-    fhss_failed_tx_t *fhss_failed_tx = fhss_failed_handle_find(fhss_structure, handle);
-    if (fhss_failed_tx) {
-        fhss_failed_tx->retries_done++;
-        if (fhss_failed_tx->retries_done >= fhss_structure->fhss_configuration.fhss_number_of_channel_retries) {
-            // No more retries. Return false to stop retransmitting.
-            fhss_failed_handle_remove(fhss_structure, handle);
-            return false;
-        }
-    } else {
-        // Create new failure handle and return true to retransmit
-        fhss_failed_handle_add(fhss_structure, handle);
-    }
-    return true;
-}
-
-void fhss_synch_state_set_cb(const fhss_api_t *api, fhss_states fhss_state, uint16_t pan_id)
-{
-    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
-    if (!fhss_structure) {
-        return;
-    }
-
-    // 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);
-        fhss_set_datarate(fhss_structure, datarate);
-        uint8_t mac_address[8];
-        fhss_structure->callbacks.read_mac_address(fhss_structure->fhss_api, mac_address);
-        fhss_structure->uc_channel_index = fhss_get_offset(fhss_structure, mac_address);
-        // Get Beacon info from storage
-        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;
-}
-
-uint32_t fhss_read_timestamp_cb(const fhss_api_t *api)
-{
-    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
-    if (!fhss_structure) {
-        return 0;
-    }
-    return (fhss_structure->platform_functions.fhss_get_timestamp(api) * fhss_structure->platform_functions.fhss_resolution_divider);
-}
-
-uint16_t fhss_get_retry_period_cb(const fhss_api_t *api, uint8_t *destination_address, uint16_t phy_mtu)
-{
-    uint16_t retry_period = 0;
-    uint16_t random_number = randLIB_get_16bit();
-    uint16_t rnd_mask;
-
-    /* Generate retry back-off period. FHSS is using the known synchronization parent info to delay retransmissions upstream.
-     *
-     */
-    if (phy_mtu < 128) {
-        // Max. random when PHY MTU below 128 is 6.4ms
-        rnd_mask = 0x7f;
-    } else if (phy_mtu < 256) {
-        // Max. random when PHY MTU below 256 is 12.8ms
-        rnd_mask = 0xff;
-    } else {
-        // Max. random when PHY MTU above 255 is 25.6ms
-        rnd_mask = 0x1ff;
-    }
-
-    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
-    if (fhss_structure) {
-        uint32_t datarate = fhss_structure->datarate;
-        uint16_t max_tx_length;
-
-        if (datarate && phy_mtu) {
-            if (fhss_compare_with_synch_parent_address(fhss_structure, destination_address) == 0) {
-                // E.g. (1000000 / (250000bit/s / 8 bits)) * 255 bytes = 8160us
-                max_tx_length = ((1000000 / (datarate / 8)) * phy_mtu);
-                /* Retrying upstream: delay the transmission until assumed hidden node has retried downstream:
-                 * Static period: max random + max tx length
-                 * 50 comes from MAC timer resolution (50us)
-                 */
-                retry_period = (rnd_mask + (max_tx_length / 50));
-            }
-        }
-    }
-
-    // Add 1 to not to ever return zero value.
-    retry_period += ((random_number & rnd_mask) + 1);
-    return retry_period;
-}
-
-int fhss_init_callbacks_cb(const fhss_api_t *api, fhss_callback_t *callbacks)
-{
-    fhss_structure_t *fhss_structure = fhss_get_object_with_api(api);
-    if (!fhss_structure || !callbacks) {
-        return -1;
-    }
-    fhss_structure->callbacks = *callbacks;
-    return 0;
-}
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_mac_interface.h b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_mac_interface.h
deleted file mode 100644
index 22f63fcded..0000000000
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_mac_interface.h
+++ /dev/null
@@ -1,33 +0,0 @@
-/*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
- * SPDX-License-Identifier: Apache-2.0
- *
- * Licensed under the Apache License, Version 2.0 (the "License");
- * you may not use this file except in compliance with the License.
- * You may obtain a copy of the License at
- *
- *     http://www.apache.org/licenses/LICENSE-2.0
- *
- * Unless required by applicable law or agreed to in writing, software
- * distributed under the License is distributed on an "AS IS" BASIS,
- * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
- * See the License for the specific language governing permissions and
- * limitations under the License.
- */
-
-#ifndef FHSS_MAC_INTERFACE_H_
-#define FHSS_MAC_INTERFACE_H_
-
-bool fhss_is_broadcast_channel_cb(const fhss_api_t *api);
-bool fhss_use_broadcast_queue_cb(const fhss_api_t *api, bool is_broadcast_addr, int frame_type);
-int fhss_tx_handle_cb(const fhss_api_t *api, bool is_broadcast_addr, uint8_t *destination_address, int frame_type, uint8_t *synch_info, uint16_t frame_length, uint8_t phy_header_length, uint8_t phy_tail_length);
-bool fhss_check_tx_conditions_cb(const fhss_api_t *api, bool is_broadcast_addr, uint8_t handle, int frame_type, uint16_t frame_length, uint8_t phy_header_length, uint8_t phy_tail_length);
-void fhss_receive_frame_cb(const fhss_api_t *api, uint16_t pan_id, uint8_t *source_address, uint32_t timestamp, uint8_t *synch_info, int frame_type);
-void fhss_data_tx_done_cb(const fhss_api_t *api, bool waiting_ack, bool tx_completed, uint8_t handle);
-bool fhss_data_tx_fail_cb(const fhss_api_t *api, uint8_t handle, int frame_type);
-void fhss_synch_state_set_cb(const fhss_api_t *api, fhss_states fhss_state, uint16_t pan_id);
-uint32_t fhss_read_timestamp_cb(const fhss_api_t *api);
-uint16_t fhss_get_retry_period_cb(const fhss_api_t *api, uint8_t *destination_address, uint16_t phy_mtu);
-int fhss_init_callbacks_cb(const fhss_api_t *api, fhss_callback_t *callbacks);
-
-#endif /* FHSS_MAC_INTERFACE_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_statistics.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_statistics.c
index 8722982376..2ab1702ed5 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_statistics.c
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_statistics.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017, Arm Limited and affiliates.
+ * Copyright (c) 2017-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -19,26 +19,27 @@
 #include "fhss_api.h"
 #include "fhss_config.h"
 #include "fhss.h"
+#include "fhss_common.h"
 #include "fhss_statistics.h"
 
 void fhss_stats_update(fhss_structure_t *fhss_structure, fhss_stats_type_t type, uint32_t update_val)
 {
-    if (fhss_structure->fhss_stats_ptr) {
+    if (fhss_structure->bs->fhss_stats_ptr) {
         switch (type) {
             case STATS_FHSS_DRIFT_COMP:
-                fhss_structure->fhss_stats_ptr->fhss_drift_compensation = update_val;
+                fhss_structure->bs->fhss_stats_ptr->fhss_drift_compensation = update_val;
                 break;
             case STATS_FHSS_HOP_COUNT:
-                fhss_structure->fhss_stats_ptr->fhss_hop_count = update_val;
+                fhss_structure->bs->fhss_stats_ptr->fhss_hop_count = update_val;
                 break;
             case STATS_FHSS_SYNCH_INTERVAL:
-                fhss_structure->fhss_stats_ptr->fhss_synch_interval = update_val;
+                fhss_structure->bs->fhss_stats_ptr->fhss_synch_interval = update_val;
                 break;
             case STATS_FHSS_AVG_SYNCH_FIX:
-                fhss_structure->fhss_stats_ptr->fhss_prev_avg_synch_fix = update_val;
+                fhss_structure->bs->fhss_stats_ptr->fhss_prev_avg_synch_fix = update_val;
                 break;
             case STATS_FHSS_SYNCH_LOST:
-                fhss_structure->fhss_stats_ptr->fhss_synch_lost += update_val;
+                fhss_structure->bs->fhss_stats_ptr->fhss_synch_lost += update_val;
                 break;
         }
     }
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws.h b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws.h
new file mode 100644
index 0000000000..03befff9f8
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws.h
@@ -0,0 +1,46 @@
+/*
+ * Copyright (c) 2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef FHSS_WS_H_
+#define FHSS_WS_H_
+
+/* WS requires at least 19 MAC retransmissions (total 1+19=20 attempts). 802.15.4 macMaxFrameRetries is 3 (total 1+3=4 attempts).
+ * At least 4 channel retries must be used: (Initial channel + WS_NUMBER_OF_CHANNEL_RETRIES) * MAC attempts = (1+4)*4=20 attempts
+ */
+#define WS_NUMBER_OF_CHANNEL_RETRIES    4
+//TX/RX slot length in milliseconds
+#define WS_MAX_TXRX_SLOT_LEN_MS         100
+typedef struct fhss_ws fhss_ws_t;
+
+struct fhss_ws
+{
+    uint8_t bc_channel;
+    uint16_t uc_slot;
+    uint16_t bc_slot;
+    bool is_on_bc_channel;
+    struct fhss_ws_configuration fhss_configuration;
+    const struct broadcast_timing_info *parent_bc_info;
+    fhss_get_neighbor_info *get_neighbor_info;
+};
+
+fhss_structure_t *fhss_ws_enable(fhss_api_t *fhss_api, const fhss_ws_configuration_t *fhss_configuration, const fhss_timer_t *fhss_timer);
+int fhss_ws_set_callbacks(fhss_structure_t *fhss_structure);
+int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8], const broadcast_timing_info_t *bc_timing_info);
+int fhss_ws_remove_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8]);
+int fhss_ws_configuration_set(fhss_structure_t *fhss_structure, const fhss_ws_configuration_t *fhss_configuration);
+int fhss_ws_set_hop_count(fhss_structure_t *fhss_structure, const uint8_t hop_count);
+
+#endif /*FHSS_WS_H_*/
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws_empty_functions.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws_empty_functions.c
new file mode 100644
index 0000000000..df6bc4813c
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/fhss/fhss_ws_empty_functions.c
@@ -0,0 +1,83 @@
+/*
+ * Copyright (c) 2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include "nsconfig.h"
+#include "ns_types.h"
+#include "fhss_api.h"
+#include "fhss_config.h"
+#include "fhss.h"
+#include "fhss_common.h"
+#include "channel_list.h"
+#include "channel_functions.h"
+#include "fhss_ws.h"
+#include "nsdynmemLIB.h"
+#include "common_functions.h"
+#include "eventOS_callback_timer.h"
+#include "randLIB.h"
+#include <string.h>
+
+#ifndef HAVE_WS
+
+fhss_structure_t *fhss_ws_enable(fhss_api_t *fhss_api, const fhss_ws_configuration_t *fhss_configuration, const fhss_timer_t *fhss_timer)
+{
+    (void) fhss_api;
+    (void) fhss_configuration;
+    (void) fhss_timer;
+
+    return NULL;
+}
+
+int fhss_ws_set_callbacks(fhss_structure_t *fhss_structure)
+{
+    (void) fhss_structure;
+    return -1;
+}
+
+int fhss_ws_set_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8], const broadcast_timing_info_t *bc_timing_info)
+{
+    (void) fhss_structure;
+    (void) eui64;
+    (void) bc_timing_info;
+
+    return -1;
+}
+
+int fhss_ws_remove_parent(fhss_structure_t *fhss_structure, const uint8_t eui64[8])
+{
+    (void) fhss_structure;
+    (void) eui64;
+
+    return -1;
+}
+
+int fhss_ws_configuration_set(fhss_structure_t *fhss_structure, const fhss_ws_configuration_t *fhss_configuration)
+{
+    (void) fhss_structure;
+    (void) fhss_configuration;
+
+    return -1;
+}
+
+int fhss_ws_set_hop_count(fhss_structure_t *fhss_structure, const uint8_t hop_count)
+{
+    (void) fhss_structure;
+    (void) hop_count;
+
+    return -1;
+}
+
+#endif // HAVE_WS
+
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/mac_neighbor_table/mac_neighbor_table.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mac_neighbor_table/mac_neighbor_table.c
new file mode 100644
index 0000000000..6b1e548212
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mac_neighbor_table/mac_neighbor_table.c
@@ -0,0 +1,274 @@
+/*
+ * Copyright (c) 2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+
+#include "nsconfig.h"
+#include "string.h"
+#include "ns_types.h"
+#include "ns_trace.h"
+#include "common_functions.h"
+#include "nsdynmemLIB.h"
+#include "Service_Libs/mac_neighbor_table/mac_neighbor_table.h"
+#include "Core/include/address.h"
+#include "platform/topo_trace.h"
+
+#define TRACE_GROUP "mnei"
+
+mac_neighbor_table_t *mac_neighbor_table_create(uint8_t table_size, neighbor_entry_remove_notify *remove_cb, neighbor_entry_nud_notify *nud_cb, void *user_indentifier)
+{
+    mac_neighbor_table_t *table_class = ns_dyn_mem_alloc(sizeof(mac_neighbor_table_t) + sizeof(mac_neighbor_table_entry_t) * table_size);
+    if (!table_class) {
+        return NULL;
+    }
+    memset(table_class, 0, sizeof(mac_neighbor_table_t));
+
+    mac_neighbor_table_entry_t *cur_ptr = &table_class->neighbor_entry_buffer[0];
+    table_class->list_total_size = table_size;
+    table_class->table_user_identifier = user_indentifier;
+    table_class->user_nud_notify_cb = nud_cb;
+    table_class->user_remove_notify_cb = remove_cb;
+    ns_list_init(&table_class->neighbour_list);
+    ns_list_init(&table_class->free_list);
+    for (uint8_t i = 0; i< table_size; i++) {
+        memset(cur_ptr, 0, sizeof(mac_neighbor_table_entry_t));
+        cur_ptr->index = i;
+        //Add to list
+        ns_list_add_to_end(&table_class->free_list,cur_ptr);
+        cur_ptr++;
+    }
+
+    return table_class;
+
+}
+
+void mac_neighbor_table_delete(mac_neighbor_table_t *table_class)
+{
+    mac_neighbor_table_neighbor_list_clean(table_class);
+    ns_dyn_mem_free(table_class);
+}
+
+static void neighbor_table_class_remove_entry(mac_neighbor_table_t *table_class, mac_neighbor_table_entry_t *entry)
+{
+    ns_list_remove(&table_class->neighbour_list, entry);
+    table_class->neighbour_list_size--;
+    if (entry->nud_active) {
+        entry->nud_active = false;
+        table_class->active_nud_process--;
+    }
+
+    if (table_class->user_remove_notify_cb) {
+        table_class->user_remove_notify_cb(entry, table_class->table_user_identifier);
+    }
+    topo_trace(TOPOLOGY_MLE, entry->mac64, TOPO_REMOVE);
+
+
+    uint8_t index = entry->index;
+    memset(entry, 0, sizeof(mac_neighbor_table_entry_t));
+    entry->index = index;
+    ns_list_add_to_end(&table_class->free_list,entry);
+}
+
+void mac_neighbor_table_neighbor_list_clean(mac_neighbor_table_t *table_class)
+{
+    if (!table_class) {
+        return;
+    }
+    ns_list_foreach_safe(mac_neighbor_table_entry_t, cur, &table_class->neighbour_list) {
+        neighbor_table_class_remove_entry(table_class, cur);
+    }
+    topo_trace(TOPOLOGY_MLE, NULL, TOPO_CLEAR);
+}
+
+
+void mac_neighbor_table_neighbor_timeout_update(mac_neighbor_table_t *table_class, uint32_t time_update)
+{
+    if (!table_class) {
+        return;
+    }
+
+    ns_list_foreach_safe(mac_neighbor_table_entry_t, cur, &table_class->neighbour_list) {
+
+        if (cur->lifetime > time_update) {
+            cur->lifetime -= time_update;
+            if (!table_class->user_nud_notify_cb ||  table_class->active_nud_process > ACTIVE_NUD_PROCESS_MAX || cur->nud_active || !cur->rx_on_idle) {
+                continue;
+            }
+
+            if (table_class->user_nud_notify_cb(cur, table_class->table_user_identifier)) {
+                table_class->active_nud_process++;
+                cur->nud_active = true;
+                tr_debug("Nud started index %u : %"PRIu32" time ", cur->index, cur->lifetime);
+            }
+
+        } else {
+            tr_debug("Node index %u time out ", cur->index);
+            neighbor_table_class_remove_entry(table_class, cur);
+        }
+    }
+}
+
+
+mac_neighbor_table_entry_t *mac_neighbor_table_entry_allocate(mac_neighbor_table_t *table_class, const uint8_t *mac64)
+{
+    if (!table_class) {
+        return NULL;
+    }
+    mac_neighbor_table_entry_t *entry = ns_list_get_first(&table_class->free_list);
+    if (!entry) {
+        return NULL;
+    }
+    //Remove from the list
+    ns_list_remove(&table_class->free_list, entry);
+    //Add to list
+    ns_list_add_to_end(&table_class->neighbour_list,entry);
+    table_class->neighbour_list_size++;
+    memcpy(entry->mac64, mac64, 8);
+    entry->mac16 = 0xffff;
+    entry->rx_on_idle = true;
+    entry->ffd_device = true;
+    entry->nud_active = false;
+    entry->advertisment = false;
+    entry->connected_device = false;
+    entry->trusted_device = false;
+    entry->lifetime = NEIGHBOR_CLASS_LINK_DEFAULT_LIFETIME;
+    entry->link_lifetime = NEIGHBOR_CLASS_LINK_DEFAULT_LIFETIME;
+    entry->link_role = NORMAL_NEIGHBOUR;
+    topo_trace(TOPOLOGY_MLE, mac64, TOPO_ADD);
+    return entry;
+}
+
+static mac_neighbor_table_entry_t *neighbor_table_class_entry_validate(mac_neighbor_table_t *table_class, mac_neighbor_table_entry_t *neighbor_entry)
+{
+    ns_list_foreach(mac_neighbor_table_entry_t, cur, &table_class->neighbour_list) {
+        if (cur == neighbor_entry) {
+            return cur;
+        }
+    }
+    return NULL;
+
+}
+
+void mac_neighbor_table_neighbor_remove(mac_neighbor_table_t *table_class, mac_neighbor_table_entry_t *neighbor_entry)
+{
+    mac_neighbor_table_entry_t *entry = neighbor_table_class_entry_validate(table_class, neighbor_entry);
+    if (entry) {
+        neighbor_table_class_remove_entry(table_class, entry);
+    }
+}
+
+
+void mac_neighbor_table_neighbor_refresh(mac_neighbor_table_t *table_class, mac_neighbor_table_entry_t *neighbor_entry, uint32_t life_time)
+{
+    neighbor_entry->lifetime = life_time;
+    neighbor_entry->link_lifetime = life_time;
+    if (neighbor_entry->nud_active) {
+        tr_debug("Node index NUD response %u : %"PRIu32" time ", neighbor_entry->index, neighbor_entry->lifetime);
+        neighbor_entry->nud_active = false;
+        table_class->active_nud_process--;
+    }
+
+}
+
+void mac_neighbor_table_neighbor_connected(mac_neighbor_table_t *table_class, mac_neighbor_table_entry_t *neighbor_entry)
+{
+    (void)table_class;
+    neighbor_entry->connected_device = true;
+}
+
+void mac_neighbor_table_trusted_neighbor(mac_neighbor_table_t *table_class, mac_neighbor_table_entry_t *neighbor_entry, bool trusted_device)
+{
+    (void)table_class;
+    neighbor_entry->trusted_device = trusted_device;
+}
+
+mac_neighbor_table_entry_t *mac_neighbor_table_address_discover(mac_neighbor_table_t *table_class, const uint8_t *address, uint8_t address_type)
+{
+    if (!table_class) {
+        return NULL;
+    }
+    uint16_t short_address;
+    if (address_type == ADDR_802_15_4_SHORT) {
+        short_address = common_read_16_bit(address);
+    } else if (address_type == ADDR_802_15_4_LONG) {
+
+    } else {
+        return NULL;
+    }
+
+    ns_list_foreach(mac_neighbor_table_entry_t, cur, &table_class->neighbour_list) {
+        if (address_type == ADDR_802_15_4_SHORT) {
+            if (cur->mac16 != 0xffff && cur->mac16 == short_address) {
+                return cur;
+            }
+        } else {
+            if (memcmp(cur->mac64, address, 8) == 0) {
+                return cur;
+            }
+        }
+    }
+
+    return NULL;
+}
+
+mac_neighbor_table_entry_t *mac_neighbor_table_attribute_discover(mac_neighbor_table_t *table_class, uint8_t index)
+{
+    ns_list_foreach(mac_neighbor_table_entry_t, cur, &table_class->neighbour_list) {
+
+        if (cur->index == index) {
+            return cur;
+        }
+    }
+    return NULL;
+}
+
+mac_neighbor_table_entry_t *mac_neighbor_entry_get_by_ll64(mac_neighbor_table_t *table_class, const uint8_t *ipv6Address, bool allocateNew, bool *new_entry_allocated)
+{
+    // Check it really is LL64 (not LL16)
+    if (memcmp(ipv6Address, ADDR_LINK_LOCAL_PREFIX , 8) != 0) {
+        return NULL;    //Mot Link Local Address
+    }
+
+    if (memcmp((ipv6Address + 8), ADDR_SHORT_ADR_SUFFIC , 6) == 0) {
+        return NULL;
+    }
+    // map
+    uint8_t temporary_mac64[8];
+    memcpy(temporary_mac64, (ipv6Address + 8), 8);
+    temporary_mac64[0] ^= 2;
+
+    return mac_neighbor_entry_get_by_mac64(table_class, temporary_mac64, allocateNew, new_entry_allocated);
+
+}
+
+mac_neighbor_table_entry_t *mac_neighbor_entry_get_by_mac64(mac_neighbor_table_t *table_class, const uint8_t *mac64, bool allocateNew, bool *new_entry_allocated)
+{
+    mac_neighbor_table_entry_t *entry = mac_neighbor_table_address_discover(table_class, mac64, ADDR_802_15_4_LONG);
+    if (entry || !allocateNew) {
+        if (new_entry_allocated) {
+            *new_entry_allocated = false;
+        }
+        return entry;
+    }
+
+    if (new_entry_allocated) {
+        *new_entry_allocated = true;
+    }
+
+    return mac_neighbor_table_entry_allocate(table_class, mac64);
+}
+
+
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/mac_neighbor_table/mac_neighbor_table.h b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mac_neighbor_table/mac_neighbor_table.h
new file mode 100644
index 0000000000..edd10a974d
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mac_neighbor_table/mac_neighbor_table.h
@@ -0,0 +1,205 @@
+/*
+ * Copyright (c) 2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef MAC_NEIGHBOR_TABLE_H_
+#define MAC_NEIGHBOR_TABLE_H_
+
+#include "ns_types.h"
+#include "ns_list.h"
+
+#define NEIGHBOR_CLASS_LINK_DEFAULT_LIFETIME 240
+
+#define ACTIVE_NUD_PROCESS_MAX 3 //Limit That how many activate NUD process is active in same time
+
+#define NORMAL_NEIGHBOUR                0
+#define SECONDARY_PARENT_NEIGHBOUR      1
+#define CHILD_NEIGHBOUR                 2
+#define PRIORITY_PARENT_NEIGHBOUR       3
+/**
+ * Generic Neighbor table entry
+ */
+typedef struct mac_neighbor_table_entry {
+    uint8_t         index;                  /*!< Unique Neighbour index */
+    uint8_t         mac64[8];               /*!< MAC64 */
+    uint16_t        mac16;                  /*!< MAC16 address for neighbor 0xffff when no 16-bit address is unknown */
+    uint32_t        lifetime;               /*!< Life time in seconds which goes down */
+    uint32_t        link_lifetime;          /*!< Configured link timeout*/
+    bool            rx_on_idle:1;           /*!< True, RX on idle allways at idle state, false disable radio */
+    bool            ffd_device:1;           /*!< True FFD device, false for RFD */
+    bool            advertisment:1;
+    bool            connected_device:1;     /*!< True Link is connected and data rx is accepted , False RX data is not accepted*/
+    bool            trusted_device:1;       /*!< True mean use normal group key, false for enable pairwise key */
+    bool            nud_active:1;           /*!< True Neighbor NUD process is active, false not active process */
+    unsigned        link_role:2;            /*!< Link role: NORMAL_NEIGHBOUR, PRIORITY_PARENT_NEIGHBOUR, SECONDARY_PARENT_NEIGHBOUR, CHILD_NEIGHBOUR */
+    ns_list_link_t  link;
+} mac_neighbor_table_entry_t;
+
+typedef NS_LIST_HEAD(mac_neighbor_table_entry_t, link) mac_neighbor_table_list_t;
+
+#define mac_neighbor_info(interface) ((interface)->mac_parameters->mac_neighbor_table) /*!< Helper macro for give mac neighbor class pointer from interface pointer. */
+
+/**
+ * Remove entry notify
+ *
+ * \param entry_ptr Pointer to removed entry
+ * \param user_data pointer for user to detect interface
+ */
+typedef void neighbor_entry_remove_notify(mac_neighbor_table_entry_t *entry_ptr, void *user_data);
+
+
+/**
+ * NUD entry notify
+ *
+ * \param entry_ptr Pointer to neighbor entry
+ * \param user_data pointer for user to detect interface
+ *
+ * \return true NUD message generated
+ * \return false When NUD is not generated
+ */
+typedef bool neighbor_entry_nud_notify(mac_neighbor_table_entry_t *entry_ptr, void *user_data);
+
+/**
+ * Neighbor table class structure
+ */
+typedef struct mac_neighbor_table_class {
+    mac_neighbor_table_list_t neighbour_list;               /*!< List of active neighbors */
+    mac_neighbor_table_list_t free_list;                    /*!< List of free neighbors entries */
+    uint32_t nud_threshold;                                 /*!< NUD threshold time which generates keep alive message */
+    uint8_t list_total_size;                                /*!< Total number allocated neighbor entries */
+    uint8_t active_nud_process;                             /*!< Indicate Active NUD Process */
+    uint8_t neighbour_list_size;                            /*!< Active Neighbor list size */
+    void *table_user_identifier;                            /*!< Table user identifier like interface pointer */
+    neighbor_entry_remove_notify *user_remove_notify_cb;    /*!< Neighbor Remove Callback notify */
+    neighbor_entry_nud_notify *user_nud_notify_cb;          /*!< Trig NUD process for neighbor */
+    mac_neighbor_table_entry_t neighbor_entry_buffer[];     /*!< Pointer for allocated neighbor table entries*/
+} mac_neighbor_table_t;
+
+
+/**
+ * \brief mac_neighbor_table_create Allocate Neighbour table class
+ *
+ * Call this only one's for interface
+ *
+ * \param table_size size of neighbor table
+ * \param remove_cb callback pointer for notify removed neighbor
+ * \param nud_cb Interface NUD operation trgger callback
+ * \param user_indentifier user identifier pointer like interface pointer
+ *
+ * \return pointer to neighbor table class when create is OK
+ * \return NULL when memory allocation happen
+ *
+ */
+mac_neighbor_table_t *mac_neighbor_table_create(uint8_t table_size, neighbor_entry_remove_notify *remove_cb, neighbor_entry_nud_notify *nud_cb, void *user_indentifier);
+
+/**
+ * mac_neighbor_table_delete Delete Neigbor table class
+ *
+ * \param table_class neighbor table class
+ */
+void mac_neighbor_table_delete(mac_neighbor_table_t *table_class);
+
+/**
+ * mac_neighbor_table_neighbor_list_clean Clean neighbour_list from giving class
+ */
+void mac_neighbor_table_neighbor_list_clean(mac_neighbor_table_t *table_class);
+
+/**
+ * mac_neighbor_table_neighbor_timeout_update Update Neighbor table timeout values
+ *
+ * \param table_class pointer to table class
+ * \param time_update in seconds
+ *
+ */
+void mac_neighbor_table_neighbor_timeout_update(mac_neighbor_table_t *table_class, uint32_t time_update);
+
+
+/**
+ * mac_neighbor_table_entry_allocate Allocate Neighbour table class entry
+ *
+ * \param table_class pointer to table class
+ * \param mac64 neighbor 64-bit mac address
+ *
+ * \return NULL allocate fail
+ * \return pointer to allocated neighbor table entry
+ */
+mac_neighbor_table_entry_t *mac_neighbor_table_entry_allocate(mac_neighbor_table_t *table_class, const uint8_t *mac64);
+
+/**
+ * mac_neighbor_table_neighbor_remove Remove neighbor from list
+ *
+ * \param table_class pointer to table class
+ * \param neighbor_entry pointer to removed entry
+ *
+ */
+void mac_neighbor_table_neighbor_remove(mac_neighbor_table_t *table_class, mac_neighbor_table_entry_t *neighbor_entry);
+
+/**
+ * mac_neighbor_table_neighbor_refresh Refresh neigbor timeout and time to live values based on giving time
+ *
+ * \param table_class pointer to table class
+ * \param neighbor_entry pointer to refreshed entry
+ * \param life_time define lifetime for neighbor
+ */
+void mac_neighbor_table_neighbor_refresh(mac_neighbor_table_t *table_class, mac_neighbor_table_entry_t *neighbor_entry, uint32_t life_time);
+
+/**
+ * mac_neighbor_table_neighbor_connected Mark neighbour connected state and data is accepted from device
+ *
+ * Call this function when node is trusted connected
+ *
+ * \param table_class pointer to table class
+ * \param neighbor_entry pointer to refreshed entry
+ */
+void mac_neighbor_table_neighbor_connected(mac_neighbor_table_t *table_class, mac_neighbor_table_entry_t *neighbor_entry);
+
+/**
+ * mac_neighbor_table_trusted_neighbor Function for manage neighbor role at mesh network
+ *
+ * Call this function when node is trusted connected
+ *
+ * \param table_class pointer to table class
+ * \param neighbor_entry pointer to refreshed entry
+ * \param trusted_device True neigbor is part of mesh and will use group key , false enable pairwose key
+ */
+void mac_neighbor_table_trusted_neighbor(mac_neighbor_table_t *table_class, mac_neighbor_table_entry_t *neighbor_entry, bool trusted_device);
+
+/**
+ * mac_neighbor_table_address_discover Discover neighbor from list by address
+ *
+ *  \param table_class pointer to table class
+ *  \param address pointer to 16-bit MAC or 64-bit address for discover
+ *  \param address_type 2 for 16-bit address and 3 for 64-bit (same than 802.15.4 define)
+ *
+ *  \return pointer to discover neighbor entry if it exist
+ */
+mac_neighbor_table_entry_t *mac_neighbor_table_address_discover(mac_neighbor_table_t *table_class, const uint8_t *address, uint8_t address_type);
+
+/**
+ * mac_neighbor_table_attribute_discover Discover neighbor from list by attribute index
+ *
+ *  \param table_class pointer to table class
+ *  \param index neighbor index
+ *
+ *  \return pointer to discover neighbor entry if it exist
+ */
+mac_neighbor_table_entry_t *mac_neighbor_table_attribute_discover(mac_neighbor_table_t *table_class, uint8_t index);
+
+mac_neighbor_table_entry_t *mac_neighbor_entry_get_by_ll64(mac_neighbor_table_t *table_class, const uint8_t *ipv6Address, bool allocateNew, bool *new_entry_allocated);
+
+mac_neighbor_table_entry_t *mac_neighbor_entry_get_by_mac64(mac_neighbor_table_t *table_class, const uint8_t *mac64, bool allocateNew, bool *new_entry_allocated);
+
+#endif /* MAC_NEIGHBOR_TABLE_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/mdns/ns_fnet_port.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mdns/ns_fnet_port.c
index e2cce7555c..63e904b191 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/mdns/ns_fnet_port.c
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mdns/ns_fnet_port.c
@@ -131,7 +131,7 @@ fnet_socket_t fnet_socket(fnet_address_family_t family, fnet_socket_type_t type,
 fnet_return_t fnet_socket_bind( fnet_socket_t s, const struct sockaddr *name, fnet_size_t namelen )
 {
     (void)namelen;
-    ns_address_t ns_source_addr = {0};
+    ns_address_t ns_source_addr;
     int8_t socket_id = (int8_t)(long)s;
     fnet_return_t fnet_ret_val = FNET_ERR;
     const struct sockaddr_in6 *namein6 = (const struct sockaddr_in6 *) name;
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service.c
index 45c5c081d3..459cd283ab 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service.c
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -35,6 +35,7 @@
 #include "Service_Libs/mle_service/mle_service_security.h"
 #include "Service_Libs/mle_service/mle_service_buffer.h"
 #include "Service_Libs/mle_service/mle_service_interface.h"
+#include "Service_Libs/mle_service/mle_service_frame_counter_table.h"
 #include "MLE/mle.h"
 #include "MLE/mle_tlv.h"
 #include "mac_common_defines.h"
@@ -413,39 +414,39 @@ static void mle_service_a_data_set(uint8_t *ptr, uint8_t *src_address, uint8_t *
 static buffer_t *mle_service_message_security_decode(buffer_t *buf, mle_security_header_t *securityHeader,
         uint8_t *security_key)
 {
-    ccm_globals_t *ccm_ptr;
+    ccm_globals_t ccm_ptr;
     uint16_t payload_len = buffer_data_length(buf);
 
-    ccm_ptr = ccm_sec_init(securityHeader->securityLevel, security_key, AES_CCM_DECRYPT, 2);
-    if (!ccm_ptr) {
+    if (!ccm_sec_init(&ccm_ptr, securityHeader->securityLevel, security_key, AES_CCM_DECRYPT, 2)) {
         return buffer_free(buf);
-    } else if (ccm_ptr->mic_len >= payload_len) {
+    } else if (ccm_ptr.mic_len >= payload_len) {
+        ccm_free(&ccm_ptr);
         return buffer_free(buf);
     }
 
     //SET Nonce
     buf->src_sa.address[8] ^= 2;
-    mle_security_aux_ccm_nonce_set(ccm_ptr->exp_nonce, &(buf->src_sa.address[8]),
+    mle_security_aux_ccm_nonce_set(ccm_ptr.exp_nonce, &(buf->src_sa.address[8]),
             securityHeader->frameCounter, securityHeader->securityLevel);
     buf->src_sa.address[8] ^= 2;
 
-    if (ccm_ptr->mic_len) {
-        payload_len -= ccm_ptr->mic_len;
-        buf->buf_end -= ccm_ptr->mic_len;
+    if (ccm_ptr.mic_len) {
+        payload_len -= ccm_ptr.mic_len;
+        buf->buf_end -= ccm_ptr.mic_len;
 
-        ccm_ptr->data_ptr = buffer_data_pointer(buf);
-        ccm_ptr->adata_ptr = mle_service->mle_adata;
-        ccm_ptr->data_len = payload_len;
-        ccm_ptr->adata_len = mle_service->mle_adata_length;
+        ccm_ptr.data_ptr = buffer_data_pointer(buf);
+        ccm_ptr.adata_ptr = mle_service->mle_adata;
+        ccm_ptr.data_len = payload_len;
+        ccm_ptr.adata_len = mle_service->mle_adata_length;
         //SET MIC
-        ccm_ptr->mic = ccm_ptr->data_ptr;
-        ccm_ptr->mic += payload_len;
+        ccm_ptr.mic = ccm_ptr.data_ptr;
+        ccm_ptr.mic += payload_len;
     } else {
-        ccm_ptr->data_len = payload_len;
-        ccm_ptr->data_ptr = buffer_data_pointer(buf);
+        ccm_ptr.data_len = payload_len;
+        ccm_ptr.data_ptr = buffer_data_pointer(buf);
     }
 
-    if (ccm_process_run(ccm_ptr) != 0) {
+    if (ccm_process_run(&ccm_ptr) != 0) {
         tr_error("MLE mic fail!");
         buf = buffer_free(buf);
     }
@@ -459,7 +460,7 @@ static buffer_t *mle_message_security_header_set(buffer_t *buf,service_instance_
     //Verify first security level
     if (security_header->securityLevel) {
         //Get Security keys
-        ccm_globals_t *ccm_ptr;
+        ccm_globals_t ccm_ptr;
         uint16_t header_size;
         uint16_t data_len;
         data_len = buffer_data_length(buf);
@@ -468,58 +469,58 @@ static buffer_t *mle_message_security_header_set(buffer_t *buf,service_instance_
             goto drop_buffer;
         }
         // Init
-        ccm_ptr = ccm_sec_init(security_header->securityLevel, ptr, AES_CCM_ENCRYPT, 2);
-        if (!ccm_ptr) {
+        if (!ccm_sec_init(&ccm_ptr, security_header->securityLevel, ptr, AES_CCM_ENCRYPT, 2)) {
             goto drop_buffer;
         }
-
         header_size = mle_security_aux_header_size(security_header->KeyIdMode);
 
         //SET Nonce
-        mle_security_aux_ccm_nonce_set(ccm_ptr->exp_nonce, srv_ptr->mac64, security_header->frameCounter, security_header->securityLevel);
-        if (ccm_ptr->mic_len) {
-            buf = buffer_headroom(buf, (ccm_ptr->mic_len + 32 + header_size));
+        mle_security_aux_ccm_nonce_set(ccm_ptr.exp_nonce, srv_ptr->mac64, security_header->frameCounter, security_header->securityLevel);
+        if (ccm_ptr.mic_len) {
+            buf = buffer_headroom(buf, (ccm_ptr.mic_len + 32 + header_size));
             if (buf) {
                 uint8_t *ptr2;
                 //Move current data to left by mic_len bytes
                 ptr = buffer_data_pointer(buf);
                 //Set new data pointer
                 ptr2 = ptr;
-                ptr2 -= ccm_ptr->mic_len;
+                ptr2 -= ccm_ptr.mic_len;
 
                 memmove(ptr2, ptr, data_len);
 
                 //Cut Mic len
-                buf->buf_end -= ccm_ptr->mic_len;
+                buf->buf_end -= ccm_ptr.mic_len;
 
-                ptr -= header_size + ccm_ptr->mic_len;
+                ptr -= header_size + ccm_ptr.mic_len;
                 ptr = mle_security_aux_header_write(ptr, security_header);
                 ptr -= header_size;
                 //Set pointer to Adata
                 ptr -= (32);
                 mle_service_a_data_set(ptr, buf->src_sa.address, buf->dst_sa.address);
                 //Create ADATA
-                ccm_ptr->adata_ptr = ptr;
-                ccm_ptr->adata_len = (32 + header_size);
+                ccm_ptr.adata_ptr = ptr;
+                ccm_ptr.adata_len = (32 + header_size);
 
                 //SET ptr to show to real payload
-                buf->buf_ptr -= ccm_ptr->mic_len;
+                buf->buf_ptr -= ccm_ptr.mic_len;
             } else {
                 tr_warn("Security header alloc fail");
+                ccm_free(&ccm_ptr);
                 buf = (buffer_t *) 0;
+                ccm_process_run(NULL);
                 return buf;
             }
         }
         ptr = buffer_data_pointer(buf);
-        ccm_ptr->data_ptr = ptr;
-        ccm_ptr->data_len = data_len;
+        ccm_ptr.data_ptr = ptr;
+        ccm_ptr.data_len = data_len;
 
-        ccm_ptr->mic = ptr;
-        ccm_ptr->mic += data_len;
-        ccm_process_run(ccm_ptr);
-        if (ccm_ptr->mic_len) {
+        ccm_ptr.mic = ptr;
+        ccm_ptr.mic += data_len;
+        ccm_process_run(&ccm_ptr);
+        if (ccm_ptr.mic_len) {
             //SET Calculated mic
-            buf->buf_end += ccm_ptr->mic_len;
+            buf->buf_end += ccm_ptr.mic_len;
         }
         buffer_data_reserve_header(buf, header_size);
     }
@@ -655,6 +656,15 @@ static int mle_service_build_packet_send(service_instance_t *srv_ptr, mle_securi
     return 0;
 }
 
+static mle_neighbor_security_counter_info_t *mle_service_get_neighbour_info(protocol_interface_info_entry_t *cur_interface, uint8_t *ll64)
+{
+    mac_neighbor_table_entry_t *neighbour = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur_interface),ll64, false, NULL);
+    if (!neighbour) {
+        return NULL;
+    }
+    return mle_service_counter_info_get(cur_interface->id, neighbour->index);
+}
+
 static void mle_service_socket_callback(void *cb)
 {
     socket_buffer_callback_t *cb_buf = cb;
@@ -678,6 +688,7 @@ static void mle_service_socket_callback(void *cb)
         tr_warn("service handler not registerd");
         goto error_handler;
     }
+    mle_msg.interface_ptr = service_handler->interface_ptr;
 
     // MLE messages are only allowed to Link local address
     if (!addr_is_ipv6_link_local(buf->src_sa.address) ||
@@ -730,7 +741,7 @@ static void mle_service_socket_callback(void *cb)
 
 
         /* MLE neighbour table frame counter check */
-        mle_neigh_table_entry_t *neighbour = NULL;
+        mle_neighbor_security_counter_info_t *neighbour = NULL;
         uint32_t keySeq;
         if (securityHeader.KeyIdMode == MAC_KEY_ID_MODE_SRC4_IDX) {
             keySeq = common_read_32_bit(securityHeader.Keysource);
@@ -739,7 +750,7 @@ static void mle_service_socket_callback(void *cb)
         }
 
         if (mle_service->mle_frame_counter_check_enabled) {
-            neighbour = mle_class_discover_entry_by_ll64(service_handler->interface_id,buf->src_sa.address);
+            neighbour = mle_service_get_neighbour_info(service_handler->interface_ptr,buf->src_sa.address);
             if (neighbour){
                 //key pending is set - incoming frame counter will be reset when new sequence is heard or lower framecounter is heard
                 if (neighbour->new_key_pending) {
@@ -813,6 +824,7 @@ static void mle_service_socket_callback(void *cb)
         }
     }
 #endif
+
     if (security_bypass) {
         /* Security by pass message handler call */
         service_handler->recv_security_bypass_cb(service_handler->interface_id, &mle_msg);
@@ -871,13 +883,13 @@ static bool mle_service_instance_timeout_handler(uint16_t ticks, service_instanc
     }
 }
 
-int mle_service_interface_register(int8_t interface_id, mle_service_receive_cb *receive_cb, uint8_t *mac64, uint8_t challengeLength)
+int mle_service_interface_register(int8_t interface_id, void *interface_ptr,  mle_service_receive_cb *receive_cb, uint8_t *mac64, uint8_t challengeLength)
 {
     service_instance_t *srv_ptr;
 
     if (challengeLength < 4 || challengeLength > 32) {
         return -1;
-    } else if (!receive_cb) {
+    } else if (!receive_cb || !interface_ptr) {
         return -1;
     } else if (!mac64) {
         return -1;
@@ -899,6 +911,7 @@ int mle_service_interface_register(int8_t interface_id, mle_service_receive_cb *
 
     srv_ptr->recv_cb = receive_cb;
     srv_ptr->challenge_length = challengeLength;
+    srv_ptr->interface_ptr = interface_ptr;
     memcpy(srv_ptr->mac64, mac64, 8);
 
     if (mle_service->mle_socket < 0) {
@@ -990,8 +1003,12 @@ void mle_service_interface_unregister(int8_t interface_id)
 
 int mle_service_reset_frame_counters(int8_t interfaceId)
 {
+    service_instance_t *srv_ptr = mle_service_interface_find(interfaceId);
+    if (!srv_ptr) {
+        return -1;
+    }
     mle_service_security_set_frame_counter(interfaceId, 0);
-    mle_class_set_new_key_pending(interfaceId);
+    mle_class_set_new_key_pending(srv_ptr->interface_ptr);
     return 0;
 }
 
@@ -1412,8 +1429,7 @@ bool mle_service_security_key_trig(int8_t interfaceId, uint8_t keyId)
 
 bool mle_service_security_set_security_key(int8_t interfaceId, const uint8_t *security_key, uint8_t keyId, bool primary)
 {
-    bool master_key_changed = false;
-    master_key_changed = mle_service_security_key_set(mle_service_security_params_get(interfaceId), security_key, keyId, primary);
+    bool master_key_changed = mle_service_security_key_set(mle_service_security_params_get(interfaceId), security_key, keyId, primary);
     if (master_key_changed && primary) {
         mle_service_reset_frame_counters(interfaceId);
     }
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service_api.h b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service_api.h
index 2efcf8e601..7055df99e8 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service_api.h
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service_api.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -114,6 +114,7 @@ typedef struct {
     uint8_t *packet_src_address; /*!< Packet Source address */
     uint8_t *packet_dst_address; /*!< Packet Destination address */
     uint8_t *data_ptr; /*!< Data pointer */
+    void    *interface_ptr;
     uint16_t data_length; /*!< Data Length */
     uint16_t src_pan_id;  /*!< Link source Pan-id */
     uint16_t dst_pan_id; /*!< Link destination Pan-id */
@@ -132,6 +133,7 @@ uint8_t retrans_max; /*!< Define max packet TX count */
 uint8_t delay; /*!< 100ms Ticks for random delay */
 } mle_message_timeout_params_t;
 
+
 /**
  *  MLE service message receiver handler call back function pointer
  *
@@ -209,13 +211,14 @@ typedef bool (mle_service_filter_cb)(int8_t interface_id, mle_message_t *mle_msg
  * Creates and shares socket for other mle services.
  *
  * \param interface_id Registered services interface Id.
+ * \param interface_ptr Pointer tointerface
  * \param receive_cb Message RX handler.
  * \param mac64 Interface 64-bit MAC.
  * \param challengeLength challenge length
  * Return values
  * 0, Register OK.
  */
-int mle_service_interface_register(int8_t interface_id, mle_service_receive_cb *receive_cb, uint8_t *mac64, uint8_t challengeLength);
+int mle_service_interface_register(int8_t interface_id, void *interface_ptr, mle_service_receive_cb *receive_cb, uint8_t *mac64, uint8_t challengeLength);
 
 /*
 * Deletes server instance.
@@ -730,6 +733,69 @@ void mle_service_set_fragmented_msg_ll_security(bool value);
 */
 void mle_service_set_accept_invalid_frame_counter(bool value);
 
+
+/**
+* Commands MLE service to allocate mle frame counter table for giving neighbor count.
+*
+* \param interface_id Interface ID
+* \param table_size Table size
+*
+* \return 0 Allocate OK
+* \return -1 Allocate Fail
+*
+*/
+int mle_service_frame_counter_table_allocate(int8_t interface_id, uint8_t table_size);
+
+/**
+* Commands MLE service to free mle frame counter table
+*
+* \param interface_id Interface ID
+*
+* \return 0 Free OK
+* \return -1 Free Fail
+*
+*/
+int mle_service_frame_counter_table_free(int8_t interface_id);
+
+/**
+* Commands MLE service to add / Update frame counter to neighbor
+*
+* \param interface_id Interface ID
+* \param attribute_index Attribute index for neighbor
+* \param frame_counter MLE frame counter
+*
+*/
+void mle_service_frame_counter_entry_add(int8_t interface_id, uint8_t attribute_index, uint32_t frame_counter);
+
+/**
+* Commands MLE service get mle frame counter for neighbor
+*
+* \param interface_id Interface ID
+* \param attribute_index Attribute index for neighbor
+*
+* \return Stored Frame Counter, may return 0 if unknow attribute
+*
+*/
+uint32_t mle_service_neighbor_frame_counter_get(int8_t interface_id, uint8_t attribute_index);
+
+/**
+* Commands MLE service frame counter new key pending state activate
+*
+* \param interface_id Interface ID
+* \param attribute_index Attribute index for neighbor
+*
+*/
+void mle_service_frame_counter_entry_new_key_pending_set(int8_t interface_id, uint8_t attribute_index);
+
+/**
+* Commands MLE service frame counter info delete
+*
+* \param interface_id Interface ID
+* \param attribute_index Attribute index for neighbor
+*
+*/
+void mle_service_frame_counter_entry_delete(int8_t interface_id, uint8_t attribute_index);
+
 #ifdef MLE_TEST
 /**
  * Set callback for MLE receiving packet filtering.
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service_frame_counter_table.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service_frame_counter_table.c
new file mode 100644
index 0000000000..6a6d146146
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service_frame_counter_table.c
@@ -0,0 +1,155 @@
+/*
+ * Copyright (c) 2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "nsconfig.h"
+#include <string.h>
+#include <ns_types.h>
+#include "ns_trace.h"
+#include "common_functions.h"
+#include "ccmLIB.h"
+#include "nsdynmemLIB.h"
+#include "Core/include/address.h"
+#include "Core/include/ns_buffer.h"
+#include "MLE/mle.h"
+#include "mac_common_defines.h"
+#include "Service_Libs/mle_service/mle_service_api.h"
+#include "Service_Libs/mle_service/mle_service_frame_counter_table.h"
+
+
+
+typedef struct {
+    mle_neighbor_security_counter_info_t *security_counter_list;
+    uint8_t table_size;
+    int8_t interface_id;
+    ns_list_link_t link;
+} mle_service_framecounter_instance_list_t;
+
+typedef NS_LIST_HEAD(mle_service_framecounter_instance_list_t, link) mle_service_counter_list_t;
+
+static mle_service_counter_list_t srv_framecounter_instance_list;
+
+
+static mle_service_framecounter_instance_list_t * mle_service_framecounter_params_get(int8_t interface_id)
+{
+    ns_list_foreach(mle_service_framecounter_instance_list_t, cur_ptr, &srv_framecounter_instance_list) {
+        if (cur_ptr->interface_id == interface_id) {
+            return cur_ptr;
+        }
+    }
+    return NULL;
+}
+
+static void mle_service_framecounter_delete(mle_neighbor_security_counter_info_t *cur_ptr)
+{
+    cur_ptr->last_key_sequence = 0;
+    cur_ptr->mle_frame_counter = 0;
+    cur_ptr->new_key_pending = false;
+}
+
+
+mle_neighbor_security_counter_info_t *mle_service_counter_info_get(int8_t interface_id, uint8_t attribute_index)
+{
+    mle_service_framecounter_instance_list_t *srv_ptr = mle_service_framecounter_params_get(interface_id);
+
+    if (!srv_ptr || !srv_ptr->security_counter_list || attribute_index >= srv_ptr->table_size) {
+        return NULL;
+    }
+    mle_neighbor_security_counter_info_t *entry = srv_ptr->security_counter_list + attribute_index;
+    return entry;
+}
+
+int mle_service_frame_counter_table_free(int8_t interface_id)
+{
+    mle_service_framecounter_instance_list_t *srv_ptr = mle_service_framecounter_params_get(interface_id);
+    if (!srv_ptr) {
+        return -1;
+    }
+    ns_list_remove(&srv_framecounter_instance_list, srv_ptr);
+    ns_dyn_mem_free(srv_ptr->security_counter_list);
+    ns_dyn_mem_free(srv_ptr);
+    return 0;
+}
+
+int mle_service_frame_counter_table_allocate(int8_t interface_id, uint8_t table_size)
+{
+
+    mle_service_framecounter_instance_list_t *srv_ptr = mle_service_framecounter_params_get(interface_id);
+    if (!srv_ptr) {
+        srv_ptr = ns_dyn_mem_alloc(sizeof(mle_service_framecounter_instance_list_t));
+        if (!srv_ptr) {
+            return -1;
+        }
+        ns_list_add_to_start(&srv_framecounter_instance_list, srv_ptr);
+        srv_ptr->interface_id = interface_id;
+        srv_ptr->table_size = 0;
+        srv_ptr->security_counter_list = NULL;
+    }
+
+    if (srv_ptr->table_size != table_size) {
+        ns_dyn_mem_free(srv_ptr->security_counter_list);
+        srv_ptr->security_counter_list = ns_dyn_mem_alloc(sizeof(mle_neighbor_security_counter_info_t) * table_size);
+    }
+
+    if (!srv_ptr->security_counter_list) {
+        mle_service_frame_counter_table_free(interface_id);
+        return -1;
+    }
+    srv_ptr->table_size = table_size;
+
+    mle_neighbor_security_counter_info_t *cur_ptr = srv_ptr->security_counter_list;
+    for (uint8_t i=0; i< table_size; i++) {
+        mle_service_framecounter_delete(cur_ptr);
+        cur_ptr++;
+    }
+    return 0;
+}
+
+void mle_service_frame_counter_entry_add(int8_t interface_id, uint8_t attribute_index, uint32_t frame_counter)
+{
+    mle_neighbor_security_counter_info_t * entry = mle_service_counter_info_get(interface_id, attribute_index);
+    if (entry) {
+        entry->mle_frame_counter = frame_counter;
+    }
+}
+
+void mle_service_frame_counter_entry_new_key_pending_set(int8_t interface_id, uint8_t attribute_index)
+{
+    mle_neighbor_security_counter_info_t * entry = mle_service_counter_info_get(interface_id, attribute_index);
+    if (entry) {
+        entry->new_key_pending = true;
+    }
+}
+
+void mle_service_frame_counter_entry_delete(int8_t interface_id, uint8_t attribute_index)
+{
+    mle_neighbor_security_counter_info_t * entry = mle_service_counter_info_get(interface_id, attribute_index);
+    if (entry) {
+        mle_service_framecounter_delete(entry);
+    }
+
+}
+
+uint32_t mle_service_neighbor_frame_counter_get(int8_t interface_id, uint8_t attribute_index)
+{
+    mle_neighbor_security_counter_info_t * entry = mle_service_counter_info_get(interface_id, attribute_index);
+    if (!entry) {
+        return 0;
+    }
+    return entry->mle_frame_counter;
+
+}
+
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service_frame_counter_table.h b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service_frame_counter_table.h
new file mode 100644
index 0000000000..afbed9d9c6
--- /dev/null
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service_frame_counter_table.h
@@ -0,0 +1,35 @@
+/*
+ * Copyright (c) 2018, Arm Limited and affiliates.
+ * SPDX-License-Identifier: Apache-2.0
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef MLE_SERVICE_FRAME_COUNTER_TABLE_H_
+#define MLE_SERVICE_FRAME_COUNTER_TABLE_H_
+
+#include <ns_types.h>
+/*
+ * MLE neighbor security counter info entry for replay attack
+ *
+ */
+typedef struct {
+    uint32_t        mle_frame_counter;  /*!< Last used security frame counter */
+    uint32_t        last_key_sequence;  /*!< Last used key sequence */
+    bool        new_key_pending:1;  /*!< New Key is pending */
+} mle_neighbor_security_counter_info_t;
+
+//Internal use
+mle_neighbor_security_counter_info_t *mle_service_counter_info_get(int8_t interface_id, uint8_t attribute_index);
+
+#endif /* MLE_SERVICE_FRAME_COUNTER_TABLE_H_ */
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service_interface.h b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service_interface.h
index e0aab504d7..32166cf343 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service_interface.h
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/mle_service/mle_service_interface.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -31,6 +31,7 @@
 typedef struct {
     mle_service_receive_cb *recv_cb; /*!< Interface message handler */
     mle_service_receive_security_bypass_cb *recv_security_bypass_cb; /*!< Interface message security bypass handler */
+    void *interface_ptr;
     uint8_t mac64[8]; /*!< Interface MAC 64 used for Security nonce and LL64 address define */
     int8_t interface_id; /*!< Interface Id */
     uint8_t challenge_length; /*!< Define MLE challenge length */
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/nd_proxy/nd_proxy.c b/features/nanostack/sal-stack-nanostack/source/Service_Libs/nd_proxy/nd_proxy.c
index 51b96b2079..fb58f5c4fa 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/nd_proxy/nd_proxy.c
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/nd_proxy/nd_proxy.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/nd_proxy/nd_proxy.h b/features/nanostack/sal-stack-nanostack/source/Service_Libs/nd_proxy/nd_proxy.h
index 4ab48e23e5..dfc0714918 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/nd_proxy/nd_proxy.h
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/nd_proxy/nd_proxy.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015-2017, Arm Limited and affiliates.
+ * Copyright (c) 2015-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/features/nanostack/sal-stack-nanostack/source/Service_Libs/whiteboard/whiteboard.h b/features/nanostack/sal-stack-nanostack/source/Service_Libs/whiteboard/whiteboard.h
index 86cc059a3e..01b9e90f18 100644
--- a/features/nanostack/sal-stack-nanostack/source/Service_Libs/whiteboard/whiteboard.h
+++ b/features/nanostack/sal-stack-nanostack/source/Service_Libs/whiteboard/whiteboard.h
@@ -37,7 +37,7 @@ extern uint16_t whiteboard_size_get(void);
 #define whiteboard_rm_entry(id, address)
 #define whiteboard_table_check_address(address) NULL
 #define whiteboard_table_update(address, eui64, status) NULL
-#define whiteboard_interface_unregister_all_address(nwk_id) -1
+#define whiteboard_interface_unregister_all_address(nwk_id)
 #define whiteboard_interface_address_cmp(address) false
 #endif
 
diff --git a/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.c b/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.c
index 9e36a43a2a..0b47ed5b72 100644
--- a/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.c
+++ b/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012-2017, Arm Limited and affiliates.
+ * Copyright (c) 2012-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -1088,8 +1088,10 @@ static const char *route_src_names[] = {
     [ROUTE_MPL]     = "MPL",
     [ROUTE_RIP]     = "RIP",
     [ROUTE_THREAD]  = "Thread",
-    [ROUTE_THREAD_BORDER_ROUTER] = "Thread BR",
+    [ROUTE_THREAD_BORDER_ROUTER] = "Thread Network data",
     [ROUTE_THREAD_PROXIED_HOST] = "Thread Proxy",
+    [ROUTE_THREAD_BBR] = "Thread BBR",
+    [ROUTE_THREAD_PROXIED_DUA_HOST] = "Thread DUA Proxy",
     [ROUTE_REDIRECT] = "Redirect",
 };
 
@@ -1199,6 +1201,9 @@ static void ipv6_route_entry_remove(ipv6_route_t *route)
 #ifdef FEA_TRACE_SUPPORT
     ipv6_route_print(route, trace_debug_print);
 #endif
+    if (route->info_autofree) {
+        ns_dyn_mem_free(route->info.info);
+    }
     if (protocol_core_buffers_in_event_queue > 0) {
         // Alert any buffers in the queue already routed by this source
         ipv6_route_source_invalidated[route->info.source] = true;
@@ -1548,6 +1553,7 @@ ipv6_route_t *ipv6_route_add_metric(const uint8_t *prefix, uint8_t prefix_len, i
         route->lifetime = lifetime;
         route->metric = metric;
         route->info.source = source;
+        route->info_autofree = false;
         route->info.info = info;
         route->info.source_id = source_id;
         route->info.interface_id = interface_id;
diff --git a/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.h b/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.h
index 5d39a1cfa8..b0858c6472 100644
--- a/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.h
+++ b/features/nanostack/sal-stack-nanostack/source/ipv6_stack/ipv6_routing_table.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012, 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2012, 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -82,6 +82,7 @@ typedef enum ipv6_route_src {
     ROUTE_THREAD,
     ROUTE_THREAD_BORDER_ROUTER,
     ROUTE_THREAD_PROXIED_HOST,
+    ROUTE_THREAD_PROXIED_DUA_HOST,
     ROUTE_THREAD_BBR,
     ROUTE_REDIRECT,     /* Only occurs in destination cache */
     ROUTE_MAX,
@@ -120,6 +121,8 @@ typedef struct ipv6_neighbour_cache {
     bool                                    send_nud_probes : 1;
     bool                                    recv_ns_aro : 1;
     bool                                    recv_na_aro : 1;
+    bool                                    use_eui64_as_slla_in_aro : 1;
+    bool                                    omit_aro_success : 1;
     int8_t                                  interface_id;
     uint8_t                                 max_ll_len;
     uint8_t                                 gc_timer;
@@ -215,6 +218,7 @@ void ipv6_destination_cache_timer(uint8_t ticks);
 #ifdef HAVE_IPV6_ND
 void ipv6_destination_redirect(const uint8_t *dest_addr, const uint8_t *sender_addr, const uint8_t *redirect_addr, int8_t interface_id, addrtype_t ll_type, const uint8_t *ll_address);
 #endif
+
 /* Combined Routing Table (RFC 4191) and Prefix List (RFC 4861) */
 /* On-link prefixes have the on_link flag set and next_hop is unset */
 typedef struct ipv6_route {
@@ -222,6 +226,7 @@ typedef struct ipv6_route {
     bool                on_link: 1;
     bool                search_skip: 1;
     bool                probe: 1;
+    bool                info_autofree:1;
     uint8_t             metric;             // 0x40 = RFC 4191 pref high, 0x80 = default, 0xC0 = RFC 4191 pref low
     ipv6_route_info_t   info;
     uint32_t            lifetime;           // (seconds); 0xFFFFFFFF means permanent
diff --git a/features/nanostack/sal-stack-nanostack/source/ipv6_stack/protocol_ipv6.c b/features/nanostack/sal-stack-nanostack/source/ipv6_stack/protocol_ipv6.c
index e0768701c9..7f01fbbe51 100644
--- a/features/nanostack/sal-stack-nanostack/source/ipv6_stack/protocol_ipv6.c
+++ b/features/nanostack/sal-stack-nanostack/source/ipv6_stack/protocol_ipv6.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2012-2017, Arm Limited and affiliates.
+ * Copyright (c) 2012-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -954,7 +954,7 @@ static void ipv6_interface_address_cb(protocol_interface_info_entry_t *interface
     }
 }
 
-void ipv6_interface_slaac_handler(protocol_interface_info_entry_t *cur, uint8_t *slaacPrefix, uint8_t prefixLen, uint32_t validLifeTime, uint32_t preferredLifeTime)
+void ipv6_interface_slaac_handler(protocol_interface_info_entry_t *cur, const uint8_t *slaacPrefix, uint8_t prefixLen, uint32_t validLifeTime, uint32_t preferredLifeTime)
 {
     if_address_entry_t *address_entry = icmpv6_slaac_address_add(cur, slaacPrefix, prefixLen, validLifeTime, preferredLifeTime, false, SLAAC_IID_DEFAULT);
     if (address_entry) {
diff --git a/features/nanostack/sal-stack-nanostack/source/ipv6_stack/protocol_ipv6.h b/features/nanostack/sal-stack-nanostack/source/ipv6_stack/protocol_ipv6.h
index 97715600c3..7d877ef2b2 100644
--- a/features/nanostack/sal-stack-nanostack/source/ipv6_stack/protocol_ipv6.h
+++ b/features/nanostack/sal-stack-nanostack/source/ipv6_stack/protocol_ipv6.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -63,7 +63,7 @@ int ipv6_prefix_register(uint8_t *prefix_64, uint32_t lifetime, uint32_t prefer_
 int ipv6_prefix_router_flag_activate(uint8_t *ipv6_address);
 
 void ipv6_nd_ra_advert(struct protocol_interface_info_entry *cur, const uint8_t *dest);
-void ipv6_interface_slaac_handler(struct protocol_interface_info_entry *cur, uint8_t *slaacPrefix, uint8_t prefixLen, uint32_t validLifeTime, uint32_t preferredLifeTime);
+void ipv6_interface_slaac_handler(struct protocol_interface_info_entry *cur, const uint8_t *slaacPrefix, uint8_t prefixLen, uint32_t validLifeTime, uint32_t preferredLifeTime);
 void ipv6_stack_route_advert_update(uint8_t *address, uint8_t prefixLength, uint8_t routePrefer);
 void ipv6_stack_route_advert_remove(uint8_t *address, uint8_t prefixLength);
 void ipv6_prefix_on_link_update(uint8_t *address);
diff --git a/features/nanostack/sal-stack-nanostack/source/libDHCPv6/dhcp_service_api.c b/features/nanostack/sal-stack-nanostack/source/libDHCPv6/dhcp_service_api.c
index 0248d1beb9..3027046fcc 100644
--- a/features/nanostack/sal-stack-nanostack/source/libDHCPv6/dhcp_service_api.c
+++ b/features/nanostack/sal-stack-nanostack/source/libDHCPv6/dhcp_service_api.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2013-2017, Arm Limited and affiliates.
+ * Copyright (c) 2013-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -369,7 +369,7 @@ uint16_t dhcp_service_init(int8_t interface_id, dhcp_instance_type_e instance_ty
             break;
         }
     }
-    srv_ptr = ns_dyn_mem_temporary_alloc(sizeof(server_instance_t));
+    srv_ptr = ns_dyn_mem_alloc(sizeof(server_instance_t));
     if (id == MAX_SERVERS || srv_ptr == NULL) {
         tr_error("Out of server instances");
         ns_dyn_mem_free(srv_ptr);
diff --git a/features/nanostack/sal-stack-nanostack/source/libNET/src/multicast_api.c b/features/nanostack/sal-stack-nanostack/source/libNET/src/multicast_api.c
index 2b21ad44d8..5f1b86c63b 100644
--- a/features/nanostack/sal-stack-nanostack/source/libNET/src/multicast_api.c
+++ b/features/nanostack/sal-stack-nanostack/source/libNET/src/multicast_api.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2017, Arm Limited and affiliates.
+ * Copyright (c) 2017-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -185,7 +185,7 @@ int_fast8_t multicast_mpl_set_default_parameters(int8_t interface_id,
                                                  uint8_t control_message_k,
                                                  uint8_t control_message_timer_expirations)
 {
-    protocol_interface_info_entry_t *interface = protocol_stack_interface_info_get(interface_id);
+    protocol_interface_info_entry_t *interface = protocol_stack_interface_info_get_by_id(interface_id);
     if (!interface) {
         return -1;
     }
diff --git a/features/nanostack/sal-stack-nanostack/source/libNET/src/net_load_balance.c b/features/nanostack/sal-stack-nanostack/source/libNET/src/net_load_balance.c
index ad79c2bd5f..e01138ccde 100644
--- a/features/nanostack/sal-stack-nanostack/source/libNET/src/net_load_balance.c
+++ b/features/nanostack/sal-stack-nanostack/source/libNET/src/net_load_balance.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/features/nanostack/sal-stack-nanostack/source/libNET/src/net_test.c b/features/nanostack/sal-stack-nanostack/source/libNET/src/net_test.c
index 668ba809e4..59dd875814 100644
--- a/features/nanostack/sal-stack-nanostack/source/libNET/src/net_test.c
+++ b/features/nanostack/sal-stack-nanostack/source/libNET/src/net_test.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2016-2017, Arm Limited and affiliates.
+ * Copyright (c) 2016-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/features/nanostack/sal-stack-nanostack/source/libNET/src/ns_net.c b/features/nanostack/sal-stack-nanostack/source/libNET/src/ns_net.c
index 88ee5ef95f..769172a183 100644
--- a/features/nanostack/sal-stack-nanostack/source/libNET/src/ns_net.c
+++ b/features/nanostack/sal-stack-nanostack/source/libNET/src/ns_net.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
@@ -66,6 +66,7 @@
 #include "6LoWPAN/Thread/thread_routing.h"
 #include "6LoWPAN/Thread/thread_bootstrap.h"
 #include "6LoWPAN/Thread/thread_management_internal.h"
+#include "6LoWPAN/ws/ws_bootstrap.h"
 #include "BorderRouter/border_router.h"
 #include "Service_Libs/mle_service/mle_service_api.h"
 #include "6LoWPAN/MAC/mac_data_poll.h"
@@ -1071,6 +1072,8 @@ int8_t arm_nwk_interface_configure_6lowpan_bootstrap_set(int8_t interface_id, ne
 
         if (net_6lowpan_mode_extension == NET_6LOWPAN_THREAD) {
             ret_val = thread_node_bootstrap_init(interface_id,bootstrap_mode);
+        } else if (net_6lowpan_mode_extension == NET_6LOWPAN_WS) {
+                ret_val = ws_bootstrap_init(interface_id,bootstrap_mode);
         } else {
             ret_val = arm_6lowpan_bootstarp_bootstrap_set(interface_id,bootstrap_mode,net_6lowpan_mode_extension);
         }
diff --git a/features/nanostack/sal-stack-nanostack/source/libNET/src/socket_api.c b/features/nanostack/sal-stack-nanostack/source/libNET/src/socket_api.c
index 84d5383ba0..567fd0e714 100644
--- a/features/nanostack/sal-stack-nanostack/source/libNET/src/socket_api.c
+++ b/features/nanostack/sal-stack-nanostack/source/libNET/src/socket_api.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2014-2017, Arm Limited and affiliates.
+ * Copyright (c) 2014-2018, Arm Limited and affiliates.
  * SPDX-License-Identifier: Apache-2.0
  *
  * Licensed under the Apache License, Version 2.0 (the "License");
diff --git a/features/nanostack/sal-stack-nanostack/sources.mk b/features/nanostack/sal-stack-nanostack/sources.mk
index dac809e23e..0217a539d4 100644
--- a/features/nanostack/sal-stack-nanostack/sources.mk
+++ b/features/nanostack/sal-stack-nanostack/sources.mk
@@ -11,6 +11,7 @@ SRCS += \
 	source/6LoWPAN/IPHC_Decode/lowpan_context.c \
 	source/6LoWPAN/MAC/beacon_handler.c \
 	source/6LoWPAN/MAC/mac_helper.c \
+	source/6LoWPAN/MAC/mac_ie_lib.c \
 	source/6LoWPAN/MAC/mac_response_handler.c \
 	source/6LoWPAN/MAC/mac_data_poll.c \
 	source/6LoWPAN/MAC/mac_pairwise_key.c \
@@ -84,17 +85,18 @@ SRCS += \
 	source/Security/TLS/tls_lib.c \
 	source/Security/TLS/tls_ccm_crypt.c \
 	source/Service_Libs/CCM_lib/ccm_security.c \
-	source/Service_Libs/fhss/fhss_beacon.c \
-	source/Service_Libs/fhss/fhss_beacon_tasklet.c \
 	source/Service_Libs/fhss/fhss_channel.c \
-	source/Service_Libs/fhss/fhss_mac_interface.c \
 	source/Service_Libs/fhss/fhss_configuration_interface.c \
 	source/Service_Libs/fhss/fhss_statistics.c \
 	source/Service_Libs/fhss/fhss.c \
+	source/Service_Libs/fhss/fhss_ws_empty_functions.c \
+	source/Service_Libs/fhss/fhss_common.c \
 	source/Service_Libs/fhss/channel_list.c \
 	source/Service_Libs/fnv_hash/fnv_hash.c \
+	source/Service_Libs/mac_neighbor_table/mac_neighbor_table.c \
 	source/Service_Libs/mle_service/mle_service.c \
 	source/Service_Libs/mle_service/mle_service_buffer.c \
+	source/Service_Libs/mle_service/mle_service_frame_counter_table.c \
 	source/Service_Libs/mle_service/mle_service_interface.c \
 	source/Service_Libs/mle_service/mle_service_security.c \
 	source/Service_Libs/blacklist/blacklist.c \
@@ -117,6 +119,7 @@ SRCS += \
 	source/6LoWPAN/Thread/thread_common.c \
 	source/6LoWPAN/Thread/thread_joiner_application.c \
 	source/6LoWPAN/Thread/thread_leader_service.c \
+	source/6LoWPAN/Thread/thread_neighbor_class.c \
 	source/6LoWPAN/Thread/thread_management_server.c \
 	source/6LoWPAN/Thread/thread_management_client.c \
 	source/6LoWPAN/Thread/thread_network_synch.c \