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.
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.
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.
15 * Copyright (C) 2020 embedd.ch
16 * Copyright (C) 2020 Felix Fietkau <nbd@nbd.name>
17 * Copyright (C) 2020 John Crispin <john@phrozen.org>
21 #include <linux/if_ether.h>
33 #include <linux/nl80211.h>
39 static struct unl unl
;
40 static struct nlattr
*tb
[NL80211_ATTR_MAX
+ 1];
42 struct nl80211_survey_req
{
43 void (*cb
)(void *priv
, struct usteer_survey_data
*d
);
47 struct nl80211_scan_req
{
48 void (*cb
)(void *priv
, struct usteer_scan_result
*r
);
52 struct nl80211_freqlist_req
{
53 void (*cb
)(void *priv
, struct usteer_freq_data
*f
);
57 static int nl80211_survey_result(struct nl_msg
*msg
, void *arg
)
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
},
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
;
71 gnlh
= nlmsg_data(nlmsg_hdr(msg
));
72 nla_parse(tb
, NL80211_ATTR_MAX
, genlmsg_attrdata(gnlh
, 0),
73 genlmsg_attrlen(gnlh
, 0), NULL
);
75 if (!tb
[NL80211_ATTR_SURVEY_INFO
])
78 if (nla_parse_nested(tb_s
, NL80211_SURVEY_INFO_MAX
,
79 tb
[NL80211_ATTR_SURVEY_INFO
], survey_policy
))
82 if (!tb_s
[NL80211_SURVEY_INFO_FREQUENCY
])
85 data
.freq
= nla_get_u32(tb_s
[NL80211_SURVEY_INFO_FREQUENCY
]);
87 if (tb_s
[NL80211_SURVEY_INFO_NOISE
])
88 data
.noise
= (int8_t) nla_get_u8(tb_s
[NL80211_SURVEY_INFO_NOISE
]);
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
]);
96 req
->cb(req
->priv
, &data
);
101 static void nl80211_get_survey(struct usteer_node
*node
, void *priv
,
102 void (*cb
)(void *priv
, struct usteer_survey_data
*d
))
104 struct usteer_local_node
*ln
= container_of(node
, struct usteer_local_node
, node
);
105 struct nl80211_survey_req req
= {
111 if (!ln
->nl80211
.present
)
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
);
122 static void nl80211_update_node_result(void *priv
, struct usteer_survey_data
*d
)
124 struct usteer_local_node
*ln
= priv
;
125 uint32_t delta
= 0, delta_busy
= 0;
127 if (d
->freq
!= ln
->node
.freq
)
131 ln
->node
.noise
= d
->noise
;
134 delta
= d
->time
- ln
->time
;
135 delta_busy
= d
->time_busy
- ln
->time_busy
;
139 ln
->time_busy
= d
->time_busy
;
142 float cur
= (100 * delta_busy
) / delta
;
144 if (ln
->load_ewma
< 0)
147 ln
->load_ewma
= 0.85 * ln
->load_ewma
+ 0.15 * cur
;
149 ln
->node
.load
= ln
->load_ewma
;
153 static void nl80211_update_node(struct uloop_timeout
*t
)
155 struct usteer_local_node
*ln
= container_of(t
, struct usteer_local_node
, nl80211
.update
);
157 uloop_timeout_set(t
, 1000);
158 ln
->ifindex
= if_nametoindex(ln
->iface
);
159 nl80211_get_survey(&ln
->node
, ln
, nl80211_update_node_result
);
162 static void nl80211_init_node(struct usteer_node
*node
)
164 struct usteer_local_node
*ln
= container_of(node
, struct usteer_local_node
, node
);
165 struct genlmsghdr
*gnlh
;
166 static bool _init
= false;
169 if (node
->type
!= NODE_TYPE_LOCAL
)
172 ln
->nl80211
.present
= false;
176 MSG(INFO
, "No ifindex found for node %s\n", usteer_node_name(node
));
181 if (unl_genl_init(&unl
, "nl80211") < 0) {
183 MSG(INFO
, "nl80211 init failed\n");
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
);
196 gnlh
= nlmsg_data(nlmsg_hdr(msg
));
197 nla_parse(tb
, NL80211_ATTR_MAX
, genlmsg_attrdata(gnlh
, 0),
198 genlmsg_attrlen(gnlh
, 0), NULL
);
200 if (!tb
[NL80211_ATTR_WIPHY
])
201 goto nla_put_failure
;
203 ln
->wiphy
= nla_get_u32(tb
[NL80211_ATTR_WIPHY
]);
205 if (tb
[NL80211_ATTR_SSID
]) {
206 int len
= nla_len(tb
[NL80211_ATTR_SSID
]);
208 if (len
>= sizeof(node
->ssid
))
209 len
= sizeof(node
->ssid
) - 1;
211 memcpy(node
->ssid
, nla_data(tb
[NL80211_ATTR_SSID
]), len
);
215 MSG(INFO
, "Found nl80211 phy on wdev %s, ssid=%s\n", usteer_node_name(node
), node
->ssid
);
217 ln
->nl80211
.present
= true;
218 ln
->nl80211
.update
.cb
= nl80211_update_node
;
219 nl80211_update_node(&ln
->nl80211
.update
);
226 static void nl80211_free_node(struct usteer_node
*node
)
228 struct usteer_local_node
*ln
= container_of(node
, struct usteer_local_node
, node
);
230 if (!ln
->nl80211
.present
)
233 uloop_timeout_cancel(&ln
->nl80211
.update
);
236 static void nl80211_update_sta(struct usteer_node
*node
, struct sta_info
*si
)
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
;
242 int signal
= NO_SIGNAL
;
244 if (!ln
->nl80211
.present
)
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
);
254 gnlh
= nlmsg_data(nlmsg_hdr(msg
));
255 nla_parse(tb
, NL80211_ATTR_MAX
, genlmsg_attrdata(gnlh
, 0),
256 genlmsg_attrlen(gnlh
, 0), NULL
);
258 if (!tb
[NL80211_ATTR_STA_INFO
])
259 goto nla_put_failure
;
261 if (nla_parse_nested(tb_sta
, NL80211_STA_INFO_MAX
,
262 tb
[NL80211_ATTR_STA_INFO
], NULL
))
263 goto nla_put_failure
;
265 if (tb_sta
[NL80211_STA_INFO_SIGNAL_AVG
])
266 signal
= (int8_t) nla_get_u8(tb_sta
[NL80211_STA_INFO_SIGNAL_AVG
]);
268 usteer_sta_info_update(si
, signal
, true);
275 static int nl80211_scan_result(struct nl_msg
*msg
, void *arg
)
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
},
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
= {
288 struct genlmsghdr
*gnlh
;
289 struct nlattr
*ie_attr
;
293 gnlh
= nlmsg_data(nlmsg_hdr(msg
));
294 nla_parse(tb
, NL80211_ATTR_MAX
, genlmsg_attrdata(gnlh
, 0),
295 genlmsg_attrlen(gnlh
, 0), NULL
);
297 if (!tb
[NL80211_ATTR_BSS
])
300 if (nla_parse_nested(bss
, NL80211_BSS_MAX
, tb
[NL80211_ATTR_BSS
],
304 if (!bss
[NL80211_BSS_BSSID
] ||
305 !bss
[NL80211_BSS_FREQUENCY
])
308 data
.freq
= nla_get_u32(bss
[NL80211_BSS_FREQUENCY
]);
309 memcpy(data
.bssid
, nla_data(bss
[NL80211_BSS_BSSID
]), sizeof(data
.bssid
));
311 if (bss
[NL80211_BSS_SIGNAL_MBM
]) {
312 int32_t signal
= nla_get_u32(bss
[NL80211_BSS_SIGNAL_MBM
]);
313 data
.signal
= signal
/ 100;
316 ie_attr
= bss
[NL80211_BSS_INFORMATION_ELEMENTS
];
318 ie_attr
= bss
[NL80211_BSS_BEACON_IES
];
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 */
331 memcpy(data
.ssid
, ie
+ 2, ie
[1]);
336 req
->cb(req
->priv
, &data
);
341 static int nl80211_scan_event_cb(struct nl_msg
*msg
, void *data
)
343 struct genlmsghdr
*gnlh
= nlmsg_data(nlmsg_hdr(msg
));
346 case NL80211_CMD_NEW_SCAN_RESULTS
:
347 case NL80211_CMD_SCAN_ABORTED
:
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
))
358 struct usteer_local_node
*ln
= container_of(node
, struct usteer_local_node
, node
);
359 struct nl80211_scan_req reqdata
= {
367 if (!ln
->nl80211
.present
)
370 msg
= unl_genl_msg(&unl
, NL80211_CMD_TRIGGER_SCAN
, false);
371 NLA_PUT_U32(msg
, NL80211_ATTR_IFINDEX
, ln
->ifindex
);
374 cur
= nla_nest_start(msg
, NL80211_ATTR_SCAN_SSIDS
);
375 NLA_PUT(msg
, 1, 0, "");
376 nla_nest_end(msg
, cur
);
379 NLA_PUT_U32(msg
, NL80211_ATTR_SCAN_FLAGS
, NL80211_SCAN_FLAG_AP
);
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
);
388 unl_genl_subscribe(&unl
, "scan");
389 ret
= unl_genl_request(&unl
, msg
, NULL
, NULL
);
393 unl_genl_loop(&unl
, nl80211_scan_event_cb
, NULL
);
396 unl_genl_unsubscribe(&unl
, "scan");
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
);
414 static int nl80211_wiphy_result(struct nl_msg
*msg
, void *arg
)
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
;
423 struct genlmsghdr
*gnlh
;
427 gnlh
= nlmsg_data(nlmsg_hdr(msg
));
428 nla_parse(tb
, NL80211_ATTR_MAX
, genlmsg_attrdata(gnlh
, 0),
429 genlmsg_attrlen(gnlh
, 0), NULL
);
431 if (!tb
[NL80211_ATTR_WIPHY_BANDS
])
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
);
438 if (!tb_band
[NL80211_BAND_ATTR_FREQS
])
441 nla_for_each_nested(nl_freq
, tb_band
[NL80211_BAND_ATTR_FREQS
],
443 struct usteer_freq_data f
= {};
445 nla_parse(tb_freq
, NL80211_FREQUENCY_ATTR_MAX
,
446 nla_data(nl_freq
), nla_len(nl_freq
), NULL
);
448 if (tb_freq
[NL80211_FREQUENCY_ATTR_DISABLED
])
451 if (tb_freq
[NL80211_FREQUENCY_ATTR_NO_IR
])
454 cur
= tb_freq
[NL80211_FREQUENCY_ATTR_FREQ
];
458 f
.freq
= nla_get_u32(cur
);
459 f
.dfs
= !!tb_freq
[NL80211_FREQUENCY_ATTR_RADAR
];
461 cur
= tb_freq
[NL80211_FREQUENCY_ATTR_MAX_TX_POWER
];
463 f
.txpower
= nla_get_u32(cur
) / 100;
465 req
->cb(req
->priv
, &f
);
472 static void nl80211_get_freqlist(struct usteer_node
*node
, void *priv
,
473 void (*cb
)(void *priv
, struct usteer_freq_data
*f
))
475 struct usteer_local_node
*ln
= container_of(node
, struct usteer_local_node
, node
);
476 struct nl80211_freqlist_req req
= {
482 if (!ln
->nl80211
.present
)
485 msg
= unl_genl_msg(&unl
, NL80211_CMD_GET_WIPHY
, false);
487 NLA_PUT_U32(msg
, NL80211_ATTR_WIPHY
, ln
->wiphy
);
488 NLA_PUT_FLAG(msg
, NL80211_ATTR_SPLIT_WIPHY_DUMP
);
490 unl_genl_request(&unl
, msg
, nl80211_wiphy_result
, &req
);
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
,
507 static void __usteer_init
usteer_nl80211_init(void)
509 list_add(&nl80211_handler
.list
, &node_handlers
);