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