Merge pull request hardbyte#1015 from hardbyte/socketcan-timestamp-ns

Add nanosecond resolution time stamping to socketcan

Author: mergify[bot]

can/interfaces/socketcan/constants.py

@@ -2,6 +2,9 @@
 Defines shared CAN constants.
 """
 
+# Generic socket constants
+SO_TIMESTAMPNS = 35
+
 CAN_ERR_FLAG = 0x20000000
 CAN_RTR_FLAG = 0x40000000
 CAN_EFF_FLAG = 0x80000000

can/interfaces/socketcan/socketcan.py

@@ -27,6 +27,15 @@
     log.error("fcntl not available on this platform")
 
 
+try:
+    from socket import CMSG_SPACE
+
+    CMSG_SPACE_available = True
+except ImportError:
+    CMSG_SPACE_available = False
+    log.error("socket.CMSG_SPACE not available on this platform")
+
+
 import can
 from can import Message, BusABC
 from can.broadcastmanager import (
@@ -38,6 +47,7 @@
 from can.interfaces.socketcan.constants import *  # CAN_RAW, CAN_*_FLAG
 from can.interfaces.socketcan.utils import pack_filters, find_available_interfaces
 
+
 # Setup BCM struct
 def bcm_header_factory(
     fields: List[Tuple[str, Union[Type[ctypes.c_uint32], Type[ctypes.c_long]]]],
@@ -494,6 +504,8 @@ def bind_socket(sock: socket.socket, channel: str = "can0") -> None:
 
     :param sock:
         The socket to be bound
+    :param channel:
+        The channel / interface to bind to
     :raises OSError:
         If the specified interface isn't found.
     """
@@ -517,24 +529,27 @@ def capture_message(
     """
     # Fetching the Arb ID, DLC and Data
     try:
+        cf, ancillary_data, msg_flags, addr = sock.recvmsg(
+            CANFD_MTU, RECEIVED_ANCILLARY_BUFFER_SIZE
+        )
         if get_channel:
-            cf, _, msg_flags, addr = sock.recvmsg(CANFD_MTU)
             channel = addr[0] if isinstance(addr, tuple) else addr
         else:
-            cf, _, msg_flags, _ = sock.recvmsg(CANFD_MTU)
             channel = None
-    except socket.error as exc:
-        raise can.CanError("Error receiving: %s" % exc)
+    except socket.error as error:
+        raise can.CanError(f"Error receiving: {error}")
 
     can_id, can_dlc, flags, data = dissect_can_frame(cf)
-    # log.debug('Received: can_id=%x, can_dlc=%x, data=%s', can_id, can_dlc, data)
 
     # Fetching the timestamp
-    binary_structure = "@LL"
-    res = fcntl.ioctl(sock.fileno(), SIOCGSTAMP, struct.pack(binary_structure, 0, 0))
-
-    seconds, microseconds = struct.unpack(binary_structure, res)
-    timestamp = seconds + microseconds * 1e-6
+    assert len(ancillary_data) == 1, "only requested a single extra field"
+    cmsg_level, cmsg_type, cmsg_data = ancillary_data[0]
+    assert (
+        cmsg_level == socket.SOL_SOCKET and cmsg_type == SO_TIMESTAMPNS
+    ), "received control message type that was not requested"
+    # see https://man7.org/linux/man-pages/man3/timespec.3.html -> struct timespec for details
+    seconds, nanoseconds = RECEIVED_TIMESTAMP_STRUCT.unpack_from(cmsg_data)
+    timestamp = seconds + nanoseconds * 1e-9
 
     # EXT, RTR, ERR flags -> boolean attributes
     #   /* special address description flags for the CAN_ID */
@@ -574,11 +589,15 @@ def capture_message(
         data=data,
     )
 
-    # log_rx.debug('Received: %s', msg)
-
     return msg
 
 
+# Constants needed for precise handling of timestamps
+if CMSG_SPACE_available:
+    RECEIVED_TIMESTAMP_STRUCT = struct.Struct("@II")
+    RECEIVED_ANCILLARY_BUFFER_SIZE = CMSG_SPACE(RECEIVED_TIMESTAMP_STRUCT.size)
+
+
 class SocketcanBus(BusABC):
     """A SocketCAN interface to CAN.
 
@@ -647,7 +666,7 @@ def __init__(
         except socket.error as error:
             log.error("Could not receive own messages (%s)", error)
 
-        # enable CAN-FD frames
+        # enable CAN-FD frames if desired
         if fd:
             try:
                 self.socket.setsockopt(SOL_CAN_RAW, CAN_RAW_FD_FRAMES, 1)
@@ -660,6 +679,13 @@ def __init__(
         except socket.error as error:
             log.error("Could not enable error frames (%s)", error)
 
+        # enable nanosecond resolution timestamping
+        # we can always do this since
+        #  1) is is guaranteed to be at least as precise as without
+        #  2) it is available since Linux 2.6.22, and CAN support was only added afterward
+        #     so this is always supported by the kernel
+        self.socket.setsockopt(socket.SOL_SOCKET, SO_TIMESTAMPNS, 1)
+
         bind_socket(self.socket, channel)
         kwargs.update(
             {

doc/interfaces/socketcan.rst

@@ -18,9 +18,8 @@ The `SocketCAN`_ documentation can be found in the Linux kernel docs at
 .. important::
 
     `python-can` versions before 2.2 had two different implementations named
-    ``socketcan_ctypes`` and ``socketcan_native``. These are now
-    deprecated and the aliases to ``socketcan`` will be removed in
-    version 4.0. 3.x releases raise a DeprecationWarning.
+    ``socketcan_ctypes`` and ``socketcan_native``. These were removed in
+    version 4.0.0 after a deprecation period.
 
 
 Socketcan Quickstart
@@ -248,7 +247,8 @@ The :class:`~can.interfaces.socketcan.SocketcanBus` specializes :class:`~can.Bus
 to ensure usage of SocketCAN Linux API. The most important differences are:
 
 - usage of SocketCAN BCM for periodic messages scheduling;
-- filtering of CAN messages on Linux kernel level.
+- filtering of CAN messages on Linux kernel level;
+- usage of nanosecond timings from the kernel.
 
 .. autoclass:: can.interfaces.socketcan.SocketcanBus
     :members: