Embed:
(wiki syntax)
Show/hide line numbers
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
Generated on Tue Jul 12 2022 12:22:12 by
1.7.2