1 // Copyright (C) 2014-2017 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
2 // This Source Code Form is subject to the terms of the Mozilla Public
3 // License, v. 2.0. If a copy of the MPL was not distributed with this
4 // file, You can obtain one at http://mozilla.org/MPL/2.0/.
5 
6 #ifndef _WIN32
7 
8 #include <thread>
9 
10 #include <boost/asio/write.hpp>
11 #include <boost/asio/read.hpp>
12 #include<sstream>
13 
14 #include <vsomeip/internal/logger.hpp>
15 
16 #include "../include/netlink_connector.hpp"
17 
18 namespace vsomeip_v3 {
19 
20 namespace {
21     const std::uint32_t ifa_request_sequence = 1;
22     const std::uint32_t ifi_request_sequence = 2;
23     const std::uint32_t rt_request_sequence = 3;
24 }
25 
register_net_if_changes_handler(const net_if_changed_handler_t & _handler)26 void netlink_connector::register_net_if_changes_handler(const net_if_changed_handler_t& _handler) {
27     handler_ = _handler;
28 }
29 
unregister_net_if_changes_handler()30 void netlink_connector::unregister_net_if_changes_handler() {
31     handler_ = nullptr;
32 }
33 
stop()34 void netlink_connector::stop() {
35     std::lock_guard<std::mutex> its_lock(socket_mutex_);
36     boost::system::error_code its_error;
37     socket_.shutdown(socket_.shutdown_both, its_error);
38     socket_.close(its_error);
39     if (its_error) {
40         VSOMEIP_WARNING << "Error closing NETLINK socket!";
41     }
42 }
43 
start()44 void netlink_connector::start() {
45     std::lock_guard<std::mutex> its_lock(socket_mutex_);
46     boost::system::error_code ec;
47     if (socket_.is_open()) {
48         socket_.close(ec);
49         if (ec) {
50             VSOMEIP_WARNING << "Error closing NETLINK socket: " << ec.message();
51         }
52     }
53     socket_.open(nl_protocol(NETLINK_ROUTE), ec);
54     if (ec) {
55         VSOMEIP_WARNING << "Error opening NETLINK socket: " << ec.message();
56         if (handler_) {
57             handler_(true, "n/a", true);
58             handler_(false, "n/a", true);
59         }
60         return;
61     }
62     if (socket_.is_open()) {
63         socket_.bind(nl_endpoint<nl_protocol>(
64                 RTMGRP_LINK |
65                 RTMGRP_IPV4_IFADDR | RTMGRP_IPV6_IFADDR |
66                 RTMGRP_IPV4_ROUTE | RTMGRP_IPV6_ROUTE |
67                 RTMGRP_IPV4_MROUTE | RTMGRP_IPV6_MROUTE), ec);
68 
69         if (ec) {
70             VSOMEIP_WARNING << "Error binding NETLINK socket: " << ec.message();
71             if (handler_) {
72                 handler_(true, "n/a", true);
73                 handler_(false, "n/a", true);
74             }
75 #ifndef VSOMEIP_ENABLE_MULTIPLE_ROUTING_MANAGERS
76             return;
77 #endif // VSOMEIP_ENABLE_MULTIPLE_ROUTING_MANAGERS
78         }
79 
80         send_ifa_request();
81 
82         socket_.async_receive(
83             boost::asio::buffer(&recv_buffer_[0], recv_buffer_size),
84             std::bind(
85                 &netlink_connector::receive_cbk,
86                 shared_from_this(),
87                 std::placeholders::_1,
88                 std::placeholders::_2
89             )
90         );
91     } else {
92         VSOMEIP_WARNING << "Error opening NETLINK socket!";
93         if (handler_) {
94             handler_(true, "n/a", true);
95             handler_(false, "n/a", true);
96         }
97     }
98 }
99 
receive_cbk(boost::system::error_code const & _error,std::size_t _bytes)100 void netlink_connector::receive_cbk(boost::system::error_code const &_error,
101                  std::size_t _bytes) {
102     if (!_error) {
103         size_t len = _bytes;
104 
105         unsigned int address(0);
106         if (address_.is_v4()) {
107             inet_pton(AF_INET, address_.to_string().c_str(), &address);
108         } else {
109             inet_pton(AF_INET6, address_.to_string().c_str(), &address);
110         }
111 
112         struct nlmsghdr *nlh = (struct nlmsghdr *)&recv_buffer_[0];
113 
114         while ((NLMSG_OK(nlh, len)) && (nlh->nlmsg_type != NLMSG_DONE)) {
115             char ifname[IF_NAMESIZE];
116             switch (nlh->nlmsg_type) {
117                 case RTM_NEWADDR: {
118                     // New Address information
119                     struct ifaddrmsg *ifa = (ifaddrmsg *)NLMSG_DATA(nlh);
120                     if (has_address(ifa, IFA_PAYLOAD(nlh), address)) {
121                         net_if_index_for_address_ = static_cast<int>(ifa->ifa_index);
122                         auto its_if = net_if_flags_.find(static_cast<int>(ifa->ifa_index));
123                         if (its_if != net_if_flags_.end()) {
124                             if ((its_if->second & IFF_UP) &&
125                                     (its_if->second & IFF_RUNNING)) {
126                                 if (handler_) {
127                                     if_indextoname(ifa->ifa_index,ifname);
128                                     handler_(true, ifname, true);
129                                     send_rt_request();
130                                 }
131                             } else {
132                                 if (handler_) {
133                                     if_indextoname(ifa->ifa_index,ifname);
134                                     handler_(true, ifname, false);
135                                 }
136                             }
137                         } else {
138                             // Request interface information
139                             // as we don't know about up/running state!
140                            send_ifi_request();
141                         }
142                     }
143                     break;
144                 }
145                 case RTM_NEWLINK: {
146                     // New Interface information
147                     struct ifinfomsg *ifi = (ifinfomsg *)NLMSG_DATA(nlh);
148                     net_if_flags_[ifi->ifi_index] = ifi->ifi_flags;
149                     if (net_if_index_for_address_ == ifi->ifi_index) {
150                         if ((ifi->ifi_flags & IFF_UP) &&
151                             (ifi->ifi_flags & IFF_RUNNING)) {
152                             if (handler_) {
153                                 if_indextoname(static_cast<unsigned int>(ifi->ifi_index),ifname);
154                                 handler_(true, ifname, true);
155                                 send_rt_request();
156                             }
157                         } else {
158                             if (handler_) {
159                                 if_indextoname(static_cast<unsigned int>(ifi->ifi_index),ifname);
160                                 handler_(true, ifname, false);
161                             }
162                         }
163                     }
164                     break;
165                 }
166                 case RTM_NEWROUTE: {
167                     struct rtmsg *routemsg = (rtmsg *)NLMSG_DATA(nlh);
168                     std::string its_route_name;
169                     if (check_sd_multicast_route_match(routemsg, RTM_PAYLOAD(nlh),
170                             &its_route_name)) {
171                         if (handler_) {
172                             handler_(false, its_route_name, true);
173                         }
174                     }
175                     break;
176                 }
177                 case RTM_DELROUTE: {
178                     struct rtmsg *routemsg = (rtmsg *)NLMSG_DATA(nlh);
179                     std::string its_route_name;
180                     if (check_sd_multicast_route_match(routemsg, RTM_PAYLOAD(nlh),
181                             &its_route_name)) {
182                         if (handler_) {
183                             handler_(false, its_route_name, false);
184                         }
185                     }
186                     break;
187                 }
188                 case NLMSG_ERROR: {
189                     struct nlmsgerr *errmsg = (nlmsgerr *)NLMSG_DATA(nlh);
190                     if (errmsg->error == 0) {
191                         // Ack from netlink
192                         break;
193                     }
194 
195                     VSOMEIP_ERROR << "netlink_connector::receive_cbk received "
196                         "error message: " << strerror(errmsg->error)
197                         << " type " << std::dec << errmsg->msg.nlmsg_type
198                         << " seq " << errmsg->msg.nlmsg_seq;
199 
200                     std::string request_type{};
201                     if (errmsg->msg.nlmsg_type == RTM_GETADDR && errmsg->msg.nlmsg_seq == ifa_request_sequence) {
202                         request_type = "address request";
203                         send_ifa_request();
204                     } else if (errmsg->msg.nlmsg_type == RTM_GETLINK && errmsg->msg.nlmsg_seq == ifi_request_sequence) {
205                         request_type = "link request";
206                         send_ifi_request();
207                     } else if (errmsg->msg.nlmsg_type == RTM_GETROUTE && errmsg->msg.nlmsg_seq == rt_request_sequence) {
208                         request_type = "route request";
209                         send_rt_request();
210                     }
211 
212                     if (!request_type.empty()) {
213                         VSOMEIP_INFO << "Retrying netlink " << request_type;
214                     }
215 
216                     break;
217                 }
218                 case NLMSG_DONE:
219                 case NLMSG_NOOP:
220                 default:
221                     break;
222             }
223             nlh = NLMSG_NEXT(nlh, len);
224         }
225         {
226             std::lock_guard<std::mutex> its_lock(socket_mutex_);
227             if (socket_.is_open()) {
228                 socket_.async_receive(
229                     boost::asio::buffer(&recv_buffer_[0], recv_buffer_size),
230                     std::bind(
231                         &netlink_connector::receive_cbk,
232                         shared_from_this(),
233                         std::placeholders::_1,
234                         std::placeholders::_2
235                     )
236                 );
237             }
238         }
239     } else {
240         if (_error != boost::asio::error::operation_aborted) {
241             VSOMEIP_WARNING << "Error receive_cbk NETLINK socket!" << _error.message();
242             boost::system::error_code its_error;
243             {
244                 std::lock_guard<std::mutex> its_lock(socket_mutex_);
245                 if (socket_.is_open()) {
246                     socket_.shutdown(socket_.shutdown_both, its_error);
247                     socket_.close(its_error);
248                     if (its_error) {
249                         VSOMEIP_WARNING << "Error closing NETLINK socket!"
250                                 << its_error.message();
251                     }
252                 }
253             }
254             if (handler_) {
255                 handler_(true, "n/a", true);
256                 handler_(false, "n/a", true);
257             }
258         }
259     }
260 }
261 
send_cbk(boost::system::error_code const & _error,std::size_t _bytes)262 void netlink_connector::send_cbk(boost::system::error_code const &_error, std::size_t _bytes) {
263     (void)_bytes;
264     if (_error) {
265         VSOMEIP_WARNING << "Netlink send error : " << _error.message();
266         if (handler_) {
267             handler_(true, "n/a", true);
268             handler_(false, "n/a", true);
269         }
270     }
271 }
272 
send_ifa_request()273 void netlink_connector::send_ifa_request() {
274     typedef struct {
275         struct nlmsghdr nlhdr;
276         struct ifaddrmsg addrmsg;
277     } netlink_address_msg;
278     netlink_address_msg get_address_msg;
279     memset(&get_address_msg, 0, sizeof(get_address_msg));
280     get_address_msg.nlhdr.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifaddrmsg));
281     get_address_msg.nlhdr.nlmsg_flags = NLM_F_REQUEST | NLM_F_ROOT;
282     get_address_msg.nlhdr.nlmsg_type = RTM_GETADDR;
283     get_address_msg.nlhdr.nlmsg_seq = ifa_request_sequence;
284     if (address_.is_v4()) {
285         get_address_msg.addrmsg.ifa_family = AF_INET;
286     } else {
287         get_address_msg.addrmsg.ifa_family = AF_INET6;
288     }
289 
290     socket_.async_send(
291         boost::asio::buffer(&get_address_msg, get_address_msg.nlhdr.nlmsg_len),
292         std::bind(
293             &netlink_connector::send_cbk,
294             shared_from_this(),
295             std::placeholders::_1,
296             std::placeholders::_2
297         )
298     );
299 }
300 
send_ifi_request()301 void netlink_connector::send_ifi_request() {
302     typedef struct {
303         struct nlmsghdr nlhdr;
304         struct ifinfomsg infomsg;
305     } netlink_link_msg;
306     netlink_link_msg get_link_msg;
307     memset(&get_link_msg, 0, sizeof(get_link_msg));
308     get_link_msg.nlhdr.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifinfomsg));
309     get_link_msg.nlhdr.nlmsg_flags = NLM_F_REQUEST | NLM_F_ROOT;
310     get_link_msg.nlhdr.nlmsg_type = RTM_GETLINK;
311     get_link_msg.infomsg.ifi_family = AF_UNSPEC;
312     get_link_msg.nlhdr.nlmsg_seq = ifi_request_sequence;
313 
314     {
315         std::lock_guard<std::mutex> its_lock(socket_mutex_);
316         socket_.async_send(
317             boost::asio::buffer(&get_link_msg, get_link_msg.nlhdr.nlmsg_len),
318             std::bind(
319                 &netlink_connector::send_cbk,
320                 shared_from_this(),
321                 std::placeholders::_1,
322                 std::placeholders::_2
323             )
324         );
325     }
326 }
327 
send_rt_request()328 void netlink_connector::send_rt_request() {
329     typedef struct {
330         struct nlmsghdr nlhdr;
331         struct rtgenmsg routemsg;
332     } netlink_route_msg;
333 
334     netlink_route_msg get_route_msg;
335     memset(&get_route_msg, 0, sizeof(get_route_msg));
336     get_route_msg.nlhdr.nlmsg_len = NLMSG_LENGTH(sizeof(struct rtgenmsg));
337     get_route_msg.nlhdr.nlmsg_flags = NLM_F_REQUEST | NLM_F_DUMP;
338     get_route_msg.nlhdr.nlmsg_type = RTM_GETROUTE;
339     get_route_msg.nlhdr.nlmsg_seq = rt_request_sequence;
340     if (multicast_address_.is_v6()) {
341         get_route_msg.routemsg.rtgen_family = AF_INET6;
342     } else {
343         get_route_msg.routemsg.rtgen_family = AF_INET;
344     }
345 
346     {
347         std::lock_guard<std::mutex> its_lock(socket_mutex_);
348         socket_.async_send(
349             boost::asio::buffer(&get_route_msg, get_route_msg.nlhdr.nlmsg_len),
350             std::bind(
351                 &netlink_connector::send_cbk,
352                 shared_from_this(),
353                 std::placeholders::_1,
354                 std::placeholders::_2
355             )
356         );
357     }
358 }
359 
has_address(struct ifaddrmsg * ifa_struct,size_t length,const unsigned int address)360 bool netlink_connector::has_address(struct ifaddrmsg * ifa_struct,
361         size_t length,
362         const unsigned int address) {
363 
364     struct rtattr *retrta;
365     retrta = static_cast<struct rtattr *>(IFA_RTA(ifa_struct));
366     while RTA_OK(retrta, length) {
367         if (retrta->rta_type == IFA_ADDRESS) {
368             char pradd[128];
369             unsigned int * tmp_address = (unsigned int *)RTA_DATA(retrta);
370             if (address_.is_v4()) {
371                 inet_ntop(AF_INET, tmp_address, pradd, sizeof(pradd));
372             } else {
373                 inet_ntop(AF_INET6, tmp_address, pradd, sizeof(pradd));
374             }
375             if (address == *tmp_address) {
376                 return true;
377             }
378         }
379         retrta = RTA_NEXT(retrta, length);
380     }
381 
382     return false;
383 }
384 
check_sd_multicast_route_match(struct rtmsg * _routemsg,size_t _length,std::string * _routename) const385 bool netlink_connector::check_sd_multicast_route_match(struct rtmsg* _routemsg,
386                                               size_t _length,
387                                               std::string* _routename) const {
388     struct rtattr *retrta;
389     retrta = static_cast<struct rtattr *>(RTM_RTA(_routemsg));
390     int if_index(0);
391     char if_name[IF_NAMESIZE] = "n/a";
392     char address[INET6_ADDRSTRLEN] = "n/a";
393     char gateway[INET6_ADDRSTRLEN] = "n/a";
394     bool matches_sd_multicast(false);
395     while (RTA_OK(retrta, _length)) {
396         if (retrta->rta_type == RTA_DST) {
397             // check if added/removed route matches on configured SD multicast address
398             size_t rtattr_length = RTA_PAYLOAD(retrta);
399             if (rtattr_length == 4 && multicast_address_.is_v4()) { // IPv4 route
400                 inet_ntop(AF_INET, RTA_DATA(retrta), address, sizeof(address));
401                 std::uint32_t netmask(0);
402                 for (int i = 31; i > 31 - _routemsg->rtm_dst_len; i--) {
403                     netmask |= static_cast<std::uint32_t>(1 << i);
404                 }
405                 const std::uint32_t dst_addr = ntohl(*((std::uint32_t *)RTA_DATA(retrta)));
406                 const std::uint32_t dst_net = (dst_addr & netmask);
407                 const std::uint32_t sd_addr = static_cast<std::uint32_t>(multicast_address_.to_v4().to_ulong());
408                 const std::uint32_t sd_net = (sd_addr & netmask);
409                 matches_sd_multicast = !(dst_net ^ sd_net);
410             } else if (rtattr_length == 16 && multicast_address_.is_v6()) { // IPv6 route
411                 inet_ntop(AF_INET6, RTA_DATA(retrta), address, sizeof(address));
412                 std::uint32_t netmask2[4] = {0,0,0,0};
413                 for (int i = 127; i > 127 - _routemsg->rtm_dst_len; i--) {
414                     if (i > 95) {
415                         netmask2[0] |= static_cast<std::uint32_t>(1 << (i-96));
416                     } else if (i > 63) {
417                         netmask2[1] |= static_cast<std::uint32_t>(1 << (i-63));
418                     } else if (i > 31) {
419                         netmask2[2] |= static_cast<std::uint32_t>(1 << (i-32));
420                     } else {
421                         netmask2[3] |= static_cast<std::uint32_t>(1 << i);
422                     }
423                 }
424 
425                 for (int i = 0; i < 4; i++) {
426 #ifndef ANDROID
427                     const std::uint32_t dst = ntohl((*(struct in6_addr*)RTA_DATA(retrta)).__in6_u.__u6_addr32[i]);
428 #else
429                     const std::uint32_t dst = ntohl((*(struct in6_addr*)RTA_DATA(retrta)).in6_u.u6_addr32[i]);
430 #endif
431                     const std::uint32_t sd = ntohl(reinterpret_cast<std::uint32_t*>(multicast_address_.to_v6().to_bytes().data())[i]);
432                     const std::uint32_t dst_net = dst & netmask2[i];
433                     const std::uint32_t sd_net = sd & netmask2[i];
434                     matches_sd_multicast = !(dst_net ^ sd_net);
435                     if (!matches_sd_multicast) {
436                         break;
437                     }
438                 }
439             }
440         } else if (retrta->rta_type == RTA_OIF) {
441             if_index = *(int *)(RTA_DATA(retrta));
442             if_indextoname(static_cast<unsigned int>(if_index),if_name);
443         } else if (retrta->rta_type == RTA_GATEWAY) {
444             size_t rtattr_length = RTA_PAYLOAD(retrta);
445             if (rtattr_length == 4) {
446                 inet_ntop(AF_INET, RTA_DATA(retrta), gateway, sizeof(gateway));
447             } else if (rtattr_length == 16) {
448                 inet_ntop(AF_INET6, RTA_DATA(retrta), gateway, sizeof(gateway));
449             }
450         }
451         retrta = RTA_NEXT(retrta, _length);
452     }
453     if (matches_sd_multicast && net_if_index_for_address_ == if_index) {
454         std::stringstream stream;
455         stream << address << "/" <<  (static_cast<uint32_t>(_routemsg->rtm_dst_len))
456                 << " if: " << if_name << " gw: " << gateway;
457         *_routename = stream.str();
458         return true;
459     } else if (if_index > 0 && net_if_index_for_address_ == if_index &&
460             _routemsg->rtm_dst_len == 0) {
461         // the default route is set to the interface on which the SD will listen
462         // therefore no explicit multicast route is required.
463         std::stringstream stream;
464         stream << "default route (0.0.0.0/0) if: " << if_name << " gw: " << gateway;
465         *_routename = stream.str();
466         return true;
467     }
468     return false;
469 }
470 
471 } // namespace vsomeip_v3
472 
473 #endif // #ifndef _WIN32
474 
475