wifi: rtw89: introudce functions to drop packets

Introudce a H2C feature to drop packets according to given parameters.
And, we implement instances to drop packets from BE, BK, VI, VO queues
by vif or sta. Then, we refine our callback of ieee80211_ops::flush to
deal with the case of drop=true via this feature.

Signed-off-by: Zong-Zhe Yang <kevin_yang@realtek.com>
Signed-off-by: Ping-Ke Shih <pkshih@realtek.com>
Signed-off-by: Kalle Valo <kvalo@kernel.org>
Link: https://lore.kernel.org/r/20220914035034.14521-3-pkshih@realtek.com

diff --git a/drivers/net/wireless/realtek/rtw89/core.h b/drivers/net/wireless/realtek/rtw89/core.h
index 4ed38dcde4444d039207e7b5dc3ebd6b1e28b6e8..70d5a10c1a9aa4761e5557ee621e86bcfdf2e760 100644 (file)
--- a/drivers/net/wireless/realtek/rtw89/core.h
+++ b/drivers/net/wireless/realtek/rtw89/core.h
@@ -2750,6 +2750,7 @@ enum rtw89_fw_feature {
        RTW89_FW_FEATURE_SCAN_OFFLOAD,
        RTW89_FW_FEATURE_TX_WAKE,
        RTW89_FW_FEATURE_CRASH_TRIGGER,
+       RTW89_FW_FEATURE_PACKET_DROP,
        RTW89_FW_FEATURE_NO_DEEP_PS,
 };
 
@@ -2898,6 +2899,32 @@ enum rtw89_flags {
        NUM_OF_RTW89_FLAGS,
 };
 
+enum rtw89_pkt_drop_sel {
+       RTW89_PKT_DROP_SEL_MACID_BE_ONCE,
+       RTW89_PKT_DROP_SEL_MACID_BK_ONCE,
+       RTW89_PKT_DROP_SEL_MACID_VI_ONCE,
+       RTW89_PKT_DROP_SEL_MACID_VO_ONCE,
+       RTW89_PKT_DROP_SEL_MACID_ALL,
+       RTW89_PKT_DROP_SEL_MG0_ONCE,
+       RTW89_PKT_DROP_SEL_HIQ_ONCE,
+       RTW89_PKT_DROP_SEL_HIQ_PORT,
+       RTW89_PKT_DROP_SEL_HIQ_MBSSID,
+       RTW89_PKT_DROP_SEL_BAND,
+       RTW89_PKT_DROP_SEL_BAND_ONCE,
+       RTW89_PKT_DROP_SEL_REL_MACID,
+       RTW89_PKT_DROP_SEL_REL_HIQ_PORT,
+       RTW89_PKT_DROP_SEL_REL_HIQ_MBSSID,
+};
+
+struct rtw89_pkt_drop_params {
+       enum rtw89_pkt_drop_sel sel;
+       enum rtw89_mac_idx mac_band;
+       u8 macid;
+       u8 port;
+       u8 mbssid;
+       bool tf_trs;
+};
+
 struct rtw89_pkt_stat {
        u16 beacon_nr;
        u32 rx_rate_cnt[RTW89_HW_RATE_NR];

diff --git a/drivers/net/wireless/realtek/rtw89/fw.c b/drivers/net/wireless/realtek/rtw89/fw.c
index 1494f3794b275c3dd62bcf042ae6327835360140..b88b71061b4c62f8bf174f8a72624ce2c5883667 100644 (file)
--- a/drivers/net/wireless/realtek/rtw89/fw.c
+++ b/drivers/net/wireless/realtek/rtw89/fw.c
@@ -225,6 +225,8 @@ static const struct __fw_feat_cfg fw_feat_tbl[] = {
        __CFG_FW_FEAT(RTL8852A, ge, 0, 13, 35, 0, SCAN_OFFLOAD),
        __CFG_FW_FEAT(RTL8852A, ge, 0, 13, 35, 0, TX_WAKE),
        __CFG_FW_FEAT(RTL8852A, ge, 0, 13, 36, 0, CRASH_TRIGGER),
+       __CFG_FW_FEAT(RTL8852A, ge, 0, 13, 38, 0, PACKET_DROP),
+       __CFG_FW_FEAT(RTL8852C, ge, 0, 27, 20, 0, PACKET_DROP),
        __CFG_FW_FEAT(RTL8852C, le, 0, 27, 33, 0, NO_DEEP_PS),
        __CFG_FW_FEAT(RTL8852C, ge, 0, 27, 34, 0, TX_WAKE),
        __CFG_FW_FEAT(RTL8852C, ge, 0, 27, 36, 0, SCAN_OFFLOAD),
@@ -2737,3 +2739,57 @@ fail:
        dev_kfree_skb_any(skb);
        return ret;
 }
+
+#define H2C_PKT_DROP_LEN 24
+int rtw89_fw_h2c_pkt_drop(struct rtw89_dev *rtwdev,
+                         const struct rtw89_pkt_drop_params *params)
+{
+       struct sk_buff *skb;
+       int ret;
+
+       skb = rtw89_fw_h2c_alloc_skb_with_hdr(rtwdev, H2C_PKT_DROP_LEN);
+       if (!skb) {
+               rtw89_err(rtwdev,
+                         "failed to alloc skb for packet drop\n");
+               return -ENOMEM;
+       }
+
+       switch (params->sel) {
+       case RTW89_PKT_DROP_SEL_MACID_BE_ONCE:
+       case RTW89_PKT_DROP_SEL_MACID_BK_ONCE:
+       case RTW89_PKT_DROP_SEL_MACID_VI_ONCE:
+       case RTW89_PKT_DROP_SEL_MACID_VO_ONCE:
+               break;
+       default:
+               rtw89_debug(rtwdev, RTW89_DBG_FW,
+                           "H2C of pkt drop might not fully support sel: %d yet\n",
+                           params->sel);
+               break;
+       }
+
+       skb_put(skb, H2C_PKT_DROP_LEN);
+       RTW89_SET_FWCMD_PKT_DROP_SEL(skb->data, params->sel);
+       RTW89_SET_FWCMD_PKT_DROP_MACID(skb->data, params->macid);
+       RTW89_SET_FWCMD_PKT_DROP_BAND(skb->data, params->mac_band);
+       RTW89_SET_FWCMD_PKT_DROP_PORT(skb->data, params->port);
+       RTW89_SET_FWCMD_PKT_DROP_MBSSID(skb->data, params->mbssid);
+       RTW89_SET_FWCMD_PKT_DROP_ROLE_A_INFO_TF_TRS(skb->data, params->tf_trs);
+
+       rtw89_h2c_pkt_set_hdr(rtwdev, skb, FWCMD_TYPE_H2C,
+                             H2C_CAT_MAC,
+                             H2C_CL_MAC_FW_OFLD,
+                             H2C_FUNC_PKT_DROP, 0, 0,
+                             H2C_PKT_DROP_LEN);
+
+       ret = rtw89_h2c_tx(rtwdev, skb, false);
+       if (ret) {
+               rtw89_err(rtwdev, "failed to send h2c\n");
+               goto fail;
+       }
+
+       return 0;
+
+fail:
+       dev_kfree_skb_any(skb);
+       return ret;
+}

diff --git a/drivers/net/wireless/realtek/rtw89/fw.h b/drivers/net/wireless/realtek/rtw89/fw.h
index 6e3aca44906a70e509b73a48490d15147018b012..72b667a460abdd4ca5587b957cbe9b29513bcab1 100644 (file)
--- a/drivers/net/wireless/realtek/rtw89/fw.h
+++ b/drivers/net/wireless/realtek/rtw89/fw.h
@@ -1823,6 +1823,36 @@ static inline void RTW89_SET_FWCMD_CPU_EXCEPTION_TYPE(void *cmd, u32 val)
        le32p_replace_bits((__le32 *)cmd, val, GENMASK(31, 0));
 }
 
+static inline void RTW89_SET_FWCMD_PKT_DROP_SEL(void *cmd, u32 val)
+{
+       le32p_replace_bits((__le32 *)cmd, val, GENMASK(7, 0));
+}
+
+static inline void RTW89_SET_FWCMD_PKT_DROP_MACID(void *cmd, u32 val)
+{
+       le32p_replace_bits((__le32 *)cmd, val, GENMASK(15, 8));
+}
+
+static inline void RTW89_SET_FWCMD_PKT_DROP_BAND(void *cmd, u32 val)
+{
+       le32p_replace_bits((__le32 *)cmd, val, GENMASK(23, 16));
+}
+
+static inline void RTW89_SET_FWCMD_PKT_DROP_PORT(void *cmd, u32 val)
+{
+       le32p_replace_bits((__le32 *)cmd, val, GENMASK(31, 24));
+}
+
+static inline void RTW89_SET_FWCMD_PKT_DROP_MBSSID(void *cmd, u32 val)
+{
+       le32p_replace_bits((__le32 *)cmd + 1, val, GENMASK(7, 0));
+}
+
+static inline void RTW89_SET_FWCMD_PKT_DROP_ROLE_A_INFO_TF_TRS(void *cmd, u32 val)
+{
+       le32p_replace_bits((__le32 *)cmd + 1, val, GENMASK(15, 8));
+}
+
 enum rtw89_btc_btf_h2c_class {
        BTFC_SET = 0x10,
        BTFC_GET = 0x11,
@@ -2591,6 +2621,7 @@ struct rtw89_fw_h2c_rf_reg_info {
 #define H2C_FUNC_OFLD_CFG              0x14
 #define H2C_FUNC_ADD_SCANOFLD_CH       0x16
 #define H2C_FUNC_SCANOFLD              0x17
+#define H2C_FUNC_PKT_DROP              0x1b
 
 /* CLASS 10 - Security CAM */
 #define H2C_CL_MAC_SEC_CAM             0xa
@@ -2718,6 +2749,8 @@ int rtw89_hw_scan_offload(struct rtw89_dev *rtwdev, struct ieee80211_vif *vif,
                          bool enable);
 void rtw89_hw_scan_abort(struct rtw89_dev *rtwdev, struct ieee80211_vif *vif);
 int rtw89_fw_h2c_trigger_cpu_exception(struct rtw89_dev *rtwdev);
+int rtw89_fw_h2c_pkt_drop(struct rtw89_dev *rtwdev,
+                         const struct rtw89_pkt_drop_params *params);
 
 static inline void rtw89_fw_h2c_init_ba_cam(struct rtw89_dev *rtwdev)
 {

diff --git a/drivers/net/wireless/realtek/rtw89/mac.c b/drivers/net/wireless/realtek/rtw89/mac.c
index 3fe3df9e0cf84580edf05006805b088cdb6c3405..d2d002d61e97d364cb76b00dd5923bca79cea21e 100644 (file)
--- a/drivers/net/wireless/realtek/rtw89/mac.c
+++ b/drivers/net/wireless/realtek/rtw89/mac.c
@@ -4708,3 +4708,48 @@ int rtw89_mac_read_xtal_si(struct rtw89_dev *rtwdev, u8 offset, u8 *val)
 
        return 0;
 }
+
+static
+void rtw89_mac_pkt_drop_sta(struct rtw89_dev *rtwdev, struct rtw89_sta *rtwsta)
+{
+       static const enum rtw89_pkt_drop_sel sels[] = {
+               RTW89_PKT_DROP_SEL_MACID_BE_ONCE,
+               RTW89_PKT_DROP_SEL_MACID_BK_ONCE,
+               RTW89_PKT_DROP_SEL_MACID_VI_ONCE,
+               RTW89_PKT_DROP_SEL_MACID_VO_ONCE,
+       };
+       struct rtw89_vif *rtwvif = rtwsta->rtwvif;
+       struct rtw89_pkt_drop_params params = {0};
+       int i;
+
+       params.mac_band = RTW89_MAC_0;
+       params.macid = rtwsta->mac_id;
+       params.port = rtwvif->port;
+       params.mbssid = 0;
+       params.tf_trs = rtwvif->trigger;
+
+       for (i = 0; i < ARRAY_SIZE(sels); i++) {
+               params.sel = sels[i];
+               rtw89_fw_h2c_pkt_drop(rtwdev, &params);
+       }
+}
+
+static void rtw89_mac_pkt_drop_vif_iter(void *data, struct ieee80211_sta *sta)
+{
+       struct rtw89_sta *rtwsta = (struct rtw89_sta *)sta->drv_priv;
+       struct rtw89_vif *rtwvif = rtwsta->rtwvif;
+       struct rtw89_dev *rtwdev = rtwvif->rtwdev;
+       struct rtw89_vif *target = data;
+
+       if (rtwvif != target)
+               return;
+
+       rtw89_mac_pkt_drop_sta(rtwdev, rtwsta);
+}
+
+void rtw89_mac_pkt_drop_vif(struct rtw89_dev *rtwdev, struct rtw89_vif *rtwvif)
+{
+       ieee80211_iterate_stations_atomic(rtwdev->hw,
+                                         rtw89_mac_pkt_drop_vif_iter,
+                                         rtwvif);
+}

diff --git a/drivers/net/wireless/realtek/rtw89/mac.h b/drivers/net/wireless/realtek/rtw89/mac.h
index cacf867a4f031ed672b47ada5e9cdcf0f01403be..9d8574714b051dfae6708657073337b973b56c9f 100644 (file)
--- a/drivers/net/wireless/realtek/rtw89/mac.h
+++ b/drivers/net/wireless/realtek/rtw89/mac.h
@@ -1003,5 +1003,6 @@ enum rtw89_mac_xtal_si_offset {
 
 int rtw89_mac_write_xtal_si(struct rtw89_dev *rtwdev, u8 offset, u8 val, u8 mask);
 int rtw89_mac_read_xtal_si(struct rtw89_dev *rtwdev, u8 offset, u8 *val);
+void rtw89_mac_pkt_drop_vif(struct rtw89_dev *rtwdev, struct rtw89_vif *rtwvif);
 
 #endif

diff --git a/drivers/net/wireless/realtek/rtw89/mac80211.c b/drivers/net/wireless/realtek/rtw89/mac80211.c
index 49d25d6e1e0de5b65ae3116b089e6485f0579aec..e82be57f291c02aa46be71510f5e16b1493c63f8 100644 (file)
--- a/drivers/net/wireless/realtek/rtw89/mac80211.c
+++ b/drivers/net/wireless/realtek/rtw89/mac80211.c
@@ -13,6 +13,7 @@
 #include "reg.h"
 #include "sar.h"
 #include "ser.h"
+#include "util.h"
 
 static void rtw89_ops_tx(struct ieee80211_hw *hw,
                         struct ieee80211_tx_control *control,
@@ -610,6 +611,20 @@ static void rtw89_ops_sta_statistics(struct ieee80211_hw *hw,
        sinfo->filled |= BIT_ULL(NL80211_STA_INFO_TX_BITRATE);
 }
 
+static
+void __rtw89_drop_packets(struct rtw89_dev *rtwdev, struct ieee80211_vif *vif)
+{
+       struct rtw89_vif *rtwvif;
+
+       if (vif) {
+               rtwvif = (struct rtw89_vif *)vif->drv_priv;
+               rtw89_mac_pkt_drop_vif(rtwdev, rtwvif);
+       } else {
+               rtw89_for_each_rtwvif(rtwdev, rtwvif)
+                       rtw89_mac_pkt_drop_vif(rtwdev, rtwvif);
+       }
+}
+
 static void rtw89_ops_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
                            u32 queues, bool drop)
 {
@@ -618,7 +633,12 @@ static void rtw89_ops_flush(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
        mutex_lock(&rtwdev->mutex);
        rtw89_leave_lps(rtwdev);
        rtw89_hci_flush_queues(rtwdev, queues, drop);
-       rtw89_mac_flush_txq(rtwdev, queues, drop);
+
+       if (drop && RTW89_CHK_FW_FEATURE(PACKET_DROP, &rtwdev->fw))
+               __rtw89_drop_packets(rtwdev, vif);
+       else
+               rtw89_mac_flush_txq(rtwdev, queues, drop);
+
        mutex_unlock(&rtwdev->mutex);
 }