mac_header_helper_functions.c

00001 /*
00002  * Copyright (c) 2016-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 #include "ns_types.h"
00018 #include "string.h"
00019 #include "mlme.h"
00020 #include "mac_api.h"
00021 #include "common_functions.h"
00022 #include "mac_common_defines.h"
00023 #include "MAC/IEEE802_15_4/mac_defines.h"
00024 #include "MAC/IEEE802_15_4/mac_mcps_sap.h"
00025 #include "MAC/IEEE802_15_4/mac_header_helper_functions.h"
00026 
00027 
00028 
00029 uint8_t mac_security_mic_length_get(uint8_t security_level)
00030 {
00031     uint8_t mic_length;
00032     switch (security_level) {
00033         case 1:
00034         case 5:
00035             mic_length = 4;
00036             break;
00037         case 2:
00038         case 6:
00039             mic_length = 8;
00040             break;
00041         case 3:
00042         case 7:
00043             mic_length = 16;
00044             break;
00045         default:
00046             mic_length = 0;
00047             break;
00048     }
00049     return mic_length;
00050 }
00051 
00052 uint8_t mac_header_security_aux_header_length(uint8_t security_level, uint8_t keyIdmode)
00053 {
00054     if (security_level == 0) {
00055         return 0;
00056     }
00057     uint8_t header_length = 5; //Header + 32-bit counter
00058     switch (keyIdmode) {
00059         case MAC_KEY_ID_MODE_SRC8_IDX:
00060             header_length += 4; //64-bit key source first part
00061         case MAC_KEY_ID_MODE_SRC4_IDX:
00062             header_length += 4; //32-bit key source inline
00063         case MAC_KEY_ID_MODE_IDX:
00064             header_length += 1;
00065             break;
00066         default:
00067 
00068             break;
00069     }
00070     return header_length;
00071 }
00072 
00073 uint8_t mac_header_address_length(const mac_fcf_sequence_t *fcf)
00074 {
00075     uint8_t address_length = 0;
00076     if( !fcf ){
00077         return address_length;
00078     }
00079 
00080     if(fcf->DstAddrMode && fcf->SrcAddrMode) {
00081         if (fcf->DstAddrMode == MAC_ADDR_MODE_16_BIT) {
00082             address_length = 4;
00083         } else {
00084             address_length = 10;
00085         }
00086 
00087         if (fcf->SrcAddrMode == MAC_ADDR_MODE_16_BIT) {
00088             address_length += 2;
00089         } else {
00090             address_length += 8;
00091         }
00092 
00093         if (!fcf->intraPan) {
00094             address_length += 2;
00095         }
00096     } else if (fcf->DstAddrMode) {
00097         if (fcf->DstAddrMode == MAC_ADDR_MODE_16_BIT) {
00098             address_length = 4;
00099         } else {
00100             address_length = 10;
00101         }
00102     } else if (fcf->SrcAddrMode){
00103         if (fcf->SrcAddrMode == MAC_ADDR_MODE_16_BIT) {
00104             address_length = 4;
00105         } else {
00106             address_length = 10;
00107         }
00108     }
00109     return address_length;
00110 
00111 }
00112 
00113 void mac_header_security_parameter_set(mac_aux_security_header_t *header, const mlme_security_t *frame_setup)
00114 {
00115     if( !header || !frame_setup ){
00116         return;
00117     }
00118     header->securityLevel = frame_setup->SecurityLevel;
00119 
00120     if (header->securityLevel) {
00121         uint8_t keysource_len = 0;
00122 
00123         header->KeyIdMode = frame_setup->KeyIdMode;
00124         switch (header->KeyIdMode) {
00125             case MAC_KEY_ID_MODE_IMPLICIT:
00126                 //Security header + 32-bit security counter
00127                 break;
00128 
00129             case MAC_KEY_ID_MODE_SRC8_IDX:
00130                 keysource_len += 4; //64-bit key source first part
00131             case MAC_KEY_ID_MODE_SRC4_IDX:
00132                 keysource_len += 4; //32-bit key source inline
00133             case MAC_KEY_ID_MODE_IDX:
00134                 //Security header + 32-bit security counter + Key index
00135                 header->KeyIndex = frame_setup->KeyIndex;
00136                 break;
00137         }
00138         if (keysource_len) {
00139             memcpy(header->Keysource, frame_setup->Keysource , keysource_len);
00140         }
00141     }
00142 }
00143 
00144 void mac_header_parse_fcf_dsn(mac_fcf_sequence_t *header, const uint8_t *ptr)
00145 {
00146     if( !header || !ptr ){
00147         return;
00148     }
00149     uint16_t fcf = common_read_16_bit_inverse(ptr);
00150     ptr += 2;
00151     header->DSN = *ptr;
00152 
00153     //Read Frame Type
00154     header->frametype = ((fcf & MAC_FCF_FRAME_TYPE_MASK) >> MAC_FCF_FRAME_TYPE_SHIFT);
00155     header->securityEnabled = ((fcf & MAC_FCF_SECURITY_BIT_MASK) >> MAC_FCF_SECURITY_BIT_SHIFT);
00156     header->framePending = ((fcf & MAC_FCF_PENDING_BIT_MASK) >> MAC_FCF_PENDING_BIT_SHIFT);
00157     header->ackRequested = ((fcf & MAC_FCF_ACK_REQ_BIT_MASK) >> MAC_FCF_ACK_REQ_BIT_SHIFT);
00158     header->intraPan = ((fcf & MAC_FCF_INTRA_PANID_MASK ) >> MAC_FCF_INTRA_PANID_SHIFT);
00159     header->DstAddrMode = ((fcf & MAC_FCF_DST_ADDR_MASK ) >> MAC_FCF_DST_ADDR_SHIFT);
00160     header->frameVersion = ((fcf & MAC_FCF_VERSION_MASK) >> MAC_FCF_VERSION_SHIFT);
00161     header->SrcAddrMode = ((fcf & MAC_FCF_SRC_ADDR_MASK ) >> MAC_FCF_SRC_ADDR_SHIFT);
00162 }
00163 
00164 static uint8_t * mac_header_write_fcf_dsn(const mac_fcf_sequence_t *header, uint8_t *ptr)
00165 {
00166     uint16_t fcf= 0;
00167     //Read Frame Type
00168     fcf |= (header->frametype << MAC_FCF_FRAME_TYPE_SHIFT);
00169     fcf |= (header->securityEnabled << MAC_FCF_SECURITY_BIT_SHIFT);
00170     fcf |= (header->framePending << MAC_FCF_PENDING_BIT_SHIFT);
00171     fcf |= (header->ackRequested << MAC_FCF_ACK_REQ_BIT_SHIFT);
00172     fcf |= (header->intraPan << MAC_FCF_INTRA_PANID_SHIFT);
00173     fcf |= (header->DstAddrMode << MAC_FCF_DST_ADDR_SHIFT);
00174     fcf |= (header->frameVersion << MAC_FCF_VERSION_SHIFT);
00175     fcf |= (header->SrcAddrMode << MAC_FCF_SRC_ADDR_SHIFT);
00176     ptr = common_write_16_bit_inverse(fcf,ptr);
00177     *ptr++ = header->DSN;
00178     return ptr;
00179 }
00180 
00181 void mac_header_security_components_read(mac_pre_parsed_frame_t *buffer, mlme_security_t *security_params)
00182 {
00183     if( !buffer || !security_params ){
00184         return;
00185     }
00186     uint8_t *ptr = mcps_mac_security_aux_header_start_pointer_get(buffer);
00187     memset(security_params, 0, sizeof(mlme_security_t));
00188     uint8_t key_source_len = 0;
00189     if (!buffer->fcf_dsn.securityEnabled) {
00190         return;
00191     }
00192 
00193     security_params->KeyIdMode = (*ptr >> 3);
00194     security_params->SecurityLevel = *ptr++;
00195     ptr += 4;
00196     switch (security_params->KeyIdMode) {
00197         case MAC_KEY_ID_MODE_IMPLICIT:
00198             break;
00199         case MAC_KEY_ID_MODE_SRC8_IDX:
00200             key_source_len += 4;
00201         case MAC_KEY_ID_MODE_SRC4_IDX:
00202             key_source_len += 4;
00203             memcpy(security_params->Keysource, ptr, key_source_len);
00204             ptr += key_source_len;
00205         case MAC_KEY_ID_MODE_IDX:
00206             security_params->KeyIndex = *ptr;
00207             break;
00208     }
00209 }
00210 
00211 uint16_t mac_header_get_src_panid(const mac_fcf_sequence_t *header, const uint8_t *ptr)
00212 {
00213     if( !header || !ptr ){
00214         return 0;
00215     }
00216     ptr += 3; //Skip FCF + DSN
00217 
00218     if (!header->intraPan) {
00219         switch (header->DstAddrMode) {
00220             case MAC_ADDR_MODE_NONE:
00221                 break;
00222             case MAC_ADDR_MODE_16_BIT:
00223                 ptr += 4;
00224                 break;
00225             case MAC_ADDR_MODE_64_BIT:
00226                 ptr += 10;
00227                 break;
00228         }
00229     }
00230     uint16_t panid = 0;
00231     panid += *ptr++;
00232     panid += (uint16_t)(*ptr++) << 8;
00233 
00234     return panid;
00235 }
00236 
00237 uint16_t mac_header_get_dst_panid(const mac_fcf_sequence_t *header, const uint8_t *ptr)
00238 {
00239     if( !header || !ptr ){
00240         return 0;
00241     }
00242     if (header->DstAddrMode == MAC_ADDR_MODE_NONE) {
00243         return 0xffff;
00244     }
00245     ptr += 3;
00246     uint16_t panid = 0;
00247     panid += *ptr++;
00248     panid += (uint16_t)(*ptr++) << 8;
00249 
00250     return panid;
00251 }
00252 
00253 void mac_header_get_src_address(const mac_fcf_sequence_t *header, const uint8_t *ptr, uint8_t *address_ptr)
00254 {
00255     if( !header || !ptr || !address_ptr ){
00256         return;
00257     }
00258     ptr += 3; //Skip FCF + DSN
00259     switch (header->DstAddrMode) {
00260         case MAC_ADDR_MODE_NONE:
00261             ptr += 2;
00262             break;
00263         case MAC_ADDR_MODE_16_BIT:
00264             ptr += 4;
00265             if (!header->intraPan) {
00266                 ptr += 2;
00267             }
00268             break;
00269         case MAC_ADDR_MODE_64_BIT:
00270             ptr += 10;
00271             if (!header->intraPan) {
00272                 ptr += 2;
00273             }
00274             break;
00275     }
00276 
00277 
00278     if (header->SrcAddrMode == MAC_ADDR_MODE_NONE) {
00279         return;
00280     }
00281     uint8_t address_len, address_index, i;
00282     if (header->SrcAddrMode == MAC_ADDR_MODE_16_BIT) {
00283         address_len = 2;
00284         address_index = 1;
00285     } else  {
00286         address_len = 8;
00287         address_index = 7;
00288     }
00289 
00290     for (i = 0; i < address_len; i++) {
00291         address_ptr[address_index - i] = *ptr++;
00292     }
00293 }
00294 
00295 void mac_header_get_dst_address(const mac_fcf_sequence_t *header, const uint8_t *ptr, uint8_t *address_ptr)
00296 {
00297     if( !header || !ptr || !address_ptr ){
00298         return;
00299     }
00300 
00301     if (header->DstAddrMode == MAC_ADDR_MODE_NONE) {
00302         return;
00303     }
00304     uint8_t address_len, address_index, i;
00305     if (header->DstAddrMode == MAC_ADDR_MODE_16_BIT) {
00306 
00307         address_len = 2;
00308         address_index = 1;
00309     } else  {
00310         address_len = 8;
00311         address_index = 7;
00312     }
00313     ptr += 5; //Skip fcf, dsn & PANID
00314 
00315     for (i = 0; i < address_len; i++) {
00316         address_ptr[address_index - i] = *ptr++;
00317     }
00318 }
00319 
00320 uint16_t mcps_payload_length_from_received_frame(const mac_pre_parsed_frame_t *buffer)
00321 {
00322     if( !buffer ){
00323         return 0;
00324     }
00325     return buffer->mac_payload_length;
00326 }
00327 
00328 uint8_t mcps_mac_header_length_from_received_frame(const mac_pre_parsed_frame_t *buffer)
00329 {
00330     if( !buffer ){
00331         return 0;
00332     }
00333     return (buffer->mac_header_length + buffer->security_aux_header_length);
00334 }
00335 
00336 uint8_t *mcps_mac_payload_pointer_get(const mac_pre_parsed_frame_t *buffer)
00337 {
00338     if( !buffer ){
00339         return NULL;
00340     }
00341     uint8_t *ptr = (uint8_t *) mac_header_message_start_pointer(buffer);
00342     ptr += (buffer->mac_header_length + buffer->security_aux_header_length);
00343     return ptr;
00344 }
00345 
00346 uint8_t *mcps_security_mic_pointer_get(const mac_pre_parsed_frame_t *buffer)
00347 {
00348     if (!buffer) {
00349         return NULL;
00350     }
00351     uint8_t *ptr = mcps_mac_payload_pointer_get(buffer);
00352     return (ptr + buffer->mac_payload_length);
00353 }
00354 
00355 uint8_t *mcps_mac_security_aux_header_start_pointer_get(const mac_pre_parsed_frame_t *buffer)
00356 {
00357     if (!buffer || !buffer->fcf_dsn.securityEnabled) {
00358         return NULL;
00359     }
00360     return (uint8_t *) (mac_header_message_start_pointer(buffer) + buffer->mac_header_length);
00361 }
00362 
00363 uint8_t mcps_mac_command_frame_id_get(const mac_pre_parsed_frame_t *buffer)
00364 {
00365     if ( !buffer ) {
00366         return 0;
00367     }
00368     const uint8_t *ptr = mcps_mac_payload_pointer_get(buffer);
00369     return *ptr;
00370 }
00371 
00372 uint32_t mcps_mac_security_frame_counter_read(const mac_pre_parsed_frame_t *buffer)
00373 {
00374     if (!buffer || !buffer->fcf_dsn.securityEnabled) {
00375         return 0xffffffff;
00376     }
00377 
00378     const uint8_t *ptr = (mac_header_message_start_pointer(buffer) + buffer->mac_header_length + 1);
00379     return common_read_32_bit_inverse(ptr);
00380 
00381 }
00382 
00383 
00384 static uint8_t *mcps_mac_frame_address_write(uint8_t *ptr, uint8_t addressType, const uint8_t *addressPtr)
00385 {
00386     if (addressType == MAC_ADDR_MODE_16_BIT) {
00387         uint16_t tempMac16 = common_read_16_bit(addressPtr);
00388         ptr = common_write_16_bit_inverse(tempMac16, ptr);
00389     } else if (addressType == MAC_ADDR_MODE_64_BIT) {
00390         uint8_t i;
00391         for (i = 0; i < 8; i++) {
00392             *ptr++ = addressPtr[7 - i];
00393         }
00394     }
00395     return ptr;
00396 }
00397 
00398 uint8_t * mcps_generic_header_write(uint8_t *ptr, const mac_pre_build_frame_t *buffer)
00399 {
00400     ptr = mac_header_write_fcf_dsn(&buffer->fcf_dsn, ptr);
00401 
00402     if (buffer->fcf_dsn.DstAddrMode) {
00403         ptr = common_write_16_bit_inverse(buffer->DstPANId, ptr);
00404         //Write DST
00405         ptr = mcps_mac_frame_address_write(ptr, buffer->fcf_dsn.DstAddrMode,  buffer->DstAddr);
00406     }
00407 
00408     if (buffer->fcf_dsn.SrcAddrMode ){
00409         if (!(buffer->fcf_dsn.intraPan)) {
00410             ptr = common_write_16_bit_inverse(buffer->SrcPANId, ptr);
00411         }
00412         ptr = mcps_mac_frame_address_write(ptr, buffer->fcf_dsn.SrcAddrMode, buffer->SrcAddr);
00413     }
00414     return ptr;
00415 }
00416