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


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