Nucleo_446 | Mbed

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

Repository details

Type:	Program
Mbed OS support:	Mbed OS
Created:	11 Oct 2018
Imports:	23
Forks:	0
Commits:	4
Dependents:	0
Dependencies:	1
Followers:	1
Important changes to repositories hosted on mbed.com

thread_common.c

Repository toolbox

Repository details

Important Information for this Arm website

Important changes to repositories hosted on mbed.com

thread_common.c

Repository toolbox

Repository details

Important Information for this Arm website

Access Warning
