diff options
Diffstat (limited to 'package/wprobe/src/user/wprobe-lib.c')
| -rw-r--r-- | package/wprobe/src/user/wprobe-lib.c | 1203 |
1 files changed, 1203 insertions, 0 deletions
diff --git a/package/wprobe/src/user/wprobe-lib.c b/package/wprobe/src/user/wprobe-lib.c new file mode 100644 index 000000000..a3b0fb5e5 --- /dev/null +++ b/package/wprobe/src/user/wprobe-lib.c @@ -0,0 +1,1203 @@ +/* + * 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 <sys/types.h> +#include <sys/socket.h> +#include <stdio.h> +#include <string.h> +#include <stdlib.h> +#include <errno.h> +#include <getopt.h> +#include <unistd.h> +#include <stdbool.h> +#include <math.h> +#include <linux/wprobe.h> +#include <netlink/netlink.h> +#include <netlink/attr.h> +#include <netlink/genl/genl.h> +#ifndef NO_LOCAL_ACCESS +#include <netlink/genl/ctrl.h> +#include <netlink/genl/family.h> +#include <endian.h> +#endif +#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 + +#if defined(BYTE_ORDER) && !defined(__BYTE_ORDER) +#define __LITTLE_ENDIAN LITTLE_ENDIAN +#define __BIG_ENDIAN BIG_ENDIAN +#define __BYTE_ORDER BYTE_ORDER +#endif + +#ifndef __BYTE_ORDER +#error Unknown endian type +#endif + +#define WPROBE_MAX_MSGLEN 65536 + +static inline __u16 __swab16(__u16 x) +{ + return x<<8 | x>>8; +} + +static inline __u32 __swab32(__u32 x) +{ + return x<<24 | x>>24 | + (x & (__u32)0x0000ff00UL)<<8 | + (x & (__u32)0x00ff0000UL)>>8; +} + +static inline __u64 __swab64(__u64 x) +{ + return x<<56 | x>>56 | + (x & (__u64)0x000000000000ff00ULL)<<40 | + (x & (__u64)0x0000000000ff0000ULL)<<24 | + (x & (__u64)0x00000000ff000000ULL)<< 8 | + (x & (__u64)0x000000ff00000000ULL)>> 8 | + (x & (__u64)0x0000ff0000000000ULL)>>24 | + (x & (__u64)0x00ff000000000000ULL)>>40; +} + + +#if __BYTE_ORDER == __LITTLE_ENDIAN +#define SWAP16(var) var = __swab16(var) +#define SWAP32(var) var = __swab32(var) +#define SWAP64(var) var = __swab64(var) +#else +#define SWAP16(var) do {} while(0) +#define SWAP32(var) do {} while(0) +#define SWAP64(var) do {} while(0) +#endif + +int wprobe_port = 17990; +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_ATTR_FLAGS] = { .type = NLA_U32 }, + [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 }, + [WPROBE_VAL_SCALE_TIME] = { .type = NLA_U64 }, + [WPROBE_ATTR_INTERVAL] = { .type = NLA_U64 }, + [WPROBE_ATTR_SAMPLES_MIN] = { .type = NLA_U32 }, + [WPROBE_ATTR_SAMPLES_MAX] = { .type = NLA_U32 }, + [WPROBE_ATTR_SAMPLES_SCALE_M] = { .type = NLA_U32 }, + [WPROBE_ATTR_SAMPLES_SCALE_D] = { .type = NLA_U32 }, + [WPROBE_ATTR_FILTER_GROUP] = { .type = NLA_NESTED }, + [WPROBE_ATTR_RXCOUNT] = { .type = NLA_U64 }, + [WPROBE_ATTR_TXCOUNT] = { .type = NLA_U64 }, +}; + +typedef int (*wprobe_cb_t)(struct nl_msg *, void *); + +struct wprobe_iface_ops { + int (*send_msg)(struct wprobe_iface *dev, struct nl_msg *msg, wprobe_cb_t cb, void *arg); + void (*free)(struct wprobe_iface *dev); +}; + +struct wprobe_attr_cb { + struct list_head *list; + char *addr; +}; + +#define WPROBE_MAGIC_STR "WPROBE" +struct wprobe_init_hdr { + struct { + char magic[sizeof(WPROBE_MAGIC_STR)]; + + /* protocol version */ + uint8_t version; + + /* extra header length (unused for now) */ + uint16_t extra; + } pre __attribute__((packed)); + union { + struct { + uint16_t genl_family; + } v0 __attribute__((packed)); + }; +} __attribute__((packed)); + +struct wprobe_msg_hdr { + __u16 status; + __u16 error; + __u32 len; +}; + +enum wprobe_resp_status { + WPROBE_MSG_DONE = 0, + WPROBE_MSG_DATA = 1, +}; + +static inline void +wprobe_swap_msg_hdr(struct wprobe_msg_hdr *mhdr) +{ + SWAP16(mhdr->status); + SWAP16(mhdr->error); + SWAP32(mhdr->len); +} + +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; +} + +static struct nl_msg * +wprobe_new_msg(struct wprobe_iface *dev, int cmd, bool dump) +{ + struct nl_msg *msg; + uint32_t flags = 0; + + msg = nlmsg_alloc_size(65536); + if (!msg) + return NULL; + + if (dump) + flags |= NLM_F_DUMP; + + genlmsg_put(msg, 0, 0, dev->genl_family, + 0, flags, cmd, 0); + + NLA_PUT_STRING(msg, WPROBE_ATTR_INTERFACE, dev->ifname); +nla_put_failure: + return msg; +} + + +static int +dump_attributes(struct wprobe_iface *dev, 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(dev, 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 dev->ops->send_msg(dev, msg, save_attribute_handler, &cb); + +nla_put_failure: + nlmsg_free(msg); + return -EINVAL; +} + +static struct wprobe_iface * +wprobe_alloc_dev(void) +{ + struct wprobe_iface *dev; + + dev = malloc(sizeof(struct wprobe_iface)); + if (!dev) + return NULL; + + memset(dev, 0, sizeof(struct wprobe_iface)); + + dev->interval = -1; + dev->scale_min = -1; + dev->scale_max = -1; + dev->scale_m = -1; + dev->scale_d = -1; + dev->sockfd = -1; + + INIT_LIST_HEAD(&dev->global_attr); + INIT_LIST_HEAD(&dev->link_attr); + INIT_LIST_HEAD(&dev->links); + return dev; +} + +static int +wprobe_init_dev(struct wprobe_iface *dev) +{ + dump_attributes(dev, false, &dev->global_attr, NULL); + dump_attributes(dev, true, &dev->link_attr, NULL); + return 0; +} + +#ifndef NO_LOCAL_ACCESS +static int n_devs = 0; +static struct nl_sock *handle = NULL; +static struct nl_cache *cache = NULL; +static struct genl_family *family = NULL; + +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; +} + +static void +wprobe_local_free(struct wprobe_iface *dev) +{ + /* should not happen */ + if (n_devs == 0) + return; + + if (--n_devs != 0) + return; + + if (cache) + nl_cache_free(cache); + if (handle) + nl_socket_free(handle); + handle = NULL; + cache = NULL; +} + +static int +wprobe_local_init(void) +{ + int ret; + + if (n_devs++ > 0) + return 0; + + handle = nl_socket_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; + } + + ret = genl_ctrl_alloc_cache(handle, &cache); + if (ret < 0) { + 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_local_free(NULL); + return -EINVAL; +} + + +static int +wprobe_local_send_msg(struct wprobe_iface *dev, struct nl_msg *msg, wprobe_cb_t 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; +} + +static const struct wprobe_iface_ops wprobe_local_ops = { + .send_msg = wprobe_local_send_msg, + .free = wprobe_local_free, +}; + +struct wprobe_iface * +wprobe_get_dev(const char *ifname) +{ + struct wprobe_iface *dev; + + if (wprobe_local_init() != 0) + return NULL; + + dev = wprobe_alloc_dev(); + if (!dev) + goto error_alloc; + + dev->ifname = strdup(ifname); + dev->ops = &wprobe_local_ops; + dev->genl_family = genl_family_get_id(family); + + if (wprobe_init_dev(dev) < 0) + goto error; + + return dev; + +error: + free(dev); +error_alloc: + wprobe_local_free(NULL); + return NULL; +} + +#endif + +static void swap_nlmsghdr(struct nlmsghdr *nlh) +{ + SWAP32(nlh->nlmsg_len); + SWAP16(nlh->nlmsg_type); + SWAP16(nlh->nlmsg_flags); + SWAP32(nlh->nlmsg_seq); + SWAP32(nlh->nlmsg_pid); +} + +static void swap_genlmsghdr(struct genlmsghdr *gnlh) +{ +#if 0 /* probably unnecessary */ + SWAP16(gnlh->reserved); +#endif +} + +static void +wprobe_swap_nested(void *data, int len, bool outgoing) +{ + void *end = data + len; + + while (data < end) { + struct nlattr *nla = data; + unsigned int type, len; + + if (!outgoing) { + SWAP16(nla->nla_len); + SWAP16(nla->nla_type); + + /* required for further sanity checks */ + if (data + nla->nla_len > end) + nla->nla_len = end - data; + } + + len = NLA_ALIGN(nla->nla_len); + type = nla->nla_type & NLA_TYPE_MASK; + + if (type <= WPROBE_ATTR_LAST) { +#if __BYTE_ORDER == __LITTLE_ENDIAN + switch(attribute_policy[type].type) { + case NLA_U16: + SWAP16(*(__u16 *)nla_data(nla)); + break; + case NLA_U32: + SWAP32(*(__u32 *)nla_data(nla)); + break; + case NLA_U64: + SWAP64(*(__u64 *)nla_data(nla)); + break; + case NLA_NESTED: + wprobe_swap_nested(nla_data(nla), nla_len(nla), outgoing); + break; + } +#endif + } + data += len; + + if (outgoing) { + SWAP16(nla->nla_len); + SWAP16(nla->nla_type); + } + if (!nla->nla_len) + break; + } +} + +static struct nl_msg * +wprobe_msg_from_network(int socket, int len) +{ + struct genlmsghdr *gnlh; + struct nlmsghdr *nlh; + struct nl_msg *msg; + void *data; + + msg = nlmsg_alloc_size(len + 32); + if (!msg) + return NULL; + + nlh = nlmsg_hdr(msg); + if (read(socket, nlh, len) != len) + goto free; + + swap_nlmsghdr(nlh); + if (nlh->nlmsg_len > len) + goto free; + + gnlh = nlmsg_data(nlh); + swap_genlmsghdr(gnlh); + + data = genlmsg_data(gnlh); + wprobe_swap_nested(data, genlmsg_len(gnlh), false); + + return msg; +free: + nlmsg_free(msg); + return NULL; +} + +static int +wprobe_msg_to_network(int socket, struct nl_msg *msg) +{ + struct nlmsghdr *nlh = nlmsg_hdr(msg); + struct wprobe_msg_hdr mhdr; + struct genlmsghdr *gnlh; + void *buf, *data; + int buflen, datalen; + int ret; + + buflen = nlh->nlmsg_len; + buf = malloc(buflen); + if (!buf) + return -ENOMEM; + + memset(&mhdr, 0, sizeof(mhdr)); + mhdr.status = WPROBE_MSG_DATA; + mhdr.len = buflen; + wprobe_swap_msg_hdr(&mhdr); + write(socket, &mhdr, sizeof(mhdr)); + + memcpy(buf, nlh, buflen); + nlh = buf; + gnlh = nlmsg_data(nlh); + data = genlmsg_data(gnlh); + datalen = genlmsg_len(gnlh); + + wprobe_swap_nested(data, datalen, true); + swap_genlmsghdr(gnlh); + swap_nlmsghdr(nlh); + ret = write(socket, buf, buflen); + free(buf); + + return ret; +} + +static int +wprobe_remote_send_msg(struct wprobe_iface *dev, struct nl_msg *msg, wprobe_cb_t callback, void *arg) +{ + struct wprobe_msg_hdr mhdr; + int msgs = 0; + + wprobe_msg_to_network(dev->sockfd, msg); + nlmsg_free(msg); + do { + if (read(dev->sockfd, &mhdr, sizeof(mhdr)) != sizeof(mhdr)) { + DPRINTF("Failed to read response header\n"); + return -1; + } + wprobe_swap_msg_hdr(&mhdr); + + switch(mhdr.status) { + case WPROBE_MSG_DATA: + if (mhdr.len > WPROBE_MAX_MSGLEN) { + fprintf(stderr, "Invalid length in received response message.\n"); + exit(1); + } + + msg = wprobe_msg_from_network(dev->sockfd, mhdr.len); + if (!msg) + return -EINVAL; + + msgs++; + callback(msg, arg); + nlmsg_free(msg); + break; + } + } while (mhdr.status != WPROBE_MSG_DONE); + + if (mhdr.error) + return -mhdr.error; + else + return msgs; +} + + +static void +wprobe_socket_dev_free(struct wprobe_iface *dev) +{ + if (dev->sockfd >= 0) + close(dev->sockfd); +} + +static const struct wprobe_iface_ops wprobe_remote_ops = { + .send_msg = wprobe_remote_send_msg, + .free = wprobe_socket_dev_free, +}; + + +#ifndef NO_LOCAL_ACCESS +int +wprobe_server_init(int socket) +{ + struct wprobe_init_hdr hdr; + int ret; + + ret = wprobe_local_init(); + if (ret != 0) + return ret; + + memset(&hdr, 0, sizeof(hdr)); + memcpy(hdr.pre.magic, WPROBE_MAGIC_STR, sizeof(WPROBE_MAGIC_STR)); + hdr.pre.version = 0; + hdr.v0.genl_family = genl_family_get_id(family); + SWAP16(hdr.v0.genl_family); + write(socket, (unsigned char *)&hdr, sizeof(hdr)); + + return 0; +} + +static int +wprobe_server_cb(struct nl_msg *msg, void *arg) +{ + int *socket = arg; + int ret; + + ret = wprobe_msg_to_network(*socket, msg); + if (ret > 0) + ret = 0; + + return ret; +} + + +int +wprobe_server_handle(int socket) +{ + struct wprobe_msg_hdr mhdr; + struct nl_msg *msg; + int ret; + + ret = read(socket, &mhdr, sizeof(mhdr)); + if (ret != sizeof(mhdr)) { + if (ret <= 0) + return -1; + + DPRINTF("Failed to read request header\n"); + return -EINVAL; + } + wprobe_swap_msg_hdr(&mhdr); + + switch(mhdr.status) { + case WPROBE_MSG_DATA: + if (mhdr.len > WPROBE_MAX_MSGLEN) { + DPRINTF("Invalid length in received response message.\n"); + return -EINVAL; + } + msg = wprobe_msg_from_network(socket, mhdr.len); + break; + default: + DPRINTF("Invalid request header type\n"); + return -ENOENT; + } + + if (!msg) { + DPRINTF("Failed to get message\n"); + return -EINVAL; + } + + ret = wprobe_local_send_msg(NULL, msg, wprobe_server_cb, &socket); + + memset(&mhdr, 0, sizeof(mhdr)); + mhdr.status = WPROBE_MSG_DONE; + if (ret < 0) + mhdr.error = (uint16_t) -ret; + + ret = write(socket, (unsigned char *)&mhdr, sizeof(mhdr)); + if (ret > 0) + ret = 0; + + return ret; +} + +void +wprobe_server_done(void) +{ + wprobe_local_free(NULL); +} +#endif + +struct wprobe_iface * +wprobe_get_from_socket(int socket, const char *name) +{ + struct wprobe_iface *dev; + struct wprobe_init_hdr hdr; + + dev = wprobe_alloc_dev(); + if (!dev) + goto out; + + dev->ops = &wprobe_remote_ops; + dev->sockfd = socket; + dev->ifname = strdup(name); + + /* read version and header length */ + if (read(socket, &hdr.pre, sizeof(hdr.pre)) != sizeof(hdr.pre)) { + DPRINTF("Could not read header\n"); + goto error; + } + + /* magic not found */ + if (memcmp(hdr.pre.magic, WPROBE_MAGIC_STR, sizeof(hdr.pre.magic)) != 0) { + DPRINTF("Magic does not match\n"); + goto error; + } + + /* unsupported version */ + if (hdr.pre.version != 0) { + DPRINTF("Protocol version does not match\n"); + goto error; + } + + if (read(socket, &hdr.v0, sizeof(hdr.v0)) != sizeof(hdr.v0)) { + DPRINTF("Could not read header data\n"); + goto error; + } + + SWAP16(hdr.pre.extra); + SWAP16(hdr.v0.genl_family); + dev->genl_family = hdr.v0.genl_family; + + if (wprobe_init_dev(dev) < 0) { + DPRINTF("Could not initialize device\n"); + goto error; + } + +out: + return dev; + +error: + wprobe_free_dev(dev); + return NULL; +} + +struct wprobe_iface * +wprobe_get_auto(const char *arg, char **err) +{ + static struct sockaddr_in sa; + static char errbuf[512]; + + struct wprobe_iface *dev = NULL; + struct hostent *h; + char *devstr = strdup(arg); + char *sep = NULL; + int sock = -1; + int len; + + if (err) + *err = NULL; + + sep = strchr(devstr, ':'); + if (!sep) { +#ifndef NO_LOCAL_ACCESS + free(devstr); + return wprobe_get_dev(arg); +#else + *err = "Invalid argument"; + goto out; +#endif + } + + *sep = 0; + sep++; + + sock = socket(AF_INET, SOCK_STREAM, 0); + if (sock < 0) + goto syserr; + + h = gethostbyname(devstr); + if (!h) { + sprintf(errbuf, "Host not found"); + goto out_err; + } + + memcpy(&sa.sin_addr, h->h_addr, h->h_length); + sa.sin_family = AF_INET; + sa.sin_port = htons(wprobe_port); + if (connect(sock, (struct sockaddr *)&sa, sizeof(sa)) < 0) + goto syserr; + + dev = wprobe_get_from_socket(sock, sep); + if (!dev) { + sprintf(errbuf, "wprobe connection initialization failed"); + goto out_err; + } + goto out; + +syserr: + if (err) { + strcpy(errbuf, "Connection failed: "); + len = strlen(errbuf); + strerror_r(errno, errbuf + len, sizeof(errbuf) - len - 1); + } +out_err: + if (err) + *err = errbuf; + if (sock >= 0) + close(sock); +out: + if (devstr) + free(devstr); + return dev; +} + +static void +free_attr_list(struct list_head *list) +{ + struct wprobe_attribute *attr, *tmp; + + list_for_each_entry_safe(attr, tmp, list, list) { + list_del(&attr->list); + free(attr); + } +} + +void +wprobe_free_dev(struct wprobe_iface *dev) +{ + if (dev->ops->free) + dev->ops->free(dev); + free_attr_list(&dev->global_attr); + free_attr_list(&dev->link_attr); + free((void *)dev->ifname); + free(dev); +} + +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(struct wprobe_iface *dev) +{ + 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(&dev->links, &cb.old_list); + cb.list = &dev->links; + + msg = wprobe_new_msg(dev, WPROBE_CMD_GET_LINKS, true); + if (!msg) + return -ENOMEM; + + err = dev->ops->send_msg(dev, 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; +} + + +struct wprobe_filter_data +{ + wprobe_filter_cb cb; + void *arg; + struct wprobe_filter_item *buf; + int buflen; +}; + +static int +dump_filter_handler(struct nl_msg *msg, void *arg) +{ + struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg)); + struct wprobe_filter_data *data = arg; + struct nlattr *p; + const char *name; + int count = 0; + int len; + + nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0), + genlmsg_attrlen(gnlh, 0), attribute_policy); + + if (!tb[WPROBE_ATTR_NAME] || !tb[WPROBE_ATTR_FILTER_GROUP]) + return -1; + + name = nla_data(tb[WPROBE_ATTR_NAME]); + nla_for_each_nested(p, tb[WPROBE_ATTR_FILTER_GROUP], len) { + count++; + } + + if (data->buflen < count) { + if (data->buf) + free(data->buf); + data->buflen = count; + data->buf = malloc(sizeof(struct wprobe_filter_item) * count); + memset(data->buf, 0, sizeof(struct wprobe_filter_item) * count); + } + + count = 0; + nla_for_each_nested(p, tb[WPROBE_ATTR_FILTER_GROUP], len) { + struct wprobe_filter_item *fi; + + nla_parse(tb, WPROBE_ATTR_LAST, nla_data(p), + nla_len(p), attribute_policy); + + if (!tb[WPROBE_ATTR_NAME] || !tb[WPROBE_ATTR_RXCOUNT] + || !tb[WPROBE_ATTR_TXCOUNT]) + continue; + + fi = &data->buf[count++]; + strncpy(fi->name, nla_data(tb[WPROBE_ATTR_NAME]), sizeof(fi->name) - 1); + fi->name[sizeof(fi->name) - 1] = 0; + fi->rx = nla_get_u64(tb[WPROBE_ATTR_RXCOUNT]); + fi->tx = nla_get_u64(tb[WPROBE_ATTR_TXCOUNT]); + } + data->cb(data->arg, name, data->buf, count); + + return 0; +} + +int +wprobe_dump_filters(struct wprobe_iface *dev, wprobe_filter_cb cb, void *arg) +{ + struct wprobe_filter_data data; + struct nl_msg *msg; + int err; + + data.buf = 0; + data.buflen = 0; + data.cb = cb; + data.arg = arg; + + msg = wprobe_new_msg(dev, WPROBE_CMD_GET_FILTER, true); + if (!msg) + return -ENOMEM; + + err = dev->ops->send_msg(dev, msg, dump_filter_handler, &data); + if (err < 0) + return err; + + return 0; +} + +int +wprobe_apply_config(struct wprobe_iface *dev) +{ + struct nl_msg *msg; + + msg = wprobe_new_msg(dev, WPROBE_CMD_CONFIG, false); + if (!msg) + return -ENOMEM; + + if (dev->interval >= 0) + NLA_PUT_MSECS(msg, WPROBE_ATTR_INTERVAL, dev->interval); + + if (dev->filter_len < 0) { + NLA_PUT(msg, WPROBE_ATTR_FILTER, 0, NULL); + dev->filter_len = 0; + } else if (dev->filter && dev->filter_len > 0) { + NLA_PUT(msg, WPROBE_ATTR_FILTER, dev->filter_len, dev->filter); + } + dev->filter = NULL; + + dev->ops->send_msg(dev, msg, NULL, NULL); + return 0; + +nla_put_failure: + nlmsg_free(msg); + return -ENOMEM; +} + +int +wprobe_measure(struct wprobe_iface *dev) +{ + struct nl_msg *msg; + + msg = wprobe_new_msg(dev, WPROBE_CMD_MEASURE, false); + if (!msg) + return -ENOMEM; + + dev->ops->send_msg(dev, msg, NULL, NULL); + return 0; +} + +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)); + if (isnan(stdev)) + stdev = 0.0f; + if (isnan(avg)) + avg = 0.0f; + attr->val.avg = avg; + attr->val.stdev = stdev; + } + } + +out: + list_add_tail(&attr->list, cb->list); + return 0; +} + + +int +wprobe_request_data(struct wprobe_iface *dev, const unsigned char *addr) +{ + struct wprobe_request_cb cb; + struct list_head *attrs; + struct nl_msg *msg; + int err; + + msg = wprobe_new_msg(dev, WPROBE_CMD_GET_INFO, true); + if (!msg) + return -ENOMEM; + + if (addr) { + attrs = &dev->link_attr; + NLA_PUT(msg, WPROBE_ATTR_MAC, 6, addr); + } else { + attrs = &dev->global_attr; + } + + INIT_LIST_HEAD(&cb.old_list); + list_splice_init(attrs, &cb.old_list); + cb.list = attrs; + + err = dev->ops->send_msg(dev, msg, save_attrdata_handler, &cb); + list_splice(&cb.old_list, attrs->prev); + return err; + +nla_put_failure: + nlmsg_free(msg); + return -ENOMEM; +} + + |