7ba67cfcb24be0d1272b3d7ee4fcfad9ed302971
[openwrt/openwrt.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 #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 write(socket, &mhdr, sizeof(mhdr));
564
565 memcpy(buf, nlh, buflen);
566 nlh = buf;
567 gnlh = nlmsg_data(nlh);
568 data = genlmsg_data(gnlh);
569 datalen = genlmsg_len(gnlh);
570
571 wprobe_swap_nested(data, datalen, true);
572 swap_genlmsghdr(gnlh);
573 swap_nlmsghdr(nlh);
574 ret = write(socket, buf, buflen);
575 free(buf);
576
577 return ret;
578 }
579
580 static int
581 wprobe_remote_send_msg(struct wprobe_iface *dev, struct nl_msg *msg, wprobe_cb_t callback, void *arg)
582 {
583 struct wprobe_msg_hdr mhdr;
584 int msgs = 0;
585
586 wprobe_msg_to_network(dev->sockfd, msg);
587 nlmsg_free(msg);
588 do {
589 if (read(dev->sockfd, &mhdr, sizeof(mhdr)) != sizeof(mhdr)) {
590 DPRINTF("Failed to read response header\n");
591 return -1;
592 }
593 wprobe_swap_msg_hdr(&mhdr);
594
595 switch(mhdr.status) {
596 case WPROBE_MSG_DATA:
597 if (mhdr.len > WPROBE_MAX_MSGLEN) {
598 fprintf(stderr, "Invalid length in received response message.\n");
599 exit(1);
600 }
601
602 msg = wprobe_msg_from_network(dev->sockfd, mhdr.len);
603 if (!msg)
604 return -EINVAL;
605
606 msgs++;
607 callback(msg, arg);
608 nlmsg_free(msg);
609 break;
610 }
611 } while (mhdr.status != WPROBE_MSG_DONE);
612
613 if (mhdr.error)
614 return -mhdr.error;
615 else
616 return msgs;
617 }
618
619
620 static void
621 wprobe_socket_dev_free(struct wprobe_iface *dev)
622 {
623 if (dev->sockfd >= 0)
624 close(dev->sockfd);
625 }
626
627 static const struct wprobe_iface_ops wprobe_remote_ops = {
628 .send_msg = wprobe_remote_send_msg,
629 .free = wprobe_socket_dev_free,
630 };
631
632
633 #ifndef NO_LOCAL_ACCESS
634 int
635 wprobe_server_init(int socket)
636 {
637 struct wprobe_init_hdr hdr;
638 int ret;
639
640 ret = wprobe_local_init();
641 if (ret != 0)
642 return ret;
643
644 memset(&hdr, 0, sizeof(hdr));
645 memcpy(hdr.pre.magic, WPROBE_MAGIC_STR, sizeof(WPROBE_MAGIC_STR));
646 hdr.pre.version = 0;
647 hdr.v0.genl_family = genl_family_get_id(family);
648 SWAP16(hdr.v0.genl_family);
649 write(socket, (unsigned char *)&hdr, sizeof(hdr));
650
651 return 0;
652 }
653
654 static int
655 wprobe_server_cb(struct nl_msg *msg, void *arg)
656 {
657 int *socket = arg;
658 int ret;
659
660 ret = wprobe_msg_to_network(*socket, msg);
661 if (ret > 0)
662 ret = 0;
663
664 return ret;
665 }
666
667
668 int
669 wprobe_server_handle(int socket)
670 {
671 struct wprobe_msg_hdr mhdr;
672 struct nl_msg *msg;
673 int ret;
674
675 ret = read(socket, &mhdr, sizeof(mhdr));
676 if (ret != sizeof(mhdr)) {
677 if (ret <= 0)
678 return -1;
679
680 DPRINTF("Failed to read request header\n");
681 return -EINVAL;
682 }
683 wprobe_swap_msg_hdr(&mhdr);
684
685 switch(mhdr.status) {
686 case WPROBE_MSG_DATA:
687 if (mhdr.len > WPROBE_MAX_MSGLEN) {
688 DPRINTF("Invalid length in received response message.\n");
689 return -EINVAL;
690 }
691 msg = wprobe_msg_from_network(socket, mhdr.len);
692 break;
693 default:
694 DPRINTF("Invalid request header type\n");
695 return -ENOENT;
696 }
697
698 if (!msg) {
699 DPRINTF("Failed to get message\n");
700 return -EINVAL;
701 }
702
703 ret = wprobe_local_send_msg(NULL, msg, wprobe_server_cb, &socket);
704
705 memset(&mhdr, 0, sizeof(mhdr));
706 mhdr.status = WPROBE_MSG_DONE;
707 if (ret < 0)
708 mhdr.error = (uint16_t) -ret;
709
710 ret = write(socket, (unsigned char *)&mhdr, sizeof(mhdr));
711 if (ret > 0)
712 ret = 0;
713
714 return ret;
715 }
716
717 void
718 wprobe_server_done(void)
719 {
720 wprobe_local_free(NULL);
721 }
722 #endif
723
724 struct wprobe_iface *
725 wprobe_get_from_socket(int socket, const char *name)
726 {
727 struct wprobe_iface *dev;
728 struct wprobe_init_hdr hdr;
729
730 dev = wprobe_alloc_dev();
731 if (!dev)
732 goto out;
733
734 dev->ops = &wprobe_remote_ops;
735 dev->sockfd = socket;
736 dev->ifname = strdup(name);
737
738 /* read version and header length */
739 if (read(socket, &hdr.pre, sizeof(hdr.pre)) != sizeof(hdr.pre)) {
740 DPRINTF("Could not read header\n");
741 goto error;
742 }
743
744 /* magic not found */
745 if (memcmp(hdr.pre.magic, WPROBE_MAGIC_STR, sizeof(hdr.pre.magic)) != 0) {
746 DPRINTF("Magic does not match\n");
747 goto error;
748 }
749
750 /* unsupported version */
751 if (hdr.pre.version != 0) {
752 DPRINTF("Protocol version does not match\n");
753 goto error;
754 }
755
756 if (read(socket, &hdr.v0, sizeof(hdr.v0)) != sizeof(hdr.v0)) {
757 DPRINTF("Could not read header data\n");
758 goto error;
759 }
760
761 SWAP16(hdr.pre.extra);
762 SWAP16(hdr.v0.genl_family);
763 dev->genl_family = hdr.v0.genl_family;
764
765 if (wprobe_init_dev(dev) < 0) {
766 DPRINTF("Could not initialize device\n");
767 goto error;
768 }
769
770 out:
771 return dev;
772
773 error:
774 wprobe_free_dev(dev);
775 return NULL;
776 }
777
778 struct wprobe_iface *
779 wprobe_get_auto(const char *arg, char **err)
780 {
781 static struct sockaddr_in sa;
782 static char errbuf[512];
783
784 struct wprobe_iface *dev = NULL;
785 struct hostent *h;
786 char *devstr = strdup(arg);
787 char *sep = NULL;
788 int sock = -1;
789 int len;
790
791 if (err)
792 *err = NULL;
793
794 sep = strchr(devstr, ':');
795 if (!sep) {
796 #ifndef NO_LOCAL_ACCESS
797 free(devstr);
798 return wprobe_get_dev(arg);
799 #else
800 if (err)
801 *err = "Invalid argument";
802 goto out;
803 #endif
804 }
805
806 *sep = 0;
807 sep++;
808
809 sock = socket(AF_INET, SOCK_STREAM, 0);
810 if (sock < 0)
811 goto syserr;
812
813 h = gethostbyname(devstr);
814 if (!h) {
815 sprintf(errbuf, "Host not found");
816 goto out_err;
817 }
818
819 memcpy(&sa.sin_addr, h->h_addr, h->h_length);
820 sa.sin_family = AF_INET;
821 sa.sin_port = htons(wprobe_port);
822 if (connect(sock, (struct sockaddr *)&sa, sizeof(sa)) < 0)
823 goto syserr;
824
825 dev = wprobe_get_from_socket(sock, sep);
826 if (!dev) {
827 sprintf(errbuf, "wprobe connection initialization failed");
828 goto out_err;
829 }
830 goto out;
831
832 syserr:
833 if (err) {
834 strcpy(errbuf, "Connection failed: ");
835 len = strlen(errbuf);
836 strerror_r(errno, errbuf + len, sizeof(errbuf) - len - 1);
837 }
838 out_err:
839 if (err)
840 *err = errbuf;
841 if (sock >= 0)
842 close(sock);
843 out:
844 if (devstr)
845 free(devstr);
846 return dev;
847 }
848
849 static void
850 free_attr_list(struct list_head *list)
851 {
852 struct wprobe_attribute *attr, *tmp;
853
854 list_for_each_entry_safe(attr, tmp, list, list) {
855 list_del(&attr->list);
856 free(attr);
857 }
858 }
859
860 void
861 wprobe_free_dev(struct wprobe_iface *dev)
862 {
863 if (dev->ops->free)
864 dev->ops->free(dev);
865 free_attr_list(&dev->global_attr);
866 free_attr_list(&dev->link_attr);
867 free((void *)dev->ifname);
868 free(dev);
869 }
870
871 static struct wprobe_link *
872 get_link(struct list_head *list, const char *addr)
873 {
874 struct wprobe_link *l;
875
876 list_for_each_entry(l, list, list) {
877 if (!memcmp(l->addr, addr, 6)) {
878 list_del_init(&l->list);
879 goto out;
880 }
881 }
882
883 /* no previous link found, allocate a new one */
884 l = malloc(sizeof(struct wprobe_link));
885 if (!l)
886 goto out;
887
888 memset(l, 0, sizeof(struct wprobe_link));
889 memcpy(l->addr, addr, sizeof(l->addr));
890 INIT_LIST_HEAD(&l->list);
891
892 out:
893 return l;
894 }
895
896 struct wprobe_save_cb {
897 struct list_head *list;
898 struct list_head old_list;
899 };
900
901 static int
902 save_link_handler(struct nl_msg *msg, void *arg)
903 {
904 struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
905 struct wprobe_link *link;
906 struct wprobe_save_cb *cb = arg;
907 const char *addr;
908
909 nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
910 genlmsg_attrlen(gnlh, 0), attribute_policy);
911
912 if (!tb[WPROBE_ATTR_MAC] || (nla_len(tb[WPROBE_ATTR_MAC]) != 6))
913 return -1;
914
915 addr = nla_data(tb[WPROBE_ATTR_MAC]);
916 link = get_link(&cb->old_list, addr);
917 if (!link)
918 return -1;
919
920 if (tb[WPROBE_ATTR_FLAGS])
921 link->flags = nla_get_u32(tb[WPROBE_ATTR_FLAGS]);
922
923 list_add_tail(&link->list, cb->list);
924 return 0;
925 }
926
927
928 int
929 wprobe_update_links(struct wprobe_iface *dev)
930 {
931 struct wprobe_link *l, *tmp;
932 struct nl_msg *msg;
933 struct wprobe_save_cb cb;
934 int err;
935
936 INIT_LIST_HEAD(&cb.old_list);
937 list_splice_init(&dev->links, &cb.old_list);
938 cb.list = &dev->links;
939
940 msg = wprobe_new_msg(dev, WPROBE_CMD_GET_LINKS, true);
941 if (!msg)
942 return -ENOMEM;
943
944 err = dev->ops->send_msg(dev, msg, save_link_handler, &cb);
945 if (err < 0)
946 return err;
947
948 list_for_each_entry_safe(l, tmp, &cb.old_list, list) {
949 list_del(&l->list);
950 free(l);
951 }
952
953 return 0;
954 }
955
956
957 struct wprobe_filter_data
958 {
959 wprobe_filter_cb cb;
960 void *arg;
961 struct wprobe_filter_item *buf;
962 int buflen;
963 };
964
965 static int
966 dump_filter_handler(struct nl_msg *msg, void *arg)
967 {
968 struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
969 struct wprobe_filter_data *data = arg;
970 struct nlattr *p;
971 const char *name;
972 int count = 0;
973 int len;
974
975 nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
976 genlmsg_attrlen(gnlh, 0), attribute_policy);
977
978 if (!tb[WPROBE_ATTR_NAME] || !tb[WPROBE_ATTR_FILTER_GROUP])
979 return -1;
980
981 name = nla_data(tb[WPROBE_ATTR_NAME]);
982 nla_for_each_nested(p, tb[WPROBE_ATTR_FILTER_GROUP], len) {
983 count++;
984 }
985
986 if (data->buflen < count) {
987 if (data->buf)
988 free(data->buf);
989 data->buflen = count;
990 data->buf = malloc(sizeof(struct wprobe_filter_item) * count);
991 memset(data->buf, 0, sizeof(struct wprobe_filter_item) * count);
992 }
993
994 count = 0;
995 nla_for_each_nested(p, tb[WPROBE_ATTR_FILTER_GROUP], len) {
996 struct wprobe_filter_item *fi;
997
998 nla_parse(tb, WPROBE_ATTR_LAST, nla_data(p),
999 nla_len(p), attribute_policy);
1000
1001 if (!tb[WPROBE_ATTR_NAME] || !tb[WPROBE_ATTR_RXCOUNT]
1002 || !tb[WPROBE_ATTR_TXCOUNT])
1003 continue;
1004
1005 fi = &data->buf[count++];
1006 strncpy(fi->name, nla_data(tb[WPROBE_ATTR_NAME]), sizeof(fi->name) - 1);
1007 fi->name[sizeof(fi->name) - 1] = 0;
1008 fi->rx = nla_get_u64(tb[WPROBE_ATTR_RXCOUNT]);
1009 fi->tx = nla_get_u64(tb[WPROBE_ATTR_TXCOUNT]);
1010 }
1011 data->cb(data->arg, name, data->buf, count);
1012
1013 return 0;
1014 }
1015
1016 int
1017 wprobe_dump_filters(struct wprobe_iface *dev, wprobe_filter_cb cb, void *arg)
1018 {
1019 struct wprobe_filter_data data;
1020 struct nl_msg *msg;
1021 int err;
1022
1023 data.buf = 0;
1024 data.buflen = 0;
1025 data.cb = cb;
1026 data.arg = arg;
1027
1028 msg = wprobe_new_msg(dev, WPROBE_CMD_GET_FILTER, true);
1029 if (!msg)
1030 return -ENOMEM;
1031
1032 err = dev->ops->send_msg(dev, msg, dump_filter_handler, &data);
1033 if (err < 0)
1034 return err;
1035
1036 return 0;
1037 }
1038
1039 int
1040 wprobe_apply_config(struct wprobe_iface *dev)
1041 {
1042 struct nl_msg *msg;
1043
1044 msg = wprobe_new_msg(dev, WPROBE_CMD_CONFIG, false);
1045 if (!msg)
1046 return -ENOMEM;
1047
1048 if (dev->interval >= 0)
1049 NLA_PUT_MSECS(msg, WPROBE_ATTR_INTERVAL, dev->interval);
1050
1051 if (dev->filter_len < 0) {
1052 NLA_PUT(msg, WPROBE_ATTR_FILTER, 0, NULL);
1053 dev->filter_len = 0;
1054 } else if (dev->filter && dev->filter_len > 0) {
1055 NLA_PUT(msg, WPROBE_ATTR_FILTER, dev->filter_len, dev->filter);
1056 }
1057 dev->filter = NULL;
1058
1059 dev->ops->send_msg(dev, msg, NULL, NULL);
1060 return 0;
1061
1062 nla_put_failure:
1063 nlmsg_free(msg);
1064 return -ENOMEM;
1065 }
1066
1067 int
1068 wprobe_measure(struct wprobe_iface *dev)
1069 {
1070 struct nl_msg *msg;
1071
1072 msg = wprobe_new_msg(dev, WPROBE_CMD_MEASURE, false);
1073 if (!msg)
1074 return -ENOMEM;
1075
1076 dev->ops->send_msg(dev, msg, NULL, NULL);
1077 return 0;
1078 }
1079
1080 struct wprobe_request_cb {
1081 struct list_head *list;
1082 struct list_head old_list;
1083 char *addr;
1084 };
1085
1086 static int
1087 save_attrdata_handler(struct nl_msg *msg, void *arg)
1088 {
1089 struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
1090 struct wprobe_request_cb *cb = arg;
1091 struct wprobe_attribute *attr;
1092 int type, id;
1093
1094 nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
1095 genlmsg_attrlen(gnlh, 0), attribute_policy);
1096
1097 if (!tb[WPROBE_ATTR_ID])
1098 return -1;
1099
1100 if (!tb[WPROBE_ATTR_TYPE])
1101 return -1;
1102
1103 id = nla_get_u32(tb[WPROBE_ATTR_ID]);
1104 list_for_each_entry(attr, &cb->old_list, list) {
1105 if (attr->id == id)
1106 goto found;
1107 }
1108 /* not found */
1109 return -1;
1110
1111 found:
1112 list_del_init(&attr->list);
1113
1114 type = nla_get_u8(tb[WPROBE_ATTR_TYPE]);
1115 if (type != attr->type) {
1116 DPRINTF("WARNING: type mismatch for %s attribute '%s' (%d != %d)\n",
1117 (cb->addr ? "link" : "global"),
1118 attr->name,
1119 type, attr->type);
1120 goto out;
1121 }
1122
1123 if ((type < WPROBE_VAL_STRING) ||
1124 (type > WPROBE_VAL_U64))
1125 goto out;
1126
1127 memset(&attr->val, 0, sizeof(attr->val));
1128
1129 #define HANDLE_INT_TYPE(_idx, _type) \
1130 case WPROBE_VAL_S##_type: \
1131 case WPROBE_VAL_U##_type: \
1132 attr->val.U##_type = nla_get_u##_type(tb[_idx]); \
1133 break
1134
1135 switch(type) {
1136 HANDLE_INT_TYPE(type, 8);
1137 HANDLE_INT_TYPE(type, 16);
1138 HANDLE_INT_TYPE(type, 32);
1139 HANDLE_INT_TYPE(type, 64);
1140 case WPROBE_VAL_STRING:
1141 /* unimplemented */
1142 break;
1143 }
1144 #undef HANDLE_TYPE
1145
1146 if (attr->flags & WPROBE_F_KEEPSTAT) {
1147 if (tb[WPROBE_VAL_SUM])
1148 attr->val.s = nla_get_u64(tb[WPROBE_VAL_SUM]);
1149
1150 if (tb[WPROBE_VAL_SUM_SQ])
1151 attr->val.ss = nla_get_u64(tb[WPROBE_VAL_SUM_SQ]);
1152
1153 if (tb[WPROBE_VAL_SAMPLES])
1154 attr->val.n = nla_get_u32(tb[WPROBE_VAL_SAMPLES]);
1155
1156 if (attr->val.n > 0) {
1157 float avg = ((float) attr->val.s) / attr->val.n;
1158 float stdev = sqrt((((float) attr->val.ss) / attr->val.n) - (avg * avg));
1159 if (isnan(stdev))
1160 stdev = 0.0f;
1161 if (isnan(avg))
1162 avg = 0.0f;
1163 attr->val.avg = avg;
1164 attr->val.stdev = stdev;
1165 }
1166 }
1167
1168 out:
1169 list_add_tail(&attr->list, cb->list);
1170 return 0;
1171 }
1172
1173
1174 int
1175 wprobe_request_data(struct wprobe_iface *dev, const unsigned char *addr)
1176 {
1177 struct wprobe_request_cb cb;
1178 struct list_head *attrs;
1179 struct nl_msg *msg;
1180 int err;
1181
1182 msg = wprobe_new_msg(dev, WPROBE_CMD_GET_INFO, true);
1183 if (!msg)
1184 return -ENOMEM;
1185
1186 if (addr) {
1187 attrs = &dev->link_attr;
1188 NLA_PUT(msg, WPROBE_ATTR_MAC, 6, addr);
1189 } else {
1190 attrs = &dev->global_attr;
1191 }
1192
1193 INIT_LIST_HEAD(&cb.old_list);
1194 list_splice_init(attrs, &cb.old_list);
1195 cb.list = attrs;
1196
1197 err = dev->ops->send_msg(dev, msg, save_attrdata_handler, &cb);
1198 list_splice(&cb.old_list, attrs->prev);
1199 return err;
1200
1201 nla_put_failure:
1202 nlmsg_free(msg);
1203 return -ENOMEM;
1204 }
1205
1206