From 32816f9bf56aeff95ddfe4a735028bee0d552f4d Mon Sep 17 00:00:00 2001
From: TomLiu <tomml.liu@mediatek.com>
Date: Wed, 21 Sep 2022 15:14:11 -0700
Subject: [PATCH 4/6] add hostapd AMPDU and AMSDU control command

---
 hostapd/config_file.c             |  18 ++++
 hostapd/ctrl_iface.c              |  44 ++++++++
 hostapd/hostapd_cli.c             |   9 ++
 src/ap/ap_config.c                |   2 +
 src/ap/ap_config.h                |   2 +
 src/ap/ap_drv_ops.c               |  23 ++++-
 src/ap/ap_drv_ops.h               |   3 +
 src/ap/hostapd.c                  |   4 +
 src/common/mtk_vendor.h           |  18 ++++
 src/drivers/driver.h              |  11 ++
 src/drivers/driver_nl80211.c      | 160 ++++++++++++++++++++++++++++++
 src/drivers/driver_nl80211.h      |   1 +
 src/drivers/driver_nl80211_capa.c |   3 +
 13 files changed, 297 insertions(+), 1 deletion(-)

diff --git a/hostapd/config_file.c b/hostapd/config_file.c
index 6b3b88d..9660543 100644
--- a/hostapd/config_file.c
+++ b/hostapd/config_file.c
@@ -4843,6 +4843,24 @@ static int hostapd_config_fill(struct hostapd_config *conf,
 	} else if (os_strcmp(buf, "ibf_enable") == 0) { /*ibf setting is per device*/
 		int val = atoi(pos);
 		conf->ibf_enable = !!val;
+	} else if (os_strcmp(buf, "ampdu") == 0) {
+		int val = atoi(pos);
+		if (val < 0 || val > 1) {
+			wpa_printf(MSG_ERROR,
+					 "Line %d: invalid ampdu value",
+					 line);
+			return 1;
+		}
+		conf->ampdu = val;
+	} else if (os_strcmp(buf, "amsdu") == 0) {
+		int val = atoi(pos);
+		if (val < 0 || val > 1) {
+			wpa_printf(MSG_ERROR,
+					 "Line %d: invalid amsdu value",
+					 line);
+			return 1;
+		}
+		conf->amsdu = 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 c881d37..e51488e 100644
--- a/hostapd/ctrl_iface.c
+++ b/hostapd/ctrl_iface.c
@@ -3522,6 +3522,48 @@ hostapd_ctrl_iface_get_ibf(struct hostapd_data *hapd, char *buf,
 }
 
 
+static int
+hostapd_ctrl_iface_get_aggregation(struct hostapd_data *hapd, char *buf,
+					 size_t buflen)
+{
+	u8 aggr;
+	int ret;
+	char *pos, *end;
+
+	pos = buf;
+	end = buf + buflen;
+
+	if (hostapd_drv_aggregation_dump(hapd, &aggr) == 0) {
+		if (aggr == 0) {
+				hapd->iconf->ampdu = 0;
+				hapd->iconf->amsdu = 0;
+				ret = os_snprintf(pos, end - pos, "[hostapd_cli] AMPDU: %u, AMSDU disabled\n",
+					hapd->iconf->ampdu);
+		} else if (aggr == 1) {
+				hapd->iconf->ampdu = 0;
+				hapd->iconf->amsdu = 1;
+				ret = os_snprintf(pos, end - pos, "[hostapd_cli] AMPDU: %u, AMSDU disabled\n",
+					hapd->iconf->ampdu);
+		} else if (aggr == 2) {
+				hapd->iconf->ampdu = 1;
+				hapd->iconf->amsdu = 0;
+				ret = os_snprintf(pos, end - pos, "[hostapd_cli] AMPDU: %u, AMSDU: %u\n",
+					hapd->iconf->ampdu, hapd->iconf->amsdu);
+		} else if (aggr == 3) {
+				hapd->iconf->ampdu = 1;
+				hapd->iconf->amsdu = 1;
+				ret = os_snprintf(pos, end - pos, "[hostapd_cli] AMPDU: %u, AMSDU: %u\n",
+					hapd->iconf->ampdu, hapd->iconf->amsdu);
+		}
+	}
+
+	if (os_snprintf_error(end - pos, ret))
+		return 0;
+
+	return ret;
+}
+
+
 static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd,
 					      char *buf, char *reply,
 					      int reply_size,
@@ -4081,6 +4123,8 @@ static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd,
 		reply_len = hostapd_ctrl_iface_get_hemu(hapd, reply, reply_size);
 	} else if (os_strncmp(buf, "GET_IBF", 7) == 0) {
 		reply_len = hostapd_ctrl_iface_get_ibf(hapd, reply, reply_size);
+	} else if (os_strncmp(buf, "GET_AGGR", 8) == 0) {
+		reply_len = hostapd_ctrl_iface_get_aggregation(hapd, reply, reply_size);
 	} else {
 		os_memcpy(reply, "UNKNOWN COMMAND\n", 16);
 		reply_len = 16;
diff --git a/hostapd/hostapd_cli.c b/hostapd/hostapd_cli.c
index c2a123a..9a5a6b1 100644
--- a/hostapd/hostapd_cli.c
+++ b/hostapd/hostapd_cli.c
@@ -1593,6 +1593,13 @@ static int hostapd_cli_cmd_get_ibf(struct wpa_ctrl *ctrl, int argc,
 }
 
 
+static int hostapd_cli_cmd_get_aggregation(struct wpa_ctrl *ctrl, int argc,
+					   char *argv[])
+{
+	return hostapd_cli_cmd(ctrl, "GET_AGGR", 0, NULL, NULL);
+}
+
+
 struct hostapd_cli_cmd {
 	const char *cmd;
 	int (*handler)(struct wpa_ctrl *ctrl, int argc, char *argv[]);
@@ -1796,6 +1803,8 @@ static const struct hostapd_cli_cmd hostapd_cli_commands[] = {
           "<tx type(0/1/2)> <interval> = runtime set inband discovery" },
 	{ "get_ibf", hostapd_cli_cmd_get_ibf, NULL,
 	  " = show iBF state (enabled/disabled)"},
+	{ "get_aggr", hostapd_cli_cmd_get_aggregation, NULL,
+		" = show AMPDU and AMSDU state"},
 	{ NULL, NULL, NULL, NULL }
 };
 
diff --git a/src/ap/ap_config.c b/src/ap/ap_config.c
index 7a96cb8..3ce8217 100644
--- a/src/ap/ap_config.c
+++ b/src/ap/ap_config.c
@@ -299,6 +299,8 @@ struct hostapd_config * hostapd_config_defaults(void)
 	conf->edcca_compensation = EDCCA_DEFAULT_COMPENSATION;
 	conf->three_wire_enable = THREE_WIRE_MODE_DISABLE;
 	conf->ibf_enable = IBF_DEFAULT_ENABLE;
+	conf->ampdu = 1;
+	conf->amsdu = 1;
 
 	return conf;
 }
diff --git a/src/ap/ap_config.h b/src/ap/ap_config.h
index 44a0e7e..3e73236 100644
--- a/src/ap/ap_config.h
+++ b/src/ap/ap_config.h
@@ -1159,6 +1159,8 @@ struct hostapd_config {
 	int *edcca_threshold;
 	u8 three_wire_enable;
 	u8 ibf_enable;
+	u8 ampdu;
+	u8 amsdu;
 };
 
 enum three_wire_mode {
diff --git a/src/ap/ap_drv_ops.c b/src/ap/ap_drv_ops.c
index 60ae825..5d07272 100644
--- a/src/ap/ap_drv_ops.c
+++ b/src/ap/ap_drv_ops.c
@@ -1077,4 +1077,25 @@ int hostapd_drv_ibf_dump(struct hostapd_data *hapd, u8 *ibf_enable)
 	if (!hapd->driver || !hapd->driver->ibf_dump)
 		return 0;
 	return hapd->driver->ibf_dump(hapd->drv_priv, ibf_enable);
-}
\ No newline at end of file
+}
+
+int hostapd_drv_ampdu_ctrl(struct hostapd_data *hapd)
+{
+	if (!hapd->driver || !hapd->driver->ampdu_ctrl)
+		return 0;
+	return hapd->driver->ampdu_ctrl(hapd->drv_priv, hapd->iconf->ampdu);
+}
+
+int hostapd_drv_amsdu_ctrl(struct hostapd_data *hapd)
+{
+	if (!hapd->driver || !hapd->driver->amsdu_ctrl)
+		return 0;
+	return hapd->driver->amsdu_ctrl(hapd->drv_priv, hapd->iconf->amsdu);
+}
+
+int hostapd_drv_aggregation_dump(struct hostapd_data *hapd, u8 *aggr)
+{
+	if (!hapd->driver || !hapd->driver->aggregation_dump)
+		return 0;
+	return hapd->driver->aggregation_dump(hapd->drv_priv, aggr);
+}
diff --git a/src/ap/ap_drv_ops.h b/src/ap/ap_drv_ops.h
index ab9aedc..4c1c201 100644
--- a/src/ap/ap_drv_ops.h
+++ b/src/ap/ap_drv_ops.h
@@ -147,6 +147,9 @@ int hostapd_drv_hemu_dump(struct hostapd_data *hapd, u8 *hemu_onoff);
 int hostapd_drv_three_wire_ctrl(struct hostapd_data *hapd);
 int hostapd_drv_ibf_ctrl(struct hostapd_data *hapd);
 int hostapd_drv_ibf_dump(struct hostapd_data *hapd, u8 *ibf_enable);
+int hostapd_drv_ampdu_ctrl(struct hostapd_data *hapd);
+int hostapd_drv_amsdu_ctrl(struct hostapd_data *hapd);
+int hostapd_drv_aggregation_dump(struct hostapd_data *hapd, u8 *aggr);
 
 #include "drivers/driver.h"
 
diff --git a/src/ap/hostapd.c b/src/ap/hostapd.c
index e44b73d..60bd6f3 100644
--- a/src/ap/hostapd.c
+++ b/src/ap/hostapd.c
@@ -2307,6 +2307,10 @@ dfs_offload:
 		goto fail;
 	if (hostapd_drv_ibf_ctrl(hapd) < 0)
 		goto fail;
+	if (hostapd_drv_ampdu_ctrl(hapd) < 0)
+		goto fail;
+	if (hostapd_drv_amsdu_ctrl(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 4050cf8..d78e538 100644
--- a/src/common/mtk_vendor.h
+++ b/src/common/mtk_vendor.h
@@ -177,6 +177,24 @@ enum mtk_vendor_attr_wireless_ctrl {
 		NUM_MTK_VENDOR_ATTRS_WIRELESS_CTRL - 1
 };
 
+enum mtk_vendor_attr_wireless_dump {
+	MTK_VENDOR_ATTR_WIRELESS_DUMP_UNSPEC,
+
+	MTK_VENDOR_ATTR_WIRELESS_DUMP_AMPDU,
+	MTK_VENDOR_ATTR_WIRELESS_DUMP_AMSDU,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_WIRELESS_DUMP,
+	MTK_VENDOR_ATTR_WIRELESS_DUMP_MAX =
+		NUM_MTK_VENDOR_ATTRS_WIRELESS_DUMP - 1
+};
+
+static const struct nla_policy
+wireless_dump_policy[NUM_MTK_VENDOR_ATTRS_WIRELESS_DUMP] = {
+	[MTK_VENDOR_ATTR_WIRELESS_DUMP_AMPDU] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_WIRELESS_DUMP_AMSDU] = { .type = NLA_U8 },
+};
+
 enum mtk_vendor_attr_rfeature_ctrl {
 	MTK_VENDOR_ATTR_RFEATURE_CTRL_UNSPEC,
 
diff --git a/src/drivers/driver.h b/src/drivers/driver.h
index 71ded61..57f3be6 100644
--- a/src/drivers/driver.h
+++ b/src/drivers/driver.h
@@ -4720,6 +4720,17 @@ struct wpa_driver_ops {
 	 *
 	 */
 	int (*ibf_dump)(void *priv, u8 *ibf_enable);
+
+	/**
+	 * ampdu_ctrl - enable/disable ampdu
+	 * amsdu_ctrl - enable/disable amsdu
+	 * aggregation_dump - get current ampdu and amsdu status
+	 * @priv: Private driver interface data
+	 *
+	 */
+	int (*ampdu_ctrl)(void *priv, u8 ampdu);
+	int (*amsdu_ctrl)(void *priv, u8 amsdu);
+	int (*aggregation_dump)(void *priv, u8 *aggr);
 };
 
 /**
diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index 00f9231..07dac13 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -12776,6 +12776,163 @@ fail:
 	return -ENOBUFS;
 }
 
+static int nl80211_enable_ampdu(void *priv, u8 ampdu)
+{
+	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_wireless_vendor_cmd_avail) {
+		wpa_printf(MSG_INFO,
+			   "nl80211: Driver does not support setting ap wireless 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_WIRELESS_CTRL))
+		goto fail;
+
+	data = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA);
+	if (!data)
+		goto fail;
+
+	nla_put_u8(msg, MTK_VENDOR_ATTR_WIRELESS_CTRL_AMPDU, ampdu);
+
+	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 ampdu. ret=%d (%s)", ret, strerror(-ret));
+	}
+
+	return ret;
+
+fail:
+	nlmsg_free(msg);
+	return -ENOBUFS;
+}
+
+static int nl80211_enable_amsdu(void *priv, u8 amsdu)
+{
+	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_wireless_vendor_cmd_avail) {
+		wpa_printf(MSG_INFO,
+			   "nl80211: Driver does not support setting ap wireless 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_WIRELESS_CTRL))
+		goto fail;
+
+	data = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA);
+	if (!data)
+		goto fail;
+
+	nla_put_u8(msg, MTK_VENDOR_ATTR_WIRELESS_CTRL_AMSDU, amsdu);
+
+	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 amsdu. ret=%d (%s)", ret, strerror(-ret));
+	}
+
+	return ret;
+
+fail:
+	nlmsg_free(msg);
+	return -ENOBUFS;
+}
+
+static int dump_aggregation_handler(struct nl_msg *msg, void *arg)
+{
+	u8 ampdu, amsdu;
+	u8 *aggr = (u8 *) arg;
+	struct nlattr *tb[NL80211_ATTR_MAX + 1];
+	struct nlattr *tb_vendor[MTK_VENDOR_ATTR_WIRELESS_DUMP_MAX + 1];
+	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
+	struct nlattr *nl_vend, *attr_ampdu, *attr_amsdu;
+
+	nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
+			genlmsg_attrlen(gnlh, 0), NULL);
+
+	nl_vend = tb[NL80211_ATTR_VENDOR_DATA];
+	if (!nl_vend)
+		return NL_SKIP;
+
+	nla_parse(tb_vendor, MTK_VENDOR_ATTR_WIRELESS_DUMP_MAX,
+			nla_data(nl_vend), nla_len(nl_vend), NULL);
+
+	attr_ampdu = tb_vendor[MTK_VENDOR_ATTR_WIRELESS_DUMP_AMPDU];
+	attr_amsdu = tb_vendor[MTK_VENDOR_ATTR_WIRELESS_DUMP_AMSDU];
+	if (!attr_ampdu || !attr_amsdu ){
+		wpa_printf(MSG_ERROR, "nl80211: cannot find vendor attributes");
+		return NL_SKIP;
+	}
+
+	ampdu = nla_get_u8(attr_ampdu);
+	amsdu = nla_get_u8(attr_amsdu);
+
+	*aggr = (ampdu<<1) + amsdu;
+	return NL_SKIP;
+}
+
+static int
+nl80211_dump_aggregation(void *priv, u8 *aggr)
+{
+	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_wireless_vendor_cmd_avail) {
+		wpa_printf(MSG_INFO,
+				 "nl80211: Driver does not support ap_wireless control");
+		return 0;
+	}
+
+	msg = nl80211_drv_msg(drv, NLM_F_DUMP, 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_WIRELESS_CTRL))
+		goto fail;
+
+	data = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA);
+	if (!data)
+		goto fail;
+
+	nla_nest_end(msg, data);
+
+	ret = send_and_recv_msgs(drv, msg, dump_aggregation_handler, aggr, NULL, NULL);
+
+	if (ret) {
+		wpa_printf(MSG_ERROR, "Failed to dump aggregation. 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",
@@ -12930,4 +13087,7 @@ const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 	.three_wire_ctrl = nl80211_enable_three_wire,
 	.ibf_ctrl = nl80211_ibf_enable,
 	.ibf_dump = nl80211_ibf_dump,
+	.ampdu_ctrl = nl80211_enable_ampdu,
+	.amsdu_ctrl = nl80211_enable_amsdu,
+	.aggregation_dump = nl80211_dump_aggregation,
 };
diff --git a/src/drivers/driver_nl80211.h b/src/drivers/driver_nl80211.h
index 607592c..e570224 100644
--- a/src/drivers/driver_nl80211.h
+++ b/src/drivers/driver_nl80211.h
@@ -185,6 +185,7 @@ struct wpa_driver_nl80211_data {
 	unsigned int mtk_hemu_vendor_cmd_avail:1;
 	unsigned int mtk_3wire_vendor_cmd_avail:1;
 	unsigned int mtk_ibf_vendor_cmd_avail:1;
+	unsigned int mtk_wireless_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 9ecc0ff..fcfa68b 100644
--- a/src/drivers/driver_nl80211_capa.c
+++ b/src/drivers/driver_nl80211_capa.c
@@ -1065,6 +1065,9 @@ static int wiphy_info_handler(struct nl_msg *msg, void *arg)
 				case MTK_NL80211_VENDOR_SUBCMD_IBF_CTRL:
 					drv->mtk_ibf_vendor_cmd_avail = 1;
 					break;
+				case MTK_NL80211_VENDOR_SUBCMD_WIRELESS_CTRL:
+					drv->mtk_wireless_vendor_cmd_avail = 1;
+					break;
 				}
 			}
 
-- 
2.18.0