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