From df3d6a354fc1243f8c862f2b61ee9ac09eabe482 Mon Sep 17 00:00:00 2001
From: Howard Hsu <howard-yh.hsu@mediatek.com>
Date: Thu, 21 Sep 2023 10:29:46 +0800
Subject: [PATCH] hostapd: mtk: add support enable/disable preamble puncture
 from mtk vendor command

This commit supports two ways to enable/disable preamble puncture
feature.

1. Add new hostapd configuration "pp_mode". The possible value could be
1 to 3. When the value is 0, it means that the firmware will turn off
the pp algorithm. When the value is 1, it means that the firmware will
enable the pp algorithm, allowing the algorithm to determine whether pp
could be applied on each txcmd. When the value is 2, it means that pp
feature is manually configured by the user. Please noted that for
current implementation, the default configuration is 0.

2. $ hostapd_cli -i <intf_name> raw set_pp mode val
The argument "val" could be 0 for PP feature disabled or 1 to configure
PP feature as auto mode.

This commit also let user check whether pp feature is enabled by
hostapd_cli command. The usage shows as below:
$ hostapd_cli -i <intf_name> raw get_pp mode

Signed-off-by: Howard Hsu <howard-yh.hsu@mediatek.com>
---
 hostapd/config_file.c             | 11 ++++++
 hostapd/ctrl_iface.c              | 59 +++++++++++++++++++++++++++++++
 src/ap/ap_config.c                |  1 +
 src/ap/ap_config.h                |  7 ++++
 src/ap/ap_drv_ops.c               |  9 +++++
 src/ap/ap_drv_ops.h               |  1 +
 src/ap/hostapd.c                  |  2 ++
 src/common/mtk_vendor.h           | 12 +++++++
 src/drivers/driver.h              |  6 ++++
 src/drivers/driver_nl80211.c      | 49 +++++++++++++++++++++++++
 src/drivers/driver_nl80211.h      |  1 +
 src/drivers/driver_nl80211_capa.c |  3 ++
 12 files changed, 161 insertions(+)

diff --git a/hostapd/config_file.c b/hostapd/config_file.c
index a751993..278f6b3 100644
--- a/hostapd/config_file.c
+++ b/hostapd/config_file.c
@@ -4801,6 +4801,7 @@ static int hostapd_config_fill(struct hostapd_config *conf,
 		conf->eht_phy_capab.mu_beamformer = atoi(pos);
 	} else if (os_strcmp(buf, "punct_bitmap") == 0) {
 		conf->punct_bitmap = atoi(pos);
+		conf->pp_mode = PP_MANUAL_MODE;
 	} else if (os_strcmp(buf, "punct_acs_threshold") == 0) {
 		int val = atoi(pos);
 
@@ -4876,6 +4877,16 @@ static int hostapd_config_fill(struct hostapd_config *conf,
 			return 1;
 		}
 		conf->amsdu = val;
+	} else if (os_strcmp(buf, "pp_mode") == 0) {
+		int val = atoi(pos);
+
+		if ((val != PP_MANUAL_MODE && conf->punct_bitmap) ||
+		    val < PP_DISABLE || val > PP_MANUAL_MODE) {
+			wpa_printf(MSG_ERROR, "Line %d: invalid pp_mode value",
+				   line);
+			return 1;
+		}
+		conf->pp_mode = (u8) val;
 	} else {
 		wpa_printf(MSG_ERROR,
 			   "Line %d: unknown configuration item '%s'",
diff --git a/hostapd/ctrl_iface.c b/hostapd/ctrl_iface.c
index 517ebd6..7315d12 100644
--- a/hostapd/ctrl_iface.c
+++ b/hostapd/ctrl_iface.c
@@ -4183,6 +4183,59 @@ hostapd_ctrl_iface_set_background_radar_mode(struct hostapd_data *hapd, char *cm
 	return os_snprintf(buf, buflen, "OK\n");
 }
 
+static int
+hostapd_ctrl_iface_set_pp(struct hostapd_data *hapd, char *cmd, char *buf,
+			  size_t buflen)
+{
+	char *pos, *config, *value;
+
+	config = cmd;
+	pos = os_strchr(config, ' ');
+	if (pos == NULL)
+		return -1;
+	*pos++ = '\0';
+
+	if (pos == NULL)
+		return -1;
+	value = pos;
+
+	if (os_strcmp(config, "mode") == 0) {
+		int val = atoi(value);
+
+		if (val < PP_DISABLE || val > PP_AUTO_MODE) {
+			wpa_printf(MSG_ERROR, "Invalid value for set_pp");
+			return -1;
+		}
+		hapd->iconf->pp_mode = (u8) val;
+		if (hostapd_drv_pp_mode_set(hapd) != 0)
+			return -1;
+	} else {
+		wpa_printf(MSG_ERROR,
+			   "Unsupported parameter %s for set_pp", config);
+		return -1;
+	}
+	return os_snprintf(buf, buflen, "OK\n");
+}
+
+static int
+hostapd_ctrl_iface_get_pp(struct hostapd_data *hapd, char *cmd, char *buf,
+			  size_t buflen)
+{
+	char *pos, *end;
+
+	pos = buf;
+	end = buf + buflen;
+
+	if (os_strcmp(cmd, "mode") == 0) {
+		return os_snprintf(pos, end - pos, "pp_mode: %d\n",
+				   hapd->iconf->pp_mode);
+	} else {
+		wpa_printf(MSG_ERROR,
+			   "Unsupported parameter %s for get_pp", cmd);
+		return -1;
+	}
+}
+
 static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd,
 					      char *buf, char *reply,
 					      int reply_size,
@@ -4769,6 +4822,12 @@ static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd,
 	} else if (os_strncmp(buf, "DUMP_AMNT", 9) == 0) {
 		reply_len = hostapd_ctrl_iface_dump_amnt(hapd, buf+10,
 							reply, reply_size);
+	} else if (os_strncmp(buf, "set_pp", 6) == 0) {
+		reply_len = hostapd_ctrl_iface_set_pp(hapd, buf + 7, reply,
+						      reply_size);
+	} else if (os_strncmp(buf, "get_pp", 6) == 0) {
+		reply_len = hostapd_ctrl_iface_get_pp(hapd, buf + 7, reply,
+						      reply_size);
 	} else if (os_strncmp(buf, "set_muru_manual_config=", 23) == 0) {
 		// Replace first ':' with a single space ' '
 		char *pos = buf + 23;
diff --git a/src/ap/ap_config.c b/src/ap/ap_config.c
index 223db56..d8dd549 100644
--- a/src/ap/ap_config.c
+++ b/src/ap/ap_config.c
@@ -302,6 +302,7 @@ struct hostapd_config * hostapd_config_defaults(void)
 	conf->three_wire_enable = THREE_WIRE_MODE_DISABLE;
 	conf->ibf_enable = IBF_DEFAULT_ENABLE;
 	conf->amsdu = 1;
+	conf->pp_mode = PP_DISABLE;
 
 	return conf;
 }
diff --git a/src/ap/ap_config.h b/src/ap/ap_config.h
index b6f05e7..9e39e82 100644
--- a/src/ap/ap_config.h
+++ b/src/ap/ap_config.h
@@ -1205,6 +1205,7 @@ struct hostapd_config {
 	u8 dfs_detect_mode;
 	u8 amsdu;
 	void *muru_config;
+	u8 pp_mode;
 };
 
 enum three_wire_mode {
@@ -1257,6 +1258,12 @@ enum mtk_vendor_attr_edcca_ctrl_mode {
 	EDCCA_CTRL_NUM,
 };
 
+enum pp_mode {
+	PP_DISABLE = 0,
+	PP_AUTO_MODE,
+	PP_MANUAL_MODE,
+};
+
 #define EDCCA_DEFAULT_COMPENSATION -6
 #define EDCCA_MIN_COMPENSATION -126
 #define EDCCA_MAX_COMPENSATION 126
diff --git a/src/ap/ap_drv_ops.c b/src/ap/ap_drv_ops.c
index 5b93ea6..d0d8279 100644
--- a/src/ap/ap_drv_ops.c
+++ b/src/ap/ap_drv_ops.c
@@ -1271,3 +1271,12 @@ int hostapd_drv_background_radar_mode(struct hostapd_data *hapd)
 	return hapd->driver->background_radar_mode(hapd->drv_priv,
 						   hapd->iconf->background_radar_mode);
 }
+
+int hostapd_drv_pp_mode_set(struct hostapd_data *hapd)
+{
+	if (!hapd->driver || !hapd->driver->pp_mode_set ||
+	    hapd->iconf->pp_mode > PP_AUTO_MODE)
+		return 0;
+	return hapd->driver->pp_mode_set(hapd->drv_priv,
+					 hapd->iconf->pp_mode);
+}
diff --git a/src/ap/ap_drv_ops.h b/src/ap/ap_drv_ops.h
index 1e7ae7a..e4c2827 100644
--- a/src/ap/ap_drv_ops.h
+++ b/src/ap/ap_drv_ops.h
@@ -163,6 +163,7 @@ int hostapd_drv_ap_trig_type(struct hostapd_data *hapd, u8 enable, u8 type);
 
 int hostapd_drv_amnt_set(struct hostapd_data *hapd, u8 amnt_idx, u8 *amnt_sta_mac);
 int hostapd_drv_amnt_dump(struct hostapd_data *hapd, u8 amnt_idx, u8 *amnt_dump_buf);
+int hostapd_drv_pp_mode_set(struct hostapd_data *hapd);
 
 #include "drivers/driver.h"
 
diff --git a/src/ap/hostapd.c b/src/ap/hostapd.c
index 2b563a5..90c6c26 100644
--- a/src/ap/hostapd.c
+++ b/src/ap/hostapd.c
@@ -2526,6 +2526,8 @@ dfs_offload:
 		goto fail;
 	if (hostapd_drv_amsdu_ctrl(hapd) < 0)
 		goto fail;
+	if (hostapd_drv_pp_mode_set(hapd) < 0)
+		goto fail;
 
 	wpa_printf(MSG_DEBUG, "%s: Setup of interface done.",
 		   iface->bss[0]->conf->iface);
diff --git a/src/common/mtk_vendor.h b/src/common/mtk_vendor.h
index 5bc1e04..6275c14 100644
--- a/src/common/mtk_vendor.h
+++ b/src/common/mtk_vendor.h
@@ -17,6 +17,7 @@ enum mtk_nl80211_vendor_subcmds {
 	MTK_NL80211_VENDOR_SUBCMD_IBF_CTRL = 0xc9,
 	MTK_NL80211_VENDOR_SUBCMD_BSS_COLOR_CTRL = 0xca,
 	MTK_NL80211_VENDOR_SUBCMD_BACKGROUND_RADAR_CTRL = 0xcb,
+	MTK_NL80211_VENDOR_SUBCMD_PP_CTRL = 0xcc,
 };
 
 enum mtk_vendor_attr_edcca_ctrl {
@@ -256,6 +257,17 @@ enum mtk_vendor_attr_background_radar_ctrl {
 		NUM_MTK_VENDOR_ATTRS_BACKGROUND_RADAR_CTRL - 1
 };
 
+enum mtk_vendor_attr_pp_ctrl {
+	MTK_VENDOR_ATTR_PP_CTRL_UNSPEC,
+
+	MTK_VENDOR_ATTR_PP_MODE,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_PP_CTRL,
+	MTK_VENDOR_ATTR_PP_CTRL_MAX =
+		NUM_MTK_VENDOR_ATTRS_PP_CTRL - 1
+};
+
 #define CSI_MAX_COUNT 256
 #define ETH_ALEN 6
 
diff --git a/src/drivers/driver.h b/src/drivers/driver.h
index bc82d28..261ed80 100644
--- a/src/drivers/driver.h
+++ b/src/drivers/driver.h
@@ -5208,6 +5208,12 @@ struct wpa_driver_ops {
 	 * @background_radar_mode: background radar mode
 	 */
 	int (*background_radar_mode)(void *priv, u8 background_radar_mode);
+	/**
+	 * pp_mode_set - Set preamble puncture operation mode
+	 * @priv: Private driver interface data
+	 * @pp_mode: Value is defined in enum pp_mode
+	 */
+	int (*pp_mode_set)(void *priv, const u8 pp_mode);
 };
 
 /**
diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index 2089ad6..3cc55dc 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -141,6 +141,11 @@ amnt_dump_policy[NUM_MTK_VENDOR_ATTRS_AMNT_DUMP] = {
 	[MTK_VENDOR_ATTR_AMNT_DUMP_RESULT] = { .type = NLA_NESTED },
 };
 
+static struct nla_policy
+pp_ctrl_policy[NUM_MTK_VENDOR_ATTRS_PP_CTRL] = {
+	[MTK_VENDOR_ATTR_PP_MODE] = { .type = NLA_U8 },
+};
+
 static struct nl_sock * nl_create_handle(struct nl_cb *cb, const char *dbg)
 {
 	struct nl_sock *handle;
@@ -14756,6 +14761,49 @@ static int nl80211_background_radar_mode(void *priv, const u8 background_radar_m
 	return ret;
 }
 
+static int nl80211_pp_mode_set(void *priv, const u8 pp_mode)
+{
+	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_pp_vendor_cmd_avail) {
+		wpa_printf(MSG_DEBUG,
+			   "nl80211: Driver does not support setting preamble puncture");
+		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_PP_CTRL))
+		goto fail;
+
+	data = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA);
+	if (!data)
+		goto fail;
+
+	nla_put_u8(msg, MTK_VENDOR_ATTR_PP_MODE, pp_mode);
+
+	nla_nest_end(msg, data);
+	ret = send_and_recv_msgs(drv, msg, NULL, NULL, NULL, NULL);
+
+	if (ret)
+		wpa_printf(MSG_ERROR, "Failed to set pp_enable. 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",
@@ -14929,4 +14977,5 @@ const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 	.amnt_set = nl80211_amnt_set,
 	.amnt_dump = nl80211_amnt_dump,
 	.background_radar_mode = nl80211_background_radar_mode,
+	.pp_mode_set = nl80211_pp_mode_set,
 };
diff --git a/src/drivers/driver_nl80211.h b/src/drivers/driver_nl80211.h
index 74ee9b1..1bba5b1 100644
--- a/src/drivers/driver_nl80211.h
+++ b/src/drivers/driver_nl80211.h
@@ -211,6 +211,7 @@ struct wpa_driver_nl80211_data {
 	unsigned int mtk_rfeatures_vendor_cmd_avail:1;
 	unsigned int mtk_amnt_vendor_cmd_avail:1;
 	unsigned int mtk_background_radar_vendor_cmd_avail:1;
+	unsigned int mtk_pp_vendor_cmd_avail:1;
 
 	u64 vendor_scan_cookie;
 	u64 remain_on_chan_cookie;
diff --git a/src/drivers/driver_nl80211_capa.c b/src/drivers/driver_nl80211_capa.c
index 90711b4..f2c42b9 100644
--- a/src/drivers/driver_nl80211_capa.c
+++ b/src/drivers/driver_nl80211_capa.c
@@ -1136,6 +1136,9 @@ static int wiphy_info_handler(struct nl_msg *msg, void *arg)
 				case MTK_NL80211_VENDOR_SUBCMD_BACKGROUND_RADAR_CTRL:
 					drv->mtk_background_radar_vendor_cmd_avail = 1;
 					break;
+				case MTK_NL80211_VENDOR_SUBCMD_PP_CTRL:
+					drv->mtk_pp_vendor_cmd_avail = 1;
+					break;
 				}
 			}
 
-- 
2.18.0