Merge branch 'packet-exchange' into scripts

packet-exchange/src/client.c

@@ -4,16 +4,18 @@
  * Bash options:
  *
  *  -a          Run the real time thread on CPU1
- *  -e          Set a txtime
+ *  -e          Set a txtime (to be used in an ETF qdisc)
  *  -f          Set the network interface to be used
  *  -i USEC     Wake up the real time thread every USEC microseconds (Default: 10ms)
  *  -l N        Wake up the real time thread N times (Default: 0)
  *  -p PRIO     Run the real time thread at priority PRIO
  *  -r USEC     Refresh the non real time main thread every USEC microseconds (Default: 50ms)
+ *  -t          Enable timestamps
  *
  * Large portions taken from cyclictest
  *
  */
+
 #define _GNU_SOURCE
 #include <errno.h>
 #include <error.h>
@@ -42,6 +44,7 @@ typedef struct thread_param {
 
   int enable_affinity;
   int enable_etf;
+  int enable_timestamps;
 
   const char *ip_address;
   char network_if[256];
@@ -91,7 +94,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 +125,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 +149,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, "aef:i:l:p:q:r:t");
 
     if (c == -1) break;
 
@@ -175,6 +178,9 @@ static void process_options(int argc, char *argv[], thread_param_t *param,
       case 'r':
         main_param->refresh_rate = atoi(optarg);
         break;
+      case 't':
+        param->enable_timestamps = 1;
+        break;
       default:
         exit(EXIT_FAILURE);
         break;

packet-exchange/src/send_packet.c

@@ -14,6 +14,7 @@
 #include <error.h>
 #include <fcntl.h>
 #include <ifaddrs.h>
+#include <inttypes.h>
 #include <linux/errqueue.h>
 #include <linux/ethtool.h>
 #include <linux/net_tstamp.h>
@@ -39,61 +40,10 @@
 #define SERVER_PORT_INT 50000
 #define CLOCK_ID CLOCK_TAI
 #define MESSAGE ((uint32_t)0x00FACADE)
-#define NSEC_PER_SEC 1000000000
+#define NSEC_PER_SEC ((uint64_t)1000000000)
 
-int send_udp_packet(const char *server_ip) {
-  int status;
-  int sockfd;
-
-  struct addrinfo hints, *servinfo, *servinfo_it;
-
-  char msg[1024] = "hello";
-  int bytes_sent = 0;
-  int msg_len = strlen(msg);
-
-  memset(&hints, 0, sizeof hints);
-  hints.ai_family = AF_UNSPEC;
-  hints.ai_socktype = SOCK_DGRAM;
-
-  status = getaddrinfo(server_ip, SERVER_PORT, &hints, &servinfo);
-  if (status != 0) {
-    fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(status));
-    return 1;
-  }
-
-  for (servinfo_it = servinfo; servinfo_it != NULL;
-       servinfo_it = servinfo_it->ai_next) {
-    sockfd = socket(servinfo->ai_family, servinfo->ai_socktype,
-                    servinfo->ai_protocol);
-    if (sockfd == -1) continue;
-    break;
-  }
-
-  if (servinfo_it == NULL) {
-    fprintf(stderr, "Failed to create socket\n");
-    return 2;
-  }
-
-  while (bytes_sent < msg_len) {
-    bytes_sent += sendto(sockfd, msg + bytes_sent, strlen(msg), 0,
-                         servinfo->ai_addr, servinfo->ai_addrlen);
-  }
-
-  freeaddrinfo(servinfo);
-
-#ifdef DEBUG_ENABLED
-  printf("Sent %d bytes to %s\n", bytes_sent, server_ip);
-#endif
-
-  close(sockfd);
-  return 0;
-}
-
-/*
- * ETF qdisc section
- */
-
-static int process_socket_error_queue(int fd);
+static void print_timestamps(struct msghdr *msg, uint64_t txtime);
+static void process_timestamps(uint64_t txtime);
 
 static int so_priority = 3;
 static struct sock_txtime sk_txtime;
@@ -101,6 +51,11 @@ static unsigned char tx_buffer[1024] = "Hi";
 static size_t tx_buffer_len = sizeof(tx_buffer);
 static int fd;
 
+static int64_t tai_offset;
+
+static int so_timestamping_flags =
+    SOF_TIMESTAMPING_TX_SOFTWARE | SOF_TIMESTAMPING_SOFTWARE;
+
 // Sets the interface
 static int set_if(char *network_if) {
   struct ifreq ifreq;
@@ -117,9 +72,16 @@ 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;
 
+  struct timespec ts_mon;
+  struct timespec ts_tai;
+  clock_gettime(CLOCK_MONOTONIC, &ts_mon);
+  clock_gettime(CLOCK_TAI, &ts_tai);
+  tai_offset = (ts_mon.tv_sec - ts_tai.tv_sec) * NSEC_PER_SEC + (ts_mon.tv_nsec - ts_tai.tv_nsec);
+
   so_priority = packet_priority;
 
   fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
@@ -138,12 +100,17 @@ void init_udp_etf(int use_etf, int packet_priority, char *network_if) {
 
   if (use_etf) {
     sk_txtime.clockid = CLOCK_ID;
-    sk_txtime.flags = 1;
-    sk_txtime.flags = SOF_TXTIME_REPORT_ERRORS;
+    sk_txtime.flags = 0;
 
     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() {
@@ -160,16 +127,16 @@ 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;
   struct msghdr msg;
   struct iovec iov;
-  struct pollfd poll_fd = {
-      .fd = fd,
-  };
-  int sendmsgerr, pollerr;
+  int sendmsgerr;
+  int res;
+  struct pollfd poll_fd = {fd, POLLPRI, 0};
 
   memset(&sin, 0, sizeof(sin));
   sin.sin_family = AF_INET;
@@ -202,17 +169,81 @@ void send_udp_packet_etf(int use_etf, uint64_t txtime, const char *server_ip) {
   if (sendmsgerr < 0)
     error(EXIT_FAILURE, errno, "sendmsg failed, ret value: %d\n", sendmsgerr);
 
-  pollerr = poll(&poll_fd, 1, 0);
-  // If the poll revents containts a POLLERR flag
-  if (pollerr == 1 && (poll_fd.revents & POLLERR)) {
-    process_socket_error_queue(fd);
+  if (use_timestamps) {
+    res = poll(&poll_fd, 1, 0);
+    if (res > 0)
+      process_timestamps(txtime);
+    else
+      fprintf(stderr, "select failed\n");
   }
 }
 
+static void process_timestamps(uint64_t txtime) {
+  char data[256];
+  struct msghdr msg;
+  struct iovec entry;
+  struct sockaddr_in from_addr;
+  struct {
+    struct cmsghdr cm;
+    char control[512];
+  } control;
+
+  memset(&msg, 0, sizeof(msg));
+  msg.msg_iov = &entry;
+  msg.msg_iovlen = 1;
+  entry.iov_base = data;
+  entry.iov_len = sizeof(data);
+  msg.msg_name = (caddr_t)&from_addr;
+  msg.msg_namelen = sizeof(from_addr);
+  msg.msg_control = &control;
+  msg.msg_controllen = sizeof(control);
+
+  if (recvmsg(fd, &msg, MSG_ERRQUEUE | MSG_DONTWAIT) == -1) {
+    fprintf(stderr, "recvmsg failed\n");
+  } else {
+    print_timestamps(&msg, txtime);
+  }
+}
+
+static void print_timestamps(struct msghdr *msg, uint64_t txtime) {
+  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);
+            uint64_t timestamp_ns =
+                stamp->tv_sec * NSEC_PER_SEC + stamp->tv_nsec;
+            printf("SO_TIMESTAMPING ");
+            printf("SW %" PRIu64 " - %" PRIu64 " (%" PRIi64 ") ", timestamp_ns,
+                   txtime, ((int64_t)timestamp_ns) - txtime - tai_offset);
+            break;
+          }
+          default:
+#ifdef DEBUG
+            printf("type %d", cmsg->cmsg_type);
+#endif
+            break;
+        }
+        break;
+      default:
+#ifdef DEBUG
+        printf("level %d type %d", cmsg->cmsg_level, cmsg->cmsg_type);
+#endif
+        break;
+    }
+    printf("\n");
+  }
+}
+
+#ifdef DEBUG
 /*
  * Code from scheduled_tx_tools
  */
-static int process_socket_error_queue(int fd) {
+static int process_socket_error_queue() {
   uint8_t msg_control[CMSG_SPACE(sizeof(struct sock_extended_err))];
   unsigned char err_buffer[sizeof(tx_buffer)];
   struct sock_extended_err *serr;
@@ -257,3 +288,4 @@ static int process_socket_error_queue(int fd) {
 
   return 0;
 }
+#endif

packet-exchange/src/send_packet.h

@@ -3,8 +3,7 @@
 
 #include <stdint.h>
 
-int send_udp_packet(const char *ip4);
-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