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

Archive Download this file

Branches:
backfire
history
master
release_2010-11-17
release_2010-12-14
release_2011-02-23
release_2011-05-28
release_2011-08-27
release_2011-11-13
release_2012-03-18
release_2012-04-09
release_2012-10-11
uclibc_nptl



interactive