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) \
  35 for (pos = rcu_dereference((head)->next); \
  36 prefetch(pos->next), pos != (head); \
  37 pos = 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
  44 static struct list_head wprobe_if;
  45 static spinlock_t wprobe_lock;
  46
  47 static 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 */
  57 struct wprobe_rtap_hdr {
  58         __u8 version;
  59         __u8 padding;
  60         __le16 len;
  61         __le32 present;
  62 };
  63
  64 static void wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l);
  65 static int wprobe_sync_data(struct wprobe_iface *dev, struct wprobe_link *l, bool query);
  66 static void wprobe_free_filter(struct wprobe_filter *f);
  67
  68 int
  69 wprobe_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 }
  86 EXPORT_SYMBOL(wprobe_add_link);
  87
  88 void
  89 wprobe_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 }
  99 EXPORT_SYMBOL(wprobe_remove_link);
 100
 101 static void
 102 wprobe_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
 114 int
 115 wprobe_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
 152 error:
 153         if (s->val)
 154                 kfree(s->val);
 155         return -ENOMEM;
 156 }
 157 EXPORT_SYMBOL(wprobe_add_iface);
 158
 159 void
 160 wprobe_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 }
 180 EXPORT_SYMBOL(wprobe_remove_iface);
 181
 182 static struct wprobe_iface *
 183 wprobe_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
 206 int
 207 wprobe_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);
 272 out:
 273         rcu_read_unlock();
 274         return 0;
 275 }
 276 EXPORT_SYMBOL(wprobe_add_frame);
 277
 278 static int
 279 wprobe_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);
 302 done:
 303         spin_unlock_irqrestore(&dev->lock, flags);
 304         return 0;
 305 }
 306 EXPORT_SYMBOL(wprobe_sync_data);
 307
 308 static void
 309 wprobe_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
 336 void
 337 wprobe_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 }
 403 EXPORT_SYMBOL(wprobe_update_stats);
 404
 405 static 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
 419 static bool
 420 wprobe_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
 431 static bool
 432 wprobe_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         }
 483 done:
 484         genlmsg_end(msg, hdr);
 485         return true;
 486
 487 nla_put_failure:
 488         genlmsg_cancel(msg, hdr);
 489         return false;
 490 }
 491
 492 static bool
 493 wprobe_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
 511 nla_put_failure:
 512         genlmsg_cancel(msg, hdr);
 513         return false;
 514 }
 515
 516
 517 static struct wprobe_link *
 518 wprobe_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
 529 static bool
 530 wprobe_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
 558 nla_put_failure:
 559         genlmsg_cancel(msg, hdr);
 560         return false;
 561 }
 562
 563 static int
 564 wprobe_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;
 602 abort:
 603         rcu_read_unlock();
 604         err = skb->len;
 605 done:
 606         return err;
 607 }
 608
 609 static bool
 610 wprobe_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
 623 nla_put_failure:
 624         genlmsg_cancel(msg, hdr);
 625         return false;
 626 }
 627
 628 static int
 629 wprobe_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;
 669 done:
 670         return err;
 671 }
 672
 673 #define WPROBE_F_LINK (1 << 31) /* for internal use */
 674 static int
 675 wprobe_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
 792 done:
 793         rcu_read_unlock();
 794         return err;
 795 }
 796 #undef WPROBE_F_LINK
 797
 798 static int
 799 wprobe_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
 817 static int
 818 wprobe_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
 837 done:
 838         rcu_read_unlock();
 839         return err;
 840 }
 841
 842 static int
 843 wprobe_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
 894 overrun:
 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
 899 static void
 900 wprobe_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
 914 static int
 915 wprobe_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
 992 error:
 993         wprobe_free_filter(f);
 994         return -ENOMEM;
 995 }
 996
 997 static int
 998 wprobe_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
1081 done:
1082         spin_unlock_irqrestore(&dev->lock, flags);
1083 done_unlocked:
1084         rcu_read_unlock();
1085         if (filter_free) {
1086                 synchronize_rcu();
1087                 wprobe_free_filter(filter_free);
1088         }
1089         return err;
1090 }
1091
1092 static 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
1125 static void __exit
1126 wprobe_exit(void)
1127 {
1128         BUG_ON(!list_empty(&wprobe_if));
1129         genl_unregister_family(&wprobe_fam);
1130 }
1131
1132
1133 static int __init
1134 wprobe_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
1153 error:
1154         genl_unregister_family(&wprobe_fam);
1155         return err;
1156 }
1157
1158 module_init(wprobe_init);
1159 module_exit(wprobe_exit);
1160 MODULE_LICENSE("GPL");
1161