Merge pull request #84 from UAVCAN/socketcan_select_refactoring

Refactored SocketCAN select() call; fixes #83
This commit is contained in:
Pavel Kirienko
2016-01-28 01:18:04 +03:00
3 changed files with 160 additions and 50 deletions
+4
View File
@@ -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.
@@ -5,6 +5,7 @@
#pragma once
#include <cerrno>
#include <cstring>
#include <stdexcept>
namespace uavcan_linux
@@ -16,10 +17,15 @@ class Exception : public std::runtime_error
{
const int errno_;
static std::string makeErrorString(const std::string& descr, int use_errno)
{
return descr + " [errno " + std::to_string(use_errno) + " \"" + std::strerror(use_errno) + "\"]";
}
public:
explicit Exception(const std::string& descr)
: std::runtime_error(descr)
, errno_(errno)
explicit Exception(const std::string& descr, int use_errno = errno)
: std::runtime_error(makeErrorString(descr, use_errno))
, errno_(use_errno)
{ }
/**
@@ -29,4 +35,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", ENETDOWN) { }
};
}
@@ -11,6 +11,7 @@
#include <vector>
#include <map>
#include <unordered_set>
#include <memory>
#include <algorithm>
#include <fcntl.h>
@@ -25,6 +26,7 @@
#include <uavcan_linux/clock.hpp>
#include <uavcan_linux/exception.hpp>
namespace uavcan_linux
{
/**
@@ -333,6 +335,7 @@ class SocketCanIface : public uavcan::ICanIface
else
{
registerError(SocketCanError::SocketReadFailure);
break;
}
}
}
@@ -378,6 +381,7 @@ public:
*/
virtual ~SocketCanIface()
{
UAVCAN_TRACE("SocketCAN", "SocketCanIface: Closing fd %d", fd_);
(void)::close(fd_);
}
@@ -512,73 +516,133 @@ 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<sockaddr*>(&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;
}
}
return s;
fail:
(void)::close(s);
return -1;
// Validate the resulting socket
{
int socket_error = 0;
::socklen_t errlen = sizeof(socket_error);
(void)::getsockopt(s, SOL_SOCKET, SO_ERROR, reinterpret_cast<void*>(&socket_error), &errlen);
if (socket_error != 0)
{
errno = socket_error;
return -1;
}
}
raii_closer.disarm();
return s;
}
};
/**
* 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
{
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<void*>(&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<SocketCanIface> ifaces_[MaxIfaces];
::pollfd pollfds_[MaxIfaces];
std::uint8_t num_ifaces_ = 0;
std::vector<std::unique_ptr<IfaceWrapper>> ifaces_;
public:
/**
@@ -587,11 +651,7 @@ public:
explicit SocketCanDriver(const SystemClock& clock)
: clock_(clock)
{
for (auto& p : pollfds_)
{
p = ::pollfd();
p.fd = -1;
}
ifaces_.reserve(uavcan::MaxCanIfaces);
}
/**
@@ -606,7 +666,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 +678,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 +713,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<SocketCanIface*>(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<const SystemClock&, int>(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();
}
};