2 * wprobe.c: Wireless probe user space library
3 * Copyright (C) 2008-2009 Felix Fietkau <nbd@openwrt.org>
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.
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.
21 #include <sys/types.h>
22 #include <sys/socket.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>
33 #define DPRINTF(fmt, ...) fprintf(stderr, "%s(%d): " fmt, __func__, __LINE__, ##__VA_ARGS__)
35 #define DPRINTF(fmt, ...) do {} while (0)
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 },
62 error_handler(struct sockaddr_nl *nla, struct nlmsgerr *err, void *arg)
70 finish_handler(struct nl_msg *msg, void *arg)
78 ack_handler(struct nl_msg *msg, void *arg)
92 nl_socket_free(handle);
102 handle = nl_socket_alloc();
104 DPRINTF("Failed to create handle\n");
108 if (genl_connect(handle)) {
109 DPRINTF("Failed to connect to generic netlink\n");
113 ret = genl_ctrl_alloc_cache(handle, &cache);
115 DPRINTF("Failed to allocate netlink cache\n");
119 family = genl_ctrl_search_by_name(cache, "wprobe");
121 DPRINTF("wprobe API not present\n");
132 static struct nl_msg *
133 wprobe_new_msg(const char *ifname, int cmd, bool dump)
145 genlmsg_put(msg, 0, 0, genl_family_get_id(family),
148 NLA_PUT_STRING(msg, WPROBE_ATTR_INTERFACE, ifname);
154 wprobe_send_msg(struct nl_msg *msg, void *callback, void *arg)
159 cb = nl_cb_alloc(NL_CB_DEFAULT);
164 nl_cb_set(cb, NL_CB_VALID, NL_CB_CUSTOM, callback, arg);
166 err = nl_send_auto_complete(handle, msg);
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);
177 nl_recvmsgs(handle, cb);
186 struct wprobe_attr_cb {
187 struct list_head *list;
192 save_attribute_handler(struct nl_msg *msg, void *arg)
194 struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
195 const char *name = "N/A";
196 struct wprobe_attribute *attr;
198 struct wprobe_attr_cb *cb = arg;
200 nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
201 genlmsg_attrlen(gnlh, 0), attribute_policy);
203 if (tb[WPROBE_ATTR_NAME])
204 name = nla_data(tb[WPROBE_ATTR_NAME]);
206 attr = malloc(sizeof(struct wprobe_attribute) + strlen(name) + 1);
210 memset(attr, 0, sizeof(struct wprobe_attribute));
212 if (tb[WPROBE_ATTR_ID])
213 attr->id = nla_get_u32(tb[WPROBE_ATTR_ID]);
215 if (tb[WPROBE_ATTR_MAC] && cb->addr)
216 memcpy(cb->addr, nla_data(tb[WPROBE_ATTR_MAC]), 6);
218 if (tb[WPROBE_ATTR_FLAGS])
219 attr->flags = nla_get_u32(tb[WPROBE_ATTR_FLAGS]);
221 if (tb[WPROBE_ATTR_TYPE])
222 type = nla_get_u8(tb[WPROBE_ATTR_TYPE]);
224 if ((type < WPROBE_VAL_STRING) ||
225 (type > WPROBE_VAL_U64))
229 strcpy(attr->name, name);
230 INIT_LIST_HEAD(&attr->list);
231 list_add(&attr->list, cb->list);
237 wprobe_dump_attributes(const char *ifname, bool link, struct list_head *list, char *addr)
240 struct wprobe_attr_cb cb;
244 msg = wprobe_new_msg(ifname, WPROBE_CMD_GET_LIST, true);
249 NLA_PUT(msg, WPROBE_ATTR_MAC, 6, "\x00\x00\x00\x00\x00\x00");
251 return wprobe_send_msg(msg, save_attribute_handler, &cb);
258 static struct wprobe_link *
259 get_link(struct list_head *list, const char *addr)
261 struct wprobe_link *l;
263 list_for_each_entry(l, list, list) {
264 if (!memcmp(l->addr, addr, 6)) {
265 list_del_init(&l->list);
270 /* no previous link found, allocate a new one */
271 l = malloc(sizeof(struct wprobe_link));
275 memset(l, 0, sizeof(struct wprobe_link));
276 memcpy(l->addr, addr, sizeof(l->addr));
277 INIT_LIST_HEAD(&l->list);
283 struct wprobe_save_cb {
284 struct list_head *list;
285 struct list_head old_list;
289 save_link_handler(struct nl_msg *msg, void *arg)
291 struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
292 struct wprobe_link *link;
293 struct wprobe_save_cb *cb = arg;
296 nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
297 genlmsg_attrlen(gnlh, 0), attribute_policy);
299 if (!tb[WPROBE_ATTR_MAC] || (nla_len(tb[WPROBE_ATTR_MAC]) != 6))
302 addr = nla_data(tb[WPROBE_ATTR_MAC]);
303 link = get_link(&cb->old_list, addr);
307 if (tb[WPROBE_ATTR_FLAGS])
308 link->flags = nla_get_u32(tb[WPROBE_ATTR_FLAGS]);
310 list_add_tail(&link->list, cb->list);
316 wprobe_update_links(const char *ifname, struct list_head *list)
318 struct wprobe_link *l, *tmp;
320 struct wprobe_save_cb cb;
323 INIT_LIST_HEAD(&cb.old_list);
324 list_splice_init(list, &cb.old_list);
327 msg = wprobe_new_msg(ifname, WPROBE_CMD_GET_LINKS, true);
331 err = wprobe_send_msg(msg, save_link_handler, &cb);
335 list_for_each_entry_safe(l, tmp, &cb.old_list, list) {
344 wprobe_measure(const char *ifname)
348 msg = wprobe_new_msg(ifname, WPROBE_CMD_MEASURE, false);
352 wprobe_send_msg(msg, NULL, NULL);
355 struct wprobe_request_cb {
356 struct list_head *list;
357 struct list_head old_list;
362 save_attrdata_handler(struct nl_msg *msg, void *arg)
364 struct genlmsghdr *gnlh = nlmsg_data(nlmsg_hdr(msg));
365 struct wprobe_request_cb *cb = arg;
366 struct wprobe_attribute *attr;
369 nla_parse(tb, WPROBE_ATTR_LAST, genlmsg_attrdata(gnlh, 0),
370 genlmsg_attrlen(gnlh, 0), attribute_policy);
372 if (!tb[WPROBE_ATTR_ID])
375 if (!tb[WPROBE_ATTR_TYPE])
378 id = nla_get_u32(tb[WPROBE_ATTR_ID]);
379 list_for_each_entry(attr, &cb->old_list, list) {
387 list_del_init(&attr->list);
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"),
398 if ((type < WPROBE_VAL_STRING) ||
399 (type > WPROBE_VAL_U64))
402 memset(&attr->val, 0, sizeof(attr->val));
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]); \
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:
421 if (attr->flags & WPROBE_F_KEEPSTAT) {
422 if (tb[WPROBE_VAL_SUM])
423 attr->val.s = nla_get_u64(tb[WPROBE_VAL_SUM]);
425 if (tb[WPROBE_VAL_SUM_SQ])
426 attr->val.ss = nla_get_u64(tb[WPROBE_VAL_SUM_SQ]);
428 if (tb[WPROBE_VAL_SAMPLES])
429 attr->val.n = nla_get_u32(tb[WPROBE_VAL_SAMPLES]);
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));
435 attr->val.stdev = stdev;
440 list_add_tail(&attr->list, cb->list);
446 wprobe_request_data(const char *ifname, struct list_head *attrs, const unsigned char *addr, int scale)
448 struct wprobe_request_cb cb;
452 msg = wprobe_new_msg(ifname, WPROBE_CMD_GET_INFO, true);
457 NLA_PUT_U32(msg, WPROBE_ATTR_FLAGS, WPROBE_F_RESET);
459 NLA_PUT_U32(msg, WPROBE_ATTR_SCALE, scale);
462 NLA_PUT(msg, WPROBE_ATTR_MAC, 6, addr);
465 INIT_LIST_HEAD(&cb.old_list);
466 list_splice_init(attrs, &cb.old_list);
469 err = wprobe_send_msg(msg, save_attrdata_handler, &cb);
470 list_splice(&cb.old_list, attrs->prev);