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


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