[][MT76][Hostapd][Add NL support for Air Monitor]
[Description]
Add vendor api for air monitor in hostapd
[Release-log]
Change-Id: I81dfb0d1a1d391e29c6997ddb9634986b85e85cc
Reviewed-on: https://gerrit.mediatek.inc/c/openwrt/feeds/mtk_openwrt_feeds/+/7333744
diff --git a/autobuild_mac80211_release/package/network/services/hostapd/patches/mtk-0027-hostapd-mtk-Add-support-for-air-monitor-from-vendor-NL.patch b/autobuild_mac80211_release/package/network/services/hostapd/patches/mtk-0027-hostapd-mtk-Add-support-for-air-monitor-from-vendor-NL.patch
new file mode 100644
index 0000000..59a0bc5
--- /dev/null
+++ b/autobuild_mac80211_release/package/network/services/hostapd/patches/mtk-0027-hostapd-mtk-Add-support-for-air-monitor-from-vendor-NL.patch
@@ -0,0 +1,506 @@
+From 8496882339b6e9dc550b491693b8029fc88ac191 Mon Sep 17 00:00:00 2001
+From: mtk23888 <dipanshu.mittal@mediatek.com>
+Date: Tue, 4 Apr 2023 13:06:41 +0800
+Subject: [PATCH] Air Monitor support in hostapd by vendor NL
+
+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 f338d0f..7f91cb6 100644
+--- a/hostapd/ctrl_iface.c
++++ b/hostapd/ctrl_iface.c
+@@ -3765,6 +3765,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");
+ }
+@@ -3818,6 +3856,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,
+@@ -4392,6 +4499,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 aad7aa5..0c4a176 100644
+--- a/hostapd/hostapd_cli.c
++++ b/hostapd/hostapd_cli.c
+@@ -1613,6 +1613,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;
+@@ -1823,6 +1834,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 52d0fff..eafa588 100644
+--- a/src/ap/ap_drv_ops.c
++++ b/src/ap/ap_drv_ops.c
+@@ -1121,3 +1121,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 659c3f8..28f2a4d 100644
+--- a/src/ap/ap_drv_ops.h
++++ b/src/ap/ap_drv_ops.h
+@@ -155,6 +155,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 32438af..74f467c 100644
+--- a/src/common/mtk_vendor.h
++++ b/src/common/mtk_vendor.h
+@@ -256,10 +256,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 6a46832..1444cbe 100644
+--- a/src/drivers/driver.h
++++ b/src/drivers/driver.h
+@@ -4809,6 +4809,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 7fae013..0470079 100644
+--- a/src/drivers/driver_nl80211.c
++++ b/src/drivers/driver_nl80211.c
+@@ -125,6 +125,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;
+@@ -13196,6 +13209,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",
+@@ -13356,4 +13534,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 7a03446..7dd88e7 100644
+--- a/src/drivers/driver_nl80211.h
++++ b/src/drivers/driver_nl80211.h
+@@ -188,6 +188,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 6f4d029..5c1a35d 100644
+--- a/src/drivers/driver_nl80211_capa.c
++++ b/src/drivers/driver_nl80211_capa.c
+@@ -1094,6 +1094,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.18.0
+