1 /* 2 * This program is free software; you can redistribute it and/or modify 3 * it under the terms of the GNU General Public License as published by 4 * the Free Software Foundation; either version 2 of the License. 5 * 6 * This program is distributed in the hope that it will be useful, 7 * but WITHOUT ANY WARRANTY; without even the implied warranty of 8 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 9 * GNU General Public License for more details. 10 * 11 * You should have received a copy of the GNU General Public License 12 * along with this program; if not, write to the Free Software 13 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307, USA. 14 * 15 * Copyright (C) 2020 embedd.ch 16 * Copyright (C) 2020 Felix Fietkau <nbd@nbd.name> 17 * Copyright (C) 2020 John Crispin <john@phrozen.org> 18 */ 19 20 #define _GNU_SOURCE 21 #include <linux/if_ether.h> 22 #include <net/if.h> 23 24 25 #include <stdio.h> 26 #include <unistd.h> 27 #include <stdbool.h> 28 #include <stdint.h> 29 #include <string.h> 30 #include <fcntl.h> 31 #include <errno.h> 32 33 #include <linux/nl80211.h> 34 #include <unl.h> 35 36 #include "usteer.h" 37 #include "node.h" 38 39 static struct unl unl; 40 static struct nlattr *tb[NL80211_ATTR_MAX + 1]; 41 42 struct nl80211_survey_req { 43 void (*cb)(void *priv, struct usteer_survey_data *d); 44 void *priv; 45 }; 46 47 struct nl80211_scan_req { 48 void (*cb)(void *priv, struct usteer_scan_result *r); 49 void *priv; 50 }; 51 52 struct nl80211_freqlist_req { 53 void (*cb)(void *priv, struct usteer_freq_data *f); 54 void *priv; 55 }; 56 57 static int nl80211_survey_result(struct nl_msg *msg, void *arg) 58 { 59 static struct nla_policy survey_policy[NL80211_SURVEY_INFO_MAX + 1] = { 60 [NL80211_SURVEY_INFO_FREQUENCY] = { .type = NLA_U32 }, 61 [NL80211_SURVEY_INFO_NOISE] = { .type = NLA_U8 }, 62 [NL80211_SURVEY_INFO_CHANNEL_TIME] = { .type = NLA_U64 }, 63 [NL80211_SURVEY_INFO_CHANNEL_TIME_BUSY] = { .type = NLA_U64 }, 64 }; 65 struct nlattr *tb[NL80211_ATTR_MAX + 1]; 66 struct nlattr *tb_s[NL80211_SURVEY_INFO_MAX + 1]; 67 struct nl80211_survey_req *req = arg; 68 struct usteer_survey_data data = {}; 69 struct genlmsghdr *gnlh; 70 71 gnlh = nlmsg_data(nlmsg_hdr(msg)); 72 nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), 73 genlmsg_attrlen(gnlh, 0), NULL); 74 75 if (!tb[NL80211_ATTR_SURVEY_INFO]) 76 return NL_SKIP; 77 78 if (nla_parse_nested(tb_s, NL80211_SURVEY_INFO_MAX, 79 tb[NL80211_ATTR_SURVEY_INFO], survey_policy)) 80 return NL_SKIP; 81 82 if (!tb_s[NL80211_SURVEY_INFO_FREQUENCY]) 83 return NL_SKIP; 84 85 data.freq = nla_get_u32(tb_s[NL80211_SURVEY_INFO_FREQUENCY]); 86 87 if (tb_s[NL80211_SURVEY_INFO_NOISE]) 88 data.noise = (int8_t) nla_get_u8(tb_s[NL80211_SURVEY_INFO_NOISE]); 89 90 if (tb_s[NL80211_SURVEY_INFO_CHANNEL_TIME] && 91 tb_s[NL80211_SURVEY_INFO_CHANNEL_TIME_BUSY]) { 92 data.time = nla_get_u64(tb_s[NL80211_SURVEY_INFO_CHANNEL_TIME]); 93 data.time_busy = nla_get_u64(tb_s[NL80211_SURVEY_INFO_CHANNEL_TIME_BUSY]); 94 } 95 96 req->cb(req->priv, &data); 97 98 return NL_SKIP; 99 } 100 101 static void nl80211_get_survey(struct usteer_node *node, void *priv, 102 void (*cb)(void *priv, struct usteer_survey_data *d)) 103 { 104 struct usteer_local_node *ln = container_of(node, struct usteer_local_node, node); 105 struct nl80211_survey_req req = { 106 .priv = priv, 107 .cb = cb, 108 }; 109 struct nl_msg *msg; 110 111 if (!ln->nl80211.present) 112 return; 113 114 msg = unl_genl_msg(&unl, NL80211_CMD_GET_SURVEY, true); 115 NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, ln->ifindex); 116 unl_genl_request(&unl, msg, nl80211_survey_result, &req); 117 118 nla_put_failure: 119 return; 120 } 121 122 static void nl80211_update_node_result(void *priv, struct usteer_survey_data *d) 123 { 124 struct usteer_local_node *ln = priv; 125 uint32_t delta = 0, delta_busy = 0; 126 127 if (d->freq != ln->node.freq) 128 return; 129 130 if (d->noise) 131 ln->node.noise = d->noise; 132 133 if (ln->time) { 134 delta = d->time - ln->time; 135 delta_busy = d->time_busy - ln->time_busy; 136 } 137 138 ln->time = d->time; 139 ln->time_busy = d->time_busy; 140 141 if (delta) { 142 float cur = (100 * delta_busy) / delta; 143 144 if (ln->load_ewma < 0) 145 ln->load_ewma = cur; 146 else 147 ln->load_ewma = 0.85 * ln->load_ewma + 0.15 * cur; 148 149 ln->node.load = ln->load_ewma; 150 } 151 } 152 153 static void nl80211_update_node(struct uloop_timeout *t) 154 { 155 struct usteer_local_node *ln = container_of(t, struct usteer_local_node, nl80211.update); 156 157 uloop_timeout_set(t, 1000); 158 ln->ifindex = if_nametoindex(ln->iface); 159 nl80211_get_survey(&ln->node, ln, nl80211_update_node_result); 160 } 161 162 static void nl80211_init_node(struct usteer_node *node) 163 { 164 struct usteer_local_node *ln = container_of(node, struct usteer_local_node, node); 165 struct genlmsghdr *gnlh; 166 static bool _init = false; 167 struct nl_msg *msg; 168 169 if (node->type != NODE_TYPE_LOCAL) 170 return; 171 172 ln->nl80211.present = false; 173 ln->wiphy = -1; 174 175 if (!ln->ifindex) { 176 MSG(INFO, "No ifindex found for node %s\n", usteer_node_name(node)); 177 return; 178 } 179 180 if (!_init) { 181 if (unl_genl_init(&unl, "nl80211") < 0) { 182 unl_free(&unl); 183 MSG(INFO, "nl80211 init failed\n"); 184 return; 185 } 186 187 _init = true; 188 } 189 190 msg = unl_genl_msg(&unl, NL80211_CMD_GET_INTERFACE, false); 191 NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, ln->ifindex); 192 unl_genl_request_single(&unl, msg, &msg); 193 if (!msg) 194 return; 195 196 gnlh = nlmsg_data(nlmsg_hdr(msg)); 197 nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), 198 genlmsg_attrlen(gnlh, 0), NULL); 199 200 if (!tb[NL80211_ATTR_WIPHY]) 201 goto nla_put_failure; 202 203 if (!tb[NL80211_ATTR_MAC]) 204 goto nla_put_failure; 205 206 ln->wiphy = nla_get_u32(tb[NL80211_ATTR_WIPHY]); 207 208 memcpy(node->bssid, nla_data(tb[NL80211_ATTR_MAC]), ETH_ALEN); 209 210 if (tb[NL80211_ATTR_SSID]) { 211 int len = nla_len(tb[NL80211_ATTR_SSID]); 212 213 if (len >= sizeof(node->ssid)) 214 len = sizeof(node->ssid) - 1; 215 216 memcpy(node->ssid, nla_data(tb[NL80211_ATTR_SSID]), len); 217 node->ssid[len] = 0; 218 } 219 220 MSG(INFO, "Found nl80211 phy on wdev %s, ssid=%s\n", usteer_node_name(node), node->ssid); 221 ln->load_ewma = -1; 222 ln->nl80211.present = true; 223 ln->nl80211.update.cb = nl80211_update_node; 224 nl80211_update_node(&ln->nl80211.update); 225 226 nla_put_failure: 227 nlmsg_free(msg); 228 return; 229 } 230 231 static void nl80211_free_node(struct usteer_node *node) 232 { 233 struct usteer_local_node *ln = container_of(node, struct usteer_local_node, node); 234 235 if (!ln->nl80211.present) 236 return; 237 238 uloop_timeout_cancel(&ln->nl80211.update); 239 } 240 241 static void nl80211_update_sta(struct usteer_node *node, struct sta_info *si) 242 { 243 struct nlattr *tb_sta[NL80211_STA_INFO_MAX + 1]; 244 struct usteer_local_node *ln = container_of(node, struct usteer_local_node, node); 245 struct genlmsghdr *gnlh; 246 struct nl_msg *msg; 247 int signal = NO_SIGNAL; 248 249 if (!ln->nl80211.present) 250 return; 251 252 msg = unl_genl_msg(&unl, NL80211_CMD_GET_STATION, false); 253 NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, ln->ifindex); 254 NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, si->sta->addr); 255 unl_genl_request_single(&unl, msg, &msg); 256 if (!msg) 257 return; 258 259 gnlh = nlmsg_data(nlmsg_hdr(msg)); 260 nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), 261 genlmsg_attrlen(gnlh, 0), NULL); 262 263 if (!tb[NL80211_ATTR_STA_INFO]) 264 goto nla_put_failure; 265 266 if (nla_parse_nested(tb_sta, NL80211_STA_INFO_MAX, 267 tb[NL80211_ATTR_STA_INFO], NULL)) 268 goto nla_put_failure; 269 270 if (tb_sta[NL80211_STA_INFO_SIGNAL_AVG]) 271 signal = (int8_t) nla_get_u8(tb_sta[NL80211_STA_INFO_SIGNAL_AVG]); 272 273 if (tb_sta[NL80211_STA_INFO_CONNECTED_TIME]) 274 si->connected_since = current_time - (nla_get_u32(tb_sta[NL80211_STA_INFO_CONNECTED_TIME]) * 1000); 275 276 usteer_sta_info_update(si, signal, true); 277 278 nla_put_failure: 279 nlmsg_free(msg); 280 return; 281 } 282 283 static int nl80211_scan_result(struct nl_msg *msg, void *arg) 284 { 285 static struct nla_policy bss_policy[NL80211_BSS_MAX + 1] = { 286 [NL80211_BSS_FREQUENCY] = { .type = NLA_U32 }, 287 [NL80211_BSS_CAPABILITY] = { .type = NLA_U16 }, 288 [NL80211_BSS_SIGNAL_MBM] = { .type = NLA_U32 }, 289 }; 290 struct nlattr *tb[NL80211_ATTR_MAX + 1]; 291 struct nlattr *bss[NL80211_BSS_MAX + 1]; 292 struct nl80211_scan_req *req = arg; 293 struct usteer_scan_result data = { 294 .signal = -127, 295 }; 296 struct genlmsghdr *gnlh; 297 struct nlattr *ie_attr; 298 int ielen = 0; 299 uint8_t *ie; 300 301 gnlh = nlmsg_data(nlmsg_hdr(msg)); 302 nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), 303 genlmsg_attrlen(gnlh, 0), NULL); 304 305 if (!tb[NL80211_ATTR_BSS]) 306 return NL_SKIP; 307 308 if (nla_parse_nested(bss, NL80211_BSS_MAX, tb[NL80211_ATTR_BSS], 309 bss_policy)) 310 return NL_SKIP; 311 312 if (!bss[NL80211_BSS_BSSID] || 313 !bss[NL80211_BSS_FREQUENCY]) 314 return NL_SKIP; 315 316 data.freq = nla_get_u32(bss[NL80211_BSS_FREQUENCY]); 317 memcpy(data.bssid, nla_data(bss[NL80211_BSS_BSSID]), sizeof(data.bssid)); 318 319 if (bss[NL80211_BSS_SIGNAL_MBM]) { 320 int32_t signal = nla_get_u32(bss[NL80211_BSS_SIGNAL_MBM]); 321 data.signal = signal / 100; 322 } 323 324 ie_attr = bss[NL80211_BSS_INFORMATION_ELEMENTS]; 325 if (!ie_attr) 326 ie_attr = bss[NL80211_BSS_BEACON_IES]; 327 328 if (!ie_attr) 329 goto skip_ie; 330 331 ie = (uint8_t *) nla_data(ie_attr); 332 ielen = nla_len(ie_attr); 333 for (; ielen >= 2 && ielen >= ie[1]; 334 ielen -= ie[1] + 2, ie += ie[1] + 2) { 335 if (ie[0] == 0) { /* SSID */ 336 if (ie[1] > 32) 337 continue; 338 339 memcpy(data.ssid, ie + 2, ie[1]); 340 } 341 } 342 343 skip_ie: 344 req->cb(req->priv, &data); 345 346 return NL_SKIP; 347 } 348 349 static int nl80211_scan_event_cb(struct nl_msg *msg, void *data) 350 { 351 struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); 352 353 switch (gnlh->cmd) { 354 case NL80211_CMD_NEW_SCAN_RESULTS: 355 case NL80211_CMD_SCAN_ABORTED: 356 unl_loop_done(&unl); 357 break; 358 } 359 360 return NL_SKIP; 361 } 362 363 static int nl80211_scan(struct usteer_node *node, struct usteer_scan_request *req, 364 void *priv, void (*cb)(void *priv, struct usteer_scan_result *r)) 365 { 366 struct usteer_local_node *ln = container_of(node, struct usteer_local_node, node); 367 struct nl80211_scan_req reqdata = { 368 .priv = priv, 369 .cb = cb, 370 }; 371 struct nl_msg *msg; 372 struct nlattr *cur; 373 int i, ret; 374 375 if (!ln->nl80211.present) 376 return -ENODEV; 377 378 msg = unl_genl_msg(&unl, NL80211_CMD_TRIGGER_SCAN, false); 379 NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, ln->ifindex); 380 381 if (!req->passive) { 382 cur = nla_nest_start(msg, NL80211_ATTR_SCAN_SSIDS); 383 NLA_PUT(msg, 1, 0, ""); 384 nla_nest_end(msg, cur); 385 } 386 387 NLA_PUT_U32(msg, NL80211_ATTR_SCAN_FLAGS, NL80211_SCAN_FLAG_AP); 388 389 if (req->n_freq) { 390 cur = nla_nest_start(msg, NL80211_ATTR_SCAN_FREQUENCIES); 391 for (i = 0; i < req->n_freq; i++) 392 NLA_PUT_U32(msg, i, req->freq[i]); 393 nla_nest_end(msg, cur); 394 } 395 396 unl_genl_subscribe(&unl, "scan"); 397 ret = unl_genl_request(&unl, msg, NULL, NULL); 398 if (ret < 0) 399 goto done; 400 401 unl_genl_loop(&unl, nl80211_scan_event_cb, NULL); 402 403 done: 404 unl_genl_unsubscribe(&unl, "scan"); 405 if (ret < 0) 406 return ret; 407 408 if (!cb) 409 return 0; 410 411 msg = unl_genl_msg(&unl, NL80211_CMD_GET_SCAN, true); 412 NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, ln->ifindex); 413 unl_genl_request(&unl, msg, nl80211_scan_result, &reqdata); 414 415 return 0; 416 417 nla_put_failure: 418 nlmsg_free(msg); 419 return -ENOMEM; 420 } 421 422 static int nl80211_wiphy_result(struct nl_msg *msg, void *arg) 423 { 424 struct nl80211_freqlist_req *req = arg; 425 struct nlattr *tb[NL80211_ATTR_MAX + 1]; 426 struct nlattr *tb_band[NL80211_BAND_ATTR_MAX + 1]; 427 struct nlattr *tb_freq[NL80211_FREQUENCY_ATTR_MAX + 1]; 428 struct nlattr *nl_band; 429 struct nlattr *nl_freq; 430 struct nlattr *cur; 431 struct genlmsghdr *gnlh; 432 int rem_band; 433 int rem_freq; 434 435 gnlh = nlmsg_data(nlmsg_hdr(msg)); 436 nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), 437 genlmsg_attrlen(gnlh, 0), NULL); 438 439 if (!tb[NL80211_ATTR_WIPHY_BANDS]) 440 return NL_SKIP; 441 442 nla_for_each_nested(nl_band, tb[NL80211_ATTR_WIPHY_BANDS], rem_band) { 443 nla_parse(tb_band, NL80211_BAND_ATTR_MAX, nla_data(nl_band), 444 nla_len(nl_band), NULL); 445 446 if (!tb_band[NL80211_BAND_ATTR_FREQS]) 447 continue; 448 449 nla_for_each_nested(nl_freq, tb_band[NL80211_BAND_ATTR_FREQS], 450 rem_freq) { 451 struct usteer_freq_data f = {}; 452 453 nla_parse(tb_freq, NL80211_FREQUENCY_ATTR_MAX, 454 nla_data(nl_freq), nla_len(nl_freq), NULL); 455 456 if (tb_freq[NL80211_FREQUENCY_ATTR_DISABLED]) 457 continue; 458 459 if (tb_freq[NL80211_FREQUENCY_ATTR_NO_IR]) 460 continue; 461 462 cur = tb_freq[NL80211_FREQUENCY_ATTR_FREQ]; 463 if (!cur) 464 continue; 465 466 f.freq = nla_get_u32(cur); 467 f.dfs = !!tb_freq[NL80211_FREQUENCY_ATTR_RADAR]; 468 469 cur = tb_freq[NL80211_FREQUENCY_ATTR_MAX_TX_POWER]; 470 if (cur) 471 f.txpower = nla_get_u32(cur) / 100; 472 473 req->cb(req->priv, &f); 474 } 475 } 476 477 return NL_SKIP; 478 } 479 480 static void nl80211_get_freqlist(struct usteer_node *node, void *priv, 481 void (*cb)(void *priv, struct usteer_freq_data *f)) 482 { 483 struct usteer_local_node *ln = container_of(node, struct usteer_local_node, node); 484 struct nl80211_freqlist_req req = { 485 .priv = priv, 486 .cb = cb 487 }; 488 struct nl_msg *msg; 489 490 if (!ln->nl80211.present) 491 return; 492 493 msg = unl_genl_msg(&unl, NL80211_CMD_GET_WIPHY, false); 494 495 NLA_PUT_U32(msg, NL80211_ATTR_WIPHY, ln->wiphy); 496 NLA_PUT_FLAG(msg, NL80211_ATTR_SPLIT_WIPHY_DUMP); 497 498 unl_genl_request(&unl, msg, nl80211_wiphy_result, &req); 499 500 return; 501 502 nla_put_failure: 503 nlmsg_free(msg); 504 } 505 506 static struct usteer_node_handler nl80211_handler = { 507 .init_node = nl80211_init_node, 508 .free_node = nl80211_free_node, 509 .update_sta = nl80211_update_sta, 510 .get_survey = nl80211_get_survey, 511 .get_freqlist = nl80211_get_freqlist, 512 .scan = nl80211_scan, 513 }; 514 515 static void __usteer_init usteer_nl80211_init(void) 516 { 517 list_add(&nl80211_handler.list, &node_handlers); 518 } 519
This page was automatically generated by LXR 0.3.1. • OpenWrt