mle_service.c

00001 /*
00002  * Copyright (c) 2015-2019, Arm Limited and affiliates.
00003  * SPDX-License-Identifier: Apache-2.0
00004  *
00005  * Licensed under the Apache License, Version 2.0 (the "License");
00006  * you may not use this file except in compliance with the License.
00007  * You may obtain a copy of the License at
00008  *
00009  *     http://www.apache.org/licenses/LICENSE-2.0
00010  *
00011  * Unless required by applicable law or agreed to in writing, software
00012  * distributed under the License is distributed on an "AS IS" BASIS,
00013  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
00014  * See the License for the specific language governing permissions and
00015  * limitations under the License.
00016  */
00017 
00018 #include "nsconfig.h"
00019 #include <string.h>
00020 #include <ns_types.h>
00021 #include "ns_trace.h"
00022 #include "eventOS_event.h"
00023 #include "eventOS_scheduler.h"
00024 #include "eventOS_event_timer.h"
00025 #include "nsdynmemLIB.h"
00026 #include "ns_list.h"
00027 #include "randLIB.h"
00028 #include "socket_api.h"
00029 #include "Core/include/ns_socket.h"
00030 #include "net_interface.h"
00031 #include "common_functions.h"
00032 #include "Common_Protocols/ipv6_constants.h"
00033 #include "NWK_INTERFACE/Include/protocol.h" // just for protocol_core_monotonic_time
00034 #include "Service_Libs/mle_service/mle_service_api.h"
00035 #include "Service_Libs/mle_service/mle_service_security.h"
00036 #include "Service_Libs/mle_service/mle_service_buffer.h"
00037 #include "Service_Libs/mle_service/mle_service_interface.h"
00038 #include "Service_Libs/mle_service/mle_service_frame_counter_table.h"
00039 #include "MLE/mle.h"
00040 #include "MLE/mle_tlv.h"
00041 #include "mac_common_defines.h"
00042 #include "6LoWPAN/MAC/mac_helper.h"
00043 
00044 #define TRACE_GROUP "mleS"
00045 
00046 /* Fixed-point randomisation limits for randlib_randomise_base() - RFC 3315
00047  * says RAND is uniformly distributed between -0.1 and +0.1
00048  */
00049 #define MLE_RAND_LOW   0x7333 // 1 - 0.1; minimum for "1+RAND"
00050 #define MLE_RAND_HIGH  0x8CCD // 1 + 0.1; maximum for "1+RAND"
00051 
00052 typedef struct {
00053     int8_t mle_socket;
00054     int8_t mle_socket_service_tasklet;
00055     uint8_t mle_adata[46];
00056     uint8_t mle_adata_length;
00057     arm_event_storage_t *mle_service_timer_storage;
00058     bool mle_frame_counter_check_enabled: 1;
00059     bool mle_frag_msg_security_enabled: 1;
00060     bool mle_accept_invalid_frame_counter: 1;
00061 } mle_service_class_t;
00062 
00063 #define MLE_SOCKET_SERVICE_TASKLET_INIT      1
00064 #define MLE_SOCKET_SERVICE_TIMER             2
00065 
00066 #define MLE_SOCKET_SERVICE_TIMER_ID          1
00067 
00068 #define MLE_SOCKET_TIMER_UPDATE_PERIOD_IN_MS 100
00069 
00070 mle_service_class_t *mle_service = NULL;
00071 
00072 #ifdef MLE_TEST
00073 static mle_service_filter_cb *receive_filter_cb = NULL;
00074 #endif
00075 
00076 static uint8_t *mle_security_aux_header_write(uint8_t *ptr, const mle_security_header_t *auxHeader);
00077 static void mle_security_aux_ccm_nonce_set(uint8_t *noncePtr, uint8_t *mac64, uint32_t securityFrameCounter, uint8_t securityLevel);
00078 static uint8_t mle_security_aux_header_size(uint8_t keyIdMode);
00079 
00080 /**
00081  * Enable service Timeout timer
00082  */
00083 static void mle_service_timer_start(void);
00084 
00085 /**
00086  * Disable service Timeout timer
00087  */
00088 static void mle_service_timer_stop(void);
00089 
00090 /**
00091  * MLE service event handler
00092  */
00093 static void mle_socket_service_tasklet(arm_event_s *event);
00094 
00095 /**
00096  * MLE service allocated and initialized
00097  */
00098 static bool mle_service_allocate(void);
00099 
00100 /**
00101  * Create TX buffer and add to list
00102  */
00103 static mle_service_msg_buf_t *mle_tr_create(uint16_t buffer_length);
00104 
00105 /**
00106  * Parse Security Header
00107  */
00108 static buffer_t *mle_service_parse_security_header(buffer_t *buf, mle_security_header_t *securityHeader,
00109                                                    uint16_t *header_len);
00110 
00111 /**
00112  * Decode secured message
00113  */
00114 static buffer_t *mle_service_message_security_decode(buffer_t *buf, mle_security_header_t *securityHeader,
00115                                                      uint8_t *security_key);
00116 
00117 /**
00118  *  Mle service generic MLE message handler
00119  */
00120 static void mle_service_socket_callback(void *cb);
00121 
00122 static int mle_service_build_packet_send(service_instance_t *srv_ptr, mle_security_components_t *sec_params, mle_service_msg_buf_t *buffer);
00123 
00124 /**
00125  * Check message sent tokens
00126  */
00127 static bool mle_service_message_tokens_check(service_instance_t *cur_ptr, bool priority);
00128 
00129 /**
00130  * Service instance timeout handler
00131  */
00132 static bool mle_service_instance_timeout_handler(uint16_t ticks, service_instance_t *cur_ptr);
00133 
00134 
00135 static void mle_service_timer_start(void)
00136 {
00137     if (!mle_service->mle_service_timer_storage) {
00138 
00139         arm_event_s event = {
00140             .receiver = mle_service->mle_socket_service_tasklet,
00141             .sender = 0,
00142             .event_id = MLE_SOCKET_SERVICE_TIMER_ID,
00143             .data_ptr = NULL,
00144             .event_type = MLE_SOCKET_SERVICE_TIMER,
00145             .priority = ARM_LIB_LOW_PRIORITY_EVENT,
00146         };
00147 
00148         mle_service->mle_service_timer_storage  = eventOS_event_timer_request_every(&event, eventOS_event_timer_ms_to_ticks(MLE_SOCKET_TIMER_UPDATE_PERIOD_IN_MS));
00149         if (!mle_service->mle_service_timer_storage) {
00150             tr_error("Mle servicetimer start fail");
00151         }
00152     }
00153 }
00154 /**
00155  * Disable service Timeout timer
00156  */
00157 static void mle_service_timer_stop(void)
00158 {
00159     if (mle_service->mle_service_timer_storage) {
00160         eventOS_cancel(mle_service->mle_service_timer_storage);
00161         mle_service->mle_service_timer_storage = NULL;
00162     }
00163 }
00164 
00165 static void mle_service_tr_timeout_handler(mle_service_msg_buf_t *cur_ptr)
00166 {
00167     uint16_t bufId = cur_ptr->msg_id;
00168     mle_service_message_timeout_cb *timeout = NULL;
00169 
00170     if (cur_ptr->timeout_cb && !cur_ptr->tokens_delay) {
00171         timeout = cur_ptr->timeout_cb;
00172     }
00173 
00174     if (!cur_ptr->delayed_response && !cur_ptr->tokens_delay) {
00175         //Update Retry Counter
00176         cur_ptr->retrans++;
00177         if (cur_ptr->retrans_max != 0) {
00178 
00179             if (cur_ptr->retrans >= cur_ptr->retrans_max) {
00180                 // retransmission count exceeded.
00181                 if (timeout) {
00182                     timeout(cur_ptr->interfaceId, cur_ptr->msg_id, true);
00183                 }
00184                 //Cuold be call timeout cb here
00185                 mle_service_msg_free(bufId);
00186                 return;
00187             } else {
00188                 if (timeout) {
00189                     if (!timeout(cur_ptr->interfaceId, cur_ptr->msg_id, false)) {
00190                         //User want to stop retry
00191                         mle_service_msg_free(bufId);
00192                         return;
00193                     }
00194                 }
00195             }
00196         } else {
00197             mle_service_msg_free(bufId);
00198             return;
00199         }
00200     }
00201     service_instance_t *srv_ptr = mle_service_interface_find(cur_ptr->interfaceId);
00202     mle_security_components_t *sec_params = mle_service_security_params_get(cur_ptr->interfaceId);
00203 
00204     if (!srv_ptr || !sec_params) {
00205         mle_service_msg_free(bufId);
00206         return;
00207     }
00208 
00209     // Check if message needs to be delayed or removed because lack of message tokens
00210     if (!mle_service_message_tokens_check(srv_ptr, cur_ptr->tokens_priority)) {
00211         if (mle_service_buffer_tokens_delay_count() <= MLE_TOKEN_BUFFER_MAX_NBR) {
00212             // If message timeout has occurred delay because of tokens
00213             if (!cur_ptr->tokens_delay) {
00214                 cur_ptr->tokens_delay = true;
00215                 // Give time to wait free tokens
00216                 if (cur_ptr->timeout < MLE_TOKEN_DELAY) {
00217                     cur_ptr->timeout = MLE_TOKEN_DELAY;
00218                 }
00219             }
00220             // Delay has already been applied, now wait just for tokens
00221             cur_ptr->delayed_response = MLE_NO_DELAY;
00222         } else {
00223             mle_service_msg_free(bufId);
00224             tr_debug("MLE tokens message freed");
00225         }
00226         return;
00227     } else {
00228         cur_ptr->tokens_delay = false;
00229     }
00230 
00231     // Randomise Challenge TLV value
00232     if (cur_ptr->challengePtr) {
00233         randLIB_get_n_bytes_random(cur_ptr->challengePtr, cur_ptr->challengeLen);
00234     }
00235 
00236     //Trig Buffer to socket
00237     mle_service_build_packet_send(srv_ptr, sec_params, cur_ptr);
00238 
00239     if (cur_ptr->retrans_max == 0) {
00240         mle_service_msg_free(bufId);//Remove packet which not need any retry after 1 retry
00241         return;
00242     }
00243 
00244     //Trig Retry or first packet
00245     if (cur_ptr->delayed_response) {
00246         cur_ptr->delayed_response = MLE_NO_DELAY;
00247         cur_ptr->timeout = cur_ptr->timeout_init;
00248         return;
00249     }
00250 
00251 
00252     // RFC 3315 says:
00253     //     RT = 2*RTprev + RAND*RTprev,
00254     // We calculate this as
00255     //     RT = RTprev + (1+RAND)*RTprev
00256     cur_ptr->timeout = cur_ptr->timeout_init + randLIB_randomise_base(cur_ptr->timeout_init, MLE_RAND_LOW, MLE_RAND_HIGH);
00257     // Catch 16-bit integer overflow
00258     if (cur_ptr->timeout < cur_ptr->timeout_init) {
00259         cur_ptr->timeout = 0xFFFF;
00260     }
00261     // Check against MRT
00262     if (cur_ptr->timeout_max != 0 && cur_ptr->timeout > cur_ptr->timeout_max) {
00263         cur_ptr->timeout = randLIB_randomise_base(cur_ptr->timeout_max, MLE_RAND_LOW, MLE_RAND_HIGH);
00264     }
00265     cur_ptr->timeout_init = cur_ptr->timeout;
00266 }
00267 
00268 /**
00269  * MLE service timeout handling
00270  */
00271 bool mle_service_timer_tick(uint16_t ticks)
00272 {
00273     if (!mle_service) {
00274         tr_debug("MLE not ready");
00275         return false;
00276     }
00277 
00278     bool active_timer_needed = false;
00279 
00280     // MLE service interface instance timeout handling
00281     if (mle_service_interface_timer_tick(ticks, mle_service_instance_timeout_handler)) {
00282         active_timer_needed = true;
00283     }
00284 
00285     // MLE service transaction timeout and retry handling
00286     if (mle_service_buffer_timer_tick(ticks, mle_service_tr_timeout_handler)) {
00287         active_timer_needed = true;
00288     }
00289 
00290     return active_timer_needed;
00291 }
00292 
00293 static void mle_socket_service_tasklet(arm_event_s *event)
00294 {
00295     if (event->event_type == MLE_SOCKET_SERVICE_TIMER) {
00296         if (!mle_service_timer_tick(1)) {
00297             mle_service_timer_stop();
00298         }
00299     }
00300 }
00301 
00302 static bool mle_service_allocate(void)
00303 {
00304     if (mle_service) {
00305         return true;
00306     }
00307     mle_service = ns_dyn_mem_alloc(sizeof(mle_service_class_t));
00308     if (!mle_service) {
00309         return false;
00310     }
00311 
00312     mle_service->mle_socket = -1;
00313     mle_service->mle_socket_service_tasklet = eventOS_event_handler_create(mle_socket_service_tasklet, MLE_SOCKET_SERVICE_TASKLET_INIT);
00314     if (mle_service->mle_socket_service_tasklet < 0) {
00315         ns_dyn_mem_free(mle_service);
00316         mle_service = NULL;
00317         return false;
00318     }
00319     mle_service->mle_service_timer_storage = NULL;
00320     mle_service->mle_frame_counter_check_enabled = false;
00321     mle_service->mle_frag_msg_security_enabled = false;
00322     mle_service->mle_accept_invalid_frame_counter = false;
00323 
00324     return true;
00325 }
00326 
00327 static mle_service_msg_buf_t *mle_tr_create(uint16_t buffer_length)
00328 {
00329     uint16_t tr_id;
00330     mle_service_msg_buf_t *msg_ptr;
00331     msg_ptr = mle_service_buffer_get(buffer_length);
00332     if (msg_ptr == NULL) {
00333         return NULL;
00334     }
00335 
00336     tr_id = randLIB_get_16bit();// 16 bits for random
00337     // Ensure a unique non-zero transaction id for each transaction
00338     while (tr_id == 0 || mle_service_buffer_find(tr_id) != NULL) {
00339         tr_id = tr_id + 1;
00340     }
00341     msg_ptr->msg_id = tr_id;
00342     return msg_ptr;
00343 }
00344 
00345 static void mle_service_set_src_ll64(service_instance_t *srv_ptr, uint8_t *address)
00346 {
00347     memcpy(address, ADDR_LINK_LOCAL_PREFIX, 8);
00348     address += 8;
00349     memcpy(address, srv_ptr->mac64, 8);
00350     *address ^= 2;
00351 
00352 }
00353 static buffer_t *mle_security_interface_aux_header_parse(buffer_t *b, mle_security_header_t *auxSecHeader)
00354 {
00355     uint8_t auxBaseHeader;
00356     uint8_t *ptr = buffer_data_pointer(b);
00357 
00358     auxBaseHeader = *ptr;
00359 
00360     auxSecHeader->KeyIdMode = (auxBaseHeader >> 3) & 3;
00361     auxSecHeader->securityLevel = auxBaseHeader & 7;
00362     auxSecHeader->frameCounter = common_read_32_bit_inverse(ptr + 1);
00363     ptr += 5;
00364 
00365     switch (auxSecHeader->KeyIdMode) {
00366         case MAC_KEY_ID_MODE_SRC8_IDX:
00367             memcpy(auxSecHeader->Keysource, ptr, 8);
00368             ptr += 8;
00369             auxSecHeader->KeyIndex = *ptr++;
00370             break;
00371         case MAC_KEY_ID_MODE_SRC4_IDX:
00372             memcpy(auxSecHeader->Keysource, ptr, 4);
00373             ptr += 4;
00374             auxSecHeader->KeyIndex = *ptr++;
00375             break;
00376         case MAC_KEY_ID_MODE_IDX:
00377             auxSecHeader->KeyIndex = *ptr++;
00378             break;
00379         case MAC_KEY_ID_MODE_IMPLICIT:
00380         default:
00381             auxSecHeader->KeyIndex = 0;
00382             break;
00383     }
00384 
00385     buffer_data_pointer_set(b, ptr);
00386 
00387     if (buffer_data_length(b) < 0) {
00388         b = buffer_free(b);
00389     }
00390 
00391     return b;
00392 }
00393 
00394 static buffer_t *mle_service_parse_security_header(buffer_t *buf, mle_security_header_t *securityHeader,
00395                                                    uint16_t *header_len)
00396 {
00397     uint16_t msg_len;
00398 
00399     msg_len = buffer_data_length(buf);
00400     buf = mle_security_interface_aux_header_parse(buf, securityHeader);
00401     if (!buf) {
00402         return NULL;
00403     }
00404     //Clear Headers off
00405     *header_len = (msg_len - buffer_data_length(buf));
00406     return buf;
00407 }
00408 
00409 /**
00410  * Build MLE ADATA for CCM security
00411  */
00412 static void mle_service_a_data_set(uint8_t *ptr, uint8_t *src_address, uint8_t *dest_address)
00413 {
00414     memcpy(ptr, src_address, 16);
00415     ptr += 16;
00416     memcpy(ptr, dest_address, 16);
00417 }
00418 
00419 static buffer_t *mle_service_message_security_decode(buffer_t *buf, mle_security_header_t *securityHeader,
00420                                                      uint8_t *security_key)
00421 {
00422     ccm_globals_t ccm_ptr;
00423     uint16_t payload_len = buffer_data_length(buf);
00424 
00425     if (!ccm_sec_init(&ccm_ptr, securityHeader->securityLevel, security_key, AES_CCM_DECRYPT, 2)) {
00426         return buffer_free(buf);
00427     } else if (ccm_ptr.mic_len >= payload_len) {
00428         ccm_free(&ccm_ptr);
00429         return buffer_free(buf);
00430     }
00431 
00432     //SET Nonce
00433     buf->src_sa .address [8] ^= 2;
00434     mle_security_aux_ccm_nonce_set(ccm_ptr.exp_nonce, &(buf->src_sa .address [8]),
00435                                    securityHeader->frameCounter, securityHeader->securityLevel);
00436     buf->src_sa .address [8] ^= 2;
00437 
00438     if (ccm_ptr.mic_len) {
00439         payload_len -= ccm_ptr.mic_len;
00440         buf->buf_end  -= ccm_ptr.mic_len;
00441 
00442         ccm_ptr.data_ptr = buffer_data_pointer(buf);
00443         ccm_ptr.adata_ptr = mle_service->mle_adata;
00444         ccm_ptr.data_len = payload_len;
00445         ccm_ptr.adata_len = mle_service->mle_adata_length;
00446         //SET MIC
00447         ccm_ptr.mic = ccm_ptr.data_ptr;
00448         ccm_ptr.mic += payload_len;
00449     } else {
00450         ccm_ptr.data_len = payload_len;
00451         ccm_ptr.data_ptr = buffer_data_pointer(buf);
00452     }
00453 
00454     if (ccm_process_run(&ccm_ptr) != 0) {
00455         tr_error("MLE mic fail!");
00456         buf = buffer_free(buf);
00457     }
00458     return buf;
00459 }
00460 
00461 
00462 static buffer_t *mle_message_security_header_set(buffer_t *buf, service_instance_t *srv_ptr, mle_security_components_t *sec_params, mle_security_header_t *security_header)
00463 {
00464     uint8_t *ptr;
00465     //Verify first security level
00466     if (security_header->securityLevel) {
00467         //Get Security keys
00468         ccm_globals_t ccm_ptr;
00469         uint16_t header_size;
00470         uint16_t data_len;
00471         data_len = buffer_data_length(buf);
00472         ptr = mle_service_security_get_key(security_header, sec_params, srv_ptr->interface_id);
00473         if (!ptr) {
00474             goto drop_buffer;
00475         }
00476         // Init
00477         if (!ccm_sec_init(&ccm_ptr, security_header->securityLevel, ptr, AES_CCM_ENCRYPT, 2)) {
00478             goto drop_buffer;
00479         }
00480         header_size = mle_security_aux_header_size(security_header->KeyIdMode);
00481 
00482         //SET Nonce
00483         mle_security_aux_ccm_nonce_set(ccm_ptr.exp_nonce, srv_ptr->mac64, security_header->frameCounter, security_header->securityLevel);
00484         if (ccm_ptr.mic_len) {
00485             buf = buffer_headroom(buf, (ccm_ptr.mic_len + 32 + header_size));
00486             if (buf) {
00487                 uint8_t *ptr2;
00488                 //Move current data to left by mic_len bytes
00489                 ptr = buffer_data_pointer(buf);
00490                 //Set new data pointer
00491                 ptr2 = ptr;
00492                 ptr2 -= ccm_ptr.mic_len;
00493 
00494                 memmove(ptr2, ptr, data_len);
00495 
00496                 //Cut Mic len
00497                 buf->buf_end  -= ccm_ptr.mic_len;
00498 
00499                 ptr -= header_size + ccm_ptr.mic_len;
00500                 ptr = mle_security_aux_header_write(ptr, security_header);
00501                 ptr -= header_size;
00502                 //Set pointer to Adata
00503                 ptr -= (32);
00504                 mle_service_a_data_set(ptr, buf->src_sa .address , buf->dst_sa .address );
00505                 //Create ADATA
00506                 ccm_ptr.adata_ptr = ptr;
00507                 ccm_ptr.adata_len = (32 + header_size);
00508 
00509                 //SET ptr to show to real payload
00510                 buf->buf_ptr  -= ccm_ptr.mic_len;
00511             } else {
00512                 tr_warn("Security header alloc fail");
00513                 ccm_free(&ccm_ptr);
00514                 buf = (buffer_t *) 0;
00515                 ccm_process_run(NULL);
00516                 return buf;
00517             }
00518         }
00519         ptr = buffer_data_pointer(buf);
00520         ccm_ptr.data_ptr = ptr;
00521         ccm_ptr.data_len = data_len;
00522 
00523         ccm_ptr.mic = ptr;
00524         ccm_ptr.mic += data_len;
00525         ccm_process_run(&ccm_ptr);
00526         if (ccm_ptr.mic_len) {
00527             //SET Calculated mic
00528             buf->buf_end  += ccm_ptr.mic_len;
00529         }
00530         buffer_data_reserve_header(buf, header_size);
00531     }
00532 
00533     buf = buffer_headroom(buf, 1);
00534     if (buf) {
00535         ptr = buffer_data_reserve_header(buf, 1);
00536         if (security_header->securityLevel) {
00537             *ptr++ = 0; // security
00538         } else {
00539             *ptr++ = 0xff; // No security
00540         }
00541     } else {
00542         tr_warn("HeadRoom Fail");
00543     }
00544     return buf;
00545 drop_buffer:
00546 
00547     return buffer_free(buf);
00548 }
00549 
00550 static int mle_service_build_packet_send(service_instance_t *srv_ptr, mle_security_components_t *sec_params, mle_service_msg_buf_t *buffer)
00551 {
00552     buffer_t *buf;
00553     uint8_t *ptr;
00554     uint16_t header_length = 1; //Include security header
00555 
00556     //allocate Buffer for MLE header and Message length
00557     if (buffer->security_header.securityLevel) {
00558         header_length += mle_security_aux_header_size(buffer->security_header.KeyIdMode);
00559 
00560         header_length += 32; //AuthData
00561     }
00562 
00563     buf = buffer_get(buffer->buf_end + header_length);
00564     if (!buf) {
00565         return -1;
00566     }
00567 
00568     if (buffer->security_header.securityLevel) {
00569         uint8_t *dada_ptr = buffer->buf + 1;
00570         uint16_t dada_length = buffer->buf_end - 1;
00571 
00572         mle_tlv_info_t option_info;
00573         //Update mle frame counter evry time for every packet
00574         buffer->security_header.frameCounter = mle_service_security_get_framecounter(sec_params, true);
00575         buffer->security_header.KeyIndex = mle_service_security_get_default_key_id(sec_params);
00576         //Update MAC frame Counter and MLE frame counter to current one allways
00577         if (mle_tlv_option_discover(dada_ptr, dada_length, MLE_TYPE_LL_FRAME_COUNTER, &option_info) == 4) {
00578             // GET frame counter from interface
00579             uint32_t counter;
00580             if (mac_helper_link_frame_counter_read(srv_ptr->interface_id, &counter) == 0) {
00581                 common_write_32_bit(counter, option_info.dataPtr);
00582             }
00583         }
00584 
00585         if (mle_tlv_option_discover(dada_ptr, dada_length, MLE_TYPE_MLE_FRAME_COUNTER, &option_info) == 4) {
00586             common_write_32_bit(buffer->security_header.frameCounter, option_info.dataPtr);
00587         }
00588     }
00589 
00590     //XXX Everything below this point is so broken. API of socket_buffer_sendmsg says buf should not have any metadata
00591 
00592     //Set LL64 Source (for mle_message_security_header_set, and sendmsg picks it up too, although it shouldn't)
00593     mle_service_set_src_ll64(srv_ptr, buf->src_sa .address );
00594     //Set Destination address (for benefit of mle_message_security_header_set, not sendmsg)
00595     memcpy(buf->dst_sa .address , buffer->dst_address, 16);
00596 
00597     // Normal Thread behaviour is for end devices to send multicasts as unicasts
00598     // to parent - this overrides.
00599     if (addr_is_ipv6_multicast(buffer->dst_address)) {
00600         buf->options .ll_broadcast_tx  = true;
00601     }
00602     buf->src_sa .addr_type  = buf->dst_sa .addr_type  = ADDR_IPV6 ;
00603     buf->src_sa .port  = buf->dst_sa .port  = MLE_ALLOCATED_PORT;
00604     //Copy Payload
00605     ptr = buffer_data_pointer(buf);
00606     memcpy(ptr, buffer->buf, buffer->buf_end);
00607     buffer_data_length_set(buf, buffer->buf_end );
00608 
00609     //Add security header and encode message
00610     buf = mle_message_security_header_set(buf, srv_ptr, sec_params, &buffer->security_header);
00611 
00612     if (!buf) {
00613         return -1;
00614     }
00615 
00616     if (mle_service->mle_frag_msg_security_enabled) {
00617         // Enable link layer security for fragmented packets
00618         buf->options .ll_sec_bypass_frag_deny  = true;
00619     }
00620 
00621     if (buffer->selected_channel) {
00622         buf->link_specific.ieee802_15_4.rf_channel_switch = true;
00623         buf->link_specific.ieee802_15_4.selected_channel = buffer->selected_rf_channel;
00624     }
00625 
00626     //Marks message sent
00627     buffer->message_sent = true;
00628 
00629     if (buffer->selected_pan_id) {
00630         buf->link_specific.ieee802_15_4.useDefaultPanId = false;
00631         buf->link_specific.ieee802_15_4.dstPanId = buffer->packet_panid;
00632     }
00633 
00634     int8_t security;
00635 
00636     if (buffer->enable_link_layer_security) {
00637         security = 1;
00638         if (buffer->psk_key_id_mode_2) {
00639             buf->link_specific.ieee802_15_4.key_id_mode = B_SECURITY_KEY_ID_2;
00640         } else {
00641             buf->link_specific.ieee802_15_4.key_id_mode = B_SECURITY_KEY_ID_MODE_DEFAULT;
00642         }
00643     } else {
00644         security = 0;
00645     }
00646     socket_setsockopt(mle_service->mle_socket, SOCKET_IPPROTO_IPV6, SOCKET_LINK_LAYER_SECURITY, &security, sizeof(int8_t));
00647 
00648     //sendmsg always takes destination from msg, not from the buffer, so must specify there
00649     ns_address_t dst_addr;
00650     dst_addr.type = ADDRESS_IPV6;
00651     memcpy(dst_addr.address, buffer->dst_address, 16);
00652     dst_addr.identifier = MLE_ALLOCATED_PORT;
00653 
00654     ns_msghdr_t msg = {
00655         .msg_name = &dst_addr,
00656         .msg_namelen = sizeof dst_addr
00657     };
00658     //Push to socket
00659     //XXX This is so broken. API of socket_buffer_sendmsg says buf should not have any metadata
00660     socket_buffer_sendmsg(mle_service->mle_socket, buf, &msg, 0);
00661     return 0;
00662 }
00663 
00664 static mle_neighbor_security_counter_info_t *mle_service_get_neighbour_info(protocol_interface_info_entry_t *cur_interface, uint8_t *ll64)
00665 {
00666     mac_neighbor_table_entry_t *neighbour = mac_neighbor_entry_get_by_ll64(mac_neighbor_info(cur_interface), ll64, false, NULL);
00667     if (!neighbour) {
00668         return NULL;
00669     }
00670     return mle_service_counter_info_get(cur_interface->id, neighbour->index );
00671 }
00672 
00673 static void mle_service_socket_callback(void *cb)
00674 {
00675     socket_buffer_callback_t *cb_buf = cb;
00676     uint8_t mle_security_byte;
00677     uint16_t security_header_length;
00678     mle_security_header_t securityHeader;
00679     mle_message_t mle_msg;
00680     if (!cb_buf) {
00681         tr_warn("mle sck cb without buf!");
00682         return;
00683     }
00684     buffer_t *buf = cb_buf->buf ;
00685     if (cb_buf->event_type != SOCKET_DATA || !buf) {
00686         return;
00687     }
00688 
00689     service_instance_t *service_handler = mle_service_interface_find(cb_buf->interface_id);
00690     mle_security_components_t *sec_params = mle_service_security_params_get(cb_buf->interface_id);
00691 
00692     if (!service_handler || !sec_params) {
00693         tr_warn("service handler not registerd");
00694         goto error_handler;
00695     }
00696     mle_msg.interface_ptr = service_handler->interface_ptr;
00697 
00698     // MLE messages are only allowed to Link local address
00699     if (!addr_is_ipv6_link_local(buf->src_sa .address ) ||
00700             addr_ipv6_scope(buf->dst_sa .address , NULL) != IPV6_SCOPE_LINK_LOCAL) {
00701         // Security failure so drop packet
00702         goto error_handler;
00703     }
00704 
00705     //Start Parsing Header
00706     mle_security_byte = buffer_pull_uint8(buf);
00707 
00708     if (mle_security_byte == 0xff) {
00709         securityHeader.KeyIndex = 0;
00710         securityHeader.KeyIdMode = MAC_KEY_ID_MODE_IDX;
00711         securityHeader.securityLevel = 0;
00712     } else if (mle_security_byte == 0) {
00713 
00714         //SET adata
00715         uint8_t *sec_header_ptr = buffer_data_pointer(buf);
00716 
00717 
00718         buf = mle_service_parse_security_header(buf, &securityHeader, &security_header_length);
00719         if (!buf) {
00720             goto error_handler;
00721         }
00722 
00723         //SET adata and length
00724         mle_service->mle_adata_length = (32 + security_header_length);
00725 
00726         //copy data
00727         mle_service_a_data_set(mle_service->mle_adata, buf->src_sa .address , buf->dst_sa .address );
00728         memcpy(&mle_service->mle_adata[32], sec_header_ptr, security_header_length);
00729     } else {
00730         goto error_handler;
00731     }
00732 
00733     bool security_bypass;
00734     securityHeader.invalid_frame_counter = false;
00735     if (securityHeader.securityLevel) {
00736 
00737         if (sec_params->sec_level == 0) {
00738             tr_debug("Drop secured packet when security is disabled");
00739             goto error_handler;
00740         }
00741         //Get A key here
00742         uint8_t *key_ptr = mle_service_security_get_key(&securityHeader, sec_params, service_handler->interface_id);
00743         if (!key_ptr) {
00744             goto error_handler;
00745         }
00746 
00747 
00748         /* MLE neighbour table frame counter check */
00749         mle_neighbor_security_counter_info_t *neighbour = NULL;
00750         uint32_t keySeq;
00751         if (securityHeader.KeyIdMode == MAC_KEY_ID_MODE_SRC4_IDX) {
00752             keySeq = common_read_32_bit(securityHeader.Keysource);
00753         } else {
00754             keySeq = (uint32_t)securityHeader.KeyIndex;
00755         }
00756 
00757         if (mle_service->mle_frame_counter_check_enabled) {
00758             neighbour = mle_service_get_neighbour_info(service_handler->interface_ptr, buf->src_sa .address );
00759             if (neighbour) {
00760                 //key pending is set - incoming frame counter will be reset when new sequence is heard or lower framecounter is heard
00761                 if (neighbour->new_key_pending) {
00762                     if ((securityHeader.frameCounter < neighbour->mle_frame_counter || keySeq != neighbour->last_key_sequence)) {
00763                         neighbour->mle_frame_counter = 0;
00764                         neighbour->last_key_sequence = keySeq;
00765                         neighbour->new_key_pending = false;
00766                         goto start_decode; //Skip validation part
00767                     }
00768                 }
00769                 //key pending not set - continue normal operation
00770                 if (keySeq < neighbour->last_key_sequence) {
00771                     tr_warn("Dropping packet because key sequence smaller: %"PRIu32" < %"PRIu32,
00772                             keySeq, neighbour->last_key_sequence);
00773                     goto error_handler;
00774                 }
00775                 if (securityHeader.frameCounter <= neighbour->mle_frame_counter && keySeq == neighbour->last_key_sequence) {
00776                     if (mle_service->mle_accept_invalid_frame_counter) {
00777                         securityHeader.invalid_frame_counter = true;
00778                     } else {
00779                         tr_warn("Dropping packet MLE frame counter is not higher:%"PRIu32" <= %"PRIu32, securityHeader.frameCounter, neighbour->mle_frame_counter);
00780                         goto error_handler;
00781                     }
00782                 }
00783             }
00784         }
00785 start_decode:
00786         buf = mle_service_message_security_decode(buf, &securityHeader, key_ptr);
00787         if (!buf) {
00788             goto error_handler;
00789         }
00790         security_bypass = false;
00791         if (mle_service->mle_frame_counter_check_enabled && !securityHeader.invalid_frame_counter) {
00792             if (neighbour) {
00793                 neighbour->mle_frame_counter = securityHeader.frameCounter;
00794                 neighbour->last_key_sequence = keySeq;
00795             }
00796         }
00797     } else {
00798         if (sec_params->sec_level) {
00799             if (!service_handler->recv_security_bypass_cb) {
00800                 tr_debug("Drop unsecured packet when security is enabled");
00801                 goto error_handler;
00802             }
00803             security_bypass = true;
00804         } else {
00805             security_bypass = false;
00806         }
00807     }
00808 
00809 
00810 
00811     mle_msg.message_type = buffer_pull_uint8(buf);
00812     mle_msg.data_length = buffer_data_length(buf);
00813     mle_msg.data_ptr = buffer_data_pointer(buf);
00814     mle_msg.dst_pan_id = buf->link_specific.ieee802_15_4.dstPanId;
00815     mle_msg.src_pan_id = buf->link_specific.ieee802_15_4.srcPanId;
00816     if (mle_message_malformed_check(mle_msg.data_ptr, mle_msg.data_length) != 0) {
00817         tr_debug("Malfor");
00818         goto error_handler;
00819     }
00820     mle_msg.packet_src_address = buf->src_sa .address ;
00821     mle_msg.packet_dst_address = buf->dst_sa .address ;
00822     mle_msg.dbm = buf->options .dbm ;
00823     mle_msg.lqi = buf->options .lqi ;
00824 
00825 #ifdef MLE_TEST
00826     if (receive_filter_cb) {
00827         if (!receive_filter_cb(service_handler->interface_id, &mle_msg, &securityHeader)) {
00828             goto error_handler;
00829         }
00830     }
00831 #endif
00832 
00833     if (security_bypass) {
00834         /* Security by pass message handler call */
00835         service_handler->recv_security_bypass_cb(service_handler->interface_id, &mle_msg);
00836     } else {
00837 
00838         if (service_handler->recv_cb) {
00839             service_handler->recv_cb(service_handler->interface_id, &mle_msg, &securityHeader);
00840         }
00841     }
00842 
00843 error_handler:
00844     if (buf) {
00845         buffer_free(buf);
00846     }
00847 }
00848 
00849 static bool mle_service_message_tokens_check(service_instance_t *cur_ptr, bool priority)
00850 {
00851     if (!cur_ptr || cur_ptr->token_bucket_size == 0) {
00852         return true;
00853     }
00854 
00855     if (priority) {
00856         if (cur_ptr->token_bucket > MLE_TOKEN_BUFFER_MIN_NBR) {
00857             cur_ptr->token_bucket--;
00858         }
00859         return true;
00860     }
00861 
00862     if (cur_ptr->token_bucket > 0) {
00863         cur_ptr->token_bucket--;
00864         return true;
00865     } else {
00866         return false;
00867     }
00868 }
00869 
00870 static bool mle_service_instance_timeout_handler(uint16_t ticks, service_instance_t *cur_ptr)
00871 {
00872     if (!cur_ptr || cur_ptr->token_bucket_size == 0 || cur_ptr->token_bucket_rate == 0) {
00873         return false;
00874     }
00875 
00876     cur_ptr->token_bucket_ticks += ticks;
00877 
00878     while (cur_ptr->token_bucket_ticks >= cur_ptr->token_bucket_rate) {
00879         cur_ptr->token_bucket_ticks -= cur_ptr->token_bucket_rate;
00880         cur_ptr->token_bucket += cur_ptr->token_bucket_count;
00881     }
00882 
00883     if (cur_ptr->token_bucket < cur_ptr->token_bucket_size) {
00884         return true;
00885     } else {
00886         cur_ptr->token_bucket = cur_ptr->token_bucket_size;
00887         return false;
00888     }
00889 }
00890 
00891 int mle_service_interface_register(int8_t interface_id, void *interface_ptr,  mle_service_receive_cb *receive_cb, uint8_t *mac64, uint8_t challengeLength)
00892 {
00893     service_instance_t *srv_ptr;
00894 
00895     if (challengeLength < 4 || challengeLength > 32) {
00896         return -1;
00897     } else if (!receive_cb || !interface_ptr) {
00898         return -1;
00899     } else if (!mac64) {
00900         return -1;
00901     }
00902 
00903     if (!mle_service_allocate()) {
00904         tr_error("dhcp Sockets data base alloc fail");
00905         return -1;
00906     }
00907 
00908     if (mle_service_security_instance_allocate(interface_id) != 0) {
00909         return -1;
00910     }
00911 
00912     srv_ptr = mle_service_interface_get(interface_id);
00913     if (!srv_ptr) {
00914         goto error_handler;
00915     }
00916 
00917     srv_ptr->recv_cb = receive_cb;
00918     srv_ptr->challenge_length = challengeLength;
00919     srv_ptr->interface_ptr = interface_ptr;
00920     memcpy(srv_ptr->mac64, mac64, 8);
00921 
00922     if (mle_service->mle_socket < 0) {
00923         if (socket_create(SOCKET_FAMILY_IPV6, SOCKET_TYPE_DGRAM, 0, &mle_service->mle_socket, MLE_ALLOCATED_PORT, mle_service_socket_callback, true) != eOK) {
00924             mle_service->mle_socket = -1;
00925         }
00926     }
00927 
00928     if (mle_service->mle_socket < 0) {
00929         tr_error("Sockets not available");
00930         goto error_handler;
00931     } else {
00932         const int16_t hops = 255;
00933         const uint32_t address_pref = SOCKET_IPV6_PREFER_SRC_6LOWPAN_LONG;
00934         const int8_t security = 0;
00935         const bool loop = false;
00936         socket_setsockopt(mle_service->mle_socket, SOCKET_IPPROTO_IPV6, SOCKET_IPV6_UNICAST_HOPS, &hops, sizeof hops);
00937         socket_setsockopt(mle_service->mle_socket, SOCKET_IPPROTO_IPV6, SOCKET_IPV6_MULTICAST_HOPS, &hops, sizeof hops);
00938         socket_setsockopt(mle_service->mle_socket, SOCKET_IPPROTO_IPV6, SOCKET_IPV6_MULTICAST_LOOP, &loop, sizeof loop);
00939         socket_setsockopt(mle_service->mle_socket, SOCKET_IPPROTO_IPV6, SOCKET_IPV6_ADDR_PREFERENCES, &address_pref, sizeof address_pref);
00940         socket_setsockopt(mle_service->mle_socket, SOCKET_IPPROTO_IPV6, SOCKET_LINK_LAYER_SECURITY, &security, sizeof security);
00941         //Set service interface id
00942         socket_setsockopt(mle_service->mle_socket, SOCKET_IPPROTO_IPV6, SOCKET_INTERFACE_SELECT, &srv_ptr->interface_id, sizeof srv_ptr->interface_id);
00943     }
00944 
00945     return 0;
00946 
00947 error_handler:
00948     mle_service_interface_delete(interface_id);
00949     mle_service_security_instance_delete(interface_id);
00950     return -1;
00951 }
00952 
00953 bool mle_service_interface_registeration_validate(int8_t interface_id)
00954 {
00955     if (mle_service_interface_find(interface_id)) {
00956         return true;
00957     }
00958 
00959     return false;
00960 }
00961 
00962 
00963 int mle_service_interface_receiver_handler_update(int8_t interface_id, mle_service_receive_cb *receive_cb)
00964 {
00965     service_instance_t *srv_ptr;
00966 
00967     srv_ptr = mle_service_interface_find(interface_id);
00968     if (!srv_ptr) {
00969         return -1;
00970     }
00971 
00972     srv_ptr->recv_cb = receive_cb;
00973 
00974     return 0;
00975 }
00976 
00977 int mle_service_interface_receiver_bypass_handler_update(int8_t interface_id, mle_service_receive_security_bypass_cb *receive_cb)
00978 {
00979     service_instance_t *srv_ptr;
00980 
00981     srv_ptr = mle_service_interface_find(interface_id);
00982     if (!srv_ptr) {
00983         return -1;
00984     }
00985 
00986     srv_ptr->recv_security_bypass_cb = receive_cb;
00987 
00988     return 0;
00989 }
00990 
00991 void mle_service_interface_unregister(int8_t interface_id)
00992 {
00993     //Remove service from list and free class
00994     mle_service_interface_delete(interface_id);
00995     //Remove Security Instance
00996     mle_service_security_instance_delete(interface_id);
00997     //Free Tx queue by unregistered interface id
00998     mle_service_buffer_clean_buffers_by_interface_id(interface_id);
00999 
01000     if (mle_service) {
01001         if (mle_service_interface_list_empty()) {
01002             tr_debug("Free Socket");
01003             socket_close(mle_service->mle_socket);
01004             mle_service->mle_socket = -1;
01005         }
01006     }
01007 }
01008 
01009 int mle_service_reset_frame_counters(int8_t interfaceId)
01010 {
01011     service_instance_t *srv_ptr = mle_service_interface_find(interfaceId);
01012     if (!srv_ptr) {
01013         return -1;
01014     }
01015     mle_service_security_set_frame_counter(interfaceId, 0);
01016     mle_class_set_new_key_pending(srv_ptr->interface_ptr);
01017     return 0;
01018 }
01019 
01020 void mle_service_interface_tx_queue_clean(int8_t interface_id)
01021 {
01022     mle_service_buffer_clean_buffers_by_interface_id(interface_id);
01023 }
01024 
01025 uint16_t mle_service_interface_tx_queue_size(int8_t interface_id)
01026 {
01027     return mle_service_buffer_count_by_interface_id(interface_id);
01028 }
01029 
01030 int mle_service_interface_token_bucket_settings_set(int8_t interface_id, uint8_t size, uint8_t rate, uint8_t count)
01031 {
01032     service_instance_t *srv_ptr;
01033 
01034     srv_ptr = mle_service_interface_find(interface_id);
01035     if (!srv_ptr) {
01036         return -1;
01037     }
01038     srv_ptr->token_bucket = size;
01039     srv_ptr->token_bucket_ticks = 0;
01040     srv_ptr->token_bucket_size = size;
01041     srv_ptr->token_bucket_rate = rate;
01042     srv_ptr->token_bucket_count = count;
01043 
01044     return 0;
01045 }
01046 
01047 uint16_t mle_service_msg_allocate(int8_t interface_id, uint16_t data_length, bool challengeAllocate, uint8_t type)
01048 {
01049     uint16_t temporary_length = 64;
01050     mle_service_msg_buf_t *buf;
01051     //Verify that Interface is registered
01052     service_instance_t *srv_ptr = mle_service_interface_find(interface_id);
01053     if (!srv_ptr) {
01054         return 0;
01055     }
01056 
01057     mle_security_components_t *sec_params = mle_service_security_params_get(interface_id);
01058     if (!sec_params) {
01059         return 0;
01060     }
01061 
01062     if (data_length > 63) {
01063         temporary_length = data_length  + 1;
01064     }
01065 
01066     if (challengeAllocate) {
01067         temporary_length += (srv_ptr->challenge_length + 2);
01068     }
01069 
01070     buf = mle_tr_create(temporary_length);
01071     if (!buf) {
01072         return 0;
01073     }
01074     //Set interface id to buffer
01075     buf->interfaceId = interface_id;
01076 
01077     mle_service_buffer_set_msg_type(buf->msg_id, type);
01078 
01079     if (challengeAllocate) {
01080         //SET Challenge all ways to front
01081         uint8_t *ptr = mle_msg_data_pointer(buf);
01082         *ptr++ = MLE_TYPE_CHALLENGE;
01083         *ptr++ = srv_ptr->challenge_length;
01084         buf->challengeLen = srv_ptr->challenge_length;
01085         buf->challengePtr = ptr;
01086         randLIB_get_n_bytes_random(ptr, srv_ptr->challenge_length);
01087         mle_msg_length_set(buf, (srv_ptr->challenge_length + 2));
01088     } else {
01089         //NO retry by default
01090         buf->timeout_init = buf->timeout = buf->timeout_max = 0;
01091         buf->retrans_max = 0;
01092     }
01093     //Set default security level and key id mode
01094     //Key id, frame counter will be updated when default need to be change
01095     buf->security_header.securityLevel = sec_params->sec_level;
01096     buf->security_header.KeyIdMode = MAC_KEY_ID_MODE_IDX;
01097 
01098     return buf->msg_id;
01099 }
01100 
01101 uint8_t *mle_service_get_data_pointer(uint16_t msgId)
01102 {
01103     return mle_service_buffer_get_data_pointer(msgId);
01104 }
01105 
01106 uint8_t *mle_service_get_payload_start_point(uint16_t msgId)
01107 {
01108     mle_service_msg_buf_t *buf = mle_service_buffer_find(msgId);
01109     if (!buf) {
01110         return NULL;
01111     }
01112     uint8_t *ptr = buf->buf;
01113 
01114     return (ptr + 1);
01115 }
01116 
01117 uint16_t  mle_service_get_payload_length(uint16_t msgId)
01118 {
01119     mle_service_msg_buf_t *buf = mle_service_buffer_find(msgId);
01120     if (!buf) {
01121         return 0;
01122     }
01123 
01124     if (buf->buf_end == 0) {
01125         return 0;
01126     }
01127 
01128     return (buf->buf_end - 1);
01129 
01130 }
01131 
01132 int mle_service_update_length(uint16_t msgId, uint16_t tail_length)
01133 {
01134     return mle_service_buffer_update_length(msgId, tail_length);
01135 }
01136 
01137 int mle_service_update_length_by_ptr(uint16_t msgId, uint8_t *data_end_ptr)
01138 {
01139     return mle_service_buffer_update_length_by_ptr(msgId, data_end_ptr);
01140 }
01141 
01142 int mle_service_msg_update_security_params(uint16_t msgId, uint8_t security_level, uint8_t key_id_mode, uint32_t keysequence)
01143 {
01144     mle_service_msg_buf_t *buf = mle_service_buffer_find(msgId);
01145     if (!buf) {
01146         return -1;
01147     }
01148 
01149     if (security_level > AES_SECURITY_LEVEL_ENC_MIC128) {
01150         return -2;
01151     } else if (security_level && key_id_mode != MAC_KEY_ID_MODE_IDX && key_id_mode != MAC_KEY_ID_MODE_SRC4_IDX) {
01152         return -2;
01153     }
01154 
01155     buf->security_header.securityLevel = security_level;
01156     buf->security_header.KeyIdMode = key_id_mode;
01157     common_write_32_bit(keysequence, buf->security_header.Keysource);
01158     return 0;
01159 }
01160 
01161 int mle_service_set_packet_callback(uint16_t msgId, mle_service_message_timeout_cb *cb)
01162 {
01163     return mle_service_buffer_set_timeout_cb(msgId, cb);
01164 }
01165 
01166 int mle_service_set_msg_response_true(uint16_t msgId)
01167 {
01168     return mle_service_buffer_set_response(msgId);
01169 }
01170 
01171 uint8_t *mle_service_get_msg_destination_address_pointer(uint16_t msgId)
01172 {
01173     return mle_service_buffer_get_msg_destination_address_pointer(msgId);
01174 }
01175 
01176 int mle_service_set_msg_destination_address(uint16_t msgId, const uint8_t *dstAddress)
01177 {
01178     if (!dstAddress) {
01179         return -2;
01180     }
01181 
01182     uint8_t *address_ptr = mle_service_buffer_get_msg_destination_address_pointer(msgId);
01183 
01184     if (!address_ptr) {
01185         return -1;
01186     }
01187     memcpy(address_ptr, dstAddress, 16);
01188     return 0;
01189 }
01190 
01191 int mle_service_set_msg_panid(uint16_t msgId, uint16_t panid)
01192 {
01193     mle_service_msg_buf_t *buf = mle_service_buffer_find(msgId);
01194 
01195     if (!buf) {
01196         return -1;
01197     }
01198     buf->selected_pan_id = true;
01199     buf->packet_panid = panid;
01200     return 0;
01201 }
01202 
01203 int mle_service_msg_free(uint16_t msgId)
01204 {
01205     return mle_service_buffer_free(mle_service_buffer_find(msgId));
01206 }
01207 
01208 bool mle_service_transaction_buffer_get_for_response(uint8_t *responseData, uint16_t length, uint16_t *msgId)
01209 {
01210     mle_service_msg_buf_t *buffer = mle_service_buffer_find_for_response(responseData, length);
01211     if (!buffer) {
01212         return false;
01213     }
01214 
01215     *msgId = buffer->msg_id;
01216     return true;
01217 }
01218 
01219 bool mle_service_check_msg_sent(uint16_t msgId)
01220 {
01221     mle_service_msg_buf_t *buf = mle_service_buffer_find(msgId);
01222 
01223     if (!buf) {
01224         return false;
01225     }
01226 
01227     return buf->message_sent;
01228 }
01229 
01230 int mle_service_message_tail_get(uint16_t msgId, uint16_t tail_length)
01231 {
01232     return mle_service_buffer_tail_get(msgId, tail_length);
01233 }
01234 
01235 static int mle_service_timeout_fill(uint16_t msgId, mle_message_timeout_params_t *timeout_params, bool timeout_in_seconds)
01236 {
01237     mle_service_msg_buf_t *buffer = mle_service_buffer_find(msgId);
01238 
01239     if (!buffer) {
01240         return -1;
01241     }
01242 
01243     buffer->timeout_max = timeout_params->timeout_max;
01244     buffer->retrans_max = timeout_params->retrans_max;
01245     buffer->delayed_response = timeout_params->delay;
01246     buffer->timeout_init = timeout_params->timeout_init;
01247 
01248     if (timeout_in_seconds) {
01249         buffer->timeout_max = buffer->timeout_max * 10;
01250         buffer->timeout_init = buffer->timeout_init * 10;
01251     }
01252 
01253     buffer->timeout_init = randLIB_randomise_base(buffer->timeout_init, MLE_RAND_LOW, MLE_RAND_HIGH);
01254 
01255     buffer->timeout = buffer->timeout_init;
01256 
01257     return 0;
01258 }
01259 
01260 int mle_service_set_msg_timeout_parameters(uint16_t msgId, mle_message_timeout_params_t *timeout_params)
01261 {
01262     return mle_service_timeout_fill(msgId, timeout_params, true);
01263 }
01264 
01265 int mle_service_set_msg_timeout_parameters_fast(uint16_t msgId, mle_message_timeout_params_t *timeout_params)
01266 {
01267     return mle_service_timeout_fill(msgId, timeout_params, false);
01268 }
01269 
01270 int mle_service_set_msg_token_bucket_priority(uint16_t msgId)
01271 {
01272     mle_service_msg_buf_t *buffer = mle_service_buffer_find(msgId);
01273 
01274     if (!buffer) {
01275         return -1;
01276     }
01277 
01278     buffer->tokens_priority = true;
01279     return 0;
01280 }
01281 
01282 int mle_service_send_message(uint16_t msgId)
01283 {
01284     mle_service_msg_buf_t *buffer = mle_service_buffer_find(msgId);
01285 
01286     if (!buffer) {
01287         return -1;
01288     }
01289 
01290     service_instance_t *srv_ptr = mle_service_interface_find(buffer->interfaceId);
01291     mle_security_components_t *sec_params = mle_service_security_params_get(buffer->interfaceId);
01292 
01293     if (!srv_ptr || !sec_params) {
01294         mle_service_buffer_free(buffer);
01295         return -1;
01296     }
01297 
01298     //Trig timer
01299     mle_service_timer_start();
01300 
01301     if (buffer->delayed_response) {
01302         //set random jitter for response
01303         buffer->timeout = randLIB_get_random_in_range(1, buffer->delayed_response);
01304         return 0;
01305     }
01306 
01307     // Check if message needs to be delayed or removed because lack of message tokens
01308     if (!mle_service_message_tokens_check(srv_ptr, buffer->tokens_priority)) {
01309         if (mle_service_buffer_tokens_delay_count() <= MLE_TOKEN_BUFFER_MAX_NBR) {
01310             buffer->tokens_delay = true;
01311             // Give time to wait free tokens
01312             if (buffer->timeout < MLE_TOKEN_DELAY) {
01313                 buffer->timeout = MLE_TOKEN_DELAY;
01314             }
01315         } else {
01316             mle_service_buffer_free(mle_service_buffer_find(buffer->msg_id));
01317             tr_debug("MLE tokens message freed");
01318         }
01319         return 0;
01320     }
01321 
01322     if (mle_service_build_packet_send(srv_ptr, sec_params, buffer) != 0) {
01323         tr_warn("MLE message sending failed, ipv6=%s", trace_ipv6(buffer->dst_address));
01324         buffer->delayed_response = 1;
01325         buffer->timeout = 1;
01326         buffer->retrans_max++;
01327     } else {
01328         if (buffer->retrans_max == 0 && !buffer->timeout) {
01329             //Free msg
01330             mle_service_buffer_free(mle_service_buffer_find(buffer->msg_id));
01331         }
01332     }
01333 
01334     return 0;
01335 }
01336 
01337 int mle_service_security_init(int8_t interfaceId, uint8_t security_level, uint32_t security_frame_counter, mle_service_key_request_by_counter_cb *key_counter_req, mle_service_security_notify_cb *notify)
01338 {
01339     mle_security_components_t *srv_ptr = mle_service_security_params_get(interfaceId);
01340     if (!srv_ptr) {
01341         return -1;
01342     }
01343 
01344     if (security_level > AES_SECURITY_LEVEL_ENC_MIC128) {
01345         return -2;
01346     }
01347 
01348     mle_service_security_parameters_init(srv_ptr);
01349     srv_ptr->sec_level = security_level;
01350     srv_ptr->key_req = key_counter_req;
01351     srv_ptr->security_notify = notify;
01352     srv_ptr->security_frame_counter = security_frame_counter;
01353     return 0;
01354 }
01355 
01356 
01357 bool mle_service_security_set_frame_counter(int8_t interfaceId, uint32_t frame_counter)
01358 {
01359     mle_security_components_t *sec_ptr = mle_service_security_params_get(interfaceId);
01360     if (!sec_ptr) {
01361         return false;
01362     }
01363 
01364     sec_ptr->security_frame_counter = frame_counter;
01365 
01366     return true;
01367 }
01368 
01369 uint32_t mle_service_security_get_frame_counter(int8_t interfaceId)
01370 {
01371     mle_security_components_t *sec_ptr = mle_service_security_params_get(interfaceId);
01372     if (!sec_ptr) {
01373         return 0;
01374     }
01375 
01376     return sec_ptr->security_frame_counter;
01377 }
01378 
01379 uint8_t *mle_service_security_default_key_get(int8_t interfaceId)
01380 {
01381     mle_security_components_t *sec_ptr = mle_service_security_params_get(interfaceId);
01382     if (!sec_ptr) {
01383         return NULL;
01384     }
01385 
01386     return mle_service_security_get_default_key(sec_ptr);
01387 }
01388 
01389 uint8_t  mle_service_security_default_key_id_get(int8_t interfaceId)
01390 {
01391     mle_security_components_t *sec_ptr = mle_service_security_params_get(interfaceId);
01392     if (!sec_ptr) {
01393         return 0;
01394     }
01395 
01396     return mle_service_security_get_default_key_id(sec_ptr);
01397 }
01398 
01399 uint8_t *mle_service_security_next_key_get(int8_t interfaceId)
01400 {
01401     mle_security_components_t *sec_ptr = mle_service_security_params_get(interfaceId);
01402     if (!sec_ptr) {
01403         return NULL;
01404     }
01405 
01406     return mle_service_security_get_next_key(sec_ptr);
01407 }
01408 
01409 uint8_t  mle_service_security_next_key_id_get(int8_t interfaceId)
01410 {
01411     mle_security_components_t *sec_ptr = mle_service_security_params_get(interfaceId);
01412     if (!sec_ptr) {
01413         return 0;
01414     }
01415 
01416     return mle_service_security_get_next_key_id(sec_ptr);
01417 }
01418 
01419 uint8_t  mle_service_security_level_get(int8_t interfaceId)
01420 {
01421     mle_security_components_t *sec_ptr = mle_service_security_params_get(interfaceId);
01422     if (!sec_ptr) {
01423         return 0;
01424     }
01425 
01426     return sec_ptr->sec_level;
01427 }
01428 
01429 bool mle_service_security_key_trig(int8_t interfaceId, uint8_t keyId)
01430 {
01431     return mle_service_security_key_update_trig(interfaceId, mle_service_security_params_get(interfaceId), keyId);
01432 
01433 }
01434 
01435 bool mle_service_security_set_security_key(int8_t interfaceId, const uint8_t *security_key, uint8_t keyId, bool primary)
01436 {
01437     bool master_key_changed = mle_service_security_key_set(mle_service_security_params_get(interfaceId), security_key, keyId, primary);
01438     if (master_key_changed && primary) {
01439         mle_service_reset_frame_counters(interfaceId);
01440     }
01441     return master_key_changed;
01442 }
01443 
01444 int mle_service_reject_message_build(int8_t interfaceId, uint8_t *dstIpv6, bool force_unsecure)
01445 {
01446     uint16_t buf_id = mle_service_msg_allocate(interfaceId, 32, false, MLE_COMMAND_REJECT);
01447     if (buf_id == 0) {
01448         return -1;
01449     }
01450     tr_debug("MLE Reject MSG Build");
01451 
01452     if (force_unsecure) {
01453         mle_service_msg_update_security_params(buf_id, 0, 0, 0);
01454     }
01455 
01456     //SET Destination address
01457     mle_service_set_msg_destination_address(buf_id, dstIpv6);
01458 
01459     //Set message priority
01460     mle_service_set_msg_token_bucket_priority(buf_id);
01461 
01462     mle_service_send_message(buf_id);
01463 
01464     return 0;
01465 }
01466 
01467 static uint8_t *mle_security_aux_header_write(uint8_t *ptr, const mle_security_header_t *auxHeader)
01468 {
01469     uint8_t auxBaseHeader;
01470     auxBaseHeader = auxHeader->securityLevel;
01471     auxBaseHeader |= (auxHeader->KeyIdMode << 3);
01472     *ptr++ = auxBaseHeader;
01473     ptr = common_write_32_bit_inverse(auxHeader->frameCounter, ptr);
01474 
01475     switch (auxHeader->KeyIdMode) {
01476         case MAC_KEY_ID_MODE_SRC8_IDX:
01477             memcpy(ptr, auxHeader->Keysource, 8);
01478             ptr += 8;
01479             *ptr++ = auxHeader->KeyIndex;
01480             break;
01481         case MAC_KEY_ID_MODE_SRC4_IDX:
01482             memcpy(ptr, auxHeader->Keysource, 4);
01483             ptr += 4;
01484             *ptr++ = auxHeader->KeyIndex;
01485             break;
01486         case MAC_KEY_ID_MODE_IDX:
01487             *ptr++ = auxHeader->KeyIndex;
01488             break;
01489         case MAC_KEY_ID_MODE_IMPLICIT:
01490         default:
01491             break;
01492     }
01493     return ptr;
01494 }
01495 
01496 static void mle_security_aux_ccm_nonce_set(uint8_t *noncePtr, uint8_t *mac64, uint32_t securityFrameCounter, uint8_t securityLevel)
01497 {
01498     memcpy(noncePtr, mac64, 8);
01499     noncePtr += 8;
01500     noncePtr = common_write_32_bit(securityFrameCounter, noncePtr);
01501     *noncePtr = securityLevel;
01502 }
01503 
01504 static uint8_t mle_security_aux_header_size(uint8_t keyIdMode)
01505 {
01506     uint8_t auxHeaderLength;
01507     if (keyIdMode == 0) {
01508         auxHeaderLength = 5; //No Key ID selected
01509     } else if (keyIdMode == 1) {
01510         auxHeaderLength = 6; //key ID without Source
01511     } else if (keyIdMode == 2) {
01512         auxHeaderLength = 10; //key ID with 32-bitSource
01513     } else {
01514         auxHeaderLength = 14; //key ID with 64-bitSource
01515     }
01516 
01517     return auxHeaderLength;
01518 }
01519 
01520 void mle_service_set_frame_counter_check(bool value)
01521 {
01522     if (mle_service) {
01523         mle_service->mle_frame_counter_check_enabled = value;
01524     }
01525 }
01526 
01527 void mle_service_set_fragmented_msg_ll_security(bool value)
01528 {
01529     if (mle_service) {
01530         mle_service->mle_frag_msg_security_enabled = value;
01531     }
01532 }
01533 
01534 int mle_service_set_msg_rf_channel(uint16_t msgId, uint8_t channel)
01535 {
01536     return mle_service_buffer_set_msg_rf_channel(msgId, channel);
01537 }
01538 
01539 int mle_service_set_msg_link_layer_security_mode(uint16_t msgId, bool use_key_id_mode_2)
01540 {
01541     return mle_service_buffer_set_msg_security_mode(msgId, use_key_id_mode_2);
01542 }
01543 
01544 void mle_service_set_accept_invalid_frame_counter(bool value)
01545 {
01546     if (mle_service) {
01547         mle_service->mle_accept_invalid_frame_counter = value;
01548     }
01549 }
01550 
01551 #ifdef MLE_TEST
01552 void mle_service_receive_filter_cb_set(mle_service_filter_cb *filter_cb)
01553 {
01554     receive_filter_cb = filter_cb;
01555 }
01556 
01557 #endif