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