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