thread_common.c

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