add wireless measurement probe infrastructure, developed at Fraunhofer FOKUS
[openwrt/svn-archive/archive.git] / package / wprobe / src / kernel / wprobe-core.c
1 /*
2 * wprobe-core.c: Wireless probe interface core
3 * Copyright (C) 2008-2009 Felix Fietkau <nbd@openwrt.org>
4 *
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.
9 *
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.
14 */
15
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>
24 #else
25 #include <linux/list.h>
26 #endif
27 #include <linux/skbuff.h>
28 #include <linux/wprobe.h>
29 #include <linux/math64.h>
30
31 #define static
32
33 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,28)
34 #define list_for_each_rcu __list_for_each_rcu
35 #endif
36
37 static struct list_head wprobe_if;
38 static spinlock_t wprobe_lock;
39
40 static struct genl_family wprobe_fam = {
41 .id = GENL_ID_GENERATE,
42 .name = "wprobe",
43 .hdrsize = 0,
44 .version = 1,
45 /* only the first set of attributes is used for queries */
46 .maxattr = WPROBE_ATTR_ID,
47 };
48
49 static void wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l);
50
51 int
52 wprobe_add_link(struct wprobe_iface *s, struct wprobe_link *l, const char *addr)
53 {
54 unsigned long flags;
55
56 INIT_LIST_HEAD(&l->list);
57 l->val = kzalloc(sizeof(struct wprobe_value) * s->n_link_items, GFP_ATOMIC);
58 if (!l->val)
59 return -ENOMEM;
60
61 l->iface = s;
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);
66
67 return 0;
68 }
69 EXPORT_SYMBOL(wprobe_add_link);
70
71 void
72 wprobe_remove_link(struct wprobe_iface *s, struct wprobe_link *l)
73 {
74 unsigned long flags;
75
76 spin_lock_irqsave(&wprobe_lock, flags);
77 list_del_rcu(&l->list);
78 spin_unlock_irqrestore(&wprobe_lock, flags);
79 synchronize_rcu();
80 kfree(l->val);
81 }
82 EXPORT_SYMBOL(wprobe_remove_link);
83
84 int
85 wprobe_add_iface(struct wprobe_iface *s)
86 {
87 unsigned long flags;
88 int vsize;
89
90 /* reset only wprobe private area */
91 memset(&s->list, 0, sizeof(struct wprobe_iface) - offsetof(struct wprobe_iface, list));
92
93 BUG_ON(!s->name);
94 INIT_LIST_HEAD(&s->list);
95 INIT_LIST_HEAD(&s->links);
96
97 vsize = max(s->n_link_items, s->n_global_items);
98 s->val = kzalloc(sizeof(struct wprobe_value) * vsize, GFP_ATOMIC);
99 if (!s->val)
100 goto error;
101
102 s->query_val = kzalloc(sizeof(struct wprobe_value) * vsize, GFP_ATOMIC);
103 if (!s->query_val)
104 goto error;
105
106 spin_lock_irqsave(&wprobe_lock, flags);
107 list_add_rcu(&s->list, &wprobe_if);
108 spin_unlock_irqrestore(&wprobe_lock, flags);
109
110 return 0;
111
112 error:
113 if (s->val)
114 kfree(s->val);
115 return -ENOMEM;
116 }
117 EXPORT_SYMBOL(wprobe_add_iface);
118
119 void
120 wprobe_remove_iface(struct wprobe_iface *s)
121 {
122 unsigned long flags;
123
124 BUG_ON(!list_empty(&s->links));
125
126 spin_lock_irqsave(&wprobe_lock, flags);
127 list_del_rcu(&s->list);
128 spin_unlock_irqrestore(&wprobe_lock, flags);
129
130 /* wait for all queries to finish before freeing the
131 * temporary value storage buffer */
132 synchronize_rcu();
133
134 kfree(s->val);
135 kfree(s->query_val);
136 }
137 EXPORT_SYMBOL(wprobe_remove_iface);
138
139 static struct wprobe_iface *
140 wprobe_get_dev(struct nlattr *attr)
141 {
142 struct wprobe_iface *dev = NULL;
143 struct wprobe_iface *p;
144 const char *name;
145 int i = 0;
146
147 if (!attr)
148 return NULL;
149
150 name = nla_data(attr);
151 list_for_each_entry_rcu(p, &wprobe_if, list) {
152 i++;
153 if (strcmp(name, p->name) != 0)
154 continue;
155
156 dev = p;
157 break;
158 }
159
160 return dev;
161 }
162
163 int
164 wprobe_sync_data(struct wprobe_iface *dev, struct wprobe_link *l, bool query)
165 {
166 struct wprobe_value *val;
167 unsigned long flags;
168 int n, err;
169
170 if (l) {
171 n = dev->n_link_items;
172 val = l->val;
173 } else {
174 n = dev->n_global_items;
175 val = dev->val;
176 }
177
178 spin_lock_irqsave(&dev->lock, flags);
179 err = dev->sync_data(dev, l, val, !query);
180 if (err)
181 goto done;
182
183 if (query)
184 memcpy(dev->query_val, val, sizeof(struct wprobe_value) * n);
185
186 wprobe_update_stats(dev, l);
187 done:
188 spin_unlock_irqrestore(&dev->lock, flags);
189 return 0;
190 }
191 EXPORT_SYMBOL(wprobe_sync_data);
192
193 void
194 wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l)
195 {
196 const struct wprobe_item *item;
197 struct wprobe_value *val;
198 int i, n;
199
200 if (l) {
201 n = dev->n_link_items;
202 item = dev->link_items;
203 val = l->val;
204 } else {
205 n = dev->n_global_items;
206 item = dev->global_items;
207 val = dev->val;
208 }
209
210 /* process statistics */
211 for (i = 0; i < n; i++) {
212 s64 v;
213
214 if (!val[i].pending)
215 continue;
216
217 val[i].n++;
218
219 switch(item[i].type) {
220 case WPROBE_VAL_S8:
221 v = val[i].S8;
222 break;
223 case WPROBE_VAL_S16:
224 v = val[i].S16;
225 break;
226 case WPROBE_VAL_S32:
227 v = val[i].S32;
228 break;
229 case WPROBE_VAL_S64:
230 v = val[i].S64;
231 break;
232 case WPROBE_VAL_U8:
233 v = val[i].U8;
234 break;
235 case WPROBE_VAL_U16:
236 v = val[i].U16;
237 break;
238 case WPROBE_VAL_U32:
239 v = val[i].U32;
240 break;
241 case WPROBE_VAL_U64:
242 v = val[i].U64;
243 break;
244 default:
245 continue;
246 }
247
248 val[i].s += v;
249 val[i].ss += v * v;
250 val[i].pending = false;
251 }
252 }
253 EXPORT_SYMBOL(wprobe_update_stats);
254
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 },
261 };
262
263 static bool
264 wprobe_check_ptr(struct list_head *list, struct list_head *ptr)
265 {
266 struct list_head *p;
267
268 list_for_each_rcu(p, list) {
269 if (ptr == p)
270 return true;
271 }
272 return false;
273 }
274
275 static bool
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,
279 int i, u32 flags)
280 {
281 struct genlmsghdr *hdr;
282 struct wprobe_value *val = dev->query_val;
283 u64 time = val[i].last - val[i].first;
284
285 hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
286 &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_INFO);
287
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);
292
293 switch(item[i].type) {
294 case WPROBE_VAL_S8:
295 case WPROBE_VAL_U8:
296 NLA_PUT_U8(msg, item[i].type, val[i].U8);
297 break;
298 case WPROBE_VAL_S16:
299 case WPROBE_VAL_U16:
300 NLA_PUT_U16(msg, item[i].type, val[i].U16);
301 break;
302 case WPROBE_VAL_S32:
303 case WPROBE_VAL_U32:
304 NLA_PUT_U32(msg, item[i].type, val[i].U32);
305 break;
306 case WPROBE_VAL_S64:
307 case WPROBE_VAL_U64:
308 NLA_PUT_U64(msg, item[i].type, val[i].U64);
309 break;
310 case WPROBE_VAL_STRING:
311 if (val[i].STRING)
312 NLA_PUT_STRING(msg, item[i].type, val[i].STRING);
313 else
314 NLA_PUT_STRING(msg, item[i].type, "");
315 /* bypass avg/stdev */
316 goto done;
317 default:
318 /* skip unknown values */
319 goto done;
320 }
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);
325 }
326 done:
327 genlmsg_end(msg, hdr);
328 return true;
329
330 nla_put_failure:
331 genlmsg_cancel(msg, hdr);
332 return false;
333 }
334
335 static bool
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)
339 {
340 struct genlmsghdr *hdr;
341
342 hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
343 &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_LIST);
344
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);
352 return true;
353
354 nla_put_failure:
355 genlmsg_cancel(msg, hdr);
356 return false;
357 }
358
359
360 static struct wprobe_link *
361 wprobe_find_link(struct wprobe_iface *dev, const char *mac)
362 {
363 struct wprobe_link *l;
364
365 list_for_each_entry_rcu(l, &dev->links, list) {
366 if (!memcmp(l->addr, mac, 6))
367 return l;
368 }
369 return NULL;
370 }
371
372 static bool
373 wprobe_dump_link(struct sk_buff *msg, struct wprobe_link *l, struct netlink_callback *cb)
374 {
375 struct genlmsghdr *hdr;
376
377 hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
378 &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_LINKS);
379 if (!hdr)
380 return false;
381
382 NLA_PUT(msg, WPROBE_ATTR_MAC, 6, l->addr);
383 genlmsg_end(msg, hdr);
384 return true;
385
386 nla_put_failure:
387 genlmsg_cancel(msg, hdr);
388 return false;
389 }
390
391 static int
392 wprobe_dump_links(struct sk_buff *skb, struct netlink_callback *cb)
393 {
394 struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0];
395 struct wprobe_link *l;
396 int err = 0;
397 int i = 0;
398
399 if (!dev) {
400 err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize,
401 wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy);
402 if (err)
403 goto done;
404
405 dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]);
406 if (!dev) {
407 err = -ENODEV;
408 goto done;
409 }
410
411 cb->args[0] = (long) dev;
412 } else {
413 if (!wprobe_check_ptr(&wprobe_if, &dev->list)) {
414 err = -ENODEV;
415 goto done;
416 }
417 }
418
419 rcu_read_lock();
420 list_for_each_entry_rcu(l, &dev->links, list) {
421 if (i < cb->args[1])
422 continue;
423
424 if (unlikely(!wprobe_dump_link(skb, l, cb)))
425 break;
426
427 i++;
428 }
429 cb->args[1] = i;
430 rcu_read_unlock();
431 err = skb->len;
432 done:
433 return err;
434 }
435 static void
436 wprobe_scale_stats(const struct wprobe_item *item, struct wprobe_value *val, int n, u32 flags)
437 {
438 u32 scale = 0;
439 int i;
440
441 for (i = 0; i < n; i++) {
442 if (!(item[i].flags & WPROBE_F_KEEPSTAT))
443 continue;
444
445 /* reset statistics, if requested */
446 if (flags & WPROBE_F_RESET)
447 scale = val[i].n;
448 else if (wprobe_fam.attrbuf[WPROBE_ATTR_SCALE])
449 scale = nla_get_u32(wprobe_fam.attrbuf[WPROBE_ATTR_SCALE]);
450
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 /= scale;
455 }
456 }
457 }
458
459 #define WPROBE_F_LINK (1 << 31) /* for internal use */
460 static int
461 wprobe_dump_info(struct sk_buff *skb, struct netlink_callback *cb)
462 {
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;
468 unsigned long flags;
469 int cmd, n, i = cb->args[3];
470 u32 vflags = cb->args[2];
471 int err = 0;
472
473 hdr = (struct genlmsghdr *)nlmsg_data(cb->nlh);
474 cmd = hdr->cmd;
475
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.
479 *
480 * access to the device and link structure is synchronized through rcu.
481 */
482 rcu_read_lock();
483 if (!dev) {
484 err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize,
485 wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy);
486 if (err)
487 goto done;
488
489 err = -ENOENT;
490 dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]);
491 if (!dev)
492 goto done;
493
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]));
497 if (!l)
498 goto done;
499
500 vflags = l->flags;
501 }
502
503 if (l) {
504 item = dev->link_items;
505 n = dev->n_link_items;
506 val = l->val;
507 } else {
508 item = dev->global_items;
509 n = dev->n_global_items;
510 val = dev->val;
511 }
512
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);
516 if (!err)
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);
520
521 if (err)
522 goto done;
523 }
524
525 if (wprobe_fam.attrbuf[WPROBE_ATTR_FLAGS])
526 vflags |= nla_get_u32(wprobe_fam.attrbuf[WPROBE_ATTR_FLAGS]);
527
528 if (wprobe_fam.attrbuf[WPROBE_ATTR_MAC])
529 vflags |= WPROBE_F_LINK;
530
531 cb->args[0] = (long) dev;
532 cb->args[1] = (long) l;
533 cb->args[2] = vflags;
534 cb->args[3] = 0;
535 } else {
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
539 * grace period */
540 err = -ENOENT;
541 if (!wprobe_check_ptr(&wprobe_if, &dev->list))
542 goto done;
543
544 if (l && !wprobe_check_ptr(&dev->links, &l->list))
545 goto done;
546 }
547
548 if (vflags & WPROBE_F_LINK) {
549 item = dev->link_items;
550 n = dev->n_link_items;
551 } else {
552 item = dev->global_items;
553 n = dev->n_global_items;
554 }
555
556 err = 0;
557 switch(cmd) {
558 case WPROBE_CMD_GET_INFO:
559 while (i < n) {
560 if (!wprobe_send_item_value(skb, cb, dev, l, item, i, vflags))
561 break;
562 i++;
563 }
564 break;
565 case WPROBE_CMD_GET_LIST:
566 while (i < n) {
567 if (!wprobe_send_item_info(skb, cb, dev, item, i))
568 break;
569 i++;
570 }
571 break;
572 default:
573 err = -EINVAL;
574 goto done;
575 }
576 cb->args[3] = i;
577 err = skb->len;
578
579 done:
580 rcu_read_unlock();
581 return err;
582 }
583 #undef WPROBE_F_LINK
584
585 static int
586 wprobe_measure(struct sk_buff *skb, struct genl_info *info)
587 {
588 struct wprobe_iface *dev;
589 struct wprobe_link *l = NULL;
590 int err = -ENOENT;
591
592 rcu_read_lock();
593 dev = wprobe_get_dev(info->attrs[WPROBE_ATTR_INTERFACE]);
594 if (!dev)
595 goto done;
596
597 if (info->attrs[WPROBE_ATTR_MAC]) {
598 l = wprobe_find_link(dev, nla_data(wprobe_fam.attrbuf[WPROBE_ATTR_MAC]));
599 if (!l)
600 goto done;
601 }
602
603 err = wprobe_sync_data(dev, l, false);
604
605 done:
606 rcu_read_unlock();
607 return err;
608 }
609
610 static struct genl_ops wprobe_ops[] = {
611 {
612 .cmd = WPROBE_CMD_GET_INFO,
613 .dumpit = wprobe_dump_info,
614 .policy = wprobe_policy,
615 },
616 {
617 .cmd = WPROBE_CMD_GET_LIST,
618 .dumpit = wprobe_dump_info,
619 .policy = wprobe_policy,
620 },
621 {
622 .cmd = WPROBE_CMD_MEASURE,
623 .doit = wprobe_measure,
624 .policy = wprobe_policy,
625 },
626 {
627 .cmd = WPROBE_CMD_GET_LINKS,
628 .dumpit = wprobe_dump_links,
629 .policy = wprobe_policy,
630 }
631 };
632
633 static void __exit
634 wprobe_exit(void)
635 {
636 BUG_ON(!list_empty(&wprobe_if));
637 genl_unregister_family(&wprobe_fam);
638 }
639
640
641 static int __init
642 wprobe_init(void)
643 {
644 int i, err;
645
646 spin_lock_init(&wprobe_lock);
647 INIT_LIST_HEAD(&wprobe_if);
648
649 err = genl_register_family(&wprobe_fam);
650 if (err)
651 return err;
652
653 for (i = 0; i < ARRAY_SIZE(wprobe_ops); i++) {
654 err = genl_register_ops(&wprobe_fam, &wprobe_ops[i]);
655 if (err)
656 goto error;
657 }
658
659 return 0;
660
661 error:
662 genl_unregister_family(&wprobe_fam);
663 return err;
664 }
665
666 module_init(wprobe_init);
667 module_exit(wprobe_exit);
668 MODULE_LICENSE("GPL");
669