thread_discovery.c

00001 /*
00002  * Copyright (c) 2016-2017, Arm Limited and affiliates.
00003  * SPDX-License-Identifier: BSD-3-Clause
00004  *
00005  * Redistribution and use in source and binary forms, with or without
00006  * modification, are permitted provided that the following conditions are met:
00007  *
00008  * 1. Redistributions of source code must retain the above copyright
00009  *    notice, this list of conditions and the following disclaimer.
00010  * 2. Redistributions in binary form must reproduce the above copyright
00011  *    notice, this list of conditions and the following disclaimer in the
00012  *    documentation and/or other materials provided with the distribution.
00013  * 3. Neither the name of the copyright holder nor the
00014  *    names of its contributors may be used to endorse or promote products
00015  *    derived from this software without specific prior written permission.
00016  *
00017  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
00018  * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
00019  * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE
00020  * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE
00021  * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR
00022  * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF
00023  * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS
00024  * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN
00025  * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE)
00026  * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00027  * POSSIBILITY OF SUCH DAMAGE.
00028  */
00029 
00030 #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     //validate message
00821     mle_tlv_info_t discovery_tlv;
00822     //Parse Message
00823     uint16_t discover_req_tlv;
00824 
00825     //Discover MLE_TYPE_DISCOVERY
00826     if (mle_tlv_option_discover(mle_msg->data_ptr, mle_msg->data_length, MLE_TYPE_DISCOVERY, &discovery_tlv) < 4) {
00827         return;
00828     }
00829 
00830     if (thread_meshcop_tlv_data_get_uint16(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_DISCOVERY_REQUEST, &discover_req_tlv) < 2) {
00831         tr_debug("discover response not include all mandatory TLV's");
00832         return;
00833     }
00834     //Validate Version
00835     uint8_t version = (uint8_t)(discover_req_tlv >> 12);
00836     if (discovery_class->version < version) {
00837         tr_debug("Dropped by version %u != %u", discovery_class->version, version);
00838         return;
00839     }
00840 
00841     bool joiner_flag = (discover_req_tlv >> 11) & 1;
00842 
00843     if (joiner_flag) {
00844         //Can we respond
00845         thread_management_server_data_t joiner_router_info;
00846         if (0 != thread_management_server_commisoner_data_get(discovery_class->interface_id , &joiner_router_info) ||
00847                  !joiner_router_info.joiner_router_enabled) {
00848             if (!thread_extension_joining_enabled(discovery_class->interface_id)) {
00849                 tr_debug("Drop by Joining disabled");
00850                 return;
00851             }
00852         }
00853     }
00854 
00855     //Validate possible blacklist
00856     if (!thread_discovery_request_filter_validate(linkConfiguration, discovery_tlv.dataPtr, discovery_tlv.tlvLen)) {
00857         tr_debug("Dropped by filter");
00858         return;
00859     }
00860 
00861     //Trig response
00862     thread_discovery_response_trig(discovery_class, mle_msg->packet_src_address, mle_msg->src_pan_id);
00863 }
00864 
00865 static bool thread_seering_data_accept_any(uint8_t length, uint8_t *data)
00866 {
00867     if (length == 1 && *data == 0xff) {
00868         return true;
00869     }
00870     return false;
00871 }
00872 
00873 static void thread_discovery_nwk_push_to_list_by_lqi(thread_nwk_discovery_response_list_t *result_list, discovery_response_list_t *nwk_info) {
00874     if (ns_list_count(result_list)) {
00875         ns_list_foreach_safe(discovery_response_list_t, cur_entry, result_list) {
00876             if (nwk_info->dbm > cur_entry->dbm) {
00877 
00878                 ns_list_add_before(result_list, cur_entry, nwk_info);
00879                 return;
00880             }
00881         }
00882         ns_list_add_to_end(result_list, nwk_info);
00883     } else {
00884         ns_list_add_to_end(result_list, nwk_info);
00885     }
00886 }
00887 
00888 static void thread_discovery_joiner_set(thread_nwk_discovery_response_list_t *result_list, discovery_response_list_t *nwk_info, bool new_accept_any) {
00889     if (ns_list_count(result_list)) {
00890 
00891         bool cur_acept_any;
00892         ns_list_foreach_safe(discovery_response_list_t, cur_entry, result_list) {
00893 
00894             cur_acept_any = thread_seering_data_accept_any(cur_entry->steering_data_valid, cur_entry->steering_data);
00895 
00896             if (!cur_acept_any && !new_accept_any) {
00897                 if (nwk_info->dbm > cur_entry->dbm) {
00898                     ns_list_add_before(result_list, cur_entry, nwk_info);
00899                     return;
00900                 }
00901             } else {
00902                 if (!new_accept_any) {
00903                     //add this before cur
00904                     ns_list_add_before(result_list, cur_entry, nwk_info);
00905                     return;
00906                 } else if (nwk_info->dbm > cur_entry->dbm) {
00907                     ns_list_add_before(result_list, cur_entry, nwk_info);
00908                     return;
00909                 }
00910             }
00911         }
00912         ns_list_add_to_end(result_list, nwk_info);
00913     } else {
00914         ns_list_add_to_end(result_list, nwk_info);
00915     }
00916 }
00917 
00918 
00919 
00920 static void thread_discovery_response_msg_handler(thread_discovery_class_t * discovery_class, mle_message_t *mle_msg)
00921 {
00922     if (!discovery_class->discovery_request || !discovery_class->discovery_request->waiting_response) {
00923         return;
00924     }
00925 
00926     mle_tlv_info_t discovery_tlv;
00927     //Parse Message
00928     uint16_t discover_response_tlv;
00929     uint8_t *nwk_name, *extented_panid;
00930     uint8_t nwk_name_length;
00931 
00932     //Discover MLE_TYPE_DISCOVERY
00933     if (mle_tlv_option_discover(mle_msg->data_ptr, mle_msg->data_length, MLE_TYPE_DISCOVERY, &discovery_tlv) < 4) {
00934         return;
00935     }
00936 
00937     nwk_name_length = thread_meshcop_tlv_find(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_NETWORK_NAME, &nwk_name);
00938 
00939     if (thread_meshcop_tlv_data_get_uint16(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_DISCOVERY_RESPONSE, &discover_response_tlv) < 2
00940         || thread_meshcop_tlv_find(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_XPANID, &extented_panid) < 8
00941         || nwk_name_length > 16) {
00942 
00943         tr_debug("discover response not include all mandatory TLV's");
00944         return;
00945     }
00946 
00947     tr_debug("Thread discovery response message RX");
00948 
00949     uint8_t version = (uint8_t)(discover_response_tlv >> 12);
00950     if (discovery_class->version > version) {
00951         tr_debug("Dropped by version %u != %u", discovery_class->version, version);
00952         return;
00953     }
00954 
00955     if (discovery_class->discovery_request->native_commisioner_scan) {
00956         bool native_commioner_bit = (discover_response_tlv >> 11) & 1;
00957         if (!native_commioner_bit) {
00958             tr_debug("Native commisioner not supported");
00959             return;
00960         }
00961     }
00962     uint16_t pan_id = mle_msg->src_pan_id;
00963     uint8_t *steering_data;
00964     uint16_t joiner_port;
00965     bool joiner_port_valid;
00966 
00967     uint8_t steerin_data_length = thread_meshcop_tlv_find(discovery_tlv.dataPtr,  discovery_tlv.tlvLen, MESHCOP_TLV_STEERING_DATA, &steering_data);
00968     if (steerin_data_length > 16) {
00969         steerin_data_length = 0;
00970     }
00971 
00972     if (thread_meshcop_tlv_data_get_uint16(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_JOINER_UDP_PORT, &joiner_port) >= 2) {
00973         joiner_port_valid = true;
00974     } else {
00975         joiner_port_valid = false;
00976     }
00977 
00978     if (discovery_class->discovery_request->joiner_flag && (!joiner_port_valid || steerin_data_length == 0)) {
00979         if (thread_extension_version_check(discovery_class->interface->thread_info->version)) {
00980             if (!discovery_class->interface->thread_info->extension_credentials_ptr) {
00981                 tr_debug("Dropped, no joiner info");
00982             }
00983         } else {
00984             tr_debug("Dropped by no valid joiner info %u %u",joiner_port_valid, steerin_data_length);
00985             return;
00986         }
00987     }
00988 
00989     discovery_response_list_t *nwk_info = thread_discover_response_msg_get_discover_from_list(
00990                     &discovery_class->discovered_network,
00991                     discovery_class->discovery_request->active_channel, pan_id);
00992     if (nwk_info) {
00993         if (nwk_info->dbm < mle_msg->dbm) {
00994             goto save_optional_data;
00995         }
00996         return;
00997     }
00998 
00999     nwk_info = thread_discover_response_msg_allocate();
01000     if (!nwk_info) {
01001         return;
01002     }
01003     //Set parameters
01004     nwk_info->version = (discover_response_tlv >> 12);
01005     nwk_info->dbm = mle_msg->dbm;
01006     nwk_info->channel = discovery_class->discovery_request->active_channel;
01007 
01008     nwk_info->pan_id = pan_id;
01009     memcpy(nwk_info->extented_pan_id, extented_panid, 8);
01010 
01011     memset(nwk_info->network_name, 0, 16);
01012     memcpy(nwk_info->network_name, nwk_name, nwk_name_length);
01013 
01014     thread_meshcop_tlv_data_get_uint16(discovery_tlv.dataPtr, discovery_tlv.tlvLen, MESHCOP_TLV_COMMISSIONER_UDP_PORT, &nwk_info->commissioner_port);
01015 
01016     thread_extension_discover_response_read(nwk_info, discover_response_tlv, discovery_tlv.dataPtr, discovery_tlv.tlvLen);
01017 
01018     //Add to last
01019     if (discovery_class->discovery_request->native_commisioner_scan) {
01020         thread_discovery_nwk_push_to_list_by_lqi(&discovery_class->discovered_network, nwk_info);
01021     } else {
01022         //Validate is steering data
01023         thread_discovery_joiner_set(&discovery_class->discovered_network, nwk_info, thread_seering_data_accept_any(nwk_info->steering_data_valid, nwk_info->steering_data));
01024     }
01025 
01026 
01027 save_optional_data:
01028     memcpy(nwk_info->extented_mac, mle_msg->packet_src_address + 8, 8);
01029     nwk_info->extented_mac[0] ^= 2;
01030     if (steerin_data_length) {
01031         memcpy(nwk_info->steering_data,steering_data, steerin_data_length);
01032         nwk_info->steering_data_valid = steerin_data_length;
01033     }
01034 
01035     if (joiner_port_valid) {
01036         nwk_info->joiner_port = joiner_port;
01037     }
01038 
01039 }
01040 
01041 static void thread_announce_discovery_message_receiver_cb(int8_t interface_id, mle_message_t *mle_msg)
01042 {
01043     if (mle_msg->message_type != MLE_COMMAND_DATASET_ANNOUNCE) {
01044         return;
01045     }
01046     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01047     if (!discovery_class || !discovery_class->thread_announce_request) {
01048         return;
01049     }
01050 
01051     tr_debug("MLE ANNOUNCE RX");
01052     uint64_t timestamp;
01053     uint16_t panid;
01054     uint8_t *ptr;
01055     uint16_t channel;
01056 
01057     tr_debug("Host Recv Dataset Announce %s", trace_ipv6(mle_msg->packet_src_address));
01058     if (8 > thread_tmfcop_tlv_data_get_uint64(mle_msg->data_ptr, mle_msg->data_length,MLE_TYPE_ACTIVE_TIMESTAMP,&timestamp)) {
01059         tr_error("Missing timestamp TLV");
01060         return;
01061     }
01062     if (2 > thread_tmfcop_tlv_data_get_uint16(mle_msg->data_ptr, mle_msg->data_length,MLE_TYPE_PANID,&panid)) {
01063         tr_error("Missing Panid TLV");
01064         return;
01065     }
01066     if (3 > thread_tmfcop_tlv_find(mle_msg->data_ptr, mle_msg->data_length,MLE_TYPE_CHANNEL,&ptr)) {
01067         tr_error("Missing Channel TLV");
01068         return;
01069     }
01070     channel = common_read_16_bit(&ptr[1]);
01071 
01072     if (timestamp <= discovery_class->thread_announce_request->active_time_stamp) {
01073         tr_debug("Drop by timestamp");
01074         return;
01075     }
01076 
01077     //Validate
01078     announce_discovery_response_t *response = NULL;
01079     if (discovery_class->thread_announce_request->network) {
01080         response = discovery_class->thread_announce_request->network;
01081         if (timestamp <= discovery_class->thread_announce_request->network->active_timestamp ) {
01082             response = NULL;
01083         }
01084     } else {
01085         discovery_class->thread_announce_request->network = ns_dyn_mem_temporary_alloc(sizeof(announce_discovery_response_t));
01086         response = discovery_class->thread_announce_request->network;
01087     }
01088 
01089     if (response) {
01090         tr_debug("Save data");
01091         response->active_timestamp = timestamp;
01092         response->channel = channel;
01093         response->pan_id = panid;
01094     }
01095 }
01096 
01097 
01098 static void thread_announce_discover_receive_cb(int8_t interface_id, mle_message_t *mle_msg, mle_security_header_t *security_headers)
01099 {
01100     //Verify security
01101     (void)security_headers;
01102 
01103     /* Check that message is from link-local scope */
01104     if(!addr_is_ipv6_link_local(mle_msg->packet_src_address)) {
01105         return;
01106     }
01107 
01108     thread_announce_discovery_message_receiver_cb(interface_id, mle_msg);
01109 }
01110 
01111 static void thread_discovery_message_receiver_cb(int8_t interface_id, mle_message_t *mle_msg)
01112 {
01113     //Discovery interface get
01114     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01115     if (!discovery_class) {
01116         return;
01117     }
01118 
01119     switch (mle_msg->message_type) {
01120         case MLE_COMMAND_DISCOVERY_REQUEST:
01121             //call message handler
01122             thread_discovery_request_msg_handler(discovery_class, mle_msg);
01123             break;
01124 
01125         case MLE_COMMAND_DISCOVERY_RESPONSE:
01126             //call message handler
01127             thread_discovery_response_msg_handler(discovery_class, mle_msg);
01128             break;
01129 
01130         default:
01131 
01132             break;
01133     }
01134 
01135 }
01136 
01137 static void thread_discover_event_handler(arm_event_s *event)
01138 {
01139     switch (event->event_type) {
01140         case THREAD_DISCOVER_INIT:
01141             thread_discover_timer_trig();
01142             break;
01143 
01144         case THREAD_DISCOVER_TIMER:
01145             //Do list in future for each of mle user
01146             thread_discover_timer_active = false;
01147             if (thread_discovery_timer_update()) {
01148                 //Request new timer
01149                 thread_discover_timer_trig();
01150             }
01151             break;
01152     }
01153 }
01154 
01155 static int8_t thread_discover_class_event_handler_init(void)
01156 {
01157     if (thread_discover_tasklet_id == -1) {
01158         //GENERATE TASKLET
01159         thread_discover_tasklet_id = eventOS_event_handler_create(&thread_discover_event_handler, THREAD_DISCOVER_INIT);
01160     }
01161     return thread_discover_tasklet_id;
01162 }
01163 
01164 
01165 int thread_discovery_init(int8_t interface_id, struct protocol_interface_info_entry *cur_interface, uint8_t version, bool reedDevice)
01166 {
01167     if (!cur_interface) {
01168         return -2;
01169     }
01170 
01171     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01172     if (discovery_class) {
01173         //Verify reed boolean
01174 
01175         thread_discovery_request_free(discovery_class);
01176         thread_announce_discovery_request_free(discovery_class);
01177         thread_discovery_server_msg_buffer_free(discovery_class);
01178         if (reedDevice) {
01179             if (!thread_discovery_server_msg_buffer_allocate(discovery_class)) {
01180                 thread_discovery_class_free(discovery_class);
01181                 return -1;
01182             }
01183         }
01184 
01185         goto return_ok;
01186     }
01187 
01188     if (thread_discover_class_event_handler_init() < 0) {
01189         return -1;
01190     }
01191 
01192     //Allocate new entry
01193     discovery_class = thread_discovery_class_allocate(reedDevice);
01194     if (!discovery_class ) {
01195         mle_service_interface_receiver_bypass_handler_update(interface_id,  NULL);
01196         return -1;
01197     }
01198     discovery_class->interface_id = interface_id;
01199 
01200 return_ok:
01201     discovery_class->discovery_server_active = false;
01202     discovery_class->interface = cur_interface;
01203     discovery_class->version = version;
01204     return 0;
01205 }
01206 
01207 /**
01208  * Reset discovery class state to idle
01209  */
01210 int thread_discovery_reset(int8_t interface_id)
01211 {
01212     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01213     if (!discovery_class ) {
01214         return -1;
01215     }
01216     thread_discovery_request_free(discovery_class);
01217     thread_announce_discovery_request_free(discovery_class);
01218     thread_discovery_server_msg_clean(discovery_class);
01219     return 0;
01220 }
01221 
01222 /**
01223  * Update discovery timer state's
01224  */
01225 static bool thread_discovery_timer_update(void)
01226 {
01227     bool keep_timer_active = false;
01228     ns_list_foreach(thread_discovery_class_t, cur_class, &thread_discovery_class_list) {
01229         if (cur_class->discovery_server_active) {
01230             if (thread_discovery_response_jitter_timer_update(cur_class)) {
01231                 keep_timer_active = true;
01232             }
01233         } else if (cur_class->discovery_request) {
01234             if (thread_discovery_request_timer_update(cur_class) ) {
01235                 keep_timer_active = true;
01236             }
01237         } else if(cur_class->thread_announce_request) {
01238             if ( thread_announce_discovery_request_timer_update(cur_class) ) {
01239                 keep_timer_active = true;
01240             }
01241         }
01242     }
01243     return keep_timer_active;
01244 }
01245 
01246 /**
01247  * Enable thread discovery request response support
01248  */
01249 int thread_discovery_responser_enable(int8_t interface_id, bool enable_service)
01250 {
01251     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01252     if (!discovery_class || !discovery_class->msg_buffers) {
01253         return -1;
01254     }
01255 
01256 
01257     //Clean server message list always
01258     thread_discovery_server_msg_clean(discovery_class);
01259     if (mle_service_interface_receiver_bypass_handler_update(interface_id,  thread_discovery_message_receiver_cb) != 0) {
01260         return -1;
01261     }
01262 
01263     discovery_class->discovery_server_active = enable_service;
01264 
01265     return 0;
01266 }
01267 
01268 static void thread_discovery_normal_receive_cb(int8_t interface_id, mle_message_t *mle_msg, mle_security_header_t *security_headers)
01269 {
01270     if (security_headers->securityLevel == 0) {
01271         thread_discovery_message_receiver_cb(interface_id, mle_msg);
01272     }
01273 }
01274 
01275 /**
01276  * Start Thread network discovery
01277  */
01278 int thread_discovery_network_scan(int8_t interface_id, thread_discover_reques_t *scan_request, thread_discovery_ready_cb *ready_cb)
01279 {
01280 
01281     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01282     if (!discovery_class || !ready_cb || !scan_request) {
01283         return -1;
01284     }
01285 
01286     //Check Is discovery started already
01287     if (discovery_class->discovery_request) {
01288         return -2;
01289     }
01290 
01291     if (!scan_request->channel_mask || (scan_request->filter_tlv_length > 0 && !scan_request->filter_tlv_data)) {
01292         return -1;
01293     }
01294 
01295     discovery_class->discovery_request = thread_discovery_request_allocate(scan_request, ready_cb);
01296 
01297     if (!discovery_class->discovery_request) {
01298         return -3;
01299     }
01300 
01301     if (mle_service_interface_register(interface_id, thread_discovery_normal_receive_cb, discovery_class->discovery_request->temporary_mac64,8) != 0) {
01302         thread_discovery_request_free(discovery_class);
01303         return -1;
01304     }
01305 
01306     if (mle_service_interface_receiver_bypass_handler_update(interface_id, thread_discovery_message_receiver_cb) != 0) {
01307         thread_discovery_request_free(discovery_class);
01308         return -1;
01309     }
01310 
01311     //Free old networks
01312     thread_discovery_free_discovered_networks(&discovery_class->discovered_network);
01313     //Set temporary mac and generate ll64
01314     thread_discovery_prepare(discovery_class->interface, discovery_class->discovery_request);
01315     return 0;
01316 }
01317 
01318 int thread_discovery_announce_network_scan(int8_t interface_id, thread_announce_discover_reques_t *scan_request, thread_announce_scan_ready_cb *ready_cb)
01319 {
01320     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01321     if (!discovery_class || !ready_cb || !scan_request) {
01322         return -1;
01323     }
01324 
01325         //Check Is discovery started already
01326         if (discovery_class->discovery_request || discovery_class->thread_announce_request) {
01327             return -2;
01328         }
01329 
01330         if (!scan_request->channel_mask) {
01331             return -1;
01332         }
01333 
01334         discovery_class->thread_announce_request = thread_announce_discovery_request_allocate(scan_request, ready_cb);
01335 
01336         if (!discovery_class->thread_announce_request) {
01337             return -3;
01338         }
01339 
01340         //Update receiver callback
01341         if (mle_service_interface_receiver_handler_update(interface_id, thread_announce_discover_receive_cb) != 0) {
01342             thread_announce_discovery_request_free(discovery_class);
01343             return -1;
01344         }
01345 
01346         if (mle_service_interface_receiver_bypass_handler_update(interface_id,  thread_announce_discovery_message_receiver_cb) != 0) {
01347             thread_discovery_request_free(discovery_class);
01348             return -1;
01349         }
01350 
01351         //Set temporary mac and generate ll64
01352         thread_announce_discovery_prepare(discovery_class->interface, discovery_class->thread_announce_request);
01353         return 0;
01354 }
01355 
01356 discovery_response_list_t *thread_discovery_network_description_get(int8_t interface_id)
01357 {
01358     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01359     if (!discovery_class ) {
01360         return NULL;
01361     }
01362 
01363     discovery_response_list_t *entry = ns_list_get_first(&discovery_class->discovered_network);
01364     if (entry) {
01365         ns_list_remove(&discovery_class->discovered_network, entry);
01366     }
01367 
01368     return entry;
01369 
01370 }
01371 
01372 #if 0
01373 /**
01374  * Free all allocated memory and free class
01375  * Not used API function, flagged away. This should be taken in use when refactoring ifdown - functionality.
01376  */
01377 int thread_discovery_free(int8_t interface_id)
01378 {
01379     thread_discovery_class_t * discovery_class = thread_discovery_class_get(interface_id);
01380     if (!discovery_class ) {
01381         return -1;
01382     }
01383 
01384     thread_discovery_class_free(discovery_class);
01385 
01386 
01387     return 0;
01388 }
01389 #endif
01390 #endif