MAC: Added address and pan id filtering for 802.15.4-2015

Jarkko Paso · Jarkko Paso · commit 78c4ae93895a · 2019-04-17T17:41:26.000+03:00
diff --git a/nanostack/platform/arm_hal_phy.h b/nanostack/platform/arm_hal_phy.h
@@ -54,6 +54,12 @@ typedef enum {
     PHY_LINK_CCA_PREPARE,       /**< Prepare for CCA after CSMA-CA: changes to CCA channel and gives permission to TX. See PHY_LINK_CCA_PREPARE status definitions for return values */
 } phy_link_tx_status_e;
 
+/** MAC filtering modes. Set corresponding bit to 1 (1 << MAC_FRAME_VERSION_X) in PHY_EXTENSION_FILTERING_SUPPORT request when PHY can handle the filtering of this frame type.
+ *  NOTE: Currently MAC supports filtering and Acking only 802.15.4-2015 frames. Any other frame version must be filtered and Acked by PHY with either HW or SW solution. */
+typedef enum {
+    MAC_FRAME_VERSION_2 = 2          /**< 802.15.4-2015 */
+} phy_link_filters_e;
+
 /** Extension types */
 typedef enum {
     PHY_EXTENSION_CTRL_PENDING_BIT, /**< Control MAC pending bit for indirect data. */
@@ -70,7 +76,8 @@ typedef enum {
     PHY_EXTENSION_GET_TIMESTAMP, /**<  Read 32-bit constant monotonic time stamp in us */
     PHY_EXTENSION_SET_CSMA_PARAMETERS, /**< CSMA parameter's are given by phy_csma_params_t structure remember type cast uint8_t pointer to structure type*/
     PHY_EXTENSION_GET_SYMBOLS_PER_SECOND, /**<  Read Symbols per seconds which will help to convert symbol time to real time  */
-    PHY_EXTENSION_SET_RF_CONFIGURATION  /**<  Set RF configuration using phy_rf_channel_parameters_s structure */
+    PHY_EXTENSION_SET_RF_CONFIGURATION,  /**<  Set RF configuration using phy_rf_channel_parameters_s structure */
+    PHY_EXTENSION_FILTERING_SUPPORT /**<  Return filtering modes that can be supported by the PHY driver. See phy_link_filters_e */
 } phy_extension_type_e;
 
 /** Address types */
diff --git a/source/MAC/IEEE802_15_4/mac_defines.h b/source/MAC/IEEE802_15_4/mac_defines.h
@@ -192,6 +192,7 @@ typedef struct protocol_interface_rf_mac_setup {
     unsigned macCurrentBE: 4;
     uint8_t macMaxCSMABackoffs;
     uint8_t backoff_period_in_10us; // max 2550us - it's 320us for standard 250kbps
+    uint8_t mac_frame_filters;
     /* MAC channel parameters */
     channel_list_s mac_channel_list;
     uint8_t scan_duration; //Needed???
@@ -294,6 +295,21 @@ typedef struct protocol_interface_rf_mac_setup {
 #define MAC_FCF_SRC_ADDR_MASK               0xc000
 #define MAC_FCF_SRC_ADDR_SHIFT              14
 
+#define MAC_FCF_DST_MODE                    0x0C
+#define MAC_FCF_DST_ADDR_NONE               0x00
+#define MAC_FCF_DST_16_BITS                 0x08
+#define MAC_FCF_DST_64_BITS                 0x0C
+#define MAC_FCF_SRC_MODE                    0xC0
+#define MAC_FCF_SEQUENCE_COMPRESSION        0x01
+#define MAC_FCF_PAN_ID_COMPRESSION          0x40
+#define MAC_FCF_SRC_64_BITS                 0xC0
+#define SHIFT_SEQ_COMP_FIELD                (0)
+#define SHIFT_VERSION_FIELD                 (4)
+#define SHIFT_PANID_COMP_FIELD              (6)
+#define VERSION_FIELD_MASK                  0x30
+#define OFFSET_DST_PAN_ID                   (3)
+#define OFFSET_DST_ADDR                     (5)
+
 /* MAC supported frame types */
 #define FC_BEACON_FRAME         0x00
 #define FC_DATA_FRAME           0x01
diff --git a/source/MAC/IEEE802_15_4/mac_mlme.c b/source/MAC/IEEE802_15_4/mac_mlme.c
@@ -1154,6 +1154,10 @@ protocol_interface_rf_mac_setup_s *mac_mlme_data_base_allocate(uint8_t *mac64, a
     bool rf_support = false;
     dev_driver->phy_driver->extension(PHY_EXTENSION_DYNAMIC_RF_SUPPORTED, (uint8_t *)&rf_support);
     entry->rf_csma_extension_supported = rf_support;
+    dev_driver->phy_driver->extension(PHY_EXTENSION_FILTERING_SUPPORT, (uint8_t *)&entry->mac_frame_filters);
+    if (entry->mac_frame_filters & (1 << MAC_FRAME_VERSION_2)) {
+        tr_debug("PHY supports 802.15.4-2015 frame filtering");
+    }
     mac_mlme_set_symbol_rate(entry);
 
     //How many 10us ticks backoff period is for waiting 20symbols which is typically 10 bytes time
diff --git a/source/MAC/IEEE802_15_4/mac_pd_sap.c b/source/MAC/IEEE802_15_4/mac_pd_sap.c
@@ -665,6 +665,120 @@ static int8_t mac_pd_sap_validate_fcf(protocol_interface_rf_mac_setup_s *rf_ptr,
     return 0;
 }
 
+static bool mac_pd_sap_panid_filter_common(const uint8_t *panid_start, uint8_t *pan_id, uint8_t frame_type)
+{
+    // PHY driver shouldn't drop received Beacon frames as they might be used by load balancing
+    if (frame_type == MAC_FRAME_BEACON) {
+        return true;
+    }
+    bool retval = true;
+    uint8_t cmp_table[2] = {0xff, 0xff};
+    if (!(pan_id[0] == 0xff && pan_id[1] == 0xff)) {
+        if (memcmp((uint8_t *)panid_start, (uint8_t *) cmp_table, 2)) {
+            retval = false;
+        }
+        if (!retval) {
+            for (uint8_t i = 0; i < 2; i++) {
+                cmp_table[1 - i] = panid_start[i];
+            }
+            if (!memcmp(pan_id, cmp_table, 2)) {
+                retval = true;
+            }
+        }
+    }
+    return retval;
+}
+
+static bool mac_pd_sap_panid_v2_filter(const uint8_t *ptr, uint8_t *pan_id, uint8_t dst_mode, uint8_t src_mode, uint8_t seq_compressed, uint8_t panid_compressed, uint8_t frame_type)
+{
+    if ((dst_mode == MAC_FCF_DST_ADDR_NONE) && (frame_type == FC_DATA_FRAME || frame_type == FC_CMD_FRAME)) {
+        return true;
+    }
+    if ((dst_mode == MAC_FCF_DST_64_BITS) && (src_mode == MAC_FCF_SRC_64_BITS) && panid_compressed) {
+        return true;
+    }
+    if (seq_compressed) {
+        ptr--;
+    }
+    return mac_pd_sap_panid_filter_common(ptr, pan_id, frame_type);
+}
+
+static bool mac_pd_sap_addr_filter_common(const uint8_t *ptr, uint8_t addr_mode, uint8_t *mac_64bit_addr, uint8_t *mac_16bit_addr)
+{
+    uint8_t cmp_table[8] = {0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff, 0xff};
+    bool retval = true;
+    switch (addr_mode) {
+        case MAC_FCF_DST_16_BITS:
+            if (memcmp((uint8_t *)ptr, (uint8_t *) cmp_table, 2)) {
+                retval = false;
+            }
+            if (!retval) {
+                for (uint8_t i = 0; i < 2; i++) {
+                    cmp_table[1 - i] = ptr[i];
+                }
+
+                if (!memcmp((uint8_t *)mac_16bit_addr, (uint8_t *) cmp_table, 2)) {
+                    retval = true;
+                }
+            }
+            break;
+        case MAC_FCF_DST_64_BITS:
+            if (memcmp((uint8_t *)ptr, (uint8_t *) cmp_table, 8)) {
+                retval = false;
+            }
+            if (!retval) {
+                for (uint8_t i = 0; i < 8; i++) {
+                    cmp_table[7 - i] = ptr[i];
+                }
+
+                if (!memcmp((uint8_t *)mac_64bit_addr, (uint8_t *) cmp_table, 8)) {
+                    retval = true;
+                }
+            }
+            break;
+        case MAC_FCF_DST_ADDR_NONE:
+            retval = true;
+            break;
+        default:
+            retval = false;
+            break;
+    }
+    return retval;
+}
+
+static bool mac_pd_sap_addr_v2_filter(const uint8_t *ptr, uint8_t *mac_64bit_addr, uint8_t *mac_16bit_addr, uint8_t dst_mode, uint8_t seq_compressed, uint8_t panid_compressed)
+{
+    if (seq_compressed) {
+        ptr--;
+    }
+    if (panid_compressed) {
+        ptr -= 2;
+    }
+    return mac_pd_sap_addr_filter_common(ptr, dst_mode, mac_64bit_addr, mac_16bit_addr);
+}
+
+static bool mac_pd_sap_rx_filter(const uint8_t *mac_header, uint8_t phy_filter_mask, uint8_t *mac_64bit_addr, uint16_t mac_16bit_addr, uint16_t pan_id)
+{
+    uint8_t dst_mode = (mac_header[1] & MAC_FCF_DST_MODE);
+    uint8_t src_mode = (mac_header[1] & MAC_FCF_SRC_MODE);
+    uint8_t seq_compressed = ((mac_header[1] & MAC_FCF_SEQUENCE_COMPRESSION) >> SHIFT_SEQ_COMP_FIELD);
+    uint8_t panid_compressed = ((mac_header[0] & MAC_FCF_PAN_ID_COMPRESSION) >> SHIFT_PANID_COMP_FIELD);
+    uint8_t frame_type = mac_header[0] & MAC_FCF_FRAME_TYPE_MASK;
+    uint8_t version = ((mac_header[1] & VERSION_FIELD_MASK) >> SHIFT_VERSION_FIELD);
+    if (version == MAC_FRAME_VERSION_2015 && !(phy_filter_mask & (1 << MAC_FRAME_VERSION_2))) {
+        uint8_t temp[2];
+        common_write_16_bit(pan_id, temp);
+        if (!mac_pd_sap_panid_v2_filter(mac_header + OFFSET_DST_PAN_ID, temp, dst_mode, src_mode, seq_compressed, panid_compressed, frame_type)) {
+            return false;
+        }
+        common_write_16_bit(mac_16bit_addr, temp);
+        if (!mac_pd_sap_addr_v2_filter(mac_header + OFFSET_DST_ADDR, mac_64bit_addr, temp, dst_mode, seq_compressed, panid_compressed)) {
+            return false;
+        }
+    }
+    return true;
+}
+
 static int8_t mac_pd_sap_generate_ack(protocol_interface_rf_mac_setup_s *rf_ptr, mac_fcf_sequence_t fcf_read, arm_pd_sap_generic_ind_t *pd_data_ind)
 {
     //Generate ACK when Extension is enabled and ACK is requested
@@ -793,6 +907,9 @@ int8_t mac_pd_sap_data_cb(void *identifier, arm_phy_sap_msg_t *message)
             if (mac_pd_sap_validate_fcf(rf_ptr, fcf_read, pd_data_ind)) {
                 goto ERROR_HANDLER;
             }
+            if (!mac_pd_sap_rx_filter(pd_data_ind->data_ptr, rf_ptr->mac_frame_filters, rf_ptr->mac64, rf_ptr->mac_short_address, rf_ptr->pan_id)) {
+                goto ERROR_HANDLER;
+            }
             if (mac_pd_sap_generate_ack(rf_ptr, fcf_read, pd_data_ind)) {
                 goto ERROR_HANDLER;
             }
