Finished Linux driver

libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp

@@ -18,6 +18,7 @@
 #include <net/if.h>
 #include <linux/can.h>
 #include <linux/can/raw.h>
+#include <poll.h>
 
 #include <uavcan/uavcan.hpp>
 #include <uavcan_linux/clock.hpp>
@@ -35,9 +36,9 @@ enum class SocketCanError
 
 /**
  * SocketCAN socket adapter maintains TX and RX queues in user space. At any moment socket's buffer contains
- * no more than one TX frame, rest is waiting in the application TX queue; when the socket procudes loopback for
+ * no more than one TX frame, rest is waiting in the user space TX queue; when the socket produces loopback for
- * previously sent TX frame the next frame from the user space TX queue will be sent to the socket. This approach
+ * the previously sent TX frame the next frame from the user space TX queue will be sent to the socket.
- * allows to properly maintain TX timeouts (http://stackoverflow.com/questions/19633015/).
+ * This approach allows to properly maintain TX timeouts (http://stackoverflow.com/questions/19633015/).
  * TX timestamping is implemented by means of reading RX timestamps of loopback frames (see "TX timestamping" on
  * linux-can mailing list, http://permalink.gmane.org/gmane.linux.can/5322).
  */
@@ -83,17 +84,33 @@ class SocketCanIface : public uavcan::ICanIface
     struct TxItem
     {
         uavcan::CanFrame frame;
-        uavcan::CanIOFlags flags;
         uavcan::MonotonicTime deadline;
+        uavcan::CanIOFlags flags;
+
+        TxItem()
+            : flags(0)
+        { }
+
+        TxItem(const uavcan::CanFrame& frame, uavcan::MonotonicTime deadline, uavcan::CanIOFlags flags)
+            : frame(frame)
+            , deadline(deadline)
+            , flags(flags)
+        { }
+
         bool operator<(const TxItem& rhs) const { return frame.priorityLowerThan(rhs.frame); }
     };
 
     struct RxItem
     {
         uavcan::CanFrame frame;
-        uavcan::CanIOFlags flags;
         uavcan::MonotonicTime ts_mono;
         uavcan::UtcTime ts_utc;
+        uavcan::CanIOFlags flags;
+
+        RxItem()
+            : flags(0)
+        { }
+
         bool operator<(const RxItem& rhs) const { return frame.priorityLowerThan(rhs.frame); }
     };
 
@@ -254,10 +271,21 @@ class SocketCanIface : public uavcan::ICanIface
     }
 
 public:
-    explicit SocketCanIface(int fd)
+    /**
-        : fd_(fd)
+     * Takes ownership of socket's file descriptor.
+     * @param fd
+     */
+    explicit SocketCanIface(int socket_fd)
+        : fd_(socket_fd)
         , has_pending_write_(false)
-    { }
+    {
+        assert(fd_ >= 0);
+    }
+
+    virtual ~SocketCanIface()
+    {
+        (void)::close(fd_);
+    }
 
     /**
      * Assumes that the socket is writeable
@@ -265,26 +293,34 @@ public:
     virtual std::int16_t send(const uavcan::CanFrame& frame, const uavcan::MonotonicTime tx_deadline,
                               const uavcan::CanIOFlags flags)
     {
-        tx_queue_.push({ frame, flags, tx_deadline });
+        tx_queue_.emplace(frame, tx_deadline, flags);
+        pollRead();     // Read poll is necessary because it can release the pending TX flag
         pollWrite();
         return 1;
     }
 
     /**
-     * Does not read from the socket, but from the RX queue. Thus, pollRead() must be executed first.
+     * Will read the socket only if RX queue is empty.
+     * Normally, poll() needs to be executed first.
      */
     virtual std::int16_t receive(uavcan::CanFrame& out_frame, uavcan::MonotonicTime& out_ts_monotonic,
                                  uavcan::UtcTime& out_ts_utc, uavcan::CanIOFlags& out_flags)
     {
         if (rx_queue_.empty())
         {
-            return 0;
+            pollRead();            // This allows to use the socket not calling poll() explicitly.
+            if (rx_queue_.empty())
+            {
+                return 0;
+            }
+        }
+        {
+            const RxItem& rx = rx_queue_.top();
+            out_frame        = rx.frame;
+            out_ts_monotonic = rx.ts_mono;
+            out_ts_utc       = rx.ts_utc;
+            out_flags        = rx.flags;
         }
-        const RxItem& rx = rx_queue_.top();
-        out_frame = rx.frame;
-        out_ts_monotonic = rx.ts_mono;
-        out_ts_utc = rx.ts_utc;
-        out_flags = rx.flags;
         rx_queue_.pop();
         assert(rx_queue_.empty() ? true : !out_frame.priorityLowerThan(rx_queue_.top().frame)); // Order check
         return 1;
@@ -429,24 +465,110 @@ public:
 
 private:
     const SystemClock clock_;
+    uavcan::LazyConstructor<SocketCanIface> ifaces_[MaxIfaces];
+    ::pollfd pollfds_[MaxIfaces];
+    std::uint8_t num_ifaces_;
 
 public:
-    virtual SocketCanIface* getIface(std::uint8_t iface_index)
+    SocketCanDriver()
+        : num_ifaces_(0)
     {
-        (void)iface_index;
+        for (auto& p : pollfds_)
-        return nullptr; // FIXME not implemented yet
+        {
-    }
+            p = ::pollfd();
-
+            p.fd = -1;
-    virtual std::uint8_t getNumIfaces() const
+        }
-    {
-        return -1; // FIXME not implemented yet
     }
 
+    /**
+     * This function may return before deadline expiration even if no requested IO operations become possible.
+     * This behavior makes implementation way simpler, and it is OK since uavcan can properly handle such
+     * early returns.
+     * Also it can return more events that were originally requested by uavcan, which is also acceptable.
+     */
     virtual std::int16_t select(uavcan::CanSelectMasks& inout_masks, uavcan::MonotonicTime blocking_deadline)
     {
-        (void)inout_masks;
+        // Poll FD set setup
-        (void)blocking_deadline;
+        for (unsigned i = 0; i < num_ifaces_; i++)
-        return -1; // FIXME not implemented yet
+        {
+            pollfds_[i].events = POLLIN;
+            if (ifaces_[i]->hasPendingTx() || (inout_masks.write & (1 << i)))
+            {
+                pollfds_[i].events |= POLLOUT;
+            }
+        }
+        // Blocking poll
+        {
+            const std::int64_t timeout_usec = (blocking_deadline - clock_.getMonotonic()).toUSec();
+            auto ts = ::timespec();
+            if (timeout_usec > 0)
+            {
+                ts.tv_sec = timeout_usec / 1000000LL;
+                ts.tv_nsec = (timeout_usec % 1000000LL) * 1000;
+            }
+            const int res = ::ppoll(pollfds_, num_ifaces_, &ts, nullptr);
+            if (res < 0)
+            {
+                return res;
+            }
+        }
+        // Handling
+        inout_masks = uavcan::CanSelectMasks();
+        for (unsigned i = 0; i < num_ifaces_; i++)
+        {
+            const bool poll_read  = pollfds_[i].revents & POLLIN;
+            const bool poll_write = pollfds_[i].revents & POLLOUT;
+            ifaces_[i]->poll(poll_read, poll_write);
+
+            const std::uint8_t iface_mask = 1 << i;
+            inout_masks.write |= iface_mask;           // Always ready to write
+            if (ifaces_[i]->hasReadyRx())
+            {
+                inout_masks.read |= iface_mask;
+            }
+        }
+        // Since all ifaces are always ready to write, return value is always the same
+        return num_ifaces_;
+    }
+
+    virtual SocketCanIface* getIface(std::uint8_t iface_index)
+    {
+        return (iface_index >= num_ifaces_) ? nullptr : static_cast<SocketCanIface*>(ifaces_[iface_index]);
+    }
+
+    virtual std::uint8_t getNumIfaces() const { return num_ifaces_; }
+
+    /**
+     * 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.
+     */
+    int addIface(const std::string& iface_name)
+    {
+        if (num_ifaces_ >= MaxIfaces)
+        {
+            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<int>(fd);
+        }
+        catch (...)
+        {
+            (void)::close(fd);
+            throw;
+        }
+        // Init pollfd
+        pollfds_[num_ifaces_].fd = fd;
+        num_ifaces_++;
+        return 0;
     }
 };
 

libuavcan_drivers/linux/test/test_socket.cpp

@@ -3,10 +3,15 @@
  */
 
 #include <iostream>
+#include <vector>
 #include <cerrno>
 #include <uavcan_linux/uavcan_linux.hpp>
 
-#define ASSERT(x) if (!(x)) { throw std::runtime_error(#x); }
+#ifndef STRINGIZE
+#  define STRINGIZE2(x)   #x
+#  define STRINGIZE(x)    STRINGIZE2(x)
+#endif
+#define ENFORCE(x) if (!(x)) { throw std::runtime_error(__FILE__ ":" STRINGIZE(__LINE__) ": " #x); }
 
 static uavcan::CanFrame makeFrame(std::uint32_t id, const std::string& data)
 {
@@ -21,16 +26,16 @@ static uavcan::MonotonicTime tsMonoOffsetMs(std::int64_t ms)
 static void testNonexistentIface()
 {
     const int sock1 = uavcan_linux::SocketCanIface::openSocket("noif9");
-    ASSERT(sock1 < 0);
+    ENFORCE(sock1 < 0);
     const int sock2 = uavcan_linux::SocketCanIface::openSocket("verylongifacenameverylongifacenameverylongifacename");
-    ASSERT(sock2 < 0);
+    ENFORCE(sock2 < 0);
 }
 
 static void testSocketRxTx(const std::string& iface_name)
 {
     const int sock1 = uavcan_linux::SocketCanIface::openSocket(iface_name);
     const int sock2 = uavcan_linux::SocketCanIface::openSocket(iface_name);
-    ASSERT(sock1 >= 0 && sock2 >= 0);
+    ENFORCE(sock1 >= 0 && sock2 >= 0);
 
     uavcan_linux::SocketCanIface if1(sock1);
     uavcan_linux::SocketCanIface if2(sock2);
@@ -38,27 +43,25 @@ static void testSocketRxTx(const std::string& iface_name)
     /*
      * Sending two frames, one of which must be returned back
      */
-    ASSERT(1 == if1.send(makeFrame(123, "if1-1"), tsMonoOffsetMs(100), 0));
+    ENFORCE(1 == if1.send(makeFrame(123, "if1-1"), tsMonoOffsetMs(100), 0));
-    ASSERT(1 == if1.send(makeFrame(456, "if1-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback));
+    ENFORCE(1 == if1.send(makeFrame(456, "if1-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback));
-    ASSERT(if1.hasPendingTx());
+    if1.poll(true, true);
-    if1.poll(true, true);    // Reads confirmation for the first, writes the second
+    if1.poll(true, true);
-    if1.poll(true, true);    // Reads confirmation for the second and stores it in RX queue, writes nothing
+    ENFORCE(0 == if1.getErrorCount());
-    ASSERT(0 == if1.getErrorCount());
+    ENFORCE(!if1.hasPendingTx());
-    ASSERT(!if1.hasPendingTx());
+    ENFORCE(if1.hasReadyRx());       // Second loopback
-    ASSERT(if1.hasReadyRx());       // Second loopback
 
     /*
      * Second iface, same thing
      */
-    ASSERT(1 == if2.send(makeFrame(321, "if2-1"), tsMonoOffsetMs(100), 0));
+    ENFORCE(1 == if2.send(makeFrame(321, "if2-1"), tsMonoOffsetMs(100), 0));
-    ASSERT(1 == if2.send(makeFrame(654, "if2-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback));
+    ENFORCE(1 == if2.send(makeFrame(654, "if2-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback));
-    ASSERT(1 == if2.send(makeFrame(1, "discard"), tsMonoOffsetMs(0), uavcan::CanIOFlagLoopback));  // Will timeout
+    ENFORCE(1 == if2.send(makeFrame(1, "discard"), tsMonoOffsetMs(0), uavcan::CanIOFlagLoopback));  // Will timeout
-    ASSERT(if2.hasPendingTx());
+    if2.poll(true, true);
-    if2.poll(true, true);    // Reads confirmation for the first, writes the second
+    if2.poll(true, true);
-    if2.poll(true, true);    // Reads confirmation for the second and stores it in RX queue, writes nothing
+    ENFORCE(1 == if2.getErrorCount());  // One timed out
-    ASSERT(1 == if2.getErrorCount());  // One timed out
+    ENFORCE(!if2.hasPendingTx());
-    ASSERT(!if2.hasPendingTx());
+    ENFORCE(if2.hasReadyRx());
-    ASSERT(if2.hasReadyRx());
 
     /*
      * No-op
@@ -76,54 +79,54 @@ static void testSocketRxTx(const std::string& iface_name)
     /*
      * Read first
      */
-    ASSERT(1 == if1.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags));
-    ASSERT(frame == makeFrame(321, "if2-1"));
+    ENFORCE(frame == makeFrame(321, "if2-1"));
-    ASSERT(flags == 0);
+    ENFORCE(flags == 0);
-    ASSERT(!ts_mono.isZero());
+    ENFORCE(!ts_mono.isZero());
-    ASSERT(!ts_utc.isZero());
+    ENFORCE(!ts_utc.isZero());
-    ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+    ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
-    ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+    ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
 
-    ASSERT(1 == if1.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags));
-    ASSERT(frame == makeFrame(456, "if1-2"));
+    ENFORCE(frame == makeFrame(456, "if1-2"));
-    ASSERT(flags == uavcan::CanIOFlagLoopback);
+    ENFORCE(flags == uavcan::CanIOFlagLoopback);
-    ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+    ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
-    ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+    ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
 
-    ASSERT(1 == if1.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags));
-    ASSERT(frame == makeFrame(654, "if2-2"));
+    ENFORCE(frame == makeFrame(654, "if2-2"));
-    ASSERT(flags == 0);
+    ENFORCE(flags == 0);
-    ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+    ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
-    ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+    ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
 
-    ASSERT(0 == if1.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(0 == if1.receive(frame, ts_mono, ts_utc, flags));
-    ASSERT(!if1.hasPendingTx());
+    ENFORCE(!if1.hasPendingTx());
-    ASSERT(!if1.hasReadyRx());
+    ENFORCE(!if1.hasReadyRx());
 
     /*
      * Read second
      */
-    ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
-    ASSERT(frame == makeFrame(123, "if1-1"));
+    ENFORCE(frame == makeFrame(123, "if1-1"));
-    ASSERT(flags == 0);
+    ENFORCE(flags == 0);
-    ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+    ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
-    ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+    ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
 
-    ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
-    ASSERT(frame == makeFrame(456, "if1-2"));
+    ENFORCE(frame == makeFrame(456, "if1-2"));
-    ASSERT(flags == 0);
+    ENFORCE(flags == 0);
-    ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+    ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
-    ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+    ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
 
-    ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
-    ASSERT(frame == makeFrame(654, "if2-2"));
+    ENFORCE(frame == makeFrame(654, "if2-2"));
-    ASSERT(flags == uavcan::CanIOFlagLoopback);
+    ENFORCE(flags == uavcan::CanIOFlagLoopback);
-    ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+    ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
-    ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+    ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
 
-    ASSERT(0 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(0 == if2.receive(frame, ts_mono, ts_utc, flags));
-    ASSERT(!if2.hasPendingTx());
+    ENFORCE(!if2.hasPendingTx());
-    ASSERT(!if2.hasReadyRx());
+    ENFORCE(!if2.hasReadyRx());
 }
 
 static void testSocketFilters(const std::string& iface_name)
@@ -132,7 +135,7 @@ static void testSocketFilters(const std::string& iface_name)
 
     const int sock1 = uavcan_linux::SocketCanIface::openSocket(iface_name);
     const int sock2 = uavcan_linux::SocketCanIface::openSocket(iface_name);
-    ASSERT(sock1 >= 0 && sock2 >= 0);
+    ENFORCE(sock1 >= 0 && sock2 >= 0);
 
     uavcan_linux::SocketCanIface if1(sock1);
     uavcan_linux::SocketCanIface if2(sock2);
@@ -151,29 +154,29 @@ static void testSocketFilters(const std::string& iface_name)
     fcs[2].id = 0;
     fcs[2].mask = CanFrame::MaskExtID | CanFrame::FlagEFF;
 
-    ASSERT(0 == if2.configureFilters(fcs, 3));
+    ENFORCE(0 == if2.configureFilters(fcs, 3));
 
     /*
      * Sending data from 1 to 2, making sure only filtered data will be accepted
      */
     const auto EFF = CanFrame::FlagEFF;
-    ASSERT(1 == if1.send(makeFrame(123,          "1"), tsMonoOffsetMs(100), 0)); // Accept 0
+    ENFORCE(1 == if1.send(makeFrame(123,          "1"), tsMonoOffsetMs(100), 0)); // Accept 0
-    ASSERT(1 == if1.send(makeFrame(123 | EFF,    "2"), tsMonoOffsetMs(100), 0)); // Accept 0
+    ENFORCE(1 == if1.send(makeFrame(123 | EFF,    "2"), tsMonoOffsetMs(100), 0)); // Accept 0
-    ASSERT(1 == if1.send(makeFrame(456,          "3"), tsMonoOffsetMs(100), 0)); // Drop
+    ENFORCE(1 == if1.send(makeFrame(456,          "3"), tsMonoOffsetMs(100), 0)); // Drop
-    ASSERT(1 == if1.send(makeFrame(456789,       "4"), tsMonoOffsetMs(100), 0)); // Drop
+    ENFORCE(1 == if1.send(makeFrame(456789,       "4"), tsMonoOffsetMs(100), 0)); // Drop
-    ASSERT(1 == if1.send(makeFrame(456789 | EFF, "5"), tsMonoOffsetMs(100), 0)); // Accept 1
+    ENFORCE(1 == if1.send(makeFrame(456789 | EFF, "5"), tsMonoOffsetMs(100), 0)); // Accept 1
-    ASSERT(1 == if1.send(makeFrame(0,            "6"), tsMonoOffsetMs(100), 0)); // Accept 2
+    ENFORCE(1 == if1.send(makeFrame(0,            "6"), tsMonoOffsetMs(100), 0)); // Accept 2
-    ASSERT(1 == if1.send(makeFrame(EFF,          "7"), tsMonoOffsetMs(100), 0)); // Drop
+    ENFORCE(1 == if1.send(makeFrame(EFF,          "7"), tsMonoOffsetMs(100), 0)); // Drop
 
     for (int i = 0; i < 7; i++)
     {
         if1.poll(true, true);
         if2.poll(true, false);
     }
-    ASSERT(!if1.hasPendingTx());
+    ENFORCE(!if1.hasPendingTx());
-    ASSERT(!if1.hasReadyRx());
+    ENFORCE(!if1.hasReadyRx());
-    ASSERT(0 == if1.getErrorCount());
+    ENFORCE(0 == if1.getErrorCount());
-    ASSERT(if2.hasReadyRx());
+    ENFORCE(if2.hasReadyRx());
 
     /*
      * Checking RX on 2
@@ -184,33 +187,124 @@ static void testSocketFilters(const std::string& iface_name)
     uavcan::UtcTime ts_utc;
     uavcan::CanIOFlags flags = 0;
 
-    ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
-    ASSERT(frame == makeFrame(0, "6"));
+    ENFORCE(frame == makeFrame(0, "6"));
-    ASSERT(flags == 0);
+    ENFORCE(flags == 0);
 
-    ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
-    ASSERT(frame == makeFrame(123 | EFF, "2"));
+    ENFORCE(frame == makeFrame(123 | EFF, "2"));
 
-    ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
-    ASSERT(frame == makeFrame(456789 | EFF, "5"));
+    ENFORCE(frame == makeFrame(456789 | EFF, "5"));
 
-    ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags));
+    ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags));
-    ASSERT(frame == makeFrame(123, "1"));
+    ENFORCE(frame == makeFrame(123, "1"));
 
-    ASSERT(!if2.hasReadyRx());
+    ENFORCE(!if2.hasReadyRx());
+}
+
+static void testDriver(const std::vector<std::string>& iface_names)
+{
+    uavcan_linux::SocketCanDriver driver;
+    for (auto ifn : iface_names)
+    {
+        std::cout << "Adding iface " << ifn << std::endl;
+        ENFORCE(0 == driver.addIface(ifn));
+    }
+
+    ENFORCE(-1 == driver.addIface("noif9"));
+    ENFORCE(-1 == driver.addIface("noif9"));
+    ENFORCE(-1 == driver.addIface("noif9"));
+
+    ENFORCE(driver.getNumIfaces() == iface_names.size());
+    ENFORCE(nullptr == driver.getIface(255));
+    ENFORCE(nullptr == driver.getIface(driver.getNumIfaces()));
+
+    const unsigned AllIfacesMask = (1 << driver.getNumIfaces()) - 1;
+
+    /*
+     * Send, no loopback
+     */
+    std::cout << "select() 1" << std::endl;
+    uavcan::CanSelectMasks masks;        // Driver provides masks for all available events
+    ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
+    ENFORCE(masks.read == 0);
+    ENFORCE(masks.write == AllIfacesMask);
+
+    for (int i = 0; i < driver.getNumIfaces(); i++)
+    {
+        ENFORCE(1 == driver.getIface(i)->send(makeFrame(123, std::to_string(i)), tsMonoOffsetMs(10), 0));
+    }
+
+    std::cout << "select() 2" << std::endl;
+    ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
+    ENFORCE(masks.read == 0);
+    ENFORCE(masks.write == AllIfacesMask);
+
+    /*
+     * Send with loopback
+     */
+    for (int i = 0; i < driver.getNumIfaces(); i++)
+    {
+        ENFORCE(1 == driver.getIface(i)->send(makeFrame(456, std::to_string(i)), tsMonoOffsetMs(10),
+                                              uavcan::CanIOFlagLoopback));
+        ENFORCE(1 == driver.getIface(i)->send(makeFrame(789, std::to_string(i)), tsMonoOffsetMs(-1), // Will timeout
+                                              uavcan::CanIOFlagLoopback));
+    }
+
+    std::cout << "select() 3" << std::endl;
+    ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
+    ENFORCE(masks.read == AllIfacesMask);
+    ENFORCE(masks.write == AllIfacesMask);
+
+    /*
+     * Receive loopback
+     */
+    const uavcan_linux::SystemClock clock(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate);
+    for (int i = 0; i < driver.getNumIfaces(); i++)
+    {
+        uavcan::CanFrame frame;
+        uavcan::MonotonicTime ts_mono;
+        uavcan::UtcTime ts_utc;
+        uavcan::CanIOFlags flags = 0;
+        ENFORCE(1 == driver.getIface(i)->receive(frame, ts_mono, ts_utc, flags));
+        ENFORCE(frame == makeFrame(456, std::to_string(i)));
+        ENFORCE(flags == uavcan::CanIOFlagLoopback);
+        ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10);
+        ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10);
+
+        ENFORCE(!driver.getIface(i)->hasPendingTx());
+        ENFORCE(!driver.getIface(i)->hasReadyRx());
+    }
+
+    std::cout << "select() 4" << std::endl;
+    masks.write = 0;
+    ENFORCE(driver.getNumIfaces() == driver.select(masks, tsMonoOffsetMs(1000)));
+    ENFORCE(masks.read == 0);
+    ENFORCE(masks.write == AllIfacesMask);
+
+    std::cout << "exit" << std::endl;
 }
 
 int main(int argc, const char** argv)
 {
     if (argc < 2)
     {
-        std::cout << "Usage:\n\t" << argv[0] << " <can-iface-name>" << std::endl;
+        std::cout << "Usage:\n\t" << argv[0] << " <can-iface-name-1> [can-iface-name-N...]" << std::endl;
         return 1;
     }
 
+    std::vector<std::string> iface_names;
+    for (int i = 1; i < argc; i++)
+    {
+        iface_names.emplace_back(argv[i]);
+    }
+
     testNonexistentIface();
-    testSocketRxTx(argv[1]);
+    testSocketRxTx(iface_names[0]);
-    testSocketFilters(argv[1]);
+    testSocketFilters(iface_names[0]);
+
+    testDriver(iface_names);
 
     return 0;
 }