package/wprobe/src/user/wprobe-lib.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 #define _ISOC99_SOURCE
  17 #define _BSD_SOURCE
  18 #include <sys/types.h>
  19 #include <sys/socket.h>
  20 #include <stdio.h>
  21 #include <string.h>
  22 #include <stdlib.h>
  23 #include <errno.h>
  24 #include <getopt.h>
  25 #include <unistd.h>
  26 #include <stdbool.h>
  27 #include <math.h>
  28 #include <linux/wprobe.h>
  29 #include <netlink/netlink.h>
  30 #include <netlink/attr.h>
  31 #include <netlink/genl/genl.h>
  32 #ifndef NO_LOCAL_ACCESS
  33 #include <netlink/genl/ctrl.h>
  34 #include <netlink/genl/family.h>
  35 #include <endian.h>
  36 #endif
  37 #include "wprobe.h"
  38
  39 #define DEBUG 1
  40 #ifdef DEBUG
  41 #define DPRINTF(fmt, ...) fprintf(stderr, "%s(%d): " fmt, __func__, __LINE__, ##__VA_ARGS__)
  42 #else
  43 #define DPRINTF(fmt, ...) do {} while (0)
  44 #endif
  45
  46 #if defined(BYTE_ORDER) && !defined(__BYTE_ORDER)
  47 #define __LITTLE_ENDIAN LITTLE_ENDIAN
  48 #define __BIG_ENDIAN BIG_ENDIAN
  49 #define __BYTE_ORDER BYTE_ORDER
  50 #endif
  51
  52 #ifndef __BYTE_ORDER
  53 #error Unknown endian type
  54 #endif
  55
  56 #define WPROBE_MAX_MSGLEN       65536
  57
  58 static inline __u16 __swab16(__u16 x)
  59 {
  60         return x<<8 | x>>8;
  61 }
  62
  63 static inline __u32 __swab32(__u32 x)
  64 {
  65         return x<<24 | x>>24 |
  66                 (x & (__u32)0x0000ff00UL)<<8 |
  67                 (x & (__u32)0x00ff0000UL)>>8;
  68 }
  69
  70 static inline __u64 __swab64(__u64 x)
  71 {
  72         return x<<56 | x>>56 |
  73                 (x & (__u64)0x000000000000ff00ULL)<<40 |
  74                 (x & (__u64)0x0000000000ff0000ULL)<<24 |
  75                 (x & (__u64)0x00000000ff000000ULL)<< 8 |
  76                 (x & (__u64)0x000000ff00000000ULL)>> 8 |
  77                 (x & (__u64)0x0000ff0000000000ULL)>>24 |
  78                 (x & (__u64)0x00ff000000000000ULL)>>40;
  79 }
  80
  81
  82 #if __BYTE_ORDER == __LITTLE_ENDIAN
  83 #define SWAP16(var) var = __swab16(var)
  84 #define SWAP32(var) var = __swab32(var)
  85 #define SWAP64(var) var = __swab64(var)
  86 #else
  87 #define SWAP16(var) do {} while(0)
  88 #define SWAP32(var) do {} while(0)
  89 #define SWAP64(var) do {} while(0)
  90 #endif
  91
  92 int wprobe_port = 17990;
  93 static struct nlattr *tb[WPROBE_ATTR_LAST+1];
  94 static struct nla_policy attribute_policy[WPROBE_ATTR_LAST+1] = {
  95         [WPROBE_ATTR_ID] = { .type = NLA_U32 },
  96         [WPROBE_ATTR_MAC] = { .type = NLA_UNSPEC, .minlen = 6, .maxlen = 6 },
  97         [WPROBE_ATTR_NAME] = { .type = NLA_STRING },
  98         [WPROBE_ATTR_FLAGS] = { .type = NLA_U32 },
  99         [WPROBE_ATTR_TYPE] = { .type = NLA_U8 },
 100         [WPROBE_ATTR_FLAGS] = { .type = NLA_U32 },
 101         [WPROBE_VAL_S8] = { .type = NLA_U8 },
 102         [WPROBE_VAL_S16] = { .type = NLA_U16 },
 103         [WPROBE_VAL_S32] = { .type = NLA_U32 },
 104         [WPROBE_VAL_S64] = { .type = NLA_U64 },
 105         [WPROBE_VAL_U8] = { .type = NLA_U8 },
 106         [WPROBE_VAL_U16] = { .type = NLA_U16 },
 107         [WPROBE_VAL_U32] = { .type = NLA_U32 },
 108         [WPROBE_VAL_U64] = { .type = NLA_U64 },
 109         [WPROBE_VAL_SUM] = { .type = NLA_U64 },
 110         [WPROBE_VAL_SUM_SQ] = { .type = NLA_U64 },
 111         [WPROBE_VAL_SAMPLES] = { .type = NLA_U32 },
 112         [WPROBE_VAL_SCALE_TIME] = { .type = NLA_U64 },
 113         [WPROBE_ATTR_INTERVAL] = { .type = NLA_U64 },
 114         [WPROBE_ATTR_SAMPLES_MIN] = { .type = NLA_U32 },
 115         [WPROBE_ATTR_SAMPLES_MAX] = { .type = NLA_U32 },
 116         [WPROBE_ATTR_SAMPLES_SCALE_M] = { .type = NLA_U32 },
 117         [WPROBE_ATTR_SAMPLES_SCALE_D] = { .type = NLA_U32 },
 118         [WPROBE_ATTR_FILTER_GROUP] = { .type = NLA_NESTED },
 119         [WPROBE_ATTR_RXCOUNT] = { .type = NLA_U64 },
 120         [WPROBE_ATTR_TXCOUNT] = { .type = NLA_U64 },
 121 };
 122
 123 typedef int (*wprobe_cb_t)(struct nl_msg *, void *);
 124
 125 struct wprobe_iface_ops {
 126         int (*send_msg)(struct wprobe_iface *dev, struct nl_msg *msg, wprobe_cb_t cb, void *arg);
 127         void (*free)(struct wprobe_iface *dev);
 128 };
 129
 130 struct wprobe_attr_cb {
 131         struct list_head *list;
 132         char *addr;
 133 };
 134
 135 #define WPROBE_MAGIC_STR "WPROBE"
 136 struct wprobe_init_hdr {
 137         struct {
 138                 char magic[sizeof(WPROBE_MAGIC_STR)];
 139
 140                 /* protocol version */
 141                 uint8_t version;
 142
 143                 /* extra header length (unused for now) */
 144                 uint16_t extra;
 145         } pre __attribute__((packed));
 146         union {
 147                 struct {
 148                         uint16_t genl_family;
 149                 } v0 __attribute__((packed));
 150         };
 151 } __attribute__((packed));
 152
 153 struct wprobe_msg_hdr {
 154         __u16 status;
 155         __u16 error;
 156         __u32 len;
 157 };
 158
 159 enum wprobe_resp_status {
 160         WPROBE_MSG_DONE = 0,
 161         WPROBE_MSG_DATA = 1,
 162 };
 163
 164 static inline void
 165 wprobe_swap_msg_hdr(struct wprobe_msg_hdr *mhdr)
 166 {
 167         SWAP16(mhdr->status);
 168         SWAP16(mhdr->error);
 169         SWAP32(mhdr->len);
 170 }
 171
 172 static int
 173 save_attribute_handler(struct nl_msg *msg, void *arg)
 174 {
 175         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 176         const char *name = "N/A";
 177         struct wprobe_attribute *attr;
 178         int type = 0;
 179         struct wprobe_attr_cb *cb = arg;
 180
 181         nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
 182                         genlmsg_attrlen(gnlh, 0), attribute_policy);
 183
 184         if (tb[WPROBE_ATTR_NAME])
 185                 name = nla_data(tb[WPROBE_ATTR_NAME]);
 186
 187         attr = malloc(sizeof(struct wprobe_attribute) + strlen(name) + 1);
 188         if (!attr)
 189                 return -1;
 190
 191         memset(attr, 0, sizeof(struct wprobe_attribute));
 192
 193         if (tb[WPROBE_ATTR_ID])
 194                 attr->id = nla_get_u32(tb[WPROBE_ATTR_ID]);
 195
 196         if (tb[WPROBE_ATTR_MAC] && cb->addr)
 197                 memcpy(cb->addr, nla_data(tb[WPROBE_ATTR_MAC]), 6);
 198
 199         if (tb[WPROBE_ATTR_FLAGS])
 200                 attr->flags = nla_get_u32(tb[WPROBE_ATTR_FLAGS]);
 201
 202         if (tb[WPROBE_ATTR_TYPE])
 203                 type = nla_get_u8(tb[WPROBE_ATTR_TYPE]);
 204
 205         if ((type < WPROBE_VAL_STRING) ||
 206                 (type > WPROBE_VAL_U64))
 207                 type = 0;
 208
 209         attr->type = type;
 210         strcpy(attr->name, name);
 211         INIT_LIST_HEAD(&attr->list);
 212         list_add(&attr->list, cb->list);
 213         return 0;
 214 }
 215
 216 static struct nl_msg *
 217 wprobe_new_msg(struct wprobe_iface *dev, int cmd, bool dump)
 218 {
 219         struct nl_msg *msg;
 220         uint32_t flags = 0;
 221
 222         msg = nlmsg_alloc_size(65536);
 223         if (!msg)
 224                 return NULL;
 225
 226         if (dump)
 227                 flags |= NLM_F_DUMP;
 228
 229         genlmsg_put(msg, 0, 0, dev->genl_family,
 230                         0, flags, cmd, 0);
 231
 232         NLA_PUT_STRING(msg, WPROBE_ATTR_INTERFACE, dev->ifname);
 233 nla_put_failure:
 234         return msg;
 235 }
 236
 237
 238 static int
 239 dump_attributes(struct wprobe_iface *dev, bool link, struct list_head *list, char *addr)
 240 {
 241         struct nl_msg *msg;
 242         struct wprobe_attr_cb cb;
 243
 244         cb.list = list;
 245         cb.addr = addr;
 246         msg = wprobe_new_msg(dev, WPROBE_CMD_GET_LIST, true);
 247         if (!msg)
 248                 return -ENOMEM;
 249
 250         if (link)
 251                 NLA_PUT(msg, WPROBE_ATTR_MAC, 6, "\x00\x00\x00\x00\x00\x00");
 252
 253         return dev->ops->send_msg(dev, msg, save_attribute_handler, &cb);
 254
 255 nla_put_failure:
 256         nlmsg_free(msg);
 257         return -EINVAL;
 258 }
 259
 260 static struct wprobe_iface *
 261 wprobe_alloc_dev(void)
 262 {
 263         struct wprobe_iface *dev;
 264
 265         dev = malloc(sizeof(struct wprobe_iface));
 266         if (!dev)
 267                 return NULL;
 268
 269         memset(dev, 0, sizeof(struct wprobe_iface));
 270
 271         dev->interval = -1;
 272         dev->scale_min = -1;
 273         dev->scale_max = -1;
 274         dev->scale_m = -1;
 275         dev->scale_d = -1;
 276         dev->sockfd = -1;
 277
 278         INIT_LIST_HEAD(&dev->global_attr);
 279         INIT_LIST_HEAD(&dev->link_attr);
 280         INIT_LIST_HEAD(&dev->links);
 281         return dev;
 282 }
 283
 284 static int
 285 wprobe_init_dev(struct wprobe_iface *dev)
 286 {
 287         dump_attributes(dev, false, &dev->global_attr, NULL);
 288         dump_attributes(dev, true, &dev->link_attr, NULL);
 289         return 0;
 290 }
 291
 292 #ifndef NO_LOCAL_ACCESS
 293 static int n_devs = 0;
 294 static struct nl_sock *handle = NULL;
 295 static struct nl_cache *cache = NULL;
 296 static struct genl_family *family = NULL;
 297
 298 static int
 299 error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg)
 300 {
 301         int *ret = arg;
 302         *ret = err->error;
 303         return NL_STOP;
 304 }
 305
 306 static int
 307 finish_handler(struct nl_msg *msg, void *arg)
 308 {
 309         int *ret = arg;
 310         *ret = 0;
 311         return NL_SKIP;
 312 }
 313
 314 static int
 315 ack_handler(struct nl_msg *msg, void *arg)
 316 {
 317         int *ret = arg;
 318         *ret = 0;
 319         return NL_STOP;
 320 }
 321
 322 static void
 323 wprobe_local_free(struct wprobe_iface *dev)
 324 {
 325         /* should not happen */
 326         if (n_devs == 0)
 327                 return;
 328
 329         if (--n_devs != 0)
 330                 return;
 331
 332         if (cache)
 333                 nl_cache_free(cache);
 334         if (handle)
 335                 nl_socket_free(handle);
 336         handle = NULL;
 337         cache = NULL;
 338 }
 339
 340 static int
 341 wprobe_local_init(void)
 342 {
 343         int ret;
 344
 345         if (n_devs++ > 0)
 346                 return 0;
 347
 348         handle = nl_socket_alloc();
 349         if (!handle) {
 350                 DPRINTF("Failed to create handle\n");
 351                 goto err;
 352         }
 353
 354         if (genl_connect(handle)) {
 355                 DPRINTF("Failed to connect to generic netlink\n");
 356                 goto err;
 357         }
 358
 359         ret = genl_ctrl_alloc_cache(handle, &cache);
 360         if (ret < 0) {
 361                 DPRINTF("Failed to allocate netlink cache\n");
 362                 goto err;
 363         }
 364
 365         family = genl_ctrl_search_by_name(cache, "wprobe");
 366         if (!family) {
 367                 DPRINTF("wprobe API not present\n");
 368                 goto err;
 369         }
 370         return 0;
 371
 372 err:
 373         wprobe_local_free(NULL);
 374         return -EINVAL;
 375 }
 376
 377
 378 static int
 379 wprobe_local_send_msg(struct wprobe_iface *dev, struct nl_msg *msg, wprobe_cb_t callback, void *arg)
 380 {
 381         struct nl_cb *cb;
 382         int err = 0;
 383
 384         cb = nl_cb_alloc(NL_CB_DEFAULT);
 385         if (!cb)
 386                 goto out_no_cb;
 387
 388         if (callback)
 389                 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, callback, arg);
 390
 391         err = nl_send_auto_complete(handle, msg);
 392         if (err < 0)
 393                 goto out;
 394
 395         err = 1;
 396
 397         nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
 398         nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
 399         nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
 400
 401         while (err > 0)
 402                 nl_recvmsgs(handle, cb);
 403
 404 out:
 405         nl_cb_put(cb);
 406 out_no_cb:
 407         nlmsg_free(msg);
 408         return err;
 409 }
 410
 411 static const struct wprobe_iface_ops wprobe_local_ops = {
 412         .send_msg = wprobe_local_send_msg,
 413         .free = wprobe_local_free,
 414 };
 415
 416 struct wprobe_iface *
 417 wprobe_get_dev(const char *ifname)
 418 {
 419         struct wprobe_iface *dev;
 420
 421         if (wprobe_local_init() != 0)
 422                 return NULL;
 423
 424         dev = wprobe_alloc_dev();
 425         if (!dev)
 426                 goto error_alloc;
 427
 428         dev->ifname = strdup(ifname);
 429         dev->ops = &wprobe_local_ops;
 430         dev->genl_family = genl_family_get_id(family);
 431
 432         if (wprobe_init_dev(dev) < 0)
 433                 goto error;
 434
 435         return dev;
 436
 437 error:
 438         free(dev);
 439 error_alloc:
 440         wprobe_local_free(NULL);
 441         return NULL;
 442 }
 443
 444 #endif
 445
 446 static void swap_nlmsghdr(struct nlmsghdr *nlh)
 447 {
 448         SWAP32(nlh->nlmsg_len);
 449         SWAP16(nlh->nlmsg_type);
 450         SWAP16(nlh->nlmsg_flags);
 451         SWAP32(nlh->nlmsg_seq);
 452         SWAP32(nlh->nlmsg_pid);
 453 }
 454
 455 static void swap_genlmsghdr(struct genlmsghdr *gnlh)
 456 {
 457 #if 0 /* probably unnecessary */
 458         SWAP16(gnlh->reserved);
 459 #endif
 460 }
 461
 462 static void
 463 wprobe_swap_nested(void *data, int len, bool outgoing)
 464 {
 465         void *end = data + len;
 466
 467         while (data < end) {
 468                 struct nlattr *nla = data;
 469                 unsigned int type, len;
 470
 471                 if (!outgoing) {
 472                         SWAP16(nla->nla_len);
 473                         SWAP16(nla->nla_type);
 474
 475                         /* required for further sanity checks */
 476                         if (data + nla->nla_len > end)
 477                                 nla->nla_len = end - data;
 478                 }
 479
 480                 len = NLA_ALIGN(nla->nla_len);
 481                 type = nla->nla_type & NLA_TYPE_MASK;
 482
 483                 if (type <= WPROBE_ATTR_LAST) {
 484 #if __BYTE_ORDER == __LITTLE_ENDIAN
 485                         switch(attribute_policy[type].type) {
 486                         case NLA_U16:
 487                                 SWAP16(*(__u16 *)nla_data(nla));
 488                                 break;
 489                         case NLA_U32:
 490                                 SWAP32(*(__u32 *)nla_data(nla));
 491                                 break;
 492                         case NLA_U64:
 493                                 SWAP64(*(__u64 *)nla_data(nla));
 494                                 break;
 495                         case NLA_NESTED:
 496                                 wprobe_swap_nested(nla_data(nla), nla_len(nla), outgoing);
 497                                 break;
 498                         }
 499 #endif
 500                 }
 501                 data += len;
 502
 503                 if (outgoing) {
 504                         SWAP16(nla->nla_len);
 505                         SWAP16(nla->nla_type);
 506                 }
 507                 if (!nla->nla_len)
 508                         break;
 509         }
 510 }
 511
 512 static struct nl_msg *
 513 wprobe_msg_from_network(int socket, int len)
 514 {
 515         struct genlmsghdr *gnlh;
 516         struct nlmsghdr *nlh;
 517         struct nl_msg *msg;
 518         void *data;
 519
 520         msg = nlmsg_alloc_size(len + 32);
 521         if (!msg)
 522                 return NULL;
 523
 524         nlh = nlmsg_hdr(msg);
 525         if (read(socket, nlh, len) != len)
 526                 goto free;
 527
 528         swap_nlmsghdr(nlh);
 529         if (nlh->nlmsg_len > len)
 530                 goto free;
 531
 532         gnlh = nlmsg_data(nlh);
 533         swap_genlmsghdr(gnlh);
 534
 535         data = genlmsg_data(gnlh);
 536         wprobe_swap_nested(data, genlmsg_len(gnlh), false);
 537
 538         return msg;
 539 free:
 540         nlmsg_free(msg);
 541         return NULL;
 542 }
 543
 544 static int
 545 wprobe_msg_to_network(int socket, struct nl_msg *msg)
 546 {
 547         struct nlmsghdr *nlh = nlmsg_hdr(msg);
 548         struct wprobe_msg_hdr mhdr;
 549         struct genlmsghdr *gnlh;
 550         void *buf, *data;
 551         int buflen, datalen;
 552         int ret;
 553
 554         buflen = nlh->nlmsg_len;
 555         buf = malloc(buflen);
 556         if (!buf)
 557                 return -ENOMEM;
 558
 559         memset(&mhdr, 0, sizeof(mhdr));
 560         mhdr.status = WPROBE_MSG_DATA;
 561         mhdr.len = buflen;
 562         wprobe_swap_msg_hdr(&mhdr);
 563         ret = write(socket, &mhdr, sizeof(mhdr));
 564         if (ret < 0)
 565                 goto out;
 566
 567         memcpy(buf, nlh, buflen);
 568         nlh = buf;
 569         gnlh = nlmsg_data(nlh);
 570         data = genlmsg_data(gnlh);
 571         datalen = genlmsg_len(gnlh);
 572
 573         wprobe_swap_nested(data, datalen, true);
 574         swap_genlmsghdr(gnlh);
 575         swap_nlmsghdr(nlh);
 576         ret = write(socket, buf, buflen);
 577
 578 out:
 579         free(buf);
 580
 581         return ret;
 582 }
 583
 584 static int
 585 wprobe_remote_send_msg(struct wprobe_iface *dev, struct nl_msg *msg, wprobe_cb_t callback, void *arg)
 586 {
 587         struct wprobe_msg_hdr mhdr;
 588         int msgs = 0;
 589
 590         wprobe_msg_to_network(dev->sockfd, msg);
 591         nlmsg_free(msg);
 592         do {
 593                 if (read(dev->sockfd, &mhdr, sizeof(mhdr)) != sizeof(mhdr)) {
 594                         DPRINTF("Failed to read response header\n");
 595                         return -1;
 596                 }
 597                 wprobe_swap_msg_hdr(&mhdr);
 598
 599                 switch(mhdr.status) {
 600                 case WPROBE_MSG_DATA:
 601                         if (mhdr.len > WPROBE_MAX_MSGLEN) {
 602                                 fprintf(stderr, "Invalid length in received response message.\n");
 603                                 exit(1);
 604                         }
 605
 606                         msg = wprobe_msg_from_network(dev->sockfd, mhdr.len);
 607                         if (!msg)
 608                                 return -EINVAL;
 609
 610                         msgs++;
 611                         callback(msg, arg);
 612                         nlmsg_free(msg);
 613                         break;
 614                 }
 615         } while (mhdr.status != WPROBE_MSG_DONE);
 616
 617         if (mhdr.error)
 618                 return -mhdr.error;
 619         else
 620                 return msgs;
 621 }
 622
 623
 624 static void
 625 wprobe_socket_dev_free(struct wprobe_iface *dev)
 626 {
 627         if (dev->sockfd >= 0)
 628                 close(dev->sockfd);
 629 }
 630
 631 static const struct wprobe_iface_ops wprobe_remote_ops = {
 632         .send_msg = wprobe_remote_send_msg,
 633         .free = wprobe_socket_dev_free,
 634 };
 635
 636
 637 #ifndef NO_LOCAL_ACCESS
 638 int
 639 wprobe_server_init(int socket)
 640 {
 641         struct wprobe_init_hdr hdr;
 642         int ret;
 643
 644         ret = wprobe_local_init();
 645         if (ret != 0)
 646                 return ret;
 647
 648         memset(&hdr, 0, sizeof(hdr));
 649         memcpy(hdr.pre.magic, WPROBE_MAGIC_STR, sizeof(WPROBE_MAGIC_STR));
 650         hdr.pre.version = 0;
 651         hdr.v0.genl_family = genl_family_get_id(family);
 652         SWAP16(hdr.v0.genl_family);
 653         write(socket, (unsigned char *)&hdr, sizeof(hdr));
 654
 655         return 0;
 656 }
 657
 658 static int
 659 wprobe_server_cb(struct nl_msg *msg, void *arg)
 660 {
 661         int *socket = arg;
 662         int ret;
 663
 664         ret = wprobe_msg_to_network(*socket, msg);
 665         if (ret > 0)
 666                 ret = 0;
 667
 668         return ret;
 669 }
 670
 671
 672 int
 673 wprobe_server_handle(int socket)
 674 {
 675         struct wprobe_msg_hdr mhdr;
 676         struct nl_msg *msg;
 677         int ret;
 678
 679         ret = read(socket, &mhdr, sizeof(mhdr));
 680         if (ret != sizeof(mhdr)) {
 681                 if (ret <= 0)
 682                         return -1;
 683
 684                 DPRINTF("Failed to read request header\n");
 685                 return -EINVAL;
 686         }
 687         wprobe_swap_msg_hdr(&mhdr);
 688
 689         switch(mhdr.status) {
 690         case WPROBE_MSG_DATA:
 691                 if (mhdr.len > WPROBE_MAX_MSGLEN) {
 692                         DPRINTF("Invalid length in received response message.\n");
 693                         return -EINVAL;
 694                 }
 695                 msg = wprobe_msg_from_network(socket, mhdr.len);
 696                 break;
 697         default:
 698                 DPRINTF("Invalid request header type\n");
 699                 return -ENOENT;
 700         }
 701
 702         if (!msg) {
 703                 DPRINTF("Failed to get message\n");
 704                 return -EINVAL;
 705         }
 706
 707         ret = wprobe_local_send_msg(NULL, msg, wprobe_server_cb, &socket);
 708
 709         memset(&mhdr, 0, sizeof(mhdr));
 710         mhdr.status = WPROBE_MSG_DONE;
 711         if (ret < 0)
 712                 mhdr.error = (uint16_t) -ret;
 713
 714         ret = write(socket, (unsigned char *)&mhdr, sizeof(mhdr));
 715         if (ret > 0)
 716                 ret = 0;
 717
 718         return ret;
 719 }
 720
 721 void
 722 wprobe_server_done(void)
 723 {
 724         wprobe_local_free(NULL);
 725 }
 726 #endif
 727
 728 struct wprobe_iface *
 729 wprobe_get_from_socket(int socket, const char *name)
 730 {
 731         struct wprobe_iface *dev;
 732         struct wprobe_init_hdr hdr;
 733
 734         dev = wprobe_alloc_dev();
 735         if (!dev)
 736                 goto out;
 737
 738         dev->ops = &wprobe_remote_ops;
 739         dev->sockfd = socket;
 740         dev->ifname = strdup(name);
 741
 742         /* read version and header length */
 743         if (read(socket, &hdr.pre, sizeof(hdr.pre)) != sizeof(hdr.pre)) {
 744                 DPRINTF("Could not read header\n");
 745                 goto error;
 746         }
 747
 748         /* magic not found */
 749         if (memcmp(hdr.pre.magic, WPROBE_MAGIC_STR, sizeof(hdr.pre.magic)) != 0) {
 750                 DPRINTF("Magic does not match\n");
 751                 goto error;
 752         }
 753
 754         /* unsupported version */
 755         if (hdr.pre.version != 0) {
 756                 DPRINTF("Protocol version does not match\n");
 757                 goto error;
 758         }
 759
 760         if (read(socket, &hdr.v0, sizeof(hdr.v0)) != sizeof(hdr.v0)) {
 761                 DPRINTF("Could not read header data\n");
 762                 goto error;
 763         }
 764
 765         SWAP16(hdr.pre.extra);
 766         SWAP16(hdr.v0.genl_family);
 767         dev->genl_family = hdr.v0.genl_family;
 768
 769         if (wprobe_init_dev(dev) < 0) {
 770                 DPRINTF("Could not initialize device\n");
 771                 goto error;
 772         }
 773
 774 out:
 775         return dev;
 776
 777 error:
 778         wprobe_free_dev(dev);
 779         return NULL;
 780 }
 781
 782 struct wprobe_iface *
 783 wprobe_get_auto(const char *arg, char **err)
 784 {
 785         static struct sockaddr_in sa;
 786         static char errbuf[512];
 787
 788         struct wprobe_iface *dev = NULL;
 789         struct hostent *h;
 790         char *devstr = strdup(arg);
 791         char *sep = NULL;
 792         int sock = -1;
 793         int len;
 794
 795         if (err)
 796                 *err = NULL;
 797
 798         sep = strchr(devstr, ':');
 799         if (!sep) {
 800 #ifndef NO_LOCAL_ACCESS
 801                 free(devstr);
 802                 return wprobe_get_dev(arg);
 803 #else
 804                 if (err)
 805                         *err = "Invalid argument";
 806                 goto out;
 807 #endif
 808         }
 809
 810         *sep = 0;
 811         sep++;
 812
 813         sock = socket(AF_INET, SOCK_STREAM, 0);
 814         if (sock < 0)
 815                 goto syserr;
 816
 817         h = gethostbyname(devstr);
 818         if (!h) {
 819                 sprintf(errbuf, "Host not found");
 820                 goto out_err;
 821         }
 822
 823         memcpy(&sa.sin_addr, h->h_addr, h->h_length);
 824         sa.sin_family = AF_INET;
 825         sa.sin_port = htons(wprobe_port);
 826         if (connect(sock, (struct sockaddr *)&sa, sizeof(sa)) < 0)
 827                 goto syserr;
 828
 829         dev = wprobe_get_from_socket(sock, sep);
 830         if (!dev) {
 831                 sprintf(errbuf, "wprobe connection initialization failed");
 832                 goto out_err;
 833         }
 834         goto out;
 835
 836 syserr:
 837         if (err) {
 838                 strcpy(errbuf, "Connection failed: ");
 839                 len = strlen(errbuf);
 840                 strerror_r(errno, errbuf + len, sizeof(errbuf) - len - 1);
 841         }
 842 out_err:
 843         if (err)
 844                 *err = errbuf;
 845         if (sock >= 0)
 846                 close(sock);
 847 out:
 848         if (devstr)
 849                 free(devstr);
 850         return dev;
 851 }
 852
 853 static void
 854 free_attr_list(struct list_head *list)
 855 {
 856         struct wprobe_attribute *attr, *tmp;
 857
 858         list_for_each_entry_safe(attr, tmp, list, list) {
 859                 list_del(&attr->list);
 860                 free(attr);
 861         }
 862 }
 863
 864 void
 865 wprobe_free_dev(struct wprobe_iface *dev)
 866 {
 867         if (dev->ops->free)
 868                 dev->ops->free(dev);
 869         free_attr_list(&dev->global_attr);
 870         free_attr_list(&dev->link_attr);
 871         free((void *)dev->ifname);
 872         free(dev);
 873 }
 874
 875 static struct wprobe_link *
 876 get_link(struct list_head *list, const char *addr)
 877 {
 878         struct wprobe_link *l;
 879
 880         list_for_each_entry(l, list, list) {
 881                 if (!memcmp(l->addr, addr, 6)) {
 882                         list_del_init(&l->list);
 883                         goto out;
 884                 }
 885         }
 886
 887         /* no previous link found, allocate a new one */
 888         l = malloc(sizeof(struct wprobe_link));
 889         if (!l)
 890                 goto out;
 891
 892         memset(l, 0, sizeof(struct wprobe_link));
 893         memcpy(l->addr, addr, sizeof(l->addr));
 894         INIT_LIST_HEAD(&l->list);
 895
 896 out:
 897         return l;
 898 }
 899
 900 struct wprobe_save_cb {
 901         struct list_head *list;
 902         struct list_head old_list;
 903 };
 904
 905 static int
 906 save_link_handler(struct nl_msg *msg, void *arg)
 907 {
 908         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 909         struct wprobe_link *link;
 910         struct wprobe_save_cb *cb = arg;
 911         const char *addr;
 912
 913         nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
 914                         genlmsg_attrlen(gnlh, 0), attribute_policy);
 915
 916         if (!tb[WPROBE_ATTR_MAC] || (nla_len(tb[WPROBE_ATTR_MAC]) != 6))
 917                 return -1;
 918
 919         addr = nla_data(tb[WPROBE_ATTR_MAC]);
 920         link = get_link(&cb->old_list, addr);
 921         if (!link)
 922                 return -1;
 923
 924         if (tb[WPROBE_ATTR_FLAGS])
 925                 link->flags = nla_get_u32(tb[WPROBE_ATTR_FLAGS]);
 926
 927         list_add_tail(&link->list, cb->list);
 928         return 0;
 929 }
 930
 931
 932 int
 933 wprobe_update_links(struct wprobe_iface *dev)
 934 {
 935         struct wprobe_link *l, *tmp;
 936         struct nl_msg *msg;
 937         struct wprobe_save_cb cb;
 938         int err;
 939
 940         INIT_LIST_HEAD(&cb.old_list);
 941         list_splice_init(&dev->links, &cb.old_list);
 942         cb.list = &dev->links;
 943
 944         msg = wprobe_new_msg(dev, WPROBE_CMD_GET_LINKS, true);
 945         if (!msg)
 946                 return -ENOMEM;
 947
 948         err = dev->ops->send_msg(dev, msg, save_link_handler, &cb);
 949         if (err < 0)
 950                 return err;
 951
 952         list_for_each_entry_safe(l, tmp, &cb.old_list, list) {
 953                 list_del(&l->list);
 954                 free(l);
 955         }
 956
 957         return 0;
 958 }
 959
 960
 961 struct wprobe_filter_data
 962 {
 963         wprobe_filter_cb cb;
 964         void *arg;
 965         struct wprobe_filter_item *buf;
 966         int buflen;
 967 };
 968
 969 static int
 970 dump_filter_handler(struct nl_msg *msg, void *arg)
 971 {
 972         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 973         struct wprobe_filter_data *data = arg;
 974         struct nlattr *p;
 975         const char *name;
 976         int count = 0;
 977         int len;
 978
 979         nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
 980                         genlmsg_attrlen(gnlh, 0), attribute_policy);
 981
 982         if (!tb[WPROBE_ATTR_NAME] || !tb[WPROBE_ATTR_FILTER_GROUP])
 983                 return -1;
 984
 985         name = nla_data(tb[WPROBE_ATTR_NAME]);
 986         nla_for_each_nested(p, tb[WPROBE_ATTR_FILTER_GROUP], len) {
 987                 count++;
 988         }
 989
 990         if (data->buflen < count) {
 991                 if (data->buf)
 992                         free(data->buf);
 993                 data->buflen = count;
 994                 data->buf = malloc(sizeof(struct wprobe_filter_item) * count);
 995                 memset(data->buf, 0, sizeof(struct wprobe_filter_item) * count);
 996         }
 997
 998         count = 0;
 999         nla_for_each_nested(p, tb[WPROBE_ATTR_FILTER_GROUP], len) {
1000                 struct wprobe_filter_item *fi;
1001
1002                 nla_parse(tb, WPROBE_ATTR_LAST, nla_data(p),
1003                                 nla_len(p), attribute_policy);
1004
1005                 if (!tb[WPROBE_ATTR_NAME] || !tb[WPROBE_ATTR_RXCOUNT]
1006                                 || !tb[WPROBE_ATTR_TXCOUNT])
1007                         continue;
1008
1009                 fi = &data->buf[count++];
1010                 strncpy(fi->name, nla_data(tb[WPROBE_ATTR_NAME]), sizeof(fi->name) - 1);
1011                 fi->name[sizeof(fi->name) - 1] = 0;
1012                 fi->rx = nla_get_u64(tb[WPROBE_ATTR_RXCOUNT]);
1013                 fi->tx = nla_get_u64(tb[WPROBE_ATTR_TXCOUNT]);
1014         }
1015         data->cb(data->arg, name, data->buf, count);
1016
1017         return 0;
1018 }
1019
1020 int
1021 wprobe_dump_filters(struct wprobe_iface *dev, wprobe_filter_cb cb, void *arg)
1022 {
1023         struct wprobe_filter_data data;
1024         struct nl_msg *msg;
1025         int err;
1026
1027         data.buf = 0;
1028         data.buflen = 0;
1029         data.cb = cb;
1030         data.arg = arg;
1031
1032         msg = wprobe_new_msg(dev, WPROBE_CMD_GET_FILTER, true);
1033         if (!msg)
1034                 return -ENOMEM;
1035
1036         err = dev->ops->send_msg(dev, msg, dump_filter_handler, &data);
1037         if (err < 0)
1038                 return err;
1039
1040         return 0;
1041 }
1042
1043 int
1044 wprobe_apply_config(struct wprobe_iface *dev)
1045 {
1046         struct nl_msg *msg;
1047
1048         msg = wprobe_new_msg(dev, WPROBE_CMD_CONFIG, false);
1049         if (!msg)
1050                 return -ENOMEM;
1051
1052         if (dev->interval >= 0)
1053                 NLA_PUT_MSECS(msg, WPROBE_ATTR_INTERVAL, dev->interval);
1054
1055         if (dev->filter_len < 0) {
1056                 NLA_PUT(msg, WPROBE_ATTR_FILTER, 0, NULL);
1057                 dev->filter_len = 0;
1058         } else if (dev->filter && dev->filter_len > 0) {
1059                 NLA_PUT(msg, WPROBE_ATTR_FILTER, dev->filter_len, dev->filter);
1060         }
1061         dev->filter = NULL;
1062
1063         dev->ops->send_msg(dev, msg, NULL, NULL);
1064         return 0;
1065
1066 nla_put_failure:
1067         nlmsg_free(msg);
1068         return -ENOMEM;
1069 }
1070
1071 int
1072 wprobe_measure(struct wprobe_iface *dev)
1073 {
1074         struct nl_msg *msg;
1075
1076         msg = wprobe_new_msg(dev, WPROBE_CMD_MEASURE, false);
1077         if (!msg)
1078                 return -ENOMEM;
1079
1080         dev->ops->send_msg(dev, msg, NULL, NULL);
1081         return 0;
1082 }
1083
1084 struct wprobe_request_cb {
1085         struct list_head *list;
1086         struct list_head old_list;
1087         char *addr;
1088 };
1089
1090 static int
1091 save_attrdata_handler(struct nl_msg *msg, void *arg)
1092 {
1093         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
1094         struct wprobe_request_cb *cb = arg;
1095         struct wprobe_attribute *attr;
1096         int type, id;
1097
1098         nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
1099                         genlmsg_attrlen(gnlh, 0), attribute_policy);
1100
1101         if (!tb[WPROBE_ATTR_ID])
1102                 return -1;
1103
1104         if (!tb[WPROBE_ATTR_TYPE])
1105                 return -1;
1106
1107         id = nla_get_u32(tb[WPROBE_ATTR_ID]);
1108         list_for_each_entry(attr, &cb->old_list, list) {
1109                 if (attr->id == id)
1110                         goto found;
1111         }
1112         /* not found */
1113         return -1;
1114
1115 found:
1116         list_del_init(&attr->list);
1117
1118         type = nla_get_u8(tb[WPROBE_ATTR_TYPE]);
1119         if (type != attr->type) {
1120                 DPRINTF("WARNING: type mismatch for %s attribute '%s' (%d != %d)\n",
1121                         (cb->addr ? "link" : "global"),
1122                         attr->name,
1123                         type, attr->type);
1124                 goto out;
1125         }
1126
1127         if ((type < WPROBE_VAL_STRING) ||
1128                 (type > WPROBE_VAL_U64))
1129                 goto out;
1130
1131         memset(&attr->val, 0, sizeof(attr->val));
1132
1133 #define HANDLE_INT_TYPE(_idx, _type) \
1134         case WPROBE_VAL_S##_type: \
1135         case WPROBE_VAL_U##_type: \
1136                 attr->val.U##_type = nla_get_u##_type(tb[_idx]); \
1137                 break
1138
1139         switch(type) {
1140                 HANDLE_INT_TYPE(type, 8);
1141                 HANDLE_INT_TYPE(type, 16);
1142                 HANDLE_INT_TYPE(type, 32);
1143                 HANDLE_INT_TYPE(type, 64);
1144                 case WPROBE_VAL_STRING:
1145                         /* unimplemented */
1146                         break;
1147         }
1148 #undef HANDLE_TYPE
1149
1150         if (attr->flags & WPROBE_F_KEEPSTAT) {
1151                 if (tb[WPROBE_VAL_SUM])
1152                         attr->val.s = nla_get_u64(tb[WPROBE_VAL_SUM]);
1153
1154                 if (tb[WPROBE_VAL_SUM_SQ])
1155                         attr->val.ss = nla_get_u64(tb[WPROBE_VAL_SUM_SQ]);
1156
1157                 if (tb[WPROBE_VAL_SAMPLES])
1158                         attr->val.n = nla_get_u32(tb[WPROBE_VAL_SAMPLES]);
1159
1160                 if (attr->val.n > 0) {
1161                         float avg = ((float) attr->val.s) / attr->val.n;
1162                         float stdev = sqrt((((float) attr->val.ss) / attr->val.n) - (avg * avg));
1163                         if (isnan(stdev))
1164                                 stdev = 0.0f;
1165                         if (isnan(avg))
1166                                 avg = 0.0f;
1167                         attr->val.avg = avg;
1168                         attr->val.stdev = stdev;
1169                 }
1170         }
1171
1172 out:
1173         list_add_tail(&attr->list, cb->list);
1174         return 0;
1175 }
1176
1177
1178 int
1179 wprobe_request_data(struct wprobe_iface *dev, const unsigned char *addr)
1180 {
1181         struct wprobe_request_cb cb;
1182         struct list_head *attrs;
1183         struct nl_msg *msg;
1184         int err;
1185
1186         msg = wprobe_new_msg(dev, WPROBE_CMD_GET_INFO, true);
1187         if (!msg)
1188                 return -ENOMEM;
1189
1190         if (addr) {
1191                 attrs = &dev->link_attr;
1192                 NLA_PUT(msg, WPROBE_ATTR_MAC, 6, addr);
1193         } else {
1194                 attrs = &dev->global_attr;
1195         }
1196
1197         INIT_LIST_HEAD(&cb.old_list);
1198         list_splice_init(attrs, &cb.old_list);
1199         cb.list = attrs;
1200
1201         err = dev->ops->send_msg(dev, msg, save_attrdata_handler, &cb);
1202         list_splice(&cb.old_list, attrs->prev);
1203         return err;
1204
1205 nla_put_failure:
1206         nlmsg_free(msg);
1207         return -ENOMEM;
1208 }
1209
1210