mbed-os | Mbed

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

Repository details

Type:	Library
Created:	12 Dec 2017
Imports:	1018
Forks:	0
Commits:	1
Dependents:	14
Dependencies:	0
Followers:	23
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
