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

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