beacon_handler.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 
00018 #include "nsconfig.h"
00019 #include <string.h>
00020 #include "beacon_handler.h"
00021 #include "nsdynmemLIB.h"
00022 #include "mlme.h"
00023 #include "mac_helper.h"
00024 #include "Service_Libs/load_balance/load_balance_api.h"
00025 #include "Service_Libs/pan_blacklist/pan_blacklist_api.h"
00026 #include "NWK_INTERFACE/Include/protocol.h"
00027 #include "ns_trace.h"
00028 #include "common_functions.h"
00029 
00030 
00031 #define TRACE_GROUP "BHlr"
00032 
00033 static mlme_pan_descriptor_t *duplicate_pan_descriptor(const mlme_pan_descriptor_t *desc);
00034 static nwk_pan_descriptor_t *get_local_description(uint16_t payload_length, uint8_t *payload_ptr);
00035 
00036 static uint8_t *beacon_optional_tlv_field_get(uint8_t len, uint8_t *ptr, uint8_t offset, uint8_t type);
00037 static bool beacon_join_priority_tlv_val_get(uint8_t len, uint8_t *ptr, uint8_t offset, uint8_t *join_priority);
00038 static bool beacon_join_priority_tlv_val_set(uint8_t len, uint8_t *ptr, uint8_t offset, uint8_t join_priority);
00039 static bool beacon_join_priority_tlv_add(uint8_t len, uint8_t *ptr, uint8_t offset, uint8_t join_priority);
00040 
00041 void beacon_received(int8_t if_id, const mlme_beacon_ind_t *data)
00042 {
00043     protocol_interface_info_entry_t *interface = protocol_stack_interface_info_get_by_id(if_id);
00044     if (!interface || !data) {
00045         return;
00046     }
00047 
00048     if (pan_blacklist_filter(&interface->pan_blaclist_cache, data->PANDescriptor.CoordPANId)) {
00049         tr_debug("Drop black listed beacon");
00050         return;
00051     }
00052 
00053     uint8_t coord_pan_address[10];
00054     common_write_16_bit(data->PANDescriptor.CoordPANId, coord_pan_address);
00055     memcpy(coord_pan_address + 2, data->PANDescriptor.CoordAddress, 8);
00056     if (data->PANDescriptor.CoordAddrMode == MAC_ADDR_MODE_16_BIT) {
00057         memset(coord_pan_address + 4, 0, 6);
00058     }
00059 
00060     if (pan_cordinator_blacklist_filter(&interface->pan_cordinator_black_list, coord_pan_address)) {
00061         tr_debug("Drop black listed beacon by coordinator");
00062         return;
00063     }
00064 
00065     nwk_scan_params_t *scan_params = &interface->mac_parameters->nwk_scan_params;
00066 
00067     if (!scan_params || !scan_params->active_scan_active) {
00068 
00069         if (interface->lb_api && data->PANDescriptor.CoordPANId != interface->mac_parameters->pan_id) {
00070 
00071             uint8_t priority;
00072             if (interface->mac_parameters->beacon_ind && beacon_join_priority_tlv_val_get(data->beacon_data_length, data->beacon_data, PLAIN_BEACON_PAYLOAD_SIZE, &priority)) {
00073                 if (interface->mac_parameters->beacon_ind(data->beacon_data, data->beacon_data_length, interface)) {
00074                     interface->lb_api->lb_beacon_notify(interface->lb_api, data, priority);
00075                 }
00076             }
00077         }
00078         return;
00079     }
00080 
00081     nwk_filter_params_s *filter = &(interface->mac_parameters->nwk_filter_params);
00082 
00083     if (filter->net_pan_id_filter != 0xffff) {
00084         if (filter->net_pan_id_filter != data->PANDescriptor.CoordPANId) {
00085             tr_debug("PAN-id Filter drop");
00086             return;
00087         }
00088     }
00089 
00090     tr_debug("Beacon indication: %s", trace_array(data->beacon_data, data->beacon_data_length));
00091     if (filter->nwk_active_scan_level) {
00092         bool dropBeacon = false;
00093 
00094         if (interface->mac_parameters->beacon_ind && data->beacon_data_length) {
00095             if (interface->mac_parameters->beacon_ind(data->beacon_data, data->beacon_data_length, interface) == 0) {
00096                 tr_debug("Beacon payload filter drop");
00097                 dropBeacon = true;
00098             }
00099         } else {
00100             tr_debug("Beacon dropped: No payload");
00101             dropBeacon = true;
00102         }
00103         if (dropBeacon) {
00104 
00105             return;
00106         }
00107     }
00108 
00109     //Here possible dynamic function API Call
00110     uint8_t *b_data = ns_dyn_mem_temporary_alloc(data->beacon_data_length);
00111     if (!b_data) {
00112         return;
00113     }
00114     uint16_t b_len = data->beacon_data_length;
00115     memcpy(b_data, data->beacon_data, b_len);
00116     uint8_t lqi = data->PANDescriptor.LinkQuality;
00117 
00118     if (interface->mac_parameters->beacon_compare_rx_cb_ptr) {
00119         uint8_t join_priority;
00120         if (beacon_join_priority_tlv_val_get(b_len, b_data, PLAIN_BEACON_PAYLOAD_SIZE, &join_priority)) {
00121             lqi = interface->mac_parameters->beacon_compare_rx_cb_ptr(
00122                           interface->id, join_priority, data->PANDescriptor.LinkQuality);
00123         }
00124     }
00125 
00126     if (scan_params->nwk_response_info) {
00127         nwk_pan_descriptor_t *cur = scan_params->nwk_response_info;
00128         nwk_pan_descriptor_t *prev = 0;
00129         mlme_pan_descriptor_t *cur_desc;
00130 
00131         while (cur) {
00132             cur_desc = cur->pan_descriptor;
00133             if (cur_desc->LogicalChannel == data->PANDescriptor.LogicalChannel &&
00134                     cur_desc->CoordPANId == data->PANDescriptor.CoordPANId) {
00135 
00136                 //Compare address to primary we need to check that we are not storage same parent twice FHSS
00137                 if (memcmp(cur_desc->CoordAddress, data->PANDescriptor.CoordAddress, 8) == 0) {
00138                     //Update allways better LQI
00139                     if (cur_desc->LinkQuality < lqi) {
00140                         cur_desc->LinkQuality = lqi;
00141                     }
00142 
00143                 } else {
00144 
00145                     //Compare LQI
00146                     if (cur_desc->LinkQuality < lqi) {
00147                         if (cur->beacon_length != b_len) {
00148 
00149                             if (b_len) {
00150                                 uint8_t *temp_payload = ns_dyn_mem_alloc(b_len);
00151                                 if (!temp_payload) {
00152                                     ns_dyn_mem_free(b_data);
00153                                     return;
00154                                 }
00155                                 ns_dyn_mem_free(cur->beacon_payload);
00156                                 cur->beacon_payload = temp_payload;
00157                                 memcpy(cur->beacon_payload, b_data, b_len);
00158                             } else {
00159                                 ns_dyn_mem_free(cur->beacon_payload);
00160                                 cur->beacon_payload = NULL;
00161                             }
00162                             cur->beacon_length = b_len;
00163                         }
00164 
00165                         cur->alternative_parent.CoordAddrMode = cur_desc->CoordAddrMode;
00166                         memcpy(cur->alternative_parent.CoordAddress, cur_desc->CoordAddress, 8);
00167                         cur->alternative_parent.LinkQuality = cur_desc->LinkQuality;
00168 
00169                         cur_desc->CoordAddrMode = data->PANDescriptor.CoordAddrMode;
00170                         memcpy(cur_desc->CoordAddress, data->PANDescriptor.CoordAddress, 8);
00171                         cur_desc->LinkQuality = lqi;
00172 
00173                     } else if (cur->alternative_parent.CoordAddrMode == MAC_ADDR_MODE_NONE || cur->alternative_parent.LinkQuality < lqi) {
00174                         cur->alternative_parent.CoordAddrMode = data->PANDescriptor.CoordAddrMode;
00175                         memcpy(cur->alternative_parent.CoordAddress, data->PANDescriptor.CoordAddress, 8);
00176                         cur->alternative_parent.LinkQuality = lqi;
00177                     }
00178 
00179                 }
00180                 ns_dyn_mem_free(b_data);
00181                 return;
00182             }
00183 
00184             if (cur) {
00185                 prev = cur;
00186                 cur = cur->next;
00187             }
00188         }
00189 
00190         nwk_pan_descriptor_t *new_entry = get_local_description(b_len, b_data);
00191         ns_dyn_mem_free(b_data);
00192         b_data = NULL;
00193         if (!new_entry) {
00194             return;
00195         }
00196 
00197         new_entry->pan_descriptor = duplicate_pan_descriptor(&data->PANDescriptor);
00198         if (!new_entry->pan_descriptor) {
00199             ns_dyn_mem_free(new_entry->beacon_payload);
00200             ns_dyn_mem_free(new_entry);
00201             return;
00202         }
00203         new_entry->pan_descriptor->LinkQuality = lqi;
00204         tr_debug("Beacon:Link new Last");
00205         prev->next = new_entry;
00206         scan_params->nwk_scan_res_size++;
00207 
00208     } else {
00209         nwk_pan_descriptor_t *new_entry = get_local_description(b_len, b_data);
00210         ns_dyn_mem_free(b_data);
00211         b_data = NULL;
00212         if (!new_entry) {
00213             return;
00214         }
00215 
00216         new_entry->pan_descriptor = duplicate_pan_descriptor(&data->PANDescriptor);
00217         if (!new_entry->pan_descriptor) {
00218             ns_dyn_mem_free(new_entry->beacon_payload);
00219             ns_dyn_mem_free(new_entry);
00220             return;
00221         }
00222         new_entry->pan_descriptor->LinkQuality = lqi;
00223         scan_params->nwk_response_info = new_entry;
00224         scan_params->nwk_scan_res_size++;
00225         return;
00226     }
00227 }
00228 
00229 static bool beacon_join_priority_tlv_val_get(uint8_t len, uint8_t *ptr, uint8_t offset, uint8_t *join_priority)
00230 {
00231     const uint8_t *tlv_ptr = beacon_optional_tlv_field_get(len, ptr, offset, BEACON_OPTION_JOIN_PRIORITY_TYPE);
00232 
00233     // If TLV is found and its contains value field
00234     if (tlv_ptr && ((*tlv_ptr & 0x0F) >= BEACON_OPTION_JOIN_PRIORITY_VAL_LEN)) {
00235         *join_priority = *(++tlv_ptr);
00236         return true;
00237     }
00238 
00239     return false;
00240 }
00241 
00242 static bool beacon_join_priority_tlv_val_set(uint8_t len, uint8_t *ptr, uint8_t offset, uint8_t join_priority)
00243 {
00244     uint8_t *tlv_ptr = beacon_optional_tlv_field_get(len, ptr, offset, BEACON_OPTION_JOIN_PRIORITY_TYPE);
00245 
00246     // If TLV is found and its contains value field
00247     if (tlv_ptr && ((*tlv_ptr & 0x0F) >= BEACON_OPTION_JOIN_PRIORITY_VAL_LEN)) {
00248         *(++tlv_ptr) = join_priority;
00249         return true;
00250     }
00251 
00252     return false;
00253 }
00254 
00255 static bool beacon_join_priority_tlv_add(uint8_t len, uint8_t *ptr, uint8_t offset, uint8_t join_priority)
00256 {
00257     // Invalid length
00258     if (len < offset + BEACON_OPTION_JOIN_PRIORITY_LEN) {
00259         return false;
00260     }
00261 
00262     ptr += offset;
00263     offset += BEACON_OPTION_JOIN_PRIORITY_LEN;
00264 
00265     // If there is already data adds TLV right after the plain beacon data
00266     if (len > offset) {
00267         memmove(ptr + BEACON_OPTION_JOIN_PRIORITY_LEN, ptr, len - offset);
00268     }
00269 
00270     *ptr++ = BEACON_OPTION_JOIN_PRIORITY_TYPE_LEN;
00271     *ptr = join_priority;
00272 
00273     return true;
00274 }
00275 
00276 static nwk_pan_descriptor_t *get_local_description(uint16_t payload_length, uint8_t *payload_ptr)
00277 {
00278     nwk_pan_descriptor_t *description = ns_dyn_mem_temporary_alloc(sizeof(nwk_pan_descriptor_t));
00279     if (description) {
00280         memset(description, 0, sizeof(nwk_pan_descriptor_t));
00281         if (payload_length) {
00282             description->beacon_payload = ns_dyn_mem_temporary_alloc(payload_length);
00283             if (!description->beacon_payload) {
00284                 ns_dyn_mem_free(description);
00285                 return NULL;
00286             }
00287             memcpy(description->beacon_payload, payload_ptr, payload_length);
00288             description->beacon_length = payload_length;
00289         }
00290         description->alternative_parent.CoordAddrMode = MAC_ADDR_MODE_NONE;
00291     }
00292     return description;
00293 }
00294 
00295 static uint8_t *beacon_optional_tlv_field_get(uint8_t len, uint8_t *ptr, uint8_t offset, uint8_t type)
00296 {
00297     uint8_t tlvs_len = 0;
00298 
00299     while (len > offset + tlvs_len) {
00300         uint8_t *tlv_ptr;
00301         tlv_ptr = ptr + offset + tlvs_len;
00302         if (*tlv_ptr == BEACON_OPTION_END_DELIMITER) {
00303             break;
00304             // If TLV is found
00305         } else if (*tlv_ptr >> 4 == type) {
00306             // Validates TLV length
00307             if (len >= offset + tlvs_len + 1 + (*tlv_ptr & 0x0F)) {
00308                 return tlv_ptr;
00309             } else {
00310                 break;
00311             }
00312         } else {
00313             // Length/type octet and value octets
00314             tlvs_len += 1 + (*tlv_ptr & 0x0F);
00315         }
00316     }
00317 
00318     return NULL;
00319 }
00320 
00321 static mlme_pan_descriptor_t *duplicate_pan_descriptor(const mlme_pan_descriptor_t *desc)
00322 {
00323     mlme_pan_descriptor_t *ret = ns_dyn_mem_temporary_alloc(sizeof(mlme_pan_descriptor_t));
00324     if (!ret) {
00325         return NULL;
00326     }
00327     memset(ret, 0, sizeof(mlme_pan_descriptor_t));
00328 
00329     ret->CoordAddrMode = desc->CoordAddrMode;
00330     ret->CoordPANId = desc->CoordPANId;
00331     memcpy(ret->CoordAddress, desc->CoordAddress, 8);
00332     ret->LogicalChannel = desc->LogicalChannel;
00333     ret->ChannelPage = desc->ChannelPage;
00334     memcpy(ret->SuperframeSpec, desc->SuperframeSpec, 2);
00335     ret->GTSPermit = desc->GTSPermit;
00336     ret->LinkQuality = desc->LinkQuality;
00337     ret->Timestamp = desc->Timestamp;
00338     ret->SecurityFailure = desc->SecurityFailure;
00339 
00340     ret->Key.SecurityLevel = desc->Key.SecurityLevel;
00341     ret->Key.KeyIdMode = desc->Key.KeyIdMode;
00342     ret->Key.KeyIndex = desc->Key.KeyIndex;
00343     memcpy(ret->Key.Keysource, desc->Key.Keysource, 8);
00344     return ret;
00345 }
00346 
00347 int8_t mac_beacon_link_beacon_compare_rx_callback_set(int8_t interface_id, beacon_compare_rx_cb *beacon_compare_rx_cb_ptr)
00348 {
00349     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
00350 
00351     if (!cur || !cur->mac_parameters) {
00352         return -1;
00353     }
00354 
00355     cur->mac_parameters->beacon_compare_rx_cb_ptr = beacon_compare_rx_cb_ptr;
00356 
00357     return 0;
00358 }
00359 
00360 static void mac_beacon_joining_priority_update(protocol_interface_info_entry_t *cur, uint8_t join_priority)
00361 {
00362     uint8_t beacon_payload_len = mac_helper_beacon_payload_length_get(cur);
00363     uint8_t *beacon_payload_ptr = mac_helper_beacon_payload_pointer_get(cur);
00364 
00365     // Checks if join priority TLV exists
00366     if (beacon_payload_len >= PLAIN_BEACON_PAYLOAD_SIZE + BEACON_OPTION_JOIN_PRIORITY_LEN) {
00367         // If TLV list exists
00368         if (beacon_payload_ptr[PLAIN_BEACON_PAYLOAD_SIZE] != BEACON_OPTION_END_DELIMITER) {
00369             uint8_t current_join_priority;
00370             uint8_t tlv_exists = beacon_join_priority_tlv_val_get(beacon_payload_len, beacon_payload_ptr, PLAIN_BEACON_PAYLOAD_SIZE, &current_join_priority);
00371             if (tlv_exists) {
00372                 if (current_join_priority != join_priority) {
00373                     beacon_join_priority_tlv_val_set(beacon_payload_len, beacon_payload_ptr, PLAIN_BEACON_PAYLOAD_SIZE, join_priority);
00374                     // Updates beacon to MAC
00375                     (void) mac_helper_beacon_payload_register(cur);
00376                 }
00377                 return;
00378             }
00379         }
00380     }
00381 
00382     // Adds join priority TLV as first field after plain beacon data
00383     if (beacon_payload_len >= PLAIN_BEACON_PAYLOAD_SIZE) {
00384         beacon_payload_len = beacon_payload_len + BEACON_OPTION_JOIN_PRIORITY_LEN;
00385         beacon_payload_ptr = mac_helper_beacon_payload_reallocate(cur, beacon_payload_len);
00386         beacon_join_priority_tlv_add(beacon_payload_len, beacon_payload_ptr,
00387                                      PLAIN_BEACON_PAYLOAD_SIZE, join_priority);
00388         // Updates beacon to MAC
00389         (void) mac_helper_beacon_payload_register(cur);
00390     }
00391 }
00392 
00393 int8_t mac_beacon_link_beacon_join_priority_tx_callback_set(int8_t interface_id, beacon_join_priority_tx_cb *beacon_join_priority_tx_cb_ptr)
00394 {
00395     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
00396 
00397     if (!cur || !cur->mac_parameters) {
00398         return -1;
00399     }
00400 
00401     cur->mac_parameters->beacon_join_priority_tx_cb_ptr = beacon_join_priority_tx_cb_ptr;
00402 
00403     return 0;
00404 }
00405 
00406 void beacon_join_priority_update(int8_t interface_id)
00407 {
00408     protocol_interface_info_entry_t *interface = protocol_stack_interface_info_get_by_id(interface_id);
00409 
00410     if (!interface || !interface->mac_parameters ||
00411             !interface->mac_parameters->beacon_join_priority_tx_cb_ptr) {
00412         return;
00413     }
00414 
00415     uint8_t join_priority = interface->mac_parameters->beacon_join_priority_tx_cb_ptr(interface_id);
00416     mac_beacon_joining_priority_update(interface, join_priority);
00417 }