diff options
author | Felix Fietkau <nbd@openwrt.org> | 2009-03-26 20:56:47 +0000 |
---|---|---|
committer | Felix Fietkau <nbd@openwrt.org> | 2009-03-26 20:56:47 +0000 |
commit | 548478cdb24d4f588659b4e64b062aabccd76047 (patch) | |
tree | c0e8bd3d854551cb6e622246a4a9e4e7e2d57f81 /package/wprobe/src/kernel/wprobe-core.c | |
parent | 47abb6b3c307fe0d4845a61a2c91101218f745a1 (diff) | |
download | mtk-20170518-548478cdb24d4f588659b4e64b062aabccd76047.zip mtk-20170518-548478cdb24d4f588659b4e64b062aabccd76047.tar.gz mtk-20170518-548478cdb24d4f588659b4e64b062aabccd76047.tar.bz2 |
add wireless measurement probe infrastructure, developed at Fraunhofer FOKUS
SVN-Revision: 15050
Diffstat (limited to 'package/wprobe/src/kernel/wprobe-core.c')
-rw-r--r-- | package/wprobe/src/kernel/wprobe-core.c | 669 |
1 files changed, 669 insertions, 0 deletions
diff --git a/package/wprobe/src/kernel/wprobe-core.c b/package/wprobe/src/kernel/wprobe-core.c new file mode 100644 index 0000000..03e9049 --- /dev/null +++ b/package/wprobe/src/kernel/wprobe-core.c @@ -0,0 +1,669 @@ +/* + * wprobe-core.c: Wireless probe interface core + * 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 <linux/kernel.h> +#include <linux/version.h> +#include <linux/module.h> +#include <linux/types.h> +#include <linux/spinlock.h> +#include <linux/rcupdate.h> +#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26) +#include <linux/rculist.h> +#else +#include <linux/list.h> +#endif +#include <linux/skbuff.h> +#include <linux/wprobe.h> +#include <linux/math64.h> + +#define static + +#if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28) +#define list_for_each_rcu __list_for_each_rcu +#endif + +static struct list_head wprobe_if; +static spinlock_t wprobe_lock; + +static struct genl_family wprobe_fam = { + .id = GENL_ID_GENERATE, + .name = "wprobe", + .hdrsize = 0, + .version = 1, + /* only the first set of attributes is used for queries */ + .maxattr = WPROBE_ATTR_ID, +}; + +static void wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l); + +int +wprobe_add_link(struct wprobe_iface *s, struct wprobe_link *l, const char *addr) +{ + unsigned long flags; + + INIT_LIST_HEAD(&l->list); + l->val = kzalloc(sizeof(struct wprobe_value) * s->n_link_items, GFP_ATOMIC); + if (!l->val) + return -ENOMEM; + + l->iface = s; + memcpy(&l->addr, addr, ETH_ALEN); + spin_lock_irqsave(&wprobe_lock, flags); + list_add_tail_rcu(&l->list, &s->links); + spin_unlock_irqrestore(&wprobe_lock, flags); + + return 0; +} +EXPORT_SYMBOL(wprobe_add_link); + +void +wprobe_remove_link(struct wprobe_iface *s, struct wprobe_link *l) +{ + unsigned long flags; + + spin_lock_irqsave(&wprobe_lock, flags); + list_del_rcu(&l->list); + spin_unlock_irqrestore(&wprobe_lock, flags); + synchronize_rcu(); + kfree(l->val); +} +EXPORT_SYMBOL(wprobe_remove_link); + +int +wprobe_add_iface(struct wprobe_iface *s) +{ + unsigned long flags; + int vsize; + + /* reset only wprobe private area */ + memset(&s->list, 0, sizeof(struct wprobe_iface) - offsetof(struct wprobe_iface, list)); + + BUG_ON(!s->name); + INIT_LIST_HEAD(&s->list); + INIT_LIST_HEAD(&s->links); + + vsize = max(s->n_link_items, s->n_global_items); + s->val = kzalloc(sizeof(struct wprobe_value) * vsize, GFP_ATOMIC); + if (!s->val) + goto error; + + s->query_val = kzalloc(sizeof(struct wprobe_value) * vsize, GFP_ATOMIC); + if (!s->query_val) + goto error; + + spin_lock_irqsave(&wprobe_lock, flags); + list_add_rcu(&s->list, &wprobe_if); + spin_unlock_irqrestore(&wprobe_lock, flags); + + return 0; + +error: + if (s->val) + kfree(s->val); + return -ENOMEM; +} +EXPORT_SYMBOL(wprobe_add_iface); + +void +wprobe_remove_iface(struct wprobe_iface *s) +{ + unsigned long flags; + + BUG_ON(!list_empty(&s->links)); + + spin_lock_irqsave(&wprobe_lock, flags); + list_del_rcu(&s->list); + spin_unlock_irqrestore(&wprobe_lock, flags); + + /* wait for all queries to finish before freeing the + * temporary value storage buffer */ + synchronize_rcu(); + + kfree(s->val); + kfree(s->query_val); +} +EXPORT_SYMBOL(wprobe_remove_iface); + +static struct wprobe_iface * +wprobe_get_dev(struct nlattr *attr) +{ + struct wprobe_iface *dev = NULL; + struct wprobe_iface *p; + const char *name; + int i = 0; + + if (!attr) + return NULL; + + name = nla_data(attr); + list_for_each_entry_rcu(p, &wprobe_if, list) { + i++; + if (strcmp(name, p->name) != 0) + continue; + + dev = p; + break; + } + + return dev; +} + +int +wprobe_sync_data(struct wprobe_iface *dev, struct wprobe_link *l, bool query) +{ + struct wprobe_value *val; + unsigned long flags; + int n, err; + + if (l) { + n = dev->n_link_items; + val = l->val; + } else { + n = dev->n_global_items; + val = dev->val; + } + + spin_lock_irqsave(&dev->lock, flags); + err = dev->sync_data(dev, l, val, !query); + if (err) + goto done; + + if (query) + memcpy(dev->query_val, val, sizeof(struct wprobe_value) * n); + + wprobe_update_stats(dev, l); +done: + spin_unlock_irqrestore(&dev->lock, flags); + return 0; +} +EXPORT_SYMBOL(wprobe_sync_data); + +void +wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l) +{ + const struct wprobe_item *item; + struct wprobe_value *val; + int i, n; + + if (l) { + n = dev->n_link_items; + item = dev->link_items; + val = l->val; + } else { + n = dev->n_global_items; + item = dev->global_items; + val = dev->val; + } + + /* process statistics */ + for (i = 0; i < n; i++) { + s64 v; + + if (!val[i].pending) + continue; + + val[i].n++; + + switch(item[i].type) { + case WPROBE_VAL_S8: + v = val[i].S8; + break; + case WPROBE_VAL_S16: + v = val[i].S16; + break; + case WPROBE_VAL_S32: + v = val[i].S32; + break; + case WPROBE_VAL_S64: + v = val[i].S64; + break; + case WPROBE_VAL_U8: + v = val[i].U8; + break; + case WPROBE_VAL_U16: + v = val[i].U16; + break; + case WPROBE_VAL_U32: + v = val[i].U32; + break; + case WPROBE_VAL_U64: + v = val[i].U64; + break; + default: + continue; + } + + val[i].s += v; + val[i].ss += v * v; + val[i].pending = false; + } +} +EXPORT_SYMBOL(wprobe_update_stats); + +static const struct nla_policy wprobe_policy[WPROBE_ATTR_ID+1] = { + [WPROBE_ATTR_INTERFACE] = { .type = NLA_NUL_STRING }, + [WPROBE_ATTR_MAC] = { .type = NLA_STRING }, + [WPROBE_ATTR_DURATION] = { .type = NLA_MSECS }, + [WPROBE_ATTR_FLAGS] = { .type = NLA_U32 }, + [WPROBE_ATTR_SCALE] = { .type = NLA_U32 }, +}; + +static bool +wprobe_check_ptr(struct list_head *list, struct list_head *ptr) +{ + struct list_head *p; + + list_for_each_rcu(p, list) { + if (ptr == p) + return true; + } + return false; +} + +static bool +wprobe_send_item_value(struct sk_buff *msg, struct netlink_callback *cb, + struct wprobe_iface *dev, struct wprobe_link *l, + const struct wprobe_item *item, + int i, u32 flags) +{ + struct genlmsghdr *hdr; + struct wprobe_value *val = dev->query_val; + u64 time = val[i].last - val[i].first; + + hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq, + &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_INFO); + + NLA_PUT_U32(msg, WPROBE_ATTR_ID, i); + NLA_PUT_U32(msg, WPROBE_ATTR_FLAGS, flags); + NLA_PUT_U8(msg, WPROBE_ATTR_TYPE, item[i].type); + NLA_PUT_U64(msg, WPROBE_ATTR_DURATION, time); + + switch(item[i].type) { + case WPROBE_VAL_S8: + case WPROBE_VAL_U8: + NLA_PUT_U8(msg, item[i].type, val[i].U8); + break; + case WPROBE_VAL_S16: + case WPROBE_VAL_U16: + NLA_PUT_U16(msg, item[i].type, val[i].U16); + break; + case WPROBE_VAL_S32: + case WPROBE_VAL_U32: + NLA_PUT_U32(msg, item[i].type, val[i].U32); + break; + case WPROBE_VAL_S64: + case WPROBE_VAL_U64: + NLA_PUT_U64(msg, item[i].type, val[i].U64); + break; + case WPROBE_VAL_STRING: + if (val[i].STRING) + NLA_PUT_STRING(msg, item[i].type, val[i].STRING); + else + NLA_PUT_STRING(msg, item[i].type, ""); + /* bypass avg/stdev */ + goto done; + default: + /* skip unknown values */ + goto done; + } + if (item[i].flags & WPROBE_F_KEEPSTAT) { + NLA_PUT_U64(msg, WPROBE_VAL_SUM, val[i].s); + NLA_PUT_U64(msg, WPROBE_VAL_SUM_SQ, val[i].ss); + NLA_PUT_U32(msg, WPROBE_VAL_SAMPLES, (u32) val[i].n); + } +done: + genlmsg_end(msg, hdr); + return true; + +nla_put_failure: + genlmsg_cancel(msg, hdr); + return false; +} + +static bool +wprobe_send_item_info(struct sk_buff *msg, struct netlink_callback *cb, + struct wprobe_iface *dev, + const struct wprobe_item *item, int i) +{ + struct genlmsghdr *hdr; + + hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq, + &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_LIST); + + if ((i == 0) && (dev->addr != NULL)) + NLA_PUT(msg, WPROBE_ATTR_MAC, 6, dev->addr); + NLA_PUT_U32(msg, WPROBE_ATTR_ID, (u32) i); + NLA_PUT_STRING(msg, WPROBE_ATTR_NAME, item[i].name); + NLA_PUT_U8(msg, WPROBE_ATTR_TYPE, item[i].type); + NLA_PUT_U32(msg, WPROBE_ATTR_FLAGS, item[i].flags); + genlmsg_end(msg, hdr); + return true; + +nla_put_failure: + genlmsg_cancel(msg, hdr); + return false; +} + + +static struct wprobe_link * +wprobe_find_link(struct wprobe_iface *dev, const char *mac) +{ + struct wprobe_link *l; + + list_for_each_entry_rcu(l, &dev->links, list) { + if (!memcmp(l->addr, mac, 6)) + return l; + } + return NULL; +} + +static bool +wprobe_dump_link(struct sk_buff *msg, struct wprobe_link *l, struct netlink_callback *cb) +{ + struct genlmsghdr *hdr; + + hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq, + &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_LINKS); + if (!hdr) + return false; + + NLA_PUT(msg, WPROBE_ATTR_MAC, 6, l->addr); + genlmsg_end(msg, hdr); + return true; + +nla_put_failure: + genlmsg_cancel(msg, hdr); + return false; +} + +static int +wprobe_dump_links(struct sk_buff *skb, struct netlink_callback *cb) +{ + struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0]; + struct wprobe_link *l; + int err = 0; + int i = 0; + + if (!dev) { + err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize, + wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy); + if (err) + goto done; + + dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]); + if (!dev) { + err = -ENODEV; + goto done; + } + + cb->args[0] = (long) dev; + } else { + if (!wprobe_check_ptr(&wprobe_if, &dev->list)) { + err = -ENODEV; + goto done; + } + } + + rcu_read_lock(); + list_for_each_entry_rcu(l, &dev->links, list) { + if (i < cb->args[1]) + continue; + + if (unlikely(!wprobe_dump_link(skb, l, cb))) + break; + + i++; + } + cb->args[1] = i; + rcu_read_unlock(); + err = skb->len; +done: + return err; +} +static void +wprobe_scale_stats(const struct wprobe_item *item, struct wprobe_value *val, int n, u32 flags) +{ + u32 scale = 0; + int i; + + for (i = 0; i < n; i++) { + if (!(item[i].flags & WPROBE_F_KEEPSTAT)) + continue; + + /* reset statistics, if requested */ + if (flags & WPROBE_F_RESET) + scale = val[i].n; + else if (wprobe_fam.attrbuf[WPROBE_ATTR_SCALE]) + scale = nla_get_u32(wprobe_fam.attrbuf[WPROBE_ATTR_SCALE]); + + if ((scale > 0) && (val[i].n >= scale)) { + val[i].s = div_s64(val[i].s, scale); + val[i].ss = div_s64(val[i].ss, scale); + val[i].n /= scale; + } + } +} + +#define WPROBE_F_LINK (1 << 31) /* for internal use */ +static int +wprobe_dump_info(struct sk_buff *skb, struct netlink_callback *cb) +{ + struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0]; + struct wprobe_link *l = (struct wprobe_link *)cb->args[1]; + struct wprobe_value *val; + const struct wprobe_item *item; + struct genlmsghdr *hdr; + unsigned long flags; + int cmd, n, i = cb->args[3]; + u32 vflags = cb->args[2]; + int err = 0; + + hdr = (struct genlmsghdr *)nlmsg_data(cb->nlh); + cmd = hdr->cmd; + + /* since the attribute value list might be too big for a single netlink + * message, the device, link and offset get stored in the netlink callback. + * if this is the first request, we need to do the full lookup for the device. + * + * access to the device and link structure is synchronized through rcu. + */ + rcu_read_lock(); + if (!dev) { + err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize, + wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy); + if (err) + goto done; + + err = -ENOENT; + dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]); + if (!dev) + goto done; + + if (cmd == WPROBE_CMD_GET_INFO) { + if (wprobe_fam.attrbuf[WPROBE_ATTR_MAC]) { + l = wprobe_find_link(dev, nla_data(wprobe_fam.attrbuf[WPROBE_ATTR_MAC])); + if (!l) + goto done; + + vflags = l->flags; + } + + if (l) { + item = dev->link_items; + n = dev->n_link_items; + val = l->val; + } else { + item = dev->global_items; + n = dev->n_global_items; + val = dev->val; + } + + /* sync data and move to temp storage for the query */ + spin_lock_irqsave(&dev->lock, flags); + err = wprobe_sync_data(dev, l, true); + if (!err) + memcpy(dev->query_val, val, n * sizeof(struct wprobe_value)); + wprobe_scale_stats(item, val, n, flags); + spin_unlock_irqrestore(&dev->lock, flags); + + if (err) + goto done; + } + + if (wprobe_fam.attrbuf[WPROBE_ATTR_FLAGS]) + vflags |= nla_get_u32(wprobe_fam.attrbuf[WPROBE_ATTR_FLAGS]); + + if (wprobe_fam.attrbuf[WPROBE_ATTR_MAC]) + vflags |= WPROBE_F_LINK; + + cb->args[0] = (long) dev; + cb->args[1] = (long) l; + cb->args[2] = vflags; + cb->args[3] = 0; + } else { + /* when pulling pointers from the callback, validate them + * against the list using rcu to make sure that we won't + * dereference pointers to free'd memory after the last + * grace period */ + err = -ENOENT; + if (!wprobe_check_ptr(&wprobe_if, &dev->list)) + goto done; + + if (l && !wprobe_check_ptr(&dev->links, &l->list)) + goto done; + } + + if (vflags & WPROBE_F_LINK) { + item = dev->link_items; + n = dev->n_link_items; + } else { + item = dev->global_items; + n = dev->n_global_items; + } + + err = 0; + switch(cmd) { + case WPROBE_CMD_GET_INFO: + while (i < n) { + if (!wprobe_send_item_value(skb, cb, dev, l, item, i, vflags)) + break; + i++; + } + break; + case WPROBE_CMD_GET_LIST: + while (i < n) { + if (!wprobe_send_item_info(skb, cb, dev, item, i)) + break; + i++; + } + break; + default: + err = -EINVAL; + goto done; + } + cb->args[3] = i; + err = skb->len; + +done: + rcu_read_unlock(); + return err; +} +#undef WPROBE_F_LINK + +static int +wprobe_measure(struct sk_buff *skb, struct genl_info *info) +{ + struct wprobe_iface *dev; + struct wprobe_link *l = NULL; + int err = -ENOENT; + + rcu_read_lock(); + dev = wprobe_get_dev(info->attrs[WPROBE_ATTR_INTERFACE]); + if (!dev) + goto done; + + if (info->attrs[WPROBE_ATTR_MAC]) { + l = wprobe_find_link(dev, nla_data(wprobe_fam.attrbuf[WPROBE_ATTR_MAC])); + if (!l) + goto done; + } + + err = wprobe_sync_data(dev, l, false); + +done: + rcu_read_unlock(); + return err; +} + +static struct genl_ops wprobe_ops[] = { + { + .cmd = WPROBE_CMD_GET_INFO, + .dumpit = wprobe_dump_info, + .policy = wprobe_policy, + }, + { + .cmd = WPROBE_CMD_GET_LIST, + .dumpit = wprobe_dump_info, + .policy = wprobe_policy, + }, + { + .cmd = WPROBE_CMD_MEASURE, + .doit = wprobe_measure, + .policy = wprobe_policy, + }, + { + .cmd = WPROBE_CMD_GET_LINKS, + .dumpit = wprobe_dump_links, + .policy = wprobe_policy, + } +}; + +static void __exit +wprobe_exit(void) +{ + BUG_ON(!list_empty(&wprobe_if)); + genl_unregister_family(&wprobe_fam); +} + + +static int __init +wprobe_init(void) +{ + int i, err; + + spin_lock_init(&wprobe_lock); + INIT_LIST_HEAD(&wprobe_if); + + err = genl_register_family(&wprobe_fam); + if (err) + return err; + + for (i = 0; i < ARRAY_SIZE(wprobe_ops); i++) { + err = genl_register_ops(&wprobe_fam, &wprobe_ops[i]); + if (err) + goto error; + } + + return 0; + +error: + genl_unregister_family(&wprobe_fam); + return err; +} + +module_init(wprobe_init); +module_exit(wprobe_exit); +MODULE_LICENSE("GPL"); + |