From e3a3b2a33385a97229956f83574b1ae60f39d2cc Mon Sep 17 00:00:00 2001
From: Yi-Chia Hsieh <yi-chia.hsieh@mediatek.com>
Date: Thu, 26 Jan 2023 09:16:00 +0800
Subject: [PATCH 20/28] hostapd: mtk: Add available color bitmap

Signed-off-by: Yi-Chia Hsieh <yi-chia.hsieh@mediatek.com>
---
 hostapd/ctrl_iface.c              |  74 +++++++++++
 hostapd/hostapd_cli.c             |  18 +++
 src/ap/ap_drv_ops.c               |  10 +-
 src/ap/ap_drv_ops.h               |   2 +
 src/common/mtk_vendor.h           |  11 ++
 src/drivers/driver.h              |   8 ++
 src/drivers/driver_nl80211.c      | 199 +++++++++++++++++++++++++++++-
 src/drivers/driver_nl80211.h      |   1 +
 src/drivers/driver_nl80211_capa.c |   3 +
 9 files changed, 324 insertions(+), 2 deletions(-)

diff --git a/hostapd/ctrl_iface.c b/hostapd/ctrl_iface.c
index d09c22f..bc7a97b 100644
--- a/hostapd/ctrl_iface.c
+++ b/hostapd/ctrl_iface.c
@@ -3720,6 +3720,76 @@ hostapd_ctrl_iface_get_amsdu(struct hostapd_data *hapd, char *buf,
 	return ret;
 }
 
+static int
+hostapd_ctrl_iface_get_bss_color(struct hostapd_data *hapd, char *buf,
+		size_t buflen)
+{
+	int ret;
+	char *pos, *end;
+	int i;
+
+	pos = buf;
+	end = buf + buflen;
+
+	if (hapd->iface->conf->he_op.he_bss_color_disabled)
+		ret = os_snprintf(buf, buflen, "BSS Color disabled\n");
+	else
+		ret = os_snprintf(buf, buflen, "BSS Color=%u\n",
+				  hapd->iface->conf->he_op.he_bss_color);
+
+	pos += ret;
+
+	return pos - buf;
+}
+
+
+static int
+hostapd_ctrl_iface_get_aval_color_bmp(struct hostapd_data *hapd, char *buf,
+		size_t buflen)
+{
+	int ret;
+	char *pos, *end;
+	int i;
+	u64 aval_color_bmp = 0;
+
+	hostapd_drv_get_aval_bss_color_bmp(hapd, &aval_color_bmp);
+	hapd->color_collision_bitmap = ~aval_color_bmp;
+
+	pos = buf;
+	end = buf + buflen;
+
+	ret = os_snprintf(buf, buflen,
+			"available color bitmap=0x%llx\n",
+			aval_color_bmp);
+	if (os_snprintf_error(end - pos, ret))
+		return pos - buf;
+	pos += ret;
+
+	for (i = 0; i < HE_OPERATION_BSS_COLOR_MAX; i++) {
+		int bit = !!((aval_color_bmp >> i) & 1LLU);
+
+		if (i % 8 == 0) {
+			ret = os_snprintf(pos, end - pos, "%2d: ", i);
+			if (os_snprintf_error(end - pos, ret))
+				return pos - buf;
+			pos += ret;
+		}
+
+		ret = os_snprintf(pos, end - pos, "%d ", bit);
+		if (os_snprintf_error(end - pos, ret))
+			return pos - buf;
+		pos += ret;
+
+		if (i % 8 == 7) {
+			ret = os_snprintf(pos, end - pos, "\n");
+			if (os_snprintf_error(end - pos, ret))
+				return pos - buf;
+			pos += ret;
+		}
+	}
+	return pos - buf;
+}
+
 
 static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd,
 					      char *buf, char *reply,
@@ -4296,6 +4366,10 @@ static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd,
 		reply_len = hostapd_ctrl_iface_set_offchan_ctrl(hapd, buf + 16, reply, reply_size);
 	} else if (os_strncmp(buf, "GET_AMSDU", 9) == 0) {
 		reply_len = hostapd_ctrl_iface_get_amsdu(hapd, reply, reply_size);
+	} else if (os_strncmp(buf, "GET_BSS_COLOR", 13) == 0) {
+		reply_len = hostapd_ctrl_iface_get_bss_color(hapd, reply, reply_size);
+	} else if (os_strncmp(buf, "AVAL_COLOR_BMP", 14) == 0) {
+		reply_len = hostapd_ctrl_iface_get_aval_color_bmp(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 36d19d8..dc01ad9 100644
--- a/hostapd/hostapd_cli.c
+++ b/hostapd/hostapd_cli.c
@@ -1598,6 +1598,20 @@ static int hostapd_cli_cmd_reload_wpa_psk(struct wpa_ctrl *ctrl, int argc,
 }
 
 
+static int hostapd_cli_cmd_get_bss_color(struct wpa_ctrl *ctrl, int argc,
+					  char *argv[])
+{
+	return wpa_ctrl_command(ctrl, "GET_BSS_COLOR");
+}
+
+
+static int hostapd_cli_cmd_get_aval_color_bmp(struct wpa_ctrl *ctrl, int argc,
+					  char *argv[])
+{
+	return wpa_ctrl_command(ctrl, "AVAL_COLOR_BMP");
+}
+
+
 #ifdef ANDROID
 static int hostapd_cli_cmd_driver(struct wpa_ctrl *ctrl, int argc, char *argv[])
 {
@@ -1819,6 +1833,10 @@ static const struct hostapd_cli_cmd hostapd_cli_commands[] = {
 	  "<addr> [req_mode=] <measurement request hexdump>  = send a Beacon report request to a station" },
 	{ "reload_wpa_psk", hostapd_cli_cmd_reload_wpa_psk, NULL,
 	  "= reload wpa_psk_file only" },
+	{ "get_bss_color", hostapd_cli_cmd_get_bss_color, NULL,
+	  "= get current BSS color" },
+	{ "get_color_bmp", hostapd_cli_cmd_get_aval_color_bmp, NULL,
+	  "= get available BSS color bitmap" },
 #ifdef ANDROID
 	{ "driver", hostapd_cli_cmd_driver, NULL,
 	  "<driver sub command> [<hex formatted data>] = send driver command data" },
diff --git a/src/ap/ap_drv_ops.c b/src/ap/ap_drv_ops.c
index 00492b8..7b686e5 100644
--- a/src/ap/ap_drv_ops.c
+++ b/src/ap/ap_drv_ops.c
@@ -1130,4 +1130,12 @@ int hostapd_drv_amsdu_dump(struct hostapd_data *hapd, u8 *amsdu)
 	if (!hapd->driver || !hapd->driver->amsdu_dump)
 		return 0;
 	return hapd->driver->amsdu_dump(hapd->drv_priv, amsdu);
-}
\ No newline at end of file
+}
+
+int hostapd_drv_get_aval_bss_color_bmp(struct hostapd_data *hapd, u64 *aval_color_bmp)
+{
+	if (!hapd->driver || !hapd->driver->get_aval_color_bmp ||
+	    hapd->iface->conf->he_op.he_bss_color_disabled)
+		return 0;
+	return hapd->driver->get_aval_color_bmp(hapd->drv_priv, aval_color_bmp);
+}
diff --git a/src/ap/ap_drv_ops.h b/src/ap/ap_drv_ops.h
index 2b8b587..47356ab 100644
--- a/src/ap/ap_drv_ops.h
+++ b/src/ap/ap_drv_ops.h
@@ -155,6 +155,8 @@ int hostapd_drv_ibf_ctrl(struct hostapd_data *hapd);
 int hostapd_drv_ibf_dump(struct hostapd_data *hapd, u8 *ibf_enable);
 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);
 
 #include "drivers/driver.h"
 
diff --git a/src/common/mtk_vendor.h b/src/common/mtk_vendor.h
index 62d79cb..a99e6f2 100644
--- a/src/common/mtk_vendor.h
+++ b/src/common/mtk_vendor.h
@@ -15,6 +15,7 @@ enum mtk_nl80211_vendor_subcmds {
 	MTK_NL80211_VENDOR_SUBCMD_EDCCA_CTRL = 0xc7,
 	MTK_NL80211_VENDOR_SUBCMD_3WIRE_CTRL = 0xc8,
 	MTK_NL80211_VENDOR_SUBCMD_IBF_CTRL = 0xc9,
+	MTK_NL80211_VENDOR_SUBCMD_BSS_COLOR_CTRL = 0xca,
 };
 
 enum mtk_vendor_attr_edcca_ctrl {
@@ -253,6 +254,16 @@ ibf_dump_policy[NUM_MTK_VENDOR_ATTRS_IBF_DUMP] = {
 	[MTK_VENDOR_ATTR_IBF_DUMP_ENABLE] = { .type = NLA_U8 },
 };
 
+enum mtk_vendor_attr_bss_color_ctrl {
+	MTK_VENDOR_ATTR_BSS_COLOR_CTRL_UNSPEC,
+
+	MTK_VENDOR_ATTR_AVAL_BSS_COLOR_BMP,
+
+	/* keep last */
+	NUM_MTK_VENDOR_ATTRS_BSS_COLOR_CTRL,
+	MTK_VENDOR_ATTR_BSS_COLOR_CTRL_MAX =
+		NUM_MTK_VENDOR_ATTRS_BSS_COLOR_CTRL - 1
+};
 
 #define CSI_MAX_COUNT 256
 #define ETH_ALEN 6
diff --git a/src/drivers/driver.h b/src/drivers/driver.h
index faa4b7b..f9f9fb5 100644
--- a/src/drivers/driver.h
+++ b/src/drivers/driver.h
@@ -5054,6 +5054,14 @@ struct wpa_driver_ops {
 	 */
 	int (*amsdu_ctrl)(void *priv, u8 amsdu);
 	int (*amsdu_dump)(void *priv, u8 *amsdu);
+
+	/**
+	 * get_aval_color_bmp - get available BSS color bitmap
+	 * @priv: Private driver interface data
+	 * @aval_color_bmp: available bss color bitmap
+	 *
+	 */
+	int (*get_aval_color_bmp)(void *priv, u64 *aval_color_bmp);
 };
 
 /**
diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index 7c20b14..18a5647 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -12498,7 +12498,6 @@ static void nl80211_parse_btm_candidate_info(struct candidate_list *candidate,
 		   num, MAC2STR(candidate->bssid), buf);
 }
 
-
 static int
 nl80211_get_bss_transition_status_handler(struct nl_msg *msg, void *arg)
 {
@@ -13886,6 +13885,203 @@ fail:
 	return -ENOBUFS;
 }
 
+static int nl80211_get_aval_color_bmp_handler(struct nl_msg *msg, void *arg)
+{
+	u64 *aval_color_bmp = arg;
+	struct nlattr *tb[NL80211_ATTR_MAX + 1];
+	struct nlattr *tb_vendor[MTK_VENDOR_ATTR_BSS_COLOR_CTRL_MAX + 1];
+	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
+	struct nlattr *nl_vend, *attr;
+
+	static const struct nla_policy
+	bss_color_ctrl_policy[NUM_MTK_VENDOR_ATTRS_BSS_COLOR_CTRL + 1] = {
+		[MTK_VENDOR_ATTR_AVAL_BSS_COLOR_BMP] = { .type = NLA_U64 },
+	};
+
+	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_BSS_COLOR_CTRL_MAX,
+			nla_data(nl_vend), nla_len(nl_vend), NULL);
+
+	*aval_color_bmp = nla_get_u64(tb_vendor[MTK_VENDOR_ATTR_AVAL_BSS_COLOR_BMP]);
+
+	return 0;
+}
+
+static int nl80211_get_aval_color_bmp(void *priv, u64 *aval_color_bmp)
+{
+	struct i802_bss *bss = priv;
+	struct wpa_driver_nl80211_data *drv = bss->drv;
+	struct nl_msg *msg;
+	struct nlattr *attr;
+	int ret;
+
+	if (!drv->mtk_bss_color_vendor_cmd_avail) {
+		wpa_printf(MSG_INFO,
+			   "nl80211: Driver does not support BSS COLOR vendor cmd");
+		return 0;
+	}
+
+	if (!(msg = nl80211_drv_msg(drv, NLM_F_DUMP, NL80211_CMD_VENDOR)) ||
+	    nla_put_u32(msg, NL80211_ATTR_VENDOR_ID, OUI_MTK) ||
+	    nla_put_u32(msg, NL80211_ATTR_VENDOR_SUBCMD,
+			MTK_NL80211_VENDOR_SUBCMD_BSS_COLOR_CTRL))
+		return -ENOBUFS;
+
+	attr = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA);
+	if (!attr) {
+		nlmsg_free(msg);
+		return -1;
+	}
+
+	nla_nest_end(msg, attr);
+
+	ret = send_and_recv_msgs(drv, msg,
+		nl80211_get_aval_color_bmp_handler, aval_color_bmp, NULL, NULL);
+
+	if (ret) {
+		wpa_printf(MSG_ERROR, "Failed to send BSS COLOR vendor cmd. ret=%d (%s) ",
+			   ret, strerror(-ret));
+	}
+	return ret;
+}
+
+static int nl80211_ap_wireless(void *priv, u8 sub_vendor_id, int value)
+{
+	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;
+
+	if (sub_vendor_id == MTK_VENDOR_ATTR_WIRELESS_CTRL_BA_BUFFER_SIZE)
+		nla_put_u16(msg, sub_vendor_id, (u16) value);
+	else
+		nla_put_u8(msg, sub_vendor_id, (u8) value);
+
+	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 ap_wireless. ret=%d (%s)", ret, strerror(-ret));
+
+	return ret;
+
+fail:
+	nlmsg_free(msg);
+	return -ENOBUFS;
+}
+
+static int nl80211_ap_rfeatures(void *priv, u8 sub_vendor_id, int value)
+{
+	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_rfeatures_vendor_cmd_avail) {
+		wpa_printf(MSG_INFO,
+			   "nl80211: Driver does not support setting ap rfeatures 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_RFEATURE_CTRL))
+		goto fail;
+
+	data = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA);
+	if (!data)
+		goto fail;
+
+	nla_put_u8(msg, sub_vendor_id, (u8) value);
+
+	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 rf_features. ret=%d (%s)", ret, strerror(-ret));
+
+	return ret;
+
+fail:
+	nlmsg_free(msg);
+	return -ENOBUFS;
+}
+
+static int nl80211_ap_trigtype(void *priv, u8 enable, u8 type)
+{
+	struct i802_bss *bss = priv;
+	struct wpa_driver_nl80211_data *drv = bss->drv;
+	struct nl_msg *msg;
+	struct nlattr *data, *data2;
+	int ret;
+
+	if (!drv->mtk_rfeatures_vendor_cmd_avail) {
+		wpa_printf(MSG_INFO,
+			   "nl80211: Driver does not support setting ap rfeatures 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_RFEATURE_CTRL))
+		goto fail;
+
+	data = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA);
+	if (!data)
+		goto fail;
+
+	data2 = nla_nest_start(msg, MTK_VENDOR_ATTR_RFEATURE_CTRL_TRIG_TYPE_CFG);
+	if (!data2)
+		goto fail;
+
+	nla_put_u8(msg, MTK_VENDOR_ATTR_RFEATURE_CTRL_TRIG_TYPE_EN, enable);
+	nla_put_u8(msg, MTK_VENDOR_ATTR_RFEATURE_CTRL_TRIG_TYPE, type);
+
+	nla_nest_end(msg, data2);
+	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 trig_type. 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",
@@ -14049,4 +14245,5 @@ const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 	.ibf_dump = nl80211_ibf_dump,
 	.amsdu_ctrl = nl80211_enable_amsdu,
 	.amsdu_dump = nl80211_dump_amsdu,
+	.get_aval_color_bmp = nl80211_get_aval_color_bmp,
 };
diff --git a/src/drivers/driver_nl80211.h b/src/drivers/driver_nl80211.h
index 3bcc770..190e7a5 100644
--- a/src/drivers/driver_nl80211.h
+++ b/src/drivers/driver_nl80211.h
@@ -204,6 +204,7 @@ struct wpa_driver_nl80211_data {
 	unsigned int mtk_3wire_vendor_cmd_avail:1;
 	unsigned int mtk_ibf_vendor_cmd_avail:1;
 	unsigned int mtk_wireless_vendor_cmd_avail:1;
+	unsigned int mtk_bss_color_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 dee99f0..a266926 100644
--- a/src/drivers/driver_nl80211_capa.c
+++ b/src/drivers/driver_nl80211_capa.c
@@ -1117,6 +1117,9 @@ static int wiphy_info_handler(struct nl_msg *msg, void *arg)
 				case MTK_NL80211_VENDOR_SUBCMD_WIRELESS_CTRL:
 					drv->mtk_wireless_vendor_cmd_avail = 1;
 					break;
+				case MTK_NL80211_VENDOR_SUBCMD_BSS_COLOR_CTRL :
+					drv->mtk_bss_color_vendor_cmd_avail = 1;
+					break;
 				}
 			}
 
-- 
2.18.0