wifi: rtw89: support for setting TID specific configuration

Add ops set_tid_config to support TID specific configuration.
We currently only support ampdu setting.

The command example is:
  iw wlan0 set tidconf tids 0x3 ampdu off
  iw wlan0 set tidconf peer xx:xx:xx:xx:xx:xx tids 0x2 ampdu on

Signed-off-by: Kuan-Chung Chen <damon.chen@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/20220912070014.10018-3-pkshih@realtek.com

3 files changed, 88 insertions, 13 deletions

diff --git a/drivers/net/wireless/realtek/rtw89/core.c b/drivers/net/wireless/realtek/rtw89/core.c
index c7405cb9ceb7..16c74477b332 100644
--- a/drivers/net/wireless/realtek/rtw89/core.c
+++ b/drivers/net/wireless/realtek/rtw89/core.c
@@ -1892,21 +1892,14 @@ static void rtw89_core_stop_tx_ba_session(struct rtw89_dev *rtwdev,
 		return;
 
 	spin_lock_bh(&rtwdev->ba_lock);
-	if (!list_empty(&rtwtxq->list)) {
-		list_del_init(&rtwtxq->list);
-		goto out;
-	}
-
-	set_bit(RTW89_TXQ_F_FORBID_BA, &rtwtxq->flags);
+	if (!test_and_set_bit(RTW89_TXQ_F_FORBID_BA, &rtwtxq->flags))
+		list_add_tail(&rtwtxq->list, &rtwdev->forbid_ba_list);
+	spin_unlock_bh(&rtwdev->ba_lock);
 
-	list_add_tail(&rtwtxq->list, &rtwdev->forbid_ba_list);
 	ieee80211_stop_tx_ba_session(sta, txq->tid);
 	cancel_delayed_work(&rtwdev->forbid_ba_work);
 	ieee80211_queue_delayed_work(rtwdev->hw, &rtwdev->forbid_ba_work,
 				     RTW89_FORBID_BA_TIMER);
-
-out:
-	spin_unlock_bh(&rtwdev->ba_lock);
 }
 
 static void rtw89_core_txq_check_agg(struct rtw89_dev *rtwdev,
@@ -1918,6 +1911,9 @@ static void rtw89_core_txq_check_agg(struct rtw89_dev *rtwdev,
 	struct ieee80211_sta *sta = txq->sta;
 	struct rtw89_sta *rtwsta = sta ? (struct rtw89_sta *)sta->drv_priv : NULL;
 
+	if (test_bit(RTW89_TXQ_F_FORBID_BA, &rtwtxq->flags))
+		return;
+
 	if (unlikely(skb->protocol == cpu_to_be16(ETH_P_PAE))) {
 		rtw89_core_stop_tx_ba_session(rtwdev, rtwtxq);
 		return;
@@ -1926,9 +1922,6 @@ static void rtw89_core_txq_check_agg(struct rtw89_dev *rtwdev,
 	if (unlikely(!sta))
 		return;
 
-	if (test_bit(RTW89_TXQ_F_FORBID_BA, &rtwtxq->flags))
-		return;
-
 	if (unlikely(test_bit(RTW89_TXQ_F_BLOCK_BA, &rtwtxq->flags)))
 		return;
 
@@ -2555,6 +2548,53 @@ int rtw89_core_sta_remove(struct rtw89_dev *rtwdev,
 	return 0;
 }
 
+static void _rtw89_core_set_tid_config(struct rtw89_dev *rtwdev,
+				       struct ieee80211_sta *sta,
+				       struct cfg80211_tid_cfg *tid_conf)
+{
+	struct ieee80211_txq *txq;
+	struct rtw89_txq *rtwtxq;
+	u32 mask = tid_conf->mask;
+	u8 tids = tid_conf->tids;
+	int tids_nbit = BITS_PER_BYTE;
+	int i;
+
+	for (i = 0; i < tids_nbit; i++, tids >>= 1) {
+		if (!tids)
+			break;
+
+		if (!(tids & BIT(0)))
+			continue;
+
+		txq = sta->txq[i];
+		rtwtxq = (struct rtw89_txq *)txq->drv_priv;
+
+		if (mask & BIT(NL80211_TID_CONFIG_ATTR_AMPDU_CTRL)) {
+			if (tid_conf->ampdu == NL80211_TID_CONFIG_ENABLE) {
+				clear_bit(RTW89_TXQ_F_FORBID_BA, &rtwtxq->flags);
+			} else {
+				if (test_bit(RTW89_TXQ_F_AMPDU, &rtwtxq->flags))
+					ieee80211_stop_tx_ba_session(sta, txq->tid);
+				spin_lock_bh(&rtwdev->ba_lock);
+				list_del_init(&rtwtxq->list);
+				set_bit(RTW89_TXQ_F_FORBID_BA, &rtwtxq->flags);
+				spin_unlock_bh(&rtwdev->ba_lock);
+			}
+		}
+	}
+}
+
+void rtw89_core_set_tid_config(struct rtw89_dev *rtwdev,
+			       struct ieee80211_sta *sta,
+			       struct cfg80211_tid_config *tid_config)
+{
+	int i;
+
+	for (i = 0; i < tid_config->n_tid_conf; i++)
+		_rtw89_core_set_tid_config(rtwdev, sta,
+					   &tid_config->tid_conf[i]);
+}
+
 static void rtw89_init_ht_cap(struct rtw89_dev *rtwdev,
 			      struct ieee80211_sta_ht_cap *ht_cap)
 {
@@ -3193,6 +3233,9 @@ static int rtw89_core_register_hw(struct rtw89_dev *rtwdev)
 	hw->wiphy->max_scan_ssids = RTW89_SCANOFLD_MAX_SSID;
 	hw->wiphy->max_scan_ie_len = RTW89_SCANOFLD_MAX_IE_LEN;
 
+	hw->wiphy->tid_config_support.vif |= BIT(NL80211_TID_CONFIG_ATTR_AMPDU_CTRL);
+	hw->wiphy->tid_config_support.peer |= BIT(NL80211_TID_CONFIG_ATTR_AMPDU_CTRL);
+
 	wiphy_ext_feature_set(hw->wiphy, NL80211_EXT_FEATURE_CAN_REPLACE_PTK0);
 
 	ret = rtw89_core_set_supported_band(rtwdev);
diff --git a/drivers/net/wireless/realtek/rtw89/core.h b/drivers/net/wireless/realtek/rtw89/core.h
index e64ecb3768c0..4ed38dcde444 100644
--- a/drivers/net/wireless/realtek/rtw89/core.h
+++ b/drivers/net/wireless/realtek/rtw89/core.h
@@ -4213,6 +4213,9 @@ int rtw89_core_sta_disconnect(struct rtw89_dev *rtwdev,
 int rtw89_core_sta_remove(struct rtw89_dev *rtwdev,
 			  struct ieee80211_vif *vif,
 			  struct ieee80211_sta *sta);
+void rtw89_core_set_tid_config(struct rtw89_dev *rtwdev,
+			       struct ieee80211_sta *sta,
+			       struct cfg80211_tid_config *tid_config);
 int rtw89_core_init(struct rtw89_dev *rtwdev);
 void rtw89_core_deinit(struct rtw89_dev *rtwdev);
 int rtw89_core_register(struct rtw89_dev *rtwdev);
diff --git a/drivers/net/wireless/realtek/rtw89/mac80211.c b/drivers/net/wireless/realtek/rtw89/mac80211.c
index a8c711ea5d45..49d25d6e1e0d 100644
--- a/drivers/net/wireless/realtek/rtw89/mac80211.c
+++ b/drivers/net/wireless/realtek/rtw89/mac80211.c
@@ -841,6 +841,34 @@ static void rtw89_ops_unassign_vif_chanctx(struct ieee80211_hw *hw,
 	mutex_unlock(&rtwdev->mutex);
 }
 
+static void rtw89_set_tid_config_iter(void *data, struct ieee80211_sta *sta)
+{
+	struct cfg80211_tid_config *tid_config = data;
+	struct rtw89_sta *rtwsta = (struct rtw89_sta *)sta->drv_priv;
+	struct rtw89_dev *rtwdev = rtwsta->rtwvif->rtwdev;
+
+	rtw89_core_set_tid_config(rtwdev, sta, tid_config);
+}
+
+static int rtw89_ops_set_tid_config(struct ieee80211_hw *hw,
+				    struct ieee80211_vif *vif,
+				    struct ieee80211_sta *sta,
+				    struct cfg80211_tid_config *tid_config)
+{
+	struct rtw89_dev *rtwdev = hw->priv;
+
+	mutex_lock(&rtwdev->mutex);
+	if (sta)
+		rtw89_core_set_tid_config(rtwdev, sta, tid_config);
+	else
+		ieee80211_iterate_stations_atomic(rtwdev->hw,
+						  rtw89_set_tid_config_iter,
+						  tid_config);
+	mutex_unlock(&rtwdev->mutex);
+
+	return 0;
+}
+
 const struct ieee80211_ops rtw89_ops = {
 	.tx			= rtw89_ops_tx,
 	.wake_tx_queue		= rtw89_ops_wake_tx_queue,
@@ -876,5 +904,6 @@ const struct ieee80211_ops rtw89_ops = {
 	.unassign_vif_chanctx	= rtw89_ops_unassign_vif_chanctx,
 	.set_sar_specs		= rtw89_ops_set_sar_specs,
 	.sta_rc_update		= rtw89_ops_sta_rc_update,
+	.set_tid_config		= rtw89_ops_set_tid_config,
 };
 EXPORT_SYMBOL(rtw89_ops);