From 623e57c672ee85f8a4a9455888237d09df405962 Mon Sep 17 00:00:00 2001
From: Shayne Chen <shayne.chen@mediatek.com>
Date: Mon, 28 Jun 2021 10:46:39 +0800
Subject: [PATCH 1105/1112] mt76: mt7915: add off channel scan support in
 testmode

Signed-off-by: Shayne Chen <shayne.chen@mediatek.com>
---
 .../wireless/mediatek/mt76/mt7915/testmode.c  | 72 +++++++++++++++++++
 .../wireless/mediatek/mt76/mt7915/testmode.h  | 10 +++
 2 files changed, 82 insertions(+)

diff --git a/mt7915/testmode.c b/mt7915/testmode.c
index 98431d6..08bb700 100644
--- a/mt7915/testmode.c
+++ b/mt7915/testmode.c
@@ -10,6 +10,7 @@ enum {
 	TM_CHANGED_TXPOWER,
 	TM_CHANGED_FREQ_OFFSET,
 	TM_CHANGED_CFG,
+	TM_CHANGED_OFF_CH_SCAN_CH,
 
 	/* must be last */
 	NUM_TM_CHANGED
@@ -19,6 +20,7 @@ static const u8 tm_change_map[] = {
 	[TM_CHANGED_TXPOWER] = MT76_TM_ATTR_TX_POWER,
 	[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,
 };
 
 struct reg_band {
@@ -36,6 +38,25 @@ struct reg_band {
 static struct reg_band reg_backup_list[TM_REG_MAX_ID];
 
 
+static u8 mt7915_tm_chan_bw(enum nl80211_chan_width width)
+{
+	static const u8 width_to_bw[] = {
+		[NL80211_CHAN_WIDTH_40] = TM_CBW_40MHZ,
+		[NL80211_CHAN_WIDTH_80] = TM_CBW_80MHZ,
+		[NL80211_CHAN_WIDTH_80P80] = TM_CBW_8080MHZ,
+		[NL80211_CHAN_WIDTH_160] = TM_CBW_160MHZ,
+		[NL80211_CHAN_WIDTH_5] = TM_CBW_5MHZ,
+		[NL80211_CHAN_WIDTH_10] = TM_CBW_10MHZ,
+		[NL80211_CHAN_WIDTH_20] = TM_CBW_20MHZ,
+		[NL80211_CHAN_WIDTH_20_NOHT] = TM_CBW_20MHZ,
+	};
+
+	if (width >= ARRAY_SIZE(width_to_bw))
+		return 0;
+
+	return width_to_bw[width];
+}
+
 static int
 mt7915_tm_set_tx_power(struct mt7915_phy *phy)
 {
@@ -206,6 +227,55 @@ mt7915_tm_set_cfg(struct mt7915_phy *phy)
 				 sizeof(req), false);
 }
 
+static int
+mt7915_tm_set_off_channel_scan(struct mt7915_phy *phy)
+{
+#define OFF_CH_SCAN_SIMPLE_RX	2
+	struct mt76_testmode_data *td = &phy->mt76->test;
+	struct mt7915_dev *dev = phy->dev;
+	struct cfg80211_chan_def *chandef = &phy->mt76->chandef;
+	int freq1 = chandef->center_freq1;
+	struct {
+		u8 cur_pri_ch;
+		u8 cur_center_ch;
+		u8 cur_bw;
+		u8 cur_tx_path;
+		u8 cur_rx_path;
+
+		u8 scan_pri_ch;
+		u8 scan_center_ch;
+		u8 scan_bw;
+		u8 scan_tx_path;
+		u8 scan_rx_path;
+
+		u8 enable;
+		u8 band_idx;
+		u8 type;
+		u8 is_5g;
+		u8 _rsv[2];
+	} __packed req = {
+		.cur_pri_ch = chandef->chan->hw_value,
+		.cur_center_ch = ieee80211_frequency_to_channel(freq1),
+		.cur_bw = mt7915_tm_chan_bw(chandef->width),
+		.cur_tx_path = td->tx_antenna_mask,
+		.cur_rx_path = td->tx_antenna_mask,
+
+		.scan_pri_ch = td->off_ch_scan_ch,
+		.scan_center_ch = td->off_ch_scan_center_ch,
+		.scan_bw = td->off_ch_scan_bw,
+		.scan_tx_path = td->off_ch_scan_path,
+		.scan_rx_path = td->off_ch_scan_path,
+
+		.enable = !!td->off_ch_scan_ch,
+		.band_idx = phy != &dev->phy,
+		.type = OFF_CH_SCAN_SIMPLE_RX,
+		.is_5g = td->off_ch_scan_ch > 14 ? 1 : 0,
+	};
+
+	return mt76_mcu_send_msg(&dev->mt76, MCU_EXT_CMD(OFFCH_SCAN_CTRL), &req,
+				 sizeof(req), false);
+}
+
 static int
 mt7915_tm_set_wmm_qid(struct mt7915_dev *dev, u8 qid, u8 aifs, u8 cw_min,
 		      u16 cw_max, u16 txop)
@@ -753,6 +823,8 @@ mt7915_tm_update_params(struct mt7915_phy *phy, u32 changed)
 		mt7915_tm_set_tx_power(phy);
 	if (changed & BIT(TM_CHANGED_CFG))
 		mt7915_tm_set_cfg(phy);
+	if (changed & BIT(TM_CHANGED_OFF_CH_SCAN_CH))
+		mt7915_tm_set_off_channel_scan(phy);
 }
 
 static int
diff --git a/mt7915/testmode.h b/mt7915/testmode.h
index a1c54c8..d22aabe 100644
--- a/mt7915/testmode.h
+++ b/mt7915/testmode.h
@@ -130,4 +130,14 @@ struct mt7915_tm_rx_stat_band {
 	__le16 mdrdy_cnt_ofdm;
 };
 
+enum {
+	TM_CBW_20MHZ,
+	TM_CBW_40MHZ,
+	TM_CBW_80MHZ,
+	TM_CBW_10MHZ,
+	TM_CBW_5MHZ,
+	TM_CBW_160MHZ,
+	TM_CBW_8080MHZ,
+};
+
 #endif
-- 
2.25.1