From 51377e7c81b4164be42de4a5c4c48ba53a638afe 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 072/104] mtk: hostapd: 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             | 12 +++++++
 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, 162 insertions(+)

diff --git a/hostapd/config_file.c b/hostapd/config_file.c
index 9467a1128..050ef290e 100644
--- a/hostapd/config_file.c
+++ b/hostapd/config_file.c
@@ -5333,6 +5333,8 @@ static int hostapd_config_fill(struct hostapd_config *conf,
 	} else if (os_strcmp(buf, "punct_bitmap") == 0) {
 		if (get_u16(pos, line, &conf->punct_bitmap))
 			return 1;
+		conf->punct_bitmap = atoi(pos);
+		conf->pp_mode = PP_MANUAL_MODE;
 	} else if (os_strcmp(buf, "punct_acs_threshold") == 0) {
 		int val = atoi(pos);
 
@@ -5415,6 +5417,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 9b072d1b2..c9b53c64e 100644
--- a/hostapd/ctrl_iface.c
+++ b/hostapd/ctrl_iface.c
@@ -4809,6 +4809,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,
@@ -5442,6 +5495,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 ba1b2a7a3..056c38f73 100644
--- a/src/ap/ap_config.c
+++ b/src/ap/ap_config.c
@@ -310,6 +310,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;
 
 	hostapd_set_and_check_bw320_offset(conf, 0);
 
diff --git a/src/ap/ap_config.h b/src/ap/ap_config.h
index 0b07be516..40edcdaa7 100644
--- a/src/ap/ap_config.h
+++ b/src/ap/ap_config.h
@@ -1293,6 +1293,7 @@ struct hostapd_config {
 	u8 dfs_detect_mode;
 	u8 amsdu;
 	void *muru_config;
+	u8 pp_mode;
 };
 
 enum three_wire_mode {
@@ -1345,6 +1346,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 2028e70fb..c71cfe1bd 100644
--- a/src/ap/ap_drv_ops.c
+++ b/src/ap/ap_drv_ops.c
@@ -1379,3 +1379,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 f0e618bcc..ef61001e5 100644
--- a/src/ap/ap_drv_ops.h
+++ b/src/ap/ap_drv_ops.h
@@ -169,6 +169,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_background_radar_mode(struct hostapd_data *hapd);
+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 0d4b79b48..cdbf81e38 100644
--- a/src/ap/hostapd.c
+++ b/src/ap/hostapd.c
@@ -2708,6 +2708,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 5bc1e0444..6275c141d 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 863748d4f..be0e89ba3 100644
--- a/src/drivers/driver.h
+++ b/src/drivers/driver.h
@@ -5326,6 +5326,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 e3f00b6d6..b47ab07ea 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -156,6 +156,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;
@@ -15085,6 +15090,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_cmd(drv, msg);
+
+	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",
@@ -15263,4 +15311,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 e9aae8d14..707bb7fe4 100644
--- a/src/drivers/driver_nl80211.h
+++ b/src/drivers/driver_nl80211.h
@@ -209,6 +209,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;
 
 	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 9bc98aae7..ba3c0817b 100644
--- a/src/drivers/driver_nl80211_capa.c
+++ b/src/drivers/driver_nl80211_capa.c
@@ -1167,6 +1167,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.39.2