Add timestamps

packet-exchange/src/client.c

@@ -42,6 +42,7 @@ typedef struct thread_param {
 
   int enable_affinity;
   int enable_etf;
+  int enable_timestamps;
 
   const char *ip_address;
   char network_if[256];
@@ -91,7 +92,7 @@ static void *packet_sending_thread(void *p) {
     if (param->max_cycles)
       if (param->stats.nb_cycles >= param->max_cycles) break;
 
-    send_udp_packet_etf(param->enable_etf, next_txtime, param->ip_address);
+    send_udp_packet(param->enable_etf, param->enable_timestamps, next_txtime, param->ip_address);
 
     add_ns(&next, param->interval);
     next_txtime += (param->interval) / 2;
@@ -122,7 +123,7 @@ int main(int argc, char *argv[]) {
   // Process bash options
   process_options(argc, argv, &param, &main_param);
 
-  init_udp_etf(param.enable_etf, main_param.packet_priority, param.network_if);
+  init_udp_etf(param.enable_etf, param.enable_timestamps, main_param.packet_priority, param.network_if);
 
   usleep(10000);
 
@@ -146,7 +147,7 @@ int main(int argc, char *argv[]) {
 static void process_options(int argc, char *argv[], thread_param_t *param,
                             main_param_t *main_param) {
   for (;;) {
-    int c = getopt(argc, argv, "aef:i:l:p:q:r:");
+    int c = getopt(argc, argv, "aehf:i:l:p:q:r:");
 
     if (c == -1) break;
 
@@ -157,6 +158,9 @@ static void process_options(int argc, char *argv[], thread_param_t *param,
       case 'e':
         param->enable_etf = 1;
         break;
+      case 'h':
+        param->enable_timestamps = 1;
+        break;
       case 'f':
         strcpy(param->network_if, optarg);
         break;

packet-exchange/src/send_packet.c

@@ -41,7 +41,11 @@
 #define MESSAGE ((uint32_t)0x00FACADE)
 #define NSEC_PER_SEC 1000000000
 
+#define DEBUG
+
 static int process_socket_error_queue(int fd);
+static void print_timestamps(struct msghdr *msg);
+static void process_timestamps(int sock);
 
 static int so_priority = 3;
 static struct sock_txtime sk_txtime;
@@ -49,6 +53,9 @@ static unsigned char tx_buffer[1024] = "Hi";
 static size_t tx_buffer_len = sizeof(tx_buffer);
 static int fd;
 
+static int so_timestamping_flags =
+    SOF_TIMESTAMPING_RX_SOFTWARE | SOF_TIMESTAMPING_TX_SOFTWARE;
+
 // Sets the interface
 static int set_if(char *network_if) {
   struct ifreq ifreq;
@@ -65,7 +72,8 @@ static int set_if(char *network_if) {
 /*
  * Init UDP socket
  */
-void init_udp_etf(int use_etf, int packet_priority, char *network_if) {
+void init_udp_etf(int use_etf, int use_timestamps, int packet_priority,
+                  char *network_if) {
   int index;
 
   so_priority = packet_priority;
@@ -92,6 +100,12 @@ void init_udp_etf(int use_etf, int packet_priority, char *network_if) {
     if (setsockopt(fd, SOL_SOCKET, SO_TXTIME, &sk_txtime, sizeof(sk_txtime)))
       error(EXIT_FAILURE, errno, "setsockopt SO_TXTIME failed\n");
   }
+
+  if (use_timestamps) {
+    if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING, &so_timestamping_flags,
+                   sizeof(so_timestamping_flags)))
+      error(EXIT_FAILURE, errno, "setsockopt SO_TIMESTAMPING failed\n");
+  }
 }
 
 uint64_t get_txtime() {
@@ -108,7 +122,8 @@ uint64_t get_txtime() {
 /*
  * Sends udp packets using the ETF qdisc
  */
-void send_udp_packet_etf(int use_etf, uint64_t txtime, const char *server_ip) {
+void send_udp_packet(int use_etf, int use_timestamps, uint64_t txtime,
+                     const char *server_ip) {
   char control[CMSG_SPACE(sizeof(txtime))] = {};
   struct sockaddr_in sin;
   struct cmsghdr *cmsg;
@@ -154,9 +169,36 @@ void send_udp_packet_etf(int use_etf, uint64_t txtime, const char *server_ip) {
   // If the poll revents containts a POLLERR flag
   if (pollerr == 1 && (poll_fd.revents & POLLERR)) {
     process_socket_error_queue(fd);
+  } else if(use_timestamps) {
+    process_timestamps(fd);
   }
 }
 
+static void process_timestamps(int sock) {
+  struct msghdr msg;
+  struct iovec entry;
+  struct {
+    struct cmsghdr cm;
+    char control[512];
+  } control;
+
+#ifdef DEBUG
+  printf("process_timestamps\n");
+#endif
+
+  memset(&msg, 0, sizeof(msg));
+  msg.msg_iov = &entry;
+  msg.msg_iovlen = 1;
+  msg.msg_control = &control;
+  msg.msg_controllen = sizeof(control);
+
+  if (recvmsg(sock, &msg, 0) == -1) {
+    fprintf(stderr, "recvmsg failed");
+    exit(EXIT_FAILURE);
+  }
+  print_timestamps(&msg);
+}
+
 /*
  * Code from scheduled_tx_tools
  */
@@ -173,6 +215,10 @@ static int process_socket_error_queue(int fd) {
                        .msg_control = msg_control,
                        .msg_controllen = sizeof(msg_control)};
 
+#ifdef DEBUG
+  printf("process_socket_error_queue\n");
+#endif
+
   if (recvmsg(fd, &msg, MSG_ERRQUEUE) == -1) {
     fprintf(stderr, "recvmsg failed");
     return -1;
@@ -205,3 +251,57 @@ static int process_socket_error_queue(int fd) {
 
   return 0;
 }
+
+static void print_timestamps(struct msghdr *msg) {
+  struct cmsghdr *cmsg;
+
+  for (cmsg = CMSG_FIRSTHDR(msg); cmsg; cmsg = CMSG_NXTHDR(msg, cmsg)) {
+    switch (cmsg->cmsg_level) {
+      case SOL_SOCKET:
+        printf("SOL_SOCKET ");
+        switch (cmsg->cmsg_type) {
+          case SO_TIMESTAMPING: {
+            struct timespec *stamp = (struct timespec *)CMSG_DATA(cmsg);
+            printf("SO_TIMESTAMPING ");
+            printf("SW %ld.%09ld ", (long)stamp->tv_sec, (long)stamp->tv_nsec);
+            stamp++;
+            printf("HW transformed %ld.%09ld ", (long)stamp->tv_sec,
+                   (long)stamp->tv_nsec);
+            stamp++;
+            printf("HW raw %ld.%09ld", (long)stamp->tv_sec,
+                   (long)stamp->tv_nsec);
+            break;
+          }
+          default:
+            printf("type %d", cmsg->cmsg_type);
+            break;
+        }
+        break;
+      case IPPROTO_IP:
+        printf("IPPROTO_IP ");
+        switch (cmsg->cmsg_type) {
+          case IP_RECVERR: {
+            struct sock_extended_err *err =
+                (struct sock_extended_err *)CMSG_DATA(cmsg);
+            printf("IP_RECVERR ee_errno '%s' ee_origin %d => %s",
+                   strerror(err->ee_errno), err->ee_origin,
+                   "probably SO_EE_ORIGIN_TIMESTAMPING");
+            break;
+          }
+          case IP_PKTINFO: {
+            struct in_pktinfo *pktinfo = (struct in_pktinfo *)CMSG_DATA(cmsg);
+            printf("IP_PKTINFO interface index %u", pktinfo->ipi_ifindex);
+            break;
+          }
+          default:
+            printf("type %d", cmsg->cmsg_type);
+            break;
+        }
+        break;
+      default:
+        printf("level %d type %d", cmsg->cmsg_level, cmsg->cmsg_type);
+        break;
+    }
+    printf("\n");
+  }
+}

packet-exchange/src/send_packet.h

@@ -3,7 +3,7 @@
 
 #include <stdint.h>
 
-void init_udp_etf(int use_etf, int so_priority, char * network_if);
-void send_udp_packet_etf(int use_etf, uint64_t txtime, const char *server_ip);
+void init_udp_etf(int use_etf, int use_timestamps, int so_priority, char * network_if);
+void send_udp_packet(int use_etf, int use_timestamps, uint64_t txtime, const char *server_ip);
 
 #endif