2 * wprobe-core.c: Wireless probe interface core
3 * Copyright (C) 2008-2009 Felix Fietkau <nbd@openwrt.org>
5 * This program is free software; you can redistribute it and/or
6 * modify it under the terms of the GNU General Public License
7 * as published by the Free Software Foundation; either version 2
8 * of the License, or (at your option) any later version.
10 * This program is distributed in the hope that it will be useful,
11 * but WITHOUT ANY WARRANTY; without even the implied warranty of
12 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
13 * GNU General Public License for more details.
16 #include <linux/kernel.h>
17 #include <linux/version.h>
18 #include <linux/module.h>
19 #include <linux/types.h>
20 #include <linux/spinlock.h>
21 #include <linux/rcupdate.h>
22 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,26)
23 #include <linux/rculist.h>
25 #include <linux/list.h>
27 #include <linux/skbuff.h>
28 #include <linux/wprobe.h>
29 #include <linux/math64.h>
33 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
34 #define list_for_each_rcu __list_for_each_rcu
37 static struct list_head wprobe_if;
38 static spinlock_t wprobe_lock;
40 static struct genl_family wprobe_fam = {
41 .id = GENL_ID_GENERATE,
45 /* only the first set of attributes is used for queries */
46 .maxattr = WPROBE_ATTR_ID,
49 static void wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l);
52 wprobe_add_link(struct wprobe_iface *s, struct wprobe_link *l, const char *addr)
56 INIT_LIST_HEAD(&l->list);
57 l->val = kzalloc(sizeof(struct wprobe_value) * s->n_link_items, GFP_ATOMIC);
62 memcpy(&l->addr, addr, ETH_ALEN);
63 spin_lock_irqsave(&wprobe_lock, flags);
64 list_add_tail_rcu(&l->list, &s->links);
65 spin_unlock_irqrestore(&wprobe_lock, flags);
69 EXPORT_SYMBOL(wprobe_add_link);
72 wprobe_remove_link(struct wprobe_iface *s, struct wprobe_link *l)
76 spin_lock_irqsave(&wprobe_lock, flags);
77 list_del_rcu(&l->list);
78 spin_unlock_irqrestore(&wprobe_lock, flags);
82 EXPORT_SYMBOL(wprobe_remove_link);
85 wprobe_add_iface(struct wprobe_iface *s)
90 /* reset only wprobe private area */
91 memset(&s->list, 0, sizeof(struct wprobe_iface) - offsetof(struct wprobe_iface, list));
94 INIT_LIST_HEAD(&s->list);
95 INIT_LIST_HEAD(&s->links);
97 vsize = max(s->n_link_items, s->n_global_items);
98 s->val = kzalloc(sizeof(struct wprobe_value) * vsize, GFP_ATOMIC);
102 s->query_val = kzalloc(sizeof(struct wprobe_value) * vsize, GFP_ATOMIC);
106 spin_lock_irqsave(&wprobe_lock, flags);
107 list_add_rcu(&s->list, &wprobe_if);
108 spin_unlock_irqrestore(&wprobe_lock, flags);
117 EXPORT_SYMBOL(wprobe_add_iface);
120 wprobe_remove_iface(struct wprobe_iface *s)
124 BUG_ON(!list_empty(&s->links));
126 spin_lock_irqsave(&wprobe_lock, flags);
127 list_del_rcu(&s->list);
128 spin_unlock_irqrestore(&wprobe_lock, flags);
130 /* wait for all queries to finish before freeing the
131 * temporary value storage buffer */
137 EXPORT_SYMBOL(wprobe_remove_iface);
139 static struct wprobe_iface *
140 wprobe_get_dev(struct nlattr *attr)
142 struct wprobe_iface *dev = NULL;
143 struct wprobe_iface *p;
150 name = nla_data(attr);
151 list_for_each_entry_rcu(p, &wprobe_if, list) {
153 if (strcmp(name, p->name) != 0)
164 wprobe_sync_data(struct wprobe_iface *dev, struct wprobe_link *l, bool query)
166 struct wprobe_value *val;
171 n = dev->n_link_items;
174 n = dev->n_global_items;
178 spin_lock_irqsave(&dev->lock, flags);
179 err = dev->sync_data(dev, l, val, !query);
184 memcpy(dev->query_val, val, sizeof(struct wprobe_value) * n);
186 wprobe_update_stats(dev, l);
188 spin_unlock_irqrestore(&dev->lock, flags);
191 EXPORT_SYMBOL(wprobe_sync_data);
194 wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l)
196 const struct wprobe_item *item;
197 struct wprobe_value *val;
201 n = dev->n_link_items;
202 item = dev->link_items;
205 n = dev->n_global_items;
206 item = dev->global_items;
210 /* process statistics */
211 for (i = 0; i < n; i++) {
219 switch(item[i].type) {
250 val[i].pending = false;
253 EXPORT_SYMBOL(wprobe_update_stats);
255 static const struct nla_policy wprobe_policy[WPROBE_ATTR_ID+1] = {
256 [WPROBE_ATTR_INTERFACE] = { .type = NLA_NUL_STRING },
257 [WPROBE_ATTR_MAC] = { .type = NLA_STRING },
258 [WPROBE_ATTR_DURATION] = { .type = NLA_MSECS },
259 [WPROBE_ATTR_FLAGS] = { .type = NLA_U32 },
260 [WPROBE_ATTR_SCALE] = { .type = NLA_U32 },
264 wprobe_check_ptr(struct list_head *list, struct list_head *ptr)
268 list_for_each_rcu(p, list) {
276 wprobe_send_item_value(struct sk_buff *msg, struct netlink_callback *cb,
277 struct wprobe_iface *dev, struct wprobe_link *l,
278 const struct wprobe_item *item,
281 struct genlmsghdr *hdr;
282 struct wprobe_value *val = dev->query_val;
283 u64 time = val[i].last - val[i].first;
285 hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
286 &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_INFO);
288 NLA_PUT_U32(msg, WPROBE_ATTR_ID, i);
289 NLA_PUT_U32(msg, WPROBE_ATTR_FLAGS, flags);
290 NLA_PUT_U8(msg, WPROBE_ATTR_TYPE, item[i].type);
291 NLA_PUT_U64(msg, WPROBE_ATTR_DURATION, time);
293 switch(item[i].type) {
296 NLA_PUT_U8(msg, item[i].type, val[i].U8);
300 NLA_PUT_U16(msg, item[i].type, val[i].U16);
304 NLA_PUT_U32(msg, item[i].type, val[i].U32);
308 NLA_PUT_U64(msg, item[i].type, val[i].U64);
310 case WPROBE_VAL_STRING:
312 NLA_PUT_STRING(msg, item[i].type, val[i].STRING);
314 NLA_PUT_STRING(msg, item[i].type, "");
315 /* bypass avg/stdev */
318 /* skip unknown values */
321 if (item[i].flags & WPROBE_F_KEEPSTAT) {
322 NLA_PUT_U64(msg, WPROBE_VAL_SUM, val[i].s);
323 NLA_PUT_U64(msg, WPROBE_VAL_SUM_SQ, val[i].ss);
324 NLA_PUT_U32(msg, WPROBE_VAL_SAMPLES, (u32) val[i].n);
327 genlmsg_end(msg, hdr);
331 genlmsg_cancel(msg, hdr);
336 wprobe_send_item_info(struct sk_buff *msg, struct netlink_callback *cb,
337 struct wprobe_iface *dev,
338 const struct wprobe_item *item, int i)
340 struct genlmsghdr *hdr;
342 hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
343 &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_LIST);
345 if ((i == 0) && (dev->addr != NULL))
346 NLA_PUT(msg, WPROBE_ATTR_MAC, 6, dev->addr);
347 NLA_PUT_U32(msg, WPROBE_ATTR_ID, (u32) i);
348 NLA_PUT_STRING(msg, WPROBE_ATTR_NAME, item[i].name);
349 NLA_PUT_U8(msg, WPROBE_ATTR_TYPE, item[i].type);
350 NLA_PUT_U32(msg, WPROBE_ATTR_FLAGS, item[i].flags);
351 genlmsg_end(msg, hdr);
355 genlmsg_cancel(msg, hdr);
360 static struct wprobe_link *
361 wprobe_find_link(struct wprobe_iface *dev, const char *mac)
363 struct wprobe_link *l;
365 list_for_each_entry_rcu(l, &dev->links, list) {
366 if (!memcmp(l->addr, mac, 6))
373 wprobe_dump_link(struct sk_buff *msg, struct wprobe_link *l, struct netlink_callback *cb)
375 struct genlmsghdr *hdr;
377 hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
378 &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_LINKS);
382 NLA_PUT(msg, WPROBE_ATTR_MAC, 6, l->addr);
383 genlmsg_end(msg, hdr);
387 genlmsg_cancel(msg, hdr);
392 wprobe_dump_links(struct sk_buff *skb, struct netlink_callback *cb)
394 struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0];
395 struct wprobe_link *l;
400 err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize,
401 wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy);
405 dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]);
411 cb->args[0] = (long) dev;
413 if (!wprobe_check_ptr(&wprobe_if, &dev->list)) {
420 list_for_each_entry_rcu(l, &dev->links, list) {
424 if (unlikely(!wprobe_dump_link(skb, l, cb)))
436 wprobe_scale_stats(const struct wprobe_item *item, struct wprobe_value *val, int n, u32 flags)
441 for (i = 0; i < n; i++) {
442 if (!(item[i].flags & WPROBE_F_KEEPSTAT))
445 /* reset statistics, if requested */
446 if (flags & WPROBE_F_RESET)
448 else if (wprobe_fam.attrbuf[WPROBE_ATTR_SCALE])
449 scale = nla_get_u32(wprobe_fam.attrbuf[WPROBE_ATTR_SCALE]);
451 if ((scale > 0) && (val[i].n > scale)) {
452 val[i].s = div_s64(val[i].s, scale);
453 val[i].ss = div_s64(val[i].ss, scale);
454 val[i].n = val[i].n / scale + 1;
459 #define WPROBE_F_LINK (1 << 31) /* for internal use */
461 wprobe_dump_info(struct sk_buff *skb, struct netlink_callback *cb)
463 struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0];
464 struct wprobe_link *l = (struct wprobe_link *)cb->args[1];
465 struct wprobe_value *val;
466 const struct wprobe_item *item;
467 struct genlmsghdr *hdr;
469 int cmd, n, i = cb->args[3];
470 u32 vflags = cb->args[2];
473 hdr = (struct genlmsghdr *)nlmsg_data(cb->nlh);
476 /* since the attribute value list might be too big for a single netlink
477 * message, the device, link and offset get stored in the netlink callback.
478 * if this is the first request, we need to do the full lookup for the device.
480 * access to the device and link structure is synchronized through rcu.
484 err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize,
485 wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy);
490 dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]);
494 if (cmd == WPROBE_CMD_GET_INFO) {
495 if (wprobe_fam.attrbuf[WPROBE_ATTR_MAC]) {
496 l = wprobe_find_link(dev, nla_data(wprobe_fam.attrbuf[WPROBE_ATTR_MAC]));
504 item = dev->link_items;
505 n = dev->n_link_items;
508 item = dev->global_items;
509 n = dev->n_global_items;
513 /* sync data and move to temp storage for the query */
514 spin_lock_irqsave(&dev->lock, flags);
515 err = wprobe_sync_data(dev, l, true);
517 memcpy(dev->query_val, val, n * sizeof(struct wprobe_value));
518 wprobe_scale_stats(item, val, n, flags);
519 spin_unlock_irqrestore(&dev->lock, flags);
525 if (wprobe_fam.attrbuf[WPROBE_ATTR_FLAGS])
526 vflags |= nla_get_u32(wprobe_fam.attrbuf[WPROBE_ATTR_FLAGS]);
528 if (wprobe_fam.attrbuf[WPROBE_ATTR_MAC])
529 vflags |= WPROBE_F_LINK;
531 cb->args[0] = (long) dev;
532 cb->args[1] = (long) l;
533 cb->args[2] = vflags;
536 /* when pulling pointers from the callback, validate them
537 * against the list using rcu to make sure that we won't
538 * dereference pointers to free'd memory after the last
541 if (!wprobe_check_ptr(&wprobe_if, &dev->list))
544 if (l && !wprobe_check_ptr(&dev->links, &l->list))
548 if (vflags & WPROBE_F_LINK) {
549 item = dev->link_items;
550 n = dev->n_link_items;
552 item = dev->global_items;
553 n = dev->n_global_items;
558 case WPROBE_CMD_GET_INFO:
560 if (!wprobe_send_item_value(skb, cb, dev, l, item, i, vflags))
565 case WPROBE_CMD_GET_LIST:
567 if (!wprobe_send_item_info(skb, cb, dev, item, i))
586 wprobe_measure(struct sk_buff *skb, struct genl_info *info)
588 struct wprobe_iface *dev;
589 struct wprobe_link *l = NULL;
593 dev = wprobe_get_dev(info->attrs[WPROBE_ATTR_INTERFACE]);
597 if (info->attrs[WPROBE_ATTR_MAC]) {
598 l = wprobe_find_link(dev, nla_data(wprobe_fam.attrbuf[WPROBE_ATTR_MAC]));
603 err = wprobe_sync_data(dev, l, false);
610 static struct genl_ops wprobe_ops[] = {
612 .cmd = WPROBE_CMD_GET_INFO,
613 .dumpit = wprobe_dump_info,
614 .policy = wprobe_policy,
617 .cmd = WPROBE_CMD_GET_LIST,
618 .dumpit = wprobe_dump_info,
619 .policy = wprobe_policy,
622 .cmd = WPROBE_CMD_MEASURE,
623 .doit = wprobe_measure,
624 .policy = wprobe_policy,
627 .cmd = WPROBE_CMD_GET_LINKS,
628 .dumpit = wprobe_dump_links,
629 .policy = wprobe_policy,
636 BUG_ON(!list_empty(&wprobe_if));
637 genl_unregister_family(&wprobe_fam);
646 spin_lock_init(&wprobe_lock);
647 INIT_LIST_HEAD(&wprobe_if);
649 err = genl_register_family(&wprobe_fam);
653 for (i = 0; i < ARRAY_SIZE(wprobe_ops); i++) {
654 err = genl_register_ops(&wprobe_fam, &wprobe_ops[i]);
662 genl_unregister_family(&wprobe_fam);
666 module_init(wprobe_init);
667 module_exit(wprobe_exit);
668 MODULE_LICENSE("GPL");