thread_common.c

00001 /*
00002  * Copyright (c) 2014-2015, 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 #include "nsconfig.h"
00030 #ifdef HAVE_THREAD
00031 #include <string.h>
00032 #include <ns_types.h>
00033 #include <nsdynmemLIB.h>
00034 #include "eventOS_event.h"
00035 #include "randLIB.h"
00036 #include "common_functions.h"
00037 
00038 #include "NWK_INTERFACE/Include/protocol.h"
00039 #include "net_thread_test.h"
00040 #include "libDHCPv6/libDHCPv6.h"
00041 #include "libDHCPv6/libDHCPv6_server.h"
00042 #include "ns_trace.h"
00043 #include "6LoWPAN/Bootstraps/protocol_6lowpan.h"
00044 #include "6LoWPAN/Bootstraps/protocol_6lowpan_interface.h"
00045 #include "6LoWPAN/Thread/thread_common.h"
00046 #include "6LoWPAN/Thread/thread_beacon.h"
00047 #include "6LoWPAN/Thread/thread_leader_service.h"
00048 #include "6LoWPAN/Thread/thread_routing.h"
00049 #include "6LoWPAN/Thread/thread_dhcpv6_client.h"
00050 #include "6LoWPAN/Thread/thread_discovery.h"
00051 #include "6LoWPAN/Thread/thread_bootstrap.h"
00052 #include "6LoWPAN/Thread/thread_router_bootstrap.h"
00053 #include "6LoWPAN/Thread/thread_lowpower_private_api.h"
00054 #include "6LoWPAN/Thread/thread_extension.h"
00055 #include "6LoWPAN/Thread/thread_bbr_api_internal.h"
00056 #include "6LoWPAN/Thread/thread_border_router_api_internal.h"
00057 #include "6LoWPAN/Thread/thread_nd.h"
00058 #include "6LoWPAN/Thread/thread_network_data_lib.h"
00059 #include "6LoWPAN/Thread/thread_joiner_application.h"
00060 #include "6LoWPAN/Thread/thread_management_internal.h"
00061 #include "6LoWPAN/Thread/thread_management_client.h"
00062 #include "6LoWPAN/Thread/thread_management_server.h"
00063 #include "6LoWPAN/Thread/thread_resolution_client.h"
00064 #include "6LoWPAN/Thread/thread_address_registration_client.h"
00065 #include "6LoWPAN/Thread/thread_resolution_client.h"
00066 #include <6LoWPAN/Thread/thread_extension_bootstrap.h>
00067 #include "MLE/mle.h"
00068 #include "Service_Libs/mle_service/mle_service_security.h"
00069 #include "Service_Libs/blacklist/blacklist.h"
00070 #include "6LoWPAN/Thread/thread_network_synch.h"
00071 #include "6LoWPAN/Thread/thread_config.h"
00072 #include "6LoWPAN/Thread/thread_tmfcop_lib.h"
00073 #include "thread_meshcop_lib.h"
00074 #include "thread_management_if.h"
00075 #include "ipv6_stack/protocol_ipv6.h"
00076 #include "Common_Protocols/ipv6.h"
00077 #include "Common_Protocols/icmpv6.h"
00078 #include "MLE/mle.h"
00079 #include "MLE/mle_tlv.h"
00080 #include "Service_Libs/nd_proxy/nd_proxy.h"
00081 #include "Service_Libs/mle_service/mle_service_api.h"
00082 #include "6LoWPAN/MAC/mac_helper.h"
00083 #include "6LoWPAN/MAC/mac_pairwise_key.h"
00084 #include "6LoWPAN/MAC/mac_data_poll.h"
00085 #include "Service_Libs/etx/etx.h"
00086 #include "Core/include/address.h"
00087 #include "6LoWPAN/Thread/thread_nvm_store.h"
00088 
00089 #define TRACE_GROUP "thrd"
00090 
00091 #define ID_MASK_UPDATE_MIN (10 * 10) /* 1 minute */
00092 #define LEADER_DATA_UPDATE_MIN (10 * 10) /* 10 seconds */
00093 
00094 /* Parent priority bits in Connectivity TLV flags byte */
00095 #define CONNECTIVITY_PP_MASK     0xC0    // Parent priority
00096 #define CONNECTIVITY_PP_LOW      0xC0
00097 #define CONNECTIVITY_PP_MEDIUM   0x00
00098 #define CONNECTIVITY_PP_HIGH     0x40
00099 #define CONNECTIVITY_PP_INVALID  0x80
00100 
00101 uint8_t thread_version = THREAD_PROTOCOL_VERSION;
00102 
00103 thread_leader_data_t *thread_leader_data_generate(void);
00104 thread_parent_info_t *thread_parent_data_allocate(thread_info_t *info);
00105 static uint8_t * thread_joining_port_tlv_write(uint16_t port, uint8_t *ptr);
00106 static uint8_t * thread_commissioner_port_tlv_write(uint16_t port, uint8_t *ptr);
00107 static void thread_tx_failure_handler(int8_t nwk_id, uint8_t accumulated_failures, mle_neigh_table_entry_t *neighbor);
00108 static void thread_address_notification_cb(struct protocol_interface_info_entry *interface, const struct if_address_entry *addr, if_address_callback_t reason);
00109 
00110 /* Helper functions*/
00111 /* Ceil log2 function */
00112 uint16_t thread_log2_aprx(uint32_t n)
00113 {
00114     if (n == 0) {
00115         return 0;
00116     }
00117     uint16_t val = 0;
00118     --n;
00119     while (n > 0 && val < 255) {
00120         ++val;
00121         n >>= 1;
00122     }
00123     return val;
00124 }
00125 
00126 
00127 
00128 void thread_anycast_address_policy_update(const thread_info_t *thread_info, bool addPolicy)
00129 {
00130     uint8_t ipv6_address[16] = {0};
00131 
00132     thread_addr_write_mesh_local_16(ipv6_address, 0xfc00, thread_info);
00133 
00134     if (addPolicy) {
00135         tr_debug("Thread Add AnycastAddress Policy");
00136         addr_policy_table_add_entry(ipv6_address, 118, 20, 40);
00137     } else {
00138         addr_policy_table_delete_entry(ipv6_address, 118);
00139     }
00140 }
00141 
00142 /**
00143  * Thread key request for MLE Message
00144  *
00145  */
00146 uint8_t *thread_management_key_request_with_sequence(int8_t interface_id, uint8_t keyId, uint32_t keySequnce)
00147 {
00148     protocol_interface_info_entry_t *cur;
00149     uint8_t *keyPtr = NULL;
00150     link_configuration_s *linkConfiguration;
00151     linkConfiguration = thread_joiner_application_get_config(interface_id);
00152     if (!linkConfiguration) {
00153         return NULL;
00154     }
00155 
00156     cur = protocol_stack_interface_info_get_by_id(interface_id);
00157     if (cur && cur->thread_info) {
00158         if (cur->thread_info->masterSecretMaterial.valid_Info) {
00159             if (keySequnce == linkConfiguration->key_sequence) {
00160                 if (mle_service_security_default_key_id_get(interface_id) == keyId) {
00161                     keyPtr =  mle_service_security_default_key_get(interface_id);
00162                 }
00163             } else if (keySequnce == (linkConfiguration->key_sequence + 1)) {
00164                 if (mle_service_security_next_key_id_get(interface_id) == keyId) {
00165                     keyPtr =  mle_service_security_next_key_get(interface_id);
00166                 }
00167             }
00168 
00169             if (!keyPtr) {
00170                 tr_debug("Gen temporary key id %"PRIu8" seq %"PRIu32, keyId, keySequnce);
00171                 thread_key_get(linkConfiguration->master_key, cur->thread_info->masterSecretMaterial.historyKey, keySequnce);
00172                 cur->thread_info->masterSecretMaterial.historyKeyId = keyId;
00173                 cur->thread_info->masterSecretMaterial.historyKeyValid = false;
00174                 keyPtr = cur->thread_info->masterSecretMaterial.historyKey;
00175             }
00176         }
00177     }
00178     return keyPtr;
00179 }
00180 uint8_t * thread_mle_service_security_notify_cb(int8_t interface_id, mle_security_event_t event, uint8_t keyId)
00181 {
00182     protocol_interface_info_entry_t *interface = protocol_stack_interface_info_get_by_id(interface_id);
00183     if (!interface) {
00184         return NULL;
00185     }
00186     switch (event) {
00187         case MLE_SEC_MAX_FRAME_COUNTER_REACHED :
00188 
00189             break;
00190         case MLE_SEC_KEY_UPDATE_NOTIFY :
00191 
00192             mac_helper_security_key_swap_next_to_default(interface);
00193 
00194             if (thread_info(interface)) {
00195                 thread_security_update_from_mac(interface);
00196             }
00197             break;
00198 
00199         case MLE_SEC_UNKNOWN_KEY :
00200             return thread_management_key_request(interface_id,keyId);
00201     }
00202     return NULL;
00203 }
00204 int8_t thread_bootstrap_up(protocol_interface_info_entry_t *cur)
00205 {
00206     int8_t ret_val = -1;
00207 
00208     if (!cur) {
00209         return -1;
00210     }
00211 
00212     if ((cur->configure_flags & INTERFACE_SETUP_MASK) != INTERFACE_SETUP_READY) {
00213         tr_debug("Interface not yet fully configured");
00214         return -5;
00215     }
00216 
00217     protocol_6lowpan_register_handlers(cur);
00218     addr_interface_set_ll64(cur, NULL);
00219     thread_interface_up(cur);
00220     ret_val = nwk_6lowpan_up(cur);
00221 
00222     cur->nwk_nd_re_scan_count = 0;
00223 
00224     return ret_val;
00225 }
00226 
00227 int8_t thread_bootstrap_down(protocol_interface_info_entry_t *cur)
00228 {
00229     if (!cur || !(cur->lowpan_info & INTERFACE_NWK_ACTIVE)) {
00230         return -1;
00231     }
00232 
00233     tr_debug("SET thread Idle");
00234     //stop polling
00235     mac_data_poll_disable(cur);
00236     //Clean mle table
00237     thread_neighbor_list_clean(cur);
00238     // store frame counters
00239     if (cur->thread_info) {
00240         thread_nvm_fast_data_t fast_data;
00241         memset(&fast_data,0,sizeof(thread_nvm_fast_data_t));
00242         link_configuration_s *linkConfiguration = thread_joiner_application_get_config(cur->id);
00243         if(linkConfiguration) {
00244             fast_data.seq_counter = linkConfiguration->key_sequence;
00245         }
00246         mac_helper_link_frame_counter_read(cur->id, &fast_data.mac_frame_counter);
00247         fast_data.mle_frame_counter=mle_service_security_get_frame_counter(cur->id);
00248         thread_nvm_store_fast_data_write(&fast_data);
00249         thread_joiner_application_configuration_nvm_save(cur->id);
00250         mac_pairwise_key_flush_list(cur->id);
00251         thread_discovery_reset(cur->id);
00252         thread_leader_mleid_rloc_map_to_nvm_write(cur);
00253         thread_bootstrap_stop(cur);
00254         mle_service_interface_unregister(cur->id);
00255         thread_management_server_delete(cur->id);
00256         thread_joiner_application_deinit(cur->id);
00257         thread_management_client_delete(cur->id);
00258         //free network Data
00259         thread_network_data_free_and_clean(&cur->thread_info->networkDataStorage);
00260         //free local also here
00261         thread_network_local_data_free_and_clean(&cur->thread_info->localServerDataBase, cur->id);
00262         thread_network_data_base_init(&cur->thread_info->networkDataStorage);
00263         cur->thread_info->thread_attached_state = THREAD_STATE_NETWORK_DISCOVER;
00264     }
00265 
00266     if (nd_proxy_downstream_interface_unregister(cur->id) == -1) {
00267         tr_warn("nd proxy unregister failed");
00268     }
00269     return nwk_6lowpan_down(cur);
00270 }
00271 
00272 
00273 bool thread_addr_is_mesh_local(const uint8_t *addr, const protocol_interface_info_entry_t *cur)
00274 {
00275     thread_info_t *info = cur ? cur->thread_info : NULL;
00276     if (info && info->threadPrivatePrefixInfo.ulaValid) {
00277         return memcmp(info->threadPrivatePrefixInfo.ulaPrefix, addr, 8) == 0;
00278     }
00279     return false;
00280 }
00281 
00282 bool thread_on_mesh_route_possible_add(thread_attach_device_mode_e threadMode)
00283 {
00284     bool addRoute;
00285     if ((threadMode == THREAD_DEVICE_MODE_SLEEPY_END_DEVICE) || (threadMode == THREAD_DEVICE_MODE_END_DEVICE)) {
00286         addRoute = false;
00287     } else {
00288         addRoute = true;
00289     }
00290 
00291     return addRoute;
00292 }
00293 
00294 bool thread_addr_is_mesh_local_16(const uint8_t *addr, const protocol_interface_info_entry_t *cur)
00295 {
00296     return thread_addr_is_mesh_local(addr, cur) && memcmp(addr + 8, ADDR_SHORT_ADR_SUFFIC, 6) == 0;
00297 }
00298 
00299 uint8_t *thread_addr_write_mesh_local_16(uint8_t ip_addr_out[16], uint16_t addr16, const thread_info_t *info)
00300 {
00301     if (!info->threadPrivatePrefixInfo.ulaValid) {
00302         return NULL;
00303     }
00304     memcpy(ip_addr_out + 8, ADDR_SHORT_ADR_SUFFIC, 6);
00305     common_write_16_bit(addr16, ip_addr_out + 14);
00306     return memcpy(ip_addr_out, info->threadPrivatePrefixInfo.ulaPrefix, 8);
00307 }
00308 
00309 bool thread_leader_data_parse(uint8_t *ptr, uint16_t dataLength, thread_leader_data_t *leaderDataBuf)
00310 {
00311     mle_tlv_info_t mle_tlv_info;
00312     if (mle_tlv_option_discover(ptr, dataLength, MLE_TYPE_LEADER_DATA, &mle_tlv_info) == 8) {
00313         uint8_t *t_ptr = mle_tlv_info.dataPtr;
00314         leaderDataBuf->partitionId = common_read_32_bit(t_ptr);
00315         t_ptr += 4;
00316         leaderDataBuf->weighting = *t_ptr++;
00317         leaderDataBuf->dataVersion = *t_ptr++;
00318         leaderDataBuf->stableDataVersion = *t_ptr++;
00319         leaderDataBuf->leaderRouterId = *t_ptr;
00320         return true;
00321     }
00322     return false;
00323 }
00324 
00325 bool thread_connectivity_tlv_parse(uint8_t *ptr, uint16_t dataLength, thread_connectivity_t *connectivityTlv)
00326 {
00327     mle_tlv_info_t mle_tlv_info;
00328     int tlv_length = mle_tlv_option_discover(ptr, dataLength, MLE_TYPE_CONNECTIVITY, &mle_tlv_info);
00329     if (tlv_length >= 7) {
00330         uint8_t *t_ptr = mle_tlv_info.dataPtr;
00331         uint8_t flags = *t_ptr++;
00332         switch (flags & CONNECTIVITY_PP_MASK)  {
00333             case CONNECTIVITY_PP_LOW:
00334                 connectivityTlv->parentPriority = THREAD_CONNECTIVITY_TLV_PARENT_PRIORITY_LOW;
00335                 break;
00336             case CONNECTIVITY_PP_HIGH:
00337                 connectivityTlv->parentPriority = THREAD_CONNECTIVITY_TLV_PARENT_PRIORITY_HIGH;
00338                 break;
00339             case CONNECTIVITY_PP_MEDIUM:
00340             default:
00341                 connectivityTlv->parentPriority = THREAD_CONNECTIVITY_TLV_PARENT_PRIORITY_MEDIUM;
00342                 break;
00343         }
00344         connectivityTlv->linkQuality3 = *t_ptr++;
00345         connectivityTlv->linkQuality2 = *t_ptr++;
00346         connectivityTlv->linkQuality1 = *t_ptr++;
00347         connectivityTlv->leaderCost = *t_ptr++;
00348         connectivityTlv->idSequence = *t_ptr++;
00349         connectivityTlv->activeRouters = *t_ptr++;
00350         if (tlv_length >= 10) {
00351             connectivityTlv->SEDBufferSize = common_read_16_bit(t_ptr);
00352             t_ptr += 2;
00353             connectivityTlv->SEDDatagramCount = *t_ptr++;
00354         } else {
00355             connectivityTlv->SEDBufferSize = THREAD_SED_BUFFER_MIN_SIZE;
00356             connectivityTlv->SEDDatagramCount = THREAD_SED_DATAGRAM_MIN_COUNT;
00357         }
00358         return true;
00359     }
00360     return false;
00361 }
00362 
00363 void thread_calculate_key_guard_timer(protocol_interface_info_entry_t *cur, link_configuration_s *linkConfiguration, bool is_init)
00364 {
00365     uint32_t key_rotation = linkConfiguration ? linkConfiguration->key_rotation : 0;
00366 
00367     if (is_init && key_rotation < 3600) {
00368         tr_warn("Attempted to set key rotation time smaller than 1 hour.");
00369         key_rotation = 3600;
00370     }
00371 
00372     cur->thread_info->masterSecretMaterial.keyRotation = key_rotation;
00373     cur->thread_info->masterSecretMaterial.keySwitchGuardTimer = is_init ? 0 : (key_rotation * 0.93);
00374 }
00375 
00376 thread_leader_data_t *thread_leader_data_generate(void)
00377 {
00378     thread_leader_data_t *leader_data;
00379     leader_data = ns_dyn_mem_alloc(sizeof(thread_leader_data_t));
00380     return leader_data;
00381 }
00382 
00383 thread_leader_data_t *thread_leader_data_get(thread_info_t *info)
00384 {
00385     if (info->thread_leader_data == NULL) {
00386         info->thread_leader_data = thread_leader_data_generate();
00387     }
00388     return info->thread_leader_data;
00389 }
00390 
00391 thread_parent_info_t *thread_parent_data_allocate(thread_info_t *info)
00392 {
00393     thread_parent_info_t *parent_data;
00394     if (!info->thread_endnode_parent) {
00395         info->thread_endnode_parent = ns_dyn_mem_alloc(sizeof(thread_parent_info_t));
00396     }
00397 
00398     parent_data = info->thread_endnode_parent;
00399     if (parent_data) {
00400         memset(parent_data,0,sizeof(thread_parent_info_t));
00401     }
00402     return parent_data;
00403 }
00404 
00405 void thread_dynamic_reed_initialize(thread_router_select_t *routerSelect)
00406 {
00407     routerSelect->jitterTimerActive = false;
00408     routerSelect->possibleDefaultParent = 0xff;
00409     routerSelect->routerDowngradeThresHold = ROUTER_DOWNGRADE_THRESHOLD;
00410     routerSelect->routerUpgradeThresHold = ROUTER_UPGRADE_THRESHOLD;
00411     routerSelect->reedAdvertisementInterval = REED_ADVERTISEMENT_INTERVAL;
00412     routerSelect->reedAdvertisementJitterInterval = REED_ADVERTISEMENT_MAX_JITTER;
00413     routerSelect->reedAdvertisementTimeout = NULL;
00414 }
00415 
00416 bool thread_leader_commissioner_create(thread_info_t *thread_info)
00417 {
00418     thread_info->registered_commissioner.commissioner_valid = false;
00419     thread_info->registered_commissioner.session_id = randLIB_get_16bit();
00420     ns_dyn_mem_free(thread_info->registered_commissioner.commissioner_id_ptr);
00421     thread_info->registered_commissioner.commissioner_id_ptr = NULL;
00422     eventOS_timeout_cancel(thread_info->registered_commissioner.commissioner_timeout);
00423     thread_info->registered_commissioner.commissioner_timeout = NULL;
00424     thread_info->registered_commissioner.commissioner_registration = THREAD_COMMISSIONER_NOT_REGISTERED;
00425     thread_info->registered_commissioner.steering_data_len = 0;
00426     return true;
00427 }
00428 
00429 
00430 void thread_data_base_init(thread_info_t *thread_info, int8_t interfaceId)
00431 {
00432     if (!thread_info) {
00433         return;
00434     }
00435     (void) interfaceId;
00436 
00437     thread_leader_commissioner_create(thread_info);
00438     thread_info->rfc6775 = false;
00439     thread_info->threadPrivatePrefixInfo.ulaValid = false;
00440     thread_info->routerIdReqCoapID = 0;
00441     thread_info->networkDataRequested = false;
00442     thread_info->proactive_an_timer = 0;
00443 
00444     thread_info->thread_device_mode = THREAD_DEVICE_MODE_END_DEVICE;
00445     thread_info->thread_attached_state = THREAD_STATE_NETWORK_DISCOVER;
00446     thread_routing_reset(&thread_info->routing);
00447 }
00448 
00449 int thread_info_allocate_and_init(protocol_interface_info_entry_t *cur)
00450 {
00451     if (thread_bootstrap_tasklet_init(cur) != 0) {
00452         return -1;
00453     }
00454 
00455     if (!cur->thread_info) {
00456         cur->thread_info = ns_dyn_mem_alloc(sizeof(thread_info_t));
00457         if (!cur->thread_info) {
00458             return -1;
00459         }
00460         memset(cur->thread_info, 0, sizeof(thread_info_t));
00461 
00462         cur->thread_info->interface_id = cur->id;
00463         cur->thread_info->testMaxActiveRouterIdLimit = 32;
00464         cur->thread_info->version = thread_version; // Default implementation version
00465         cur->thread_info->thread_device_mode = THREAD_DEVICE_MODE_END_DEVICE;
00466         cur->thread_info->childUpdateReqTimer = -1;
00467 
00468         thread_routing_init(&cur->thread_info->routing);
00469         thread_network_local_server_data_base_init(&cur->thread_info->localServerDataBase);
00470         memset(&cur->thread_info->registered_commissioner,0,sizeof(thread_commissioner_t));
00471         thread_dynamic_reed_initialize(&cur->thread_info->routerSelectParameters);
00472         thread_extension_allocate(cur);
00473         ns_list_init(&cur->thread_info->childIdReqPending);
00474         ns_list_init(&cur->thread_info->child_mcast_list);
00475         if (!thread_leader_data_get(cur->thread_info)){
00476             return -1;
00477         }
00478     } else {
00479         thread_network_data_free_and_clean(&cur->thread_info->networkDataStorage);
00480         thread_network_local_data_free_and_clean(&cur->thread_info->localServerDataBase, cur->id);
00481         thread_leader_service_leader_data_free(cur->thread_info);
00482         thread_data_base_init(cur->thread_info, cur->id);
00483         thread_dynamic_reed_initialize(&cur->thread_info->routerSelectParameters);
00484     }
00485     thread_network_data_base_init(&cur->thread_info->networkDataStorage);
00486     //SET Thread Bootstrap Up & Down
00487     cur->if_up = thread_bootstrap_up;
00488     cur->if_down = thread_bootstrap_down;
00489     cur->mac_parameters->beacon_ind = thread_beacon_indication;
00490     cur->mac_parameters->mac_in_direct_entry_timeout = 30000;
00491     cur->thread_info->routerShortAddress = 0xfffe; //Default value Not Router
00492     //memset(cur->thread_info->lastValidRouteMask, 0, (N_THREAD_ROUTERS+7)/8);
00493     cur->thread_info->releaseRouterId = false;
00494 
00495     // Test data
00496     cur->thread_info->testRandomPartitionId = 0;
00497 
00498     return 0;
00499 }
00500 
00501 void thread_info_deallocate(protocol_interface_info_entry_t *cur)
00502 {
00503     if (cur->thread_info) {
00504         //Release DHCPv6 leasequery Service
00505         thread_nd_service_delete(cur->id);
00506         thread_network_data_free_and_clean(&cur->thread_info->networkDataStorage);
00507         thread_network_local_data_free_and_clean(&cur->thread_info->localServerDataBase, cur->id);
00508         thread_leader_service_leader_data_free(cur->thread_info);
00509         thread_data_base_init(cur->thread_info, cur->id);
00510         thread_routing_free(&cur->thread_info->routing);
00511         thread_extension_free(cur);
00512         thread_extension_bootstrap_free(cur);
00513         if (cur->thread_info->thread_endnode_parent) {
00514             ns_dyn_mem_free(cur->thread_info->thread_endnode_parent);
00515             cur->thread_info->thread_endnode_parent = NULL;
00516         }
00517         ns_dyn_mem_free(cur->thread_info);
00518         cur->thread_info = 0;
00519         cur->mesh_callbacks = NULL;
00520     }
00521 }
00522 
00523 thread_leader_info_t *thread_allocate_and_init_leader_private_data(void)
00524 {
00525     thread_leader_info_t *leader_info = ns_dyn_mem_alloc(sizeof(thread_leader_info_t));
00526     if (leader_info) {
00527         leader_info->leader_id_seq_timer = ID_SEQUENCE_PERIOD;
00528         leader_info->leader_nvm_sync_timer = 0;
00529     }
00530     return leader_info;
00531 }
00532 
00533 thread_route_cost_t thread_link_quality_to_cost(thread_link_quality_e quality)
00534 {
00535     switch (quality) {
00536         case QUALITY_20dB:
00537             return 1;
00538         case QUALITY_10dB:
00539             return 2;
00540         case QUALITY_2dB:
00541             return 4;
00542         default:
00543         case QUALITY_BAD:
00544             return THREAD_COST_INFINITE;
00545     }
00546 }
00547 
00548 thread_route_cost_t thread_link_cost_sum(thread_route_cost_t a, thread_route_cost_t b)
00549 {
00550     if (a == THREAD_COST_INFINITE || b == THREAD_COST_INFINITE) {
00551         return THREAD_COST_INFINITE;
00552     }
00553 
00554     if (a + b > THREAD_MAX_ROUTE_COST) {
00555         return THREAD_COST_INFINITE;
00556     }
00557 
00558     return a + b;
00559 }
00560 
00561 thread_link_quality_e thread_link_margin_to_quality(thread_link_margin_t margin)
00562 {
00563     if (margin > (20 << THREAD_LINK_MARGIN_SCALING)) {
00564         return QUALITY_20dB;
00565     } else if (margin > (10 << THREAD_LINK_MARGIN_SCALING)) {
00566         return QUALITY_10dB;
00567     } else if (margin > (2 << THREAD_LINK_MARGIN_SCALING)) {
00568         return QUALITY_2dB;
00569     } else {
00570         return QUALITY_BAD;
00571     }
00572 }
00573 
00574 uint_fast8_t thread_sum_rx_path_cost_and_link_cost(uint8_t inMargim, uint8_t outMargin, uint8_t pathCost)
00575 {
00576     thread_route_cost_t linkCost, rxCost;
00577     if (inMargim < outMargin) {
00578         linkCost = thread_link_quality_to_cost(thread_link_margin_to_quality(inMargim));
00579     } else {
00580         linkCost = thread_link_quality_to_cost(thread_link_margin_to_quality(outMargin));
00581     }
00582     rxCost = pathCost;
00583 
00584     return thread_link_cost_sum(linkCost, rxCost);
00585 }
00586 
00587 thread_registered_mcast_addr_t *thread_registered_mcast_addr_entry_allocate(void)
00588 {
00589     thread_registered_mcast_addr_t *addr = ns_dyn_mem_alloc(sizeof(thread_registered_mcast_addr_t));
00590 
00591     if (addr) {
00592         memset(addr, 0, sizeof(thread_registered_mcast_addr_t));
00593         ns_list_init(&addr->children);
00594     }
00595 
00596     return addr;
00597 }
00598 
00599 thread_mcast_child_t *thread_mcast_addr_child_entry_allocate(void)
00600 {
00601     thread_mcast_child_t *child = ns_dyn_mem_alloc(sizeof(thread_mcast_child_t));
00602 
00603     if (child) {
00604         memset(child, 0, sizeof(thread_mcast_child_t));
00605     }
00606 
00607     return child;
00608 }
00609 
00610 void thread_registered_mcast_addr_entry_clean(protocol_interface_info_entry_t *cur)
00611 {
00612     ns_list_foreach_safe(thread_registered_mcast_addr_t, entry, &cur->thread_info->child_mcast_list) {
00613         ns_list_remove(&cur->thread_info->child_mcast_list, entry);
00614 
00615         ns_list_foreach_safe(thread_mcast_child_t, child, &entry->children) {
00616             ns_list_remove(&entry->children, child);
00617             ns_dyn_mem_free(child);
00618         }
00619 
00620         ns_dyn_mem_free(entry);
00621     }
00622 }
00623 
00624 void thread_child_mcast_entries_remove(protocol_interface_info_entry_t *cur, const uint8_t *mac64)
00625 {
00626     ns_list_foreach_safe(thread_registered_mcast_addr_t, entry, &cur->thread_info->child_mcast_list) {
00627         ns_list_foreach_safe(thread_mcast_child_t, child, &entry->children) {
00628             if (memcmp(child->mac64, mac64, 8) == 0) {
00629                 ns_list_remove(&entry->children, child);
00630                 ns_dyn_mem_free(child);
00631             }
00632         }
00633     }
00634 
00635     // Remove empty multicast address entries
00636     ns_list_foreach_safe(thread_registered_mcast_addr_t, entry, &cur->thread_info->child_mcast_list) {
00637         if (ns_list_is_empty(&entry->children)) {
00638             ns_list_remove(&cur->thread_info->child_mcast_list, entry);
00639             ns_dyn_mem_free(entry);
00640         }
00641     }
00642 }
00643 
00644 thread_registered_mcast_addr_t *thread_registered_mcast_addr_entry_find(protocol_interface_info_entry_t *cur, const uint8_t *mcast_addr)
00645 {
00646     ns_list_foreach(thread_registered_mcast_addr_t, entry, &cur->thread_info->child_mcast_list) {
00647         if (memcmp(entry->address, mcast_addr, 16) == 0) {
00648             return entry;
00649         }
00650     }
00651 
00652     return NULL;
00653 }
00654 
00655 thread_mcast_child_t *thread_child_mcast_entry_find(thread_mcast_children_list_t *children, const uint8_t *mac64)
00656 {
00657     ns_list_foreach(thread_mcast_child_t, entry, children) {
00658         if (memcmp(entry->mac64, mac64, 8) == 0) {
00659             return entry;
00660         }
00661     }
00662 
00663     return NULL;
00664 }
00665 
00666 thread_mcast_child_t *thread_child_mcast_entry_get(protocol_interface_info_entry_t *cur, const uint8_t *mcast_addr, const uint8_t *mac64)
00667 {
00668     mle_neigh_table_entry_t *mle_entry = mle_class_get_by_link_address(cur->id, mac64, ADDR_802_15_4_LONG );
00669 
00670     if (!mle_entry) {
00671         tr_error("No MLE entry.");
00672         return NULL;
00673     }
00674 
00675     if (mle_entry->mode & MLE_RX_ON_IDLE) {
00676         /* Not a sleepy child */
00677         tr_debug("Not a sleepy child");
00678         return NULL;
00679     }
00680 
00681     thread_registered_mcast_addr_t *addr = thread_registered_mcast_addr_entry_find(cur, mcast_addr);
00682 
00683     if (!addr) {
00684         addr = thread_registered_mcast_addr_entry_allocate();
00685 
00686         if (addr) {
00687             memcpy(addr->address, mcast_addr, 16);
00688             ns_list_add_to_end(&cur->thread_info->child_mcast_list, addr);
00689         } else {
00690             return NULL;
00691         }
00692     }
00693 
00694     thread_mcast_child_t *child = thread_child_mcast_entry_find(&addr->children, mac64);
00695 
00696     if (!child) {
00697         child = thread_mcast_addr_child_entry_allocate();
00698 
00699         if (child) {
00700             memcpy(child->mac64, mac64, 8);
00701             ns_list_add_to_end(&addr->children, child);
00702         } else {
00703             return NULL;
00704         }
00705     }
00706 
00707     return child;
00708 }
00709 
00710 void thread_child_id_request_info_init(thread_pending_child_id_req_t *child_info)
00711 {
00712     if (child_info) {
00713         child_info->routeReq = false;
00714         child_info->networkDataReq = false;
00715         child_info->shortAddressReq = false;
00716     }
00717 }
00718 
00719 thread_pending_child_id_req_t *thread_child_id_request_allocate(void)
00720 {
00721     thread_pending_child_id_req_t *req = ns_dyn_mem_alloc(sizeof(thread_pending_child_id_req_t));
00722     memset(req->eiid, 0 , 8);
00723     thread_child_id_request_info_init(req);
00724     return req;
00725 }
00726 
00727 void thread_child_id_request_entry_clean(protocol_interface_info_entry_t *cur)
00728 {
00729     ns_list_foreach_safe(thread_pending_child_id_req_t, entry, &cur->thread_info->childIdReqPending) {
00730         ns_list_remove(&cur->thread_info->childIdReqPending, entry);
00731         ns_dyn_mem_free(entry);
00732     }
00733 }
00734 
00735 thread_pending_child_id_req_t *thread_child_id_request_entry_get(protocol_interface_info_entry_t *cur, uint8_t *euid64)
00736 {
00737     thread_pending_child_id_req_t *req;
00738     ns_list_foreach(thread_pending_child_id_req_t, entry, &cur->thread_info->childIdReqPending) {
00739         if (memcmp(entry->euid64, euid64, 8) == 0) {
00740             thread_child_id_request_info_init(entry);
00741             return entry;
00742         }
00743     }
00744     req = thread_child_id_request_allocate();
00745     if (req) {
00746         tr_debug("Add to list ID REQ");
00747         memcpy(req->euid64, euid64, 8);
00748         ns_list_add_to_end(&cur->thread_info->childIdReqPending, req);
00749     }
00750     return req;
00751 }
00752 
00753 thread_pending_child_id_req_t *thread_child_id_request_entry_get_from_the_list(protocol_interface_info_entry_t *cur)
00754 {
00755     thread_pending_child_id_req_t *req;
00756 
00757     req = ns_list_get_first(&cur->thread_info->childIdReqPending);
00758     if (req) {
00759         ns_list_remove(&cur->thread_info->childIdReqPending, req);
00760     }
00761 
00762     return req;
00763 
00764 }
00765 
00766 void thread_child_id_request_entry_remove(protocol_interface_info_entry_t *cur, thread_pending_child_id_req_t *entry)
00767 {
00768     if (entry) {
00769         ns_list_remove(&cur->thread_info->childIdReqPending, entry);
00770         ns_dyn_mem_free(entry);
00771     }
00772 }
00773 
00774 int thread_init(protocol_interface_info_entry_t *cur)
00775 {
00776     if (!cur->thread_info) {
00777         return -1;
00778     }
00779 
00780     if (thread_mle_class_init(cur->id) != 0) {
00781         return -1;
00782     }
00783 
00784     // set mle security - first allocate instance and then set security
00785     mle_service_security_instance_allocate(cur->id);
00786     if (mle_service_security_init(cur->id, 5,
00787             0,
00788             thread_management_key_request_with_sequence,
00789             thread_mle_service_security_notify_cb) != 0) {
00790         tr_error("Mle Service security init Fail");
00791         return -1;
00792     }
00793 
00794     mle_class_router_challenge(cur->id, NULL);
00795 
00796     if (etx_accum_failures_callback_register(cur->nwk_id, cur->id, 1, thread_tx_failure_handler) != 1) {
00797         return -1;
00798     }
00799 
00800     addr_notification_register(thread_address_notification_cb);
00801 
00802     thread_leader_service_leader_data_free(cur->thread_info);
00803     thread_data_base_init(cur->thread_info, cur->id);
00804     mac_helper_pib_boolean_set(cur,macThreadForceLongAddressForBeacon , true);
00805     mac_helper_mac16_address_set(cur, 0xffff);
00806     mle_class_mode_set(cur->id, MLE_CLASS_END_DEVICE);
00807     return 0;
00808 }
00809 
00810 int thread_attach_ready(protocol_interface_info_entry_t *cur)
00811 {
00812     if (!cur->thread_info) {
00813         return -1;
00814     }
00815 
00816     switch (cur->thread_info->thread_attached_state) {
00817         case THREAD_STATE_CONNECTED:
00818         case THREAD_STATE_CONNECTED_ROUTER:
00819             return 0;
00820             break;
00821         default:
00822             break;
00823     }
00824 
00825     return -1;
00826 }
00827 
00828 bool thread_attach_active_router(protocol_interface_info_entry_t *cur)
00829 {
00830     if(cur->thread_info->thread_attached_state == THREAD_STATE_CONNECTED_ROUTER) {
00831         return true;
00832     }
00833 
00834     return false;
00835 }
00836 
00837 bool thread_scan_mask_validation(protocol_interface_info_entry_t *cur, uint8_t mask)
00838 {
00839     uint8_t maskCompare = 0;
00840 
00841     if (cur->thread_info->thread_device_mode != THREAD_DEVICE_MODE_ROUTER) {
00842         return false;
00843     }
00844 
00845     switch (cur->thread_info->thread_attached_state) {
00846         case THREAD_STATE_CONNECTED_ROUTER:
00847             maskCompare  = 0x80;
00848             break;
00849         case THREAD_STATE_CONNECTED:
00850             maskCompare = 0x40;
00851             break;
00852         default:
00853             maskCompare = 0;
00854             break;
00855     }
00856     return (mask & maskCompare);
00857 }
00858 
00859 int thread_route_ready_to_leader(protocol_interface_info_entry_t *cur)
00860 {
00861     int retVal = -1;
00862     switch (cur->thread_info->thread_attached_state) {
00863         case THREAD_STATE_CONNECTED_ROUTER:
00864             if (cur->thread_info->leader_private_data) {
00865                 //We are the leader
00866                 retVal = 0;
00867             } else if (thread_routing_cost_get_by_router_id(&cur->thread_info->routing, cur->thread_info->thread_leader_data->leaderRouterId) != 0) {
00868                 retVal = 0;
00869             }
00870             break;
00871         case THREAD_STATE_CONNECTED:
00872             if (cur->thread_info->thread_endnode_parent) {
00873                 retVal = 0;
00874             }
00875             break;
00876         default:
00877             break;
00878     }
00879     return retVal;
00880 }
00881 static void thread_child_update_req_timer(protocol_interface_info_entry_t *cur, uint16_t seconds)
00882 {
00883     if (cur->thread_info->childUpdateReqTimer == -1) {
00884         return;
00885     }
00886     if (cur->thread_info->childUpdateReqTimer > seconds) {
00887         cur->thread_info->childUpdateReqTimer -= seconds;
00888     } else {
00889         cur->thread_info->childUpdateReqTimer = 0;
00890     }
00891 
00892     if (cur->thread_info->childUpdateReqTimer == 0) {
00893         thread_bootstrap_child_update_trig(cur);
00894         cur->thread_info->childUpdateReqTimer = -1; // disable
00895     }
00896 }
00897 
00898 static void thread_key_switch_timer(protocol_interface_info_entry_t *cur, uint16_t seconds)
00899 {
00900     if (cur->thread_info->masterSecretMaterial.keySwitchGuardTimer > seconds) {
00901         cur->thread_info->masterSecretMaterial.keySwitchGuardTimer -= seconds;
00902     } else {
00903         cur->thread_info->masterSecretMaterial.keySwitchGuardTimer = 0;
00904     }
00905 
00906     if (cur->thread_info->masterSecretMaterial.keyRotation > seconds) {
00907         cur->thread_info->masterSecretMaterial.keyRotation -= seconds;
00908     } else {
00909         cur->thread_info->masterSecretMaterial.keyRotation = 0;
00910     }
00911 
00912     if (cur->thread_info->masterSecretMaterial.keyRotation == 0) {
00913         link_configuration_s *linkConfiguration;
00914         linkConfiguration = thread_joiner_application_get_config(cur->id);
00915 
00916         if (!linkConfiguration) {
00917             return;
00918         }
00919 
00920         tr_debug("thrKeyRotation == 0: sync key material by %"PRIu32, linkConfiguration->key_sequence + 1);
00921         thread_management_key_sets_calc(cur, linkConfiguration, linkConfiguration->key_sequence + 1);
00922         thread_calculate_key_guard_timer(cur, linkConfiguration, false);
00923     }
00924 }
00925 
00926 void thread_seconds_timer(protocol_interface_info_entry_t *cur, uint32_t ticks)
00927 {
00928     uint8_t leader_address[16];
00929     uint8_t commissioner_address[16];
00930     uint16_t commissioner_port;
00931     thread_info_t *thread_info = cur->thread_info;
00932 
00933     if (!thread_info) {
00934         return;
00935     }
00936 
00937     blacklist_ttl_update(ticks);
00938 
00939     // if we have pending configurations timer is made for it.
00940     thread_joiner_application_seconds_timer(cur->id, ticks);
00941     thread_resolution_client_timer(cur->id, ticks);
00942     thread_key_switch_timer(cur, ticks);
00943     thread_child_update_req_timer(cur, ticks);
00944 
00945     if (!thread_bootstrap_should_register_address(cur)) {
00946         /* Only FTD refreshes the address registration timer */
00947         thread_address_registration_timer(cur, ticks);
00948     }
00949 
00950     if (thread_attach_ready(cur) != 0) {
00951         return;
00952     }
00953     // Store all Dynamic information periodically. Now saved every 1s
00954     thread_bootstrap_dynamic_configuration_save(cur);
00955 
00956     // TODO should this be somewhere else? in joiner application?
00957     if (thread_joiner_application_next_active_config_exists(cur->id)) {
00958         thread_management_get_leader_address(cur->id, leader_address);
00959         thread_management_client_active_set(cur->id, leader_address);
00960         thread_joiner_application_next_active_config_delete(cur->id);
00961     }
00962 
00963     if (thread_joiner_application_next_pending_config_exists(cur->id)) {
00964         thread_management_get_leader_address(cur->id, leader_address);
00965         thread_management_client_pending_set(cur->id, leader_address );
00966         thread_joiner_application_next_pending_config_delete(cur->id);
00967     }
00968 
00969     // Check if we need to make application provisioning
00970     if (PROVISIONING_STATUS_NOT_DONE == thread_joiner_application_provisioning_get(cur->id) &&
00971         thread_management_get_commissioner_address(cur->id, commissioner_address, &commissioner_port) == 0) {
00972         // Provisioning not done and commissioner is present
00973         thread_management_client_provision_request(cur->id,  commissioner_address, commissioner_port);
00974     }
00975 
00976 
00977     // add more checks here when to become router
00978     // If we are doing attach to new partition, do not upgrade
00979     if(cur->nwk_bootstrap_state != ER_BOOTSRAP_DONE && cur->nwk_bootstrap_state != ER_MLE_ATTACH_READY) {
00980         return;
00981     }
00982 
00983     thread_router_bootstrap_timer(cur, ticks);
00984     thread_border_router_seconds_timer(cur->id, ticks);
00985     thread_bbr_seconds_timer(cur->id, ticks);
00986     thread_lowpower_timer(cur, ticks);
00987     thread_nvm_store_seconds_timer(ticks);
00988 
00989     if (cur->lowpan_info & INTERFACE_NWK_BOOTSRAP_ACTIVE) {
00990         nwk_bootsrap_state_update(ARM_NWK_BOOTSTRAP_READY, cur);
00991     }
00992 }
00993 
00994 void thread_network_data_request_send(protocol_interface_info_entry_t *cur, uint8_t *requestDstAddress, bool delaydTrig)
00995 {
00996     thread_info_t *thread_info = cur->thread_info;
00997     uint8_t req_tlv = MLE_TYPE_NETWORK_DATA;
00998 
00999     if (!cur->thread_info) {
01000         return;
01001     }
01002 
01003     tr_debug("Send MLE network data request");
01004 
01005     if(cur->thread_info->networkDataRequested){
01006         tr_debug("Pending data request found");
01007         return;
01008     }
01009 
01010     cur->thread_info->networkDataRequested = true;
01011 
01012     thread_tlv_request(thread_info->interface_id, requestDstAddress,delaydTrig, &req_tlv, 1);
01013 }
01014 
01015 void thread_timer(protocol_interface_info_entry_t *cur, uint8_t ticks)
01016 {
01017     thread_info_t *thread_info = cur->thread_info;
01018     if (!thread_info) {
01019         return;
01020     }
01021 
01022     if (thread_i_am_router(cur)) {
01023 
01024         if (thread_routing_timer(thread_info, ticks)) {
01025             thread_router_bootstrap_mle_advertise(cur);
01026         }
01027 
01028     } else {
01029 
01030     }
01031 }
01032 
01033 /* This test is used to indicate that we're following the Thread 1.0 oddities
01034  * arising from not having LL16 addresses on the mesh. For example, LL16
01035  * addresses go through the MAC and only one radio hop, and UL16/GP16 addresses
01036  * have special semantics on the PAN.
01037  */
01038 bool thread_insist_that_mesh_isnt_a_link(const protocol_interface_info_entry_t *cur)
01039 {
01040     return cur->thread_info && !cur->thread_info->rfc6775;
01041 }
01042 
01043 int8_t thread_beacon_create_payload(struct protocol_interface_info_entry *cur)
01044 {
01045     uint8_t *ptr;
01046     uint8_t payload_len;
01047     link_configuration_s *leader_link_setup;
01048     thread_management_server_data_t server_data;
01049 
01050     leader_link_setup = thread_joiner_application_get_config(cur->id);
01051 
01052     if (!leader_link_setup) {
01053         return -1;
01054     }
01055 
01056     if(!(leader_link_setup->securityPolicy & SECURITY_POLICY_BEACON_PAYLOAD_ENABLED)){
01057         mac_helper_beacon_payload_reallocate(cur, 0);
01058         return mac_helper_beacon_payload_register(cur);
01059     }
01060     if (thread_management_server_commisoner_data_get(cur->id, &server_data) != 0) {
01061         return mac_helper_beacon_payload_register(cur);
01062     }
01063 
01064     payload_len = 2/*header*/ + 16/*Network id*/ + 8/*extented PANID*/;
01065 
01066     if (cur->thread_info->registered_commissioner.commissioner_valid && cur->thread_info->registered_commissioner.steering_data_len) {
01067         payload_len += cur->thread_info->registered_commissioner.steering_data_len + 2;
01068     }
01069 
01070     if (server_data.joiner_router_enabled) {
01071         payload_len +=  4/*Joiner UDP port*/;
01072     }
01073     if((leader_link_setup->securityPolicy & SECURITY_POLICY_NATIVE_COMMISSIONING_ALLOWED)){
01074         payload_len +=  4/*Commissioner UDP port*/;
01075     }
01076 
01077     ptr = mac_helper_beacon_payload_reallocate(cur, payload_len);
01078     if (!ptr) {
01079         return -1;
01080     }
01081 
01082     *ptr++ = THREAD_BEACON_PROTOCOL_ID;
01083     *ptr = THREAD_BEACON_PROTOCOL_VERSION << THREAD_BEACON_VERSION_SHIFT;
01084 
01085     if (cur->thread_info->registered_commissioner.commissioner_valid && cur->thread_info->registered_commissioner.steering_data_len > 0) {
01086         *ptr |= THREAD_BEACON_JOINING_PERMITTED_BIT;    // permit join bit set on
01087     }
01088     if((leader_link_setup->securityPolicy & SECURITY_POLICY_NATIVE_COMMISSIONING_ALLOWED)){
01089         *ptr |= THREAD_BEACON_NATIVE_COMMISSIONER_BIT;
01090     }
01091     ptr++;
01092     memcpy(ptr, leader_link_setup->name, 16);
01093     ptr += 16;
01094     memcpy(ptr, leader_link_setup->extented_pan_id, 8);
01095     ptr += 8;
01096 
01097     if (server_data.joiner_router_enabled) {
01098         /* MESHCOP_TLV_JOINER_UDP_PORT */
01099         ptr = thread_joining_port_tlv_write(server_data.joiner_router_port, ptr);
01100     }
01101     if((leader_link_setup->securityPolicy & SECURITY_POLICY_NATIVE_COMMISSIONING_ALLOWED)){
01102         /* MESHCOP_TLV_COMMISSIONER_UDP_PORT */
01103         ptr = thread_commissioner_port_tlv_write(server_data.commissioner_port, ptr);
01104     }
01105 
01106 
01107     if (cur->thread_info->registered_commissioner.commissioner_valid && cur->thread_info->registered_commissioner.steering_data_len > 0){
01108         ptr = thread_nd_commission_data_write_steering_data(ptr, cur->thread_info->registered_commissioner.steering_data, cur->thread_info->registered_commissioner.steering_data_len);
01109     }
01110 
01111     return mac_helper_beacon_payload_register(cur);
01112 }
01113 
01114 uint8_t thread_beacon_indication(uint8_t *ptr, uint8_t len, protocol_interface_info_entry_t *cur)
01115 {
01116     (void)ptr;
01117     (void)len;
01118     (void)cur;
01119     return 1;
01120 }
01121 
01122 static uint8_t *thread_linkquality_write(int8_t interface_id, uint8_t *buffer)
01123 {
01124     uint8_t lqi1 = 0, lqi2 = 0, lqi3 = 0;
01125     thread_link_quality_e thread_link_quality;
01126     mle_neigh_table_list_t *neigh_list = mle_class_active_list_get(interface_id);
01127 
01128     ns_list_foreach(mle_neigh_table_entry_t, cur, neigh_list) {
01129         if (thread_is_router_addr(cur->short_adr)) {
01130             // Only count routers to link quality
01131             thread_link_quality = thread_link_margin_to_quality(cur->link_margin);
01132             switch (thread_link_quality) {
01133                 case QUALITY_20dB:
01134                     lqi3++;
01135                     break;
01136                 case QUALITY_10dB:
01137                     lqi2++;
01138                     break;
01139                 case QUALITY_2dB:
01140                     lqi1++;
01141                     break;
01142                 case QUALITY_BAD:
01143                     break;
01144 
01145             }
01146         }
01147     }
01148 
01149     *buffer++ = lqi3;
01150     *buffer++ = lqi2;
01151     *buffer++ = lqi1;
01152     return buffer;
01153 }
01154 
01155 uint8_t thread_route_option_size(protocol_interface_info_entry_t *cur)
01156 {
01157     return 2 + MLE_ROUTE_MIN_OPTION_LEN + thread_routing_get_route_data_size(cur);
01158 
01159 }
01160 uint8_t *thread_route_option_write(protocol_interface_info_entry_t *cur, uint8_t *ptr)
01161 {
01162     uint8_t *len_ptr;
01163     uint8_t *saved_ptr;
01164 
01165     saved_ptr = ptr;
01166     *ptr++ = MLE_TYPE_ROUTE;
01167     len_ptr = ptr++;
01168     /* ptr now points to ID sequence */
01169 
01170     /* Can write the data straight into the buffer */
01171     if (thread_routing_get_route_data(cur,
01172                                       ptr, /* ptr to ID sequence (1 byte) */
01173                                       ptr + 1, /* ptr to ID mask (MLE_ROUTE_ID_MASK_SIZE bytes) */
01174                                       ptr + MLE_ROUTE_MIN_OPTION_LEN, /* ptr to router table data */
01175                                       len_ptr) != 0) /* ptr to length */ { /* 0 -> SUCCESS */
01176         /* Point to beginning of buffer again */
01177         ptr = saved_ptr;
01178     } else {
01179         /* Function sets length to router table length - adjust for ID sequence and ID mask length */
01180         *len_ptr += MLE_ROUTE_MIN_OPTION_LEN;
01181         /* Advance buffer pointer past value field */
01182         ptr += *len_ptr;
01183     }
01184 
01185     return ptr;
01186 }
01187 
01188 uint8_t *thread_connectivity_tlv_write(uint8_t *ptr, protocol_interface_info_entry_t *cur, uint8_t mode)
01189 {
01190     thread_info_t *thread = thread_info(cur);
01191     //Set Connectivity
01192     *ptr++ = MLE_TYPE_CONNECTIVITY;
01193     *ptr++ = 10;
01194 
01195     // determine parent priority
01196     if ((mode & MLE_DEV_MASK) == MLE_RFD_DEV && (3*mle_class_rfd_entry_count_get(cur->id) > 2*THREAD_MAX_MTD_CHILDREN)) {
01197         *ptr++ = CONNECTIVITY_PP_LOW;
01198     } else if (!(mode & MLE_RX_ON_IDLE) && (3*mle_class_sleepy_entry_count_get(cur->id) > 2*THREAD_MAX_SED_CHILDREN)) {
01199         *ptr++ = CONNECTIVITY_PP_LOW;
01200     } else if (3*thread_router_bootstrap_child_count_get(cur) > 2*thread->maxChildCount) {
01201         // 1/3 of the child capacity remaining, PP=low
01202         *ptr++ = CONNECTIVITY_PP_LOW;
01203     } else if (mle_class_free_entry_count_get(cur->id) < THREAD_FREE_MLE_ENTRY_THRESHOLD) {
01204         // If only few entries available in the MLE table, change priority to low
01205         *ptr++ = CONNECTIVITY_PP_LOW;
01206     } else {
01207         *ptr++ = CONNECTIVITY_PP_MEDIUM;
01208     }
01209 
01210     ptr = thread_linkquality_write(cur->id, ptr);
01211 
01212     // Route Cost To leader
01213     if (thread->thread_attached_state == THREAD_STATE_CONNECTED_ROUTER){
01214         // Leader cost
01215         *ptr++ = thread_routing_cost_get_by_router_id(&thread->routing, thread->thread_leader_data->leaderRouterId);
01216         //Router ID sequence
01217         *ptr++ = thread->routing.router_id_sequence;
01218     } else {
01219         // Leader cost when attached as child (REED,FED,SED,MED)
01220         *ptr++ = thread->thread_endnode_parent->pathCostToLeader;
01221         //Router ID sequence
01222         if (thread->routing.router_id_sequence_valid) {
01223             *ptr++ = thread->routing.router_id_sequence;
01224         } else {// We dont know the correct value
01225             *ptr++ = 0;
01226         }
01227     }
01228 
01229     // active Routers
01230     uint8_t activeRouters = 0;
01231     if (cur->thread_info->leader_private_data) {
01232         activeRouters = thread_routing_count_active_routers_from_mask(cur->thread_info->leader_private_data->master_router_id_mask);
01233     } else if (thread->thread_device_mode == THREAD_DEVICE_MODE_ROUTER ) {
01234         activeRouters = thread_routing_count_active_routers(&thread->routing);
01235     }
01236     *ptr++ = activeRouters;
01237 
01238     //Set SED Buffer size and datagram count
01239     ptr = common_write_16_bit(THREAD_SED_BUFFER_SIZE, ptr);
01240     *ptr++ = THREAD_SED_DATAGRAM_COUNT;
01241 
01242     return ptr;
01243 }
01244 
01245 uint16_t thread_network_data_tlv_length(protocol_interface_info_entry_t *cur, bool fullist)
01246 {
01247     if (fullist) {
01248         return cur->thread_info->networkDataStorage.network_data_len;
01249     }
01250 
01251     return thread_network_data_generate_stable_set(cur, NULL);
01252 }
01253 
01254 uint16_t thread_network_data_generate_stable_set(protocol_interface_info_entry_t *cur, uint8_t *result_ptr)
01255 {
01256     uint8_t *dptr;
01257     uint16_t network_data_len = 0;
01258     uint8_t length;
01259     uint8_t type;
01260 
01261     dptr = cur->thread_info->networkDataStorage.network_data;
01262     uint16_t network_data_length = cur->thread_info->networkDataStorage.network_data_len;
01263 
01264     tr_debug("Generating stable set from network data of size %d",
01265              cur->thread_info->networkDataStorage.network_data_len);
01266 
01267     while (network_data_length) {
01268         if (network_data_length >= 2) {
01269             type = *dptr++;
01270             length = *dptr++;
01271 
01272             if (length == 0) {
01273                 // 0 is not valid length for TLV
01274                 return 0;
01275             }
01276 
01277             if (!(type & THREAD_NWK_STABLE_DATA)) {
01278                 // Skip this TLV altogether...
01279                 network_data_length -= 2 + length;
01280                 dptr += length;
01281                 continue;
01282             }
01283 
01284             type &= THREAD_NWK_DATA_TYPE_MASK;
01285             network_data_length -= 2;
01286 
01287             if (network_data_length < length) {
01288                 tr_error("Length error");
01289                 return 0;
01290             }
01291 
01292             // Set length ready for next check
01293             network_data_length -= length;
01294 
01295             if (type == THREAD_NWK_DATA_TYPE_PREFIX) {
01296                 uint8_t *length_ptr = NULL;
01297                 length -= 2;
01298 
01299                 uint8_t prefix_bytes_len = prefixBits_to_bytes(*(dptr+1));
01300 
01301                 if (prefix_bytes_len > length) {
01302                     return 0;
01303                 }
01304 
01305                 length -= prefix_bytes_len;
01306 
01307                 if (result_ptr) {
01308                     *result_ptr++ = type | THREAD_NWK_STABLE_DATA;
01309                     length_ptr = result_ptr;
01310                     result_ptr++;
01311                     memcpy(result_ptr, dptr, 2 + prefix_bytes_len);
01312                     result_ptr += 2 + prefix_bytes_len;
01313                 }
01314                 dptr += 2 + prefix_bytes_len;
01315 
01316                 network_data_len += 2 + 2 + prefix_bytes_len;
01317                 uint16_t total_tlv_length = 2 + prefix_bytes_len;
01318 
01319                 uint8_t context_id = thread_border_router_prefix_context_id(dptr, length);
01320                 tr_debug("Resolved Context ID: %d", context_id);
01321 
01322                 while (length > 2) {
01323                     type = *dptr++;
01324                     uint8_t subLength = *dptr++;
01325                     thread_border_router_tlv_entry_t genericService;
01326 
01327                     if (!(type & THREAD_NWK_STABLE_DATA)) {
01328                         // Skip this sub-TLV altogether...
01329                         dptr += subLength;
01330                         length -= 2 + subLength;
01331                         continue;
01332                     }
01333 
01334                     type &= THREAD_NWK_DATA_TYPE_MASK;
01335                     length -= 2;
01336 
01337                     tr_debug("SubType: %02x, %s", type, trace_array(dptr, subLength));
01338                     total_tlv_length += 2 + subLength;
01339                     network_data_len += 2 + subLength;
01340                     tr_debug("Total TLV length: %d", total_tlv_length);
01341                     if (subLength <= length) {
01342                         length -= subLength;
01343                         if (type == THREAD_NWK_DATA_TYPE_BORDER_ROUTER) {
01344                             if (result_ptr) {
01345                                 *result_ptr++ = type | THREAD_NWK_STABLE_DATA;
01346                                 *result_ptr++ = subLength;
01347                             }
01348                             while (subLength) {
01349                                 dptr += 2;
01350                                 uint16_t flags = common_read_16_bit(dptr);
01351                                 dptr += 2;
01352                                 subLength -= THREAD_BORDER_ROUTER_TLV_LENGTH;
01353                                 genericService.P_dhcp = ((flags >> THREAD_P_DHCP_BIT_MOVE) & 1);
01354                                 if (result_ptr) {
01355                                     if (genericService.P_dhcp) {
01356                                         result_ptr = common_write_16_bit(0xfc00 | context_id, result_ptr);
01357                                     } else {
01358                                         result_ptr = common_write_16_bit(0xfffe, result_ptr);
01359                                     }
01360                                     result_ptr = common_write_16_bit(flags, result_ptr);
01361                                 }
01362                             }
01363                         } else if (type == THREAD_NWK_DATA_TYPE_ROUTE) {
01364                             if (result_ptr) {
01365                                 *result_ptr++ = type | THREAD_NWK_STABLE_DATA;
01366                                 *result_ptr++ = subLength;
01367                             }
01368                             while (subLength) {
01369                                 dptr += 2;
01370                                 uint8_t preference = *dptr++;
01371                                 subLength -= THREAD_HAS_ROUTE_TLV_LENGTH;
01372                                 if (result_ptr) {
01373                                     result_ptr = common_write_16_bit(0xfffe, result_ptr);
01374                                     *result_ptr++ = preference;
01375                                 }
01376                             }
01377                         } else {
01378                             if (result_ptr) {
01379                                 *result_ptr++ = type | THREAD_NWK_STABLE_DATA;
01380                                 *result_ptr++ = subLength;
01381                                 memcpy(result_ptr, dptr, subLength);
01382                                 result_ptr += subLength;
01383                             }
01384 
01385                             dptr += subLength;
01386                         }
01387 
01388                     } else {
01389                         tr_error("Length fail");
01390                         return 0;
01391                     }
01392                 }
01393 
01394                 if (result_ptr) {
01395                     *length_ptr = total_tlv_length;
01396                 }
01397 
01398             } else if (type == THREAD_NWK_DATA_TYPE_SERVICE_DATA) {
01399                 uint8_t * length_ptr = NULL;
01400                 uint16_t total_tlv_length = 0;
01401                 uint16_t copy_length = 1;
01402                 uint8_t T = (*dptr) >> 7;
01403                 uint8_t S_id = *dptr & 0x0f;
01404 
01405                 if (!T) {
01406                     network_data_len += 4;
01407                     total_tlv_length += 4;
01408                     copy_length += 4;
01409                     length -= 4;
01410                 }
01411 
01412                 uint16_t service_data_length = *(dptr+copy_length);
01413 
01414                 if (result_ptr) {
01415                     *result_ptr++ = type | THREAD_NWK_STABLE_DATA;
01416                     length_ptr = result_ptr;
01417                     result_ptr++;
01418                     memcpy(result_ptr, dptr, 1 + copy_length + service_data_length);
01419                     result_ptr += 1 + copy_length + service_data_length;
01420                 }
01421                 network_data_len += 2 + 2 + service_data_length;
01422                 total_tlv_length += 2 + service_data_length;
01423                 dptr += 1 + copy_length + service_data_length;
01424                 length -= 2 + service_data_length;
01425 
01426                 while (length > 2) {
01427                     type = *dptr++;
01428                     uint8_t subLength = *dptr++;
01429 
01430                     if (!(type & THREAD_NWK_STABLE_DATA)) {
01431                         // Skip this sub-TLV altogether...
01432                         length -= 2 + subLength;
01433                         dptr += subLength;
01434                         continue;
01435                     }
01436 
01437                     length -= 2;
01438                     type &= THREAD_NWK_DATA_TYPE_MASK;
01439 
01440                     if (subLength <= length) {
01441                         tr_debug("SubType: %02x, length: %d, data: %s", type, length, trace_array(dptr, subLength));
01442                         total_tlv_length += 2 + subLength;
01443                         network_data_len += 2 + subLength;
01444                         length -= subLength;
01445                         if (result_ptr) {
01446                             *result_ptr++ = type | THREAD_NWK_STABLE_DATA;
01447                             *result_ptr++ = subLength;
01448                             uint8_t *rloc_ptr = result_ptr;
01449                             // Copy the remaining
01450                             memcpy(result_ptr, dptr, subLength);
01451                             result_ptr += subLength;
01452                             // Copy RLOC but replace with ALOC
01453                             common_write_16_bit(0xfc10 | S_id, rloc_ptr);
01454                         }
01455                         dptr += subLength;
01456                     } else {
01457                         tr_debug("Server sub-TLV parse fail!");
01458                         return 0;
01459                     }
01460                 }
01461 
01462                 if (result_ptr) {
01463                     *length_ptr = total_tlv_length;
01464                 }
01465             } else {
01466                 tr_debug("Unknown TLV: %d, length: %d", type, length);
01467                 network_data_len += 2 + length;
01468                 if (result_ptr) {
01469                     *result_ptr++ = type | THREAD_NWK_STABLE_DATA;
01470                     *result_ptr++ = length;
01471                     memcpy(result_ptr, dptr, length);
01472                     result_ptr += length;
01473                 }
01474                 dptr += length;
01475             }
01476         } else {
01477             tr_error("Length failure");
01478             return 0;
01479         }
01480     }
01481 
01482     return network_data_len;
01483 }
01484 
01485 uint16_t thread_network_data_tlv_size(protocol_interface_info_entry_t *cur, bool fullist)
01486 {
01487     uint16_t length = thread_network_data_tlv_length(cur, fullist);
01488 
01489     if (length > 254) {
01490         // 2 extra bytes for extended TLV format
01491         length += 2;
01492     }
01493 
01494     return length;
01495 }
01496 
01497 uint8_t *thread_network_data_tlv_write(protocol_interface_info_entry_t *cur, uint8_t *ptr, bool fulllist)
01498 {
01499     uint16_t length = thread_network_data_tlv_length(cur, fulllist);
01500     *ptr++ = MLE_TYPE_NETWORK_DATA;
01501 
01502     if (length > 254) {
01503         *ptr++ = 0xff;
01504         ptr = common_write_16_bit(length, ptr);
01505     } else {
01506         *ptr++ = length;
01507     }
01508 
01509     if (fulllist) {
01510         if (cur->thread_info->networkDataStorage.network_data_len > 0) {
01511             memcpy(ptr, cur->thread_info->networkDataStorage.network_data,
01512                    cur->thread_info->networkDataStorage.network_data_len);
01513             ptr += cur->thread_info->networkDataStorage.network_data_len;
01514         }
01515         return ptr;
01516     }
01517 
01518     uint16_t size = thread_network_data_generate_stable_set(cur, ptr);
01519     return ptr + size;
01520 }
01521 
01522 uint8_t *thread_active_timestamp_write(protocol_interface_info_entry_t *cur, uint8_t *ptr)
01523 {
01524     link_configuration_s *configuration;
01525 
01526     configuration = thread_joiner_application_get_config(cur->id);
01527     if (!configuration) {
01528         tr_error("No configuration");
01529         return ptr;
01530     }
01531     *ptr++ = MLE_TYPE_ACTIVE_TIMESTAMP;
01532     *ptr++ = 8;
01533     ptr = common_write_64_bit(configuration->timestamp, ptr);
01534     return ptr;
01535 }
01536 
01537 uint16_t thread_active_operational_dataset_size(protocol_interface_info_entry_t *cur)
01538 {
01539     return 2 + thread_joiner_application_active_config_length(cur->id, NULL, 0, mle_active_configuration_dataset_ignore_tlvs, mle_active_configuration_dataset_ignore_tlvs_size());
01540 }
01541 
01542 uint8_t *thread_active_operational_dataset_write(protocol_interface_info_entry_t *cur, uint8_t *ptr)
01543 {
01544     int length;
01545     length = thread_joiner_application_active_config_length(cur->id, NULL, 0, mle_active_configuration_dataset_ignore_tlvs, mle_active_configuration_dataset_ignore_tlvs_size());
01546     if (length < 1) {
01547         return ptr;
01548     }
01549     *ptr++ = MLE_TYPE_OPERATIONAL_DATASET;
01550     *ptr++ = length;
01551     ptr = thread_joiner_application_active_config_write(cur->id, ptr, NULL, 0, mle_active_configuration_dataset_ignore_tlvs, mle_active_configuration_dataset_ignore_tlvs_size());
01552 
01553     return ptr;
01554 }
01555 bool thread_active_operational_dataset_process(protocol_interface_info_entry_t *cur, uint8_t *ptr, uint16_t len, uint64_t dataset_timestamp)
01556 {
01557     //make a copy of the incoming timestamp
01558     uint64_t timestamp;
01559     link_configuration_s *link_configuration;
01560 
01561     if(!cur || !cur->thread_info || !ptr || !len){
01562         return false;
01563     }
01564     link_configuration = thread_joiner_application_get_config(cur->id);
01565     if (!link_configuration) {
01566         return false;
01567     }
01568     tr_debug("process Active dataset");
01569     timestamp = thread_joiner_application_active_timestamp_get(cur->id);
01570 
01571     if (timestamp > dataset_timestamp) {
01572         tr_debug("We have newer timestamp");
01573         thread_joiner_application_next_active_config_save(cur->id);
01574     }
01575     if (timestamp == dataset_timestamp) {
01576         // No changes required
01577         return false;
01578     }
01579     tr_debug("Update Active dataset");
01580     // New active operational dataset received;
01581     thread_joiner_application_update_configuration(cur->id, ptr, len, false);
01582     thread_joiner_application_active_timestamp_set(cur->id,dataset_timestamp);
01583     thread_configuration_thread_activate(cur,link_configuration);
01584     thread_joiner_application_configuration_nvm_save(cur->id);
01585     return true;
01586 }
01587 
01588 uint8_t thread_pending_timestamp_tlv_size(protocol_interface_info_entry_t *cur)
01589 {
01590     if (!thread_joiner_application_pending_config_timestamp_get(cur->id) ) {
01591         return 0;
01592     }
01593     return 2 + 8;
01594 }
01595 
01596 uint8_t *thread_pending_timestamp_write(protocol_interface_info_entry_t *cur, uint8_t *ptr)
01597 {
01598     uint64_t pending_timestamp;
01599 
01600     pending_timestamp = thread_joiner_application_pending_config_timestamp_get(cur->id);
01601     if (!pending_timestamp ) {
01602         return ptr;
01603     }
01604     *ptr++ = MLE_TYPE_PENDING_TIMESTAMP;
01605     *ptr++ = 8;
01606     ptr = common_write_64_bit(pending_timestamp, ptr);
01607     return ptr;
01608 }
01609 
01610 uint16_t thread_pending_operational_dataset_size(protocol_interface_info_entry_t *cur)
01611 {
01612 
01613     if (!thread_joiner_application_pending_config_exists(cur->id)) {
01614         return 0;
01615     }
01616     // Pending set always includes delay timer but not pending timestamp
01617     return thread_joiner_application_pending_config_length(cur->id, NULL, 0, mle_pending_configuration_dataset_ignore_tlvs, mle_pending_configuration_dataset_ignore_tlvs_size());
01618 }
01619 
01620 uint8_t *thread_pending_operational_dataset_write(protocol_interface_info_entry_t *cur, uint8_t *ptr)
01621 {
01622     int dataset_length;
01623     if (!thread_joiner_application_pending_config_exists(cur->id)) {
01624         return ptr;
01625     }
01626 
01627     dataset_length = thread_joiner_application_pending_config_length(cur->id, NULL, 0, mle_pending_configuration_dataset_ignore_tlvs, mle_pending_configuration_dataset_ignore_tlvs_size());
01628     if (dataset_length < 1) {
01629         return ptr;
01630     }
01631     *ptr++ = MLE_TYPE_PENDING_OPERATIONAL_DATASET;
01632     *ptr++ = dataset_length;
01633     ptr = thread_joiner_application_pending_config_build(cur->id, ptr, NULL, 0, mle_pending_configuration_dataset_ignore_tlvs, mle_pending_configuration_dataset_ignore_tlvs_size());
01634     return ptr;
01635 }
01636 
01637 bool thread_pending_operational_dataset_process(protocol_interface_info_entry_t *cur, uint64_t mle_pending_timestamp, uint8_t *ptr, uint16_t len)
01638 {
01639     uint32_t delay_timer;
01640 
01641     if(!cur || !cur->thread_info ){
01642         return false;
01643     }
01644     tr_debug("process pending dataset");
01645     if( !ptr || !len){
01646         // No pending set received
01647         return false;
01648     }
01649 
01650     if (4 > thread_meshcop_tlv_data_get_uint32(ptr, len,MESHCOP_TLV_DELAY_TIMER, &delay_timer)){
01651         tr_warn("Delay timer not present");
01652         return false;
01653     }
01654 
01655     if (mle_pending_timestamp < thread_joiner_application_pending_config_timestamp_get(cur->id) ) {
01656         // Saving this config for later use first we get the current active
01657         tr_debug("save pending set for future");
01658         thread_joiner_application_next_pending_config_save(cur->id);
01659     }
01660 
01661     if( 0 != thread_joiner_application_pending_config_create(cur->id, ptr, len)){
01662         tr_error("pending set creation failed");
01663         return false;
01664     }
01665     tr_debug("updating pending dataset");
01666     thread_joiner_application_pending_config_timestamp_set(cur->id,mle_pending_timestamp);
01667     thread_joiner_application_pending_config_enable(cur->id,delay_timer);
01668     return true;
01669 }
01670 
01671 uint8_t thread_leader_data_tlv_size(protocol_interface_info_entry_t *cur)
01672 {
01673     if (!cur || !cur->thread_info || !cur->thread_info->thread_leader_data) {
01674         return 0;
01675     }
01676     return 9;
01677 }
01678 
01679 uint8_t *thread_leader_data_tlv_write(uint8_t *ptr, protocol_interface_info_entry_t *cur)
01680 {
01681     if (!cur || !cur->thread_info || !cur->thread_info->thread_leader_data) {
01682         return ptr;
01683     }
01684     *ptr++ = MLE_TYPE_LEADER_DATA; /* MLE TLV Type */
01685     *ptr++ = 8; /* MLE TLV Value: NWD:LDR TLV Length (7) */
01686     ptr = common_write_32_bit(cur->thread_info->thread_leader_data->partitionId, ptr);
01687     *ptr++ = cur->thread_info->thread_leader_data->weighting;
01688     *ptr++ = cur->thread_info->thread_leader_data->dataVersion;
01689     *ptr++ = cur->thread_info->thread_leader_data->stableDataVersion;
01690     *ptr++ = cur->thread_info->thread_leader_data->leaderRouterId;
01691     return ptr;
01692 }
01693 
01694 uint8_t *thread_address_registration_tlv_write(uint8_t *ptr, protocol_interface_info_entry_t *cur)
01695 {
01696     uint8_t thread_realm_local_mcast_addr[16];
01697     lowpan_context_t *ctx;
01698     uint8_t *address_len_ptr;
01699 
01700     if (thread_info(cur)->thread_device_mode != THREAD_DEVICE_MODE_SLEEPY_END_DEVICE &&
01701         thread_info(cur)->thread_device_mode != THREAD_DEVICE_MODE_END_DEVICE) {
01702         // No address registration for others than MED or SED
01703         return ptr;
01704     }
01705     *ptr++ = MLE_TYPE_ADDRESS_REGISTRATION;
01706     address_len_ptr = ptr++;
01707 
01708     *address_len_ptr = 0;
01709 
01710     // Register all global addressess
01711     ns_list_foreach(if_address_entry_t, e, &cur->ip_addresses) {
01712 
01713         if (*address_len_ptr > 148 ) {
01714             // Maximum length of address registrations
01715             continue;
01716         }
01717         if (addr_ipv6_scope(e->address, cur) == IPV6_SCOPE_GLOBAL || (addr_ipv6_scope(e->address, cur) == IPV6_SCOPE_REALM_LOCAL
01718                 && !thread_addr_is_mesh_local_16(e->address, cur))) {
01719             ctx = lowpan_context_get_by_address(&cur->lowpan_contexts, e->address);
01720             if (ctx) {
01721                 //Write TLV to list
01722                 *ptr++ = (ctx->cid | 0x80);
01723                 memcpy(ptr, e->address + 8, 8);
01724                 ptr += 8;
01725                 *address_len_ptr += 9;
01726             } else {
01727                 *ptr++ = 0;
01728                 memcpy(ptr, e->address, 16);
01729                 ptr += 16;
01730                 *address_len_ptr += 17;
01731             }
01732         }
01733     }
01734 
01735     /* Registers multicast addresses to the parent */
01736     thread_bootstrap_all_nodes_address_generate(thread_realm_local_mcast_addr, cur->thread_info->threadPrivatePrefixInfo.ulaPrefix, 3);
01737 
01738     ns_list_foreach(if_group_entry_t, entry, &cur->ip_groups)
01739     {
01740         if (*address_len_ptr > 148) {
01741             // Maximum length of address registrations
01742             continue;
01743         }
01744         if (addr_ipv6_multicast_scope(entry->group) < IPV6_SCOPE_REALM_LOCAL) {
01745             /* Skip Link Local multicast address */
01746             continue;
01747         }
01748 
01749         if (addr_ipv6_equal(entry->group, thread_realm_local_mcast_addr)) {
01750             /* Skip well-known realm-local all Thread nodes multicast address */
01751             continue;
01752         }
01753         if (addr_ipv6_equal(entry->group, ADDR_ALL_MPL_FORWARDERS)) {
01754             /* Skip All MPL Forwarders address */
01755             continue;
01756         }
01757         if (addr_ipv6_equal(entry->group, ADDR_REALM_LOCAL_ALL_NODES)) {
01758             /* Skip Mesh local all nodes */
01759             continue;
01760         }
01761         if (addr_ipv6_equal(entry->group, ADDR_REALM_LOCAL_ALL_ROUTERS)) {
01762             /* Skip Mesh local all routers */
01763             continue;
01764         }
01765 
01766         *ptr++ = 0;
01767         memcpy(ptr, entry->group, 16);
01768         ptr += 16;
01769         *address_len_ptr += 17;
01770     }
01771     if (*address_len_ptr == 0) {
01772         // No address added remove type and length
01773         ptr -= 2;
01774     }
01775     return ptr;
01776 
01777 }
01778 
01779 int thread_link_reject_send(protocol_interface_info_entry_t *interface, const uint8_t *ll64)
01780 {
01781     uint16_t buf_id;
01782     uint32_t keySequence;
01783     uint8_t *ptr;
01784     mle_message_timeout_params_t timeout;
01785 
01786     buf_id = mle_service_msg_allocate(interface->id, 32, false, MLE_COMMAND_REJECT);
01787     if (buf_id == 0) {
01788         return -1;
01789     }
01790 
01791     thread_management_get_current_keysequence(interface->id, &keySequence);
01792     mle_service_msg_update_security_params(buf_id, 5, 2, keySequence);
01793 
01794     mle_service_set_msg_destination_address(buf_id, ll64);
01795 
01796     ptr = mle_service_get_data_pointer(buf_id);
01797 
01798     // write status TLV
01799     *ptr++ = MLE_TYPE_STATUS;
01800     *ptr++ = 1;
01801     *ptr++ = MLE_STATUS_ERROR;
01802 
01803     if (mle_service_update_length_by_ptr(buf_id,ptr)!= 0) {
01804         tr_debug("Buffer overflow at message write");
01805     }
01806 
01807     timeout.retrans_max = 0;
01808     timeout.timeout_init = 0;
01809     timeout.timeout_max = 0;
01810     timeout.delay = MLE_NO_DELAY;
01811 
01812     mle_service_set_msg_timeout_parameters(buf_id, &timeout);
01813 
01814     return mle_service_send_message(buf_id);
01815 
01816 }
01817 
01818 static uint8_t * thread_joining_port_tlv_write(uint16_t port, uint8_t *ptr)
01819 {
01820     *ptr++ = MESHCOP_TLV_JOINER_UDP_PORT;
01821     *ptr++ = 2;
01822     return common_write_16_bit(port, ptr);
01823 }
01824 
01825 static uint8_t * thread_commissioner_port_tlv_write(uint16_t port, uint8_t *ptr)
01826 {
01827     *ptr++ = MESHCOP_TLV_COMMISSIONER_UDP_PORT;
01828     *ptr++ = 2;
01829     return common_write_16_bit(port, ptr);
01830 }
01831 
01832 static void thread_tx_failure_handler(int8_t nwk_id, uint8_t accumulated_failures, mle_neigh_table_entry_t *neighbor)
01833 {
01834     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(nwk_id);
01835 
01836     tr_debug("In Thread TX_FAIL handler, accumulated_failures=%d", accumulated_failures);
01837 
01838     if (!cur || !cur->thread_info) {
01839         return;
01840     }
01841 
01842     if (accumulated_failures >= THREAD_MAC_TRANSMISSIONS*THREAD_FAILED_CHILD_TRANSMISSIONS) {
01843         mle_class_remove_entry(cur->id, neighbor);
01844     }
01845 }
01846 
01847 /* Called when MLE link to neighbour lost, or ETX callback says link is bad */
01848 void thread_reset_neighbour_info(protocol_interface_info_entry_t *cur, mle_neigh_table_entry_t *neighbour)
01849 {
01850     thread_parent_info_t *thread_endnode_parent = thread_info(cur)->thread_endnode_parent;
01851 
01852     if (!thread_i_am_router(cur) && thread_endnode_parent && thread_endnode_parent->shortAddress == neighbour->short_adr) {
01853         if(cur->nwk_bootstrap_state != ER_CHILD_ID_REQ) {
01854             tr_warn("End device lost parent, reset!\n");
01855             thread_bootstrap_connection_error(cur->id, CON_PARENT_CONNECT_DOWN, NULL);
01856         }
01857     }
01858 
01859     thread_routing_remove_link(cur, neighbour->short_adr);
01860     thread_router_bootstrap_reset_child_info(cur, neighbour);
01861     protocol_6lowpan_release_long_link_address_from_neighcache(cur, neighbour->mac64);
01862     mac_helper_devicetable_remove(cur->mac_api, neighbour->attribute_index);
01863 }
01864 
01865 uint8_t thread_get_router_count_from_route_tlv(mle_tlv_info_t *routeTlv)
01866 {
01867     if (!routeTlv)
01868         return 0;
01869 
01870     if (routeTlv->tlvLen < (MLE_ROUTE_ID_MASK_SIZE + 1))
01871         return 0;
01872 
01873     if (!routeTlv->dataPtr)
01874         return 0;
01875 
01876     return routeTlv->tlvLen - MLE_ROUTE_ID_MASK_SIZE - 1;
01877 }
01878 
01879 static void thread_address_notification_cb(struct protocol_interface_info_entry *interface, const struct if_address_entry *addr, if_address_callback_t reason)
01880 {
01881     if (thread_attach_ready(interface) != 0) {
01882         return;
01883     }
01884 
01885     if (reason != ADDR_CALLBACK_DAD_COMPLETE && reason != ADDR_CALLBACK_DELETED) {
01886         return;
01887     }
01888 
01889     if (addr_ipv6_scope(addr->address, interface) > IPV6_SCOPE_REALM_LOCAL) {
01890         tr_debug("Global address changed: %s", trace_ipv6(addr->address));
01891 
01892         if (thread_bootstrap_should_register_address(interface)) {
01893             interface->thread_info->childUpdateReqTimer = 1;
01894         } else {
01895             if (reason == ADDR_CALLBACK_DAD_COMPLETE) {
01896                 /* Send address notification (our parent doesn't do that for us) */
01897                 thread_extension_address_registration(interface, addr->address, NULL);
01898             }
01899         }
01900     }
01901 }
01902 
01903 void thread_mcast_group_change(struct protocol_interface_info_entry *interface, if_group_entry_t *group, bool addr_added)
01904 {
01905     (void) addr_added;
01906 
01907     group->mld_timer = 0;
01908 
01909     if (thread_attach_ready(interface) != 0) {
01910         return;
01911     }
01912 
01913     tr_debug("Thread multicast address changed: %s", trace_ipv6(group->group));
01914 
01915     if (thread_bootstrap_should_register_address(interface)) {
01916         /* Trigger Child Update Request only if MTD child's multicast address change */
01917         if (addr_ipv6_multicast_scope(group->group) > IPV6_SCOPE_LINK_LOCAL) {
01918             interface->thread_info->childUpdateReqTimer = 1;
01919         }
01920     } else {
01921         thread_extension_mcast_subscrition_change(interface, group, addr_added);
01922     }
01923 }
01924 
01925 void thread_old_partition_data_purge(protocol_interface_info_entry_t *cur)
01926 {
01927     /* Partition has been changed. Wipe out data related to old partition */
01928     thread_management_client_pending_coap_request_kill(cur->id);
01929 
01930     /* Reset previous routing information */
01931     thread_routing_reset(&cur->thread_info->routing);
01932 
01933     /* Flush address cache */
01934     ipv6_neighbour_cache_flush(&cur->ipv6_neighbour_cache);
01935 
01936 }
01937 
01938 #endif
01939