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 struct nl_sock *handle = NULL;
  39 static struct nl_cache *cache = NULL;
  40 static struct genl_family *family = NULL;
  41 static struct nlattr *tb[WPROBE_ATTR_LAST+1];
  42 static struct nla_policy attribute_policy[WPROBE_ATTR_LAST+1] = {
  43         [WPROBE_ATTR_ID] = { .type = NLA_U32 },
  44         [WPROBE_ATTR_MAC] = { .type = NLA_UNSPEC, .minlen = 6, .maxlen = 6 },
  45         [WPROBE_ATTR_NAME] = { .type = NLA_STRING },
  46         [WPROBE_ATTR_FLAGS] = { .type = NLA_U32 },
  47         [WPROBE_ATTR_TYPE] = { .type = NLA_U8 },
  48         [WPROBE_VAL_S8] = { .type = NLA_U8 },
  49         [WPROBE_VAL_S16] = { .type = NLA_U16 },
  50         [WPROBE_VAL_S32] = { .type = NLA_U32 },
  51         [WPROBE_VAL_S64] = { .type = NLA_U64 },
  52         [WPROBE_VAL_U8] = { .type = NLA_U8 },
  53         [WPROBE_VAL_U16] = { .type = NLA_U16 },
  54         [WPROBE_VAL_U32] = { .type = NLA_U32 },
  55         [WPROBE_VAL_U64] = { .type = NLA_U64 },
  56         [WPROBE_VAL_SUM] = { .type = NLA_U64 },
  57         [WPROBE_VAL_SUM_SQ] = { .type = NLA_U64 },
  58         [WPROBE_VAL_SAMPLES] = { .type = NLA_U32 },
  59 };
  60
  61 static int
  62 error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg)
  63 {
  64         int *ret = arg;
  65         *ret = err->error;
  66         return NL_STOP;
  67 }
  68
  69 static int
  70 finish_handler(struct nl_msg *msg, void *arg)
  71 {
  72         int *ret = arg;
  73         *ret = 0;
  74         return NL_SKIP;
  75 }
  76
  77 static int
  78 ack_handler(struct nl_msg *msg, void *arg)
  79 {
  80         int *ret = arg;
  81         *ret = 0;
  82         return NL_STOP;
  83 }
  84
  85
  86 void
  87 wprobe_free(void)
  88 {
  89         if (cache)
  90                 nl_cache_free(cache);
  91         if (handle)
  92                 nl_socket_free(handle);
  93         handle = NULL;
  94         cache = NULL;
  95 }
  96
  97 int
  98 wprobe_init(void)
  99 {
 100         int ret;
 101
 102         handle = nl_socket_alloc();
 103         if (!handle) {
 104                 DPRINTF("Failed to create handle\n");
 105                 goto err;
 106         }
 107
 108         if (genl_connect(handle)) {
 109                 DPRINTF("Failed to connect to generic netlink\n");
 110                 goto err;
 111         }
 112
 113         ret = genl_ctrl_alloc_cache(handle, &cache);
 114         if (ret < 0) {
 115                 DPRINTF("Failed to allocate netlink cache\n");
 116                 goto err;
 117         }
 118
 119         family = genl_ctrl_search_by_name(cache, "wprobe");
 120         if (!family) {
 121                 DPRINTF("wprobe API not present\n");
 122                 goto err;
 123         }
 124         return 0;
 125
 126 err:
 127         wprobe_free();
 128         return -EINVAL;
 129 }
 130
 131
 132 static struct nl_msg *
 133 wprobe_new_msg(const char *ifname, int cmd, bool dump)
 134 {
 135         struct nl_msg *msg;
 136         uint32_t flags = 0;
 137
 138         msg = nlmsg_alloc();
 139         if (!msg)
 140                 return NULL;
 141
 142         if (dump)
 143                 flags |= NLM_F_DUMP;
 144
 145         genlmsg_put(msg, 0, 0, genl_family_get_id(family),
 146                         0, flags, cmd, 0);
 147
 148         NLA_PUT_STRING(msg, WPROBE_ATTR_INTERFACE, ifname);
 149 nla_put_failure:
 150         return msg;
 151 }
 152
 153 static int
 154 wprobe_send_msg(struct nl_msg *msg, void *callback, void *arg)
 155 {
 156         struct nl_cb *cb;
 157         int err = 0;
 158
 159         cb = nl_cb_alloc(NL_CB_DEFAULT);
 160         if (!cb)
 161                 goto out_no_cb;
 162
 163         if (callback)
 164                 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, callback, arg);
 165
 166         err = nl_send_auto_complete(handle, msg);
 167         if (err < 0)
 168                 goto out;
 169
 170         err = 1;
 171
 172         nl_cb_err(cb, NL_CB_CUSTOM, error_handler, &err);
 173         nl_cb_set(cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &err);
 174         nl_cb_set(cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &err);
 175
 176         while (err > 0)
 177                 nl_recvmsgs(handle, cb);
 178
 179 out:
 180         nl_cb_put(cb);
 181 out_no_cb:
 182         nlmsg_free(msg);
 183         return err;
 184 }
 185
 186 struct wprobe_attr_cb {
 187         struct list_head *list;
 188         char *addr;
 189 };
 190
 191 static int
 192 save_attribute_handler(struct nl_msg *msg, void *arg)
 193 {
 194         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 195         const char *name = "N/A";
 196         struct wprobe_attribute *attr;
 197         int type = 0;
 198         struct wprobe_attr_cb *cb = arg;
 199
 200         nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
 201                         genlmsg_attrlen(gnlh, 0), attribute_policy);
 202
 203         if (tb[WPROBE_ATTR_NAME])
 204                 name = nla_data(tb[WPROBE_ATTR_NAME]);
 205
 206         attr = malloc(sizeof(struct wprobe_attribute) + strlen(name) + 1);
 207         if (!attr)
 208                 return -1;
 209
 210         memset(attr, 0, sizeof(struct wprobe_attribute));
 211
 212         if (tb[WPROBE_ATTR_ID])
 213                 attr->id = nla_get_u32(tb[WPROBE_ATTR_ID]);
 214
 215         if (tb[WPROBE_ATTR_MAC] && cb->addr)
 216                 memcpy(cb->addr, nla_data(tb[WPROBE_ATTR_MAC]), 6);
 217
 218         if (tb[WPROBE_ATTR_FLAGS])
 219                 attr->flags = nla_get_u32(tb[WPROBE_ATTR_FLAGS]);
 220
 221         if (tb[WPROBE_ATTR_TYPE])
 222                 type = nla_get_u8(tb[WPROBE_ATTR_TYPE]);
 223
 224         if ((type < WPROBE_VAL_STRING) ||
 225                 (type > WPROBE_VAL_U64))
 226                 type = 0;
 227
 228         attr->type = type;
 229         strcpy(attr->name, name);
 230         INIT_LIST_HEAD(&attr->list);
 231         list_add(&attr->list, cb->list);
 232         return 0;
 233 }
 234
 235
 236 int
 237 wprobe_dump_attributes(const char *ifname, 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(ifname, 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 wprobe_send_msg(msg, save_attribute_handler, &cb);
 252
 253 nla_put_failure:
 254         nlmsg_free(msg);
 255         return -EINVAL;
 256 }
 257
 258 static struct wprobe_link *
 259 get_link(struct list_head *list, const char *addr)
 260 {
 261         struct wprobe_link *l;
 262
 263         list_for_each_entry(l, list, list) {
 264                 if (!memcmp(l->addr, addr, 6)) {
 265                         list_del_init(&l->list);
 266                         goto out;
 267                 }
 268         }
 269
 270         /* no previous link found, allocate a new one */
 271         l = malloc(sizeof(struct wprobe_link));
 272         if (!l)
 273                 goto out;
 274
 275         memset(l, 0, sizeof(struct wprobe_link));
 276         memcpy(l->addr, addr, sizeof(l->addr));
 277         INIT_LIST_HEAD(&l->list);
 278
 279 out:
 280         return l;
 281 }
 282
 283 struct wprobe_save_cb {
 284         struct list_head *list;
 285         struct list_head old_list;
 286 };
 287
 288 static int
 289 save_link_handler(struct nl_msg *msg, void *arg)
 290 {
 291         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 292         struct wprobe_link *link;
 293         struct wprobe_save_cb *cb = arg;
 294         const char *addr;
 295
 296         nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
 297                         genlmsg_attrlen(gnlh, 0), attribute_policy);
 298
 299         if (!tb[WPROBE_ATTR_MAC] || (nla_len(tb[WPROBE_ATTR_MAC]) != 6))
 300                 return -1;
 301
 302         addr = nla_data(tb[WPROBE_ATTR_MAC]);
 303         link = get_link(&cb->old_list, addr);
 304         if (!link)
 305                 return -1;
 306
 307         if (tb[WPROBE_ATTR_FLAGS])
 308                 link->flags = nla_get_u32(tb[WPROBE_ATTR_FLAGS]);
 309
 310         list_add_tail(&link->list, cb->list);
 311         return 0;
 312 }
 313
 314
 315 int
 316 wprobe_update_links(const char *ifname, struct list_head *list)
 317 {
 318         struct wprobe_link *l, *tmp;
 319         struct nl_msg *msg;
 320         struct wprobe_save_cb cb;
 321         int err;
 322
 323         INIT_LIST_HEAD(&cb.old_list);
 324         list_splice_init(list, &cb.old_list);
 325         cb.list = list;
 326
 327         msg = wprobe_new_msg(ifname, WPROBE_CMD_GET_LINKS, true);
 328         if (!msg)
 329                 return -ENOMEM;
 330
 331         err = wprobe_send_msg(msg, save_link_handler, &cb);
 332         if (err < 0)
 333                 return err;
 334
 335         list_for_each_entry_safe(l, tmp, &cb.old_list, list) {
 336                 list_del(&l->list);
 337                 free(l);
 338         }
 339
 340         return 0;
 341 }
 342
 343 void
 344 wprobe_measure(const char *ifname)
 345 {
 346         struct nl_msg *msg;
 347
 348         msg = wprobe_new_msg(ifname, WPROBE_CMD_MEASURE, false);
 349         if (!msg)
 350                 return;
 351
 352         wprobe_send_msg(msg, NULL, NULL);
 353 }
 354
 355 struct wprobe_request_cb {
 356         struct list_head *list;
 357         struct list_head old_list;
 358         char *addr;
 359 };
 360
 361 static int
 362 save_attrdata_handler(struct nl_msg *msg, void *arg)
 363 {
 364         struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
 365         struct wprobe_request_cb *cb = arg;
 366         struct wprobe_attribute *attr;
 367         int type, id;
 368
 369         nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
 370                         genlmsg_attrlen(gnlh, 0), attribute_policy);
 371
 372         if (!tb[WPROBE_ATTR_ID])
 373                 return -1;
 374
 375         if (!tb[WPROBE_ATTR_TYPE])
 376                 return -1;
 377
 378         id = nla_get_u32(tb[WPROBE_ATTR_ID]);
 379         list_for_each_entry(attr, &cb->old_list, list) {
 380                 if (attr->id == id)
 381                         goto found;
 382         }
 383         /* not found */
 384         return -1;
 385
 386 found:
 387         list_del_init(&attr->list);
 388
 389         type = nla_get_u8(tb[WPROBE_ATTR_TYPE]);
 390         if (type != attr->type) {
 391                 DPRINTF("WARNING: type mismatch for %s attribute '%s' (%d != %d)\n",
 392                         (cb->addr ? "link" : "global"),
 393                         attr->name,
 394                         type, attr->type);
 395                 goto out;
 396         }
 397
 398         if ((type < WPROBE_VAL_STRING) ||
 399                 (type > WPROBE_VAL_U64))
 400                 goto out;
 401
 402         memset(&attr->val, 0, sizeof(attr->val));
 403
 404 #define HANDLE_INT_TYPE(_idx, _type) \
 405         case WPROBE_VAL_S##_type: \
 406         case WPROBE_VAL_U##_type: \
 407                 attr->val.U##_type = nla_get_u##_type(tb[_idx]); \
 408                 break
 409
 410         switch(type) {
 411                 HANDLE_INT_TYPE(type, 8);
 412                 HANDLE_INT_TYPE(type, 16);
 413                 HANDLE_INT_TYPE(type, 32);
 414                 HANDLE_INT_TYPE(type, 64);
 415                 case WPROBE_VAL_STRING:
 416                         /* unimplemented */
 417                         break;
 418         }
 419 #undef HANDLE_TYPE
 420
 421         if (attr->flags & WPROBE_F_KEEPSTAT) {
 422                 if (tb[WPROBE_VAL_SUM])
 423                         attr->val.s = nla_get_u64(tb[WPROBE_VAL_SUM]);
 424
 425                 if (tb[WPROBE_VAL_SUM_SQ])
 426                         attr->val.ss = nla_get_u64(tb[WPROBE_VAL_SUM_SQ]);
 427
 428                 if (tb[WPROBE_VAL_SAMPLES])
 429                         attr->val.n = nla_get_u32(tb[WPROBE_VAL_SAMPLES]);
 430
 431                 if (attr->val.n > 0) {
 432                         float avg = ((float) attr->val.s) / attr->val.n;
 433                         float stdev = sqrt((((float) attr->val.ss) / attr->val.n) - (avg * avg));
 434                         attr->val.avg = avg;
 435                         attr->val.stdev = stdev;
 436                 }
 437         }
 438
 439 out:
 440         list_add_tail(&attr->list, cb->list);
 441         return 0;
 442 }
 443
 444
 445 int
 446 wprobe_request_data(const char *ifname, struct list_head *attrs, const unsigned char *addr, int scale)
 447 {
 448         struct wprobe_request_cb cb;
 449         struct nl_msg *msg;
 450         int err;
 451
 452         msg = wprobe_new_msg(ifname, WPROBE_CMD_GET_INFO, true);
 453         if (!msg)
 454                 return -ENOMEM;
 455
 456         if (scale < 0)
 457                 NLA_PUT_U32(msg, WPROBE_ATTR_FLAGS, WPROBE_F_RESET);
 458         else if (scale > 0)
 459                 NLA_PUT_U32(msg, WPROBE_ATTR_SCALE, scale);
 460
 461         if (addr)
 462                 NLA_PUT(msg, WPROBE_ATTR_MAC, 6, addr);
 463
 464 nla_put_failure:
 465         INIT_LIST_HEAD(&cb.old_list);
 466         list_splice_init(attrs, &cb.old_list);
 467         cb.list = attrs;
 468
 469         err = wprobe_send_msg(msg, save_attrdata_handler, &cb);
 470         list_splice(&cb.old_list, attrs->prev);
 471         return err;
 472 }
 473
 474