/*
 * 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
 * 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 <nsdynmemLIB.h>
#include <ns_list.h>
#include "randLIB.h"
#include "ns_trace.h"
#include "common_functions.h"
#include "thread_management_if.h"
#include "thread_border_router_api.h"
#include "6LoWPAN/Thread/thread_config.h"
#include "6LoWPAN/Thread/thread_common.h"
#include "6LoWPAN/Thread/thread_bootstrap.h"
#include "6LoWPAN/Thread/thread_dhcpv6_client.h"
#include "6LoWPAN/Thread/thread_discovery.h"
#include "6LoWPAN/Thread/thread_joiner_application.h"
#include "6LoWPAN/Thread/thread_network_data_lib.h"
#include "6LoWPAN/Thread/thread_nd.h"
#include "6LoWPAN/Thread/thread_tmfcop_lib.h"
#include "6LoWPAN/Thread/thread_management_internal.h"
#include "6LoWPAN/Thread/thread_management_server.h"
#include "6LoWPAN/Thread/thread_commissioning_if.h"
#include "6LoWPAN/Thread/thread_extension.h"
#include "6LoWPAN/Thread/thread_leader_service.h"
#include "6LoWPAN/Thread/thread_router_bootstrap.h"
#include "6LoWPAN/Thread/thread_network_synch.h"
#include "6LoWPAN/Thread/thread_nvm_store.h"
#include "Service_Libs/mle_service/mle_service_api.h"
#include "6LoWPAN/Thread/thread_host_bootstrap.h"
#include "6LoWPAN/Thread/thread_border_router_api_internal.h"
#include "MPL/mpl.h"
#include "MLE/mle.h"
#include "6LoWPAN/MAC/mac_helper.h"
#include "mac_api.h"
#include "6LoWPAN/MAC/mac_data_poll.h"
#include "coap_service_api.h"

#define TRACE_GROUP "tLdr"

#ifdef HAVE_THREAD_LEADER_SERVICE

/*
 * Leader instance data.
 */
typedef struct thread_leader_service {
    int8_t interface_id;
    int8_t coap_service_id;
    bool leader_enabled: 1;
    ns_list_link_t link;
} thread_leader_service_t;

/*
 * Helper structure for router_id handling
 */
typedef struct {
    uint8_t     routerID[8];
    uint32_t    validLifeTime;
    uint32_t    preferredLifeTime;
    uint16_t    shortAddress;
    uint8_t     dataSeq;
} thread_leader_service_router_id_resp_t;

static NS_LIST_DEFINE(leader_service_instance_list, thread_leader_service_t, link);

static void thread_leader_service_commissioner_timeout_cb(void* arg);

/* Private functions */
static thread_leader_service_t *thread_leader_service_find_by_interface(int8_t interface_id)
{
    thread_leader_service_t *this = NULL;
    ns_list_foreach(thread_leader_service_t, cur_br, &leader_service_instance_list) {
        if (cur_br->interface_id == interface_id) {
            this = cur_br;
            break;
        }
    }
    return this;
}

static thread_leader_service_t *thread_leader_service_find_by_service(int8_t service_id)
{
    thread_leader_service_t *this = NULL;
    ns_list_foreach(thread_leader_service_t, cur_br, &leader_service_instance_list) {
        if (cur_br->coap_service_id == service_id) {
            this = cur_br;
            break;
        }
    }
    return this;
}

static char* thread_commissioning_if_commissioner_id_get(int8_t interface_id)
{
    protocol_interface_info_entry_t *cur;

    cur = protocol_stack_interface_info_get_by_id(interface_id);
    if (!cur || !cur->thread_info || !cur->thread_info->registered_commissioner.commissioner_valid) {
        return NULL;
    }

    return cur->thread_info->registered_commissioner.commissioner_id_ptr;
}


static bool thread_leader_service_commissioner_address_changed(int8_t interface_id, uint8_t border_router_address[static 16])
{
    protocol_interface_info_entry_t *cur;

    cur = protocol_stack_interface_info_get_by_id(interface_id);
    if (!cur || !cur->thread_info || !cur->thread_info->registered_commissioner.commissioner_valid) {
        return false;
    }

    if(!memcmp(cur->thread_info->registered_commissioner.border_router_address, border_router_address, 16)) {
        // Border router address is same
        return false;
    }
    if( common_read_16_bit(&border_router_address[14]) >= 0xfc00 ) {
        // Address is not valid RLOC we dont change address to this.
        return false;
    }
    memcpy(cur->thread_info->registered_commissioner.border_router_address, border_router_address, 16);
    thread_leader_service_network_data_changed(cur,false,true);
    // This will trigger advertisement either from following commands or from leader tick
    return true;
}
static int thread_leader_service_steering_data_set(int8_t interface_id, uint8_t *buf_ptr, uint16_t length)
{
    protocol_interface_info_entry_t *cur;
    link_configuration_s *linkConfiguration;

    linkConfiguration = thread_joiner_application_get_config(interface_id);
    if (!linkConfiguration) {
        return -1;
    }

    cur = protocol_stack_interface_info_get_by_id(interface_id);
    if (!cur || !cur->thread_info || !cur->thread_info->leader_private_data || !cur->thread_info->registered_commissioner.commissioner_valid) {
        return -1;
    }
    if (length > 16) {
        return -2;
    }

    tr_debug("Save new steering data %s", trace_array(buf_ptr, length));

    cur->thread_info->registered_commissioner.steering_data_len = 0;

    if (buf_ptr && length > 0) {
        memcpy(cur->thread_info->registered_commissioner.steering_data, buf_ptr, length);
        cur->thread_info->registered_commissioner.steering_data_len = length;
    }

    thread_leader_service_network_data_changed(cur, false, true);

    return 0;
}

static bool thread_leader_service_commissioner_unregister(int8_t interface_id, uint16_t session_id)
{
    protocol_interface_info_entry_t *cur;
    link_configuration_s *linkConfiguration;

    linkConfiguration = thread_joiner_application_get_config(interface_id);
    if(!linkConfiguration)
        return false;

    cur = protocol_stack_interface_info_get_by_id(interface_id);
    if (!cur || !cur->thread_info || !cur->thread_info->leader_private_data)
        return false;

    if (cur->thread_info->registered_commissioner.session_id != session_id) {
        tr_debug("Commissioner session id not valid - unregister failed");
        return false;
    }
    // Removing commissioner we need to increase the session id
    cur->thread_info->registered_commissioner.session_id++;

    if(cur->thread_info->registered_commissioner.commissioner_id_ptr){
       ns_dyn_mem_free(cur->thread_info->registered_commissioner.commissioner_id_ptr);
       cur->thread_info->registered_commissioner.commissioner_id_ptr = NULL;
    }

    cur->thread_info->registered_commissioner.commissioner_valid = false;
    cur->thread_info->registered_commissioner.steering_data_len = 0;

    /* Send unregistration without delay */
    thread_leader_service_network_data_changed(cur, false, true);

    eventOS_timeout_cancel(cur->thread_info->registered_commissioner.commissioner_timeout);
    cur->thread_info->registered_commissioner.commissioner_timeout = NULL;
    cur->thread_info->registered_commissioner.commissioner_registration = THREAD_COMMISSIONER_NOT_REGISTERED;

    return true;
}

static bool thread_leader_service_commissioner_session_refresh(int8_t interface_id, uint16_t session_id)
{
    protocol_interface_info_entry_t *cur;

    // Check pointers
    cur = protocol_stack_interface_info_get_by_id(interface_id);
    if (!cur || !cur->thread_info || !cur->thread_info->leader_private_data || !cur->thread_info->registered_commissioner.commissioner_valid) {
        return false;
    }

    if(cur->thread_info->registered_commissioner.commissioner_registration == THREAD_COMMISSIONER_NOT_REGISTERED) {
        tr_debug("Commissioner not registered - refresh failed");
        return false;
    }

    if (cur->thread_info->registered_commissioner.session_id != session_id) {
        tr_debug("Commissioner session id not valid - refresh failed");
        return false;
    }

    cur->thread_info->registered_commissioner.commissioner_registration = THREAD_COMMISSIONER_REGISTERED;

    eventOS_timeout_cancel(cur->thread_info->registered_commissioner.commissioner_timeout);
    cur->thread_info->registered_commissioner.commissioner_timeout = eventOS_timeout_ms(thread_leader_service_commissioner_timeout_cb, COMMISSIONER_OBSOLETE_TIMEOUT, cur->thread_info);

    return true;
}

static void thread_leader_service_commissioner_timeout_cb(void* arg)
{
    thread_info_t *thread_ptr = arg;

    thread_ptr->registered_commissioner.commissioner_timeout = NULL;

    if (!thread_ptr->registered_commissioner.commissioner_valid) {
        return;
    }

    if (!thread_ptr->leader_private_data) {
        return;
    }

    switch(thread_ptr->registered_commissioner.commissioner_registration)
    {
    case THREAD_COMMISSIONER_REGISTRATION_OBSOLETE:
        thread_ptr->registered_commissioner.commissioner_registration = THREAD_COMMISSIONER_NOT_REGISTERED;
        if(false == thread_leader_service_commissioner_unregister(thread_ptr->interface_id, thread_ptr->registered_commissioner.session_id)){
            tr_debug("Commissioner registration remove failed");
        }
        else{
            tr_debug("Commissioner registration removed");
        }
        break;
    case THREAD_COMMISSIONER_REGISTERED:
        tr_debug("Commissioner registration obsoleted");
        thread_ptr->registered_commissioner.commissioner_registration = THREAD_COMMISSIONER_REGISTRATION_OBSOLETE;
        thread_ptr->registered_commissioner.commissioner_timeout = eventOS_timeout_ms(thread_leader_service_commissioner_timeout_cb, COMMISSIONER_REMOVE_TIMEOUT, thread_ptr);
        break;
    case THREAD_COMMISSIONER_NOT_REGISTERED:
    default:
       break;
    }
}


static int thread_leader_service_commissioner_register(int8_t interface_id, uint8_t border_router_address[static 16],
                                  uint8_t *commissioner_id_ptr, uint16_t commissioner_id_len,
                                  uint16_t *session_id)
{
    /* Save commissioner data to new commissioner structure and start advertise it
    Commissioner session id
    commissioner timestamp
    commissioner credentials -> static configuration
    security policy -> static configuration
    network name -> static configuration
    border router locator
    steering data
    */
    protocol_interface_info_entry_t *cur;
    link_configuration_s *linkConfiguration;

    tr_debug("Register interface %d commissioner: %s", interface_id, trace_ipv6(border_router_address));

    linkConfiguration = thread_joiner_application_get_config(interface_id);
    if (!linkConfiguration) {
        return -1;
    }

    cur = protocol_stack_interface_info_get_by_id(interface_id);
    if (!cur || !cur->thread_info || !cur->thread_info->leader_private_data) {
        return -1;
    }

    /* If commissioner already registered */
    if(cur->thread_info->registered_commissioner.commissioner_valid &&
            cur->thread_info->registered_commissioner.commissioner_registration == THREAD_COMMISSIONER_REGISTERED) {
        return -2;
    }

    ns_dyn_mem_free(cur->thread_info->registered_commissioner.commissioner_id_ptr);
    cur->thread_info->registered_commissioner.commissioner_id_ptr = ns_dyn_mem_alloc(commissioner_id_len + 1);
    if (!cur->thread_info->registered_commissioner.commissioner_id_ptr) {
        return -1;
    }
    memcpy(cur->thread_info->registered_commissioner.commissioner_id_ptr, commissioner_id_ptr, commissioner_id_len);
    cur->thread_info->registered_commissioner.commissioner_id_ptr[commissioner_id_len] = 0;

    // Initialise values
    cur->thread_info->registered_commissioner.session_id++;
    if (cur->thread_info->registered_commissioner.session_id == 0) {
        cur->thread_info->registered_commissioner.session_id++;
    }

    if (session_id) {
        *session_id = cur->thread_info->registered_commissioner.session_id;
    }

    //SET Border Router Locator
    memcpy(cur->thread_info->registered_commissioner.border_router_address, border_router_address, 16);
    cur->thread_info->registered_commissioner.commissioner_valid = true;

    cur->thread_info->registered_commissioner.commissioner_registration = THREAD_COMMISSIONER_REGISTERED;

    eventOS_timeout_cancel(cur->thread_info->registered_commissioner.commissioner_timeout);
    cur->thread_info->registered_commissioner.commissioner_timeout = eventOS_timeout_ms(thread_leader_service_commissioner_timeout_cb, COMMISSIONER_OBSOLETE_TIMEOUT, cur->thread_info);

    /* Delay network data propagation, perhaps we receive more data */
    thread_leader_service_network_data_changed(cur, false, true);

    return 0;
}



static int thread_leader_service_active_set_cb(int8_t service_id, uint8_t source_address[16], uint16_t source_port, sn_coap_hdr_s *request_ptr)
{
    thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
    link_configuration_s *linkConfiguration;
    sn_coap_msg_code_e response_code = COAP_MSG_CODE_RESPONSE_CHANGED;
    uint16_t session_id;
    uint8_t payload[5]; // 4 + 1
    uint8_t *ptr;
    uint8_t *response_ptr;
    uint64_t timestamp;
    bool msg_from_commissioner = false;
    bool changing_connectivity = false;
    (void) source_address;
    (void) source_port;
    int ret = 0;

    if (!this) {
        return -1;
    }
    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(this->interface_id);
    linkConfiguration = thread_joiner_application_get_config(this->interface_id);
    if(!cur || !cur->thread_info || !linkConfiguration || !cur->thread_info->leader_private_data){
        return -1;
    }

    tr_info("thread management Active set");

    response_ptr = payload;

    if (!thread_management_server_source_address_check(this->interface_id, source_address)) {
        response_code = COAP_MSG_CODE_RESPONSE_BAD_REQUEST;
        goto send_error_response;
    }

    if (3 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_CHANNEL, &ptr) &&
         (linkConfiguration->rfChannel != common_read_16_bit(&ptr[1]) || linkConfiguration->channel_page != *ptr)){
        tr_debug("Channel changed");
        changing_connectivity = true;
        // validity check for channel page
        if (*ptr != 0 || common_read_16_bit(&ptr[1]) < 11 || common_read_16_bit(&ptr[1]) > 26) {
            tr_warn("invalid channel");
            ret = -1;
            goto send_response;
        }
    }
    if (2 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_PANID, &ptr) &&
         linkConfiguration->panId != common_read_16_bit(ptr)){
        tr_debug("PANID changed");
        changing_connectivity = true;
        if (common_read_16_bit(ptr) > 0xfffe) {
            tr_warn("invalid panid");
            ret = -1;
            goto send_response;
        }
    }
    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) < 1) {
            tr_warn("invalid security policy");
            ret = -1;
            goto send_response;
        }
    }

    if ( 8 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_NETWORK_MESH_LOCAL_ULA, &ptr) &&
         memcmp(linkConfiguration->mesh_local_ula_prefix,ptr,8) != 0) {
        tr_debug("ML prefix changed");
        changing_connectivity = true;
    }
    if ( 16 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_NETWORK_MASTER_KEY, &ptr) &&
         memcmp(linkConfiguration->master_key,ptr,16) != 0) {
        tr_debug("Master key changed");
        changing_connectivity = true;
    }

    if (2 <= thread_meshcop_tlv_data_get_uint16(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_COMMISSIONER_SESSION_ID, &session_id)) {
        // Session id present must be valid
        tr_info("message from commissioner");
        msg_from_commissioner = true;
        if (cur->thread_info->registered_commissioner.session_id != session_id) {
            tr_warn("Invalid commissioner session id");
            ret = -1;
            goto send_response;
        }
        thread_leader_service_commissioner_address_changed(this->interface_id, source_address);
        // Check if address changed
        if (changing_connectivity == true) {
            tr_warn("Changes affecting connectivity rejected");
            ret = -1;
            goto send_response;
        }
    }

    if(8 > thread_meshcop_tlv_data_get_uint64(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_ACTIVE_TIME_STAMP, &timestamp)){
        tr_warn("no timestamp");
        ret = -1;
        goto send_response;
    }
    if (timestamp < linkConfiguration->timestamp){
        tr_warn("older timestamp");
        ret = -1;
        goto send_response;
    }
    if (timestamp == linkConfiguration->timestamp){
        tr_warn("same timestamp");
        ret = -1;
        goto send_response;
    }
    if (!msg_from_commissioner) {
        tr_debug("message from end device");
        if (cur->thread_info->registered_commissioner.commissioner_registration != THREAD_COMMISSIONER_NOT_REGISTERED) {
            // Node changed settings and Registered commissioner send changed_notify to commissioner
            tr_info("Notify commissioner that settings are changed");
            coap_service_request_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, cur->thread_info->registered_commissioner.border_router_address,
                         THREAD_MANAGEMENT_PORT, COAP_MSG_TYPE_CONFIRMABLE, COAP_MSG_CODE_REQUEST_POST, THREAD_URI_DATASET_CHANGED,
                         COAP_CT_OCTET_STREAM, NULL, 0, NULL);
        }
    }
    if (changing_connectivity) {

        tr_info("creating pending configuration");

        if (thread_joiner_application_pending_config_exists(this->interface_id) &&
            thread_joiner_application_pending_config_timestamp_get(this->interface_id) >= timestamp ) {
            tr_info("Pending config is newer we reject");
            ret = -1;
            goto send_response;
        }
        if (thread_joiner_application_pending_config_create(this->interface_id, request_ptr->payload_ptr, request_ptr->payload_len) != 0) {
            tr_error("Could not create pending config");
            response_code = COAP_MSG_CODE_RESPONSE_INTERNAL_SERVER_ERROR;
            goto send_error_response;
        }

        // Start distributing pending configuration
        thread_joiner_application_pending_config_add_missing_fields(this->interface_id);
        thread_joiner_application_pending_config_timestamp_set(this->interface_id, timestamp);
        thread_joiner_application_pending_config_enable(this->interface_id, thread_delay_timer_default*1000);

    } else {
        tr_info("updating active configuration");
        ret = thread_joiner_application_update_configuration(cur->id, request_ptr->payload_ptr, request_ptr->payload_len, true);
        // link configuration is changed, read configuration again
        linkConfiguration = thread_joiner_application_get_config(cur->id);
        if (!linkConfiguration) {
            tr_error("Could not find current config");
            response_code = COAP_MSG_CODE_RESPONSE_INTERNAL_SERVER_ERROR;
            goto send_error_response;
        }
        // Activate new configuration.
        thread_configuration_thread_activate(cur, linkConfiguration);
    }
    thread_joiner_application_configuration_nvm_save(this->interface_id);

    // Start timer to propagate new data
    thread_leader_service_network_data_changed(cur, true, true);

send_response:
    // build response
    response_ptr = thread_meshcop_tlv_data_write_uint8(response_ptr, MESHCOP_TLV_STATE, ret == 0 ? 1 : 0xff);

send_error_response:
    coap_service_response_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, request_ptr, response_code, COAP_CT_OCTET_STREAM, payload, response_ptr - payload);
    return 0;
}

static int thread_leader_service_pending_set_cb(int8_t service_id, uint8_t source_address[16], uint16_t source_port, sn_coap_hdr_s *request_ptr)
{
    thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
    link_configuration_s *linkConfiguration;
    uint8_t *ptr;
    sn_coap_msg_code_e response_code = COAP_MSG_CODE_RESPONSE_CHANGED;
    uint16_t session_id;
    uint8_t payload[5]; // 4 + 1
    uint8_t *secret_ptr;
    uint64_t active_timestamp;
    uint64_t pending_timestamp;
    uint32_t delay_timer;
    (void) source_address;
    (void) source_port;
    int ret = 0;
    bool msg_from_commissioner = false;
    bool master_key_changed = false;

    ptr = payload;
    if (!this) {
        return -1;
    }
    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(this->interface_id);
    linkConfiguration = thread_joiner_application_get_config(this->interface_id);
     if(!cur || !cur->thread_info || !linkConfiguration || !cur->thread_info->leader_private_data){
        return -1;
    }

    tr_info("thread management Pending set");

    if (!thread_management_server_source_address_check(this->interface_id, source_address)) {
        response_code = COAP_MSG_CODE_RESPONSE_BAD_REQUEST;
        goto send_error_response;
    }

    if (2 <= thread_meshcop_tlv_data_get_uint16(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_COMMISSIONER_SESSION_ID, &session_id)) {
        // Session id present must be valid
        if (cur->thread_info->registered_commissioner.session_id != session_id) {
            tr_warn("Invalid commissioner session id");
            ret = -1;
            goto send_response;
        }
        thread_leader_service_commissioner_address_changed(this->interface_id, source_address);
        msg_from_commissioner = true;
    }
    if ( 16 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_NETWORK_MASTER_KEY, &secret_ptr) &&
         memcmp(linkConfiguration->master_key,secret_ptr,16) != 0) {
        tr_debug("Master key changed");
        master_key_changed = true;
    }

    if(8 > thread_meshcop_tlv_data_get_uint64(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_PENDING_TIMESTAMP, &pending_timestamp)){
        tr_warn("no timestamp");
        ret = -1;
        goto send_response;
    }
    if(4 > thread_meshcop_tlv_data_get_uint32(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_DELAY_TIMER, &delay_timer)){
        tr_warn("no delay timer");
        ret = -1;
        goto send_response;
    }
    if(8 > thread_meshcop_tlv_data_get_uint64(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_ACTIVE_TIME_STAMP, &active_timestamp)){
        tr_warn("no active timestamp");
        ret = -1;
        goto send_response;
    }
    if (active_timestamp <= linkConfiguration->timestamp && !master_key_changed) {
        tr_warn("Timestamp is lower or same, without Master key change");
        ret = -1;
        goto send_response;
    }

    if (thread_joiner_application_pending_config_exists(this->interface_id) &&
        pending_timestamp <= thread_joiner_application_pending_config_timestamp_get(this->interface_id)){
        tr_warn("Older or same pending timestamp");
        ret = -1;
        goto send_response;
    }

    if (delay_timer > THREAD_MAX_DELAY_TIMER_SECONDS*1000) {
        delay_timer = THREAD_MAX_DELAY_TIMER_SECONDS*1000;
    }

    if (master_key_changed &&
            delay_timer < thread_delay_timer_default*1000) {
        //If message Changes the master key default value is used
        delay_timer = thread_delay_timer_default*1000;
    }
    if (delay_timer < THREAD_DELAY_TIMER_MINIMAL_SECONDS*1000) {
        // Absolute minimum value allowed
        delay_timer = THREAD_DELAY_TIMER_MINIMAL_SECONDS*1000;
    }

    if (thread_joiner_application_pending_config_create(this->interface_id, request_ptr->payload_ptr, request_ptr->payload_len) != 0){
        tr_error("Could not create pending config");
        response_code = COAP_MSG_CODE_RESPONSE_INTERNAL_SERVER_ERROR;
        goto send_error_response;
    }

    if (!msg_from_commissioner) {
        tr_debug("pending set from end device");
        if (cur->thread_info->registered_commissioner.commissioner_registration != THREAD_COMMISSIONER_NOT_REGISTERED) {
            // Node changed settings and Registered commissioner send changed_notify to commissioner
            tr_info("Notify commissioner that settings are changed");
            coap_service_request_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, cur->thread_info->registered_commissioner.border_router_address,
                         THREAD_MANAGEMENT_PORT, COAP_MSG_TYPE_CONFIRMABLE, COAP_MSG_CODE_REQUEST_POST, THREAD_URI_DATASET_CHANGED,
                         COAP_CT_OCTET_STREAM, NULL, 0, NULL);
        }
    }

    tr_info("creating new pending set delay: %"PRIu32, delay_timer);
    // Activate pending configuration
    thread_joiner_application_pending_config_add_missing_fields(this->interface_id);
    thread_joiner_application_pending_config_timestamp_set(this->interface_id, pending_timestamp);
    thread_joiner_application_pending_config_enable(this->interface_id, delay_timer);

    // Start timer to propagate new data
    thread_leader_service_network_data_changed(cur, true, true);

send_response:
    // build response
    ptr = thread_meshcop_tlv_data_write_uint8(ptr, MESHCOP_TLV_STATE, ret == 0 ? 1 : 0xff);

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);
    return 0;
}

static int thread_leader_service_commissioner_set_cb(int8_t service_id, uint8_t source_address[16], uint16_t source_port, sn_coap_hdr_s *request_ptr)
{
    thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
    sn_coap_msg_code_e response_code = COAP_MSG_CODE_RESPONSE_CHANGED;
    uint16_t session_id;
    uint16_t br_locator;
    uint8_t payload[5]; // 4 + 1
    uint8_t *ptr;
    uint8_t *steering_data_ptr;
    uint16_t steering_data_len;
    (void) source_address;
    (void) source_port;
    int ret = -2;

    ptr = payload;
    if (!this) {
        return -1;
    }
    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(this->interface_id);
    if(!cur || !cur->thread_info || !cur->thread_info->leader_private_data){
        return -1;
    }

    tr_info("thread management commissioner set");

    if (!thread_management_server_source_address_check(this->interface_id, source_address)) {
        response_code = COAP_MSG_CODE_RESPONSE_BAD_REQUEST;
        goto send_error_response;
    }
    //Check if the CoAp payload is greater than maximum commissioner data size and reject
    if (request_ptr->payload_len > THREAD_MAX_COMMISSIONER_DATA_SIZE){
        tr_error("Payload length greater than maximum commissioner data size");
        ret = -1;
        goto send_response;
    }
    //TODO must be checked against a list of all non permissible TLVs
    if (2 <= thread_meshcop_tlv_data_get_uint16(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_BORDER_ROUTER_LOCATOR, &br_locator)){
        tr_error("BORDER ROUTER TLV should not be present");
        ret = -1;
        goto send_response;
    }

    // TODO must be checked against commissioner address but there is no way to get it.
    if (2 != thread_meshcop_tlv_data_get_uint16(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_COMMISSIONER_SESSION_ID, &session_id)) {
        // Session id not present must be valid
        tr_warn("Commissioner session id not present");
        ret = -1;
        goto send_response;
    }
    if (cur->thread_info->registered_commissioner.session_id != session_id) {
        tr_warn("Invalid commissioner session id");
        ret = -1;
        goto send_response;
    }

    /* Set border router address */
    thread_leader_service_commissioner_address_changed(this->interface_id, source_address);

    /* Set steering data */
    if(thread_meshcop_tlv_exist(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_STEERING_DATA)){
        steering_data_len = thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_STEERING_DATA, &steering_data_ptr);
        ret = thread_leader_service_steering_data_set(this->interface_id, steering_data_ptr, steering_data_len);
        tr_info("steering data set : %s, ret %d", trace_array(steering_data_ptr, steering_data_len), ret);
    }

send_response:
    // build response
    ptr = thread_meshcop_tlv_data_write_uint8(ptr, MESHCOP_TLV_STATE, ret == 0 ? 1 : 0xff);

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);
    return 0;
}

static void thread_leader_allocate_router_id_by_allocated_id(thread_leader_info_t *info, uint8_t router_id, const uint8_t *eui64)
{
    memcpy(info->thread_router_id_list[router_id].eui64, eui64, 8);
    bit_set(info->master_router_id_mask, router_id);
    info->thread_router_id_list[router_id].validLifeTime = 0xffffffff;
    info->thread_router_id_list[router_id].reUsePossible = false;
}

void thread_leader_mleid_rloc_map_populate(thread_nvm_mleid_rloc_map *mleid_rloc_map, thread_leader_info_t *leader_private_info)
{
    for (uint8_t i = 0; i < 64; i++) {
        if (bit_test(leader_private_info->master_router_id_mask, i)) {
            memcpy(mleid_rloc_map->mleid_rloc_map[i].mle_id, leader_private_info->thread_router_id_list[i].eui64, 8);
        }
    }
}

int thread_leader_mleid_rloc_map_to_nvm_write(protocol_interface_info_entry_t *cur)
{
    if (!cur->thread_info->leader_private_data) {
        return -1;
    }

    thread_nvm_mleid_rloc_map *mleid_rloc_map = ns_dyn_mem_temporary_alloc(sizeof(thread_nvm_mleid_rloc_map));
    if (!mleid_rloc_map) {
        return -2;
    }
    memset(mleid_rloc_map, 0, sizeof(thread_nvm_mleid_rloc_map));
    thread_leader_mleid_rloc_map_populate(mleid_rloc_map, cur->thread_info->leader_private_data);
    thread_nvm_store_mleid_rloc_map_write(mleid_rloc_map);
    ns_dyn_mem_free(mleid_rloc_map);
    return 0;
}

static int thread_leader_service_router_id_allocate(const uint8_t *eui64, protocol_interface_info_entry_t *cur, thread_leader_service_router_id_resp_t *reponse)
{
    int ret_val = -1;
    uint8_t allocated_id = 64;
    uint8_t i;
    uint8_t compareId;
    thread_leader_info_t *leader_private_ptr = 0;
    thread_info_t *thread_info = cur->thread_info;
    uint8_t activeRouterCount;

    if (!thread_info || !thread_info->leader_private_data) {
        return -1;
    }

    leader_private_ptr = thread_info->leader_private_data;
    //Check if we have already allocated router address for this MAC address
    for (i = 0; i < 64; i++) {
        if (bit_test(leader_private_ptr->master_router_id_mask, i)) {
            //Active ID
            if (memcmp(eui64, leader_private_ptr->thread_router_id_list[i].eui64, 8) == 0) {
                allocated_id = i;
                break;
            }
        }
    }

    if (allocated_id  < 64) {
        goto alloc_ready;
    }
    activeRouterCount = thread_routing_count_active_routers_from_mask(leader_private_ptr->master_router_id_mask);
    if (activeRouterCount >= cur->thread_info->testMaxActiveRouterIdLimit) {
        // Only 32 router id allowed for one Thread partition
        goto alloc_ready;
    }


    //Try to allocate the same router ID that was previously used
    if (thread_is_router_addr(reponse->shortAddress)) {
        compareId = thread_router_id_from_addr(reponse->shortAddress);
        tr_debug("HInt: %02x", compareId);
        if (!bit_test(leader_private_ptr->master_router_id_mask, compareId)) {
            //Take Free entry By Hint if valid lifetime is zero
            if (leader_private_ptr->thread_router_id_list[compareId].reUsePossible) {
                allocated_id = compareId;
            }
        }
    }

    if (allocated_id  < 64) {
        goto alloc_new;
    }

    //We need to allocate new Router ID
    uint16_t random_base = randLIB_get_random_in_range(0,64);
    // New addresses are assigned randomly
    for (i = 0; i < 64; i++) {
        uint16_t id = (random_base + i) % (MAX_ROUTER_ID + 1);
        if (!bit_test(leader_private_ptr->master_router_id_mask, id)) {
            if (leader_private_ptr->thread_router_id_list[id].reUsePossible) {
                allocated_id = id;
                // new id allocated save to nvm after delay
                leader_private_ptr->leader_nvm_sync_timer = LEADER_NVM_SYNC_DELAY;
                break;
            }
        }
    }

alloc_new:
    if (allocated_id < 64) {
        //Update Route MASK and push NEW Route Info
        thread_leader_allocate_router_id_by_allocated_id(leader_private_ptr, allocated_id, eui64);

        //Increment Route TLV data sequency
        thread_leader_service_update_id_set(cur);
    }

alloc_ready:

    if (allocated_id < 64) {
        thread_leader_allocate_router_id_by_allocated_id(leader_private_ptr, allocated_id, eui64);
        reponse->shortAddress = (allocated_id << 10);
        memcpy(reponse->routerID, leader_private_ptr->master_router_id_mask, 8);
        reponse->dataSeq = leader_private_ptr->maskSeq;
        reponse->preferredLifeTime = 0xffffffff;
        reponse->validLifeTime = 0xffffffff;
        ret_val = 0;
    }
    return ret_val;
}

static int thread_leader_service_leader_data_allocate(protocol_interface_info_entry_t *cur)
{
    thread_leader_info_t *leader_info = 0;
    leader_info = thread_allocate_and_init_leader_private_data();
    if (leader_info == NULL) {
        return -1;
    }
    cur->thread_info->leader_private_data = leader_info;
    cur->thread_info->thread_device_mode = THREAD_DEVICE_MODE_ROUTER;
    cur->thread_info->thread_attached_state = THREAD_STATE_CONNECTED_ROUTER;
    return 0;
}

static int thread_leader_service_router_id_deallocate(const uint8_t *eui64, protocol_interface_info_entry_t *cur)
{
    uint8_t i;
    thread_leader_info_t *leader_private_ptr = 0;
    thread_info_t *thread_info = cur->thread_info;
    if (thread_info && thread_info->leader_private_data) {
        leader_private_ptr = thread_info->leader_private_data;
        //Check by MAC address
        for (i = 0; i < 64; i++) {
            if (bit_test(leader_private_ptr->master_router_id_mask, i)) {
                //Active ID
                if (memcmp(eui64, leader_private_ptr->thread_router_id_list[i].eui64, 8) == 0) {
                    tr_debug("Release Router Id %d", i);
                    leader_private_ptr->leader_nvm_sync_timer = LEADER_NVM_SYNC_DELAY;
                    thread_leader_service_route_mask_bit_clear(leader_private_ptr, i);
                    leader_private_ptr->thread_router_id_list[i].reUsePossible = true;
                    leader_private_ptr->thread_router_id_list[i].validLifeTime = 0;
                    //Increment Route TLV data sequency
                    thread_leader_service_update_id_set(cur);
                    thread_routing_activate(&cur->thread_info->routing);
                    return 0;
                }
            }
        }
    }

    return -1;
}

static int thread_leader_service_routerid_assign(protocol_interface_info_entry_t *cur, const uint8_t mac[8], uint8_t router_id_mask_out[9], uint16_t *router_id)
{
    thread_leader_service_router_id_resp_t router_id_response;
    tr_debug("RId Server assign");
    router_id_response.shortAddress = *router_id; // Set Hint

    if (thread_leader_service_router_id_allocate(mac, cur, &router_id_response) != 0) {
        tr_debug("Assign Fail");
        return -1;
    }
    *router_id = router_id_response.shortAddress;
    router_id_mask_out[0] = router_id_response.dataSeq;
    memcpy(&router_id_mask_out[1], router_id_response.routerID , 8);
    return 0;
}

static int thread_leader_service_routerid_release(int8_t interface_id, uint8_t mac[8], uint16_t router_id)
{
    (void) router_id;
    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
    if (!cur) {
        tr_error("No Interface");
        return -1;
    }

    if (!cur->thread_info) {
        tr_error("Not Thread Interface");
        return -1;
    }

    if (!cur->thread_info->leader_private_data) {
        tr_error("Not Leader");
        return -1;
    }

    return thread_leader_service_router_id_deallocate(mac, cur);
}

/**
 * Router id assignment callback for leader
 * uri = tn/d/as
 */
static int thread_leader_service_assign_cb(int8_t service_id, uint8_t source_address[16], uint16_t source_port, sn_coap_hdr_s *request_ptr)
{
    thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
    uint8_t payload[24]; //!< response payload 4 + 9, 4 + 2, 4 + 1
    uint8_t *mac_ptr = NULL;
    uint8_t router_id_mask[9] = {0};
    uint16_t router_id = 0xfffe;
    uint8_t *ptr;
    uint8_t *status;
    (void) source_address;
    (void) source_port;

    if (!this) {
        return -1;
    }

    tr_debug("Thread router id assign request");
    protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(this->interface_id);

    if (!cur || !cur->thread_info) {
        return -1;
    }

    // LOCATOR is optional others are mandatory
    (void)thread_meshcop_tlv_data_get_uint16(request_ptr->payload_ptr, request_ptr->payload_len, TMFCOP_TLV_RLOC16, &router_id);

    if (8 > thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, TMFCOP_TLV_MAC_ADDRESS, &mac_ptr) ) {
        // Failure in message
        return -1;
    }

        // Failure in message
    if (1 > thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, TMFCOP_TLV_STATUS, &status) ) {
        return -1;
    }

    int ret = -1;
    if (*status == THREAD_COAP_STATUS_TLV_TOO_FEW_ROUTERS) {
        uint8_t activeRouterCount = 0;
        if (cur->thread_info->leader_private_data) {
            activeRouterCount = thread_routing_count_active_routers_from_mask(cur->thread_info->leader_private_data->master_router_id_mask);
        }

        if (activeRouterCount < cur->thread_info->routerSelectParameters.routerUpgradeThresHold) {
            ret = thread_leader_service_routerid_assign(cur, mac_ptr, router_id_mask, &router_id);
        } else {
            tr_info("Router ID not assigned, routers %d >= %d(upgrade threshold)", activeRouterCount, cur->thread_info->routerSelectParameters.routerUpgradeThresHold);
        }
    } else {
        ret = thread_leader_service_routerid_assign(cur, mac_ptr, router_id_mask, &router_id);
    }

    ptr = payload;
    if (ret == 0) {
        tr_debug("Thread router id mac %s, mask %s, locator %x", trace_array(mac_ptr, 8), trace_array(router_id_mask, 9), router_id);
        ptr = thread_tmfcop_tlv_data_write_uint8(ptr, TMFCOP_TLV_STATUS, THREAD_COAP_STATUS_TLV_SUCCESS);
        ptr = thread_tmfcop_tlv_data_write_uint16(ptr, TMFCOP_TLV_RLOC16, router_id);
        ptr = thread_tmfcop_tlv_data_write(ptr, TMFCOP_TLV_ROUTER_MASK, 9, router_id_mask);
    } else {
        ptr = thread_tmfcop_tlv_data_write_uint8(ptr, TMFCOP_TLV_STATUS, THREAD_COAP_STATUS_TLV_NO_ADDRESS_AVAILABLE);
    }

    coap_service_response_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, request_ptr, COAP_MSG_CODE_RESPONSE_CHANGED, COAP_CT_OCTET_STREAM, payload, ptr - payload);
    return 0;
}

/**
 * Router id assignment callback for leader
 * uri = tn/d/ar
 */
static int thread_leader_service_release_cb(int8_t service_id, uint8_t source_address[16], uint16_t source_port, sn_coap_hdr_s *request_ptr)
{
    thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
    uint8_t payload[5]; //!< response payload 4 + 1
    uint8_t *mac_ptr;
    uint16_t router_id;
    uint8_t *ptr;
    (void) source_address;
    (void) source_port;

    if (!this) {
        return -1;
    }

    tr_debug("Thread router id release request");
    if (2 > thread_meshcop_tlv_data_get_uint16(request_ptr->payload_ptr, request_ptr->payload_len, TMFCOP_TLV_RLOC16, &router_id)) {
        return -1;
    }

    if (8 > thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, TMFCOP_TLV_MAC_ADDRESS, &mac_ptr)) {
        // Failure in message
        return -1;
    }
    tr_debug("Thread router release id mac %s, locator %x", trace_array(mac_ptr, 8), router_id);

    thread_leader_service_routerid_release(this->interface_id, mac_ptr, router_id);

    ptr = payload;
    ptr = thread_tmfcop_tlv_data_write_uint8(ptr, TMFCOP_TLV_STATUS, 0);
    coap_service_response_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, request_ptr, COAP_MSG_CODE_RESPONSE_CHANGED, COAP_CT_OCTET_STREAM, payload, ptr - payload);

    return 0;
}

/**
 * Thread commissioning petition handler
 * uri = tn/mc/lp
 */
static int thread_leader_service_petition_cb(int8_t service_id, uint8_t source_address[16], uint16_t source_port, sn_coap_hdr_s *request_ptr)
{
    thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
    sn_coap_msg_code_e response_code = COAP_MSG_CODE_RESPONSE_CHANGED;
    uint8_t payload[79];// max length for commissioner id is 64 + 4 byte header + 4 + 1 + 4 + 2
    uint8_t *ptr;
    uint16_t session_id = 0;
    uint8_t *tlv_data_ptr = NULL;
    uint16_t tlv_length;
    char *commissioner_id_ptr = NULL;

    (void) source_port;

    ptr = payload;
    if (!this) {
        return -1;
    }

    tr_debug("Thread management commissioner petition");

    if (!thread_management_server_source_address_check(this->interface_id, source_address)) {
        response_code = COAP_MSG_CODE_RESPONSE_BAD_REQUEST;
        goto send_error_response;
    }

    // save values from message
    tlv_length = thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_COMMISSIONER_ID, &tlv_data_ptr);

    // register to thread interface
    int ret = thread_leader_service_commissioner_register(this->interface_id, source_address, tlv_data_ptr, tlv_length, &session_id);

    // Build response
    if (ret == 0) {
        // Register OK
        ptr = thread_meshcop_tlv_data_write_uint8(ptr, MESHCOP_TLV_STATE, 1);
        ptr = thread_meshcop_tlv_data_write(ptr, MESHCOP_TLV_COMMISSIONER_ID, tlv_length, tlv_data_ptr);
        ptr = thread_meshcop_tlv_data_write_uint16(ptr, MESHCOP_TLV_COMMISSIONER_SESSION_ID, session_id);
    } else if (ret == -2) {
        // Commissioner already registered - reject and send commissioner ID
        ptr = thread_meshcop_tlv_data_write_uint8(ptr, MESHCOP_TLV_STATE, 0xff);
        commissioner_id_ptr = thread_commissioning_if_commissioner_id_get(this->interface_id);
        if(commissioner_id_ptr)
            ptr = thread_meshcop_tlv_data_write(ptr, MESHCOP_TLV_COMMISSIONER_ID, strlen(commissioner_id_ptr), (uint8_t *)commissioner_id_ptr);
    }
    else{
        // Reject, anyhow, no commissioner registered - no commissioner ID
        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 ? 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);
    return 0;
}

/**
 * Thread commissioning keep alive handler
 * uri = tn/mc/la
 */
static int thread_leader_service_petition_ka_cb(int8_t service_id, uint8_t source_address[16], uint16_t source_port, sn_coap_hdr_s *request_ptr)
{
    thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
    sn_coap_msg_code_e response_code = COAP_MSG_CODE_RESPONSE_CHANGED;
    uint8_t payload[5]; //status 4 + 1
    uint8_t *ptr;
    uint16_t session_id = 0;
    bool state = false;

    (void) source_address;
    (void) source_port;

    if (!this) {
        return -1;
    }

    tr_debug("Thread management commissioner keep alive");

    if (!thread_management_server_source_address_check(this->interface_id, source_address)) {
        response_code = COAP_MSG_CODE_RESPONSE_BAD_REQUEST;
        ptr = payload;
        goto send_error_response;
    }

    if (2 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_COMMISSIONER_SESSION_ID, &ptr)) {
        session_id = common_read_16_bit(ptr);
    }
    if (1 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_STATE, &ptr))
    {
        if (*ptr == 0x01) {
            state = thread_leader_service_commissioner_session_refresh(this->interface_id, session_id);
        } else if(*ptr == 0xff) {
            thread_leader_service_commissioner_unregister(this->interface_id, session_id);
            state = false;
        }
    }
    if (state == true) {
        thread_leader_service_commissioner_address_changed(this->interface_id, source_address);
    }
    tr_info("Commissioner keep alive req recv, interface: %d session id %d state %d", this->interface_id, session_id, state);
    ptr = payload;
    ptr = thread_meshcop_tlv_data_write_uint8(ptr, MESHCOP_TLV_STATE, state == true ? 1 : 0xff);

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);
    return 0;
}

static int thread_leader_service_network_data_register(protocol_interface_info_entry_t *cur, uint8_t *network_data_ptr, uint16_t network_data_length, uint16_t routerId)
{
    tr_debug("Register network data for %x", routerId);
    // Mark all data to be cleared
    thread_network_data_router_id_mark_delete(&cur->thread_info->networkDataStorage, routerId, true);

    // update data from this device
    int retVal = thread_bootstrap_network_data_process(cur, network_data_ptr, network_data_length);

    if (retVal < 0) {
        tr_warn("network data update failed: %s", trace_array(network_data_ptr, network_data_length));
        return retVal;
    }
    thread_network_data_router_id_free(&cur->thread_info->networkDataStorage, true, cur);

    thread_leader_service_network_data_changed(cur, false, false);
    return 0;
}

/**
 * Thread network data registration handler
 * uri = tn/sd
 */
static int thread_leader_service_register_cb(int8_t service_id, uint8_t source_address[16], uint16_t source_port, sn_coap_hdr_s *request_ptr)
{
    thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
    uint16_t payload_len;
    uint8_t *payload_ptr;
    uint16_t network_data_len;
    uint8_t *network_data_ptr;
    protocol_interface_info_entry_t *cur;
    sn_coap_msg_code_e response_code = COAP_MSG_CODE_RESPONSE_CHANGED;
    int8_t ret = -3;
    uint16_t routerId;
    uint16_t old_rloc;
    (void) source_port;
    tr_debug("register network data");

    if (!this) {
        return -1;
    }

    cur = protocol_stack_interface_info_get_by_id(this->interface_id);
    if (!cur || !cur->thread_info) {
        return -1;
    }

    //send data to leader api.
    routerId = common_read_16_bit(&source_address[14]);
    payload_len = request_ptr->payload_len;
    payload_ptr = request_ptr->payload_ptr;
    request_ptr->payload_ptr = NULL;


    if (2 <= thread_tmfcop_tlv_data_get_uint16(payload_ptr, payload_len, TMFCOP_TLV_RLOC16, &old_rloc)) {
        // This will delete everything from old rloc
        tr_warn("Remove network data from: %x", old_rloc);
        ret = thread_leader_service_network_data_register(cur, NULL, 0, old_rloc);
    }

    if (!thread_tmfcop_tlv_exist(payload_ptr, payload_len, TMFCOP_TLV_NETWORK_DATA)) {
        goto send_response;
    }
    network_data_len = thread_tmfcop_tlv_find(payload_ptr, payload_len, TMFCOP_TLV_NETWORK_DATA, &network_data_ptr);

    /* Check that network data does NOT produce Network Data TLV bigger than MAX_NETWORK_DATA_SIZE */
    int32_t len = thread_network_data_resulting_tlv_size(&cur->thread_info->networkDataStorage, network_data_ptr, network_data_len, routerId);

    if (len <= 0 || len > THREAD_MAX_NETWORK_DATA_SIZE) {
        tr_warn("Network data would produce Network Data TLV size %"PRId32, len);
        response_code = COAP_MSG_CODE_RESPONSE_BAD_REQUEST;
        goto send_response;
    }

    if (network_data_len < 256) {
        ret = thread_leader_service_network_data_register(cur, network_data_ptr, network_data_len, routerId);
    }
    if (ret != 0) {
        /* error happened*/
        response_code = COAP_MSG_CODE_RESPONSE_BAD_REQUEST; // corruption is the default or not enough room
        if (ret == -2) {
            response_code = COAP_MSG_CODE_RESPONSE_FORBIDDEN;    // Not leader
        }
    }

send_response:
    coap_service_response_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, request_ptr, response_code, COAP_CT_OCTET_STREAM, NULL, 0);
    return 0;
}

static int thread_leader_service_leader_start(protocol_interface_info_entry_t *cur)
{
    link_configuration_s *linkConfiguration;
    linkConfiguration = thread_joiner_application_get_config(cur->id);
    if (!linkConfiguration) {
        return -1;
    }

    //Allocate Setup
    if (thread_leader_service_leader_data_allocate(cur) != 0) {
        return -1;
    }

    if (thread_leader_service_start(cur->id)) {
        thread_nd_service_delete(cur->id);
        return -1;
    }
    if (thread_beacon_create_payload(cur) != 0) {
        thread_nd_service_delete(cur->id);
        return -1;
    }
    cur->thread_info->rfc6775 = false;
    cur->thread_info->leader_private_data->leader_id_seq_timer = ID_SEQUENCE_PERIOD;

    cur->lowpan_info &= ~INTERFACE_NWK_BOOTSRAP_PANA_AUTHENTICATION;
    cur->lowpan_info |= INTERFACE_NWK_ROUTER_DEVICE;
    cur->lowpan_info &= ~INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE;
    mac_helper_default_security_level_set(cur, SEC_ENC_MIC32);
    mac_helper_default_security_key_id_mode_set(cur,MAC_KEY_ID_MODE_IDX);
    thread_discovery_responser_enable(cur->id, true);
    if (cur->mac_api) {
        mlme_start_t start_req;
        memset(&start_req, 0, sizeof(mlme_start_t));
        start_req.BeaconOrder = 15;
        start_req.SuperframeOrder = 15;
        start_req.PANCoordinator = true;
        start_req.LogicalChannel = cur->mac_parameters->mac_channel;
        start_req.PANId = cur->mac_parameters->pan_id;
        cur->mac_api->mlme_req(cur->mac_api, MLME_START , &start_req);
    }
    return 0;
}

static int thread_leader_service_leader_init(protocol_interface_info_entry_t *cur)
{
    //Clean All allocated stuff's
    thread_info_t *thread_info = cur->thread_info;
    // mark and delete previous leader network data information
    thread_network_data_router_id_mark_delete(&thread_info->networkDataStorage,thread_router_addr_from_id(cur->thread_info->thread_leader_data->leaderRouterId), true);
    thread_network_data_router_id_free(&thread_info->networkDataStorage, false, cur);
    thread_leader_service_leader_data_free(thread_info);

    thread_info->rfc6775 = false;
    thread_routing_free(&thread_info->routing);
    ipv6_route_table_remove_info(cur->id, ROUTE_THREAD, NULL);
    ipv6_route_table_remove_info(cur->id, ROUTE_THREAD_BORDER_ROUTER, NULL);
    thread_dhcp_client_delete(cur->id);
    thread_nd_service_delete(cur->id);
    mpl_clear_realm_scope_seeds(cur);
    ipv6_neighbour_cache_flush(&cur->ipv6_neighbour_cache);
    thread_neighbor_list_clean(cur);
    cur->mesh_callbacks = NULL;
    cur->lowpan_info &= ~INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE;

    return thread_leader_service_leader_start(cur);
}

static void thread_leader_service_leader_data_initialize(protocol_interface_info_entry_t *cur, uint8_t routerId)
{
    thread_leader_data_t *leader_data = cur->thread_info->thread_leader_data;

    if (cur->thread_info->thread_leader_data) {
        leader_data->partitionId = randLIB_get_32bit(); //Generate Random Instance
        leader_data->leaderRouterId = routerId; //Set leader data to zero by Default
        leader_data->dataVersion = randLIB_get_8bit();
        leader_data->stableDataVersion = randLIB_get_8bit();
        leader_data->weighting = cur->thread_info->partition_weighting;
    }
}

static void thread_leader_service_private_routemask_init(thread_leader_info_t *leader_info)
{
    memset(leader_info, 0, sizeof(thread_leader_info_t));
    leader_info->maskSeq = randLIB_get_8bit();
    uint8_t i;
    for (i=0; i<64; i++) {
        leader_info->thread_router_id_list[i].reUsePossible = true;
        leader_info->thread_router_id_list[i].validLifeTime = 0xffffffff;
    }
}

static void thread_leader_service_interface_setup_activate(protocol_interface_info_entry_t *cur)
{
    uint8_t routerId;
    thread_leader_info_t *private = cur->thread_info->leader_private_data;

    routerId = thread_router_id_from_addr(cur->thread_info->routerShortAddress);

    thread_leader_service_leader_data_initialize(cur, routerId);
    // Test code
    if(cur->thread_info->testRandomPartitionId != 0){
        cur->thread_info->thread_leader_data->partitionId = cur->thread_info->testRandomPartitionId;
        cur->thread_info->testRandomPartitionId = 0;
    }
    thread_leader_service_private_routemask_init(private);
    //SET Router ID
    thread_leader_allocate_router_id_by_allocated_id(private, routerId, cur->mac);
    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;
    thread_bootstrap_update_ml16_address(cur, cur->thread_info->routerShortAddress);
    thread_generate_ml64_address(cur);
    thread_extension_address_generate(cur);
    thread_bootstrap_routing_activate(cur);
    thread_routing_update_id_set(cur, private->maskSeq, private->master_router_id_mask);
    thread_routing_activate(&cur->thread_info->routing);
    cur->thread_info->leader_private_data->leader_id_seq_timer = ID_SEQUENCE_PERIOD;
}

/*
 * Function to clear network data from router children.
 * When router is removed, network data from router child must be removed as well.
 */
static void thread_leader_router_child_network_data_clear(protocol_interface_info_entry_t *cur, uint16_t router_short_addr)
{
    uint16_t child_short_addr;

    child_short_addr = thread_leader_service_child_id_from_networkdata_get(cur->thread_info, router_short_addr);
    while (child_short_addr != 0) {
        tr_debug("Removing Network data from child %04x", child_short_addr);
        thread_leader_service_network_data_register(cur, NULL, 0, child_short_addr);
        child_short_addr = thread_leader_service_child_id_from_networkdata_get(cur->thread_info, router_short_addr);
    }
}


/* Public functions */

int thread_leader_service_init(int8_t interface_id, uint8_t coap_service_id)
{
    thread_leader_service_t *this = thread_leader_service_find_by_interface(interface_id);

    if (this) {
        return 0;
    }

    this = ns_dyn_mem_alloc(sizeof(thread_leader_service_t));
    if (!this) {
        return -2;
    }

    this->interface_id = interface_id;
    this->coap_service_id = coap_service_id;
    this->leader_enabled = false;

    ns_list_add_to_start(&leader_service_instance_list, this);

    return 0;
}

void thread_leader_service_delete(int8_t interface_id)
{
    thread_leader_service_t *this = thread_leader_service_find_by_interface(interface_id);
    if (!this) {
        return;
    }

    if (this->leader_enabled) {
        thread_leader_service_stop(interface_id);
    }

    ns_list_remove(&leader_service_instance_list, this);
    ns_dyn_mem_free(this);
}

int thread_leader_service_start(int8_t interface_id)
{
    thread_leader_service_t *this = thread_leader_service_find_by_interface(interface_id);

    if (!this) {
        return -1;
    }

    this->leader_enabled = true;
    /**
     * Router id uri register
     */
    coap_service_register_uri(this->coap_service_id, THREAD_URI_ROUTER_ID_ASSIGNMENT, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_assign_cb);
    coap_service_register_uri(this->coap_service_id, THREAD_URI_ROUTER_ID_RELEASE, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_release_cb);
    /**
     * Thread commission registration uri handling
     */
    coap_service_register_uri(this->coap_service_id, THREAD_URI_LEADER_PETITION, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_petition_cb);
    coap_service_register_uri(this->coap_service_id, THREAD_URI_LEADER_KEEP_ALIVE, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_petition_ka_cb);
    /**
     * Thread network data management handler
     */
    coap_service_register_uri(this->coap_service_id, THREAD_URI_NETWORK_DATA, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_register_cb);
    coap_service_register_uri(this->coap_service_id, THREAD_URI_ACTIVE_SET, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_active_set_cb);
    coap_service_register_uri(this->coap_service_id, THREAD_URI_PENDING_SET, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_pending_set_cb);
    coap_service_register_uri(this->coap_service_id, THREAD_URI_COMMISSIONER_SET, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_commissioner_set_cb);

    return 0;
}

void thread_leader_service_stop(int8_t interface_id)
{
    thread_leader_service_t *this = thread_leader_service_find_by_interface(interface_id);

    if (!this) {
        return;
    }

    this->leader_enabled = false;
    coap_service_unregister_uri(this->coap_service_id, THREAD_URI_ROUTER_ID_ASSIGNMENT);
    coap_service_unregister_uri(this->coap_service_id, THREAD_URI_ROUTER_ID_RELEASE);
    coap_service_unregister_uri(this->coap_service_id, THREAD_URI_LEADER_PETITION);
    coap_service_unregister_uri(this->coap_service_id, THREAD_URI_LEADER_KEEP_ALIVE);
    coap_service_unregister_uri(this->coap_service_id, THREAD_URI_NETWORK_DATA);
    coap_service_unregister_uri(this->coap_service_id, THREAD_URI_ACTIVE_SET);
    coap_service_unregister_uri(this->coap_service_id, THREAD_URI_PENDING_SET);
    coap_service_unregister_uri(this->coap_service_id, THREAD_URI_COMMISSIONER_SET);
    return;
}
void thread_leader_service_network_data_changed(protocol_interface_info_entry_t *cur, bool force_stable_update, bool force_unstable_update)
{
    if (!cur->thread_info) {
        return;
    }

    if (force_stable_update) {
        cur->thread_info->networkDataStorage.stableUpdatePushed = true;
    }
    if (force_unstable_update) {
        cur->thread_info->networkDataStorage.temporaryUpdatePushed = true;
    }
    // Generate network data from network data structures
    if (cur->thread_info->networkDataStorage.stableUpdatePushed) {
        tr_debug("Temporary Network Data Update");
        cur->thread_info->thread_leader_data->stableDataVersion++;
        cur->thread_info->thread_leader_data->dataVersion++;
    } else if (cur->thread_info->networkDataStorage.temporaryUpdatePushed) {
        tr_debug("Stable Network Data Update");
        cur->thread_info->thread_leader_data->dataVersion++;
    } else {
        // No changes in network data detected
        return;
    }
    tr_debug("network data changed stable:%d, unstable:%d", cur->thread_info->networkDataStorage.stableUpdatePushed, cur->thread_info->networkDataStorage.temporaryUpdatePushed);
    thread_leader_service_generate_network_data(cur);

    /* set timer to update new network data */
    if (0 == thread_info(cur)->networkDataStorage.network_data_update_delay) {
        thread_info(cur)->networkDataStorage.network_data_update_delay = THREAD_COMMISSION_DATA_UPDATE_DELAY;
    }
}

void thread_leader_service_timer(protocol_interface_info_entry_t *cur, uint32_t ticks)
{
    if (!cur->thread_info->leader_private_data) {
        // I am not a leader
        return;
    }

    // check for network data update
    if (thread_info(cur)->networkDataStorage.network_data_update_delay) {
        if (thread_info(cur)->networkDataStorage.network_data_update_delay > ticks) {
            thread_info(cur)->networkDataStorage.network_data_update_delay -= ticks;
        } else {
            // Send New network data.
            thread_info(cur)->networkDataStorage.network_data_update_delay = 0;
            thread_bootstrap_network_data_update(cur);
        }
    }

    if (cur->thread_info->leader_private_data->leader_id_seq_timer) {
        if (cur->thread_info->leader_private_data->leader_id_seq_timer > ticks) {
            cur->thread_info->leader_private_data->leader_id_seq_timer -= ticks;
        } else {
            cur->thread_info->leader_private_data->leader_id_seq_timer = 0;
        }
    } else  {
        // Leader id sequence number updated
        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->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);
    }

    if (cur->thread_info->leader_private_data->leader_nvm_sync_timer) {
        if ((cur->thread_info->leader_private_data->leader_nvm_sync_timer) > ticks) {
            cur->thread_info->leader_private_data->leader_nvm_sync_timer -= ticks;
        }
        else {
            cur->thread_info->leader_private_data->leader_nvm_sync_timer = 0;
            thread_leader_mleid_rloc_map_to_nvm_write(cur);
        }
    }

    thread_leader_service_router_id_valid_lifetime_update(cur, ticks);

}

void thread_leader_service_leader_data_free(thread_info_t *thread_info)
{
    if (thread_info && thread_info->thread_leader_data) {
        ns_dyn_mem_free(thread_info->leader_private_data);
        thread_info->leader_private_data = NULL;
    }
}

void thread_leader_service_thread_partitition_generate(int8_t interface_id, bool newPartition)
{
    protocol_interface_info_entry_t *cur;
    link_configuration_s *linkConfiguration = thread_joiner_application_get_config(interface_id);
    uint8_t *parent_mac_addr = NULL;
    uint16_t leaderRloc;

    cur = protocol_stack_interface_info_get_by_id(interface_id);
    if (!cur || !cur->thread_info) {
        return;
    }

    if (!linkConfiguration) {
        goto failure_exit;
    }

    mle_service_interface_tx_queue_clean(interface_id);

    if (!newPartition) {
        if (cur->nwk_bootstrap_state == ER_ACTIVE_SCAN && cur->nwk_nd_re_scan_count < THREAD_NETWORK_ACTIVE_SCAN_COUNTER) {
            tr_debug("RE trig Active Scan");
            cur->bootsrap_state_machine_cnt = randLIB_get_random_in_range(1, 6);
            return;
        }
    }

    if (cur->thread_info->thread_device_mode != THREAD_DEVICE_MODE_ROUTER) {
        tr_debug("End device did not find networks");
        goto failure_exit;
    }
    leaderRloc = cur->thread_info->routerShortAddress;

    //Set For Destination cache clean purpose
    cur->mac_parameters->pan_id = linkConfiguration->panId;

    if (thread_leader_service_leader_init(cur) != 0) {
        tr_debug("Leader allocate Fail");
        goto failure_exit;
    }

    /* Delete old parent info */
    if (cur->thread_info->thread_endnode_parent) {
        ns_dyn_mem_free(cur->thread_info->thread_endnode_parent);
        cur->thread_info->thread_endnode_parent = NULL;
    }

    tr_debug("Leader Link generated & Started");

    //Run Nomal Init
    thread_interface_init(cur);

    // Init clears the address
    cur->thread_info->routerShortAddress = leaderRloc;

    if (cur->thread_info->routerShortAddress == 0xfffe ||
        !thread_is_router_addr(cur->thread_info->routerShortAddress)) {
        //If we were REED do not use reed address
        cur->thread_info->routerShortAddress = thread_router_addr_from_id(randLIB_get_random_in_range(0,MAX_ROUTER_ID));
    }

    // clean network data
    thread_network_data_free_and_clean(&cur->thread_info->networkDataStorage);
    thread_network_data_base_init(&cur->thread_info->networkDataStorage);
    thread_leader_service_interface_setup_activate(cur);

    //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;
    mac_helper_pib_boolean_set(cur, macRxOnWhenIdle, true);
    cur->interface_mode = INTERFACE_UP;
    cur->nwk_mode = ARM_NWK_GP_IP_MODE;
    thread_bootstrap_ready(cur);
    thread_configuration_mle_activate(cur);
    if (cur->thread_info->thread_endnode_parent) {
        parent_mac_addr = cur->thread_info->thread_endnode_parent->mac64;
    }
    thread_nvm_store_link_info_write(parent_mac_addr, mac_helper_mac16_address_get(cur));

    if (thread_nd_own_service_list_data_size(&cur->thread_info->localServerDataBase)) {
        // We publish our services if we have some BUG leader cannot remove old ones
        thread_border_router_publish(cur->id);
    }
    return;

failure_exit:
    nwk_bootsrap_state_update(ARM_NWK_NWK_SCAN_FAIL, cur);
}

int thread_leader_service_thread_partitition_restart(int8_t interface_id, mle_tlv_info_t *routing)
{
    protocol_interface_info_entry_t *cur;
    cur = protocol_stack_interface_info_get_by_id(interface_id);
    if (!cur || !cur->thread_info) {
        return -1;
    }
    if (!thread_info(cur)->leader_private_data) {
        thread_info(cur)->leader_private_data = thread_allocate_and_init_leader_private_data();
    }
    if (!thread_info(cur)->leader_private_data) {
        return -1;
    }
    if (thread_leader_service_start(cur->id)) {
        return -1;
    }
    //Learn network data, we remove own data from here it should be re given by application
    //thread_management_network_data_register(cur->id, networkData.dataPtr, networkData.tlvLen, address16 );
    thread_nvm_mleid_rloc_map *mleid_rloc_map = ns_dyn_mem_temporary_alloc(sizeof(thread_nvm_mleid_rloc_map));
    if (!mleid_rloc_map) {
        return -1;
    }
    if (thread_nvm_store_mleid_rloc_map_read(mleid_rloc_map) != THREAD_NVM_FILE_SUCCESS) {
        memset(mleid_rloc_map, 0, sizeof(thread_nvm_mleid_rloc_map));
    }
    // initialize private data
    thread_info(cur)->leader_private_data->maskSeq = *routing->dataPtr;
    memcpy(thread_info(cur)->leader_private_data->master_router_id_mask,routing->dataPtr + 1, 8);
    for (int i = 0; i < 64; i++) {
        if (bit_test(thread_info(cur)->leader_private_data->master_router_id_mask, i)) {
            //Active ID
            thread_info(cur)->leader_private_data->thread_router_id_list[i].reUsePossible = false;
            memcpy(thread_info(cur)->leader_private_data->thread_router_id_list[i].eui64, mleid_rloc_map->mleid_rloc_map[i].mle_id, 8);
        } else {
            // Free id
            thread_info(cur)->leader_private_data->thread_router_id_list[i].reUsePossible = true;
            // clear the mleid in both local router id list and nvm
            memset(thread_info(cur)->leader_private_data->thread_router_id_list[i].eui64, 0, 8);
            memset(mleid_rloc_map->mleid_rloc_map[i].mle_id, 0, 8);
        }
        thread_info(cur)->leader_private_data->thread_router_id_list[i].validLifeTime = 0xffffffff;
    }
    // write back updated map to store
    thread_nvm_store_mleid_rloc_map_write(mleid_rloc_map);
    ns_dyn_mem_free(mleid_rloc_map);
    // Clear network data (if exists) and propagate new empty network data
    thread_network_data_free_and_clean(&cur->thread_info->networkDataStorage);
    thread_network_data_base_init(&cur->thread_info->networkDataStorage);
    // Update router sequence id to prevent network fragmentation in case of Leader was temporarily down
    // and routers were not able to get new sequence id for NETWORK_ID_TIMEOUT duration.
    thread_leader_service_update_id_set(cur);
    return 0;
}

void thread_leader_service_update_id_set(protocol_interface_info_entry_t *cur)
{
    thread_leader_info_t *leader_private_ptr = cur->thread_info->leader_private_data;

    leader_private_ptr->maskSeq++;
    thread_routing_update_id_set(cur, leader_private_ptr->maskSeq, leader_private_ptr->master_router_id_mask);
}

void thread_leader_service_router_id_valid_lifetime_update(protocol_interface_info_entry_t *cur, uint32_t tickUpdate)
{
    uint8_t i;
    bool update_routeTable = false;
    thread_info_t *thread_info = cur->thread_info;
    thread_leader_info_t *leader_private_ptr = 0;
    if (thread_info && thread_info->leader_private_data) {
        leader_private_ptr = thread_info->leader_private_data;

        for (i = 0; i < 64; i++) {
            if (bit_test(leader_private_ptr->master_router_id_mask, i)) {
                if (i != thread_info->thread_leader_data->leaderRouterId) {
                    //Active ID
                    if (leader_private_ptr->thread_router_id_list[i].validLifeTime != 0xffffffff) {
                        if (leader_private_ptr->thread_router_id_list[i].validLifeTime > tickUpdate) {
                            //Increment Time
                            leader_private_ptr->thread_router_id_list[i].validLifeTime -= tickUpdate;
                        } else {
                            //RElease
                            tr_debug("Router ID bit Release: %x start re assign delay", i);
                            leader_private_ptr->thread_router_id_list[i].validLifeTime = ROUTER_ID_REUSE_DELAY;
                            leader_private_ptr->thread_router_id_list[i].reUsePossible = false;
                            thread_leader_service_route_mask_bit_clear(leader_private_ptr, i);
                            thread_leader_service_network_data_register(cur, NULL, 0, thread_router_addr_from_id(i));
                            thread_leader_router_child_network_data_clear(cur, thread_router_addr_from_id(i));
                            thread_routing_activate(&thread_info->routing);
                            update_routeTable = true;
                        }
                    }
                }
            } else {
                if (leader_private_ptr->thread_router_id_list[i].validLifeTime != 0xffffffff) {
                    if (leader_private_ptr->thread_router_id_list[i].validLifeTime > tickUpdate) {
                        //Increment Time
                        leader_private_ptr->thread_router_id_list[i].validLifeTime -= tickUpdate;
                    } else {
                        //RElease
                        tr_debug("Router ID available again: %x", i);
                        leader_private_ptr->thread_router_id_list[i].validLifeTime = 0xffffffff;
                        leader_private_ptr->thread_router_id_list[i].reUsePossible = true;
                    }
                }

            }
        }
        if (update_routeTable) {
            thread_leader_service_update_id_set(cur);
        }
    }
}

bool thread_leader_service_route_mask_bit_clear(thread_leader_info_t *info, uint8_t router_id)
{
    if (bit_test(info->master_router_id_mask, router_id)) {
        bit_clear(info->master_router_id_mask, router_id);
        return true;
    }
    return false;
}

static uint8_t thread_unstable_commission_data_length(protocol_interface_info_entry_t *cur)
{
    uint8_t localDataLength = 0;

    localDataLength += 4; // Commissioner session ID

    if (cur->thread_info->registered_commissioner.commissioner_valid) {
        localDataLength += 4; // Border router address
        if (cur->thread_info->registered_commissioner.steering_data_len) {
            localDataLength += cur->thread_info->registered_commissioner.steering_data_len;
            localDataLength += 2;
        }
    }
    return localDataLength;
}

static uint8_t *thread_unstable_commission_data_write(protocol_interface_info_entry_t *cur, uint8_t *ptr)
{

    uint8_t length = thread_unstable_commission_data_length(cur);
    if( !length) {
        // No unstable TLV in thread 1.0 or if TLV is empty we skip it
        return ptr;
    }
    *ptr++ = THREAD_NWK_DATA_TYPE_COMMISSION_DATA ;
    *ptr++ = length;
    ptr = thread_meshcop_tlv_data_write_uint16(ptr,MESHCOP_TLV_COMMISSIONER_SESSION_ID, cur->thread_info->registered_commissioner.session_id);
    if (cur->thread_info->registered_commissioner.commissioner_valid) {
        //SET Commision TLV
        ptr = thread_meshcop_tlv_data_write(ptr,MESHCOP_TLV_BORDER_ROUTER_LOCATOR, 2, &cur->thread_info->registered_commissioner.border_router_address[14]);
        if (cur->thread_info->registered_commissioner.steering_data_len) {
            ptr = thread_meshcop_tlv_data_write(ptr,MESHCOP_TLV_STEERING_DATA, cur->thread_info->registered_commissioner.steering_data_len, cur->thread_info->registered_commissioner.steering_data);
        }
    }
    return ptr;
}

static uint16_t thread_leader_service_network_data_length(protocol_interface_info_entry_t *cur, bool fullist)
{
    uint16_t networkDataLength = 0;
    uint16_t prefixDataLength = 0;
    uint16_t commissionDataLength = 0;

    prefixDataLength = thread_network_data_prefix_set_size(&cur->thread_info->networkDataStorage, fullist);
    if (prefixDataLength) {
        networkDataLength += prefixDataLength;
    }
    prefixDataLength = thread_network_data_service_set_size(&cur->thread_info->networkDataStorage, fullist);
    if (prefixDataLength) {
        networkDataLength += prefixDataLength;
    }
    commissionDataLength = thread_unstable_commission_data_length(cur);
    if (fullist && commissionDataLength) {
        networkDataLength += 2 + commissionDataLength;
    }

    return networkDataLength;
}

static uint8_t *thread_leader_service_network_data_tlv_write(protocol_interface_info_entry_t *cur, uint8_t *ptr)
{
    ptr = thread_network_data_prefix_set_write(&cur->thread_info->networkDataStorage, ptr);
    ptr = thread_network_data_service_set_write(&cur->thread_info->networkDataStorage, ptr);
    ptr = thread_unstable_commission_data_write(cur, ptr);

    return ptr;
}

void thread_leader_service_generate_network_data(protocol_interface_info_entry_t *cur)
{
    int16_t size = thread_leader_service_network_data_length(cur, true);

    if (size > 0) {
        tr_debug("Leader network data size %d", size);
        thread_leader_service_network_data_tlv_write(cur, cur->thread_info->networkDataStorage.network_data);
        cur->thread_info->networkDataStorage.network_data_len = size;
    }
}

uint16_t thread_leader_service_child_id_from_networkdata_get(thread_info_t *thread_info, uint16_t router_short_addr)
{
    return thread_network_data_service_child_id_from_networkdata_get(&thread_info->networkDataStorage, router_short_addr);
}

void thread_leader_service_router_state_changed(thread_info_t *thread_info, uint8_t router_id, bool available, int8_t interface_id)
{
    thread_leader_info_t *info = thread_info->leader_private_data;
    (void)interface_id;

    if (!info) {
        return;
    }
    if (available) {
        /* Stop the timer */
        info->thread_router_id_list[router_id].validLifeTime = 0xffffffff;
    } else {
        /* Reset the timer to ROUTER_ID_REUSE_DELAY, if it's not already running. */
        if (info->thread_router_id_list[router_id].validLifeTime == 0xffffffff) {
            tr_debug("Router ID: %d cost infinite", router_id);

            info->thread_router_id_list[router_id].validLifeTime = ROUTER_ID_INFINITY_DELAY;
            info->thread_router_id_list[router_id].reUsePossible = false;
        }
    }
}

#endif // HAVE_THREAD_LEADER_SERVICE */
