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