Add software-pps program

.gitignore

@@ -7,6 +7,7 @@ clockres/build/clockres_arm
 packet-exchange/build/server
 packet-exchange/build/client
 gettime/build/gettime
+software-pps/build/software-pps
 
 scripts/packet-histogram_stop-options*
 

software-pps/build/Makefile

+PROG = software-pps
+SRCDIR = ../src
+
+SRCS  = software-pps.c
+SRCS += common.c
+SRCS += pulse.c
+
+OBJS = $(SRCS:%.c=%.o)
+
+ifeq ($(DEBUG),)
+CFLAGS = -O2
+else
+CFLAGS = -Og -g -Wall -Wextra
+endif
+
+CFLAGS += -MD -MP
+CFLAGS += -I $(SRCDIR)
+CFLAGS += -std=gnu99
+
+LLIBS = -pthread
+
+vpath %.c $(SRCDIR)
+
+$(PROG): $(OBJS)
+	$(CC) $(LDFLAGS) $(LDIRS) $^ $(LLIBS) -o $@
+
+-include $(subst .c,.d,$(SRCS))
+
+clean:
+	$(RM) $(OBJS) $(PROG) $(subst .c,.d,$(SRCS))
+
+.PHONY: clean

software-pps/src/common.c

+#define _GNU_SOURCE
+#include "common.h"
+
+#include <inttypes.h>
+#include <signal.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <string.h>
+#include <time.h>
+#include <unistd.h>
+
+uint64_t ts_to_uint(struct timespec t) {
+  return t.tv_sec * NSEC_PER_SEC + t.tv_nsec;
+}
+struct timespec uint_to_ts(uint64_t t) {
+  struct timespec ts;
+  ts.tv_sec = t / NSEC_PER_SEC;
+  ts.tv_nsec = t - (ts.tv_sec * NSEC_PER_SEC);
+  return ts;
+}
+
+void add_ns(struct timespec *t, uint64_t ns) {
+  t->tv_nsec += ns;
+
+  while (t->tv_nsec >= ((int64_t) NSEC_PER_SEC)) {
+    t->tv_sec += 1;
+    t->tv_nsec -= NSEC_PER_SEC;
+  }
+}
+
+uint64_t calcdiff_ns(struct timespec t1, struct timespec t2) {
+  uint64_t diff;
+  diff = NSEC_PER_SEC * (uint64_t)((int)t1.tv_sec - (int)t2.tv_sec);
+  diff += ((int)t1.tv_nsec - (int)t2.tv_nsec);
+  return diff;
+}
+
+int _max_(int a, int b) { return a > b ? a : b; }
+int _min_(int a, int b) { return a < b ? a : b; }
+

software-pps/src/common.h

+#ifndef UTILITIES_H
+#define UTILITIES_H
+
+#define _GNU_SOURCE
+#include <inttypes.h>
+#include <signal.h>
+#include <stdint.h>
+#include <time.h>
+#include <unistd.h>
+
+#ifdef WITH_XDP
+#include <bpf/bpf.h>
+#include <bpf/libbpf.h>
+#include <bpf/xsk.h>
+#include <linux/if_ether.h>
+#include <linux/if_link.h>
+#include <linux/if_xdp.h>
+#include <linux/ip.h>
+#include <linux/udp.h>
+#endif
+
+#define NSEC_PER_SEC UINT64_C(1000000000)
+#define SERVER_PORT "50000"
+#define SERVER_PORT_INT 50000
+
+#define MAX_KERNEL_LATENCY 1000
+#define MAX_RTT 1000
+#define MAX_JITTER 1000
+
+#define MAX_BUFFER_SIZE 1024
+
+#define TIMESTAMP_BUFFER_SIZE 4096
+#define err(...)                              \
+	do {                                  \
+		fprintf(stderr, __VA_ARGS__); \
+		exit(EXIT_FAILURE);           \
+	} while (0)
+
+#define err_errno(...) error(EXIT_FAILURE, errno, __VA_ARGS__);
+
+uint64_t ts_to_uint(struct timespec t);
+struct timespec uint_to_ts(uint64_t t);
+void add_ns(struct timespec *t, uint64_t ns);
+uint64_t calcdiff_ns(struct timespec t1, struct timespec t2);
+
+void init_signals(void (*_sighand)(int));
+
+int _min_(int a, int b);
+int _max_(int a, int b);
+
+int histogram_min(uint64_t *histogram, int max_value);
+int histogram_max(uint64_t *histogram, int max_value);
+
+extern void (*previous_handlers[NSIG])(int);
+
+#endif

software-pps/src/pulse.c

+#include "pulse.h"
+
+#include <fcntl.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <sys/stat.h>
+#include <unistd.h>
+
+static int fd;
+
+void enable_pulse() { fd = open("/dev/ttyS0", O_WRONLY); }
+
+void send_pulse() { write(fd, "1", 1); }

software-pps/src/pulse.h

+#ifndef PULSE_H
+#define PULSE_H
+
+void enable_pulse();
+void send_pulse();
+
+#endif

software-pps/src/software-pps.c

+/*
+ * Software PPS
+ *
+ * Large portions taken from cyclictest
+ *
+ */
+
+#define _GNU_SOURCE
+#include <errno.h>
+#include <error.h>
+#include <limits.h>
+#include <pthread.h>
+#include <sched.h>
+#include <signal.h>
+#include <stdint.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/mman.h>
+#include <time.h>
+#include <unistd.h>
+
+#include "pulse.h"
+#include "common.h"
+
+// Structs
+
+typedef struct thread_param {
+  uint64_t interval;
+  unsigned int max_cycles;
+  int priority;
+  int affinity_cpu;
+  uint64_t start_ts;
+
+} thread_param_t;
+
+typedef struct main_param {
+  int refresh_rate;
+  int verbose;
+} main_param_t;
+
+// Static functions
+
+static void process_options(int argc, char *argv[]);
+static uint64_t nb_cycles;
+
+// Static variables
+
+static main_param_t main_params;
+static thread_param_t thread_params;
+
+static void help(char *argv[]) {
+  printf(
+      "Usage: %s [-a CPU -p PRIO -i USEC -r USEC -s NS -l N -v]\n\n"
+      "       -h                 Show help\n"
+      "       -a CPU             Pin the real time thread to CPU\n"
+      "       -p PRIO            RT thread priority\n"
+      "       -i USEC            RT thread wake-up interval (default: 10ms)\n"
+      "       -r USEC            non-RT main thread refresh interval\n"
+      "       -l N_CYCLES        RT thread cycles amount (default: infinite)\n"
+      "       -s NS              Common start time reference\n"
+      "       -v                 Verbose\n"
+      "\n",
+      argv[0]);
+}
+
+/*
+ * Real-time thread:
+ */
+static void *pps_thread(void *p) {
+  (void)p;
+  struct timespec next;
+  int ret;
+  cpu_set_t mask;
+
+  // Set thread CPU affinity
+  if (thread_params.affinity_cpu != -1) {
+    CPU_ZERO(&mask);
+    CPU_SET(thread_params.affinity_cpu, &mask);
+    if (sched_setaffinity(0, sizeof(mask), &mask))
+      error(EXIT_FAILURE, errno, "Could not set CPU affinity to CPU %d\n",
+            thread_params.affinity_cpu);
+  }
+
+  clock_gettime(CLOCK_REALTIME, &next);
+
+  if (thread_params.start_ts) {
+    if(thread_params.start_ts < ts_to_uint(next)) {
+      fprintf(stderr, "start timestamp is in the past, aborting...\n");
+      exit(EXIT_FAILURE);
+    }
+    if(thread_params.start_ts > (ts_to_uint(next) + UINT64_C(3600000000000))) {
+      fprintf(stderr, "start timestamp is too high, aborting...\n");
+      exit(EXIT_FAILURE);
+    }
+    next = uint_to_ts(thread_params.start_ts);
+  }
+
+  // Packet sending loop
+  for (nb_cycles = 0;; nb_cycles++) {
+    if (thread_params.max_cycles &&
+        nb_cycles >= ((unsigned int)thread_params.max_cycles))
+      break;
+
+
+    send_pulse();
+    add_ns(&next, thread_params.interval);
+
+    ret = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &next, NULL);
+    if(ret) {
+      fprintf(stderr, "clock_nanosleep returned error: %d, aborting...\n", ret);
+      exit(EXIT_FAILURE);
+    }
+  }
+
+  pthread_exit(NULL);
+}
+
+/*
+ * Main thread:
+ *  - Has non-real time priority
+ *  - Handles the IO and creates the real time thread
+ */
+int main(int argc, char *argv[]) {
+  pthread_t thread;
+  struct sched_param param;
+  pthread_attr_t attr;
+
+  // Default configuration values
+  thread_params.interval = 100000 * 1000;
+  thread_params.max_cycles = 0;
+  thread_params.priority = 98;
+  thread_params.affinity_cpu = -1;
+  thread_params.start_ts = 0;
+  main_params.refresh_rate = 50000;
+  main_params.verbose = 0;
+
+  // Lock all current and future pages from preventing of being paged to
+  // swap
+  if (mlockall(MCL_CURRENT | MCL_FUTURE)) {
+    fprintf(stderr, "mlockall failed");
+    exit(EXIT_FAILURE);
+  }
+
+  // Process bash options
+  process_options(argc, argv);
+
+  enable_pulse();
+
+  // Initialize pthread attributes (default values)
+  if (pthread_attr_init(&attr)) {
+    fprintf(stderr, "init pthread attributes failed\n");
+    exit(EXIT_FAILURE);
+  }
+  // Set a specific stack size
+  if (pthread_attr_setstacksize(&attr, PTHREAD_STACK_MIN)) {
+    fprintf(stderr, "pthread setstacksize failed\n");
+    exit(EXIT_FAILURE);
+  }
+  // Set scheduler policy and priority of pthread
+  if (pthread_attr_setschedpolicy(&attr, SCHED_FIFO)) {
+    fprintf(stderr, "pthread setschedpolicy failed\n");
+    exit(EXIT_FAILURE);
+  }
+  param.sched_priority = thread_params.priority;
+  if (pthread_attr_setschedparam(&attr, &param)) {
+    fprintf(stderr, "pthread setschedparam failed\n");
+    exit(EXIT_FAILURE);
+  }
+  /* Use scheduling parameters of attr */
+  if (pthread_attr_setinheritsched(&attr, PTHREAD_EXPLICIT_SCHED)) {
+    fprintf(stderr, "pthread setinheritsched failed\n");
+    exit(EXIT_FAILURE);
+  }
+
+  // Create the real time thread
+  if (pthread_create(&thread, &attr, pps_thread, NULL))
+    error(EXIT_FAILURE, errno, "Couldn't create pps thread");
+
+  // Verbose loop
+  for (;;) {
+    usleep(main_params.refresh_rate);
+
+    if (main_params.verbose) {
+        printf("Toggles: %9" PRIu64 "\n", nb_cycles);
+        printf("\033[%dA", 1);
+    }
+
+    if (thread_params.max_cycles)
+      if (thread_params.max_cycles == nb_cycles) break;
+  }
+
+  exit(EXIT_SUCCESS);
+}
+
+
+/* Process bash options
+ */
+static void process_options(int argc, char *argv[]) {
+  for (;;) {
+    int c = getopt(argc, argv, "a:hi:l:p:r:s:v");
+
+    if (c == -1) break;
+
+    switch (c) {
+      case 'a':
+        thread_params.affinity_cpu = atoi(optarg);
+        break;
+      case 'h':
+        help(argv);
+        exit(EXIT_SUCCESS);
+        break;
+      case 'i':
+        thread_params.interval = strtoull(optarg, NULL, 10) * 1000;
+        break;
+      case 'l':
+        thread_params.max_cycles = atoi(optarg);
+        break;
+      case 'p':
+        thread_params.priority = atoi(optarg);
+        break;
+      case 's':
+        thread_params.start_ts = strtoull(optarg, NULL, 10);
+        break;
+      case 'r':
+        main_params.refresh_rate = atoi(optarg);
+        break;
+      case 'v':
+        main_params.verbose = 1;
+        break;
+    }
+  }
+}