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