mac_header_helper_functions.c

00001 /*
00002  * Copyright (c) 2016-2018, 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 "nsconfig.h"
00018 #include "ns_types.h"
00019 #include "string.h"
00020 #include "ns_trace.h"
00021 #include "mlme.h"
00022 #include "mac_api.h"
00023 #include "fhss_api.h "
00024 #include "common_functions.h"
00025 #include "mac_common_defines.h"
00026 #include "MAC/IEEE802_15_4/mac_defines.h"
00027 #include "MAC/IEEE802_15_4/mac_mcps_sap.h"
00028 #include "MAC/IEEE802_15_4/mac_header_helper_functions.h"
00029 
00030 static uint8_t *mcps_mac_security_aux_header_start_pointer_get(const mac_pre_parsed_frame_t *buffer);
00031 static uint8_t * mac_header_information_elements_write(const mac_pre_build_frame_t *buffer, uint8_t *ptr);
00032 
00033 
00034 static uint8_t mac_fcf_lenght(const mac_fcf_sequence_t *header)
00035 {
00036     uint8_t length;
00037     if (header->frameVersion == MAC_FRAME_VERSION_2015) {
00038         if (header->sequenceNumberSuppress) {
00039             length = 2; //Skip FCF
00040         } else {
00041             length = 3; //Skip FCF + DSN
00042         }
00043     } else {
00044         length= 3; //Skip FCF + DSN
00045     }
00046     return length;
00047 }
00048 
00049 bool mac_dst_panid_present(const mac_fcf_sequence_t *header)
00050 {
00051     bool presents = false;
00052     if (header->DstAddrMode && header->SrcAddrMode) {
00053         if (header->frameVersion == MAC_FRAME_VERSION_2015) {
00054             if (header->DstAddrMode == MAC_ADDR_MODE_64_BIT && header->SrcAddrMode == MAC_ADDR_MODE_64_BIT &&  header->intraPan) {
00055 
00056             } else {
00057                 presents = true;
00058             }
00059         } else {
00060             presents = true;
00061         }
00062 
00063     } else if (header->DstAddrMode && !header->SrcAddrMode) {
00064         if (header->frameVersion == MAC_FRAME_VERSION_2015) {
00065             if (!header->intraPan) {
00066                 presents = true;
00067             }
00068         } else {
00069             presents = true;
00070         }
00071 
00072     } else if (!header->DstAddrMode && !header->SrcAddrMode) {
00073         if (header->frameVersion == MAC_FRAME_VERSION_2015) {
00074             if (header->intraPan) {
00075                 presents = true;
00076             }
00077         }
00078     }
00079 
00080     return presents;
00081 }
00082 
00083 bool mac_src_panid_present(const mac_fcf_sequence_t *header)
00084 {
00085     bool presents = false;
00086     if (header->DstAddrMode && header->SrcAddrMode) {
00087         if (header->frameVersion == MAC_FRAME_VERSION_2015) {
00088             if (header->DstAddrMode == MAC_ADDR_MODE_64_BIT && header->SrcAddrMode == MAC_ADDR_MODE_64_BIT) {
00089 
00090             } else if (!header->intraPan) {
00091                 presents = true;
00092             }
00093         } else if (!header->intraPan) {
00094             presents = true;
00095         }
00096 
00097     } else if (header->SrcAddrMode) {
00098         if (header->frameVersion == MAC_FRAME_VERSION_2015) {
00099             if (!header->intraPan) {
00100                 presents = true;
00101             }
00102         } else {
00103             presents = true;
00104         }
00105     }
00106     return presents;
00107 }
00108 
00109 uint8_t mac_address_length(uint8_t address_mode)
00110 {
00111     uint8_t length = 0;
00112     switch (address_mode) {
00113             case MAC_ADDR_MODE_NONE:
00114                 break;
00115             case MAC_ADDR_MODE_16_BIT:
00116                 length = 2;
00117                 break;
00118             case MAC_ADDR_MODE_64_BIT:
00119                 length = 8;
00120                 break;
00121         }
00122     return length;
00123 }
00124 
00125 static uint8_t mac_dst_address_length_with_panid(const mac_fcf_sequence_t *header)
00126 {
00127     uint8_t length = 0;
00128 
00129     if (header->DstPanPresents) {
00130         length += 2;
00131     }
00132 
00133     length += mac_address_length(header->DstAddrMode);
00134 
00135     return length;
00136 }
00137 
00138 static uint8_t mac_src_address_length_with_panid(const mac_fcf_sequence_t *header)
00139 {
00140     uint8_t length = 0;
00141 
00142     if (header->SrcPanPresents) {
00143         length += 2;
00144     }
00145 
00146     length += mac_address_length(header->SrcAddrMode);
00147 
00148     return length;
00149 }
00150 
00151 
00152 uint8_t mac_security_mic_length_get(uint8_t security_level)
00153 {
00154     uint8_t mic_length;
00155     switch (security_level) {
00156         case 1:
00157         case 5:
00158             mic_length = 4;
00159             break;
00160         case 2:
00161         case 6:
00162             mic_length = 8;
00163             break;
00164         case 3:
00165         case 7:
00166             mic_length = 16;
00167             break;
00168         default:
00169             mic_length = 0;
00170             break;
00171     }
00172     return mic_length;
00173 }
00174 
00175 uint8_t mac_header_security_aux_header_length(uint8_t security_level, uint8_t keyIdmode)
00176 {
00177     if (security_level == 0) {
00178         return 0;
00179     }
00180     uint8_t header_length = 5; //Header + 32-bit counter
00181     switch (keyIdmode) {
00182         case MAC_KEY_ID_MODE_SRC8_IDX:
00183             header_length += 4; //64-bit key source first part
00184             /* fall through */
00185         case MAC_KEY_ID_MODE_SRC4_IDX:
00186             header_length += 4; //32-bit key source inline
00187             /* fall through */
00188         case MAC_KEY_ID_MODE_IDX:
00189             header_length += 1;
00190             break;
00191         default:
00192 
00193             break;
00194     }
00195     return header_length;
00196 }
00197 
00198 uint8_t mac_header_address_length(const mac_fcf_sequence_t *fcf)
00199 {
00200     uint8_t address_length = 0;
00201 
00202     address_length += mac_dst_address_length_with_panid(fcf);
00203     address_length += mac_src_address_length_with_panid(fcf);
00204 
00205     return address_length;
00206 
00207 }
00208 
00209 void mac_header_security_parameter_set(mac_aux_security_header_t *header, const mlme_security_t *frame_setup)
00210 {
00211     header->securityLevel = frame_setup->SecurityLevel;
00212 
00213     if (header->securityLevel) {
00214         uint8_t keysource_len = 0;
00215 
00216         header->KeyIdMode = frame_setup->KeyIdMode;
00217         switch (header->KeyIdMode) {
00218             case MAC_KEY_ID_MODE_IMPLICIT:
00219                 //Security header + 32-bit security counter
00220                 break;
00221 
00222             case MAC_KEY_ID_MODE_SRC8_IDX:
00223                 keysource_len += 4; //64-bit key source first part
00224                 /* fall through */
00225             case MAC_KEY_ID_MODE_SRC4_IDX:
00226                 keysource_len += 4; //32-bit key source inline
00227                 /* fall through */
00228             case MAC_KEY_ID_MODE_IDX:
00229                 //Security header + 32-bit security counter + Key index
00230                 header->KeyIndex = frame_setup->KeyIndex;
00231                 break;
00232         }
00233         if (keysource_len) {
00234             memcpy(header->Keysource, frame_setup->Keysource , keysource_len);
00235         }
00236     }
00237 }
00238 
00239 const uint8_t * mac_header_parse_fcf_dsn(mac_fcf_sequence_t *header, const uint8_t *ptr)
00240 {
00241     uint16_t fcf = common_read_16_bit_inverse(ptr);
00242     ptr += 2;
00243 
00244     //Read Frame Type
00245     header->frametype = ((fcf & MAC_FCF_FRAME_TYPE_MASK) >> MAC_FCF_FRAME_TYPE_SHIFT);
00246     header->securityEnabled = ((fcf & MAC_FCF_SECURITY_BIT_MASK) >> MAC_FCF_SECURITY_BIT_SHIFT);
00247     header->framePending = ((fcf & MAC_FCF_PENDING_BIT_MASK) >> MAC_FCF_PENDING_BIT_SHIFT);
00248     header->ackRequested = ((fcf & MAC_FCF_ACK_REQ_BIT_MASK) >> MAC_FCF_ACK_REQ_BIT_SHIFT);
00249     header->intraPan = ((fcf & MAC_FCF_INTRA_PANID_MASK ) >> MAC_FCF_INTRA_PANID_SHIFT);
00250 
00251     header->DstAddrMode = ((fcf & MAC_FCF_DST_ADDR_MASK ) >> MAC_FCF_DST_ADDR_SHIFT);
00252     header->frameVersion = ((fcf & MAC_FCF_VERSION_MASK) >> MAC_FCF_VERSION_SHIFT);
00253     header->SrcAddrMode = ((fcf & MAC_FCF_SRC_ADDR_MASK ) >> MAC_FCF_SRC_ADDR_SHIFT);
00254 
00255     if (header->frameVersion == MAC_FRAME_VERSION_2015 ) {
00256         header->sequenceNumberSuppress = ((fcf & MAC_FCF_SEQ_NUM_SUPPRESS_MASK ) >> MAC_FCF_SEQ_NUM_SUPPRESS_SHIFT);
00257         header->informationElementsPresets = ((fcf & MAC_FCF_IE_PRESENTS_MASK ) >> MAC_FCF_IE_PRESENTS_SHIFT);
00258     } else {
00259         //SET False to ALL 2015 Extension's by default
00260         header->sequenceNumberSuppress = false;
00261         header->informationElementsPresets = false;
00262     }
00263 
00264     if (header->frameVersion < MAC_FRAME_VERSION_2015 || (header->frameVersion ==  MAC_FRAME_VERSION_2015 &&  !header->sequenceNumberSuppress)) {
00265         header->DSN = *ptr++;
00266     } else {
00267         header->DSN = 0;
00268     }
00269     return ptr;
00270 
00271 }
00272 
00273 static uint8_t * mac_header_write_fcf_dsn(const mac_fcf_sequence_t *header, uint8_t *ptr)
00274 {
00275     uint16_t fcf= 0;
00276     //Read Frame Type
00277     fcf |= (header->frametype << MAC_FCF_FRAME_TYPE_SHIFT);
00278     fcf |= (header->securityEnabled << MAC_FCF_SECURITY_BIT_SHIFT);
00279     fcf |= (header->framePending << MAC_FCF_PENDING_BIT_SHIFT);
00280     fcf |= (header->ackRequested << MAC_FCF_ACK_REQ_BIT_SHIFT);
00281     fcf |= (header->intraPan << MAC_FCF_INTRA_PANID_SHIFT);
00282     fcf |= (header->sequenceNumberSuppress << MAC_FCF_SEQ_NUM_SUPPRESS_SHIFT);
00283     fcf |= (header->informationElementsPresets << MAC_FCF_IE_PRESENTS_SHIFT);
00284     fcf |= (header->DstAddrMode << MAC_FCF_DST_ADDR_SHIFT);
00285     fcf |= (header->frameVersion << MAC_FCF_VERSION_SHIFT);
00286     fcf |= (header->SrcAddrMode << MAC_FCF_SRC_ADDR_SHIFT);
00287     ptr = common_write_16_bit_inverse(fcf,ptr);
00288     if (header->frameVersion < MAC_FRAME_VERSION_2015 || (header->frameVersion ==  MAC_FRAME_VERSION_2015 &&  !header->sequenceNumberSuppress)) {
00289         *ptr++ = header->DSN;
00290     }
00291     return ptr;
00292 }
00293 
00294 uint16_t mac_header_off_set_to_aux_header(const mac_fcf_sequence_t *fcf)
00295 {
00296     //Skip first FCF & address field
00297     uint16_t offset = mac_fcf_lenght(fcf);//Skip FCF + DSN
00298     offset += mac_dst_address_length_with_panid(fcf);
00299     offset += mac_address_length(fcf->SrcAddrMode);
00300     if (fcf->SrcPanPresents) {
00301         offset += 2; //Skip PanId
00302     }
00303     return offset;
00304 }
00305 
00306 void mac_header_security_aux_header_parse(const uint8_t *ptr, mlme_security_t *security_params)
00307 {
00308     uint8_t key_source_len = 0;
00309     security_params->KeyIdMode = (*ptr >> 3);
00310     security_params->SecurityLevel = *ptr++;
00311     ptr += 4; //Skip Frame counter
00312     switch (security_params->KeyIdMode) {
00313         case MAC_KEY_ID_MODE_IMPLICIT:
00314             break;
00315         case MAC_KEY_ID_MODE_SRC8_IDX:
00316             key_source_len += 4;
00317             /* fall through */
00318         case MAC_KEY_ID_MODE_SRC4_IDX:
00319             key_source_len += 4;
00320             memcpy(security_params->Keysource, ptr, key_source_len);
00321             ptr += key_source_len;
00322             /* fall through */
00323         case MAC_KEY_ID_MODE_IDX:
00324             security_params->KeyIndex = *ptr;
00325             break;
00326     }
00327 }
00328 
00329 void mac_header_security_components_read(mac_pre_parsed_frame_t *buffer, mlme_security_t *security_params)
00330 {
00331     memset(security_params, 0, sizeof(mlme_security_t));
00332     if (!buffer->fcf_dsn.securityEnabled) {
00333         return;
00334     }
00335 
00336     mac_header_security_aux_header_parse(mcps_mac_security_aux_header_start_pointer_get(buffer), security_params);
00337 
00338 }
00339 
00340 uint16_t mac_header_get_src_panid(const mac_fcf_sequence_t *header, const uint8_t *ptr)
00341 {
00342 
00343     if (!header->SrcPanPresents) {
00344         if (!header->DstPanPresents) {
00345             return 0xffff;
00346         }
00347         return mac_header_get_dst_panid(header, ptr);
00348     }
00349 
00350     ptr += mac_fcf_lenght(header);//Skip FCF + DSN
00351 
00352     ptr += mac_dst_address_length_with_panid(header); //Skip Dst panID & Address
00353 
00354     return common_read_16_bit_inverse(ptr);
00355 }
00356 
00357 uint16_t mac_header_get_dst_panid(const mac_fcf_sequence_t *header, const uint8_t *ptr)
00358 {
00359     if (!header->DstPanPresents) {
00360         return 0xffff;
00361     }
00362 
00363     ptr += mac_fcf_lenght(header);//Skip FCF + DSN
00364 
00365     return common_read_16_bit_inverse(ptr);
00366 }
00367 
00368 void mac_header_get_src_address(const mac_fcf_sequence_t *header, const uint8_t *ptr, uint8_t *address_ptr)
00369 {
00370 
00371     if (header->SrcAddrMode == MAC_ADDR_MODE_NONE) {
00372         return;
00373     }
00374 
00375     ptr += mac_fcf_lenght(header);//Skip FCF + DSN
00376 
00377     ptr += mac_dst_address_length_with_panid(header);
00378 
00379     uint8_t address_len, address_index, i;
00380 
00381     address_len = mac_address_length(header->SrcAddrMode);
00382 
00383     if (header->SrcPanPresents) {
00384         ptr += 2; //Skip PanId
00385     }
00386     address_index = address_len - 1;
00387 
00388 
00389     for (i = 0; i < address_len; i++) {
00390         address_ptr[address_index - i] = *ptr++;
00391     }
00392 }
00393 
00394 void mac_header_get_dst_address(const mac_fcf_sequence_t *header, const uint8_t *ptr, uint8_t *address_ptr)
00395 {
00396 
00397     if (header->DstAddrMode == MAC_ADDR_MODE_NONE) {
00398         return;
00399     }
00400     uint8_t address_len, address_index, i;
00401 
00402     ptr += mac_fcf_lenght(header);//Skip FCF + DSN
00403 
00404     address_len = mac_address_length(header->DstAddrMode);
00405 
00406     if (header->DstPanPresents) {
00407         ptr += 2; //Skip PanId
00408     }
00409     address_index = address_len - 1;
00410 
00411     for (i = 0; i < address_len; i++) {
00412         address_ptr[address_index - i] = *ptr++;
00413     }
00414 }
00415 
00416 static uint16_t mac_payload_length_calc_with_ie(uint16_t payload_length, uint16_t payload_ie_length)
00417 {
00418     uint16_t length = payload_length;
00419     if (payload_ie_length) {
00420         if (length) {
00421             length += 2;
00422         }
00423         length += payload_ie_length;
00424     }
00425     return length;
00426 }
00427 
00428 uint8_t mcps_mac_header_length_from_received_frame(const mac_pre_parsed_frame_t *buffer)
00429 {
00430     return (buffer->mac_header_length + buffer->security_aux_header_length + buffer->header_ie_length);
00431 }
00432 
00433 uint8_t *mcps_mac_payload_pointer_get(const mac_pre_parsed_frame_t *buffer)
00434 {
00435     uint8_t *ptr = (uint8_t *) mac_header_message_start_pointer(buffer);
00436     ptr += mcps_mac_header_length_from_received_frame(buffer);
00437     return ptr;
00438 }
00439 
00440 uint8_t *mcps_security_mic_pointer_get(const mac_pre_parsed_frame_t *buffer)
00441 {
00442     uint8_t *ptr = mcps_mac_payload_pointer_get(buffer) + buffer->mac_payload_length;
00443     return ptr;
00444 }
00445 
00446 static uint8_t *mcps_mac_security_aux_header_start_pointer_get(const mac_pre_parsed_frame_t *buffer)
00447 {
00448     if (!buffer->fcf_dsn.securityEnabled) {
00449         return NULL;
00450     }
00451     return (uint8_t *) (mac_header_message_start_pointer(buffer) + buffer->mac_header_length);
00452 }
00453 
00454 uint8_t mcps_mac_command_frame_id_get(const mac_pre_parsed_frame_t *buffer)
00455 {
00456     const uint8_t *ptr = mcps_mac_payload_pointer_get(buffer);
00457     return *ptr;
00458 }
00459 
00460 uint32_t mcps_mac_security_frame_counter_read(const mac_pre_parsed_frame_t *buffer)
00461 {
00462     if (!buffer->fcf_dsn.securityEnabled) {
00463         return 0xffffffff;
00464     }
00465 
00466     const uint8_t *ptr = (mac_header_message_start_pointer(buffer) + buffer->mac_header_length + 1);
00467     return common_read_32_bit_inverse(ptr);
00468 
00469 }
00470 
00471 static uint8_t *mcps_mac_frame_address_write(uint8_t *ptr, uint8_t addressType, const uint8_t *addressPtr)
00472 {
00473     if (addressType == MAC_ADDR_MODE_16_BIT) {
00474         uint16_t tempMac16 = common_read_16_bit(addressPtr);
00475         ptr = common_write_16_bit_inverse(tempMac16, ptr);
00476     } else if (addressType == MAC_ADDR_MODE_64_BIT) {
00477         uint8_t i;
00478         for (i = 0; i < 8; i++) {
00479             *ptr++ = addressPtr[7 - i];
00480         }
00481     }
00482     return ptr;
00483 }
00484 
00485 static uint8_t *mac_security_interface_aux_security_header_write(uint8_t *ptr, const mac_aux_security_header_t *auxHeader)
00486 {
00487     uint8_t auxBaseHeader;
00488     auxBaseHeader = auxHeader->securityLevel;
00489     auxBaseHeader |= (auxHeader->KeyIdMode << 3);
00490     *ptr++ = auxBaseHeader;
00491     ptr = common_write_32_bit_inverse(auxHeader->frameCounter, ptr);
00492 
00493     switch (auxHeader->KeyIdMode) {
00494         case MAC_KEY_ID_MODE_SRC8_IDX:
00495             memcpy(ptr, auxHeader->Keysource, 8);
00496             ptr += 8;
00497             *ptr++ = auxHeader->KeyIndex;
00498             break;
00499         case MAC_KEY_ID_MODE_SRC4_IDX:
00500             memcpy(ptr, auxHeader->Keysource, 4);
00501             ptr += 4;
00502             *ptr++ = auxHeader->KeyIndex;
00503             break;
00504         case MAC_KEY_ID_MODE_IDX:
00505             *ptr++ = auxHeader->KeyIndex;
00506             break;
00507         default:
00508             break;
00509     }
00510     return ptr;
00511 }
00512 
00513 uint8_t * mac_generic_packet_write(struct protocol_interface_rf_mac_setup *rf_ptr, uint8_t *ptr, const mac_pre_build_frame_t *buffer)
00514 {
00515     ptr = mac_header_write_fcf_dsn(&buffer->fcf_dsn, ptr);
00516 
00517     if (buffer->fcf_dsn.DstPanPresents) {
00518         ptr = common_write_16_bit_inverse(buffer->DstPANId, ptr);
00519     }
00520 
00521     if (buffer->fcf_dsn.DstAddrMode) {
00522         //Write DST
00523         ptr = mcps_mac_frame_address_write(ptr, buffer->fcf_dsn.DstAddrMode,  buffer->DstAddr);
00524     }
00525 
00526     if (buffer->fcf_dsn.SrcPanPresents) {
00527         ptr = common_write_16_bit_inverse(buffer->SrcPANId, ptr);
00528     }
00529 
00530     if (buffer->fcf_dsn.SrcAddrMode ) {
00531         ptr = mcps_mac_frame_address_write(ptr, buffer->fcf_dsn.SrcAddrMode, buffer->SrcAddr);
00532     }
00533 
00534     if (buffer->fcf_dsn.securityEnabled) {
00535         ptr = mac_security_interface_aux_security_header_write(ptr, &buffer->aux_header);
00536     }
00537     uint8_t *ie_start = ptr;
00538     //Copy Payload and set IE Elemets
00539     ptr = mac_header_information_elements_write(buffer, ptr);
00540     if (buffer->mac_payload_length) {
00541         memcpy(ptr, buffer->mac_payload, buffer->mac_payload_length );
00542         ptr += buffer->mac_payload_length;
00543     }
00544     if (rf_ptr->fhss_api) {
00545         rf_ptr->fhss_api->write_synch_info(rf_ptr->fhss_api, ie_start, buffer->headerIeLength, FHSS_DATA_FRAME, buffer->tx_time);
00546     }
00547     return ptr;
00548 }
00549 
00550 static uint8_t *mac_write_ie_vector_list(ns_ie_iovec_t *list, uint16_t length, uint8_t *ptr)
00551 {
00552     const ns_ie_iovec_t *msg_iov = list;
00553     for (uint_fast16_t i = 0; i < length; i++, msg_iov++) {
00554         memcpy(ptr, msg_iov->ieBase, msg_iov->iovLen);
00555         ptr += msg_iov->iovLen;
00556     }
00557     return ptr;
00558 }
00559 
00560 static bool mac_parse_header_ie(mac_header_IE_t *header_element, uint8_t *ptr)
00561 {
00562     uint16_t ie_dummy = common_read_16_bit_inverse(ptr);
00563     if (ie_dummy & 0x8000) {
00564         return false;
00565     }
00566     header_element->length = (ie_dummy & 0x007f);
00567     header_element->id = ((ie_dummy & 0x7f80  ) >> 7 );
00568     header_element->content_ptr = ptr + 2;
00569     return true;
00570 }
00571 
00572 static bool mac_parse_payload_ie(mac_payload_IE_t *payload_element, uint8_t *ptr)
00573 {
00574     uint16_t ie_dummy = common_read_16_bit_inverse(ptr);
00575     if (!(ie_dummy & 0x8000)) {
00576         return false;
00577     }
00578     payload_element->length = (ie_dummy & 0x07ff);
00579     payload_element->id = ((ie_dummy & 0x7800  ) >> 11);
00580     payload_element->content_ptr = ptr + 2;
00581     return true;
00582 }
00583 
00584 
00585 bool mac_header_information_elements_parse(mac_pre_parsed_frame_t *buffer)
00586 {
00587     uint8_t *ptr = (mac_header_message_start_pointer(buffer) + buffer->mac_header_length + buffer->security_aux_header_length);
00588 
00589     buffer->headerIePtr = NULL;
00590     buffer->headerIeLength = 0;
00591     buffer->payloadsIePtr = NULL;
00592     buffer->payloadsIeLength = 0;
00593     buffer->header_ie_length = 0;
00594     if (!buffer->fcf_dsn.informationElementsPresets) {
00595         buffer->macPayloadPtr = ptr;
00596         return true;
00597     }
00598 
00599     if (buffer->mac_payload_length < 2) {
00600         return false;
00601     }
00602 
00603     mac_header_IE_t header_ie;
00604     buffer->headerIePtr = ptr;
00605 
00606     while (buffer->mac_payload_length >= 2) {
00607 
00608         if (!mac_parse_header_ie(&header_ie, ptr)) {
00609             return false;
00610         }
00611 
00612         buffer->mac_payload_length -= 2;
00613         if (header_ie.length > buffer->mac_payload_length) {
00614             return false;
00615         }
00616 
00617         buffer->mac_payload_length -= header_ie.length;
00618 
00619         buffer->header_ie_length  += header_ie.length +2;
00620         ptr += (2 + header_ie.length);
00621 
00622         if (header_ie.id == MAC_HEADER_TERMINATION1_IE_ID) {
00623             break;
00624         } else if (header_ie.id == MAC_HEADER_TERMINATION2_IE_ID) {
00625             buffer->macPayloadPtr = ptr;
00626             return true;
00627         }
00628         buffer->headerIeLength += header_ie.length +2;
00629     }
00630 
00631     return true;
00632 }
00633 
00634 
00635 bool mac_payload_information_elements_parse(mac_pre_parsed_frame_t *buffer)
00636 {
00637     uint8_t *ptr = (mac_header_message_start_pointer(buffer) + buffer->mac_header_length + buffer->security_aux_header_length + buffer->header_ie_length);
00638     if (!buffer->fcf_dsn.informationElementsPresets) {
00639         return true;
00640     }
00641 
00642     if (buffer->mac_payload_length < 2) {
00643         return false;
00644     }
00645 
00646     //Parse Payload IE
00647     buffer->payloadsIePtr = ptr;
00648     mac_payload_IE_t payload_ie;
00649     while (buffer->mac_payload_length >= 2) {
00650 
00651         if (!mac_parse_payload_ie(&payload_ie, ptr)) {
00652             return false;
00653         }
00654         buffer->mac_payload_length -= 2;
00655         if (payload_ie.length > buffer->mac_payload_length) {
00656             return false;
00657         }
00658         buffer->mac_payload_length -= payload_ie.length;
00659 
00660         if (payload_ie.id == MAC_PAYLOAD_TERMINATION_IE_GROUP_ID) {
00661             break;
00662         }
00663         buffer->payloadsIeLength += payload_ie.length + 2;
00664         buffer->macPayloadPtr += payload_ie.length + 2;
00665         ptr += (2 + payload_ie.length);
00666 
00667     }
00668     buffer->macPayloadPtr = ptr;
00669     return true;
00670 }
00671 
00672 
00673 static uint8_t *mac_header_ie_terimate(uint8_t *ptr, uint8_t type)
00674 {
00675     uint16_t ie_dummy = 0;
00676     ie_dummy |= (type << 7 );
00677     return common_write_16_bit_inverse(ie_dummy, ptr);
00678 
00679 
00680 }
00681 
00682 static uint8_t *mac_payload_ie_terimate(uint8_t *ptr)
00683 {
00684     uint16_t ie_dummy = 0;
00685     ie_dummy |= (MAC_PAYLOAD_TERMINATION_IE_GROUP_ID << 11 );
00686     ie_dummy |= (1 << 15);
00687     return common_write_16_bit_inverse(ie_dummy, ptr);
00688 }
00689 
00690 
00691 void mac_header_information_elements_preparation(mac_pre_build_frame_t *buffer)
00692 {
00693     if (buffer->message_builded) {
00694         return;
00695     }
00696 
00697     if (buffer->headerIeLength ||  buffer->payloadsIeLength) {
00698         buffer->fcf_dsn.frameVersion = MAC_FRAME_VERSION_2015;
00699         buffer->fcf_dsn.informationElementsPresets = true;
00700         buffer->message_builded = true;
00701         //Write Header elements
00702         if (buffer->headerIeLength) {
00703             buffer->mac_header_length_with_security += buffer->headerIeLength;
00704             if (!buffer->payloadsIeLength && !buffer->mac_payload_length) {
00705                 //No termination needed
00706                 return;
00707             }
00708             //Terminate
00709             buffer->mac_header_length_with_security += 2;
00710         } else {
00711             buffer->mac_header_length_with_security += 2;
00712         }
00713     }
00714 }
00715 
00716 uint16_t mac_buffer_total_payload_length(mac_pre_build_frame_t *buffer)
00717 {
00718     return mac_payload_length_calc_with_ie(buffer->mac_payload_length, buffer->payloadsIeLength);
00719 }
00720 
00721 
00722 static uint8_t * mac_header_information_elements_write(const mac_pre_build_frame_t *buffer, uint8_t *ptr)
00723 {
00724     if (buffer->fcf_dsn.frameVersion == MAC_FRAME_VERSION_2015 && buffer->fcf_dsn.informationElementsPresets) {
00725         //Write Header elements
00726         if (buffer->headerIeLength) {
00727             ptr = mac_write_ie_vector_list(buffer->ie_elements.headerIeVectorList, buffer->ie_elements.headerIovLength, ptr);
00728 
00729             if (!buffer->payloadsIeLength && !buffer->mac_payload_length) {
00730                 //No termination needed
00731                 return ptr;
00732             } else if (!buffer->payloadsIeLength && buffer->mac_payload_length) {
00733                 return mac_header_ie_terimate(ptr, MAC_HEADER_TERMINATION2_IE_ID);
00734             }
00735         }
00736         //Add Header Termination
00737         ptr = mac_header_ie_terimate(ptr, MAC_HEADER_TERMINATION1_IE_ID);
00738 
00739         if (buffer->payloadsIeLength) {
00740             ptr = mac_write_ie_vector_list(buffer->ie_elements.payloadIeVectorList, buffer->ie_elements.payloadIovLength, ptr);
00741             if (buffer->mac_payload_length) {
00742                 ptr = mac_payload_ie_terimate(ptr);
00743             }
00744         }
00745     }
00746     return ptr;
00747 }
00748