From 39933ba41de2dc0b226d2ecb8ee735ecf97a7653 Mon Sep 17 00:00:00 2001 From: Pavel Kirienko Date: Mon, 31 Mar 2014 17:13:33 +0400 Subject: [PATCH] Finished Linux driver --- .../linux/include/uavcan_linux/socketcan.hpp | 176 ++++++++++-- libuavcan_drivers/linux/test/test_socket.cpp | 264 ++++++++++++------ 2 files changed, 328 insertions(+), 112 deletions(-) diff --git a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp index 3ee82a83fb..3e101b1827 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include @@ -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 - * previously sent TX frame the next frame from the user space TX queue will be sent to the socket. This approach - * allows to properly maintain TX timeouts (http://stackoverflow.com/questions/19633015/). + * no more than one TX frame, rest is waiting in the user space TX queue; when the socket produces loopback for + * the previously sent TX frame the next frame from the user space TX queue will be sent to the socket. + * 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 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; - return nullptr; // FIXME not implemented yet - } - - virtual std::uint8_t getNumIfaces() const - { - return -1; // FIXME not implemented yet + for (auto& p : pollfds_) + { + p = ::pollfd(); + p.fd = -1; + } } + /** + * 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; - (void)blocking_deadline; - return -1; // FIXME not implemented yet + // Poll FD set setup + for (unsigned i = 0; i < num_ifaces_; i++) + { + 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(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(fd); + } + catch (...) + { + (void)::close(fd); + throw; + } + // Init pollfd + pollfds_[num_ifaces_].fd = fd; + num_ifaces_++; + return 0; } }; diff --git a/libuavcan_drivers/linux/test/test_socket.cpp b/libuavcan_drivers/linux/test/test_socket.cpp index 50052866d8..b6007c5698 100644 --- a/libuavcan_drivers/linux/test/test_socket.cpp +++ b/libuavcan_drivers/linux/test/test_socket.cpp @@ -3,10 +3,15 @@ */ #include +#include #include #include -#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)); - ASSERT(1 == if1.send(makeFrame(456, "if1-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback)); - ASSERT(if1.hasPendingTx()); - if1.poll(true, true); // Reads confirmation for the first, writes the second - if1.poll(true, true); // Reads confirmation for the second and stores it in RX queue, writes nothing - ASSERT(0 == if1.getErrorCount()); - ASSERT(!if1.hasPendingTx()); - ASSERT(if1.hasReadyRx()); // Second loopback + ENFORCE(1 == if1.send(makeFrame(123, "if1-1"), tsMonoOffsetMs(100), 0)); + ENFORCE(1 == if1.send(makeFrame(456, "if1-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback)); + if1.poll(true, true); + if1.poll(true, true); + ENFORCE(0 == if1.getErrorCount()); + ENFORCE(!if1.hasPendingTx()); + ENFORCE(if1.hasReadyRx()); // Second loopback /* * Second iface, same thing */ - ASSERT(1 == if2.send(makeFrame(321, "if2-1"), tsMonoOffsetMs(100), 0)); - ASSERT(1 == if2.send(makeFrame(654, "if2-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback)); - ASSERT(1 == if2.send(makeFrame(1, "discard"), tsMonoOffsetMs(0), uavcan::CanIOFlagLoopback)); // Will timeout - ASSERT(if2.hasPendingTx()); - if2.poll(true, true); // Reads confirmation for the first, writes the second - if2.poll(true, true); // Reads confirmation for the second and stores it in RX queue, writes nothing - ASSERT(1 == if2.getErrorCount()); // One timed out - ASSERT(!if2.hasPendingTx()); - ASSERT(if2.hasReadyRx()); + ENFORCE(1 == if2.send(makeFrame(321, "if2-1"), tsMonoOffsetMs(100), 0)); + ENFORCE(1 == if2.send(makeFrame(654, "if2-2"), tsMonoOffsetMs(100), uavcan::CanIOFlagLoopback)); + ENFORCE(1 == if2.send(makeFrame(1, "discard"), tsMonoOffsetMs(0), uavcan::CanIOFlagLoopback)); // Will timeout + if2.poll(true, true); + if2.poll(true, true); + ENFORCE(1 == if2.getErrorCount()); // One timed out + ENFORCE(!if2.hasPendingTx()); + ENFORCE(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)); - ASSERT(frame == makeFrame(321, "if2-1")); - ASSERT(flags == 0); - ASSERT(!ts_mono.isZero()); - ASSERT(!ts_utc.isZero()); - ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10); - ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10); + ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags)); + ENFORCE(frame == makeFrame(321, "if2-1")); + ENFORCE(flags == 0); + ENFORCE(!ts_mono.isZero()); + ENFORCE(!ts_utc.isZero()); + ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10); + ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10); - ASSERT(1 == if1.receive(frame, ts_mono, ts_utc, flags)); - ASSERT(frame == makeFrame(456, "if1-2")); - ASSERT(flags == uavcan::CanIOFlagLoopback); - ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10); - ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10); + ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags)); + ENFORCE(frame == makeFrame(456, "if1-2")); + ENFORCE(flags == uavcan::CanIOFlagLoopback); + ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10); + ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10); - ASSERT(1 == if1.receive(frame, ts_mono, ts_utc, flags)); - ASSERT(frame == makeFrame(654, "if2-2")); - ASSERT(flags == 0); - ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10); - ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10); + ENFORCE(1 == if1.receive(frame, ts_mono, ts_utc, flags)); + ENFORCE(frame == makeFrame(654, "if2-2")); + ENFORCE(flags == 0); + ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10); + ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10); - ASSERT(0 == if1.receive(frame, ts_mono, ts_utc, flags)); - ASSERT(!if1.hasPendingTx()); - ASSERT(!if1.hasReadyRx()); + ENFORCE(0 == if1.receive(frame, ts_mono, ts_utc, flags)); + ENFORCE(!if1.hasPendingTx()); + ENFORCE(!if1.hasReadyRx()); /* * Read second */ - ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags)); - ASSERT(frame == makeFrame(123, "if1-1")); - ASSERT(flags == 0); - ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10); - ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10); + ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags)); + ENFORCE(frame == makeFrame(123, "if1-1")); + ENFORCE(flags == 0); + ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10); + ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10); - ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags)); - ASSERT(frame == makeFrame(456, "if1-2")); - ASSERT(flags == 0); - ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10); - ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10); + ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags)); + ENFORCE(frame == makeFrame(456, "if1-2")); + ENFORCE(flags == 0); + ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10); + ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10); - ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags)); - ASSERT(frame == makeFrame(654, "if2-2")); - ASSERT(flags == uavcan::CanIOFlagLoopback); - ASSERT((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10); - ASSERT((clock.getUtc() - ts_utc).getAbs().toMSec() < 10); + ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags)); + ENFORCE(frame == makeFrame(654, "if2-2")); + ENFORCE(flags == uavcan::CanIOFlagLoopback); + ENFORCE((clock.getMonotonic() - ts_mono).getAbs().toMSec() < 10); + ENFORCE((clock.getUtc() - ts_utc).getAbs().toMSec() < 10); - ASSERT(0 == if2.receive(frame, ts_mono, ts_utc, flags)); - ASSERT(!if2.hasPendingTx()); - ASSERT(!if2.hasReadyRx()); + ENFORCE(0 == if2.receive(frame, ts_mono, ts_utc, flags)); + ENFORCE(!if2.hasPendingTx()); + 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 - ASSERT(1 == if1.send(makeFrame(123 | EFF, "2"), tsMonoOffsetMs(100), 0)); // Accept 0 - ASSERT(1 == if1.send(makeFrame(456, "3"), tsMonoOffsetMs(100), 0)); // Drop - ASSERT(1 == if1.send(makeFrame(456789, "4"), tsMonoOffsetMs(100), 0)); // Drop - ASSERT(1 == if1.send(makeFrame(456789 | EFF, "5"), tsMonoOffsetMs(100), 0)); // Accept 1 - ASSERT(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(123, "1"), tsMonoOffsetMs(100), 0)); // Accept 0 + ENFORCE(1 == if1.send(makeFrame(123 | EFF, "2"), tsMonoOffsetMs(100), 0)); // Accept 0 + ENFORCE(1 == if1.send(makeFrame(456, "3"), tsMonoOffsetMs(100), 0)); // Drop + ENFORCE(1 == if1.send(makeFrame(456789, "4"), tsMonoOffsetMs(100), 0)); // Drop + ENFORCE(1 == if1.send(makeFrame(456789 | EFF, "5"), tsMonoOffsetMs(100), 0)); // Accept 1 + ENFORCE(1 == if1.send(makeFrame(0, "6"), tsMonoOffsetMs(100), 0)); // Accept 2 + 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()); - ASSERT(!if1.hasReadyRx()); - ASSERT(0 == if1.getErrorCount()); - ASSERT(if2.hasReadyRx()); + ENFORCE(!if1.hasPendingTx()); + ENFORCE(!if1.hasReadyRx()); + ENFORCE(0 == if1.getErrorCount()); + 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)); - ASSERT(frame == makeFrame(0, "6")); - ASSERT(flags == 0); + ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags)); + ENFORCE(frame == makeFrame(0, "6")); + ENFORCE(flags == 0); - ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags)); - ASSERT(frame == makeFrame(123 | EFF, "2")); + ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags)); + ENFORCE(frame == makeFrame(123 | EFF, "2")); - ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags)); - ASSERT(frame == makeFrame(456789 | EFF, "5")); + ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags)); + ENFORCE(frame == makeFrame(456789 | EFF, "5")); - ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags)); - ASSERT(frame == makeFrame(123, "1")); + ENFORCE(1 == if2.receive(frame, ts_mono, ts_utc, flags)); + ENFORCE(frame == makeFrame(123, "1")); - ASSERT(!if2.hasReadyRx()); + ENFORCE(!if2.hasReadyRx()); +} + +static void testDriver(const std::vector& 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] << " " << std::endl; + std::cout << "Usage:\n\t" << argv[0] << " [can-iface-name-N...]" << std::endl; return 1; } + std::vector iface_names; + for (int i = 1; i < argc; i++) + { + iface_names.emplace_back(argv[i]); + } + testNonexistentIface(); - testSocketRxTx(argv[1]); - testSocketFilters(argv[1]); + testSocketRxTx(iface_names[0]); + testSocketFilters(iface_names[0]); + + testDriver(iface_names); return 0; }