package/wprobe/src/user/wprobe.c

   1 /*
   2  * wprobe.c: Wireless probe user space library
   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 <stdio.h>
  17 #include <string.h>
  18 #include <stdlib.h>
  19 #include <errno.h>
  20 #include <getopt.h>
  21 #include <sys/types.h>
  22 #include <sys/socket.h>
  23 #include <math.h>
  24 #include <linux/wprobe.h>
  25 #include <netlink/netlink.h>
  26 #include <netlink/genl/genl.h>
  27 #include <netlink/genl/ctrl.h>
  28 #include <netlink/genl/family.h>
  29 #include "wprobe.h"
  30
  31 #define DEBUG 1
  32 #ifdef DEBUG
  33 #define DPRINTF(fmt, ...) fprintf(stderr, "%s(%d): " fmt, __func__, __LINE__, ##__VA_ARGS__)
  34 #else
  35 #define DPRINTF(fmt, ...) do {} while (0)
  36 #endif
  37
  38 static int n_devs = 0;
  39 static struct nl_sock *handle = NULL;
  40 static struct nl_cache *cache = NULL;
  41 static struct genl_family *family = NULL;
  42 static struct nlattr *tb[WPROBE_ATTR_LAST+1];
  43 static struct nla_policy attribute_policy[WPROBE_ATTR_LAST+1] = {
  44         [WPROBE_ATTR_ID] = { .type = NLA_U32 },
  45         [WPROBE_ATTR_MAC] = { .type = NLA_UNSPEC, .minlen = 6, .maxlen = 6 },
  46         [WPROBE_ATTR_NAME] = { .type = NLA_STRING },
  47         [WPROBE_ATTR_FLAGS] = { .type = NLA_U32 },
  48         [WPROBE_ATTR_TYPE] = { .type = NLA_U8 },
  49         [WPROBE_VAL_S8] = { .type = NLA_U8 },
  50         [WPROBE_VAL_S16] = { .type = NLA_U16 },
  51         [WPROBE_VAL_S32] = { .type = NLA_U32 },
  52         [WPROBE_VAL_S64] = { .type = NLA_U64 },
  53         [WPROBE_VAL_U8] = { .type = NLA_U8 },
  54         [WPROBE_VAL_U16] = { .type = NLA_U16 },
  55         [WPROBE_VAL_U32] = { .type = NLA_U32 },
  56         [WPROBE_VAL_U64] = { .type = NLA_U64 },
  57         [WPROBE_VAL_SUM] = { .type = NLA_U64 },
  58         [WPROBE_VAL_SUM_SQ] = { .type = NLA_U64 },
  59         [WPROBE_VAL_SAMPLES] = { .type = NLA_U32 },
  60 };
  61
  62 static int
  63 error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg)
  64 {
  65         int *ret = arg;
  66         *ret = err->error;
  67         return NL_STOP;
  68 }
  69
  70 static int
  71 finish_handler(struct nl_msg *msg, void *arg)
  72 {
  73         int *ret = arg;
  74         *ret = 0;
  75         return NL_SKIP;
  76 }
  77
  78 static int
  79 ack_handler(struct nl_msg *msg, void *arg)
  80 {
  81         int *ret = arg;
  82         *ret = 0;
  83         return NL_STOP;
  84 }
  85
  86
  87 static void
  88 wprobe_free(void)
  89 {
  90         /* should not happen */
  91         if (n_devs == 0)
  92                 return;
  93
  94         if (--n_devs != 0)
  95                 return;
  96
  97         if (cache)
  98                 nl_cache_free(cache);
  99         if (handle)
 100                 nl_socket_free(handle);
 101         handle = NULL;
 102         cache = NULL;
 103 }
 104
 105 static int
 106 wprobe_init(void)
 107 {
 108         int ret;
 109
 110         if (n_devs++ > 0)
 111                 return 0;
 112
 113         handle = nl_socket_alloc();
 114         if (!handle) {
 115                 DPRINTF("Failed to create handle\n");
 116                 goto err;
 117         }
 118
 119         if (genl_connect(handle)) {
 120                 DPRINTF("Failed to connect to generic netlink\n");
 121                 goto err;
 122         }
 123
 124         ret = genl_ctrl_alloc_cache(handle, &cache);
 125         if (ret < 0) {
 126                 DPRINTF("Failed to allocate netlink cache\n");
 127                 goto err;
 128         }
 129
 130         family = genl_ctrl_search_by_name(cache, "wprobe");
 131         if (!family) {
 132                 DPRINTF("wprobe API not present\n");
 133                 goto err;
 134         }
 135         return 0;
 136
 137 err:
 138         wprobe_free();
 139         return -EINVAL;
 140 }
 141
 142
 143 static struct nl_msg *
 144 wprobe_new_msg(const char *ifname, int cmd, bool dump)
 145 {
 146         struct nl_msg *msg;
 147         uint32_t flags = 0;
 148
 149         msg = nlmsg_alloc();
 150         if (!msg)
 151                 return NULL;
 152
 153         if (dump)
 154                 flags |= NLM_F_DUMP;
 155
 156         genlmsg_put(msg, 0, 0, genl_family_get_id(family),
 157                         0, flags, cmd, 0);
 158
 159         NLA_PUT_STRING(msg, WPROBE_ATTR_INTERFACE, ifname);
 160 nla_put_failure:
 161         return msg;
 162 }
 163
 164 static int
 165 wprobe_send_msg(struct nl_msg *msg, void *callback, void *arg)
 166 {
 167         struct nl_cb *cb;
 168         int err = 0;
 169
 170         cb = nl_cb_alloc(NL_CB_DEFAULT);
 171         if (!cb)
 172                 goto out_no_cb;
 173
 174         if (callback)
 175                 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, callback, arg);
 176
 177         err = nl_send_auto_complete(handle, msg);
 178         if (err < 0)
 179                 goto out;
 180
 181         err = 1;
 182
 183         nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
 184         nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
 185         nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
 186
 187         while (err > 0)
 188                 nl_recvmsgs(handle, cb);
 189
 190 out:
 191         nl_cb_put(cb);
 192 out_no_cb:
 193         nlmsg_free(msg);
 194         return err;
 195 }
 196
 197 struct wprobe_attr_cb {
 198         struct list_head *list;
 199         char *addr;
 200 };
 201
 202 static int
 203 save_attribute_handler(struct nl_msg *msg, void *arg)
 204 {
 205         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 206         const char *name = "N/A";
 207         struct wprobe_attribute *attr;
 208         int type = 0;
 209         struct wprobe_attr_cb *cb = arg;
 210
 211         nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
 212                         genlmsg_attrlen(gnlh, 0), attribute_policy);
 213
 214         if (tb[WPROBE_ATTR_NAME])
 215                 name = nla_data(tb[WPROBE_ATTR_NAME]);
 216
 217         attr = malloc(sizeof(struct wprobe_attribute) + strlen(name) + 1);
 218         if (!attr)
 219                 return -1;
 220
 221         memset(attr, 0, sizeof(struct wprobe_attribute));
 222
 223         if (tb[WPROBE_ATTR_ID])
 224                 attr->id = nla_get_u32(tb[WPROBE_ATTR_ID]);
 225
 226         if (tb[WPROBE_ATTR_MAC] && cb->addr)
 227                 memcpy(cb->addr, nla_data(tb[WPROBE_ATTR_MAC]), 6);
 228
 229         if (tb[WPROBE_ATTR_FLAGS])
 230                 attr->flags = nla_get_u32(tb[WPROBE_ATTR_FLAGS]);
 231
 232         if (tb[WPROBE_ATTR_TYPE])
 233                 type = nla_get_u8(tb[WPROBE_ATTR_TYPE]);
 234
 235         if ((type < WPROBE_VAL_STRING) ||
 236                 (type > WPROBE_VAL_U64))
 237                 type = 0;
 238
 239         attr->type = type;
 240         strcpy(attr->name, name);
 241         INIT_LIST_HEAD(&attr->list);
 242         list_add(&attr->list, cb->list);
 243         return 0;
 244 }
 245
 246
 247 static int
 248 dump_attributes(const char *ifname, bool link, struct list_head *list, char *addr)
 249 {
 250         struct nl_msg *msg;
 251         struct wprobe_attr_cb cb;
 252
 253         cb.list = list;
 254         cb.addr = addr;
 255         msg = wprobe_new_msg(ifname, WPROBE_CMD_GET_LIST, true);
 256         if (!msg)
 257                 return -ENOMEM;
 258
 259         if (link)
 260                 NLA_PUT(msg, WPROBE_ATTR_MAC, 6, "\x00\x00\x00\x00\x00\x00");
 261
 262         return wprobe_send_msg(msg, save_attribute_handler, &cb);
 263
 264 nla_put_failure:
 265         nlmsg_free(msg);
 266         return -EINVAL;
 267 }
 268
 269 struct wprobe_iface *
 270 wprobe_get_dev(const char *ifname)
 271 {
 272         struct wprobe_iface *dev;
 273
 274         if (wprobe_init() != 0)
 275                 return NULL;
 276
 277         dev = malloc(sizeof(struct wprobe_iface));
 278         if (!dev)
 279                 return NULL;
 280
 281         memset(dev, 0, sizeof(struct wprobe_iface));
 282         dev->ifname = strdup(ifname);
 283         if (!dev->ifname)
 284                 goto error;
 285
 286         dev->interval = -1;
 287         dev->scale_min = -1;
 288         dev->scale_max = -1;
 289         dev->scale_m = -1;
 290         dev->scale_d = -1;
 291
 292         INIT_LIST_HEAD(&dev->global_attr);
 293         INIT_LIST_HEAD(&dev->link_attr);
 294         INIT_LIST_HEAD(&dev->links);
 295
 296         dump_attributes(ifname, false, &dev->global_attr, NULL);
 297         dump_attributes(ifname, true, &dev->link_attr, NULL);
 298
 299         return dev;
 300
 301 error:
 302         free(dev);
 303         return NULL;
 304 }
 305
 306 static void
 307 free_attr_list(struct list_head *list)
 308 {
 309         struct wprobe_attribute *attr, *tmp;
 310
 311         list_for_each_entry_safe(attr, tmp, list, list) {
 312                 list_del(&attr->list);
 313                 free(attr);
 314         }
 315 }
 316
 317 void
 318 wprobe_free_dev(struct wprobe_iface *dev)
 319 {
 320         wprobe_free();
 321         free_attr_list(&dev->global_attr);
 322         free_attr_list(&dev->link_attr);
 323         free((void *)dev->ifname);
 324         free(dev);
 325 }
 326
 327 static struct wprobe_link *
 328 get_link(struct list_head *list, const char *addr)
 329 {
 330         struct wprobe_link *l;
 331
 332         list_for_each_entry(l, list, list) {
 333                 if (!memcmp(l->addr, addr, 6)) {
 334                         list_del_init(&l->list);
 335                         goto out;
 336                 }
 337         }
 338
 339         /* no previous link found, allocate a new one */
 340         l = malloc(sizeof(struct wprobe_link));
 341         if (!l)
 342                 goto out;
 343
 344         memset(l, 0, sizeof(struct wprobe_link));
 345         memcpy(l->addr, addr, sizeof(l->addr));
 346         INIT_LIST_HEAD(&l->list);
 347
 348 out:
 349         return l;
 350 }
 351
 352 struct wprobe_save_cb {
 353         struct list_head *list;
 354         struct list_head old_list;
 355 };
 356
 357 static int
 358 save_link_handler(struct nl_msg *msg, void *arg)
 359 {
 360         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 361         struct wprobe_link *link;
 362         struct wprobe_save_cb *cb = arg;
 363         const char *addr;
 364
 365         nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
 366                         genlmsg_attrlen(gnlh, 0), attribute_policy);
 367
 368         if (!tb[WPROBE_ATTR_MAC] || (nla_len(tb[WPROBE_ATTR_MAC]) != 6))
 369                 return -1;
 370
 371         addr = nla_data(tb[WPROBE_ATTR_MAC]);
 372         link = get_link(&cb->old_list, addr);
 373         if (!link)
 374                 return -1;
 375
 376         if (tb[WPROBE_ATTR_FLAGS])
 377                 link->flags = nla_get_u32(tb[WPROBE_ATTR_FLAGS]);
 378
 379         list_add_tail(&link->list, cb->list);
 380         return 0;
 381 }
 382
 383
 384 int
 385 wprobe_update_links(struct wprobe_iface *dev)
 386 {
 387         struct wprobe_link *l, *tmp;
 388         struct nl_msg *msg;
 389         struct wprobe_save_cb cb;
 390         int err;
 391
 392         INIT_LIST_HEAD(&cb.old_list);
 393         list_splice_init(&dev->links, &cb.old_list);
 394         cb.list = &dev->links;
 395
 396         msg = wprobe_new_msg(dev->ifname, WPROBE_CMD_GET_LINKS, true);
 397         if (!msg)
 398                 return -ENOMEM;
 399
 400         err = wprobe_send_msg(msg, save_link_handler, &cb);
 401         if (err < 0)
 402                 return err;
 403
 404         list_for_each_entry_safe(l, tmp, &cb.old_list, list) {
 405                 list_del(&l->list);
 406                 free(l);
 407         }
 408
 409         return 0;
 410 }
 411
 412 int
 413 wprobe_apply_config(struct wprobe_iface *dev)
 414 {
 415         struct nl_msg *msg;
 416
 417         msg = wprobe_new_msg(dev->ifname, WPROBE_CMD_CONFIG, false);
 418         if (!msg)
 419                 return -ENOMEM;
 420
 421         if (dev->interval >= 0)
 422                 NLA_PUT_MSECS(msg, WPROBE_ATTR_INTERVAL, dev->interval);
 423
 424         wprobe_send_msg(msg, NULL, NULL);
 425         return 0;
 426
 427 nla_put_failure:
 428         nlmsg_free(msg);
 429         return -ENOMEM;
 430 }
 431
 432 int
 433 wprobe_measure(struct wprobe_iface *dev)
 434 {
 435         struct nl_msg *msg;
 436
 437         msg = wprobe_new_msg(dev->ifname, WPROBE_CMD_MEASURE, false);
 438         if (!msg)
 439                 return -ENOMEM;
 440
 441         wprobe_send_msg(msg, NULL, NULL);
 442         return 0;
 443 }
 444
 445 struct wprobe_request_cb {
 446         struct list_head *list;
 447         struct list_head old_list;
 448         char *addr;
 449 };
 450
 451 static int
 452 save_attrdata_handler(struct nl_msg *msg, void *arg)
 453 {
 454         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 455         struct wprobe_request_cb *cb = arg;
 456         struct wprobe_attribute *attr;
 457         int type, id;
 458
 459         nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
 460                         genlmsg_attrlen(gnlh, 0), attribute_policy);
 461
 462         if (!tb[WPROBE_ATTR_ID])
 463                 return -1;
 464
 465         if (!tb[WPROBE_ATTR_TYPE])
 466                 return -1;
 467
 468         id = nla_get_u32(tb[WPROBE_ATTR_ID]);
 469         list_for_each_entry(attr, &cb->old_list, list) {
 470                 if (attr->id == id)
 471                         goto found;
 472         }
 473         /* not found */
 474         return -1;
 475
 476 found:
 477         list_del_init(&attr->list);
 478
 479         type = nla_get_u8(tb[WPROBE_ATTR_TYPE]);
 480         if (type != attr->type) {
 481                 DPRINTF("WARNING: type mismatch for %s attribute '%s' (%d != %d)\n",
 482                         (cb->addr ? "link" : "global"),
 483                         attr->name,
 484                         type, attr->type);
 485                 goto out;
 486         }
 487
 488         if ((type < WPROBE_VAL_STRING) ||
 489                 (type > WPROBE_VAL_U64))
 490                 goto out;
 491
 492         memset(&attr->val, 0, sizeof(attr->val));
 493
 494 #define HANDLE_INT_TYPE(_idx, _type) \
 495         case WPROBE_VAL_S##_type: \
 496         case WPROBE_VAL_U##_type: \
 497                 attr->val.U##_type = nla_get_u##_type(tb[_idx]); \
 498                 break
 499
 500         switch(type) {
 501                 HANDLE_INT_TYPE(type, 8);
 502                 HANDLE_INT_TYPE(type, 16);
 503                 HANDLE_INT_TYPE(type, 32);
 504                 HANDLE_INT_TYPE(type, 64);
 505                 case WPROBE_VAL_STRING:
 506                         /* unimplemented */
 507                         break;
 508         }
 509 #undef HANDLE_TYPE
 510
 511         if (attr->flags & WPROBE_F_KEEPSTAT) {
 512                 if (tb[WPROBE_VAL_SUM])
 513                         attr->val.s = nla_get_u64(tb[WPROBE_VAL_SUM]);
 514
 515                 if (tb[WPROBE_VAL_SUM_SQ])
 516                         attr->val.ss = nla_get_u64(tb[WPROBE_VAL_SUM_SQ]);
 517
 518                 if (tb[WPROBE_VAL_SAMPLES])
 519                         attr->val.n = nla_get_u32(tb[WPROBE_VAL_SAMPLES]);
 520
 521                 if (attr->val.n > 0) {
 522                         float avg = ((float) attr->val.s) / attr->val.n;
 523                         float stdev = sqrt((((float) attr->val.ss) / attr->val.n) - (avg * avg));
 524                         if (isnan(stdev))
 525                                 stdev = 0.0f;
 526                         if (isnan(avg))
 527                                 avg = 0.0f;
 528                         attr->val.avg = avg;
 529                         attr->val.stdev = stdev;
 530                 }
 531         }
 532
 533 out:
 534         list_add_tail(&attr->list, cb->list);
 535         return 0;
 536 }
 537
 538
 539 int
 540 wprobe_request_data(struct wprobe_iface *dev, const unsigned char *addr)
 541 {
 542         struct wprobe_request_cb cb;
 543         struct list_head *attrs;
 544         struct nl_msg *msg;
 545         int err;
 546
 547         msg = wprobe_new_msg(dev->ifname, WPROBE_CMD_GET_INFO, true);
 548         if (!msg)
 549                 return -ENOMEM;
 550
 551         if (addr) {
 552                 attrs = &dev->link_attr;
 553                 NLA_PUT(msg, WPROBE_ATTR_MAC, 6, addr);
 554         } else {
 555                 attrs = &dev->global_attr;
 556         }
 557
 558         INIT_LIST_HEAD(&cb.old_list);
 559         list_splice_init(attrs, &cb.old_list);
 560         cb.list = attrs;
 561
 562         err = wprobe_send_msg(msg, save_attrdata_handler, &cb);
 563         list_splice(&cb.old_list, attrs->prev);
 564         return err;
 565
 566 nla_put_failure:
 567         nlmsg_free(msg);
 568         return -ENOMEM;
 569 }
 570
 571