mle_service.c

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