3 * Copyright 2012, Pontus Fuchs <pontus.fuchs@gmail.com>
7 Parts of this file was copied from iw:
9 Copyright (c) 2007, 2008 Johannes Berg
10 Copyright (c) 2007 Andy Lutomirski
11 Copyright (c) 2007 Mike Kershaw
12 Copyright (c) 2008-2009 Luis R. Rodriguez
14 Permission to use, copy, modify, and/or distribute this software for any
15 purpose with or without fee is hereby granted, provided that the above
16 copyright notice and this permission notice appear in all copies.
18 THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
19 WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
20 MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
21 ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
22 WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
23 ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
24 OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
32 #include <glib/gstdio.h>
34 #include "ws80211_utils.h"
36 #if defined(HAVE_LIBNL) && defined(HAVE_NL80211)
42 #include <sys/ioctl.h>
44 #include <netlink/genl/genl.h>
45 #include <netlink/genl/family.h>
46 #include <netlink/genl/ctrl.h>
47 #include <netlink/msg.h>
48 #include <netlink/attr.h>
50 #include <linux/nl80211.h>
52 /* libnl 1.x compatibility code */
54 #define nl_sock nl_handle
55 static inline struct nl_handle *nl_socket_alloc(void)
57 return nl_handle_alloc();
60 static inline void nl_socket_free(struct nl_sock *h)
64 #endif /* HAVE_LIBNL1 */
66 struct nl80211_state {
67 struct nl_sock *nl_sock;
71 static struct nl80211_state nl_state;
73 int ws80211_init(void)
77 struct nl80211_state *state = &nl_state;
79 state->nl_sock = nl_socket_alloc();
80 if (!state->nl_sock) {
81 fprintf(stderr, "Failed to allocate netlink socket.\n");
85 if (genl_connect(state->nl_sock)) {
86 fprintf(stderr, "Failed to connect to generic netlink.\n");
88 goto out_handle_destroy;
91 state->nl80211_id = genl_ctrl_resolve(state->nl_sock, "nl80211");
92 if (state->nl80211_id < 0) {
93 fprintf(stderr, "nl80211 not found.\n");
95 goto out_handle_destroy;
101 nl_socket_free(state->nl_sock);
106 static int error_handler(struct sockaddr_nl *nla _U_, struct nlmsgerr *err,
109 int *ret = (int *)arg;
114 static int finish_handler(struct nl_msg *msg _U_, void *arg)
116 int *ret = (int *)arg;
121 static int ack_handler(struct nl_msg *msg _U_, void *arg)
123 int *ret = (int *)arg;
128 static int nl80211_do_cmd(struct nl_msg *msg, struct nl_cb *cb)
132 if (!nl_state.nl_sock)
135 err = nl_send_auto_complete(nl_state.nl_sock, msg);
141 nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
142 nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
143 nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
146 nl_recvmsgs(nl_state.nl_sock, cb);
153 struct nliface_cookie
160 * And now for a steaming heap of suck.
162 * The nla_for_each_nested() macro defined by at least some versions of the
163 * Linux kernel's headers doesn't do the casting required when compiling
164 * with a C++ compiler or with -Wc++-compat, so we get warnings, and those
165 * warnings are fatal when we compile this file.
167 * So we replace it with our own version, which does the requisite cast.
171 * nla_for_each_nested - iterate over nested attributes
172 * @pos: loop counter, set to current attribute
173 * @nla: attribute containing the nested attributes
174 * @rem: initialized to len, holds bytes currently remaining in stream
176 #undef nla_for_each_nested
177 #define nla_for_each_nested(pos, nla, rem) \
178 nla_for_each_attr(pos, (struct nlattr *)nla_data(nla), nla_len(nla), rem)
180 static int get_phys_handler(struct nl_msg *msg, void *arg)
182 struct nlattr *tb_msg[NL80211_ATTR_MAX + 1];
183 struct genlmsghdr *gnlh = (struct genlmsghdr *)nlmsg_data(nlmsg_hdr(msg));
185 struct nliface_cookie *cookie = (struct nliface_cookie *)arg;
187 struct nlattr *tb_band[NL80211_BAND_ATTR_MAX + 1];
189 struct nlattr *tb_freq[NL80211_FREQUENCY_ATTR_MAX + 1];
190 static struct nla_policy freq_policy[NL80211_FREQUENCY_ATTR_MAX + 1] = {
191 [NL80211_FREQUENCY_ATTR_FREQ] = { .type = NLA_U32 },
192 [NL80211_FREQUENCY_ATTR_DISABLED] = { .type = NLA_FLAG },
193 [NL80211_FREQUENCY_ATTR_PASSIVE_SCAN] = { .type = NLA_FLAG },
194 [NL80211_FREQUENCY_ATTR_NO_IBSS] = { .type = NLA_FLAG },
195 [NL80211_FREQUENCY_ATTR_RADAR] = { .type = NLA_FLAG },
196 [NL80211_FREQUENCY_ATTR_MAX_TX_POWER] = { .type = NLA_U32 },
199 struct nlattr *nl_band;
200 struct nlattr *nl_freq;
201 struct nlattr *nl_mode;
203 int rem_band, rem_freq, rem_mode;
204 struct ws80211_interface *iface;
207 nla_parse(tb_msg, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
208 genlmsg_attrlen(gnlh, 0), NULL);
210 if (!tb_msg[NL80211_ATTR_WIPHY_BANDS])
213 if (tb_msg[NL80211_ATTR_SUPPORTED_IFTYPES]) {
214 nla_for_each_nested(nl_mode, tb_msg[NL80211_ATTR_SUPPORTED_IFTYPES], rem_mode) {
215 if (nla_type(nl_mode) == NL80211_IFTYPE_MONITOR)
222 iface = (struct ws80211_interface *)g_malloc0(sizeof(*iface));
226 iface->frequencies = g_array_new(FALSE, FALSE, sizeof(int));
227 iface->channel_types = 1 << WS80211_CHAN_NO_HT;
229 if (tb_msg[NL80211_ATTR_WIPHY_NAME]) {
230 iface->ifname = g_strdup_printf("%s.mon",
231 nla_get_string(tb_msg[NL80211_ATTR_WIPHY_NAME]));
234 nla_for_each_nested(nl_band, tb_msg[NL80211_ATTR_WIPHY_BANDS], rem_band) {
237 nla_parse(tb_band, NL80211_BAND_ATTR_MAX,
238 (struct nlattr *)nla_data(nl_band),
239 nla_len(nl_band), NULL);
241 #ifdef NL80211_BAND_ATTR_HT_CAPA
242 if (tb_band[NL80211_BAND_ATTR_HT_CAPA]) {
244 iface->channel_types |= 1 << WS80211_CHAN_HT20;
245 ht40 = !!(nla_get_u16(tb_band[NL80211_BAND_ATTR_HT_CAPA]) & 0x02);
247 iface->channel_types |= 1 << WS80211_CHAN_HT40MINUS;
248 iface->channel_types |= 1 << WS80211_CHAN_HT40PLUS;
251 #endif /* NL80211_BAND_ATTR_HT_CAPA */
253 nla_for_each_nested(nl_freq, tb_band[NL80211_BAND_ATTR_FREQS], rem_freq) {
255 nla_parse(tb_freq, NL80211_FREQUENCY_ATTR_MAX,
256 (struct nlattr *)nla_data(nl_freq),
257 nla_len(nl_freq), freq_policy);
258 if (!tb_freq[NL80211_FREQUENCY_ATTR_FREQ])
260 if (tb_freq[NL80211_FREQUENCY_ATTR_DISABLED])
263 freq = nla_get_u32(tb_freq[NL80211_FREQUENCY_ATTR_FREQ]);
264 g_array_append_val(iface->frequencies, freq);
268 /* Can frequency be set? Only newer versions of cfg80211 supports this */
269 #ifdef HAVE_NL80211_CMD_SET_CHANNEL
270 if (tb_msg[NL80211_ATTR_SUPPORTED_COMMANDS]) {
272 struct nlattr *nl_cmd;
273 nla_for_each_nested(nl_cmd, tb_msg[NL80211_ATTR_SUPPORTED_COMMANDS], cmd) {
274 if(nla_get_u32(nl_cmd) == NL80211_CMD_SET_CHANNEL)
275 iface->can_set_freq = TRUE;
279 iface->can_set_freq = TRUE;
281 g_array_append_val(cookie->interfaces, iface);
287 static int ws80211_get_phys(GArray *interfaces)
289 struct nliface_cookie cookie;
294 fprintf(stderr, "failed to allocate netlink message\n");
298 cb = nl_cb_alloc(NL_CB_DEFAULT);
300 cookie.interfaces = interfaces;
302 genlmsg_put(msg, 0, 0, nl_state.nl80211_id, 0,
303 NLM_F_DUMP, NL80211_CMD_GET_WIPHY, 0);
305 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, get_phys_handler, &cookie);
307 return nl80211_do_cmd(msg, cb);
311 static int get_freq_wext(const char *ifname)
315 /* Ugly hack to avoid incuding wireless.h */
317 char name1[IFNAMSIZ];
324 fd = socket(AF_INET, SOCK_DGRAM, 0);
328 g_strlcpy(wrq.name1, ifname, IFNAMSIZ);
330 if (ioctl(fd, 0x8B05, &wrq) == 0) {
340 struct ws80211_iface_info *pub;
345 static int get_iface_info_handler(struct nl_msg *msg, void *arg)
347 struct genlmsghdr *gnlh = (struct genlmsghdr *)nlmsg_data(nlmsg_hdr(msg));
348 struct nlattr *tb_msg[NL80211_ATTR_MAX + 1];
349 struct __iface_info *iface_info = (struct __iface_info *)arg;
351 nla_parse(tb_msg, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0),
352 genlmsg_attrlen(gnlh, 0), NULL);
354 if (tb_msg[NL80211_ATTR_IFTYPE]) {
355 iface_info->type = nla_get_u32(tb_msg[NL80211_ATTR_IFTYPE]);
357 if (tb_msg[NL80211_ATTR_WIPHY]) {
358 iface_info->phyidx = nla_get_u32(tb_msg[NL80211_ATTR_WIPHY]);
361 if (tb_msg[NL80211_ATTR_WIPHY_FREQ]) {
362 iface_info->pub->current_freq = nla_get_u32(tb_msg[NL80211_ATTR_WIPHY_FREQ]);
363 iface_info->pub->current_chan_type = WS80211_CHAN_NO_HT;
365 if (tb_msg[NL80211_ATTR_WIPHY_CHANNEL_TYPE]) {
366 switch (nla_get_u32(tb_msg[NL80211_ATTR_WIPHY_CHANNEL_TYPE])) {
368 case NL80211_CHAN_NO_HT:
369 iface_info->pub->current_chan_type = WS80211_CHAN_NO_HT;
372 case NL80211_CHAN_HT20:
373 iface_info->pub->current_chan_type = WS80211_CHAN_HT20;
376 case NL80211_CHAN_HT40MINUS:
377 iface_info->pub->current_chan_type = WS80211_CHAN_HT40MINUS;
380 case NL80211_CHAN_HT40PLUS:
381 iface_info->pub->current_chan_type = WS80211_CHAN_HT40PLUS;
391 static int __ws80211_get_iface_info(const char *name, struct __iface_info *iface_info)
398 fprintf(stderr, "failed to allocate netlink message\n");
402 cb = nl_cb_alloc(NL_CB_DEFAULT);
404 devidx = if_nametoindex(name);
406 genlmsg_put(msg, 0, 0, nl_state.nl80211_id, 0,
407 0, NL80211_CMD_GET_INTERFACE, 0);
408 NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, devidx);
410 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, get_iface_info_handler, iface_info);
412 if (nl80211_do_cmd(msg, cb))
415 /* Old kernels cant get the current freq via netlink. Try WEXT too :( */
416 if (iface_info->pub->current_freq == -1)
417 iface_info->pub->current_freq = get_freq_wext(name);
421 fprintf(stderr, "building message failed\n");
425 int ws80211_get_iface_info(const char *name, struct ws80211_iface_info *iface_info)
427 struct __iface_info __iface_info;
429 memset(iface_info, 0, sizeof(*iface_info));
430 __iface_info.pub = iface_info;
431 __iface_info.type = -1;
432 __iface_info.phyidx= -1;
433 __iface_info.pub->current_freq = -1;
434 __iface_info.pub->current_chan_type = WS80211_CHAN_NO_HT;
436 return __ws80211_get_iface_info(name, &__iface_info);
439 static int ws80211_populate_devices(GArray *interfaces)
449 struct ws80211_iface_info pub;
450 struct __iface_info iface_info;
451 struct ws80211_interface *iface;
453 /* Get a list of phy's that can handle monitor mode */
454 ws80211_get_phys(interfaces);
456 fh = g_fopen("/proc/net/dev", "r");
458 fprintf(stderr, "Cannot open /proc/net/dev");
462 /* Skip the first two lines */
463 for (i = 0; i < 2; i++) {
464 ret = fgets(line, sizeof(line), fh);
466 fprintf(stderr, "Error parsing /proc/net/dev");
472 /* Update names of user created monitor interfaces */
473 while(fgets(line, sizeof(line), fh)) {
474 t = index(line, ':');
479 while (*t && *t == ' ')
481 memset(&iface_info, 0, sizeof(iface_info));
482 iface_info.pub = &pub;
483 __ws80211_get_iface_info(t, &iface_info);
485 if (iface_info.type == NL80211_IFTYPE_MONITOR) {
486 for (j = 0; j < interfaces->len; j++) {
487 iface = g_array_index(interfaces, struct ws80211_interface *, j);
488 t2 = g_strdup_printf("phy%d.mon", iface_info.phyidx);
490 if (!strcmp(t2, iface->ifname)) {
491 g_free(iface->ifname);
492 iface->ifname = g_strdup(t);
503 static int ws80211_iface_up(const char *ifname)
508 sock = socket(AF_PACKET, SOCK_RAW, 0);
512 g_strlcpy(ifreq.ifr_name, ifname, sizeof(ifreq.ifr_name));
514 if (ioctl(sock, SIOCGIFFLAGS, &ifreq))
517 ifreq.ifr_flags |= IFF_UP;
519 if (ioctl(sock, SIOCSIFFLAGS, &ifreq))
530 static int ws80211_create_on_demand_interface(const char *name)
532 int devidx, phyidx, err;
536 devidx = if_nametoindex(name);
538 return ws80211_iface_up(name);
540 if (sscanf(name, "phy%d.mon", &phyidx) != 1)
543 cb = nl_cb_alloc(NL_CB_DEFAULT);
546 fprintf(stderr, "failed to allocate netlink message\n");
550 genlmsg_put(msg, 0, 0, nl_state.nl80211_id, 0,
551 0, NL80211_CMD_NEW_INTERFACE, 0);
552 NLA_PUT_U32(msg, NL80211_ATTR_WIPHY, phyidx);
554 NLA_PUT_STRING(msg, NL80211_ATTR_IFNAME, name);
555 NLA_PUT_U32(msg, NL80211_ATTR_IFTYPE, NL80211_IFTYPE_MONITOR);
557 err = nl80211_do_cmd(msg, cb);
560 return ws80211_iface_up(name);
563 fprintf(stderr, "building message failed\n");
567 int ws80211_set_freq(const char *name, int freq, int chan_type)
573 err = ws80211_create_on_demand_interface(name);
579 fprintf(stderr, "failed to allocate netlink message\n");
583 cb = nl_cb_alloc(NL_CB_DEFAULT);
585 devidx = if_nametoindex(name);
587 #ifdef HAVE_NL80211_CMD_SET_CHANNEL
588 genlmsg_put(msg, 0, 0, nl_state.nl80211_id, 0,
589 0, NL80211_CMD_SET_CHANNEL, 0);
591 genlmsg_put(msg, 0, 0, nl_state.nl80211_id, 0,
592 0, NL80211_CMD_SET_WIPHY, 0);
595 NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, devidx);
596 NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_FREQ, freq);
600 #ifdef NL80211_BAND_ATTR_HT_CAPA
601 case WS80211_CHAN_NO_HT:
602 NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_CHANNEL_TYPE, NL80211_CHAN_NO_HT);
605 case WS80211_CHAN_HT20:
606 NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_CHANNEL_TYPE, NL80211_CHAN_HT20);
609 case WS80211_CHAN_HT40MINUS:
610 NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_CHANNEL_TYPE, NL80211_CHAN_HT40MINUS);
613 case WS80211_CHAN_HT40PLUS:
614 NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_CHANNEL_TYPE, NL80211_CHAN_HT40PLUS);
621 err = nl80211_do_cmd(msg, cb);
625 fprintf(stderr, "building message failed\n");
630 void ws80211_free_interfaces(GArray *interfaces)
632 struct ws80211_interface *iface;
637 while (interfaces->len) {
638 iface = g_array_index(interfaces, struct ws80211_interface *, 0);
639 g_array_remove_index(interfaces, 0);
640 g_array_free(iface->frequencies, TRUE);
641 g_free(iface->ifname);
644 g_array_free(interfaces, TRUE);
647 GArray* ws80211_find_interfaces(void)
651 if (!nl_state.nl_sock)
654 interfaces = g_array_new(FALSE, FALSE, sizeof(struct ws80211_interface *));
658 if (ws80211_populate_devices(interfaces)) {
659 ws80211_free_interfaces(interfaces);
665 int ws80211_frequency_to_channel(int freq)
671 return (freq - 2407) / 5;
673 return freq / 5 - 1000;
677 ws80211_str_to_chan_type(const gchar *s)
683 if (!strcmp(s, CHAN_NO_HT))
684 ret = WS80211_CHAN_NO_HT;
685 if (!strcmp(s, CHAN_HT20))
686 ret = WS80211_CHAN_HT20;
687 if (!strcmp(s, CHAN_HT40MINUS))
688 ret = WS80211_CHAN_HT40MINUS;
689 if (!strcmp(s, CHAN_HT40PLUS))
690 ret = WS80211_CHAN_HT40PLUS;
695 *ws80211_chan_type_to_str(int type)
698 case WS80211_CHAN_NO_HT:
700 case WS80211_CHAN_HT20:
702 case WS80211_CHAN_HT40MINUS:
703 return CHAN_HT40MINUS;
704 case WS80211_CHAN_HT40PLUS:
705 return CHAN_HT40PLUS;
710 #else /* HAVE_LIBNL */
711 int ws80211_init(void)
716 GArray* ws80211_find_interfaces(void)
721 int ws80211_get_iface_info(const char *name _U_, struct ws80211_iface_info *iface_info _U_)
726 void ws80211_free_interfaces(GArray *interfaces _U_)
730 int ws80211_frequency_to_channel(int freq _U_)
735 int ws80211_set_freq(const char *name _U_, int freq _U_, int chan_type _U_)
740 int ws80211_str_to_chan_type(const gchar *s _U_)
745 const gchar *ws80211_chan_type_to_str(int type _U_)
749 #endif /* HAVE_LIBNL && HAVE_NL80211 */