socketcan: support use of SO_TIMESTAMPING for hardware timestamps

The current implemenation of socketcan utilises SO_TIMESTAMPNS
which only offers system timestamps.

I've looked at how can-utils candump.c configures hardware
timestamping and implemented this in socketcan as a new option
'can_hardware_timestamps' which is disabled by default to avoid
any potential adverse impact on existing usage.

Additionally modify logger.py to provide an additional '-H' flag
in the same way that candump does in order to use this
functionality.

Author: David Peverley

can/interfaces/socketcan/constants.py

@@ -4,6 +4,10 @@
 
 # Generic socket constants
 SO_TIMESTAMPNS = 35
+SO_TIMESTAMPING = 37
+SOF_TIMESTAMPING_SOFTWARE = 1 << 4
+SOF_TIMESTAMPING_RX_SOFTWARE = 1 << 3
+SOF_TIMESTAMPING_RAW_HARDWARE = 1 << 6
 
 CAN_ERR_FLAG = 0x20000000
 CAN_RTR_FLAG = 0x40000000

can/interfaces/socketcan/socketcan.py

@@ -42,9 +42,9 @@
 
 
 # Constants needed for precise handling of timestamps
-RECEIVED_TIMESTAMP_STRUCT = struct.Struct("@ll")
+RECEIVED_TIMESPEC_STRUCT = struct.Struct("@ll")
 RECEIVED_ANCILLARY_BUFFER_SIZE = (
-    CMSG_SPACE(RECEIVED_TIMESTAMP_STRUCT.size) if CMSG_SPACE_available else 0
+    CMSG_SPACE(RECEIVED_TIMESPEC_STRUCT.size * 3) if CMSG_SPACE_available else 0
 )
 
 
@@ -556,11 +556,26 @@ def capture_message(
     # Fetching the timestamp
     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 == constants.SO_TIMESTAMPNS
+    assert cmsg_level == socket.SOL_SOCKET and cmsg_type in (
+        constants.SO_TIMESTAMPNS,
+        constants.SO_TIMESTAMPING,
     ), "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)
+    if cmsg_type == constants.SO_TIMESTAMPNS:
+        seconds, nanoseconds = RECEIVED_TIMESPEC_STRUCT.unpack_from(cmsg_data)
+    else:
+        # cmsg_type == constants.SO_TIMESTAMPING
+        #
+        # stamp[0] is the software timestamp
+        # stamp[1] is deprecated
+        # stamp[2] is the raw hardware timestamp
+        # See chapter 2.1.2 Receive timestamps in
+        # linux/Documentation/networking/timestamping.txt
+        offset = struct.calcsize(RECEIVED_TIMESPEC_STRUCT.format) * 2
+        seconds, nanoseconds = RECEIVED_TIMESPEC_STRUCT.unpack_from(
+            cmsg_data, offset=offset
+        )
+
     if nanoseconds >= 1e9:
         raise can.CanOperationError(
             f"Timestamp nanoseconds field was out of range: {nanoseconds} not less than 1e9"
@@ -619,6 +634,7 @@ def __init__(
         self,
         channel: str = "",
         receive_own_messages: bool = False,
+        can_hardware_timestamps: bool = False,
         local_loopback: bool = True,
         fd: bool = False,
         can_filters: Optional[CanFilters] = None,
@@ -642,6 +658,9 @@ def __init__(
             channel using :attr:`can.Message.channel`.
         :param receive_own_messages:
             If transmitted messages should also be received by this bus.
+        :param bool can_hardware_timestamps:
+            Use hardware timestamp for can messages if available instead of
+            the system timestamp.
         :param local_loopback:
             If local loopback should be enabled on this bus.
             Please note that local loopback does not mean that messages sent
@@ -659,6 +678,7 @@ def __init__(
         self.socket = create_socket()
         self.channel = channel
         self.channel_info = f"socketcan channel '{channel}'"
+        self.can_hardware_timestamps = can_hardware_timestamps
         self._bcm_sockets: Dict[str, socket.socket] = {}
         self._is_filtered = False
         self._task_id = 0
@@ -703,12 +723,25 @@ def __init__(
             except OSError as error:
                 log.error("Could not enable error frames (%s)", error)
 
-        # enable nanosecond resolution timestamping
-        # we can always do this since
-        #  1) it 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, constants.SO_TIMESTAMPNS, 1)
+        if not self.can_hardware_timestamps:
+            # Utilise SOF_TIMESTAMPNS interface :
+            # we can always do this since
+            #  1) it 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, constants.SO_TIMESTAMPNS, 1)
+        else:
+            # Utilise SOF_TIMESTAMPNS interface :
+            # Allows us to use hardware timestamps where available
+            timestamping_flags = (
+                constants.SOF_TIMESTAMPING_SOFTWARE
+                | constants.SOF_TIMESTAMPING_RX_SOFTWARE
+                | constants.SOF_TIMESTAMPING_RAW_HARDWARE
+            )
+
+            self.socket.setsockopt(
+                socket.SOL_SOCKET, constants.SO_TIMESTAMPING, timestamping_flags
+            )
 
         try:
             bind_socket(self.socket, channel)

can/logger.py

@@ -46,6 +46,13 @@ def _create_base_argument_parser(parser: argparse.ArgumentParser) -> None:
         choices=sorted(can.VALID_INTERFACES),
     )
 
+    parser.add_argument(
+        "-H",
+        "--hardwarets",
+        help="Read hardware timestamps instead of system timestamps.",
+        action="store_true",
+    )
+
     parser.add_argument(
         "-b", "--bitrate", type=int, help="Bitrate to use for the CAN bus."
     )
@@ -109,6 +116,8 @@ def _create_bus(parsed_args: argparse.Namespace, **kwargs: Any) -> can.BusABC:
         config["data_bitrate"] = parsed_args.data_bitrate
     if getattr(parsed_args, "can_filters", None):
         config["can_filters"] = parsed_args.can_filters
+    if parsed_args.hardwarets:
+        config["can_hardware_timestamps"] = True
 
     return Bus(parsed_args.channel, **config)