From 587088bb187e6e279711f451020988eeefaa0782 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 28 Jan 2016 00:12:27 +0300 Subject: [PATCH 1/4] Refactored SocketCAN select() call; fixes #83 Features: - The driver will throw if it's fed a non-existing or malfunctioning interface during initialization - When an interface becomes down/disconnected while the node is running, the driver will silently exclude it from the IO loop and continue to run on the remaining interfaces. - When all interfaces become down/disconnected, the driver will throw AllIfacesDownException() from SocketCanDriver::select(). --- libuavcan_drivers/linux/CMakeLists.txt | 4 + .../linux/include/uavcan_linux/exception.hpp | 9 ++ .../linux/include/uavcan_linux/socketcan.hpp | 133 +++++++++++++----- 3 files changed, 110 insertions(+), 36 deletions(-) diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index ef61149fe8..9577b227ca 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -45,6 +45,10 @@ endif () include_directories(include) set(CMAKE_CXX_FLAGS "-Wall -Wextra -pedantic -std=c++11") # GCC or Clang +if(CMAKE_BUILD_TYPE STREQUAL "Debug") + add_definitions(-DUAVCAN_DEBUG=1) +endif() + # # Tests # These aren't installed, an average library user should not care about them. diff --git a/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp b/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp index a04b4ddb8c..fc8840ae59 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp @@ -29,4 +29,13 @@ public: int getErrno() const { return errno_; } }; +/** + * This exception is thrown when all available interfaces become down. + */ +class AllIfacesDownException : public Exception +{ +public: + AllIfacesDownException() : Exception("All ifaces are down") { } +}; + } diff --git a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp index 6aa0504696..1ebb0df045 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp @@ -11,6 +11,7 @@ #include #include #include +#include #include #include @@ -25,6 +26,7 @@ #include #include + namespace uavcan_linux { /** @@ -333,6 +335,7 @@ class SocketCanIface : public uavcan::ICanIface else { registerError(SocketCanError::SocketReadFailure); + break; } } } @@ -557,6 +560,18 @@ public: goto fail; } } + // Validate the resulting socket + { + int error = 0; + ::socklen_t errlen = sizeof(error); + (void)::getsockopt(s, SOL_SOCKET, SO_ERROR, reinterpret_cast(&error), &errlen); + if (error != 0) + { + goto fail; + UAVCAN_TRACE("SocketCAN", "Socket error: iface='%s' errno=%d error=%d", + iface_name.c_str(), errno, error); + } + } return s; fail: @@ -571,14 +586,33 @@ public: */ class SocketCanDriver : public uavcan::ICanDriver { -public: - static constexpr unsigned MaxIfaces = uavcan::MaxCanIfaces; + class IfaceWrapper : public SocketCanIface + { + bool down_ = false; + + public: + IfaceWrapper(const SystemClock& clock, int fd) : SocketCanIface(clock, fd) { } + + void updateDownStatusFromPollResult(const ::pollfd& pfd) + { + assert(pfd.fd == this->getFileDescriptor()); + if (!down_ && (pfd.revents & POLLERR)) + { + int error = 0; + ::socklen_t errlen = sizeof(error); + (void)::getsockopt(pfd.fd, SOL_SOCKET, SO_ERROR, reinterpret_cast(&error), &errlen); + + down_ = error == ENETDOWN || error == ENODEV; + + UAVCAN_TRACE("SocketCAN", "Iface %d is dead; error %d", this->getFileDescriptor(), error); + } + } + + bool isDown() const { return down_; } + }; -private: const SystemClock& clock_; - uavcan::LazyConstructor ifaces_[MaxIfaces]; - ::pollfd pollfds_[MaxIfaces]; - std::uint8_t num_ifaces_ = 0; + std::vector> ifaces_; public: /** @@ -587,11 +621,7 @@ public: explicit SocketCanDriver(const SystemClock& clock) : clock_(clock) { - for (auto& p : pollfds_) - { - p = ::pollfd(); - p.fd = -1; - } + ifaces_.reserve(uavcan::MaxCanIfaces); } /** @@ -606,7 +636,7 @@ public: { // Detecting whether we need to block at all bool need_block = (inout_masks.write == 0); // Write queue is infinite - for (unsigned i = 0; need_block && (i < num_ifaces_); i++) + for (unsigned i = 0; need_block && (i < ifaces_.size()); i++) { const bool need_read = inout_masks.read & (1 << i); if (need_read && ifaces_[i]->hasReadyRx()) @@ -618,15 +648,31 @@ public: if (need_block) { // Poll FD set setup - for (unsigned i = 0; i < num_ifaces_; i++) + ::pollfd pollfds[uavcan::MaxCanIfaces] = {}; + unsigned num_pollfds = 0; + IfaceWrapper* pollfd_index_to_iface[uavcan::MaxCanIfaces] = { }; + + for (unsigned i = 0; i < ifaces_.size(); i++) { - pollfds_[i].events = POLLIN; - if (ifaces_[i]->hasReadyTx() || (inout_masks.write & (1 << i))) + if (!ifaces_[i]->isDown()) { - pollfds_[i].events |= POLLOUT; + pollfds[num_pollfds].fd = ifaces_[i]->getFileDescriptor(); + pollfds[num_pollfds].events = POLLIN; + if (ifaces_[i]->hasReadyTx() || (inout_masks.write & (1U << i))) + { + pollfds[num_pollfds].events |= POLLOUT; + } + pollfd_index_to_iface[num_pollfds] = ifaces_[i].get(); + num_pollfds++; } } + // This is where we abort when the last iface goes down + if (num_pollfds == 0) + { + throw AllIfacesDownException(); + } + // Timeout conversion const std::int64_t timeout_usec = (blocking_deadline - clock_.getMonotonic()).toUSec(); auto ts = ::timespec(); @@ -637,75 +683,90 @@ public: } // Blocking here - const int res = ::ppoll(pollfds_, num_ifaces_, &ts, nullptr); + const int res = ::ppoll(pollfds, num_pollfds, &ts, nullptr); if (res < 0) { return res; } // Handling poll output - for (unsigned i = 0; i < num_ifaces_; i++) + for (unsigned i = 0; i < num_pollfds; i++) { - const bool poll_read = pollfds_[i].revents & POLLIN; - const bool poll_write = pollfds_[i].revents & POLLOUT; - ifaces_[i]->poll(poll_read, poll_write); + pollfd_index_to_iface[i]->updateDownStatusFromPollResult(pollfds[i]); + + const bool poll_read = pollfds[i].revents & POLLIN; + const bool poll_write = pollfds[i].revents & POLLOUT; + pollfd_index_to_iface[i]->poll(poll_read, poll_write); } } // Writing the output masks inout_masks = uavcan::CanSelectMasks(); - for (unsigned i = 0; i < num_ifaces_; i++) + for (unsigned i = 0; i < ifaces_.size(); i++) { - const std::uint8_t iface_mask = 1 << i; - inout_masks.write |= iface_mask; // Always ready to write + if (!ifaces_[i]->isDown()) + { + inout_masks.write |= std::uint8_t(1U << i); // Always ready to write if not down + } if (ifaces_[i]->hasReadyRx()) { - inout_masks.read |= iface_mask; + inout_masks.read |= std::uint8_t(1U << i); // Readability depends only on RX buf, even if down } } - // Since all ifaces are always ready to write, return value is always the same - return num_ifaces_; + + // Return value is irrelevant as long as it's non-negative + return ifaces_.size(); } SocketCanIface* getIface(std::uint8_t iface_index) override { - return (iface_index >= num_ifaces_) ? nullptr : static_cast(ifaces_[iface_index]); + return (iface_index >= ifaces_.size()) ? nullptr : ifaces_[iface_index].get(); } - std::uint8_t getNumIfaces() const override { return num_ifaces_; } + std::uint8_t getNumIfaces() const override { return ifaces_.size(); } /** * Adds one iface by name. Will fail if there are @ref MaxIfaces ifaces registered already. * @param iface_name E.g. "can0", "vcan1" - * @return Negative on error, zero on success. + * @return Negative on error, interface index on success. * @throws uavcan_linux::Exception. */ int addIface(const std::string& iface_name) { - if (num_ifaces_ >= MaxIfaces) + if (ifaces_.size() >= uavcan::MaxCanIfaces) { return -1; } + // Open the socket const int fd = SocketCanIface::openSocket(iface_name); if (fd < 0) { return fd; } + // Construct the iface - upon successful construction the iface will take ownership of the fd. try { - ifaces_[num_ifaces_].construct(clock_, fd); + ifaces_.emplace_back(new IfaceWrapper(clock_, fd)); } catch (...) { (void)::close(fd); throw; } - // Init pollfd - pollfds_[num_ifaces_].fd = fd; - num_ifaces_++; - return 0; + + UAVCAN_TRACE("SocketCAN", "New iface '%s' fd %d", iface_name.c_str(), fd); + + return ifaces_.size() - 1; + } + + /** + * Returns false if the specified interface is functioning, true if it became unavailable. + */ + bool isIfaceDown(std::uint8_t iface_index) const + { + return ifaces_.at(iface_index)->isDown(); } }; From 597e0b4356db14c8d70697c34aa2d68977a96e61 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 28 Jan 2016 00:50:28 +0300 Subject: [PATCH 2/4] SocketCAN: Proper error reporting from openSocket() --- .../linux/include/uavcan_linux/exception.hpp | 8 ++- .../linux/include/uavcan_linux/socketcan.hpp | 60 +++++++++++++------ 2 files changed, 49 insertions(+), 19 deletions(-) diff --git a/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp b/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp index fc8840ae59..e6fc8ba111 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp @@ -5,6 +5,7 @@ #pragma once #include +#include #include namespace uavcan_linux @@ -16,9 +17,14 @@ class Exception : public std::runtime_error { const int errno_; + static std::string makeErrorString(const std::string& descr) + { + return descr + " [errno " + std::to_string(errno) + " \"" + std::strerror(errno) + "\"]"; + } + public: explicit Exception(const std::string& descr) - : std::runtime_error(descr) + : std::runtime_error(makeErrorString(descr)) , errno_(errno) { } diff --git a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp index 1ebb0df045..cc08b6ae8c 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp @@ -381,6 +381,7 @@ public: */ virtual ~SocketCanIface() { + UAVCAN_TRACE("SocketCAN", "SocketCanIface: Closing fd %d", fd_); (void)::close(fd_); } @@ -515,68 +516,91 @@ public: */ static int openSocket(const std::string& iface_name) { + errno = 0; + const int s = ::socket(PF_CAN, SOCK_RAW, CAN_RAW); if (s < 0) { return s; } + + class RaiiCloser + { + int fd_; + public: + RaiiCloser(int filedesc) : fd_(filedesc) + { + assert(fd_ >= 0); + } + ~RaiiCloser() + { + if (fd_ >= 0) + { + UAVCAN_TRACE("SocketCAN", "RaiiCloser: Closing fd %d", fd_); + (void)::close(fd_); + } + } + void disarm() { fd_ = -1; } + } raii_closer(s); + // Detect the iface index auto ifr = ::ifreq(); if (iface_name.length() >= IFNAMSIZ) { - goto fail; + errno = ENAMETOOLONG; + return -1; } (void)std::strncpy(ifr.ifr_name, iface_name.c_str(), iface_name.length()); if (::ioctl(s, SIOCGIFINDEX, &ifr) < 0 || ifr.ifr_ifindex < 0) { - goto fail; + return -1; } - // Bind to a CAN iface + + // Bind to the specified CAN iface { auto addr = ::sockaddr_can(); addr.can_family = AF_CAN; addr.can_ifindex = ifr.ifr_ifindex; if (::bind(s, reinterpret_cast(&addr), sizeof(addr)) < 0) { - goto fail; + return -1; } } + // Configure { const int on = 1; // Timestamping if (::setsockopt(s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) { - goto fail; + return -1; } // Socket loopback if (::setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &on, sizeof(on)) < 0) { - goto fail; + return -1; } // Non-blocking if (::fcntl(s, F_SETFL, O_NONBLOCK) < 0) { - goto fail; + return -1; } } + // Validate the resulting socket { - int error = 0; - ::socklen_t errlen = sizeof(error); - (void)::getsockopt(s, SOL_SOCKET, SO_ERROR, reinterpret_cast(&error), &errlen); - if (error != 0) + int socket_error = 0; + ::socklen_t errlen = sizeof(socket_error); + (void)::getsockopt(s, SOL_SOCKET, SO_ERROR, reinterpret_cast(&socket_error), &errlen); + if (socket_error != 0) { - goto fail; - UAVCAN_TRACE("SocketCAN", "Socket error: iface='%s' errno=%d error=%d", - iface_name.c_str(), errno, error); + errno = socket_error; + return -1; } } - return s; - fail: - (void)::close(s); - return -1; + raii_closer.disarm(); + return s; } }; From 3d0186c54704ef5b408cd84d01f62a9d8ce6a718 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 28 Jan 2016 00:54:22 +0300 Subject: [PATCH 3/4] SocketCAN doc comments --- libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp index cc08b6ae8c..d56980bcc6 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp @@ -607,6 +607,12 @@ public: /** * Multiplexing container for multiple SocketCAN sockets. * Uses ppoll() for multiplexing. + * + * When an interface becomes down/disconnected while the node is running, + * the driver will silently exclude it from the IO loop and continue to run on the remaining interfaces. + * When all interfaces become down/disconnected, the driver will throw @ref AllIfacesDownException + * from @ref SocketCanDriver::select(). + * Whether a certain interface is down can be checked with @ref SocketCanDriver::isIfaceDown(). */ class SocketCanDriver : public uavcan::ICanDriver { From f86a4b98d199c7bdecaed1db463e79d6f719df2b Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 28 Jan 2016 01:06:29 +0300 Subject: [PATCH 4/4] SocketCAN: Setting correct errno when throwing AllIfacesDownException --- .../linux/include/uavcan_linux/exception.hpp | 12 ++++++------ 1 file changed, 6 insertions(+), 6 deletions(-) diff --git a/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp b/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp index e6fc8ba111..394da2a816 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/exception.hpp @@ -17,15 +17,15 @@ class Exception : public std::runtime_error { const int errno_; - static std::string makeErrorString(const std::string& descr) + static std::string makeErrorString(const std::string& descr, int use_errno) { - return descr + " [errno " + std::to_string(errno) + " \"" + std::strerror(errno) + "\"]"; + return descr + " [errno " + std::to_string(use_errno) + " \"" + std::strerror(use_errno) + "\"]"; } public: - explicit Exception(const std::string& descr) - : std::runtime_error(makeErrorString(descr)) - , errno_(errno) + explicit Exception(const std::string& descr, int use_errno = errno) + : std::runtime_error(makeErrorString(descr, use_errno)) + , errno_(use_errno) { } /** @@ -41,7 +41,7 @@ public: class AllIfacesDownException : public Exception { public: - AllIfacesDownException() : Exception("All ifaces are down") { } + AllIfacesDownException() : Exception("All ifaces are down", ENETDOWN) { } }; }