Change function to send UDP packets, add ETF qdisc support

packet-exchange/src/client.c

@@ -4,12 +4,17 @@
  * Bash options:
  *
  *  -a          Run the real time thread on CPU1
- *  -i USEC     Wake up the real time thread every USEC microseconds
- *  -l N        Wake up the real time thread N times
+ *  -e          Set a txtime
+ *  -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
+ *  -r USEC     Refresh the non real time main thread every USEC microseconds (Default: 50ms)
+ *
+ * Large portions taken from cyclictest
  *
  */
+#define _GNU_SOURCE
 #include <errno.h>
 #include <error.h>
 #include <pthread.h>
@@ -23,8 +28,6 @@
 
 #include "send_packet.h"
 
-#define _GNU_SOURCE
-
 #define CLOCK_ID CLOCK_MONOTONIC
 #define NSEC_PER_SEC UINT64_C(1000000000)
 
@@ -38,8 +41,10 @@ typedef struct thread_param {
   int max_cycles;
 
   int enable_affinity;
+  int enable_etf;
 
   const char *ip_address;
+  char network_if[256];
   thread_stat_t stats;
 } thread_param_t;
 
@@ -47,6 +52,7 @@ typedef struct main_param {
   int refresh_rate;
 } main_param_t;
 
+static inline void add_ns(struct timespec *t, uint64_t ns);
 static void process_options(int argc, char *argv[], thread_param_t *param,
                             main_param_t *main_param);
 
@@ -54,6 +60,7 @@ static void process_options(int argc, char *argv[], thread_param_t *param,
 // Sends packets at a regular intervall
 static void *packet_sending_thread(void *p) {
   struct timespec next;
+  uint64_t next_txtime;
   struct sched_param priority;
   thread_param_t *param = (thread_param_t *)p;
   cpu_set_t mask;
@@ -72,20 +79,21 @@ static void *packet_sending_thread(void *p) {
     error(EXIT_FAILURE, errno, "Couldn't set priority");
 
   clock_gettime(CLOCK_ID, &next);
+  next_txtime = next.tv_sec * NSEC_PER_SEC + next.tv_nsec;
+  // Wait around 1 second
+  next_txtime += (NSEC_PER_SEC / param->interval) * param->interval;
+  // Send packet while thread is sleeping
+  next_txtime += (param->interval) / 2;
 
   // Packet sending loop
   for (param->stats.nb_cycles = 0;; param->stats.nb_cycles++) {
     if (param->max_cycles)
       if (param->stats.nb_cycles >= param->max_cycles) break;
 
-    send_udp_packet(param->ip_address);
-
-    next.tv_nsec += param->interval;
+    send_udp_packet_etf(param->enable_etf, next_txtime, param->ip_address);
 
-    if ((unsigned int)next.tv_nsec >= NSEC_PER_SEC) {
-      next.tv_sec += 1;
-      next.tv_nsec -= NSEC_PER_SEC;
-    }
+    add_ns(&next, param->interval);
+    next_txtime += (param->interval) / 2;
 
     clock_nanosleep(CLOCK_ID, TIMER_ABSTIME, &next, NULL);
   }
@@ -104,12 +112,15 @@ int main(int argc, char *argv[]) {
   param.interval = 100000 * 1000;
   param.max_cycles = 0;
   param.priority = 99;
+  param.enable_affinity = 0;
 
   main_param.refresh_rate = 50000;
 
   // Process bash options
   process_options(argc, argv, &param, &main_param);
 
+  init_udp_etf(param.enable_affinity, param.network_if);
+
   usleep(10000);
 
   if (pthread_create(&thread, NULL, packet_sending_thread, (void *)&param))
@@ -132,7 +143,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, "ai:l:p:r:");
+    int c = getopt(argc, argv, "aef:i:l:p:r:");
 
     if (c == -1) break;
 
@@ -140,6 +151,12 @@ static void process_options(int argc, char *argv[], thread_param_t *param,
       case 'a':
         param->enable_affinity = 1;
         break;
+      case 'e':
+        param->enable_etf = 1;
+        break;
+      case 'f':
+        strcpy(param->network_if, optarg);
+        break;
       case 'i':
         param->interval = atoi(optarg) * 1000;
         break;
@@ -165,3 +182,11 @@ static void process_options(int argc, char *argv[], thread_param_t *param,
   param->ip_address = argv[optind];
 }
 
+static inline void add_ns(struct timespec *t, uint64_t ns) {
+  t->tv_nsec += ns;
+
+  if ((unsigned int)t->tv_nsec >= NSEC_PER_SEC) {
+    t->tv_sec += 1;
+    t->tv_nsec -= NSEC_PER_SEC;
+  }
+}

packet-exchange/src/send_packet.c

+/*
+ *
+ * UDP packet sending functions
+ *
+ * Large portions taken from scheduled tx tools gist
+ * from linutronix
+ *
+ */
+
 #include "send_packet.h"
 
-#include <stdio.h>
-#include <stdlib.h>
-#include <unistd.h>
+#include <arpa/inet.h>
 #include <errno.h>
-#include <string.h>
+#include <error.h>
+#include <fcntl.h>
+#include <ifaddrs.h>
+#include <linux/errqueue.h>
+#include <linux/ethtool.h>
+#include <linux/net_tstamp.h>
+#include <linux/sockios.h>
+#include <net/if.h>
 #include <netdb.h>
-#include <sys/types.h>
 #include <netinet/in.h>
+#include <poll.h>
+#include <pthread.h>
+#include <sched.h>
+#include <signal.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/ioctl.h>
 #include <sys/socket.h>
-
-#include <arpa/inet.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
 
 #define SERVER_PORT "50000"
+#define SERVER_PORT_INT 50000
+#define CLOCK_ID CLOCK_TAI
+#define MESSAGE ((uint32_t)0x00FACADE)
 
-int send_udp_packet(const char * server_ip) {
+int send_udp_packet(const char *server_ip) {
   int status;
   int sockfd;
 
@@ -29,38 +55,128 @@ int send_udp_packet(const char * server_ip) {
   hints.ai_socktype = SOCK_DGRAM;
 
   status = getaddrinfo(server_ip, SERVER_PORT, &hints, &servinfo);
-  if( status != 0 ) {
+  if (status != 0) {
     fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(status));
-    printf("getaddrinfo error, exiting...\n");
     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) {
-      printf("Socket error, continuing...\n");
-      continue;
-    }
+  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) {
+  if (servinfo_it == NULL) {
     fprintf(stderr, "Failed to create socket\n");
-    printf("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);
+    bytes_sent += sendto(sockfd, msg + bytes_sent, strlen(msg), 0,
+                         servinfo->ai_addr, servinfo->ai_addrlen);
   }
 
   freeaddrinfo(servinfo);
 
-  #ifdef DEBUG_ENABLED
+#ifdef DEBUG_ENABLED
   printf("Sent %d bytes to %s\n", bytes_sent, server_ip);
-  #endif
+#endif
 
   close(sockfd);
   return 0;
 }
+
+/*
+ * ETF qdisc section
+ */
+
+static int so_priority = 3;
+static struct sock_txtime sk_txtime;
+static uint32_t tx_buffer = MESSAGE;
+static uint32_t tx_buffer_len = sizeof(MESSAGE);
+static int fd;
+
+// Sets the interface
+static int set_if(char * network_if) {
+  struct ifreq ifreq;
+
+  memset(&ifreq, 0, sizeof(ifreq));
+  strncpy(ifreq.ifr_name, network_if, sizeof(ifreq.ifr_name) - 1);
+
+  if (ioctl(fd, SIOCGIFINDEX, &ifreq))
+    error(EXIT_FAILURE, errno, "ioctl SIOCGIFINDEX failed\n");
+
+  return ifreq.ifr_ifindex;
+}
+
+/*
+ * Init UDP socket
+ */
+void init_udp_etf(int use_etf, char * network_if) {
+  int index;
+
+  fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
+  if (fd < 0) error(EXIT_FAILURE, errno, "Socket creation failed\n");
+
+  index = set_if(network_if);
+  if (index < 0) error(EXIT_FAILURE, errno, "Couldn't set interface\n");
+
+  if (setsockopt(fd, SOL_SOCKET, SO_PRIORITY, &so_priority,
+                 sizeof(so_priority)))
+    error(EXIT_FAILURE, errno, "Couldn't set socket priority\n");
+
+  if (setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, network_if, strlen(network_if)))
+    error(EXIT_FAILURE, errno, "setsockopt SO_BINDTODEVICE failed\n");
+
+  if (use_etf) {
+    sk_txtime.clockid = CLOCK_ID;
+    sk_txtime.flags = 0;
+    // sk_txtime.flags = SOF_TXTIME_REPORT_ERRORS;
+
+    if (setsockopt(fd, SOL_SOCKET, SO_TXTIME, &sk_txtime, sizeof(sk_txtime)))
+      error(EXIT_FAILURE, errno, "setsockopt SO_TXTIME failed\n");
+  }
+}
+
+/*
+ * Sends udp packets using the ETF qdisc
+ */
+void send_udp_packet_etf(int use_etf, 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;
+  int sendmsgerr;
+
+  memset(&sin, 0, sizeof(sin));
+  sin.sin_family = AF_INET;
+  sin.sin_addr.s_addr = inet_addr(server_ip);
+  sin.sin_port = htons(SERVER_PORT_INT);
+
+  iov.iov_base = &tx_buffer;
+  iov.iov_len = tx_buffer_len;
+
+  memset(&msg, 0, sizeof(msg));
+  msg.msg_name = &sin;
+  msg.msg_namelen = sizeof(sin);
+  msg.msg_iov = &iov;
+  msg.msg_iovlen = 1;
+
+  if (use_etf) {
+    // We specify the transmission time in the CMSG.
+    msg.msg_control = control;
+    msg.msg_controllen = sizeof(control);
+
+    cmsg = CMSG_FIRSTHDR(&msg);
+    cmsg->cmsg_level = SOL_SOCKET;
+    cmsg->cmsg_type = SCM_TXTIME;
+    cmsg->cmsg_len = CMSG_LEN(sizeof(__u64));
+    *((__u64 *)CMSG_DATA(cmsg)) = txtime;
+  }
+
+  sendmsgerr = sendmsg(fd, &msg, 0);
+  if (sendmsgerr) error(EXIT_FAILURE, errno, "sendmsg failed, ret value: %d\n", sendmsgerr);
+}

packet-exchange/src/send_packet.h

 #ifndef SEND_PACKET_H
 #define SEND_PACKET_H
 
-int send_udp_packet(const char * ip4);
+#include <stdint.h>
+
+int send_udp_packet(const char *ip4);
+void init_udp_etf(int use_etf, char * network_if);
+void send_udp_packet_etf(int use_etf, uint64_t txtime, const char *server_ip);
 
 #endif

packet-exchange/src/server.c

@@ -7,6 +7,8 @@
  *  -p PRIO     Run the real time thread at priority PRIO
  *  -r USEC     Refresh the non real time main thread every USEC microseconds
  *
+ * Large portions taken from cyclictest
+ *
  */
 
 #include <arpa/inet.h>