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