From 06f750bee3e0a9798aa583b27257ef26dd4a1a8c Mon Sep 17 00:00:00 2001
From: Evelyn Tsai <evelyn.tsai@mediatek.com>
Date: Fri, 12 May 2023 05:18:48 +0800
Subject: [PATCH 24/32] hostapd: mtk: Air Monitor support in hostapd by vendor

Signed-off-by: mtk23888 <dipanshu.mittal@mediatek.com>
---
 hostapd/ctrl_iface.c              | 113 +++++++++++++++++++
 hostapd/hostapd_cli.c             |  15 +++
 src/ap/ap_drv_ops.c               |  14 +++
 src/ap/ap_drv_ops.h               |   3 +
 src/common/mtk_vendor.h           |   8 ++
 src/drivers/driver.h              |  16 +++
 src/drivers/driver_nl80211.c      | 180 ++++++++++++++++++++++++++++++
 src/drivers/driver_nl80211.h      |   1 +
 src/drivers/driver_nl80211_capa.c |   2 +
 9 files changed, 352 insertions(+)

diff --git a/hostapd/ctrl_iface.c b/hostapd/ctrl_iface.c
index e0ed700db..9989a04ce 100644
--- a/hostapd/ctrl_iface.c
+++ b/hostapd/ctrl_iface.c
@@ -3876,6 +3876,44 @@ hostapd_ctrl_iface_ap_wireless(struct hostapd_data *hapd, char *cmd,
 
 	if (hostapd_drv_ap_wireless(hapd, (u8) sub_cmd, atoi(value)) != 0)
 		return -1;
+	return os_snprintf(buf, buflen, "OK\n");
+}
+
+static int
+hostapd_ctrl_iface_set_amnt(struct hostapd_data *hapd, char *cmd,
+					char *buf, size_t buflen)
+{
+	char *tmp, sta_mac[ETH_ALEN] = {0};
+	int amnt_idx = 0;
+
+	tmp = strtok_r(cmd, " ", &cmd);
+
+	if (!tmp) {
+		wpa_printf(MSG_ERROR, "Error in command format\n");
+		return -1;
+	}
+
+	amnt_idx = strtol(tmp, &tmp, 10);
+
+	if (amnt_idx < 0 || amnt_idx > 15) {
+		wpa_printf(MSG_ERROR, "Wrong AMNT index %d\n", amnt_idx);
+		return -1;
+	}
+
+	if (!cmd) {
+		wpa_printf(MSG_ERROR, "Error in command format\n");
+		return -1;
+	}
+
+	if (hwaddr_aton(cmd, sta_mac) < 0) {
+		wpa_printf(MSG_ERROR, "station mac is not right.\n");
+		return -1;
+	}
+
+	if (hostapd_drv_amnt_set(hapd, amnt_idx, sta_mac)) {
+		wpa_printf(MSG_ERROR, "Not able to set amnt index\n");
+		return -1;
+	}
 
 	return os_snprintf(buf, buflen, "OK\n");
 }
@@ -3929,6 +3967,75 @@ exit:
 	return os_snprintf(buf, buflen, "OK\n");
 }
 
+static int
+hostapd_ctrl_iface_dump_amnt(struct hostapd_data *hapd, char *cmd,
+				char *buf, size_t buflen)
+{
+	char *tmp;
+	int amnt_idx = 0, ret = 0;
+	struct amnt_resp_data *resp_buf;
+	char *pos, *end;
+	struct amnt_data *res;
+
+	pos = buf;
+	end = buf + buflen;
+
+	tmp = strtok_r(cmd, " ", &cmd);
+
+	if (!tmp) {
+		wpa_printf(MSG_ERROR, "Error in command format\n");
+		return -1;
+	}
+
+	amnt_idx = strtoul(tmp, &tmp, 0);
+
+	if ((amnt_idx < 0 || amnt_idx > 15) && amnt_idx != 0xff) {
+		wpa_printf(MSG_ERROR, "Wrong AMNT index\n");
+		return -1;
+	}
+
+	if (amnt_idx == 0xff)
+		resp_buf = (struct amnt_resp_data *) os_zalloc(AIR_MONITOR_MAX_ENTRY
+							* sizeof(struct amnt_data) + 1);
+	else
+		resp_buf = (struct amnt_resp_data *) os_zalloc(sizeof(struct amnt_data) + 1);
+
+	if (resp_buf == NULL) {
+		wpa_printf(MSG_ERROR, "Error in memory allocation\n");
+		return -1;
+	}
+
+	if (hostapd_drv_amnt_dump(hapd, amnt_idx, (u8 *)resp_buf)) {
+		wpa_printf(MSG_ERROR, "Not able to set amnt index\n");
+		os_free(resp_buf);
+		return -1;
+	}
+
+	for (int i = 0; i < resp_buf->sta_num && i < AIR_MONITOR_MAX_ENTRY; i++) {
+		res = &resp_buf->resp_data[i];
+		ret = os_snprintf(pos, end - pos,
+				"[hostapd_cli] amnt_idx: %d, addr="MACSTR
+				", rssi=%d/%d/%d/%d, last_seen=%u\n",
+				res->idx,
+				MAC2STR(res->addr), res->rssi[0],
+				res->rssi[1], res->rssi[2],
+				res->rssi[3], res->last_seen);
+		if (os_snprintf_error(end - pos, ret)) {
+			os_free(resp_buf);
+			return 0;
+		}
+		pos = pos + ret;
+	}
+
+	os_free(resp_buf);
+
+	if (pos == buf)
+		return os_snprintf(buf, buflen, "Index %d is not monitored\n",
+				amnt_idx);
+	else
+		return pos - buf;
+}
+
 static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd,
 					      char *buf, char *reply,
 					      int reply_size,
@@ -4512,6 +4619,12 @@ static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd,
 		reply_len = hostapd_ctrl_iface_ap_wireless(hapd, buf + 12, reply, reply_size);
 	} else if (os_strncmp(buf, "ap_rfeatures ", 13) == 0) {
 		reply_len = hostapd_ctrl_iface_ap_rfeatures(hapd, buf + 13, reply, reply_size);
+	} else if (os_strncmp(buf, "SET_AMNT", 8) == 0) {
+		reply_len = hostapd_ctrl_iface_set_amnt(hapd, buf+9,
+							reply, reply_size);
+	} else if (os_strncmp(buf, "DUMP_AMNT", 9) == 0) {
+		reply_len = hostapd_ctrl_iface_dump_amnt(hapd, buf+10,
+							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 e9e156d28..6d763f327 100644
--- a/hostapd/hostapd_cli.c
+++ b/hostapd/hostapd_cli.c
@@ -1633,6 +1633,17 @@ static int hostapd_cli_cmd_get_amsdu(struct wpa_ctrl *ctrl, int argc,
 	return hostapd_cli_cmd(ctrl, "GET_AMSDU", 0, NULL, NULL);
 }
 
+static int hostapd_cli_cmd_set_amnt(struct wpa_ctrl *ctrl, int argc,
+					   char *argv[])
+{
+	return hostapd_cli_cmd(ctrl, "SET_AMNT", 2, argc, argv);
+}
+
+static int hostapd_cli_cmd_dump_amnt(struct wpa_ctrl *ctrl, int argc,
+					   char *argv[])
+{
+	return hostapd_cli_cmd(ctrl, "DUMP_AMNT", 1, argc, argv);
+}
 
 struct hostapd_cli_cmd {
 	const char *cmd;
@@ -1847,6 +1858,10 @@ static const struct hostapd_cli_cmd hostapd_cli_commands[] = {
 	  " = show iBF state (enabled/disabled)"},
 	{ "get_amsdu", hostapd_cli_cmd_get_amsdu, NULL,
 		" = show AMSDU state"},
+	{ "set_amnt", hostapd_cli_cmd_set_amnt, NULL,
+		" = Set Station index and mac to monitor"},
+	{ "dump_amnt", hostapd_cli_cmd_dump_amnt, NULL,
+		" = Dump RSSI of monitoring Station"},
 	{ NULL, NULL, NULL, NULL }
 };
 
diff --git a/src/ap/ap_drv_ops.c b/src/ap/ap_drv_ops.c
index 11444c7eb..b90dd5722 100644
--- a/src/ap/ap_drv_ops.c
+++ b/src/ap/ap_drv_ops.c
@@ -1243,3 +1243,17 @@ int hostapd_drv_ap_trig_type(struct hostapd_data *hapd, u8 enable, u8 type)
 		return 0;
 	return hapd->driver->ap_trigtype(hapd->drv_priv, enable, type);
 }
+
+int hostapd_drv_amnt_set(struct hostapd_data *hapd, u8 amnt_idx, u8 *amnt_sta_mac)
+{
+	if (!hapd->driver || !hapd->driver->amnt_set)
+		return 0;
+	return hapd->driver->amnt_set(hapd->drv_priv, amnt_idx, amnt_sta_mac);
+}
+
+int hostapd_drv_amnt_dump(struct hostapd_data *hapd, u8 amnt_idx, u8 *amnt_dump_buf)
+{
+	if (!hapd->driver || !hapd->driver->amnt_dump)
+		return 0;
+	return hapd->driver->amnt_dump(hapd->drv_priv, amnt_idx, amnt_dump_buf);
+}
diff --git a/src/ap/ap_drv_ops.h b/src/ap/ap_drv_ops.h
index 946ee2d68..1e2a92acb 100644
--- a/src/ap/ap_drv_ops.h
+++ b/src/ap/ap_drv_ops.h
@@ -161,6 +161,9 @@ int hostapd_drv_ap_wireless(struct hostapd_data *hapd, u8 sub_vendor_id, int val
 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);
 
+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);
+
 #include "drivers/driver.h"
 
 int hostapd_drv_wnm_oper(struct hostapd_data *hapd,
diff --git a/src/common/mtk_vendor.h b/src/common/mtk_vendor.h
index 0b23c76ad..dd1ca2164 100644
--- a/src/common/mtk_vendor.h
+++ b/src/common/mtk_vendor.h
@@ -258,10 +258,18 @@ struct csi_data {
 	u16 rx_idx;
 };
 
+#define AIR_MONITOR_MAX_ENTRY 16
+
 struct amnt_data {
 	u8 idx;
 	u8 addr[ETH_ALEN];
 	s8 rssi[4];
 	u32 last_seen;
 };
+
+struct amnt_resp_data {
+	u8 sta_num;
+	struct amnt_data resp_data[0];
+};
+
 #endif /* MTK_VENDOR_H */
diff --git a/src/drivers/driver.h b/src/drivers/driver.h
index 3efea9f68..5c70ec78f 100644
--- a/src/drivers/driver.h
+++ b/src/drivers/driver.h
@@ -5163,6 +5163,22 @@ struct wpa_driver_ops {
 	* @type: trigger type
 	*/
 	int (*ap_trigtype)(void *priv, u8 enable, u8 type);
+
+	/**
+	* amnt_set - add/delete station from monitoring
+	* @priv: Private driver interface data
+	* @amnt_idx: Monitor Index
+	* @amnt_sta_mac: station mac address
+	*/
+	int (*amnt_set)(void *priv, u8 amnt_idx, u8 *amnt_sta_mac);
+
+	/**
+	* amnt_dump - Dump particular/ all station
+	* @priv: Private driver interface data
+	* @amnt_idx: Monitor Index
+	* @amnt_dump_buf: Buffer to print
+	*/
+	int (*amnt_dump)(void *priv, u8 amnt_idx, u8 *amnt_dump_buf);
 };
 
 /**
diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c
index 642d75bd1..b923e5c7e 100644
--- a/src/drivers/driver_nl80211.c
+++ b/src/drivers/driver_nl80211.c
@@ -128,6 +128,19 @@ wireless_ctrl_policy[NUM_MTK_VENDOR_ATTRS_WIRELESS_CTRL] = {
 	[MTK_VENDOR_ATTR_WIRELESS_CTRL_CERT] = {.type = NLA_U8 },
 };
 
+static struct nla_policy
+amnt_ctrl_policy[NUM_MTK_VENDOR_ATTRS_AMNT_CTRL] = {
+	[MTK_VENDOR_ATTR_AMNT_CTRL_SET] = {.type = NLA_NESTED },
+	[MTK_VENDOR_ATTR_AMNT_CTRL_DUMP] = { .type = NLA_NESTED },
+};
+
+static struct nla_policy
+amnt_dump_policy[NUM_MTK_VENDOR_ATTRS_AMNT_DUMP] = {
+	[MTK_VENDOR_ATTR_AMNT_DUMP_INDEX] = {.type = NLA_U8 },
+	[MTK_VENDOR_ATTR_AMNT_DUMP_LEN] = { .type = NLA_U8 },
+	[MTK_VENDOR_ATTR_AMNT_DUMP_RESULT] = { .type = NLA_NESTED },
+};
+
 static struct nl_sock * nl_create_handle(struct nl_cb *cb, const char *dbg)
 {
 	struct nl_sock *handle;
@@ -14412,6 +14425,171 @@ fail:
 	return -ENOBUFS;
 }
 
+static int
+nl80211_amnt_set(void *priv, u8 amnt_idx, u8 *amnt_sta_mac)
+{
+	struct i802_bss *bss = priv;
+	struct wpa_driver_nl80211_data *drv = bss->drv;
+	struct nl_msg *msg;
+	struct nlattr *data;
+	void *tb1;
+	int ret;
+
+	if (!drv->mtk_amnt_vendor_cmd_avail) {
+		wpa_printf(MSG_ERROR,
+			"nl80211: Driver does not support air monitor");
+		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_AMNT_CTRL))
+		goto fail;
+
+	data = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA | NLA_F_NESTED);
+	if (!data)
+		goto fail;
+
+	tb1 = nla_nest_start(msg, MTK_VENDOR_ATTR_AMNT_CTRL_SET);
+	if (!tb1)
+		goto fail;
+
+	nla_put_u8(msg, MTK_VENDOR_ATTR_AMNT_SET_INDEX, amnt_idx);
+
+	nla_put(msg, MTK_VENDOR_ATTR_AMNT_SET_MACADDR, ETH_ALEN, amnt_sta_mac);
+
+	nla_nest_end(msg, tb1);
+	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 air monitor. ret=%d (%s)",
+			ret, strerror(-ret));
+
+	return ret;
+
+fail:
+	nlmsg_free(msg);
+	return -ENOBUFS;
+
+}
+
+static int
+mt76_amnt_dump_cb(struct nl_msg *msg, void *arg)
+{
+	struct nlattr *tb[NL80211_ATTR_MAX + 1];
+	struct nlattr *tb1[NUM_MTK_VENDOR_ATTRS_AMNT_CTRL];
+	struct nlattr *tb2[NUM_MTK_VENDOR_ATTRS_AMNT_DUMP];
+	struct nlattr *attr, *cur, *data;
+	struct amnt_data *res;
+	int len = 0, rem;
+	struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
+	struct amnt_resp_data *amnt_dump = (struct amnt_resp_data *)arg;
+
+	nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
+		genlmsg_attrlen(gnlh, 0), NULL);
+
+	attr = tb[NL80211_ATTR_VENDOR_DATA];
+	if (!attr)
+		return NL_SKIP;
+
+	nla_parse_nested(tb1, MTK_VENDOR_ATTR_AMNT_CTRL_MAX,
+			attr, amnt_ctrl_policy);
+
+	if (!tb1[MTK_VENDOR_ATTR_AMNT_CTRL_DUMP])
+		return NL_SKIP;
+
+	nla_parse_nested(tb2, NUM_MTK_VENDOR_ATTRS_AMNT_DUMP,
+			tb1[MTK_VENDOR_ATTR_AMNT_CTRL_DUMP], amnt_dump_policy);
+
+	if (!tb2[MTK_VENDOR_ATTR_AMNT_DUMP_LEN])
+		return NL_SKIP;
+
+	len = nla_get_u8(tb2[MTK_VENDOR_ATTR_AMNT_DUMP_LEN]);
+	if (!len)
+		return 0;
+
+	if (!tb2[MTK_VENDOR_ATTR_AMNT_DUMP_RESULT])
+		return NL_SKIP;
+
+	data = tb2[MTK_VENDOR_ATTR_AMNT_DUMP_RESULT];
+
+	nla_for_each_nested(cur, data, rem) {
+		if (amnt_dump->sta_num >= AIR_MONITOR_MAX_ENTRY)
+			return NL_SKIP;
+		res = (struct amnt_data *) nla_data(cur);
+		wpa_printf(MSG_ERROR, "[vendor] amnt_idx: %d, "
+			"addr="MACSTR", "
+			"rssi=%d/%d/%d/%d, last_seen=%u\n",
+			res->idx,
+			MAC2STR(res->addr),
+			res->rssi[0], res->rssi[1], res->rssi[2],
+			res->rssi[3], res->last_seen);
+		os_memcpy(&amnt_dump->resp_data[amnt_dump->sta_num], res,
+			sizeof(struct amnt_data));
+		amnt_dump->sta_num++;
+	}
+	return 0;
+}
+
+static int
+nl80211_amnt_dump(void *priv, u8 amnt_idx, u8 *dump_buf)
+{
+	struct i802_bss *bss = priv;
+	struct wpa_driver_nl80211_data *drv = bss->drv;
+	struct nl_msg *msg;
+	struct nlattr *data;
+	void *tb1;
+	int ret;
+
+	if (!drv->mtk_amnt_vendor_cmd_avail) {
+		wpa_printf(MSG_INFO,
+			"nl80211: Driver does not support air monitor");
+		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_AMNT_CTRL))
+		goto fail;
+
+	data = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA | NLA_F_NESTED);
+	if (!data)
+		goto fail;
+
+	tb1 = nla_nest_start(msg, MTK_VENDOR_ATTR_AMNT_CTRL_DUMP
+			| NLA_F_NESTED);
+	if (!tb1)
+		goto fail;
+
+	nla_put_u8(msg, MTK_VENDOR_ATTR_AMNT_DUMP_INDEX, amnt_idx);
+
+	nla_nest_end(msg, tb1);
+	nla_nest_end(msg, data);
+
+	ret = send_and_recv_msgs(drv, msg, mt76_amnt_dump_cb,
+				dump_buf, NULL, NULL);
+
+	if (ret)
+		wpa_printf(MSG_ERROR, "Failed to Dump air monitor. 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",
@@ -14580,4 +14758,6 @@ const struct wpa_driver_ops wpa_driver_nl80211_ops = {
 	.ap_wireless = nl80211_ap_wireless,
 	.ap_rfeatures = nl80211_ap_rfeatures,
 	.ap_trigtype = nl80211_ap_trigtype,
+	.amnt_set = nl80211_amnt_set,
+	.amnt_dump = nl80211_amnt_dump,
 };
diff --git a/src/drivers/driver_nl80211.h b/src/drivers/driver_nl80211.h
index fc5217d61..0d85adfee 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_wireless_vendor_cmd_avail:1;
 	unsigned int mtk_bss_color_vendor_cmd_avail:1;
 	unsigned int mtk_rfeatures_vendor_cmd_avail:1;
+	unsigned int mtk_amnt_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 c1cf5b533..8c8b84ed3 100644
--- a/src/drivers/driver_nl80211_capa.c
+++ b/src/drivers/driver_nl80211_capa.c
@@ -1127,6 +1127,8 @@ static int wiphy_info_handler(struct nl_msg *msg, void *arg)
 					break;
 				case MTK_NL80211_VENDOR_SUBCMD_BSS_COLOR_CTRL :
 					drv->mtk_bss_color_vendor_cmd_avail = 1;
+				case MTK_NL80211_VENDOR_SUBCMD_AMNT_CTRL:
+					drv->mtk_amnt_vendor_cmd_avail = 1;
 					break;
 				case MTK_NL80211_VENDOR_SUBCMD_RFEATURE_CTRL:
 					drv->mtk_rfeatures_vendor_cmd_avail = 1;
-- 
2.39.2