beacon_handler.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 
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     nwk_pan_descriptor_t *description = ns_dyn_mem_temporary_alloc(sizeof(nwk_pan_descriptor_t));
00278     if (description) {
00279         memset(description, 0, sizeof(nwk_pan_descriptor_t));
00280         if (payload_length) {
00281             description->beacon_payload = ns_dyn_mem_temporary_alloc(payload_length);
00282             if (!description->beacon_payload) {
00283                 ns_dyn_mem_free(description);
00284                 return NULL;
00285             }
00286             memcpy(description->beacon_payload, payload_ptr, payload_length);
00287             description->beacon_length = payload_length;
00288         }
00289         description->alternative_parent.CoordAddrMode = MAC_ADDR_MODE_NONE;
00290     }
00291     return description;
00292 }
00293 
00294 static uint8_t *beacon_optional_tlv_field_get(uint8_t len, uint8_t *ptr, uint8_t offset, uint8_t type)
00295 {
00296     uint8_t tlvs_len = 0;
00297 
00298     while (len > offset + tlvs_len) {
00299         uint8_t *tlv_ptr;
00300         tlv_ptr = ptr + offset + tlvs_len;
00301         if (*tlv_ptr == BEACON_OPTION_END_DELIMITER) {
00302             break;
00303         // If TLV is found
00304         } else if (*tlv_ptr >> 4 == type) {
00305             // Validates TLV length
00306             if (len >= offset + tlvs_len + 1 + (*tlv_ptr & 0x0F)) {
00307                 return tlv_ptr;
00308             } else {
00309                 break;
00310             }
00311         } else {
00312             // Length/type octet and value octets
00313             tlvs_len += 1 + (*tlv_ptr & 0x0F);
00314         }
00315     }
00316 
00317     return NULL;
00318 }
00319 
00320 static mlme_pan_descriptor_t *duplicate_pan_descriptor(const mlme_pan_descriptor_t *desc)
00321 {
00322     mlme_pan_descriptor_t *ret = ns_dyn_mem_temporary_alloc(sizeof(mlme_pan_descriptor_t));
00323     if(!ret){
00324         return NULL;
00325     }
00326     memset(ret, 0, sizeof(mlme_pan_descriptor_t));
00327 
00328     ret->CoordAddrMode = desc->CoordAddrMode;
00329     ret->CoordPANId = desc->CoordPANId;
00330     memcpy(ret->CoordAddress, desc->CoordAddress, 8);
00331     ret->LogicalChannel = desc->LogicalChannel;
00332     ret->ChannelPage = desc->ChannelPage;
00333     memcpy(ret->SuperframeSpec, desc->SuperframeSpec, 2);
00334     ret->GTSPermit = desc->GTSPermit;
00335     ret->LinkQuality = desc->LinkQuality;
00336     ret->Timestamp = desc->Timestamp;
00337     ret->SecurityFailure = desc->SecurityFailure;
00338 
00339     ret->Key.SecurityLevel = desc->Key.SecurityLevel;
00340     ret->Key.KeyIdMode = desc->Key.KeyIdMode;
00341     ret->Key.KeyIndex = desc->Key.KeyIndex;
00342     memcpy(ret->Key.Keysource, desc->Key.Keysource, 8);
00343     return ret;
00344 }
00345 
00346 int8_t mac_beacon_link_beacon_compare_rx_callback_set(int8_t interface_id, beacon_compare_rx_cb *beacon_compare_rx_cb_ptr)
00347 {
00348     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
00349 
00350     if (!cur || !cur->mac_parameters) {
00351         return -1;
00352     }
00353 
00354     cur->mac_parameters->beacon_compare_rx_cb_ptr = beacon_compare_rx_cb_ptr;
00355 
00356     return 0;
00357 }
00358 
00359 static void mac_beacon_joining_priority_update(protocol_interface_info_entry_t *cur, uint8_t join_priority)
00360 {
00361     uint8_t beacon_payload_len = mac_helper_beacon_payload_length_get(cur);
00362     uint8_t *beacon_payload_ptr = mac_helper_beacon_payload_pointer_get(cur);
00363 
00364     // Checks if join priority TLV exists
00365     if (beacon_payload_len >= PLAIN_BEACON_PAYLOAD_SIZE + BEACON_OPTION_JOIN_PRIORITY_LEN) {
00366         // If TLV list exists
00367         if (beacon_payload_ptr[PLAIN_BEACON_PAYLOAD_SIZE] != BEACON_OPTION_END_DELIMITER) {
00368             uint8_t current_join_priority;
00369             uint8_t tlv_exists = beacon_join_priority_tlv_val_get(beacon_payload_len, beacon_payload_ptr, PLAIN_BEACON_PAYLOAD_SIZE, &current_join_priority);
00370             if (tlv_exists) {
00371                 if (current_join_priority != join_priority) {
00372                     beacon_join_priority_tlv_val_set(beacon_payload_len, beacon_payload_ptr, PLAIN_BEACON_PAYLOAD_SIZE, join_priority);
00373                     // Updates beacon to MAC
00374                     (void) mac_helper_beacon_payload_register(cur);
00375                 }
00376                 return;
00377             }
00378         }
00379     }
00380 
00381     // Adds join priority TLV as first field after plain beacon data
00382     if (beacon_payload_len >= PLAIN_BEACON_PAYLOAD_SIZE) {
00383         beacon_payload_len = beacon_payload_len + BEACON_OPTION_JOIN_PRIORITY_LEN;
00384         beacon_payload_ptr = mac_helper_beacon_payload_reallocate(cur, beacon_payload_len);
00385         beacon_join_priority_tlv_add(beacon_payload_len, beacon_payload_ptr,
00386                                      PLAIN_BEACON_PAYLOAD_SIZE, join_priority);
00387         // Updates beacon to MAC
00388         (void) mac_helper_beacon_payload_register(cur);
00389     }
00390 }
00391 
00392 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)
00393 {
00394     protocol_interface_info_entry_t *cur = protocol_stack_interface_info_get_by_id(interface_id);
00395 
00396     if (!cur || !cur->mac_parameters) {
00397         return -1;
00398     }
00399 
00400     cur->mac_parameters->beacon_join_priority_tx_cb_ptr = beacon_join_priority_tx_cb_ptr;
00401 
00402     return 0;
00403 }
00404 
00405 void beacon_join_priority_update(int8_t interface_id)
00406 {
00407     protocol_interface_info_entry_t *interface = protocol_stack_interface_info_get_by_id(interface_id);
00408 
00409     if (!interface || !interface->mac_parameters ||
00410         !interface->mac_parameters->beacon_join_priority_tx_cb_ptr) {
00411         return;
00412     }
00413 
00414     uint8_t join_priority = interface->mac_parameters->beacon_join_priority_tx_cb_ptr(interface_id);
00415     mac_beacon_joining_priority_update(interface, join_priority);
00416 }