mt76: testmode: introduce dbdc support

Add testmode support for DBDC NICs (both MT7615D and MT7915D work).
Testmode data and parameters are moved from per-dev to per-phy
for maintaining the value of each band.

Signed-off-by: Shayne Chen <shayne.chen@mediatek.com>
Acked-by: Ryder Lee <ryder.lee@mediatek.com>
Signed-off-by: Felix Fietkau <nbd@nbd.name>

4 files changed, 89 insertions, 73 deletions

diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/mac.c b/drivers/net/wireless/mediatek/mt76/mt7915/mac.c
index ecabab77a913..4335f66d193e 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7915/mac.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/mac.c
@@ -601,18 +601,19 @@ void mt7915_mac_fill_rx_vector(struct mt7915_dev *dev, struct sk_buff *skb)
 #endif
 
 static void
-mt7915_mac_write_txwi_tm(struct mt7915_dev *dev, struct mt76_phy *mphy,
-			 __le32 *txwi, struct sk_buff *skb)
+mt7915_mac_write_txwi_tm(struct mt7915_phy *phy, __le32 *txwi,
+			 struct sk_buff *skb)
 {
 #ifdef CONFIG_NL80211_TESTMODE
-	struct mt76_testmode_data *td = &dev->mt76.test;
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct mt7915_dev *dev = phy->dev;
 	u8 rate_idx = td->tx_rate_idx;
 	u8 nss = td->tx_rate_nss;
 	u8 bw, mode;
 	u16 rateval = 0;
 	u32 val;
 
-	if (skb != dev->mt76.test.tx_skb)
+	if (skb != phy->mt76->test.tx_skb)
 		return;
 
 	switch (td->tx_rate_mode) {
@@ -644,7 +645,7 @@ mt7915_mac_write_txwi_tm(struct mt7915_dev *dev, struct mt76_phy *mphy,
 		break;
 	}
 
-	switch (mphy->chandef.width) {
+	switch (phy->mt76->chandef.width) {
 	case NL80211_CHAN_WIDTH_40:
 		bw = 1;
 		break;
@@ -902,8 +903,8 @@ void mt7915_mac_write_txwi(struct mt7915_dev *dev, __le32 *txwi,
 		txwi[3] |= cpu_to_le32(MT_TXD3_BA_DISABLE);
 	}
 
-	if (mt76_testmode_enabled(&dev->mt76))
-		mt7915_mac_write_txwi_tm(dev, mphy, txwi, skb);
+	if (mt76_testmode_enabled(mphy))
+		mt7915_mac_write_txwi_tm(mphy->priv, txwi, skb);
 }
 
 static void
@@ -1051,20 +1052,19 @@ mt7915_tx_complete_status(struct mt76_dev *mdev, struct sk_buff *skb,
 		status.rate = &msta->stats.tx_rate;
 	}
 
-	hw = mt76_tx_status_get_hw(mdev, skb);
-
 #ifdef CONFIG_NL80211_TESTMODE
-	if (skb == mdev->test.tx_skb) {
+	if (mt76_is_testmode_skb(mdev, skb, &hw)) {
 		struct mt7915_phy *phy = mt7915_hw_phy(hw);
 		struct ieee80211_vif *vif = phy->monitor_vif;
 		struct mt7915_vif *mvif = (struct mt7915_vif *)vif->drv_priv;
 
 		mt76_tx_complete_skb(mdev, mvif->sta.wcid.idx, skb);
-
 		return;
 	}
 #endif
 
+	hw = mt76_tx_status_get_hw(mdev, skb);
+
 	if (info->flags & IEEE80211_TX_CTL_AMPDU)
 		info->flags |= IEEE80211_TX_STAT_AMPDU;
 
diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/main.c b/drivers/net/wireless/mediatek/mt76/mt7915/main.c
index ce348c5ccc6c..3e0458fee937 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7915/main.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/main.c
@@ -44,12 +44,12 @@ static int mt7915_start(struct ieee80211_hw *hw)
 		mt7915_mac_enable_nf(dev, 1);
 	}
 
-	mt7915_mcu_set_sku_en(phy, !mt76_testmode_enabled(&dev->mt76));
+	mt7915_mcu_set_sku_en(phy, !mt76_testmode_enabled(phy->mt76));
 	mt7915_mcu_set_chan_info(phy, MCU_EXT_CMD_SET_RX_PATH);
 
 	set_bit(MT76_STATE_RUNNING, &phy->mt76->state);
 
-	if (!mt76_testmode_enabled(&dev->mt76))
+	if (!mt76_testmode_enabled(phy->mt76))
 		ieee80211_queue_delayed_work(hw, &phy->mac_work,
 					     MT7915_WATCHDOG_TIME);
 
@@ -70,7 +70,7 @@ static void mt7915_stop(struct ieee80211_hw *hw)
 
 	mutex_lock(&dev->mt76.mutex);
 
-	mt76_testmode_reset(&dev->mt76, true);
+	mt76_testmode_reset(phy->mt76, true);
 
 	clear_bit(MT76_STATE_RUNNING, &phy->mt76->state);
 
@@ -153,7 +153,7 @@ static int mt7915_add_interface(struct ieee80211_hw *hw,
 
 	mutex_lock(&dev->mt76.mutex);
 
-	mt76_testmode_reset(&dev->mt76, true);
+	mt76_testmode_reset(phy->mt76, true);
 
 	if (vif->type == NL80211_IFTYPE_MONITOR &&
 	    is_zero_ether_addr(vif->addr))
@@ -228,7 +228,7 @@ static void mt7915_remove_interface(struct ieee80211_hw *hw,
 	/* TODO: disable beacon for the bss */
 
 	mutex_lock(&dev->mt76.mutex);
-	mt76_testmode_reset(&dev->mt76, true);
+	mt76_testmode_reset(phy->mt76, true);
 	mutex_unlock(&dev->mt76.mutex);
 
 	if (vif == phy->monitor_vif)
@@ -298,7 +298,7 @@ out:
 
 	mt76_txq_schedule_all(phy->mt76);
 
-	if (!mt76_testmode_enabled(&dev->mt76))
+	if (!mt76_testmode_enabled(phy->mt76))
 		ieee80211_queue_delayed_work(phy->mt76->hw, &phy->mac_work,
 					     MT7915_WATCHDOG_TIME);
 
@@ -365,9 +365,9 @@ static int mt7915_config(struct ieee80211_hw *hw, u32 changed)
 
 	if (changed & IEEE80211_CONF_CHANGE_CHANNEL) {
 #ifdef CONFIG_NL80211_TESTMODE
-		if (dev->mt76.test.state != MT76_TM_STATE_OFF) {
+		if (phy->mt76->test.state != MT76_TM_STATE_OFF) {
 			mutex_lock(&dev->mt76.mutex);
-			mt76_testmode_reset(&dev->mt76, false);
+			mt76_testmode_reset(phy->mt76, false);
 			mutex_unlock(&dev->mt76.mutex);
 		}
 #endif
@@ -396,7 +396,7 @@ static int mt7915_config(struct ieee80211_hw *hw, u32 changed)
 
 		mt76_rmw_field(dev, MT_DMA_DCR0(band), MT_DMA_DCR0_RXD_G5_EN,
 			       enabled);
-		mt76_testmode_reset(&dev->mt76, true);
+		mt76_testmode_reset(phy->mt76, true);
 		mt76_wr(dev, MT_WF_RFCR(band), phy->rxfilter);
 	}
 
diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/mcu.c b/drivers/net/wireless/mediatek/mt76/mt7915/mcu.c
index 592c0db7aeed..636873cb991e 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7915/mcu.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/mcu.c
@@ -3186,6 +3186,7 @@ int mt7915_mcu_set_chan_info(struct mt7915_phy *phy, int cmd)
 	struct mt7915_dev *dev = phy->dev;
 	struct cfg80211_chan_def *chandef = &phy->mt76->chandef;
 	int freq1 = chandef->center_freq1;
+	bool ext_phy = phy != &dev->phy;
 	struct {
 		u8 control_ch;
 		u8 center_ch;
@@ -3209,16 +3210,21 @@ int mt7915_mcu_set_chan_info(struct mt7915_phy *phy, int cmd)
 		.bw = mt7915_mcu_chan_bw(chandef),
 		.tx_streams_num = hweight8(phy->mt76->antenna_mask),
 		.rx_streams = phy->mt76->antenna_mask,
-		.band_idx = phy != &dev->phy,
+		.band_idx = ext_phy,
 		.channel_band = chandef->chan->band,
 	};
 
 #ifdef CONFIG_NL80211_TESTMODE
-	if (dev->mt76.test.tx_antenna_mask &&
-	    (dev->mt76.test.state == MT76_TM_STATE_TX_FRAMES ||
-	     dev->mt76.test.state == MT76_TM_STATE_RX_FRAMES)) {
-		req.tx_streams_num = fls(dev->mt76.test.tx_antenna_mask);
-		req.rx_streams = dev->mt76.test.tx_antenna_mask;
+	if (phy->mt76->test.tx_antenna_mask &&
+	    (phy->mt76->test.state == MT76_TM_STATE_TX_FRAMES ||
+	     phy->mt76->test.state == MT76_TM_STATE_RX_FRAMES)) {
+		req.tx_streams_num = fls(phy->mt76->test.tx_antenna_mask);
+		req.rx_streams = phy->mt76->test.tx_antenna_mask;
+
+		if (ext_phy) {
+			req.tx_streams_num = 2;
+			req.rx_streams >>= 2;
+		}
 	}
 #endif
 
diff --git a/drivers/net/wireless/mediatek/mt76/mt7915/testmode.c b/drivers/net/wireless/mediatek/mt76/mt7915/testmode.c
index fe56ab18e974..e648f718adb8 100644
--- a/drivers/net/wireless/mediatek/mt76/mt7915/testmode.c
+++ b/drivers/net/wireless/mediatek/mt76/mt7915/testmode.c
@@ -70,8 +70,8 @@ mt7915_tm_set_tx_power(struct mt7915_phy *phy)
 	};
 	u8 *tx_power = NULL;
 
-	if (dev->mt76.test.state != MT76_TM_STATE_OFF)
-		tx_power = dev->mt76.test.tx_power;
+	if (phy->mt76->test.state != MT76_TM_STATE_OFF)
+		tx_power = phy->mt76->test.tx_power;
 
 	/* Tx power of the other antennas are the same as antenna 0 */
 	if (tx_power && tx_power[0])
@@ -85,11 +85,13 @@ mt7915_tm_set_tx_power(struct mt7915_phy *phy)
 }
 
 static int
-mt7915_tm_set_freq_offset(struct mt7915_dev *dev, bool en, u32 val)
+mt7915_tm_set_freq_offset(struct mt7915_phy *phy, bool en, u32 val)
 {
+	struct mt7915_dev *dev = phy->dev;
 	struct mt7915_tm_cmd req = {
 		.testmode_en = en,
 		.param_idx = MCU_ATE_SET_FREQ_OFFSET,
+		.param.freq.band = phy != &dev->phy,
 		.param.freq.freq_offset = cpu_to_le32(val),
 	};
 
@@ -115,9 +117,9 @@ mt7915_tm_mode_ctrl(struct mt7915_dev *dev, bool enable)
 }
 
 static int
-mt7915_tm_set_trx(struct mt7915_dev *dev, struct mt7915_phy *phy,
-		  int type, bool en)
+mt7915_tm_set_trx(struct mt7915_phy *phy, int type, bool en)
 {
+	struct mt7915_dev *dev = phy->dev;
 	struct mt7915_tm_cmd req = {
 		.testmode_en = 1,
 		.param_idx = MCU_ATE_SET_TRX,
@@ -131,14 +133,15 @@ mt7915_tm_set_trx(struct mt7915_dev *dev, struct mt7915_phy *phy,
 }
 
 static void
-mt7915_tm_reg_backup_restore(struct mt7915_dev *dev, struct mt7915_phy *phy)
+mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
 {
 	int n_regs = ARRAY_SIZE(reg_backup_list);
+	struct mt7915_dev *dev = phy->dev;
 	bool ext_phy = phy != &dev->phy;
 	u32 *b = dev->test.reg_backup;
 	int i;
 
-	if (dev->mt76.test.state == MT76_TM_STATE_OFF) {
+	if (dev->mphy.test.state == MT76_TM_STATE_OFF) {
 		for (i = 0; i < n_regs; i++)
 			mt76_wr(dev, reg_backup_list[i].band[ext_phy], b[i]);
 		return;
@@ -182,95 +185,101 @@ mt7915_tm_reg_backup_restore(struct mt7915_dev *dev, struct mt7915_phy *phy)
 }
 
 static void
-mt7915_tm_init(struct mt7915_dev *dev)
+mt7915_tm_init(struct mt7915_phy *phy)
 {
-	bool en = !(dev->mt76.test.state == MT76_TM_STATE_OFF);
+	bool en = !(phy->mt76->test.state == MT76_TM_STATE_OFF);
+	struct mt7915_dev *dev = phy->dev;
 
-	if (!test_bit(MT76_STATE_RUNNING, &dev->phy.mt76->state))
+	if (!test_bit(MT76_STATE_RUNNING, &phy->mt76->state))
 		return;
 
 	mt7915_tm_mode_ctrl(dev, en);
-	mt7915_tm_reg_backup_restore(dev, &dev->phy);
-	mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_TXRX, !en);
+	mt7915_tm_reg_backup_restore(phy);
+	mt7915_tm_set_trx(phy, TM_MAC_TXRX, !en);
 
-	mt7915_mcu_add_bss_info(&dev->phy, dev->phy.monitor_vif, en);
+	mt7915_mcu_add_bss_info(phy, phy->monitor_vif, en);
 }
 
 static void
-mt7915_tm_set_tx_frames(struct mt7915_dev *dev, bool en)
+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 sk_buff *skb = dev->mt76.test.tx_skb;
+	struct sk_buff *skb = phy->mt76->test.tx_skb;
+	struct mt7915_dev *dev = phy->dev;
 	struct ieee80211_tx_info *info;
 
-	mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_RX_RXV, false);
+	mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, false);
 
 	if (en) {
-		u8 tx_ant = dev->mt76.test.tx_antenna_mask;
+		u8 tx_ant = phy->mt76->test.tx_antenna_mask;
 
 		mutex_unlock(&dev->mt76.mutex);
-		mt7915_set_channel(&dev->phy);
+		mt7915_set_channel(phy);
 		mutex_lock(&dev->mt76.mutex);
 
-		mt7915_mcu_set_chan_info(&dev->phy, MCU_EXT_CMD_SET_RX_PATH);
+		mt7915_mcu_set_chan_info(phy, MCU_EXT_CMD_SET_RX_PATH);
+
+		if (phy != &dev->phy)
+			tx_ant >>= 2;
 		dev->test.spe_idx = spe_idx_map[tx_ant];
 	}
 
-	mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_TX, en);
+	mt7915_tm_set_trx(phy, TM_MAC_TX, en);
 
 	if (!en || !skb)
 		return;
 
 	info = IEEE80211_SKB_CB(skb);
-	info->control.vif = dev->phy.monitor_vif;
+	info->control.vif = phy->monitor_vif;
 }
 
 static void
-mt7915_tm_set_rx_frames(struct mt7915_dev *dev, bool en)
+mt7915_tm_set_rx_frames(struct mt7915_phy *phy, bool en)
 {
+	struct mt7915_dev *dev = phy->dev;
 	if (en) {
 		mutex_unlock(&dev->mt76.mutex);
-		mt7915_set_channel(&dev->phy);
+		mt7915_set_channel(phy);
 		mutex_lock(&dev->mt76.mutex);
 
-		mt7915_mcu_set_chan_info(&dev->phy, MCU_EXT_CMD_SET_RX_PATH);
+		mt7915_mcu_set_chan_info(phy, MCU_EXT_CMD_SET_RX_PATH);
 	}
 
-	mt7915_tm_set_trx(dev, &dev->phy, TM_MAC_RX_RXV, en);
+	mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, en);
 }
 
 static void
-mt7915_tm_update_params(struct mt7915_dev *dev, u32 changed)
+mt7915_tm_update_params(struct mt7915_phy *phy, u32 changed)
 {
-	struct mt76_testmode_data *td = &dev->mt76.test;
-	bool en = dev->mt76.test.state != MT76_TM_STATE_OFF;
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	bool en = phy->mt76->test.state != MT76_TM_STATE_OFF;
 
 	if (changed & BIT(TM_CHANGED_FREQ_OFFSET))
-		mt7915_tm_set_freq_offset(dev, en, en ? td->freq_offset : 0);
+		mt7915_tm_set_freq_offset(phy, en, en ? td->freq_offset : 0);
 	if (changed & BIT(TM_CHANGED_TXPOWER))
-		mt7915_tm_set_tx_power(&dev->phy);
+		mt7915_tm_set_tx_power(phy);
 }
 
 static int
-mt7915_tm_set_state(struct mt76_dev *mdev, enum mt76_testmode_state state)
+mt7915_tm_set_state(struct mt76_phy *mphy, enum mt76_testmode_state state)
 {
-	struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
-	struct mt76_testmode_data *td = &mdev->test;
+	struct mt76_testmode_data *td = &mphy->test;
+	struct mt7915_phy *phy = mphy->priv;
 	enum mt76_testmode_state prev_state = td->state;
 
-	mdev->test.state = state;
+	mphy->test.state = state;
 
 	if (prev_state == MT76_TM_STATE_TX_FRAMES)
-		mt7915_tm_set_tx_frames(dev, false);
+		mt7915_tm_set_tx_frames(phy, false);
 	else if (state == MT76_TM_STATE_TX_FRAMES)
-		mt7915_tm_set_tx_frames(dev, true);
+		mt7915_tm_set_tx_frames(phy, true);
 	else if (prev_state == MT76_TM_STATE_RX_FRAMES)
-		mt7915_tm_set_rx_frames(dev, false);
+		mt7915_tm_set_rx_frames(phy, false);
 	else if (state == MT76_TM_STATE_RX_FRAMES)
-		mt7915_tm_set_rx_frames(dev, true);
+		mt7915_tm_set_rx_frames(phy, true);
 	else if (prev_state == MT76_TM_STATE_OFF || state == MT76_TM_STATE_OFF)
-		mt7915_tm_init(dev);
+		mt7915_tm_init(phy);
 
 	if ((state == MT76_TM_STATE_IDLE &&
 	     prev_state == MT76_TM_STATE_OFF) ||
@@ -286,18 +295,18 @@ mt7915_tm_set_state(struct mt76_dev *mdev, enum mt76_testmode_state state)
 				changed |= BIT(i);
 		}
 
-		mt7915_tm_update_params(dev, changed);
+		mt7915_tm_update_params(phy, changed);
 	}
 
 	return 0;
 }
 
 static int
-mt7915_tm_set_params(struct mt76_dev *mdev, struct nlattr **tb,
+mt7915_tm_set_params(struct mt76_phy *mphy, struct nlattr **tb,
 		     enum mt76_testmode_state new_state)
 {
-	struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
-	struct mt76_testmode_data *td = &dev->mt76.test;
+	struct mt76_testmode_data *td = &mphy->test;
+	struct mt7915_phy *phy = mphy->priv;
 	u32 changed = 0;
 	int i;
 
@@ -307,7 +316,7 @@ mt7915_tm_set_params(struct mt76_dev *mdev, struct nlattr **tb,
 	    td->state == MT76_TM_STATE_OFF)
 		return 0;
 
-	if (td->tx_antenna_mask & ~dev->phy.chainmask)
+	if (td->tx_antenna_mask & ~phy->chainmask)
 		return -EINVAL;
 
 	for (i = 0; i < ARRAY_SIZE(tm_change_map); i++) {
@@ -315,15 +324,16 @@ mt7915_tm_set_params(struct mt76_dev *mdev, struct nlattr **tb,
 			changed |= BIT(i);
 	}
 
-	mt7915_tm_update_params(dev, changed);
+	mt7915_tm_update_params(phy, changed);
 
 	return 0;
 }
 
 static int
-mt7915_tm_dump_stats(struct mt76_dev *mdev, struct sk_buff *msg)
+mt7915_tm_dump_stats(struct mt76_phy *mphy, struct sk_buff *msg)
 {
-	struct mt7915_dev *dev = container_of(mdev, struct mt7915_dev, mt76);
+	struct mt7915_phy *phy = mphy->priv;
+	struct mt7915_dev *dev = phy->dev;
 	void *rx, *rssi;
 	int i;