thread_discovery.c

00001 /*
00002  * Copyright (c) 2016-2018, Arm Limited and affiliates.
00003  * SPDX-License-Identifier: BSD-3-Clause
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  * 1. Redistributions of source code must retain the above copyright
00009  *    notice, this list of conditions and the following disclaimer.
00010  * 2. Redistributions in binary form must reproduce the above copyright
00011  *    notice, this list of conditions and the following disclaimer in the
00012  *    documentation and/or other materials provided with the distribution.
00013  * 3. Neither the name of the copyright holder nor the
00014  *    names of its contributors may be used to endorse or promote products
00015  *    derived from this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #include "nsconfig.h"
00031 #ifdef HAVE_THREAD
00032 #include "ns_types.h"
00033 #include "eventOS_event.h"
00034 #include "eventOS_event_timer.h"
00035 #include "thread_config.h"
00036 #include "common_functions.h"
00037 #include <string.h>
00038 #include "ns_trace.h"
00039 #include "ns_list.h"
00040 #include "randLIB.h"
00041 #include <nsdynmemLIB.h>
00042 #include "thread_discovery.h"
00043 #include "NWK_INTERFACE/Include/protocol.h"
00044 #include "6LoWPAN/Thread/thread_common.h"
00045 #include "6LoWPAN/Thread/thread_management_server.h"
00046 #include "6LoWPAN/Thread/thread_joiner_application.h"
00047 #include "6LoWPAN/Thread/thread_management_internal.h"
00048 #include "6LoWPAN/Thread/thread_bootstrap.h"
00049 #include "6LoWPAN/Thread/thread_ccm.h"
00050 #include "Service_Libs/mle_service/mle_service_api.h"
00051 #include "MLE/mle.h"
00052 #include "MLE/mle_tlv.h"
00053 #include "thread_tmfcop_lib.h"
00054 #include "thread_meshcop_lib.h"
00055 #include "6LoWPAN/MAC/mac_helper.h"
00056 #include "mac_api.h"
00057 #include "6LoWPAN/MAC/mac_data_poll.h"
00058 
00059 typedef enum {
00060     THREAD_DISCOVER_INIT = 0,
00061     THREAD_DISCOVER_TIMER
00062 } thread_discover_event_id_e;
00063 
00064 typedef struct {
00065     uint8_t type;
00066     uint8_t *data;
00067     uint16_t length;
00068 } mescop_tlv_t;
00069 
00070 #define TRACE_GROUP "tdis"
00071 
00072 static int8_t thread_discover_tasklet_id = -1;
00073 static bool thread_discover_timer_active = false;
00074 
00075 #define discover_optional_start_pointer(x)  (&(x)->filter_tlv_data[0])
00076 
00077 //Discovery request class
00078 typedef struct {
00079     uint16_t active_timer;
00080     uint32_t channel_mask;
00081     uint16_t random_panid;
00082     uint8_t temporary_mac64[8];
00083     thread_discovery_ready_cb *response_cb;
00084     uint8_t active_channel;
00085     uint8_t channel_page;
00086     bool waiting_response: 1;
00087     bool joiner_flag: 1;
00088     bool native_commisioner_scan: 1;
00089     uint8_t filter_tlv_length; //Optional Filter data length
00090     uint8_t filter_tlv_data[]; //Do not anything after this definition
00091 } thread_discovery_request_info_t;
00092 
00093 
00094 //Discovery request class
00095 typedef struct {
00096     uint16_t active_timer;
00097     uint32_t channel_mask;
00098     uint8_t active_channel;
00099     uint16_t pan_id;
00100     uint64_t active_time_stamp;
00101     thread_announce_scan_ready_cb *response_cb;
00102     announce_discovery_response_t *network;
00103     bool waiting_response: 1;
00104 } thread_announce_request_info_t;
00105 
00106 
00107 typedef struct {
00108     uint8_t extentedAddress[8];
00109     uint16_t panId;
00110     uint8_t timer;
00111     bool randomJitter;
00112     ns_list_link_t link;
00113 } thread_discovery_response_msg_t;
00114 
00115 typedef NS_LIST_HEAD (thread_discovery_response_msg_t, link) thread_discovery_response_msg_list;
00116 
00117 typedef struct {
00118     struct protocol_interface_info_entry *interface;
00119     thread_discovery_request_info_t *discovery_request;
00120     thread_announce_request_info_t *thread_announce_request;
00121     thread_discovery_response_msg_list srv_respose_msg_list;
00122     thread_discovery_response_msg_list srv_respose_msg_buffers;
00123     thread_discovery_response_msg_t *msg_buffers;
00124     thread_nwk_discovery_response_list_t discovered_network;
00125     int8_t interface_id;
00126     uint8_t version;
00127     bool discovery_server_active;
00128     ns_list_link_t link;
00129 } thread_discovery_class_t;
00130 
00131 #define THREAD_DISCOVER_TIMER_PERIOD 50
00132 
00133 
00134 static NS_LIST_DEFINE(thread_discovery_class_list, thread_discovery_class_t, link);
00135 
00136 static bool thread_discovery_timer_update(void);
00137 static void thread_discover_timer_trig(void);
00138 static discovery_response_list_t *thread_discover_response_msg_allocate(void);
00139 static discovery_response_list_t *thread_discover_response_msg_get_discover_from_list(thread_nwk_discovery_response_list_t *list, uint8_t channel, uint16_t panId);
00140 
00141 
00142 static int stringlen(const char *s, int n)
00143 {
00144     char *end = memchr(s, 0, n);
00145     return end ? end - s : n;
00146 }
00147 
00148 static void thread_discover_timer_trig(void)
00149 {
00150     if (thread_discover_timer_active || thread_discover_tasklet_id == -1) {
00151 
00152     }
00153     eventOS_event_timer_request(3, THREAD_DISCOVER_TIMER, thread_discover_tasklet_id, THREAD_DISCOVER_TIMER_PERIOD);
00154     thread_discover_timer_active = true;
00155 }
00156 
00157 static thread_discovery_request_info_t *thread_discovery_request_allocate(thread_discover_reques_t *scan_request, thread_discovery_ready_cb *response_cb)
00158 {
00159     thread_discovery_request_info_t *discover_request = ns_dyn_mem_temporary_alloc(sizeof(thread_discovery_request_info_t) + scan_request->filter_tlv_length);
00160     if (discover_request) {
00161         discover_request->waiting_response = false;
00162         discover_request->active_timer = 0;
00163         discover_request->channel_mask = scan_request->channel_mask;
00164         discover_request->joiner_flag = scan_request->joiner_flag;
00165         discover_request->native_commisioner_scan = scan_request->native_commisioner;
00166         discover_request->filter_tlv_length = scan_request->filter_tlv_length;
00167         discover_request->random_panid = randLIB_get_random_in_range(1, 0xfffd); //Generate random pan-id
00168         randLIB_get_n_bytes_random(discover_request->temporary_mac64, 8); //Generate random temporary mac64
00169 
00170         discover_request->response_cb = response_cb;
00171         if (discover_request->filter_tlv_length) {
00172             memcpy(discover_optional_start_pointer(discover_request), scan_request->filter_tlv_data, discover_request->filter_tlv_length);
00173         }
00174     }
00175     return discover_request;
00176 }
00177 
00178 
00179 static thread_announce_request_info_t *thread_announce_discovery_request_allocate(thread_announce_discover_reques_t *scan_request, thread_announce_scan_ready_cb *response_cb)
00180 {
00181     thread_announce_request_info_t *discover_request = ns_dyn_mem_temporary_alloc(sizeof(thread_announce_request_info_t));
00182     if (discover_request) {
00183         discover_request->waiting_response = false;
00184         discover_request->active_timer = 0;
00185         discover_request->channel_mask = scan_request->channel_mask;
00186         discover_request->pan_id = scan_request->pan_id;
00187         discover_request->active_time_stamp = scan_request->active_timestamp;
00188         discover_request->response_cb = response_cb;
00189         discover_request->network = NULL;
00190     }
00191     return discover_request;
00192 }
00193 
00194 static void thread_discovery_server_msg_clean(thread_discovery_class_t *class)
00195 {
00196     class->discovery_server_active = false;
00197     ns_list_foreach_safe(thread_discovery_response_msg_t, cur, &class->srv_respose_msg_list) {
00198         ns_list_remove(&class->srv_respose_msg_list, cur);
00199         ns_list_add_to_start(&class->srv_respose_msg_buffers, cur);
00200     }
00201 }
00202 
00203 static bool thread_discovery_server_msg_buffer_allocate(thread_discovery_class_t *class)
00204 {
00205     thread_discovery_response_msg_t *msg_buffer = ns_dyn_mem_alloc(sizeof(thread_discovery_class_t) * 4);
00206     if (!msg_buffer) {
00207         return false;
00208     }
00209     class->msg_buffers = msg_buffer;
00210     for (uint8_t i = 0; i < 4; i++) {
00211         ns_list_add_to_start(&class->srv_respose_msg_buffers, msg_buffer++);
00212     }
00213     return true;
00214 }
00215 
00216 static void thread_discovery_server_msg_buffer_free(thread_discovery_class_t *class)
00217 {
00218     thread_discovery_server_msg_clean(class);
00219     ns_dyn_mem_free(class->msg_buffers);
00220     class->msg_buffers = NULL;
00221 }
00222 
00223 static bool thread_discovery_response_pending_at_list(thread_discovery_response_msg_list *list, uint8_t *extended_address)
00224 {
00225     ns_list_foreach(thread_discovery_response_msg_t, cur, list) {
00226         if (memcmp(cur->extentedAddress, extended_address, 8) == 0) {
00227             return true;
00228         }
00229     }
00230     return false;
00231 }
00232 
00233 static thread_discovery_response_msg_t *thread_discovery_response_allocate(thread_discovery_response_msg_list *list)
00234 {
00235     thread_discovery_response_msg_t *entry = ns_list_get_first(list);
00236     if (entry) {
00237         //Remove from the list
00238         ns_list_remove(list, entry);
00239         // -1 because timer accuracy is 50ms and message sending must happen before 300ms
00240         entry->timer = randLIB_get_random_in_range(1, THREAD_DISCOVERY_MAX_JITTER / THREAD_DISCOVER_TIMER_PERIOD - 1);
00241         entry->randomJitter = true;
00242     }
00243 
00244     return entry;
00245 }
00246 
00247 static void thread_discovery_response_trig(thread_discovery_class_t *class, uint8_t *ll64, uint16_t dstPanId)
00248 {
00249     //Start DoS verify
00250     if (thread_discovery_response_pending_at_list(&class->srv_respose_msg_list, ll64 + 8)) {
00251         tr_debug("Too Agressive Discovery");
00252         return;
00253     }
00254 
00255     //Allocate new response
00256     thread_discovery_response_msg_t *entry = thread_discovery_response_allocate(&class->srv_respose_msg_buffers);
00257     if (!entry) {
00258         tr_debug("Too much discovery");
00259         return;
00260     }
00261     //end DoS verify
00262     /* Add response messaged trigger to list */
00263     memcpy(entry->extentedAddress, ll64 + 8, 8);
00264     entry->panId = dstPanId;
00265     //Add to list
00266     ns_list_add_to_end(&class->srv_respose_msg_list, entry);
00267     thread_discover_timer_trig();
00268 
00269 }
00270 
00271 static discovery_response_list_t *thread_discover_response_msg_get_discover_from_list(thread_nwk_discovery_response_list_t *list, uint8_t channel, uint16_t panId)
00272 {
00273     //Get last
00274     discovery_response_list_t *entry = ns_list_get_last(list);
00275     while (entry) {
00276         if (entry->channel == channel && entry->pan_id == panId) {
00277             return entry;
00278         }
00279         entry = ns_list_get_previous(list, entry);
00280 
00281     }
00282     return NULL;
00283 }
00284 
00285 static discovery_response_list_t *thread_discover_response_msg_allocate(void)
00286 {
00287     discovery_response_list_t *msg = ns_dyn_mem_temporary_alloc(sizeof(discovery_response_list_t));
00288     if (msg) {
00289         memset(msg, 0, sizeof(discovery_response_list_t));
00290     }
00291     return msg;
00292 }
00293 
00294 static void thread_discovery_request_free(thread_discovery_class_t *class)
00295 {
00296     if (!class->discovery_request) {
00297         return;
00298     }
00299 
00300     ns_dyn_mem_free(class->discovery_request);
00301     class->discovery_request = NULL;
00302 
00303 }
00304 
00305 static void thread_announce_discovery_request_free(thread_discovery_class_t *class)
00306 {
00307     if (!class->thread_announce_request) {
00308         return;
00309     }
00310     ns_dyn_mem_free(class->thread_announce_request->network);
00311     ns_dyn_mem_free(class->thread_announce_request);
00312     class->thread_announce_request = NULL;
00313 
00314 }
00315 
00316 static void thread_discovery_prepare(protocol_interface_info_entry_t *interface, thread_discovery_request_info_t *discovery)
00317 {
00318     mac_helper_default_security_level_set(interface, SEC_ENC_MIC32);
00319     mac_helper_beacon_payload_reallocate(interface, 0); // No beacons for thread
00320     mac_data_poll_init(interface);
00321     mac_helper_mac16_address_set(interface, 0xffff);
00322     mac_helper_mac64_set(interface, discovery->temporary_mac64);
00323     thread_set_link_local_address(interface); // only to generate IID
00324     discovery->active_timer = 1;
00325     discovery->waiting_response = false;
00326     thread_discover_timer_trig();
00327 }
00328 
00329 static void thread_announce_discovery_prepare(protocol_interface_info_entry_t *interface, thread_announce_request_info_t *discovery)
00330 {
00331     mac_helper_default_security_level_set(interface, SEC_ENC_MIC32);
00332     if (thread_info(interface)->thread_device_mode == THREAD_DEVICE_MODE_SLEEPY_END_DEVICE) {
00333         thread_end_device_mode_set(interface, false);
00334     }
00335     mac_data_poll_init(interface);
00336     mac_helper_mac16_address_set(interface, 0xffff);
00337     discovery->active_timer = 1;
00338     discovery->waiting_response = false;
00339     thread_discover_timer_trig();
00340 }
00341 
00342 static bool thread_discovery_proces_ready(thread_discovery_request_info_t *discovery)
00343 {
00344 
00345     //Get next free channel
00346     uint8_t i;
00347     uint32_t mask = 1;
00348 
00349     for (i = 0; i < 32; i++) {
00350         if (discovery->channel_mask & mask) {
00351             discovery->channel_mask &= ~mask;
00352             discovery->active_channel = i;
00353             discovery->channel_page = 0; //Support only chnnel page 0
00354             return false;
00355         }
00356         mask <<= 1;
00357     }
00358     return true;
00359 }
00360 
00361 static bool thread_announce_discovery_process_ready(thread_announce_request_info_t *discovery)
00362 {
00363 
00364     //Get next free channel
00365     uint8_t i;
00366     uint32_t mask = 0x80000000;
00367 
00368     for (i = 0; i < 32; i++) {
00369         if (discovery->channel_mask & mask) {
00370             discovery->channel_mask &= ~mask;
00371             discovery->active_channel = i;
00372             return false;
00373         }
00374         mask >>= 1;
00375     }
00376     return true;
00377 }
00378 
00379 static void thread_discovery_process_end(protocol_interface_info_entry_t *interface)
00380 {
00381     mlme_reset_t reset;
00382     reset.SetDefaultPIB = true;
00383     interface->mac_api->mlme_req(interface->mac_api, MLME_RESET, &reset);
00384 }
00385 
00386 /**
00387  * Activate mac to selected channel and pan-id
00388  */
00389 static void thread_discovery_link_activate(protocol_interface_info_entry_t *interface, uint16_t pan_id, uint8_t channel, uint8_t channel_page)
00390 {
00391     mlme_start_t start_req;
00392     memset(&start_req, 0, sizeof(mlme_start_t));
00393 
00394     interface->mac_parameters->pan_id = pan_id;
00395     interface->mac_parameters->mac_channel = channel;
00396 
00397     start_req.PANId = pan_id;
00398     start_req.LogicalChannel = channel;
00399     start_req.ChannelPage = channel_page;
00400     start_req.BeaconOrder = 0x0f;
00401     start_req.SuperframeOrder = 0x0f;
00402 
00403     if (interface->mac_api) {
00404         interface->mac_api->mlme_req(interface->mac_api, MLME_START, (void *)&start_req);
00405     }
00406 }
00407 
00408 static uint16_t thread_discover_tlv_get(uint8_t version, bool dynamic_bit)
00409 {
00410     uint16_t thread_discover_tlv = 0;
00411 
00412     thread_discover_tlv |= (uint16_t)(version << 12);
00413 
00414     if (dynamic_bit) {
00415         thread_discover_tlv |= (uint16_t)(1 << 11);
00416     }
00417 
00418     return thread_discover_tlv;
00419 }
00420 
00421 
00422 #ifdef HAVE_THREAD_V2
00423 static uint8_t thread_discovery_ccm_response_len(protocol_interface_info_entry_t *cur)
00424 {
00425     uint8_t length = 0;
00426     uint8_t domain_name_len;
00427     // AE port
00428     if (!cur || !cur->thread_info->ccm_info) {
00429         return 0;
00430     }
00431     if (cur->thread_info->ccm_info->relay_port_ae) {
00432         length += 4;
00433     }
00434     // NMK port
00435     if (cur->thread_info->ccm_info->relay_port_nmkp) {
00436         length += 4;
00437     }
00438     /* Thread 1.2 CCM add-ons */
00439     if (cur->thread_info->version >= THREAD_VERSION_1_2 && thread_info(cur)->ccm_credentials_ptr) {
00440         // Calculate also following optional TLV's:
00441         // Thread domain name TLV
00442         domain_name_len = thread_ccm_thread_name_length_get(cur);
00443         if (domain_name_len) {
00444             length += domain_name_len + 2;
00445         }
00446         // AE steering data
00447         // NMKP Steering Data
00448     }
00449     return length;
00450 }
00451 
00452 static uint8_t *thread_discovery_ccm_response_write(protocol_interface_info_entry_t *cur, uint8_t *ptr)
00453 {
00454     if (!cur || !cur->thread_info->ccm_info) {
00455         return ptr;
00456     }
00457     // AE port
00458     if (cur->thread_info->ccm_info->relay_port_ae) {
00459         ptr = thread_meshcop_tlv_data_write_uint16(ptr, MESHCOP_TLV_AE_PORT, cur->thread_info->ccm_info->relay_port_ae);
00460     }
00461     // NMK port
00462     if (cur->thread_info->ccm_info->relay_port_nmkp) {
00463         ptr = thread_meshcop_tlv_data_write_uint16(ptr, MESHCOP_TLV_NMKP_PORT, cur->thread_info->ccm_info->relay_port_nmkp);
00464     }
00465     /* Thread 1.2 CCM add-ons */
00466     if (cur->thread_info->version >= THREAD_VERSION_1_2 && thread_info(cur)->ccm_credentials_ptr) {
00467         // Thread domain name TLV
00468         if (thread_ccm_thread_name_length_get(cur)) {
00469             ptr = thread_meshcop_tlv_data_write(ptr, MESHCOP_TLV_DOMAIN_NAME, thread_ccm_thread_name_length_get(cur), thread_ccm_thread_name_ptr_get(cur));
00470         }
00471         // Build also following optional TLV's, when supported:
00472         // AE steering data
00473         // NMKP Steering Data
00474     }
00475     return ptr;
00476 }
00477 void thread_discovery_ccm_response_read(discovery_response_list_t *nwk_info, uint16_t discover_response_tlv, uint8_t *data_ptr, uint16_t data_len)
00478 {
00479     uint8_t domain_data_len;
00480     uint8_t *domain_data_ptr;
00481 
00482     domain_data_len = thread_meshcop_tlv_find(data_ptr, data_len, MESHCOP_TLV_DOMAIN_NAME, &domain_data_ptr);
00483     if (domain_data_len > 16) {
00484         domain_data_len = 0;
00485     }
00486 
00487     if (domain_data_len) {
00488         memcpy(nwk_info->ccm_info.domain_name, domain_data_ptr, domain_data_len);
00489     }
00490 
00491     thread_meshcop_tlv_data_get_uint16(data_ptr, data_len, MESHCOP_TLV_AE_PORT, &nwk_info->ccm_info.ae_port);
00492     thread_meshcop_tlv_data_get_uint16(data_ptr, data_len, MESHCOP_TLV_NMKP_PORT, &nwk_info->ccm_info.nmk_port);
00493     nwk_info->ccm_info.ccm_supported = (discover_response_tlv >> 10) & 1;
00494 }
00495 
00496 void thread_discovery_ccm_info_write(uint16_t *data, uint8_t version, uint16_t securityPolicy)
00497 {
00498     if (version == 3 && !(securityPolicy & THREAD_SECURITY_POLICY_CCM_DISABLED)) {
00499         *data |= (uint16_t)(1 << 10);
00500     }
00501 }
00502 
00503 bool thread_discovery_ccm_joining_enabled(int8_t interface_id)
00504 {
00505     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
00506 
00507     if (!cur || !cur->thread_info->ccm_info) {
00508         return false;
00509     }
00510     if (cur->thread_info->ccm_info->relay_port_ae ||
00511             cur->thread_info->ccm_info->relay_port_nmkp) {
00512         tr_warn("Commercial joiner router enabled");
00513         return true;
00514     }
00515     return false;
00516 }
00517 
00518 #else
00519 #define thread_discovery_ccm_response_len(cur) (0)
00520 #define thread_discovery_ccm_response_write(cur, ptr) (ptr)
00521 #define thread_discovery_ccm_response_read(nwk_info, discover_response_tlv, data_ptr, data_len)
00522 #define thread_discovery_ccm_info_write(data, version, securityPolicy)
00523 #define thread_discovery_ccm_joining_enabled(interface_id) (false)
00524 
00525 #endif
00526 
00527 static int thread_discovery_request_send(thread_discovery_class_t *class, thread_discovery_request_info_t *discovery)
00528 {
00529     protocol_interface_info_entry_t *cur = class->interface;
00530 
00531     uint16_t buf_id = mle_service_msg_allocate(cur->id, 4 + discovery->filter_tlv_length + 2, false, MLE_COMMAND_DISCOVERY_REQUEST);
00532     if (buf_id == 0) {
00533         return -1;
00534     }
00535 
00536     mle_service_set_msg_destination_address(buf_id, ADDR_LINK_LOCAL_ALL_NODES);
00537     mle_service_msg_update_security_params(buf_id, 0, 0, 0);
00538     //Enable link layer security
00539     mle_service_set_msg_link_layer_security_mode(buf_id, true);
00540     uint8_t *ptr = mle_service_get_data_pointer(buf_id);
00541     uint16_t discover_request_tlv = thread_discover_tlv_get(class->version, discovery->joiner_flag);
00542 
00543     *ptr++ = MLE_TYPE_DISCOVERY;
00544     *ptr++ = 4 + discovery->filter_tlv_length;
00545 
00546     ptr = thread_meshcop_tlv_data_write_uint16(ptr, MESHCOP_TLV_DISCOVERY_REQUEST, discover_request_tlv);
00547     if (discovery->filter_tlv_length) {
00548         memcpy(ptr, discover_optional_start_pointer(discovery), discovery->filter_tlv_length);
00549         ptr += discovery->filter_tlv_length;
00550     }
00551 
00552     if (mle_service_update_length_by_ptr(buf_id, ptr) != 0) {
00553         tr_debug("Buffer overflow at message write");
00554     }
00555 
00556     mle_message_timeout_params_t timeout;
00557     timeout.retrans_max = 0;
00558     timeout.timeout_init = 0;
00559     timeout.timeout_max = 0;
00560     timeout.delay = MLE_NO_DELAY;
00561 
00562     mle_service_set_msg_timeout_parameters(buf_id, &timeout);
00563     mle_service_set_msg_panid(buf_id, 0xffff);
00564     mle_service_send_message(buf_id);
00565     discovery->waiting_response = true;
00566     return 0;
00567 }
00568 
00569 static int thread_discovery_announce_request_send(thread_discovery_class_t *class, thread_announce_request_info_t *discovery)
00570 {
00571     protocol_interface_info_entry_t *cur = class->interface;
00572 
00573 
00574     uint16_t buf_id = mle_service_msg_allocate(cur->id, 19, false, MLE_COMMAND_DATASET_ANNOUNCE);
00575     if (buf_id == 0) {
00576         return -1;
00577     }
00578 
00579     mle_service_set_msg_destination_address(buf_id, ADDR_LINK_LOCAL_ALL_NODES);
00580     uint32_t keySequence;
00581     thread_management_get_current_keysequence(cur->id, &keySequence);
00582     mle_service_msg_update_security_params(buf_id, 5, 2, keySequence);
00583     mle_service_set_msg_link_layer_security_mode(buf_id, true);
00584     uint8_t *ptr = mle_service_get_data_pointer(buf_id);
00585 
00586     uint8_t channel_tlv[3];
00587     ptr = thread_meshcop_tlv_data_write_uint64(ptr, MLE_TYPE_ACTIVE_TIMESTAMP, discovery->active_time_stamp);
00588     ptr = thread_meshcop_tlv_data_write_uint16(ptr, MLE_TYPE_PANID, discovery->pan_id);
00589     channel_tlv[0] = 0;
00590     common_write_16_bit(discovery->active_channel, &channel_tlv[1]);
00591     ptr = thread_meshcop_tlv_data_write(ptr, MLE_TYPE_CHANNEL, 3, channel_tlv);
00592 
00593 
00594 
00595     if (mle_service_update_length_by_ptr(buf_id, ptr) != 0) {
00596         tr_debug("Buffer overflow at message write");
00597     }
00598 
00599     mle_message_timeout_params_t timeout;
00600     timeout.retrans_max = 0;
00601     timeout.timeout_init = 0;
00602     timeout.timeout_max = 0;
00603     timeout.delay = MLE_NO_DELAY;
00604 
00605     mle_service_set_msg_timeout_parameters(buf_id, &timeout);
00606     mle_service_set_msg_panid(buf_id, 0xffff);
00607     mle_service_send_message(buf_id);
00608     discovery->waiting_response = true;
00609     return 0;
00610 }
00611 
00612 static int thread_discovery_response_send(thread_discovery_class_t *class, thread_discovery_response_msg_t *msg_buffers)
00613 {
00614     link_configuration_s *linkConfiguration = thread_joiner_application_get_config(class->interface_id);
00615     if (!linkConfiguration) {
00616         return -1;
00617     }
00618     thread_management_server_data_t server_data;
00619     if (thread_management_server_commisoner_data_get(class->interface_id, &server_data) != 0) {
00620         return -1;
00621     }
00622 
00623     protocol_interface_info_entry_t *cur = class->interface;
00624 
00625     // Calculate length
00626     uint16_t message_length = 16; // Default data to response always
00627     message_length += stringlen((char *)&linkConfiguration->name, 16);
00628 
00629     // [Joiner UDP Port TLV]
00630     if (server_data.joiner_router_enabled && server_data.joiner_router_port) {
00631         message_length += 4;
00632         // [Steering Data TLV]
00633         if (cur->thread_info->registered_commissioner.commissioner_valid && cur->thread_info->registered_commissioner.steering_data_len) {
00634             message_length += cur->thread_info->registered_commissioner.steering_data_len + 2;
00635         }
00636     }
00637 
00638     // [Commissioner UDP Port TLV]
00639     if (linkConfiguration->securityPolicy & SECURITY_POLICY_NATIVE_COMMISSIONING_ALLOWED) {
00640         message_length += 4;
00641     }
00642 
00643     message_length +=  thread_discovery_ccm_response_len(cur);
00644 
00645     uint16_t buf_id = mle_service_msg_allocate(class->interface_id, message_length + 2, false, MLE_COMMAND_DISCOVERY_RESPONSE);
00646     if (buf_id == 0) {
00647         return -1;
00648     }
00649 
00650     //SET ll64 from euid64
00651     uint8_t *ll64 = mle_service_get_msg_destination_address_pointer(buf_id);
00652     memcpy(ll64, ADDR_LINK_LOCAL_PREFIX, 8);
00653     memcpy(ll64 + 8, msg_buffers->extentedAddress, 8);
00654     //No link layer security and no mle security.
00655     mle_service_msg_update_security_params(buf_id, 0, 0, 0);
00656     uint8_t *ptr = mle_service_get_data_pointer(buf_id);
00657 
00658     *ptr++ = MLE_TYPE_DISCOVERY;
00659     *ptr++ = message_length;
00660     uint16_t discover_response_tlv = thread_discover_tlv_get(class->version, (linkConfiguration->securityPolicy & SECURITY_POLICY_NATIVE_COMMISSIONING_ALLOWED));
00661 
00662     thread_discovery_ccm_info_write(&discover_response_tlv, class->version, linkConfiguration->securityPolicy);
00663 
00664     ptr = thread_meshcop_tlv_data_write_uint16(ptr, MESHCOP_TLV_DISCOVERY_RESPONSE, discover_response_tlv);
00665     ptr = thread_meshcop_tlv_data_write(ptr, MESHCOP_TLV_XPANID, 8, linkConfiguration->extented_pan_id);
00666     ptr = thread_meshcop_tlv_data_write(ptr, MESHCOP_TLV_NETWORK_NAME, stringlen((char *)&linkConfiguration->name, 16), linkConfiguration->name);
00667     //Optional Part
00668 
00669     if (linkConfiguration->securityPolicy & SECURITY_POLICY_NATIVE_COMMISSIONING_ALLOWED) {
00670         ptr = thread_meshcop_tlv_data_write_uint16(ptr, MESHCOP_TLV_COMMISSIONER_UDP_PORT, server_data.commissioner_port);
00671     }
00672 
00673     if (server_data.joiner_router_enabled && server_data.joiner_router_port) {
00674         ptr = thread_meshcop_tlv_data_write_uint16(ptr, MESHCOP_TLV_JOINER_UDP_PORT, server_data.joiner_router_port);
00675         if (cur->thread_info->registered_commissioner.commissioner_valid && cur->thread_info->registered_commissioner.steering_data_len) {
00676             ptr = thread_meshcop_tlv_data_write(ptr, MESHCOP_TLV_STEERING_DATA, cur->thread_info->registered_commissioner.steering_data_len, cur->thread_info->registered_commissioner.steering_data);
00677         }
00678     }
00679 
00680     ptr =  thread_discovery_ccm_response_write(cur, ptr);
00681 
00682     if (mle_service_update_length_by_ptr(buf_id, ptr) != 0) {
00683         tr_debug("Buffer overflow at message write");
00684     }
00685 
00686     mle_message_timeout_params_t timeout;
00687     timeout.retrans_max = 0;
00688     timeout.timeout_init = 0;
00689     timeout.timeout_max = 0;
00690     timeout.delay = MLE_NO_DELAY;
00691 
00692     mle_service_set_msg_timeout_parameters(buf_id, &timeout);
00693     mle_service_set_msg_panid(buf_id, msg_buffers->panId);
00694     mle_service_send_message(buf_id);
00695     return 0;
00696 }
00697 
00698 
00699 static bool thread_discovery_request_timer_update(thread_discovery_class_t *class)
00700 {
00701     class->discovery_request->active_timer--;
00702     if (class->discovery_request->active_timer) {
00703         return true;
00704     }
00705 
00706     if (thread_discovery_proces_ready(class->discovery_request)) {
00707         thread_discovery_process_end(class->interface);
00708         class->discovery_request->response_cb(class->interface, &class->discovered_network);
00709         //Free Entry
00710         thread_discovery_request_free(class);
00711         return false;
00712     }
00713 
00714     //Switch channel and set pan-id
00715     thread_discovery_link_activate(class->interface, class->discovery_request->random_panid, class->discovery_request->active_channel, class->discovery_request->channel_page);
00716     tr_debug("Discover channel %u", class->discovery_request->active_channel);
00717     //Process packet
00718     thread_discovery_request_send(class, class->discovery_request);
00719     //Set timer
00720     class->discovery_request->active_timer = THREAD_DISCOVERY_TIMEOUT / THREAD_DISCOVER_TIMER_PERIOD;
00721     return true;
00722 
00723 }
00724 
00725 static bool thread_announce_discovery_request_timer_update(thread_discovery_class_t *class)
00726 {
00727     class->thread_announce_request->active_timer--;
00728     if (class->thread_announce_request->active_timer) {
00729         return true;
00730     }
00731 
00732     if (thread_announce_discovery_process_ready(class->thread_announce_request)) {
00733         thread_discovery_process_end(class->interface);
00734         announce_discovery_response_t *result = class->thread_announce_request->network;
00735         thread_announce_scan_ready_cb *response_cb = class->thread_announce_request->response_cb;
00736         class->thread_announce_request->network = NULL;
00737         thread_announce_discovery_request_free(class);
00738         response_cb(class->interface, result);
00739         return false;
00740     }
00741 
00742     //Switch channel and set pan-id
00743     thread_discovery_link_activate(class->interface, class->thread_announce_request->pan_id, class->thread_announce_request->active_channel, 0);
00744     tr_debug("Announce Discover channel %u", class->thread_announce_request->active_channel);
00745     //Process packet
00746     thread_discovery_announce_request_send(class, class->thread_announce_request);
00747     //Set timer
00748     class->thread_announce_request->active_timer = THREAD_DISCOVERY_TIMEOUT / THREAD_DISCOVER_TIMER_PERIOD;
00749     return true;
00750 
00751 }
00752 
00753 static bool thread_discovery_response_jitter_timer_update(thread_discovery_class_t *class)
00754 {
00755     bool pending_list = false;
00756     ns_list_foreach_safe(thread_discovery_response_msg_t, cur, &class->srv_respose_msg_list) {
00757         cur->timer--;
00758         if (!cur->timer) {
00759             //Send a packet
00760             thread_discovery_response_send(class, cur);
00761             ns_list_remove(&class->srv_respose_msg_list, cur);
00762             ns_list_add_to_start(&class->srv_respose_msg_buffers, cur);
00763         } else {
00764             pending_list = true;
00765         }
00766     }
00767 
00768     return pending_list;
00769 
00770 }
00771 
00772 static thread_discovery_class_t *thread_discovery_class_get(int8_t interface_id)
00773 {
00774     ns_list_foreach(thread_discovery_class_t, cur_class, &thread_discovery_class_list) {
00775         if (cur_class->interface_id == interface_id) {
00776             return cur_class;
00777         }
00778     }
00779     return NULL;
00780 }
00781 
00782 static void thread_discovery_free_discovered_networks(thread_nwk_discovery_response_list_t *list)
00783 {
00784     ns_list_foreach_safe(discovery_response_list_t, cur, list) {
00785         ns_list_remove(list, cur);
00786         ns_dyn_mem_free(cur);
00787     }
00788 }
00789 
00790 static thread_discovery_class_t *thread_discovery_class_allocate(bool reedDevice)
00791 {
00792     thread_discovery_class_t *class_ptr = ns_dyn_mem_alloc(sizeof(thread_discovery_class_t));
00793 
00794     if (class_ptr) {
00795         ns_list_init(&class_ptr->srv_respose_msg_list);
00796         ns_list_init(&class_ptr->srv_respose_msg_buffers);
00797         ns_list_init(&class_ptr->discovered_network);
00798 
00799         if (reedDevice) {
00800             if (!thread_discovery_server_msg_buffer_allocate(class_ptr)) {
00801                 ns_dyn_mem_free(class_ptr);
00802                 return NULL;
00803             }
00804         } else {
00805             class_ptr->msg_buffers = NULL;
00806         }
00807         class_ptr->discovery_request = NULL;
00808         class_ptr->thread_announce_request = NULL;
00809         ns_list_add_to_start(&thread_discovery_class_list, class_ptr);
00810     }
00811     return class_ptr;
00812 }
00813 
00814 static void thread_discovery_class_free(thread_discovery_class_t *class_ptr)
00815 {
00816     if (!class_ptr) {
00817         return;
00818     }
00819     ns_list_remove(&thread_discovery_class_list, class_ptr);
00820 
00821     thread_discovery_server_msg_buffer_free(class_ptr);
00822     thread_discovery_free_discovered_networks(&class_ptr->discovered_network);
00823     thread_discovery_request_free(class_ptr);
00824     thread_announce_discovery_request_free(class_ptr);
00825     ns_dyn_mem_free(class_ptr);
00826 }
00827 
00828 bool thread_discovery_tlv_spesific_data_discover(const uint8_t *ptr, uint16_t length, const mescop_tlv_t *compare_tlv)
00829 {
00830     const uint8_t *p;
00831     if (!ptr || length < 2 || !compare_tlv || !compare_tlv->data || !compare_tlv->length) {
00832         return false;
00833     }
00834 
00835     p = ptr;
00836     while (p != NULL) {
00837         const uint8_t *tlv_data_ptr;
00838         uint16_t tlv_data_length;
00839         //tr_info("tlv_find first check");
00840         // check if we have enough length for normal length tlv
00841         if (p + 2 > ptr + length) {
00842             break;    //must have at least type and short length
00843         }
00844 
00845         if (p[1] == 0xff) {
00846             // Long length format
00847             if (p + 4 > ptr + length) {
00848                 break;    // check if enough length for long length
00849             }
00850 
00851             tlv_data_length = common_read_16_bit(&p[2]);
00852             tlv_data_ptr = p + 4;
00853         } else {
00854             tlv_data_length = p[1];
00855             tlv_data_ptr = p + 2;
00856         }
00857         //tr_info("tlv_find check: %d, type: %d", tlv_data_length, *p);
00858 
00859         // check if length of tlv is correct
00860         if (tlv_data_ptr + tlv_data_length > ptr + length) {
00861             break;    //length goes past the data block
00862         }
00863 
00864         if (*p == compare_tlv->type) {
00865             if (tlv_data_length == compare_tlv->length) {
00866                 if (memcmp(tlv_data_ptr, compare_tlv->data, tlv_data_length) == 0) {
00867                     return true;
00868                 }
00869             }
00870         }
00871 
00872         p = tlv_data_ptr + tlv_data_length;
00873     }
00874     return false;
00875 }
00876 
00877 static bool thread_discovery_request_filter_validate(link_configuration_s *linkConfiguration, uint8_t *data_ptr, uint16_t length)
00878 {
00879     //Validate PAN-ID
00880     uint8_t pan_id[2];
00881 
00882     mescop_tlv_t compare_tlv;
00883     //Validate PAN-id, ExtentedPAN-id & Network name
00884 
00885     compare_tlv.data = pan_id;
00886     compare_tlv.length = 2;
00887     compare_tlv.type = MESHCOP_TLV_PANID;
00888 
00889     common_write_16_bit(linkConfiguration->panId, compare_tlv.data);
00890 
00891     if (thread_discovery_tlv_spesific_data_discover(data_ptr, length, &compare_tlv)) {
00892         return false;
00893     }
00894 
00895     compare_tlv.data = linkConfiguration->extented_pan_id;
00896     compare_tlv.length = 8;
00897     compare_tlv.type = MESHCOP_TLV_XPANID;
00898 
00899     if (thread_discovery_tlv_spesific_data_discover(data_ptr, length, &compare_tlv)) {
00900         return false;
00901     }
00902 
00903     return true;
00904 
00905 }
00906 
00907 static void thread_discovery_request_msg_handler(thread_discovery_class_t *discovery_class, mle_message_t *mle_msg)
00908 {
00909     //Validate that server is enabled
00910     if (!discovery_class->discovery_server_active) {
00911         return;
00912     }
00913 
00914     link_configuration_s *linkConfiguration = thread_joiner_application_get_config(discovery_class->interface_id);
00915     if (!linkConfiguration) {
00916         return;
00917     }
00918     tr_debug("Thread discovery request message RX");
00919 
00920     // Check if we have room for new neighbor
00921     if (mle_class_free_entry_count_get(discovery_class->interface) < 1) {
00922         tr_debug("MLE table full, skip request");
00923         return;
00924     }
00925 
00926     //validate message
00927     mle_tlv_info_t discovery_tlv;
00928     //Parse Message
00929     uint16_t discover_req_tlv;
00930 
00931     //Discover MLE_TYPE_DISCOVERY
00932     if (mle_tlv_option_discover(mle_msg->data_ptr, mle_msg->data_length, MLE_TYPE_DISCOVERY, &discovery_tlv) < 4) {
00933         return;
00934     }
00935 
00936     if (thread_meshcop_tlv_data_get_uint16(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_DISCOVERY_REQUEST, &discover_req_tlv) < 2) {
00937         tr_debug("discover response not include all mandatory TLV's");
00938         return;
00939     }
00940     //Validate Version
00941     uint8_t version = (uint8_t)(discover_req_tlv >> 12);
00942     if (discovery_class->version < version) {
00943         tr_debug("Dropped by version %u != %u", discovery_class->version, version);
00944         return;
00945     }
00946 
00947     bool joiner_flag = (discover_req_tlv >> 11) & 1;
00948 
00949     if (joiner_flag) {
00950         //Can we respond
00951         thread_management_server_data_t joiner_router_info;
00952         if (0 != thread_management_server_commisoner_data_get(discovery_class->interface_id, &joiner_router_info) ||
00953                 !joiner_router_info.joiner_router_enabled) {
00954             if (!thread_discovery_ccm_joining_enabled(discovery_class->interface_id)) {
00955                 tr_debug("Drop by Joining disabled");
00956                 return;
00957             }
00958         }
00959     }
00960 
00961     //Validate possible blacklist
00962     if (!thread_discovery_request_filter_validate(linkConfiguration, discovery_tlv.dataPtr, discovery_tlv.tlvLen)) {
00963         tr_debug("Dropped by filter");
00964         return;
00965     }
00966 
00967     //Trig response
00968     thread_discovery_response_trig(discovery_class, mle_msg->packet_src_address, mle_msg->src_pan_id);
00969 }
00970 
00971 static bool thread_seering_data_accept_any(uint8_t length, uint8_t *data)
00972 {
00973     if (length == 1 && *data == 0xff) {
00974         return true;
00975     }
00976     return false;
00977 }
00978 
00979 static void thread_discovery_nwk_push_to_list_by_lqi(thread_nwk_discovery_response_list_t *result_list, discovery_response_list_t *nwk_info)
00980 {
00981     if (ns_list_count(result_list)) {
00982         ns_list_foreach_safe(discovery_response_list_t, cur_entry, result_list) {
00983             if (nwk_info->dbm > cur_entry->dbm) {
00984 
00985                 ns_list_add_before(result_list, cur_entry, nwk_info);
00986                 return;
00987             }
00988         }
00989         ns_list_add_to_end(result_list, nwk_info);
00990     } else {
00991         ns_list_add_to_end(result_list, nwk_info);
00992     }
00993 }
00994 
00995 static void thread_discovery_joiner_set(thread_nwk_discovery_response_list_t *result_list, discovery_response_list_t *nwk_info, bool new_accept_any)
00996 {
00997     if (ns_list_count(result_list)) {
00998 
00999         bool cur_acept_any;
01000         ns_list_foreach_safe(discovery_response_list_t, cur_entry, result_list) {
01001 
01002             cur_acept_any = thread_seering_data_accept_any(cur_entry->steering_data_valid, cur_entry->steering_data);
01003 
01004             if (!cur_acept_any && !new_accept_any) {
01005                 if (nwk_info->dbm > cur_entry->dbm) {
01006                     ns_list_add_before(result_list, cur_entry, nwk_info);
01007                     return;
01008                 }
01009             } else {
01010                 if (!new_accept_any) {
01011                     //add this before cur
01012                     ns_list_add_before(result_list, cur_entry, nwk_info);
01013                     return;
01014                 } else if (nwk_info->dbm > cur_entry->dbm) {
01015                     ns_list_add_before(result_list, cur_entry, nwk_info);
01016                     return;
01017                 }
01018             }
01019         }
01020         ns_list_add_to_end(result_list, nwk_info);
01021     } else {
01022         ns_list_add_to_end(result_list, nwk_info);
01023     }
01024 }
01025 
01026 
01027 
01028 static void thread_discovery_response_msg_handler(thread_discovery_class_t *discovery_class, mle_message_t *mle_msg)
01029 {
01030     if (!discovery_class->discovery_request || !discovery_class->discovery_request->waiting_response) {
01031         return;
01032     }
01033 
01034     mle_tlv_info_t discovery_tlv;
01035     //Parse Message
01036     uint16_t discover_response_tlv;
01037     uint8_t *nwk_name, *extented_panid;
01038     uint8_t nwk_name_length;
01039 
01040     //Discover MLE_TYPE_DISCOVERY
01041     if (mle_tlv_option_discover(mle_msg->data_ptr, mle_msg->data_length, MLE_TYPE_DISCOVERY, &discovery_tlv) < 4) {
01042         return;
01043     }
01044 
01045     nwk_name_length = thread_meshcop_tlv_find(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_NETWORK_NAME, &nwk_name);
01046 
01047     if (thread_meshcop_tlv_data_get_uint16(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_DISCOVERY_RESPONSE, &discover_response_tlv) < 2
01048             || thread_meshcop_tlv_find(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_XPANID, &extented_panid) < 8
01049             || nwk_name_length > 16) {
01050 
01051         tr_debug("discover response not include all mandatory TLV's");
01052         return;
01053     }
01054 
01055     tr_debug("Thread discovery response message RX");
01056 
01057     uint8_t version = (uint8_t)(discover_response_tlv >> 12);
01058     if (discovery_class->version > version) {
01059         tr_debug("Dropped by version %u != %u", discovery_class->version, version);
01060         return;
01061     }
01062 
01063     if (discovery_class->discovery_request->native_commisioner_scan) {
01064         bool native_commioner_bit = (discover_response_tlv >> 11) & 1;
01065         if (!native_commioner_bit) {
01066             tr_debug("Native commisioner not supported");
01067             return;
01068         }
01069     }
01070     uint16_t pan_id = mle_msg->src_pan_id;
01071     uint8_t *steering_data;
01072     uint16_t joiner_port;
01073     bool joiner_port_valid;
01074 
01075     uint8_t steerin_data_length = thread_meshcop_tlv_find(discovery_tlv.dataPtr,  discovery_tlv.tlvLen, MESHCOP_TLV_STEERING_DATA, &steering_data);
01076     if (steerin_data_length > 16) {
01077         steerin_data_length = 0;
01078     }
01079 
01080     if (thread_meshcop_tlv_data_get_uint16(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_JOINER_UDP_PORT, &joiner_port) >= 2) {
01081         joiner_port_valid = true;
01082     } else {
01083         joiner_port_valid = false;
01084     }
01085 
01086     if (discovery_class->discovery_request->joiner_flag && (!joiner_port_valid || steerin_data_length == 0)) {
01087         if (discovery_class->interface->thread_info->version >= THREAD_VERSION_1_2) {
01088             if (!discovery_class->interface->thread_info->ccm_credentials_ptr) {
01089                 tr_debug("Dropped, no joiner info");
01090             }
01091         } else {
01092             tr_debug("Dropped by no valid joiner info %u %u", joiner_port_valid, steerin_data_length);
01093             return;
01094         }
01095     }
01096 
01097     discovery_response_list_t *nwk_info = thread_discover_response_msg_get_discover_from_list(
01098                                               &discovery_class->discovered_network,
01099                                               discovery_class->discovery_request->active_channel, pan_id);
01100     if (nwk_info) {
01101         if (nwk_info->dbm < mle_msg->dbm) {
01102             goto save_optional_data;
01103         }
01104         return;
01105     }
01106 
01107     nwk_info = thread_discover_response_msg_allocate();
01108     if (!nwk_info) {
01109         return;
01110     }
01111     //Set parameters
01112     nwk_info->version = (discover_response_tlv >> 12);
01113     nwk_info->dbm = mle_msg->dbm;
01114     nwk_info->channel = discovery_class->discovery_request->active_channel;
01115 
01116     nwk_info->pan_id = pan_id;
01117     memcpy(nwk_info->extented_pan_id, extented_panid, 8);
01118 
01119     memset(nwk_info->network_name, 0, 16);
01120     memcpy(nwk_info->network_name, nwk_name, nwk_name_length);
01121 
01122     thread_meshcop_tlv_data_get_uint16(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_COMMISSIONER_UDP_PORT, &nwk_info->commissioner_port);
01123 
01124     thread_discovery_ccm_response_read(nwk_info, discover_response_tlv, discovery_tlv.dataPtr, discovery_tlv.tlvLen);
01125 
01126     //Add to last
01127     if (discovery_class->discovery_request->native_commisioner_scan) {
01128         thread_discovery_nwk_push_to_list_by_lqi(&discovery_class->discovered_network, nwk_info);
01129     } else {
01130         //Validate is steering data
01131         thread_discovery_joiner_set(&discovery_class->discovered_network, nwk_info, thread_seering_data_accept_any(nwk_info->steering_data_valid, nwk_info->steering_data));
01132     }
01133 
01134 
01135 save_optional_data:
01136     memcpy(nwk_info->extented_mac, mle_msg->packet_src_address + 8, 8);
01137     nwk_info->extented_mac[0] ^= 2;
01138     if (steerin_data_length) {
01139         memcpy(nwk_info->steering_data, steering_data, steerin_data_length);
01140         nwk_info->steering_data_valid = steerin_data_length;
01141     }
01142 
01143     if (joiner_port_valid) {
01144         nwk_info->joiner_port = joiner_port;
01145     }
01146 
01147 }
01148 
01149 static void thread_announce_discovery_message_receiver_cb(int8_t interface_id, mle_message_t *mle_msg)
01150 {
01151     if (mle_msg->message_type != MLE_COMMAND_DATASET_ANNOUNCE) {
01152         return;
01153     }
01154     thread_discovery_class_t *discovery_class = thread_discovery_class_get(interface_id);
01155     if (!discovery_class || !discovery_class->thread_announce_request) {
01156         return;
01157     }
01158 
01159     tr_debug("MLE ANNOUNCE RX");
01160     uint64_t timestamp;
01161     uint16_t panid;
01162     uint8_t *ptr;
01163     uint16_t channel;
01164 
01165     tr_debug("Host Recv Dataset Announce %s", trace_ipv6(mle_msg->packet_src_address));
01166     if (8 > thread_tmfcop_tlv_data_get_uint64(mle_msg->data_ptr, mle_msg->data_length, MLE_TYPE_ACTIVE_TIMESTAMP, &timestamp)) {
01167         tr_error("Missing timestamp TLV");
01168         return;
01169     }
01170     if (2 > thread_tmfcop_tlv_data_get_uint16(mle_msg->data_ptr, mle_msg->data_length, MLE_TYPE_PANID, &panid)) {
01171         tr_error("Missing Panid TLV");
01172         return;
01173     }
01174     if (3 > thread_tmfcop_tlv_find(mle_msg->data_ptr, mle_msg->data_length, MLE_TYPE_CHANNEL, &ptr)) {
01175         tr_error("Missing Channel TLV");
01176         return;
01177     }
01178     channel = common_read_16_bit(&ptr[1]);
01179 
01180     if (timestamp <= discovery_class->thread_announce_request->active_time_stamp) {
01181         tr_debug("Drop by timestamp");
01182         return;
01183     }
01184 
01185     //Validate
01186     announce_discovery_response_t *response = NULL;
01187     if (discovery_class->thread_announce_request->network) {
01188         response = discovery_class->thread_announce_request->network;
01189         if (timestamp <= discovery_class->thread_announce_request->network->active_timestamp) {
01190             response = NULL;
01191         }
01192     } else {
01193         discovery_class->thread_announce_request->network = ns_dyn_mem_temporary_alloc(sizeof(announce_discovery_response_t));
01194         response = discovery_class->thread_announce_request->network;
01195     }
01196 
01197     if (response) {
01198         tr_debug("Save data");
01199         response->active_timestamp = timestamp;
01200         response->channel = channel;
01201         response->pan_id = panid;
01202     }
01203 }
01204 
01205 
01206 static void thread_announce_discover_receive_cb(int8_t interface_id, mle_message_t *mle_msg, mle_security_header_t *security_headers)
01207 {
01208     //Verify security
01209     (void)security_headers;
01210 
01211     /* Check that message is from link-local scope */
01212     if (!addr_is_ipv6_link_local(mle_msg->packet_src_address)) {
01213         return;
01214     }
01215 
01216     thread_announce_discovery_message_receiver_cb(interface_id, mle_msg);
01217 }
01218 
01219 static void thread_discovery_message_receiver_cb(int8_t interface_id, mle_message_t *mle_msg)
01220 {
01221     //Discovery interface get
01222     thread_discovery_class_t *discovery_class = thread_discovery_class_get(interface_id);
01223     if (!discovery_class) {
01224         return;
01225     }
01226 
01227     switch (mle_msg->message_type) {
01228         case MLE_COMMAND_DISCOVERY_REQUEST:
01229             //call message handler
01230             thread_discovery_request_msg_handler(discovery_class, mle_msg);
01231             break;
01232 
01233         case MLE_COMMAND_DISCOVERY_RESPONSE:
01234             //call message handler
01235             thread_discovery_response_msg_handler(discovery_class, mle_msg);
01236             break;
01237 
01238         default:
01239 
01240             break;
01241     }
01242 
01243 }
01244 
01245 static void thread_discover_event_handler(arm_event_s *event)
01246 {
01247     switch (event->event_type) {
01248         case THREAD_DISCOVER_INIT:
01249             thread_discover_timer_trig();
01250             break;
01251 
01252         case THREAD_DISCOVER_TIMER:
01253             //Do list in future for each of mle user
01254             thread_discover_timer_active = false;
01255             if (thread_discovery_timer_update()) {
01256                 //Request new timer
01257                 thread_discover_timer_trig();
01258             }
01259             break;
01260     }
01261 }
01262 
01263 static int8_t thread_discover_class_event_handler_init(void)
01264 {
01265     if (thread_discover_tasklet_id == -1) {
01266         //GENERATE TASKLET
01267         thread_discover_tasklet_id = eventOS_event_handler_create(&thread_discover_event_handler, THREAD_DISCOVER_INIT);
01268     }
01269     return thread_discover_tasklet_id;
01270 }
01271 
01272 
01273 int thread_discovery_init(int8_t interface_id, struct protocol_interface_info_entry *cur_interface, uint8_t version, bool reedDevice)
01274 {
01275     if (!cur_interface) {
01276         return -2;
01277     }
01278 
01279     thread_discovery_class_t *discovery_class = thread_discovery_class_get(interface_id);
01280     if (discovery_class) {
01281         //Verify reed boolean
01282 
01283         thread_discovery_request_free(discovery_class);
01284         thread_announce_discovery_request_free(discovery_class);
01285         thread_discovery_server_msg_buffer_free(discovery_class);
01286         if (reedDevice) {
01287             if (!thread_discovery_server_msg_buffer_allocate(discovery_class)) {
01288                 thread_discovery_class_free(discovery_class);
01289                 return -1;
01290             }
01291         }
01292 
01293         goto return_ok;
01294     }
01295 
01296     if (thread_discover_class_event_handler_init() < 0) {
01297         return -1;
01298     }
01299 
01300     //Allocate new entry
01301     discovery_class = thread_discovery_class_allocate(reedDevice);
01302     if (!discovery_class) {
01303         mle_service_interface_receiver_bypass_handler_update(interface_id,  NULL);
01304         return -1;
01305     }
01306     discovery_class->interface_id = interface_id;
01307 
01308 return_ok:
01309     discovery_class->discovery_server_active = false;
01310     discovery_class->interface = cur_interface;
01311     discovery_class->version = version;
01312     return 0;
01313 }
01314 
01315 /**
01316  * Reset discovery class state to idle
01317  */
01318 int thread_discovery_reset(int8_t interface_id)
01319 {
01320     thread_discovery_class_t *discovery_class = thread_discovery_class_get(interface_id);
01321     if (!discovery_class) {
01322         return -1;
01323     }
01324     thread_discovery_request_free(discovery_class);
01325     thread_announce_discovery_request_free(discovery_class);
01326     thread_discovery_server_msg_clean(discovery_class);
01327     return 0;
01328 }
01329 
01330 /**
01331  * Update discovery timer state's
01332  */
01333 static bool thread_discovery_timer_update(void)
01334 {
01335     bool keep_timer_active = false;
01336     ns_list_foreach(thread_discovery_class_t, cur_class, &thread_discovery_class_list) {
01337         if (cur_class->discovery_server_active) {
01338             if (thread_discovery_response_jitter_timer_update(cur_class)) {
01339                 keep_timer_active = true;
01340             }
01341         } else if (cur_class->discovery_request) {
01342             if (thread_discovery_request_timer_update(cur_class)) {
01343                 keep_timer_active = true;
01344             }
01345         } else if (cur_class->thread_announce_request) {
01346             if (thread_announce_discovery_request_timer_update(cur_class)) {
01347                 keep_timer_active = true;
01348             }
01349         }
01350     }
01351     return keep_timer_active;
01352 }
01353 
01354 /**
01355  * Enable thread discovery request response support
01356  */
01357 int thread_discovery_responser_enable(int8_t interface_id, bool enable_service)
01358 {
01359     thread_discovery_class_t *discovery_class = thread_discovery_class_get(interface_id);
01360     if (!discovery_class || !discovery_class->msg_buffers) {
01361         return -1;
01362     }
01363 
01364 
01365     //Clean server message list always
01366     thread_discovery_server_msg_clean(discovery_class);
01367     if (mle_service_interface_receiver_bypass_handler_update(interface_id,  thread_discovery_message_receiver_cb) != 0) {
01368         return -1;
01369     }
01370 
01371     discovery_class->discovery_server_active = enable_service;
01372 
01373     return 0;
01374 }
01375 
01376 static void thread_discovery_normal_receive_cb(int8_t interface_id, mle_message_t *mle_msg, mle_security_header_t *security_headers)
01377 {
01378     if (security_headers->securityLevel == 0) {
01379         thread_discovery_message_receiver_cb(interface_id, mle_msg);
01380     }
01381 }
01382 
01383 /**
01384  * Start Thread network discovery
01385  */
01386 int thread_discovery_network_scan(struct protocol_interface_info_entry *cur_interface, thread_discover_reques_t *scan_request, thread_discovery_ready_cb *ready_cb)
01387 {
01388 
01389     thread_discovery_class_t *discovery_class = thread_discovery_class_get(cur_interface->id);
01390     if (!discovery_class || !ready_cb || !scan_request) {
01391         return -1;
01392     }
01393 
01394     //Check Is discovery started already
01395     if (discovery_class->discovery_request) {
01396         return -2;
01397     }
01398 
01399     if (!scan_request->channel_mask || (scan_request->filter_tlv_length > 0 && !scan_request->filter_tlv_data)) {
01400         return -1;
01401     }
01402 
01403     discovery_class->discovery_request = thread_discovery_request_allocate(scan_request, ready_cb);
01404 
01405     if (!discovery_class->discovery_request) {
01406         return -3;
01407     }
01408 
01409     if (mle_service_interface_register(cur_interface->id, cur_interface, thread_discovery_normal_receive_cb, discovery_class->discovery_request->temporary_mac64, 8) != 0) {
01410         thread_discovery_request_free(discovery_class);
01411         return -1;
01412     }
01413 
01414     if (mle_service_interface_receiver_bypass_handler_update(cur_interface->id, thread_discovery_message_receiver_cb) != 0) {
01415         thread_discovery_request_free(discovery_class);
01416         return -1;
01417     }
01418 
01419     //Free old networks
01420     thread_discovery_free_discovered_networks(&discovery_class->discovered_network);
01421     //Set temporary mac and generate ll64
01422     thread_discovery_prepare(discovery_class->interface, discovery_class->discovery_request);
01423     return 0;
01424 }
01425 
01426 int thread_discovery_announce_network_scan(int8_t interface_id, thread_announce_discover_reques_t *scan_request, thread_announce_scan_ready_cb *ready_cb)
01427 {
01428     thread_discovery_class_t *discovery_class = thread_discovery_class_get(interface_id);
01429     if (!discovery_class || !ready_cb || !scan_request) {
01430         return -1;
01431     }
01432 
01433     //Check Is discovery started already
01434     if (discovery_class->discovery_request || discovery_class->thread_announce_request) {
01435         return -2;
01436     }
01437 
01438     if (!scan_request->channel_mask) {
01439         return -1;
01440     }
01441 
01442     discovery_class->thread_announce_request = thread_announce_discovery_request_allocate(scan_request, ready_cb);
01443 
01444     if (!discovery_class->thread_announce_request) {
01445         return -3;
01446     }
01447 
01448     //Update receiver callback
01449     if (mle_service_interface_receiver_handler_update(interface_id, thread_announce_discover_receive_cb) != 0) {
01450         thread_announce_discovery_request_free(discovery_class);
01451         return -1;
01452     }
01453 
01454     if (mle_service_interface_receiver_bypass_handler_update(interface_id,  thread_announce_discovery_message_receiver_cb) != 0) {
01455         thread_discovery_request_free(discovery_class);
01456         return -1;
01457     }
01458 
01459     //Set temporary mac and generate ll64
01460     thread_announce_discovery_prepare(discovery_class->interface, discovery_class->thread_announce_request);
01461     return 0;
01462 }
01463 
01464 discovery_response_list_t *thread_discovery_network_description_get(int8_t interface_id)
01465 {
01466     thread_discovery_class_t *discovery_class = thread_discovery_class_get(interface_id);
01467     if (!discovery_class) {
01468         return NULL;
01469     }
01470 
01471     discovery_response_list_t *entry = ns_list_get_first(&discovery_class->discovered_network);
01472     if (entry) {
01473         ns_list_remove(&discovery_class->discovered_network, entry);
01474     }
01475 
01476     return entry;
01477 
01478 }
01479 
01480 #if 0
01481 /**
01482  * Free all allocated memory and free class
01483  * Not used API function, flagged away. This should be taken in use when refactoring ifdown - functionality.
01484  */
01485 int thread_discovery_free(int8_t interface_id)
01486 {
01487     thread_discovery_class_t *discovery_class = thread_discovery_class_get(interface_id);
01488     if (!discovery_class) {
01489         return -1;
01490     }
01491 
01492     thread_discovery_class_free(discovery_class);
01493 
01494 
01495     return 0;
01496 }
01497 #endif
01498 #endif