[PATCH 4/4] wifi: rtw89: 8922a: add helper of set_channel

[Ping-Ke Shih <pkshih@xxxxxxxxxxx>]

Reset hardware state to prevent hardware stays at abnormal state during
setting channel. Besides, add preparation for MLO/DBCC before setting
channel, and reconfigure registers after that.

Signed-off-by: Ping-Ke Shih <pkshih@xxxxxxxxxxx>
---
 drivers/net/wireless/realtek/rtw89/rtw8922a.c | 51 +++++++++++++++++++
 .../net/wireless/realtek/rtw89/rtw8922a_rfk.c | 23 +++++++++
 .../net/wireless/realtek/rtw89/rtw8922a_rfk.h |  2 +
 3 files changed, 76 insertions(+)

diff --git a/drivers/net/wireless/realtek/rtw89/rtw8922a.c b/drivers/net/wireless/realtek/rtw89/rtw8922a.c
index dbbf6f344f3a..247f95d1d59a 100644
--- a/drivers/net/wireless/realtek/rtw89/rtw8922a.c
+++ b/drivers/net/wireless/realtek/rtw89/rtw8922a.c
@@ -1774,6 +1774,37 @@ static void rtw8922a_set_channel_bb(struct rtw89_dev *rtwdev,
 	rtw8922a_tssi_reset(rtwdev, RF_PATH_AB, phy_idx);
 }
 
+static void rtw8922a_pre_set_channel_bb(struct rtw89_dev *rtwdev,
+					enum rtw89_phy_idx phy_idx)
+{
+	if (!rtwdev->dbcc_en)
+		return;
+
+	if (phy_idx == RTW89_PHY_0) {
+		rtw89_phy_write32_mask(rtwdev, R_DBCC, B_DBCC_EN, 0x0);
+		rtw89_phy_write32_mask(rtwdev, R_EMLSR, B_EMLSR_PARM, 0x6180);
+		rtw89_phy_write32_mask(rtwdev, R_EMLSR, B_EMLSR_PARM, 0xBBAB);
+		rtw89_phy_write32_mask(rtwdev, R_EMLSR, B_EMLSR_PARM, 0xABA9);
+		rtw89_phy_write32_mask(rtwdev, R_EMLSR, B_EMLSR_PARM, 0xEBA9);
+		rtw89_phy_write32_mask(rtwdev, R_EMLSR, B_EMLSR_PARM, 0xEAA9);
+	} else {
+		rtw89_phy_write32_mask(rtwdev, R_DBCC, B_DBCC_EN, 0x0);
+		rtw89_phy_write32_mask(rtwdev, R_EMLSR, B_EMLSR_PARM, 0xBBAB);
+		rtw89_phy_write32_mask(rtwdev, R_EMLSR, B_EMLSR_PARM, 0xAFFF);
+		rtw89_phy_write32_mask(rtwdev, R_EMLSR, B_EMLSR_PARM, 0xEFFF);
+		rtw89_phy_write32_mask(rtwdev, R_EMLSR, B_EMLSR_PARM, 0xEEFF);
+	}
+}
+
+static void rtw8922a_post_set_channel_bb(struct rtw89_dev *rtwdev,
+					 enum rtw89_mlo_dbcc_mode mode)
+{
+	if (!rtwdev->dbcc_en)
+		return;
+
+	rtw8922a_ctrl_mlo(rtwdev, mode);
+}
+
 static void rtw8922a_set_channel(struct rtw89_dev *rtwdev,
 				 const struct rtw89_chan *chan,
 				 enum rtw89_mac_idx mac_idx,
@@ -1863,6 +1894,25 @@ void rtw8922a_hal_reset(struct rtw89_dev *rtwdev,
 	}
 }
 
+static void rtw8922a_set_channel_help(struct rtw89_dev *rtwdev, bool enter,
+				      struct rtw89_channel_help_params *p,
+				      const struct rtw89_chan *chan,
+				      enum rtw89_mac_idx mac_idx,
+				      enum rtw89_phy_idx phy_idx)
+{
+	if (enter) {
+		rtw8922a_pre_set_channel_bb(rtwdev, phy_idx);
+		rtw8922a_pre_set_channel_rf(rtwdev, phy_idx);
+	}
+
+	rtw8922a_hal_reset(rtwdev, phy_idx, mac_idx, chan->band_type, &p->tx_en, enter);
+
+	if (!enter) {
+		rtw8922a_post_set_channel_bb(rtwdev, rtwdev->mlo_dbcc_mode);
+		rtw8922a_post_set_channel_rf(rtwdev, phy_idx);
+	}
+}
+
 static void rtw8922a_rfk_init(struct rtw89_dev *rtwdev)
 {
 	struct rtw89_rfk_mcc_info *rfk_mcc = &rtwdev->rfk_mcc;
@@ -2169,6 +2219,7 @@ static const struct rtw89_chip_ops rtw8922a_chip_ops = {
 	.read_rf		= rtw89_phy_read_rf_v2,
 	.write_rf		= rtw89_phy_write_rf_v2,
 	.set_channel		= rtw8922a_set_channel,
+	.set_channel_help	= rtw8922a_set_channel_help,
 	.read_efuse		= rtw8922a_read_efuse,
 	.read_phycap		= rtw8922a_read_phycap,
 	.fem_setup		= NULL,
diff --git a/drivers/net/wireless/realtek/rtw89/rtw8922a_rfk.c b/drivers/net/wireless/realtek/rtw89/rtw8922a_rfk.c
index 72953b7358df..2a371829268c 100644
--- a/drivers/net/wireless/realtek/rtw89/rtw8922a_rfk.c
+++ b/drivers/net/wireless/realtek/rtw89/rtw8922a_rfk.c
@@ -353,3 +353,26 @@ void rtw8922a_rfk_hw_init(struct rtw89_dev *rtwdev)
 
 	rtw8922a_rfk_pll_init(rtwdev);
 }
+
+void rtw8922a_pre_set_channel_rf(struct rtw89_dev *rtwdev, enum rtw89_phy_idx phy_idx)
+{
+	bool mlo_1_1;
+
+	if (!rtwdev->dbcc_en)
+		return;
+
+	mlo_1_1 = rtw89_is_mlo_1_1(rtwdev);
+	if (mlo_1_1)
+		rtw8922a_set_syn01(rtwdev, RF_SYN_ALLON);
+	else if (phy_idx == RTW89_PHY_0)
+		rtw8922a_set_syn01(rtwdev, RF_SYN_ON_OFF);
+	else
+		rtw8922a_set_syn01(rtwdev, RF_SYN_OFF_ON);
+
+	fsleep(1000);
+}
+
+void rtw8922a_post_set_channel_rf(struct rtw89_dev *rtwdev, enum rtw89_phy_idx phy_idx)
+{
+	rtw8922a_rfk_mlo_ctrl(rtwdev);
+}
diff --git a/drivers/net/wireless/realtek/rtw89/rtw8922a_rfk.h b/drivers/net/wireless/realtek/rtw89/rtw8922a_rfk.h
index 27a2ff8166d0..66bdd57c1eea 100644
--- a/drivers/net/wireless/realtek/rtw89/rtw8922a_rfk.h
+++ b/drivers/net/wireless/realtek/rtw89/rtw8922a_rfk.h
@@ -12,5 +12,7 @@ void rtw8922a_set_channel_rf(struct rtw89_dev *rtwdev,
 			     const struct rtw89_chan *chan,
 			     enum rtw89_phy_idx phy_idx);
 void rtw8922a_rfk_hw_init(struct rtw89_dev *rtwdev);
+void rtw8922a_pre_set_channel_rf(struct rtw89_dev *rtwdev, enum rtw89_phy_idx phy_idx);
+void rtw8922a_post_set_channel_rf(struct rtw89_dev *rtwdev, enum rtw89_phy_idx phy_idx);
 
 #endif
-- 
2.25.1