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) \
  38 for (pos = rcu_dereference((head)->next); \
  39 prefetch(pos->next), pos != (head); \
  40 pos = rcu_dereference(pos->next))
  41 #endif
  42
  43 #define WPROBE_MIN_INTERVAL             100 /* minimum measurement interval in msecs */
  44 #define WPROBE_MAX_FILTER_SIZE  1024
  45 #define WPROBE_MAX_FRAME_SIZE   1900
  46
  47 static struct list_head wprobe_if;
  48 static spinlock_t wprobe_lock;
  49
  50 static struct genl_family wprobe_fam = {
  51         .id = GENL_ID_GENERATE,
  52         .name = "wprobe",
  53         .hdrsize = 0,
  54         .version = 1,
  55         /* only the first set of attributes is used for queries */
  56         .maxattr = WPROBE_ATTR_LAST,
  57 };
  58
  59 /* fake radiotap header */
  60 struct wprobe_rtap_hdr {
  61         __u8 version;
  62         __u8 padding;
  63         __le16 len;
  64         __le32 present;
  65 };
  66
  67 static void wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l);
  68 static int wprobe_sync_data(struct wprobe_iface *dev, struct wprobe_link *l, bool query);
  69 static void wprobe_free_filter(struct wprobe_filter *f);
  70
  71 int
  72 wprobe_add_link(struct wprobe_iface *s, struct wprobe_link *l, const char *addr)
  73 {
  74         unsigned long flags;
  75
  76         INIT_LIST_HEAD(&l->list);
  77         l->val = kzalloc(sizeof(struct wprobe_value) * s->n_link_items, GFP_ATOMIC);
  78         if (!l->val)
  79                 return -ENOMEM;
  80
  81         l->iface = s;
  82         memcpy(&l->addr, addr, ETH_ALEN);
  83         spin_lock_irqsave(&wprobe_lock, flags);
  84         list_add_tail_rcu(&l->list, &s->links);
  85         spin_unlock_irqrestore(&wprobe_lock, flags);
  86
  87         return 0;
  88 }
  89 EXPORT_SYMBOL(wprobe_add_link);
  90
  91 void
  92 wprobe_remove_link(struct wprobe_iface *s, struct wprobe_link *l)
  93 {
  94         unsigned long flags;
  95
  96         spin_lock_irqsave(&wprobe_lock, flags);
  97         list_del_rcu(&l->list);
  98         spin_unlock_irqrestore(&wprobe_lock, flags);
  99         synchronize_rcu();
 100         kfree(l->val);
 101 }
 102 EXPORT_SYMBOL(wprobe_remove_link);
 103
 104 static void
 105 wprobe_measure_timer(unsigned long data)
 106 {
 107         struct wprobe_iface *dev = (struct wprobe_iface *) data;
 108
 109         /* set next measurement interval */
 110         mod_timer(&dev->measure_timer, jiffies +
 111                 msecs_to_jiffies(dev->measure_interval));
 112
 113         /* perform measurement */
 114         wprobe_sync_data(dev, NULL, false);
 115 }
 116
 117 int
 118 wprobe_add_iface(struct wprobe_iface *s)
 119 {
 120         unsigned long flags;
 121         int vsize;
 122
 123         /* reset only wprobe private area */
 124         memset(&s->list, 0, sizeof(struct wprobe_iface) - offsetof(struct wprobe_iface, list));
 125
 126         BUG_ON(!s->name);
 127         INIT_LIST_HEAD(&s->list);
 128         INIT_LIST_HEAD(&s->links);
 129         setup_timer(&s->measure_timer, wprobe_measure_timer, (unsigned long) s);
 130
 131         s->val = kzalloc(sizeof(struct wprobe_value) * s->n_global_items, GFP_ATOMIC);
 132         if (!s->val)
 133                 goto error;
 134
 135         vsize = max(s->n_link_items, s->n_global_items);
 136         s->query_val = kzalloc(sizeof(struct wprobe_value) * vsize, GFP_ATOMIC);
 137         if (!s->query_val)
 138                 goto error;
 139
 140         /* initialize defaults to be able to handle overflow,
 141          * user space will need to handle this if it keeps an
 142          * internal histogram */
 143         s->scale_min = 20;
 144         s->scale_max = (1 << 31);
 145
 146         s->scale_m = 1;
 147         s->scale_d = 10;
 148
 149         spin_lock_irqsave(&wprobe_lock, flags);
 150         list_add_rcu(&s->list, &wprobe_if);
 151         spin_unlock_irqrestore(&wprobe_lock, flags);
 152
 153         return 0;
 154
 155 error:
 156         if (s->val)
 157                 kfree(s->val);
 158         return -ENOMEM;
 159 }
 160 EXPORT_SYMBOL(wprobe_add_iface);
 161
 162 void
 163 wprobe_remove_iface(struct wprobe_iface *s)
 164 {
 165         unsigned long flags;
 166
 167         BUG_ON(!list_empty(&s->links));
 168
 169         del_timer_sync(&s->measure_timer);
 170         spin_lock_irqsave(&wprobe_lock, flags);
 171         list_del_rcu(&s->list);
 172         spin_unlock_irqrestore(&wprobe_lock, flags);
 173
 174         /* wait for all queries to finish before freeing the
 175          * temporary value storage buffer */
 176         synchronize_rcu();
 177
 178         kfree(s->val);
 179         kfree(s->query_val);
 180         if (s->active_filter)
 181                 wprobe_free_filter(s->active_filter);
 182 }
 183 EXPORT_SYMBOL(wprobe_remove_iface);
 184
 185 static struct wprobe_iface *
 186 wprobe_get_dev(struct nlattr *attr)
 187 {
 188         struct wprobe_iface *dev = NULL;
 189         struct wprobe_iface *p;
 190         const char *name;
 191         int i = 0;
 192
 193         if (!attr)
 194                 return NULL;
 195
 196         name = nla_data(attr);
 197         list_for_each_entry_rcu(p, &wprobe_if, list) {
 198                 i++;
 199                 if (strcmp(name, p->name) != 0)
 200                         continue;
 201
 202                 dev = p;
 203                 break;
 204         }
 205
 206         return dev;
 207 }
 208
 209 int
 210 wprobe_add_frame(struct wprobe_iface *dev, const struct wprobe_wlan_hdr *hdr, void *data, int len)
 211 {
 212         struct wprobe_wlan_hdr *new_hdr;
 213         struct wprobe_filter *f;
 214         struct sk_buff *skb;
 215         unsigned long flags;
 216         int i, j;
 217
 218         rcu_read_lock();
 219         f = rcu_dereference(dev->active_filter);
 220         if (!f)
 221                 goto out;
 222
 223         spin_lock_irqsave(&f->lock, flags);
 224
 225         skb = f->skb;
 226         skb->len = sizeof(struct wprobe_rtap_hdr);
 227         skb->tail = skb->data + skb->len;
 228         if (len + skb->len > WPROBE_MAX_FRAME_SIZE)
 229                 len = WPROBE_MAX_FRAME_SIZE - skb->len;
 230
 231         new_hdr = (struct wprobe_wlan_hdr *) skb_put(skb, f->hdrlen);
 232         memcpy(new_hdr, hdr, sizeof(struct wprobe_wlan_hdr));
 233         new_hdr->len = cpu_to_be16(new_hdr->len);
 234
 235         memcpy(skb_put(skb, len), data, len);
 236
 237         for(i = 0; i < f->n_groups; i++) {
 238                 struct wprobe_filter_group *fg = &f->groups[i];
 239                 bool found = false;
 240                 int def = -1;
 241
 242                 for (j = 0; j < fg->n_items; j++) {
 243                         struct wprobe_filter_item *fi = fg->items[j];
 244
 245                         if (!fi->hdr.n_items) {
 246                                 def = j;
 247                                 continue;
 248                         }
 249 #if LINUX_VERSION_CODE >= KERNEL_VERSION(2,6,38)
 250                         if (sk_run_filter(skb, fi->filter) == 0)
 251                                 continue;
 252 #else
 253                         if (sk_run_filter(skb, fi->filter, fi->hdr.n_items) == 0)
 254                                 continue;
 255 #endif
 256
 257                         found = true;
 258                         break;
 259                 }
 260                 if (!found && def >= 0) {
 261                         j = def;
 262                         found = true;
 263                 }
 264                 if (found) {
 265                         struct wprobe_filter_counter *c = &fg->counters[j];
 266
 267                         if (hdr->type >= WPROBE_PKT_TX)
 268                                 c->tx++;
 269                         else
 270                                 c->rx++;
 271                 }
 272         }
 273
 274         spin_unlock_irqrestore(&f->lock, flags);
 275 out:
 276         rcu_read_unlock();
 277         return 0;
 278 }
 279 EXPORT_SYMBOL(wprobe_add_frame);
 280
 281 static int
 282 wprobe_sync_data(struct wprobe_iface *dev, struct wprobe_link *l, bool query)
 283 {
 284         struct wprobe_value *val;
 285         unsigned long flags;
 286         int n, err;
 287
 288         if (l) {
 289                 n = dev->n_link_items;
 290                 val = l->val;
 291         } else {
 292                 n = dev->n_global_items;
 293                 val = dev->val;
 294         }
 295
 296         spin_lock_irqsave(&dev->lock, flags);
 297         err = dev->sync_data(dev, l, val, !query);
 298         if (err)
 299                 goto done;
 300
 301         if (query)
 302                 memcpy(dev->query_val, val, sizeof(struct wprobe_value) * n);
 303
 304         wprobe_update_stats(dev, l);
 305 done:
 306         spin_unlock_irqrestore(&dev->lock, flags);
 307         return 0;
 308 }
 309 EXPORT_SYMBOL(wprobe_sync_data);
 310
 311 static void
 312 wprobe_scale_stats(struct wprobe_iface *dev, const struct wprobe_item *item,
 313                    struct wprobe_value *val, int n)
 314 {
 315         u64 scale_ts = jiffies_64;
 316         int i;
 317
 318         for (i = 0; i < n; i++) {
 319                 if (!(item[i].flags & WPROBE_F_KEEPSTAT))
 320                         continue;
 321
 322                 if (val[i].n <= dev->scale_min)
 323                         continue;
 324
 325                 /* FIXME: div_s64 seems to be very imprecise here, even when
 326                  * the values are scaled up */
 327                 val[i].s *= dev->scale_m;
 328                 val[i].s = div_s64(val[i].s, dev->scale_d);
 329
 330                 val[i].ss *= dev->scale_m;
 331                 val[i].ss = div_s64(val[i].ss, dev->scale_d);
 332
 333                 val[i].n = (val[i].n * dev->scale_m) / dev->scale_d;
 334                 val[i].scale_timestamp = scale_ts;
 335         }
 336 }
 337
 338
 339 void
 340 wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l)
 341 {
 342         const struct wprobe_item *item;
 343         struct wprobe_value *val;
 344         bool scale_stats = false;
 345         int i, n;
 346
 347         if (l) {
 348                 n = dev->n_link_items;
 349                 item = dev->link_items;
 350                 val = l->val;
 351         } else {
 352                 n = dev->n_global_items;
 353                 item = dev->global_items;
 354                 val = dev->val;
 355         }
 356
 357         /* process statistics */
 358         for (i = 0; i < n; i++) {
 359                 s64 v;
 360
 361                 if (!val[i].pending)
 362                         continue;
 363
 364                 val[i].n++;
 365                 if ((item[i].flags & WPROBE_F_KEEPSTAT) &&
 366                         (dev->scale_max > 0) && (val[i].n > dev->scale_max)) {
 367                         scale_stats = true;
 368                 }
 369
 370                 switch(item[i].type) {
 371                 case WPROBE_VAL_S8:
 372                         v = val[i].S8;
 373                         break;
 374                 case WPROBE_VAL_S16:
 375                         v = val[i].S16;
 376                         break;
 377                 case WPROBE_VAL_S32:
 378                         v = val[i].S32;
 379                         break;
 380                 case WPROBE_VAL_S64:
 381                         v = val[i].S64;
 382                         break;
 383                 case WPROBE_VAL_U8:
 384                         v = val[i].U8;
 385                         break;
 386                 case WPROBE_VAL_U16:
 387                         v = val[i].U16;
 388                         break;
 389                 case WPROBE_VAL_U32:
 390                         v = val[i].U32;
 391                         break;
 392                 case WPROBE_VAL_U64:
 393                         v = val[i].U64;
 394                         break;
 395                 default:
 396                         continue;
 397                 }
 398
 399                 val[i].s += v;
 400                 val[i].ss += v * v;
 401                 val[i].pending = false;
 402         }
 403         if (scale_stats)
 404                 wprobe_scale_stats(dev, item, val, n);
 405 }
 406 EXPORT_SYMBOL(wprobe_update_stats);
 407
 408 static const struct nla_policy wprobe_policy[WPROBE_ATTR_LAST+1] = {
 409         [WPROBE_ATTR_INTERFACE] = { .type = NLA_NUL_STRING },
 410         [WPROBE_ATTR_MAC] = { .type = NLA_STRING },
 411         [WPROBE_ATTR_FLAGS] = { .type = NLA_U32 },
 412
 413         /* config */
 414         [WPROBE_ATTR_INTERVAL] = { .type = NLA_MSECS },
 415         [WPROBE_ATTR_SAMPLES_MIN] = { .type = NLA_U32 },
 416         [WPROBE_ATTR_SAMPLES_MAX] = { .type = NLA_U32 },
 417         [WPROBE_ATTR_SAMPLES_SCALE_M] = { .type = NLA_U32 },
 418         [WPROBE_ATTR_SAMPLES_SCALE_D] = { .type = NLA_U32 },
 419         [WPROBE_ATTR_FILTER] = { .type = NLA_BINARY, .len = 32768 },
 420 };
 421
 422 static bool
 423 wprobe_check_ptr(struct list_head *list, struct list_head *ptr)
 424 {
 425         struct list_head *p;
 426
 427         list_for_each_rcu(p, list) {
 428                 if (ptr == p)
 429                         return true;
 430         }
 431         return false;
 432 }
 433
 434 static bool
 435 wprobe_send_item_value(struct sk_buff *msg, struct netlink_callback *cb,
 436                        struct wprobe_iface *dev, struct wprobe_link *l,
 437                        const struct wprobe_item *item,
 438                        int i, u32 flags)
 439 {
 440         struct genlmsghdr *hdr;
 441         struct wprobe_value *val = dev->query_val;
 442         u64 time = val[i].last - val[i].first;
 443
 444         hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
 445                         &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_INFO);
 446
 447         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         }
 486 done:
 487         genlmsg_end(msg, hdr);
 488         return true;
 489
 490 nla_put_failure:
 491         genlmsg_cancel(msg, hdr);
 492         return false;
 493 }
 494
 495 static bool
 496 wprobe_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
 514 nla_put_failure:
 515         genlmsg_cancel(msg, hdr);
 516         return false;
 517 }
 518
 519
 520 static struct wprobe_link *
 521 wprobe_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
 532 static bool
 533 wprobe_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
 561 nla_put_failure:
 562         genlmsg_cancel(msg, hdr);
 563         return false;
 564 }
 565
 566 static int
 567 wprobe_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;
 605 abort:
 606         rcu_read_unlock();
 607         err = skb->len;
 608 done:
 609         return err;
 610 }
 611
 612 static bool
 613 wprobe_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
 626 nla_put_failure:
 627         genlmsg_cancel(msg, hdr);
 628         return false;
 629 }
 630
 631 static int
 632 wprobe_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;
 672 done:
 673         return err;
 674 }
 675
 676 #define WPROBE_F_LINK (1 << 31) /* for internal use */
 677 static int
 678 wprobe_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
 795 done:
 796         rcu_read_unlock();
 797         return err;
 798 }
 799 #undef WPROBE_F_LINK
 800
 801 static int
 802 wprobe_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
 820 static int
 821 wprobe_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
 840 done:
 841         rcu_read_unlock();
 842         return err;
 843 }
 844
 845 static int
 846 wprobe_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
 897 overrun:
 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
 902 static void
 903 wprobe_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
 917 static int
 918 wprobe_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
 995 error:
 996         wprobe_free_filter(f);
 997         return -ENOMEM;
 998 }
 999
1000 static int
1001 wprobe_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
1084 done:
1085         spin_unlock_irqrestore(&dev->lock, flags);
1086 done_unlocked:
1087         rcu_read_unlock();
1088         if (filter_free) {
1089                 synchronize_rcu();
1090                 wprobe_free_filter(filter_free);
1091         }
1092         return err;
1093 }
1094
1095 static 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
1128 static void __exit
1129 wprobe_exit(void)
1130 {
1131         BUG_ON(!list_empty(&wprobe_if));
1132         genl_unregister_family(&wprobe_fam);
1133 }
1134
1135
1136 static int __init
1137 wprobe_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
1156 error:
1157         genl_unregister_family(&wprobe_fam);
1158         return err;
1159 }
1160
1161 module_init(wprobe_init);
1162 module_exit(wprobe_exit);
1163 MODULE_LICENSE("GPL");
1164