mle_service.c

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