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


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