diff options
Diffstat (limited to 'package/wprobe/src/user/wprobe.c')
| -rw-r--r-- | package/wprobe/src/user/wprobe.c | 472 |
1 files changed, 472 insertions, 0 deletions
diff --git a/package/wprobe/src/user/wprobe.c b/package/wprobe/src/user/wprobe.c new file mode 100644 index 0000000000..d591207daf --- /dev/null +++ b/package/wprobe/src/user/wprobe.c @@ -0,0 +1,472 @@ +/* + * wprobe.c: Wireless probe user space library + * Copyright (C) 2008-2009 Felix Fietkau <nbd@openwrt.org> + * + * This program is free software; you can redistribute it and/or + * modify it under the terms of the GNU General Public License + * as published by the Free Software Foundation; either version 2 + * of the License, or (at your option) any later version. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <errno.h> +#include <getopt.h> +#include <sys/types.h> +#include <sys/socket.h> +#include <math.h> +#include <linux/wprobe.h> +#include <netlink/netlink.h> +#include <netlink/genl/genl.h> +#include <netlink/genl/ctrl.h> +#include <netlink/genl/family.h> +#include "wprobe.h" + +#define DEBUG 1 +#ifdef DEBUG +#define DPRINTF(fmt, ...) fprintf(stderr, "%s(%d): " fmt, __func__, __LINE__, ##__VA_ARGS__) +#else +#define DPRINTF(fmt, ...) do {} while (0) +#endif + +static struct nl_handle *handle = NULL; +static struct nl_cache *cache = NULL; +static struct genl_family *family = NULL; +static struct nlattr *tb[WPROBE_ATTR_LAST+1]; +static struct nla_policy attribute_policy[WPROBE_ATTR_LAST+1] = { + [WPROBE_ATTR_ID] = { .type = NLA_U32 }, + [WPROBE_ATTR_MAC] = { .type = NLA_UNSPEC, .minlen = 6, .maxlen = 6 }, + [WPROBE_ATTR_NAME] = { .type = NLA_STRING }, + [WPROBE_ATTR_FLAGS] = { .type = NLA_U32 }, + [WPROBE_ATTR_TYPE] = { .type = NLA_U8 }, + [WPROBE_VAL_S8] = { .type = NLA_U8 }, + [WPROBE_VAL_S16] = { .type = NLA_U16 }, + [WPROBE_VAL_S32] = { .type = NLA_U32 }, + [WPROBE_VAL_S64] = { .type = NLA_U64 }, + [WPROBE_VAL_U8] = { .type = NLA_U8 }, + [WPROBE_VAL_U16] = { .type = NLA_U16 }, + [WPROBE_VAL_U32] = { .type = NLA_U32 }, + [WPROBE_VAL_U64] = { .type = NLA_U64 }, + [WPROBE_VAL_SUM] = { .type = NLA_U64 }, + [WPROBE_VAL_SUM_SQ] = { .type = NLA_U64 }, + [WPROBE_VAL_SAMPLES] = { .type = NLA_U32 }, +}; + +static int +error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg) +{ + int *ret = arg; + *ret = err->error; + return NL_STOP; +} + +static int +finish_handler(struct nl_msg *msg, void *arg) +{ + int *ret = arg; + *ret = 0; + return NL_SKIP; +} + +static int +ack_handler(struct nl_msg *msg, void *arg) +{ + int *ret = arg; + *ret = 0; + return NL_STOP; +} + + +void +wprobe_free(void) +{ + if (cache) + nl_cache_free(cache); + if (handle) + nl_handle_destroy(handle); + handle = NULL; + cache = NULL; +} + +int +wprobe_init(void) +{ + handle = nl_handle_alloc(); + if (!handle) { + DPRINTF("Failed to create handle\n"); + goto err; + } + + if (genl_connect(handle)) { + DPRINTF("Failed to connect to generic netlink\n"); + goto err; + } + + cache = genl_ctrl_alloc_cache(handle); + if (!cache) { + DPRINTF("Failed to allocate netlink cache\n"); + goto err; + } + + family = genl_ctrl_search_by_name(cache, "wprobe"); + if (!family) { + DPRINTF("wprobe API not present\n"); + goto err; + } + return 0; + +err: + wprobe_free(); + return -EINVAL; +} + + +static struct nl_msg * +wprobe_new_msg(const char *ifname, int cmd, bool dump) +{ + struct nl_msg *msg; + uint32_t flags = 0; + + msg = nlmsg_alloc(); + if (!msg) + return NULL; + + if (dump) + flags |= NLM_F_DUMP; + + genlmsg_put(msg, 0, 0, genl_family_get_id(family), + 0, flags, cmd, 0); + + NLA_PUT_STRING(msg, WPROBE_ATTR_INTERFACE, ifname); +nla_put_failure: + return msg; +} + +static int +wprobe_send_msg(struct nl_msg *msg, void *callback, void *arg) +{ + struct nl_cb *cb; + int err = 0; + + cb = nl_cb_alloc(NL_CB_DEFAULT); + if (!cb) + goto out_no_cb; + + if (callback) + nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, callback, arg); + + err = nl_send_auto_complete(handle, 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(handle, cb); + +out: + nl_cb_put(cb); +out_no_cb: + nlmsg_free(msg); + return err; +} + +struct wprobe_attr_cb { + struct list_head *list; + char *addr; +}; + +static int +save_attribute_handler(struct nl_msg *msg, void *arg) +{ + struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); + const char *name = "N/A"; + struct wprobe_attribute *attr; + int type = 0; + struct wprobe_attr_cb *cb = arg; + + nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0), + genlmsg_attrlen(gnlh, 0), attribute_policy); + + if (tb[WPROBE_ATTR_NAME]) + name = nla_data(tb[WPROBE_ATTR_NAME]); + + attr = malloc(sizeof(struct wprobe_attribute) + strlen(name) + 1); + if (!attr) + return -1; + + memset(attr, 0, sizeof(struct wprobe_attribute)); + + if (tb[WPROBE_ATTR_ID]) + attr->id = nla_get_u32(tb[WPROBE_ATTR_ID]); + + if (tb[WPROBE_ATTR_MAC] && cb->addr) + memcpy(cb->addr, nla_data(tb[WPROBE_ATTR_MAC]), 6); + + if (tb[WPROBE_ATTR_FLAGS]) + attr->flags = nla_get_u32(tb[WPROBE_ATTR_FLAGS]); + + if (tb[WPROBE_ATTR_TYPE]) + type = nla_get_u8(tb[WPROBE_ATTR_TYPE]); + + if ((type < WPROBE_VAL_STRING) || + (type > WPROBE_VAL_U64)) + type = 0; + + attr->type = type; + strcpy(attr->name, name); + INIT_LIST_HEAD(&attr->list); + list_add(&attr->list, cb->list); + return 0; +} + + +int +wprobe_dump_attributes(const char *ifname, bool link, struct list_head *list, char *addr) +{ + struct nl_msg *msg; + struct wprobe_attr_cb cb; + + cb.list = list; + cb.addr = addr; + msg = wprobe_new_msg(ifname, WPROBE_CMD_GET_LIST, true); + if (!msg) + return -ENOMEM; + + if (link) + NLA_PUT(msg, WPROBE_ATTR_MAC, 6, "\x00\x00\x00\x00\x00\x00"); + + return wprobe_send_msg(msg, save_attribute_handler, &cb); + +nla_put_failure: + nlmsg_free(msg); + return -EINVAL; +} + +static struct wprobe_link * +get_link(struct list_head *list, const char *addr) +{ + struct wprobe_link *l; + + list_for_each_entry(l, list, list) { + if (!memcmp(l->addr, addr, 6)) { + list_del_init(&l->list); + goto out; + } + } + + /* no previous link found, allocate a new one */ + l = malloc(sizeof(struct wprobe_link)); + if (!l) + goto out; + + memset(l, 0, sizeof(struct wprobe_link)); + memcpy(l->addr, addr, sizeof(l->addr)); + INIT_LIST_HEAD(&l->list); + +out: + return l; +} + +struct wprobe_save_cb { + struct list_head *list; + struct list_head old_list; +}; + +static int +save_link_handler(struct nl_msg *msg, void *arg) +{ + struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); + struct wprobe_link *link; + struct wprobe_save_cb *cb = arg; + const char *addr; + + nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0), + genlmsg_attrlen(gnlh, 0), attribute_policy); + + if (!tb[WPROBE_ATTR_MAC] || (nla_len(tb[WPROBE_ATTR_MAC]) != 6)) + return -1; + + addr = nla_data(tb[WPROBE_ATTR_MAC]); + link = get_link(&cb->old_list, addr); + if (!link) + return -1; + + if (tb[WPROBE_ATTR_FLAGS]) + link->flags = nla_get_u32(tb[WPROBE_ATTR_FLAGS]); + + list_add_tail(&link->list, cb->list); + return 0; +} + + +int +wprobe_update_links(const char *ifname, struct list_head *list) +{ + struct wprobe_link *l, *tmp; + struct nl_msg *msg; + struct wprobe_save_cb cb; + int err; + + INIT_LIST_HEAD(&cb.old_list); + list_splice_init(list, &cb.old_list); + cb.list = list; + + msg = wprobe_new_msg(ifname, WPROBE_CMD_GET_LINKS, true); + if (!msg) + return -ENOMEM; + + err = wprobe_send_msg(msg, save_link_handler, &cb); + if (err < 0) + return err; + + list_for_each_entry_safe(l, tmp, &cb.old_list, list) { + list_del(&l->list); + free(l); + } + + return 0; +} + +void +wprobe_measure(const char *ifname) +{ + struct nl_msg *msg; + + msg = wprobe_new_msg(ifname, WPROBE_CMD_MEASURE, false); + if (!msg) + return; + + wprobe_send_msg(msg, NULL, NULL); +} + +struct wprobe_request_cb { + struct list_head *list; + struct list_head old_list; + char *addr; +}; + +static int +save_attrdata_handler(struct nl_msg *msg, void *arg) +{ + struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); + struct wprobe_request_cb *cb = arg; + struct wprobe_attribute *attr; + int type, id; + + nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0), + genlmsg_attrlen(gnlh, 0), attribute_policy); + + if (!tb[WPROBE_ATTR_ID]) + return -1; + + if (!tb[WPROBE_ATTR_TYPE]) + return -1; + + id = nla_get_u32(tb[WPROBE_ATTR_ID]); + list_for_each_entry(attr, &cb->old_list, list) { + if (attr->id == id) + goto found; + } + /* not found */ + return -1; + +found: + list_del_init(&attr->list); + + type = nla_get_u8(tb[WPROBE_ATTR_TYPE]); + if (type != attr->type) { + DPRINTF("WARNING: type mismatch for %s attribute '%s' (%d != %d)\n", + (cb->addr ? "link" : "global"), + attr->name, + type, attr->type); + goto out; + } + + if ((type < WPROBE_VAL_STRING) || + (type > WPROBE_VAL_U64)) + goto out; + + memset(&attr->val, 0, sizeof(attr->val)); + +#define HANDLE_INT_TYPE(_idx, _type) \ + case WPROBE_VAL_S##_type: \ + case WPROBE_VAL_U##_type: \ + attr->val.U##_type = nla_get_u##_type(tb[_idx]); \ + break + + switch(type) { + HANDLE_INT_TYPE(type, 8); + HANDLE_INT_TYPE(type, 16); + HANDLE_INT_TYPE(type, 32); + HANDLE_INT_TYPE(type, 64); + case WPROBE_VAL_STRING: + /* unimplemented */ + break; + } +#undef HANDLE_TYPE + + if (attr->flags & WPROBE_F_KEEPSTAT) { + if (tb[WPROBE_VAL_SUM]) + attr->val.s = nla_get_u64(tb[WPROBE_VAL_SUM]); + + if (tb[WPROBE_VAL_SUM_SQ]) + attr->val.ss = nla_get_u64(tb[WPROBE_VAL_SUM_SQ]); + + if (tb[WPROBE_VAL_SAMPLES]) + attr->val.n = nla_get_u32(tb[WPROBE_VAL_SAMPLES]); + + if (attr->val.n > 0) { + float avg = ((float) attr->val.s) / attr->val.n; + float stdev = sqrt((((float) attr->val.ss) / attr->val.n) - (avg * avg)); + attr->val.avg = avg; + attr->val.stdev = stdev; + } + } + +out: + list_add_tail(&attr->list, cb->list); + return 0; +} + + +int +wprobe_request_data(const char *ifname, struct list_head *attrs, const unsigned char *addr, int scale) +{ + struct wprobe_request_cb cb; + struct nl_msg *msg; + int err; + + msg = wprobe_new_msg(ifname, WPROBE_CMD_GET_INFO, true); + if (!msg) + return -ENOMEM; + + if (scale < 0) + NLA_PUT_U32(msg, WPROBE_ATTR_FLAGS, WPROBE_F_RESET); + else if (scale > 0) + NLA_PUT_U32(msg, WPROBE_ATTR_SCALE, scale); + + if (addr) + NLA_PUT(msg, WPROBE_ATTR_MAC, 6, addr); + +nla_put_failure: + INIT_LIST_HEAD(&cb.old_list); + list_splice_init(attrs, &cb.old_list); + cb.list = attrs; + + err = wprobe_send_msg(msg, save_attrdata_handler, &cb); + list_splice(&cb.old_list, attrs->prev); + return err; +} + + |