wifi: rtw89: constrain TX power according to Transmit Power Envelope

Calculate a TX power constraint based on content of ieee80211 Transmit
Power Envelope (TPE). Since HW control registers aren't designed as many
as all kinds of TPE fields, we strictly intersect all TPE inputs in driver.
Then, according to result, constrain TX power via TX power limit/limit_RU.

Besides, extend dbgfs txpwr_table to show info about 6 GHz regulatory.
Signed-off-by: Zong-Zhe Yang <kevin_yang@realtek.com>
Signed-off-by: Ping-Ke Shih <pkshih@realtek.com>
Link: https://patch.msgid.link/20240626023237.7901-1-pkshih@realtek.com

drivers/net/wireless/realtek/rtw89/core.c

@@ -3376,8 +3376,12 @@ int rtw89_core_sta_add(struct rtw89_dev *rtwdev,
 	if (vif->type == NL80211_IFTYPE_STATION && !sta->tdls) {
 		/* for station mode, assign the mac_id from itself */
 		rtwsta->mac_id = rtwvif->mac_id;
-		/* must do rtw89_reg_6ghz_power_recalc() before rfk channel */
-		rtw89_reg_6ghz_power_recalc(rtwdev, rtwvif, true);
+
+		/* must do rtw89_reg_6ghz_recalc() before rfk channel */
+		ret = rtw89_reg_6ghz_recalc(rtwdev, rtwvif, true);
+		if (ret)
+			return ret;
+
 		rtw89_btc_ntfy_role_info(rtwdev, rtwvif, rtwsta,
 					 BTC_ROLE_MSTS_STA_CONN_START);
 		rtw89_chip_rfk_channel(rtwdev);
@@ -3565,7 +3569,7 @@ int rtw89_core_sta_remove(struct rtw89_dev *rtwdev,
 	int ret;
 
 	if (vif->type == NL80211_IFTYPE_STATION && !sta->tdls) {
-		rtw89_reg_6ghz_power_recalc(rtwdev, rtwvif, false);
+		rtw89_reg_6ghz_recalc(rtwdev, rtwvif, false);
 		rtw89_btc_ntfy_role_info(rtwdev, rtwvif, rtwsta,
 					 BTC_ROLE_MSTS_STA_DIS_CONN);
 	} else if (vif->type == NL80211_IFTYPE_AP || sta->tdls) {

drivers/net/wireless/realtek/rtw89/core.h

@@ -746,6 +746,14 @@ enum rtw89_reg_6ghz_power {
 	RTW89_REG_6GHZ_POWER_DFLT = RTW89_REG_6GHZ_POWER_VLP,
 };
 
+#define RTW89_MIN_VALID_POWER_CONSTRAINT (-10) /* unit: dBm */
+
+/* calculate based on ieee80211 Transmit Power Envelope */
+struct rtw89_reg_6ghz_tpe {
+	bool valid;
+	s8 constraint; /* unit: dBm */
+};
+
 enum rtw89_fw_pkt_ofld_type {
 	RTW89_PKT_OFLD_TYPE_PROBE_RSP = 0,
 	RTW89_PKT_OFLD_TYPE_PS_POLL = 1,
@@ -3397,6 +3405,7 @@ struct rtw89_vif {
 	bool chanctx_assigned; /* only valid when running with chanctx_ops */
 	enum rtw89_sub_entity_idx sub_entity_idx;
 	enum rtw89_reg_6ghz_power reg_6ghz_power;
+	struct rtw89_reg_6ghz_tpe reg_6ghz_tpe;
 
 	u8 mac_id;
 	u8 port;
@@ -4941,6 +4950,7 @@ struct rtw89_regd {
 struct rtw89_regulatory_info {
 	const struct rtw89_regd *regd;
 	enum rtw89_reg_6ghz_power reg_6ghz_power;
+	struct rtw89_reg_6ghz_tpe reg_6ghz_tpe;
 	DECLARE_BITMAP(block_unii4, RTW89_REGD_MAX_COUNTRY_NUM);
 	DECLARE_BITMAP(block_6ghz, RTW89_REGD_MAX_COUNTRY_NUM);
 	DECLARE_BITMAP(block_6ghz_sp, RTW89_REGD_MAX_COUNTRY_NUM);
@@ -6535,8 +6545,8 @@ void rtw89_core_scan_start(struct rtw89_dev *rtwdev, struct rtw89_vif *rtwvif,
 			   const u8 *mac_addr, bool hw_scan);
 void rtw89_core_scan_complete(struct rtw89_dev *rtwdev,
 			      struct ieee80211_vif *vif, bool hw_scan);
-void rtw89_reg_6ghz_power_recalc(struct rtw89_dev *rtwdev,
-				 struct rtw89_vif *rtwvif, bool active);
+int rtw89_reg_6ghz_recalc(struct rtw89_dev *rtwdev, struct rtw89_vif *rtwvif,
+			  bool active);
 void rtw89_core_update_p2p_ps(struct rtw89_dev *rtwdev, struct ieee80211_vif *vif);
 void rtw89_core_ntfy_btc_event(struct rtw89_dev *rtwdev, enum rtw89_btc_hmsg event);
 

drivers/net/wireless/realtek/rtw89/debug.c

@@ -818,6 +818,28 @@ static const struct dbgfs_txpwr_table *dbgfs_txpwr_tables[RTW89_CHIP_GEN_NUM] =
 	[RTW89_CHIP_BE] = &dbgfs_txpwr_table_be,
 };
 
+static
+void rtw89_debug_priv_txpwr_table_get_regd(struct seq_file *m,
+					   struct rtw89_dev *rtwdev,
+					   const struct rtw89_chan *chan)
+{
+	const struct rtw89_regulatory_info *regulatory = &rtwdev->regulatory;
+	const struct rtw89_reg_6ghz_tpe *tpe6 = &regulatory->reg_6ghz_tpe;
+
+	seq_printf(m, "[Chanctx] band %u, ch %u, bw %u\n",
+		   chan->band_type, chan->channel, chan->band_width);
+
+	seq_puts(m, "[Regulatory] ");
+	__print_regd(m, rtwdev, chan);
+
+	if (chan->band_type == RTW89_BAND_6G) {
+		seq_printf(m, "[reg6_pwr_type] %u\n", regulatory->reg_6ghz_power);
+
+		if (tpe6->valid)
+			seq_printf(m, "[TPE] %d dBm\n", tpe6->constraint);
+	}
+}
+
 static int rtw89_debug_priv_txpwr_table_get(struct seq_file *m, void *v)
 {
 	struct rtw89_debugfs_priv *debugfs_priv = m->private;
@@ -831,8 +853,7 @@ static int rtw89_debug_priv_txpwr_table_get(struct seq_file *m, void *v)
 	rtw89_leave_ps_mode(rtwdev);
 	chan = rtw89_chan_get(rtwdev, RTW89_SUB_ENTITY_0);
 
-	seq_puts(m, "[Regulatory] ");
-	__print_regd(m, rtwdev, chan);
+	rtw89_debug_priv_txpwr_table_get_regd(m, rtwdev, chan);
 
 	seq_puts(m, "[SAR]\n");
 	rtw89_print_sar(m, rtwdev, chan->freq);

drivers/net/wireless/realtek/rtw89/mac80211.c

@@ -487,6 +487,9 @@ static void rtw89_ops_link_info_changed(struct ieee80211_hw *hw,
 	if (changed & BSS_CHANGED_CQM)
 		rtw89_fw_h2c_set_bcn_fltr_cfg(rtwdev, vif, true);
 
+	if (changed & BSS_CHANGED_TPE)
+		rtw89_reg_6ghz_recalc(rtwdev, rtwvif, true);
+
 	mutex_unlock(&rtwdev->mutex);
 }
 

drivers/net/wireless/realtek/rtw89/phy.c

@@ -1847,6 +1847,36 @@ static s8 rtw89_phy_txpwr_rf_to_mac(struct rtw89_dev *rtwdev, s8 txpwr_rf)
 	return txpwr_rf >> (chip->txpwr_factor_rf - chip->txpwr_factor_mac);
 }
 
+static s8 rtw89_phy_txpwr_dbm_to_mac(struct rtw89_dev *rtwdev, s8 dbm)
+{
+	const struct rtw89_chip_info *chip = rtwdev->chip;
+
+	return clamp_t(s16, dbm << chip->txpwr_factor_mac, -64, 63);
+}
+
+static s8 rtw89_phy_txpwr_dbm_without_tolerance(s8 dbm)
+{
+	const u8 tssi_deviation_point = 0;
+	const u8 tssi_max_deviation = 2;
+
+	if (dbm <= tssi_deviation_point)
+		dbm -= tssi_max_deviation;
+
+	return dbm;
+}
+
+static s8 rtw89_phy_get_tpe_constraint(struct rtw89_dev *rtwdev, u8 band)
+{
+	struct rtw89_regulatory_info *regulatory = &rtwdev->regulatory;
+	const struct rtw89_reg_6ghz_tpe *tpe = &regulatory->reg_6ghz_tpe;
+	s8 cstr = S8_MAX;
+
+	if (band == RTW89_BAND_6G && tpe->valid)
+		cstr = rtw89_phy_txpwr_dbm_without_tolerance(tpe->constraint);
+
+	return rtw89_phy_txpwr_dbm_to_mac(rtwdev, cstr);
+}
+
 s8 rtw89_phy_read_txpwr_byrate(struct rtw89_dev *rtwdev, u8 band, u8 bw,
 			       const struct rtw89_rate_desc *rate_desc)
 {
@@ -1921,6 +1951,7 @@ s8 rtw89_phy_read_txpwr_limit(struct rtw89_dev *rtwdev, u8 band,
 	u8 regd = rtw89_regd_get(rtwdev, band);
 	u8 reg6 = regulatory->reg_6ghz_power;
 	s8 lmt = 0, sar;
+	s8 cstr;
 
 	switch (band) {
 	case RTW89_BAND_2G:
@@ -1953,8 +1984,9 @@ s8 rtw89_phy_read_txpwr_limit(struct rtw89_dev *rtwdev, u8 band,
 
 	lmt = rtw89_phy_txpwr_rf_to_mac(rtwdev, lmt);
 	sar = rtw89_query_sar(rtwdev, freq);
+	cstr = rtw89_phy_get_tpe_constraint(rtwdev, band);
 
-	return min(lmt, sar);
+	return min3(lmt, sar, cstr);
 }
 EXPORT_SYMBOL(rtw89_phy_read_txpwr_limit);
 
@@ -2178,6 +2210,7 @@ s8 rtw89_phy_read_txpwr_limit_ru(struct rtw89_dev *rtwdev, u8 band,
 	u8 regd = rtw89_regd_get(rtwdev, band);
 	u8 reg6 = regulatory->reg_6ghz_power;
 	s8 lmt_ru = 0, sar;
+	s8 cstr;
 
 	switch (band) {
 	case RTW89_BAND_2G:
@@ -2210,8 +2243,9 @@ s8 rtw89_phy_read_txpwr_limit_ru(struct rtw89_dev *rtwdev, u8 band,
 
 	lmt_ru = rtw89_phy_txpwr_rf_to_mac(rtwdev, lmt_ru);
 	sar = rtw89_query_sar(rtwdev, freq);
+	cstr = rtw89_phy_get_tpe_constraint(rtwdev, band);
 
-	return min(lmt_ru, sar);
+	return min3(lmt_ru, sar, cstr);
 }
 
 static void

drivers/net/wireless/realtek/rtw89/regd.c

@@ -714,7 +714,154 @@ void rtw89_regd_notifier(struct wiphy *wiphy, struct regulatory_request *request
 	mutex_unlock(&rtwdev->mutex);
 }
 
-static void __rtw89_reg_6ghz_power_recalc(struct rtw89_dev *rtwdev)
+/* Maximum Transmit Power field (@raw) can be EIRP or PSD.
+ * Both units are 0.5 dB-based. Return a constraint in dB.
+ */
+static s8 tpe_get_constraint(s8 raw)
+{
+	const u8 hw_deviation = 3; /* unit: 0.5 dB */
+	const u8 antenna_gain = 10; /* unit: 0.5 dB */
+	const u8 array_gain = 6; /* unit: 0.5 dB */
+	const u8 offset = hw_deviation + antenna_gain + array_gain;
+
+	return (raw - offset) / 2;
+}
+
+static void tpe_intersect_constraint(struct rtw89_reg_6ghz_tpe *tpe, s8 cstr)
+{
+	if (tpe->valid) {
+		tpe->constraint = min(tpe->constraint, cstr);
+		return;
+	}
+
+	tpe->constraint = cstr;
+	tpe->valid = true;
+}
+
+static void tpe_deal_with_eirp(struct rtw89_reg_6ghz_tpe *tpe,
+			       const struct ieee80211_parsed_tpe_eirp *eirp)
+{
+	unsigned int i;
+	s8 cstr;
+
+	if (!eirp->valid)
+		return;
+
+	for (i = 0; i < eirp->count; i++) {
+		cstr = tpe_get_constraint(eirp->power[i]);
+		tpe_intersect_constraint(tpe, cstr);
+	}
+}
+
+static s8 tpe_convert_psd_to_eirp(s8 psd)
+{
+	static const unsigned int mlog20 = 1301;
+
+	return psd + 10 * mlog20 / 1000;
+}
+
+static void tpe_deal_with_psd(struct rtw89_reg_6ghz_tpe *tpe,
+			      const struct ieee80211_parsed_tpe_psd *psd)
+{
+	unsigned int i;
+	s8 cstr_psd;
+	s8 cstr;
+
+	if (!psd->valid)
+		return;
+
+	for (i = 0; i < psd->count; i++) {
+		cstr_psd = tpe_get_constraint(psd->power[i]);
+		cstr = tpe_convert_psd_to_eirp(cstr_psd);
+		tpe_intersect_constraint(tpe, cstr);
+	}
+}
+
+static void rtw89_calculate_tpe(struct rtw89_dev *rtwdev,
+				struct rtw89_reg_6ghz_tpe *result_tpe,
+				const struct ieee80211_parsed_tpe *parsed_tpe)
+{
+	static const u8 category = IEEE80211_TPE_CAT_6GHZ_DEFAULT;
+
+	tpe_deal_with_eirp(result_tpe, &parsed_tpe->max_local[category]);
+	tpe_deal_with_eirp(result_tpe, &parsed_tpe->max_reg_client[category]);
+	tpe_deal_with_psd(result_tpe, &parsed_tpe->psd_local[category]);
+	tpe_deal_with_psd(result_tpe, &parsed_tpe->psd_reg_client[category]);
+}
+
+static bool __rtw89_reg_6ghz_tpe_recalc(struct rtw89_dev *rtwdev)
+{
+	struct rtw89_regulatory_info *regulatory = &rtwdev->regulatory;
+	struct rtw89_reg_6ghz_tpe new = {};
+	struct rtw89_vif *rtwvif;
+	bool changed = false;
+
+	rtw89_for_each_rtwvif(rtwdev, rtwvif) {
+		const struct rtw89_reg_6ghz_tpe *tmp;
+		const struct rtw89_chan *chan;
+
+		chan = rtw89_chan_get(rtwdev, rtwvif->sub_entity_idx);
+		if (chan->band_type != RTW89_BAND_6G)
+			continue;
+
+		tmp = &rtwvif->reg_6ghz_tpe;
+		if (!tmp->valid)
+			continue;
+
+		tpe_intersect_constraint(&new, tmp->constraint);
+	}
+
+	if (memcmp(&regulatory->reg_6ghz_tpe, &new,
+		   sizeof(regulatory->reg_6ghz_tpe)) != 0)
+		changed = true;
+
+	if (changed) {
+		if (new.valid)
+			rtw89_debug(rtwdev, RTW89_DBG_REGD,
+				    "recalc 6 GHz reg TPE to %d dBm\n",
+				    new.constraint);
+		else
+			rtw89_debug(rtwdev, RTW89_DBG_REGD,
+				    "recalc 6 GHz reg TPE to none\n");
+
+		regulatory->reg_6ghz_tpe = new;
+	}
+
+	return changed;
+}
+
+static int rtw89_reg_6ghz_tpe_recalc(struct rtw89_dev *rtwdev,
+				     struct rtw89_vif *rtwvif, bool active,
+				     unsigned int *changed)
+{
+	struct ieee80211_vif *vif = rtwvif_to_vif(rtwvif);
+	struct ieee80211_bss_conf *bss_conf = &vif->bss_conf;
+	struct rtw89_reg_6ghz_tpe *tpe = &rtwvif->reg_6ghz_tpe;
+
+	memset(tpe, 0, sizeof(*tpe));
+
+	if (!active || rtwvif->reg_6ghz_power != RTW89_REG_6GHZ_POWER_STD)
+		goto bottom;
+
+	rtw89_calculate_tpe(rtwdev, tpe, &bss_conf->tpe);
+	if (!tpe->valid)
+		goto bottom;
+
+	if (tpe->constraint < RTW89_MIN_VALID_POWER_CONSTRAINT) {
+		rtw89_err(rtwdev,
+			  "%s: constraint %d dBm is less than min valid val\n",
+			  __func__, tpe->constraint);
+
+		tpe->valid = false;
+		return -EINVAL;
+	}
+
+bottom:
+	*changed += __rtw89_reg_6ghz_tpe_recalc(rtwdev);
+	return 0;
+}
+
+static bool __rtw89_reg_6ghz_power_recalc(struct rtw89_dev *rtwdev)
 {
 	struct rtw89_regulatory_info *regulatory = &rtwdev->regulatory;
 	const struct rtw89_regd *regd = regulatory->regd;
@@ -751,23 +898,21 @@ static void __rtw89_reg_6ghz_power_recalc(struct rtw89_dev *rtwdev)
 	}
 
 	if (regulatory->reg_6ghz_power == sel)
-		return;
+		return false;
 
 	rtw89_debug(rtwdev, RTW89_DBG_REGD,
 		    "recalc 6 GHz reg power type to %d\n", sel);
 
 	regulatory->reg_6ghz_power = sel;
-
-	rtw89_core_set_chip_txpwr(rtwdev);
+	return true;
 }
 
-void rtw89_reg_6ghz_power_recalc(struct rtw89_dev *rtwdev,
-				 struct rtw89_vif *rtwvif, bool active)
+static int rtw89_reg_6ghz_power_recalc(struct rtw89_dev *rtwdev,
+				       struct rtw89_vif *rtwvif, bool active,
+				       unsigned int *changed)
 {
 	struct ieee80211_vif *vif = rtwvif_to_vif(rtwvif);
 
-	lockdep_assert_held(&rtwdev->mutex);
-
 	if (active) {
 		switch (vif->bss_conf.power_type) {
 		case IEEE80211_REG_VLP_AP:
@@ -787,5 +932,32 @@ void rtw89_reg_6ghz_power_recalc(struct rtw89_dev *rtwdev,
 		rtwvif->reg_6ghz_power = RTW89_REG_6GHZ_POWER_DFLT;
 	}
 
-	__rtw89_reg_6ghz_power_recalc(rtwdev);
+	*changed += __rtw89_reg_6ghz_power_recalc(rtwdev);
+	return 0;
+}
+
+int rtw89_reg_6ghz_recalc(struct rtw89_dev *rtwdev, struct rtw89_vif *rtwvif,
+			  bool active)
+{
+	unsigned int changed = 0;
+	int ret;
+
+	lockdep_assert_held(&rtwdev->mutex);
+
+	/* The result of reg_6ghz_tpe may depend on reg_6ghz_power type,
+	 * so must do reg_6ghz_tpe_recalc() after reg_6ghz_power_recalc().
+	 */
+
+	ret = rtw89_reg_6ghz_power_recalc(rtwdev, rtwvif, active, &changed);
+	if (ret)
+		return ret;
+
+	ret = rtw89_reg_6ghz_tpe_recalc(rtwdev, rtwvif, active, &changed);
+	if (ret)
+		return ret;
+
+	if (changed)
+		rtw89_core_set_chip_txpwr(rtwdev);
+
+	return 0;
 }