mac_filter.c

00001 /*
00002  * Copyright (c) 2014-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 "ns_types.h"
00021 #include "ns_list.h"
00022 #include "ns_trace.h"
00023 #include "common_functions.h"
00024 #include "nsdynmemLIB.h"
00025 #include "mac_filter_api.h"
00026 #include "mac_filter.h"
00027 #include "mac_common_defines.h"
00028 
00029 #include "MAC/IEEE802_15_4/mac_mcps_sap.h"
00030 #include "MAC/IEEE802_15_4/mac_header_helper_functions.h"
00031 
00032 #define TRACE_GROUP "mFil"
00033 
00034 // Signal floor assumed to be the noise level of radio for packet we cannot hear
00035 #define MAC_FILTER_SIGNAL_FLOOR -94
00036 
00037 // define this if you want extra debug info for this file
00038 //#define EXTRA_DEBUG_INFO
00039 
00040 #ifdef EXTRA_DEBUG_INFO
00041 #define tr_debug_extra(...) tr_debug(__VA_ARGS__)
00042 #else
00043 #define tr_debug_extra(...)
00044 #endif
00045 
00046 typedef struct filter_struct {
00047 
00048     union address_union {
00049         uint16_t mac16;
00050         uint8_t mac64[8];
00051     } address;
00052     int16_t lqi_m;
00053     int16_t lqi_add;
00054     int16_t dbm_m;
00055     int16_t dbm_add;
00056     ns_list_link_t link;
00057 } filter_t;
00058 
00059 typedef NS_LIST_HEAD (filter_t, link) filter_list_t;
00060 
00061 typedef struct instance_struct {
00062     filter_list_t short_filter_list;
00063     filter_list_t long_filter_list;
00064     mac_filter_map_short_to_extended_cb *resolve_long_cb;
00065     mac_filter_map_extented_to_short_cb *resolve_short_cb;
00066     bool enabled:1;
00067     int8_t interface_id;
00068     int16_t lqi_m;
00069     int16_t lqi_add;
00070     int16_t dbm_m;
00071     int16_t dbm_add;
00072     ns_list_link_t link;
00073 } filter_instance_t;
00074 
00075 static NS_LIST_DEFINE(instance_list, filter_instance_t, link);
00076 
00077 static filter_instance_t *filter_instance_find(int8_t interface_id)
00078 {
00079     filter_instance_t *this = NULL;
00080     ns_list_foreach(filter_instance_t, cur_ptr, &instance_list) {
00081         if (cur_ptr->interface_id == interface_id) {
00082             this = cur_ptr;
00083             break;
00084         }
00085     }
00086     return this;
00087 }
00088 static filter_t *filter_find_long(filter_instance_t *instance_ptr, uint8_t mac64[8])
00089 {
00090     filter_t *this = NULL;
00091     tr_debug_extra("mac_filter_find_long %s",trace_array(mac64,8));
00092 
00093     ns_list_foreach(filter_t, cur_ptr, &instance_ptr->long_filter_list) {
00094         if (memcmp(cur_ptr->address.mac64, mac64, 8) == 0) {
00095             this = cur_ptr;
00096             break;
00097         }
00098     }
00099     return this;
00100 }
00101 
00102 static filter_t *filter_find_short(filter_instance_t *instance_ptr, uint16_t mac16)
00103 {
00104     filter_t *this = NULL;
00105 
00106     ns_list_foreach(filter_t, cur_ptr, &instance_ptr->short_filter_list) {
00107         if (cur_ptr->address.mac16 == mac16) {
00108             this = cur_ptr;
00109             break;
00110         }
00111     }
00112     return this;
00113 }
00114 static filter_instance_t *mac_filter_create(int8_t interface_id)
00115 {
00116     filter_instance_t *this;
00117     tr_debug_extra("mac_filter_create");
00118     this = ns_dyn_mem_alloc(sizeof(filter_instance_t));
00119     if (!this) {
00120         return NULL;
00121     }
00122     memset(this, 0 , sizeof(filter_instance_t));
00123     this->interface_id = interface_id;
00124     this->enabled = false;
00125     ns_list_init(&this->long_filter_list);
00126     ns_list_init(&this->short_filter_list);
00127     ns_list_add_to_start(&instance_list, this);
00128     return this;
00129 }
00130 
00131 static void mac_filter_delete(filter_instance_t *this)
00132 {
00133     tr_debug_extra("mac_filter_delete");
00134     ns_list_remove(&instance_list, this);
00135     ns_dyn_mem_free(this);
00136     return;
00137 }
00138 
00139 
00140 /**
00141  * Public functions.
00142  */
00143 
00144 int_fast8_t mac_filter_start(int8_t interface_id, int16_t lqi_m, int16_t lqi_add, int16_t dbm_m, int16_t dbm_add)
00145 {
00146     filter_instance_t *this = filter_instance_find(interface_id);
00147 
00148     tr_debug_extra("mac_filter_start");
00149     if (!this) {
00150         this = mac_filter_create(interface_id);
00151     }
00152     if (!this) {
00153         return -1;
00154     }
00155     this->enabled = true;
00156     this->lqi_m = lqi_m;
00157     this->lqi_add = lqi_add;
00158     this->dbm_m = dbm_m;
00159     this->dbm_add = dbm_add;
00160     return 0;
00161 }
00162 
00163 void mac_filter_stop(int8_t interface_id)
00164 {
00165     filter_instance_t *this = filter_instance_find(interface_id);
00166     tr_debug_extra("mac_filter_stop");
00167     if (!this)
00168         return;
00169 
00170     mac_filter_clear(interface_id);
00171     this->enabled = false;
00172     if (!this->resolve_long_cb && !this->resolve_short_cb && !this->enabled) {
00173         // no usage anymore we can safely delete this
00174         mac_filter_delete(this);
00175     }
00176 }
00177 
00178 int_fast8_t mac_filter_set_address_mapping(int8_t interface_id, mac_filter_map_short_to_extended_cb *long_cb, mac_filter_map_extented_to_short_cb *short_cb)
00179 {
00180     filter_instance_t *this = filter_instance_find(interface_id);
00181 
00182     tr_debug_extra("mac_filter_set_address_mapping");
00183     if (!this) {
00184         this = mac_filter_create(interface_id);
00185     }
00186     if (!this) {
00187         return -1;
00188     }
00189     this->resolve_long_cb = long_cb;
00190     this->resolve_short_cb = short_cb;
00191     if (!this->resolve_long_cb && !this->resolve_short_cb && !this->enabled) {
00192         // no usage anymore we can safely delete this
00193         mac_filter_delete(this);
00194     }
00195     return 0;
00196 }
00197 
00198 int_fast8_t mac_filter_clear(int8_t interface_id)
00199 {
00200     filter_instance_t *this = filter_instance_find(interface_id);
00201     tr_debug_extra("mac_filter_clear");
00202     if (!this)
00203         return -1;
00204 
00205     ns_list_foreach_safe(filter_t, cur_ptr, &this->long_filter_list) {
00206         ns_list_remove(&this->long_filter_list, cur_ptr);
00207         ns_dyn_mem_free(cur_ptr);
00208     }
00209     ns_list_foreach_safe(filter_t, cur_ptr, &this->short_filter_list) {
00210         ns_list_remove(&this->short_filter_list, cur_ptr);
00211         ns_dyn_mem_free(cur_ptr);
00212     }
00213     return 0;
00214 }
00215 
00216 int_fast8_t mac_filter_delete_short(int8_t interface_id, uint16_t mac16)
00217 {
00218     filter_instance_t *this = filter_instance_find(interface_id);
00219     filter_t *filter_ptr;
00220     if (!this)
00221         return -1;
00222     filter_ptr = filter_find_short(this,mac16);
00223     if (filter_ptr) {
00224         ns_list_remove(&this->short_filter_list, filter_ptr);
00225         ns_dyn_mem_free(filter_ptr);
00226     }
00227     return 0;
00228 }
00229 
00230 int_fast8_t mac_filter_delete_long(int8_t interface_id, uint8_t mac64[8])
00231 {
00232     filter_instance_t *this = filter_instance_find(interface_id);
00233     filter_t *filter_ptr;
00234     if (!this || !mac64)
00235         return -1;
00236     filter_ptr = filter_find_long(this,mac64);
00237     if (filter_ptr) {
00238         ns_list_remove(&this->short_filter_list, filter_ptr);
00239         ns_dyn_mem_free(filter_ptr);
00240     }
00241     return 0;
00242 }
00243 
00244 int_fast8_t mac_filter_add_short(int8_t interface_id, uint16_t mac16, int16_t lqi_m, int16_t lqi_add, int16_t dbm_m, int16_t dbm_add)
00245 {
00246     filter_instance_t *this = filter_instance_find(interface_id);
00247     filter_t *filter_ptr;
00248     if (!this)
00249         return -1;
00250     filter_ptr = ns_dyn_mem_alloc(sizeof(filter_t));
00251     if (!filter_ptr)
00252         return -2;
00253 
00254     ns_list_add_to_end(&this->short_filter_list, filter_ptr);
00255     filter_ptr->address.mac16 = mac16;
00256     filter_ptr->lqi_m = lqi_m;
00257     filter_ptr->lqi_add = lqi_add;
00258     filter_ptr->dbm_m = dbm_m;
00259     filter_ptr->dbm_add = dbm_add;
00260 
00261     return 0;
00262 }
00263 
00264 int_fast8_t mac_filter_add_long(int8_t interface_id, uint8_t mac64[8], int16_t lqi_m, int16_t lqi_add, int16_t dbm_m, int16_t dbm_add)
00265 {
00266     filter_instance_t *this = filter_instance_find(interface_id);
00267     filter_t *filter_ptr;
00268     tr_debug_extra("mac_filter_add_long mac %s filter found lqi_m %d lqi_add %d dbm_m %d dbm_add %d",trace_array(mac64,8),lqi_m,lqi_add,dbm_m,dbm_add);
00269     if (!this || !mac64)
00270         return -1;
00271     filter_ptr = ns_dyn_mem_alloc(sizeof(filter_t));
00272     if (!filter_ptr)
00273         return -2;
00274 
00275     ns_list_add_to_end(&this->long_filter_list, filter_ptr);
00276     memcpy(filter_ptr->address.mac64,mac64,8);
00277     filter_ptr->lqi_m = lqi_m;
00278     filter_ptr->lqi_add = lqi_add;
00279     filter_ptr->dbm_m = dbm_m;
00280     filter_ptr->dbm_add = dbm_add;
00281 
00282     return 0;
00283 }
00284 
00285 int_fast8_t mac_filter_modify_link_quality(int8_t interface_id, mac_pre_parsed_frame_t *mac_frame)
00286 {
00287     filter_instance_t *this = filter_instance_find(interface_id);
00288     filter_t *filter_ptr = NULL;
00289     int16_t lqi_m;
00290     int16_t lqi_add;
00291     int16_t dbm_m;
00292     int16_t dbm_add;
00293 
00294     if (!this || !mac_frame)
00295         return 0;
00296 
00297     if (!this->enabled)
00298         return 0;
00299 
00300     tr_debug_extra("mac_filter_modify_link_quality lqi %d dbm %d",mac_frame->LQI,mac_frame->dbm);
00301 
00302     if (mac_frame->fcf_dsn.SrcAddrMode == MAC_ADDR_MODE_NONE) {
00303         return 0;
00304     }
00305     uint8_t srcAddress[8];
00306     mac_header_get_src_address(&mac_frame->fcf_dsn, mac_header_message_start_pointer(mac_frame), srcAddress);
00307 
00308 
00309 //Find filter for specific address
00310     if (mac_frame->fcf_dsn.SrcAddrMode == MAC_ADDR_MODE_16_BIT) {
00311         uint16_t mac16 = common_read_16_bit(srcAddress);
00312         filter_ptr = filter_find_short(this,mac16);
00313         if (!filter_ptr && this->resolve_long_cb) {
00314             // Get the long mapping
00315             uint8_t mac64[8];
00316             if (this->resolve_long_cb(this->interface_id, mac64, mac16) == 0) {
00317                 filter_ptr = filter_find_long(this,mac64);
00318             }
00319         }
00320     } else {
00321         filter_ptr = filter_find_long(this,srcAddress);
00322         if (!filter_ptr && this->resolve_short_cb) {
00323             // Get the short mapping
00324             uint16_t mac16;
00325             if (this->resolve_short_cb(this->interface_id, srcAddress, &mac16) == 0) {
00326                 filter_ptr = filter_find_short(this,mac16);
00327             }
00328         }
00329     }
00330 
00331     // check if filter found or use the defaults
00332     if (filter_ptr) {
00333         lqi_m = filter_ptr->lqi_m;
00334         lqi_add = filter_ptr->lqi_add;
00335         dbm_m = filter_ptr->dbm_m;
00336         dbm_add = filter_ptr->dbm_add;
00337         tr_debug_extra("mac_filter_modify_link_quality filter found lqi_m %d lqi_add %d dbm_m %d dbm_add %d",lqi_m,lqi_add,dbm_m,dbm_add);
00338     } else {
00339         lqi_m = this->lqi_m;
00340         lqi_add = this->lqi_add;
00341         dbm_m = this->dbm_m;
00342         dbm_add = this->dbm_add;
00343     }
00344 
00345 
00346     //calculate
00347     int16_t lqi = ( (mac_frame->LQI * lqi_m ) >> 8 ) + lqi_add;
00348     // Saturate
00349     if (lqi > 255) {
00350         mac_frame->LQI = 255;
00351     } else  if (lqi < 0) {
00352         mac_frame->LQI = 0;
00353     } else {
00354         mac_frame->LQI = lqi;
00355     }
00356 
00357     //calculate
00358     int16_t dbm = ( ((mac_frame->dbm - MAC_FILTER_SIGNAL_FLOOR) * dbm_m ) >> 8 ) + dbm_add + MAC_FILTER_SIGNAL_FLOOR;
00359     // Saturate
00360     if (dbm > 127) {
00361         mac_frame->dbm = 127;
00362     } else  if (dbm < -128) {
00363         mac_frame->dbm = -128;
00364     } else {
00365         mac_frame->dbm = dbm;
00366     }
00367 
00368     tr_debug_extra("mac_filter_modify_link_quality result lqi %d dbm %d",mac_frame->LQI,mac_frame->dbm);
00369     // If quality goes below treshold packet is dropped
00370     if ( ( mac_frame->dbm < MAC_FILTER_SIGNAL_FLOOR ) || ( mac_frame->LQI < 1 )) {
00371         tr_debug_extra("Filter dropped packet");
00372         return 1;
00373     }
00374     return 0;
00375 }
00376