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#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) \
38for (pos = rcu_dereference((head)->next); \
39prefetch(pos->next), pos != (head); \
40pos = 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
47static struct list_head wprobe_if;
48static spinlock_t wprobe_lock;
49
50static 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 */
60struct wprobe_rtap_hdr {
61    __u8 version;
62    __u8 padding;
63    __le16 len;
64    __le32 present;
65};
66
67static void wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l);
68static int wprobe_sync_data(struct wprobe_iface *dev, struct wprobe_link *l, bool query);
69static void wprobe_free_filter(struct wprobe_filter *f);
70
71int
72wprobe_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}
89EXPORT_SYMBOL(wprobe_add_link);
90
91void
92wprobe_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}
102EXPORT_SYMBOL(wprobe_remove_link);
103
104static void
105wprobe_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
117int
118wprobe_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
155error:
156    if (s->val)
157        kfree(s->val);
158    return -ENOMEM;
159}
160EXPORT_SYMBOL(wprobe_add_iface);
161
162void
163wprobe_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}
183EXPORT_SYMBOL(wprobe_remove_iface);
184
185static struct wprobe_iface *
186wprobe_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
209int
210wprobe_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);
275out:
276    rcu_read_unlock();
277    return 0;
278}
279EXPORT_SYMBOL(wprobe_add_frame);
280
281static int
282wprobe_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);
305done:
306    spin_unlock_irqrestore(&dev->lock, flags);
307    return 0;
308}
309EXPORT_SYMBOL(wprobe_sync_data);
310
311static void
312wprobe_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
339void
340wprobe_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}
406EXPORT_SYMBOL(wprobe_update_stats);
407
408static 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
422static bool
423wprobe_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
434static bool
435wprobe_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    NLA_PUT_U32(msg, WPROBE_ATTR_ID, i);
448    NLA_PUT_U32(msg, WPROBE_ATTR_FLAGS, flags);
449    NLA_PUT_U8(msg, WPROBE_ATTR_TYPE, item[i].type);
450    NLA_PUT_U64(msg, WPROBE_ATTR_DURATION, time);
451
452    switch(item[i].type) {
453    case WPROBE_VAL_S8:
454    case WPROBE_VAL_U8:
455        NLA_PUT_U8(msg, item[i].type, val[i].U8);
456        break;
457    case WPROBE_VAL_S16:
458    case WPROBE_VAL_U16:
459        NLA_PUT_U16(msg, item[i].type, val[i].U16);
460        break;
461    case WPROBE_VAL_S32:
462    case WPROBE_VAL_U32:
463        NLA_PUT_U32(msg, item[i].type, val[i].U32);
464        break;
465    case WPROBE_VAL_S64:
466    case WPROBE_VAL_U64:
467        NLA_PUT_U64(msg, item[i].type, val[i].U64);
468        break;
469    case WPROBE_VAL_STRING:
470        if (val[i].STRING)
471            NLA_PUT_STRING(msg, item[i].type, val[i].STRING);
472        else
473            NLA_PUT_STRING(msg, item[i].type, "");
474        /* bypass avg/stdev */
475        goto done;
476    default:
477        /* skip unknown values */
478        goto done;
479    }
480    if (item[i].flags & WPROBE_F_KEEPSTAT) {
481        NLA_PUT_U64(msg, WPROBE_VAL_SUM, val[i].s);
482        NLA_PUT_U64(msg, WPROBE_VAL_SUM_SQ, val[i].ss);
483        NLA_PUT_U32(msg, WPROBE_VAL_SAMPLES, (u32) val[i].n);
484        NLA_PUT_MSECS(msg, WPROBE_VAL_SCALE_TIME, val[i].scale_timestamp);
485    }
486done:
487    genlmsg_end(msg, hdr);
488    return true;
489
490nla_put_failure:
491    genlmsg_cancel(msg, hdr);
492    return false;
493}
494
495static bool
496wprobe_send_item_info(struct sk_buff *msg, struct netlink_callback *cb,
497                       struct wprobe_iface *dev,
498                       const struct wprobe_item *item, int i)
499{
500    struct genlmsghdr *hdr;
501
502    hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
503            &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_LIST);
504
505    if ((i == 0) && (dev->addr != NULL))
506        NLA_PUT(msg, WPROBE_ATTR_MAC, 6, dev->addr);
507    NLA_PUT_U32(msg, WPROBE_ATTR_ID, (u32) i);
508    NLA_PUT_STRING(msg, WPROBE_ATTR_NAME, item[i].name);
509    NLA_PUT_U8(msg, WPROBE_ATTR_TYPE, item[i].type);
510    NLA_PUT_U32(msg, WPROBE_ATTR_FLAGS, item[i].flags);
511    genlmsg_end(msg, hdr);
512    return true;
513
514nla_put_failure:
515    genlmsg_cancel(msg, hdr);
516    return false;
517}
518
519
520static struct wprobe_link *
521wprobe_find_link(struct wprobe_iface *dev, const char *mac)
522{
523    struct wprobe_link *l;
524
525    list_for_each_entry_rcu(l, &dev->links, list) {
526        if (!memcmp(l->addr, mac, 6))
527            return l;
528    }
529    return NULL;
530}
531
532static bool
533wprobe_dump_filter_group(struct sk_buff *msg, struct wprobe_filter_group *fg, struct netlink_callback *cb)
534{
535    struct genlmsghdr *hdr;
536    struct nlattr *group, *item;
537    int i;
538
539    hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
540            &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_FILTER);
541    if (!hdr)
542        return false;
543
544    NLA_PUT_STRING(msg, WPROBE_ATTR_NAME, fg->name);
545    group = nla_nest_start(msg, WPROBE_ATTR_FILTER_GROUP);
546    for (i = 0; i < fg->n_items; i++) {
547        struct wprobe_filter_item *fi = fg->items[i];
548        struct wprobe_filter_counter *fc = &fg->counters[i];
549
550        item = nla_nest_start(msg, WPROBE_ATTR_FILTER_GROUP);
551        NLA_PUT_STRING(msg, WPROBE_ATTR_NAME, fi->hdr.name);
552        NLA_PUT_U64(msg, WPROBE_ATTR_RXCOUNT, fc->rx);
553        NLA_PUT_U64(msg, WPROBE_ATTR_TXCOUNT, fc->tx);
554        nla_nest_end(msg, item);
555    }
556
557    nla_nest_end(msg, group);
558    genlmsg_end(msg, hdr);
559    return true;
560
561nla_put_failure:
562    genlmsg_cancel(msg, hdr);
563    return false;
564}
565
566static int
567wprobe_dump_filters(struct sk_buff *skb, struct netlink_callback *cb)
568{
569    struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0];
570    struct wprobe_filter *f;
571    int err = 0;
572    int i = 0;
573
574    if (!dev) {
575        err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize,
576                wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy);
577        if (err)
578            goto done;
579
580        dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]);
581        if (!dev) {
582            err = -ENODEV;
583            goto done;
584        }
585
586        cb->args[0] = (long) dev;
587        cb->args[1] = 0;
588    } else {
589        if (!wprobe_check_ptr(&wprobe_if, &dev->list)) {
590            err = -ENODEV;
591            goto done;
592        }
593    }
594
595    rcu_read_lock();
596    f = rcu_dereference(dev->active_filter);
597    if (!f)
598        goto abort;
599
600    for (i = cb->args[1]; i < f->n_groups; i++) {
601        if (unlikely(!wprobe_dump_filter_group(skb, &f->groups[i], cb)))
602            break;
603    }
604    cb->args[1] = i;
605abort:
606    rcu_read_unlock();
607    err = skb->len;
608done:
609    return err;
610}
611
612static bool
613wprobe_dump_link(struct sk_buff *msg, struct wprobe_link *l, struct netlink_callback *cb)
614{
615    struct genlmsghdr *hdr;
616
617    hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
618            &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_LINKS);
619    if (!hdr)
620        return false;
621
622    NLA_PUT(msg, WPROBE_ATTR_MAC, 6, l->addr);
623    genlmsg_end(msg, hdr);
624    return true;
625
626nla_put_failure:
627    genlmsg_cancel(msg, hdr);
628    return false;
629}
630
631static int
632wprobe_dump_links(struct sk_buff *skb, struct netlink_callback *cb)
633{
634    struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0];
635    struct wprobe_link *l;
636    int err = 0;
637    int i = 0;
638
639    if (!dev) {
640        err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize,
641                wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy);
642        if (err)
643            goto done;
644
645        dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]);
646        if (!dev) {
647            err = -ENODEV;
648            goto done;
649        }
650
651        cb->args[0] = (long) dev;
652    } else {
653        if (!wprobe_check_ptr(&wprobe_if, &dev->list)) {
654            err = -ENODEV;
655            goto done;
656        }
657    }
658
659    rcu_read_lock();
660    list_for_each_entry_rcu(l, &dev->links, list) {
661        if (i < cb->args[1])
662            continue;
663
664        if (unlikely(!wprobe_dump_link(skb, l, cb)))
665            break;
666
667        i++;
668    }
669    cb->args[1] = i;
670    rcu_read_unlock();
671    err = skb->len;
672done:
673    return err;
674}
675
676#define WPROBE_F_LINK (1 << 31) /* for internal use */
677static int
678wprobe_dump_info(struct sk_buff *skb, struct netlink_callback *cb)
679{
680    struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0];
681    struct wprobe_link *l = (struct wprobe_link *)cb->args[1];
682    struct wprobe_value *val;
683    const struct wprobe_item *item;
684    struct genlmsghdr *hdr;
685    unsigned long flags;
686    int cmd, n, i = cb->args[3];
687    u32 vflags = cb->args[2];
688    int err = 0;
689
690    hdr = (struct genlmsghdr *)nlmsg_data(cb->nlh);
691    cmd = hdr->cmd;
692
693    /* since the attribute value list might be too big for a single netlink
694     * message, the device, link and offset get stored in the netlink callback.
695     * if this is the first request, we need to do the full lookup for the device.
696     *
697     * access to the device and link structure is synchronized through rcu.
698     */
699    rcu_read_lock();
700    if (!dev) {
701        err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize,
702                wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy);
703        if (err)
704            goto done;
705
706        err = -ENOENT;
707        dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]);
708        if (!dev)
709            goto done;
710
711        if (cmd == WPROBE_CMD_GET_INFO) {
712            if (wprobe_fam.attrbuf[WPROBE_ATTR_MAC]) {
713                l = wprobe_find_link(dev, nla_data(wprobe_fam.attrbuf[WPROBE_ATTR_MAC]));
714                if (!l)
715                    goto done;
716
717                vflags = l->flags;
718            }
719
720            if (l) {
721                item = dev->link_items;
722                n = dev->n_link_items;
723                val = l->val;
724            } else {
725                item = dev->global_items;
726                n = dev->n_global_items;
727                val = dev->val;
728            }
729
730            /* sync data and move to temp storage for the query */
731            spin_lock_irqsave(&dev->lock, flags);
732            err = wprobe_sync_data(dev, l, true);
733            if (!err)
734                memcpy(dev->query_val, val, n * sizeof(struct wprobe_value));
735            spin_unlock_irqrestore(&dev->lock, flags);
736
737            if (err)
738                goto done;
739        }
740
741        if (wprobe_fam.attrbuf[WPROBE_ATTR_FLAGS])
742            vflags |= nla_get_u32(wprobe_fam.attrbuf[WPROBE_ATTR_FLAGS]);
743
744        if (wprobe_fam.attrbuf[WPROBE_ATTR_MAC])
745            vflags |= WPROBE_F_LINK;
746
747        cb->args[0] = (long) dev;
748        cb->args[1] = (long) l;
749        cb->args[2] = vflags;
750        cb->args[3] = 0;
751    } else {
752        /* when pulling pointers from the callback, validate them
753         * against the list using rcu to make sure that we won't
754         * dereference pointers to free'd memory after the last
755         * grace period */
756        err = -ENOENT;
757        if (!wprobe_check_ptr(&wprobe_if, &dev->list))
758            goto done;
759
760        if (l && !wprobe_check_ptr(&dev->links, &l->list))
761            goto done;
762    }
763
764    if (vflags & WPROBE_F_LINK) {
765        item = dev->link_items;
766        n = dev->n_link_items;
767    } else {
768        item = dev->global_items;
769        n = dev->n_global_items;
770    }
771
772    err = 0;
773    switch(cmd) {
774    case WPROBE_CMD_GET_INFO:
775        while (i < n) {
776            if (!wprobe_send_item_value(skb, cb, dev, l, item, i, vflags))
777                break;
778            i++;
779        }
780        break;
781    case WPROBE_CMD_GET_LIST:
782        while (i < n) {
783            if (!wprobe_send_item_info(skb, cb, dev, item, i))
784                break;
785            i++;
786        }
787        break;
788    default:
789        err = -EINVAL;
790        goto done;
791    }
792    cb->args[3] = i;
793    err = skb->len;
794
795done:
796    rcu_read_unlock();
797    return err;
798}
799#undef WPROBE_F_LINK
800
801static int
802wprobe_update_auto_measurement(struct wprobe_iface *dev, u32 interval)
803{
804    if (interval && (interval < WPROBE_MIN_INTERVAL))
805        return -EINVAL;
806
807    if (!interval && dev->measure_interval)
808        del_timer_sync(&dev->measure_timer);
809
810    dev->measure_interval = interval;
811    if (!interval)
812        return 0;
813
814    /* kick of a new measurement immediately */
815    mod_timer(&dev->measure_timer, jiffies + 1);
816
817    return 0;
818}
819
820static int
821wprobe_measure(struct sk_buff *skb, struct genl_info *info)
822{
823    struct wprobe_iface *dev;
824    struct wprobe_link *l = NULL;
825    int err = -ENOENT;
826
827    rcu_read_lock();
828    dev = wprobe_get_dev(info->attrs[WPROBE_ATTR_INTERFACE]);
829    if (!dev)
830        goto done;
831
832    if (info->attrs[WPROBE_ATTR_MAC]) {
833        l = wprobe_find_link(dev, nla_data(wprobe_fam.attrbuf[WPROBE_ATTR_MAC]));
834        if (!l)
835            goto done;
836    }
837
838    err = wprobe_sync_data(dev, l, false);
839
840done:
841    rcu_read_unlock();
842    return err;
843}
844
845static int
846wprobe_check_filter(void *data, int datalen, int gs)
847{
848    struct wprobe_filter_item_hdr *hdr;
849    void *orig_data = data;
850    void *end = data + datalen;
851    int i, j, k, is, cur_is;
852
853    for (i = j = is = 0; i < gs; i++) {
854        hdr = data;
855        data += sizeof(*hdr);
856
857        if (data > end)
858            goto overrun;
859
860        hdr->name[31] = 0;
861        cur_is = be32_to_cpu(hdr->n_items);
862        hdr->n_items = cur_is;
863        is += cur_is;
864        for (j = 0; j < cur_is; j++) {
865            struct sock_filter *sf;
866            int n_items;
867
868            hdr = data;
869            data += sizeof(*hdr);
870            if (data > end)
871                goto overrun;
872
873            hdr->name[31] = 0;
874            n_items = be32_to_cpu(hdr->n_items);
875            hdr->n_items = n_items;
876
877            if (n_items > 1024)
878                goto overrun;
879
880            sf = data;
881            if (n_items > 0) {
882                for (k = 0; k < n_items; k++) {
883                    sf->code = be16_to_cpu(sf->code);
884                    sf->k = be32_to_cpu(sf->k);
885                    sf++;
886                }
887                if (sk_chk_filter(data, n_items) != 0) {
888                    printk("%s: filter check failed at group %d, item %d\n", __func__, i, j);
889                    return 0;
890                }
891            }
892            data += n_items * sizeof(struct sock_filter);
893        }
894    }
895    return is;
896
897overrun:
898    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);
899    return 0;
900}
901
902static void
903wprobe_free_filter(struct wprobe_filter *f)
904{
905    if (f->skb)
906        kfree_skb(f->skb);
907    if (f->data)
908        kfree(f->data);
909    if (f->items)
910        kfree(f->items);
911    if (f->counters)
912        kfree(f->counters);
913    kfree(f);
914}
915
916
917static int
918wprobe_set_filter(struct wprobe_iface *dev, void *data, int len)
919{
920    struct wprobe_filter_hdr *fhdr;
921    struct wprobe_rtap_hdr *rtap;
922    struct wprobe_filter *f;
923    int i, j, cur_is, is, gs;
924
925    if (len < sizeof(*fhdr))
926        return -EINVAL;
927
928    fhdr = data;
929    data += sizeof(*fhdr);
930    len -= sizeof(*fhdr);
931
932    if (memcmp(fhdr->magic, "WPFF", 4) != 0) {
933        printk(KERN_ERR "%s: filter rejected (invalid magic)\n", __func__);
934        return -EINVAL;
935    }
936
937    gs = be16_to_cpu(fhdr->n_groups);
938    is = wprobe_check_filter(data, len, gs);
939    if (is == 0)
940        return -EINVAL;
941
942    f = kzalloc(sizeof(struct wprobe_filter) +
943        gs * sizeof(struct wprobe_filter_group), GFP_ATOMIC);
944    if (!f)
945        return -ENOMEM;
946
947    f->skb = alloc_skb(WPROBE_MAX_FRAME_SIZE, GFP_ATOMIC);
948    if (!f->skb)
949        goto error;
950
951    f->data = kmalloc(len, GFP_ATOMIC);
952    if (!f->data)
953        goto error;
954
955    f->items = kzalloc(sizeof(struct wprobe_filter_item *) * is, GFP_ATOMIC);
956    if (!f->items)
957        goto error;
958
959    f->counters = kzalloc(sizeof(struct wprobe_filter_counter) * is, GFP_ATOMIC);
960    if (!f->counters)
961        goto error;
962
963    spin_lock_init(&f->lock);
964    memcpy(f->data, data, len);
965    f->n_groups = gs;
966
967    if (f->hdrlen < sizeof(struct wprobe_wlan_hdr))
968        f->hdrlen = sizeof(struct wprobe_wlan_hdr);
969
970    rtap = (struct wprobe_rtap_hdr *)skb_put(f->skb, sizeof(*rtap));
971    memset(rtap, 0, sizeof(*rtap));
972    rtap->len = cpu_to_le16(sizeof(struct wprobe_rtap_hdr) + f->hdrlen);
973    data = f->data;
974
975    cur_is = 0;
976    for (i = 0; i < gs; i++) {
977        struct wprobe_filter_item_hdr *hdr = data;
978        struct wprobe_filter_group *g = &f->groups[i];
979
980        data += sizeof(*hdr);
981        g->name = hdr->name;
982        g->items = &f->items[cur_is];
983        g->counters = &f->counters[cur_is];
984        g->n_items = hdr->n_items;
985
986        for (j = 0; j < g->n_items; j++) {
987            hdr = data;
988            f->items[cur_is++] = data;
989            data += sizeof(*hdr) + hdr->n_items * sizeof(struct sock_filter);
990        }
991    }
992    rcu_assign_pointer(dev->active_filter, f);
993    return 0;
994
995error:
996    wprobe_free_filter(f);
997    return -ENOMEM;
998}
999
1000static int
1001wprobe_set_config(struct sk_buff *skb, struct genl_info *info)
1002{
1003    struct wprobe_iface *dev;
1004    unsigned long flags;
1005    int err = -ENOENT;
1006    u32 scale_min, scale_max;
1007    u32 scale_m, scale_d;
1008    struct nlattr *attr;
1009    struct wprobe_filter *filter_free = NULL;
1010
1011    rcu_read_lock();
1012    dev = wprobe_get_dev(info->attrs[WPROBE_ATTR_INTERFACE]);
1013    if (!dev)
1014        goto done_unlocked;
1015
1016    err = -EINVAL;
1017    spin_lock_irqsave(&dev->lock, flags);
1018    if (info->attrs[WPROBE_ATTR_MAC]) {
1019        /* not supported yet */
1020        goto done;
1021    }
1022
1023    if (info->attrs[WPROBE_ATTR_FLAGS]) {
1024        u32 flags = nla_get_u32(info->attrs[WPROBE_ATTR_FLAGS]);
1025
1026        if (flags & BIT(WPROBE_F_RESET)) {
1027            struct wprobe_link *l;
1028
1029            memset(dev->val, 0, sizeof(struct wprobe_value) * dev->n_global_items);
1030            list_for_each_entry_rcu(l, &dev->links, list) {
1031                memset(l->val, 0, sizeof(struct wprobe_value) * dev->n_link_items);
1032            }
1033        }
1034    }
1035
1036    if (info->attrs[WPROBE_ATTR_SAMPLES_MIN] ||
1037        info->attrs[WPROBE_ATTR_SAMPLES_MAX]) {
1038        if ((attr = info->attrs[WPROBE_ATTR_SAMPLES_MIN]))
1039            scale_min = nla_get_u32(attr);
1040        else
1041            scale_min = dev->scale_min;
1042
1043        if ((attr = info->attrs[WPROBE_ATTR_SAMPLES_MAX]))
1044            scale_max = nla_get_u32(attr);
1045        else
1046            scale_max = dev->scale_max;
1047
1048        if ((!scale_min && !scale_max) ||
1049            (scale_min && scale_max && (scale_min < scale_max))) {
1050            dev->scale_min = scale_min;
1051            dev->scale_max = scale_max;
1052        } else {
1053            goto done;
1054        }
1055    }
1056
1057    if (info->attrs[WPROBE_ATTR_SAMPLES_SCALE_M] &&
1058        info->attrs[WPROBE_ATTR_SAMPLES_SCALE_D]) {
1059
1060        scale_m = nla_get_u32(info->attrs[WPROBE_ATTR_SAMPLES_SCALE_M]);
1061        scale_d = nla_get_u32(info->attrs[WPROBE_ATTR_SAMPLES_SCALE_D]);
1062
1063        if (!scale_d || (scale_m > scale_d))
1064            goto done;
1065
1066        dev->scale_m = scale_m;
1067        dev->scale_d = scale_d;
1068    }
1069
1070    if ((attr = info->attrs[WPROBE_ATTR_FILTER])) {
1071        filter_free = rcu_dereference(dev->active_filter);
1072        rcu_assign_pointer(dev->active_filter, NULL);
1073        if (nla_len(attr) > 0)
1074            wprobe_set_filter(dev, nla_data(attr), nla_len(attr));
1075    }
1076
1077    err = 0;
1078    if (info->attrs[WPROBE_ATTR_INTERVAL]) {
1079        /* change of measurement interval requested */
1080        err = wprobe_update_auto_measurement(dev,
1081            (u32) nla_get_u64(info->attrs[WPROBE_ATTR_INTERVAL]));
1082    }
1083
1084done:
1085    spin_unlock_irqrestore(&dev->lock, flags);
1086done_unlocked:
1087    rcu_read_unlock();
1088    if (filter_free) {
1089        synchronize_rcu();
1090        wprobe_free_filter(filter_free);
1091    }
1092    return err;
1093}
1094
1095static struct genl_ops wprobe_ops[] = {
1096    {
1097        .cmd = WPROBE_CMD_GET_INFO,
1098        .dumpit = wprobe_dump_info,
1099        .policy = wprobe_policy,
1100    },
1101    {
1102        .cmd = WPROBE_CMD_GET_LIST,
1103        .dumpit = wprobe_dump_info,
1104        .policy = wprobe_policy,
1105    },
1106    {
1107        .cmd = WPROBE_CMD_MEASURE,
1108        .doit = wprobe_measure,
1109        .policy = wprobe_policy,
1110    },
1111    {
1112        .cmd = WPROBE_CMD_GET_LINKS,
1113        .dumpit = wprobe_dump_links,
1114        .policy = wprobe_policy,
1115    },
1116    {
1117        .cmd = WPROBE_CMD_CONFIG,
1118        .doit = wprobe_set_config,
1119        .policy = wprobe_policy,
1120    },
1121    {
1122        .cmd = WPROBE_CMD_GET_FILTER,
1123        .dumpit = wprobe_dump_filters,
1124        .policy = wprobe_policy,
1125    },
1126};
1127
1128static void __exit
1129wprobe_exit(void)
1130{
1131    BUG_ON(!list_empty(&wprobe_if));
1132    genl_unregister_family(&wprobe_fam);
1133}
1134
1135
1136static int __init
1137wprobe_init(void)
1138{
1139    int i, err;
1140
1141    spin_lock_init(&wprobe_lock);
1142    INIT_LIST_HEAD(&wprobe_if);
1143
1144    err = genl_register_family(&wprobe_fam);
1145    if (err)
1146        return err;
1147
1148    for (i = 0; i < ARRAY_SIZE(wprobe_ops); i++) {
1149        err = genl_register_ops(&wprobe_fam, &wprobe_ops[i]);
1150        if (err)
1151            goto error;
1152    }
1153
1154    return 0;
1155
1156error:
1157    genl_unregister_family(&wprobe_fam);
1158    return err;
1159}
1160
1161module_init(wprobe_init);
1162module_exit(wprobe_exit);
1163MODULE_LICENSE("GPL");
1164
1165

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