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