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