udp: Add support for SO_TIMESTAMP

Adds support for timestamping received UDP packets, either in
hardware or in kernel. Builds on the existing support of SO_TIMESTAMP
for SocketCAN.

Implementation uses CLOCK_REALTIME for timestamping to match the
behavior of Linux. This could be made configurable in future if needed.

include/nuttx/net/netdev.h

@@ -442,6 +442,18 @@ struct net_driver_s
   struct netdev_statistics_s d_statistics;
 #endif
 
+#if defined(CONFIG_NET_TIMESTAMP)
+  /* Reception timestamp of packet being currently processed.
+   * If CONFIG_ARCH_HAVE_NETDEV_TIMESTAMP is true, the timestamp is provided
+   * by hardware driver. Otherwise it is filled in by kernel when packet
+   * enters ipv4_input or ipv6_input.
+   *
+   * The timestamp is in CLOCK_REALTIME.
+   */
+
+  struct timespec d_rxtime;
+#endif
+
   /* Application callbacks:
    *
    * Network device event handlers are retained in a 'list' and are called

net/Kconfig

@@ -23,6 +23,10 @@ config ARCH_HAVE_NETDEV_STATISTICS
 	bool
 	default n
 
+config ARCH_HAVE_NETDEV_TIMESTAMP
+	bool
+	default n
+
 config NET_WRITE_BUFFERS
 	bool
 	default n

net/devif/ipv4_input.c

@@ -491,6 +491,12 @@ int ipv4_input(FAR struct net_driver_s *dev)
   FAR uint8_t *buf;
   int ret;
 
+  /* Store reception timestamp if enabled and not provided by hardware. */
+
+#if defined(CONFIG_NET_TIMESTAMP) && !defined(CONFIG_ARCH_HAVE_NETDEV_TIMESTAMP)
+  clock_gettime(CLOCK_REALTIME, &dev->d_rxtime);
+#endif
+
   if (dev->d_iob != NULL)
     {
       buf = dev->d_buf;

net/devif/ipv6_input.c

@@ -609,6 +609,12 @@ int ipv6_input(FAR struct net_driver_s *dev)
   FAR uint8_t *buf;
   int ret;
 
+  /* Store reception timestamp if enabled and not provided by hardware. */
+
+#if defined(CONFIG_NET_TIMESTAMP) && !defined(CONFIG_ARCH_HAVE_NETDEV_TIMESTAMP)
+  clock_gettime(CLOCK_REALTIME, &dev->d_rxtime);
+#endif
+
   if (dev->d_iob != NULL)
     {
       buf = dev->d_buf;

net/inet/inet_sockif.c

@@ -747,6 +747,27 @@ static int inet_get_socketlevel_option(FAR struct socket *psock, int option,
         }
 #endif
 
+#ifdef CONFIG_NET_TIMESTAMP
+      case SO_TIMESTAMP:
+        {
+          if (*value_len != sizeof(int))
+            {
+              return -EINVAL;
+            }
+
+          if (psock->s_type == SOCK_DGRAM)
+            {
+              FAR struct udp_conn_s *conn = psock->s_conn;
+              *(FAR int *)value = (conn->timestamp != 0);
+            }
+          else
+            {
+              return -ENOPROTOOPT;
+            }
+        }
+        break;
+#endif
+
       default:
         return -ENOPROTOOPT;
     }
@@ -1028,6 +1049,36 @@ static int inet_set_socketlevel_option(FAR struct socket *psock, int option,
         break;
 #endif
 
+#ifdef CONFIG_NET_TIMESTAMP
+      case SO_TIMESTAMP: /* Report receive timestamps as cmsg */
+        {
+          if (value_len < sizeof(int))
+            {
+              return -EINVAL;
+            }
+
+          if (psock->s_type == SOCK_DGRAM)
+            {
+              net_lock();
+
+              /* For now the timestamp enable is just boolean.
+               * If SO_TIMESTAMPING support is added in future, it can be
+               * expanded to flags field for rx/tx timestamps.
+               */
+
+              FAR struct udp_conn_s *conn = psock->s_conn;
+              conn->timestamp = (*((FAR int *)value) != 0);
+
+              net_unlock();
+            }
+          else
+            {
+              return -ENOPROTOOPT;
+            }
+        }
+        break;
+  #endif
+
       default:
         return -ENOPROTOOPT;
     }

net/socket/Kconfig

@@ -74,9 +74,10 @@ config NET_SOLINGER
 config NET_TIMESTAMP
 	bool "SO_TIMESTAMP socket option"
 	default n
-	depends on NET_CAN
+	depends on NET_CAN || NET_ETHERNET
 	---help---
-		Enable or disable support for the SO_TIMESTAMP socket option. Currently only tested & implemented in SocketCAN but should work on all sockets
+		Enable or disable support for the SO_TIMESTAMP socket option.
+		Supported on SocketCAN and Ethernet/UDP.
 
 config NET_BINDTODEVICE
 	bool "SO_BINDTODEVICE socket option Bind-to-device support"

net/udp/udp.h

@@ -156,6 +156,10 @@ struct udp_conn_s
    */
 
   struct udp_poll_s pollinfo[CONFIG_NET_UDP_NPOLLWAITERS];
+
+#ifdef CONFIG_NET_TIMESTAMP
+  int timestamp; /* Nonzero when SO_TIMESTAMP is enabled */
+#endif
 };
 
 /* This structure supports UDP write buffering.  It is simply a container

net/udp/udp_callback.c

@@ -28,6 +28,7 @@
 #include <stdint.h>
 #include <string.h>
 #include <debug.h>
+#include <sys/time.h>
 
 #include <nuttx/net/netconfig.h>
 #include <nuttx/net/netdev.h>
@@ -153,11 +154,26 @@ static uint16_t udp_datahandler(FAR struct net_driver_s *dev,
 #endif /* CONFIG_NET_IPv4 */
 
   /* Copy the meta info into the I/O buffer chain, just before data.
-   * Layout: |datalen|ifindex|src_addr_size|src_addr|data|
+   * Layout: |datalen|ifindex|src_addr_size|src_addr|[timestamp]|data|
    */
 
   offset = (dev->d_appdata - iob->io_data) - iob->io_offset;
 
+#ifdef CONFIG_NET_TIMESTAMP
+  /* Store timestamp while packet is being queued.
+   * This is done unconditionally to avoid race condition when SO_TIMESTAMP
+   * gets enabled after packet is received but before it is read.
+   */
+
+  offset -= sizeof(struct timespec);
+  ret = iob_trycopyin(iob, (FAR const uint8_t *)&dev->d_rxtime,
+                      sizeof(struct timespec), offset, true);
+  if (ret < 0)
+    {
+      goto errout;
+    }
+#endif
+
   offset -= src_addr_size;
   ret = iob_trycopyin(iob, src_addr, src_addr_size, offset, true);
   if (ret < 0)

net/udp/udp_recvfrom.c

@@ -30,6 +30,7 @@
 #include <debug.h>
 #include <assert.h>
 
+#include <sys/time.h>
 #include <nuttx/semaphore.h>
 #include <nuttx/net/net.h>
 #include <nuttx/mm/iob.h>
@@ -63,6 +64,19 @@ struct udp_recvfrom_s
  * Private Functions
  ****************************************************************************/
 
+#ifdef CONFIG_NET_TIMESTAMP
+static void udp_store_cmsg_timestamp(FAR struct udp_recvfrom_s *pstate,
+                                     FAR struct timespec *timestamp)
+{
+  FAR struct msghdr *msg = pstate->ir_msg;
+  struct timeval tv;
+
+  TIMESPEC_TO_TIMEVAL(&tv, timestamp);
+  cmsg_append(msg, SOL_SOCKET, SO_TIMESTAMP,
+              &tv, sizeof(struct timeval));
+}
+#endif
+
 #ifdef CONFIG_NET_SOCKOPTS
 static void udp_recvpktinfo(FAR struct udp_recvfrom_s *pstate,
                             FAR void *srcaddr, uint8_t ifindex)
@@ -178,7 +192,7 @@ static inline void udp_readahead(struct udp_recvfrom_s *pstate)
 #endif
 
       /* Unflatten saved connection information
-       * Layout: |datalen|ifindex|src_addr_size|src_addr|data|
+       * Layout: |datalen|ifindex|src_addr_size|src_addr|[timestamp]|data|
        */
 
       recvlen = iob_copyout((FAR uint8_t *)&datalen, iob,
@@ -202,6 +216,22 @@ static inline void udp_readahead(struct udp_recvfrom_s *pstate)
       offset += src_addr_size;
       DEBUGASSERT(recvlen == src_addr_size);
 
+#ifdef CONFIG_NET_TIMESTAMP
+      /* Unpack stored timestamp if SO_TIMESTAMP socket option is enabled */
+
+      if (conn->timestamp)
+        {
+          struct timespec timestamp;
+          recvlen = iob_copyout((FAR uint8_t *)&timestamp, iob,
+                                sizeof(struct timespec), offset);
+          DEBUGASSERT(recvlen == sizeof(struct timespec));
+
+          udp_store_cmsg_timestamp(pstate, &timestamp);
+        }
+
+      offset += sizeof(struct timespec);
+#endif
+
       /* Copy to user */
 
       recvlen = iob_copyout(pstate->ir_msg->msg_iov->iov_base, iob,
@@ -434,6 +464,15 @@ static uint16_t udp_eventhandler(FAR struct net_driver_s *dev,
 
       else if ((flags & UDP_NEWDATA) != 0)
         {
+          /* Save packet timestamp, if requested */
+
+#ifdef CONFIG_NET_TIMESTAMP
+          if (pstate->ir_conn->timestamp)
+            {
+              udp_store_cmsg_timestamp(pstate, &dev->d_rxtime);
+            }
+#endif
+
           /* Save the sender's address in the caller's 'from' location */
 
           udp_sender(dev, pstate);