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         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 }