From 5719f0432f1df81f1834096fb0e6a0439d01bf57 Mon Sep 17 00:00:00 2001
From: Shayne Chen <shayne.chen@mediatek.com>
Date: Mon, 3 Jan 2022 17:09:53 +0800
Subject: [PATCH] mt76: mt7915: rework rx testmode stats

---
 mac80211.c        |  3 +-
 mt76.h            |  5 +++
 mt76_connac_mcu.h |  1 +
 mt7915/mcu.h      |  1 +
 mt7915/testmode.c | 82 ++++++++++++++++++++++++++++++++++++++---------
 mt7915/testmode.h | 28 ++++++++++++++++
 testmode.c        |  3 ++
 testmode.h        |  3 ++
 8 files changed, 109 insertions(+), 17 deletions(-)

diff --git a/mac80211.c b/mac80211.c
index a46884d6..d74300f3 100644
--- a/mac80211.c
+++ b/mac80211.c
@@ -751,7 +751,8 @@ void mt76_rx(struct mt76_dev *dev, enum mt76_rxq_id q, struct sk_buff *skb)
 	}
 
 #ifdef CONFIG_NL80211_TESTMODE
-	if (phy->test.state == MT76_TM_STATE_RX_FRAMES) {
+	if (!(phy->test.flag & MT_TM_FW_RX_COUNT) &&
+	    phy->test.state == MT76_TM_STATE_RX_FRAMES) {
 		phy->test.rx_stats.packets[q]++;
 		if (status->flag & RX_FLAG_FAILED_FCS_CRC)
 			phy->test.rx_stats.fcs_error[q]++;
diff --git a/mt76.h b/mt76.h
index 4da77d47..94541cd7 100644
--- a/mt76.h
+++ b/mt76.h
@@ -612,6 +612,8 @@ struct mt76_testmode_ops {
 	int (*dump_stats)(struct mt76_phy *phy, struct sk_buff *msg);
 };
 
+#define MT_TM_FW_RX_COUNT	BIT(0)
+
 struct mt76_testmode_data {
 	enum mt76_testmode_state state;
 
@@ -643,6 +645,8 @@ struct mt76_testmode_data {
 
 	u8 addr[3][ETH_ALEN];
 
+	u8 flag;
+
 	u32 tx_pending;
 	u32 tx_queued;
 	u16 tx_queued_limit;
@@ -650,6 +654,7 @@ struct mt76_testmode_data {
 	struct {
 		u64 packets[__MT_RXQ_MAX];
 		u64 fcs_error[__MT_RXQ_MAX];
+		u64 len_mismatch;
 	} rx_stats;
 };
 
diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
index f1d7c05b..e94d6706 100644
--- a/mt76_connac_mcu.h
+++ b/mt76_connac_mcu.h
@@ -1130,6 +1130,7 @@ enum {
 	MCU_EXT_CMD_OFFCH_SCAN_CTRL = 0x9a,
 	MCU_EXT_CMD_SET_RDD_TH = 0x9d,
 	MCU_EXT_CMD_MURU_CTRL = 0x9f,
+	MCU_EXT_CMD_RX_STAT = 0xa4,
 	MCU_EXT_CMD_SET_SPR = 0xa8,
 	MCU_EXT_CMD_GROUP_PRE_CAL_INFO = 0xab,
 	MCU_EXT_CMD_DPD_PRE_CAL_INFO = 0xac,
diff --git a/mt7915/mcu.h b/mt7915/mcu.h
index cd1edf55..110e4f36 100644
--- a/mt7915/mcu.h
+++ b/mt7915/mcu.h
@@ -9,6 +9,7 @@
 enum {
 	MCU_ATE_SET_TRX = 0x1,
 	MCU_ATE_SET_FREQ_OFFSET = 0xa,
+	MCU_ATE_SET_PHY_COUNT = 0x11,
 	MCU_ATE_SET_SLOT_TIME = 0x13,
 	MCU_ATE_CLEAN_TXQUEUE = 0x1c,
 };
diff --git a/mt7915/testmode.c b/mt7915/testmode.c
index 71793a83..94b24e42 100644
--- a/mt7915/testmode.c
+++ b/mt7915/testmode.c
@@ -133,6 +133,21 @@ mt7915_tm_clean_hwq(struct mt7915_phy *phy, u8 wcid)
 				 sizeof(req), false);
 }
 
+static int
+mt7915_tm_set_phy_count(struct mt7915_phy *phy, u8 control)
+{
+	struct mt7915_dev *dev = phy->dev;
+	struct mt7915_tm_cmd req = {
+		.testmode_en = 1,
+		.param_idx = MCU_ATE_SET_PHY_COUNT,
+		.param.cfg.enable = control,
+		.param.cfg.band = phy != &dev->phy,
+	};
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(ATE_CTRL), &req,
+				 sizeof(req), false);
+}
+
 static int
 mt7915_tm_set_slot_time(struct mt7915_phy *phy, u8 slot_time, u8 sifs)
 {
@@ -439,6 +454,8 @@ mt7915_tm_init(struct mt7915_phy *phy, bool 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);
 }
@@ -504,18 +521,63 @@ mt7915_tm_set_tx_frames(struct mt7915_phy *phy, bool en)
 	mt7915_tm_set_trx(phy, TM_MAC_TX, en);
 }
 
+static int
+mt7915_tm_get_rx_stats(struct mt7915_phy *phy, bool clear)
+{
+#define CMD_RX_STAT_BAND	0x3
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct mt7915_tm_rx_stat_band *rs_band;
+	struct mt7915_dev *dev = phy->dev;
+	struct sk_buff *skb;
+	struct {
+		u8 format_id;
+		u8 band;
+		u8 _rsv[2];
+	} __packed req = {
+		.format_id = CMD_RX_STAT_BAND,
+		.band = phy != &dev->phy,
+	};
+	int ret;
+
+	ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_EXT_CMD(RX_STAT),
+					&req, sizeof(req), true, &skb);
+	if (ret)
+		return ret;
+
+	rs_band = (struct mt7915_tm_rx_stat_band *)skb->data;
+	/* pr_info("mdrdy_cnt = %d\n", le32_to_cpu(rs_band->mdrdy_cnt)); */
+	/* pr_info("fcs_err = %d\n", le16_to_cpu(rs_band->fcs_err)); */
+	/* pr_info("len_mismatch = %d\n", le16_to_cpu(rs_band->len_mismatch)); */
+	/* pr_info("fcs_ok = %d\n", le16_to_cpu(rs_band->fcs_succ)); */
+
+	if (!clear) {
+		enum mt76_rxq_id q = req.band ? MT_RXQ_BAND1 : MT_RXQ_MAIN;
+
+		td->rx_stats.packets[q] += le32_to_cpu(rs_band->mdrdy_cnt);
+		td->rx_stats.fcs_error[q] += le16_to_cpu(rs_band->fcs_err);
+		td->rx_stats.len_mismatch += le16_to_cpu(rs_band->len_mismatch);
+	}
+
+	dev_kfree_skb(skb);
+
+	return 0;
+}
+
 static void
 mt7915_tm_set_rx_frames(struct mt7915_phy *phy, bool en)
 {
 	mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, false);
 
 	if (en) {
-		struct mt7915_dev *dev = phy->dev;
-
 		mt7915_tm_update_channel(phy);
 
 		/* read-clear */
-		mt76_rr(dev, MT_MIB_SDR3(phy != &dev->phy));
+		mt7915_tm_get_rx_stats(phy, true);
+
+		/* clear fw count */
+		mt7915_tm_set_phy_count(phy, 0);
+		mt7915_tm_set_phy_count(phy, 1);
+
 		mt7915_tm_set_trx(phy, TM_MAC_RX_RXV, en);
 	}
 }
@@ -739,12 +801,8 @@ static int
 mt7915_tm_dump_stats(struct mt76_phy *mphy, struct sk_buff *msg)
 {
 	struct mt7915_phy *phy = mphy->priv;
-	struct mt7915_dev *dev = phy->dev;
-	enum mt76_rxq_id q;
 	void *rx, *rssi;
-	u16 fcs_err;
 	int i;
-	u32 cnt;
 
 	rx = nla_nest_start(msg, MT76_TM_STATS_ATTR_LAST_RX);
 	if (!rx)
@@ -788,15 +846,7 @@ mt7915_tm_dump_stats(struct mt76_phy *mphy, struct sk_buff *msg)
 
 	nla_nest_end(msg, rx);
 
-	cnt = mt76_rr(dev, MT_MIB_SDR3(phy->band_idx));
-	fcs_err = is_mt7915(&dev->mt76) ? FIELD_GET(MT_MIB_SDR3_FCS_ERR_MASK, cnt) :
-		FIELD_GET(MT_MIB_SDR3_FCS_ERR_MASK_MT7916, cnt);
-
-	q = phy->band_idx ? MT_RXQ_BAND1 : MT_RXQ_MAIN;
-	mphy->test.rx_stats.packets[q] += fcs_err;
-	mphy->test.rx_stats.fcs_error[q] += fcs_err;
-
-	return 0;
+	return mt7915_tm_get_rx_stats(phy, false);
 }
 
 const struct mt76_testmode_ops mt7915_testmode_ops = {
diff --git a/mt7915/testmode.h b/mt7915/testmode.h
index 5573ac30..a1c54c89 100644
--- a/mt7915/testmode.h
+++ b/mt7915/testmode.h
@@ -33,6 +33,12 @@ struct mt7915_tm_clean_txq {
 	u8 rsv;
 };
 
+struct mt7915_tm_cfg {
+	u8 enable;
+	u8 band;
+	u8 _rsv[2];
+};
+
 struct mt7915_tm_cmd {
 	u8 testmode_en;
 	u8 param_idx;
@@ -43,6 +49,7 @@ struct mt7915_tm_cmd {
 		struct mt7915_tm_freq_offset freq;
 		struct mt7915_tm_slot_time slot;
 		struct mt7915_tm_clean_txq clean;
+		struct mt7915_tm_cfg cfg;
 		u8 test[72];
 	} param;
 } __packed;
@@ -102,4 +109,25 @@ enum {
 	TAM_ARB_OP_MODE_FORCE_SU = 5,
 };
 
+struct mt7915_tm_rx_stat_band {
+	u8 category;
+
+	/* mac */
+	__le16 fcs_err;
+	__le16 len_mismatch;
+	__le16 fcs_succ;
+	__le32 mdrdy_cnt;
+	/* phy */
+	__le16 fcs_err_cck;
+	__le16 fcs_err_ofdm;
+	__le16 pd_cck;
+	__le16 pd_ofdm;
+	__le16 sig_err_cck;
+	__le16 sfd_err_cck;
+	__le16 sig_err_ofdm;
+	__le16 tag_err_ofdm;
+	__le16 mdrdy_cnt_cck;
+	__le16 mdrdy_cnt_ofdm;
+};
+
 #endif
diff --git a/testmode.c b/testmode.c
index b6ceddcb..70fe79a4 100644
--- a/testmode.c
+++ b/testmode.c
@@ -556,6 +556,9 @@ mt76_testmode_dump_stats(struct mt76_phy *phy, struct sk_buff *msg)
 	    nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_PACKETS, rx_packets,
 			      MT76_TM_STATS_ATTR_PAD) ||
 	    nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_FCS_ERROR, rx_fcs_error,
+			      MT76_TM_STATS_ATTR_PAD) ||
+	    nla_put_u64_64bit(msg, MT76_TM_STATS_ATTR_RX_LEN_MISMATCH,
+			      td->rx_stats.len_mismatch,
 			      MT76_TM_STATS_ATTR_PAD))
 		return -EMSGSIZE;
 
diff --git a/testmode.h b/testmode.h
index 5e2792d8..89613266 100644
--- a/testmode.h
+++ b/testmode.h
@@ -101,6 +101,8 @@ enum mt76_testmode_attr {
  * @MT76_TM_STATS_ATTR_RX_FCS_ERROR: number of rx packets with FCS error (u64)
  * @MT76_TM_STATS_ATTR_LAST_RX: information about the last received packet
  *	see &enum mt76_testmode_rx_attr
+ * @MT76_TM_STATS_ATTR_RX_LEN_MISMATCH: number of rx packets with length
+ *	mismatch error (u64)
  */
 enum mt76_testmode_stats_attr {
 	MT76_TM_STATS_ATTR_UNSPEC,
@@ -113,6 +115,7 @@ enum mt76_testmode_stats_attr {
 	MT76_TM_STATS_ATTR_RX_PACKETS,
 	MT76_TM_STATS_ATTR_RX_FCS_ERROR,
 	MT76_TM_STATS_ATTR_LAST_RX,
+	MT76_TM_STATS_ATTR_RX_LEN_MISMATCH,
 
 	/* keep last */
 	NUM_MT76_TM_STATS_ATTRS,
-- 
2.18.0