package/wprobe/src/kernel/wprobe-core.c

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