Merge pull request #21 from TomDeRybel/master

pavel-kirienko · web-flow · commit 2f057788686a · 2020-10-28T21:11:02.000+02:00
socketcan - added loopback and kernel time-stamping
diff --git a/socketcan/libcanard/src/socketcan.c b/socketcan/libcanard/src/socketcan.c
@@ -1,6 +1,6 @@
 /// This software is distributed under the terms of the MIT License.
 /// Copyright (c) 2020 UAVCAN Development Team.
-/// Author: Pavel Kirienko <pavel.kirienko@zubax.com>
+/// Authors: Pavel Kirienko <pavel.kirienko@zubax.com>, Tom De Rybel <tom.derybel@robocow.be>
 
 // This is needed to enable the necessary declarations in sys/
 #ifndef _GNU_SOURCE
@@ -14,17 +14,18 @@
 #    include <linux/can/raw.h>
 #    include <net/if.h>
 #    include <sys/ioctl.h>
+#    include <sys/socket.h>
 #else
 #    error "Unsupported OS -- feel free to add support for your OS here. " \
         "Zephyr and NuttX are known to support the SocketCAN API."
 #endif
 
 #include <assert.h>
 #include <errno.h>
-#include <unistd.h>
 #include <poll.h>
 #include <string.h>
 #include <time.h>
+#include <unistd.h>
 
 #define KILO 1000L
 #define MEGA (KILO * KILO)
@@ -101,12 +102,27 @@ SocketCANFD socketcanOpen(const char* const iface_name, const bool can_fd)
         }
     }
 
+    // Enable CAN FD if required.
     if (ok && can_fd)
     {
         const int en = 1;
         ok           = 0 == setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, &en, sizeof(en));
     }
 
+    // Enable timestamping.
+    if (ok)
+    {
+        const int en = 1;
+        ok           = 0 == setsockopt(fd, SOL_SOCKET, SO_TIMESTAMP, &en, sizeof(en));
+    }
+
+    // Enable outgoing-frame loop-back.
+    if (ok)
+    {
+        const int en = 1;
+        ok           = 0 == setsockopt(fd, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &en, sizeof(en));
+    }
+
     if (ok)
     {
         return fd;
@@ -152,7 +168,8 @@ int16_t socketcanPop(const SocketCANFD       fd,
                      CanardFrame* const      out_frame,
                      const size_t            payload_buffer_size,
                      void* const             payload_buffer,
-                     const CanardMicrosecond timeout_usec)
+                     const CanardMicrosecond timeout_usec,
+                     bool* const             loopback)
 {
     if ((out_frame == NULL) || (payload_buffer == NULL))
     {
@@ -162,43 +179,91 @@ int16_t socketcanPop(const SocketCANFD       fd,
     const int16_t poll_result = doPoll(fd, POLLIN, timeout_usec);
     if (poll_result > 0)
     {
-        struct timespec ts;
-        if (clock_gettime(CLOCK_TAI, &ts) < 0)  // TODO: request the timestamp from the kernel.
-        {
-            return getNegatedErrno();
-        }
-
+        // Initialize the message header scatter/gather array. It is to hold a single CAN FD frame struct.
         // We use the CAN FD struct regardless of whether the CAN FD socket option is set.
         // Per the user manual, this is acceptable because they are binary compatible.
-        struct canfd_frame cfd;
-        const ssize_t      read_size = read(fd, &cfd, sizeof(cfd));
+        struct canfd_frame sockcan_frame = {0};  // CAN FD frame storage.
+        struct iovec       iov           = {
+            // Scatter/gather array items struct.
+            .iov_base = &sockcan_frame,        // Starting address.
+            .iov_len  = sizeof(sockcan_frame)  // Number of bytes to transfer.
+
+        };
+
+        // Determine the size of the ancillary data and zero-initialize the buffer for it.
+        // We require space for both the receive message header (implied in CMSG_SPACE) and the time stamp.
+        // The ancillary data buffer is wrapped in a union to ensure it is suitably aligned.
+        // See the cmsg(3) man page (release 5.08 dated 2020-06-09, or later) for details.
+        union
+        {
+            uint8_t        buf[CMSG_SPACE(sizeof(struct timeval))];
+            struct cmsghdr align;
+        } control;
+        (void) memset(control.buf, 0, sizeof(control.buf));
+
+        // Initialize the message header used by recvmsg.
+        struct msghdr msg  = {0};                  // Message header struct.
+        msg.msg_iov        = &iov;                 // Scatter/gather array.
+        msg.msg_iovlen     = 1;                    // Number of elements in the scatter/gather array.
+        msg.msg_control    = control.buf;          // Ancillary data.
+        msg.msg_controllen = sizeof(control.buf);  // Ancillary data buffer length.
+
+        // Non-blocking receive messages from the socket and validate.
+        const ssize_t read_size = recvmsg(fd, &msg, MSG_DONTWAIT);
         if (read_size < 0)
         {
-            return getNegatedErrno();
+            return getNegatedErrno();  // Error occurred -- return the negated error code.
         }
         if ((read_size != CAN_MTU) && (read_size != CANFD_MTU))
         {
             return -EIO;
         }
-        if (cfd.len > payload_buffer_size)
+        if (sockcan_frame.len > payload_buffer_size)
         {
             return -EFBIG;
         }
 
-        const bool valid = ((cfd.can_id & CAN_EFF_FLAG) != 0) &&  // Extended frame
-                           ((cfd.can_id & CAN_RTR_FLAG) == 0) &&  // Not RTR frame
-                           ((cfd.can_id & CAN_ERR_FLAG) == 0);    // Not error frame
+        const bool valid = ((sockcan_frame.can_id & CAN_EFF_FLAG) != 0) &&  // Extended frame
+                           ((sockcan_frame.can_id & CAN_ERR_FLAG) == 0) &&  // Not RTR frame
+                           ((sockcan_frame.can_id & CAN_RTR_FLAG) == 0);    // Not error frame
         if (!valid)
         {
             return 0;  // Not an extended data frame -- drop silently and return early.
         }
 
+        // Handle the loopback frame logic.
+        const bool loopback_frame = ((uint32_t) msg.msg_flags & (uint32_t) MSG_CONFIRM) != 0;
+        if (loopback == NULL && loopback_frame)
+        {
+            return 0;  // The loopback pointer is NULL and this is a loopback frame -- drop silently and return early.
+        }
+        if (loopback != NULL)
+        {
+            *loopback = loopback_frame;
+        }
+
+        // Obtain the CAN frame time stamp from the kernel.
+        // This time stamp is from the CLOCK_REALTIME kernel source.
+        const struct cmsghdr* cmsg = CMSG_FIRSTHDR(&msg);
+        struct timeval        tv   = {0};
+        assert(cmsg != NULL);
+        if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMP)
+        {
+            (void) memcpy(&tv, CMSG_DATA(cmsg), sizeof(tv));  // Copy to avoid alignment problems
+            assert(tv.tv_sec >= 0 && tv.tv_usec >= 0);
+        }
+        else
+        {
+            assert(0);
+            return -EIO;
+        }
+
         (void) memset(out_frame, 0, sizeof(CanardFrame));
-        out_frame->timestamp_usec  = (CanardMicrosecond)((ts.tv_sec * MEGA) + (ts.tv_nsec / KILO));
-        out_frame->extended_can_id = cfd.can_id & CAN_EFF_MASK;
-        out_frame->payload_size    = cfd.len;
+        out_frame->timestamp_usec  = (CanardMicrosecond)(((uint64_t) tv.tv_sec * MEGA) + (uint64_t) tv.tv_usec);
+        out_frame->extended_can_id = sockcan_frame.can_id & CAN_EFF_MASK;
+        out_frame->payload_size    = sockcan_frame.len;
         out_frame->payload         = payload_buffer;
-        (void) memcpy(payload_buffer, &cfd.data[0], cfd.len);
+        (void) memcpy(payload_buffer, &sockcan_frame.data[0], sockcan_frame.len);
     }
     return poll_result;
 }
diff --git a/socketcan/libcanard/src/socketcan.h b/socketcan/libcanard/src/socketcan.h
@@ -12,6 +12,15 @@
 /// To integrate the library into your application, just copy-paste the c/h files into your project tree.
 ///
 /// --------------------------------------------------------------------------------------------------------------------
+/// Changelog
+///
+/// v2.0 - Added loop-back functionality.
+///        API change in socketcanPop(): loopback flag added.
+///      - Changed to kernel-based time-stamping for received frames for improved accuracy.
+///        API change in socketcanPop(): time stamp clock source is now CLOCK_REALTIME, vs CLOCK_TAI before.
+///
+/// v1.0 - Initial release
+/// --------------------------------------------------------------------------------------------------------------------
 ///
 /// This software is distributed under the terms of the MIT License.
 /// Copyright (c) 2020 UAVCAN Development Team.
@@ -36,13 +45,6 @@ typedef int SocketCANFD;
 /// On failure, a negated errno is returned.
 /// To discard the socket just call close() on it; no additional de-initialization activities are required.
 /// The argument can_fd enables support for CAN FD frames.
-///
-/// If you need the outgoing frames to be looped back, set option CAN_RAW_RECV_OWN_MSGS on the returned socket handle:
-///   const int on = 1;
-///   int err = setsockopt(s, SOL_CAN_RAW, CAN_RAW_RECV_OWN_MSGS, &on, sizeof(on));
-/// It is currently not possible to distinguish loopback frames from true RX frames. If you need that,
-/// consider using recvmsg(..) instead of this wrapper as shown in the following example:
-/// https://github.com/UAVCAN/platform_specific_components/blob/4745ef59f57b7e1c34705b127ea8c7a35e3874c1/linux/libuavcan/include/uavcan_linux/socketcan.hpp#L210-L270
 SocketCANFD socketcanOpen(const char* const iface_name, const bool can_fd);
 
 /// Enqueue a new extended CAN data frame for transmission.
@@ -55,15 +57,19 @@ int16_t socketcanPush(const SocketCANFD fd, const CanardFrame* const frame, cons
 /// If the received frame is not an extended-ID data frame, it will be dropped and the function will return early.
 /// The payload pointer of the returned frame will point to the payload_buffer. It can be a stack-allocated array.
 /// The payload_buffer_size shall be large enough (64 bytes is enough for CAN FD), otherwise an error is returned.
-/// The timestamp of the received frame will be set to the CLOCK_TAI sampled near the moment of its arrival.
+/// The received frame timestamp will be set to CLOCK_REALTIME by the kernel, sampled near the moment of its arrival.
+/// The loopback flag pointer is used to both indicate and control behavior when a looped-back message is received.
+/// If the flag pointer is NULL, loopback frames are silently dropped; if not NULL, they are accepted and indicated
+/// using this flag.
 /// The function will block until a frame is received or until the timeout is expired. It may return early.
 /// Zero timeout makes the operation non-blocking.
 /// Returns 1 on success, 0 on timeout, negated errno on error.
 int16_t socketcanPop(const SocketCANFD       fd,
                      CanardFrame* const      out_frame,
                      const size_t            payload_buffer_size,
                      void* const             payload_buffer,
-                     const CanardMicrosecond timeout_usec);
+                     const CanardMicrosecond timeout_usec,
+                     bool* const             loopback);
 
 /// The configuration of a single extended 29-bit data frame acceptance filter.
 /// Bits above the 29-th shall be cleared.
diff --git a/socketcan/libcanard/test/test_io.cpp b/socketcan/libcanard/test/test_io.cpp
@@ -29,25 +29,26 @@ TEST_CASE("IO-Classic")  // Catch2 does not support parametrized tests yet.
 
     char buf[255]{};
     fr = {};
-    REQUIRE(1 == socketcanPop(sb, &fr, sizeof(buf), buf, 1000));
+    REQUIRE(1 == socketcanPop(sb, &fr, sizeof(buf), buf, 1000, nullptr));
     REQUIRE(fr.timestamp_usec > 0);
     REQUIRE(fr.extended_can_id == 0x1234U);
     REQUIRE(fr.payload_size == 6);
     REQUIRE(0 == std::memcmp(fr.payload, "Hello", 6));
     auto old_ts = fr.timestamp_usec;
 
     fr = {};
-    REQUIRE(1 == socketcanPop(sa, &fr, sizeof(buf), buf, 1000));
+    REQUIRE(0 == socketcanPop(sa, &fr, sizeof(buf), buf, 1000, nullptr));  // Loopback frame.
+    REQUIRE(1 == socketcanPop(sa, &fr, sizeof(buf), buf, 1000, nullptr));  // Received actual frame.
     REQUIRE(fr.timestamp_usec > 0);
     REQUIRE(fr.timestamp_usec >= old_ts);
     REQUIRE(fr.extended_can_id == 0x4321U);
     REQUIRE(fr.payload_size == 7);
     REQUIRE(0 == std::memcmp(fr.payload, "World!", 7));
 
-    REQUIRE(0 == socketcanPop(sa, &fr, sizeof(buf), buf, 0));
-    REQUIRE(0 == socketcanPop(sa, &fr, sizeof(buf), buf, 1000));
-    REQUIRE(-EINVAL == socketcanPop(sa, &fr, 0, nullptr, 1000));
-    REQUIRE(-EINVAL == socketcanPop(sa, nullptr, sizeof(buf), buf, 1000));
+    REQUIRE(0 == socketcanPop(sa, &fr, sizeof(buf), buf, 0, nullptr));
+    REQUIRE(0 == socketcanPop(sa, &fr, sizeof(buf), buf, 1000, nullptr));
+    REQUIRE(-EINVAL == socketcanPop(sa, &fr, 0, nullptr, 1000, nullptr));
+    REQUIRE(-EINVAL == socketcanPop(sa, nullptr, sizeof(buf), buf, 1000, nullptr));
 
     REQUIRE(-EINVAL == socketcanPush(sa, nullptr, 1'000'000));
 
@@ -85,28 +86,71 @@ TEST_CASE("IO-FD")
 
     char buf[255]{};
     fr = {};
-    REQUIRE(1 == socketcanPop(sb, &fr, sizeof(buf), buf, 1000));
+    REQUIRE(1 == socketcanPop(sb, &fr, sizeof(buf), buf, 1000, nullptr));
     REQUIRE(fr.timestamp_usec > 0);
     REQUIRE(fr.extended_can_id == 0x1234U);
     REQUIRE(fr.payload_size == 13);
     REQUIRE(0 == std::memcmp(fr.payload, "Hello world!", 13));
     auto old_ts = fr.timestamp_usec;
 
     fr = {};
-    REQUIRE(1 == socketcanPop(sa, &fr, sizeof(buf), buf, 1000));
+    REQUIRE(0 == socketcanPop(sa, &fr, sizeof(buf), buf, 1000, nullptr));  // Loopback frame.
+    REQUIRE(1 == socketcanPop(sa, &fr, sizeof(buf), buf, 1000, nullptr));  // Received actual frame.
     REQUIRE(fr.timestamp_usec > 0);
     REQUIRE(fr.timestamp_usec >= old_ts);
     REQUIRE(fr.extended_can_id == 0x4321U);
     REQUIRE(fr.payload_size == 10);
     REQUIRE(0 == std::memcmp(fr.payload, "0123456789", 10));
 
-    REQUIRE(0 == socketcanPop(sa, &fr, sizeof(buf), buf, 0));
-    REQUIRE(0 == socketcanPop(sa, &fr, sizeof(buf), buf, 1000));
-    REQUIRE(-EINVAL == socketcanPop(sa, &fr, 0, nullptr, 1000));
-    REQUIRE(-EINVAL == socketcanPop(sa, nullptr, sizeof(buf), buf, 1000));
+    REQUIRE(0 == socketcanPop(sa, &fr, sizeof(buf), buf, 0, nullptr));
+    REQUIRE(0 == socketcanPop(sa, &fr, sizeof(buf), buf, 1000, nullptr));
+    REQUIRE(-EINVAL == socketcanPop(sa, &fr, 0, nullptr, 1000, nullptr));
+    REQUIRE(-EINVAL == socketcanPop(sa, nullptr, sizeof(buf), buf, 1000, nullptr));
 
     REQUIRE(-EINVAL == socketcanPush(sa, nullptr, 1'000'000));
 
     ::close(sa);
     ::close(sb);
 }
+
+TEST_CASE("IO-FD-Loopback")
+{
+    const char* const iface_name = "vcan0";
+
+    const auto sa = socketcanOpen(iface_name, true);
+    const auto sb = socketcanOpen(iface_name, true);
+    REQUIRE(sa >= 0);
+    REQUIRE(sb >= 0);
+
+    CanardFrame fr{};
+    fr.extended_can_id = 0x1234U;
+    fr.payload_size    = 13;
+    fr.payload         = "Hello World!";
+    REQUIRE(1 == socketcanPush(sa, &fr, 1'000'000));  // Send frame on sa.
+
+    bool loopback = true;
+    char buf[255]{};
+    fr = {};
+    REQUIRE(1 == socketcanPop(sb, &fr, sizeof(buf), buf, 1000, &loopback));  // Receive actual frame on sb.
+    REQUIRE(loopback == false);
+    REQUIRE(fr.timestamp_usec > 0);
+    REQUIRE(fr.extended_can_id == 0x1234U);
+    REQUIRE(fr.payload_size == 13);
+    REQUIRE(0 == std::memcmp(fr.payload, "Hello World!", 13));
+    auto old_ts = fr.timestamp_usec;
+
+    fr = {};
+    REQUIRE(1 == socketcanPop(sa, &fr, sizeof(buf), buf, 1000, &loopback));  // Receive loopback frame on sa.
+    REQUIRE(loopback == true);
+    REQUIRE(fr.timestamp_usec > 0);
+    REQUIRE(fr.timestamp_usec >= old_ts);
+    REQUIRE(fr.extended_can_id == 0x1234U);
+    REQUIRE(fr.payload_size == 13);
+    REQUIRE(0 == std::memcmp(fr.payload, "Hello World!", 13));
+
+    REQUIRE(0 == socketcanPop(sa, &fr, sizeof(buf), buf, 0, nullptr));     // No more frames.
+    REQUIRE(0 == socketcanPop(sa, &fr, sizeof(buf), buf, 1000, nullptr));  // No more frames.
+
+    ::close(sa);
+    ::close(sb);
+}
