thread_router_bootstrap.c

00001 /*
00002  * Copyright (c) 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 
00030 /*
00031  * \file thread_router_bootstrap.c
00032  * \brief Add short description about this file!!!
00033  *
00034  */
00035 #include "nsconfig.h"
00036 #include <string.h>
00037 #include <ns_types.h>
00038 #include <ns_list.h>
00039 #include <nsdynmemLIB.h>
00040 #include "eventOS_event.h"
00041 #include "eventOS_event_timer.h"
00042 #include "randLIB.h"
00043 #include "shalib.h"
00044 #include "common_functions.h"
00045 #include "NWK_INTERFACE/Include/protocol.h"
00046 #include "net_thread_test.h"
00047 #include "ns_trace.h"
00048 #include "6LoWPAN/Bootstraps/protocol_6lowpan.h"
00049 #include "6LoWPAN/Thread/thread_common.h"
00050 #include "6LoWPAN/Thread/thread_routing.h"
00051 #include "6LoWPAN/Thread/thread_nd.h"
00052 #include "6LoWPAN/Thread/thread_bootstrap.h"
00053 #include "6LoWPAN/Thread/thread_router_bootstrap.h"
00054 #include "6LoWPAN/Thread/thread_host_bootstrap.h"
00055 #include "6LoWPAN/Thread/thread_management_internal.h"
00056 #include "6LoWPAN/Thread/thread_network_synch.h"
00057 #include "6LoWPAN/Thread/thread_discovery.h"
00058 #include "6LoWPAN/Thread/thread_joiner_application.h"
00059 #include "6LoWPAN/Thread/thread_address_registration_client.h"
00060 #include "6LoWPAN/Thread/thread_management_client.h"
00061 #include "6LoWPAN/Thread/thread_management_server.h"
00062 #include "6LoWPAN/Thread/thread_extension.h"
00063 #include "6LoWPAN/Thread/thread_leader_service.h"
00064 #include "6LoWPAN/Thread/thread_beacon.h"
00065 #include "6LoWPAN/Thread/thread_network_data_lib.h"
00066 #include "6LoWPAN/Thread/thread_lowpower_private_api.h"
00067 #include "6LoWPAN/Thread/thread_tmfcop_lib.h"
00068 #include "6LoWPAN/Thread/thread_nvm_store.h"
00069 #include "thread_management_if.h"
00070 #include "Common_Protocols/ipv6.h"
00071 #include "Common_Protocols/icmpv6.h"
00072 #include "Common_Protocols/icmpv6_radv.h"
00073 #include "MLE/mle.h"
00074 #include "MLE/mle_tlv.h"
00075 #include "thread_config.h"
00076 #include "multicast_api.h"
00077 #include "Service_Libs/nd_proxy/nd_proxy.h"
00078 #include "Service_Libs/mle_service/mle_service_api.h"
00079 #include "Service_Libs/blacklist/blacklist.h"
00080 #include "thread_dhcpv6_client.h"
00081 #include "6LoWPAN/MAC/mac_helper.h"
00082 #include "mac_api.h"
00083 #include "6LoWPAN/MAC/mac_data_poll.h"
00084 #include "thread_border_router_api.h"
00085 #include "Core/include/address.h"
00086 
00087 #ifdef HAVE_THREAD_ROUTER
00088 
00089 #define TRACE_GROUP "trbs"
00090 
00091 static bool thread_rfd_device(uint8_t mode);
00092 static uint8_t *thread_tlv_add(protocol_interface_info_entry_t *cur, uint8_t *ptr, uint8_t tlv_type, uint8_t mode);
00093 
00094 static void mle_multicast_push_to_sleep_child(protocol_interface_info_entry_t *cur, buffer_t *buf);
00095 
00096 static void thread_bootstrap_client_router_id_cb(int8_t interface_id, int8_t status, uint16_t router_rloc, const uint8_t router_mask_ptr[9]);
00097 static void thread_router_synch_receive_cb(int8_t interface_id, mle_message_t *mle_msg, mle_security_header_t *security_headers);
00098 static void thread_bootstrap_client_router_id_release_cb(int8_t interface_id, int8_t status, uint16_t router_rloc, const uint8_t router_mask_ptr[9]);
00099 
00100 static int thread_router_synch_accept_request_build(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, uint16_t shortAddress, uint8_t *challenge, uint8_t chalLen, uint8_t *tlvReq, uint8_t tlvReqLen);
00101 static int thread_router_accept_to_endevice(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, uint8_t *challenge, uint8_t chalLen);
00102 static int thread_router_accept_request_build(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, uint16_t shortAddress, uint8_t *challenge, uint8_t chalLen, uint8_t type, bool rssi_tlv, uint8_t rssi);
00103 static int thread_child_update_response(protocol_interface_info_entry_t *cur, uint8_t *dst_address, uint8_t mode, uint16_t short_address, uint32_t timeout, mle_tlv_info_t *addressRegisterTlv,mle_tlv_info_t *tlvReq, mle_tlv_info_t *challengeTlv, uint64_t active_timestamp, uint64_t pending_timestamp);
00104 static int mle_build_and_send_data_response_msg(protocol_interface_info_entry_t *cur, uint8_t *dst_address, uint8_t *data_ptr, uint16_t data_len, mle_tlv_info_t *request_tlv, uint8_t mode);
00105 static int thread_attach_parent_response_build(protocol_interface_info_entry_t *cur, uint8_t *dstAddress, uint8_t *challenge, uint16_t chalLen, uint8_t linkMargin, uint8_t scanMask, uint8_t mode);
00106 static int mle_attach_child_id_response_build(protocol_interface_info_entry_t *cur, uint8_t *dstAddress,thread_pending_child_id_req_t *child_req,mle_neigh_table_entry_t *neigh_info);
00107 
00108 static int8_t thread_router_bootstrap_synch_request_send(protocol_interface_info_entry_t *cur);
00109 static bool thread_child_id_request(protocol_interface_info_entry_t *cur, mle_neigh_table_entry_t *entry_temp);
00110 
00111 static bool thread_rfd_device(uint8_t mode)
00112 {
00113     if ((mode & MLE_DEV_MASK) != MLE_RFD_DEV) {
00114         return false;
00115     }
00116     return true;
00117 }
00118 
00119 static bool thread_router_parent_address_check(protocol_interface_info_entry_t *cur, uint8_t *source_addr)
00120 {
00121     if (thread_info(cur)->thread_endnode_parent &&
00122             addr_iid_matches_eui64(source_addr + 8, thread_info(cur)->thread_endnode_parent->mac64)) {
00123         return true;
00124     }
00125 
00126     return false;
00127 }
00128 
00129 static void thread_send_proactive_an(protocol_interface_info_entry_t *cur)
00130 {
00131     ns_list_foreach(if_address_entry_t, e, &cur->ip_addresses) {
00132         if (e->source == ADDR_SOURCE_DHCP || e->source == ADDR_SOURCE_SLAAC) {
00133             uint8_t br_ml_addr[16];
00134             ns_list_foreach(thread_network_data_prefix_cache_entry_t, prefix, &cur->thread_info->networkDataStorage.localPrefixList) {
00135                 uint8_t prefixBytesLen = prefixBits_to_bytes(prefix->servicesPrefixLen);
00136 
00137                 if (memcmp(e->address, prefix->servicesPrefix, prefixBytesLen) != 0) {
00138                     continue;
00139                 }
00140 
00141                 ns_list_foreach(thread_network_server_data_entry_t, br, &prefix->borderRouterList) {
00142                     if (br->routerID == 0xfffe) {
00143                         continue;
00144                     }
00145 
00146                     uint16_t rloc16 = mac_helper_mac16_address_get(cur);
00147 
00148                     thread_addr_write_mesh_local_16(br_ml_addr, br->routerID, cur->thread_info);
00149                     tr_debug("Sending proactive AN to border router: %s", trace_ipv6(br_ml_addr));
00150                     thread_management_client_proactive_an(cur->id, e->address , rloc16, cur->iid_slaac, br_ml_addr);
00151                 }
00152             }
00153         }
00154     }
00155 }
00156 
00157 int thread_router_bootstrap_mle_advertise(protocol_interface_info_entry_t *cur)
00158 {
00159     /* How was this 40 calculated? Seems to be more than enough, at least.
00160      * MODE = 2+1, SRC_ADDR = 2+2, LINK = 2+1+4*neighbours, ROUTE = 2+MLE_ROUTE_MIN_OPTION_LEN+routers
00161      * Total = 10 + neighbours * 4
00162      */
00163     uint16_t neig_cache_size = 40 + 3;
00164     uint8_t *ptr;
00165 
00166     if (!cur) {
00167         return -1;
00168     }
00169 
00170     if (!thread_is_connected(cur)) {
00171         return -1;
00172     }
00173 
00174     if (thread_i_am_router(cur)) {
00175         neig_cache_size += thread_route_option_size(cur);
00176         neig_cache_size += thread_leader_data_tlv_size(cur);
00177     }
00178 
00179     uint16_t buf_id = mle_service_msg_allocate(cur->id, neig_cache_size, false, MLE_COMMAND_ADVERTISEMENT);
00180     if (buf_id == 0) {
00181         return -1;
00182     }
00183 
00184     uint32_t keySequence;
00185     if (thread_management_get_current_keysequence(cur->id, &keySequence) == 0) {
00186         mle_service_msg_update_security_params(buf_id, 5, 2, keySequence);
00187     }
00188 
00189     //SET Destination address
00190     mle_service_set_msg_destination_address(buf_id, ADDR_LINK_LOCAL_ALL_NODES);
00191 
00192     ptr = mle_service_get_data_pointer(buf_id);
00193     ptr = mle_general_write_source_address(ptr, cur);
00194     if (thread_i_am_router(cur)) {
00195         ptr = thread_route_option_write(cur, ptr);
00196     } else {
00197         *ptr++ = MLE_TYPE_ROUTE;
00198          *ptr++ = 0;
00199     }
00200 
00201     ptr = thread_leader_data_tlv_write(ptr, cur);
00202 
00203     if (mle_service_update_length_by_ptr(buf_id,ptr)!= 0) {
00204         tr_debug("Buffer overflow at message write");
00205     }
00206 
00207     tr_debug("Send MLE Secured Advertisement");
00208     return mle_service_send_message(buf_id);
00209 
00210 }
00211 
00212 static void clone_multicast_to_unicast(protocol_interface_info_entry_t *cur, mle_neigh_table_entry_t *mle_entry, buffer_t *buf)
00213 {
00214     link_configuration_s *linkConfiguration = thread_joiner_application_get_config(cur->id);
00215     if (!linkConfiguration) {
00216         return;
00217     }
00218 
00219     buffer_t *new_buf = buffer_clone(buf);
00220     if ((new_buf->info  & B_DIR_MASK) == B_DIR_UP) {
00221         buffer_data_pointer(new_buf)[IPV6_HDROFF_HOP_LIMIT] = --new_buf->options .hop_limit ;
00222     }
00223 
00224     new_buf->info  = (buffer_info_t) (B_DIR_DOWN | B_TO_IPV6_TXRX | B_FROM_IPV6_FWD);
00225     uint8_t next_hop[16];
00226     memcpy(next_hop, ADDR_LINK_LOCAL_PREFIX, 8);
00227     memcpy(&next_hop[8], mle_entry->mac64, 8);
00228     next_hop[8] ^= 2;
00229 
00230     ipv6_buffer_route_to(new_buf, next_hop, cur);
00231     protocol_push(new_buf);
00232     tr_debug("Cloned %s to %s", tr_ipv6(buf->dst_sa .address ), tr_ipv6(next_hop));
00233 }
00234 
00235 static void mle_multicast_push_to_sleep_child(protocol_interface_info_entry_t *cur, buffer_t *buf)
00236 {
00237     mle_neigh_table_list_t *mle_neigh_list = mle_class_active_list_get(cur->id);
00238     if (!mle_neigh_list) {
00239         return;
00240     }
00241     ns_list_foreach(mle_neigh_table_entry_t, cur_entry, mle_neigh_list) {
00242         if (cur_entry->threadNeighbor) {
00243             if (!(cur_entry->mode & MLE_RX_ON_IDLE)) {
00244                 clone_multicast_to_unicast(cur, cur_entry, buf);
00245             }
00246         }
00247     }
00248 }
00249 
00250 static void thread_multicast_forward_to_child(protocol_interface_info_entry_t *cur, buffer_t *buffer)
00251 {
00252     mle_multicast_push_to_sleep_child(cur, buffer);
00253 }
00254 
00255 static void thread_registered_mcast_forward_to_child(protocol_interface_info_entry_t *cur, buffer_t *buffer)
00256 {
00257     thread_registered_mcast_addr_t *addr = thread_registered_mcast_addr_entry_find(cur, buffer->dst_sa .address );
00258 
00259     if (addr) {
00260         tr_debug("Forwarding to registered multicast address: %s", trace_ipv6(addr->address));
00261 
00262         ns_list_foreach(thread_mcast_child_t, child, &addr->children) {
00263             mle_neigh_table_entry_t *mle_entry = mle_class_get_by_link_address(cur->id, child->mac64, ADDR_802_15_4_LONG );
00264             if (mle_entry && mle_entry->threadNeighbor && !(mle_entry->mode & MLE_RX_ON_IDLE)) {
00265                 clone_multicast_to_unicast(cur, mle_entry, buffer);
00266             }
00267         }
00268     }
00269 }
00270 
00271 void thread_router_bootstrap_multicast_forwarder_enable(protocol_interface_info_entry_t *cur, buffer_t *buf)
00272 {
00273     //tr_debug("Considering forward %s (%s)", tr_ipv6(buf->dst_sa.address), (buf->info & B_DIR_MASK) == B_DIR_UP ? "up" : "down");
00274     if (addr_ipv6_multicast_scope(buf->dst_sa .address ) <= IPV6_SCOPE_INTERFACE_LOCAL) {
00275         return;
00276     }
00277 
00278     // Safe guard: do NOT forward received link-local
00279     // multicast packets to our sleepy children
00280     if ((buf->info  & B_DIR_MASK) == B_DIR_UP &&
00281         addr_ipv6_multicast_scope(buf->dst_sa .address ) <= IPV6_SCOPE_LINK_LOCAL) {
00282         tr_debug("Received link-local multicast; return...");
00283         return;
00284     }
00285 
00286     uint8_t addr[16];
00287     thread_bootstrap_all_nodes_address_generate(addr, cur->thread_info->threadPrivatePrefixInfo.ulaPrefix, IPV6_SCOPE_LINK_LOCAL);
00288     if (addr_ipv6_equal(buf->dst_sa .address , addr)) {
00289         thread_multicast_forward_to_child(cur, buf);
00290         return;
00291     }
00292     thread_bootstrap_all_nodes_address_generate(addr, cur->thread_info->threadPrivatePrefixInfo.ulaPrefix, IPV6_SCOPE_REALM_LOCAL);
00293     if (addr_ipv6_equal(buf->dst_sa .address , addr)) {
00294         thread_multicast_forward_to_child(cur, buf);
00295         return;
00296     }
00297     thread_registered_mcast_forward_to_child(cur, buf);
00298 }
00299 
00300 static void thread_router_synch_receive_cb(int8_t interface_id, mle_message_t *mle_msg, mle_security_header_t *security_headers)
00301 {
00302     thread_leader_data_t leaderData;
00303     uint8_t linkMarginfronNeigh;
00304     uint16_t version, shortAddress, address16;
00305     uint32_t llFrameCounter;
00306     mle_tlv_info_t routing;
00307     mle_neigh_table_entry_t *entry_temp;
00308     tr_debug("Thread MLE message router sync");
00309     //State machine What packet shuold accept in this case
00310     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
00311     if (!cur) {
00312         return;
00313     }
00314 
00315     /* Check that message is from link-local scope */
00316     if(!addr_is_ipv6_link_local(mle_msg->packet_src_address)) {
00317         return;
00318     }
00319 
00320     if (!thread_leader_data_parse(mle_msg->data_ptr, mle_msg->data_length, &leaderData)) {
00321         return;
00322     }
00323 
00324     //Validate ready TLV for router Synch
00325     if ((!mle_tlv_read_16_bit_tlv(MLE_TYPE_VERSION, mle_msg->data_ptr, mle_msg->data_length, &version)) ||
00326             (!mle_tlv_read_16_bit_tlv(MLE_TYPE_ADDRESS16, mle_msg->data_ptr, mle_msg->data_length, &address16)) ||
00327             (!mle_tlv_read_32_bit_tlv(MLE_TYPE_LL_FRAME_COUNTER, mle_msg->data_ptr, mle_msg->data_length, &llFrameCounter)) ||
00328             (!mle_tlv_read_tlv(MLE_TYPE_ROUTE, mle_msg->data_ptr, mle_msg->data_length, &routing)) ||
00329             (!mle_tlv_read_16_bit_tlv(MLE_TYPE_SRC_ADDRESS, mle_msg->data_ptr, mle_msg->data_length, &shortAddress))) {
00330         return ;
00331     }
00332 
00333     uint8_t linkMargin = thread_compute_link_margin(mle_msg->dbm);
00334 
00335     switch (mle_msg->message_type) {
00336         case MLE_COMMAND_ACCEPT: {
00337             tr_info("Accept (ROUTER handler)");
00338             uint32_t mleFrameCounter;
00339             bool new_neigbour;
00340             uint16_t messageId = mle_tlv_validate_response(mle_msg->data_ptr, mle_msg->data_length);
00341 
00342             if (messageId == 0) {
00343                 tr_debug("No matching challenge");
00344                 return;
00345             }
00346             /*Link accept command has an optional MLE Frame counter TLV, if this is not present use link layer frame counter TLV
00347              * as MLE frame counter TLV*/
00348             if(!mle_tlv_read_32_bit_tlv(MLE_TYPE_MLE_FRAME_COUNTER, mle_msg->data_ptr, mle_msg->data_length, &mleFrameCounter)) {
00349                 mleFrameCounter = llFrameCounter;
00350             }
00351 
00352             if (!thread_is_router_addr(address16)){
00353                 return;
00354             }
00355             //Allocate neighbor entry
00356 
00357             entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, true, &new_neigbour);
00358             if (!entry_temp) {
00359                 return;
00360             }
00361 
00362             //Free Response
00363             mle_service_msg_free(messageId);
00364             entry_temp->threadNeighbor = true;
00365             entry_temp->short_adr = shortAddress;
00366             //when allocating neighbour entry, use MLE Frame counter if present to validate further advertisements from the neighbour
00367             entry_temp->mle_frame_counter = mleFrameCounter;
00368             if (entry_temp->timeout_rx) {
00369                 mle_entry_timeout_refresh(entry_temp);
00370             } else {
00371                 mle_tlv_info_t mle_tlv_info;
00372                 uint32_t timeout_tlv;
00373                 if (mle_tlv_option_discover(mle_msg->data_ptr, mle_msg->data_length, MLE_TYPE_TIMEOUT, &mle_tlv_info) > 0) {
00374                     timeout_tlv = common_read_32_bit(mle_tlv_info.dataPtr);
00375                     mle_entry_timeout_update(entry_temp, timeout_tlv);
00376                 } else {
00377                     mle_entry_timeout_update(entry_temp, THREAD_DEFAULT_LINK_LIFETIME);
00378                 }
00379             }
00380 
00381             if (thread_is_router_addr(shortAddress)) {
00382                 entry_temp->handshakeReady = 1;
00383             }
00384 
00385             entry_temp->mode |= MLE_THREAD_REQ_FULL_DATA_SET;
00386 
00387             if (mle_tlv_read_8_bit_tlv(MLE_TYPE_RSSI, mle_msg->data_ptr, mle_msg->data_length, &linkMarginfronNeigh)) {
00388                 thread_routing_update_link_margin(cur, entry_temp->short_adr, linkMargin, linkMarginfronNeigh);
00389             }
00390 
00391             mac_helper_devicetable_set(entry_temp, cur, llFrameCounter, security_headers->KeyIndex, new_neigbour);
00392 
00393             //Copy Leader Data
00394             *cur->thread_info->thread_leader_data = leaderData;
00395             thread_bootstrap_update_ml16_address(cur, address16);
00396             thread_info(cur)->routerShortAddress = address16;
00397             thread_info(cur)->thread_attached_state = THREAD_STATE_CONNECTED_ROUTER;
00398             if (cur->thread_info->thread_leader_data->leaderRouterId == thread_router_id_from_addr(address16)) {
00399                 if (thread_leader_service_thread_partitition_restart(interface_id, &routing)) {
00400                     return;
00401                 }
00402                 thread_network_data_request_send(cur, mle_msg->packet_src_address, false);
00403                 // force leader to learn active/pending sets from data response
00404                 cur->thread_info->leader_synced = true;
00405                 // Prevent the Leader to learn network data from data response
00406                 cur->thread_info->networkDataRequested = false;
00407                 tr_info("Router synch OK as Leader");
00408             } else {
00409                // Decrement data version and request network data to be updated
00410                cur->thread_info->thread_leader_data->dataVersion--;
00411                cur->thread_info->thread_leader_data->stableDataVersion--;
00412                thread_network_data_request_send(cur, mle_msg->packet_src_address, true);
00413                // remove any existing rloc mapping in nvm
00414                thread_nvm_store_mleid_rloc_map_remove();
00415                tr_info("Router synch OK as Router");
00416             }
00417 
00418             thread_router_bootstrap_route_tlv_push(cur, routing.dataPtr, routing.tlvLen , linkMargin, entry_temp);
00419             thread_bootstrap_attached_ready(cur);
00420         }
00421 
00422         break;
00423         default:
00424 
00425             break;
00426     }
00427 }
00428 
00429 static int thread_router_synch_accept_request_build(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, uint16_t shortAddress, uint8_t *challenge, uint8_t chalLen, uint8_t *tlvReq, uint8_t tlvReqLen)
00430 {
00431     uint16_t length = 127;
00432     uint8_t i, tlvType;
00433     mle_message_timeout_params_t timeout;
00434     struct link_configuration *linkConfiguration;
00435     linkConfiguration = thread_joiner_application_get_config(cur->id);
00436     if (!linkConfiguration) {
00437         return -1;
00438     }
00439 
00440     tr_debug("MLE Router Link Synh response");
00441 
00442     for (i = 0; i < tlvReqLen; i++) {
00443         tlvType = tlvReq[i];
00444         if (tlvType == MLE_TYPE_NETWORK_DATA) {
00445             length += 2 + thread_network_data_tlv_size(cur, true);
00446             break;
00447         }
00448     }
00449 
00450     uint16_t bufId = mle_service_msg_allocate(cur->id, length, false, MLE_COMMAND_ACCEPT);
00451     if (bufId == 0) {
00452         return -1;
00453     }
00454 
00455     uint8_t *ptr = mle_service_get_data_pointer(bufId);
00456 
00457     ptr = thread_leader_data_tlv_write(ptr, cur);
00458     ptr = mle_tlv_write_version(ptr, cur->thread_info->version);
00459     ptr = mle_general_write_link_layer_framecounter(ptr, cur);
00460     //SET MLE Frame Counter
00461     ptr = mle_tlv_write_framecounter(ptr, mle_service_security_get_frame_counter(cur->id));
00462 
00463     ptr = mle_tlv_write_response(ptr, challenge, chalLen);
00464     ptr = mle_general_write_source_address(ptr, cur);
00465 
00466     for (i = 0; i < tlvReqLen; i++) {
00467         tlvType = tlvReq[i];
00468         if (tlvType == MLE_TYPE_NETWORK_DATA) {
00469             ptr = thread_network_data_tlv_write(cur, ptr, true);
00470         } else if (tlvType == MLE_TYPE_ROUTE) {
00471             ptr = thread_route_option_write(cur, ptr);
00472         } else if (tlvType == MLE_TYPE_ADDRESS16) {
00473             ptr = mle_tlv_write_short_address(ptr, shortAddress);
00474         } else if (tlvType == MLE_TYPE_RSSI) {
00475 
00476         }
00477     }
00478 
00479     timeout.retrans_max = 0;
00480     timeout.timeout_init = 0;
00481     timeout.timeout_max = 0;
00482     timeout.delay = MLE_STANDARD_RESPONSE_DELAY;
00483     if (mle_service_update_length_by_ptr(bufId,ptr)!= 0) {
00484         tr_debug("Buffer overflow at message write");
00485     }
00486     //SET Destination address
00487     mle_service_set_msg_destination_address(bufId, mle_msg->packet_src_address);
00488     mle_service_set_msg_timeout_parameters(bufId, &timeout);
00489     uint32_t keySequence;
00490     thread_management_get_current_keysequence(cur->id, &keySequence);
00491     mle_service_msg_update_security_params(bufId, 5, 2, keySequence);
00492 
00493     mle_service_send_message(bufId);
00494     return 0;
00495 }
00496 
00497 static int thread_router_accept_to_endevice(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, uint8_t *challenge, uint8_t chalLen)
00498 {
00499     mle_message_timeout_params_t timeout;
00500     uint16_t bufId = mle_service_msg_allocate(cur->id, 64, false, MLE_COMMAND_ACCEPT);
00501     if (bufId == 0) {
00502         return -1;
00503     }
00504 
00505     if (addr_is_ipv6_multicast(mle_msg->packet_src_address)) {
00506         timeout.delay = MLE_STANDARD_RESPONSE_DELAY;
00507     } else {
00508         timeout.delay = MLE_NO_DELAY;
00509     }
00510     tr_debug("MLE Router Link Request response to REED or ED");
00511 
00512     uint8_t *ptr = mle_service_get_data_pointer(bufId);
00513 
00514     timeout.retrans_max = 0;
00515     timeout.timeout_init = 0;
00516     timeout.timeout_max = 0;
00517 
00518     ptr = mle_tlv_write_version(ptr, cur->thread_info->version);
00519     ptr = mle_general_write_link_layer_framecounter(ptr, cur);
00520     //SET MLE Frame Counter
00521     ptr = mle_tlv_write_framecounter(ptr, mle_service_security_get_frame_counter(cur->id));
00522     ptr = mle_tlv_write_response(ptr, challenge, chalLen);
00523     ptr = thread_leader_data_tlv_write(ptr, cur);
00524     ptr = mle_general_write_source_address(ptr, cur);
00525 
00526     if (mle_service_update_length_by_ptr(bufId,ptr)!= 0) {
00527         tr_debug("Buffer overflow at message write");
00528     }
00529     //SET Destination address
00530     mle_service_set_msg_destination_address(bufId, mle_msg->packet_src_address);
00531     mle_service_set_msg_timeout_parameters(bufId, &timeout);
00532     uint32_t keySequence;
00533     thread_management_get_current_keysequence(cur->id, &keySequence);
00534     mle_service_msg_update_security_params(bufId, 5, 2, keySequence);
00535     mle_service_send_message(bufId);
00536     return 0;
00537 
00538 }
00539 
00540 static int thread_router_accept_request_build(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, uint16_t shortAddress, uint8_t *challenge, uint8_t chalLen, uint8_t type, bool rssi_tlv, uint8_t rssi)
00541 {
00542     mle_message_timeout_params_t timeout;
00543     uint16_t bufId;
00544     (void)shortAddress;
00545     uint32_t keySequence;
00546     if (type == MLE_COMMAND_ACCEPT) {
00547         timeout.retrans_max = 0;
00548         timeout.timeout_init = 0;
00549         timeout.timeout_max = 0;
00550         bufId = mle_service_msg_allocate(cur->id, 128, false, type);
00551     } else {
00552         timeout.retrans_max = 1;
00553         timeout.timeout_init = 2;
00554         timeout.timeout_max = 3;
00555         bufId = mle_service_msg_allocate(cur->id, 128, true, type);
00556     }
00557     if (bufId == 0) {
00558         return -1;
00559     }
00560 
00561     if (addr_is_ipv6_multicast(mle_msg->packet_dst_address)) {
00562         timeout.delay = MLE_HALF_SECOND_MAX_DELAY;
00563     } else {
00564         timeout.delay = MLE_NO_DELAY;
00565     }
00566 
00567     tr_debug("MLE Router Link Request response");
00568 
00569     thread_management_get_current_keysequence(cur->id, &keySequence);
00570     mle_service_msg_update_security_params(bufId, 5, 2, keySequence);
00571 
00572     uint8_t *ptr = mle_service_get_data_pointer(bufId);
00573 
00574     ptr = thread_leader_data_tlv_write(ptr, cur);
00575     if (rssi_tlv) {
00576         ptr = mle_tlv_write_version(ptr, cur->thread_info->version);
00577         ptr = mle_tlv_rssi_tlv(ptr, rssi);
00578         if (type != MLE_COMMAND_ACCEPT) {
00579             uint8_t req_tlv = MLE_TYPE_RSSI;
00580             ptr = mle_tlv_req_tlv(ptr, &req_tlv, 1);
00581 
00582         }
00583     }
00584 
00585     if (challenge && chalLen) {
00586         ptr = mle_general_write_link_layer_framecounter(ptr, cur);
00587         //SET MLE Frame Counter
00588         ptr = mle_tlv_write_framecounter(ptr, mle_service_security_get_frame_counter(cur->id));
00589         ptr = mle_tlv_write_response(ptr, challenge, chalLen);
00590     }
00591 
00592     ptr = mle_general_write_source_address(ptr, cur);
00593 
00594     if (mle_service_update_length_by_ptr(bufId,ptr)!= 0) {
00595         tr_debug("Buffer overflow at message write");
00596     }
00597     mle_service_set_msg_destination_address(bufId, mle_msg->packet_src_address);
00598     mle_service_set_msg_timeout_parameters(bufId, &timeout);
00599     mle_service_send_message(bufId);
00600     return 0;
00601 
00602 }
00603 
00604 static uint16_t thread_tlv_len(protocol_interface_info_entry_t *cur, uint8_t *tlv_ptr, uint16_t tlv_len, uint8_t mode)
00605 {// Calculates the extra length for bigger TLVs it is assumed that smaller ones fit in extra reserved space
00606     uint16_t len = 0;
00607     for (uint16_t i = 0; i < tlv_len; i++) {
00608         if (*tlv_ptr == MLE_TYPE_ROUTE) {
00609             len +=  thread_route_option_size(cur);
00610         } else if (*tlv_ptr == MLE_TYPE_NETWORK_DATA) {
00611             len += 2 + thread_network_data_tlv_size(cur, mode & MLE_THREAD_REQ_FULL_DATA_SET);
00612         } else if (*tlv_ptr == MLE_TYPE_ADDRESS16) {
00613             //Short Address
00614             len += 4;
00615         }
00616         tlv_ptr++;
00617     }
00618     return len;
00619 }
00620 
00621 static uint8_t *thread_tlv_add(protocol_interface_info_entry_t *cur, uint8_t *ptr, uint8_t tlv_type, uint8_t mode)
00622 {
00623     struct link_configuration *linkConfiguration;
00624     linkConfiguration = thread_joiner_application_get_config(cur->id);
00625     if (!linkConfiguration) {
00626         return ptr;
00627     }
00628 
00629     switch (tlv_type) {
00630         case MLE_TYPE_SRC_ADDRESS:
00631             /* Add a MLE Source Address TLV */
00632             ptr = mle_general_write_source_address(ptr, cur); /* TODO cur normally first in param list */
00633             break;
00634         case MLE_TYPE_MODE: {
00635             /* Add a MLE Mode TLV */
00636             *ptr++ = MLE_TYPE_MODE;
00637             *ptr++ = 1;
00638             *ptr++ = thread_mode_get_by_interface_ptr(cur);
00639         }
00640         break;
00641         case MLE_TYPE_TIMEOUT:
00642             /* Add a MLE Timeout TLV */
00643             ptr = mle_tlv_write_timeout(ptr, cur->thread_info->host_link_timeout);
00644             break;
00645         case MLE_TYPE_LL_FRAME_COUNTER:
00646             /* Add a MLE Timeout TLV */
00647             ptr = mle_general_write_link_layer_framecounter(ptr, cur);
00648             break;
00649         case MLE_TYPE_NWK_PARAM:
00650             /* Add a MLE Network Parameter TLV */
00651             /* TODO */
00652             break;
00653         case MLE_TYPE_MLE_FRAME_COUNTER:
00654             /* Add a MLE MLE Frame Counter TLV */
00655             /* TODO */
00656             /* Not ostensibly used in Thread */
00657             break;
00658         case MLE_TYPE_ROUTE:
00659             /* Add a MLE Router Table TLV */
00660             ptr = thread_route_option_write(cur, ptr);
00661             break;
00662         case MLE_TYPE_ADDRESS16:
00663             /* Add a MLE Short Address TLV */
00664             /* TODO */
00665             break;
00666         case MLE_TYPE_LEADER_DATA:
00667             ptr = thread_leader_data_tlv_write(ptr, cur);
00668             break;
00669         case MLE_TYPE_NETWORK_DATA:
00670             ptr = thread_network_data_tlv_write(cur, ptr, mode & MLE_THREAD_REQ_FULL_DATA_SET);
00671             break;
00672         case MLE_TYPE_SCAN_MASK:
00673             break;
00674         case MLE_TYPE_CONNECTIVITY:
00675             break;
00676         case MLE_TYPE_RSSI:
00677             break;
00678         default:
00679             /* No other TLV type can be requested */
00680             /* LQI possibly - but not for thread */
00681             break;
00682     }
00683     return ptr;
00684 }
00685 
00686 static int thread_child_update_response(protocol_interface_info_entry_t *cur, uint8_t *dst_address, uint8_t mode, uint16_t short_address, uint32_t timeout, mle_tlv_info_t *addressRegisterTlv,mle_tlv_info_t *tlvReq, mle_tlv_info_t *challengeTlv, uint64_t active_timestamp, uint64_t pending_timestamp)
00687 {
00688     uint16_t i;
00689     uint16_t len = 64;
00690     uint32_t keySequence;
00691     bool add_active_configuration = false;
00692     bool add_pending_configuration = false;
00693     uint64_t own_pending_timestamp = 0;
00694     uint8_t *ptr;
00695     if (!thread_info(cur)) {
00696         return -1;
00697     }
00698     link_configuration_s *link_configuration;
00699     link_configuration = thread_joiner_application_get_config(cur->id);
00700 
00701     if (!link_configuration) {
00702         return -1;
00703     }
00704 
00705     len += 10; // active timestamp tlv size
00706     len += thread_pending_timestamp_tlv_size(cur);
00707 
00708     if (!active_timestamp || active_timestamp != link_configuration->timestamp) {
00709         len += thread_active_operational_dataset_size(cur);
00710         add_active_configuration = true;
00711     }
00712     own_pending_timestamp = thread_joiner_application_pending_config_timestamp_get(cur->id);
00713     // if pending config is not in sync from requested device
00714     if (!pending_timestamp ||
00715             (own_pending_timestamp && own_pending_timestamp != pending_timestamp)) {
00716         len += thread_pending_operational_dataset_size(cur);
00717         add_pending_configuration = true;
00718     }
00719     if (tlvReq && tlvReq->tlvLen) {
00720         mle_tlv_ignore(tlvReq->dataPtr, tlvReq->tlvLen, MLE_TYPE_LL_FRAME_COUNTER);
00721         len += thread_tlv_len(cur, tlvReq->dataPtr, tlvReq->tlvLen, mode);
00722     }
00723     if (addressRegisterTlv && addressRegisterTlv->tlvLen){
00724         len += addressRegisterTlv->tlvLen;
00725     }
00726 
00727     uint16_t bufId = mle_service_msg_allocate(cur->id, len, false, MLE_COMMAND_CHILD_UPDATE_RESPONSE);
00728 
00729     if (bufId == 0) {
00730         return -1;
00731     }
00732 
00733     tr_debug("MLE Child synch response");
00734 
00735     thread_management_get_current_keysequence(cur->id, &keySequence);
00736     mle_service_msg_update_security_params(bufId, 5, 2, keySequence);
00737 
00738     ptr = mle_service_get_data_pointer(bufId);
00739 
00740     ptr = mle_tlv_write_mode(ptr, mode); //Write Mode Allways
00741 
00742     if (addressRegisterTlv && addressRegisterTlv->tlvLen){
00743         *ptr++ = MLE_TYPE_ADDRESS_REGISTRATION;
00744         *ptr++ = addressRegisterTlv->tlvLen;
00745         memcpy(ptr, addressRegisterTlv->dataPtr, addressRegisterTlv->tlvLen);
00746         ptr += addressRegisterTlv->tlvLen;
00747     }
00748     //Set SRC
00749     ptr = mle_general_write_source_address(ptr, cur);
00750     //SET leader data
00751     ptr = thread_leader_data_tlv_write(ptr, cur);
00752     if (timeout) {
00753         ptr = mle_tlv_write_timeout(ptr, timeout);
00754     }
00755 
00756     if (challengeTlv && challengeTlv->tlvLen) {
00757         ptr = mle_tlv_write_response(ptr, challengeTlv->dataPtr, challengeTlv->tlvLen);
00758         ptr = mle_general_write_link_layer_framecounter(ptr, cur);
00759         //SET MLE Frame Counter
00760         ptr = mle_tlv_write_framecounter(ptr, mle_service_security_get_frame_counter(cur->id));
00761     }
00762 
00763     if (tlvReq && tlvReq->tlvLen) {
00764         /* Go through the requested TLV mask */
00765         for (i = 0; i < tlvReq->tlvLen; i++) {
00766             ptr = thread_tlv_add(cur, ptr, tlvReq->dataPtr[i], mode);
00767         }
00768         if (mle_tlv_requested(tlvReq->dataPtr, tlvReq->tlvLen, MLE_TYPE_ADDRESS16)) {
00769             ptr = mle_tlv_write_short_address(ptr, short_address);
00770         }
00771         if (mle_tlv_requested(tlvReq->dataPtr, tlvReq->tlvLen, MLE_TYPE_NETWORK_DATA)) {
00772             ptr = thread_active_timestamp_write(cur,ptr);
00773             ptr = thread_pending_timestamp_write(cur,ptr);
00774             if (add_active_configuration) {
00775                 ptr = thread_active_operational_dataset_write(cur, ptr);
00776             }
00777             if (add_pending_configuration) {
00778                 ptr = thread_pending_operational_dataset_write(cur, ptr);
00779             }
00780         }
00781     }
00782 
00783     if (mle_service_update_length_by_ptr(bufId,ptr)!= 0) {
00784         tr_debug("Buffer overflow at message write");
00785     }
00786     mle_service_set_msg_destination_address(bufId, dst_address);
00787     mle_service_send_message(bufId);
00788     return 0;
00789 
00790 }
00791 
00792 static int mle_build_and_send_data_response_msg(protocol_interface_info_entry_t *cur, uint8_t *dst_address, uint8_t *data_ptr, uint16_t data_len, mle_tlv_info_t *request_tlv, uint8_t mode)
00793 {
00794     uint16_t length = 64 + 20 + 4; // source address 4 bytes
00795     uint8_t *ptr;
00796     uint_fast16_t i;
00797     uint64_t active_timestamp = 0;
00798     uint64_t pending_timestamp = 0;
00799     bool add_active_configuration = false;
00800     bool add_pending_configuration = false;
00801     bool active_timestamp_present_in_request = false;
00802     bool pending_timestamp_present_in_request = false;
00803     uint64_t own_pending_timestamp = 0;
00804     link_configuration_s *link_configuration;
00805     link_configuration = thread_joiner_application_get_config(cur->id);
00806 
00807     if (!link_configuration) {
00808         return 0;
00809     }
00810 
00811     active_timestamp_present_in_request = mle_tlv_read_64_bit_tlv(MLE_TYPE_ACTIVE_TIMESTAMP, data_ptr, data_len, &active_timestamp);
00812     pending_timestamp_present_in_request = mle_tlv_read_64_bit_tlv(MLE_TYPE_PENDING_TIMESTAMP, data_ptr, data_len, &pending_timestamp);
00813 
00814     length += thread_pending_timestamp_tlv_size(cur);
00815 
00816     if (!active_timestamp_present_in_request || active_timestamp != link_configuration->timestamp) {
00817         length += thread_active_operational_dataset_size(cur);
00818         add_active_configuration = true;
00819     }
00820     own_pending_timestamp = thread_joiner_application_pending_config_timestamp_get(cur->id);
00821     // if pending config is not in sync from requested device
00822     if (!pending_timestamp_present_in_request ||
00823             (own_pending_timestamp && own_pending_timestamp != pending_timestamp)) {
00824         length += thread_pending_operational_dataset_size(cur);
00825         add_pending_configuration = true;
00826     }
00827 
00828     // Leader data is added by default
00829     mle_tlv_ignore(request_tlv->dataPtr, request_tlv->tlvLen, MLE_TYPE_LEADER_DATA);
00830     length += thread_tlv_len(cur, request_tlv->dataPtr, request_tlv->tlvLen, mode);
00831 
00832     //link metrics info
00833     length += thread_lowpower_link_metrics_length(cur, dst_address);
00834 
00835     uint16_t bufId =  mle_service_msg_allocate(cur->id, length, false, MLE_COMMAND_DATA_RESPONSE);
00836 
00837     if (bufId == 0) {
00838         return -1;
00839     }
00840 
00841     tr_debug("Send MLE data response, %s mode=%x", trace_ipv6(dst_address), mode);
00842     ptr = mle_service_get_data_pointer(bufId);
00843 
00844     ptr = thread_leader_data_tlv_write(ptr, cur);
00845     /* Go through the requested TLV mask */
00846     for (i = 0; i < request_tlv->tlvLen; i++) {
00847         ptr = thread_tlv_add(cur, ptr, request_tlv->dataPtr[i], mode);
00848     }
00849 
00850     ptr = thread_active_timestamp_write(cur,ptr);
00851     ptr = thread_pending_timestamp_write(cur,ptr);
00852     ptr = mle_general_write_source_address(ptr, cur);
00853     if (add_active_configuration) {
00854         ptr = thread_active_operational_dataset_write(cur, ptr);
00855     }
00856     if (add_pending_configuration) {
00857         ptr = thread_pending_operational_dataset_write(cur, ptr);
00858     }
00859     ptr = thread_lowpower_link_metrics_write(cur, dst_address, ptr);
00860 
00861     if (mle_service_update_length_by_ptr(bufId,ptr)!= 0) {
00862         tr_debug("Buffer overflow at message write");
00863     }
00864     mle_service_set_msg_destination_address(bufId, dst_address);
00865     //Set Security
00866     uint32_t keySequence;
00867     thread_management_get_current_keysequence(cur->id, &keySequence);
00868     mle_service_msg_update_security_params(bufId, 5, 2, keySequence);
00869 
00870     mle_service_send_message(bufId);
00871     return 0;
00872 
00873 
00874 }
00875 
00876 //this responds with zeros currently, needs to be updated after results are computed
00877 
00878 static int thread_attach_parent_response_build(protocol_interface_info_entry_t *cur, uint8_t *dstAddress, uint8_t *challenge, uint16_t chalLen, uint8_t linkMargin, uint8_t scanMask, uint8_t mode)
00879 {
00880     /**
00881      * This Message need to add Next Items:
00882      * - MLE_TYPE_SRC_ADDRESS
00883      * - MLE_TYPE_LEADER_DATA
00884      * - MLE_TYPE_LL_FRAME_COUNTER
00885      * - MLE_TYPE_MLE FRAME_COUNTER
00886      * - MLE_TYPE_CHALLENGE
00887      * - MLE_TYPE_RESPONSE
00888      * - MLE_TYPE_CONNECTIVITY
00889      * - MLE_TYPE_RSSI
00890      * - MLE_TYPE_VERSION
00891      */
00892 
00893     mle_message_timeout_params_t timeout;
00894 
00895     uint16_t bufId = mle_service_msg_allocate(cur->id, 128, true, MLE_COMMAND_PARENT_RESPONSE);
00896     if (bufId == 0) {
00897         return -1;
00898     }
00899     tr_debug("Build MLE Parent response");
00900 
00901     uint32_t keySequence;
00902     uint8_t *ptr = mle_service_get_data_pointer(bufId);
00903 
00904     //Set Security
00905     thread_management_get_current_keysequence(cur->id, &keySequence);
00906     mle_service_msg_update_security_params(bufId, 5, 2, keySequence);
00907 
00908     //SET Leader Data
00909     ptr = thread_leader_data_tlv_write(ptr, cur);
00910 
00911     // SET Connectivity TLV
00912     ptr = thread_connectivity_tlv_write(ptr, cur, mode);
00913 
00914     //SET LL Frame Counter
00915     ptr = mle_general_write_link_layer_framecounter(ptr, cur);
00916     //SET MLE Frame Counter
00917     ptr = mle_tlv_write_framecounter(ptr, mle_service_security_get_frame_counter(cur->id));
00918     //SET Source Address
00919     ptr = mle_general_write_source_address(ptr, cur);
00920     //SET Response
00921     if (challenge != NULL) {
00922         ptr = mle_tlv_write_response(ptr, challenge, chalLen);
00923     }
00924     //RSSI
00925     ptr = mle_tlv_rssi_tlv(ptr, linkMargin);
00926     //Set Version
00927     ptr = mle_tlv_write_version(ptr, cur->thread_info->version);
00928 
00929     if (mle_service_update_length_by_ptr(bufId,ptr)!= 0) {
00930         tr_debug("Buffer overflow at message write");
00931     }
00932     timeout.retrans_max = 1;
00933     timeout.timeout_init = 6;
00934     timeout.timeout_max = 6;
00935 
00936     switch (scanMask & 0xc0) {
00937         case 0x80:
00938             timeout.delay = MLE_HALF_SECOND_MAX_DELAY;
00939             break;
00940         default:
00941             timeout.delay = MLE_STANDARD_RESPONSE_DELAY;
00942             break;
00943     }
00944 
00945     //SET Destination address
00946     mle_service_set_msg_destination_address(bufId, dstAddress);
00947     mle_service_set_msg_timeout_parameters(bufId, &timeout);
00948     mle_service_send_message(bufId);
00949     return 0;
00950 }
00951 
00952 int thread_router_bootstrap_reset_child_info(protocol_interface_info_entry_t *cur, mle_neigh_table_entry_t *child)
00953 {
00954     /* Cleanup occurs for /any/ link we lose to something that looks like a child address,
00955      * not just links that are /now/ our children.
00956      * Due to REED/partition transitions the address may not look like a current child address;
00957      * we could be holding a child entry for future repromotion to router with same ID.
00958      */
00959     if (thread_is_router_addr(child->short_adr) || child->short_adr >= 0xfffe) {
00960         return -1;
00961     }
00962     tr_debug("Child free %x", child->short_adr);
00963     thread_dynamic_storage_child_info_clear(cur->id, child);
00964 
00965     /* As we are losing a link to a child address, we can assume that if we have an IP neighbour cache
00966      * mapping to that address, it is no longer valid. We must have been their parent, and they must be
00967      * finding a new parent, and hence a new 16-bit address. (Losing a link to a router address would not
00968      * invalidate our IP->16-bit mapping.)
00969      */
00970     protocol_6lowpan_release_short_link_address_from_neighcache(cur, child->short_adr);
00971 
00972     // If Child's RLOC16 appears in the Network Data send the RLOC16 to the Leader
00973     if (thread_network_data_services_registered(&cur->thread_info->networkDataStorage, child->short_adr)) {
00974         tr_debug("Remove references to Child's RLOC16 from the Network Data");
00975         thread_management_client_network_data_unregister(cur->id, child->short_adr);
00976     }
00977 
00978     // Clear all (sleepy) child registrations to multicast groups
00979     thread_child_mcast_entries_remove(cur, child->mac64);
00980 
00981     return 0;
00982 }
00983 
00984 void thread_router_bootstrap_child_information_clear(protocol_interface_info_entry_t *cur)
00985 {
00986     /* make sure that the child info (from previous partition if any)
00987         is cleared if no router address is got from leader */
00988 
00989     if (!cur->thread_info) {
00990         return;
00991     }
00992 
00993     // Remove mle neighbour entries for children in previous partition
00994     mle_neigh_table_list_t *entry_list = mle_class_active_list_get(cur->id);
00995     if (!entry_list) {
00996         return;
00997     }
00998     ns_list_foreach_safe(mle_neigh_table_entry_t, table_entry, entry_list) {
00999         if (table_entry->short_adr < 0xfffe && !thread_is_router_addr(table_entry->short_adr)) {
01000             mle_class_remove_entry(cur->id, table_entry);
01001         }
01002     }
01003 }
01004 static void thread_router_bootstrap_invalid_child_information_clear(protocol_interface_info_entry_t *cur, uint16_t router_rloc)
01005 {
01006 
01007     tr_debug("Thread Short address changed old: %x new: %x", cur->thread_info->routerShortAddress, router_rloc);
01008 
01009     mle_neigh_table_list_t *entry_list = mle_class_active_list_get(cur->id);
01010     if (!entry_list) {
01011         return;
01012     }
01013 
01014     // scrub neighbours with child addresses that are not ours
01015     ns_list_foreach_safe(mle_neigh_table_entry_t, table_entry, entry_list) {
01016         if (table_entry->short_adr < 0xfffe &&
01017                 !thread_is_router_addr(table_entry->short_adr) &&
01018                 thread_router_addr_from_addr(table_entry->short_adr) != router_rloc) {
01019             mle_class_remove_entry(cur->id, table_entry);
01020         }
01021     }
01022 }
01023 
01024 static void thread_bootstrap_client_router_id_cb(int8_t interface_id, int8_t status, uint16_t router_rloc, const uint8_t router_mask_ptr[9])
01025 {
01026     uint8_t ml16[16];
01027     protocol_interface_info_entry_t *cur;
01028     uint8_t parent_router_id = FAST_ROUTE_NO_ROUTE;
01029 
01030     cur = protocol_stack_interface_info_get_by_id(interface_id);
01031     tr_debug("RouterID CB");
01032     if (!cur) {
01033         return;
01034     }
01035     if (!cur->thread_info->routerIdReqCoapID) {
01036         return;
01037     }
01038     cur->thread_info->routerIdReqCoapID = 0;
01039 
01040     if (cur->thread_info->thread_device_mode != THREAD_DEVICE_MODE_ROUTER ||
01041         cur->thread_info->leader_private_data ) {
01042         // Test api can cause these states or some out of sync operation.
01043         tr_warn("Router address for non-REED");
01044         return;
01045     }
01046 
01047     if (status < 0) {
01048         thread_bootstrap_router_id_get_fail(cur);
01049         return;
01050     }
01051 
01052     uint8_t routeId = *router_mask_ptr++;
01053 
01054     if(cur->thread_info->thread_endnode_parent) {
01055         parent_router_id = cur->thread_info->thread_endnode_parent->router_id;
01056     }
01057 
01058     thread_router_bootstrap_invalid_child_information_clear(cur,router_rloc);
01059 
01060     // Release network data from old address
01061     cur->thread_info->localServerDataBase.release_old_address = true;
01062 
01063     //ADD New ML16
01064     // This should be used thread_bootstrap_update_ml16_address(cur, router_rloc);
01065     thread_clean_old_16_bit_address_based_addresses(cur);
01066     mac_helper_mac16_address_set(cur, router_rloc);
01067     cur->thread_info->routerShortAddress = router_rloc;
01068     memcpy(ml16, cur->thread_info->threadPrivatePrefixInfo.ulaPrefix, 8);
01069     memcpy(&ml16[8], ADDR_SHORT_ADR_SUFFIC , 6);
01070     common_write_16_bit(router_rloc, &ml16[14]);
01071 
01072     cur->global_address_available = true;
01073     //Register UL16
01074     if_address_entry_t *def_address = addr_add(cur, ml16, 64, ADDR_SOURCE_UNKNOWN, 0xffffffff, 0xffffffff, true);
01075     if (!def_address) {
01076         return;
01077     }
01078 
01079     //  /* XXX Is short_src_adr ever reset? Is it undefined if info not in msg? */
01080     tr_debug("Route seq %d Router mask: %s", routeId, trace_array(router_mask_ptr, 8));
01081     cur->thread_info->routing.router_id_sequence_valid = false;
01082     if (parent_router_id != FAST_ROUTE_NO_ROUTE) {
01083         thread_routing_force_next_hop(cur, routeId, router_mask_ptr, parent_router_id);
01084     } else {
01085         tr_warn("No parent information available, no initial route set.");
01086         thread_routing_update_id_set(cur, routeId, router_mask_ptr);
01087     }
01088     thread_bootstrap_attached_active_router(cur);
01089 
01090 }
01091 
01092 void thread_router_bootstrap_router_id_request(protocol_interface_info_entry_t *cur, uint8_t status)
01093 {
01094     int router_id_req_status;
01095     tr_debug("Router ID Request");
01096     if (cur->thread_info->routerIdReqCoapID) {
01097         tr_warn("Router ID already requested");
01098         return;
01099     }
01100 
01101     router_id_req_status = thread_management_client_router_id_get(cur->id, cur->mac, cur->thread_info->routerShortAddress, thread_bootstrap_client_router_id_cb, status);
01102     tr_debug("Coap address req, ID=%d", router_id_req_status);
01103     if (router_id_req_status > 0) {
01104         cur->thread_info->routerIdReqCoapID = (uint16_t)router_id_req_status;
01105     }
01106 }
01107 
01108 static int mle_attach_child_id_response_build(protocol_interface_info_entry_t *cur, uint8_t *dstAddress,thread_pending_child_id_req_t *child_req,mle_neigh_table_entry_t *neigh_info)
01109 {
01110     uint16_t len = 12 + 4; //Leader data and short address
01111     uint8_t *ptr;
01112     bool fullList = false;
01113     uint64_t pending_timestamp;
01114     struct link_configuration *linkConfiguration;
01115     linkConfiguration = thread_joiner_application_get_config(cur->id);
01116     if (!linkConfiguration) {
01117         return -1;
01118     }
01119 
01120     if (neigh_info->mode & MLE_THREAD_REQ_FULL_DATA_SET) {
01121         fullList = true;
01122     }
01123 
01124     if (child_req->shortAddressReq) {
01125         //Short Address
01126         len += 4;
01127     }
01128 
01129     if (child_req->networkDataReq) {
01130         len += 2 + thread_network_data_tlv_size(cur, fullList);
01131     }
01132 
01133     if (child_req->routeReq) {
01134         len += thread_route_option_size(cur);
01135     }
01136     if (child_req->request_active_config || child_req->active_timestamp != linkConfiguration->timestamp) {
01137         len += thread_active_operational_dataset_size(cur);
01138     }
01139 
01140     //always put active timestamp
01141     len += 10;
01142     len += thread_pending_timestamp_tlv_size(cur);
01143     pending_timestamp = thread_joiner_application_pending_config_timestamp_get(cur->id);
01144     if (pending_timestamp && pending_timestamp != child_req->pending_timestamp) {
01145         len += thread_pending_operational_dataset_size(cur);
01146     }
01147 
01148     mle_message_timeout_params_t timeout;
01149     uint16_t bufId = mle_service_msg_allocate(cur->id, len , false, MLE_COMMAND_CHILD_ID_RESPONSE);
01150     if (bufId == 0) {
01151         return -1;
01152     }
01153     uint32_t keySequence;
01154     thread_management_get_current_keysequence(cur->id, &keySequence);
01155     mle_service_msg_update_security_params(bufId, 5, 2, keySequence);
01156 
01157     tr_debug("Send MLE Child ID response");
01158 
01159     ptr = mle_service_get_data_pointer(bufId);
01160 
01161     ptr = thread_leader_data_tlv_write(ptr, cur);
01162 
01163     ptr = mle_general_write_source_address(ptr, cur);
01164 
01165     if (child_req->shortAddressReq) {
01166         ptr = mle_tlv_write_short_address(ptr, neigh_info->short_adr);
01167     }
01168 
01169     if (child_req->routeReq) {
01170         ptr = thread_route_option_write(cur, ptr);
01171     }
01172 
01173     if (child_req->networkDataReq) {
01174         ptr = thread_network_data_tlv_write(cur, ptr, fullList);
01175     }
01176 
01177     if (child_req->active_timestamp != linkConfiguration->timestamp) {
01178         ptr = thread_active_operational_dataset_write(cur, ptr);
01179     }
01180 
01181     //always write active timestamp
01182     ptr = thread_active_timestamp_write(cur,ptr);
01183     if (pending_timestamp > 0 && pending_timestamp != child_req->pending_timestamp) {
01184         ptr = thread_pending_operational_dataset_write(cur, ptr);
01185     }
01186 
01187     ptr = thread_pending_timestamp_write(cur,ptr);
01188 
01189 
01190     if (mle_service_update_length_by_ptr(bufId,ptr)!= 0) {
01191         tr_debug("Buffer overflow at message write");
01192     }
01193     timeout.retrans_max = 0;
01194     timeout.timeout_init = 0;
01195     timeout.timeout_max = 0;
01196     timeout.delay = MLE_NO_DELAY;
01197     mle_service_set_msg_destination_address(bufId, dstAddress);
01198     mle_service_set_msg_timeout_parameters(bufId, &timeout);
01199     mle_service_send_message(bufId);
01200     return 0;
01201 
01202 
01203 }
01204 uint16_t thread_router_bootstrap_child_count_get(protocol_interface_info_entry_t *cur)
01205 {
01206     uint16_t child_count = 0;
01207     uint16_t router_address = thread_info(cur)->routerShortAddress;
01208     if (router_address >= 0xfffe) {
01209         router_address = mac_helper_mac16_address_get(cur);
01210     }
01211     if (router_address & THREAD_CHILD_MASK) {
01212         return 0; //I am child
01213     }
01214     if (router_address >= 0xfffe) {
01215         return 0;
01216     }
01217     mle_neigh_table_list_t *mle_table = mle_class_active_list_get(cur->id);
01218     if (!mle_table) {
01219         return -1;
01220     }
01221     ns_list_foreach(mle_neigh_table_entry_t, cur_entry, mle_table) {
01222         if (thread_router_addr_from_addr(cur_entry->short_adr) == router_address) {
01223             child_count++;
01224         }
01225     }
01226     return child_count;
01227 }
01228 
01229 static uint16_t thread_router_bootstrap_child_address_generate(protocol_interface_info_entry_t *cur)
01230 {
01231     mle_neigh_table_list_t *mle_table = mle_class_active_list_get(cur->id);
01232     if (!mle_table) {
01233         return -1;
01234     }
01235 
01236     if (thread_router_bootstrap_child_count_get(cur) >= cur->thread_info->maxChildCount) {
01237         tr_info("Maximum count %d reached", cur->thread_info->maxChildCount);
01238         return 0xffff;
01239     }
01240 
01241     bool address_allocated = false;
01242     for (uint16_t i = 0; i < cur->thread_info->maxChildCount; i++) {
01243         address_allocated = false;
01244         cur->thread_info->lastAllocatedChildAddress = (cur->thread_info->lastAllocatedChildAddress % THREAD_MAX_CHILD_ID_COUNT) + 1;
01245         ns_list_foreach(mle_neigh_table_entry_t, cur_entry, mle_table) {
01246             if ( (cur_entry->short_adr & THREAD_CHILD_MASK) == cur->thread_info->lastAllocatedChildAddress) {
01247                 address_allocated = true;
01248                 break;
01249             }
01250         }
01251         if (!address_allocated){
01252             break;
01253         }
01254     }
01255     if (address_allocated){
01256         // all possible addresses already allocated
01257         return 0xffff;
01258     }
01259     return ((mac_helper_mac16_address_get(cur) & THREAD_ROUTER_MASK) | cur->thread_info->lastAllocatedChildAddress);
01260 }
01261 
01262 static bool thread_child_id_request(protocol_interface_info_entry_t *cur, mle_neigh_table_entry_t *entry_temp)
01263 {
01264         //Remove All Short address links from router
01265         if (entry_temp->short_adr != 0xffff) {
01266             protocol_6lowpan_release_short_link_address_from_neighcache(cur, entry_temp->short_adr);
01267         }
01268 
01269         //allocate child address if current is router, 0xffff or not our child
01270         if (!thread_addr_is_child(mac_helper_mac16_address_get(cur), entry_temp->short_adr)) {
01271             entry_temp->short_adr = thread_router_bootstrap_child_address_generate(cur);
01272         }
01273 
01274         if (entry_temp->short_adr == 0xffff) {
01275             return false;
01276         }
01277 
01278         // Store this child info to the NVM
01279         thread_dynamic_storage_child_info_store(cur->id, entry_temp);
01280     //}
01281     return true;
01282 }
01283 
01284 void thread_router_bootstrap_child_id_handler(protocol_interface_info_entry_t *cur)
01285 {
01286     thread_pending_child_id_req_t *req;
01287     bool new_neigbour = false;
01288 
01289     if (cur->thread_info->thread_attached_state == THREAD_STATE_CONNECTED) {
01290         if (!cur->thread_info->routerIdReqCoapID) {
01291             tr_info("Upgrade REED to Router");
01292             thread_router_bootstrap_router_id_request(cur, THREAD_COAP_STATUS_TLV_HAVE_CHILD_ID_REQUEST);
01293         }
01294         return;
01295     }
01296     req = thread_child_id_request_entry_get_from_the_list(cur);
01297 
01298     if (!req) {
01299         tr_debug("Child Id Req pending list empty");
01300         return;
01301     }
01302 
01303     if (cur->thread_info->thread_attached_state != THREAD_STATE_CONNECTED_ROUTER) {
01304         // We are not router can not process
01305         goto free_request;
01306     }
01307 
01308     uint8_t ll64[16];
01309     //set LL64 destination address
01310     memcpy(ll64, ADDR_LINK_LOCAL_PREFIX , 8);
01311     memcpy(&ll64[8], req->euid64 , 8);
01312     ll64[8] ^= 2;
01313     //Allocate entry
01314     mle_neigh_table_entry_t *entry_temp = mle_class_get_entry_by_ll64(cur->id, req->linkMargin, ll64, true, &new_neigbour);
01315 
01316     if (!entry_temp) {
01317         //Send link reject
01318         thread_link_reject_send(cur, ll64);
01319         goto free_request;
01320     }
01321 
01322     // If mac frame couter is less than previous we need to leave the old one
01323 
01324     //Update or set neigh info
01325     entry_temp->holdTime = 90;
01326 
01327     mle_entry_timeout_update(entry_temp, req->timeout);
01328     entry_temp->mode = req->mode;
01329     entry_temp->threadNeighbor = true;
01330     entry_temp->handshakeReady = 1;
01331     entry_temp->mle_frame_counter = req->mleFrameCounter;
01332 
01333     if (req->shortAddressReq) {
01334         if (!thread_child_id_request(cur, entry_temp)) {
01335             mle_class_remove_entry(cur->id, entry_temp);
01336             thread_link_reject_send(cur, ll64);
01337             goto free_request;
01338         }
01339     }
01340     if (new_neigbour) {
01341         mac_helper_devicetable_set(entry_temp, cur, req->frameCounter, req->keyId, new_neigbour);
01342     } else {
01343         // in get response handler this will update the short address from MLE table
01344         mlme_get_t get_req;
01345         get_req.attr = macDeviceTable;
01346         get_req.attr_index = entry_temp->attribute_index;
01347         cur->mac_api->mlme_req(cur->mac_api, MLME_GET, &get_req);
01348     }
01349 
01350     //Register MLEID if RRFD device
01351     if ((entry_temp->mode & MLE_DEV_MASK) == MLE_RFD_DEV) {
01352         uint8_t tempIPv6Address[16];
01353         memcpy(tempIPv6Address, thread_info(cur)->threadPrivatePrefixInfo.ulaPrefix, 8);
01354         memcpy(&tempIPv6Address[8], req->eiid, 8);
01355         memcpy(&entry_temp->mlEid, req->eiid, 8);
01356 
01357         tr_debug("Register %s", trace_ipv6(tempIPv6Address));
01358         //Register GP --> 16
01359         thread_nd_address_registration(cur, tempIPv6Address, entry_temp->short_adr, cur->mac_parameters->pan_id, entry_temp->mac64);
01360     }
01361 
01362     mle_attach_child_id_response_build(cur,ll64,req, entry_temp);
01363 
01364     free_request:
01365     ns_dyn_mem_free(req);
01366     thread_bootstrap_child_id_request(cur);
01367 }
01368 
01369 static void thread_address_registration_tlv_parse(uint8_t *ptr, uint16_t data_length, protocol_interface_info_entry_t *cur, uint16_t mac16, uint8_t *mac64)
01370 {
01371     lowpan_context_t *ctx;
01372     uint8_t tempIPv6Address[16];
01373     uint8_t ctxId;
01374 
01375     while (data_length) {
01376         //Read
01377         ctxId = *ptr++;
01378         if (ctxId & 0x80) {
01379             ctxId &= 0x0f;
01380             ctx = lowpan_contex_get_by_id(&cur->lowpan_contexts, ctxId);
01381             if (ctx) {
01382                 memcpy(tempIPv6Address, ctx->prefix, 8);
01383                 memcpy(&tempIPv6Address[8], ptr, 8);
01384                 tr_debug("Register %s", trace_ipv6(tempIPv6Address));
01385                 //Register GP --> 16
01386                 thread_nd_address_registration(cur, tempIPv6Address, mac16, cur->mac_parameters->pan_id, mac64);
01387                 thread_extension_address_registration(cur, tempIPv6Address, mac64);
01388             } else {
01389                 tr_debug("No Context %u", ctxId);
01390             }
01391             ptr += 8;
01392             data_length -= 9;
01393         } else {
01394             tr_debug("Register %s", trace_ipv6(ptr));
01395 
01396             if (addr_is_ipv6_multicast(ptr)) {
01397                 // Register multicast address (higher scope than link-local)
01398                 if (addr_ipv6_multicast_scope(ptr) > IPV6_SCOPE_LINK_LOCAL) {
01399                     addr_add_group(cur, ptr);
01400                     if (thread_child_mcast_entry_get(cur, ptr, mac64)) {
01401                         tr_debug("Added sleepy multicast registration entry.");
01402                     }
01403                 }
01404             } else {
01405                 //Register GP --> 16
01406                 thread_nd_address_registration(cur, ptr, mac16, cur->mac_parameters->pan_id, mac64);
01407                 thread_extension_address_registration(cur, ptr, mac64);
01408             }
01409 
01410             ptr += 16;
01411             data_length -= 17;
01412         }
01413     }
01414 }
01415 
01416 void thread_router_bootstrap_mle_receive_cb(int8_t interface_id, mle_message_t *mle_msg, mle_security_header_t *security_headers)
01417 {
01418     thread_leader_data_t leaderData;
01419     mle_neigh_table_entry_t *entry_temp;
01420     bool rssiTLV;
01421     bool leaderDataReceived;
01422     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
01423     if (!cur) {
01424         return;
01425     }
01426 
01427     //TLV REQUSTED
01428     if (mle_tlv_type_requested(MLE_TYPE_RSSI, mle_msg->data_ptr, mle_msg->data_length)) {
01429         rssiTLV = true;
01430     } else {
01431         rssiTLV = false;
01432     }
01433 
01434     if (thread_leader_data_parse(mle_msg->data_ptr, mle_msg->data_length, &leaderData)) {
01435         leaderDataReceived = true;
01436     } else {
01437         leaderDataReceived = false;
01438     }
01439 
01440 
01441     uint8_t linkMargin = thread_compute_link_margin(mle_msg->dbm);
01442     tr_debug("Thread MLE REED Handler");
01443     switch (mle_msg->message_type) {
01444             case MLE_COMMAND_PARENT_REQUEST: {
01445                 uint8_t scanMask, mode;
01446                 uint16_t version;
01447                 mle_tlv_info_t challengeTlv;
01448                 tr_info("Recv MLE Parent Request");
01449 
01450                 if (cur->nwk_bootstrap_state != ER_BOOTSRAP_DONE && cur->nwk_bootstrap_state != ER_MLE_ATTACH_READY) {
01451                     // If own attach is ongoing, do not process parent request
01452                     return;
01453                 }
01454 
01455                 if (security_headers->KeyIdMode != MAC_KEY_ID_MODE_SRC4_IDX) {
01456                     tr_debug("Wrong key mode %u ", security_headers->KeyIdMode);
01457                     return;
01458                 }
01459 
01460                 if (!thread_router_bootstrap_routing_allowed(cur)) {
01461                     tr_debug("R bit is off in security policy; drop packet");
01462                     return;
01463                 }
01464 
01465                 if (thread_am_reed(cur)) {
01466                     // If we are in REED mode and receive PARENT_REQ from our parent, don't send response.
01467                     if (thread_router_parent_address_check(cur, mle_msg->packet_src_address)) {
01468                         tr_debug("Drop PARENT_REQ from own parent");
01469                         return;
01470                     }
01471                 }
01472 
01473                 // parent request received
01474                 entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, false, NULL);
01475                 if (entry_temp) {
01476                     entry_temp->mode = (MLE_FFD_DEV | MLE_RX_ON_IDLE | MLE_THREAD_REQ_FULL_DATA_SET);
01477                 }
01478                 if(!entry_temp) {
01479                     if (!mle_tlv_read_8_bit_tlv(MLE_TYPE_MODE, mle_msg->data_ptr, mle_msg->data_length, &mode)) {
01480                         tr_debug("NO Mode");
01481                         return;
01482                     }
01483 
01484                     // New child trying to attach check if we have room
01485                     if (mle_service_interface_tx_queue_size(cur->id) > THREAD_MAX_PARALLEL_MLE_PARENT_REQUEST) {
01486                         tr_info("Drop MLE message: too many simultaneous MLE messages");
01487                         return;
01488                     }
01489 
01490                     if (mle_class_free_entry_count_get(cur->id) < 1) {
01491                         tr_info("Drop MLE message: no space left in the MLE table");
01492                         return;
01493                     }
01494 
01495                     if ((mode & MLE_DEV_MASK) == MLE_RFD_DEV && mle_class_rfd_entry_count_get(cur->id) >= THREAD_MAX_MTD_CHILDREN) {
01496                         tr_info("Drop MLE message: maximum MTD child count reached.");
01497                         return;
01498                     }
01499 
01500                     if (!(mode & MLE_RX_ON_IDLE) && mle_class_sleepy_entry_count_get(cur->id) >= THREAD_MAX_SED_CHILDREN) {
01501                         tr_info("Drop MLE message: maximum SED child count reached.");
01502                         return;
01503                     }
01504 
01505                     if (cur->thread_info->thread_attached_state == THREAD_STATE_CONNECTED_ROUTER &&
01506                             thread_router_bootstrap_child_count_get(cur) >= cur->thread_info->maxChildCount) {
01507                         tr_info("Drop MLE message: maximum Child count reached (%d)", cur->thread_info->maxChildCount);
01508                         return;
01509                     }
01510                 }
01511 
01512                 if (thread_route_ready_to_leader(cur) != 0) {
01513                     tr_debug("Leader Path infinite");
01514                     return;
01515                 }
01516 
01517                 /**
01518                      * MLE attached First packet Mandatory TLV:s are validated at this function
01519                      * - MLE_TYPE_SCAN_MASK
01520                      * - MLE_TYPE_CHALLENGE
01521                      * - MLE_TYPE_VERSION
01522                      */
01523                 //Validate Mask
01524                 if (!mle_tlv_read_8_bit_tlv(MLE_TYPE_SCAN_MASK, mle_msg->data_ptr, mle_msg->data_length, &scanMask)) {
01525                     tr_debug("NO Scan mask");
01526                     return;
01527                 }
01528 
01529                 if (!mle_tlv_read_8_bit_tlv(MLE_TYPE_MODE, mle_msg->data_ptr, mle_msg->data_length, &mode)) {
01530                     tr_debug("NO Mode");
01531                     return;
01532                 }
01533 
01534                 if (!thread_scan_mask_validation(cur, scanMask)) {
01535                     tr_debug("Not my type");
01536                     return;
01537                 }
01538 
01539                 if(cur->thread_info->thread_attached_state == THREAD_STATE_CONNECTED &&
01540                    31 < thread_routing_count_active_routers(&cur->thread_info->routing)) {
01541                     tr_info("No possibility to upgrade to router");
01542                     return;
01543                 }
01544 
01545                 if (!mle_tlv_read_16_bit_tlv(MLE_TYPE_VERSION, mle_msg->data_ptr, mle_msg->data_length, &version)) {
01546                     tr_debug("NO version");
01547                     return;
01548                 }
01549 
01550                 //Read Challenge for Build Response
01551                 if (!mle_tlv_read_tlv(MLE_TYPE_CHALLENGE, mle_msg->data_ptr, mle_msg->data_length, &challengeTlv)) {
01552                     tr_debug("No challenge");
01553                     return;
01554                 }
01555 
01556                 if (thread_attach_parent_response_build(cur, mle_msg->packet_src_address, challengeTlv.dataPtr, challengeTlv.tlvLen, linkMargin, scanMask, mode) == 0) {
01557                     tr_debug("Send MLE Parent response");
01558                 }
01559                 break;
01560             }
01561 
01562             case MLE_COMMAND_CHILD_ID_REQUEST:
01563                 tr_info("Recv MLE Child ID Request from %s", trace_ipv6(mle_msg->packet_src_address));
01564                 if (cur->nwk_bootstrap_state != ER_BOOTSRAP_DONE && cur->nwk_bootstrap_state != ER_MLE_ATTACH_READY) {
01565                     // If own attach is ongoing, do not process child ID req
01566                     return;
01567                 }
01568 
01569                 if (!thread_router_bootstrap_routing_allowed(cur)) {
01570                     tr_debug("R bit is off in security policy; drop packet");
01571                     return;
01572                 }
01573 
01574                 // If we are in REED mode and receive child IR request from our parent, call connection error.
01575                 if (thread_am_reed(cur)) {
01576                     if (thread_router_parent_address_check(cur, mle_msg->packet_src_address)) {
01577                         tr_debug("Child ID req from own parent -> connection error");
01578                         thread_bootstrap_connection_error(cur->id, CON_PARENT_CONNECT_DOWN, NULL);
01579                         return;
01580                     }
01581                 }
01582 
01583                 if (security_headers->KeyIdMode != MAC_KEY_ID_MODE_SRC4_IDX) {
01584                     tr_debug("Wrong key mode %u ", security_headers->KeyIdMode);
01585                     return;
01586                 } else {
01587                     thread_pending_child_id_req_t *id_req;
01588                     uint8_t tlvType;
01589                     mle_tlv_info_t tlvRequest, addressRegisteredTlv;
01590                     uint8_t *t_ptr;
01591 
01592                     uint16_t messageId = mle_tlv_validate_response(mle_msg->data_ptr, mle_msg->data_length);
01593 
01594                     if (messageId == 0) {
01595                         tr_debug("No matching challenge");
01596                         return;
01597                     }
01598 
01599                     /* Perform key synchronization */
01600                     thread_management_key_synch_req(cur->id, common_read_32_bit(security_headers->Keysource));
01601 
01602                     //Free Response
01603                     mle_service_msg_free(messageId);
01604 
01605                     mle_msg->packet_src_address[8]  ^= 2;
01606                     id_req = thread_child_id_request_entry_get(cur, (mle_msg->packet_src_address + 8));
01607                     mle_msg->packet_src_address[8]  ^= 2;
01608 
01609                     if (!id_req) {
01610                         tr_debug("No room for child id req");
01611                         return;
01612                     }
01613 
01614                     /**
01615                          * MLE attached Tree packet Mandatory TLV:s are validated at this function
01616                          * - MLE_TYPE_TIMEOUT
01617                          * - MLE_TYPE_MODE
01618                          * - MLE_TYPE_LL_FRAME_COUNTER
01619                          * - MLE_TYPE_TLV_REQUEST
01620                          * - MLE_TYPE_VERSION
01621                          * Optional:
01622                          * - MLE_TYPE_ADDRESS_REGISTRATION
01623                          * - MLE_TYPE_MLE_FRAME_COUNTER
01624                          */
01625 
01626                     if ((!mle_tlv_read_32_bit_tlv(MLE_TYPE_TIMEOUT, mle_msg->data_ptr, mle_msg->data_length, &id_req->timeout)) ||
01627                             (!mle_tlv_read_8_bit_tlv(MLE_TYPE_MODE, mle_msg->data_ptr, mle_msg->data_length, &id_req->mode)) ||
01628                             (!mle_tlv_read_16_bit_tlv(MLE_TYPE_VERSION, mle_msg->data_ptr, mle_msg->data_length, &id_req->version)) ||
01629                             (!mle_tlv_read_32_bit_tlv(MLE_TYPE_LL_FRAME_COUNTER, mle_msg->data_ptr, mle_msg->data_length, &id_req->frameCounter)) ||
01630                             (!mle_tlv_read_tlv(MLE_TYPE_TLV_REQUEST, mle_msg->data_ptr, mle_msg->data_length, &tlvRequest))) {
01631                         thread_child_id_request_entry_remove(cur, id_req);
01632                         tr_debug("Illegal child id req");
01633                         return;
01634                     }
01635                     //If MLE MLE_TYPE_MLE_FRAME_COUNTER TLV is present then use it for validating further messages else use link layer frame counter
01636                     if (!mle_tlv_read_32_bit_tlv(MLE_TYPE_MLE_FRAME_COUNTER, mle_msg->data_ptr, mle_msg->data_length, &id_req->mleFrameCounter)) {
01637                         id_req->mleFrameCounter = id_req->frameCounter;
01638                     }
01639 
01640                     t_ptr = tlvRequest.dataPtr;
01641 
01642                     for (uint8_t i = 0; i < tlvRequest.tlvLen; i++) {
01643                         tlvType = *t_ptr++;
01644                         if (tlvType == MLE_TYPE_ADDRESS16) {
01645                             id_req->shortAddressReq = true;
01646                         } else if (tlvType == MLE_TYPE_NETWORK_DATA) {
01647                             id_req->networkDataReq = true;
01648                         } else if (tlvType == MLE_TYPE_ROUTE) {
01649                             id_req->routeReq = true;
01650                         }
01651                     }
01652                     if (mle_tlv_read_tlv(MLE_TYPE_ADDRESS_REGISTRATION, mle_msg->data_ptr, mle_msg->data_length, &addressRegisteredTlv)) {
01653                         uint8_t context;
01654                         t_ptr = addressRegisteredTlv.dataPtr;
01655                         context = *t_ptr++;
01656                         if (context & 0x80) {
01657                             context &= 0x0f;
01658                             if (addressRegisteredTlv.tlvLen == 9 && (context == 0)) {
01659                                 memcpy(id_req->eiid, t_ptr, 8);
01660                             }
01661                         } else {
01662                             t_ptr += 8;
01663                             memcpy(id_req->eiid, t_ptr, 8);
01664                         }
01665                     }
01666 
01667 
01668                     id_req->keyId = security_headers->KeyIndex;
01669                     id_req->keySeq = common_read_32_bit(security_headers->Keysource);
01670                     id_req->linkMargin = linkMargin;
01671 
01672                     id_req->active_timestamp = 0;
01673                     id_req->request_active_config = false;
01674                     if (!mle_tlv_read_64_bit_tlv(MLE_TYPE_ACTIVE_TIMESTAMP, mle_msg->data_ptr, mle_msg->data_length, &id_req->active_timestamp)) {
01675                         id_req->request_active_config = true;
01676                     }
01677                     if (!mle_tlv_read_64_bit_tlv(MLE_TYPE_PENDING_TIMESTAMP, mle_msg->data_ptr, mle_msg->data_length, &id_req->pending_timestamp)) {
01678                         id_req->pending_timestamp = 0;
01679                     }
01680 
01681                     //Push Event to queue
01682                     if (thread_bootstrap_child_id_request(cur) != 0) {
01683                         thread_child_id_request_entry_remove(cur, id_req);
01684                     }
01685 
01686                     break;
01687                 }
01688 
01689             case MLE_COMMAND_REQUEST:{
01690                 uint16_t shortAddress, version;
01691                 mle_tlv_info_t addressRegisteredTlv, requestTlv, challengeTlv;
01692                 uint8_t response_type = MLE_COMMAND_ACCEPT_AND_REQUEST;
01693                 //Router attachment
01694                 tr_info("Recv Router Link request");
01695 
01696                 if (!thread_attach_active_router(cur)) {
01697                     return; //Do respond until we are reed
01698                 }
01699 
01700                 if (!mle_tlv_read_tlv(MLE_TYPE_CHALLENGE, mle_msg->data_ptr, mle_msg->data_length, &challengeTlv)) {
01701                     return;
01702                 }
01703                 if (!mle_tlv_read_16_bit_tlv(MLE_TYPE_VERSION, mle_msg->data_ptr, mle_msg->data_length, &version)) {
01704                     return;
01705                 }
01706                 if (!mle_tlv_read_tlv(MLE_TYPE_TLV_REQUEST, mle_msg->data_ptr, mle_msg->data_length, &requestTlv)) {
01707                     /**
01708                      * This cuold cause problem if any dummy coder add this to message
01709                      * Validate is it REED or End device message link synch Request
01710                      * - MLE_TYPE_SRC_ADDRESS
01711                      * - MLE_TYPE_VERSION
01712                      * - MLE_TYPE_CHALLENGE
01713                      */
01714                     if (mle_tlv_read_16_bit_tlv(MLE_TYPE_SRC_ADDRESS, mle_msg->data_ptr, mle_msg->data_length, &shortAddress)) {
01715                         //Correct TLV's lets response
01716                         if (!thread_is_router_addr(shortAddress)) {
01717                             if (leaderDataReceived && thread_info(cur)->thread_leader_data->partitionId == leaderData.partitionId) {
01718                                 //REED or end device send response
01719                                 thread_router_accept_to_endevice(cur, mle_msg, challengeTlv.dataPtr, challengeTlv.tlvLen);
01720                             } else {
01721                                 tr_debug("Drop Link Request from REED/ED in a different partition.");
01722                                 // Do nothing... ignore.
01723                             }
01724                         }
01725                     }
01726                     return;
01727                 }
01728 
01729                 entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, false, NULL);
01730 
01731                 if (!mle_tlv_read_16_bit_tlv(MLE_TYPE_SRC_ADDRESS, mle_msg->data_ptr, mle_msg->data_length, &shortAddress)) {
01732 
01733                     /**
01734                      * MLE Router Synch mandatory options are
01735                      * - MLE_TYPE_VERSION
01736                      * - MLE_TYPE_CHALLENGE
01737                      * - MLE_TYPE_TLV_REQUEST
01738                      *
01739                      */
01740 
01741                     if (entry_temp && entry_temp->handshakeReady) {
01742                         mle_entry_timeout_refresh(entry_temp);
01743                         thread_router_synch_accept_request_build(cur, mle_msg, entry_temp->short_adr, challengeTlv.dataPtr, challengeTlv.tlvLen, requestTlv.dataPtr, requestTlv.tlvLen);
01744                     }
01745                 }
01746                 bool update_mac_mib = false;
01747                 /**
01748                  * MLE attached First packet Mandatory TLV:s are validated at this function
01749                  * - MLE_TYPE_SRC_ADDRESS
01750                  * - MLE_TYPE_CHALLENGE
01751                  * - MLE_TYPE_LEADER_DATA
01752                  * - MLE_TYPE_VERSION
01753                  * - MLE_TYPE_TLV_REQUEST
01754                  */
01755 
01756                 //Read Leader Data and verify connectivity
01757                 if (!leaderDataReceived) {
01758                     return;
01759                 }
01760 
01761                 //validate partition id
01762                 if (thread_info(cur)->thread_leader_data->partitionId != leaderData.partitionId) {
01763                     tr_debug("Drop link request from wrong partition");
01764                     return;
01765                 }
01766 
01767                 if (entry_temp) {
01768                     /*remove from child list when becoming router*/
01769                     // Was this previously our child? If yes, update.
01770                     if ((entry_temp->short_adr & THREAD_CHILD_MASK) && thread_router_addr_from_addr(entry_temp->short_adr) == cur->thread_info->routerShortAddress) {
01771                         thread_dynamic_storage_child_info_clear(cur->id, entry_temp);
01772                         protocol_6lowpan_release_short_link_address_from_neighcache(cur, entry_temp->short_adr);
01773                     }
01774                     update_mac_mib = true;
01775                     entry_temp->short_adr = shortAddress; // short address refreshed
01776 
01777                     if (entry_temp->handshakeReady) {
01778                         if (mle_tlv_read_tlv(MLE_TYPE_ADDRESS_REGISTRATION, mle_msg->data_ptr, mle_msg->data_length, &addressRegisteredTlv)) {
01779                             if (thread_rfd_device(entry_temp->mode)) {
01780                                 thread_address_registration_tlv_parse(addressRegisteredTlv.dataPtr, addressRegisteredTlv.tlvLen, cur, entry_temp->short_adr, entry_temp->mac64);
01781                             }
01782                         }
01783 
01784                         mle_entry_timeout_refresh(entry_temp);
01785                         if (!mle_neigh_entry_frame_counter_update(entry_temp, mle_msg->data_ptr, mle_msg->data_length, cur, security_headers->KeyIndex) &&  update_mac_mib) {
01786                             //GET
01787                             mlme_get_t get_req;
01788                             get_req.attr = macDeviceTable;
01789                             get_req.attr_index = entry_temp->attribute_index;
01790                             cur->mac_api->mlme_req(cur->mac_api, MLME_GET, &get_req);
01791                         }
01792                         response_type = MLE_COMMAND_ACCEPT;
01793                     }
01794                     response_type = MLE_COMMAND_ACCEPT;
01795                 }
01796 
01797 
01798                 bool accept_link = false;
01799                 if (entry_temp) {
01800                     // We know already so we can accept
01801                     accept_link = true;
01802                 } else if (addr_is_ipv6_multicast(mle_msg->packet_dst_address)) {
01803                     if (thread_bootstrap_link_create_check(cur, shortAddress) &&
01804                         thread_bootstrap_link_create_allowed(cur, shortAddress, mle_msg->packet_src_address)) {
01805                         // Multicast and we can respond to message
01806                         accept_link = true;
01807                     }
01808                 } else if ( thread_bootstrap_link_create_check(cur, shortAddress) ) {
01809                     // Unicast request and we should make link
01810                     accept_link = true;
01811                 }
01812 
01813                 if (accept_link) {
01814                     if (!thread_is_router_addr(shortAddress)) {
01815                         response_type = MLE_COMMAND_ACCEPT;
01816                     }
01817                     thread_router_accept_request_build(cur, mle_msg, shortAddress, challengeTlv.dataPtr, challengeTlv.tlvLen, response_type, rssiTLV, linkMargin);
01818                 }
01819 
01820                 break;
01821             }
01822 
01823             case MLE_COMMAND_ACCEPT_AND_REQUEST: {
01824                 uint8_t linkMarginfronNeigh;
01825                 uint16_t version, shortAddress;
01826                 uint32_t llFrameCounter;
01827                 mle_tlv_info_t requestTlv, challengeTlv;
01828                 bool createNew, new_entry;
01829                 tr_info("Recv Router Accept & Request");
01830                 uint16_t messageId = mle_tlv_validate_response(mle_msg->data_ptr, mle_msg->data_length);
01831 
01832                 if (messageId == 0) {
01833                     tr_debug("No matching challenge");
01834                     return;
01835                 }
01836 
01837                 if (!addr_is_ipv6_multicast(mle_service_get_msg_destination_address_pointer(messageId))) {
01838                     //Free Response only if it is unicast
01839                     mle_service_msg_free(messageId);
01840                 }
01841 
01842                 if ((!mle_tlv_read_tlv(MLE_TYPE_CHALLENGE, mle_msg->data_ptr, mle_msg->data_length, &challengeTlv)) ||
01843                         (!mle_tlv_read_16_bit_tlv(MLE_TYPE_VERSION, mle_msg->data_ptr, mle_msg->data_length, &version)) ||
01844                         (!mle_tlv_read_tlv(MLE_TYPE_TLV_REQUEST, mle_msg->data_ptr, mle_msg->data_length, &requestTlv)) ||
01845                         (!mle_tlv_read_8_bit_tlv(MLE_TYPE_RSSI, mle_msg->data_ptr, mle_msg->data_length, &linkMarginfronNeigh)) ||
01846                         (!mle_tlv_read_32_bit_tlv(MLE_TYPE_LL_FRAME_COUNTER, mle_msg->data_ptr, mle_msg->data_length, &llFrameCounter)) ||
01847                         (!mle_tlv_read_16_bit_tlv(MLE_TYPE_SRC_ADDRESS, mle_msg->data_ptr, mle_msg->data_length, &shortAddress))) {
01848                     thread_link_reject_send(cur, mle_msg->packet_src_address);
01849                     return;
01850                 }
01851 
01852                 /* Call to determine whether or not we should create a new link */
01853                 createNew = thread_bootstrap_link_create_check(cur, shortAddress);
01854 
01855                 //Send Response
01856                 entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, createNew, &new_entry);
01857                 if (entry_temp) {
01858 
01859                     if (security_headers->KeyIdMode == MAC_KEY_ID_MODE_SRC4_IDX) {
01860                         thread_management_key_synch_req(cur->id, common_read_32_bit(security_headers->Keysource));
01861                     }
01862 
01863                     entry_temp->threadNeighbor = true;
01864                     entry_temp->short_adr = shortAddress;
01865                     mac_helper_devicetable_set(entry_temp, cur, llFrameCounter, security_headers->KeyIndex, new_entry);
01866                     if (entry_temp->timeout_rx) {
01867                         mle_entry_timeout_refresh(entry_temp);
01868                     } else {
01869                         mle_entry_timeout_update(entry_temp, THREAD_DEFAULT_LINK_LIFETIME);
01870                     }
01871 
01872                     if (thread_is_router_addr(shortAddress)) {
01873                         entry_temp->handshakeReady = 1;
01874                     }
01875 
01876                     entry_temp->mode |= MLE_THREAD_REQ_FULL_DATA_SET;
01877 
01878                     thread_routing_update_link_margin(cur, entry_temp->short_adr, linkMargin, linkMarginfronNeigh);
01879                     //Read Source address and Challenge
01880                     mac_data_poll_protocol_poll_mode_decrement(cur);
01881                     thread_router_accept_request_build(cur, mle_msg, entry_temp->short_adr, challengeTlv.dataPtr, challengeTlv.tlvLen, MLE_COMMAND_ACCEPT, rssiTLV, linkMargin);
01882 
01883                     blacklist_update(mle_msg->packet_src_address, true);
01884                 } else {
01885                     thread_link_reject_send(cur, mle_msg->packet_src_address);
01886                 }
01887 
01888                 break;
01889             }
01890 
01891             case MLE_COMMAND_CHILD_UPDATE_REQUEST:
01892                 tr_info("Recv Router Child Update request");
01893                 {
01894                     uint8_t mode;
01895                     uint8_t status;
01896                     uint32_t timeout = 0;
01897                     uint64_t active_timestamp = 0;
01898                     uint64_t pending_timestamp = 0;
01899                     mle_tlv_info_t addressRegisterTlv = {0};
01900                     mle_tlv_info_t challengeTlv = {0};
01901                     mle_tlv_info_t tlv_req = {0};
01902                     entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, false, NULL);
01903 
01904                     if (mle_tlv_read_8_bit_tlv(MLE_TYPE_STATUS, mle_msg->data_ptr, mle_msg->data_length, &status)) {
01905                         if (1 == status && thread_check_is_this_my_parent(cur, entry_temp)) {
01906                             tr_debug("parent has removed REED");
01907                             thread_bootstrap_connection_error(cur->id, CON_PARENT_CONNECT_DOWN, NULL);
01908                         }
01909                         return;
01910                     }
01911 
01912                     mle_tlv_read_tlv(MLE_TYPE_CHALLENGE, mle_msg->data_ptr, mle_msg->data_length, &challengeTlv);
01913 
01914                     if (!entry_temp) {
01915                         tr_debug("Not Neighbor anymore");
01916                         thread_host_bootstrap_child_update_negative_response(cur, mle_msg->packet_src_address, &challengeTlv);
01917                         return;
01918                     }
01919 
01920                     if (!mle_tlv_read_8_bit_tlv(MLE_TYPE_MODE, mle_msg->data_ptr, mle_msg->data_length, &mode)) {
01921                         tr_debug("No Mode");
01922                         return;
01923                     }
01924 
01925                     //Keep alive updated
01926                     entry_temp->ttl = entry_temp->timeout_rx;
01927                     entry_temp->last_contact_time = protocol_core_monotonic_time;
01928                     entry_temp->mode = mode;
01929 
01930                     addressRegisterTlv.tlvType = MLE_TYPE_UNASSIGNED;
01931                     mle_tlv_read_tlv(MLE_TYPE_ADDRESS_REGISTRATION, mle_msg->data_ptr, mle_msg->data_length, &addressRegisterTlv);
01932                     mle_tlv_read_64_bit_tlv(MLE_TYPE_ACTIVE_TIMESTAMP, mle_msg->data_ptr, mle_msg->data_length, &active_timestamp);
01933                     mle_tlv_read_64_bit_tlv(MLE_TYPE_PENDING_TIMESTAMP, mle_msg->data_ptr, mle_msg->data_length, &pending_timestamp);
01934 
01935                     mle_tlv_read_tlv(MLE_TYPE_TLV_REQUEST, mle_msg->data_ptr, mle_msg->data_length, &tlv_req);
01936 
01937                     if (addressRegisterTlv.tlvType == MLE_TYPE_ADDRESS_REGISTRATION &&
01938                         thread_rfd_device(entry_temp->mode)) {
01939 
01940                         tr_debug("Register child address");
01941                         thread_address_registration_tlv_parse(addressRegisterTlv.dataPtr, addressRegisterTlv.tlvLen, cur, entry_temp->short_adr, entry_temp->mac64);
01942                     }
01943 
01944                     if (mle_tlv_read_32_bit_tlv(MLE_TYPE_TIMEOUT, mle_msg->data_ptr, mle_msg->data_length, &timeout)) {
01945 
01946                         tr_debug("Setting child timeout, value=%"PRIu32, timeout);
01947                         entry_temp->holdTime = 90;
01948                         mle_entry_timeout_update(entry_temp, timeout);
01949                     }
01950                     if (!leaderDataReceived) {
01951                         tr_debug("Child synch req");
01952                     }
01953 
01954                     tr_debug("Keep-Alive -->Respond Child");
01955                     //Response
01956                     thread_child_update_response(cur, mle_msg->packet_src_address, mode, entry_temp->short_adr, timeout, &addressRegisterTlv, &tlv_req, &challengeTlv, active_timestamp, pending_timestamp);
01957 
01958                 }
01959                 break;
01960 
01961             case MLE_COMMAND_UPDATE:
01962                 tr_info("Recv Router Update");
01963                 break;
01964 
01965             case MLE_COMMAND_UPDATE_REQUEST:
01966                 tr_info("Recv Router update Request");
01967                 break;
01968 
01969             case MLE_COMMAND_DATA_REQUEST: {
01970                 mle_tlv_info_t requestTlv;
01971                 tr_info("Recv Router Data Request");
01972                 entry_temp = mle_class_get_entry_by_ll64(cur->id, linkMargin, mle_msg->packet_src_address, false, NULL);
01973                 if (!entry_temp || !mle_tlv_read_tlv(MLE_TYPE_TLV_REQUEST, mle_msg->data_ptr, mle_msg->data_length, &requestTlv)) {
01974                     return;
01975                 }
01976                 mle_build_and_send_data_response_msg(cur, mle_msg->packet_src_address, mle_msg->data_ptr, mle_msg->data_length, &requestTlv, entry_temp->mode);
01977             }
01978             break;
01979             default:
01980                 tr_warn("Recv Router UNKNOWN message %d", mle_msg->message_type);
01981                 break;
01982 
01983         }
01984 }
01985 
01986 static int8_t thread_router_bootstrap_synch_request_send(protocol_interface_info_entry_t *cur)
01987 {
01988     uint8_t req_tlv[3];
01989     mle_message_timeout_params_t timeout;
01990     uint32_t keySequence;
01991     uint16_t buf_id = mle_service_msg_allocate(cur->id, 32, true, MLE_COMMAND_REQUEST);
01992     if (buf_id == 0) {
01993         return -1;
01994     }
01995     thread_management_get_current_keysequence(cur->id, &keySequence);
01996     mle_service_msg_update_security_params(buf_id, 5, 2, keySequence);
01997     mle_service_set_msg_destination_address(buf_id, ADDR_LINK_LOCAL_ALL_ROUTERS);
01998     req_tlv[0] = MLE_TYPE_ADDRESS16;
01999     req_tlv[1] = MLE_TYPE_ROUTE;
02000     uint8_t *ptr = mle_service_get_data_pointer(buf_id);
02001     ptr = mle_tlv_write_version(ptr, cur->thread_info->version);
02002     ptr = mle_tlv_req_tlv(ptr, req_tlv, 2);
02003     if (mle_service_update_length_by_ptr(buf_id,ptr)!= 0) {
02004         tr_debug("Buffer overflow at message write");
02005     }
02006 
02007     timeout.retrans_max = THREAD_REQUEST_MAX_RETRY_CNT;
02008     timeout.timeout_init = 1;
02009     timeout.timeout_max = 3;
02010     timeout.delay = MLE_NO_DELAY;
02011 
02012     mle_service_set_packet_callback(buf_id, thread_device_synch_timeout);
02013     mle_service_set_msg_timeout_parameters(buf_id, &timeout);
02014     mle_service_send_message(buf_id);
02015     return 0;
02016 }
02017 
02018 static int8_t thread_router_synch_new_router(protocol_interface_info_entry_t *cur, const uint8_t *destAddress)
02019 {
02020     mle_message_timeout_params_t timeout;
02021     uint32_t keySequence;
02022     if (thread_info(cur)) {
02023         if (!thread_info(cur)->thread_leader_data) {
02024             return -1;
02025         }
02026     }
02027 
02028     uint16_t bufId = mle_service_msg_allocate(cur->id, 64, true, MLE_COMMAND_REQUEST);
02029 
02030     if (bufId == 0) {
02031         return -1;
02032     }
02033     uint8_t req_tlv = MLE_TYPE_RSSI;
02034 
02035     thread_management_get_current_keysequence(cur->id, &keySequence);
02036     mle_service_msg_update_security_params(bufId, 5, 2, keySequence);
02037 
02038     mle_service_set_msg_destination_address(bufId, destAddress);
02039 
02040 
02041     uint8_t *ptr = mle_service_get_data_pointer(bufId);
02042     ptr = mle_tlv_write_version(ptr, cur->thread_info->version);
02043     ptr = thread_leader_data_tlv_write(ptr, cur);
02044     ptr = mle_general_write_source_address(ptr, cur);
02045     ptr = mle_tlv_req_tlv(ptr, &req_tlv, 1);
02046 
02047     if (!addr_is_ipv6_multicast(destAddress)) {
02048         timeout.retrans_max = 2;
02049         timeout.timeout_init = 2;
02050         timeout.timeout_max = 3;
02051         timeout.delay = MLE_STANDARD_RESPONSE_DELAY;
02052     } else {
02053         // Multicasts need transaction timer
02054         timeout.retrans_max = 1;
02055         timeout.timeout_init = 2;
02056         timeout.timeout_max = 3;
02057         timeout.delay = 0;
02058     }
02059     mle_service_set_packet_callback(bufId, thread_link_request_timeout);
02060     mle_service_set_msg_timeout_parameters(bufId, &timeout);
02061 
02062     if (mle_service_update_length_by_ptr(bufId,ptr)!= 0) {
02063         tr_debug("Buffer overflow at message write");
02064     }
02065 
02066 
02067     mle_service_send_message(bufId);
02068     return 0;
02069 }
02070 
02071 int thread_router_bootstrap_link_synch_start(protocol_interface_info_entry_t *cur)
02072 {
02073     //Send Link Request for
02074     if (thread_router_bootstrap_synch_request_send(cur) == 0) {
02075         //SET Router synch receiver handler
02076         mle_service_interface_receiver_handler_update(cur->id, thread_router_synch_receive_cb);
02077         cur->nwk_bootstrap_state = ER_MLE_SYNCH;
02078         return 0;
02079     }
02080     return -1;
02081 }
02082 
02083 bool thread_router_bootstrap_router_downgrade(protocol_interface_info_entry_t *cur)
02084 {
02085     uint8_t activeRouterCount;
02086     uint8_t goodLinks;
02087 
02088     if (cur->thread_info->thread_attached_state != THREAD_STATE_CONNECTED_ROUTER) {
02089         return false;
02090     }
02091 
02092     // if we are commissioner border router we have registered our address and cant upgrade or downgrade
02093     if (cur->thread_info->registered_commissioner.commissioner_valid &&
02094             addr_get_entry(cur,cur->thread_info->registered_commissioner.border_router_address) ) {
02095         return false;
02096     }
02097 
02098     // spec: Not be the Leader
02099     if (cur->thread_info->leader_private_data) {
02100         if (!thread_router_bootstrap_routing_allowed(cur)) {
02101             tr_debug("Cannot be leader anymore");
02102             // Settings have changed, cannot be leader anymore
02103             thread_bootstrap_reset_restart(cur->id);
02104         }
02105         // Leader can not downgrade
02106         return false;
02107     }
02108 
02109     if (!thread_router_bootstrap_routing_allowed(cur)) {
02110         return true;
02111     }
02112 
02113     //spec: If the number of active Routers on the network exceeds ROUTER_DOWNGRADE_THRESHOLD
02114     activeRouterCount = thread_routing_count_active_routers(&cur->thread_info->routing);
02115     if (activeRouterCount <= cur->thread_info->routerSelectParameters.routerDowngradeThresHold) {
02116         return false;
02117     }
02118 
02119     uint_fast8_t asGood;
02120     goodLinks = thread_routing_count_neighbours_for_downgrade(&cur->thread_info->routing, &asGood);
02121 
02122     //spec: Have at least MIN_DOWNGRADE_NEIGHBORS neighbors in set M.
02123     if (goodLinks < MIN_DOWNGRADE_NEIGHBORS) {
02124         return false;
02125     }
02126 
02127     //spec: Have at least one neighbor that has as good or better quality links to all Routers in M.
02128     if (asGood == 0) {
02129         return false;
02130     }
02131 
02132     //spec: Not be the sole Border Router for a particular Provisioning Domain
02133     if (!ns_list_is_empty(&cur->thread_info->localServerDataBase.prefix_list)) {
02134         return false;
02135     }
02136 
02137     uint16_t child_count = thread_router_bootstrap_child_count_get(cur);
02138     //Cant downgrade if child count is larger than 3 times exceed routers
02139     if (child_count > (3 * (activeRouterCount - cur->thread_info->routerSelectParameters.routerDowngradeThresHold))) {
02140         return false;
02141     }
02142     tr_info("Reed downgrade:ChildCount %u Active Routers %u good links %u downgrade threshold %u", child_count, activeRouterCount, goodLinks, cur->thread_info->routerSelectParameters.routerDowngradeThresHold);
02143 
02144     return true;
02145 }
02146 
02147 bool thread_router_bootstrap_reed_upgrade(protocol_interface_info_entry_t *cur)
02148 {
02149     uint8_t activeRouterCount;
02150 
02151     if (!thread_router_bootstrap_routing_allowed(cur)) {
02152         return false;
02153     }
02154 
02155     if (thread_am_router(cur)) {
02156         return false;
02157     }
02158 
02159     // if we are commissioner border router we have registered our address and cant upgrade or downgrade
02160     if (cur->thread_info->registered_commissioner.commissioner_valid &&
02161             addr_get_entry(cur,cur->thread_info->registered_commissioner.border_router_address) ) {
02162         return false;
02163     }
02164 
02165     if (!cur->thread_info->routing.router_id_sequence_valid) {
02166         // In case routing information is not up-to-date yet...
02167         return false;
02168     }
02169 
02170     activeRouterCount = thread_routing_count_active_routers(&cur->thread_info->routing);
02171 
02172     if (activeRouterCount >= cur->thread_info->routerSelectParameters.routerUpgradeThresHold) {
02173         return false;
02174     }
02175     tr_info("Reed Upgrade:Active Routers %u upgrade threshold %u", activeRouterCount, cur->thread_info->routerSelectParameters.routerUpgradeThresHold);
02176 
02177     return true;
02178 }
02179 
02180 void thread_router_bootstrap_child_id_reject(protocol_interface_info_entry_t *cur)
02181 {
02182     thread_pending_child_id_req_t *req;
02183     req = thread_child_id_request_entry_get_from_the_list(cur);
02184     while (req) {
02185         tr_debug("Remove entry from list");
02186         //Remove entry from list
02187         mle_class_remove_neighbour(cur->id, req->euid64, ADDR_802_15_4_LONG );
02188 
02189         ns_dyn_mem_free(req);
02190         req = thread_child_id_request_entry_get_from_the_list(cur);
02191     }
02192 }
02193 
02194 void thread_router_bootstrap_active_router_attach(protocol_interface_info_entry_t *cur)
02195 {
02196     uint8_t *parent_mac_addr = NULL;
02197     arm_nwk_6lowpan_thread_test_print_routing_database(cur->id);
02198     uint16_t address16 = mac_helper_mac16_address_get(cur);
02199     cur->thread_info->thread_attached_state = THREAD_STATE_CONNECTED_ROUTER;
02200 
02201     if (cur->thread_info->thread_leader_data->leaderRouterId == address16 &&
02202         !cur->thread_info->leader_private_data ) {
02203         // Error we are set up as leader but no private data
02204         tr_error("Leader setup error");
02205     }
02206     cur->lowpan_info |= INTERFACE_NWK_ROUTER_DEVICE;
02207     thread_routing_activate(&cur->thread_info->routing);
02208     thread_router_synch_new_router(cur, ADDR_LINK_LOCAL_ALL_ROUTERS);
02209     mle_class_mode_set(cur->id, MLE_CLASS_ROUTER);
02210     thread_bootstrap_ready(cur);
02211     thread_bootstrap_network_prefixes_process(cur);
02212     thread_nd_service_activate(cur->id);
02213     thread_router_bootstrap_mle_advertise(cur);
02214     if (cur->thread_info->thread_endnode_parent) {
02215         parent_mac_addr = cur->thread_info->thread_endnode_parent->mac64;
02216     }
02217     thread_nvm_store_link_info_write(parent_mac_addr, mac_helper_mac16_address_get(cur));
02218 }
02219 
02220 static int thread_validate_own_routeid_from_new_mask(const uint8_t *master_router_id_mask, uint8_t router_id)
02221 {
02222     int ret_val = -1;
02223     if (bit_test(master_router_id_mask, router_id)) {
02224         ret_val = 0;
02225     }
02226     return ret_val;
02227 }
02228 
02229 int thread_router_bootstrap_route_tlv_push(protocol_interface_info_entry_t *cur, uint8_t *route_tlv, uint8_t route_len, uint8_t linkMargin, mle_neigh_table_entry_t *entry)
02230 {
02231     (void) route_len;
02232 
02233     uint8_t route_id_seq;
02234     const uint8_t *router_id_mask, *route_data;
02235 
02236     route_id_seq = *route_tlv++;
02237     router_id_mask = route_tlv;
02238     route_tlv += 8;
02239     route_data = route_tlv;
02240     uint16_t mac16 = mac_helper_mac16_address_get(cur);
02241 
02242     if (!thread_is_router_addr(entry->short_adr)) {
02243         // Received route tlv from non router ignore
02244         tr_info("drop route Processing from end device %x", entry->short_adr);
02245         return 0;
02246     }
02247 
02248     if (thread_i_am_router(cur)) {
02249         thread_routing_info_t *routing = &cur->thread_info->routing;
02250         if (routing->router_id_sequence_valid && common_serial_number_greater_8(route_id_seq, routing->router_id_sequence)) {
02251             thread_routing_leader_connection_validate(cur->thread_info,routing->networkFragmentationTimer);
02252             routing->networkFragmentationTimer = 0;
02253             if (thread_validate_own_routeid_from_new_mask(router_id_mask, thread_router_id_from_addr(mac16)) != 0) {
02254 
02255                 tr_debug("RouterID not valid any More");
02256                 thread_bootstrap_connection_error(cur->id, CON_ERROR_NETWORK_KICK, NULL);
02257                 return 0;
02258             }
02259         }
02260     } else if (!thread_info(cur)->thread_endnode_parent ||
02261                thread_info(cur)->thread_endnode_parent->shortAddress != entry->short_adr ) {
02262         return 0;
02263     }
02264 
02265     /* XXX Is short_src_adr ever reset? Is it undefined if info not in msg? */
02266     /* Don't add routing link if MLE link is NOT bi-directional (i.e. we can only hear) */
02267     if (entry->handshakeReady) {
02268         thread_routing_add_link(cur, entry->short_adr, linkMargin, route_id_seq, router_id_mask, route_data, false);
02269     }
02270 
02271     return 0;
02272 }
02273 
02274 
02275 static void thread_bootstrap_client_router_id_release_cb(int8_t interface_id, int8_t status, uint16_t router_rloc, const uint8_t router_mask_ptr[9])
02276 {
02277     protocol_interface_info_entry_t *cur;
02278     (void) status;
02279     (void)router_mask_ptr;
02280     (void)router_rloc;
02281     cur = protocol_stack_interface_info_get_by_id(interface_id);
02282     tr_debug("RouterID ReleaseCB");
02283     if (!cur) {
02284         return;
02285     }
02286 
02287     if (!cur->thread_info->releaseRouterId) {
02288         return;
02289     }
02290     cur->thread_info->releaseRouterId = false;
02291 
02292     thread_bootstrap_router_id_release_ready(cur);
02293     return;
02294 
02295 }
02296 static uint32_t thread_reed_timeout_calculate(thread_router_select_t *routerSelect)
02297 {
02298     return randLIB_get_random_in_range(routerSelect->reedAdvertisementInterval,(routerSelect->reedAdvertisementInterval)+(routerSelect->reedAdvertisementJitterInterval));
02299 }
02300 
02301 
02302 static int thread_reed_advertise (protocol_interface_info_entry_t *cur)
02303 {
02304     uint32_t keySequence;
02305     tr_debug("MLE REED ADVERTISEMENT STARTED");
02306     struct link_configuration *linkConfiguration;
02307     linkConfiguration = thread_joiner_application_get_config(cur->id);
02308     if (!linkConfiguration) {
02309         return -1;
02310     }
02311 
02312     if (!thread_info(cur)) {
02313         return -1;
02314     }
02315 
02316     uint16_t bufId = mle_service_msg_allocate(cur->id, 16, false, MLE_COMMAND_ADVERTISEMENT);
02317     if (bufId == 0) {
02318         return -1;
02319     }
02320 
02321     thread_management_get_current_keysequence(cur->id, &keySequence);
02322     mle_service_msg_update_security_params(bufId, 5, 2, keySequence);
02323 
02324     uint8_t *ptr = mle_service_get_data_pointer(bufId);
02325 
02326     /*Add Leader Data & Source Address TLVs */
02327     ptr = thread_leader_data_tlv_write(ptr, cur);
02328     ptr = mle_general_write_source_address(ptr, cur);
02329     if (mle_service_update_length_by_ptr(bufId,ptr)!= 0) {
02330         tr_debug("Buffer overflow at message write");
02331     }
02332 
02333     mle_service_set_msg_destination_address(bufId, ADDR_LINK_LOCAL_ALL_NODES);
02334     mle_service_send_message(bufId);
02335     return 0;
02336 }
02337 
02338 static void thread_reed_advertisements_cb(void* arg)
02339 {
02340     if(!arg)
02341         return;
02342     protocol_interface_info_entry_t *cur = arg;
02343 
02344     cur->thread_info->routerSelectParameters.reedAdvertisementTimeout = NULL;
02345     if (cur->thread_info->thread_attached_state == THREAD_STATE_CONNECTED &&
02346             cur->thread_info->thread_device_mode == THREAD_DEVICE_MODE_ROUTER){
02347         thread_reed_advertise(cur);
02348         thread_router_bootstrap_child_information_clear(cur);
02349         cur->thread_info->routerSelectParameters.reedAdvertisementTimeout = eventOS_timeout_ms(thread_reed_advertisements_cb, thread_reed_timeout_calculate(&cur->thread_info->routerSelectParameters) * 1000, cur);
02350     }
02351 }
02352 
02353 void thread_router_bootstrap_reed_advertisements_start(protocol_interface_info_entry_t *cur)
02354 {
02355     uint32_t timeout = THREAD_REED_ADVERTISEMENT_DELAY;
02356     if(!cur)
02357         return;
02358     eventOS_timeout_cancel(cur->thread_info->routerSelectParameters.reedAdvertisementTimeout);
02359     cur->thread_info->routerSelectParameters.reedAdvertisementTimeout = NULL;
02360 
02361     cur->thread_info->reedJitterTimer = thread_router_bootstrap_random_upgrade_jitter();
02362 
02363     if (!thread_is_connected(cur)){
02364         return;
02365     }
02366     if (cur->thread_info->releaseRouterId  ||
02367             thread_router_bootstrap_child_count_get(cur) == 0) {
02368         // If we dont have any children or are are downgrading send REED advertisement immediately
02369         timeout = 1;
02370     }
02371     cur->thread_info->routerSelectParameters.reedAdvertisementTimeout = eventOS_timeout_ms(thread_reed_advertisements_cb,timeout, cur);
02372 }
02373 
02374 void thread_router_bootstrap_reed_merge_advertisement(protocol_interface_info_entry_t *cur)
02375 {
02376     if (cur->thread_info->reedMergeAdvTimer > 1) {
02377         return;
02378     }
02379     thread_reed_advertise(cur);
02380     // 120s second timer reinitialised before next merge advertisement
02381     cur->thread_info->reedMergeAdvTimer = THREAD_REED_MERGE_ADVERTISEMENT_INTERVAL;
02382 
02383 }
02384 void thread_router_bootstrap_router_id_release(protocol_interface_info_entry_t *cur)
02385 {
02386     tr_debug("Router ID Release");
02387     if (thread_management_client_router_id_release(cur->id, cur->mac, cur->thread_info->routerShortAddress, thread_bootstrap_client_router_id_release_cb) == 0) {
02388         // Release message sent succesfully
02389         cur->thread_info->routerShortAddress = 0xfffe;
02390         cur->thread_info->releaseRouterId = true;
02391     }
02392 }
02393 
02394 uint32_t thread_router_bootstrap_random_upgrade_jitter()
02395 {
02396    if (thread_router_selection_jitter <= 2) {
02397        return 1;
02398    }
02399     return randLIB_get_random_in_range(2,thread_router_selection_jitter);
02400 }
02401 
02402 void thread_router_bootstrap_timer(protocol_interface_info_entry_t *cur, uint32_t ticks)
02403 {
02404     thread_info_t *thread_info = cur->thread_info;
02405 
02406     if (thread_am_host(cur)) {
02407         return;
02408     }
02409 
02410     //Do we have a childs and we are REED state
02411     if (cur->thread_info->thread_attached_state == THREAD_STATE_CONNECTED && thread_router_bootstrap_child_count_get(cur)) {
02412         tr_debug("Trig quick router ID request");
02413         thread_bootstrap_attched_upgrade_reed(cur);
02414         return;
02415     }
02416 
02417     if (thread_info->reedJitterTimer > ticks) {
02418         // Reed status is checked every random jitter values
02419         thread_info->reedJitterTimer -= ticks;
02420     } else {
02421         thread_info->reedJitterTimer = thread_router_bootstrap_random_upgrade_jitter();
02422         if (thread_router_bootstrap_reed_upgrade(cur)) {
02423             //Check UpGrade possibility
02424             tr_debug("REED Upgrade to router");
02425             thread_bootstrap_attched_upgrade_reed(cur);
02426         }
02427         if (thread_router_bootstrap_router_downgrade(cur)) {
02428             tr_debug("Router downgrade to REED");
02429             thread_bootstrap_attached_downgrade_router(cur);
02430         }
02431     }
02432 
02433     if (cur->thread_info->reedMergeAdvTimer > ticks) {
02434         cur->thread_info->reedMergeAdvTimer -= ticks;
02435     } else {
02436         cur->thread_info->reedMergeAdvTimer = 0;
02437     }
02438 
02439     if (!thread_info->leader_private_data && thread_info->thread_attached_state == THREAD_STATE_CONNECTED_ROUTER) {
02440         // Non leader router checks
02441         if (thread_info->routing.activated) {
02442             if (thread_info->routing.networkFragmentationTimer++ >= thread_info->routing.networkIdTimeout) {
02443                 tr_debug("Leader Connection Lost");
02444                 thread_bootstrap_connection_error(cur->id, CON_ERROR_NETWORK_REATTACH, NULL);
02445             }
02446         } else {
02447             tr_debug("Routing Disabled?");
02448         }
02449     }
02450 
02451     if (thread_info->proactive_an_timer) {
02452         if (thread_info->proactive_an_timer > ticks) {
02453             thread_info->proactive_an_timer -= ticks;
02454         } else {
02455             thread_send_proactive_an(cur);
02456             thread_info->proactive_an_timer = 0;
02457         }
02458     }
02459 
02460     thread_leader_service_timer(cur,ticks);
02461     return;
02462 }
02463 
02464 void thread_router_bootstrap_advertiment_analyze(protocol_interface_info_entry_t *cur, uint8_t *src_address, mle_neigh_table_entry_t *entry_temp, uint16_t shortAddress)
02465 {
02466     if (entry_temp) {
02467         entry_temp->threadNeighbor = true;
02468 
02469         if (entry_temp->timeout_rx == 0 || thread_is_router_addr(shortAddress)) {
02470             entry_temp->timeout_rx = THREAD_DEFAULT_LINK_LIFETIME / MLE_TIMER_TICKS_SECONDS;
02471             entry_temp->timeout_rx++;
02472         }
02473 
02474         if (thread_is_router_addr(shortAddress)) {
02475             //Update MAC Security PIB table by get & set Operation
02476             mlme_get_t get_req;
02477             get_req.attr = macDeviceTable;
02478             get_req.attr_index = entry_temp->attribute_index;
02479             cur->mac_api->mlme_req(cur->mac_api, MLME_GET, &get_req);
02480             entry_temp->ttl = entry_temp->timeout_rx;
02481         }
02482     } else {
02483         //
02484         if (!thread_attach_active_router(cur)) {
02485             // We are not router
02486             return;
02487         }
02488         if (!thread_is_router_addr(shortAddress)) {
02489             // Reed advertisement
02490             return;
02491         }
02492         if (thread_bootstrap_link_create_check(cur, shortAddress) &&
02493                 thread_bootstrap_link_create_allowed(cur, shortAddress, src_address)) {
02494             //Challenge new router neighbor
02495             tr_debug("Synch new neighbor");
02496             thread_router_synch_new_router(cur, src_address);
02497         }
02498     }
02499 }
02500 static void thread_router_bootstrap_dhcp_server_any_cast_address_update(protocol_interface_info_entry_t *cur, uint16_t anycastAddress)
02501 {
02502     uint8_t ipv6_address[16];
02503 
02504     thread_addr_write_mesh_local_16(ipv6_address, anycastAddress, cur->thread_info);
02505     tr_debug("generate DHCP anycast address %s", trace_ipv6(ipv6_address));
02506     addr_add(cur, ipv6_address, 124, ADDR_SOURCE_THREAD_ALOC, 0xffffffff, 0, true);
02507 }
02508 
02509 static void thread_bootstrap_dhcp_anycast_address_generate(protocol_interface_info_entry_t *cur)
02510 {
02511     uint8_t ipv6_address[16];
02512     thread_addr_write_mesh_local_16(ipv6_address, 0xfc00, cur->thread_info);
02513     addr_delete_matching(cur,ipv6_address,124,ADDR_SOURCE_THREAD_ALOC);
02514 
02515     ns_list_foreach(thread_network_data_prefix_cache_entry_t, curPrefix, &cur->thread_info->networkDataStorage.localPrefixList) {
02516         // Go through all prefixes
02517         ns_list_foreach(thread_network_server_data_entry_t, curBorderRouter, &curPrefix->borderRouterList) {
02518             if (curBorderRouter->P_dhcp &&
02519                 curBorderRouter->routerID == cur->mac_parameters->mac_short_address) {
02520                 // We host this dhcp service we must add anycast address
02521                 ns_list_foreach(thread_network_data_context_entry_t, curRoute, &curPrefix->contextList) {
02522                     // Search what is the context id for this prefix
02523                     uint16_t anycastAddress = 0xfc00;
02524                     anycastAddress |= curRoute->cid;
02525                     thread_router_bootstrap_dhcp_server_any_cast_address_update(cur, anycastAddress);
02526                 }
02527             }
02528         }
02529     }
02530 }
02531 
02532 static void thread_bootstrap_service_anycast_address_generate(protocol_interface_info_entry_t *cur)
02533 {
02534     uint8_t ipv6_address[16];
02535 
02536     // Delete old address
02537     thread_addr_write_mesh_local_16(ipv6_address, 0xfc10, cur->thread_info);
02538     addr_delete_matching(cur,ipv6_address,124,ADDR_SOURCE_THREAD_ALOC);
02539 
02540     ns_list_foreach(thread_network_data_service_cache_entry_t, curService, &cur->thread_info->networkDataStorage.service_list) {
02541         // Go through all prefixes
02542         ns_list_foreach(thread_network_data_service_server_entry_t, curServiceServer, &curService->server_list) {
02543             if (curServiceServer->router_id == cur->mac_parameters->mac_short_address) {
02544                 // We host this service we must add anycast address
02545                 // Search what is the context id for this prefix
02546                 thread_addr_write_mesh_local_16(ipv6_address, 0xfc10 | curService->S_id, cur->thread_info);
02547                 tr_debug("generate Service anycast address %s", trace_ipv6(ipv6_address));
02548                 addr_add(cur, ipv6_address, 124, ADDR_SOURCE_THREAD_ALOC/*?*/, 0xffffffff, 0, true);
02549             }
02550         }
02551     }
02552 }
02553 
02554 
02555 static void thread_router_bootstrap_commissioner_aloc_generate(protocol_interface_info_entry_t *cur)
02556 {
02557     uint8_t commissioner_anycast[16];
02558 
02559     // Delete old address
02560     thread_addr_write_mesh_local_16(commissioner_anycast, 0xfc30 + (cur->thread_info->registered_commissioner.session_id % 8), cur->thread_info);
02561     addr_delete_matching(cur,commissioner_anycast,125,ADDR_SOURCE_THREAD_ALOC);
02562 
02563     if (!cur->thread_info->registered_commissioner.commissioner_valid) {
02564         // No commissioner available
02565         return;
02566     }
02567 
02568     if (!addr_get_entry(cur,cur->thread_info->registered_commissioner.border_router_address) ) {
02569         // Not our address
02570         return;
02571     }
02572     // Add commissioning border router address
02573     tr_debug("generate commissioner anycast address %s", trace_ipv6(commissioner_anycast));
02574     addr_add(cur,commissioner_anycast,64,ADDR_SOURCE_THREAD_ALOC,0xffffffff,0,true);
02575 }
02576 
02577 void thread_router_bootstrap_anycast_address_register(protocol_interface_info_entry_t *cur)
02578 {
02579     uint8_t leader_anycast_address[16];
02580 
02581     tr_debug("Register anycast address:");
02582 
02583     // Generate leader ALOC address
02584     thread_addr_write_mesh_local_16(leader_anycast_address, 0xfc00, cur->thread_info);
02585 
02586     if (cur->thread_info->leader_private_data) {
02587         tr_debug("Register leader anycast address: %s", trace_ipv6(leader_anycast_address));
02588         addr_add(cur, leader_anycast_address, 128, ADDR_SOURCE_UNKNOWN, 0xffffffff, 0, true);
02589     } else {
02590         // Delete Leader ALOC if ml prefix was changed address Source is the defining rule
02591         addr_delete_matching(cur,leader_anycast_address,128,ADDR_SOURCE_UNKNOWN);
02592 
02593     }
02594     thread_bootstrap_dhcp_anycast_address_generate(cur);
02595     thread_bootstrap_service_anycast_address_generate(cur);
02596     thread_router_bootstrap_commissioner_aloc_generate(cur);
02597     thread_extension_aloc_generate(cur);
02598 }
02599 
02600 static int thread_router_bootstrap_network_data_propagation(protocol_interface_info_entry_t *cur, uint8_t *childUnicastAddress, bool fullList)
02601 {
02602     uint8_t *payload_ptr;
02603     uint16_t payload_size = 16 + 4 + 20 + 4; //Version 4 bytes and Source address 4 bytes
02604     struct link_configuration *linkConfiguration;
02605     linkConfiguration = thread_joiner_application_get_config(cur->id);
02606     if (!linkConfiguration) {
02607         return -1;
02608     }
02609 
02610     if (!thread_info(cur)) {
02611         return -1;
02612     }
02613 
02614     payload_size += 2 + thread_network_data_tlv_size(cur, fullList);
02615     payload_size += thread_pending_timestamp_tlv_size(cur);
02616 
02617     mle_message_timeout_params_t timeout;
02618     uint16_t buf_id = mle_service_msg_allocate(cur->id, payload_size, false, MLE_COMMAND_DATA_RESPONSE);
02619     if (buf_id == 0) {
02620         return -1;
02621     }
02622 
02623     tr_debug("Send MLE data response network data changed");
02624     payload_ptr = mle_service_get_data_pointer(buf_id);
02625     uint8_t *address_ptr = mle_service_get_msg_destination_address_pointer(buf_id);
02626 
02627     /* Send to ULA16 of DATA_REQUEST originator. */
02628     /* Current_gp_prefix should have ULA prefix in */
02629     if (childUnicastAddress) {
02630         memcpy(address_ptr, childUnicastAddress, 16);
02631     } else {
02632         memcpy(address_ptr, ADDR_LINK_LOCAL_ALL_NODES, 16);
02633     }
02634 
02635     /*Add Leader Data & Network Data */
02636     payload_ptr = thread_leader_data_tlv_write(payload_ptr, cur);
02637     payload_ptr = thread_network_data_tlv_write(cur, payload_ptr, fullList);
02638 
02639     payload_ptr = thread_active_timestamp_write(cur, payload_ptr);
02640     payload_ptr = thread_pending_timestamp_write(cur, payload_ptr);
02641     payload_ptr = mle_general_write_source_address(payload_ptr, cur);
02642 
02643     if (mle_service_update_length_by_ptr(buf_id,payload_ptr)!= 0) {
02644         tr_debug("Buffer overflow at message write");
02645     }
02646     timeout.retrans_max = 0;
02647     timeout.timeout_init = 0;
02648     timeout.timeout_max = 0;
02649     timeout.delay = MLE_HALF_SECOND_MAX_DELAY;
02650     mle_service_set_msg_timeout_parameters(buf_id, &timeout);
02651     //Set Security
02652     uint32_t keySequence;
02653     thread_management_get_current_keysequence(cur->id, &keySequence);
02654     mle_service_msg_update_security_params(buf_id, 5, 2, keySequence);
02655 
02656     mle_service_send_message(buf_id);
02657     return 0;
02658 
02659 }
02660 
02661 static void thread_router_bootstrap_network_data_push_to_sleep_child(protocol_interface_info_entry_t *cur, bool stableDataUpdate)
02662 {
02663     uint8_t childLinkLocalAddress[16];
02664     mle_neigh_table_list_t *mle_table = mle_class_active_list_get(cur->id);
02665     memcpy(childLinkLocalAddress, ADDR_LINK_LOCAL_PREFIX, 8);
02666     ns_list_foreach(mle_neigh_table_entry_t, cur_entry, mle_table) {
02667         if (cur_entry->threadNeighbor) {
02668             if (!(cur_entry->mode & MLE_RX_ON_IDLE)) {
02669                 memcpy(&childLinkLocalAddress[8], cur_entry->mac64, 8);
02670                 childLinkLocalAddress[8] ^= 2;
02671                 if (cur_entry->mode & MLE_THREAD_REQ_FULL_DATA_SET) {
02672                     thread_router_bootstrap_network_data_propagation(cur, childLinkLocalAddress, true);
02673                 } else {
02674                     if (stableDataUpdate) {
02675                         thread_router_bootstrap_network_data_propagation(cur, childLinkLocalAddress, false);
02676                     }
02677                 }
02678             }
02679         }
02680     }
02681 }
02682 
02683 void thread_router_bootstrap_network_data_distribute(protocol_interface_info_entry_t *cur)
02684 {
02685     if (!thread_i_am_router(cur)) {
02686         return;
02687     }
02688     tr_debug("Propagate New Network Data");
02689     thread_router_bootstrap_network_data_propagation(cur, NULL, true);
02690     thread_router_bootstrap_network_data_push_to_sleep_child(cur, cur->thread_info->networkDataStorage.stableUpdatePushed);
02691 }
02692 
02693 bool thread_router_bootstrap_routing_allowed(struct protocol_interface_info_entry *cur)
02694 {
02695     link_configuration_s *link_conf_ptr = thread_management_configuration_get(cur->id);
02696 
02697     if (!link_conf_ptr) {
02698         return true;
02699     }
02700 
02701     return !(!thread_extension_version_check(cur->thread_info->version) && !(link_conf_ptr->securityPolicy & SECURITY_POLICY_ALL_ROUTERS_JOIN_ALLOWED));
02702 }
02703 
02704 void thread_router_bootstrap_address_change_notify_send(protocol_interface_info_entry_t *cur)
02705 {
02706     thread_info(cur)->proactive_an_timer = THREAD_PROACTIVE_AN_SEND_DELAY;
02707 }
02708 
02709 #endif /* HAVE_THREAD_ROUTER */