Root/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
41static struct list_head wprobe_if;
42static spinlock_t wprobe_lock;
43
44static 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 */
54struct wprobe_rtap_hdr {
55    __u8 version;
56    __u8 padding;
57    __le16 len;
58    __le32 present;
59};
60
61static void wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l);
62static int wprobe_sync_data(struct wprobe_iface *dev, struct wprobe_link *l, bool query);
63static void wprobe_free_filter(struct wprobe_filter *f);
64
65int
66wprobe_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}
83EXPORT_SYMBOL(wprobe_add_link);
84
85void
86wprobe_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}
96EXPORT_SYMBOL(wprobe_remove_link);
97
98static void
99wprobe_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
111int
112wprobe_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
149error:
150    if (s->val)
151        kfree(s->val);
152    return -ENOMEM;
153}
154EXPORT_SYMBOL(wprobe_add_iface);
155
156void
157wprobe_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}
177EXPORT_SYMBOL(wprobe_remove_iface);
178
179static struct wprobe_iface *
180wprobe_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
203int
204wprobe_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);
264out:
265    rcu_read_unlock();
266    return 0;
267}
268EXPORT_SYMBOL(wprobe_add_frame);
269
270static int
271wprobe_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);
294done:
295    spin_unlock_irqrestore(&dev->lock, flags);
296    return 0;
297}
298EXPORT_SYMBOL(wprobe_sync_data);
299
300static void
301wprobe_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
328void
329wprobe_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}
395EXPORT_SYMBOL(wprobe_update_stats);
396
397static 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
411static bool
412wprobe_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
423static bool
424wprobe_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    }
475done:
476    genlmsg_end(msg, hdr);
477    return true;
478
479nla_put_failure:
480    genlmsg_cancel(msg, hdr);
481    return false;
482}
483
484static bool
485wprobe_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
503nla_put_failure:
504    genlmsg_cancel(msg, hdr);
505    return false;
506}
507
508
509static struct wprobe_link *
510wprobe_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
521static bool
522wprobe_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
550nla_put_failure:
551    genlmsg_cancel(msg, hdr);
552    return false;
553}
554
555static int
556wprobe_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;
594abort:
595    rcu_read_unlock();
596    err = skb->len;
597done:
598    return err;
599}
600
601static bool
602wprobe_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
615nla_put_failure:
616    genlmsg_cancel(msg, hdr);
617    return false;
618}
619
620static int
621wprobe_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;
661done:
662    return err;
663}
664
665#define WPROBE_F_LINK (1 << 31) /* for internal use */
666static int
667wprobe_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
784done:
785    rcu_read_unlock();
786    return err;
787}
788#undef WPROBE_F_LINK
789
790static int
791wprobe_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
809static int
810wprobe_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
829done:
830    rcu_read_unlock();
831    return err;
832}
833
834static int
835wprobe_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
886overrun:
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
891static void
892wprobe_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
906static int
907wprobe_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
984error:
985    wprobe_free_filter(f);
986    return -ENOMEM;
987}
988
989static int
990wprobe_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
1073done:
1074    spin_unlock_irqrestore(&dev->lock, flags);
1075done_unlocked:
1076    rcu_read_unlock();
1077    if (filter_free) {
1078        synchronize_rcu();
1079        wprobe_free_filter(filter_free);
1080    }
1081    return err;
1082}
1083
1084static 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
1117static void __exit
1118wprobe_exit(void)
1119{
1120    BUG_ON(!list_empty(&wprobe_if));
1121    genl_unregister_family(&wprobe_fam);
1122}
1123
1124
1125static int __init
1126wprobe_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
1145error:
1146    genl_unregister_family(&wprobe_fam);
1147    return err;
1148}
1149
1150module_init(wprobe_init);
1151module_exit(wprobe_exit);
1152MODULE_LICENSE("GPL");
1153
1154

Archive Download this file

Branches:
backfire
history
master
release_2010-11-17
release_2010-12-14
release_2011-02-23
release_2011-05-28
release_2011-08-27
release_2011-11-13
release_2012-03-18
release_2012-04-09
release_2012-10-11
uclibc_nptl



interactive