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 an option which
is disabled by default to avoid any potential adverse impact
on existing usage. This is using the same param 'use_system_timestamp'
as established by neovi_bus.py.

I've also modified 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
 )
 
 
@@ -557,10 +557,24 @@ def capture_message(
     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
+        cmsg_level == socket.SOL_SOCKET and (
+           (cmsg_type == constants.SO_TIMESTAMPNS) or 
+           (cmsg_type == 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)
+    
+    if 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 +633,7 @@ def __init__(
         self,
         channel: str = "",
         receive_own_messages: bool = False,
+        use_system_timestamp: bool = True,
         local_loopback: bool = True,
         fd: bool = False,
         can_filters: Optional[CanFilters] = None,
@@ -642,6 +657,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 use_system_timestamp:
+            Use system timestamp for can messages instead of the hardware time
+            stamp
         :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 +677,7 @@ def __init__(
         self.socket = create_socket()
         self.channel = channel
         self.channel_info = f"socketcan channel '{channel}'"
+        self.use_system_timestamp = use_system_timestamp
         self._bcm_sockets: Dict[str, socket.socket] = {}
         self._is_filtered = False
         self._task_id = 0
@@ -703,12 +722,21 @@ 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 self.use_system_timestamp:
+            # 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["use_system_timestamp"] = False
 
     return Bus(parsed_args.channel, **config)