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