mac_ie_lib.c

00001 /*
00002  * Copyright (c) 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 "common_functions.h"
00021 #include "mac_common_defines.h"
00022 #include "mac_ie_lib.h"
00023 
00024 
00025 #define MAC_IE_HEADER_LENGTH_MASK 0x007f
00026 #define MAC_IE_HEADER_ID_MASK 0x7f80
00027 #define MAC_IE_PAYLOAD_LENGTH_MASK 0x07ff
00028 #define MAC_IE_PAYLOAD_ID_MASK 0x7800
00029 #define MAC_IE_TYPE_PAYLOAD_MASK 0x8000
00030 
00031 #define MAC_NESTED_LONG_IE_PAYLOAD_LENGTH_MASK 0x07ff
00032 #define MAC_NESTED_LONG_IE_PAYLOAD_ID_MASK 0x7800
00033 #define MAC_NESTED_SHORT_IE_PAYLOAD_LENGTH_MASK 0x00ff
00034 #define MAC_NESTED_SHORT_IE_PAYLOAD_ID_MASK 0x7f00
00035 #define MAC_NESTED_IE_TYPE_LONG_MASK 0x8000
00036 
00037 static void mac_ie_header_parse(mac_header_IE_t *header_element, uint8_t *ptr)
00038 {
00039     uint16_t ie_dummy = common_read_16_bit_inverse(ptr);
00040     header_element->length = (ie_dummy & MAC_IE_HEADER_LENGTH_MASK);
00041     header_element->id = ((ie_dummy & MAC_IE_HEADER_ID_MASK) >> 7);
00042     header_element->content_ptr = ptr + 2;
00043 }
00044 
00045 static void mac_ie_payload_parse(mac_payload_IE_t *payload_element, uint8_t *ptr)
00046 {
00047     uint16_t ie_dummy = common_read_16_bit_inverse(ptr);
00048     payload_element->length = (ie_dummy & MAC_IE_PAYLOAD_LENGTH_MASK);
00049     payload_element->id = ((ie_dummy & MAC_IE_PAYLOAD_ID_MASK) >> 11);
00050     payload_element->content_ptr = ptr + 2;
00051 }
00052 
00053 static void mac_ie_nested_id_parse(mac_nested_payload_IE_t *element, uint8_t *ptr)
00054 {
00055     uint16_t ie_dummy = common_read_16_bit_inverse(ptr);
00056 
00057     if (ie_dummy & MAC_NESTED_IE_TYPE_LONG_MASK) {
00058         element->type_long = true;
00059         element->length = (ie_dummy & MAC_NESTED_LONG_IE_PAYLOAD_LENGTH_MASK);
00060         element->id = ((ie_dummy & MAC_NESTED_LONG_IE_PAYLOAD_ID_MASK) >> 11);
00061     } else {
00062         element->type_long = false;
00063         element->length = (ie_dummy & MAC_NESTED_SHORT_IE_PAYLOAD_LENGTH_MASK);
00064         element->id = ((ie_dummy & MAC_NESTED_SHORT_IE_PAYLOAD_ID_MASK) >> 8);
00065     }
00066 
00067     element->content_ptr = ptr + 2;
00068 }
00069 
00070 
00071 uint8_t *mac_ie_header_base_write(uint8_t *ptr, uint8_t type, uint16_t length)
00072 {
00073     uint16_t ie_dummy = 0; //Header Type
00074     ie_dummy |= (length & MAC_IE_HEADER_LENGTH_MASK);
00075     ie_dummy |= ((type << 7) &  MAC_IE_HEADER_ID_MASK);
00076     return common_write_16_bit_inverse(ie_dummy, ptr);
00077 }
00078 
00079 uint8_t *mac_ie_payload_base_write(uint8_t *ptr, uint8_t type, uint16_t length)
00080 {
00081 
00082     uint16_t ie_dummy = MAC_IE_TYPE_PAYLOAD_MASK; //Payload type
00083     ie_dummy |= (length & MAC_IE_PAYLOAD_LENGTH_MASK);
00084     ie_dummy |= ((type << 11) &  MAC_IE_PAYLOAD_ID_MASK);
00085     return common_write_16_bit_inverse(ie_dummy, ptr);
00086 }
00087 
00088 uint8_t *mac_ie_nested_ie_long_base_write(uint8_t *ptr, uint8_t sub_id, uint16_t length)
00089 {
00090     uint16_t ie_dummy = MAC_NESTED_IE_TYPE_LONG_MASK;
00091     ie_dummy |= (length & MAC_NESTED_LONG_IE_PAYLOAD_LENGTH_MASK);
00092     ie_dummy |= ((sub_id << 11) &  MAC_NESTED_LONG_IE_PAYLOAD_ID_MASK);
00093 
00094     return common_write_16_bit_inverse(ie_dummy, ptr);
00095 }
00096 
00097 uint8_t *mac_ie_nested_ie_short_base_write(uint8_t *ptr, uint8_t sub_id, uint16_t length)
00098 {
00099     uint16_t ie_dummy = 0;
00100     ie_dummy |= (length & MAC_NESTED_SHORT_IE_PAYLOAD_LENGTH_MASK);
00101     ie_dummy |= ((sub_id << 8) &  MAC_NESTED_SHORT_IE_PAYLOAD_ID_MASK);
00102 
00103     return common_write_16_bit_inverse(ie_dummy, ptr);
00104 }
00105 
00106 uint16_t mac_ie_payload_discover(uint8_t *payload_ptr, uint16_t length, mac_payload_IE_t *payload_ie)
00107 {
00108     mac_payload_IE_t ie_element;
00109     while (length >= 2) {
00110         mac_ie_payload_parse(&ie_element, payload_ptr);
00111         if (payload_ie->id == ie_element.id) {
00112             payload_ie->content_ptr = ie_element.content_ptr;
00113             payload_ie->length = ie_element.length;
00114             return ie_element.length;
00115         }
00116 
00117         length -= ie_element.length + 2;
00118 
00119         payload_ptr += ie_element.length + 2;
00120     }
00121     return 0;
00122 }
00123 
00124 uint16_t mac_ie_nested_discover(uint8_t *payload_ptr, uint16_t length, mac_nested_payload_IE_t *nested_ie)
00125 {
00126     mac_nested_payload_IE_t ie_element;
00127     while (length >= 2) {
00128         mac_ie_nested_id_parse(&ie_element, payload_ptr);
00129 
00130         if (length < ie_element.length + 2) {
00131             return 0;
00132         }
00133 
00134         if (nested_ie->id == ie_element.id && nested_ie->type_long == ie_element.type_long) {
00135             nested_ie->content_ptr = ie_element.content_ptr;
00136             nested_ie->length = ie_element.length;
00137             return ie_element.length;
00138         }
00139 
00140         length -= ie_element.length + 2;
00141 
00142         payload_ptr += ie_element.length + 2;
00143     }
00144     return 0;
00145 }
00146 
00147 uint8_t mac_ie_header_discover(uint8_t *header_ptr, uint16_t length, mac_header_IE_t *header_ie)
00148 {
00149     mac_header_IE_t ie_element;
00150     while (length >= 2) {
00151         mac_ie_header_parse(&ie_element, header_ptr);
00152         if (header_ie->id == ie_element.id) {
00153             header_ie->content_ptr = ie_element.content_ptr;
00154             header_ie->length = ie_element.length;
00155             return ie_element.length;
00156         }
00157 
00158         length -= ie_element.length + 2;
00159 
00160         header_ptr += ie_element.length + 2;
00161     }
00162     return 0;
00163 }
00164 
00165 uint8_t mac_ie_header_sub_id_discover(uint8_t *header_ptr, uint16_t length, mac_header_IE_t *header_ie, uint8_t sub_id)
00166 {
00167     mac_header_IE_t ie_element;
00168     uint8_t *sub_id_ptr;
00169     while (length > 2) {
00170         mac_ie_header_parse(&ie_element, header_ptr);
00171         sub_id_ptr = ie_element.content_ptr;
00172         if (ie_element.length && header_ie->id == ie_element.id && *sub_id_ptr == sub_id) {
00173             sub_id_ptr++;
00174             ie_element.length--;
00175             header_ie->content_ptr = sub_id_ptr;
00176             header_ie->length = ie_element.length;
00177             return ie_element.length;
00178         }
00179 
00180         length -= ie_element.length + 2;
00181 
00182         header_ptr += ie_element.length + 2;
00183     }
00184     return 0;
00185 }