From 567a6f7be24a7f87d550f1cf3e1f1796e1770b2a Mon Sep 17 00:00:00 2001
From: Shayne Chen <shayne.chen@mediatek.com>
Date: Mon, 6 Jun 2022 19:46:26 +0800
Subject: [PATCH 1/3] mt76: mt7915: rework testmode init registers

---
 mt7915/mmio.c     |  2 ++
 mt7915/regs.h     | 16 +++++++++++++--
 mt7915/testmode.c | 52 ++++++++++++++++++++++++++++++++++-------------
 testmode.c        |  3 +--
 4 files changed, 55 insertions(+), 18 deletions(-)

diff --git a/mt7915/mmio.c b/mt7915/mmio.c
index be1b8ea7..9c2c5086 100644
--- a/mt7915/mmio.c
+++ b/mt7915/mmio.c
@@ -68,6 +68,7 @@ static const u32 mt7986_reg[] = {
 };
 
 static const u32 mt7915_offs[] = {
+	[TMAC_TCR2]		= 0x05c,
 	[TMAC_CDTR]		= 0x090,
 	[TMAC_ODTR]		= 0x094,
 	[TMAC_ATCR]		= 0x098,
@@ -142,6 +143,7 @@ static const u32 mt7915_offs[] = {
 };
 
 static const u32 mt7916_offs[] = {
+	[TMAC_TCR2]		= 0x004,
 	[TMAC_CDTR]		= 0x0c8,
 	[TMAC_ODTR]		= 0x0cc,
 	[TMAC_ATCR]		= 0x00c,
diff --git a/mt7915/regs.h b/mt7915/regs.h
index 5180dd93..2e445373 100644
--- a/mt7915/regs.h
+++ b/mt7915/regs.h
@@ -32,6 +32,7 @@ enum reg_rev {
 };
 
 enum offs_rev {
+	TMAC_TCR2,
 	TMAC_CDTR,
 	TMAC_ODTR,
 	TMAC_ATCR,
@@ -182,6 +183,12 @@ enum offs_rev {
 #define MT_TRB_RXPSR0_RX_WTBL_PTR	GENMASK(25, 16)
 #define MT_TRB_RXPSR0_RX_RMAC_PTR	GENMASK(9, 0)
 
+#define MT_MDP_TOP_DBG_WDT_CTRL			MT_MDP(0x0d0)
+#define MT_MDP_TOP_DBG_WDT_CTRL_TDP_DIS_BLK	BIT(7)
+
+#define MT_MDP_TOP_DBG_CTRL			MT_MDP(0x0dc)
+#define MT_MDP_TOP_DBG_CTRL_ENQ_MODE		BIT(30)
+
 /* TMAC: band 0(0x820e4000), band 1(0x820f4000) */
 #define MT_WF_TMAC_BASE(_band)		((_band) ? 0x820f4000 : 0x820e4000)
 #define MT_WF_TMAC(_band, ofs)		(MT_WF_TMAC_BASE(_band) + (ofs))
@@ -190,6 +197,9 @@ enum offs_rev {
 #define MT_TMAC_TCR0_TX_BLINK		GENMASK(7, 6)
 #define MT_TMAC_TCR0_TBTT_STOP_CTRL	BIT(25)
 
+#define MT_TMAC_TCR2(_band)		MT_WF_TMAC(_band, __OFFS(TMAC_TCR2))
+#define MT_TMAC_TCR2_SCH_DET_DIS	BIT(19)
+
 #define MT_TMAC_CDTR(_band)		MT_WF_TMAC(_band, __OFFS(TMAC_CDTR))
  #define MT_TMAC_ODTR(_band)		MT_WF_TMAC(_band, __OFFS(TMAC_ODTR))
 #define MT_TIMEOUT_VAL_PLCP		GENMASK(15, 0)
@@ -461,8 +471,10 @@ enum offs_rev {
 #define MT_AGG_PCR0_VHT_PROT		BIT(13)
 #define MT_AGG_PCR0_PTA_WIN_DIS		BIT(15)
 
-#define MT_AGG_PCR1_RTS0_NUM_THRES	GENMASK(31, 23)
-#define MT_AGG_PCR1_RTS0_LEN_THRES	GENMASK(19, 0)
+#define MT_AGG_PCR1_RTS0_NUM_THRES		GENMASK(31, 23)
+#define MT_AGG_PCR1_RTS0_LEN_THRES		GENMASK(19, 0)
+#define MT_AGG_PCR1_RTS0_NUM_THRES_MT7916	GENMASK(29, 24)
+#define MT_AGG_PCR1_RTS0_LEN_THRES_MT7916	GENMASK(22, 0)
 
 #define MT_AGG_ACR0(_band)		MT_WF_AGG(_band, __OFFS(AGG_ACR0))
 #define MT_AGG_ACR_CFEND_RATE		GENMASK(13, 0)
diff --git a/mt7915/testmode.c b/mt7915/testmode.c
index a979460f..819fafe4 100644
--- a/mt7915/testmode.c
+++ b/mt7915/testmode.c
@@ -30,7 +30,7 @@ struct reg_band {
 		{ _list.band[0] = MT_##_reg(0, _idx);	\
 		  _list.band[1] = MT_##_reg(1, _idx); }
 
-#define TM_REG_MAX_ID	17
+#define TM_REG_MAX_ID	20
 static struct reg_band reg_backup_list[TM_REG_MAX_ID];
 
 
@@ -335,7 +335,7 @@ mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
 {
 	int n_regs = ARRAY_SIZE(reg_backup_list);
 	struct mt7915_dev *dev = phy->dev;
-	u32 *b = phy->test.reg_backup;
+	u32 *b = phy->test.reg_backup, val;
 	int i;
 
 	REG_BAND_IDX(reg_backup_list[0], AGG_PCR0, 0);
@@ -347,18 +347,28 @@ mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
 	REG_BAND(reg_backup_list[6], AGG_MRCR);
 	REG_BAND(reg_backup_list[7], TMAC_TFCR0);
 	REG_BAND(reg_backup_list[8], TMAC_TCR0);
-	REG_BAND(reg_backup_list[9], AGG_ATCR1);
-	REG_BAND(reg_backup_list[10], AGG_ATCR3);
-	REG_BAND(reg_backup_list[11], TMAC_TRCR0);
-	REG_BAND(reg_backup_list[12], TMAC_ICR0);
-	REG_BAND_IDX(reg_backup_list[13], ARB_DRNGR0, 0);
-	REG_BAND_IDX(reg_backup_list[14], ARB_DRNGR0, 1);
-	REG_BAND(reg_backup_list[15], WF_RFCR);
-	REG_BAND(reg_backup_list[16], WF_RFCR1);
+	REG_BAND(reg_backup_list[9], TMAC_TCR2);
+	REG_BAND(reg_backup_list[10], AGG_ATCR1);
+	REG_BAND(reg_backup_list[11], AGG_ATCR3);
+	REG_BAND(reg_backup_list[12], TMAC_TRCR0);
+	REG_BAND(reg_backup_list[13], TMAC_ICR0);
+	REG_BAND_IDX(reg_backup_list[14], ARB_DRNGR0, 0);
+	REG_BAND_IDX(reg_backup_list[15], ARB_DRNGR0, 1);
+	REG_BAND(reg_backup_list[16], WF_RFCR);
+	REG_BAND(reg_backup_list[17], WF_RFCR1);
+
+	if (is_mt7916(&dev->mt76)) {
+		reg_backup_list[18].band[phy->band_idx] = MT_MDP_TOP_DBG_WDT_CTRL;
+		reg_backup_list[19].band[phy->band_idx] = MT_MDP_TOP_DBG_CTRL;
+	}
 
 	if (phy->mt76->test.state == MT76_TM_STATE_OFF) {
-		for (i = 0; i < n_regs; i++)
-			mt76_wr(dev, reg_backup_list[i].band[phy->band_idx], b[i]);
+		for (i = 0; i < n_regs; i++) {
+			u8 reg = reg_backup_list[i].band[phy->band_idx];
+
+			if (reg)
+				mt76_wr(dev, reg, b[i]);
+		}
 		return;
 	}
 
@@ -378,8 +388,13 @@ mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
 		   MT_AGG_PCR0_BW40_PROT | MT_AGG_PCR0_BW80_PROT);
 	mt76_set(dev, MT_AGG_PCR0(phy->band_idx, 0), MT_AGG_PCR0_PTA_WIN_DIS);
 
-	mt76_wr(dev, MT_AGG_PCR0(phy->band_idx, 1), MT_AGG_PCR1_RTS0_NUM_THRES |
-		MT_AGG_PCR1_RTS0_LEN_THRES);
+	if (is_mt7915(&dev->mt76))
+		val = MT_AGG_PCR1_RTS0_NUM_THRES | MT_AGG_PCR1_RTS0_LEN_THRES;
+	else
+		val = MT_AGG_PCR1_RTS0_NUM_THRES_MT7916 |
+		      MT_AGG_PCR1_RTS0_LEN_THRES_MT7916;
+
+	mt76_wr(dev, MT_AGG_PCR0(phy->band_idx, 1), val);
 
 	mt76_clear(dev, MT_AGG_MRCR(phy->band_idx), MT_AGG_MRCR_BAR_CNT_LIMIT |
 		   MT_AGG_MRCR_LAST_RTS_CTS_RN | MT_AGG_MRCR_RTS_FAIL_LIMIT |
@@ -392,10 +407,19 @@ mt7915_tm_reg_backup_restore(struct mt7915_phy *phy)
 
 	mt76_wr(dev, MT_TMAC_TFCR0(phy->band_idx), 0);
 	mt76_clear(dev, MT_TMAC_TCR0(phy->band_idx), MT_TMAC_TCR0_TBTT_STOP_CTRL);
+	mt76_set(dev, MT_TMAC_TCR2(phy->band_idx), MT_TMAC_TCR2_SCH_DET_DIS);
 
 	/* config rx filter for testmode rx */
 	mt76_wr(dev, MT_WF_RFCR(phy->band_idx), 0xcf70a);
 	mt76_wr(dev, MT_WF_RFCR1(phy->band_idx), 0);
+
+	if (is_mt7916(&dev->mt76)) {
+		/* enable MDP Tx block mode */
+		mt76_clear(dev, MT_MDP_TOP_DBG_WDT_CTRL,
+			   MT_MDP_TOP_DBG_WDT_CTRL_TDP_DIS_BLK);
+		mt76_clear(dev, MT_MDP_TOP_DBG_CTRL,
+			   MT_MDP_TOP_DBG_CTRL_ENQ_MODE);
+	}
 }
 
 static void
diff --git a/testmode.c b/testmode.c
index 0accc71a..57cdfdf6 100644
--- a/testmode.c
+++ b/testmode.c
@@ -447,8 +447,7 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_LDPC], &td->tx_rate_ldpc, 0, 1) ||
 	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_STBC], &td->tx_rate_stbc, 0, 1) ||
 	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_LTF], &td->tx_ltf, 0, 2) ||
-	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_ANTENNA],
-			   &td->tx_antenna_mask, 0, 0xff) ||
+	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_ANTENNA], &td->tx_antenna_mask, 1, 0xff) ||
 	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_SPE_IDX], &td->tx_spe_idx, 0, 27) ||
 	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_DUTY_CYCLE],
 			   &td->tx_duty_cycle, 0, 99) ||
-- 
2.25.1