thread_leader_service.c

00001 /*
00002  * Copyright (c) 2016-2017, Arm Limited and affiliates.
00003  * SPDX-License-Identifier: BSD-3-Clause
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  * 1. Redistributions of source code must retain the above copyright
00009  *    notice, this list of conditions and the following disclaimer.
00010  * 2. Redistributions in binary form must reproduce the above copyright
00011  *    notice, this list of conditions and the following disclaimer in the
00012  *    documentation and/or other materials provided with the distribution.
00013  * 3. Neither the name of the copyright holder nor the
00014  *    names of its contributors may be used to endorse or promote products
00015  *    derived from this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "nsconfig.h"
00031 
00032 #include <string.h>
00033 #include "ns_types.h"
00034 #include <nsdynmemLIB.h>
00035 #include <ns_list.h>
00036 #include "randLIB.h"
00037 #include "ns_trace.h"
00038 #include "common_functions.h"
00039 #include "thread_management_if.h"
00040 #include "thread_border_router_api.h"
00041 #include "6LoWPAN/Thread/thread_config.h"
00042 #include "6LoWPAN/Thread/thread_common.h"
00043 #include "6LoWPAN/Thread/thread_bootstrap.h"
00044 #include "6LoWPAN/Thread/thread_dhcpv6_client.h"
00045 #include "6LoWPAN/Thread/thread_discovery.h"
00046 #include "6LoWPAN/Thread/thread_joiner_application.h"
00047 #include "6LoWPAN/Thread/thread_network_data_lib.h"
00048 #include "6LoWPAN/Thread/thread_nd.h"
00049 #include "6LoWPAN/Thread/thread_tmfcop_lib.h"
00050 #include "6LoWPAN/Thread/thread_management_internal.h"
00051 #include "6LoWPAN/Thread/thread_management_server.h"
00052 #include "6LoWPAN/Thread/thread_commissioning_if.h"
00053 #include "6LoWPAN/Thread/thread_extension.h"
00054 #include "6LoWPAN/Thread/thread_leader_service.h"
00055 #include "6LoWPAN/Thread/thread_router_bootstrap.h"
00056 #include "6LoWPAN/Thread/thread_network_synch.h"
00057 #include "Service_Libs/mle_service/mle_service_api.h"
00058 #include "6LoWPAN/Thread/thread_host_bootstrap.h"
00059 #include "6LoWPAN/Thread/thread_border_router_api_internal.h"
00060 #include "MPL/mpl.h"
00061 #include "MLE/mle.h"
00062 #include "6LoWPAN/MAC/mac_helper.h"
00063 #include "mac_api.h"
00064 #include "6LoWPAN/MAC/mac_data_poll.h"
00065 #include "coap_service_api.h"
00066 
00067 #define TRACE_GROUP "tLdr"
00068 
00069 #ifdef HAVE_THREAD_LEADER_SERVICE
00070 
00071 /*
00072  * Leader instance data.
00073  */
00074 typedef struct thread_leader_service {
00075     int8_t interface_id;
00076     int8_t coap_service_id;
00077     bool leader_enabled: 1;
00078     ns_list_link_t link;
00079 } thread_leader_service_t;
00080 
00081 /*
00082  * Helper structure for router_id handling
00083  */
00084 typedef struct {
00085     uint8_t     routerID[8];
00086     uint32_t    validLifeTime;
00087     uint32_t    preferredLifeTime;
00088     uint16_t    shortAddress;
00089     uint8_t     dataSeq;
00090 } thread_leader_service_router_id_resp_t;
00091 
00092 static NS_LIST_DEFINE(leader_service_instance_list, thread_leader_service_t, link);
00093 
00094 static void thread_leader_service_commissioner_timeout_cb(void* arg);
00095 
00096 /* Private functions */
00097 static thread_leader_service_t *thread_leader_service_find_by_interface(int8_t interface_id)
00098 {
00099     thread_leader_service_t *this = NULL;
00100     ns_list_foreach(thread_leader_service_t, cur_br, &leader_service_instance_list) {
00101         if (cur_br->interface_id == interface_id) {
00102             this = cur_br;
00103             break;
00104         }
00105     }
00106     return this;
00107 }
00108 
00109 static thread_leader_service_t *thread_leader_service_find_by_service(int8_t service_id)
00110 {
00111     thread_leader_service_t *this = NULL;
00112     ns_list_foreach(thread_leader_service_t, cur_br, &leader_service_instance_list) {
00113         if (cur_br->coap_service_id == service_id) {
00114             this = cur_br;
00115             break;
00116         }
00117     }
00118     return this;
00119 }
00120 
00121 static char* thread_commissioning_if_commissioner_id_get(int8_t interface_id)
00122 {
00123     protocol_interface_info_entry_t *cur;
00124 
00125     cur = protocol_stack_interface_info_get_by_id(interface_id);
00126     if (!cur || !cur->thread_info || !cur->thread_info->registered_commissioner.commissioner_valid) {
00127         return NULL;
00128     }
00129 
00130     return cur->thread_info->registered_commissioner.commissioner_id_ptr;
00131 }
00132 
00133 
00134 static bool thread_leader_service_commissioner_address_changed(int8_t interface_id, uint8_t border_router_address[static 16])
00135 {
00136     protocol_interface_info_entry_t *cur;
00137 
00138     cur = protocol_stack_interface_info_get_by_id(interface_id);
00139     if (!cur || !cur->thread_info || !cur->thread_info->registered_commissioner.commissioner_valid) {
00140         return false;
00141     }
00142 
00143     if(!memcmp(cur->thread_info->registered_commissioner.border_router_address, border_router_address, 16)) {
00144         // Border router address is same
00145         return false;
00146     }
00147     if( common_read_16_bit(&border_router_address[14]) >= 0xfc00 ) {
00148         // Address is not valid RLOC we dont change address to this.
00149         return false;
00150     }
00151     memcpy(cur->thread_info->registered_commissioner.border_router_address, border_router_address, 16);
00152     thread_leader_service_network_data_changed(cur,false,true);
00153     // This will trigger advertisement either from following commands or from leader tick
00154     return true;
00155 }
00156 static int thread_leader_service_steering_data_set(int8_t interface_id, uint8_t *buf_ptr, uint16_t length)
00157 {
00158     protocol_interface_info_entry_t *cur;
00159     link_configuration_s *linkConfiguration;
00160 
00161     linkConfiguration = thread_joiner_application_get_config(interface_id);
00162     if (!linkConfiguration) {
00163         return -1;
00164     }
00165 
00166     cur = protocol_stack_interface_info_get_by_id(interface_id);
00167     if (!cur || !cur->thread_info || !cur->thread_info->leader_private_data || !cur->thread_info->registered_commissioner.commissioner_valid) {
00168         return -1;
00169     }
00170     if (length > 16) {
00171         return -2;
00172     }
00173 
00174     tr_debug("Save new steering data %s", trace_array(buf_ptr, length));
00175 
00176     cur->thread_info->registered_commissioner.steering_data_len = 0;
00177 
00178     if (buf_ptr && length > 0) {
00179         memcpy(cur->thread_info->registered_commissioner.steering_data, buf_ptr, length);
00180         cur->thread_info->registered_commissioner.steering_data_len = length;
00181     }
00182 
00183     thread_leader_service_network_data_changed(cur, false, true);
00184 
00185     return 0;
00186 }
00187 
00188 static bool thread_leader_service_commissioner_unregister(int8_t interface_id, uint16_t session_id)
00189 {
00190     protocol_interface_info_entry_t *cur;
00191     link_configuration_s *linkConfiguration;
00192 
00193     linkConfiguration = thread_joiner_application_get_config(interface_id);
00194     if(!linkConfiguration)
00195         return false;
00196 
00197     cur = protocol_stack_interface_info_get_by_id(interface_id);
00198     if (!cur || !cur->thread_info || !cur->thread_info->leader_private_data)
00199         return false;
00200 
00201     if (cur->thread_info->registered_commissioner.session_id != session_id) {
00202         tr_debug("Commissioner session id not valid - unregister failed");
00203         return false;
00204     }
00205     // Removing commissioner we need to increase the session id
00206     cur->thread_info->registered_commissioner.session_id++;
00207 
00208     if(cur->thread_info->registered_commissioner.commissioner_id_ptr){
00209        ns_dyn_mem_free(cur->thread_info->registered_commissioner.commissioner_id_ptr);
00210        cur->thread_info->registered_commissioner.commissioner_id_ptr = NULL;
00211     }
00212 
00213     cur->thread_info->registered_commissioner.commissioner_valid = false;
00214     cur->thread_info->registered_commissioner.steering_data_len = 0;
00215 
00216     /* Send unregistration without delay */
00217     thread_leader_service_network_data_changed(cur, false, true);
00218 
00219     eventOS_timeout_cancel(cur->thread_info->registered_commissioner.commissioner_timeout);
00220     cur->thread_info->registered_commissioner.commissioner_timeout = NULL;
00221     cur->thread_info->registered_commissioner.commissioner_registration = THREAD_COMMISSIONER_NOT_REGISTERED;
00222 
00223     return true;
00224 }
00225 
00226 static bool thread_leader_service_commissioner_session_refresh(int8_t interface_id, uint16_t session_id)
00227 {
00228     protocol_interface_info_entry_t *cur;
00229 
00230     // Check pointers
00231     cur = protocol_stack_interface_info_get_by_id(interface_id);
00232     if (!cur || !cur->thread_info || !cur->thread_info->leader_private_data || !cur->thread_info->registered_commissioner.commissioner_valid) {
00233         return false;
00234     }
00235 
00236     if(cur->thread_info->registered_commissioner.commissioner_registration == THREAD_COMMISSIONER_NOT_REGISTERED) {
00237         tr_debug("Commissioner not registered - refresh failed");
00238         return false;
00239     }
00240 
00241     if (cur->thread_info->registered_commissioner.session_id != session_id) {
00242         tr_debug("Commissioner session id not valid - refresh failed");
00243         return false;
00244     }
00245 
00246     cur->thread_info->registered_commissioner.commissioner_registration = THREAD_COMMISSIONER_REGISTERED;
00247 
00248     eventOS_timeout_cancel(cur->thread_info->registered_commissioner.commissioner_timeout);
00249     cur->thread_info->registered_commissioner.commissioner_timeout = eventOS_timeout_ms(thread_leader_service_commissioner_timeout_cb, COMMISSIONER_OBSOLETE_TIMEOUT, cur->thread_info);
00250 
00251     return true;
00252 }
00253 
00254 static void thread_leader_service_commissioner_timeout_cb(void* arg)
00255 {
00256     thread_info_t *thread_ptr = arg;
00257 
00258     thread_ptr->registered_commissioner.commissioner_timeout = NULL;
00259 
00260     if (!thread_ptr->registered_commissioner.commissioner_valid) {
00261         return;
00262     }
00263 
00264     if (!thread_ptr->leader_private_data) {
00265         return;
00266     }
00267 
00268     switch(thread_ptr->registered_commissioner.commissioner_registration)
00269     {
00270     case THREAD_COMMISSIONER_REGISTRATION_OBSOLETE:
00271         thread_ptr->registered_commissioner.commissioner_registration = THREAD_COMMISSIONER_NOT_REGISTERED;
00272         if(false == thread_leader_service_commissioner_unregister(thread_ptr->interface_id, thread_ptr->registered_commissioner.session_id)){
00273             tr_debug("Commissioner registration remove failed");
00274         }
00275         else{
00276             tr_debug("Commissioner registration removed");
00277         }
00278         break;
00279     case THREAD_COMMISSIONER_REGISTERED:
00280         tr_debug("Commissioner registration obsoleted");
00281         thread_ptr->registered_commissioner.commissioner_registration = THREAD_COMMISSIONER_REGISTRATION_OBSOLETE;
00282         thread_ptr->registered_commissioner.commissioner_timeout = eventOS_timeout_ms(thread_leader_service_commissioner_timeout_cb, COMMISSIONER_REMOVE_TIMEOUT, thread_ptr);
00283         break;
00284     case THREAD_COMMISSIONER_NOT_REGISTERED:
00285     default:
00286        break;
00287     }
00288 }
00289 
00290 
00291 static int thread_leader_service_commissioner_register(int8_t interface_id, uint8_t border_router_address[static 16],
00292                                   uint8_t *commissioner_id_ptr, uint16_t commissioner_id_len,
00293                                   uint16_t *session_id)
00294 {
00295     /* Save commissioner data to new commissioner structure and start advertise it
00296     Commissioner session id
00297     commissioner timestamp
00298     commissioner credentials -> static configuration
00299     security policy -> static configuration
00300     network name -> static configuration
00301     border router locator
00302     steering data
00303     */
00304     protocol_interface_info_entry_t *cur;
00305     link_configuration_s *linkConfiguration;
00306 
00307     tr_debug("Register interface %d commissioner: %s", interface_id, trace_ipv6(border_router_address));
00308 
00309     linkConfiguration = thread_joiner_application_get_config(interface_id);
00310     if (!linkConfiguration) {
00311         return -1;
00312     }
00313 
00314     cur = protocol_stack_interface_info_get_by_id(interface_id);
00315     if (!cur || !cur->thread_info || !cur->thread_info->leader_private_data) {
00316         return -1;
00317     }
00318 
00319     /* If commissioner already registered */
00320     if(cur->thread_info->registered_commissioner.commissioner_valid &&
00321             cur->thread_info->registered_commissioner.commissioner_registration == THREAD_COMMISSIONER_REGISTERED) {
00322         return -2;
00323     }
00324 
00325     ns_dyn_mem_free(cur->thread_info->registered_commissioner.commissioner_id_ptr);
00326     cur->thread_info->registered_commissioner.commissioner_id_ptr = ns_dyn_mem_alloc(commissioner_id_len + 1);
00327     if (!cur->thread_info->registered_commissioner.commissioner_id_ptr) {
00328         return -1;
00329     }
00330     memcpy(cur->thread_info->registered_commissioner.commissioner_id_ptr, commissioner_id_ptr, commissioner_id_len);
00331     cur->thread_info->registered_commissioner.commissioner_id_ptr[commissioner_id_len] = 0;
00332 
00333     // Initialise values
00334     cur->thread_info->registered_commissioner.session_id++;
00335     if (cur->thread_info->registered_commissioner.session_id == 0) {
00336         cur->thread_info->registered_commissioner.session_id++;
00337     }
00338 
00339     if (session_id) {
00340         *session_id = cur->thread_info->registered_commissioner.session_id;
00341     }
00342 
00343     //SET Border Router Locator
00344     memcpy(cur->thread_info->registered_commissioner.border_router_address, border_router_address, 16);
00345     cur->thread_info->registered_commissioner.commissioner_valid = true;
00346 
00347     cur->thread_info->registered_commissioner.commissioner_registration = THREAD_COMMISSIONER_REGISTERED;
00348 
00349     eventOS_timeout_cancel(cur->thread_info->registered_commissioner.commissioner_timeout);
00350     cur->thread_info->registered_commissioner.commissioner_timeout = eventOS_timeout_ms(thread_leader_service_commissioner_timeout_cb, COMMISSIONER_OBSOLETE_TIMEOUT, cur->thread_info);
00351 
00352     /* Delay network data propagation, perhaps we receive more data */
00353     thread_leader_service_network_data_changed(cur, false, true);
00354 
00355     return 0;
00356 }
00357 
00358 
00359 
00360 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)
00361 {
00362     thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
00363     link_configuration_s *linkConfiguration;
00364     sn_coap_msg_code_e response_code = COAP_MSG_CODE_RESPONSE_CHANGED;
00365     uint16_t session_id;
00366     uint8_t payload[5]; // 4 + 1
00367     uint8_t *ptr;
00368     uint8_t *response_ptr;
00369     uint64_t timestamp;
00370     bool msg_from_commissioner = false;
00371     bool changing_connectivity = false;
00372     (void) source_address;
00373     (void) source_port;
00374     int ret = 0;
00375 
00376     if (!this) {
00377         return -1;
00378     }
00379     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(this->interface_id);
00380     linkConfiguration = thread_joiner_application_get_config(this->interface_id);
00381     if(!cur || !cur->thread_info || !linkConfiguration || !cur->thread_info->leader_private_data){
00382         return -1;
00383     }
00384 
00385     tr_info("thread management Active set");
00386 
00387     response_ptr = payload;
00388 
00389     if (!thread_management_server_source_address_check(this->interface_id, source_address)) {
00390         response_code = COAP_MSG_CODE_RESPONSE_BAD_REQUEST;
00391         goto send_error_response;
00392     }
00393 
00394     if (3 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_CHANNEL, &ptr) &&
00395          (linkConfiguration->rfChannel != common_read_16_bit(&ptr[1]) || linkConfiguration->channel_page != *ptr)){
00396         tr_debug("Channel changed");
00397         changing_connectivity = true;
00398         // validity check for channel page
00399         if (*ptr != 0 || common_read_16_bit(&ptr[1]) < 11 || common_read_16_bit(&ptr[1]) > 26) {
00400             tr_warn("invalid channel");
00401             ret = -1;
00402             goto send_response;
00403         }
00404     }
00405     if (2 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_PANID, &ptr) &&
00406          linkConfiguration->panId != common_read_16_bit(ptr)){
00407         tr_debug("PANID changed");
00408         changing_connectivity = true;
00409         if (common_read_16_bit(ptr) > 0xfffe) {
00410             tr_warn("invalid panid");
00411             ret = -1;
00412             goto send_response;
00413         }
00414     }
00415     if (3 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_SECURITY_POLICY, &ptr)) {
00416         if (common_read_16_bit(ptr) < 3600) {
00417             tr_warn("invalid security policy");
00418             ret = -1;
00419             goto send_response;
00420         }
00421     }
00422 
00423     if ( 8 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_NETWORK_MESH_LOCAL_ULA, &ptr) &&
00424          memcmp(linkConfiguration->mesh_local_ula_prefix,ptr,8) != 0) {
00425         tr_debug("ML prefix changed");
00426         changing_connectivity = true;
00427     }
00428     if ( 16 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_NETWORK_MASTER_KEY, &ptr) &&
00429          memcmp(linkConfiguration->master_key,ptr,16) != 0) {
00430         tr_debug("Master key changed");
00431         changing_connectivity = true;
00432     }
00433 
00434     if (2 <= thread_meshcop_tlv_data_get_uint16(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_COMMISSIONER_SESSION_ID, &session_id)) {
00435         // Session id present must be valid
00436         tr_info("message from commissioner");
00437         msg_from_commissioner = true;
00438         if (cur->thread_info->registered_commissioner.session_id != session_id) {
00439             tr_warn("Invalid commissioner session id");
00440             ret = -1;
00441             goto send_response;
00442         }
00443         thread_leader_service_commissioner_address_changed(this->interface_id, source_address);
00444         // Check if address changed
00445         if (changing_connectivity == true) {
00446             tr_warn("Changes affecting connectivity rejected");
00447             ret = -1;
00448             goto send_response;
00449         }
00450     }
00451 
00452     if(8 > thread_meshcop_tlv_data_get_uint64(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_ACTIVE_TIME_STAMP, &timestamp)){
00453         tr_warn("no timestamp");
00454         ret = -1;
00455         goto send_response;
00456     }
00457     if (timestamp < linkConfiguration->timestamp){
00458         tr_warn("older timestamp");
00459         ret = -1;
00460         goto send_response;
00461     }
00462     if (timestamp == linkConfiguration->timestamp){
00463         tr_warn("same timestamp");
00464         ret = -1;
00465         goto send_response;
00466     }
00467     if (!msg_from_commissioner) {
00468         tr_debug("message from end device");
00469         if (cur->thread_info->registered_commissioner.commissioner_registration != THREAD_COMMISSIONER_NOT_REGISTERED) {
00470             // Node changed settings and Registered commissioner send changed_notify to commissioner
00471             tr_info("Notify commissioner that settings are changed");
00472             coap_service_request_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, cur->thread_info->registered_commissioner.border_router_address,
00473                          THREAD_MANAGEMENT_PORT, COAP_MSG_TYPE_CONFIRMABLE, COAP_MSG_CODE_REQUEST_POST, THREAD_URI_DATASET_CHANGED,
00474                          COAP_CT_OCTET_STREAM, NULL, 0, NULL);
00475         }
00476     }
00477     if (changing_connectivity) {
00478 
00479         tr_info("creating pending configuration");
00480 
00481         if (thread_joiner_application_pending_config_exists(this->interface_id) &&
00482             thread_joiner_application_pending_config_timestamp_get(this->interface_id) >= timestamp ) {
00483             tr_info("Pending config is newer we reject");
00484             ret = -1;
00485             goto send_response;
00486         }
00487         if (thread_joiner_application_pending_config_create(this->interface_id, request_ptr->payload_ptr, request_ptr->payload_len) != 0) {
00488             tr_error("Could not create pending config");
00489             response_code = COAP_MSG_CODE_RESPONSE_INTERNAL_SERVER_ERROR;
00490             goto send_error_response;
00491         }
00492 
00493         // Start distributing pending configuration
00494         thread_joiner_application_pending_config_add_missing_fields(this->interface_id);
00495         thread_joiner_application_pending_config_timestamp_set(this->interface_id, timestamp);
00496         thread_joiner_application_pending_config_enable(this->interface_id, thread_delay_timer_default*1000);
00497 
00498     } else {
00499         tr_info("updating active configuration");
00500         ret = thread_joiner_application_update_configuration(cur->id, request_ptr->payload_ptr, request_ptr->payload_len, true);
00501         // link configuration is changed, read configuration again
00502         linkConfiguration = thread_joiner_application_get_config(cur->id);
00503         if (!linkConfiguration) {
00504             tr_error("Could not find current config");
00505             response_code = COAP_MSG_CODE_RESPONSE_INTERNAL_SERVER_ERROR;
00506             goto send_error_response;
00507         }
00508         // Activate new configuration.
00509         thread_configuration_thread_activate(cur, linkConfiguration);
00510     }
00511     thread_joiner_application_configuration_nvm_save(this->interface_id);
00512 
00513     // Start timer to propagate new data
00514     thread_leader_service_network_data_changed(cur, true, true);
00515 
00516 send_response:
00517     // build response
00518     response_ptr = thread_meshcop_tlv_data_write_uint8(response_ptr, MESHCOP_TLV_STATE, ret == 0 ? 1 : 0xff);
00519 
00520 send_error_response:
00521     coap_service_response_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, request_ptr, response_code, COAP_CT_OCTET_STREAM, payload, response_ptr - payload);
00522     return 0;
00523 }
00524 
00525 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)
00526 {
00527     thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
00528     link_configuration_s *linkConfiguration;
00529     uint8_t *ptr;
00530     sn_coap_msg_code_e response_code = COAP_MSG_CODE_RESPONSE_CHANGED;
00531     uint16_t session_id;
00532     uint8_t payload[5]; // 4 + 1
00533     uint8_t *secret_ptr;
00534     uint64_t active_timestamp;
00535     uint64_t pending_timestamp;
00536     uint32_t delay_timer;
00537     (void) source_address;
00538     (void) source_port;
00539     int ret = 0;
00540     bool msg_from_commissioner = false;
00541     bool master_key_changed = false;
00542 
00543     ptr = payload;
00544     if (!this) {
00545         return -1;
00546     }
00547     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(this->interface_id);
00548     linkConfiguration = thread_joiner_application_get_config(this->interface_id);
00549      if(!cur || !cur->thread_info || !linkConfiguration || !cur->thread_info->leader_private_data){
00550         return -1;
00551     }
00552 
00553     tr_info("thread management Pending set");
00554 
00555     if (!thread_management_server_source_address_check(this->interface_id, source_address)) {
00556         response_code = COAP_MSG_CODE_RESPONSE_BAD_REQUEST;
00557         goto send_error_response;
00558     }
00559 
00560     if (2 <= thread_meshcop_tlv_data_get_uint16(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_COMMISSIONER_SESSION_ID, &session_id)) {
00561         // Session id present must be valid
00562         if (cur->thread_info->registered_commissioner.session_id != session_id) {
00563             tr_warn("Invalid commissioner session id");
00564             ret = -1;
00565             goto send_response;
00566         }
00567         thread_leader_service_commissioner_address_changed(this->interface_id, source_address);
00568         msg_from_commissioner = true;
00569     }
00570     if ( 16 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_NETWORK_MASTER_KEY, &secret_ptr) &&
00571          memcmp(linkConfiguration->master_key,secret_ptr,16) != 0) {
00572         tr_debug("Master key changed");
00573         master_key_changed = true;
00574     }
00575 
00576     if(8 > thread_meshcop_tlv_data_get_uint64(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_PENDING_TIMESTAMP, &pending_timestamp)){
00577         tr_warn("no timestamp");
00578         ret = -1;
00579         goto send_response;
00580     }
00581     if(4 > thread_meshcop_tlv_data_get_uint32(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_DELAY_TIMER, &delay_timer)){
00582         tr_warn("no delay timer");
00583         ret = -1;
00584         goto send_response;
00585     }
00586     if(8 > thread_meshcop_tlv_data_get_uint64(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_ACTIVE_TIME_STAMP, &active_timestamp)){
00587         tr_warn("no active timestamp");
00588         ret = -1;
00589         goto send_response;
00590     }
00591     if (active_timestamp <= linkConfiguration->timestamp && !master_key_changed) {
00592         tr_warn("Timestamp is lower or same, without Master key change");
00593         ret = -1;
00594         goto send_response;
00595     }
00596 
00597     if (thread_joiner_application_pending_config_exists(this->interface_id) &&
00598         pending_timestamp <= thread_joiner_application_pending_config_timestamp_get(this->interface_id)){
00599         tr_warn("Older or same pending timestamp");
00600         ret = -1;
00601         goto send_response;
00602     }
00603 
00604     if (delay_timer > THREAD_MAX_DELAY_TIMER_SECONDS*1000) {
00605         delay_timer = THREAD_MAX_DELAY_TIMER_SECONDS*1000;
00606     }
00607 
00608     if (master_key_changed &&
00609             delay_timer < thread_delay_timer_default*1000) {
00610         //If message Changes the master key default value is used
00611         delay_timer = thread_delay_timer_default*1000;
00612     }
00613     if (delay_timer < THREAD_DELAY_TIMER_MINIMAL_SECONDS*1000) {
00614         // Absolute minimum value allowed
00615         delay_timer = THREAD_DELAY_TIMER_MINIMAL_SECONDS*1000;
00616     }
00617 
00618     if (thread_joiner_application_pending_config_create(this->interface_id, request_ptr->payload_ptr, request_ptr->payload_len) != 0){
00619         tr_error("Could not create pending config");
00620         response_code = COAP_MSG_CODE_RESPONSE_INTERNAL_SERVER_ERROR;
00621         goto send_error_response;
00622     }
00623 
00624     if (!msg_from_commissioner) {
00625         tr_debug("pending set from end device");
00626         if (cur->thread_info->registered_commissioner.commissioner_registration != THREAD_COMMISSIONER_NOT_REGISTERED) {
00627             // Node changed settings and Registered commissioner send changed_notify to commissioner
00628             tr_info("Notify commissioner that settings are changed");
00629             coap_service_request_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, cur->thread_info->registered_commissioner.border_router_address,
00630                          THREAD_MANAGEMENT_PORT, COAP_MSG_TYPE_CONFIRMABLE, COAP_MSG_CODE_REQUEST_POST, THREAD_URI_DATASET_CHANGED,
00631                          COAP_CT_OCTET_STREAM, NULL, 0, NULL);
00632         }
00633     }
00634 
00635     tr_info("creating new pending set delay: %"PRIu32, delay_timer);
00636     // Activate pending configuration
00637     thread_joiner_application_pending_config_add_missing_fields(this->interface_id);
00638     thread_joiner_application_pending_config_timestamp_set(this->interface_id, pending_timestamp);
00639     thread_joiner_application_pending_config_enable(this->interface_id, delay_timer);
00640 
00641     // Start timer to propagate new data
00642     thread_leader_service_network_data_changed(cur, true, true);
00643 
00644 send_response:
00645     // build response
00646     ptr = thread_meshcop_tlv_data_write_uint8(ptr, MESHCOP_TLV_STATE, ret == 0 ? 1 : 0xff);
00647 
00648 send_error_response:
00649     coap_service_response_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, request_ptr, response_code, COAP_CT_OCTET_STREAM, payload, ptr - payload);
00650     return 0;
00651 }
00652 
00653 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)
00654 {
00655     thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
00656     sn_coap_msg_code_e response_code = COAP_MSG_CODE_RESPONSE_CHANGED;
00657     uint16_t session_id;
00658     uint16_t br_locator;
00659     uint8_t payload[5]; // 4 + 1
00660     uint8_t *ptr;
00661     uint8_t *steering_data_ptr;
00662     uint16_t steering_data_len;
00663     (void) source_address;
00664     (void) source_port;
00665     int ret = -2;
00666 
00667     ptr = payload;
00668     if (!this) {
00669         return -1;
00670     }
00671     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(this->interface_id);
00672     if(!cur || !cur->thread_info || !cur->thread_info->leader_private_data){
00673         return -1;
00674     }
00675 
00676     tr_info("thread management commissioner set");
00677 
00678     if (!thread_management_server_source_address_check(this->interface_id, source_address)) {
00679         response_code = COAP_MSG_CODE_RESPONSE_BAD_REQUEST;
00680         goto send_error_response;
00681     }
00682     //Check if the CoAp payload is greater than maximum commissioner data size and reject
00683     if (request_ptr->payload_len > THREAD_MAX_COMMISSIONER_DATA_SIZE){
00684         tr_error("Payload length greater than maximum commissioner data size");
00685         ret = -1;
00686         goto send_response;
00687     }
00688     //TODO must be checked against a list of all non permissible TLVs
00689     if (2 <= thread_meshcop_tlv_data_get_uint16(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_BORDER_ROUTER_LOCATOR, &br_locator)){
00690         tr_error("BORDER ROUTER TLV should not be present");
00691         ret = -1;
00692         goto send_response;
00693     }
00694 
00695     // TODO must be checked against commissioner address but there is no way to get it.
00696     if (2 != thread_meshcop_tlv_data_get_uint16(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_COMMISSIONER_SESSION_ID, &session_id)) {
00697         // Session id not present must be valid
00698         tr_warn("Commissioner session id not present");
00699         ret = -1;
00700         goto send_response;
00701     }
00702     if (cur->thread_info->registered_commissioner.session_id != session_id) {
00703         tr_warn("Invalid commissioner session id");
00704         ret = -1;
00705         goto send_response;
00706     }
00707 
00708     /* Set border router address */
00709     thread_leader_service_commissioner_address_changed(this->interface_id, source_address);
00710 
00711     /* Set steering data */
00712     if(thread_meshcop_tlv_exist(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_STEERING_DATA)){
00713         steering_data_len = thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_STEERING_DATA, &steering_data_ptr);
00714         ret = thread_leader_service_steering_data_set(this->interface_id, steering_data_ptr, steering_data_len);
00715         tr_info("steering data set : %s, ret %d", trace_array(steering_data_ptr, steering_data_len), ret);
00716     }
00717 
00718 send_response:
00719     // build response
00720     ptr = thread_meshcop_tlv_data_write_uint8(ptr, MESHCOP_TLV_STATE, ret == 0 ? 1 : 0xff);
00721 
00722 send_error_response:
00723     coap_service_response_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, request_ptr, response_code, COAP_CT_OCTET_STREAM, payload, ptr - payload);
00724     return 0;
00725 }
00726 
00727 static void thread_leader_allocate_router_id_by_allocated_id(thread_leader_info_t *info, uint8_t router_id, const uint8_t *eui64)
00728 {
00729     memcpy(info->thread_router_id_list[router_id].eui64, eui64, 8);
00730     bit_set(info->master_router_id_mask, router_id);
00731     info->thread_router_id_list[router_id].validLifeTime = 0xffffffff;
00732     info->thread_router_id_list[router_id].reUsePossible = false;
00733 }
00734 
00735 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)
00736 {
00737     int ret_val = -1;
00738     uint8_t allocated_id = 64;
00739     uint8_t i;
00740     uint8_t compareId;
00741     thread_leader_info_t *leader_private_ptr = 0;
00742     thread_info_t *thread_info = cur->thread_info;
00743     uint8_t activeRouterCount;
00744 
00745     if (!thread_info || !thread_info->leader_private_data) {
00746         return -1;
00747     }
00748 
00749     leader_private_ptr = thread_info->leader_private_data;
00750     //Check if we have already allocated router address for this MAC address
00751     for (i = 0; i < 64; i++) {
00752         if (bit_test(leader_private_ptr->master_router_id_mask, i)) {
00753             //Active ID
00754             if (memcmp(eui64, leader_private_ptr->thread_router_id_list[i].eui64, 8) == 0) {
00755                 allocated_id = i;
00756                 break;
00757             }
00758         }
00759     }
00760 
00761     if (allocated_id  < 64) {
00762         goto alloc_ready;
00763     }
00764     activeRouterCount = thread_routing_count_active_routers_from_mask(leader_private_ptr->master_router_id_mask);
00765     if (activeRouterCount >= cur->thread_info->testMaxActiveRouterIdLimit) {
00766         // Only 32 router id allowed for one Thread partition
00767         goto alloc_ready;
00768     }
00769 
00770 
00771     //Try to allocate the same router ID that was previously used
00772     if (thread_is_router_addr(reponse->shortAddress)) {
00773         compareId = thread_router_id_from_addr(reponse->shortAddress);
00774         tr_debug("HInt: %02x", compareId);
00775         if (!bit_test(leader_private_ptr->master_router_id_mask, compareId)) {
00776             //Take Free entry By Hint if valid lifetime is zero
00777             if (leader_private_ptr->thread_router_id_list[compareId].reUsePossible) {
00778                 allocated_id = compareId;
00779             }
00780         }
00781     }
00782 
00783     if (allocated_id  < 64) {
00784         goto alloc_new;
00785     }
00786 
00787     //We need to allocate new Router ID
00788     uint16_t random_base = randLIB_get_random_in_range(0,64);
00789     // New addresses are assigned randomly
00790     for (i = 0; i < 64; i++) {
00791         uint16_t id = (random_base + i) % (MAX_ROUTER_ID + 1);
00792         if (!bit_test(leader_private_ptr->master_router_id_mask, id)) {
00793             if (leader_private_ptr->thread_router_id_list[id].reUsePossible) {
00794                 allocated_id = id;
00795                 break;
00796             }
00797         }
00798     }
00799 
00800 alloc_new:
00801     if (allocated_id < 64) {
00802         //Update Route MASK and push NEW Route Info
00803         thread_leader_allocate_router_id_by_allocated_id(leader_private_ptr, allocated_id, eui64);
00804 
00805         //Increment Route TLV data sequency
00806         thread_leader_service_update_id_set(cur);
00807     }
00808 
00809 alloc_ready:
00810 
00811     if (allocated_id < 64) {
00812         thread_leader_allocate_router_id_by_allocated_id(leader_private_ptr, allocated_id, eui64);
00813         reponse->shortAddress = (allocated_id << 10);
00814         memcpy(reponse->routerID, leader_private_ptr->master_router_id_mask, 8);
00815         reponse->dataSeq = leader_private_ptr->maskSeq;
00816         reponse->preferredLifeTime = 0xffffffff;
00817         reponse->validLifeTime = 0xffffffff;
00818         ret_val = 0;
00819     }
00820     return ret_val;
00821 }
00822 
00823 static int thread_leader_service_leader_data_allocate(protocol_interface_info_entry_t *cur)
00824 {
00825     thread_leader_info_t *leader_info = 0;
00826     leader_info = thread_allocate_and_init_leader_private_data();
00827     if (leader_info == NULL) {
00828         return -1;
00829     }
00830     cur->thread_info->leader_private_data = leader_info;
00831     cur->thread_info->thread_device_mode = THREAD_DEVICE_MODE_ROUTER;
00832     cur->thread_info->thread_attached_state = THREAD_STATE_CONNECTED_ROUTER;
00833     return 0;
00834 }
00835 
00836 static int thread_leader_service_router_id_deallocate(const uint8_t *eui64, protocol_interface_info_entry_t *cur)
00837 {
00838     uint8_t i;
00839     thread_leader_info_t *leader_private_ptr = 0;
00840     thread_info_t *thread_info = cur->thread_info;
00841     if (thread_info && thread_info->leader_private_data) {
00842         leader_private_ptr = thread_info->leader_private_data;
00843         //Check by MAC address
00844         for (i = 0; i < 64; i++) {
00845             if (bit_test(leader_private_ptr->master_router_id_mask, i)) {
00846                 //Active ID
00847                 if (memcmp(eui64, leader_private_ptr->thread_router_id_list[i].eui64, 8) == 0) {
00848                     tr_debug("Release Router Id %d", i);
00849                     thread_leader_service_route_mask_bit_clear(leader_private_ptr, i);
00850                     leader_private_ptr->thread_router_id_list[i].reUsePossible = true;
00851                     leader_private_ptr->thread_router_id_list[i].validLifeTime = 0;
00852                     //Increment Route TLV data sequency
00853                     thread_leader_service_update_id_set(cur);
00854                     thread_routing_activate(&cur->thread_info->routing);
00855                     return 0;
00856                 }
00857             }
00858         }
00859     }
00860 
00861     return -1;
00862 }
00863 
00864 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)
00865 {
00866     thread_leader_service_router_id_resp_t router_id_response;
00867     tr_debug("RId Server assign");
00868     router_id_response.shortAddress = *router_id; // Set Hint
00869 
00870     if (thread_leader_service_router_id_allocate(mac, cur, &router_id_response) != 0) {
00871         tr_debug("Assign Fail");
00872         return -1;
00873     }
00874     *router_id = router_id_response.shortAddress;
00875     router_id_mask_out[0] = router_id_response.dataSeq;
00876     memcpy(&router_id_mask_out[1], router_id_response.routerID , 8);
00877     return 0;
00878 }
00879 
00880 static int thread_leader_service_routerid_release(int8_t interface_id, uint8_t mac[8], uint16_t router_id)
00881 {
00882     (void) router_id;
00883     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
00884     if (!cur) {
00885         tr_error("No Interface");
00886         return -1;
00887     }
00888 
00889     if (!cur->thread_info) {
00890         tr_error("Not Thread Interface");
00891         return -1;
00892     }
00893 
00894     if (!cur->thread_info->leader_private_data) {
00895         tr_error("Not Leader");
00896         return -1;
00897     }
00898 
00899     return thread_leader_service_router_id_deallocate(mac, cur);
00900 }
00901 
00902 /**
00903  * Router id assignment callback for leader
00904  * uri = tn/d/as
00905  */
00906 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)
00907 {
00908     thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
00909     uint8_t payload[24]; //!< response payload 4 + 9, 4 + 2, 4 + 1
00910     uint8_t *mac_ptr = NULL;
00911     uint8_t router_id_mask[9] = {0};
00912     uint16_t router_id = 0xfffe;
00913     uint8_t *ptr;
00914     uint8_t *status;
00915     (void) source_address;
00916     (void) source_port;
00917 
00918     if (!this) {
00919         return -1;
00920     }
00921 
00922     tr_debug("Thread router id assign request");
00923     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(this->interface_id);
00924 
00925     if (!cur || !cur->thread_info) {
00926         return -1;
00927     }
00928 
00929     // LOCATOR is optional others are mandatory
00930     (void)thread_meshcop_tlv_data_get_uint16(request_ptr->payload_ptr, request_ptr->payload_len, TMFCOP_TLV_RLOC16, &router_id);
00931 
00932     if (8 > thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, TMFCOP_TLV_MAC_ADDRESS, &mac_ptr) ) {
00933         // Failure in message
00934         return -1;
00935     }
00936 
00937         // Failure in message
00938     if (1 > thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, TMFCOP_TLV_STATUS, &status) ) {
00939         return -1;
00940     }
00941 
00942     int ret = -1;
00943     if (*status == THREAD_COAP_STATUS_TLV_TOO_FEW_ROUTERS) {
00944         uint8_t activeRouterCount = 0;
00945         if (cur->thread_info->leader_private_data) {
00946             activeRouterCount = thread_routing_count_active_routers_from_mask(cur->thread_info->leader_private_data->master_router_id_mask);
00947         }
00948 
00949         if (activeRouterCount < cur->thread_info->routerSelectParameters.routerUpgradeThresHold) {
00950             ret = thread_leader_service_routerid_assign(cur, mac_ptr, router_id_mask, &router_id);
00951         } else {
00952             tr_info("Router ID not assigned, routers %d >= %d(upgrade threshold)", activeRouterCount, cur->thread_info->routerSelectParameters.routerUpgradeThresHold);
00953         }
00954     } else {
00955         ret = thread_leader_service_routerid_assign(cur, mac_ptr, router_id_mask, &router_id);
00956     }
00957 
00958     ptr = payload;
00959     if (ret == 0) {
00960         tr_debug("Thread router id mac %s, mask %s, locator %x", trace_array(mac_ptr, 8), trace_array(router_id_mask, 9), router_id);
00961         ptr = thread_tmfcop_tlv_data_write_uint8(ptr, TMFCOP_TLV_STATUS, THREAD_COAP_STATUS_TLV_SUCCESS);
00962         ptr = thread_tmfcop_tlv_data_write_uint16(ptr, TMFCOP_TLV_RLOC16, router_id);
00963         ptr = thread_tmfcop_tlv_data_write(ptr, TMFCOP_TLV_ROUTER_MASK, 9, router_id_mask);
00964     } else {
00965         ptr = thread_tmfcop_tlv_data_write_uint8(ptr, TMFCOP_TLV_STATUS, THREAD_COAP_STATUS_TLV_NO_ADDRESS_AVAILABLE);
00966     }
00967 
00968     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);
00969     return 0;
00970 }
00971 
00972 /**
00973  * Router id assignment callback for leader
00974  * uri = tn/d/ar
00975  */
00976 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)
00977 {
00978     thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
00979     uint8_t payload[5]; //!< response payload 4 + 1
00980     uint8_t *mac_ptr;
00981     uint16_t router_id;
00982     uint8_t *ptr;
00983     (void) source_address;
00984     (void) source_port;
00985 
00986     if (!this) {
00987         return -1;
00988     }
00989 
00990     tr_debug("Thread router id release request");
00991     if (2 > thread_meshcop_tlv_data_get_uint16(request_ptr->payload_ptr, request_ptr->payload_len, TMFCOP_TLV_RLOC16, &router_id)) {
00992         return -1;
00993     }
00994 
00995     if (8 > thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, TMFCOP_TLV_MAC_ADDRESS, &mac_ptr)) {
00996         // Failure in message
00997         return -1;
00998     }
00999     tr_debug("Thread router release id mac %s, locator %x", trace_array(mac_ptr, 8), router_id);
01000 
01001     thread_leader_service_routerid_release(this->interface_id, mac_ptr, router_id);
01002 
01003     ptr = payload;
01004     ptr = thread_tmfcop_tlv_data_write_uint8(ptr, TMFCOP_TLV_STATUS, 0);
01005     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);
01006 
01007     return 0;
01008 }
01009 
01010 /**
01011  * Thread commissioning petition handler
01012  * uri = tn/mc/lp
01013  */
01014 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)
01015 {
01016     thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
01017     sn_coap_msg_code_e response_code = COAP_MSG_CODE_RESPONSE_CHANGED;
01018     uint8_t payload[79];// max length for commissioner id is 64 + 4 byte header + 4 + 1 + 4 + 2
01019     uint8_t *ptr;
01020     uint16_t session_id = 0;
01021     uint8_t *tlv_data_ptr = NULL;
01022     uint16_t tlv_length;
01023     char *commissioner_id_ptr = NULL;
01024 
01025     (void) source_port;
01026 
01027     ptr = payload;
01028     if (!this) {
01029         return -1;
01030     }
01031 
01032     tr_debug("Thread management commissioner petition");
01033 
01034     if (!thread_management_server_source_address_check(this->interface_id, source_address)) {
01035         response_code = COAP_MSG_CODE_RESPONSE_BAD_REQUEST;
01036         goto send_error_response;
01037     }
01038 
01039     // save values from message
01040     tlv_length = thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_COMMISSIONER_ID, &tlv_data_ptr);
01041 
01042     // register to thread interface
01043     int ret = thread_leader_service_commissioner_register(this->interface_id, source_address, tlv_data_ptr, tlv_length, &session_id);
01044 
01045     // Build response
01046     if (ret == 0) {
01047         // Register OK
01048         ptr = thread_meshcop_tlv_data_write_uint8(ptr, MESHCOP_TLV_STATE, 1);
01049         ptr = thread_meshcop_tlv_data_write(ptr, MESHCOP_TLV_COMMISSIONER_ID, tlv_length, tlv_data_ptr);
01050         ptr = thread_meshcop_tlv_data_write_uint16(ptr, MESHCOP_TLV_COMMISSIONER_SESSION_ID, session_id);
01051     } else if (ret == -2) {
01052         // Commissioner already registered - reject and send commissioner ID
01053         ptr = thread_meshcop_tlv_data_write_uint8(ptr, MESHCOP_TLV_STATE, 0xff);
01054         commissioner_id_ptr = thread_commissioning_if_commissioner_id_get(this->interface_id);
01055         if(commissioner_id_ptr)
01056             ptr = thread_meshcop_tlv_data_write(ptr, MESHCOP_TLV_COMMISSIONER_ID, strlen(commissioner_id_ptr), (uint8_t *)commissioner_id_ptr);
01057     }
01058     else{
01059         // Reject, anyhow, no commissioner registered - no commissioner ID
01060         ptr = thread_meshcop_tlv_data_write_uint8(ptr, MESHCOP_TLV_STATE, 0xff);
01061     }
01062 
01063     tr_debug("Petition req recv id %s, RESP session id: %d ret %d", commissioner_id_ptr, session_id, ret);
01064 
01065 send_error_response:
01066     coap_service_response_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, request_ptr, response_code, COAP_CT_OCTET_STREAM, payload, ptr - payload);
01067     return 0;
01068 }
01069 
01070 /**
01071  * Thread commissioning keep alive handler
01072  * uri = tn/mc/la
01073  */
01074 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)
01075 {
01076     thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
01077     sn_coap_msg_code_e response_code = COAP_MSG_CODE_RESPONSE_CHANGED;
01078     uint8_t payload[5]; //status 4 + 1
01079     uint8_t *ptr;
01080     uint16_t session_id = 0;
01081     bool state = false;
01082 
01083     (void) source_address;
01084     (void) source_port;
01085 
01086     if (!this) {
01087         return -1;
01088     }
01089 
01090     tr_debug("Thread management commissioner keep alive");
01091 
01092     if (!thread_management_server_source_address_check(this->interface_id, source_address)) {
01093         response_code = COAP_MSG_CODE_RESPONSE_BAD_REQUEST;
01094         ptr = payload;
01095         goto send_error_response;
01096     }
01097 
01098     if (2 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_COMMISSIONER_SESSION_ID, &ptr)) {
01099         session_id = common_read_16_bit(ptr);
01100     }
01101     if (1 <= thread_meshcop_tlv_find(request_ptr->payload_ptr, request_ptr->payload_len, MESHCOP_TLV_STATE, &ptr))
01102     {
01103         if (*ptr == 0x01) {
01104             state = thread_leader_service_commissioner_session_refresh(this->interface_id, session_id);
01105         } else if(*ptr == 0xff) {
01106             thread_leader_service_commissioner_unregister(this->interface_id, session_id);
01107             state = false;
01108         }
01109     }
01110     if (state == true) {
01111         thread_leader_service_commissioner_address_changed(this->interface_id, source_address);
01112     }
01113     tr_info("Commissioner keep alive req recv, interface: %d session id %d state %d", this->interface_id, session_id, state);
01114     ptr = payload;
01115     ptr = thread_meshcop_tlv_data_write_uint8(ptr, MESHCOP_TLV_STATE, state == true ? 1 : 0xff);
01116 
01117 send_error_response:
01118     coap_service_response_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, request_ptr, response_code, COAP_CT_OCTET_STREAM, payload, ptr - payload);
01119     return 0;
01120 }
01121 
01122 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)
01123 {
01124     tr_debug("thread leader Register request");
01125     // Mark all data to be cleared
01126     thread_network_data_router_id_mark_delete(&cur->thread_info->networkDataStorage, routerId, true);
01127 
01128     // update data from this device
01129     int retVal = thread_bootstrap_network_data_process(cur, network_data_ptr, network_data_length);
01130 
01131     if (retVal < 0) {
01132         tr_warn("network data update failed: %s", trace_array(network_data_ptr, network_data_length));
01133         return retVal;
01134     }
01135     thread_network_data_router_id_free(&cur->thread_info->networkDataStorage, true, cur);
01136 
01137     thread_leader_service_network_data_changed(cur, false, false);
01138     return 0;
01139 }
01140 
01141 /**
01142  * Thread network data registration handler
01143  * uri = tn/sd
01144  */
01145 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)
01146 {
01147     thread_leader_service_t *this = thread_leader_service_find_by_service(service_id);
01148     uint16_t payload_len;
01149     uint8_t *payload_ptr;
01150     uint16_t network_data_len;
01151     uint8_t *network_data_ptr;
01152     protocol_interface_info_entry_t *cur;
01153     sn_coap_msg_code_e response_code = COAP_MSG_CODE_RESPONSE_CHANGED;
01154     int8_t ret = -3;
01155     uint16_t routerId;
01156     uint16_t old_rloc;
01157     (void) source_port;
01158     tr_debug("register network data");
01159 
01160     if (!this) {
01161         return -1;
01162     }
01163 
01164     cur = protocol_stack_interface_info_get_by_id(this->interface_id);
01165     if (!cur || !cur->thread_info) {
01166         return -1;
01167     }
01168 
01169     //send data to leader api.
01170     routerId = common_read_16_bit(&source_address[14]);
01171     payload_len = request_ptr->payload_len;
01172     payload_ptr = request_ptr->payload_ptr;
01173     request_ptr->payload_ptr = NULL;
01174 
01175 
01176     if (2 <= thread_tmfcop_tlv_data_get_uint16(payload_ptr, payload_len, TMFCOP_TLV_RLOC16, &old_rloc)) {
01177         // This will delete everything from old rloc
01178         ret = thread_leader_service_network_data_register(cur, NULL, 0, old_rloc);
01179         tr_warn("Removed network data from: %x", old_rloc);
01180     }
01181 
01182     if (!thread_tmfcop_tlv_exist(payload_ptr, payload_len, TMFCOP_TLV_NETWORK_DATA)) {
01183         goto send_response;
01184     }
01185     network_data_len = thread_tmfcop_tlv_find(payload_ptr, payload_len, TMFCOP_TLV_NETWORK_DATA, &network_data_ptr);
01186 
01187     /* Check that network data does NOT produce Network Data TLV bigger than MAX_NETWORK_DATA_SIZE */
01188     int32_t len = thread_network_data_resulting_tlv_size(&cur->thread_info->networkDataStorage, network_data_ptr, network_data_len, routerId);
01189 
01190     if (len <= 0 || len > THREAD_MAX_NETWORK_DATA_SIZE) {
01191         tr_warn("Network data would produce Network Data TLV size %"PRId32, len);
01192         response_code = COAP_MSG_CODE_RESPONSE_BAD_REQUEST;
01193         goto send_response;
01194     }
01195 
01196     if (network_data_len < 256) {
01197         ret = thread_leader_service_network_data_register(cur, network_data_ptr, network_data_len, routerId);
01198     }
01199     if (ret != 0) {
01200         /* error happened*/
01201         response_code = COAP_MSG_CODE_RESPONSE_BAD_REQUEST; // corruption is the default or not enough room
01202         if (ret == -2) {
01203             response_code = COAP_MSG_CODE_RESPONSE_FORBIDDEN;    // Not leader
01204         }
01205     }
01206 
01207 send_response:
01208     coap_service_response_send(this->coap_service_id, COAP_REQUEST_OPTIONS_NONE, request_ptr, response_code, COAP_CT_OCTET_STREAM, NULL, 0);
01209     return 0;
01210 }
01211 
01212 static int thread_leader_service_leader_start(protocol_interface_info_entry_t *cur)
01213 {
01214     link_configuration_s *linkConfiguration;
01215     linkConfiguration = thread_joiner_application_get_config(cur->id);
01216     if (!linkConfiguration) {
01217         return -1;
01218     }
01219 
01220     //Allocate Setup
01221     if (thread_leader_service_leader_data_allocate(cur) != 0) {
01222         return -1;
01223     }
01224 
01225     if (thread_leader_service_start(cur->id)) {
01226         thread_nd_service_delete(cur->id);
01227         return -1;
01228     }
01229     if (thread_beacon_create_payload(cur) != 0) {
01230         thread_nd_service_delete(cur->id);
01231         return -1;
01232     }
01233     cur->thread_info->rfc6775 = false;
01234     cur->thread_info->leader_private_data->leader_id_seq_timer = ID_SEQUENCE_PERIOD;
01235 
01236     cur->lowpan_info &= ~INTERFACE_NWK_BOOTSRAP_PANA_AUTHENTICATION;
01237     cur->lowpan_info |= INTERFACE_NWK_ROUTER_DEVICE;
01238     cur->lowpan_info &= ~INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE;
01239     mac_helper_default_security_level_set(cur, SEC_ENC_MIC32);
01240     mac_helper_default_security_key_id_mode_set(cur,MAC_KEY_ID_MODE_IDX);
01241     thread_discovery_responser_enable(cur->id, true);
01242     if (cur->mac_api) {
01243         mlme_start_t start_req;
01244         memset(&start_req, 0, sizeof(mlme_start_t));
01245         start_req.BeaconOrder = 15;
01246         start_req.SuperframeOrder = 15;
01247         start_req.PANCoordinator = true;
01248         start_req.LogicalChannel = cur->mac_parameters->mac_channel;
01249         start_req.PANId = cur->mac_parameters->pan_id;
01250         cur->mac_api->mlme_req(cur->mac_api, MLME_START , &start_req);
01251     }
01252     return 0;
01253 }
01254 
01255 static int thread_leader_service_leader_init(protocol_interface_info_entry_t *cur)
01256 {
01257     //Clean All allocated stuff's
01258     thread_info_t *thread_info = cur->thread_info;
01259     thread_leader_service_leader_data_free(thread_info);
01260 
01261     thread_info->rfc6775 = false;
01262     thread_routing_free(&thread_info->routing);
01263     ipv6_route_table_remove_info(cur->id, ROUTE_THREAD, NULL);
01264     ipv6_route_table_remove_info(cur->id, ROUTE_THREAD_BORDER_ROUTER, NULL);
01265     thread_dhcp_client_delete(cur->id);
01266     thread_nd_service_delete(cur->id);
01267     mpl_clear_realm_scope_seeds(cur);
01268     ipv6_neighbour_cache_flush(&cur->ipv6_neighbour_cache);
01269     thread_clean_all_routers_from_neighbor_list(cur->id);
01270     cur->mesh_callbacks = NULL;
01271     cur->lowpan_info &= ~INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE;
01272 
01273     return thread_leader_service_leader_start(cur);
01274 }
01275 
01276 static void thread_leader_service_leader_data_initialize(thread_leader_data_t *leader_data, uint8_t routerId)
01277 {
01278     if (leader_data) {
01279         leader_data->partitionId = randLIB_get_32bit(); //Generate Random Instance
01280         leader_data->leaderRouterId = routerId; //Set leader data to zero by Default
01281         leader_data->dataVersion = randLIB_get_8bit();
01282         leader_data->stableDataVersion = randLIB_get_8bit();
01283         leader_data->weighting = THREAD_DEFAULT_WEIGHTING;
01284     }
01285 }
01286 
01287 static void thread_leader_service_private_routemask_init(thread_leader_info_t *leader_info)
01288 {
01289     memset(leader_info, 0, sizeof(thread_leader_info_t));
01290     leader_info->maskSeq = randLIB_get_8bit();
01291     uint8_t i;
01292     for (i=0; i<64; i++) {
01293         leader_info->thread_router_id_list[i].reUsePossible = true;
01294         leader_info->thread_router_id_list[i].validLifeTime = 0xffffffff;
01295     }
01296 }
01297 
01298 static void thread_leader_service_interface_setup_activate(protocol_interface_info_entry_t *cur)
01299 {
01300     uint8_t routerId;
01301     thread_leader_info_t *private = cur->thread_info->leader_private_data;
01302 
01303     routerId = thread_router_id_from_addr(cur->thread_info->routerShortAddress);
01304 
01305     thread_leader_service_leader_data_initialize(cur->thread_info->thread_leader_data, routerId);
01306     // Test code
01307     if(cur->thread_info->testRandomPartitionId != 0){
01308         cur->thread_info->thread_leader_data->partitionId = cur->thread_info->testRandomPartitionId;
01309         cur->thread_info->testRandomPartitionId = 0;
01310     }
01311     thread_leader_service_private_routemask_init(private);
01312     //SET Router ID
01313     thread_leader_allocate_router_id_by_allocated_id(private, routerId, cur->mac);
01314     thread_old_partition_data_purge(cur->thread_info);
01315     cur->lowpan_address_mode = NET_6LOWPAN_GP16_ADDRESS;
01316     thread_bootstrap_update_ml16_address(cur, cur->thread_info->routerShortAddress);
01317     thread_generate_ml64_address(cur);
01318     thread_extension_address_generate(cur);
01319     thread_bootstrap_routing_activate(cur);
01320     thread_routing_update_id_set(cur, private->maskSeq, private->master_router_id_mask);
01321     thread_routing_activate(&cur->thread_info->routing);
01322     cur->thread_info->leader_private_data->leader_id_seq_timer = ID_SEQUENCE_PERIOD;
01323 }
01324 
01325 /*
01326  * Function to clear network data from router children.
01327  * When router is removed, network data from router child must be removed as well.
01328  */
01329 static void thread_leader_router_child_network_data_clear(protocol_interface_info_entry_t *cur, uint16_t router_short_addr)
01330 {
01331     uint16_t child_short_addr;
01332 
01333     child_short_addr = thread_leader_service_child_id_from_networkdata_get(cur->thread_info, router_short_addr);
01334     while (child_short_addr != 0) {
01335         tr_debug("Removing Network data from child %04x", child_short_addr);
01336         thread_leader_service_network_data_register(cur, NULL, 0, child_short_addr);
01337         child_short_addr = thread_leader_service_child_id_from_networkdata_get(cur->thread_info, router_short_addr);
01338     }
01339 }
01340 
01341 
01342 /* Public functions */
01343 
01344 int thread_leader_service_init(int8_t interface_id, uint8_t coap_service_id)
01345 {
01346     thread_leader_service_t *this = thread_leader_service_find_by_interface(interface_id);
01347 
01348     if (this) {
01349         return 0;
01350     }
01351 
01352     this = ns_dyn_mem_alloc(sizeof(thread_leader_service_t));
01353     if (!this) {
01354         return -2;
01355     }
01356 
01357     this->interface_id = interface_id;
01358     this->coap_service_id = coap_service_id;
01359     this->leader_enabled = false;
01360 
01361     ns_list_add_to_start(&leader_service_instance_list, this);
01362 
01363     return 0;
01364 }
01365 
01366 void thread_leader_service_delete(int8_t interface_id)
01367 {
01368     thread_leader_service_t *this = thread_leader_service_find_by_interface(interface_id);
01369     if (!this) {
01370         return;
01371     }
01372 
01373     if (this->leader_enabled) {
01374         thread_leader_service_stop(interface_id);
01375     }
01376 
01377     ns_list_remove(&leader_service_instance_list, this);
01378     ns_dyn_mem_free(this);
01379 }
01380 
01381 int thread_leader_service_start(int8_t interface_id)
01382 {
01383     thread_leader_service_t *this = thread_leader_service_find_by_interface(interface_id);
01384 
01385     if (!this) {
01386         return -1;
01387     }
01388 
01389     this->leader_enabled = true;
01390     /**
01391      * Router id uri register
01392      */
01393     coap_service_register_uri(this->coap_service_id, THREAD_URI_ROUTER_ID_ASSIGNMENT, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_assign_cb);
01394     coap_service_register_uri(this->coap_service_id, THREAD_URI_ROUTER_ID_RELEASE, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_release_cb);
01395     /**
01396      * Thread commission registration uri handling
01397      */
01398     coap_service_register_uri(this->coap_service_id, THREAD_URI_LEADER_PETITION, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_petition_cb);
01399     coap_service_register_uri(this->coap_service_id, THREAD_URI_LEADER_KEEP_ALIVE, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_petition_ka_cb);
01400     /**
01401      * Thread network data management handler
01402      */
01403     coap_service_register_uri(this->coap_service_id, THREAD_URI_NETWORK_DATA, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_register_cb);
01404     coap_service_register_uri(this->coap_service_id, THREAD_URI_ACTIVE_SET, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_active_set_cb);
01405     coap_service_register_uri(this->coap_service_id, THREAD_URI_PENDING_SET, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_pending_set_cb);
01406     coap_service_register_uri(this->coap_service_id, THREAD_URI_COMMISSIONER_SET, COAP_SERVICE_ACCESS_GET_ALLOWED, thread_leader_service_commissioner_set_cb);
01407 
01408     return 0;
01409 }
01410 
01411 void thread_leader_service_stop(int8_t interface_id)
01412 {
01413     thread_leader_service_t *this = thread_leader_service_find_by_interface(interface_id);
01414 
01415     if (!this) {
01416         return;
01417     }
01418 
01419     this->leader_enabled = false;
01420     coap_service_unregister_uri(this->coap_service_id, THREAD_URI_ROUTER_ID_ASSIGNMENT);
01421     coap_service_unregister_uri(this->coap_service_id, THREAD_URI_ROUTER_ID_RELEASE);
01422     coap_service_unregister_uri(this->coap_service_id, THREAD_URI_LEADER_PETITION);
01423     coap_service_unregister_uri(this->coap_service_id, THREAD_URI_LEADER_KEEP_ALIVE);
01424     coap_service_unregister_uri(this->coap_service_id, THREAD_URI_NETWORK_DATA);
01425     coap_service_unregister_uri(this->coap_service_id, THREAD_URI_ACTIVE_SET);
01426     coap_service_unregister_uri(this->coap_service_id, THREAD_URI_PENDING_SET);
01427     coap_service_unregister_uri(this->coap_service_id, THREAD_URI_COMMISSIONER_SET);
01428     return;
01429 }
01430 void thread_leader_service_network_data_changed(protocol_interface_info_entry_t *cur, bool force_stable_update, bool force_unstable_update)
01431 {
01432     if (!cur->thread_info) {
01433         return;
01434     }
01435 
01436     if (force_stable_update) {
01437         cur->thread_info->networkDataStorage.stableUpdatePushed = true;
01438     }
01439     if (force_unstable_update) {
01440         cur->thread_info->networkDataStorage.temporaryUpdatePushed = true;
01441     }
01442     // Generate network data from network data structures
01443     if (cur->thread_info->networkDataStorage.stableUpdatePushed) {
01444         tr_debug("Temporary Network Data Update");
01445         cur->thread_info->thread_leader_data->stableDataVersion++;
01446         cur->thread_info->thread_leader_data->dataVersion++;
01447     } else if (cur->thread_info->networkDataStorage.temporaryUpdatePushed) {
01448         tr_debug("Stable Network Data Update");
01449         cur->thread_info->thread_leader_data->dataVersion++;
01450     } else {
01451         // No changes in network data detected
01452         return;
01453     }
01454     tr_debug("network data changed stable:%d, unstable:%d", cur->thread_info->networkDataStorage.stableUpdatePushed, cur->thread_info->networkDataStorage.temporaryUpdatePushed);
01455     thread_leader_service_generate_network_data(cur);
01456 
01457     /* set timer to update new network data */
01458     if (0 == thread_info(cur)->networkDataStorage.network_data_update_delay) {
01459         thread_info(cur)->networkDataStorage.network_data_update_delay = THREAD_COMMISSION_DATA_UPDATE_DELAY;
01460     }
01461 }
01462 
01463 void thread_leader_service_timer(protocol_interface_info_entry_t *cur, uint32_t ticks)
01464 {
01465     if (!cur->thread_info->leader_private_data) {
01466         // I am not a leader
01467         return;
01468     }
01469 
01470     // check for network data update
01471     if (thread_info(cur)->networkDataStorage.network_data_update_delay) {
01472         if (thread_info(cur)->networkDataStorage.network_data_update_delay > ticks) {
01473             thread_info(cur)->networkDataStorage.network_data_update_delay -= ticks;
01474         } else {
01475             // Send New network data.
01476             thread_info(cur)->networkDataStorage.network_data_update_delay = 0;
01477             thread_bootstrap_network_data_update(cur);
01478         }
01479     }
01480     if (cur->thread_info->leader_private_data->leader_id_seq_timer) {
01481         if (cur->thread_info->leader_private_data->leader_id_seq_timer > ticks) {
01482             cur->thread_info->leader_private_data->leader_id_seq_timer -= ticks;
01483         } else {
01484             cur->thread_info->leader_private_data->leader_id_seq_timer = 0;
01485         }
01486     } else  {
01487         // Leader id sequence number updated
01488         cur->thread_info->leader_private_data->leader_id_seq_timer = ID_SEQUENCE_PERIOD;
01489 
01490         thread_leader_service_update_id_set(cur);
01491         thread_network_data_context_re_use_timer_update(&cur->thread_info->networkDataStorage, 10, &cur->lowpan_contexts);
01492         // Send update to network data if needed
01493         thread_leader_service_network_data_changed(cur, false, false);
01494     }
01495 
01496     thread_leader_service_router_id_valid_lifetime_update(cur, ticks);
01497 
01498 }
01499 
01500 void thread_leader_service_leader_data_free(thread_info_t *thread_info)
01501 {
01502     if (thread_info && thread_info->thread_leader_data) {
01503         ns_dyn_mem_free(thread_info->leader_private_data);
01504         thread_info->leader_private_data = NULL;
01505     }
01506 }
01507 
01508 void thread_leader_service_thread_partitition_generate(int8_t interface_id, bool newPartition)
01509 {
01510     uint16_t leaderRloc;
01511     protocol_interface_info_entry_t *cur;
01512     link_configuration_s *linkConfiguration;
01513     linkConfiguration = thread_joiner_application_get_config(interface_id);
01514 
01515 
01516     cur = protocol_stack_interface_info_get_by_id(interface_id);
01517     if (!cur || !cur->thread_info) {
01518         return;
01519     }
01520 
01521     if (!linkConfiguration) {
01522         goto failure_exit;
01523     }
01524 
01525     mle_service_interface_tx_queue_clean(interface_id);
01526 
01527     if (!newPartition) {
01528         if (cur->nwk_bootstrap_state == ER_ACTIVE_SCAN && cur->nwk_nd_re_scan_count < THREAD_NETWORK_ACTIVE_SCAN_COUNTER) {
01529             tr_debug("RE trig Active Scan");
01530             cur->bootsrap_state_machine_cnt = randLIB_get_random_in_range(1, 6);
01531             return;
01532         }
01533     }
01534 
01535     if (cur->thread_info->thread_device_mode != THREAD_DEVICE_MODE_ROUTER) {
01536         tr_debug("End device did not find networks");
01537         goto failure_exit;
01538     }
01539     leaderRloc = cur->thread_info->routerShortAddress;
01540 
01541     //Set For Destination cache clean purpose
01542     cur->mac_parameters->pan_id = linkConfiguration->panId;
01543 
01544     if (thread_leader_service_leader_init(cur) != 0) {
01545         tr_debug("Leader allocate Fail");
01546         goto failure_exit;
01547     }
01548 
01549     /* Delete old parent info */
01550     if (cur->thread_info->thread_endnode_parent) {
01551         ns_dyn_mem_free(cur->thread_info->thread_endnode_parent);
01552         cur->thread_info->thread_endnode_parent = NULL;
01553     }
01554 
01555     tr_debug("Leader Link generated & Started");
01556 
01557     //Run Nomal Init
01558     thread_interface_init(cur);
01559 
01560     // Init clears the address
01561     cur->thread_info->routerShortAddress = leaderRloc;
01562 
01563     if (cur->thread_info->routerShortAddress == 0xfffe ||
01564         !thread_is_router_addr(cur->thread_info->routerShortAddress)) {
01565         //If we were REED do not use reed address
01566         cur->thread_info->routerShortAddress = thread_router_addr_from_id(randLIB_get_random_in_range(0,MAX_ROUTER_ID));
01567     }
01568 
01569     // clean network data
01570     thread_network_data_free_and_clean(&cur->thread_info->networkDataStorage);
01571     thread_network_data_base_init(&cur->thread_info->networkDataStorage);
01572     thread_leader_service_interface_setup_activate(cur);
01573 
01574     //memset(cur->thread_info->lastValidRouteMask, 0, (N_THREAD_ROUTERS+7)/8);
01575     //SET SHort Address & PAN-ID
01576     cur->lowpan_info &= ~INTERFACE_NWK_CONF_MAC_RX_OFF_IDLE;
01577     mle_class_mode_set(cur->id, MLE_CLASS_ROUTER);
01578     mac_helper_pib_boolean_set(cur, macRxOnWhenIdle, true);
01579     cur->interface_mode = INTERFACE_UP;
01580     cur->nwk_mode = ARM_NWK_GP_IP_MODE;
01581     thread_bootstrap_ready(cur);
01582     thread_configuration_mle_activate(cur);
01583     thread_nvm_store_link_info_file_write(cur);
01584 
01585     if (thread_nd_own_service_list_data_size(&cur->thread_info->localServerDataBase)) {
01586         // We publish our services if we have some BUG leader cannot remove old ones
01587         thread_border_router_publish(cur->id);
01588     }
01589     return;
01590 
01591 failure_exit:
01592     nwk_bootsrap_state_update(ARM_NWK_NWK_SCAN_FAIL, cur);
01593 }
01594 
01595 int thread_leader_service_thread_partitition_restart(int8_t interface_id, mle_tlv_info_t *routing)
01596 {
01597     protocol_interface_info_entry_t *cur;
01598     cur = protocol_stack_interface_info_get_by_id(interface_id);
01599     if (!cur || !cur->thread_info) {
01600         return -1;
01601     }
01602     if (!thread_info(cur)->leader_private_data) {
01603         thread_info(cur)->leader_private_data = thread_allocate_and_init_leader_private_data();
01604     }
01605     if (!thread_info(cur)->leader_private_data) {
01606         return -1;
01607     }
01608     if (thread_leader_service_start(cur->id)) {
01609         return -1;
01610     }
01611     //Learn network data, we remove own data from here it should be re given by application
01612     //thread_management_network_data_register(cur->id, networkData.dataPtr, networkData.tlvLen, address16 );
01613 
01614     // initialize private data
01615     thread_info(cur)->leader_private_data->maskSeq = *routing->dataPtr;
01616     memcpy(thread_info(cur)->leader_private_data->master_router_id_mask,routing->dataPtr + 1,8);
01617     for (int i = 0; i < 64; i++) {
01618         memset(thread_info(cur)->leader_private_data->thread_router_id_list[i].eui64,0,8);
01619         if (bit_test(thread_info(cur)->leader_private_data->master_router_id_mask, i)) {
01620             //Active ID
01621             thread_info(cur)->leader_private_data->thread_router_id_list[i].reUsePossible = false;
01622 
01623         } else {
01624             // Free id
01625             thread_info(cur)->leader_private_data->thread_router_id_list[i].reUsePossible = true;
01626         }
01627         thread_info(cur)->leader_private_data->thread_router_id_list[i].validLifeTime = 0xffffffff;
01628     }
01629     // Clear network data (if exists) and propagate new empty network data
01630     thread_network_data_free_and_clean(&cur->thread_info->networkDataStorage);
01631     thread_network_data_base_init(&cur->thread_info->networkDataStorage);
01632     thread_leader_service_network_data_changed(cur, true, true);
01633     return 0;
01634 }
01635 
01636 void thread_leader_service_update_id_set(protocol_interface_info_entry_t *cur)
01637 {
01638     thread_leader_info_t *leader_private_ptr = cur->thread_info->leader_private_data;
01639 
01640     leader_private_ptr->maskSeq++;
01641     thread_routing_update_id_set(cur, leader_private_ptr->maskSeq, leader_private_ptr->master_router_id_mask);
01642 }
01643 
01644 void thread_leader_service_router_id_valid_lifetime_update(protocol_interface_info_entry_t *cur, uint32_t tickUpdate)
01645 {
01646     uint8_t i;
01647     bool update_routeTable = false;
01648     thread_info_t *thread_info = cur->thread_info;
01649     thread_leader_info_t *leader_private_ptr = 0;
01650     if (thread_info && thread_info->leader_private_data) {
01651         leader_private_ptr = thread_info->leader_private_data;
01652 
01653         for (i = 0; i < 64; i++) {
01654             if (bit_test(leader_private_ptr->master_router_id_mask, i)) {
01655                 if (i != thread_info->thread_leader_data->leaderRouterId) {
01656                     //Active ID
01657                     if (leader_private_ptr->thread_router_id_list[i].validLifeTime != 0xffffffff) {
01658                         if (leader_private_ptr->thread_router_id_list[i].validLifeTime > tickUpdate) {
01659                             //Increment Time
01660                             leader_private_ptr->thread_router_id_list[i].validLifeTime -= tickUpdate;
01661                         } else {
01662                             //RElease
01663                             tr_debug("Router ID bit Release: %x start re assign delay", i);
01664                             leader_private_ptr->thread_router_id_list[i].validLifeTime = ROUTER_ID_REUSE_DELAY;
01665                             leader_private_ptr->thread_router_id_list[i].reUsePossible = false;
01666                             thread_leader_service_route_mask_bit_clear(leader_private_ptr, i);
01667                             thread_leader_service_network_data_register(cur, NULL, 0, thread_router_addr_from_id(i));
01668                             thread_leader_router_child_network_data_clear(cur, thread_router_addr_from_id(i));
01669                             thread_routing_activate(&thread_info->routing);
01670                             update_routeTable = true;
01671                         }
01672                     }
01673                 }
01674             } else {
01675                 if (leader_private_ptr->thread_router_id_list[i].validLifeTime != 0xffffffff) {
01676                     if (leader_private_ptr->thread_router_id_list[i].validLifeTime > tickUpdate) {
01677                         //Increment Time
01678                         leader_private_ptr->thread_router_id_list[i].validLifeTime -= tickUpdate;
01679                     } else {
01680                         //RElease
01681                         tr_debug("Router ID available again: %x", i);
01682                         leader_private_ptr->thread_router_id_list[i].validLifeTime = 0xffffffff;
01683                         leader_private_ptr->thread_router_id_list[i].reUsePossible = true;
01684                     }
01685                 }
01686 
01687             }
01688         }
01689         if (update_routeTable) {
01690             thread_leader_service_update_id_set(cur);
01691         }
01692     }
01693 }
01694 
01695 bool thread_leader_service_route_mask_bit_clear(thread_leader_info_t *info, uint8_t router_id)
01696 {
01697     if (bit_test(info->master_router_id_mask, router_id)) {
01698         bit_clear(info->master_router_id_mask, router_id);
01699         return true;
01700     }
01701     return false;
01702 }
01703 
01704 static uint8_t thread_unstable_commission_data_length(protocol_interface_info_entry_t *cur)
01705 {
01706     uint8_t localDataLength = 0;
01707 
01708     localDataLength += 4; // Commissioner session ID
01709 
01710     if (cur->thread_info->registered_commissioner.commissioner_valid) {
01711         localDataLength += 4; // Border router address
01712         if (cur->thread_info->registered_commissioner.steering_data_len) {
01713             localDataLength += cur->thread_info->registered_commissioner.steering_data_len;
01714             localDataLength += 2;
01715         }
01716     }
01717     return localDataLength;
01718 }
01719 
01720 static uint8_t *thread_unstable_commission_data_write(protocol_interface_info_entry_t *cur, uint8_t *ptr)
01721 {
01722 
01723     uint8_t length = thread_unstable_commission_data_length(cur);
01724     if( !length) {
01725         // No unstable TLV in thread 1.0 or if TLV is empty we skip it
01726         return ptr;
01727     }
01728     *ptr++ = THREAD_NWK_DATA_TYPE_COMMISSION_DATA ;
01729     *ptr++ = length;
01730     ptr = thread_meshcop_tlv_data_write_uint16(ptr,MESHCOP_TLV_COMMISSIONER_SESSION_ID, cur->thread_info->registered_commissioner.session_id);
01731     if (cur->thread_info->registered_commissioner.commissioner_valid) {
01732         //SET Commision TLV
01733         ptr = thread_meshcop_tlv_data_write(ptr,MESHCOP_TLV_BORDER_ROUTER_LOCATOR, 2, &cur->thread_info->registered_commissioner.border_router_address[14]);
01734         if (cur->thread_info->registered_commissioner.steering_data_len) {
01735             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);
01736         }
01737     }
01738     return ptr;
01739 }
01740 
01741 static uint16_t thread_leader_service_network_data_length(protocol_interface_info_entry_t *cur, bool fullist)
01742 {
01743     uint16_t networkDataLength = 0;
01744     uint16_t prefixDataLength = 0;
01745     uint16_t commissionDataLength = 0;
01746 
01747     prefixDataLength = thread_network_data_prefix_set_size(&cur->thread_info->networkDataStorage, fullist);
01748     if (prefixDataLength) {
01749         networkDataLength += prefixDataLength;
01750     }
01751     prefixDataLength = thread_network_data_service_set_size(&cur->thread_info->networkDataStorage, fullist);
01752     if (prefixDataLength) {
01753         networkDataLength += prefixDataLength;
01754     }
01755     commissionDataLength = thread_unstable_commission_data_length(cur);
01756     if (fullist && commissionDataLength) {
01757         networkDataLength += 2 + commissionDataLength;
01758     }
01759 
01760     return networkDataLength;
01761 }
01762 
01763 static uint8_t *thread_leader_service_network_data_tlv_write(protocol_interface_info_entry_t *cur, uint8_t *ptr)
01764 {
01765     ptr = thread_network_data_prefix_set_write(&cur->thread_info->networkDataStorage, ptr);
01766     ptr = thread_network_data_service_set_write(&cur->thread_info->networkDataStorage, ptr);
01767     ptr = thread_unstable_commission_data_write(cur, ptr);
01768 
01769     return ptr;
01770 }
01771 
01772 void thread_leader_service_generate_network_data(protocol_interface_info_entry_t *cur)
01773 {
01774     int16_t size = thread_leader_service_network_data_length(cur, true);
01775 
01776     if (size > 0) {
01777         tr_debug("Leader network data size %d", size);
01778         thread_leader_service_network_data_tlv_write(cur, cur->thread_info->networkDataStorage.network_data);
01779         cur->thread_info->networkDataStorage.network_data_len = size;
01780     }
01781 }
01782 
01783 uint16_t thread_leader_service_child_id_from_networkdata_get(thread_info_t *thread_info, uint16_t router_short_addr)
01784 {
01785     return thread_network_data_service_child_id_from_networkdata_get(&thread_info->networkDataStorage, router_short_addr);
01786 }
01787 
01788 void thread_leader_service_router_state_changed(thread_info_t *thread_info, uint8_t router_id, bool available, int8_t interface_id)
01789 {
01790     thread_leader_info_t *info = thread_info->leader_private_data;
01791     (void)interface_id;
01792 
01793     if (!info) {
01794         return;
01795     }
01796     if (available) {
01797         /* Stop the timer */
01798         info->thread_router_id_list[router_id].validLifeTime = 0xffffffff;
01799     } else {
01800         /* Reset the timer to ROUTER_ID_REUSE_DELAY, if it's not already running. */
01801         if (info->thread_router_id_list[router_id].validLifeTime == 0xffffffff) {
01802             tr_debug("Router ID: %d cost infinite", router_id);
01803 
01804             info->thread_router_id_list[router_id].validLifeTime = ROUTER_ID_INFINITY_DELAY;
01805             info->thread_router_id_list[router_id].reUsePossible = false;
01806         }
01807     }
01808 }
01809 
01810 
01811 #endif // HAVE_THREAD_LEADER_SERVICE */