Important changes to repositories hosted on mbed.com
Mbed hosted mercurial repositories are deprecated and are due to be permanently deleted in July 2026.
To keep a copy of this software download the repository Zip archive or clone locally using Mercurial.
It is also possible to export all your personal repositories from the account settings page.
Dependents: TYBLE16_simple_data_logger TYBLE16_MP3_Air
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, ¤t_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 }
Generated on Tue Jul 12 2022 13:54:02 by
