[PATCH rtw-next 05/16] wifi: rtw89: phy: support static PD level setting

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

From: Eric Huang <echuang@realtek.com>

PD (Packet Detection) threshold is a key parameter in the DIG
(Dynamic Initial Gain) algorithm that determines the sensitivity
of packet detection. Current implementation only supports dynamic
PD adjustment based on environment. This patch adds support for
static PD threshold via debugfs allowing users to set a fixed PD
value for testing or specific scenarios.

Signed-off-by: Eric Huang <echuang@realtek.com>
Signed-off-by: Ping-Ke Shih <pkshih@realtek.com>
---
 drivers/net/wireless/realtek/rtw89/core.h  |  4 +
 drivers/net/wireless/realtek/rtw89/debug.c | 94 ++++++++++++++++++++++
 drivers/net/wireless/realtek/rtw89/phy.c   | 44 +++++++---
 3 files changed, 131 insertions(+), 11 deletions(-)

diff --git a/drivers/net/wireless/realtek/rtw89/core.h b/drivers/net/wireless/realtek/rtw89/core.h
index 4e920472c8aa..8618a7c30ee0 100644
--- a/drivers/net/wireless/realtek/rtw89/core.h
+++ b/drivers/net/wireless/realtek/rtw89/core.h
@@ -5180,6 +5180,7 @@ enum rtw89_dm_type {
 	RTW89_DM_MLO,
 	RTW89_DM_HW_SCAN,
 	RTW89_DM_INACTIVE_PS,
+	RTW89_DM_DIG_PD,
 };
 
 #define RTW89_THERMAL_PROT_LV_MAX 5
@@ -5221,6 +5222,9 @@ struct rtw89_hal {
 
 	u8 thermal_prot_th;
 	u8 thermal_prot_lv; /* 0 ~ RTW89_THERMAL_PROT_LV_MAX */
+
+	u8 fixed_dig_pd_th; /* v = (X(dBm) + 102)/2 */
+	s8 fixed_dig_cck_pd_th; /* dBm */
 };
 
 #define RTW89_MAX_MAC_ID_NUM 128
diff --git a/drivers/net/wireless/realtek/rtw89/debug.c b/drivers/net/wireless/realtek/rtw89/debug.c
index ebe3b1f9c39d..f7672d7e0229 100644
--- a/drivers/net/wireless/realtek/rtw89/debug.c
+++ b/drivers/net/wireless/realtek/rtw89/debug.c
@@ -87,6 +87,7 @@ struct rtw89_debugfs {
 	struct rtw89_debugfs_priv phy_info;
 	struct rtw89_debugfs_priv stations;
 	struct rtw89_debugfs_priv disable_dm;
+	struct rtw89_debugfs_priv static_pd_th;
 	struct rtw89_debugfs_priv mlo_mode;
 	struct rtw89_debugfs_priv beacon_info;
 	struct rtw89_debugfs_priv diag_mac;
@@ -4349,6 +4350,7 @@ static const struct rtw89_disabled_dm_info {
 	DM_INFO(MLO),
 	DM_INFO(HW_SCAN),
 	DM_INFO(INACTIVE_PS),
+	DM_INFO(DIG_PD),
 };
 
 static ssize_t
@@ -4394,6 +4396,96 @@ rtw89_debug_priv_disable_dm_set(struct rtw89_dev *rtwdev,
 	return count;
 }
 
+#define RTW89_DIG_PD_TH_MIN_DBM		-102
+#define RTW89_DIG_PD_TH_MAX_DBM		-40
+#define RTW89_DIG_PD_TH_STEP		2
+
+static inline s8 rtw89_dig_pd_th_to_dbm(u8 reg_val)
+{
+	return RTW89_DIG_PD_TH_MIN_DBM + RTW89_DIG_PD_TH_STEP * reg_val;
+}
+
+static inline u8 rtw89_dig_pd_th_dbm_to_reg(s8 dbm)
+{
+	return (dbm - RTW89_DIG_PD_TH_MIN_DBM) / RTW89_DIG_PD_TH_STEP;
+}
+
+static ssize_t
+rtw89_debug_priv_static_pd_th_get(struct rtw89_dev *rtwdev,
+				  struct rtw89_debugfs_priv *debugfs_priv,
+				  char *buf, size_t bufsz)
+{
+	struct rtw89_hal *hal = &rtwdev->hal;
+	char *p = buf, *end = buf + bufsz;
+	bool disabled;
+	s8 pd_th_dbm;
+	s8 cck_pd_th;
+
+	disabled = hal->disabled_dm_bitmap & BIT(RTW89_DM_DIG_PD);
+
+	if (disabled) {
+		pd_th_dbm = rtw89_dig_pd_th_to_dbm(hal->fixed_dig_pd_th);
+		cck_pd_th = hal->fixed_dig_cck_pd_th;
+
+		p += scnprintf(p, end - p, "DIG: static\n");
+		p += scnprintf(p, end - p, "OFDM PD threshold: %d dBm\n", pd_th_dbm);
+		p += scnprintf(p, end - p, "CCK PD threshold: %d dBm\n", cck_pd_th);
+	} else {
+		p += scnprintf(p, end - p, "DIG: dynamic\n");
+	}
+
+	p += scnprintf(p, end - p, "\nUsage: echo <mode> [pd_th] > static_pd_th\n");
+	p += scnprintf(p, end - p, "  mode: 0 = dynamic, 1 = static\n");
+	p += scnprintf(p, end - p, "  pd_th: PD threshold in dBm (-102 ~ -40)\n");
+
+	return p - buf;
+}
+
+static ssize_t
+rtw89_debug_priv_static_pd_th_set(struct rtw89_dev *rtwdev,
+				  struct rtw89_debugfs_priv *debugfs_priv,
+				  const char *buf, size_t count)
+{
+	struct rtw89_hal *hal = &rtwdev->hal;
+	int ret;
+	u32 mode;
+	s32 pd_th_dbm;
+
+	ret = sscanf(buf, "%u %d", &mode, &pd_th_dbm);
+	if (ret < 1)
+		return -EINVAL;
+
+	if (mode > 1)
+		return -EINVAL;
+
+	if (mode == 0) {
+		rtw89_core_dm_disable_clr(rtwdev, RTW89_DM_DIG_PD);
+		hal->fixed_dig_pd_th = 0;
+		hal->fixed_dig_cck_pd_th = 0;
+
+		rtw89_debug(rtwdev, RTW89_DBG_DIG,
+			    "DIG static mode disabled\n");
+	} else {
+		if (ret < 2 || pd_th_dbm < RTW89_DIG_PD_TH_MIN_DBM ||
+		    pd_th_dbm > RTW89_DIG_PD_TH_MAX_DBM)
+			return -EINVAL;
+
+		rtw89_core_dm_disable_set(rtwdev, RTW89_DM_DIG_PD);
+		hal->fixed_dig_pd_th = clamp(rtw89_dig_pd_th_dbm_to_reg(pd_th_dbm),
+					     0, 0x1f);
+		/* CCK uses dBm directly */
+		hal->fixed_dig_cck_pd_th = pd_th_dbm;
+
+		rtw89_debug(rtwdev, RTW89_DBG_DIG,
+			    "DIG static mode: PD=0x%02x (%d dBm), CCK=%d dBm\n",
+			    hal->fixed_dig_pd_th,
+			    rtw89_dig_pd_th_to_dbm(hal->fixed_dig_pd_th),
+			    hal->fixed_dig_cck_pd_th);
+	}
+
+	return count;
+}
+
 static void rtw89_debug_mlo_mode_set_mlsr(struct rtw89_dev *rtwdev,
 					  unsigned int link_id)
 {
@@ -4882,6 +4974,7 @@ static const struct rtw89_debugfs rtw89_debugfs_templ = {
 	.phy_info = rtw89_debug_priv_get(phy_info),
 	.stations = rtw89_debug_priv_get(stations, RLOCK),
 	.disable_dm = rtw89_debug_priv_set_and_get(disable_dm, RWLOCK),
+	.static_pd_th = rtw89_debug_priv_set_and_get(static_pd_th, RWLOCK),
 	.mlo_mode = rtw89_debug_priv_set_and_get(mlo_mode, RWLOCK),
 	.beacon_info = rtw89_debug_priv_get(beacon_info),
 	.diag_mac = rtw89_debug_priv_get(diag_mac, RSIZE_16K, RLOCK),
@@ -4930,6 +5023,7 @@ void rtw89_debugfs_add_sec1(struct rtw89_dev *rtwdev, struct dentry *debugfs_top
 	rtw89_debugfs_add_r(phy_info);
 	rtw89_debugfs_add_r(stations);
 	rtw89_debugfs_add_rw(disable_dm);
+	rtw89_debugfs_add_rw(static_pd_th);
 	rtw89_debugfs_add_rw(mlo_mode);
 	rtw89_debugfs_add_r(beacon_info);
 	rtw89_debugfs_add_r(diag_mac);
diff --git a/drivers/net/wireless/realtek/rtw89/phy.c b/drivers/net/wireless/realtek/rtw89/phy.c
index e70d0e283987..f26cbde6bdd0 100644
--- a/drivers/net/wireless/realtek/rtw89/phy.c
+++ b/drivers/net/wireless/realtek/rtw89/phy.c
@@ -7162,11 +7162,19 @@ static void rtw89_phy_dig_dyn_pd_th(struct rtw89_dev *rtwdev,
 	const struct rtw89_chan *chan = rtw89_mgnt_chan_get(rtwdev, bb->phy_idx);
 	const struct rtw89_dig_regs *dig_regs = rtwdev->chip->dig_regs;
 	struct rtw89_dig_info *dig = &bb->dig;
+	struct rtw89_hal *hal = &rtwdev->hal;
 	u8 final_rssi, under_region = dig->pd_low_th_ofst;
 	s8 cck_cca_th;
 	u32 pd_val;
 
-	pd_val = __rtw89_phy_dig_dyn_pd_th(rtwdev, bb, rssi, enable, chan);
+	if (hal->disabled_dm_bitmap & BIT(RTW89_DM_DIG_PD)) {
+		pd_val = hal->fixed_dig_pd_th;
+
+		rtw89_debug(rtwdev, RTW89_DBG_DIG,
+			    "Fixed DIG: PD_low=%d\n", pd_val);
+	} else {
+		pd_val = __rtw89_phy_dig_dyn_pd_th(rtwdev, bb, rssi, enable, chan);
+	}
 	dig->bak_dig = pd_val;
 
 	rtw89_phy_write32_idx(rtwdev, dig_regs->seg0_pd_reg,
@@ -7174,17 +7182,24 @@ static void rtw89_phy_dig_dyn_pd_th(struct rtw89_dev *rtwdev,
 	rtw89_phy_write32_idx(rtwdev, dig_regs->seg0_pd_reg,
 			      dig_regs->pd_spatial_reuse_en, enable, bb->phy_idx);
 
-	if (!rtwdev->hal.support_cckpd)
+	if (!hal->support_cckpd)
 		return;
 
-	final_rssi = min_t(u8, rssi, dig->igi_rssi);
-	under_region = rtw89_phy_dig_cal_under_region(rtwdev, bb, chan);
-	cck_cca_th = max_t(s8, final_rssi - under_region, CCKPD_TH_MIN_RSSI);
-	pd_val = (u32)(cck_cca_th - IGI_RSSI_MAX);
+	if (hal->disabled_dm_bitmap & BIT(RTW89_DM_DIG_PD)) {
+		pd_val = hal->fixed_dig_cck_pd_th;
 
-	rtw89_debug(rtwdev, RTW89_DBG_DIG,
-		    "igi=%d, cck_ccaTH=%d, backoff=%d, cck_PD_low=((%d))dB\n",
-		    final_rssi, cck_cca_th, under_region, pd_val);
+		rtw89_debug(rtwdev, RTW89_DBG_DIG,
+			    "Fixed DIG: cck_PD_low=((%d))dB\n", pd_val);
+	} else {
+		final_rssi = min_t(u8, rssi, dig->igi_rssi);
+		under_region = rtw89_phy_dig_cal_under_region(rtwdev, bb, chan);
+		cck_cca_th = max_t(s8, final_rssi - under_region, CCKPD_TH_MIN_RSSI);
+		pd_val = (u32)(cck_cca_th - IGI_RSSI_MAX);
+
+		rtw89_debug(rtwdev, RTW89_DBG_DIG,
+			    "igi=%d, cck_ccaTH=%d, backoff=%d, cck_PD_low=((%d))dB\n",
+			    final_rssi, cck_cca_th, under_region, pd_val);
+	}
 
 	rtw89_phy_write32_idx(rtwdev, dig_regs->bmode_pd_reg,
 			      dig_regs->bmode_cca_rssi_limit_en, enable, bb->phy_idx);
@@ -7315,6 +7330,7 @@ static void rtw89_phy_dig_ctrl(struct rtw89_dev *rtwdev, struct rtw89_bb_ctx *bb
 {
 	const struct rtw89_dig_regs *dig_regs = rtwdev->chip->dig_regs;
 	struct rtw89_dig_info *dig = &bb->dig;
+	struct rtw89_hal *hal = &rtwdev->hal;
 	bool en_dig;
 	u32 pd_val;
 
@@ -7323,10 +7339,16 @@ static void rtw89_phy_dig_ctrl(struct rtw89_dev *rtwdev, struct rtw89_bb_ctx *bb
 
 	if (pause_dig) {
 		en_dig = false;
-		pd_val = 0;
+		if (hal->disabled_dm_bitmap & BIT(RTW89_DM_DIG_PD))
+			pd_val = hal->fixed_dig_pd_th;
+		else
+			pd_val = 0;
 	} else {
 		en_dig = rtwdev->total_sta_assoc > 0;
-		pd_val = restore ? dig->bak_dig : 0;
+		if (hal->disabled_dm_bitmap & BIT(RTW89_DM_DIG_PD))
+			pd_val = hal->fixed_dig_pd_th;
+		else
+			pd_val = restore ? dig->bak_dig : 0;
 	}
 
 	rtw89_debug(rtwdev, RTW89_DBG_DIG, "%s <%s> PD_low=%d", __func__,
-- 
2.25.1