upgrade to the new version of wprobe - includes reconfigurable layer 2 statistics...
[openwrt/svn-archive/archive.git] / 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