From 8d0b090a987138808ac8b06978fab0ce58cc47a7 Mon Sep 17 00:00:00 2001
From: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
Date: Thu, 27 Oct 2022 17:42:07 +0800
Subject: [PATCH 1012/1049] wifi: mt76: testmode: add ZWDFS test mode support

Signed-off-by: StanleyYP Wang <StanleyYP.Wang@mediatek.com>
---
 mt76.h            |   9 ++
 mt76_connac_mcu.h |   2 +
 mt7915/mcu.c      |  66 +++++++++++
 mt7915/mcu.h      |  46 ++++++++
 mt7915/mt7915.h   |   4 +
 mt7915/regs.h     |   2 +
 mt7915/testmode.c | 288 ++++++++++++++++++++++++++++++++++++++++++++++
 testmode.c        |  25 +++-
 testmode.h        |  45 ++++++++
 tools/fields.c    |  22 ++++
 10 files changed, 508 insertions(+), 1 deletion(-)

diff --git a/mt76.h b/mt76.h
index 95b5cc92..1abb6499 100644
--- a/mt76.h
+++ b/mt76.h
@@ -793,6 +793,15 @@ struct mt76_testmode_data {
 		u64 fcs_error[__MT_RXQ_MAX];
 		u64 len_mismatch;
 	} rx_stats;
+
+	u8 offchan_ch;
+	u8 offchan_center_ch;
+	u8 offchan_bw;
+
+	u8 ipi_threshold;
+	u32 ipi_period;
+	u8 ipi_antenna_idx;
+	u8 ipi_reset;
 };
 
 struct mt76_vif {
diff --git a/mt76_connac_mcu.h b/mt76_connac_mcu.h
index 1a32268b..4d85d323 100644
--- a/mt76_connac_mcu.h
+++ b/mt76_connac_mcu.h
@@ -1233,6 +1233,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_IPI_HIST_CTRL = 0xa3,
 	MCU_EXT_CMD_RX_STAT = 0xa4,
 	MCU_EXT_CMD_SET_SPR = 0xa8,
 	MCU_EXT_CMD_GROUP_PRE_CAL_INFO = 0xab,
@@ -1243,6 +1244,7 @@ enum {
 	MCU_EXT_CMD_RX_STAT_USER_CTRL = 0xb3,
 	MCU_EXT_CMD_CERT_CFG = 0xb7,
 	MCU_EXT_CMD_CSI_CTRL = 0xc2,
+	MCU_EXT_CMD_IPI_HIST_SCAN = 0xc5,
 };
 
 enum {
diff --git a/mt7915/mcu.c b/mt7915/mcu.c
index d983432f..61cf60e8 100644
--- a/mt7915/mcu.c
+++ b/mt7915/mcu.c
@@ -2759,6 +2759,7 @@ mt7915_mcu_background_chain_ctrl(struct mt7915_phy *phy,
 		req.monitor_chan = chandef->chan->hw_value;
 		req.monitor_central_chan =
 			ieee80211_frequency_to_channel(chandef->center_freq1);
+		req.monitor_bw = mt76_connac_chan_bw(chandef);
 		req.band_idx = phy->mt76->band_idx;
 		req.scan_mode = 2;
 		break;
@@ -4907,3 +4908,68 @@ int mt7915_mcu_set_amsdu_algo(struct mt7915_dev *dev, u16 wcid, u8 enable)
 	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(MEC_CTRL), &req, sizeof(req), true);
 }
 #endif
+
+int mt7915_mcu_ipi_hist_ctrl(struct mt7915_phy *phy, void *data, u8 cmd, bool wait_resp)
+{
+	struct mt7915_dev *dev = phy->dev;
+	struct sk_buff *skb;
+	int ret;
+	struct {
+		u8 ipi_hist_idx;
+		u8 band_idx;
+		u8 set_val;
+		u8 rsv;
+		int idle_power_th;
+		u32 idle_power_max_cnt;
+		u32 idle_power_duration;
+		u32 idle_power_cmd_type;
+	} __packed req = {
+		.ipi_hist_idx = cmd,
+		.band_idx = phy->mt76->band_idx,
+	};
+
+	if (!wait_resp)
+		return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(IPI_HIST_CTRL),
+					 &req, sizeof(req), true);
+
+	ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_EXT_QUERY(IPI_HIST_CTRL),
+					&req, sizeof(req), wait_resp, &skb);
+
+	if (ret)
+		return ret;
+
+	memcpy(data, skb->data, sizeof(struct mt7915_mcu_rdd_ipi_ctrl));
+	dev_kfree_skb(skb);
+
+	return 0;
+}
+
+int mt7915_mcu_ipi_hist_scan(struct mt7915_phy *phy, void *data, u8 mode, bool wait_resp)
+{
+	struct mt7915_dev *dev = phy->dev;
+	struct sk_buff *skb;
+	int ret;
+	struct rdd_ipi_hist_scan {
+		u8 mode;
+		u8 pd_setting;
+		u8 band_idx;
+		u8 rsv;
+	} __packed req = {
+		.mode = mode,
+		.pd_setting = 1,
+		.band_idx = phy->mt76->band_idx,
+	};
+
+	ret = mt76_mcu_send_and_get_msg(&dev->mt76, MCU_EXT_CMD(IPI_HIST_SCAN),
+					&req, sizeof(req), wait_resp, &skb);
+	if (ret)
+		return ret;
+
+	if (!wait_resp)
+		return 0;
+
+	memcpy(data, skb->data, sizeof(struct mt7915_mcu_rdd_ipi_scan));
+	dev_kfree_skb(skb);
+
+	return 0;
+}
diff --git a/mt7915/mcu.h b/mt7915/mcu.h
index c791c7fa..066246bb 100644
--- a/mt7915/mcu.h
+++ b/mt7915/mcu.h
@@ -698,6 +698,52 @@ enum CSI_CHAIN_TYPE {
 };
 #endif
 
+enum {
+	RDD_SET_IPI_CR_INIT,		/* CR initialization */
+	RDD_SET_IPI_HIST_RESET,		/* Reset IPI histogram counter */
+	RDD_SET_IDLE_POWER,		/* Idle power info */
+	RDD_SET_IPI_HIST_NUM
+};
+
+enum {
+	RDD_IPI_HIST_0,			/* IPI count for power <= -92 (dBm) */
+	RDD_IPI_HIST_1,			/* IPI count for -92 < power <= -89 (dBm) */
+	RDD_IPI_HIST_2,			/* IPI count for -89 < power <= -86 (dBm) */
+	RDD_IPI_HIST_3,			/* IPI count for -86 < power <= -83 (dBm) */
+	RDD_IPI_HIST_4,			/* IPI count for -83 < power <= -80 (dBm) */
+	RDD_IPI_HIST_5,			/* IPI count for -80 < power <= -75 (dBm) */
+	RDD_IPI_HIST_6,			/* IPI count for -75 < power <= -70 (dBm) */
+	RDD_IPI_HIST_7,			/* IPI count for -70 < power <= -65 (dBm) */
+	RDD_IPI_HIST_8,			/* IPI count for -65 < power <= -60 (dBm) */
+	RDD_IPI_HIST_9,			/* IPI count for -60 < power <= -55 (dBm) */
+	RDD_IPI_HIST_10,		/* IPI count for -55 < power        (dBm) */
+	RDD_IPI_FREE_RUN_CNT,		/* IPI count for counter++ per 8 us */
+	RDD_IPI_HIST_ALL_CNT,		/* Get all IPI */
+	RDD_IPI_HIST_0_TO_10_CNT,	/* Get IPI histogram 0 to 10 */
+	RDD_IPI_HIST_2_TO_10_CNT,	/* Get IPI histogram 2 to 10 */
+	RDD_TX_ASSERT_TIME,		/* Get band 1 TX assert time */
+	RDD_IPI_HIST_NUM
+};
+
+#define RDM_NF_MAX_WF_IDX		8
+#define POWER_INDICATE_HIST_MAX		RDD_IPI_FREE_RUN_CNT
+#define IPI_HIST_TYPE_NUM		(POWER_INDICATE_HIST_MAX + 1)
+
+struct mt7915_mcu_rdd_ipi_ctrl {
+	u8 ipi_hist_idx;
+	u8 band_idx;
+	u8 rsv[2];
+	u32 ipi_hist_val[IPI_HIST_TYPE_NUM];
+	u32 tx_assert_time;						/* unit: us */
+} __packed;
+
+struct mt7915_mcu_rdd_ipi_scan {
+	u32 ipi_hist_val[RDM_NF_MAX_WF_IDX][POWER_INDICATE_HIST_MAX];
+	u8 band_idx;
+	u8 rsv[2];
+	u8 tx_assert_time;						/* unit: us */
+} __packed;
+
 /* MURU */
 #define OFDMA_DL                       BIT(0)
 #define OFDMA_UL                       BIT(1)
diff --git a/mt7915/mt7915.h b/mt7915/mt7915.h
index d77a5f0c..827ee295 100644
--- a/mt7915/mt7915.h
+++ b/mt7915/mt7915.h
@@ -293,6 +293,7 @@ struct mt7915_phy {
 
 	struct mt76_mib_stats mib;
 	struct mt76_channel_state state_ts;
+	struct delayed_work ipi_work;
 
 	u8 stats_work_count;
 	struct list_head stats_list;
@@ -753,6 +754,9 @@ int mt7915_vendor_amnt_sta_remove(struct mt7915_phy *phy,
 				  struct ieee80211_sta *sta);
 #endif
 
+int mt7915_mcu_ipi_hist_ctrl(struct mt7915_phy *phy, void *data, u8 cmd, bool wait_resp);
+int mt7915_mcu_ipi_hist_scan(struct mt7915_phy *phy, void *data, u8 mode, bool wait_resp);
+
 #ifdef MTK_DEBUG
 int mt7915_mtk_init_debugfs(struct mt7915_phy *phy, struct dentry *dir);
 int mt7915_dbg_mcu_wa_cmd(struct mt7915_dev *dev, int cmd, u32 a1, u32 a2, u32 a3, bool wait_resp);
diff --git a/mt7915/regs.h b/mt7915/regs.h
index 67837975..44863e88 100644
--- a/mt7915/regs.h
+++ b/mt7915/regs.h
@@ -1209,6 +1209,8 @@ enum offs_rev {
 #define MT_WF_IRPI_NSS(phy, nss)	MT_WF_IRPI(0x6000 + ((phy) << 20) + ((nss) << 16))
 #define MT_WF_IRPI_NSS_MT7916(phy, nss)	MT_WF_IRPI(0x1000 + ((phy) << 20) + ((nss) << 16))
 
+#define MT_WF_IPI_RESET			0x831a3008
+
 /* PHY */
 #define MT_WF_PHY_BASE			0x83080000
 #define MT_WF_PHY(ofs)			(MT_WF_PHY_BASE + (ofs))
diff --git a/mt7915/testmode.c b/mt7915/testmode.c
index 453319e1..caa35906 100644
--- a/mt7915/testmode.c
+++ b/mt7915/testmode.c
@@ -14,6 +14,12 @@ enum {
 	TM_CHANGED_AID,
 	TM_CHANGED_CFG,
 	TM_CHANGED_TXBF_ACT,
+	TM_CHANGED_OFF_CHAN_CH,
+	TM_CHANGED_OFF_CHAN_CENTER_CH,
+	TM_CHANGED_OFF_CHAN_BW,
+	TM_CHANGED_IPI_THRESHOLD,
+	TM_CHANGED_IPI_PERIOD,
+	TM_CHANGED_IPI_RESET,
 
 	/* must be last */
 	NUM_TM_CHANGED
@@ -26,6 +32,12 @@ static const u8 tm_change_map[] = {
 	[TM_CHANGED_AID] = MT76_TM_ATTR_AID,
 	[TM_CHANGED_CFG] = MT76_TM_ATTR_CFG,
 	[TM_CHANGED_TXBF_ACT] = MT76_TM_ATTR_TXBF_ACT,
+	[TM_CHANGED_OFF_CHAN_CH] = MT76_TM_ATTR_OFF_CH_SCAN_CH,
+	[TM_CHANGED_OFF_CHAN_CENTER_CH] = MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH,
+	[TM_CHANGED_OFF_CHAN_BW] = MT76_TM_ATTR_OFF_CH_SCAN_BW,
+	[TM_CHANGED_IPI_THRESHOLD] = MT76_TM_ATTR_IPI_THRESHOLD,
+	[TM_CHANGED_IPI_PERIOD] = MT76_TM_ATTR_IPI_PERIOD,
+	[TM_CHANGED_IPI_RESET] = MT76_TM_ATTR_IPI_RESET,
 };
 
 struct reg_band {
@@ -984,6 +996,272 @@ mt7915_tm_set_txbf(struct mt7915_phy *phy)
 	return 0;
 }
 
+static u8
+mt7915_tm_get_center_chan(struct mt7915_phy *phy, struct cfg80211_chan_def *chandef,
+			  int width_mhz)
+{
+	struct mt76_phy *mphy = phy->mt76;
+	const struct ieee80211_channel *chan = mphy->sband_5g.sband.channels;
+	u32 bitmap, i, offset, size = 32;
+	u16 first_control = 0, control_chan = chandef->chan->hw_value;
+	static const u32 width_to_bitmap[] = {
+		[NL80211_CHAN_WIDTH_20_NOHT] = 0x0,
+		[NL80211_CHAN_WIDTH_20] = 0x0,
+		[NL80211_CHAN_WIDTH_40] = 0x55554055,
+		[NL80211_CHAN_WIDTH_80] = 0x44444011,
+		[NL80211_CHAN_WIDTH_80P80] = 0x0,
+		[NL80211_CHAN_WIDTH_160] = 0x04004001,
+	};
+
+	bitmap = width_to_bitmap[chandef->width];
+	if (!bitmap)
+		return control_chan;
+
+	offset = width_mhz / 10 - 2;
+	for (i = 0; i < size; i++) {
+		if (!((1 << i) & bitmap))
+			continue;
+
+		if (control_chan >= chan[i].hw_value)
+			first_control = chan[i].hw_value;
+		else
+			break;
+	}
+
+	if (chandef->width == NL80211_CHAN_WIDTH_40 &&
+	    control_chan >= chan[size].hw_value)
+		return chan[size].hw_value + offset;
+	else if (first_control == 0)
+		return control_chan;
+
+	return first_control + offset;
+}
+
+static int
+mt7915_tm_set_offchan(struct mt7915_phy *phy, bool no_center)
+{
+	struct mt76_phy *mphy = phy->mt76;
+	struct mt7915_dev *dev = phy->dev;
+	struct ieee80211_hw *hw = mphy->hw;
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct cfg80211_chan_def chandef = {};
+	struct ieee80211_channel *chan;
+	int ret, freq = ieee80211_channel_to_frequency(td->offchan_ch, NL80211_BAND_5GHZ);
+	int width_mhz;
+	const int bw_to_mhz[] = {
+		[NL80211_CHAN_WIDTH_20_NOHT] = 20,
+		[NL80211_CHAN_WIDTH_20] = 20,
+		[NL80211_CHAN_WIDTH_40] = 40,
+		[NL80211_CHAN_WIDTH_80] = 80,
+		[NL80211_CHAN_WIDTH_80P80] = 80,
+		[NL80211_CHAN_WIDTH_160] = 160,
+	};
+
+	if (!mphy->cap.has_5ghz || !freq) {
+		ret = -EINVAL;
+		dev_info(dev->mt76.dev, "Failed to set offchan (invalid band or channel)!\n");
+		goto out;
+	}
+
+	chandef.width = td->offchan_bw;
+	width_mhz = bw_to_mhz[chandef.width];
+	chan = ieee80211_get_channel(hw->wiphy, freq);
+	if (!chan) {
+		ret = -EINVAL;
+		dev_info(dev->mt76.dev, "Failed to set offchan (invalid control channel)!\n");
+		goto out;
+	}
+	chandef.chan = chan;
+
+	if (no_center)
+		td->offchan_center_ch = mt7915_tm_get_center_chan(phy, &chandef, width_mhz);
+	chandef.center_freq1 = ieee80211_channel_to_frequency(td->offchan_center_ch,
+							      NL80211_BAND_5GHZ);
+	if (!cfg80211_chandef_valid(&chandef)) {
+		ret = -EINVAL;
+		dev_info(dev->mt76.dev, "Failed to set offchan, chandef is invalid!\n");
+		goto out;
+	}
+
+	memset(&dev->rdd2_chandef, 0, sizeof(struct cfg80211_chan_def));
+
+	ret = mt7915_mcu_rdd_background_enable(phy, &chandef);
+
+	if (ret)
+		goto out;
+
+	dev->rdd2_phy = phy;
+	dev->rdd2_chandef = chandef;
+
+	return ret;
+
+out:
+	td->offchan_ch = 0;
+	td->offchan_center_ch = 0;
+	td->offchan_bw = 0;
+
+	return ret;
+}
+
+static void
+mt7915_tm_dump_ipi(struct mt7915_phy *phy, void *data, u8 antenna_num,
+		   u8 start_antenna_idx, bool is_scan)
+{
+#define PRECISION	100
+	struct mt7915_dev *dev = phy->dev;
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct mt7915_mcu_rdd_ipi_scan *scan_data;
+	struct mt7915_mcu_rdd_ipi_ctrl *ctrl_data;
+	u32 ipi_idx, ipi_free_count, ipi_percentage, ipi_hist_count_th, ipi_hist_total_count;
+	u32 self_idle_ratio, ipi_idle_ratio, channel_load, tx_assert_time;
+	u8 i, antenna_idx = start_antenna_idx;
+	u32 *ipi_hist_data;
+	const char *power_lower_bound, *power_upper_bound;
+	static const char * const ipi_idx_to_power_bound[] = {
+		[RDD_IPI_HIST_0] = "-92",
+		[RDD_IPI_HIST_1] = "-89",
+		[RDD_IPI_HIST_2] = "-86",
+		[RDD_IPI_HIST_3] = "-83",
+		[RDD_IPI_HIST_4] = "-80",
+		[RDD_IPI_HIST_5] = "-75",
+		[RDD_IPI_HIST_6] = "-70",
+		[RDD_IPI_HIST_7] = "-65",
+		[RDD_IPI_HIST_8] = "-60",
+		[RDD_IPI_HIST_9] = "-55",
+		[RDD_IPI_HIST_10] = "inf",
+	};
+
+	if (is_scan) {
+		scan_data = (struct mt7915_mcu_rdd_ipi_scan *)data;
+		tx_assert_time = scan_data->tx_assert_time;
+	} else {
+		ctrl_data = (struct mt7915_mcu_rdd_ipi_ctrl *)data;
+		tx_assert_time = ctrl_data->tx_assert_time;
+	}
+
+	for (i = 0; i < antenna_num; i++) {
+		ipi_free_count = 0;
+		ipi_hist_count_th = 0;
+		ipi_hist_total_count = 0;
+		ipi_hist_data = is_scan ? scan_data->ipi_hist_val[antenna_idx] :
+					  ctrl_data->ipi_hist_val;
+
+		dev_info(dev->mt76.dev, "Antenna index: %d\n", antenna_idx);
+		for (ipi_idx = 0; ipi_idx < POWER_INDICATE_HIST_MAX; ipi_idx++) {
+			power_lower_bound = ipi_idx ? ipi_idx_to_power_bound[ipi_idx - 1] :
+						      "-inf";
+			power_upper_bound = ipi_idx_to_power_bound[ipi_idx];
+
+			dev_info(dev->mt76.dev,
+				 "IPI %d (power range: (%s, %s] dBm): ipi count = %d\n",
+				 ipi_idx, power_lower_bound,
+				 power_upper_bound, ipi_hist_data[ipi_idx]);
+
+			if (td->ipi_threshold <= ipi_idx && ipi_idx <= RDD_IPI_HIST_10)
+				ipi_hist_count_th += ipi_hist_data[ipi_idx];
+
+			ipi_hist_total_count += ipi_hist_data[ipi_idx];
+		}
+		ipi_free_count = is_scan ? ipi_hist_total_count :
+					   ipi_hist_data[RDD_IPI_FREE_RUN_CNT];
+
+		dev_info(dev->mt76.dev,
+			 "IPI threshold %d: ipi_hist_count_th = %d, ipi_free_count = %d\n",
+			 td->ipi_threshold, ipi_hist_count_th, ipi_free_count);
+		dev_info(dev->mt76.dev, "TX assert time =  %d [ms]\n",
+			 tx_assert_time / 1000);
+
+		// Calculate channel load = (self idle ratio - idle ratio) / self idle ratio
+		if (ipi_hist_count_th >= UINT_MAX / (100 * PRECISION))
+			ipi_percentage = 100 * PRECISION *
+					(ipi_hist_count_th / (100 * PRECISION)) /
+					(ipi_free_count / (100 * PRECISION));
+		else
+			ipi_percentage = PRECISION * 100 * ipi_hist_count_th / ipi_free_count;
+
+		ipi_idle_ratio = ((100 * PRECISION) - ipi_percentage) / PRECISION;
+
+		self_idle_ratio = PRECISION * 100 *
+				  (td->ipi_period - (tx_assert_time / 1000)) /
+				  td->ipi_period / PRECISION;
+
+		if (self_idle_ratio < ipi_idle_ratio)
+			channel_load = 0;
+		else
+			channel_load = self_idle_ratio - ipi_idle_ratio;
+
+		if (self_idle_ratio <= td->ipi_threshold) {
+			dev_info(dev->mt76.dev,
+				 "band[%d]: self idle ratio = %d%%, idle ratio = %d%%\n",
+				 phy->mt76->band_idx, self_idle_ratio, ipi_idle_ratio);
+			return;
+		}
+
+		channel_load = (100 * channel_load) / self_idle_ratio;
+		dev_info(dev->mt76.dev,
+			 "band[%d]: chan load = %d%%, self idle ratio = %d%%, idle ratio = %d%%\n",
+			 phy->mt76->band_idx, channel_load, self_idle_ratio, ipi_idle_ratio);
+		antenna_idx++;
+	}
+}
+
+static void
+mt7915_tm_ipi_work(struct work_struct *work)
+{
+	struct mt7915_phy *phy = container_of(work, struct mt7915_phy, ipi_work.work);
+	struct mt7915_dev *dev = phy->dev;
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	u8 start_antenna_idx = 0, antenna_num = 1;
+
+	if (!is_mt7915(&dev->mt76)) {
+		struct mt7915_mcu_rdd_ipi_scan data;
+
+		if (phy->mt76->band_idx)
+			start_antenna_idx = 4;
+
+		/* Use all antenna */
+		if (td->ipi_antenna_idx == MT76_TM_IPI_ANTENNA_ALL)
+			antenna_num = 4;
+		else
+			start_antenna_idx += td->ipi_antenna_idx;
+
+		mt7915_mcu_ipi_hist_scan(phy, &data, 0, true);
+		mt7915_tm_dump_ipi(phy, &data, antenna_num, start_antenna_idx, true);
+	} else {
+		struct mt7915_mcu_rdd_ipi_ctrl data;
+
+		start_antenna_idx = 4;
+		mt7915_mcu_ipi_hist_ctrl(phy, &data, RDD_IPI_HIST_ALL_CNT, true);
+		mt7915_tm_dump_ipi(phy, &data, antenna_num, start_antenna_idx, false);
+	}
+}
+
+static inline void
+mt7915_tm_reset_ipi(struct mt7915_phy *phy)
+{
+#define IPI_RESET_BIT	BIT(2)
+	struct mt7915_dev *dev = phy->dev;
+
+	if (is_mt7915(&dev->mt76))
+		mt7915_mcu_ipi_hist_ctrl(phy, NULL, RDD_SET_IPI_HIST_RESET, false);
+	else
+		mt76_set(dev, MT_WF_IPI_RESET, IPI_RESET_BIT);
+}
+
+static int
+mt7915_tm_set_ipi(struct mt7915_phy *phy)
+{
+	struct mt76_testmode_data *td = &phy->mt76->test;
+
+	mt7915_tm_reset_ipi(phy);
+
+	cancel_delayed_work(&phy->ipi_work);
+	ieee80211_queue_delayed_work(phy->mt76->hw, &phy->ipi_work,
+				     msecs_to_jiffies(td->ipi_period));
+
+	return 0;
+}
+
 static int
 mt7915_tm_set_wmm_qid(struct mt7915_phy *phy, u8 qid, u8 aifs, u8 cw_min,
 		      u16 cw_max, u16 txop, u8 tx_cmd)
@@ -1273,6 +1551,8 @@ mt7915_tm_init(struct mt7915_phy *phy, bool en)
 		phy->mt76->test.tx_mpdu_len = 0;
 		phy->test.bf_en = 0;
 		mt7915_tm_set_entry(phy);
+	} else {
+		INIT_DELAYED_WORK(&phy->ipi_work, mt7915_tm_ipi_work);
 	}
 }
 
@@ -2038,6 +2318,14 @@ mt7915_tm_update_params(struct mt7915_phy *phy, u32 changed)
 		mt7915_tm_set_cfg(phy);
 	if (changed & BIT(TM_CHANGED_TXBF_ACT))
 		mt7915_tm_set_txbf(phy);
+	if ((changed & BIT(TM_CHANGED_OFF_CHAN_CH)) &&
+	    (changed & BIT(TM_CHANGED_OFF_CHAN_BW)))
+		mt7915_tm_set_offchan(phy, !(changed & BIT(TM_CHANGED_OFF_CHAN_CENTER_CH)));
+	if ((changed & BIT(TM_CHANGED_IPI_THRESHOLD)) &&
+	    (changed & BIT(TM_CHANGED_IPI_PERIOD)))
+		mt7915_tm_set_ipi(phy);
+	if (changed & BIT(TM_CHANGED_IPI_RESET))
+		mt7915_tm_reset_ipi(phy);
 }
 
 static int
diff --git a/testmode.c b/testmode.c
index 898ef3ae..dc972e3e 100644
--- a/testmode.c
+++ b/testmode.c
@@ -26,6 +26,13 @@ const struct nla_policy mt76_tm_policy[NUM_MT76_TM_ATTRS] = {
 	[MT76_TM_ATTR_TX_TIME] = { .type = NLA_U32 },
 	[MT76_TM_ATTR_FREQ_OFFSET] = { .type = NLA_U32 },
 	[MT76_TM_ATTR_DRV_DATA] = { .type = NLA_NESTED },
+	[MT76_TM_ATTR_OFF_CH_SCAN_CH] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_OFF_CH_SCAN_PATH] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_IPI_THRESHOLD] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_IPI_PERIOD] = { .type = NLA_U32 },
+	[MT76_TM_ATTR_IPI_ANTENNA_INDEX] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_IPI_RESET] = { .type = NLA_U8 },
 };
 EXPORT_SYMBOL_GPL(mt76_tm_policy);
 
@@ -407,6 +414,7 @@ mt76_testmode_init_defaults(struct mt76_phy *phy)
 	td->tx_count = 1;
 	td->tx_rate_mode = MT76_TM_TX_MODE_OFDM;
 	td->tx_rate_nss = 1;
+	td->ipi_antenna_idx = MT76_TM_IPI_ANTENNA_ALL;
 
 	memcpy(td->addr[0], phy->macaddr, ETH_ALEN);
 	memcpy(td->addr[1], phy->macaddr, ETH_ALEN);
@@ -615,6 +623,9 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 	if (tb[MT76_TM_ATTR_TX_RATE_IDX])
 		td->tx_rate_idx = nla_get_u8(tb[MT76_TM_ATTR_TX_RATE_IDX]);
 
+	if (tb[MT76_TM_ATTR_IPI_PERIOD])
+		td->ipi_period = nla_get_u32(tb[MT76_TM_ATTR_IPI_PERIOD]);
+
 	if (mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_MODE], &td->tx_rate_mode,
 			   0, MT76_TM_TX_MODE_MAX) ||
 	    mt76_tm_get_u8(tb[MT76_TM_ATTR_TX_RATE_NSS], &td->tx_rate_nss,
@@ -631,7 +642,16 @@ int mt76_testmode_cmd(struct ieee80211_hw *hw, struct ieee80211_vif *vif,
 			   &td->tx_power_control, 0, 1) ||
 	    mt76_tm_get_u8(tb[MT76_TM_ATTR_AID], &td->aid, 0, 16) ||
 	    mt76_tm_get_u8(tb[MT76_TM_ATTR_RU_ALLOC], &td->ru_alloc, 0, 0xff) ||
-	    mt76_tm_get_u8(tb[MT76_TM_ATTR_RU_IDX], &td->ru_idx, 0, 68))
+	    mt76_tm_get_u8(tb[MT76_TM_ATTR_RU_IDX], &td->ru_idx, 0, 68) ||
+	    mt76_tm_get_u8(tb[MT76_TM_ATTR_OFF_CH_SCAN_CH], &td->offchan_ch, 36, 196) ||
+	    mt76_tm_get_u8(tb[MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH], &td->offchan_center_ch,
+			   36, 196) ||
+	    mt76_tm_get_u8(tb[MT76_TM_ATTR_OFF_CH_SCAN_BW],
+			   &td->offchan_bw, NL80211_CHAN_WIDTH_20_NOHT, NL80211_CHAN_WIDTH_160) ||
+	    mt76_tm_get_u8(tb[MT76_TM_ATTR_IPI_THRESHOLD], &td->ipi_threshold, 0, 10) ||
+	    mt76_tm_get_u8(tb[MT76_TM_ATTR_IPI_ANTENNA_INDEX], &td->ipi_antenna_idx,
+			   MT76_TM_IPI_ANTENNA_0, MT76_TM_IPI_ANTENNA_ALL) ||
+	    mt76_tm_get_u8(tb[MT76_TM_ATTR_IPI_RESET], &td->ipi_reset, 0, 1))
 		goto out;
 
 	if (tb[MT76_TM_ATTR_TX_LENGTH]) {
@@ -866,6 +886,9 @@ int mt76_testmode_dump(struct ieee80211_hw *hw, struct sk_buff *msg,
 	    nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_MODE, td->tx_rate_mode) ||
 	    nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_SGI, td->tx_rate_sgi) ||
 	    nla_put_u8(msg, MT76_TM_ATTR_TX_RATE_STBC, td->tx_rate_stbc) ||
+	    nla_put_u8(msg, MT76_TM_ATTR_OFF_CH_SCAN_CH, td->offchan_ch) ||
+	    nla_put_u8(msg, MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH, td->offchan_center_ch) ||
+	    nla_put_u8(msg, MT76_TM_ATTR_OFF_CH_SCAN_BW, td->offchan_bw) ||
 	    (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_LTF) &&
 	     nla_put_u8(msg, MT76_TM_ATTR_TX_LTF, td->tx_ltf)) ||
 	    (mt76_testmode_param_present(td, MT76_TM_ATTR_TX_ANTENNA) &&
diff --git a/testmode.h b/testmode.h
index 1d7aef86..b39cf511 100644
--- a/testmode.h
+++ b/testmode.h
@@ -64,6 +64,20 @@
  *	(nested, u8 attrs)
  *
  * @MT76_TM_ATTR_CFG: config testmode rf feature (nested, see &mt76_testmode_cfg)
+ * @MT76_TM_ATTR_TXBF_ACT: txbf setting actions (u8)
+ * @MT76_TM_ATTR_TXBF_PARAM: txbf parameters (nested)
+ *
+ * @MT76_TM_ATTR_OFF_CH_SCAN_CH: config the channel of background chain (ZWDFS) (u8)
+ * @MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH: config the center channel of background chain (ZWDFS) (u8)
+ * @MT76_TM_ATTR_OFF_CH_SCAN_BW: config the bandwidth of background chain (ZWDFS) (u8)
+ * @MT76_TM_ATTR_OFF_CH_SCAN_PATH: config the tx path of background chain (ZWDFS) (u8)
+ *
+ * @MT76_TM_ATTR_IPI_THRESHOLD: config the IPI index you want to read (u8)
+ * @MT76_TM_ATTR_IPI_PERIOD: config the time period for reading
+ *			     the histogram of specific IPI index (u8)
+ * @MT76_TM_ATTR_IPI_ANTENNA_INDEX: config the antenna index for reading
+ *				    the histogram of specific IPI index (u8)
+ * @MT76_TM_ATTR_IPI_RESET: Reset the IPI counter
  *
  */
 enum mt76_testmode_attr {
@@ -118,6 +132,16 @@ enum mt76_testmode_attr {
 	MT76_TM_ATTR_TXBF_ACT,
 	MT76_TM_ATTR_TXBF_PARAM,
 
+	MT76_TM_ATTR_OFF_CH_SCAN_CH,
+	MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH,
+	MT76_TM_ATTR_OFF_CH_SCAN_BW,
+	MT76_TM_ATTR_OFF_CH_SCAN_PATH,
+
+	MT76_TM_ATTR_IPI_THRESHOLD,
+	MT76_TM_ATTR_IPI_PERIOD,
+	MT76_TM_ATTR_IPI_ANTENNA_INDEX,
+	MT76_TM_ATTR_IPI_RESET,
+
 	/* keep last */
 	NUM_MT76_TM_ATTRS,
 	MT76_TM_ATTR_MAX = NUM_MT76_TM_ATTRS - 1,
@@ -296,4 +320,25 @@ enum mt76_testmode_txbf_act {
 	MT76_TM_TXBF_ACT_MAX = NUM_MT76_TM_TXBF_ACT - 1,
 };
 
+/**
+ * enum mt76_testmode_ipi_antenna - specify antenna index for ipi count
+ *
+ * @MT76_TM_IPI_ANTENNA_0: use antenna 0
+ * @MT76_TM_IPI_ANTENNA_1: use antenna 1
+ * @MT76_TM_IPI_ANTENNA_2: use antenna 2
+ * @MT76_TM_IPI_ANTENNA_3: use antenna 3
+ * @MT76_TM_IPI_ANTENNA_ALL: use all antenna
+ */
+enum mt76_testmode_ipi_antenna {
+	MT76_TM_IPI_ANTENNA_0,
+	MT76_TM_IPI_ANTENNA_1,
+	MT76_TM_IPI_ANTENNA_2,
+	MT76_TM_IPI_ANTENNA_3,
+	MT76_TM_IPI_ANTENNA_ALL,
+
+	/* keep last */
+	NUM_MT76_TM_IPI_ANTENNA,
+	MT76_TM_IPI_ANTENNA_MAX = NUM_MT76_TM_IPI_ANTENNA - 1,
+};
+
 #endif
diff --git a/tools/fields.c b/tools/fields.c
index b0ee84d2..e2cf4b92 100644
--- a/tools/fields.c
+++ b/tools/fields.c
@@ -46,6 +46,14 @@ static const char * const testmode_txbf_act[] = {
 	[MT76_TM_TXBF_ACT_E2P_UPDATE] = "e2p_update",
 };
 
+static const char * const testmode_offchan_bw[] = {
+	[NL80211_CHAN_WIDTH_20_NOHT] = "NOHT",
+	[NL80211_CHAN_WIDTH_20] = "20",
+	[NL80211_CHAN_WIDTH_40] = "40",
+	[NL80211_CHAN_WIDTH_80] = "80",
+	[NL80211_CHAN_WIDTH_160] = "160",
+};
+
 static void print_enum(const struct tm_field *field, struct nlattr *attr)
 {
 	unsigned int i = nla_get_u8(attr);
@@ -414,6 +422,13 @@ static const struct tm_field testdata_fields[NUM_MT76_TM_ATTRS] = {
 	FIELD(u8, RU_IDX, "ru_idx"),
 	FIELD_ENUM(TXBF_ACT, "txbf_act", testmode_txbf_act),
 	FIELD_ARRAY(u16_hex, TXBF_PARAM, "txbf_param"),
+	FIELD(u8, OFF_CH_SCAN_CH, "offchan_ch"),
+	FIELD(u8, OFF_CH_SCAN_CENTER_CH, "offchan_center_ch"),
+	FIELD_ENUM(OFF_CH_SCAN_BW, "offchan_bw", testmode_offchan_bw),
+	FIELD(u8, IPI_THRESHOLD, "ipi_threshold"),
+	FIELD(u32, IPI_PERIOD, "ipi_period"),
+	FIELD(u8, IPI_ANTENNA_INDEX, "ipi_antenna_idx"),
+	FIELD(u8, IPI_RESET, "ipi_reset"),
 	FIELD_MAC(MAC_ADDRS, "mac_addrs"),
 	FIELD_NESTED_RO(STATS, stats, "",
 			.print_extra = print_extra_stats),
@@ -446,6 +461,13 @@ static struct nla_policy testdata_policy[NUM_MT76_TM_ATTRS] = {
 	[MT76_TM_ATTR_RU_IDX] = { .type = NLA_U8 },
 	[MT76_TM_ATTR_STATS] = { .type = NLA_NESTED },
 	[MT76_TM_ATTR_TXBF_ACT] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_OFF_CH_SCAN_CH] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_OFF_CH_SCAN_CENTER_CH] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_OFF_CH_SCAN_BW] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_IPI_THRESHOLD] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_IPI_PERIOD] = { .type = NLA_U32 },
+	[MT76_TM_ATTR_IPI_ANTENNA_INDEX] = { .type = NLA_U8 },
+	[MT76_TM_ATTR_IPI_RESET] = { .type = NLA_U8 },
 };
 
 const struct tm_field msg_field = {
-- 
2.18.0