From 8027e94f1564089d719a6fb0eab7d29bb2981bf0 Mon Sep 17 00:00:00 2001
From: Shayne Chen <shayne.chen@mediatek.com>
Date: Tue, 11 May 2021 16:24:09 +0800
Subject: [PATCH 1110/1112] mt76: mt7915: implement aid support in testmode

Add support for virtual stations in mt7915 testmode.

Signed-off-by: Shayne Chen <shayne.chen@mediatek.com>
---
 .../wireless/mediatek/mt76/mt76_connac_mcu.c  |   5 +
 .../net/wireless/mediatek/mt76/mt7915/mac.c   |  25 +-
 .../wireless/mediatek/mt76/mt7915/testmode.c  | 231 +++++++++++++++---
 3 files changed, 216 insertions(+), 45 deletions(-)

diff --git a/mt76_connac_mcu.c b/mt76_connac_mcu.c
index eac096c..a361ab6 100644
--- a/mt76_connac_mcu.c
+++ b/mt76_connac_mcu.c
@@ -389,6 +389,7 @@ void mt76_connac_mcu_sta_basic_tlv(struct sk_buff *skb,
 	switch (vif->type) {
 	case NL80211_IFTYPE_MESH_POINT:
 	case NL80211_IFTYPE_AP:
+	case NL80211_IFTYPE_MONITOR:
 		if (vif->p2p)
 			conn_type = CONNECTION_P2P_GC;
 		else
@@ -577,6 +578,10 @@ void mt76_connac_mcu_wtbl_generic_tlv(struct mt76_dev *dev,
 					     wtbl_tlv, sta_wtbl);
 	spe = (struct wtbl_spe *)tlv;
 	spe->spe_idx = 24;
+
+	/* check */
+	if (vif->type == NL80211_IFTYPE_MONITOR)
+		rx->rca1 = 0;
 }
 EXPORT_SYMBOL_GPL(mt76_connac_mcu_wtbl_generic_tlv);
 
diff --git a/mt7915/mac.c b/mt7915/mac.c
index fb42446..2ad4cb1 100644
--- a/mt7915/mac.c
+++ b/mt7915/mac.c
@@ -906,16 +906,28 @@ mt7915_mac_write_txwi_tm(struct mt7915_phy *phy, __le32 *txwi,
 {
 #ifdef CONFIG_NL80211_TESTMODE
 	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct mt76_testmode_sta_data *sd = &td->sd;
 	const struct ieee80211_rate *r;
-	u8 bw, mode, nss = td->tx_rate_nss;
-	u8 rate_idx = td->tx_rate_idx;
+	u8 bw, mode, nss, rate_idx;
 	u16 rateval = 0;
 	u32 val;
 	bool cck = false;
 	int band;
 
-	if (skb != phy->mt76->test.tx_skb)
-		return;
+	if (mt76_testmode_has_sta(phy->mt76)) {
+		struct mt76_testmode_sta *tm_sta;
+		int i;
+
+		mt76_testmode_for_each_sta(phy->mt76, i, tm_sta) {
+			if (tm_sta->tx_skb == skb) {
+				sd = &tm_sta->sd;
+				break;
+			}
+		}
+	}
+
+	nss = sd->tx_rate_nss;
+	rate_idx = sd->tx_rate_idx;
 
 	switch (td->tx_rate_mode) {
 	case MT76_TM_TX_MODE_HT:
@@ -1005,7 +1017,7 @@ mt7915_mac_write_txwi_tm(struct mt7915_phy *phy, __le32 *txwi,
 	if (mode >= MT_PHY_TYPE_HE_SU)
 		val |= FIELD_PREP(MT_TXD6_HELTF, td->tx_ltf);
 
-	if (td->tx_rate_ldpc || (bw > 0 && mode >= MT_PHY_TYPE_HE_SU))
+	if (sd->tx_rate_ldpc || (bw > 0 && mode >= MT_PHY_TYPE_HE_SU))
 		val |= MT_TXD6_LDPC;
 
 	txwi[1] &= ~cpu_to_le32(MT_TXD1_VTA);
@@ -1474,6 +1486,9 @@ mt7915_mac_tx_free(struct mt7915_dev *dev, void *data, int len)
 				continue;
 
 			msta = container_of(wcid, struct mt7915_sta, wcid);
+			if (mt76_testmode_enabled(msta->vif->phy->mt76))
+				continue;
+
 			spin_lock_bh(&dev->sta_poll_lock);
 			if (list_empty(&msta->poll_list))
 				list_add_tail(&msta->poll_list, &dev->sta_poll_list);
diff --git a/mt7915/testmode.c b/mt7915/testmode.c
index 054829e..29c173d 100644
--- a/mt7915/testmode.c
+++ b/mt7915/testmode.c
@@ -11,6 +11,7 @@ enum {
 	TM_CHANGED_FREQ_OFFSET,
 	TM_CHANGED_CFG,
 	TM_CHANGED_OFF_CH_SCAN_CH,
+	TM_CHANGED_AID,
 
 	/* must be last */
 	NUM_TM_CHANGED
@@ -21,6 +22,7 @@ static const u8 tm_change_map[] = {
 	[TM_CHANGED_FREQ_OFFSET] = MT76_TM_ATTR_FREQ_OFFSET,
 	[TM_CHANGED_CFG] = MT76_TM_ATTR_CFG,
 	[TM_CHANGED_OFF_CH_SCAN_CH] = MT76_TM_ATTR_OFF_CH_SCAN_CH,
+	[TM_CHANGED_AID] = MT76_TM_ATTR_AID,
 };
 
 struct reg_band {
@@ -142,18 +144,33 @@ mt7915_tm_set_trx(struct mt7915_phy *phy, int type, bool en)
 }
 
 static int
-mt7915_tm_clean_hwq(struct mt7915_phy *phy, u8 wcid)
+mt7915_tm_clean_hwq(struct mt7915_phy *phy)
 {
 	struct mt7915_dev *dev = phy->dev;
 	struct mt7915_tm_cmd req = {
 		.testmode_en = 1,
 		.param_idx = MCU_ATE_CLEAN_TXQUEUE,
-		.param.clean.wcid = wcid,
 		.param.clean.band = phy != &dev->phy,
 	};
+	struct mt76_testmode_sta *tm_sta;
+	int ret, i;
 
-	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), &req,
-				 sizeof(req), false);
+	if (!mt76_testmode_has_sta(phy->mt76)) {
+		req.param.clean.wcid = dev->mt76.global_wcid.idx;
+
+		return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL),
+					 &req, sizeof(req), false);
+	}
+
+	mt76_testmode_for_each_sta(phy->mt76, i, tm_sta) {
+		req.param.clean.wcid = phy->mt76->test.tm_wcid[i]->idx;
+		ret = mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL),
+					&req, sizeof(req), false);
+		if (ret)
+			return ret;
+	}
+
+	return 0;
 }
 
 static int
@@ -530,27 +547,109 @@ mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
 	}
 }
 
+static int
+mt7915_tm_sta_add(struct mt7915_phy *phy, u8 aid,
+		  struct mt76_testmode_sta_data *sd)
+{
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct mt76_testmode_sta *tm_sta;
+
+	if (!aid)
+		return 0;
+
+	if (!td->tm_wcid[aid]) {
+		struct ieee80211_vif *vif = phy->monitor_vif;
+		struct ieee80211_sband_iftype_data *data;
+		struct ieee80211_supported_band *sband;
+		struct ieee80211_sta *sta;
+		struct mt7915_sta *msta;
+		int ret;
+
+		sta = kzalloc(sizeof(*sta) + phy->mt76->hw->sta_data_size +
+			      sizeof(*tm_sta), GFP_KERNEL);
+		if (!sta)
+			return -ENOMEM;
+
+		if (phy->mt76->chandef.chan->band == NL80211_BAND_5GHZ) {
+			sband = &phy->mt76->sband_5g.sband;
+			data = phy->iftype[NL80211_BAND_5GHZ];
+		} else {
+			sband = &phy->mt76->sband_2g.sband;
+			data = phy->iftype[NL80211_BAND_2GHZ];
+		}
+
+		ether_addr_copy(sta->addr, phy->mt76->macaddr);
+		sta->addr[0] += aid * 4;
+		memcpy(&sta->ht_cap, &sband->ht_cap, sizeof(sta->ht_cap));
+		memcpy(&sta->vht_cap, &sband->vht_cap, sizeof(sta->vht_cap));
+		memcpy(&sta->he_cap, &data[NL80211_IFTYPE_STATION].he_cap,
+		       sizeof(sta->he_cap));
+		sta->aid = aid;
+		sta->wme = 1;
+
+		ret = mt7915_mac_sta_add(&phy->dev->mt76, vif, sta);
+		if (ret) {
+			kfree(sta);
+			return ret;
+		}
+
+		msta = (struct mt7915_sta *)sta->drv_priv;
+		td->tm_wcid[aid] = &msta->wcid;
+		td->tm_sta_mask |= BIT(aid - 1);
+	}
+
+	tm_sta = mt76_testmode_aid_get_sta(phy->mt76, aid);
+	memcpy(&tm_sta->sd, sd, sizeof(tm_sta->sd));
+
+	return 0;
+}
+
 static void
-mt7915_tm_init(struct mt7915_phy *phy, bool en)
+mt7915_tm_sta_remove(struct mt7915_phy *phy, u8 aid)
 {
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct mt76_wcid *wcid = td->tm_wcid[aid];
 	struct mt7915_dev *dev = phy->dev;
+	struct ieee80211_sta *sta = wcid_to_sta(wcid);
 
-	if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
+	mt7915_mac_sta_remove(&dev->mt76, phy->monitor_vif, sta);
+	mt76_wcid_mask_clear(dev->mt76.wcid_mask, wcid->idx);
+
+	kfree(sta);
+	td->tm_wcid[aid] = NULL;
+	td->tm_sta_mask &= ~BIT(aid - 1);
+}
+
+static void
+mt7915_tm_sta_remove_all(struct mt7915_phy *phy)
+{
+	int i;
+
+	if (!mt76_testmode_has_sta(phy->mt76))
 		return;
 
-	mt7915_mcu_set_sku_en(phy, !en);
+	for (i = 1; i < ARRAY_SIZE(phy->mt76->test.tm_wcid); i++) {
+		if (phy->mt76->test.tm_wcid[i])
+			mt7915_tm_sta_remove(phy, i);
+	}
+}
 
-	mt7915_tm_mode_ctrl(dev, en);
-	mt7915_tm_reg_backup_restore(phy);
-	mt7915_tm_set_trx(phy, TM_MAC_TXRX, !en);
+static int
+mt7915_tm_set_sta(struct mt7915_phy *phy)
+{
+	struct mt76_testmode_data *td = &phy->mt76->test;
 
-	mt7915_mcu_add_bss_info(phy, phy->monitor_vif, en);
-	mt7915_mcu_add_sta(dev, phy->monitor_vif, NULL, en);
+	if (!td->aid) {
+		mt7915_tm_sta_remove_all(phy);
+		return 0;
+	}
 
-	phy->mt76->test.flag |= MT_TM_FW_RX_COUNT;
+	if (td->tx_count == 0) {
+		mt7915_tm_sta_remove(phy, td->aid);
+		return 0;
+	}
 
-	if (!en)
-		mt7915_tm_set_tam_arb(phy, en, 0);
+	return mt7915_tm_sta_add(phy, td->aid, &td->sd);
 }
 
 static void
@@ -563,22 +662,48 @@ mt7915_tm_update_channel(struct mt7915_phy *phy)
 	mt7915_mcu_set_chan_info(phy, MCU_EXT_CMD(SET_RX_PATH));
 }
 
+static bool
+mt7915_tm_check_skb(struct mt7915_phy *phy)
+{
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct ieee80211_tx_info *info;
+
+	if (!mt76_testmode_has_sta(phy->mt76)) {
+		if (!td->tx_skb)
+			return false;
+
+		info = IEEE80211_SKB_CB(td->tx_skb);
+		info->control.vif = phy->monitor_vif;
+	} else {
+		struct mt76_testmode_sta *tm_sta;
+		int i;
+
+		mt76_testmode_for_each_sta(phy->mt76, i, tm_sta) {
+			if (!tm_sta->tx_skb)
+				return false;
+
+			info = IEEE80211_SKB_CB(tm_sta->tx_skb);
+			info->control.vif = phy->monitor_vif;
+		}
+	}
+
+	return true;
+}
+
 static void
 mt7915_tm_set_tx_frames(struct mt7915_phy *phy, bool en)
 {
 	static const u8 spe_idx_map[] = {0, 0, 1, 0, 3, 2, 4, 0,
 					 9, 8, 6, 10, 16, 12, 18, 0};
 	struct mt76_testmode_data *td = &phy->mt76->test;
-	struct mt7915_dev *dev = phy->dev;
-	struct ieee80211_tx_info *info;
-	u8 duty_cycle = td->tx_duty_cycle;
-	u32 tx_time = td->tx_time;
-	u32 ipg = td->tx_ipg;
 
 	mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, false);
-	mt7915_tm_clean_hwq(phy, dev->mt76.global_wcid.idx);
+	mt7915_tm_set_trx(phy, TM_MAC_TX, false);
 
 	if (en) {
+		u32 tx_time = td->tx_time, ipg = td->tx_ipg;
+		u8 duty_cycle = td->tx_duty_cycle;
+
 		mt7915_tm_update_channel(phy);
 
 		if (td->tx_spe_idx) {
@@ -586,30 +711,29 @@ mt7915_tm_set_tx_frames(struct mt7915_phy *phy, bool en)
 		} else {
 			phy->test.spe_idx = spe_idx_map[td->tx_antenna_mask];
 		}
-	}
 
-	mt7915_tm_set_tam_arb(phy, en,
-			      td->tx_rate_mode == MT76_TM_TX_MODE_HE_MU);
-
-	/* if all three params are set, duty_cycle will be ignored */
-	if (duty_cycle && tx_time && !ipg) {
-		ipg = tx_time * 100 / duty_cycle - tx_time;
-	} else if (duty_cycle && !tx_time && ipg) {
-		if (duty_cycle < 100)
-			tx_time = duty_cycle * ipg / (100 - duty_cycle);
-	}
+		/* if all three params are set, duty_cycle will be ignored */
+		if (duty_cycle && tx_time && !ipg) {
+			ipg = tx_time * 100 / duty_cycle - tx_time;
+		} else if (duty_cycle && !tx_time && ipg) {
+			if (duty_cycle < 100)
+				tx_time = duty_cycle * ipg / (100 - duty_cycle);
+		}
 
-	mt7915_tm_set_ipg_params(phy, ipg, td->tx_rate_mode);
-	mt7915_tm_set_tx_len(phy, tx_time);
+		mt7915_tm_set_ipg_params(phy, ipg, td->tx_rate_mode);
+		mt7915_tm_set_tx_len(phy, tx_time);
 
-	if (ipg)
-		td->tx_queued_limit = MT76_TM_TIMEOUT * 1000000 / ipg / 2;
+		if (ipg)
+			td->tx_queued_limit = MT76_TM_TIMEOUT * 1000000 / ipg / 2;
 
-	if (!en || !td->tx_skb)
-		return;
+		if (!mt7915_tm_check_skb(phy))
+			return;
+	} else {
+		mt7915_tm_clean_hwq(phy);
+	}
 
-	info = IEEE80211_SKB_CB(td->tx_skb);
-	info->control.vif = phy->monitor_vif;
+	mt7915_tm_set_tam_arb(phy, en,
+			      td->tx_rate_mode == MT76_TM_TX_MODE_HE_MU);
 
 	mt7915_tm_set_trx(phy, TM_MAC_TX, en);
 }
@@ -811,6 +935,31 @@ out:
 				 sizeof(req), true);
 }
 
+static void
+mt7915_tm_init(struct mt7915_phy *phy, bool en)
+{
+	struct mt7915_dev *dev = phy->dev;
+
+	if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
+		return;
+
+	mt7915_mcu_set_sku_en(phy, !en);
+
+	mt7915_tm_mode_ctrl(dev, en);
+	mt7915_tm_reg_backup_restore(phy);
+	mt7915_tm_set_trx(phy, TM_MAC_TXRX, !en);
+
+	mt7915_mcu_add_bss_info(phy, phy->monitor_vif, en);
+	mt7915_mcu_add_sta(dev, phy->monitor_vif, NULL, en);
+
+	phy->mt76->test.flag |= MT_TM_FW_RX_COUNT;
+
+	if (!en) {
+		mt7915_tm_set_tam_arb(phy, en, 0);
+		mt7915_tm_sta_remove_all(phy);
+	}
+}
+
 static void
 mt7915_tm_update_params(struct mt7915_phy *phy, u32 changed)
 {
@@ -825,6 +974,8 @@ mt7915_tm_update_params(struct mt7915_phy *phy, u32 changed)
 		mt7915_tm_set_cfg(phy);
 	if (changed & BIT(TM_CHANGED_OFF_CH_SCAN_CH))
 		mt7915_tm_set_off_channel_scan(phy);
+	if (changed & BIT(TM_CHANGED_AID))
+		mt7915_tm_set_sta(phy);
 }
 
 static int
-- 
2.25.1