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