linux: put #ifdef __linux__ around code
[olsrd.git] / src / linux / nl80211_link_info.c
1
2 /*
3  * The olsr.org Optimized Link-State Routing daemon(olsrd)
4  * Copyright (c) 2012 Fox-IT B.V. <opensource@fox-it.com>
5  * All rights reserved.
6  *
7  * Redistribution and use in source and binary forms, with or without
8  * modification, are permitted provided that the following conditions
9  * are met:
10  *
11  * * Redistributions of source code must retain the above copyright
12  *   notice, this list of conditions and the following disclaimer.
13  * * Redistributions in binary form must reproduce the above copyright
14  *   notice, this list of conditions and the following disclaimer in
15  *   the documentation and/or other materials provided with the
16  *   distribution.
17  * * Neither the name of olsr.org, olsrd nor the names of its
18  *   contributors may be used to endorse or promote products derived
19  *   from this software without specific prior written permission.
20  *
21  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31  * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32  * POSSIBILITY OF SUCH DAMAGE.
33  *
34  * Visit http://www.olsr.org for more information.
35  *
36  * If you find this software useful feel free to make a donation
37  * to the project. For more information see the website or contact
38  * the copyright holders.
39  *
40  */
41
42 #ifdef __linux__
43 #ifdef LINUX_NL80211 /* Optional - not supported on all platforms */
44
45 #include <stdlib.h>
46 #include <stdbool.h>
47
48 #include <net/if.h>
49 #include <net/ethernet.h>
50 #include <netinet/ether.h>
51 #include <linux/nl80211.h>
52 #include <linux/if_ether.h>
53
54 #include <netlink/netlink.h>
55 #include <netlink/genl/genl.h>
56 #include <netlink/genl/ctrl.h>
57 #include <netlink/route/addr.h>
58 #include <netlink/route/neighbour.h>
59
60 #include "nl80211_link_info.h"
61 #include "lq_plugin_ffeth_nl80211.h"
62 #include "interfaces.h"
63 #include "olsr.h"
64 #include "log.h"
65 #include "fpm.h"
66
67
68 // Static values for testing
69 #define REFERENCE_BANDWIDTH_MBIT_SEC 54
70
71 #if !defined(CONFIG_LIBNL20) && !defined(CONFIG_LIBNL30)
72 #define nl_sock nl_handle
73 static inline struct nl_handle *nl_socket_alloc(void)
74 {
75         return nl_handle_alloc();
76 }
77
78 static inline void nl_socket_free(struct nl_sock *sock)
79 {
80         nl_handle_destroy(sock);
81 }
82 #endif
83
84 #define ASSERT_NOT_NULL(PARAM) do { \
85                 if ((PARAM) == NULL) { \
86                         olsr_exit("Pointer value for '" #PARAM "' cannot be NULL", EXIT_FAILURE); \
87                 } \
88         } while (0)
89
90 struct nl80211_link_info_context {
91         int *finish;
92         struct lq_nl80211_data **nl80211;
93 };
94
95 static int netlink_id = 0;
96 static struct nl_sock *gen_netlink_socket = NULL; // Socket for NL80211
97 static struct nl_sock *rt_netlink_socket = NULL; // Socket for ARP cache
98
99
100 /**
101  * Opens two netlink connections to the Linux kernel. One connection to retreive
102  * wireless 802.11 information and one for querying the ARP cache.
103  */
104 static void connect_netlink(void) {
105         if ((gen_netlink_socket = nl_socket_alloc()) == NULL) {
106                 olsr_exit("Failed allocating memory for netlink socket", EXIT_FAILURE);
107         }
108
109         if (genl_connect(gen_netlink_socket) != 0) {
110                 olsr_exit("Failed to connect with generic netlink", EXIT_FAILURE);
111         }
112         
113         if ((netlink_id = genl_ctrl_resolve(gen_netlink_socket, "nl80211")) < 0) {
114                 olsr_exit("Failed to resolve netlink nl80211 module", EXIT_FAILURE);
115         }
116
117         if ((rt_netlink_socket = nl_socket_alloc()) == NULL) {
118                 olsr_exit("Failed allocating memory for netlink socket", EXIT_FAILURE);
119         }
120
121         if ((nl_connect(rt_netlink_socket, NETLINK_ROUTE)) != 0) {
122                 olsr_exit("Failed to connect with NETLINK_ROUTE", EXIT_FAILURE);
123         }
124 }
125
126 static int parse_nl80211_message(struct nl_msg *msg, void *arg) {
127         struct nl80211_link_info_context *context = (struct nl80211_link_info_context *) arg;
128         struct genlmsghdr *header = nlmsg_data(nlmsg_hdr(msg));
129         struct nlattr *attributes[NL80211_ATTR_MAX + 1];
130         struct nlattr *station_info[NL80211_STA_INFO_MAX + 1];
131         struct nlattr *rate_info[NL80211_RATE_INFO_MAX + 1];
132         struct lq_nl80211_data *lq_data = NULL;
133         uint8_t signal;
134         uint16_t bandwidth;
135
136         static struct nla_policy station_attr_policy[NL80211_STA_INFO_MAX + 1] = {
137                 [NL80211_STA_INFO_INACTIVE_TIME] = { .type = NLA_U32 }, // Last activity from remote station (msec)
138                 [NL80211_STA_INFO_SIGNAL] = { .type = NLA_U8 }, // Signal strength of last received PPDU (dBm)
139                 [NL80211_STA_INFO_TX_BITRATE] = { .type = NLA_NESTED }, // Transmit bitrate to this station
140         };
141         static struct nla_policy station_rate_policy[NL80211_RATE_INFO_MAX + 1] = {
142                 [NL80211_RATE_INFO_BITRATE] = { .type = NLA_U16 }, // Bitrate (100kbit/s)
143         };
144
145         ASSERT_NOT_NULL(msg);
146         ASSERT_NOT_NULL(context);
147
148         if (nla_parse(attributes, NL80211_ATTR_MAX, genlmsg_attrdata(header, 0), genlmsg_attrlen(header, 0), NULL) != 0) {
149                 *(context->finish) = 1;
150                 return NL_STOP;
151         }
152
153         if (!attributes[NL80211_ATTR_STA_INFO]) {
154                 olsr_syslog(OLSR_LOG_INFO, "Did not receive station info in netlink reply");
155                 return NL_SKIP;
156         }
157
158         if (nla_parse_nested(station_info, NL80211_STA_INFO_MAX, attributes[NL80211_ATTR_STA_INFO],
159                                 station_attr_policy) < 0) {
160                 *(context->finish) = 1;
161                 return NL_STOP;
162         }
163         if (nla_parse_nested(rate_info, NL80211_RATE_INFO_MAX, station_info[NL80211_STA_INFO_TX_BITRATE],
164                                 station_rate_policy) < 0) {
165                 *(context->finish) = 1;
166                 return NL_STOP;
167         }
168
169         if (nla_len(attributes[NL80211_ATTR_MAC]) != ETHER_ADDR_LEN) {
170                 olsr_syslog(OLSR_LOG_ERR, "Attribute NL80211_ATTR_MAC length is not equal to ETHER_ADDR_LEN");
171                 *(context->finish) = 1;
172                 return NL_STOP;
173         }
174
175         if (station_info[NL80211_STA_INFO_SIGNAL]) {
176                 signal = nla_get_u8(station_info[NL80211_STA_INFO_SIGNAL]);
177         }
178         if (rate_info[NL80211_RATE_INFO_BITRATE]) {
179                 bandwidth = nla_get_u16(rate_info[NL80211_RATE_INFO_BITRATE]);
180         }
181
182         if (bandwidth != 0 || signal != 0) {
183                 lq_data = olsr_malloc(sizeof(struct lq_nl80211_data), "new lq_nl80211_data struct");
184                 memcpy(lq_data->mac, nla_data(attributes[NL80211_ATTR_MAC]), ETHER_ADDR_LEN);
185                 lq_data->signal = signal;
186                 lq_data->bandwidth = bandwidth;
187                 lq_data->next = NULL;
188
189                 if (context->nl80211 == NULL) { // Linked list does not exist yet
190                         *(context->nl80211) = lq_data;
191                 } else { // Append to head of linked list
192                         lq_data->next = *(context->nl80211);
193                         *(context->nl80211) = lq_data;
194                 }
195         }
196
197         return NL_SKIP;
198 }
199
200 static int error_handler(struct sockaddr_nl __attribute__ ((unused)) *nla, struct nlmsgerr __attribute__ ((unused)) *err, void *arg) {
201         int *finish = arg;
202
203         ASSERT_NOT_NULL(arg);
204
205         *finish = 1;
206         return NL_STOP;
207 }
208
209 static int finish_handler(struct nl_msg __attribute__ ((unused)) *msg, void *arg) {
210         int *finish = arg;
211
212         ASSERT_NOT_NULL(arg);
213
214         *finish = 1;
215         return NL_SKIP;
216 }
217
218 static int ack_handler(struct nl_msg __attribute__ ((unused)) *nla, void *arg) {
219         int *finish = arg;
220
221         ASSERT_NOT_NULL(arg);
222
223         *finish = 1;
224         return NL_STOP;
225 }
226
227 /**
228  * Requests the NL80211 data for a specific interface.
229  *
230  * @param iface         Interface to get all the NL80211 station information for.
231  * @param nl80211       Pointer to linked list with station information. If set to NULL
232  *                                      the pointer will point to a new linked list, if there was any
233  *                                      information retreived.
234  */
235 static void nl80211_link_info_for_interface(struct interface *iface, struct lq_nl80211_data **nl80211) {
236         int finish = 0;
237         struct nl_msg *request_message = NULL;
238         struct nl_cb *request_cb = NULL;
239         struct nl80211_link_info_context link_context = { &finish, nl80211 };
240
241         ASSERT_NOT_NULL(iface);
242
243         if (! iface->is_wireless) {
244                 // Remove in production code
245                 olsr_syslog(OLSR_LOG_INFO, "Link entry %s is not a wireless link", iface->int_name);
246                 return;
247         }
248
249         if ((request_message = nlmsg_alloc()) == NULL) {
250                 olsr_exit("Failed to allocate nl_msg struct", EXIT_FAILURE);
251         }
252
253         genlmsg_put(request_message, NL_AUTO_PID, NL_AUTO_SEQ, netlink_id, 0, NLM_F_DUMP, NL80211_CMD_GET_STATION, 0);
254
255         if (nla_put_u32(request_message, NL80211_ATTR_IFINDEX, iface->if_index) == -1) {
256                 olsr_syslog(OLSR_LOG_ERR, "Failed to add interface index to netlink message");
257                 exit(1);
258         }
259
260 #ifdef NL_DEBUG
261         if ((request_cb = nl_cb_alloc(NL_CB_DEBUG)) == NULL) {
262 #else
263         if ((request_cb = nl_cb_alloc(NL_CB_DEFAULT)) == NULL) {
264 #endif
265                 olsr_exit("Failed to alloc nl_cb struct", EXIT_FAILURE);
266         }
267
268         if (nl_cb_set(request_cb, NL_CB_VALID, NL_CB_CUSTOM, parse_nl80211_message, &link_context) != 0) {
269                 olsr_syslog(OLSR_LOG_ERR, "Failed to set netlink message callback");
270                 exit(1);
271         }
272
273         nl_cb_err(request_cb, NL_CB_CUSTOM, error_handler, &finish);
274         nl_cb_set(request_cb, NL_CB_FINISH, NL_CB_CUSTOM, finish_handler, &finish);
275         nl_cb_set(request_cb, NL_CB_ACK, NL_CB_CUSTOM, ack_handler, &finish);
276
277         if (nl_send_auto_complete(gen_netlink_socket, request_message) < 0) {
278                 olsr_syslog(OLSR_LOG_ERR, "Failed sending the request message with netlink");
279                 exit(1);
280         }
281
282         while (! finish) {
283                 nl_recvmsgs(gen_netlink_socket, request_cb);
284         }
285
286         nlmsg_free(request_message);
287 }
288
289 /**
290  * Uses the linux ARP cache to find a MAC address for a neighbor. Does not do
291  * actual ARP if it's not found in the cache.
292  *
293  * @param link          Neighbor to find MAC address of.
294  * @param mac           Pointer to buffer of size ETHER_ADDR_LEN that will be
295  *                                      used to write MAC address in (if found).
296  * @returns                     True if MAC address is found.
297  */
298 static bool mac_of_neighbor(struct link_entry *link, unsigned char *mac) {
299         bool success = false;
300         struct nl_cache *cache = NULL;
301         struct rtnl_neigh *neighbor = NULL;
302         struct nl_addr *neighbor_addr_filter = NULL;
303         struct nl_addr *neighbor_mac_addr = NULL;
304         void *addr = NULL;
305         size_t addr_size = 0;
306
307         if (olsr_cnf->ip_version == AF_INET6) {
308                 addr = &(link->neighbor_iface_addr.v6);
309                 addr_size = sizeof(struct in6_addr);
310         } else {
311                 addr = &(link->neighbor_iface_addr.v4);
312                 addr_size = sizeof(struct in_addr);
313         }
314
315         if ((neighbor_addr_filter = nl_addr_build(olsr_cnf->ip_version, addr, addr_size)) == NULL) {
316                 olsr_syslog(OLSR_LOG_ERR, "Failed to build nl_addr struct from link ip address");
317                 goto cleanup;
318         }
319
320 #if !defined(CONFIG_LIBNL20) && !defined(CONFIG_LIBNL30)
321         if ((cache = rtnl_neigh_alloc_cache(rt_netlink_socket)) == NULL) {
322 #else
323         if (rtnl_neigh_alloc_cache(rt_netlink_socket, &cache) != 0) {
324 #endif
325                 olsr_syslog(OLSR_LOG_ERR, "Failed to allocate netlink neighbor cache");
326                 goto cleanup;
327         }
328
329         if ((neighbor = rtnl_neigh_get(cache, link->inter->if_index, neighbor_addr_filter)) == NULL) {
330                 olsr_syslog(OLSR_LOG_INFO, "Neighbor MAC address not found in ARP cache");
331                 goto cleanup;
332         }
333
334         if ((neighbor_mac_addr = rtnl_neigh_get_lladdr(neighbor)) != NULL) {
335                 if (nl_addr_get_len(neighbor_mac_addr) != ETHER_ADDR_LEN) {
336                         olsr_syslog(OLSR_LOG_ERR, "Found a netlink nieghbor but address is not ETHER_ADDR_LEN long");
337                         goto cleanup;
338                 }
339                 memcpy(mac, nl_addr_get_binary_addr(neighbor_mac_addr), ETHER_ADDR_LEN);
340                 success = true;
341         }
342
343 cleanup:
344         if (cache)
345                 nl_cache_free(cache);
346         if (neighbor)
347                 rtnl_neigh_put(neighbor);
348         if (neighbor_addr_filter)
349                 nl_addr_put(neighbor_addr_filter);
350         // neighbor_mac_filter does not need cleanup
351
352         return success;
353 }
354
355 void nl80211_link_info_init(void) {
356         connect_netlink();
357 }
358
359 void nl80211_link_info_cleanup(void) {
360         nl_socket_free(gen_netlink_socket);
361         nl_socket_free(rt_netlink_socket);
362 }
363
364 static void free_lq_nl80211_data(struct lq_nl80211_data *nl80211) {
365         struct lq_nl80211_data *next = nl80211;
366         while (nl80211) {
367                 next = nl80211->next;
368                 free(nl80211);
369                 nl80211 = next; 
370         }
371 }
372
373 /**
374  * Find a object in the linked list that matches the MAC address.
375  *
376  * @param nl80211_list          Pointer to the linked list to find the object in.
377  * @param mac                           MAC address to look for, MUST be ETHER_ADDR_LEN long.
378  *
379  * @returns                                     Pointer to object or NULL on failure.
380  */
381 static struct lq_nl80211_data *find_lq_nl80211_data_by_mac(struct lq_nl80211_data *nl80211_list, unsigned char *mac) {
382         ASSERT_NOT_NULL(nl80211_list);
383         ASSERT_NOT_NULL(mac);
384
385         while (nl80211_list) {
386                 if (memcmp(mac, nl80211_list->mac, ETHER_ADDR_LEN) == 0) {
387                         return nl80211_list;
388                 }
389                 nl80211_list = nl80211_list->next;
390         }
391
392         return NULL;
393 }
394
395 static uint8_t bandwidth_to_quality(uint16_t bandwidth) {
396         fpm ratio;
397         fpm fp_bandwidth;
398         fpm penalty;
399
400         if (bandwidth == 0) {
401                 return 0;
402         }
403
404         // BandwidthPenalty = 1 - (Bandwidth / ReferenceBandwidth)
405
406         fp_bandwidth = itofpm(bandwidth);
407         fp_bandwidth = fpmidiv(fp_bandwidth, 10); // 100Kbit/sec to Mbit/sec
408
409         ratio = fpmidiv(fp_bandwidth, REFERENCE_BANDWIDTH_MBIT_SEC);
410         penalty = fpmsub(itofpm(1), ratio);
411
412         // Convert to 255 based number
413         penalty = fpmimul(penalty, 255);
414
415         return fpmtoi(penalty);
416 }
417
418 static uint8_t signal_to_quality(int8_t signal) {
419         // Map dBm levels to quality penalties
420         struct signal_penalty {
421                 int8_t signal;
422                 uint8_t penalty; // 255=1.0
423         };
424         // Must be ordered
425         static struct signal_penalty signal_quality_table[] = {
426                 { -75, 30 }, { -80, 60}, { -85, 120 }, { -90, 160 }, { -95, 200 }, { -100, 255 }
427         };
428         static size_t TABLE_SIZE = sizeof(signal_quality_table) / sizeof(struct signal_penalty);
429
430         unsigned int i = 0;
431         uint8_t penalty = 0;
432         for (i = 0; i < TABLE_SIZE; i++) {
433                 if (signal <= signal_quality_table[i].signal) {
434                         penalty = signal_quality_table[i].penalty;
435                 } else {
436                         break;
437                 }
438         }
439
440         return penalty;
441 }
442
443 void nl80211_link_info_get(void) {
444         struct interface *next_interface = NULL;
445         struct lq_nl80211_data *nl80211_list = NULL;
446         struct link_entry *link = NULL;
447         struct lq_nl80211_data *lq_data = NULL;
448         struct lq_ffeth_hello *lq_ffeth = NULL;
449         unsigned char mac_address[ETHER_ADDR_LEN];
450
451         uint8_t penalty_bandwidth;
452         uint8_t penalty_signal;
453
454         // Get latest 802.11 status information for all interfaces
455         // This list will contain OLSR and non-OLSR nodes
456         for (next_interface = ifnet; next_interface; next_interface = next_interface->int_next) {
457                 nl80211_link_info_for_interface(next_interface, &nl80211_list);
458         }
459
460         if (nl80211_list == NULL) {
461                 olsr_syslog(OLSR_LOG_INFO, "Failed to retreive any NL80211 data");
462                 return;
463         }
464
465         OLSR_FOR_ALL_LINK_ENTRIES(link) {
466                 lq_ffeth = (struct lq_ffeth_hello *) link->linkquality;
467                 lq_ffeth->lq.valueBandwidth = 0;
468                 lq_ffeth->lq.valueRSSI = 0;
469                 lq_ffeth->smoothed_lq.valueBandwidth = 0;
470                 lq_ffeth->smoothed_lq.valueRSSI = 0;
471
472                 if (mac_of_neighbor(link, mac_address)) {
473                         if ((lq_data = find_lq_nl80211_data_by_mac(nl80211_list, mac_address)) != NULL) {
474                                 penalty_bandwidth = bandwidth_to_quality(lq_data->bandwidth);
475                                 penalty_signal = signal_to_quality(lq_data->signal);
476
477                                 lq_ffeth->lq.valueBandwidth = penalty_bandwidth;
478                                 lq_ffeth->lq.valueRSSI = penalty_signal;
479                                 lq_ffeth->smoothed_lq.valueBandwidth = penalty_bandwidth;
480                                 lq_ffeth->smoothed_lq.valueRSSI = penalty_signal;
481
482                                 olsr_syslog(OLSR_LOG_INFO, "Apply 802.11: iface(%s) neighbor(%s) bandwidth(%dMb = %d) rssi(%ddBm = %d)",
483                                                 link->if_name, ether_ntoa((struct ether_addr *)mac_address),
484                                                 lq_data->bandwidth / 10, penalty_bandwidth, lq_data->signal, penalty_signal);
485                         } else
486                                 olsr_syslog(OLSR_LOG_INFO, "NO match ;-(!");
487                 }
488         } OLSR_FOR_ALL_LINK_ENTRIES_END(link)
489
490         free_lq_nl80211_data(nl80211_list);
491 }
492
493 #endif /* LINUX_NL80211 */
494 #endif /* __linux__ */