vsomeip 2.10.11

1 files changed, 438 insertions, 0 deletions

diff --git a/implementation/endpoints/src/netlink_connector.cpp b/implementation/endpoints/src/netlink_connector.cpp
new file mode 100644
index 00000000..94b75cf1
--- /dev/null
+++ b/implementation/endpoints/src/netlink_connector.cpp
@@ -0,0 +1,438 @@
+// Copyright (C) 2014-2017 Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
+// This Source Code Form is subject to the terms of the Mozilla Public
+// License, v. 2.0. If a copy of the MPL was not distributed with this
+// file, You can obtain one at http://mozilla.org/MPL/2.0/.
+
+#ifndef _WIN32
+
+#include <thread>
+
+#include <boost/asio/write.hpp>
+#include <boost/asio/read.hpp>
+#include<sstream>
+
+#include "../include/netlink_connector.hpp"
+#include "../../logging/include/logger.hpp"
+
+namespace vsomeip {
+
+void netlink_connector::register_net_if_changes_handler(net_if_changed_handler_t _handler) {
+    handler_ = _handler;
+}
+
+void netlink_connector::unregister_net_if_changes_handler() {
+    handler_ = nullptr;
+}
+
+void netlink_connector::stop() {
+    std::lock_guard<std::mutex> its_lock(socket_mutex_);
+    boost::system::error_code its_error;
+    socket_.shutdown(socket_.shutdown_both, its_error);
+    socket_.close(its_error);
+    if (its_error) {
+        VSOMEIP_WARNING << "Error closing NETLINK socket!";
+    }
+}
+
+void netlink_connector::start() {
+    std::lock_guard<std::mutex> its_lock(socket_mutex_);
+    boost::system::error_code ec;
+    if (socket_.is_open()) {
+        socket_.close(ec);
+        if (ec) {
+            VSOMEIP_WARNING << "Error closing NETLINK socket: " << ec.message();
+        }
+    }
+    socket_.open(nl_protocol(NETLINK_ROUTE), ec);
+    if (ec) {
+        VSOMEIP_WARNING << "Error opening NETLINK socket: " << ec.message();
+        if (handler_) {
+            handler_(true, "n/a", true);
+            handler_(false, "n/a", true);
+        }
+        return;
+    }
+    if (socket_.is_open()) {
+        socket_.bind(nl_endpoint<nl_protocol>(
+                RTMGRP_LINK |
+                RTMGRP_IPV4_IFADDR | RTMGRP_IPV6_IFADDR |
+                RTMGRP_IPV4_ROUTE | RTMGRP_IPV6_ROUTE |
+                RTMGRP_IPV4_MROUTE | RTMGRP_IPV6_MROUTE), ec);
+
+        if (ec) {
+            VSOMEIP_WARNING << "Error binding NETLINK socket: " << ec.message();
+            if (handler_) {
+                handler_(true, "n/a", true);
+                handler_(false, "n/a", true);
+            }
+            return;
+        }
+
+        send_ifa_request();
+
+        socket_.async_receive(
+            boost::asio::buffer(&recv_buffer_[0], recv_buffer_size),
+            std::bind(
+                &netlink_connector::receive_cbk,
+                shared_from_this(),
+                std::placeholders::_1,
+                std::placeholders::_2
+            )
+        );
+    } else {
+        VSOMEIP_WARNING << "Error opening NETLINK socket!";
+        if (handler_) {
+            handler_(true, "n/a", true);
+            handler_(false, "n/a", true);
+        }
+    }
+}
+
+void netlink_connector::receive_cbk(boost::system::error_code const &_error,
+                 std::size_t _bytes) {
+    if (!_error) {
+        size_t len = _bytes;
+
+        unsigned int address(0);
+        if (address_.is_v4()) {
+            inet_pton(AF_INET, address_.to_string().c_str(), &address);
+        } else {
+            inet_pton(AF_INET6, address_.to_string().c_str(), &address);
+        }
+
+        struct nlmsghdr *nlh = (struct nlmsghdr *)&recv_buffer_[0];
+
+        while ((NLMSG_OK(nlh, len)) && (nlh->nlmsg_type != NLMSG_DONE)) {
+            char ifname[1024];
+            switch (nlh->nlmsg_type) {
+                case RTM_NEWADDR: {
+                    // New Address information
+                    struct ifaddrmsg *ifa = (ifaddrmsg *)NLMSG_DATA(nlh);
+                    if (has_address(ifa, IFA_PAYLOAD(nlh), address)) {
+                        net_if_index_for_address_ = ifa->ifa_index;
+                        auto its_if = net_if_flags_.find(ifa->ifa_index);
+                        if (its_if != net_if_flags_.end()) {
+                            if ((its_if->second & IFF_UP) &&
+                                    (its_if->second & IFF_RUNNING)) {
+                                if (handler_) {
+                                    if_indextoname(ifa->ifa_index,ifname);
+                                    handler_(true, ifname, true);
+                                    send_rt_request();
+                                }
+                            } else {
+                                if (handler_) {
+                                    if_indextoname(ifa->ifa_index,ifname);
+                                    handler_(true, ifname, false);
+                                }
+                            }
+                        } else {
+                            // Request interface information
+                            // as we don't know about up/running state!
+                           send_ifi_request();
+                        }
+                    }
+                    break;
+                }
+                case RTM_NEWLINK: {
+                    // New Interface information
+                    struct ifinfomsg *ifi = (ifinfomsg *)NLMSG_DATA(nlh);
+                    net_if_flags_[ifi->ifi_index] = ifi->ifi_flags;
+                    if (net_if_index_for_address_ == ifi->ifi_index) {
+                        if ((ifi->ifi_flags & IFF_UP) &&
+                            (ifi->ifi_flags & IFF_RUNNING)) {
+                            if (handler_) {
+                                if_indextoname(ifi->ifi_index,ifname);
+                                handler_(true, ifname, true);
+                                send_rt_request();
+                            }
+                        } else {
+                            if (handler_) {
+                                if_indextoname(ifi->ifi_index,ifname);
+                                handler_(true, ifname, false);
+                            }
+                        }
+                    }
+                    break;
+                }
+                case RTM_NEWROUTE: {
+                    struct rtmsg *routemsg = (rtmsg *)NLMSG_DATA(nlh);
+                    std::string its_route_name;
+                    if (check_sd_multicast_route_match(routemsg, RTM_PAYLOAD(nlh),
+                            &its_route_name)) {
+                        if (handler_) {
+                            handler_(false, its_route_name, true);
+                        }
+                    }
+                    break;
+                }
+                case RTM_DELROUTE: {
+                    struct rtmsg *routemsg = (rtmsg *)NLMSG_DATA(nlh);
+                    std::string its_route_name;
+                    if (check_sd_multicast_route_match(routemsg, RTM_PAYLOAD(nlh),
+                            &its_route_name)) {
+                        if (handler_) {
+                            handler_(false, its_route_name, false);
+                        }
+                    }
+                    break;
+                }
+                case NLMSG_ERROR: {
+                    struct nlmsgerr *errmsg = (nlmsgerr *)NLMSG_DATA(nlh);
+                    VSOMEIP_ERROR << "netlink_connector::receive_cbk received "
+                        "error message: " << std::dec << nlh->nlmsg_type
+                        << " seq " << errmsg->msg.nlmsg_seq;
+                    break;
+                }
+                case NLMSG_DONE:
+                case NLMSG_NOOP:
+                default:
+                    break;
+            }
+            nlh = NLMSG_NEXT(nlh, len);
+        }
+        {
+            std::lock_guard<std::mutex> its_lock(socket_mutex_);
+            if (socket_.is_open()) {
+                socket_.async_receive(
+                    boost::asio::buffer(&recv_buffer_[0], recv_buffer_size),
+                    std::bind(
+                        &netlink_connector::receive_cbk,
+                        shared_from_this(),
+                        std::placeholders::_1,
+                        std::placeholders::_2
+                    )
+                );
+            }
+        }
+    } else {
+        if (_error != boost::asio::error::operation_aborted) {
+            VSOMEIP_WARNING << "Error receive_cbk NETLINK socket!" << _error.message();
+            boost::system::error_code its_error;
+            {
+                std::lock_guard<std::mutex> its_lock(socket_mutex_);
+                if (socket_.is_open()) {
+                    socket_.shutdown(socket_.shutdown_both, its_error);
+                    socket_.close(its_error);
+                    if (its_error) {
+                        VSOMEIP_WARNING << "Error closing NETLINK socket!"
+                                << its_error.message();
+                    }
+                }
+            }
+            if (handler_) {
+                handler_(true, "n/a", true);
+                handler_(false, "n/a", true);
+            }
+        }
+    }
+}
+
+void netlink_connector::send_cbk(boost::system::error_code const &_error, std::size_t _bytes) {
+    (void)_bytes;
+    if (_error) {
+        VSOMEIP_WARNING << "Netlink send error : " << _error.message();
+        if (handler_) {
+            handler_(true, "n/a", true);
+            handler_(false, "n/a", true);
+        }
+    }
+}
+
+void netlink_connector::send_ifa_request() {
+    typedef struct {
+        struct nlmsghdr nlhdr;
+        struct ifaddrmsg addrmsg;
+    } netlink_address_msg;
+    netlink_address_msg get_address_msg;
+    memset(&get_address_msg, 0, sizeof(get_address_msg));
+    get_address_msg.nlhdr.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifaddrmsg));
+    get_address_msg.nlhdr.nlmsg_flags = NLM_F_REQUEST | NLM_F_ROOT;
+    get_address_msg.nlhdr.nlmsg_type = RTM_GETADDR;
+    get_address_msg.nlhdr.nlmsg_seq = 1;
+    if (address_.is_v4()) {
+        get_address_msg.addrmsg.ifa_family = AF_INET;
+    } else {
+        get_address_msg.addrmsg.ifa_family = AF_INET6;
+    }
+
+    socket_.async_send(
+        boost::asio::buffer(&get_address_msg, get_address_msg.nlhdr.nlmsg_len),
+        std::bind(
+            &netlink_connector::send_cbk,
+            shared_from_this(),
+            std::placeholders::_1,
+            std::placeholders::_2
+        )
+    );
+}
+
+void netlink_connector::send_ifi_request() {
+    typedef struct {
+        struct nlmsghdr nlhdr;
+        struct ifinfomsg infomsg;
+    } netlink_link_msg;
+    netlink_link_msg get_link_msg;
+    memset(&get_link_msg, 0, sizeof(get_link_msg));
+    get_link_msg.nlhdr.nlmsg_len = NLMSG_LENGTH(sizeof(struct ifinfomsg));
+    get_link_msg.nlhdr.nlmsg_flags = NLM_F_REQUEST | NLM_F_ROOT;
+    get_link_msg.nlhdr.nlmsg_type = RTM_GETLINK;
+    get_link_msg.infomsg.ifi_family = AF_UNSPEC;
+    get_link_msg.nlhdr.nlmsg_seq = 2;
+
+    {
+        std::lock_guard<std::mutex> its_lock(socket_mutex_);
+        socket_.async_send(
+            boost::asio::buffer(&get_link_msg, get_link_msg.nlhdr.nlmsg_len),
+            std::bind(
+                &netlink_connector::send_cbk,
+                shared_from_this(),
+                std::placeholders::_1,
+                std::placeholders::_2
+            )
+        );
+    }
+}
+
+void netlink_connector::send_rt_request() {
+    typedef struct {
+        struct nlmsghdr nlhdr;
+        struct rtgenmsg routemsg;
+    } netlink_route_msg;
+
+    netlink_route_msg get_route_msg;
+    memset(&get_route_msg, 0, sizeof(get_route_msg));
+    get_route_msg.nlhdr.nlmsg_len = NLMSG_LENGTH(sizeof(struct rtgenmsg));
+    get_route_msg.nlhdr.nlmsg_flags = NLM_F_REQUEST | NLM_F_DUMP;
+    get_route_msg.nlhdr.nlmsg_type = RTM_GETROUTE;
+    get_route_msg.nlhdr.nlmsg_seq = 3;
+    if (multicast_address_.is_v6()) {
+        get_route_msg.routemsg.rtgen_family = AF_INET6;
+    } else {
+        get_route_msg.routemsg.rtgen_family = AF_INET;
+    }
+
+    {
+        std::lock_guard<std::mutex> its_lock(socket_mutex_);
+        socket_.async_send(
+            boost::asio::buffer(&get_route_msg, get_route_msg.nlhdr.nlmsg_len),
+            std::bind(
+                &netlink_connector::send_cbk,
+                shared_from_this(),
+                std::placeholders::_1,
+                std::placeholders::_2
+            )
+        );
+    }
+}
+
+bool netlink_connector::has_address(struct ifaddrmsg * ifa_struct,
+        size_t length,
+        const unsigned int address) {
+
+    struct rtattr *retrta;
+    retrta = static_cast<struct rtattr *>(IFA_RTA(ifa_struct));
+    while RTA_OK(retrta, length) {
+        if (retrta->rta_type == IFA_ADDRESS) {
+            char pradd[128];
+            unsigned int * tmp_address = (unsigned int *)RTA_DATA(retrta);
+            if (address_.is_v4()) {
+                inet_ntop(AF_INET, tmp_address, pradd, sizeof(pradd));
+            } else {
+                inet_ntop(AF_INET6, tmp_address, pradd, sizeof(pradd));
+            }
+            if (address == *tmp_address) {
+                return true;
+            }
+        }
+        retrta = RTA_NEXT(retrta, length);
+    }
+
+    return false;
+}
+
+bool netlink_connector::check_sd_multicast_route_match(struct rtmsg* _routemsg,
+                                              size_t _length,
+                                              std::string* _routename) const {
+    struct rtattr *retrta;
+    retrta = static_cast<struct rtattr *>(RTM_RTA(_routemsg));
+    int if_index(0);
+    char if_name[1024] = "n/a";
+    char address[INET6_ADDRSTRLEN] = "n/a";
+    char gateway[INET6_ADDRSTRLEN] = "n/a";
+    bool matches_sd_multicast(false);
+    while (RTA_OK(retrta, _length)) {
+        if (retrta->rta_type == RTA_DST) {
+            // check if added/removed route matches on configured SD multicast address
+            size_t rtattr_length = RTA_PAYLOAD(retrta);
+            if (rtattr_length == 4 && multicast_address_.is_v4()) { // IPv4 route
+                inet_ntop(AF_INET, RTA_DATA(retrta), address, sizeof(address));
+                std::uint32_t netmask(0);
+                for (int i = 31; i > 31 - _routemsg->rtm_dst_len; i--) {
+                    netmask |= (1 << i);
+                }
+                const std::uint32_t dst_addr = ntohl(*((std::uint32_t *)RTA_DATA(retrta)));
+                const std::uint32_t dst_net = (dst_addr & netmask);
+                const std::uint32_t sd_addr = static_cast<std::uint32_t>(multicast_address_.to_v4().to_ulong());
+                const std::uint32_t sd_net = (sd_addr & netmask);
+                matches_sd_multicast = !(dst_net ^ sd_net);
+            } else if (rtattr_length == 16 && multicast_address_.is_v6()) { // IPv6 route
+                inet_ntop(AF_INET6, RTA_DATA(retrta), address, sizeof(address));
+                std::uint32_t netmask2[4] = {0,0,0,0};
+                for (int i = 127; i > 127 - _routemsg->rtm_dst_len; i--) {
+                    if (i > 95) {
+                        netmask2[0] |= (1 << (i-96));
+                    } else if (i > 63) {
+                        netmask2[1] |= (1 << (i-63));
+                    } else if (i > 31) {
+                        netmask2[2] |= (1 << (i-32));
+                    } else {
+                        netmask2[3] |= (1 << i);
+                    }
+                }
+
+                for (int i = 0; i < 4; i++) {
+                    const std::uint32_t dst = ntohl((*(struct in6_addr*)RTA_DATA(retrta)).__in6_u.__u6_addr32[i]);
+                    const std::uint32_t sd = ntohl(reinterpret_cast<std::uint32_t*>(multicast_address_.to_v6().to_bytes().data())[i]);
+                    const std::uint32_t dst_net = dst & netmask2[i];
+                    const std::uint32_t sd_net = sd & netmask2[i];
+                    matches_sd_multicast = !(dst_net ^ sd_net);
+                    if (!matches_sd_multicast) {
+                        break;
+                    }
+                }
+            }
+        } else if (retrta->rta_type == RTA_OIF) {
+            if_index = *(int *)(RTA_DATA(retrta));
+            if_indextoname(if_index,if_name);
+        } else if (retrta->rta_type == RTA_GATEWAY) {
+            size_t rtattr_length = RTA_PAYLOAD(retrta);
+            if (rtattr_length == 4) {
+                inet_ntop(AF_INET, RTA_DATA(retrta), gateway, sizeof(gateway));
+            } else if (rtattr_length == 16) {
+                inet_ntop(AF_INET6, RTA_DATA(retrta), gateway, sizeof(gateway));
+            }
+        }
+        retrta = RTA_NEXT(retrta, _length);
+    }
+    if (matches_sd_multicast && net_if_index_for_address_ == if_index) {
+        std::stringstream stream;
+        stream << address << "/" <<  (static_cast<uint32_t>(_routemsg->rtm_dst_len))
+                << " if: " << if_name << " gw: " << gateway;
+        *_routename = stream.str();
+        return true;
+    } else if (if_index > 0 && net_if_index_for_address_ == if_index &&
+            _routemsg->rtm_dst_len == 0) {
+        // the default route is set to the interface on which the SD will listen
+        // therefore no explicit multicast route is required.
+        std::stringstream stream;
+        stream << "default route (0.0.0.0/0) if: " << if_name << " gw: " << gateway;
+        *_routename = stream.str();
+        return true;
+    }
+    return false;
+}
+
+} // namespace vsomeip
+
+#endif
+