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 static struct list_head wprobe_if;
  38 static spinlock_t wprobe_lock;
  39
  40 static struct genl_family wprobe_fam = {
  41         .id = GENL_ID_GENERATE,
  42         .name = "wprobe",
  43         .hdrsize = 0,
  44         .version = 1,
  45         /* only the first set of attributes is used for queries */
  46         .maxattr = WPROBE_ATTR_ID,
  47 };
  48
  49 static void wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l);
  50
  51 int
  52 wprobe_add_link(struct wprobe_iface *s, struct wprobe_link *l, const char *addr)
  53 {
  54         unsigned long flags;
  55
  56         INIT_LIST_HEAD(&l->list);
  57         l->val = kzalloc(sizeof(struct wprobe_value) * s->n_link_items, GFP_ATOMIC);
  58         if (!l->val)
  59                 return -ENOMEM;
  60
  61         l->iface = s;
  62         memcpy(&l->addr, addr, ETH_ALEN);
  63         spin_lock_irqsave(&wprobe_lock, flags);
  64         list_add_tail_rcu(&l->list, &s->links);
  65         spin_unlock_irqrestore(&wprobe_lock, flags);
  66
  67         return 0;
  68 }
  69 EXPORT_SYMBOL(wprobe_add_link);
  70
  71 void
  72 wprobe_remove_link(struct wprobe_iface *s, struct wprobe_link *l)
  73 {
  74         unsigned long flags;
  75
  76         spin_lock_irqsave(&wprobe_lock, flags);
  77         list_del_rcu(&l->list);
  78         spin_unlock_irqrestore(&wprobe_lock, flags);
  79         synchronize_rcu();
  80         kfree(l->val);
  81 }
  82 EXPORT_SYMBOL(wprobe_remove_link);
  83
  84 int
  85 wprobe_add_iface(struct wprobe_iface *s)
  86 {
  87         unsigned long flags;
  88         int vsize;
  89
  90         /* reset only wprobe private area */
  91         memset(&s->list, 0, sizeof(struct wprobe_iface) - offsetof(struct wprobe_iface, list));
  92
  93         BUG_ON(!s->name);
  94         INIT_LIST_HEAD(&s->list);
  95         INIT_LIST_HEAD(&s->links);
  96
  97         vsize = max(s->n_link_items, s->n_global_items);
  98         s->val = kzalloc(sizeof(struct wprobe_value) * vsize, GFP_ATOMIC);
  99         if (!s->val)
 100                 goto error;
 101
 102         s->query_val = kzalloc(sizeof(struct wprobe_value) * vsize, GFP_ATOMIC);
 103         if (!s->query_val)
 104                 goto error;
 105
 106         spin_lock_irqsave(&wprobe_lock, flags);
 107         list_add_rcu(&s->list, &wprobe_if);
 108         spin_unlock_irqrestore(&wprobe_lock, flags);
 109
 110         return 0;
 111
 112 error:
 113         if (s->val)
 114                 kfree(s->val);
 115         return -ENOMEM;
 116 }
 117 EXPORT_SYMBOL(wprobe_add_iface);
 118
 119 void
 120 wprobe_remove_iface(struct wprobe_iface *s)
 121 {
 122         unsigned long flags;
 123
 124         BUG_ON(!list_empty(&s->links));
 125
 126         spin_lock_irqsave(&wprobe_lock, flags);
 127         list_del_rcu(&s->list);
 128         spin_unlock_irqrestore(&wprobe_lock, flags);
 129
 130         /* wait for all queries to finish before freeing the
 131          * temporary value storage buffer */
 132         synchronize_rcu();
 133
 134         kfree(s->val);
 135         kfree(s->query_val);
 136 }
 137 EXPORT_SYMBOL(wprobe_remove_iface);
 138
 139 static struct wprobe_iface *
 140 wprobe_get_dev(struct nlattr *attr)
 141 {
 142         struct wprobe_iface *dev = NULL;
 143         struct wprobe_iface *p;
 144         const char *name;
 145         int i = 0;
 146
 147         if (!attr)
 148                 return NULL;
 149
 150         name = nla_data(attr);
 151         list_for_each_entry_rcu(p, &wprobe_if, list) {
 152                 i++;
 153                 if (strcmp(name, p->name) != 0)
 154                         continue;
 155
 156                 dev = p;
 157                 break;
 158         }
 159
 160         return dev;
 161 }
 162
 163 int
 164 wprobe_sync_data(struct wprobe_iface *dev, struct wprobe_link *l, bool query)
 165 {
 166         struct wprobe_value *val;
 167         unsigned long flags;
 168         int n, err;
 169
 170         if (l) {
 171                 n = dev->n_link_items;
 172                 val = l->val;
 173         } else {
 174                 n = dev->n_global_items;
 175                 val = dev->val;
 176         }
 177
 178         spin_lock_irqsave(&dev->lock, flags);
 179         err = dev->sync_data(dev, l, val, !query);
 180         if (err)
 181                 goto done;
 182
 183         if (query)
 184                 memcpy(dev->query_val, val, sizeof(struct wprobe_value) * n);
 185
 186         wprobe_update_stats(dev, l);
 187 done:
 188         spin_unlock_irqrestore(&dev->lock, flags);
 189         return 0;
 190 }
 191 EXPORT_SYMBOL(wprobe_sync_data);
 192
 193 void
 194 wprobe_update_stats(struct wprobe_iface *dev, struct wprobe_link *l)
 195 {
 196         const struct wprobe_item *item;
 197         struct wprobe_value *val;
 198         int i, n;
 199
 200         if (l) {
 201                 n = dev->n_link_items;
 202                 item = dev->link_items;
 203                 val = l->val;
 204         } else {
 205                 n = dev->n_global_items;
 206                 item = dev->global_items;
 207                 val = dev->val;
 208         }
 209
 210         /* process statistics */
 211         for (i = 0; i < n; i++) {
 212                 s64 v;
 213
 214                 if (!val[i].pending)
 215                         continue;
 216
 217                 val[i].n++;
 218
 219                 switch(item[i].type) {
 220                 case WPROBE_VAL_S8:
 221                         v = val[i].S8;
 222                         break;
 223                 case WPROBE_VAL_S16:
 224                         v = val[i].S16;
 225                         break;
 226                 case WPROBE_VAL_S32:
 227                         v = val[i].S32;
 228                         break;
 229                 case WPROBE_VAL_S64:
 230                         v = val[i].S64;
 231                         break;
 232                 case WPROBE_VAL_U8:
 233                         v = val[i].U8;
 234                         break;
 235                 case WPROBE_VAL_U16:
 236                         v = val[i].U16;
 237                         break;
 238                 case WPROBE_VAL_U32:
 239                         v = val[i].U32;
 240                         break;
 241                 case WPROBE_VAL_U64:
 242                         v = val[i].U64;
 243                         break;
 244                 default:
 245                         continue;
 246                 }
 247
 248                 val[i].s += v;
 249                 val[i].ss += v * v;
 250                 val[i].pending = false;
 251         }
 252 }
 253 EXPORT_SYMBOL(wprobe_update_stats);
 254
 255 static const struct nla_policy wprobe_policy[WPROBE_ATTR_ID+1] = {
 256         [WPROBE_ATTR_INTERFACE] = { .type = NLA_NUL_STRING },
 257         [WPROBE_ATTR_MAC] = { .type = NLA_STRING },
 258         [WPROBE_ATTR_DURATION] = { .type = NLA_MSECS },
 259         [WPROBE_ATTR_FLAGS] = { .type = NLA_U32 },
 260         [WPROBE_ATTR_SCALE] = { .type = NLA_U32 },
 261 };
 262
 263 static bool
 264 wprobe_check_ptr(struct list_head *list, struct list_head *ptr)
 265 {
 266         struct list_head *p;
 267
 268         list_for_each_rcu(p, list) {
 269                 if (ptr == p)
 270                         return true;
 271         }
 272         return false;
 273 }
 274
 275 static bool
 276 wprobe_send_item_value(struct sk_buff *msg, struct netlink_callback *cb,
 277                        struct wprobe_iface *dev, struct wprobe_link *l,
 278                        const struct wprobe_item *item,
 279                        int i, u32 flags)
 280 {
 281         struct genlmsghdr *hdr;
 282         struct wprobe_value *val = dev->query_val;
 283         u64 time = val[i].last - val[i].first;
 284
 285         hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
 286                         &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_INFO);
 287
 288         NLA_PUT_U32(msg, WPROBE_ATTR_ID, i);
 289         NLA_PUT_U32(msg, WPROBE_ATTR_FLAGS, flags);
 290         NLA_PUT_U8(msg, WPROBE_ATTR_TYPE, item[i].type);
 291         NLA_PUT_U64(msg, WPROBE_ATTR_DURATION, time);
 292
 293         switch(item[i].type) {
 294         case WPROBE_VAL_S8:
 295         case WPROBE_VAL_U8:
 296                 NLA_PUT_U8(msg, item[i].type, val[i].U8);
 297                 break;
 298         case WPROBE_VAL_S16:
 299         case WPROBE_VAL_U16:
 300                 NLA_PUT_U16(msg, item[i].type, val[i].U16);
 301                 break;
 302         case WPROBE_VAL_S32:
 303         case WPROBE_VAL_U32:
 304                 NLA_PUT_U32(msg, item[i].type, val[i].U32);
 305                 break;
 306         case WPROBE_VAL_S64:
 307         case WPROBE_VAL_U64:
 308                 NLA_PUT_U64(msg, item[i].type, val[i].U64);
 309                 break;
 310         case WPROBE_VAL_STRING:
 311                 if (val[i].STRING)
 312                         NLA_PUT_STRING(msg, item[i].type, val[i].STRING);
 313                 else
 314                         NLA_PUT_STRING(msg, item[i].type, "");
 315                 /* bypass avg/stdev */
 316                 goto done;
 317         default:
 318                 /* skip unknown values */
 319                 goto done;
 320         }
 321         if (item[i].flags & WPROBE_F_KEEPSTAT) {
 322                 NLA_PUT_U64(msg, WPROBE_VAL_SUM, val[i].s);
 323                 NLA_PUT_U64(msg, WPROBE_VAL_SUM_SQ, val[i].ss);
 324                 NLA_PUT_U32(msg, WPROBE_VAL_SAMPLES, (u32) val[i].n);
 325         }
 326 done:
 327         genlmsg_end(msg, hdr);
 328         return true;
 329
 330 nla_put_failure:
 331         genlmsg_cancel(msg, hdr);
 332         return false;
 333 }
 334
 335 static bool
 336 wprobe_send_item_info(struct sk_buff *msg, struct netlink_callback *cb,
 337                        struct wprobe_iface *dev,
 338                        const struct wprobe_item *item, int i)
 339 {
 340         struct genlmsghdr *hdr;
 341
 342         hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
 343                         &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_LIST);
 344
 345         if ((i == 0) && (dev->addr != NULL))
 346                 NLA_PUT(msg, WPROBE_ATTR_MAC, 6, dev->addr);
 347         NLA_PUT_U32(msg, WPROBE_ATTR_ID, (u32) i);
 348         NLA_PUT_STRING(msg, WPROBE_ATTR_NAME, item[i].name);
 349         NLA_PUT_U8(msg, WPROBE_ATTR_TYPE, item[i].type);
 350         NLA_PUT_U32(msg, WPROBE_ATTR_FLAGS, item[i].flags);
 351         genlmsg_end(msg, hdr);
 352         return true;
 353
 354 nla_put_failure:
 355         genlmsg_cancel(msg, hdr);
 356         return false;
 357 }
 358
 359
 360 static struct wprobe_link *
 361 wprobe_find_link(struct wprobe_iface *dev, const char *mac)
 362 {
 363         struct wprobe_link *l;
 364
 365         list_for_each_entry_rcu(l, &dev->links, list) {
 366                 if (!memcmp(l->addr, mac, 6))
 367                         return l;
 368         }
 369         return NULL;
 370 }
 371
 372 static bool
 373 wprobe_dump_link(struct sk_buff *msg, struct wprobe_link *l, struct netlink_callback *cb)
 374 {
 375         struct genlmsghdr *hdr;
 376
 377         hdr = genlmsg_put(msg, NETLINK_CB(cb->skb).pid, cb->nlh->nlmsg_seq,
 378                         &wprobe_fam, NLM_F_MULTI, WPROBE_CMD_GET_LINKS);
 379         if (!hdr)
 380                 return false;
 381
 382         NLA_PUT(msg, WPROBE_ATTR_MAC, 6, l->addr);
 383         genlmsg_end(msg, hdr);
 384         return true;
 385
 386 nla_put_failure:
 387         genlmsg_cancel(msg, hdr);
 388         return false;
 389 }
 390
 391 static int
 392 wprobe_dump_links(struct sk_buff *skb, struct netlink_callback *cb)
 393 {
 394         struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0];
 395         struct wprobe_link *l;
 396         int err = 0;
 397         int i = 0;
 398
 399         if (!dev) {
 400                 err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize,
 401                                 wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy);
 402                 if (err)
 403                         goto done;
 404
 405                 dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]);
 406                 if (!dev) {
 407                         err = -ENODEV;
 408                         goto done;
 409                 }
 410
 411                 cb->args[0] = (long) dev;
 412         } else {
 413                 if (!wprobe_check_ptr(&wprobe_if, &dev->list)) {
 414                         err = -ENODEV;
 415                         goto done;
 416                 }
 417         }
 418
 419         rcu_read_lock();
 420         list_for_each_entry_rcu(l, &dev->links, list) {
 421                 if (i < cb->args[1])
 422                         continue;
 423
 424                 if (unlikely(!wprobe_dump_link(skb, l, cb)))
 425                         break;
 426
 427                 i++;
 428         }
 429         cb->args[1] = i;
 430         rcu_read_unlock();
 431         err = skb->len;
 432 done:
 433         return err;
 434 }
 435 static void
 436 wprobe_scale_stats(const struct wprobe_item *item, struct wprobe_value *val, int n, u32 flags)
 437 {
 438         u32 scale = 0;
 439         int i;
 440
 441         for (i = 0; i < n; i++) {
 442                 if (!(item[i].flags & WPROBE_F_KEEPSTAT))
 443                         continue;
 444
 445                 /* reset statistics, if requested */
 446                 if (flags & WPROBE_F_RESET)
 447                         scale = val[i].n;
 448                 else if (wprobe_fam.attrbuf[WPROBE_ATTR_SCALE])
 449                         scale = nla_get_u32(wprobe_fam.attrbuf[WPROBE_ATTR_SCALE]);
 450
 451                 if ((scale > 0) && (val[i].n >= scale)) {
 452                         val[i].s = div_s64(val[i].s, scale);
 453                         val[i].ss = div_s64(val[i].ss, scale);
 454                         val[i].n /= scale;
 455                 }
 456         }
 457 }
 458
 459 #define WPROBE_F_LINK (1 << 31) /* for internal use */
 460 static int
 461 wprobe_dump_info(struct sk_buff *skb, struct netlink_callback *cb)
 462 {
 463         struct wprobe_iface *dev = (struct wprobe_iface *)cb->args[0];
 464         struct wprobe_link *l = (struct wprobe_link *)cb->args[1];
 465         struct wprobe_value *val;
 466         const struct wprobe_item *item;
 467         struct genlmsghdr *hdr;
 468         unsigned long flags;
 469         int cmd, n, i = cb->args[3];
 470         u32     vflags = cb->args[2];
 471         int err = 0;
 472
 473         hdr = (struct genlmsghdr *)nlmsg_data(cb->nlh);
 474         cmd = hdr->cmd;
 475
 476         /* since the attribute value list might be too big for a single netlink
 477          * message, the device, link and offset get stored in the netlink callback.
 478          * if this is the first request, we need to do the full lookup for the device.
 479          *
 480          * access to the device and link structure is synchronized through rcu.
 481          */
 482         rcu_read_lock();
 483         if (!dev) {
 484                 err = nlmsg_parse(cb->nlh, GENL_HDRLEN + wprobe_fam.hdrsize,
 485                                 wprobe_fam.attrbuf, wprobe_fam.maxattr, wprobe_policy);
 486                 if (err)
 487                         goto done;
 488
 489                 err = -ENOENT;
 490                 dev = wprobe_get_dev(wprobe_fam.attrbuf[WPROBE_ATTR_INTERFACE]);
 491                 if (!dev)
 492                         goto done;
 493
 494                 if (cmd == WPROBE_CMD_GET_INFO) {
 495                         if (wprobe_fam.attrbuf[WPROBE_ATTR_MAC]) {
 496                                 l = wprobe_find_link(dev, nla_data(wprobe_fam.attrbuf[WPROBE_ATTR_MAC]));
 497                                 if (!l)
 498                                         goto done;
 499
 500                                 vflags = l->flags;
 501                         }
 502
 503                         if (l) {
 504                                 item = dev->link_items;
 505                                 n = dev->n_link_items;
 506                                 val = l->val;
 507                         } else {
 508                                 item = dev->global_items;
 509                                 n = dev->n_global_items;
 510                                 val = dev->val;
 511                         }
 512
 513                         /* sync data and move to temp storage for the query */
 514                         spin_lock_irqsave(&dev->lock, flags);
 515                         err = wprobe_sync_data(dev, l, true);
 516                         if (!err)
 517                                 memcpy(dev->query_val, val, n * sizeof(struct wprobe_value));
 518                         wprobe_scale_stats(item, val, n, flags);
 519                         spin_unlock_irqrestore(&dev->lock, flags);
 520
 521                         if (err)
 522                                 goto done;
 523                 }
 524
 525                 if (wprobe_fam.attrbuf[WPROBE_ATTR_FLAGS])
 526                         vflags |= nla_get_u32(wprobe_fam.attrbuf[WPROBE_ATTR_FLAGS]);
 527
 528                 if (wprobe_fam.attrbuf[WPROBE_ATTR_MAC])
 529                         vflags |= WPROBE_F_LINK;
 530
 531                 cb->args[0] = (long) dev;
 532                 cb->args[1] = (long) l;
 533                 cb->args[2] = vflags;
 534                 cb->args[3] = 0;
 535         } else {
 536                 /* when pulling pointers from the callback, validate them
 537                  * against the list using rcu to make sure that we won't
 538                  * dereference pointers to free'd memory after the last
 539                  * grace period */
 540                 err = -ENOENT;
 541                 if (!wprobe_check_ptr(&wprobe_if, &dev->list))
 542                         goto done;
 543
 544                 if (l && !wprobe_check_ptr(&dev->links, &l->list))
 545                         goto done;
 546         }
 547
 548         if (vflags & WPROBE_F_LINK) {
 549                 item = dev->link_items;
 550                 n = dev->n_link_items;
 551         } else {
 552                 item = dev->global_items;
 553                 n = dev->n_global_items;
 554         }
 555
 556         err = 0;
 557         switch(cmd) {
 558         case WPROBE_CMD_GET_INFO:
 559                 while (i < n) {
 560                         if (!wprobe_send_item_value(skb, cb, dev, l, item, i, vflags))
 561                                 break;
 562                         i++;
 563                 }
 564                 break;
 565         case WPROBE_CMD_GET_LIST:
 566                 while (i < n) {
 567                         if (!wprobe_send_item_info(skb, cb, dev, item, i))
 568                                 break;
 569                         i++;
 570                 }
 571                 break;
 572         default:
 573                 err = -EINVAL;
 574                 goto done;
 575         }
 576         cb->args[3] = i;
 577         err = skb->len;
 578
 579 done:
 580         rcu_read_unlock();
 581         return err;
 582 }
 583 #undef WPROBE_F_LINK
 584
 585 static int
 586 wprobe_measure(struct sk_buff *skb, struct genl_info *info)
 587 {
 588         struct wprobe_iface *dev;
 589         struct wprobe_link *l = NULL;
 590         int err = -ENOENT;
 591
 592         rcu_read_lock();
 593         dev = wprobe_get_dev(info->attrs[WPROBE_ATTR_INTERFACE]);
 594         if (!dev)
 595                 goto done;
 596
 597         if (info->attrs[WPROBE_ATTR_MAC]) {
 598                 l = wprobe_find_link(dev, nla_data(wprobe_fam.attrbuf[WPROBE_ATTR_MAC]));
 599                 if (!l)
 600                         goto done;
 601         }
 602
 603         err = wprobe_sync_data(dev, l, false);
 604
 605 done:
 606         rcu_read_unlock();
 607         return err;
 608 }
 609
 610 static struct genl_ops wprobe_ops[] = {
 611         {
 612                 .cmd = WPROBE_CMD_GET_INFO,
 613                 .dumpit = wprobe_dump_info,
 614                 .policy = wprobe_policy,
 615         },
 616         {
 617                 .cmd = WPROBE_CMD_GET_LIST,
 618                 .dumpit = wprobe_dump_info,
 619                 .policy = wprobe_policy,
 620         },
 621         {
 622                 .cmd = WPROBE_CMD_MEASURE,
 623                 .doit = wprobe_measure,
 624                 .policy = wprobe_policy,
 625         },
 626         {
 627                 .cmd = WPROBE_CMD_GET_LINKS,
 628                 .dumpit = wprobe_dump_links,
 629                 .policy = wprobe_policy,
 630         }
 631 };
 632
 633 static void __exit
 634 wprobe_exit(void)
 635 {
 636         BUG_ON(!list_empty(&wprobe_if));
 637         genl_unregister_family(&wprobe_fam);
 638 }
 639
 640
 641 static int __init
 642 wprobe_init(void)
 643 {
 644         int i, err;
 645
 646         spin_lock_init(&wprobe_lock);
 647         INIT_LIST_HEAD(&wprobe_if);
 648
 649         err = genl_register_family(&wprobe_fam);
 650         if (err)
 651                 return err;
 652
 653         for (i = 0; i < ARRAY_SIZE(wprobe_ops); i++) {
 654                 err = genl_register_ops(&wprobe_fam, &wprobe_ops[i]);
 655                 if (err)
 656                         goto error;
 657         }
 658
 659         return 0;
 660
 661 error:
 662         genl_unregister_family(&wprobe_fam);
 663         return err;
 664 }
 665
 666 module_init(wprobe_init);
 667 module_exit(wprobe_exit);
 668 MODULE_LICENSE("GPL");
 669