| From 71b229a9e0a12ebc6d5f298f40042cc67fb285e1 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/32] 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 bad9790ca..997b50aad 100644 |
| --- a/hostapd/ctrl_iface.c |
| +++ b/hostapd/ctrl_iface.c |
| @@ -3756,6 +3756,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, |
| @@ -4332,6 +4402,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 0a374be8e..e9e156d28 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 a7226cfa9..9615ca8ce 100644 |
| --- a/src/ap/ap_drv_ops.c |
| +++ b/src/ap/ap_drv_ops.c |
| @@ -1213,4 +1213,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 422cb5222..136ea2349 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 ace993bc8..e27fe69b3 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 { |
| @@ -256,6 +257,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 592f506ec..91cb475ac 100644 |
| --- a/src/drivers/driver.h |
| +++ b/src/drivers/driver.h |
| @@ -5133,6 +5133,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 4ac8b6e0b..b8730472d 100644 |
| --- a/src/drivers/driver_nl80211.c |
| +++ b/src/drivers/driver_nl80211.c |
| @@ -12736,7 +12736,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) |
| { |
| @@ -14164,6 +14163,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", |
| @@ -14328,4 +14524,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 0100314ba..fd1e57cc2 100644 |
| --- a/src/drivers/driver_nl80211.h |
| +++ b/src/drivers/driver_nl80211.h |
| @@ -207,6 +207,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 f14706d77..4f8e92024 100644 |
| --- a/src/drivers/driver_nl80211_capa.c |
| +++ b/src/drivers/driver_nl80211_capa.c |
| @@ -1125,6 +1125,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.39.2 |
| |