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