thread_lowpower_api.c

00001 /*
00002  * Copyright (c) 2017-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 
00031 #include "nsconfig.h"
00032 #include <ns_types.h>
00033 #include <nsdynmemLIB.h>
00034 #include "thread_lowpower_api.h"
00035 #include <string.h>
00036 #include "ns_list.h"
00037 #include "ns_trace.h"
00038 #include "common_functions.h"
00039 
00040 
00041 #include "thread_common.h"
00042 #include "thread_config.h"
00043 #include "eventOS_event_timer.h"
00044 #include "MLE/mle.h"
00045 #include "NWK_INTERFACE/Include/protocol.h"
00046 #include "thread_lowpower_api.h"
00047 #include "6LoWPAN/MAC/mac_data_poll.h"
00048 #include "6LoWPAN/Thread/thread_bootstrap.h"
00049 #include "6LoWPAN/Thread/thread_management_client.h"
00050 #include "6LoWPAN/Thread/thread_tmfcop_lib.h"
00051 #include "thread_management_if.h"
00052 #include "6LoWPAN/Thread/thread_joiner_application.h"
00053 #include "6LoWPAN/Thread/thread_management_internal.h"
00054 #include "NWK_INTERFACE/Include/protocol.h"
00055 
00056 #ifdef HAVE_THREAD_V2
00057 
00058 #define TRACE_GROUP "lpwr"
00059 
00060 #define METRICS_LEN 18
00061 
00062 typedef struct data_response_s {
00063     thread_lowpower_resp_cb *data_response_cb;
00064     uint16_t request_wait_timer;
00065     uint16_t series_flags;
00066     uint8_t probe_count;
00067     uint8_t destination_address[16];
00068     int8_t interface_id;
00069     uint8_t metrics_requested[METRICS_LEN];
00070     uint8_t metrics_len;
00071     uint8_t series_id;
00072 } data_response_t;
00073 
00074 typedef struct data_metrics_s {
00075     uint8_t metrics_requested[METRICS_LEN];
00076     uint8_t metrics_value[METRICS_LEN];
00077     uint8_t req_metrics_len;
00078     uint8_t address[16];
00079     uint16_t timeout_timer;
00080     bool link_metrics_ready: 1;
00081     uint16_t query_ID;
00082     uint8_t forward_series_id;
00083     uint8_t forward_series_flags;
00084     ns_list_link_t link;
00085 } data_metrics_t;
00086 
00087 static NS_LIST_DEFINE(thread_lowpower_data_metrics_instance_list, data_metrics_t, link);
00088 
00089 static data_response_t data_response;
00090 data_response_t *data_response_ptr = &data_response;
00091 //Link Metrics Query TLV
00092 #define LINK_METRIC_SUB_TLV_LINK_METRIC_REPORT         0
00093 #define LINK_METRIC_SUB_TLV_LINK_METRIC_QUERY_ID       1
00094 #define LINK_METRIC_SUB_TLV_LINK_METRIC_QUERY_OPTIONS  2
00095 
00096 // Link Metrics Management TLV
00097 #define LINK_METRIC_SUB_TLV_FORWARD_PROBE              3
00098 #define LINK_METRIC_SUB_TLV_REVERSE_PROBE              4
00099 #define LINK_METRIC_SUB_TLV_STATUS                     5
00100 #define LINK_METRIC_SUB_TLV_TRACKING_CAPABILITIES      6
00101 #define LINK_METRIC_SUB_TLV_ENHANCED_ACK_CONFIGURATION 7
00102 
00103 
00104 static data_metrics_t *thread_lowpower_data_metrics_find_by_source_address(uint8_t *address)
00105 {
00106     data_metrics_t *this = NULL;
00107     ns_list_foreach(data_metrics_t, cur_ptr, &thread_lowpower_data_metrics_instance_list) {
00108         if (memcmp(cur_ptr->address, address, 16) == 0) {
00109             this = cur_ptr;
00110             break;
00111         }
00112     }
00113     return this;
00114 }
00115 
00116 static data_metrics_t *thread_lowpower_data_metrics_create(uint8_t *address)
00117 {
00118     data_metrics_t *this = thread_lowpower_data_metrics_find_by_source_address(address);
00119 
00120     if (!this) {
00121         this = ns_dyn_mem_alloc(sizeof(data_metrics_t));
00122     }
00123     if (!this) {
00124         return NULL;
00125     }
00126     memcpy(this->address, address, 16);
00127     memset(this->metrics_requested, 0, METRICS_LEN);
00128     memset(this->metrics_value, 0, METRICS_LEN);
00129     this->req_metrics_len = 0;
00130     this->timeout_timer = 0;
00131     this->link_metrics_ready = false;
00132     this->query_ID = 0;
00133     ns_list_add_to_start(&thread_lowpower_data_metrics_instance_list, this);
00134     return this;
00135 }
00136 
00137 void thread_lowpower_data_metrics_delete(uint8_t *address)
00138 {
00139     data_metrics_t *this = thread_lowpower_data_metrics_find_by_source_address(address);
00140     if (!this) {
00141         return;
00142     }
00143     ns_list_remove(&thread_lowpower_data_metrics_instance_list, this);
00144     ns_dyn_mem_free(this);
00145     return;
00146 }
00147 
00148 static uint8_t *thread_lowpower_tlv_write_link_metrics_query_id(uint8_t *ptr, uint16_t query_id)
00149 {
00150     *ptr++ = LINK_METRIC_SUB_TLV_LINK_METRIC_QUERY_ID;
00151     *ptr++ = 2;
00152     ptr = common_write_16_bit(query_id, ptr);
00153     return ptr;
00154 }
00155 static uint8_t *thread_lowpower_tlv_write_link_metrics_query_options(uint8_t *ptr, uint8_t *req_list_ptr, uint8_t req_list_len)
00156 {
00157     *ptr++ = LINK_METRIC_SUB_TLV_LINK_METRIC_QUERY_OPTIONS;
00158     *ptr++ = req_list_len;
00159     memcpy(ptr, req_list_ptr, req_list_len);
00160     ptr += req_list_len;
00161     return ptr;
00162 }
00163 static uint8_t *thread_lowpower_tlv_write_link_metrics_query(uint8_t *ptr, uint8_t *req_list_ptr, uint8_t req_list_len, uint16_t query_id)
00164 {
00165     *ptr++ = MLE_TYPE_LINK_METRICS_QUERY;
00166     if (req_list_len) {
00167         *ptr++ = 6 + req_list_len; // query id type + len + query id = 4 bytes, query options type + len + flags = 2 bytes + flags
00168     } else {
00169         *ptr++ = 4; // query id type + len + query id = 4 bytes
00170     }
00171     ptr = thread_lowpower_tlv_write_link_metrics_query_id(ptr, query_id);
00172     if (req_list_len) {
00173         ptr = thread_lowpower_tlv_write_link_metrics_query_options(ptr, req_list_ptr, req_list_len);
00174     }
00175     return ptr;
00176 }
00177 static uint8_t *thread_lowpower_tlv_write_metrics_forward_probe(uint8_t *ptr, uint8_t series_id, uint8_t series_flags, uint16_t timeout, uint8_t *req_list_ptr, uint8_t req_list_len)
00178 {
00179     *ptr++ = LINK_METRIC_SUB_TLV_FORWARD_PROBE;
00180     *ptr++ = 4 + req_list_len; // query id type + len + query id = 4 bytes, query options type + len + flags = 2 bytes + flags
00181     *ptr++ = series_id;
00182     *ptr++ = series_flags;
00183     ptr = common_write_16_bit(timeout, ptr);
00184     memcpy(ptr, req_list_ptr, req_list_len);
00185     ptr += req_list_len;
00186     return ptr;
00187 }
00188 static uint8_t *thread_lowpower_tlv_write_status(uint8_t *ptr, uint16_t status)
00189 {
00190     *ptr++ = LINK_METRIC_SUB_TLV_STATUS;
00191     *ptr++ = 2;
00192     ptr = common_write_16_bit(status, ptr);
00193     return ptr;
00194 }
00195 static uint8_t *thread_lowpower_tlv_write_metrics_management_forward_probe_request(uint8_t *ptr, uint8_t series_id, uint8_t series_flags, uint16_t timeout, uint8_t *req_list_ptr, uint8_t req_list_len)
00196 {
00197     *ptr++ = MLE_TYPE_LINK_METRICS_MANAGEMENT;
00198     *ptr++ =  6 + req_list_len; // query id type + len + query id = 4 bytes, query options type + len + flags = 2 bytes + flags
00199     ptr = thread_lowpower_tlv_write_metrics_forward_probe(ptr, series_id, series_flags, timeout, req_list_ptr, req_list_len);
00200     return ptr;
00201 }
00202 static uint8_t *thread_lowpower_tlv_write_metrics_management_status(uint8_t *ptr, uint16_t status)
00203 {
00204     *ptr++ = MLE_TYPE_LINK_METRICS_MANAGEMENT;
00205     *ptr++ =  4;
00206     ptr = thread_lowpower_tlv_write_status(ptr, status);
00207     return ptr;
00208 }
00209 static int thread_lowpower_requested_metrics_management_query_save(uint8_t *address, uint8_t *data_ptr, uint16_t data_len)
00210 {
00211     mle_tlv_info_t query_info;
00212     uint8_t *forward_probe_ptr;
00213     uint8_t forward_probe_len;
00214     data_metrics_t *this;
00215 
00216     if (mle_tlv_option_discover(data_ptr, data_len, MLE_TYPE_LINK_METRICS_MANAGEMENT, &query_info) < 4) {
00217         // No query TLV present
00218         return 0;
00219     }
00220     forward_probe_len = thread_tmfcop_tlv_find(query_info.dataPtr, query_info.tlvLen, LINK_METRIC_SUB_TLV_FORWARD_PROBE, &forward_probe_ptr);
00221 
00222     if (forward_probe_len < 5) {
00223         // Not present or length not enough
00224         return 0;
00225     }
00226 
00227     this = thread_lowpower_data_metrics_create(address);
00228 
00229     if (!this) {
00230         //query aready exists
00231         tr_error("query not created");
00232         return -3;
00233     }
00234     tr_debug("saving link metrics requested");
00235 
00236     this->link_metrics_ready = false;
00237     memcpy(this->address, address, 16);
00238     this->forward_series_id = *forward_probe_ptr++;
00239     this->forward_series_flags = *forward_probe_ptr++;
00240     this->timeout_timer = common_read_16_bit(forward_probe_ptr);
00241     forward_probe_ptr += 2;
00242     this->req_metrics_len = forward_probe_len - 4; // after 4 bytes omes the query types
00243 
00244     // Check that we dont go over maximum query count
00245     if (this->req_metrics_len > METRICS_LEN) {
00246         this->req_metrics_len = METRICS_LEN;
00247     }
00248     for (uint8_t i = 0; i < this->req_metrics_len; i++) {
00249         //save the read bytes for computing the results
00250         this->metrics_requested[i] = *forward_probe_ptr++;
00251     }
00252     tr_info("Forward probe query made by %s id:%d timeout:%d", trace_ipv6(this->address), this->forward_series_id, this->timeout_timer);
00253     return 0;
00254 }
00255 static int thread_lowpower_requested_single_query_save(uint8_t *address, uint8_t *data_ptr, uint16_t data_len)
00256 {
00257     mle_tlv_info_t query_info;
00258     uint8_t *query_options_ptr;
00259     uint8_t query_options_len;
00260     data_metrics_t *this = NULL;
00261 
00262     if (!mle_tlv_type_requested(MLE_TYPE_LINK_METRICS_REPORT, data_ptr, data_len)) {
00263         //No single link metric requested so cant respond no need to process query
00264         return 0;
00265     }
00266     if (mle_tlv_option_discover(data_ptr, data_len, MLE_TYPE_LINK_METRICS_QUERY, &query_info) < 6) {
00267         return 0;
00268     }
00269     tr_debug("Query tlv found %s", trace_array(data_ptr, data_len));
00270 
00271     if (query_info.tlvLen < 6) {
00272         tr_warn("malformed query tlv"); // query tlv contains query id (length 4 bytes), query options - atleast 2 bytes
00273         return -2;
00274     }
00275 
00276     query_options_len = thread_tmfcop_tlv_find(query_info.dataPtr, query_info.tlvLen, LINK_METRIC_SUB_TLV_LINK_METRIC_QUERY_OPTIONS, &query_options_ptr);
00277 
00278     if (!query_options_len) {
00279         tr_warn("No Option TLV found");
00280         return 0;
00281     }
00282 
00283     this = thread_lowpower_data_metrics_create(address);
00284 
00285     if (!this) {
00286         //query aready exists
00287         tr_error("query not created");
00288         return -3;
00289     }
00290     tr_debug("saving link metrics requested");
00291     memcpy(this->address, address, 16);
00292     this->timeout_timer = 2; // We wait max 2 seconds for single probe Although 0 is enough as response should be written immediately
00293     this->link_metrics_ready = true;
00294     this->req_metrics_len = query_options_len; // first 4 bytes are for query id, 5th byte is query options sub tlv type
00295     thread_tmfcop_tlv_data_get_uint16(query_info.dataPtr, query_info.tlvLen, LINK_METRIC_SUB_TLV_LINK_METRIC_QUERY_ID, &this->query_ID);
00296 
00297     /* Go through the requested data - first 4 bytes id and next two bytes type and length of query options subtlv.
00298      * So start from the 7th byte*/
00299     // query tlv contains query id (length 4 bytes), query options - atleast 2 bytes
00300     if (this->req_metrics_len > METRICS_LEN) {
00301         this->req_metrics_len = METRICS_LEN;
00302     }
00303     for (uint8_t i = 0; i < this->req_metrics_len; i++) {
00304         //save the read bytes for computing the results
00305         this->metrics_requested[i] = *query_options_ptr++;
00306     }
00307 
00308     return 0;
00309 }
00310 
00311 static int thread_lowpower_mle_command_send(protocol_interface_info_entry_t *cur, uint8_t *address, uint8_t series_id, uint8_t series_flags, uint16_t timeout, uint8_t *metrics_ptr, uint8_t metrics_len)
00312 {
00313     uint32_t keySequence;
00314     uint16_t buf_id;
00315     uint8_t request_tlv[1];
00316     uint8_t mle_command = MLE_COMMAND_DATA_REQUEST;
00317 
00318     if (series_flags || timeout) {
00319         mle_command = MLE_COMMAND_METRIC_MANAGEMENT_REQUEST;
00320     }
00321     //Leader data 10 bytes
00322     //query: query tlv (1 byte), query length (1 byte), query_id (4 bytes), query_options (1 byte) + query_len (1 byte) + query (length bytes) = 8 + length
00323     //request tlv 3 bytes
00324     buf_id = mle_service_msg_allocate(cur->id, 10 + 8 + metrics_len + 3 + (1 + thread_leader_data_tlv_size(cur)), false, mle_command);
00325 
00326     if (0 == buf_id) {
00327         return -1;
00328     }
00329     uint8_t *ptr = mle_service_get_data_pointer(buf_id);
00330     //request TLV requesting link metrics report
00331     if (series_flags || timeout) {
00332         // Always enough room
00333         ptr = thread_lowpower_tlv_write_metrics_management_forward_probe_request(ptr, series_id, series_flags, timeout, metrics_ptr, metrics_len);
00334     } else {
00335         request_tlv[0] = MLE_TYPE_LINK_METRICS_REPORT;
00336         ptr = mle_tlv_req_tlv(ptr, request_tlv, 1);
00337         ptr = thread_lowpower_tlv_write_link_metrics_query(ptr, metrics_ptr, metrics_len, series_id);
00338         ptr = thread_active_timestamp_write(cur, ptr); // 10 bytes
00339         //SET Leader Data
00340         ptr = thread_leader_data_tlv_write(ptr, cur);
00341     }
00342 
00343     if (0 != mle_service_update_length_by_ptr(buf_id, ptr)) {
00344         tr_error("Buffer overflow at message write");
00345     }
00346 
00347     mac_data_poll_init_protocol_poll(cur);
00348     mle_service_set_msg_destination_address(buf_id, address);
00349     //Set Security
00350     thread_management_get_current_keysequence(cur->id, &keySequence);
00351     mle_service_msg_update_security_params(buf_id, 5, 2, keySequence);
00352     mle_service_send_message(buf_id);
00353 
00354     return 0;
00355 }
00356 static int thread_lowpower_mle_probe_send(protocol_interface_info_entry_t *cur, uint8_t *address)
00357 {
00358     uint32_t keySequence;
00359     uint16_t buf_id;
00360 
00361     //Leader data 10 bytes
00362     //query: query tlv (1 byte), query length (1 byte), query_id (4 bytes), query_options (1 byte) + query_len (1 byte) + query (length bytes) = 8 + length
00363     //request tlv 3 bytes
00364     buf_id = mle_service_msg_allocate(cur->id, 10, false, MLE_COMMAND_PROBE);
00365 
00366     if (0 == buf_id) {
00367         return -1;
00368     }
00369     uint8_t *ptr = mle_service_get_data_pointer(buf_id);
00370     ptr = thread_leader_data_tlv_write(ptr, cur);
00371 
00372     if (0 != mle_service_update_length_by_ptr(buf_id, ptr)) {
00373         tr_error("Buffer overflow at message write");
00374     }
00375 
00376     mac_data_poll_init_protocol_poll(cur);
00377     mle_service_set_msg_destination_address(buf_id, address);
00378     //Set Security
00379     thread_management_get_current_keysequence(cur->id, &keySequence);
00380     mle_service_msg_update_security_params(buf_id, 5, 2, keySequence);
00381     mle_service_send_message(buf_id);
00382 
00383     return 0;
00384 }
00385 int thread_lowpower_test_probe_send(int8_t interface_id, uint8_t *address, uint8_t *metrics_ptr, uint8_t metrics_len, thread_lowpower_resp_cb response_cb)
00386 {
00387     return thread_lowpower_metrics_management_request_send(interface_id, address, 0, 0, 0, metrics_ptr, metrics_len, response_cb);
00388 }
00389 int thread_lowpower_metrics_management_request_send(int8_t interface_id, uint8_t *address, uint8_t series_id, uint8_t series_flags, uint16_t timeout,  uint8_t *metrics_ptr, uint8_t metrics_len, thread_lowpower_resp_cb response_cb)
00390 {
00391     uint8_t dest_address[16] = {0};
00392     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
00393 
00394     if (!cur) {
00395         return -1;
00396     }
00397 
00398     if (data_response_ptr->data_response_cb) {
00399         tr_warn("low power request already in process");
00400         return -2;
00401     }
00402 
00403     //if no address is specified use parent's address
00404     if (!address) {
00405         thread_management_get_parent_address(interface_id, dest_address);
00406     } else {
00407         memcpy(dest_address, address, 16);
00408     }
00409 
00410     tr_info("destination address = %s metrics queried %s", trace_ipv6(dest_address), trace_array(metrics_ptr, metrics_len));
00411     data_response_ptr->data_response_cb = response_cb;
00412     memcpy(data_response_ptr->destination_address, dest_address, 16);
00413     data_response_ptr->interface_id = interface_id;
00414     data_response_ptr->request_wait_timer = timeout; // wait for 3s for a response to the probe req
00415     data_response_ptr->series_flags = series_flags;
00416     data_response_ptr->series_id =  series_id;
00417     data_response_ptr->probe_count = 3 + 1; // last probe is result rerieval message
00418     thread_lowpower_mle_command_send(cur, dest_address, series_id, series_flags, timeout, metrics_ptr, metrics_len);
00419 
00420     return 0;
00421 }
00422 
00423 static void thread_lowpower_request_timeout_cb(data_response_t *data_req, uint8_t *metrics_ptr, uint8_t metrics_len)
00424 {
00425     if (metrics_len == 0) {
00426         tr_warn("Clearing timers without response");
00427     }
00428     if (data_response_ptr->data_response_cb) {
00429         data_response_ptr->data_response_cb(data_req->destination_address, data_req->interface_id, metrics_ptr, metrics_len);
00430     }
00431     data_req->request_wait_timer = 0;
00432     data_req->data_response_cb = NULL;
00433     memset(data_req->destination_address, 0, 16);
00434     data_req->interface_id = 0;
00435     return;
00436 }
00437 
00438 void thread_lowpower_process_response(uint8_t *src_address, int8_t instance_id, uint8_t *data_ptr, uint16_t data_len)
00439 {
00440     (void) instance_id;
00441     mle_tlv_info_t linkMetricsReport;
00442     if (memcmp(src_address, data_response_ptr->destination_address, 16) != 0) {
00443         return;
00444     }
00445 
00446     if (!mle_tlv_read_tlv(MLE_TYPE_LINK_METRICS_REPORT, data_ptr, data_len, &linkMetricsReport)) {
00447         return;
00448     }
00449 
00450     thread_lowpower_request_timeout_cb(data_response_ptr, data_ptr, data_len);
00451 
00452     return;
00453 }
00454 
00455 int thread_lowpower_process_request(mle_message_t *mle_msg)
00456 {
00457     if (!mle_msg->packet_src_address) {
00458         return -1;
00459     }
00460     if (mle_msg->message_type != MLE_COMMAND_DATA_REQUEST &&
00461             mle_msg->message_type != MLE_COMMAND_METRIC_MANAGEMENT_REQUEST) {
00462         // No need to process
00463         return 0;
00464     }
00465     // Process single shot queries
00466     thread_lowpower_requested_single_query_save(mle_msg->packet_src_address, mle_msg->data_ptr, mle_msg->data_length);
00467     // Check if we have metrics management query
00468     thread_lowpower_requested_metrics_management_query_save(mle_msg->packet_src_address, mle_msg->data_ptr, mle_msg->data_length);
00469 
00470     if (mle_tlv_type_requested(MLE_TYPE_LINK_METRICS_REPORT, mle_msg->data_ptr, mle_msg->data_length)) {
00471         //request made for metrics report
00472         tr_debug("link metrics report requested");
00473         data_metrics_t *metrics = thread_lowpower_data_metrics_find_by_source_address(mle_msg->packet_src_address);
00474         if (metrics) {
00475             // make report ready so the next data response message will include the report
00476             tr_debug("link metrics report ready");
00477             metrics->link_metrics_ready = true;
00478         }
00479     }
00480 
00481     return 0;
00482 }
00483 
00484 int thread_lowpower_timer(protocol_interface_info_entry_t *cur, uint32_t ticks)
00485 {
00486     (void) cur;
00487     if (data_response_ptr->request_wait_timer > 0) {
00488         if (data_response_ptr->series_flags) {
00489             if (data_response_ptr->probe_count > 0) {
00490                 if (data_response_ptr->probe_count == 1) {
00491                     // Send response retrieval after last probe
00492                     thread_lowpower_mle_command_send(cur, data_response_ptr->destination_address, data_response_ptr->series_id, 0, 0, NULL, 0);
00493 
00494                 } else {
00495                     thread_lowpower_mle_probe_send(cur, data_response_ptr->destination_address);
00496                 }
00497                 data_response_ptr->probe_count--;
00498             }
00499             // Send probe we only support MLE probe
00500         }
00501 
00502         if (data_response_ptr->request_wait_timer > ticks) {
00503             data_response_ptr->request_wait_timer -= ticks;
00504         } else {
00505             thread_lowpower_request_timeout_cb(data_response_ptr, NULL, 0);
00506         }
00507     }
00508     ns_list_foreach_safe(data_metrics_t, low_power_metric_ptr, &thread_lowpower_data_metrics_instance_list) {
00509         if (low_power_metric_ptr->timeout_timer > ticks) {
00510             low_power_metric_ptr->timeout_timer -= ticks;
00511         } else {
00512             //query is timed out.
00513             thread_lowpower_data_metrics_delete(low_power_metric_ptr->address);
00514         }
00515     }
00516     return 0;
00517 }
00518 
00519 uint8_t thread_lowpower_link_metrics_length(protocol_interface_info_entry_t *cur, uint8_t *destination_address)
00520 {
00521     (void) cur;
00522     data_metrics_t *metrics = thread_lowpower_data_metrics_find_by_source_address(destination_address);
00523     if (!metrics) {
00524         return 0;
00525     }
00526     if (!metrics->link_metrics_ready) {
00527         return 0;
00528     }
00529     //link metrics report (4 bytes)
00530     return 4 + metrics->req_metrics_len * 2;
00531 
00532 }
00533 
00534 uint8_t *thread_lowpower_link_metrics_write(protocol_interface_info_entry_t *cur, uint8_t *destination_address, uint8_t *ptr)
00535 {
00536     (void)cur;
00537     data_metrics_t *metrics = thread_lowpower_data_metrics_find_by_source_address(destination_address);
00538     if (!metrics) {
00539         return ptr;
00540     }
00541     if (!metrics->link_metrics_ready) {
00542         tr_debug("link metrics not ready for destination");
00543         return ptr;
00544     }
00545     *ptr++ = MLE_TYPE_LINK_METRICS_REPORT;
00546     uint8_t req_metrics_len = metrics->req_metrics_len; // first 4 bytes are for query id, 5th byte is query options sub tlv type
00547     *ptr++ = req_metrics_len * 4; // metric report subtlv, metric report length, metric, and response - each one byte
00548 
00549     /* Go through the requested data - first 4 bytes id and next two bytes type and length of query options subtlv.
00550      * So start from the 7th byte*/
00551     // assuming one byte result follows one byte request flag
00552     for (uint8_t i = 0; i < req_metrics_len; i++) {
00553         //save the read bytes for computing the results
00554         *ptr++ = LINK_METRIC_SUB_TLV_LINK_METRIC_REPORT;
00555         *ptr++ = 2; // report length 2 bytes
00556         *ptr++ = metrics->metrics_requested[i];
00557         *ptr++ = metrics->metrics_value[i];
00558     }
00559 
00560     tr_debug("link metrics requested %s", trace_array(metrics->metrics_requested, metrics->req_metrics_len));
00561     tr_debug("link metrics values %s", trace_array(metrics->metrics_value, metrics->req_metrics_len));
00562 
00563     thread_lowpower_data_metrics_delete(metrics->address);
00564     // this assumes request flags are written first followed by results
00565     //    for (uint8_t i = (6 + req_metrics_len); i < (6 + req_metrics_len*2); i++) {
00566     //        *ptr++ = thread_lowpower_data_metrics_requested_ptr->thread_link_metrics_responses[i-(6 + req_metrics_len)] ; //add zeros as results - needs to be updated
00567     //    }
00568     return ptr;
00569 }
00570 
00571 static int thread_lowpower_metrics_management_query_response_msg(protocol_interface_info_entry_t *cur, uint8_t *dst_address)
00572 {
00573     uint16_t length = 16; // Leader data 10 bytes + link metrics status 6 bytes
00574     uint8_t *ptr;
00575 
00576     //link metrics info
00577     length += thread_lowpower_link_metrics_length(cur, dst_address);
00578 
00579     uint16_t bufId =  mle_service_msg_allocate(cur->id, length, false, MLE_COMMAND_METRIC_MANAGEMENT_RESPONSE);
00580 
00581     if (bufId == 0) {
00582         return -1;
00583     }
00584 
00585     tr_debug("Send MLE Management Query response, %s", trace_ipv6(dst_address));
00586     ptr = mle_service_get_data_pointer(bufId);
00587 
00588     ptr = thread_leader_data_tlv_write(ptr, cur);
00589     ptr = thread_lowpower_link_metrics_write(cur, dst_address, ptr);
00590     ptr = thread_lowpower_tlv_write_metrics_management_status(ptr, 0);
00591 
00592     if (mle_service_update_length_by_ptr(bufId, ptr) != 0) {
00593         tr_debug("Buffer overflow at message write");
00594     }
00595     mle_service_set_msg_destination_address(bufId, dst_address);
00596     //Set Security
00597     uint32_t keySequence;
00598     thread_management_get_current_keysequence(cur->id, &keySequence);
00599     mle_service_msg_update_security_params(bufId, 5, 2, keySequence);
00600 
00601     mle_service_send_message(bufId);
00602     return 0;
00603 
00604 
00605 }
00606 
00607 void thread_lowpower_metrics_management_query_request_process(protocol_interface_info_entry_t *cur, mle_message_t *mle_msg, mle_security_header_t *security_headers, uint8_t linkMargin)
00608 {
00609     (void)cur;
00610     (void)security_headers;
00611     (void)linkMargin;
00612 
00613     tr_info("Recv MLE Metrics Management Query");
00614 
00615     thread_lowpower_metrics_management_query_response_msg(cur, mle_msg->packet_src_address);
00616 }
00617 
00618 
00619 #endif /* HAVE_THREAD_V2 */