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_extension.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     //Get next free channel
00345     uint8_t i;
00346     uint32_t mask = 1;
00347 
00348     for (i=0; i<32; i++)
00349     {
00350         if (discovery->channel_mask & mask)
00351         {
00352             discovery->channel_mask &= ~mask;
00353             discovery->active_channel = i;
00354             discovery->channel_page = 0; //Support only chnnel page 0
00355             return false;
00356         }
00357         mask <<= 1;
00358     }
00359     return true;
00360 }
00361 
00362 static bool thread_announce_discovery_process_ready(thread_announce_request_info_t *discovery) {
00363 
00364     //Get next free channel
00365     uint8_t i;
00366     uint32_t mask = 0x80000000;
00367 
00368     for (i=0; i<32; i++)
00369     {
00370         if (discovery->channel_mask & mask)
00371         {
00372             discovery->channel_mask &= ~mask;
00373             discovery->active_channel = i;
00374             return false;
00375         }
00376         mask >>= 1;
00377     }
00378     return true;
00379 }
00380 
00381 static void thread_discovery_process_end(protocol_interface_info_entry_t *interface)
00382 {
00383     mlme_reset_t reset;
00384     reset.SetDefaultPIB = true;
00385     interface->mac_api->mlme_req(interface->mac_api, MLME_RESET, &reset);
00386 }
00387 
00388 /**
00389  * Activate mac to selected channel and pan-id
00390  */
00391 static void thread_discovery_link_activate(protocol_interface_info_entry_t *interface, uint16_t pan_id, uint8_t channel, uint8_t channel_page)
00392 {
00393     mlme_start_t start_req;
00394     memset(&start_req, 0, sizeof(mlme_start_t));
00395 
00396     interface->mac_parameters->pan_id = pan_id;
00397     interface->mac_parameters->mac_channel = channel;
00398 
00399     start_req.PANId = pan_id;
00400     start_req.LogicalChannel = channel;
00401     start_req.ChannelPage = channel_page;
00402     start_req.BeaconOrder = 0x0f;
00403     start_req.SuperframeOrder = 0x0f;
00404 
00405     if( interface->mac_api ){
00406         interface->mac_api->mlme_req(interface->mac_api, MLME_START, (void*)&start_req);
00407     }
00408 }
00409 
00410 static uint16_t thread_discover_tlv_get(uint8_t version, bool dynamic_bit)
00411 {
00412     uint16_t thread_discover_tlv = 0;
00413 
00414     thread_discover_tlv |= (uint16_t) (version << 12);
00415 
00416     if (dynamic_bit) {
00417         thread_discover_tlv |= (uint16_t) (1 << 11);
00418     }
00419 
00420     return thread_discover_tlv;
00421 }
00422 
00423 
00424 static int thread_discovery_request_send(thread_discovery_class_t *class, thread_discovery_request_info_t *discovery)
00425 {
00426     protocol_interface_info_entry_t *cur = class->interface;
00427 
00428     uint16_t buf_id = mle_service_msg_allocate(cur->id, 4 + discovery->filter_tlv_length + 2, false, MLE_COMMAND_DISCOVERY_REQUEST);
00429     if (buf_id == 0) {
00430         return -1;
00431     }
00432 
00433     mle_service_set_msg_destination_address(buf_id, ADDR_LINK_LOCAL_ALL_NODES);
00434     mle_service_msg_update_security_params(buf_id, 0, 0, 0);
00435     //Enable link layer security
00436     mle_service_set_msg_link_layer_security_mode(buf_id, true);
00437     uint8_t *ptr = mle_service_get_data_pointer(buf_id);
00438     uint16_t discover_request_tlv = thread_discover_tlv_get(class->version, discovery->joiner_flag);
00439 
00440     *ptr++ = MLE_TYPE_DISCOVERY;
00441     *ptr++ = 4 + discovery->filter_tlv_length;
00442 
00443     ptr = thread_meshcop_tlv_data_write_uint16(ptr, MESHCOP_TLV_DISCOVERY_REQUEST, discover_request_tlv);
00444     if (discovery->filter_tlv_length) {
00445         memcpy(ptr, discover_optional_start_pointer(discovery), discovery->filter_tlv_length);
00446         ptr += discovery->filter_tlv_length;
00447     }
00448 
00449     if (mle_service_update_length_by_ptr(buf_id,ptr)!= 0) {
00450         tr_debug("Buffer overflow at message write");
00451     }
00452 
00453     mle_message_timeout_params_t timeout;
00454     timeout.retrans_max = 0;
00455     timeout.timeout_init = 0;
00456     timeout.timeout_max = 0;
00457     timeout.delay = MLE_NO_DELAY;
00458 
00459     mle_service_set_msg_timeout_parameters(buf_id, &timeout);
00460     mle_service_set_msg_panid(buf_id, 0xffff);
00461     mle_service_send_message(buf_id);
00462     discovery->waiting_response = true;
00463     return 0;
00464 }
00465 
00466 static int thread_discovery_announce_request_send(thread_discovery_class_t *class, thread_announce_request_info_t *discovery)
00467 {
00468     protocol_interface_info_entry_t *cur = class->interface;
00469 
00470 
00471     uint16_t buf_id = mle_service_msg_allocate(cur->id, 19, false, MLE_COMMAND_DATASET_ANNOUNCE);
00472     if (buf_id == 0) {
00473         return -1;
00474     }
00475 
00476     mle_service_set_msg_destination_address(buf_id, ADDR_LINK_LOCAL_ALL_NODES);
00477     uint32_t keySequence;
00478     thread_management_get_current_keysequence(cur->id, &keySequence);
00479     mle_service_msg_update_security_params(buf_id, 5, 2, keySequence);
00480     mle_service_set_msg_link_layer_security_mode(buf_id, true);
00481     uint8_t *ptr = mle_service_get_data_pointer(buf_id);
00482 
00483     uint8_t channel_tlv[3];
00484     ptr = thread_meshcop_tlv_data_write_uint64(ptr,MLE_TYPE_ACTIVE_TIMESTAMP, discovery->active_time_stamp);
00485     ptr = thread_meshcop_tlv_data_write_uint16(ptr,MLE_TYPE_PANID, discovery->pan_id);
00486     channel_tlv[0] = 0;
00487     common_write_16_bit(discovery->active_channel, &channel_tlv[1]);
00488     ptr = thread_meshcop_tlv_data_write(ptr,MLE_TYPE_CHANNEL, 3, channel_tlv);
00489 
00490 
00491 
00492     if (mle_service_update_length_by_ptr(buf_id,ptr)!= 0) {
00493         tr_debug("Buffer overflow at message write");
00494     }
00495 
00496     mle_message_timeout_params_t timeout;
00497     timeout.retrans_max = 0;
00498     timeout.timeout_init = 0;
00499     timeout.timeout_max = 0;
00500     timeout.delay = MLE_NO_DELAY;
00501 
00502     mle_service_set_msg_timeout_parameters(buf_id, &timeout);
00503     mle_service_set_msg_panid(buf_id, 0xffff);
00504     mle_service_send_message(buf_id);
00505     discovery->waiting_response = true;
00506     return 0;
00507 }
00508 
00509 
00510 
00511 
00512 static int thread_discovery_response_send(thread_discovery_class_t *class, thread_discovery_response_msg_t *msg_buffers)
00513 {
00514     link_configuration_s *linkConfiguration = thread_joiner_application_get_config(class->interface_id);
00515     if (!linkConfiguration) {
00516         return -1;
00517     }
00518     thread_management_server_data_t server_data;
00519     if (thread_management_server_commisoner_data_get(class->interface_id, &server_data) != 0) {
00520         return -1;
00521     }
00522 
00523     protocol_interface_info_entry_t *cur = class->interface;
00524 
00525     // Calculate length
00526     uint16_t message_length = 16; // Default data to response always
00527     message_length += stringlen((char*)&linkConfiguration->name, 16);
00528 
00529     // [Joiner UDP Port TLV]
00530     if (server_data.joiner_router_enabled && server_data.joiner_router_port) {
00531         message_length += 4;
00532         // [Steering Data TLV]
00533         if (cur->thread_info->registered_commissioner.commissioner_valid && cur->thread_info->registered_commissioner.steering_data_len){
00534             message_length += cur->thread_info->registered_commissioner.steering_data_len + 2;
00535         }
00536     }
00537 
00538     // [Commissioner UDP Port TLV]
00539     if (linkConfiguration->securityPolicy & SECURITY_POLICY_NATIVE_COMMISSIONING_ALLOWED) {
00540         message_length += 4;
00541     }
00542 
00543     message_length +=  thread_extension_discover_response_len(cur);
00544 
00545     uint16_t buf_id = mle_service_msg_allocate(class->interface_id, message_length + 2, false, MLE_COMMAND_DISCOVERY_RESPONSE);
00546     if (buf_id == 0) {
00547         return -1;
00548     }
00549 
00550     //SET ll64 from euid64
00551     uint8_t *ll64 = mle_service_get_msg_destination_address_pointer(buf_id);
00552     memcpy(ll64, ADDR_LINK_LOCAL_PREFIX, 8);
00553     memcpy(ll64 + 8,msg_buffers->extentedAddress , 8);
00554     //No link layer security and no mle security.
00555     mle_service_msg_update_security_params(buf_id, 0, 0, 0);
00556     uint8_t *ptr = mle_service_get_data_pointer(buf_id);
00557 
00558     *ptr++ = MLE_TYPE_DISCOVERY;
00559     *ptr++ = message_length;
00560     uint16_t discover_response_tlv = thread_discover_tlv_get(class->version, (linkConfiguration->securityPolicy & SECURITY_POLICY_NATIVE_COMMISSIONING_ALLOWED));
00561 
00562     thread_extension_discover_response_tlv_write(&discover_response_tlv, class->version, linkConfiguration->securityPolicy);
00563 
00564     ptr = thread_meshcop_tlv_data_write_uint16(ptr, MESHCOP_TLV_DISCOVERY_RESPONSE, discover_response_tlv);
00565     ptr = thread_meshcop_tlv_data_write(ptr, MESHCOP_TLV_XPANID, 8, linkConfiguration->extented_pan_id);
00566     ptr = thread_meshcop_tlv_data_write(ptr, MESHCOP_TLV_NETWORK_NAME, stringlen((char*)&linkConfiguration->name, 16), linkConfiguration->name);
00567     //Optional Part
00568 
00569     if (linkConfiguration->securityPolicy & SECURITY_POLICY_NATIVE_COMMISSIONING_ALLOWED) {
00570         ptr = thread_meshcop_tlv_data_write_uint16(ptr, MESHCOP_TLV_COMMISSIONER_UDP_PORT, server_data.commissioner_port);
00571     }
00572 
00573     if (server_data.joiner_router_enabled && server_data.joiner_router_port) {
00574         ptr = thread_meshcop_tlv_data_write_uint16(ptr, MESHCOP_TLV_JOINER_UDP_PORT, server_data.joiner_router_port);
00575         if (cur->thread_info->registered_commissioner.commissioner_valid && cur->thread_info->registered_commissioner.steering_data_len) {
00576             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);
00577         }
00578     }
00579 
00580     ptr =  thread_extension_discover_response_write(cur, ptr);
00581 
00582     if (mle_service_update_length_by_ptr(buf_id,ptr)!= 0) {
00583         tr_debug("Buffer overflow at message write");
00584     }
00585 
00586     mle_message_timeout_params_t timeout;
00587     timeout.retrans_max = 0;
00588     timeout.timeout_init = 0;
00589     timeout.timeout_max = 0;
00590     timeout.delay = MLE_NO_DELAY;
00591 
00592     mle_service_set_msg_timeout_parameters(buf_id, &timeout);
00593     mle_service_set_msg_panid(buf_id, msg_buffers->panId);
00594     mle_service_send_message(buf_id);
00595     return 0;
00596 }
00597 
00598 
00599 static bool thread_discovery_request_timer_update(thread_discovery_class_t *class)
00600 {
00601     class->discovery_request->active_timer--;
00602     if (class->discovery_request->active_timer) {
00603         return true;
00604     }
00605 
00606     if (thread_discovery_proces_ready(class->discovery_request)) {
00607         thread_discovery_process_end(class->interface);
00608         class->discovery_request->response_cb(class->interface, &class->discovered_network);
00609         //Free Entry
00610         thread_discovery_request_free(class);
00611         return false;
00612     }
00613 
00614     //Switch channel and set pan-id
00615     thread_discovery_link_activate(class->interface, class->discovery_request->random_panid, class->discovery_request->active_channel, class->discovery_request->channel_page);
00616     tr_debug("Discover channel %u", class->discovery_request->active_channel);
00617     //Process packet
00618     thread_discovery_request_send(class, class->discovery_request);
00619     //Set timer
00620     class->discovery_request->active_timer = THREAD_DISCOVERY_TIMEOUT / THREAD_DISCOVER_TIMER_PERIOD;
00621     return true;
00622 
00623 }
00624 
00625 static bool thread_announce_discovery_request_timer_update(thread_discovery_class_t *class)
00626 {
00627     class->thread_announce_request->active_timer--;
00628     if (class->thread_announce_request->active_timer) {
00629         return true;
00630     }
00631 
00632     if (thread_announce_discovery_process_ready(class->thread_announce_request) ) {
00633         thread_discovery_process_end(class->interface);
00634         announce_discovery_response_t *result = class->thread_announce_request->network;
00635         thread_announce_scan_ready_cb *response_cb = class->thread_announce_request->response_cb;
00636         class->thread_announce_request->network = NULL;
00637         thread_announce_discovery_request_free(class);
00638         response_cb(class->interface, result);
00639         return false;
00640     }
00641 
00642     //Switch channel and set pan-id
00643     thread_discovery_link_activate(class->interface, class->thread_announce_request->pan_id, class->thread_announce_request->active_channel, 0);
00644     tr_debug("Announce Discover channel %u", class->thread_announce_request->active_channel);
00645     //Process packet
00646     thread_discovery_announce_request_send(class, class->thread_announce_request);
00647     //Set timer
00648     class->thread_announce_request->active_timer = THREAD_DISCOVERY_TIMEOUT / THREAD_DISCOVER_TIMER_PERIOD;
00649     return true;
00650 
00651 }
00652 
00653 static bool thread_discovery_response_jitter_timer_update(thread_discovery_class_t *class)
00654 {
00655     bool pending_list = false;
00656     ns_list_foreach_safe(thread_discovery_response_msg_t, cur, &class->srv_respose_msg_list) {
00657         cur->timer--;
00658         if (!cur->timer) {
00659             //Send a packet
00660             thread_discovery_response_send(class, cur);
00661             ns_list_remove(&class->srv_respose_msg_list, cur);
00662             ns_list_add_to_start(&class->srv_respose_msg_buffers, cur);
00663         } else {
00664             pending_list = true;
00665         }
00666     }
00667 
00668     return pending_list;
00669 
00670 }
00671 
00672 static thread_discovery_class_t *thread_discovery_class_get(int8_t interface_id)
00673 {
00674     ns_list_foreach(thread_discovery_class_t, cur_class, &thread_discovery_class_list) {
00675         if (cur_class->interface_id == interface_id) {
00676             return cur_class;
00677         }
00678     }
00679     return NULL;
00680 }
00681 
00682 static void thread_discovery_free_discovered_networks(thread_nwk_discovery_response_list_t *list)
00683 {
00684     ns_list_foreach_safe(discovery_response_list_t, cur, list) {
00685         ns_list_remove(list, cur);
00686         ns_dyn_mem_free(cur);
00687     }
00688 }
00689 
00690 static thread_discovery_class_t *thread_discovery_class_allocate(bool reedDevice)
00691 {
00692     thread_discovery_class_t *class_ptr = ns_dyn_mem_alloc(sizeof(thread_discovery_class_t));
00693 
00694     if (class_ptr) {
00695         ns_list_init(&class_ptr->srv_respose_msg_list);
00696         ns_list_init(&class_ptr->srv_respose_msg_buffers);
00697         ns_list_init(&class_ptr->discovered_network);
00698 
00699         if (reedDevice) {
00700             if (!thread_discovery_server_msg_buffer_allocate(class_ptr)) {
00701                 ns_dyn_mem_free(class_ptr);
00702                 return NULL;
00703             }
00704         } else {
00705             class_ptr->msg_buffers = NULL;
00706         }
00707         class_ptr->discovery_request = NULL;
00708         class_ptr->thread_announce_request = NULL;
00709         ns_list_add_to_start(&thread_discovery_class_list, class_ptr);
00710     }
00711     return class_ptr;
00712 }
00713 
00714 static void thread_discovery_class_free(thread_discovery_class_t *class_ptr)
00715 {
00716     if (!class_ptr) {
00717         return;
00718     }
00719     ns_list_remove(&thread_discovery_class_list, class_ptr);
00720 
00721     thread_discovery_server_msg_buffer_free(class_ptr);
00722     thread_discovery_free_discovered_networks(&class_ptr->discovered_network);
00723     thread_discovery_request_free(class_ptr);
00724     thread_announce_discovery_request_free(class_ptr);
00725     ns_dyn_mem_free(class_ptr);
00726 }
00727 
00728 bool thread_discovery_tlv_spesific_data_discover(const uint8_t *ptr, uint16_t length, const mescop_tlv_t *compare_tlv)
00729 {
00730     const uint8_t *p;
00731     if (!ptr || length < 2 || !compare_tlv || !compare_tlv->data || !compare_tlv->length) {
00732         return false;
00733     }
00734 
00735     p = ptr;
00736     while (p != NULL) {
00737         const uint8_t *tlv_data_ptr;
00738         uint16_t tlv_data_length;
00739         //tr_info("tlv_find first check");
00740         // check if we have enough length for normal length tlv
00741         if (p + 2 > ptr + length) {
00742             break;    //must have at least type and short length
00743         }
00744 
00745         if (p[1] == 0xff) {
00746             // Long length format
00747             if (p + 4 > ptr + length) {
00748                 break;    // check if enough length for long length
00749             }
00750 
00751             tlv_data_length = common_read_16_bit(&p[2]);
00752             tlv_data_ptr = p + 4;
00753         } else {
00754             tlv_data_length = p[1];
00755             tlv_data_ptr = p + 2;
00756         }
00757         //tr_info("tlv_find check: %d, type: %d", tlv_data_length, *p);
00758 
00759         // check if length of tlv is correct
00760         if (tlv_data_ptr + tlv_data_length > ptr + length) {
00761             break;    //length goes past the data block
00762         }
00763 
00764         if (*p == compare_tlv->type) {
00765             if (tlv_data_length == compare_tlv->length) {
00766                 if (memcmp(tlv_data_ptr, compare_tlv->data, tlv_data_length) == 0) {
00767                     return true;
00768                 }
00769             }
00770         }
00771 
00772         p = tlv_data_ptr + tlv_data_length;
00773     }
00774     return false;
00775 }
00776 
00777 static bool thread_discovery_request_filter_validate(link_configuration_s *linkConfiguration, uint8_t *data_ptr, uint16_t length)
00778 {
00779     //Validate PAN-ID
00780     uint8_t pan_id[2];
00781 
00782     mescop_tlv_t compare_tlv;
00783     //Validate PAN-id, ExtentedPAN-id & Network name
00784 
00785     compare_tlv.data = pan_id;
00786     compare_tlv.length = 2;
00787     compare_tlv.type = MESHCOP_TLV_PANID;
00788 
00789     common_write_16_bit(linkConfiguration->panId, compare_tlv.data);
00790 
00791     if (thread_discovery_tlv_spesific_data_discover(data_ptr, length, &compare_tlv) ) {
00792         return false;
00793     }
00794 
00795     compare_tlv.data = linkConfiguration->extented_pan_id;
00796     compare_tlv.length = 8;
00797     compare_tlv.type = MESHCOP_TLV_XPANID;
00798 
00799     if (thread_discovery_tlv_spesific_data_discover(data_ptr, length, &compare_tlv) ) {
00800         return false;
00801     }
00802 
00803     return true;
00804 
00805 }
00806 
00807 static void thread_discovery_request_msg_handler(thread_discovery_class_t * discovery_class, mle_message_t *mle_msg)
00808 {
00809     //Validate that server is enabled
00810     if (!discovery_class->discovery_server_active ) {
00811         return;
00812     }
00813 
00814     link_configuration_s *linkConfiguration = thread_joiner_application_get_config(discovery_class->interface_id);
00815     if (!linkConfiguration) {
00816         return;
00817     }
00818     tr_debug("Thread discovery request message RX");
00819 
00820     // Check if we have room for new neighbor
00821     if (mle_class_free_entry_count_get(discovery_class->interface) < 1) {
00822         tr_debug("MLE table full, skip request");
00823         return;
00824     }
00825 
00826     //validate message
00827     mle_tlv_info_t discovery_tlv;
00828     //Parse Message
00829     uint16_t discover_req_tlv;
00830 
00831     //Discover MLE_TYPE_DISCOVERY
00832     if (mle_tlv_option_discover(mle_msg->data_ptr, mle_msg->data_length, MLE_TYPE_DISCOVERY, &discovery_tlv) < 4) {
00833         return;
00834     }
00835 
00836     if (thread_meshcop_tlv_data_get_uint16(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_DISCOVERY_REQUEST, &discover_req_tlv) < 2) {
00837         tr_debug("discover response not include all mandatory TLV's");
00838         return;
00839     }
00840     //Validate Version
00841     uint8_t version = (uint8_t)(discover_req_tlv >> 12);
00842     if (discovery_class->version < version) {
00843         tr_debug("Dropped by version %u != %u", discovery_class->version, version);
00844         return;
00845     }
00846 
00847     bool joiner_flag = (discover_req_tlv >> 11) & 1;
00848 
00849     if (joiner_flag) {
00850         //Can we respond
00851         thread_management_server_data_t joiner_router_info;
00852         if (0 != thread_management_server_commisoner_data_get(discovery_class->interface_id , &joiner_router_info) ||
00853                  !joiner_router_info.joiner_router_enabled) {
00854             if (!thread_extension_joining_enabled(discovery_class->interface_id)) {
00855                 tr_debug("Drop by Joining disabled");
00856                 return;
00857             }
00858         }
00859     }
00860 
00861     //Validate possible blacklist
00862     if (!thread_discovery_request_filter_validate(linkConfiguration, discovery_tlv.dataPtr, discovery_tlv.tlvLen)) {
00863         tr_debug("Dropped by filter");
00864         return;
00865     }
00866 
00867     //Trig response
00868     thread_discovery_response_trig(discovery_class, mle_msg->packet_src_address, mle_msg->src_pan_id);
00869 }
00870 
00871 static bool thread_seering_data_accept_any(uint8_t length, uint8_t *data)
00872 {
00873     if (length == 1 && *data == 0xff) {
00874         return true;
00875     }
00876     return false;
00877 }
00878 
00879 static void thread_discovery_nwk_push_to_list_by_lqi(thread_nwk_discovery_response_list_t *result_list, discovery_response_list_t *nwk_info) {
00880     if (ns_list_count(result_list)) {
00881         ns_list_foreach_safe(discovery_response_list_t, cur_entry, result_list) {
00882             if (nwk_info->dbm > cur_entry->dbm) {
00883 
00884                 ns_list_add_before(result_list, cur_entry, nwk_info);
00885                 return;
00886             }
00887         }
00888         ns_list_add_to_end(result_list, nwk_info);
00889     } else {
00890         ns_list_add_to_end(result_list, nwk_info);
00891     }
00892 }
00893 
00894 static void thread_discovery_joiner_set(thread_nwk_discovery_response_list_t *result_list, discovery_response_list_t *nwk_info, bool new_accept_any) {
00895     if (ns_list_count(result_list)) {
00896 
00897         bool cur_acept_any;
00898         ns_list_foreach_safe(discovery_response_list_t, cur_entry, result_list) {
00899 
00900             cur_acept_any = thread_seering_data_accept_any(cur_entry->steering_data_valid, cur_entry->steering_data);
00901 
00902             if (!cur_acept_any && !new_accept_any) {
00903                 if (nwk_info->dbm > cur_entry->dbm) {
00904                     ns_list_add_before(result_list, cur_entry, nwk_info);
00905                     return;
00906                 }
00907             } else {
00908                 if (!new_accept_any) {
00909                     //add this before cur
00910                     ns_list_add_before(result_list, cur_entry, nwk_info);
00911                     return;
00912                 } else if (nwk_info->dbm > cur_entry->dbm) {
00913                     ns_list_add_before(result_list, cur_entry, nwk_info);
00914                     return;
00915                 }
00916             }
00917         }
00918         ns_list_add_to_end(result_list, nwk_info);
00919     } else {
00920         ns_list_add_to_end(result_list, nwk_info);
00921     }
00922 }
00923 
00924 
00925 
00926 static void thread_discovery_response_msg_handler(thread_discovery_class_t * discovery_class, mle_message_t *mle_msg)
00927 {
00928     if (!discovery_class->discovery_request || !discovery_class->discovery_request->waiting_response) {
00929         return;
00930     }
00931 
00932     mle_tlv_info_t discovery_tlv;
00933     //Parse Message
00934     uint16_t discover_response_tlv;
00935     uint8_t *nwk_name, *extented_panid;
00936     uint8_t nwk_name_length;
00937 
00938     //Discover MLE_TYPE_DISCOVERY
00939     if (mle_tlv_option_discover(mle_msg->data_ptr, mle_msg->data_length, MLE_TYPE_DISCOVERY, &discovery_tlv) < 4) {
00940         return;
00941     }
00942 
00943     nwk_name_length = thread_meshcop_tlv_find(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_NETWORK_NAME, &nwk_name);
00944 
00945     if (thread_meshcop_tlv_data_get_uint16(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_DISCOVERY_RESPONSE, &discover_response_tlv) < 2
00946         || thread_meshcop_tlv_find(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_XPANID, &extented_panid) < 8
00947         || nwk_name_length > 16) {
00948 
00949         tr_debug("discover response not include all mandatory TLV's");
00950         return;
00951     }
00952 
00953     tr_debug("Thread discovery response message RX");
00954 
00955     uint8_t version = (uint8_t)(discover_response_tlv >> 12);
00956     if (discovery_class->version > version) {
00957         tr_debug("Dropped by version %u != %u", discovery_class->version, version);
00958         return;
00959     }
00960 
00961     if (discovery_class->discovery_request->native_commisioner_scan) {
00962         bool native_commioner_bit = (discover_response_tlv >> 11) & 1;
00963         if (!native_commioner_bit) {
00964             tr_debug("Native commisioner not supported");
00965             return;
00966         }
00967     }
00968     uint16_t pan_id = mle_msg->src_pan_id;
00969     uint8_t *steering_data;
00970     uint16_t joiner_port;
00971     bool joiner_port_valid;
00972 
00973     uint8_t steerin_data_length = thread_meshcop_tlv_find(discovery_tlv.dataPtr,  discovery_tlv.tlvLen, MESHCOP_TLV_STEERING_DATA, &steering_data);
00974     if (steerin_data_length > 16) {
00975         steerin_data_length = 0;
00976     }
00977 
00978     if (thread_meshcop_tlv_data_get_uint16(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_JOINER_UDP_PORT, &joiner_port) >= 2) {
00979         joiner_port_valid = true;
00980     } else {
00981         joiner_port_valid = false;
00982     }
00983 
00984     if (discovery_class->discovery_request->joiner_flag && (!joiner_port_valid || steerin_data_length == 0)) {
00985         if (thread_extension_version_check(discovery_class->interface->thread_info->version)) {
00986             if (!discovery_class->interface->thread_info->extension_credentials_ptr) {
00987                 tr_debug("Dropped, no joiner info");
00988             }
00989         } else {
00990             tr_debug("Dropped by no valid joiner info %u %u",joiner_port_valid, steerin_data_length);
00991             return;
00992         }
00993     }
00994 
00995     discovery_response_list_t *nwk_info = thread_discover_response_msg_get_discover_from_list(
00996                     &discovery_class->discovered_network,
00997                     discovery_class->discovery_request->active_channel, pan_id);
00998     if (nwk_info) {
00999         if (nwk_info->dbm < mle_msg->dbm) {
01000             goto save_optional_data;
01001         }
01002         return;
01003     }
01004 
01005     nwk_info = thread_discover_response_msg_allocate();
01006     if (!nwk_info) {
01007         return;
01008     }
01009     //Set parameters
01010     nwk_info->version = (discover_response_tlv >> 12);
01011     nwk_info->dbm = mle_msg->dbm;
01012     nwk_info->channel = discovery_class->discovery_request->active_channel;
01013 
01014     nwk_info->pan_id = pan_id;
01015     memcpy(nwk_info->extented_pan_id, extented_panid, 8);
01016 
01017     memset(nwk_info->network_name, 0, 16);
01018     memcpy(nwk_info->network_name, nwk_name, nwk_name_length);
01019 
01020     thread_meshcop_tlv_data_get_uint16(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_COMMISSIONER_UDP_PORT, &nwk_info->commissioner_port);
01021 
01022     thread_extension_discover_response_read(nwk_info, discover_response_tlv, discovery_tlv.dataPtr, discovery_tlv.tlvLen);
01023 
01024     //Add to last
01025     if (discovery_class->discovery_request->native_commisioner_scan) {
01026         thread_discovery_nwk_push_to_list_by_lqi(&discovery_class->discovered_network, nwk_info);
01027     } else {
01028         //Validate is steering data
01029         thread_discovery_joiner_set(&discovery_class->discovered_network, nwk_info, thread_seering_data_accept_any(nwk_info->steering_data_valid, nwk_info->steering_data));
01030     }
01031 
01032 
01033 save_optional_data:
01034     memcpy(nwk_info->extented_mac, mle_msg->packet_src_address + 8, 8);
01035     nwk_info->extented_mac[0] ^= 2;
01036     if (steerin_data_length) {
01037         memcpy(nwk_info->steering_data,steering_data, steerin_data_length);
01038         nwk_info->steering_data_valid = steerin_data_length;
01039     }
01040 
01041     if (joiner_port_valid) {
01042         nwk_info->joiner_port = joiner_port;
01043     }
01044 
01045 }
01046 
01047 static void thread_announce_discovery_message_receiver_cb(int8_t interface_id, mle_message_t *mle_msg)
01048 {
01049     if (mle_msg->message_type != MLE_COMMAND_DATASET_ANNOUNCE) {
01050         return;
01051     }
01052     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01053     if (!discovery_class || !discovery_class->thread_announce_request) {
01054         return;
01055     }
01056 
01057     tr_debug("MLE ANNOUNCE RX");
01058     uint64_t timestamp;
01059     uint16_t panid;
01060     uint8_t *ptr;
01061     uint16_t channel;
01062 
01063     tr_debug("Host Recv Dataset Announce %s", trace_ipv6(mle_msg->packet_src_address));
01064     if (8 > thread_tmfcop_tlv_data_get_uint64(mle_msg->data_ptr, mle_msg->data_length,MLE_TYPE_ACTIVE_TIMESTAMP,&timestamp)) {
01065         tr_error("Missing timestamp TLV");
01066         return;
01067     }
01068     if (2 > thread_tmfcop_tlv_data_get_uint16(mle_msg->data_ptr, mle_msg->data_length,MLE_TYPE_PANID,&panid)) {
01069         tr_error("Missing Panid TLV");
01070         return;
01071     }
01072     if (3 > thread_tmfcop_tlv_find(mle_msg->data_ptr, mle_msg->data_length,MLE_TYPE_CHANNEL,&ptr)) {
01073         tr_error("Missing Channel TLV");
01074         return;
01075     }
01076     channel = common_read_16_bit(&ptr[1]);
01077 
01078     if (timestamp <= discovery_class->thread_announce_request->active_time_stamp) {
01079         tr_debug("Drop by timestamp");
01080         return;
01081     }
01082 
01083     //Validate
01084     announce_discovery_response_t *response = NULL;
01085     if (discovery_class->thread_announce_request->network) {
01086         response = discovery_class->thread_announce_request->network;
01087         if (timestamp <= discovery_class->thread_announce_request->network->active_timestamp ) {
01088             response = NULL;
01089         }
01090     } else {
01091         discovery_class->thread_announce_request->network = ns_dyn_mem_temporary_alloc(sizeof(announce_discovery_response_t));
01092         response = discovery_class->thread_announce_request->network;
01093     }
01094 
01095     if (response) {
01096         tr_debug("Save data");
01097         response->active_timestamp = timestamp;
01098         response->channel = channel;
01099         response->pan_id = panid;
01100     }
01101 }
01102 
01103 
01104 static void thread_announce_discover_receive_cb(int8_t interface_id, mle_message_t *mle_msg, mle_security_header_t *security_headers)
01105 {
01106     //Verify security
01107     (void)security_headers;
01108 
01109     /* Check that message is from link-local scope */
01110     if(!addr_is_ipv6_link_local(mle_msg->packet_src_address)) {
01111         return;
01112     }
01113 
01114     thread_announce_discovery_message_receiver_cb(interface_id, mle_msg);
01115 }
01116 
01117 static void thread_discovery_message_receiver_cb(int8_t interface_id, mle_message_t *mle_msg)
01118 {
01119     //Discovery interface get
01120     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01121     if (!discovery_class) {
01122         return;
01123     }
01124 
01125     switch (mle_msg->message_type) {
01126         case MLE_COMMAND_DISCOVERY_REQUEST:
01127             //call message handler
01128             thread_discovery_request_msg_handler(discovery_class, mle_msg);
01129             break;
01130 
01131         case MLE_COMMAND_DISCOVERY_RESPONSE:
01132             //call message handler
01133             thread_discovery_response_msg_handler(discovery_class, mle_msg);
01134             break;
01135 
01136         default:
01137 
01138             break;
01139     }
01140 
01141 }
01142 
01143 static void thread_discover_event_handler(arm_event_s *event)
01144 {
01145     switch (event->event_type) {
01146         case THREAD_DISCOVER_INIT:
01147             thread_discover_timer_trig();
01148             break;
01149 
01150         case THREAD_DISCOVER_TIMER:
01151             //Do list in future for each of mle user
01152             thread_discover_timer_active = false;
01153             if (thread_discovery_timer_update()) {
01154                 //Request new timer
01155                 thread_discover_timer_trig();
01156             }
01157             break;
01158     }
01159 }
01160 
01161 static int8_t thread_discover_class_event_handler_init(void)
01162 {
01163     if (thread_discover_tasklet_id == -1) {
01164         //GENERATE TASKLET
01165         thread_discover_tasklet_id = eventOS_event_handler_create(&thread_discover_event_handler, THREAD_DISCOVER_INIT);
01166     }
01167     return thread_discover_tasklet_id;
01168 }
01169 
01170 
01171 int thread_discovery_init(int8_t interface_id, struct protocol_interface_info_entry *cur_interface, uint8_t version, bool reedDevice)
01172 {
01173     if (!cur_interface) {
01174         return -2;
01175     }
01176 
01177     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01178     if (discovery_class) {
01179         //Verify reed boolean
01180 
01181         thread_discovery_request_free(discovery_class);
01182         thread_announce_discovery_request_free(discovery_class);
01183         thread_discovery_server_msg_buffer_free(discovery_class);
01184         if (reedDevice) {
01185             if (!thread_discovery_server_msg_buffer_allocate(discovery_class)) {
01186                 thread_discovery_class_free(discovery_class);
01187                 return -1;
01188             }
01189         }
01190 
01191         goto return_ok;
01192     }
01193 
01194     if (thread_discover_class_event_handler_init() < 0) {
01195         return -1;
01196     }
01197 
01198     //Allocate new entry
01199     discovery_class = thread_discovery_class_allocate(reedDevice);
01200     if (!discovery_class ) {
01201         mle_service_interface_receiver_bypass_handler_update(interface_id,  NULL);
01202         return -1;
01203     }
01204     discovery_class->interface_id = interface_id;
01205 
01206 return_ok:
01207     discovery_class->discovery_server_active = false;
01208     discovery_class->interface = cur_interface;
01209     discovery_class->version = version;
01210     return 0;
01211 }
01212 
01213 /**
01214  * Reset discovery class state to idle
01215  */
01216 int thread_discovery_reset(int8_t interface_id)
01217 {
01218     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01219     if (!discovery_class ) {
01220         return -1;
01221     }
01222     thread_discovery_request_free(discovery_class);
01223     thread_announce_discovery_request_free(discovery_class);
01224     thread_discovery_server_msg_clean(discovery_class);
01225     return 0;
01226 }
01227 
01228 /**
01229  * Update discovery timer state's
01230  */
01231 static bool thread_discovery_timer_update(void)
01232 {
01233     bool keep_timer_active = false;
01234     ns_list_foreach(thread_discovery_class_t, cur_class, &thread_discovery_class_list) {
01235         if (cur_class->discovery_server_active) {
01236             if (thread_discovery_response_jitter_timer_update(cur_class)) {
01237                 keep_timer_active = true;
01238             }
01239         } else if (cur_class->discovery_request) {
01240             if (thread_discovery_request_timer_update(cur_class) ) {
01241                 keep_timer_active = true;
01242             }
01243         } else if(cur_class->thread_announce_request) {
01244             if ( thread_announce_discovery_request_timer_update(cur_class) ) {
01245                 keep_timer_active = true;
01246             }
01247         }
01248     }
01249     return keep_timer_active;
01250 }
01251 
01252 /**
01253  * Enable thread discovery request response support
01254  */
01255 int thread_discovery_responser_enable(int8_t interface_id, bool enable_service)
01256 {
01257     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01258     if (!discovery_class || !discovery_class->msg_buffers) {
01259         return -1;
01260     }
01261 
01262 
01263     //Clean server message list always
01264     thread_discovery_server_msg_clean(discovery_class);
01265     if (mle_service_interface_receiver_bypass_handler_update(interface_id,  thread_discovery_message_receiver_cb) != 0) {
01266         return -1;
01267     }
01268 
01269     discovery_class->discovery_server_active = enable_service;
01270 
01271     return 0;
01272 }
01273 
01274 static void thread_discovery_normal_receive_cb(int8_t interface_id, mle_message_t *mle_msg, mle_security_header_t *security_headers)
01275 {
01276     if (security_headers->securityLevel == 0) {
01277         thread_discovery_message_receiver_cb(interface_id, mle_msg);
01278     }
01279 }
01280 
01281 /**
01282  * Start Thread network discovery
01283  */
01284 int thread_discovery_network_scan(struct protocol_interface_info_entry *cur_interface, thread_discover_reques_t *scan_request, thread_discovery_ready_cb *ready_cb)
01285 {
01286 
01287     thread_discovery_class_t * discovery_class = thread_discovery_class_get(cur_interface->id);
01288     if (!discovery_class || !ready_cb || !scan_request) {
01289         return -1;
01290     }
01291 
01292     //Check Is discovery started already
01293     if (discovery_class->discovery_request) {
01294         return -2;
01295     }
01296 
01297     if (!scan_request->channel_mask || (scan_request->filter_tlv_length > 0 && !scan_request->filter_tlv_data)) {
01298         return -1;
01299     }
01300 
01301     discovery_class->discovery_request = thread_discovery_request_allocate(scan_request, ready_cb);
01302 
01303     if (!discovery_class->discovery_request) {
01304         return -3;
01305     }
01306 
01307     if (mle_service_interface_register(cur_interface->id, cur_interface, thread_discovery_normal_receive_cb, discovery_class->discovery_request->temporary_mac64,8) != 0) {
01308         thread_discovery_request_free(discovery_class);
01309         return -1;
01310     }
01311 
01312     if (mle_service_interface_receiver_bypass_handler_update(cur_interface->id, thread_discovery_message_receiver_cb) != 0) {
01313         thread_discovery_request_free(discovery_class);
01314         return -1;
01315     }
01316 
01317     //Free old networks
01318     thread_discovery_free_discovered_networks(&discovery_class->discovered_network);
01319     //Set temporary mac and generate ll64
01320     thread_discovery_prepare(discovery_class->interface, discovery_class->discovery_request);
01321     return 0;
01322 }
01323 
01324 int thread_discovery_announce_network_scan(int8_t interface_id, thread_announce_discover_reques_t *scan_request, thread_announce_scan_ready_cb *ready_cb)
01325 {
01326     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01327     if (!discovery_class || !ready_cb || !scan_request) {
01328         return -1;
01329     }
01330 
01331         //Check Is discovery started already
01332         if (discovery_class->discovery_request || discovery_class->thread_announce_request) {
01333             return -2;
01334         }
01335 
01336         if (!scan_request->channel_mask) {
01337             return -1;
01338         }
01339 
01340         discovery_class->thread_announce_request = thread_announce_discovery_request_allocate(scan_request, ready_cb);
01341 
01342         if (!discovery_class->thread_announce_request) {
01343             return -3;
01344         }
01345 
01346         //Update receiver callback
01347         if (mle_service_interface_receiver_handler_update(interface_id, thread_announce_discover_receive_cb) != 0) {
01348             thread_announce_discovery_request_free(discovery_class);
01349             return -1;
01350         }
01351 
01352         if (mle_service_interface_receiver_bypass_handler_update(interface_id,  thread_announce_discovery_message_receiver_cb) != 0) {
01353             thread_discovery_request_free(discovery_class);
01354             return -1;
01355         }
01356 
01357         //Set temporary mac and generate ll64
01358         thread_announce_discovery_prepare(discovery_class->interface, discovery_class->thread_announce_request);
01359         return 0;
01360 }
01361 
01362 discovery_response_list_t *thread_discovery_network_description_get(int8_t interface_id)
01363 {
01364     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01365     if (!discovery_class ) {
01366         return NULL;
01367     }
01368 
01369     discovery_response_list_t *entry = ns_list_get_first(&discovery_class->discovered_network);
01370     if (entry) {
01371         ns_list_remove(&discovery_class->discovered_network, entry);
01372     }
01373 
01374     return entry;
01375 
01376 }
01377 
01378 #if 0
01379 /**
01380  * Free all allocated memory and free class
01381  * Not used API function, flagged away. This should be taken in use when refactoring ifdown - functionality.
01382  */
01383 int thread_discovery_free(int8_t interface_id)
01384 {
01385     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01386     if (!discovery_class ) {
01387         return -1;
01388     }
01389 
01390     thread_discovery_class_free(discovery_class);
01391 
01392 
01393     return 0;
01394 }
01395 #endif
01396 #endif