nl80211.c

   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         ln->wiphy = nla_get_u32(tb[NL80211_ATTR_WIPHY]);
 204
 205         if (tb[NL80211_ATTR_SSID]) {
 206                 int len = nla_len(tb[NL80211_ATTR_SSID]);
 207
 208                 if (len >= sizeof(node->ssid))
 209                         len = sizeof(node->ssid) - 1;
 210
 211                 memcpy(node->ssid, nla_data(tb[NL80211_ATTR_SSID]), len);
 212                 node->ssid[len] = 0;
 213         }
 214
 215         MSG(INFO, "Found nl80211 phy on wdev %s, ssid=%s\n", usteer_node_name(node), node->ssid);
 216         ln->load_ewma = -1;
 217         ln->nl80211.present = true;
 218         ln->nl80211.update.cb = nl80211_update_node;
 219         nl80211_update_node(&ln->nl80211.update);
 220
 221 nla_put_failure:
 222         nlmsg_free(msg);
 223         return;
 224 }
 225
 226 static void nl80211_free_node(struct usteer_node *node)
 227 {
 228         struct usteer_local_node *ln = container_of(node, struct usteer_local_node, node);
 229
 230         if (!ln->nl80211.present)
 231                 return;
 232
 233         uloop_timeout_cancel(&ln->nl80211.update);
 234 }
 235
 236 static void nl80211_update_sta(struct usteer_node *node, struct sta_info *si)
 237 {
 238         struct nlattr *tb_sta[NL80211_STA_INFO_MAX + 1];
 239         struct usteer_local_node *ln = container_of(node, struct usteer_local_node, node);
 240         struct genlmsghdr *gnlh;
 241         struct nl_msg *msg;
 242         int signal = NO_SIGNAL;
 243
 244         if (!ln->nl80211.present)
 245                 return;
 246
 247         msg = unl_genl_msg(&unl, NL80211_CMD_GET_STATION, false);
 248         NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, ln->ifindex);
 249         NLA_PUT(msg, NL80211_ATTR_MAC, ETH_ALEN, si->sta->addr);
 250         unl_genl_request_single(&unl, msg, &msg);
 251         if (!msg)
 252                 return;
 253
 254         gnlh = nlmsg_data(nlmsg_hdr(msg));
 255         nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 256                   genlmsg_attrlen(gnlh, 0), NULL);
 257
 258         if (!tb[NL80211_ATTR_STA_INFO])
 259                 goto nla_put_failure;
 260
 261         if (nla_parse_nested(tb_sta, NL80211_STA_INFO_MAX,
 262                              tb[NL80211_ATTR_STA_INFO], NULL))
 263                 goto nla_put_failure;
 264
 265         if (tb_sta[NL80211_STA_INFO_SIGNAL_AVG])
 266                 signal = (int8_t) nla_get_u8(tb_sta[NL80211_STA_INFO_SIGNAL_AVG]);
 267
 268         usteer_sta_info_update(si, signal, true);
 269
 270 nla_put_failure:
 271         nlmsg_free(msg);
 272         return;
 273 }
 274
 275 static int nl80211_scan_result(struct nl_msg *msg, void *arg)
 276 {
 277         static struct nla_policy bss_policy[NL80211_BSS_MAX + 1] = {
 278                 [NL80211_BSS_FREQUENCY] = { .type = NLA_U32 },
 279                 [NL80211_BSS_CAPABILITY] = { .type = NLA_U16 },
 280                 [NL80211_BSS_SIGNAL_MBM] = { .type = NLA_U32 },
 281         };
 282         struct nlattr *tb[NL80211_ATTR_MAX + 1];
 283         struct nlattr *bss[NL80211_BSS_MAX + 1];
 284         struct nl80211_scan_req *req = arg;
 285         struct usteer_scan_result data = {
 286                 .signal = -127,
 287         };
 288         struct genlmsghdr *gnlh;
 289         struct nlattr *ie_attr;
 290         int ielen = 0;
 291         uint8_t *ie;
 292
 293         gnlh = nlmsg_data(nlmsg_hdr(msg));
 294         nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 295                   genlmsg_attrlen(gnlh, 0), NULL);
 296
 297         if (!tb[NL80211_ATTR_BSS])
 298                 return NL_SKIP;
 299
 300         if (nla_parse_nested(bss, NL80211_BSS_MAX, tb[NL80211_ATTR_BSS],
 301                              bss_policy))
 302                 return NL_SKIP;
 303
 304         if (!bss[NL80211_BSS_BSSID] ||
 305             !bss[NL80211_BSS_FREQUENCY])
 306                 return NL_SKIP;
 307
 308         data.freq = nla_get_u32(bss[NL80211_BSS_FREQUENCY]);
 309         memcpy(data.bssid, nla_data(bss[NL80211_BSS_BSSID]), sizeof(data.bssid));
 310
 311         if (bss[NL80211_BSS_SIGNAL_MBM]) {
 312                 int32_t signal = nla_get_u32(bss[NL80211_BSS_SIGNAL_MBM]);
 313                 data.signal = signal / 100;
 314         }
 315
 316         ie_attr = bss[NL80211_BSS_INFORMATION_ELEMENTS];
 317         if (!ie_attr)
 318                 ie_attr = bss[NL80211_BSS_BEACON_IES];
 319
 320         if (!ie_attr)
 321                 goto skip_ie;
 322
 323         ie = (uint8_t *) nla_data(ie_attr);
 324         ielen = nla_len(ie_attr);
 325         for (; ielen >= 2 && ielen >= ie[1];
 326              ielen -= ie[1] + 2, ie += ie[1] + 2) {
 327                 if (ie[0] == 0) { /* SSID */
 328                         if (ie[1] > 32)
 329                                 continue;
 330
 331                         memcpy(data.ssid, ie + 2, ie[1]);
 332                 }
 333         }
 334
 335 skip_ie:
 336         req->cb(req->priv, &data);
 337
 338         return NL_SKIP;
 339 }
 340
 341 static int nl80211_scan_event_cb(struct nl_msg *msg, void *data)
 342 {
 343         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 344
 345         switch (gnlh->cmd) {
 346         case NL80211_CMD_NEW_SCAN_RESULTS:
 347         case NL80211_CMD_SCAN_ABORTED:
 348                 unl_loop_done(&unl);
 349                 break;
 350         }
 351
 352         return NL_SKIP;
 353 }
 354
 355 static int nl80211_scan(struct usteer_node *node, struct usteer_scan_request *req,
 356                         void *priv, void (*cb)(void *priv, struct usteer_scan_result *r))
 357 {
 358         struct usteer_local_node *ln = container_of(node, struct usteer_local_node, node);
 359         struct nl80211_scan_req reqdata = {
 360                 .priv = priv,
 361                 .cb = cb,
 362         };
 363         struct nl_msg *msg;
 364         struct nlattr *cur;
 365         int i, ret;
 366
 367         if (!ln->nl80211.present)
 368                 return -ENODEV;
 369
 370         msg = unl_genl_msg(&unl, NL80211_CMD_TRIGGER_SCAN, false);
 371         NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, ln->ifindex);
 372
 373         if (!req->passive) {
 374                 cur = nla_nest_start(msg, NL80211_ATTR_SCAN_SSIDS);
 375                 NLA_PUT(msg, 1, 0, "");
 376                 nla_nest_end(msg, cur);
 377         }
 378
 379         NLA_PUT_U32(msg, NL80211_ATTR_SCAN_FLAGS, NL80211_SCAN_FLAG_AP);
 380
 381         if (req->n_freq) {
 382                 cur = nla_nest_start(msg, NL80211_ATTR_SCAN_FREQUENCIES);
 383                 for (i = 0; i < req->n_freq; i++)
 384                         NLA_PUT_U32(msg, i, req->freq[i]);
 385                 nla_nest_end(msg, cur);
 386         }
 387
 388         unl_genl_subscribe(&unl, "scan");
 389         ret = unl_genl_request(&unl, msg, NULL, NULL);
 390         if (ret < 0)
 391                 goto done;
 392
 393         unl_genl_loop(&unl, nl80211_scan_event_cb, NULL);
 394
 395 done:
 396         unl_genl_unsubscribe(&unl, "scan");
 397         if (ret < 0)
 398                 return ret;
 399
 400         if (!cb)
 401                 return 0;
 402
 403         msg = unl_genl_msg(&unl, NL80211_CMD_GET_SCAN, true);
 404         NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, ln->ifindex);
 405         unl_genl_request(&unl, msg, nl80211_scan_result, &reqdata);
 406
 407         return 0;
 408
 409 nla_put_failure:
 410         nlmsg_free(msg);
 411         return -ENOMEM;
 412 }
 413
 414 static int nl80211_wiphy_result(struct nl_msg *msg, void *arg)
 415 {
 416         struct nl80211_freqlist_req *req = arg;
 417         struct nlattr *tb[NL80211_ATTR_MAX + 1];
 418         struct nlattr *tb_band[NL80211_BAND_ATTR_MAX + 1];
 419         struct nlattr *tb_freq[NL80211_FREQUENCY_ATTR_MAX + 1];
 420         struct nlattr *nl_band;
 421         struct nlattr *nl_freq;
 422         struct nlattr *cur;
 423         struct genlmsghdr *gnlh;
 424         int rem_band;
 425         int rem_freq;
 426
 427         gnlh = nlmsg_data(nlmsg_hdr(msg));
 428         nla_parse(tb, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
 429                   genlmsg_attrlen(gnlh, 0), NULL);
 430
 431         if (!tb[NL80211_ATTR_WIPHY_BANDS])
 432                 return NL_SKIP;
 433
 434         nla_for_each_nested(nl_band, tb[NL80211_ATTR_WIPHY_BANDS], rem_band) {
 435                 nla_parse(tb_band, NL80211_BAND_ATTR_MAX, nla_data(nl_band),
 436                           nla_len(nl_band), NULL);
 437
 438                 if (!tb_band[NL80211_BAND_ATTR_FREQS])
 439                         continue;
 440
 441                 nla_for_each_nested(nl_freq, tb_band[NL80211_BAND_ATTR_FREQS],
 442                                     rem_freq) {
 443                         struct usteer_freq_data f = {};
 444
 445                         nla_parse(tb_freq, NL80211_FREQUENCY_ATTR_MAX,
 446                                   nla_data(nl_freq), nla_len(nl_freq), NULL);
 447
 448                         if (tb_freq[NL80211_FREQUENCY_ATTR_DISABLED])
 449                                 continue;
 450
 451                         if (tb_freq[NL80211_FREQUENCY_ATTR_NO_IR])
 452                                 continue;
 453
 454                         cur = tb_freq[NL80211_FREQUENCY_ATTR_FREQ];
 455                         if (!cur)
 456                                 continue;
 457
 458                         f.freq = nla_get_u32(cur);
 459                         f.dfs = !!tb_freq[NL80211_FREQUENCY_ATTR_RADAR];
 460
 461                         cur = tb_freq[NL80211_FREQUENCY_ATTR_MAX_TX_POWER];
 462                         if (cur)
 463                                 f.txpower = nla_get_u32(cur) / 100;
 464
 465                         req->cb(req->priv, &f);
 466                 }
 467         }
 468
 469         return NL_SKIP;
 470 }
 471
 472 static void nl80211_get_freqlist(struct usteer_node *node, void *priv,
 473                                  void (*cb)(void *priv, struct usteer_freq_data *f))
 474 {
 475         struct usteer_local_node *ln = container_of(node, struct usteer_local_node, node);
 476         struct nl80211_freqlist_req req = {
 477                 .priv = priv,
 478                 .cb = cb
 479         };
 480         struct nl_msg *msg;
 481
 482         if (!ln->nl80211.present)
 483                 return;
 484
 485         msg = unl_genl_msg(&unl, NL80211_CMD_GET_WIPHY, false);
 486
 487         NLA_PUT_U32(msg, NL80211_ATTR_WIPHY, ln->wiphy);
 488         NLA_PUT_FLAG(msg, NL80211_ATTR_SPLIT_WIPHY_DUMP);
 489
 490         unl_genl_request(&unl, msg, nl80211_wiphy_result, &req);
 491
 492         return;
 493
 494 nla_put_failure:
 495         nlmsg_free(msg);
 496 }
 497
 498 static struct usteer_node_handler nl80211_handler = {
 499         .init_node = nl80211_init_node,
 500         .free_node = nl80211_free_node,
 501         .update_sta = nl80211_update_sta,
 502         .get_survey = nl80211_get_survey,
 503         .get_freqlist = nl80211_get_freqlist,
 504         .scan = nl80211_scan,
 505 };
 506
 507 static void __usteer_init usteer_nl80211_init(void)
 508 {
 509         list_add(&nl80211_handler.list, &node_handlers);
 510 }