• source navigation  • diff markup  • identifier search  • freetext search  • 

Sources/usteer/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 }
516 

This page was automatically generated by LXR 0.3.1.  •  OpenWrt