recipes-wifi/hostapd/files/patches-2.10.3/0120-mtk-hostapd-Add-txpower-vendor-command.patch

From 4c0dd29eb15b2cef5cf5256459a19d59e5c98adc Mon Sep 17 00:00:00 2001
From: Allen Ye <allen.ye@mediatek.com>
Date: Fri, 22 Dec 2023 18:09:20 +0800
Subject: [PATCH 120/126] mtk: hostapd: Add txpower vendor command

Porting and refactor from wifi6 power vendor cmd. Add lpi psd control,
sku index and duplicate mode enhancement.

Signed-off-by: Allen Ye <allen.ye@mediatek.com>
---
 hostapd/config_file.c             |  8 +++++
 src/ap/ap_config.c                |  4 +++
 src/ap/ap_config.h                |  3 ++
 src/ap/ap_drv_ops.c               | 16 +++++++++
 src/ap/ap_drv_ops.h               |  1 +
 src/ap/hostapd.c                  |  2 ++
 src/ap/ieee802_11_he.c            |  3 ++
 src/common/mtk_vendor.h           | 15 +++++++++
 src/drivers/driver.h              | 10 ++++++
 src/drivers/driver_nl80211.c      | 55 +++++++++++++++++++++++++++++++
 src/drivers/driver_nl80211.h      |  1 +
 src/drivers/driver_nl80211_capa.c |  3 ++
 12 files changed, 121 insertions(+)

diff --git a/hostapd/config_file.c b/hostapd/config_file.c
index f7bfc357a..944669270 100644
--- a/hostapd/config_file.c
+++ b/hostapd/config_file.c
@@ -5570,6 +5570,14 @@ static int hostapd_config_fill(struct hostapd_config *conf,
 			return 1;
 		}
 		conf->band_idx = (u8) val;
+	} else if (os_strcmp(buf, "lpi_psd") == 0) {
+		u8 en = strtol(pos, NULL, 10);
+		conf->lpi_psd = !!en;
+	} else if (os_strcmp(buf, "sku_idx") == 0) {
+		conf->sku_idx = strtol(pos, NULL, 10);
+	} else if (os_strcmp(buf, "lpi_bcn_enhance") == 0) {
+		u8 en = strtol(pos, NULL, 10);
+		conf->lpi_bcn_enhance = !!en;
 	} else {
 		wpa_printf(MSG_ERROR,
 			   "Line %d: unknown configuration item '%s'",
diff --git a/src/ap/ap_config.c b/src/ap/ap_config.c
index 4528df823..bb5ec78a1 100644
--- a/src/ap/ap_config.c
+++ b/src/ap/ap_config.c
@@ -315,6 +315,10 @@ struct hostapd_config * hostapd_config_defaults(void)
 	conf->pp_mode = PP_FW_MODE;
 	conf->band_idx = 255;
 
+	conf->lpi_psd = 0;
+	conf->sku_idx = 0;
+	conf->lpi_bcn_enhance = 0;
+
 	hostapd_set_and_check_bw320_offset(conf, 0);
 
 	return conf;
diff --git a/src/ap/ap_config.h b/src/ap/ap_config.h
index 9c3a28cf4..ea8507cea 100644
--- a/src/ap/ap_config.h
+++ b/src/ap/ap_config.h
@@ -1356,6 +1356,9 @@ struct hostapd_config {
 	void *muru_config;
 	u8 pp_mode;
 	u8 band_idx;
+	u8 lpi_psd;
+	u8 sku_idx;
+	u8 lpi_bcn_enhance;
 };
 
 enum three_wire_mode {
diff --git a/src/ap/ap_drv_ops.c b/src/ap/ap_drv_ops.c
index a25a67cdd..2f53c518f 100644
--- a/src/ap/ap_drv_ops.c
+++ b/src/ap/ap_drv_ops.c
@@ -1391,6 +1391,22 @@ int hostapd_drv_get_aval_bss_color_bmp(struct hostapd_data *hapd, u64 *aval_colo
 	return hapd->driver->get_aval_color_bmp(hapd->drv_priv, aval_color_bmp);
 }
 
+int hostapd_drv_txpower_ctrl(struct hostapd_data *hapd)
+{
+	s8 link_id = -1;
+
+	if (!hapd->driver || !hapd->driver->txpower_ctrl)
+		return 0;
+
+	if (hapd->conf->mld_ap)
+		link_id = hapd->mld_link_id;
+
+	return hapd->driver->txpower_ctrl(hapd->drv_priv, hapd->iconf->lpi_psd,
+					  hapd->iconf->sku_idx,
+					  hapd->iconf->lpi_bcn_enhance,
+					  link_id);
+}
+
 int hostapd_drv_ap_wireless(struct hostapd_data *hapd, u8 sub_vendor_id, int value)
 {
 	s8 link_id = -1;
diff --git a/src/ap/ap_drv_ops.h b/src/ap/ap_drv_ops.h
index 7f108bc1d..a8c60b6f6 100644
--- a/src/ap/ap_drv_ops.h
+++ b/src/ap/ap_drv_ops.h
@@ -167,6 +167,7 @@ int hostapd_drv_amsdu_ctrl(struct hostapd_data *hapd);
 int hostapd_drv_amsdu_dump(struct hostapd_data *hapd, u8 *amsdu);
 int hostapd_drv_get_aval_bss_color_bmp(struct hostapd_data *hapd,
 				       u64 *aval_color_bmp);
+int hostapd_drv_txpower_ctrl(struct hostapd_data *hapd);
 int hostapd_drv_ap_wireless(struct hostapd_data *hapd, u8 sub_vendor_id, int value);
 int hostapd_drv_ap_rfeatures(struct hostapd_data *hapd, u8 sub_vendor_id, int value);
 int hostapd_drv_ap_trig_type(struct hostapd_data *hapd, u8 enable, u8 type);
diff --git a/src/ap/hostapd.c b/src/ap/hostapd.c
index 9388f6070..6f7f13f9f 100644
--- a/src/ap/hostapd.c
+++ b/src/ap/hostapd.c
@@ -2879,6 +2879,8 @@ dfs_offload:
 		goto fail;
 	if (hostapd_drv_amsdu_ctrl(hapd) < 0)
 		goto fail;
+	if (hostapd_drv_txpower_ctrl(hapd) < 0)
+		goto fail;
 
 	wpa_printf(MSG_DEBUG, "%s: Setup of interface done.",
 		   iface->bss[0]->conf->iface);
diff --git a/src/ap/ieee802_11_he.c b/src/ap/ieee802_11_he.c
index 3b6b2041c..f3a5679a0 100644
--- a/src/ap/ieee802_11_he.c
+++ b/src/ap/ieee802_11_he.c
@@ -264,6 +264,9 @@ u8 * hostapd_eid_he_operation(struct hostapd_data *hapd, u8 *eid)
 		else
 			control = center_idx_to_bw_6ghz(seg0);
 
+		if (hapd->iconf->lpi_bcn_enhance)
+			control |= HE_6GHZ_OPER_INFO_CTRL_DUP_BEACON;
+
 		control |= hapd->iconf->he_6ghz_reg_pwr_type <<
 			HE_6GHZ_OPER_INFO_CTRL_REG_INFO_SHIFT;
 
diff --git a/src/common/mtk_vendor.h b/src/common/mtk_vendor.h
index 933f0099d..6d75d39c2 100644
--- a/src/common/mtk_vendor.h
+++ b/src/common/mtk_vendor.h
@@ -19,6 +19,7 @@ enum mtk_nl80211_vendor_subcmds {
 	MTK_NL80211_VENDOR_SUBCMD_BACKGROUND_RADAR_CTRL = 0xcb,
 	MTK_NL80211_VENDOR_SUBCMD_PP_CTRL = 0xcc,
 	MTK_NL80211_VENDOR_SUBCMD_BEACON_CTRL = 0xcd,
+	MTK_NL80211_VENDOR_SUBCMD_TXPOWER_CTRL = 0xce,
 	MTK_NL80211_VENDOR_SUBCMD_EML_CTRL = 0xd3,
 };
 
@@ -310,6 +311,20 @@ enum mtk_vendor_attr_eml_ctrl {
 		NUM_MTK_VENDOR_ATTRS_EML_CTRL -1
 };
 
+enum mtk_vendor_attr_txpower_ctrl {
+	MTK_VENDOR_ATTR_TXPOWER_CTRL_UNSPEC,
+
+	MTK_VENDOR_ATTR_TXPOWER_CTRL_LPI_PSD,
+	MTK_VENDOR_ATTR_TXPOWER_CTRL_SKU_IDX,
+	MTK_VENDOR_ATTR_TXPOWER_CTRL_LPI_BCN_ENHANCE,
+	MTK_VENDOR_ATTR_TXPOWER_CTRL_LINK_ID,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_TXPOWER_CTRL,
+	MTK_VENDOR_ATTR_TXPOWER_CTRL_MAX =
+		NUM_MTK_VENDOR_ATTRS_TXPOWER_CTRL - 1
+};
+
 #define CSI_BW20_DATA_COUNT	64
 #define CSI_BW40_DATA_COUNT	128
 #define CSI_BW80_DATA_COUNT	256
diff --git a/src/drivers/driver.h b/src/drivers/driver.h
index fe327e560..55f62f537 100644
--- a/src/drivers/driver.h
+++ b/src/drivers/driver.h
@@ -5443,6 +5443,16 @@ struct wpa_driver_ops {
 	* @dump_buf: Dump_struct that store csi data and related info
 	*/
 	int (*csi_dump)(void *priv, u8 band_idx, void *dump_buf);
+	/**
+	* txpower_ctrl - ctrl txpower operation
+	* @priv: Private driver interface data
+	* @lpi_psd: 1 to enable lpi psd compensate, 0 to disable
+	* @lpi_bcn_enhance: 1 to enable beacon duplicate enhancement in 6G lpi mode, 0 to disable enhancement
+	* @sku_idx: index used to indicate which sku table should be used
+	* @link_id: MLD link id. -1 if this is an non-MLD AP
+	*/
+	int (*txpower_ctrl)(void *priv, u8 lpi_psd, u8 sku_idx, u8 lpi_bcn_enhance,
+			    u8 link_id);
 };
 
 /**
diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index 05b231c52..395bb5926 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -195,6 +195,14 @@ static struct nla_policy csi_data_policy[NUM_MTK_VENDOR_ATTRS_CSI_DATA] = {
 	[MTK_VENDOR_ATTR_CSI_DATA_CHAIN_INFO] = { .type = NLA_U32 },
 };
 
+static struct nla_policy
+txpower_ctrl_policy[NUM_MTK_VENDOR_ATTRS_TXPOWER_CTRL] = {
+	[MTK_VENDOR_ATTR_TXPOWER_CTRL_LPI_PSD] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_TXPOWER_CTRL_SKU_IDX] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_TXPOWER_CTRL_LPI_BCN_ENHANCE] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_TXPOWER_CTRL_LINK_ID] = { .type = NLA_U8 },
+};
+
 static struct nl_sock * nl_create_handle(struct nl_cb *cb, const char *dbg)
 {
 	struct nl_sock *handle;
@@ -15592,6 +15600,52 @@ fail:
 	return -ENOBUFS;
 }
 
+static int nl80211_txpower_ctrl(void *priv, u8 lpi_psd, u8 sku_idx, u8 lpi_bcn_enhance,
+				u8 link_id)
+{
+	struct i802_bss *bss = priv;
+	struct wpa_driver_nl80211_data *drv = bss->drv;
+	struct nl_msg *msg;
+	struct nlattr *data;
+	int ret;
+
+	if (!drv->mtk_txpower_vendor_cmd_avail) {
+		wpa_printf(MSG_INFO,
+			   "nl80211: Driver does not support setting txpower control");
+		return 0;
+	}
+
+	msg = nl80211_drv_msg(drv, 0, NL80211_CMD_VENDOR);
+	if (!msg)
+		goto fail;
+
+	if (nla_put_u32(msg, NL80211_ATTR_VENDOR_ID, OUI_MTK) ||
+		nla_put_u32(msg, NL80211_ATTR_VENDOR_SUBCMD,
+			    MTK_NL80211_VENDOR_SUBCMD_TXPOWER_CTRL))
+		goto fail;
+
+	data = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA);
+	if (!data)
+		goto fail;
+
+	nla_put_u8(msg, MTK_VENDOR_ATTR_TXPOWER_CTRL_LPI_PSD, lpi_psd);
+	nla_put_u8(msg, MTK_VENDOR_ATTR_TXPOWER_CTRL_SKU_IDX, sku_idx);
+	nla_put_u8(msg, MTK_VENDOR_ATTR_TXPOWER_CTRL_LPI_BCN_ENHANCE, lpi_bcn_enhance);
+	nla_put_u8(msg, MTK_VENDOR_ATTR_TXPOWER_CTRL_LINK_ID, link_id);
+
+	nla_nest_end(msg, data);
+	ret = send_and_recv_cmd(drv, msg);
+	if (ret)
+		wpa_printf(MSG_ERROR, "Failed to set power. ret=%d (%s)",
+			   ret, strerror(-ret));
+
+	return ret;
+
+fail:
+	nlmsg_free(msg);
+	return -ENOBUFS;
+}
+
 const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 	.name = "nl80211",
 	.desc = "Linux nl80211/cfg80211",
@@ -15779,4 +15833,5 @@ const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 #endif
 	.csi_set = nl80211_csi_set,
 	.csi_dump = nl80211_csi_dump,
+	.txpower_ctrl = nl80211_txpower_ctrl,
 };
diff --git a/src/drivers/driver_nl80211.h b/src/drivers/driver_nl80211.h
index 2cc40e0dc..d32c0ff4d 100644
--- a/src/drivers/driver_nl80211.h
+++ b/src/drivers/driver_nl80211.h
@@ -214,6 +214,7 @@ struct wpa_driver_nl80211_data {
 	unsigned int mtk_beacon_ctrl_vendor_cmd_avail:1;
 	unsigned int mtk_csi_vendor_cmd_avail:1;
 	unsigned int mtk_eml_vendor_cmd_avail:1;
+	unsigned int mtk_txpower_vendor_cmd_avail:1;
 
 	u32 ignore_next_local_disconnect;
 	u32 ignore_next_local_deauth;
diff --git a/src/drivers/driver_nl80211_capa.c b/src/drivers/driver_nl80211_capa.c
index c1327a679..fc7f9062f 100644
--- a/src/drivers/driver_nl80211_capa.c
+++ b/src/drivers/driver_nl80211_capa.c
@@ -1179,6 +1179,9 @@ static int wiphy_info_handler(struct nl_msg *msg, void *arg)
 				case MTK_NL80211_VENDOR_SUBCMD_EML_CTRL:
 					drv->mtk_eml_vendor_cmd_avail = 1;
 					break;
+				case MTK_NL80211_VENDOR_SUBCMD_TXPOWER_CTRL:
+					drv->mtk_txpower_vendor_cmd_avail = 1;
+					break;
 				}
 			}
 
-- 
2.18.0