diff options
author | Anders Broman <anders.broman@ericsson.com> | 2012-06-20 16:05:34 +0000 |
---|---|---|
committer | Anders Broman <anders.broman@ericsson.com> | 2012-06-20 16:05:34 +0000 |
commit | 9ee113d384fdfb3fefea56e4d9a2fc511b7a729d (patch) | |
tree | 16db3d763f2303257cb4b31eb0f7ea53758cf15b /ws80211_utils.c | |
parent | fe3c093534cc3fac93c9499b6757b5189640e91a (diff) |
Add ws80211_utils.[ch] of bug https://bugs.wireshark.org/bugzilla/show_bug.cgi?id=6973
sligtly modified to compile on Windows. See if it breaks any other platform.
svn path=/trunk/; revision=43410
Diffstat (limited to 'ws80211_utils.c')
-rw-r--r-- | ws80211_utils.c | 660 |
1 files changed, 660 insertions, 0 deletions
diff --git a/ws80211_utils.c b/ws80211_utils.c new file mode 100644 index 0000000000..1599a5f765 --- /dev/null +++ b/ws80211_utils.c @@ -0,0 +1,660 @@ +/* + * ws80211 utilities + * Copyright 2012, Pontus Fuchs <pontus.fuchs@gmail.com> + +$Id$ + +Parts of this file was copied from iw: + +Copyright (c) 2007, 2008 Johannes Berg +Copyright (c) 2007 Andy Lutomirski +Copyright (c) 2007 Mike Kershaw +Copyright (c) 2008-2009 Luis R. Rodriguez + +Permission to use, copy, modify, and/or distribute this software for any +purpose with or without fee is hereby granted, provided that the above +copyright notice and this permission notice appear in all copies. + +THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES +WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF +MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR +ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES +WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN +ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF +OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE. +*/ + +#include <config.h> + +#include <stdio.h> + +#include <glib.h> +#include <glib/gstdio.h> + +#include "ws80211_utils.h" + +#ifdef HAVE_LIBNL +#include <strings.h> +#include <errno.h> +#include <unistd.h> + +#include <net/if.h> +#include <sys/ioctl.h> + +#include <netlink/genl/genl.h> +#include <netlink/genl/family.h> +#include <netlink/genl/ctrl.h> +#include <netlink/msg.h> +#include <netlink/attr.h> + +struct nl80211_state { + struct nl_sock *nl_sock; + int nl80211_id; +}; + +static struct nl80211_state nl_state; + +int ws80211_init(void) +{ + int err; + + struct nl80211_state *state = &nl_state; + + state->nl_sock = nl_socket_alloc(); + if (!state->nl_sock) { + fprintf(stderr, "Failed to allocate netlink socket.\n"); + return -ENOMEM; + } + + if (genl_connect(state->nl_sock)) { + fprintf(stderr, "Failed to connect to generic netlink.\n"); + err = -ENOLINK; + goto out_handle_destroy; + } + + state->nl80211_id = genl_ctrl_resolve(state->nl_sock, "nl80211"); + if (state->nl80211_id < 0) { + fprintf(stderr, "nl80211 not found.\n"); + err = -ENOENT; + goto out_handle_destroy; + } + + return 0; + + out_handle_destroy: + nl_socket_free(state->nl_sock); + state->nl_sock = 0; + return err; +} + +static int error_handler(struct sockaddr_nl *nla _U_, struct nlmsgerr *err, + void *arg) +{ + int *ret = arg; + *ret = err->error; + return NL_STOP; +} + +static int finish_handler(struct nl_msg *msg _U_, void *arg) +{ + int *ret = arg; + *ret = 0; + return NL_SKIP; +} + +static int ack_handler(struct nl_msg *msg _U_, void *arg) +{ + int *ret = arg; + *ret = 0; + return NL_STOP; +} + +static int nl80211_do_cmd(struct nl_msg *msg, struct nl_cb *cb) +{ + int err; + + if (!nl_state.nl_sock) + return -ENOLINK; + + err = nl_send_auto_complete(nl_state.nl_sock, msg); + if (err < 0) + goto out; + + err = 1; + + nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err); + nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err); + nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err); + + while (err > 0) + nl_recvmsgs(nl_state.nl_sock, cb); + out: + nl_cb_put(cb); + + return err; +} + +struct nliface_cookie +{ + char *ifname; + GArray *interfaces; +}; + +static int get_phys_handler(struct nl_msg *msg, void *arg) +{ + struct nlattr *tb_msg[NL80211_ATTR_MAX + 1]; + struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); + + struct nliface_cookie *cookie = arg; + + struct nlattr *tb_band[NL80211_BAND_ATTR_MAX + 1]; + + struct nlattr *tb_freq[NL80211_FREQUENCY_ATTR_MAX + 1]; + static struct nla_policy freq_policy[NL80211_FREQUENCY_ATTR_MAX + 1] = { + [NL80211_FREQUENCY_ATTR_FREQ] = { .type = NLA_U32 }, + [NL80211_FREQUENCY_ATTR_DISABLED] = { .type = NLA_FLAG }, + [NL80211_FREQUENCY_ATTR_PASSIVE_SCAN] = { .type = NLA_FLAG }, + [NL80211_FREQUENCY_ATTR_NO_IBSS] = { .type = NLA_FLAG }, + [NL80211_FREQUENCY_ATTR_RADAR] = { .type = NLA_FLAG }, + [NL80211_FREQUENCY_ATTR_MAX_TX_POWER] = { .type = NLA_U32 }, + }; + + struct nlattr *nl_band; + struct nlattr *nl_freq; + struct nlattr *nl_cmd; + struct nlattr *nl_mode; + int bandidx = 1; + int rem_band, rem_freq, rem_mode; + struct ws80211_interface *iface; + int cap_monitor = 0; + + nla_parse(tb_msg, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), + genlmsg_attrlen(gnlh, 0), NULL); + + if (!tb_msg[NL80211_ATTR_WIPHY_BANDS]) + return NL_SKIP; + + if (tb_msg[NL80211_ATTR_SUPPORTED_IFTYPES]) { + nla_for_each_nested(nl_mode, tb_msg[NL80211_ATTR_SUPPORTED_IFTYPES], rem_mode) { + if (nla_type(nl_mode) == NL80211_IFTYPE_MONITOR) + cap_monitor = 1; + } + } + if (!cap_monitor) + return NL_SKIP; + + iface = g_malloc0(sizeof(*iface)); + if (!iface) + return NL_SKIP; + + iface->frequencies = g_array_new(FALSE, FALSE, sizeof(int)); + iface->channel_types = 1 << NL80211_CHAN_NO_HT; + + if (tb_msg[NL80211_ATTR_WIPHY_NAME]) { + char *phyname; + phyname = g_strdup(nla_get_string(tb_msg[NL80211_ATTR_WIPHY_NAME])); + iface->ifname = g_strdup_printf("%s.mon", phyname); + } + + nla_for_each_nested(nl_band, tb_msg[NL80211_ATTR_WIPHY_BANDS], rem_band) { + bandidx++; + + nla_parse(tb_band, NL80211_BAND_ATTR_MAX, nla_data(nl_band), + nla_len(nl_band), NULL); + + if (tb_band[NL80211_BAND_ATTR_HT_CAPA]) { + gboolean ht40; + iface->channel_types |= 1 << NL80211_CHAN_HT20; + ht40 = !!(nla_get_u16(tb_band[NL80211_BAND_ATTR_HT_CAPA]) & 0x02); + if (ht40) { + iface->channel_types |= 1 << NL80211_CHAN_HT40MINUS; + iface->channel_types |= 1 << NL80211_CHAN_HT40PLUS; + } + } + + nla_for_each_nested(nl_freq, tb_band[NL80211_BAND_ATTR_FREQS], rem_freq) { + uint32_t freq; + nla_parse(tb_freq, NL80211_FREQUENCY_ATTR_MAX, nla_data(nl_freq), + nla_len(nl_freq), freq_policy); + if (!tb_freq[NL80211_FREQUENCY_ATTR_FREQ]) + continue; + if (tb_freq[NL80211_FREQUENCY_ATTR_DISABLED]) + continue; + + freq = nla_get_u32(tb_freq[NL80211_FREQUENCY_ATTR_FREQ]); + g_array_append_val(iface->frequencies, freq); + } + } + + /* Can frequency be set? */ + if (tb_msg[NL80211_ATTR_SUPPORTED_COMMANDS]) { + int cmd; + nla_for_each_nested(nl_cmd, tb_msg[NL80211_ATTR_SUPPORTED_COMMANDS], cmd) { + if(nla_get_u32(nl_cmd) == NL80211_CMD_SET_CHANNEL) + iface->can_set_freq = TRUE; + } + } + g_array_append_val(cookie->interfaces, iface); + + return NL_SKIP; +} + + +static int ws80211_get_phys(GArray *interfaces) +{ + struct nliface_cookie cookie; + struct nl_msg *msg; + struct nl_cb *cb; + msg = nlmsg_alloc(); + if (!msg) { + fprintf(stderr, "failed to allocate netlink message\n"); + return 2; + } + + cb = nl_cb_alloc(NL_CB_DEFAULT); + + cookie.interfaces = interfaces; + + genlmsg_put(msg, 0, 0, nl_state.nl80211_id, 0, + NLM_F_DUMP, NL80211_CMD_GET_WIPHY, 0); + + nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, get_phys_handler, &cookie); + + return nl80211_do_cmd(msg, cb); + +} + +static int get_freq_wext(const char *ifname) +{ + int fd; + int ret = -1; + /* Ugly hack to avoid incuding wireless.h */ + struct { + char name1[IFNAMSIZ]; + __s32 m; + __s16 e; + __u8 i; + __u8 flags; + } wrq; + + fd = socket(AF_INET, SOCK_DGRAM, 0); + if (fd == -1) + return -1; + + strncpy(wrq.name1, ifname, IFNAMSIZ); + /* SIOCGIWFREQ */ + if (ioctl(fd, 0x8B05, &wrq) == 0) { + if (wrq.e == 6) + ret = wrq.m; + } + close(fd); + return ret; +} + +struct __iface_info +{ + struct ws80211_iface_info *pub; + int type; + int phyidx; +}; + +static int get_iface_info_handler(struct nl_msg *msg, void *arg) +{ + struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); + struct nlattr *tb_msg[NL80211_ATTR_MAX + 1]; + struct __iface_info *iface_info = arg; + + nla_parse(tb_msg, NL80211_ATTR_MAX, genlmsg_attrdata(gnlh, 0), + genlmsg_attrlen(gnlh, 0), NULL); + + if (tb_msg[NL80211_ATTR_IFTYPE]) { + iface_info->type = nla_get_u32(tb_msg[NL80211_ATTR_IFTYPE]); + } + if (tb_msg[NL80211_ATTR_WIPHY]) { + iface_info->phyidx = nla_get_u32(tb_msg[NL80211_ATTR_WIPHY]); + } + + if (tb_msg[NL80211_ATTR_WIPHY_FREQ]) { + iface_info->pub->current_freq = nla_get_u32(tb_msg[NL80211_ATTR_WIPHY_FREQ]); + iface_info->pub->current_chan_type = NL80211_CHAN_NO_HT; + + if (tb_msg[NL80211_ATTR_WIPHY_CHANNEL_TYPE]) + iface_info->pub->current_chan_type = nla_get_u32(tb_msg[NL80211_ATTR_WIPHY_CHANNEL_TYPE]); + + } + return NL_SKIP; +} + + +static int __ws80211_get_iface_info(const char *name, struct __iface_info *iface_info) +{ + int devidx; + struct nl_msg *msg; + struct nl_cb *cb; + msg = nlmsg_alloc(); + if (!msg) { + fprintf(stderr, "failed to allocate netlink message\n"); + return 2; + } + + cb = nl_cb_alloc(NL_CB_DEFAULT); + + devidx = if_nametoindex(name); + + genlmsg_put(msg, 0, 0, nl_state.nl80211_id, 0, + 0, NL80211_CMD_GET_INTERFACE, 0); + NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, devidx); + + nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, get_iface_info_handler, iface_info); + + if (nl80211_do_cmd(msg, cb)) + return -1; + + /* Old kernels cant get the current freq via netlink. Try WEXT too :( */ + if (iface_info->pub->current_freq == -1) + iface_info->pub->current_freq = get_freq_wext(name); + return 0; + +nla_put_failure: + fprintf(stderr, "building message failed\n"); + return -1; +} + +int ws80211_get_iface_info(const char *name, struct ws80211_iface_info *iface_info) +{ + struct __iface_info __iface_info; + + memset(iface_info, 0, sizeof(*iface_info)); + __iface_info.pub = iface_info; + __iface_info.type = -1; + __iface_info.phyidx= -1; + __iface_info.pub->current_freq = -1; + __iface_info.pub->current_chan_type = -1; + + return __ws80211_get_iface_info(name, &__iface_info); +} + +static int ws80211_populate_devices(GArray *interfaces) +{ + FILE *fh; + char line[200]; + char *t; + gchar *t2; + char *ret; + int i; + unsigned int j; + + struct ws80211_iface_info pub; + struct __iface_info iface_info; + struct ws80211_interface *iface; + + /* Get a list of phy's that can handle monitor mode */ + ws80211_get_phys(interfaces); + + fh = g_fopen("/proc/net/dev", "r"); + if(!fh) { + fprintf(stderr, "Cannot open /proc/net/dev"); + return -ENOENT; + } + + /* Skip the first two lines */ + for (i = 0; i < 2; i++) { + ret = fgets(line, sizeof(line), fh); + if (ret == NULL) { + fprintf(stderr, "Error parsing /proc/net/dev"); + return -1; + } + } + + /* Update names of user created monitor interfaces */ + while(fgets(line, sizeof(line), fh)) { + t = index(line, ':'); + if (!t) + continue; + *t = 0; + t = line; + while (*t && *t == ' ') + t++; + memset(&iface_info, 0, sizeof(iface_info)); + iface_info.pub = &pub; + __ws80211_get_iface_info(t, &iface_info); + + if (iface_info.type == NL80211_IFTYPE_MONITOR) { + for (j = 0; j < interfaces->len; j++) { + iface = g_array_index(interfaces, struct ws80211_interface *, j); + t2 = g_strdup_printf("phy%d.mon", iface_info.phyidx); + if (t2) { + if (!strcmp(t2, iface->ifname)) { + g_free(iface->ifname); + iface->ifname = g_strdup(t); + } + g_free(t2); + } + } + } + } + fclose(fh); + return 0; +} + +static int ws80211_iface_up(const char *ifname) +{ + int sock; + struct ifreq ifreq; + + sock = socket(AF_PACKET, SOCK_RAW, 0); + if (sock == -1) + return -1; + + strncpy(ifreq.ifr_name, ifname, sizeof(ifreq.ifr_name)); + + if (ioctl(sock, SIOCGIFFLAGS, &ifreq)) + goto out_err; + + ifreq.ifr_flags |= IFF_UP; + + if (ioctl(sock, SIOCSIFFLAGS, &ifreq)) + goto out_err; + + close(sock); + return 0; + +out_err: + close(sock); + return -1; +} + +static int ws80211_create_on_demand_interface(const char *name) +{ + int devidx, phyidx, err; + struct nl_msg *msg; + struct nl_cb *cb; + + devidx = if_nametoindex(name); + if (devidx) + return 0; + + if (sscanf(name, "phy%d.mon", &phyidx) != 1) + return -EINVAL; + + cb = nl_cb_alloc(NL_CB_DEFAULT); + msg = nlmsg_alloc(); + if (!msg) { + fprintf(stderr, "failed to allocate netlink message\n"); + return 2; + } + + genlmsg_put(msg, 0, 0, nl_state.nl80211_id, 0, + 0, NL80211_CMD_NEW_INTERFACE, 0); + NLA_PUT_U32(msg, NL80211_ATTR_WIPHY, phyidx); + + NLA_PUT_STRING(msg, NL80211_ATTR_IFNAME, name); + NLA_PUT_U32(msg, NL80211_ATTR_IFTYPE, NL80211_IFTYPE_MONITOR); + + err = nl80211_do_cmd(msg, cb); + if (err) + return err; + return ws80211_iface_up(name); + +nla_put_failure: + fprintf(stderr, "building message failed\n"); + return 2; +} + +int ws80211_set_freq(const char *name, int freq, int chan_type) +{ + int devidx, err; + struct nl_msg *msg; + struct nl_cb *cb; + + err = ws80211_create_on_demand_interface(name); + if (err) + return err; + + msg = nlmsg_alloc(); + if (!msg) { + fprintf(stderr, "failed to allocate netlink message\n"); + return 2; + } + + cb = nl_cb_alloc(NL_CB_DEFAULT); + + devidx = if_nametoindex(name); + + genlmsg_put(msg, 0, 0, nl_state.nl80211_id, 0, + 0, NL80211_CMD_SET_CHANNEL, 0); + NLA_PUT_U32(msg, NL80211_ATTR_IFINDEX, devidx); + NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_FREQ, freq); + + if (chan_type != -1) + NLA_PUT_U32(msg, NL80211_ATTR_WIPHY_CHANNEL_TYPE, chan_type); + + err = nl80211_do_cmd(msg, cb); + return err; + +nla_put_failure: + fprintf(stderr, "building message failed\n"); + return 2; + +} + +void ws80211_free_interfaces(GArray *interfaces) +{ + struct ws80211_interface *iface; + + if (!interfaces) + return; + + while (interfaces->len) { + iface = g_array_index(interfaces, struct ws80211_interface *, 0); + g_array_remove_index(interfaces, 0); + g_array_free(iface->frequencies, TRUE); + g_free(iface->ifname); + g_free(iface); + } + g_array_free(interfaces, TRUE); +} + +GArray* ws80211_find_interfaces(void) +{ + GArray *interfaces; + + if (!nl_state.nl_sock) + return NULL; + + interfaces = g_array_new(FALSE, FALSE, sizeof(struct ws80211_interface *)); + if (!interfaces) + return NULL; + + if (ws80211_populate_devices(interfaces)) { + ws80211_free_interfaces(interfaces); + return NULL; + } + return interfaces; +} + +int ws80211_frequency_to_channel(int freq) +{ + if (freq == 2484) + return 14; + + if (freq < 2484) + return (freq - 2407) / 5; + + return freq / 5 - 1000; +} + +int +ws80211_str_to_chan_type(gchar *s) +{ + int ret = -1; + if (!s) + return -1; + + if (!strcmp(s, CHAN_NO_HT)) + ret = WS80211_CHAN_NO_HT; + if (!strcmp(s, CHAN_HT20)) + ret = WS80211_CHAN_HT20; + if (!strcmp(s, CHAN_HT40MINUS)) + ret = WS80211_CHAN_HT40MINUS; + if (!strcmp(s, CHAN_HT40PLUS)) + ret = WS80211_CHAN_HT40PLUS; + return ret; +} + +gchar +*ws80211_chan_type_to_str(int type) +{ + switch (type) { + case WS80211_CHAN_NO_HT: + return CHAN_NO_HT; + case WS80211_CHAN_HT20: + return CHAN_HT20; + case WS80211_CHAN_HT40MINUS: + return CHAN_HT40MINUS; + case WS80211_CHAN_HT40PLUS: + return CHAN_HT40PLUS; + } + return NULL; +} + +#else /* HAVE_LIBNL */ +int ws80211_init(void) +{ + return -1; +} + +GArray* ws80211_find_interfaces(void) +{ + return NULL; +} + +int ws80211_get_iface_info(const char *name _U_, struct ws80211_iface_info *iface_info _U_) +{ + return -1; +} + +void ws80211_free_interfaces(GArray *interfaces _U_) +{ +}; + +int ws80211_frequency_to_channel(int freq _U_) +{ + return -1; +} + +int ws80211_set_freq(const char *name _U_, int freq _U_, int chan_type _U_) +{ + return -1; +} + +int ws80211_str_to_chan_type(gchar *s _U_) +{ + return -1; +} + +gchar *ws80211_chan_type_to_str(int type _U_) +{ + return NULL; +} +#endif /* HAVE_LIBNL */ |