projects / openwrt / svn-archive / archive.git / blob
? search:


68d07172a36a12637a6ba2d3486f1efb79be51b6
[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(pos, head) \
35 for (pos = rcu_dereference((head)->next); \
36 prefetch(pos->next), pos != (head); \
37 pos = rcu_dereference(pos->next))
38 #endif
39
40 #define WPROBE_MIN_INTERVAL 100 /* minimum measurement interval in msecs */
41 #define WPROBE_MAX_FILTER_SIZE 1024
42 #define WPROBE_MAX_FRAME_SIZE 1900
43
44 static struct list_head wprobe_if;
45 static spinlock_t wprobe_lock;
46
47 static struct genl_family wprobe_fam = {
48 .id = GENL_ID_GENERATE,
49 .name = "wprobe",
50 .hdrsize = 0,
51 .version = 1,
52 /* only the first set of attributes is used for queries */
53 .maxattr = WPROBE_ATTR_LAST,
54 };
55
56 /* fake radiotap header */
57 struct wprobe_rtap_hdr {
58 __u8 version;
59 __u8 padding;
60 __le16 len;
61 __le32 present;
62 };
63
64 static void wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l);
65 static int wprobe_sync_data(struct wprobe_iface *dev, struct wprobe_link *l, bool query);
66 static void wprobe_free_filter(struct wprobe_filter *f);
67
68 int
69 wprobe_add_link(struct wprobe_iface *s, struct wprobe_link *l, const char *addr)
70 {
71 unsigned long flags;
72
73 INIT_LIST_HEAD(&l->list);
74 l->val = kzalloc(sizeof(struct wprobe_value) * s->n_link_items, GFP_ATOMIC);
75 if (!l->val)
76 return -ENOMEM;
77
78 l->iface = s;
79 memcpy(&l->addr, addr, ETH_ALEN);
80 spin_lock_irqsave(&wprobe_lock, flags);
81 list_add_tail_rcu(&l->list, &s->links);
82 spin_unlock_irqrestore(&wprobe_lock, flags);
83
84 return 0;
85 }
86 EXPORT_SYMBOL(wprobe_add_link);
87
88 void
89 wprobe_remove_link(struct wprobe_iface *s, struct wprobe_link *l)
90 {
91 unsigned long flags;
92
93 spin_lock_irqsave(&wprobe_lock, flags);
94 list_del_rcu(&l->list);
95 spin_unlock_irqrestore(&wprobe_lock, flags);
96 synchronize_rcu();
97 kfree(l->val);
98 }
99 EXPORT_SYMBOL(wprobe_remove_link);
100
101 static void
102 wprobe_measure_timer(unsigned long data)
103 {
104 struct wprobe_iface *dev = (struct wprobe_iface *) data;
105
106 /* set next measurement interval */
107 mod_timer(&dev->measure_timer, jiffies +
108 msecs_to_jiffies(dev->measure_interval));
109
110 /* perform measurement */
111 wprobe_sync_data(dev, NULL, false);
112 }
113
114 int
115 wprobe_add_iface(struct wprobe_iface *s)
116 {
117 unsigned long flags;
118 int vsize;
119
120 /* reset only wprobe private area */
121 memset(&s->list, 0, sizeof(struct wprobe_iface) - offsetof(struct wprobe_iface, list));
122
123 BUG_ON(!s->name);
124 INIT_LIST_HEAD(&s->list);
125 INIT_LIST_HEAD(&s->links);
126 setup_timer(&s->measure_timer, wprobe_measure_timer, (unsigned long) s);
127
128 s->val = kzalloc(sizeof(struct wprobe_value) * s->n_global_items, GFP_ATOMIC);
129 if (!s->val)
130 goto error;
131
132 vsize = max(s->n_link_items, s->n_global_items);
133 s->query_val = kzalloc(sizeof(struct wprobe_value) * vsize, GFP_ATOMIC);
134 if (!s->query_val)
135 goto error;
136
137 /* initialize defaults to be able to handle overflow,
138 * user space will need to handle this if it keeps an
139 * internal histogram */
140 s->scale_min = 20;
141 s->scale_max = (1 << 31);
142
143 s->scale_m = 1;
144 s->scale_d = 10;
145
146 spin_lock_irqsave(&wprobe_lock, flags);
147 list_add_rcu(&s->list, &wprobe_if);
148 spin_unlock_irqrestore(&wprobe_lock, flags);
149
150 return 0;
151
152 error:
153 if (s->val)
154 kfree(s->val);
155 return -ENOMEM;
156 }
157 EXPORT_SYMBOL(wprobe_add_iface);
158
159 void
160 wprobe_remove_iface(struct wprobe_iface *s)
161 {
162 unsigned long flags;
163
164 BUG_ON(!list_empty(&s->links));
165
166 del_timer_sync(&s->measure_timer);
167 spin_lock_irqsave(&wprobe_lock, flags);
168 list_del_rcu(&s->list);
169 spin_unlock_irqrestore(&wprobe_lock, flags);
170
171 /* wait for all queries to finish before freeing the
172 * temporary value storage buffer */
173 synchronize_rcu();
174
175 kfree(s->val);
176 kfree(s->query_val);
177 if (s->active_filter)
178 wprobe_free_filter(s->active_filter);
179 }
180 EXPORT_SYMBOL(wprobe_remove_iface);
181
182 static struct wprobe_iface *
183 wprobe_get_dev(struct nlattr *attr)
184 {
185 struct wprobe_iface *dev = NULL;
186 struct wprobe_iface *p;
187 const char *name;
188 int i = 0;
189
190 if (!attr)
191 return NULL;
192
193 name = nla_data(attr);
194 list_for_each_entry_rcu(p, &wprobe_if, list) {
195 i++;
196 if (strcmp(name, p->name) != 0)
197 continue;
198
199 dev = p;
200 break;
201 }
202
203 return dev;
204 }
205
206 int
207 wprobe_add_frame(struct wprobe_iface *dev, const struct wprobe_wlan_hdr *hdr, void *data, int len)
208 {
209 struct wprobe_wlan_hdr *new_hdr;
210 struct wprobe_filter *f;
211 struct sk_buff *skb;
212 unsigned long flags;
213 int i, j;
214
215 rcu_read_lock();
216 f = rcu_dereference(dev->active_filter);
217 if (!f)
218 goto out;
219
220 spin_lock_irqsave(&f->lock, flags);
221
222 skb = f->skb;
223 skb->len = sizeof(struct wprobe_rtap_hdr);
224 skb->tail = skb->data + skb->len;
225 if (len + skb->len > WPROBE_MAX_FRAME_SIZE)
226 len = WPROBE_MAX_FRAME_SIZE - skb->len;
227
228 new_hdr = (struct wprobe_wlan_hdr *) skb_put(skb, f->hdrlen);
229 memcpy(new_hdr, hdr, sizeof(struct wprobe_wlan_hdr));
230 new_hdr->len = cpu_to_be16(new_hdr->len);
231
232 memcpy(skb_put(skb, len), data, len);
233
234 for(i = 0; i < f->n_groups; i++) {
235 struct wprobe_filter_group *fg = &f->groups[i];
236 bool found = false;
237 int def = -1;
238
239 for (j = 0; j < fg->n_items; j++) {
240 struct wprobe_filter_item *fi = fg->items[j];
241
242 if (!fi->hdr.n_items) {
243 def = j;
244 continue;
245 }
246 if (sk_run_filter(skb, fi->filter, fi->hdr.n_items) == 0)
247 continue;
248
249 found = true;
250 break;
251 }
252 if (!found && def >= 0) {
253 j = def;
254 found = true;
255 }
256 if (found) {
257 struct wprobe_filter_counter *c = &fg->counters[j];
258
259 if (hdr->type >= WPROBE_PKT_TX)
260 c->tx++;
261 else
262 c->rx++;
263 }
264 }
265
266 spin_unlock_irqrestore(&f->lock, flags);
267 out:
268 rcu_read_unlock();
269 return 0;
270 }
271 EXPORT_SYMBOL(wprobe_add_frame);
272
273 static int
274 wprobe_sync_data(struct wprobe_iface *dev, struct wprobe_link *l, bool query)
275 {
276 struct wprobe_value *val;
277 unsigned long flags;
278 int n, err;
279
280 if (l) {
281 n = dev->n_link_items;
282 val = l->val;
283 } else {
284 n = dev->n_global_items;
285 val = dev->val;
286 }
287
288 spin_lock_irqsave(&dev->lock, flags);
289 err = dev->sync_data(dev, l, val, !query);
290 if (err)
291 goto done;
292
293 if (query)
294 memcpy(dev->query_val, val, sizeof(struct wprobe_value) * n);
295
296 wprobe_update_stats(dev, l);
297 done:
298 spin_unlock_irqrestore(&dev->lock, flags);
299 return 0;
300 }
301 EXPORT_SYMBOL(wprobe_sync_data);
302
303 static void
304 wprobe_scale_stats(struct wprobe_iface *dev, const struct wprobe_item *item,
305 struct wprobe_value *val, int n)
306 {
307 u64 scale_ts = jiffies_64;
308 int i;
309
310 for (i = 0; i < n; i++) {
311 if (!(item[i].flags & WPROBE_F_KEEPSTAT))
312 continue;
313
314 if (val[i].n <= dev->scale_min)
315 continue;
316
317 /* FIXME: div_s64 seems to be very imprecise here, even when
318 * the values are scaled up */
319 val[i].s *= dev->scale_m;
320 val[i].s = div_s64(val[i].s, dev->scale_d);
321
322 val[i].ss *= dev->scale_m;
323 val[i].ss = div_s64(val[i].ss, dev->scale_d);
324
325 val[i].n = (val[i].n * dev->scale_m) / dev->scale_d;
326 val[i].scale_timestamp = scale_ts;
327 }
328 }
329
330
331 void
332 wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l)
333 {
334 const struct wprobe_item *item;
335 struct wprobe_value *val;
336 bool scale_stats = false;
337 int i, n;
338
339 if (l) {
340 n = dev->n_link_items;
341 item = dev->link_items;
342 val = l->val;
343 } else {
344 n = dev->n_global_items;
345 item = dev->global_items;
346 val = dev->val;
347 }
348
349 /* process statistics */
350 for (i = 0; i < n; i++) {
351 s64 v;
352
353 if (!val[i].pending)
354 continue;
355
356 val[i].n++;
357 if ((item[i].flags & WPROBE_F_KEEPSTAT) &&
358 (dev->scale_max > 0) && (val[i].n > dev->scale_max)) {
359 scale_stats = true;
360 }
361
362 switch(item[i].type) {
363 case WPROBE_VAL_S8:
364 v = val[i].S8;
365 break;
366 case WPROBE_VAL_S16:
367 v = val[i].S16;
368 break;
369 case WPROBE_VAL_S32:
370 v = val[i].S32;
371 break;
372 case WPROBE_VAL_S64:
373 v = val[i].S64;
374 break;
375 case WPROBE_VAL_U8:
376 v = val[i].U8;
377 break;
378 case WPROBE_VAL_U16:
379 v = val[i].U16;
380 break;
381 case WPROBE_VAL_U32:
382 v = val[i].U32;
383 break;
384 case WPROBE_VAL_U64:
385 v = val[i].U64;
386 break;
387 default:
388 continue;
389 }
390
391 val[i].s += v;
392 val[i].ss += v * v;
393 val[i].pending = false;
394 }
395 if (scale_stats)
396 wprobe_scale_stats(dev, item, val, n);
397 }
398 EXPORT_SYMBOL(wprobe_update_stats);
399
400 static const struct nla_policy wprobe_policy[WPROBE_ATTR_LAST+1] = {
401 [WPROBE_ATTR_INTERFACE] = { .type = NLA_NUL_STRING },
402 [WPROBE_ATTR_MAC] = { .type = NLA_STRING },
403 [WPROBE_ATTR_FLAGS] = { .type = NLA_U32 },
404
405 /* config */
406 [WPROBE_ATTR_INTERVAL] = { .type = NLA_MSECS },
407 [WPROBE_ATTR_SAMPLES_MIN] = { .type = NLA_U32 },
408 [WPROBE_ATTR_SAMPLES_MAX] = { .type = NLA_U32 },
409 [WPROBE_ATTR_SAMPLES_SCALE_M] = { .type = NLA_U32 },
410 [WPROBE_ATTR_SAMPLES_SCALE_D] = { .type = NLA_U32 },
411 [WPROBE_ATTR_FILTER] = { .type = NLA_BINARY, .len = 32768 },
412 };
413
414 static bool
415 wprobe_check_ptr(struct list_head *list, struct list_head *ptr)
416 {
417 struct list_head *p;
418
419 list_for_each_rcu(p, list) {
420 if (ptr == p)
421 return true;
422 }
423 return false;
424 }
425
426 static bool
427 wprobe_send_item_value(struct sk_buff *msg, struct netlink_callback *cb,
428 struct wprobe_iface *dev, struct wprobe_link *l,
429 const struct wprobe_item *item,
430 int i, u32 flags)
431 {
432 struct genlmsghdr *hdr;
433 struct wprobe_value *val = dev->query_val;
434 u64 time = val[i].last - val[i].first;
435
436 hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
437 &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_INFO);
438
439 NLA_PUT_U32(msg, WPROBE_ATTR_ID, i);
440 NLA_PUT_U32(msg, WPROBE_ATTR_FLAGS, flags);
441 NLA_PUT_U8(msg, WPROBE_ATTR_TYPE, item[i].type);
442 NLA_PUT_U64(msg, WPROBE_ATTR_DURATION, time);
443
444 switch(item[i].type) {
445 case WPROBE_VAL_S8:
446 case WPROBE_VAL_U8:
447 NLA_PUT_U8(msg, item[i].type, val[i].U8);
448 break;
449 case WPROBE_VAL_S16:
450 case WPROBE_VAL_U16:
451 NLA_PUT_U16(msg, item[i].type, val[i].U16);
452 break;
453 case WPROBE_VAL_S32:
454 case WPROBE_VAL_U32:
455 NLA_PUT_U32(msg, item[i].type, val[i].U32);
456 break;
457 case WPROBE_VAL_S64:
458 case WPROBE_VAL_U64:
459 NLA_PUT_U64(msg, item[i].type, val[i].U64);
460 break;
461 case WPROBE_VAL_STRING:
462 if (val[i].STRING)
463 NLA_PUT_STRING(msg, item[i].type, val[i].STRING);
464 else
465 NLA_PUT_STRING(msg, item[i].type, "");
466 /* bypass avg/stdev */
467 goto done;
468 default:
469 /* skip unknown values */
470 goto done;
471 }
472 if (item[i].flags & WPROBE_F_KEEPSTAT) {
473 NLA_PUT_U64(msg, WPROBE_VAL_SUM, val[i].s);
474 NLA_PUT_U64(msg, WPROBE_VAL_SUM_SQ, val[i].ss);
475 NLA_PUT_U32(msg, WPROBE_VAL_SAMPLES, (u32) val[i].n);
476 NLA_PUT_MSECS(msg, WPROBE_VAL_SCALE_TIME, val[i].scale_timestamp);
477 }
478 done:
479 genlmsg_end(msg, hdr);
480 return true;
481
482 nla_put_failure:
483 genlmsg_cancel(msg, hdr);
484 return false;
485 }
486
487 static bool
488 wprobe_send_item_info(struct sk_buff *msg, struct netlink_callback *cb,
489 struct wprobe_iface *dev,
490 const struct wprobe_item *item, int i)
491 {
492 struct genlmsghdr *hdr;
493
494 hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
495 &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_LIST);
496
497 if ((i == 0) && (dev->addr != NULL))
498 NLA_PUT(msg, WPROBE_ATTR_MAC, 6, dev->addr);
499 NLA_PUT_U32(msg, WPROBE_ATTR_ID, (u32) i);
500 NLA_PUT_STRING(msg, WPROBE_ATTR_NAME, item[i].name);
501 NLA_PUT_U8(msg, WPROBE_ATTR_TYPE, item[i].type);
502 NLA_PUT_U32(msg, WPROBE_ATTR_FLAGS, item[i].flags);
503 genlmsg_end(msg, hdr);
504 return true;
505
506 nla_put_failure:
507 genlmsg_cancel(msg, hdr);
508 return false;
509 }
510
511
512 static struct wprobe_link *
513 wprobe_find_link(struct wprobe_iface *dev, const char *mac)
514 {
515 struct wprobe_link *l;
516
517 list_for_each_entry_rcu(l, &dev->links, list) {
518 if (!memcmp(l->addr, mac, 6))
519 return l;
520 }
521 return NULL;
522 }
523
524 static bool
525 wprobe_dump_filter_group(struct sk_buff *msg, struct wprobe_filter_group *fg, struct netlink_callback *cb)
526 {
527 struct genlmsghdr *hdr;
528 struct nlattr *group, *item;
529 int i;
530
531 hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
532 &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_FILTER);
533 if (!hdr)
534 return false;
535
536 NLA_PUT_STRING(msg, WPROBE_ATTR_NAME, fg->name);
537 group = nla_nest_start(msg, WPROBE_ATTR_FILTER_GROUP);
538 for (i = 0; i < fg->n_items; i++) {
539 struct wprobe_filter_item *fi = fg->items[i];
540 struct wprobe_filter_counter *fc = &fg->counters[i];
541
542 item = nla_nest_start(msg, WPROBE_ATTR_FILTER_GROUP);
543 NLA_PUT_STRING(msg, WPROBE_ATTR_NAME, fi->hdr.name);
544 NLA_PUT_U64(msg, WPROBE_ATTR_RXCOUNT, fc->rx);
545 NLA_PUT_U64(msg, WPROBE_ATTR_TXCOUNT, fc->tx);
546 nla_nest_end(msg, item);
547 }
548
549 nla_nest_end(msg, group);
550 genlmsg_end(msg, hdr);
551 return true;
552
553 nla_put_failure:
554 genlmsg_cancel(msg, hdr);
555 return false;
556 }
557
558 static int
559 wprobe_dump_filters(struct sk_buff *skb, struct netlink_callback *cb)
560 {
561 struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0];
562 struct wprobe_filter *f;
563 int err = 0;
564 int i = 0;
565
566 if (!dev) {
567 err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize,
568 wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy);
569 if (err)
570 goto done;
571
572 dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]);
573 if (!dev) {
574 err = -ENODEV;
575 goto done;
576 }
577
578 cb->args[0] = (long) dev;
579 cb->args[1] = 0;
580 } else {
581 if (!wprobe_check_ptr(&wprobe_if, &dev->list)) {
582 err = -ENODEV;
583 goto done;
584 }
585 }
586
587 rcu_read_lock();
588 f = rcu_dereference(dev->active_filter);
589 if (!f)
590 goto abort;
591
592 for (i = cb->args[1]; i < f->n_groups; i++) {
593 if (unlikely(!wprobe_dump_filter_group(skb, &f->groups[i], cb)))
594 break;
595 }
596 cb->args[1] = i;
597 abort:
598 rcu_read_unlock();
599 err = skb->len;
600 done:
601 return err;
602 }
603
604 static bool
605 wprobe_dump_link(struct sk_buff *msg, struct wprobe_link *l, struct netlink_callback *cb)
606 {
607 struct genlmsghdr *hdr;
608
609 hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
610 &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_LINKS);
611 if (!hdr)
612 return false;
613
614 NLA_PUT(msg, WPROBE_ATTR_MAC, 6, l->addr);
615 genlmsg_end(msg, hdr);
616 return true;
617
618 nla_put_failure:
619 genlmsg_cancel(msg, hdr);
620 return false;
621 }
622
623 static int
624 wprobe_dump_links(struct sk_buff *skb, struct netlink_callback *cb)
625 {
626 struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0];
627 struct wprobe_link *l;
628 int err = 0;
629 int i = 0;
630
631 if (!dev) {
632 err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize,
633 wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy);
634 if (err)
635 goto done;
636
637 dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]);
638 if (!dev) {
639 err = -ENODEV;
640 goto done;
641 }
642
643 cb->args[0] = (long) dev;
644 } else {
645 if (!wprobe_check_ptr(&wprobe_if, &dev->list)) {
646 err = -ENODEV;
647 goto done;
648 }
649 }
650
651 rcu_read_lock();
652 list_for_each_entry_rcu(l, &dev->links, list) {
653 if (i < cb->args[1])
654 continue;
655
656 if (unlikely(!wprobe_dump_link(skb, l, cb)))
657 break;
658
659 i++;
660 }
661 cb->args[1] = i;
662 rcu_read_unlock();
663 err = skb->len;
664 done:
665 return err;
666 }
667
668 #define WPROBE_F_LINK (1 << 31) /* for internal use */
669 static int
670 wprobe_dump_info(struct sk_buff *skb, struct netlink_callback *cb)
671 {
672 struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0];
673 struct wprobe_link *l = (struct wprobe_link *)cb->args[1];
674 struct wprobe_value *val;
675 const struct wprobe_item *item;
676 struct genlmsghdr *hdr;
677 unsigned long flags;
678 int cmd, n, i = cb->args[3];
679 u32 vflags = cb->args[2];
680 int err = 0;
681
682 hdr = (struct genlmsghdr *)nlmsg_data(cb->nlh);
683 cmd = hdr->cmd;
684
685 /* since the attribute value list might be too big for a single netlink
686 * message, the device, link and offset get stored in the netlink callback.
687 * if this is the first request, we need to do the full lookup for the device.
688 *
689 * access to the device and link structure is synchronized through rcu.
690 */
691 rcu_read_lock();
692 if (!dev) {
693 err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize,
694 wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy);
695 if (err)
696 goto done;
697
698 err = -ENOENT;
699 dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]);
700 if (!dev)
701 goto done;
702
703 if (cmd == WPROBE_CMD_GET_INFO) {
704 if (wprobe_fam.attrbuf[WPROBE_ATTR_MAC]) {
705 l = wprobe_find_link(dev, nla_data(wprobe_fam.attrbuf[WPROBE_ATTR_MAC]));
706 if (!l)
707 goto done;
708
709 vflags = l->flags;
710 }
711
712 if (l) {
713 item = dev->link_items;
714 n = dev->n_link_items;
715 val = l->val;
716 } else {
717 item = dev->global_items;
718 n = dev->n_global_items;
719 val = dev->val;
720 }
721
722 /* sync data and move to temp storage for the query */
723 spin_lock_irqsave(&dev->lock, flags);
724 err = wprobe_sync_data(dev, l, true);
725 if (!err)
726 memcpy(dev->query_val, val, n * sizeof(struct wprobe_value));
727 spin_unlock_irqrestore(&dev->lock, flags);
728
729 if (err)
730 goto done;
731 }
732
733 if (wprobe_fam.attrbuf[WPROBE_ATTR_FLAGS])
734 vflags |= nla_get_u32(wprobe_fam.attrbuf[WPROBE_ATTR_FLAGS]);
735
736 if (wprobe_fam.attrbuf[WPROBE_ATTR_MAC])
737 vflags |= WPROBE_F_LINK;
738
739 cb->args[0] = (long) dev;
740 cb->args[1] = (long) l;
741 cb->args[2] = vflags;
742 cb->args[3] = 0;
743 } else {
744 /* when pulling pointers from the callback, validate them
745 * against the list using rcu to make sure that we won't
746 * dereference pointers to free'd memory after the last
747 * grace period */
748 err = -ENOENT;
749 if (!wprobe_check_ptr(&wprobe_if, &dev->list))
750 goto done;
751
752 if (l && !wprobe_check_ptr(&dev->links, &l->list))
753 goto done;
754 }
755
756 if (vflags & WPROBE_F_LINK) {
757 item = dev->link_items;
758 n = dev->n_link_items;
759 } else {
760 item = dev->global_items;
761 n = dev->n_global_items;
762 }
763
764 err = 0;
765 switch(cmd) {
766 case WPROBE_CMD_GET_INFO:
767 while (i < n) {
768 if (!wprobe_send_item_value(skb, cb, dev, l, item, i, vflags))
769 break;
770 i++;
771 }
772 break;
773 case WPROBE_CMD_GET_LIST:
774 while (i < n) {
775 if (!wprobe_send_item_info(skb, cb, dev, item, i))
776 break;
777 i++;
778 }
779 break;
780 default:
781 err = -EINVAL;
782 goto done;
783 }
784 cb->args[3] = i;
785 err = skb->len;
786
787 done:
788 rcu_read_unlock();
789 return err;
790 }
791 #undef WPROBE_F_LINK
792
793 static int
794 wprobe_update_auto_measurement(struct wprobe_iface *dev, u32 interval)
795 {
796 if (interval && (interval < WPROBE_MIN_INTERVAL))
797 return -EINVAL;
798
799 if (!interval && dev->measure_interval)
800 del_timer_sync(&dev->measure_timer);
801
802 dev->measure_interval = interval;
803 if (!interval)
804 return 0;
805
806 /* kick of a new measurement immediately */
807 mod_timer(&dev->measure_timer, jiffies + 1);
808
809 return 0;
810 }
811
812 static int
813 wprobe_measure(struct sk_buff *skb, struct genl_info *info)
814 {
815 struct wprobe_iface *dev;
816 struct wprobe_link *l = NULL;
817 int err = -ENOENT;
818
819 rcu_read_lock();
820 dev = wprobe_get_dev(info->attrs[WPROBE_ATTR_INTERFACE]);
821 if (!dev)
822 goto done;
823
824 if (info->attrs[WPROBE_ATTR_MAC]) {
825 l = wprobe_find_link(dev, nla_data(wprobe_fam.attrbuf[WPROBE_ATTR_MAC]));
826 if (!l)
827 goto done;
828 }
829
830 err = wprobe_sync_data(dev, l, false);
831
832 done:
833 rcu_read_unlock();
834 return err;
835 }
836
837 static int
838 wprobe_check_filter(void *data, int datalen, int gs)
839 {
840 struct wprobe_filter_item_hdr *hdr;
841 void *orig_data = data;
842 void *end = data + datalen;
843 int i, j, k, is, cur_is;
844
845 for (i = j = is = 0; i < gs; i++) {
846 hdr = data;
847 data += sizeof(*hdr);
848
849 if (data > end)
850 goto overrun;
851
852 hdr->name[31] = 0;
853 cur_is = be32_to_cpu(hdr->n_items);
854 hdr->n_items = cur_is;
855 is += cur_is;
856 for (j = 0; j < cur_is; j++) {
857 struct sock_filter *sf;
858 int n_items;
859
860 hdr = data;
861 data += sizeof(*hdr);
862 if (data > end)
863 goto overrun;
864
865 hdr->name[31] = 0;
866 n_items = be32_to_cpu(hdr->n_items);
867 hdr->n_items = n_items;
868
869 if (n_items > 1024)
870 goto overrun;
871
872 sf = data;
873 if (n_items > 0) {
874 for (k = 0; k < n_items; k++) {
875 sf->code = be16_to_cpu(sf->code);
876 sf->k = be32_to_cpu(sf->k);
877 sf++;
878 }
879 if (sk_chk_filter(data, n_items) != 0) {
880 printk("%s: filter check failed at group %d, item %d\n", __func__, i, j);
881 return 0;
882 }
883 }
884 data += n_items * sizeof(struct sock_filter);
885 }
886 }
887 return is;
888
889 overrun:
890 printk(KERN_ERR "%s: overrun during filter check at group %d, item %d, offset=%d, len=%d\n", __func__, i, j, (data - orig_data), datalen);
891 return 0;
892 }
893
894 static void
895 wprobe_free_filter(struct wprobe_filter *f)
896 {
897 if (f->skb)
898 kfree_skb(f->skb);
899 if (f->data)
900 kfree(f->data);
901 if (f->items)
902 kfree(f->items);
903 if (f->counters)
904 kfree(f->counters);
905 kfree(f);
906 }
907
908
909 static int
910 wprobe_set_filter(struct wprobe_iface *dev, void *data, int len)
911 {
912 struct wprobe_filter_hdr *fhdr;
913 struct wprobe_rtap_hdr *rtap;
914 struct wprobe_filter *f;
915 int i, j, cur_is, is, gs;
916
917 if (len < sizeof(*fhdr))
918 return -EINVAL;
919
920 fhdr = data;
921 data += sizeof(*fhdr);
922 len -= sizeof(*fhdr);
923
924 if (memcmp(fhdr->magic, "WPFF", 4) != 0) {
925 printk(KERN_ERR "%s: filter rejected (invalid magic)\n", __func__);
926 return -EINVAL;
927 }
928
929 gs = be16_to_cpu(fhdr->n_groups);
930 is = wprobe_check_filter(data, len, gs);
931 if (is == 0)
932 return -EINVAL;
933
934 f = kzalloc(sizeof(struct wprobe_filter) +
935 gs * sizeof(struct wprobe_filter_group), GFP_ATOMIC);
936 if (!f)
937 return -ENOMEM;
938
939 f->skb = alloc_skb(WPROBE_MAX_FRAME_SIZE, GFP_ATOMIC);
940 if (!f->skb)
941 goto error;
942
943 f->data = kmalloc(len, GFP_ATOMIC);
944 if (!f->data)
945 goto error;
946
947 f->items = kzalloc(sizeof(struct wprobe_filter_item *) * is, GFP_ATOMIC);
948 if (!f->items)
949 goto error;
950
951 f->counters = kzalloc(sizeof(struct wprobe_filter_counter) * is, GFP_ATOMIC);
952 if (!f->counters)
953 goto error;
954
955 spin_lock_init(&f->lock);
956 memcpy(f->data, data, len);
957 f->n_groups = gs;
958
959 if (f->hdrlen < sizeof(struct wprobe_wlan_hdr))
960 f->hdrlen = sizeof(struct wprobe_wlan_hdr);
961
962 rtap = (struct wprobe_rtap_hdr *)skb_put(f->skb, sizeof(*rtap));
963 memset(rtap, 0, sizeof(*rtap));
964 rtap->len = cpu_to_le16(sizeof(struct wprobe_rtap_hdr) + f->hdrlen);
965 data = f->data;
966
967 cur_is = 0;
968 for (i = 0; i < gs; i++) {
969 struct wprobe_filter_item_hdr *hdr = data;
970 struct wprobe_filter_group *g = &f->groups[i];
971
972 data += sizeof(*hdr);
973 g->name = hdr->name;
974 g->items = &f->items[cur_is];
975 g->counters = &f->counters[cur_is];
976 g->n_items = hdr->n_items;
977
978 for (j = 0; j < g->n_items; j++) {
979 hdr = data;
980 f->items[cur_is++] = data;
981 data += sizeof(*hdr) + hdr->n_items * sizeof(struct sock_filter);
982 }
983 }
984 rcu_assign_pointer(dev->active_filter, f);
985 return 0;
986
987 error:
988 wprobe_free_filter(f);
989 return -ENOMEM;
990 }
991
992 static int
993 wprobe_set_config(struct sk_buff *skb, struct genl_info *info)
994 {
995 struct wprobe_iface *dev;
996 unsigned long flags;
997 int err = -ENOENT;
998 u32 scale_min, scale_max;
999 u32 scale_m, scale_d;
1000 struct nlattr *attr;
1001 struct wprobe_filter *filter_free = NULL;
1002
1003 rcu_read_lock();
1004 dev = wprobe_get_dev(info->attrs[WPROBE_ATTR_INTERFACE]);
1005 if (!dev)
1006 goto done_unlocked;
1007
1008 err = -EINVAL;
1009 spin_lock_irqsave(&dev->lock, flags);
1010 if (info->attrs[WPROBE_ATTR_MAC]) {
1011 /* not supported yet */
1012 goto done;
1013 }
1014
1015 if (info->attrs[WPROBE_ATTR_FLAGS]) {
1016 u32 flags = nla_get_u32(info->attrs[WPROBE_ATTR_FLAGS]);
1017
1018 if (flags & BIT(WPROBE_F_RESET)) {
1019 struct wprobe_link *l;
1020
1021 memset(dev->val, 0, sizeof(struct wprobe_value) * dev->n_global_items);
1022 list_for_each_entry_rcu(l, &dev->links, list) {
1023 memset(l->val, 0, sizeof(struct wprobe_value) * dev->n_link_items);
1024 }
1025 }
1026 }
1027
1028 if (info->attrs[WPROBE_ATTR_SAMPLES_MIN] ||
1029 info->attrs[WPROBE_ATTR_SAMPLES_MAX]) {
1030 if ((attr = info->attrs[WPROBE_ATTR_SAMPLES_MIN]))
1031 scale_min = nla_get_u32(attr);
1032 else
1033 scale_min = dev->scale_min;
1034
1035 if ((attr = info->attrs[WPROBE_ATTR_SAMPLES_MAX]))
1036 scale_max = nla_get_u32(attr);
1037 else
1038 scale_max = dev->scale_max;
1039
1040 if ((!scale_min && !scale_max) ||
1041 (scale_min && scale_max && (scale_min < scale_max))) {
1042 dev->scale_min = scale_min;
1043 dev->scale_max = scale_max;
1044 } else {
1045 goto done;
1046 }
1047 }
1048
1049 if (info->attrs[WPROBE_ATTR_SAMPLES_SCALE_M] &&
1050 info->attrs[WPROBE_ATTR_SAMPLES_SCALE_D]) {
1051
1052 scale_m = nla_get_u32(info->attrs[WPROBE_ATTR_SAMPLES_SCALE_M]);
1053 scale_d = nla_get_u32(info->attrs[WPROBE_ATTR_SAMPLES_SCALE_D]);
1054
1055 if (!scale_d || (scale_m > scale_d))
1056 goto done;
1057
1058 dev->scale_m = scale_m;
1059 dev->scale_d = scale_d;
1060 }
1061
1062 if ((attr = info->attrs[WPROBE_ATTR_FILTER])) {
1063 filter_free = rcu_dereference(dev->active_filter);
1064 rcu_assign_pointer(dev->active_filter, NULL);
1065 if (nla_len(attr) > 0)
1066 wprobe_set_filter(dev, nla_data(attr), nla_len(attr));
1067 }
1068
1069 err = 0;
1070 if (info->attrs[WPROBE_ATTR_INTERVAL]) {
1071 /* change of measurement interval requested */
1072 err = wprobe_update_auto_measurement(dev,
1073 (u32) nla_get_u64(info->attrs[WPROBE_ATTR_INTERVAL]));
1074 }
1075
1076 done:
1077 spin_unlock_irqrestore(&dev->lock, flags);
1078 done_unlocked:
1079 rcu_read_unlock();
1080 if (filter_free) {
1081 synchronize_rcu();
1082 wprobe_free_filter(filter_free);
1083 }
1084 return err;
1085 }
1086
1087 static struct genl_ops wprobe_ops[] = {
1088 {
1089 .cmd = WPROBE_CMD_GET_INFO,
1090 .dumpit = wprobe_dump_info,
1091 .policy = wprobe_policy,
1092 },
1093 {
1094 .cmd = WPROBE_CMD_GET_LIST,
1095 .dumpit = wprobe_dump_info,
1096 .policy = wprobe_policy,
1097 },
1098 {
1099 .cmd = WPROBE_CMD_MEASURE,
1100 .doit = wprobe_measure,
1101 .policy = wprobe_policy,
1102 },
1103 {
1104 .cmd = WPROBE_CMD_GET_LINKS,
1105 .dumpit = wprobe_dump_links,
1106 .policy = wprobe_policy,
1107 },
1108 {
1109 .cmd = WPROBE_CMD_CONFIG,
1110 .doit = wprobe_set_config,
1111 .policy = wprobe_policy,
1112 },
1113 {
1114 .cmd = WPROBE_CMD_GET_FILTER,
1115 .dumpit = wprobe_dump_filters,
1116 .policy = wprobe_policy,
1117 },
1118 };
1119
1120 static void __exit
1121 wprobe_exit(void)
1122 {
1123 BUG_ON(!list_empty(&wprobe_if));
1124 genl_unregister_family(&wprobe_fam);
1125 }
1126
1127
1128 static int __init
1129 wprobe_init(void)
1130 {
1131 int i, err;
1132
1133 spin_lock_init(&wprobe_lock);
1134 INIT_LIST_HEAD(&wprobe_if);
1135
1136 err = genl_register_family(&wprobe_fam);
1137 if (err)
1138 return err;
1139
1140 for (i = 0; i < ARRAY_SIZE(wprobe_ops); i++) {
1141 err = genl_register_ops(&wprobe_fam, &wprobe_ops[i]);
1142 if (err)
1143 goto error;
1144 }
1145
1146 return 0;
1147
1148 error:
1149 genl_unregister_family(&wprobe_fam);
1150 return err;
1151 }
1152
1153 module_init(wprobe_init);
1154 module_exit(wprobe_exit);
1155 MODULE_LICENSE("GPL");
1156
OpenWrt SVN history