[PATCH rtw-next 4/9] wifi: rtw89: 8922d: add set TX power callback

[Ping-Ke Shih <pkshih@realtek.com>]

Set TX power depends on operating channel. The Tx power factors are data
rate, channel, bandwidth and etc. Also, consider SAR as a factor of TX
power limit.

Signed-off-by: Ping-Ke Shih <pkshih@realtek.com>
---
 drivers/net/wireless/realtek/rtw89/reg.h      |  13 ++
 drivers/net/wireless/realtek/rtw89/rtw8922d.c | 115 ++++++++++++++++++
 2 files changed, 128 insertions(+)

diff --git a/drivers/net/wireless/realtek/rtw89/reg.h b/drivers/net/wireless/realtek/rtw89/reg.h
index 1a5a5b30a28e..37de1c827814 100644
--- a/drivers/net/wireless/realtek/rtw89/reg.h
+++ b/drivers/net/wireless/realtek/rtw89/reg.h
@@ -10235,6 +10235,8 @@
 #define B_TSSI_CONT_EN BIT(3)
 #define R_P0_TXPWRB_BE 0xE61C
 #define R_P1_TXPWRB_BE 0xE71C
+#define R_P0_TXPWRB_BE4 0x2251C
+#define R_P1_TXPWRB_BE4 0x2261C
 #define B_TXPWRB_MAX_BE GENMASK(20, 12)
 #define R_TSSI_MAP_OFST_P0 0xE620
 #define R_TSSI_MAP_OFST_P1 0xE720
@@ -10531,13 +10533,24 @@
 #define B_TXPWR_RSTB0_BE4 BIT(16)
 #define R_TSSI_EN_P0_BE4 0x22510
 #define B_TSSI_EN_P0_BE4 GENMASK(3, 0)
+#define R_TXAGC_REF_DBM_PATH0_TBL0_BE4 0x22528
+#define B_TXAGC_OFDM_REF_DBM_PATH0_TBL0_BE4 GENMASK(8, 0)
+#define B_TXAGC_CCK_REF_DBM_PATH0_TBL0_BE4 GENMASK(17, 9)
 #define R_USED_TSSI_TRK_ON_P0_BE4 0x22534
 #define B_USED_TSSI_TRK_ON_P0_BE4 BIT(22)
+#define R_TSSI_K_OFDM_PATH0_TBL0_BE4 0x225A0
+#define B_TSSI_K_OFDM_PATH0_TBL0_BE4 GENMASK(29, 20)
 #define R_TSSI_DCK_MOV_AVG_LEN_P0_BE4 0x225CC
 #define B_TSSI_DCK_MOV_AVG_LEN_P0_BE4 GENMASK(8, 6)
 #define R_TXPWR_RSTB1_BE4 0x2260C
 #define B_TXPWR_RSTB1_BE4 BIT(16)
 
+#define R_TXAGC_REF_DBM_PATH0_TBL1_BE4 0x23528
+#define B_TXAGC_OFDM_REF_DBM_PATH0_TBL1_BE4 GENMASK(8, 0)
+#define B_TXAGC_CCK_REF_DBM_PATH0_TBL1_BE4 GENMASK(17, 9)
+#define R_TSSI_K_OFDM_PATH0_TBL1_BE4 0x235A0
+#define B_TSSI_K_OFDM_PATH0_TBL1_BE4 GENMASK(29, 20)
+
 #define R_OFDM_OFST_P0_BE4 0x240C8
 #define B_OFDM_OFST_P0_BE4 GENMASK(31, 24)
 #define R_PATH0_RXIDX_INIT_BE4 0x24108
diff --git a/drivers/net/wireless/realtek/rtw89/rtw8922d.c b/drivers/net/wireless/realtek/rtw89/rtw8922d.c
index 2e6f4504caeb..9c62a5f12962 100644
--- a/drivers/net/wireless/realtek/rtw89/rtw8922d.c
+++ b/drivers/net/wireless/realtek/rtw89/rtw8922d.c
@@ -11,6 +11,7 @@
 #include "reg.h"
 #include "rtw8922d.h"
 #include "rtw8922d_rfk.h"
+#include "sar.h"
 #include "util.h"
 
 #define RTW8922D_FW_FORMAT_MAX 0
@@ -2322,6 +2323,120 @@ static void rtw8922d_rfk_track(struct rtw89_dev *rtwdev)
 	rtw8922d_lck_track(rtwdev);
 }
 
+static const struct rtw89_reg_def rtw8922d_txpwr_ref[][3] = {
+	{{ .addr = R_TXAGC_REF_DBM_PATH0_TBL0_BE4,
+	   .mask = B_TXAGC_OFDM_REF_DBM_PATH0_TBL0_BE4 },
+	 { .addr = R_TXAGC_REF_DBM_PATH0_TBL0_BE4,
+	   .mask = B_TXAGC_CCK_REF_DBM_PATH0_TBL0_BE4 },
+	 { .addr = R_TSSI_K_OFDM_PATH0_TBL0_BE4,
+	   .mask = B_TSSI_K_OFDM_PATH0_TBL0_BE4 }
+	},
+	{{ .addr = R_TXAGC_REF_DBM_PATH0_TBL1_BE4,
+	   .mask = B_TXAGC_OFDM_REF_DBM_PATH0_TBL1_BE4 },
+	 { .addr = R_TXAGC_REF_DBM_PATH0_TBL1_BE4,
+	   .mask = B_TXAGC_CCK_REF_DBM_PATH0_TBL1_BE4 },
+	 { .addr = R_TSSI_K_OFDM_PATH0_TBL1_BE4,
+	   .mask = B_TSSI_K_OFDM_PATH0_TBL1_BE4 }
+	},
+};
+
+static void rtw8922d_set_txpwr_diff(struct rtw89_dev *rtwdev,
+				    const struct rtw89_chan *chan,
+				    enum rtw89_phy_idx phy_idx)
+{
+	s16 pwr_ofst = rtw89_phy_ant_gain_pwr_offset(rtwdev, chan);
+	const struct rtw89_chip_info *chip = rtwdev->chip;
+	static const u32 path_ofst[] = {0x0, 0x100};
+	const struct rtw89_reg_def *txpwr_ref;
+	s16 tssi_k_ofst = abs(pwr_ofst);
+	s16 ofst_dec[RF_PATH_NUM_8922D];
+	s16 tssi_k[RF_PATH_NUM_8922D];
+	s16 pwr_ref_ofst;
+	s16 pwr_ref = 16;
+	u8 i;
+
+	pwr_ref <<= chip->txpwr_factor_rf;
+	pwr_ref_ofst = pwr_ref - rtw89_phy_txpwr_bb_to_rf(rtwdev, abs(pwr_ofst));
+
+	ofst_dec[RF_PATH_A] = pwr_ofst > 0 ? pwr_ref : pwr_ref_ofst;
+	ofst_dec[RF_PATH_B] = pwr_ofst > 0 ? pwr_ref_ofst : pwr_ref;
+	tssi_k[RF_PATH_A] = pwr_ofst > 0 ? 0 : tssi_k_ofst;
+	tssi_k[RF_PATH_B] = pwr_ofst > 0 ? tssi_k_ofst : 0;
+
+	for (i = 0; i < RF_PATH_NUM_8922D; i++) {
+		txpwr_ref = rtw8922d_txpwr_ref[phy_idx];
+
+		rtw89_phy_write32_mask(rtwdev, txpwr_ref[0].addr + path_ofst[i],
+				       txpwr_ref[0].mask, ofst_dec[i]);
+		rtw89_phy_write32_mask(rtwdev, txpwr_ref[1].addr + path_ofst[i],
+				       txpwr_ref[1].mask, ofst_dec[i]);
+		rtw89_phy_write32_mask(rtwdev, txpwr_ref[2].addr + path_ofst[i],
+				       txpwr_ref[2].mask, tssi_k[i]);
+	}
+}
+
+static void rtw8922d_set_txpwr_ref(struct rtw89_dev *rtwdev,
+				   const struct rtw89_chan *chan,
+				   enum rtw89_phy_idx phy_idx)
+{
+	s16 ref_ofdm = 0;
+	s16 ref_cck = 0;
+
+	rtw89_debug(rtwdev, RTW89_DBG_TXPWR, "[TXPWR] set txpwr reference\n");
+
+	rtw8922d_set_txpwr_diff(rtwdev, chan, phy_idx);
+
+	rtw89_mac_txpwr_write32_mask(rtwdev, phy_idx, R_BE_PWR_REF_CTRL,
+				     B_BE_PWR_REF_CTRL_OFDM, ref_ofdm);
+	rtw89_mac_txpwr_write32_mask(rtwdev, phy_idx, R_BE_PWR_REF_CTRL,
+				     B_BE_PWR_REF_CTRL_CCK, ref_cck);
+}
+
+static void rtw8922d_set_txpwr_sar_diff(struct rtw89_dev *rtwdev,
+					const struct rtw89_chan *chan,
+					enum rtw89_phy_idx phy_idx)
+{
+	struct rtw89_sar_parm sar_parm = {
+		.center_freq = chan->freq,
+		.force_path = true,
+	};
+	s16 sar_rf;
+	s8 sar_mac;
+
+	if (phy_idx != RTW89_PHY_0)
+		return;
+
+	sar_parm.path = RF_PATH_A;
+	sar_mac = rtw89_query_sar(rtwdev, &sar_parm);
+	sar_rf = rtw89_phy_txpwr_mac_to_rf(rtwdev, sar_mac);
+	rtw89_phy_write32_mask(rtwdev, R_P0_TXPWRB_BE4, B_TXPWRB_MAX_BE, sar_rf);
+
+	sar_parm.path = RF_PATH_B;
+	sar_mac = rtw89_query_sar(rtwdev, &sar_parm);
+	sar_rf = rtw89_phy_txpwr_mac_to_rf(rtwdev, sar_mac);
+	rtw89_phy_write32_mask(rtwdev, R_P1_TXPWRB_BE4, B_TXPWRB_MAX_BE, sar_rf);
+}
+
+static void rtw8922d_set_txpwr(struct rtw89_dev *rtwdev,
+			       const struct rtw89_chan *chan,
+			       enum rtw89_phy_idx phy_idx)
+{
+	rtw89_phy_set_txpwr_byrate(rtwdev, chan, phy_idx);
+	rtw89_phy_set_txpwr_offset(rtwdev, chan, phy_idx);
+	rtw89_phy_set_txpwr_limit(rtwdev, chan, phy_idx);
+	rtw89_phy_set_txpwr_limit_ru(rtwdev, chan, phy_idx);
+	rtw8922d_set_txpwr_ref(rtwdev, chan, phy_idx);
+	rtw8922d_set_txpwr_sar_diff(rtwdev, chan, phy_idx);
+}
+
+static void rtw8922d_set_txpwr_ctrl(struct rtw89_dev *rtwdev,
+				    enum rtw89_phy_idx phy_idx)
+{
+	const struct rtw89_chan *chan = rtw89_mgnt_chan_get(rtwdev, phy_idx);
+
+	rtw8922d_set_txpwr_ref(rtwdev, chan, phy_idx);
+}
+
 MODULE_FIRMWARE(RTW8922D_MODULE_FIRMWARE);
 MODULE_FIRMWARE(RTW8922DS_MODULE_FIRMWARE);
 MODULE_AUTHOR("Realtek Corporation");
-- 
2.25.1