From 587088bb187e6e279711f451020988eeefaa0782 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Thu, 28 Jan 2016 00:12:27 +0300 Subject: [PATCH] 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(); } };