diff --git a/libuavcan_drivers/linux/CMakeLists.txt b/libuavcan_drivers/linux/CMakeLists.txt index d4b9c4e567..ba115a62ed 100644 --- a/libuavcan_drivers/linux/CMakeLists.txt +++ b/libuavcan_drivers/linux/CMakeLists.txt @@ -16,8 +16,10 @@ install(DIRECTORY include/uavcan_linux DESTINATION include) # find_library(UAVCAN_LIB uavcan REQUIRED) include_directories(include) -set(CMAKE_CXX_FLAGS "-Wall -Wextra -Werror -pedantic -std=c++0x -g3 -O1") # GCC or Clang +set(CMAKE_CXX_FLAGS "-Wall -Wextra -Werror -pedantic -std=c++0x -g3") # GCC or Clang add_executable(test_clock test/test_clock.cpp) -target_link_libraries(test_clock ${UAVCAN_LIB}) -target_link_libraries(test_clock rt) +target_link_libraries(test_clock ${UAVCAN_LIB} rt) + +add_executable(test_socket test/test_socket.cpp) +target_link_libraries(test_socket ${UAVCAN_LIB} rt) diff --git a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp index 8d1c8b69c3..3ee82a83fb 100644 --- a/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp +++ b/libuavcan_drivers/linux/include/uavcan_linux/socketcan.hpp @@ -1 +1,453 @@ - +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +namespace uavcan_linux +{ + +enum class SocketCanError +{ + SocketReadFailure, + SocketWriteFailure, + TxTimeout +}; + +/** + * 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/). + * 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). + */ +class SocketCanIface : public uavcan::ICanIface +{ + static inline ::can_frame makeSocketCanFrame(const uavcan::CanFrame& uavcan_frame) + { + ::can_frame sockcan_frame { uavcan_frame.id & uavcan::CanFrame::MaskExtID, uavcan_frame.dlc, { } }; + std::copy(uavcan_frame.data, uavcan_frame.data + uavcan_frame.dlc, sockcan_frame.data); + if (uavcan_frame.isExtended()) + { + sockcan_frame.can_id |= CAN_EFF_FLAG; + } + if (uavcan_frame.isErrorFrame()) + { + sockcan_frame.can_id |= CAN_ERR_FLAG; + } + if (uavcan_frame.isRemoteTransmissionRequest()) + { + sockcan_frame.can_id |= CAN_RTR_FLAG; + } + return sockcan_frame; + } + + static inline uavcan::CanFrame makeUavcanFrame(const ::can_frame& sockcan_frame) + { + uavcan::CanFrame uavcan_frame(sockcan_frame.can_id & CAN_EFF_MASK, sockcan_frame.data, sockcan_frame.can_dlc); + if (sockcan_frame.can_id & CAN_EFF_FLAG) + { + uavcan_frame.id |= uavcan::CanFrame::FlagEFF; + } + if (sockcan_frame.can_id & CAN_ERR_FLAG) + { + uavcan_frame.id |= uavcan::CanFrame::FlagERR; + } + if (sockcan_frame.can_id & CAN_RTR_FLAG) + { + uavcan_frame.id |= uavcan::CanFrame::FlagRTR; + } + return uavcan_frame; + } + + struct TxItem + { + uavcan::CanFrame frame; + uavcan::CanIOFlags flags; + uavcan::MonotonicTime deadline; + 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; + bool operator<(const RxItem& rhs) const { return frame.priorityLowerThan(rhs.frame); } + }; + + const SystemClock clock_; + const int fd_; + std::map errors_; + std::priority_queue tx_queue_; // TODO: Use pool allocator + std::priority_queue rx_queue_; // TODO: Use pool allocator + std::unordered_multiset pending_loopback_ids_; // TODO: Use pool allocator + bool has_pending_write_; + + void registerError(SocketCanError e) { errors_[e]++; } + + bool wasInPendingLoopbackSet(const uavcan::CanFrame& frame) + { + if (pending_loopback_ids_.count(frame.id) > 0) + { + pending_loopback_ids_.erase(frame.id); + return true; + } + return false; + } + + int write(const uavcan::CanFrame& frame) const + { + const ::can_frame sockcan_frame = makeSocketCanFrame(frame); + const int res = ::write(fd_, &sockcan_frame, sizeof(sockcan_frame)); + if (res <= 0) + { + return res; + } + if (res != sizeof(sockcan_frame)) + { + return -1; + } + return 1; + } + + /** + * SocketCAN git show 1e55659ce6ddb5247cee0b1f720d77a799902b85 + * MSG_DONTROUTE is set for any packet from localhost, + * MSG_CONFIRM is set for any pakcet of your socket. + * Diff: https://git.ucsd.edu/abuss/linux/commit/1e55659ce6ddb5247cee0b1f720d77a799902b85 + * Man: https://www.kernel.org/doc/Documentation/networking/can.txt (chapter 4.1.6). + */ + int read(uavcan::CanFrame& frame, uavcan::UtcTime& ts_utc, bool& loopback) const + { + auto iov = ::iovec(); + auto sockcan_frame = ::can_frame(); + iov.iov_base = &sockcan_frame; + iov.iov_len = sizeof(sockcan_frame); + + struct Control + { + cmsghdr cm; + std::uint8_t control[sizeof(::timeval)]; + }; + auto control = Control(); + + auto msg = ::msghdr(); + msg.msg_iov = &iov; + msg.msg_iovlen = 1; + msg.msg_control = &control; + msg.msg_controllen = sizeof(control); + + const int res = ::recvmsg(fd_, &msg, MSG_DONTWAIT); + if (res <= 0) + { + return (res < 0 && errno == EWOULDBLOCK) ? 0 : res; + } + frame = makeUavcanFrame(sockcan_frame); + /* + * Timestamp + */ + const ::cmsghdr* const cmsg = CMSG_FIRSTHDR(&msg); + assert(cmsg != nullptr); + if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMP) + { + auto tv = ::timeval(); + std::memcpy(&tv, CMSG_DATA(cmsg), sizeof(tv)); // Copy to avoid alignment problems + assert(tv.tv_sec >= 0 && tv.tv_usec >= 0); + ts_utc = uavcan::UtcTime::fromUSec(std::uint64_t(tv.tv_sec) * 1000000ULL + tv.tv_usec); + } + else + { + assert(0); + return -1; + } + /* + * Flags + */ + loopback = !!(msg.msg_flags & MSG_CONFIRM); + return 1; + } + + void pollWrite() + { + while (!tx_queue_.empty() && !has_pending_write_) + { + const TxItem tx = tx_queue_.top(); + tx_queue_.pop(); + assert(tx_queue_.empty() ? true : !tx.frame.priorityLowerThan(tx_queue_.top().frame)); // Order check + if (tx.deadline >= clock_.getMonotonic()) + { + const int res = write(tx.frame); + if (res == 1) + { + has_pending_write_ = true; + if (tx.flags & uavcan::CanIOFlagLoopback) + { + pending_loopback_ids_.insert(tx.frame.id); + } + } + else + { + registerError(SocketCanError::SocketWriteFailure); + } + } + else + { + registerError(SocketCanError::TxTimeout); + } + } + } + + void pollRead() + { + while (true) + { + RxItem rx; + rx.ts_mono = clock_.getMonotonic(); // Monotonic timestamp is not required to be precise (unlike UTC) + bool loopback = false; + const int res = read(rx.frame, rx.ts_utc, loopback); + if (res == 1) + { + assert(!rx.ts_utc.isZero()); + bool accept = true; + if (loopback) // We receive loopback for all CAN frames + { + has_pending_write_ = false; + rx.flags |= uavcan::CanIOFlagLoopback; + accept = wasInPendingLoopbackSet(rx.frame); // Do we need to send this loopback into the lib? + } + if (accept) + { + rx_queue_.push(rx); + } + } + else if (res == 0) + { + break; + } + else + { + registerError(SocketCanError::SocketReadFailure); + } + } + } + +public: + explicit SocketCanIface(int fd) + : fd_(fd) + , has_pending_write_(false) + { } + + /** + * Assumes that the socket is writeable + */ + 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 }); + pollWrite(); + return 1; + } + + /** + * Does not read from the socket, but from the RX queue. Thus, pollRead() must 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; + } + 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; + } + + /** + * Performs socket read/write. + * @param read Socket is readable + * @param write Socket is writeable + */ + void poll(bool read, bool write) + { + if (read) + { + pollRead(); // Read poll must be executed first because it may release pending TX flag + } + if (write) + { + pollWrite(); + } + } + + bool hasPendingTx() const { return !tx_queue_.empty(); } + bool hasReadyRx() const { return !rx_queue_.empty(); } + + virtual std::int16_t configureFilters(const uavcan::CanFilterConfig* const filter_configs, + const std::uint16_t num_configs) + { + if (filter_configs == nullptr) + { + assert(0); + return -1; + } + std::vector< ::can_filter> filts(num_configs); + for (unsigned i = 0; i < num_configs; i++) + { + const uavcan::CanFilterConfig& fc = filter_configs[i]; + filts[i].can_id = fc.id & uavcan::CanFrame::MaskExtID; + filts[i].can_mask = fc.mask & uavcan::CanFrame::MaskExtID; + if (fc.id & uavcan::CanFrame::FlagEFF) + { + filts[i].can_id |= CAN_EFF_FLAG; + } + if (fc.id & uavcan::CanFrame::FlagRTR) + { + filts[i].can_id |= CAN_RTR_FLAG; + } + if (fc.mask & uavcan::CanFrame::FlagEFF) + { + filts[i].can_mask |= CAN_EFF_FLAG; + } + if (fc.mask & uavcan::CanFrame::FlagRTR) + { + filts[i].can_mask |= CAN_RTR_FLAG; + } + } + int ret = setsockopt(fd_, SOL_CAN_RAW, CAN_RAW_FILTER, filts.data(), sizeof(::can_filter) * num_configs); + return (ret < 0) ? -1 : 0; + } + + virtual std::uint16_t getNumFilters() const { return 255; } + + virtual std::uint64_t getErrorCount() const + { + std::uint64_t ec = 0; + for (auto& kv : errors_) { ec += kv.second; } + return ec; + } + + const decltype(errors_)& getErrors() const { return errors_; } + + int getFileDescriptor() const { return fd_; } + + /** + * Open and configure a CAN socket on iface specified by name. + * @param iface_name String containing iface name, e.g. "can0", "vcan1", "slcan0" + * @return Socket descriptor or negative number on error. + */ + static int openSocket(const std::string& iface_name) + { + const int s = ::socket(PF_CAN, SOCK_RAW, CAN_RAW); + if (s < 0) + { + return s; + } + // Detect the iface index + auto ifr = ::ifreq(); + if (iface_name.length() >= IFNAMSIZ) + { + goto fail; + } + std::strncpy(ifr.ifr_name, iface_name.c_str(), iface_name.length()); + if (::ioctl(s, SIOCGIFINDEX, &ifr) < 0 || ifr.ifr_ifindex < 0) + { + goto fail; + } + // Bind to a 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; + } + } + // Configure + { + const int on = 1; + // Timestamping + if (::setsockopt(s, SOL_SOCKET, SO_TIMESTAMP, &on, sizeof(on)) < 0) + { + goto fail; + } + // Socket loopback + if (::setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &on, sizeof(on)) < 0) + { + goto fail; + } + // Non-blocking + if (::fcntl(s, F_SETFL , O_NONBLOCK) < 0) + { + goto fail; + } + } + return s; + + fail: + ::close(s); + return -1; + } +}; + + +/** + * Multiplexing container for multiple SocketCAN sockets + */ +class SocketCanDriver : public uavcan::ICanDriver +{ +public: + static constexpr unsigned MaxIfaces = uavcan::MaxCanIfaces; + +private: + const SystemClock clock_; + +public: + virtual SocketCanIface* getIface(std::uint8_t iface_index) + { + (void)iface_index; + return nullptr; // FIXME not implemented yet + } + + virtual std::uint8_t getNumIfaces() const + { + return -1; // FIXME not implemented yet + } + + 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 + } +}; + +} diff --git a/libuavcan_drivers/linux/test/test_socket.cpp b/libuavcan_drivers/linux/test/test_socket.cpp new file mode 100644 index 0000000000..50052866d8 --- /dev/null +++ b/libuavcan_drivers/linux/test/test_socket.cpp @@ -0,0 +1,216 @@ +/* + * Copyright (C) 2014 Pavel Kirienko + */ + +#include +#include +#include + +#define ASSERT(x) if (!(x)) { throw std::runtime_error(#x); } + +static uavcan::CanFrame makeFrame(std::uint32_t id, const std::string& data) +{ + return uavcan::CanFrame(id, reinterpret_cast(data.c_str()), data.length()); +} + +static uavcan::MonotonicTime tsMonoOffsetMs(std::int64_t ms) +{ + return uavcan_linux::SystemClock().getMonotonic() + uavcan::MonotonicDuration::fromMSec(ms); +} + +static void testNonexistentIface() +{ + const int sock1 = uavcan_linux::SocketCanIface::openSocket("noif9"); + ASSERT(sock1 < 0); + const int sock2 = uavcan_linux::SocketCanIface::openSocket("verylongifacenameverylongifacenameverylongifacename"); + ASSERT(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); + + uavcan_linux::SocketCanIface if1(sock1); + uavcan_linux::SocketCanIface if2(sock2); + + /* + * 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 + + /* + * 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()); + + /* + * No-op + */ + if1.poll(true, true); + if2.poll(true, true); + + uavcan::CanFrame frame; + uavcan::MonotonicTime ts_mono; + uavcan::UtcTime ts_utc; + uavcan::CanIOFlags flags = 0; + + const uavcan_linux::SystemClock clock(uavcan_linux::ClockAdjustmentMode::PerDriverPrivate); + + /* + * 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); + + 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); + + 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); + + ASSERT(0 == if1.receive(frame, ts_mono, ts_utc, flags)); + ASSERT(!if1.hasPendingTx()); + ASSERT(!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); + + 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); + + 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); + + ASSERT(0 == if2.receive(frame, ts_mono, ts_utc, flags)); + ASSERT(!if2.hasPendingTx()); + ASSERT(!if2.hasReadyRx()); +} + +static void testSocketFilters(const std::string& iface_name) +{ + using uavcan::CanFrame; + + const int sock1 = uavcan_linux::SocketCanIface::openSocket(iface_name); + const int sock2 = uavcan_linux::SocketCanIface::openSocket(iface_name); + ASSERT(sock1 >= 0 && sock2 >= 0); + + uavcan_linux::SocketCanIface if1(sock1); + uavcan_linux::SocketCanIface if2(sock2); + + /* + * Configuring filters + */ + uavcan::CanFilterConfig fcs[3]; + // STD/EXT 123 + fcs[0].id = 123; + fcs[0].mask = CanFrame::MaskExtID; + // Only EXT 456789 + fcs[1].id = 456789 | CanFrame::FlagEFF; + fcs[1].mask = CanFrame::MaskExtID | CanFrame::FlagEFF; + // Only STD 0 + fcs[2].id = 0; + fcs[2].mask = CanFrame::MaskExtID | CanFrame::FlagEFF; + + ASSERT(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 + + 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()); + + /* + * Checking RX on 2 + * Notice how the frames were reordered according to CAN bus arbitration rules + */ + uavcan::CanFrame frame; + uavcan::MonotonicTime ts_mono; + 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); + + ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags)); + ASSERT(frame == makeFrame(123 | EFF, "2")); + + ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags)); + ASSERT(frame == makeFrame(456789 | EFF, "5")); + + ASSERT(1 == if2.receive(frame, ts_mono, ts_utc, flags)); + ASSERT(frame == makeFrame(123, "1")); + + ASSERT(!if2.hasReadyRx()); +} + +int main(int argc, const char** argv) +{ + if (argc < 2) + { + std::cout << "Usage:\n\t" << argv[0] << " " << std::endl; + return 1; + } + + testNonexistentIface(); + testSocketRxTx(argv[1]); + testSocketFilters(argv[1]); + + return 0; +}