developer | 2fd555a | 2023-07-25 05:48:15 +0800 | [diff] [blame] | 1 | From 9b40b6ccf6fd2f4463b389df46d38610001df04e Mon Sep 17 00:00:00 2001 |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 2 | From: Yi-Chia Hsieh <yi-chia.hsieh@mediatek.com> |
| 3 | Date: Thu, 26 Jan 2023 09:16:00 +0800 |
developer | 2fd555a | 2023-07-25 05:48:15 +0800 | [diff] [blame] | 4 | Subject: [PATCH 22/35] hostapd: mtk: Add available color bitmap |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 5 | |
| 6 | Signed-off-by: Yi-Chia Hsieh <yi-chia.hsieh@mediatek.com> |
| 7 | --- |
| 8 | hostapd/ctrl_iface.c | 74 +++++++++++++++++++++++++++++++ |
| 9 | hostapd/hostapd_cli.c | 18 ++++++++ |
| 10 | src/ap/ap_drv_ops.c | 10 ++++- |
| 11 | src/ap/ap_drv_ops.h | 2 + |
| 12 | src/common/mtk_vendor.h | 11 +++++ |
| 13 | src/drivers/driver.h | 8 ++++ |
| 14 | src/drivers/driver_nl80211.c | 66 +++++++++++++++++++++++++++ |
| 15 | src/drivers/driver_nl80211.h | 1 + |
| 16 | src/drivers/driver_nl80211_capa.c | 3 ++ |
| 17 | 9 files changed, 192 insertions(+), 1 deletion(-) |
| 18 | |
| 19 | diff --git a/hostapd/ctrl_iface.c b/hostapd/ctrl_iface.c |
developer | 2fd555a | 2023-07-25 05:48:15 +0800 | [diff] [blame] | 20 | index 9ba6992..e1e9270 100644 |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 21 | --- a/hostapd/ctrl_iface.c |
| 22 | +++ b/hostapd/ctrl_iface.c |
developer | 2fd555a | 2023-07-25 05:48:15 +0800 | [diff] [blame] | 23 | @@ -3645,6 +3645,76 @@ hostapd_ctrl_iface_get_amsdu(struct hostapd_data *hapd, char *buf, |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 24 | return ret; |
| 25 | } |
| 26 | |
| 27 | +static int |
| 28 | +hostapd_ctrl_iface_get_bss_color(struct hostapd_data *hapd, char *buf, |
| 29 | + size_t buflen) |
| 30 | +{ |
| 31 | + int ret; |
| 32 | + char *pos, *end; |
| 33 | + int i; |
| 34 | + |
| 35 | + pos = buf; |
| 36 | + end = buf + buflen; |
| 37 | + |
| 38 | + if (hapd->iface->conf->he_op.he_bss_color_disabled) |
| 39 | + ret = os_snprintf(buf, buflen, "BSS Color disabled\n"); |
| 40 | + else |
| 41 | + ret = os_snprintf(buf, buflen, "BSS Color=%u\n", |
| 42 | + hapd->iface->conf->he_op.he_bss_color); |
| 43 | + |
| 44 | + pos += ret; |
| 45 | + |
| 46 | + return pos - buf; |
| 47 | +} |
| 48 | + |
| 49 | + |
| 50 | +static int |
| 51 | +hostapd_ctrl_iface_get_aval_color_bmp(struct hostapd_data *hapd, char *buf, |
| 52 | + size_t buflen) |
| 53 | +{ |
| 54 | + int ret; |
| 55 | + char *pos, *end; |
| 56 | + int i; |
| 57 | + u64 aval_color_bmp = 0; |
| 58 | + |
| 59 | + hostapd_drv_get_aval_bss_color_bmp(hapd, &aval_color_bmp); |
| 60 | + hapd->color_collision_bitmap = ~aval_color_bmp; |
| 61 | + |
| 62 | + pos = buf; |
| 63 | + end = buf + buflen; |
| 64 | + |
| 65 | + ret = os_snprintf(buf, buflen, |
| 66 | + "available color bitmap=0x%llx\n", |
| 67 | + aval_color_bmp); |
| 68 | + if (os_snprintf_error(end - pos, ret)) |
| 69 | + return pos - buf; |
| 70 | + pos += ret; |
| 71 | + |
| 72 | + for (i = 0; i < HE_OPERATION_BSS_COLOR_MAX; i++) { |
| 73 | + int bit = !!((aval_color_bmp >> i) & 1LLU); |
| 74 | + |
| 75 | + if (i % 8 == 0) { |
| 76 | + ret = os_snprintf(pos, end - pos, "%2d: ", i); |
| 77 | + if (os_snprintf_error(end - pos, ret)) |
| 78 | + return pos - buf; |
| 79 | + pos += ret; |
| 80 | + } |
| 81 | + |
| 82 | + ret = os_snprintf(pos, end - pos, "%d ", bit); |
| 83 | + if (os_snprintf_error(end - pos, ret)) |
| 84 | + return pos - buf; |
| 85 | + pos += ret; |
| 86 | + |
| 87 | + if (i % 8 == 7) { |
| 88 | + ret = os_snprintf(pos, end - pos, "\n"); |
| 89 | + if (os_snprintf_error(end - pos, ret)) |
| 90 | + return pos - buf; |
| 91 | + pos += ret; |
| 92 | + } |
| 93 | + } |
| 94 | + return pos - buf; |
| 95 | +} |
| 96 | + |
| 97 | |
| 98 | static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd, |
| 99 | char *buf, char *reply, |
developer | 2fd555a | 2023-07-25 05:48:15 +0800 | [diff] [blame] | 100 | @@ -4212,6 +4282,10 @@ static int hostapd_ctrl_iface_receive_process(struct hostapd_data *hapd, |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 101 | reply_len = hostapd_ctrl_iface_set_offchan_ctrl(hapd, buf + 16, reply, reply_size); |
| 102 | } else if (os_strncmp(buf, "GET_AMSDU", 9) == 0) { |
| 103 | reply_len = hostapd_ctrl_iface_get_amsdu(hapd, reply, reply_size); |
| 104 | + } else if (os_strncmp(buf, "GET_BSS_COLOR", 13) == 0) { |
| 105 | + reply_len = hostapd_ctrl_iface_get_bss_color(hapd, reply, reply_size); |
| 106 | + } else if (os_strncmp(buf, "AVAL_COLOR_BMP", 14) == 0) { |
| 107 | + reply_len = hostapd_ctrl_iface_get_aval_color_bmp(hapd, reply, reply_size); |
| 108 | } else { |
| 109 | os_memcpy(reply, "UNKNOWN COMMAND\n", 16); |
| 110 | reply_len = 16; |
| 111 | diff --git a/hostapd/hostapd_cli.c b/hostapd/hostapd_cli.c |
developer | 81939a5 | 2023-03-25 15:31:11 +0800 | [diff] [blame] | 112 | index 6231e51..aad7aa5 100644 |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 113 | --- a/hostapd/hostapd_cli.c |
| 114 | +++ b/hostapd/hostapd_cli.c |
| 115 | @@ -1578,6 +1578,20 @@ static int hostapd_cli_cmd_reload_wpa_psk(struct wpa_ctrl *ctrl, int argc, |
| 116 | } |
| 117 | |
| 118 | |
| 119 | +static int hostapd_cli_cmd_get_bss_color(struct wpa_ctrl *ctrl, int argc, |
| 120 | + char *argv[]) |
| 121 | +{ |
| 122 | + return wpa_ctrl_command(ctrl, "GET_BSS_COLOR"); |
| 123 | +} |
| 124 | + |
| 125 | + |
| 126 | +static int hostapd_cli_cmd_get_aval_color_bmp(struct wpa_ctrl *ctrl, int argc, |
| 127 | + char *argv[]) |
| 128 | +{ |
| 129 | + return wpa_ctrl_command(ctrl, "AVAL_COLOR_BMP"); |
| 130 | +} |
| 131 | + |
| 132 | + |
| 133 | #ifdef ANDROID |
| 134 | static int hostapd_cli_cmd_driver(struct wpa_ctrl *ctrl, int argc, char *argv[]) |
| 135 | { |
| 136 | @@ -1795,6 +1809,10 @@ static const struct hostapd_cli_cmd hostapd_cli_commands[] = { |
| 137 | "<addr> [req_mode=] <measurement request hexdump> = send a Beacon report request to a station" }, |
| 138 | { "reload_wpa_psk", hostapd_cli_cmd_reload_wpa_psk, NULL, |
| 139 | "= reload wpa_psk_file only" }, |
| 140 | + { "get_bss_color", hostapd_cli_cmd_get_bss_color, NULL, |
| 141 | + "= get current BSS color" }, |
| 142 | + { "get_color_bmp", hostapd_cli_cmd_get_aval_color_bmp, NULL, |
| 143 | + "= get available BSS color bitmap" }, |
| 144 | #ifdef ANDROID |
| 145 | { "driver", hostapd_cli_cmd_driver, NULL, |
| 146 | "<driver sub command> [<hex formatted data>] = send driver command data" }, |
| 147 | diff --git a/src/ap/ap_drv_ops.c b/src/ap/ap_drv_ops.c |
developer | 81939a5 | 2023-03-25 15:31:11 +0800 | [diff] [blame] | 148 | index 342a7a8..40b9a20 100644 |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 149 | --- a/src/ap/ap_drv_ops.c |
| 150 | +++ b/src/ap/ap_drv_ops.c |
| 151 | @@ -1091,4 +1091,12 @@ int hostapd_drv_amsdu_dump(struct hostapd_data *hapd, u8 *amsdu) |
| 152 | if (!hapd->driver || !hapd->driver->amsdu_dump) |
| 153 | return 0; |
| 154 | return hapd->driver->amsdu_dump(hapd->drv_priv, amsdu); |
| 155 | -} |
| 156 | \ No newline at end of file |
| 157 | +} |
| 158 | + |
| 159 | +int hostapd_drv_get_aval_bss_color_bmp(struct hostapd_data *hapd, u64 *aval_color_bmp) |
| 160 | +{ |
| 161 | + if (!hapd->driver || !hapd->driver->get_aval_color_bmp || |
| 162 | + hapd->iface->conf->he_op.he_bss_color_disabled) |
| 163 | + return 0; |
| 164 | + return hapd->driver->get_aval_color_bmp(hapd->drv_priv, aval_color_bmp); |
| 165 | +} |
| 166 | diff --git a/src/ap/ap_drv_ops.h b/src/ap/ap_drv_ops.h |
developer | 81939a5 | 2023-03-25 15:31:11 +0800 | [diff] [blame] | 167 | index 79fef71..136a3ac 100644 |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 168 | --- a/src/ap/ap_drv_ops.h |
| 169 | +++ b/src/ap/ap_drv_ops.h |
| 170 | @@ -149,6 +149,8 @@ int hostapd_drv_ibf_ctrl(struct hostapd_data *hapd); |
| 171 | int hostapd_drv_ibf_dump(struct hostapd_data *hapd, u8 *ibf_enable); |
| 172 | int hostapd_drv_amsdu_ctrl(struct hostapd_data *hapd); |
| 173 | int hostapd_drv_amsdu_dump(struct hostapd_data *hapd, u8 *amsdu); |
| 174 | +int hostapd_drv_get_aval_bss_color_bmp(struct hostapd_data *hapd, |
| 175 | + u64 *aval_color_bmp); |
| 176 | |
| 177 | #include "drivers/driver.h" |
| 178 | |
| 179 | diff --git a/src/common/mtk_vendor.h b/src/common/mtk_vendor.h |
developer | 81939a5 | 2023-03-25 15:31:11 +0800 | [diff] [blame] | 180 | index 62d79cb..a99e6f2 100644 |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 181 | --- a/src/common/mtk_vendor.h |
| 182 | +++ b/src/common/mtk_vendor.h |
| 183 | @@ -15,6 +15,7 @@ enum mtk_nl80211_vendor_subcmds { |
| 184 | MTK_NL80211_VENDOR_SUBCMD_EDCCA_CTRL = 0xc7, |
| 185 | MTK_NL80211_VENDOR_SUBCMD_3WIRE_CTRL = 0xc8, |
| 186 | MTK_NL80211_VENDOR_SUBCMD_IBF_CTRL = 0xc9, |
| 187 | + MTK_NL80211_VENDOR_SUBCMD_BSS_COLOR_CTRL = 0xca, |
| 188 | }; |
| 189 | |
| 190 | enum mtk_vendor_attr_edcca_ctrl { |
developer | 81939a5 | 2023-03-25 15:31:11 +0800 | [diff] [blame] | 191 | @@ -253,6 +254,16 @@ ibf_dump_policy[NUM_MTK_VENDOR_ATTRS_IBF_DUMP] = { |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 192 | [MTK_VENDOR_ATTR_IBF_DUMP_ENABLE] = { .type = NLA_U8 }, |
| 193 | }; |
| 194 | |
| 195 | +enum mtk_vendor_attr_bss_color_ctrl { |
| 196 | + MTK_VENDOR_ATTR_BSS_COLOR_CTRL_UNSPEC, |
| 197 | + |
| 198 | + MTK_VENDOR_ATTR_AVAL_BSS_COLOR_BMP, |
| 199 | + |
| 200 | + /* keep last */ |
| 201 | + NUM_MTK_VENDOR_ATTRS_BSS_COLOR_CTRL, |
| 202 | + MTK_VENDOR_ATTR_BSS_COLOR_CTRL_MAX = |
| 203 | + NUM_MTK_VENDOR_ATTRS_BSS_COLOR_CTRL - 1 |
| 204 | +}; |
| 205 | |
| 206 | #define CSI_MAX_COUNT 256 |
| 207 | #define ETH_ALEN 6 |
| 208 | diff --git a/src/drivers/driver.h b/src/drivers/driver.h |
developer | 81939a5 | 2023-03-25 15:31:11 +0800 | [diff] [blame] | 209 | index 36f465c..7f6392f 100644 |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 210 | --- a/src/drivers/driver.h |
| 211 | +++ b/src/drivers/driver.h |
developer | 81939a5 | 2023-03-25 15:31:11 +0800 | [diff] [blame] | 212 | @@ -4771,6 +4771,14 @@ struct wpa_driver_ops { |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 213 | */ |
| 214 | int (*amsdu_ctrl)(void *priv, u8 amsdu); |
| 215 | int (*amsdu_dump)(void *priv, u8 *amsdu); |
| 216 | + |
| 217 | + /** |
| 218 | + * get_aval_color_bmp - get available BSS color bitmap |
| 219 | + * @priv: Private driver interface data |
| 220 | + * @aval_color_bmp: available bss color bitmap |
| 221 | + * |
| 222 | + */ |
| 223 | + int (*get_aval_color_bmp)(void *priv, u64 *aval_color_bmp); |
| 224 | }; |
| 225 | |
| 226 | /** |
| 227 | diff --git a/src/drivers/driver_nl80211.c b/src/drivers/driver_nl80211.c |
developer | 81939a5 | 2023-03-25 15:31:11 +0800 | [diff] [blame] | 228 | index 3753409..f9a8763 100644 |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 229 | --- a/src/drivers/driver_nl80211.c |
| 230 | +++ b/src/drivers/driver_nl80211.c |
developer | 81939a5 | 2023-03-25 15:31:11 +0800 | [diff] [blame] | 231 | @@ -12485,6 +12485,71 @@ static int nl80211_dpp_listen(void *priv, bool enable) |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 232 | } |
| 233 | #endif /* CONFIG_DPP */ |
| 234 | |
| 235 | +static int nl80211_get_aval_color_bmp_handler(struct nl_msg *msg, void *arg) |
| 236 | +{ |
| 237 | + u64 *aval_color_bmp = arg; |
| 238 | + struct nlattr *tb[NL80211_ATTR_MAX + 1]; |
| 239 | + struct nlattr *tb_vendor[MTK_VENDOR_ATTR_BSS_COLOR_CTRL_MAX + 1]; |
| 240 | + struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); |
| 241 | + struct nlattr *nl_vend, *attr; |
| 242 | + |
| 243 | + static const struct nla_policy |
| 244 | + bss_color_ctrl_policy[NUM_MTK_VENDOR_ATTRS_BSS_COLOR_CTRL + 1] = { |
| 245 | + [MTK_VENDOR_ATTR_AVAL_BSS_COLOR_BMP] = { .type = NLA_U64 }, |
| 246 | + }; |
| 247 | + |
| 248 | + nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), |
| 249 | + genlmsg_attrlen(gnlh, 0), NULL); |
| 250 | + |
| 251 | + nl_vend = tb[NL80211_ATTR_VENDOR_DATA]; |
| 252 | + if (!nl_vend) |
| 253 | + return NL_SKIP; |
| 254 | + |
| 255 | + nla_parse(tb_vendor, MTK_VENDOR_ATTR_BSS_COLOR_CTRL_MAX, |
| 256 | + nla_data(nl_vend), nla_len(nl_vend), NULL); |
| 257 | + |
| 258 | + *aval_color_bmp = nla_get_u64(tb_vendor[MTK_VENDOR_ATTR_AVAL_BSS_COLOR_BMP]); |
| 259 | + |
| 260 | + return 0; |
| 261 | +} |
| 262 | + |
| 263 | +static int nl80211_get_aval_color_bmp(void *priv, u64 *aval_color_bmp) |
| 264 | +{ |
| 265 | + struct i802_bss *bss = priv; |
| 266 | + struct wpa_driver_nl80211_data *drv = bss->drv; |
| 267 | + struct nl_msg *msg; |
| 268 | + struct nlattr *attr; |
| 269 | + int ret; |
| 270 | + |
| 271 | + if (!drv->mtk_bss_color_vendor_cmd_avail) { |
| 272 | + wpa_printf(MSG_INFO, |
| 273 | + "nl80211: Driver does not support BSS COLOR vendor cmd"); |
| 274 | + return 0; |
| 275 | + } |
| 276 | + |
| 277 | + if (!(msg = nl80211_drv_msg(drv, NLM_F_DUMP, NL80211_CMD_VENDOR)) || |
| 278 | + nla_put_u32(msg, NL80211_ATTR_VENDOR_ID, OUI_MTK) || |
| 279 | + nla_put_u32(msg, NL80211_ATTR_VENDOR_SUBCMD, |
| 280 | + MTK_NL80211_VENDOR_SUBCMD_BSS_COLOR_CTRL)) |
| 281 | + return -ENOBUFS; |
| 282 | + |
| 283 | + attr = nla_nest_start(msg, NL80211_ATTR_VENDOR_DATA); |
| 284 | + if (!attr) { |
| 285 | + nlmsg_free(msg); |
| 286 | + return -1; |
| 287 | + } |
| 288 | + |
| 289 | + nla_nest_end(msg, attr); |
| 290 | + |
| 291 | + ret = send_and_recv_msgs(drv, msg, |
| 292 | + nl80211_get_aval_color_bmp_handler, aval_color_bmp, NULL, NULL); |
| 293 | + |
| 294 | + if (ret) { |
| 295 | + wpa_printf(MSG_ERROR, "Failed to send BSS COLOR vendor cmd. ret=%d (%s) ", |
| 296 | + ret, strerror(-ret)); |
| 297 | + } |
| 298 | + return ret; |
| 299 | +} |
| 300 | |
| 301 | #ifdef CONFIG_TESTING_OPTIONS |
| 302 | |
developer | 81939a5 | 2023-03-25 15:31:11 +0800 | [diff] [blame] | 303 | @@ -13096,4 +13161,5 @@ const struct wpa_driver_ops wpa_driver_nl80211_ops = { |
| 304 | .ibf_dump = nl80211_ibf_dump, |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 305 | .amsdu_ctrl = nl80211_enable_amsdu, |
| 306 | .amsdu_dump = nl80211_dump_amsdu, |
| 307 | + .get_aval_color_bmp = nl80211_get_aval_color_bmp, |
| 308 | }; |
| 309 | diff --git a/src/drivers/driver_nl80211.h b/src/drivers/driver_nl80211.h |
developer | 81939a5 | 2023-03-25 15:31:11 +0800 | [diff] [blame] | 310 | index 21d48cc..72c7abd 100644 |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 311 | --- a/src/drivers/driver_nl80211.h |
| 312 | +++ b/src/drivers/driver_nl80211.h |
| 313 | @@ -186,6 +186,7 @@ struct wpa_driver_nl80211_data { |
| 314 | unsigned int mtk_3wire_vendor_cmd_avail:1; |
| 315 | unsigned int mtk_ibf_vendor_cmd_avail:1; |
| 316 | unsigned int mtk_wireless_vendor_cmd_avail:1; |
| 317 | + unsigned int mtk_bss_color_vendor_cmd_avail:1; |
| 318 | |
| 319 | u64 vendor_scan_cookie; |
| 320 | u64 remain_on_chan_cookie; |
| 321 | diff --git a/src/drivers/driver_nl80211_capa.c b/src/drivers/driver_nl80211_capa.c |
developer | 81939a5 | 2023-03-25 15:31:11 +0800 | [diff] [blame] | 322 | index d8078bc..e7c6f39 100644 |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 323 | --- a/src/drivers/driver_nl80211_capa.c |
| 324 | +++ b/src/drivers/driver_nl80211_capa.c |
developer | 81939a5 | 2023-03-25 15:31:11 +0800 | [diff] [blame] | 325 | @@ -1092,6 +1092,9 @@ static int wiphy_info_handler(struct nl_msg *msg, void *arg) |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 326 | case MTK_NL80211_VENDOR_SUBCMD_WIRELESS_CTRL: |
| 327 | drv->mtk_wireless_vendor_cmd_avail = 1; |
| 328 | break; |
| 329 | + case MTK_NL80211_VENDOR_SUBCMD_BSS_COLOR_CTRL : |
| 330 | + drv->mtk_bss_color_vendor_cmd_avail = 1; |
| 331 | + break; |
| 332 | } |
| 333 | } |
| 334 | |
| 335 | -- |
developer | 81939a5 | 2023-03-25 15:31:11 +0800 | [diff] [blame] | 336 | 2.18.0 |
developer | 011fec9 | 2023-02-17 07:31:31 +0800 | [diff] [blame] | 337 | |