Update XDP, filter out packets not sent on the correct port

3f5754b5 · Joanne Hugé · 166a884d · 3f5754b5 · 3f5754b5 · 3f5754b5
Commit 3f5754b5 authored Sep 17, 2020 by Joanne Hugé
24 changed files
--- a/.gitignore
+++ b/.gitignore
 *.o
+*.ll
 *.d
 latency-measure/build/main
 latency-measure/build/main

--- a/clockres/src/clockres.c
+++ b/clockres/src/clockres.c
@@ -45,6 +45,21 @@ static inline uint64_t calcdiff_ns(struct timespec t1, struct timespec t2);
 static inline uint64_t max(uint64_t a, uint64_t b);
 static inline uint64_t min(uint64_t a, uint64_t b);
+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"
+      "       -f                 Trace\n"
+      "       -g                 Use function_graph when tracing\n"
+      "\n",
+      argv[0]);
+}
 // Real-time thread
 static void *timerthread(void *p) {
  struct timespec previous, current, next;
@@ -174,11 +189,15 @@ 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, "l:p:i:r:f:ag");
+    int c = getopt(argc, argv, "hl:p:i:r:f:ag");
    if (c == -1) break;
    switch (c) {
+      case 'h':
+        help(argv);
+        exit(EXIT_SUCCESS);
+        break;
      case 'p':
        param->priority = atoi(optarg);
        break;
@@ -202,6 +221,7 @@ static void process_options(int argc, char *argv[], thread_param_t *param,
        main_param->enable_graph = 1;
        break;
      default:
+        help(argv);
        exit(EXIT_FAILURE);
        break;
    }

--- a/packet-exchange/src/bpf_endian.h
+++ b/packet-exchange/src/bpf_endian.h
+/* SPDX-License-Identifier: GPL-2.0 */
+/* Copied from $(LINUX)/tools/testing/selftests/bpf/bpf_endian.h */
+#ifndef __BPF_ENDIAN__
+#define __BPF_ENDIAN__
+#include <linux/swab.h>
+/* LLVM's BPF target selects the endianness of the CPU
+ * it compiles on, or the user specifies (bpfel/bpfeb),
+ * respectively. The used __BYTE_ORDER__ is defined by
+ * the compiler, we cannot rely on __BYTE_ORDER from
+ * libc headers, since it doesn't reflect the actual
+ * requested byte order.
+ *
+ * Note, LLVM's BPF target has different __builtin_bswapX()
+ * semantics. It does map to BPF_ALU | BPF_END | BPF_TO_BE
+ * in bpfel and bpfeb case, which means below, that we map
+ * to cpu_to_be16(). We could use it unconditionally in BPF
+ * case, but better not rely on it, so that this header here
+ * can be used from application and BPF program side, which
+ * use different targets.
+ */
+#if __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__
+# define __bpf_ntohs(x)__builtin_bswap16(x)
+# define __bpf_htons(x)__builtin_bswap16(x)
+# define __bpf_constant_ntohs(x)___constant_swab16(x)
+# define __bpf_constant_htons(x)___constant_swab16(x)
+# define __bpf_ntohl(x)__builtin_bswap32(x)
+# define __bpf_htonl(x)__builtin_bswap32(x)
+# define __bpf_constant_ntohl(x)___constant_swab32(x)
+# define __bpf_constant_htonl(x)___constant_swab32(x)
+#elif __BYTE_ORDER__ == __ORDER_BIG_ENDIAN__
+# define __bpf_ntohs(x)(x)
+# define __bpf_htons(x)(x)
+# define __bpf_constant_ntohs(x)(x)
+# define __bpf_constant_htons(x)(x)
+# define __bpf_ntohl(x)(x)
+# define __bpf_htonl(x)(x)
+# define __bpf_constant_ntohl(x)(x)
+# define __bpf_constant_htonl(x)(x)
+#else
+# error "Fix your compiler's __BYTE_ORDER__?!"
+#endif
+#define bpf_htons(x)\
+  (__builtin_constant_p(x) ?\
+   __bpf_constant_htons(x) : __bpf_htons(x))
+#define bpf_ntohs(x)\
+  (__builtin_constant_p(x) ?\
+   __bpf_constant_ntohs(x) : __bpf_ntohs(x))
+#define bpf_htonl(x)\
+  (__builtin_constant_p(x) ?\
+   __bpf_constant_htonl(x) : __bpf_htonl(x))
+#define bpf_ntohl(x)\
+  (__builtin_constant_p(x) ?\
+   __bpf_constant_ntohl(x) : __bpf_ntohl(x))
+#endif /* __BPF_ENDIAN__ */
--- a/packet-exchange/src/client.c
+++ b/packet-exchange/src/client.c
@@ -166,6 +166,8 @@ static void *packet_sending_thread(void *p) {
    }
  }
+  printf("Will start at %" PRIu64 "\n", ts_to_uint(next));
  // Packet sending loop
  for (nb_cycles = 0;; nb_cycles++) {
    if (thread_params.max_cycles &&

--- a/packet-exchange/src/common.c
+++ b/packet-exchange/src/common.c
@@ -82,6 +82,14 @@ void add_ns(struct timespec *t, uint64_t ns) {
    t->tv_nsec -= NSEC_PER_SEC;
  }
 }
+void substract_ns(struct timespec *t, uint64_t ns) {
+  t->tv_nsec -= ns;
+  while (t->tv_nsec < 0) {
+    t->tv_sec -= 1;
+    t->tv_nsec += NSEC_PER_SEC;
+  }
+}
 uint64_t calcdiff_ns(struct timespec t1, struct timespec t2) {
  uint64_t diff;
@@ -89,6 +97,12 @@ uint64_t calcdiff_ns(struct timespec t1, struct timespec t2) {
  diff += ((int)t1.tv_nsec - (int)t2.tv_nsec);
  return diff;
 }
+int64_t calcdiff_ns_signed(struct timespec t1, struct timespec t2) {
+  int64_t diff;
+  diff = NSEC_PER_SEC * ((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; }

--- a/packet-exchange/src/common.h
+++ b/packet-exchange/src/common.h
@@ -41,7 +41,9 @@
 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);
+void substract_ns(struct timespec *t, uint64_t ns);
 uint64_t calcdiff_ns(struct timespec t1, struct timespec t2);
+int64_t calcdiff_ns_signed(struct timespec t1, struct timespec t2);
 void set_latency_target(void);
 void init_signals(void (*_sighand)(int));

--- a/packet-exchange/src/parsing_helpers.h
+++ b/packet-exchange/src/parsing_helpers.h
+/* SPDX-License-Identifier: GPL-2.0 */
+/*
+ * This file contains parsing functions that are used in the packetXX XDP
+ * programs. The functions are marked as __always_inline, and fully defined in
+ * this header file to be included in the BPF program.
+ *
+ * Each helper parses a packet header, including doing bounds checking, and
+ * returns the type of its contents if successful, and -1 otherwise.
+ *
+ * For Ethernet and IP headers, the content type is the type of the payload
+ * (h_proto for Ethernet, nexthdr for IPv6), for ICMP it is the ICMP type field.
+ * All return values are in host byte order.
+ *
+ * The versions of the functions included here are slightly expanded versions of
+ * the functions in the packet01 lesson. For instance, the Ethernet header
+ * parsing has support for parsing VLAN tags.
+ */
+#ifndef __PARSING_HELPERS_H
+#define __PARSING_HELPERS_H
+#include <stddef.h>
+#include <linux/if_ether.h>
+#include <linux/if_packet.h>
+#include <linux/ip.h>
+#include <linux/ipv6.h>
+#include <linux/icmp.h>
+#include <linux/icmpv6.h>
+#include <linux/udp.h>
+#include <linux/tcp.h>
+/* Header cursor to keep track of current parsing position */
+struct hdr_cursor {
+	void *pos;
+};
+/*
+ *	struct vlan_hdr - vlan header
+ *	@h_vlan_TCI: priority and VLAN ID
+ *	@h_vlan_encapsulated_proto: packet type ID or len
+ */
+struct vlan_hdr {
+	__be16	h_vlan_TCI;
+	__be16	h_vlan_encapsulated_proto;
+};
+/*
+ * Struct icmphdr_common represents the common part of the icmphdr and icmp6hdr
+ * structures.
+ */
+struct icmphdr_common {
+	__u8		type;
+	__u8		code;
+	__sum16	cksum;
+};
+/* Allow users of header file to redefine VLAN max depth */
+#ifndef VLAN_MAX_DEPTH
+#define VLAN_MAX_DEPTH 4
+#endif
+static __always_inline int proto_is_vlan(__u16 h_proto)
+{
+	return !!(h_proto == bpf_htons(ETH_P_8021Q) ||
+		  h_proto == bpf_htons(ETH_P_8021AD));
+}
+/* Notice, parse_ethhdr() will skip VLAN tags, by advancing nh->pos and returns
+ * next header EtherType, BUT the ethhdr pointer supplied still points to the
+ * Ethernet header. Thus, caller can look at eth->h_proto to see if this was a
+ * VLAN tagged packet.
+ */
+static __always_inline int parse_ethhdr(struct hdr_cursor *nh, void *data_end,
+					struct ethhdr **ethhdr)
+{
+	struct ethhdr *eth = nh->pos;
+	int hdrsize = sizeof(*eth);
+	struct vlan_hdr *vlh;
+	__u16 h_proto;
+	int i;
+	/* Byte-count bounds check; check if current pointer + size of header
+	 * is after data_end.
+	 */
+	if (nh->pos + hdrsize > data_end)
+		return -1;
+	nh->pos += hdrsize;
+	*ethhdr = eth;
+	vlh = nh->pos;
+	h_proto = eth->h_proto;
+	/* Use loop unrolling to avoid the verifier restriction on loops;
+	 * support up to VLAN_MAX_DEPTH layers of VLAN encapsulation.
+	 */
+	#pragma unroll
+	for (i = 0; i < VLAN_MAX_DEPTH; i++) {
+		if (!proto_is_vlan(h_proto))
+			break;
+		if (vlh + 1 > data_end)
+			break;
+		h_proto = vlh->h_vlan_encapsulated_proto;
+		vlh++;
+	}
+	nh->pos = vlh;
+	return h_proto; /* network-byte-order */
+}
+static __always_inline int parse_ip6hdr(struct hdr_cursor *nh,
+					void *data_end,
+					struct ipv6hdr **ip6hdr)
+{
+	struct ipv6hdr *ip6h = nh->pos;
+	/* Pointer-arithmetic bounds check; pointer +1 points to after end of
+	 * thing being pointed to. We will be using this style in the remainder
+	 * of the tutorial.
+	 */
+	if (ip6h + 1 > data_end)
+		return -1;
+	nh->pos = ip6h + 1;
+	*ip6hdr = ip6h;
+	return ip6h->nexthdr;
+}
+static __always_inline int parse_iphdr(struct hdr_cursor *nh,
+				       void *data_end,
+				       struct iphdr **iphdr)
+{
+	struct iphdr *iph = nh->pos;
+	int hdrsize;
+	if (iph + 1 > data_end)
+		return -1;
+	hdrsize = iph->ihl * 4;
+	/* Variable-length IPv4 header, need to use byte-based arithmetic */
+	if (nh->pos + hdrsize > data_end)
+		return -1;
+	nh->pos += hdrsize;
+	*iphdr = iph;
+	return iph->protocol;
+}
+static __always_inline int parse_icmp6hdr(struct hdr_cursor *nh,
+					  void *data_end,
+					  struct icmp6hdr **icmp6hdr)
+{
+	struct icmp6hdr *icmp6h = nh->pos;
+	if (icmp6h + 1 > data_end)
+		return -1;
+	nh->pos   = icmp6h + 1;
+	*icmp6hdr = icmp6h;
+	return icmp6h->icmp6_type;
+}
+static __always_inline int parse_icmphdr(struct hdr_cursor *nh,
+					 void *data_end,
+					 struct icmphdr **icmphdr)
+{
+	struct icmphdr *icmph = nh->pos;
+	if (icmph + 1 > data_end)
+		return -1;
+	nh->pos  = icmph + 1;
+	*icmphdr = icmph;
+	return icmph->type;
+}
+static __always_inline int parse_icmphdr_common(struct hdr_cursor *nh,
+						void *data_end,
+						struct icmphdr_common **icmphdr)
+{
+	struct icmphdr_common *h = nh->pos;
+	if (h + 1 > data_end)
+		return -1;
+	nh->pos  = h + 1;
+	*icmphdr = h;
+	return h->type;
+}
+/*
+ * parse_tcphdr: parse the udp header and return the length of the udp payload
+ */
+static __always_inline int parse_udphdr(struct hdr_cursor *nh,
+					void *data_end,
+					struct udphdr **udphdr)
+{
+	int len;
+	struct udphdr *h = nh->pos;
+	if (h + 1 > data_end)
+		return -1;
+	nh->pos  = h + 1;
+	*udphdr = h;
+	len = bpf_ntohs(h->len) - sizeof(struct udphdr);
+	if (len < 0)
+		return -1;
+	return len;
+}
+/*
+ * parse_tcphdr: parse and return the length of the tcp header
+ */
+static __always_inline int parse_tcphdr(struct hdr_cursor *nh,
+					void *data_end,
+					struct tcphdr **tcphdr)
+{
+	int len;
+	struct tcphdr *h = nh->pos;
+	if (h + 1 > data_end)
+		return -1;
+	len = h->doff * 4;
+	if ((void *) h + len > data_end)
+		return -1;
+	nh->pos  = h + 1;
+	*tcphdr = h;
+	return len;
+}
+#endif /* __PARSING_HELPERS_H */
--- a/packet-exchange/src/recv_packet.c
+++ b/packet-exchange/src/recv_packet.c
@@ -256,11 +256,11 @@ static void open_xdp_socket(char *network_if) {
 /*
 * Init XDP socket
 */
-void init_xdp_recv(ingress_param_t * _params) {
+void init_xdp_recv(ingress_param_t * _params, ingress_stat_t *_stats) {
 	int ret, prog_fd, xsks_map = 0;
 	struct bpf_prog_load_attr prog_load_attr = {
 	    .prog_type = BPF_PROG_TYPE_XDP,
-	    .file = "/home/oli/rt-measures/packet-exchange/build/xdp_kern.o",
+	    .file = "/home/oli/tsn-measures/packet-exchange/build/xdp_kern.o",
 	};
 	struct xsk_umem_config cfg = {
 	    .fill_size = XSK_RING_PROD__DEFAULT_NUM_DESCS,
@@ -274,6 +274,7 @@ void init_xdp_recv(ingress_param_t * _params) {
 	void *buffer = NULL;
 	params = _params;
+	stats = _stats;
 	ret = bpf_prog_load_xattr(&prog_load_attr, &obj, &prog_fd);
 	if (ret || prog_fd < 0) err("bpf_prog_load_xattr() failed");
@@ -312,11 +313,55 @@ void setup_poll_fd(void) {
 static int received;
 static uint32_t idx_rx = 0, idx;
+static void poll_wakeup(struct timespec ts, int margin) {
+  int ret;
+  struct timespec ts_prev, current;
+  ts_prev = ts;
+  substract_ns(&ts_prev, margin * 1000);
+  ret = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &ts_prev, NULL);
+  if (ret) {
+    fprintf(stderr, "clock_nanosleep returned error: %d, aborting...\n", ret);
+    exit(EXIT_FAILURE);
+  }
+  do {
+    clock_gettime(CLOCK_REALTIME, &current);
+  } while(calcdiff_ns_signed(ts, current) > 1000);
+}
+static void parse_raw_packet(uint64_t addr, size_t len)
+{
+    char *packet;
+    struct ethhdr *eth;
+    struct iphdr *ip;
+    struct udphdr *udp;
+    size_t min_len = sizeof(*eth) + sizeof(*ip) + sizeof(*udp);
+    if (len <= min_len) {
+      stats->xdp_data = NULL;
+        return;
+    }
+    packet = xsk_umem__get_data(xdp_socket.umem.buffer, addr);
+    eth = (struct ethhdr *)packet;
+    ip  = (struct iphdr *)(packet + sizeof(*eth));
+    udp = (struct udphdr *)(packet + sizeof(*eth) + sizeof(*ip));
+    stats->xdp_data = packet + sizeof(*eth) + sizeof(*ip) + sizeof(*udp);
+}
 /*
 * Receive XDP socket
 */
-int recv_xdp_packet(void) {
+int recv_xdp_packet(struct timespec next) {
 	int ret;
+        uint64_t addr;
+        uint32_t len;
+        struct timespec next_pre, current;
+        int k = 0;
 	if (params->xdp_polling_mode == 0) {
 		ret = poll(fds, 1, -1);
@@ -327,17 +372,31 @@ int recv_xdp_packet(void) {
 			error(EXIT_FAILURE, errno, "poll failed");
 		received = xsk_ring_cons__peek(&xdp_socket.rx, 1, &idx_rx);
 	} else {
-		do {
-			received =
+          next_pre = next;
-			    xsk_ring_cons__peek(&xdp_socket.rx, 1, &idx_rx);
+          substract_ns(&next_pre, 120 * 1000);
-		} while (!received);
+          ret = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &next_pre, NULL);
+          if (ret) {
+            fprintf(stderr, "clock_nanosleep returned error: %d, aborting...\n", ret);
+            exit(EXIT_FAILURE);
          }
-	if (!received) return 1;
+          do {
+            received = xsk_ring_cons__peek(&xdp_socket.rx, 1, &idx_rx);
+          } while(!received);
+	}
 	if (received != 1)
-		error(EXIT_FAILURE, errno,
+		error(EXIT_FAILURE, errno, "Received %d packets", received);
-		      "Received more packets than expected");
+         /* Get the packet */
+        addr = xsk_ring_cons__rx_desc(&xdp_socket.rx, idx_rx)->addr;
+        len  = xsk_ring_cons__rx_desc(&xdp_socket.rx, idx_rx)->len;
+        /* Parse it */
+        parse_raw_packet(xsk_umem__add_offset_to_addr(addr), len);
 	return 0;
 }
 void recv_xdp_cleanup(void) {
@@ -368,10 +427,10 @@ void close_xdp_socket(void) {
 }
 #else
-void init_xdp_recv(ingress_param_t * _params) { (void) _params; }
+void init_xdp_recv(ingress_param_t * _params, ingress_stat_t *_stats) { (void) _params; (void) _stats; }
 void setup_poll_fd(void) {}
 void close_xdp_socket(void) {}
-int recv_xdp_packet(void) { return 0; }
+int recv_xdp_packet(struct timespec next) { (void) next; return 0; }
 void recv_xdp_cleanup(void) {}
 #endif

--- a/packet-exchange/src/recv_packet.h
+++ b/packet-exchange/src/recv_packet.h
@@ -29,6 +29,7 @@ typedef struct ingress_stat {
 	uint64_t high_jitter;
 	char data[MAX_BUFFER_SIZE];
+	char * xdp_data;
 } ingress_stat_t;
 void init_udp_recv(ingress_param_t *_params, ingress_stat_t *stats,
@@ -55,8 +56,8 @@ struct xdpsock {
 };
 #endif
-void init_xdp_recv(ingress_param_t * _params);
+void init_xdp_recv(ingress_param_t * _params, ingress_stat_t *_stats);
-int recv_xdp_packet(void);
+int recv_xdp_packet(struct timespec next);
 void recv_xdp_cleanup(void);
 void setup_poll_fd(void);
 void close_xdp_socket(void);

--- a/packet-exchange/src/server.c
+++ b/packet-exchange/src/server.c
@@ -30,6 +30,8 @@
 #include "send_packet.h"
 #include "tracer.h"
+#define ERROR_MARGIN_NS 400000
 // Structs
 enum TSNTask { RECV_PACKET_TASK, RTT_TASK, XDP_TASK };
@@ -44,6 +46,11 @@ typedef struct thread_param {
  int enable_diff_ts;
  int enable_receive_tracemark;
+  uint64_t start_ts;
+  int poll;
+  int poll_margin;
 } thread_param_t;
 typedef struct main_params {
@@ -95,6 +102,8 @@ static void help(char *argv[]) {
      "       -r USEC            non-RT main thread refresh interval\n"
      "       -d BUF_LEN         Set the length of tx buffer\n"
      "       -c                 Receive timestamp and emit signal\n"
+      "       -s NS              Common start time reference\n"
+      "       -P USEC            Do polling to wakeup signal thread with specified margin\n"
      "       -C                 Receive timestamp and print difference with current time\n"
      "       -b CLIENT_IP       Server side RTT\n"
      "       -g                 Print histograms to sdtout on exit\n"
@@ -110,10 +119,30 @@ static void help(char *argv[]) {
      argv[0]);
 }
+static void poll_wakeup(struct timespec ts, int margin) {
+  int ret;
+  struct timespec ts_prev, current;
+  ts_prev = ts;
+  substract_ns(&ts_prev, margin * 1000);
+  ret = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &ts_prev, NULL);
+  if (ret) {
+    fprintf(stderr, "clock_nanosleep returned error: %d, aborting...\n", ret);
+    exit(EXIT_FAILURE);
+  }
+  do {
+    clock_gettime(CLOCK_REALTIME, &current);
+  } while(calcdiff_ns_signed(ts, current) > 1000);
+}
 static void *emit_signal_thread(void *p) {
  (void)p;
  cpu_set_t mask;
  struct timespec current;
+  struct timespec previous_emit, previous_ts;
+  int64_t emit_diff, ts_diff;
  // Set thread CPU affinity
  if (thread_params.affinity_cpu) {
@@ -130,9 +159,28 @@ static void *emit_signal_thread(void *p) {
    pthread_cond_wait(&emit_signal_ts_received, &emit_signal_mutex);
    clock_gettime(CLOCK_REALTIME, &current);
-    clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &emit_signal_next, NULL);
+    poll_wakeup(emit_signal_next, thread_params.poll_margin);
    toggle_gpio();
+    // Check if something went wrong
+    if(i > 0) {
+      clock_gettime(CLOCK_REALTIME, &current);
+      emit_diff = calcdiff_ns_signed(current, previous_emit);
+      ts_diff   = calcdiff_ns_signed(emit_signal_next, previous_ts);
+      if((emit_diff < ((int64_t)thread_params.interval) - ERROR_MARGIN_NS) ||
+         (emit_diff > ((int64_t)thread_params.interval) + ERROR_MARGIN_NS)) {
+        fprintf(stderr, "Signal emission interval reached error threshold: %" PRIi64 "\n", emit_diff);
+        exit(EXIT_FAILURE);
+      }
+      if((ts_diff < ((int64_t)thread_params.interval) - ERROR_MARGIN_NS) ||
+         (ts_diff > ((int64_t)thread_params.interval) + ERROR_MARGIN_NS)) {
+        fprintf(stderr, "Timestamp interval reached error threshold: %" PRIi64 "\n", ts_diff);
+        exit(EXIT_FAILURE);
+      }
+    }
+    previous_emit = current;
+    previous_ts = emit_signal_next;
  }
  pthread_mutex_unlock(&emit_signal_mutex);
@@ -144,7 +192,7 @@ static void *emit_signal_thread(void *p) {
 */
 static void *tsn_thread(void *p) {
  (void)p;
-  struct timespec current, previous;
+  struct timespec current, previous, next;
  cpu_set_t mask;
  char tracemark_message[128];
@@ -161,6 +209,23 @@ static void *tsn_thread(void *p) {
  if (tsn_task == XDP_TASK) setup_poll_fd();
+  if (thread_params.start_ts) {
+    clock_gettime(CLOCK_REALTIME, &next);
+    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);
+    add_ns(&next, thread_params.interval);
+  }
+  if(thread_params.start_ts)
+    printf("Will start at %" PRIu64 "\n", ts_to_uint(next));
  // Packet receiving loop
  for (ingress_stats.packets_received = 0;; ingress_stats.packets_received++) {
    // RTT
@@ -170,32 +235,33 @@ static void *tsn_thread(void *p) {
      // Receive packet
    } else if (tsn_task == RECV_PACKET_TASK || tsn_task == XDP_TASK) {
      // Receive UDP or XDP packet
      if (tsn_task == RECV_PACKET_TASK)
        recv_udp_packet();
      else
-        recv_xdp_packet();
+        recv_xdp_packet(next);
+      // Get time for statistics
      clock_gettime(CLOCK_REALTIME, &current);
      recv_xdp_cleanup();
+      if (thread_params.start_ts)
+        add_ns(&next, thread_params.interval);
      if(thread_params.enable_receive_tracemark) {
        sprintf(ts_tracemark_buf, "%" PRIu64, ts_to_uint(current));
        tracemark(ts_tracemark_buf);
      }
-      if(thread_params.enable_diff_ts) {
+      // Get signal timestamp
-        uint64_t send_ts = decode(ingress_stats.data);
-        int64_t diff_us = (((int64_t) ts_to_uint(current)) - ((int64_t)send_ts)) / 1000;
-        min_diff_ts = _min_(diff_us, min_diff_ts);
-        max_diff_ts = _max_(diff_us, max_diff_ts);
-        avg_diff_ts = (avg_diff_ts * ingress_stats.packets_received + diff_us) / (ingress_stats.packets_received + 1);
-      }
-      // Emit signal
      if (thread_params.emit_signal) {
-        uint64_t emit_signal_t = decode(ingress_stats.data);
+        uint64_t emit_signal_t;
+        if(tsn_task == XDP_TASK)
+          emit_signal_t = decode(ingress_stats.xdp_data);
+        else
+          emit_signal_t = decode(ingress_stats.data);
        pthread_mutex_lock(&emit_signal_mutex);
        emit_signal_next = uint_to_ts(emit_signal_t);
@@ -203,6 +269,32 @@ static void *tsn_thread(void *p) {
        pthread_mutex_unlock(&emit_signal_mutex);
      }
+      // Update packet latency estimation statistics
+      if(thread_params.enable_diff_ts) {
+        uint64_t send_ts;
+        if(tsn_task == XDP_TASK)
+          send_ts = decode(ingress_stats.xdp_data);
+        else
+          send_ts = decode(ingress_stats.data);
+        int64_t diff_us = (((int64_t) ts_to_uint(current)) - ((int64_t)send_ts)) / 1000;
+        min_diff_ts = _min_(diff_us, min_diff_ts);
+        max_diff_ts = _max_(diff_us, max_diff_ts);
+        avg_diff_ts = (avg_diff_ts * ingress_stats.packets_received + diff_us) / (ingress_stats.packets_received + 1);
+        // If the latency hits the tracing threshold, stop tracing
+        if (main_params.enable_tracing &&
+            (max_diff_ts > ((int64_t)thread_params.latency_threshold))) {
+          sprintf(tracemark_message, "Threshold hit: %" PRIi64 "\n",
+                  max_diff_ts);
+          tracemark(tracemark_message);
+          tracing(0);
+          printf(tracemark_message);
+          exit(EXIT_SUCCESS);
+        }
+      }
+      else {
        // Update stats
        if (ingress_stats.packets_received) {
          int interval_us = calcdiff_ns(current, previous) / 1000;
@@ -226,7 +318,6 @@ static void *tsn_thread(void *p) {
            else
              jitter_hist[dist_to_interval]++;
          }
-      }
          // If the latency hits the tracing threshold, stop tracing
          int jitter = ingress_stats.max_interval - ingress_stats.min_interval;
@@ -239,6 +330,8 @@ static void *tsn_thread(void *p) {
            printf(tracemark_message);
            exit(EXIT_SUCCESS);
          }
+        }
+      }
      previous = current;
    }
@@ -304,6 +397,9 @@ int main(int argc, char *argv[]) {
  thread_params.affinity_cpu = 0;
  thread_params.enable_diff_ts = 0;
  thread_params.enable_receive_tracemark = 0;
+  thread_params.poll = 0;
+  thread_params.poll_margin = 75;
+  thread_params.start_ts = 0;
  main_params.refresh_rate = 50000;
  main_params.verbose = 0;
  main_params.enable_tracing = 0;
@@ -335,7 +431,7 @@ int main(int argc, char *argv[]) {
  // Initialize the XDP or UDP packet receiving socket
  if (tsn_task == XDP_TASK)
-    init_xdp_recv(&ingress_params);
+    init_xdp_recv(&ingress_params, &ingress_stats);
  else
    init_udp_recv(&ingress_params, &ingress_stats, enable_histograms,
                  kernel_latency_hist);
@@ -461,7 +557,7 @@ static void process_options(int argc, char *argv[]) {
  int network_if_specified = 0;
  for (;;) {
-    int c = getopt(argc, argv, "a:b:cCd:f:ghi:p:r:tvx:XT:GMS");
+    int c = getopt(argc, argv, "a:b:cCs:d:f:ghi:p:r:tvx:XT:GMSP:");
    if (c == -1) break;
@@ -479,6 +575,9 @@ static void process_options(int argc, char *argv[]) {
      case 'C':
        thread_params.enable_diff_ts = 1;
        break;
+      case 's':
+        thread_params.start_ts = strtoull(optarg, NULL, 10);
+        break;
      case 'd':
        ingress_params.tx_buffer_len = atoi(optarg);
        if (ingress_params.tx_buffer_len < 1) {
@@ -529,6 +628,10 @@ static void process_options(int argc, char *argv[]) {
      case 'S':
        ingress_params.enable_ts_tracemark = 1;
        break;
+      case 'P':
+        thread_params.poll = 1;
+        thread_params.poll_margin = atoi(optarg);
+        break;
    }
  }

--- a/packet-exchange/src/xdp_kern.c
+++ b/packet-exchange/src/xdp_kern.c
@@ -9,6 +9,10 @@
 #include <linux/udp.h>
 #include "bpf_helpers.h"
+#include "bpf_endian.h"
+#include "parsing_helpers.h"
+#define UDP_PORT 50000
 #define bpf_printk(fmt, ...)                                               \
 	({                                                                 \
@@ -23,38 +27,45 @@ struct bpf_map_def SEC("maps") xsks_map = {
 	.max_entries = 64,
 };
-static inline int parse_ipv4(void *data, unsigned long long nh_off,
-			     void *data_end) {
-	struct iphdr *iph = data + nh_off;
-	if ((void *)(iph + 1) > data_end) return 0;
-	return iph->protocol;
-}
 SEC("xdp_sock")
-int xdp_sock_prog(struct xdp_md *ctx) {
+int xdp_sock_prog(struct xdp_md *ctx)
+{
+	int eth_type, ip_type, index;
+	struct ethhdr *eth;
+	struct iphdr *iphdr;
+	struct ipv6hdr *ipv6hdr;
 	void *data_end = (void *)(long)ctx->data_end;
 	void *data = (void *)(long)ctx->data;
-	struct ethhdr *eth = data;
+	struct hdr_cursor nh = { .pos = data };
-	int idx = ctx->rx_queue_index;
-	unsigned int ipproto = 0;
+        index = ctx->rx_queue_index;
-	unsigned long long nh_off;
-	/* Check if it's a UDP frame: If UDP -> Redirect to active xsk for user
+	eth_type = parse_ethhdr(&nh, data_end, &eth);
-	 * space. If not -> pass to stack.
+	if (eth_type < 0)
-	 */
+		return XDP_PASS;
-	nh_off = sizeof(*eth);
-	if (data + nh_off > data_end) return XDP_PASS;
+	if (eth_type == bpf_htons(ETH_P_IP)) {
+		ip_type = parse_iphdr(&nh, data_end, &iphdr);
+	} else if (eth_type == bpf_htons(ETH_P_IPV6)) {
+		ip_type = parse_ip6hdr(&nh, data_end, &ipv6hdr);
+	} else {
+		return XDP_PASS;
+	}
-	if (eth->h_proto == __builtin_bswap16(ETH_P_IP))
+	// only support UDP for now
-		ipproto = parse_ipv4(data, nh_off, data_end);
+	if (ip_type != IPPROTO_UDP)
+		return XDP_PASS;
-	if (ipproto != IPPROTO_UDP) return XDP_PASS;
+	struct udphdr *udphdr;
+	// don't mess with ports outside our purview, if specified
+	if (parse_udphdr(&nh, data_end, &udphdr) < 0)
+		return XDP_PASS;
+	if (bpf_ntohs(udphdr->dest) != UDP_PORT)
+		return XDP_PASS;
 	/* If socket bound to rx_queue then redirect to user space */
-	if (bpf_map_lookup_elem(&xsks_map, &idx))
+	if (bpf_map_lookup_elem(&xsks_map, &index))
-		return bpf_redirect_map(&xsks_map, idx, 0);
+		return bpf_redirect_map(&xsks_map, index, 0);
 	/* Else pass to Linux' network stack */
 	return XDP_PASS;

--- a/scripts/exec-ssh-nohup
+++ b/scripts/exec-ssh-nohup
@@ -7,7 +7,7 @@ send -- "sudo echo\r"
 expect "assword"
 send -- "olimex\r"
 expect "oli@"
-send -- "sudo nohup [lindex $argv 1] > /dev/null > /dev/null &\r"
+send -- "sudo nohup [lindex $argv 1] > /dev/null > [lindex $argv 2] &\r"
 expect "stdout\r"
 send -- \x03
 expect "oli@"
--- a/scripts/packet-histogram-start
+++ b/scripts/packet-histogram-start
@@ -97,8 +97,8 @@ stop_opt_filename=${stop_opt_name}${stop_opt_index}
 # Client histogram
 if [ -n "$client_histogram" ]; then
-  $script_dir/exec-ssh-nohup $server_board "run-server";
+  $script_dir/exec-ssh-nohup $server_board "run-server" server_log;
-  $script_dir/exec-ssh-nohup $client_board "run-client -p -i $interval -t -g $server_board";
+  $script_dir/exec-ssh-nohup $client_board "run-client -p -i $interval -t -g $server_board" client_log;
  echo "$stop_opts -c client_i${interval}_pfast -i $interval $client_board $server_board" > $script_dir/${stop_opt_filename};
@@ -107,16 +107,16 @@ elif [ -n "$server_histogram" ]; then
  # Server pfifo_fast qdisc histogram
  if [ -n "$server_pfifo" ]; then
-    $script_dir/exec-ssh-nohup $server_board "run-server -g ${interval}$server_opts";
+    $script_dir/exec-ssh-nohup $server_board "run-server -g ${interval}$server_opts" server_log;
-    $script_dir/exec-ssh-nohup $client_board "run-client -p -i $interval $server_board";
+    $script_dir/exec-ssh-nohup $client_board "run-client -p -i $interval $server_board" client_log;
    echo "$stop_opts -s server_i${interval} $server_opts -p $client_board $server_board" > $script_dir/${stop_opt_filename};
  # Server ETF qdisc histogram
  elif [ -n "$server_etf" ]; then
-    $script_dir/exec-ssh-nohup $server_board "run-server -g ${interval}$server_opts";
+    $script_dir/exec-ssh-nohup $server_board "run-server -g ${interval}$server_opts" server_log;
-    $script_dir/exec-ssh-nohup $client_board "run-client -e $etf_delta -o $etf_offset -i $interval $server_board";
+    $script_dir/exec-ssh-nohup $client_board "run-client -e $etf_delta -o $etf_offset -i $interval $server_board" client_log;
    echo "$stop_opts -s server_i${interval} $server_opts -e $etf_delta -o $etf_offset $client_board $server_board" > $script_dir/${stop_opt_filename};
@@ -127,7 +127,7 @@ elif [ -n "$server_histogram" ]; then
 # cyclictest histogram
 elif [ -n "$cyclictest_histogram" ]; then
-  $script_dir/exec-ssh-nohup $board "run-cyclictest -g $cyclictest_opts -i $interval";
+  $script_dir/exec-ssh-nohup $board "run-cyclictest -g $cyclictest_opts -i $interval" cyclictest_log;
  echo "$stop_opts -C cyclictest_hist $cyclictest_opts $board" > $script_dir/${stop_opt_filename};

--- a/scripts/run-client
+++ b/scripts/run-client
@@ -6,14 +6,13 @@ usage() {
  cat << ENDUSAGE
 Usage: $0 [-h] QDISC_OPT [CLIENT_OPTS] BOARD_HOSTNAME
   -h              Show help
-   QDISC_OPTS: (-e DELTA [-o USEC] -H | -p) [-q]
+   QDISC_OPTS: [-e DELTA [-o USEC] -H] [-q]
         Which qdisc to use (will call create-qdisc script)
         -e DELTA       Use ETF (Earlier Txtime First) qdisc with specified delta
                        (check tc-etf man page for more information)
         -o USEC        Offset in userspace program to set the timestamp passed to ETF qdisc
         -H             Use hardware offloading for the ETF qdisc (not all hardware supports this)
-         -p             Use pfifo_fast qdisc (default one)
+         -q             Setup qdiscs with create-qdisc script
-         -q             Don't setup qdiscs with create-qdisc script
   CLIENT_OPTS:    -bgt (-c DELAY -s NS | -d TX_BUF_LEN) -i INTERVAL -I if -a CPU [TRACE_OPTS]
         Options passed to the C client program (everything here is optional)
         -b             Measure round trip time 
@@ -46,19 +45,22 @@ ENDUSAGE
 }
 # Default interval
-interval=100000
+interval=1000
 # Default options
 interface="eth0"
 cpu="1"
-client_options="-p 98"
+client_options="-p 95"
 qdisc_options=""
 etf_offset=500
 tracecmd_events="-e irq -e sched -e net_dev_start_xmit -e net_dev_xmit -e net_dev_xmit_timeout"
 tracecmd_opts=""
-while getopts "a:bc:d:e:o:ghi:pqs:tB:E:I:HP:TS:" opt; do
+while getopts "a:bc:d:e:o:ghi:qs:tB:E:I:HP:TS:" opt; do
  case "${opt}" in
+    h )
+        usage
+        ;;
    a )
        cpu=${OPTARG}
        ;;
@@ -74,7 +76,6 @@ while getopts "a:bc:d:e:o:ghi:pqs:tB:E:I:HP:TS:" opt; do
    e )
        use_etf=1
        delta=$((${OPTARG}*1000))
-        qdisc_options+="-e $delta"
        ;;
    o )
        etf_offset=${OPTARG}
@@ -83,20 +84,14 @@ while getopts "a:bc:d:e:o:ghi:pqs:tB:E:I:HP:TS:" opt; do
        client_options+=" -g"
        use_histogram=1
        ;;
-    h )
-        usage
-        exit 1
-        ;;
    i )
        interval=${OPTARG}
        ;;
    p )
        use_pfast=1
-        client_options+=" -q 1"
-        qdisc_options+="-p"
      ;;
    q )
-        dont_create_qdisc=1
+        create_qdisc=1
      ;;
    s )
        client_options+=" -s ${OPTARG}"
@@ -133,27 +128,16 @@ while getopts "a:bc:d:e:o:ghi:pqs:tB:E:I:HP:TS:" opt; do
      ;;
  esac
 done
 shift $((OPTIND-1))
 if [ -z "$1" ]; then
  usage
 fi
 qdisc_options+=" -I $interface"
 client_options+=" -a $cpu"
 board_name=$1
 board_ip=$(cat /etc/hosts | grep $board_name | awk '{print $1}')
-if [ -z "${use_etf}" ] && [ -z "${use_pfast}" ]; then
-  usage
-fi
-if [ -n "${use_etf}" ] && [ -n "${use_pfast}" ]; then
-  usage
-fi
 if [ -z "${use_histogram}" ]; then
  client_options+=" -v"
 else
@@ -170,11 +154,15 @@ fi
 if [ -n "${use_etf}" ]; then
  client_options+=" -e $etf_offset -q 8"
+  qdisc_options+="-e $delta"
+else
+  client_options+=" -q 1"
+  qdisc_options+="-p"
 fi
 client_options+=" -i $interval"
-if [ -z "$dont_create_qdisc" ]; then
+if [ -n "$create_qdisc" ]; then
  echo "create-qdisc $qdisc_options";
  $script_dir/create-qdisc $qdisc_options;
 fi
@@ -185,13 +173,19 @@ make client;
 cd $script_dir;
 if [ -n "${use_histogram}" ]; then
  echo "client $client_options $interface $board_ip > $output;mv $output ~/";
  $script_dir/../packet-exchange/build/client $client_options $interface $board_ip > $output;
  mv $output ~/;
 elif [ -n "${use_tracer}" ]; then
  echo "trace-cmd record $tracecmd_opts $tracecmd_events ./client $client_options $interface $board_ip";
  trace-cmd record $tracecmd_opts $tracecmd_events $script_dir/../packet-exchange/build/client $client_options $interface $board_ip;
 else
  echo "client $client_options $interface $board_ip";
  $script_dir/../packet-exchange/build/client $client_options $interface $board_ip;
 fi
--- a/scripts/run-server
+++ b/scripts/run-server
@@ -13,14 +13,18 @@ Usage: $0 [-h] [-I if] [SERVER] | TCPDUMP [TRACE_OPTS]
   SERVER: -bct ((-x | -X) POLL) -g INTERVAL -a CPU
         Options passed to the C server program (everything here is optional)
         -b             Send back packets for round trip measurements
-         -c             Emit a signal on GPIO at the timestamp given in packet
+         -c USEC        Emit a signal on GPIO at the timestamp given in packet, specify interval
                        (to be used with -c option in client program)
-         -C             Measure difference between current time and timestamp sent in tx data
+         -O USEC        Do polling to wakeup signal thread with specified margin
+         -C USEC        Measure difference between current time and timestamp sent in tx data
         -t             Use SO_TIMESTAMPS to see how much time packet spent in kernel
         -x POLL        Use XDP sockets, with a global libbpf installation
         -X POLL        Use XDP sockets, with libbpf located in \$HOME/libbpf folder
            POLL: Polling mode used in server program, 0 to poll with poll function,
                  1 to do active polling
+         -s NS          Specify a CLOCK_REALTIME timestamp at which client should start
+                        (to be used with PTP) (interval needs to be specified with -j)
+         -j USEC        Specify interval (used with -s)
         -g USEC        Generate histograms for measures with the specified interval
         -a CPU         CPU on which to pin the program
   TCPDUMP: -d NB_PACKETS [-i INTERVAL]
@@ -48,7 +52,7 @@ ENDUSAGE
 # Default options
 interface="eth0"
-server_options="-p 98"
+server_options="-p 95"
 make_opts=""
 ip="10.100.21."
 tcpdump_interval=1000000
@@ -56,8 +60,14 @@ tracecmd_events="-e irq -e sched -e net -e napi"
 tracecmd_opts=""
 cpu=1
-while getopts "a:b:cChtx:X:d:i:g:I:T:E:P:B:MSQ" opt; do
+while getopts "j:a:b:c:C:htx:X:s:d:i:g:I:T:E:P:B:MSQO:" opt; do
  case "${opt}" in
+    h )
+        usage
+        ;;
+    j )
+        server_options+=" -i ${OPTARG}"
+        ;;
    a )
        cpu=${OPTARG}
        ;;
@@ -66,10 +76,13 @@ while getopts "a:b:cChtx:X:d:i:g:I:T:E:P:B:MSQ" opt; do
        board_name=${OPTARG}
        ;;
    c )
-        server_options+=" -c"
+        server_options+=" -c -i ${OPTARG}"
+        ;;
+    O )
+        server_options+=" -P ${OPTARG} "
      ;;
    C )
-        server_options+=" -C"
+        server_options+=" -C -i ${OPTARG}"
        ;;
    d )
        use_tcpdump=1
@@ -83,10 +96,6 @@ while getopts "a:b:cChtx:X:d:i:g:I:T:E:P:B:MSQ" opt; do
        server_options+=" -g -i $interval"
        use_histogram=1
        ;;
-    h )
-        usage
-        exit 1
-        ;;
    t )
        server_options+=" -t"
        ;;
@@ -101,6 +110,9 @@ while getopts "a:b:cChtx:X:d:i:g:I:T:E:P:B:MSQ" opt; do
        server_options+=" -x ${OPTARG}"
 	make_opts=" -e WITH_GIT_XDP=1"
        ;;
+    s )
+        server_options+=" -s ${OPTARG}"
+      ;;
    B )
        tracecmd_opts+=" -m ${OPTARG} -b ${OPTARG}"
      ;;

--- a/scripts/run-software-pps
+++ b/scripts/run-software-pps
@@ -9,6 +9,7 @@ Usage: $0 [-h] [-i USEC -g -s -t MSEC] BOARD1_HOSTNAME BOARD2_HOSTNAME
   -i USEC        Signal period
   -g             Use GPIO instead of serial
   -t MSEC        Set the start timestamp offset
+   -P USEC        Do polling to wakeup with specified margin
   BOARD_HOSTNAME Uses /etc/hosts to find the IP address associated to the hostname
 ENDUSAGE
 1>&2;
@@ -20,7 +21,7 @@ interval=1000
 pp_opts=""
 ts_offset=6000
-while getopts "hi:gt:" opt; do
+while getopts "hi:gt:P:" opt; do
  case "$opt" in
    h)
      usage
@@ -34,6 +35,9 @@ while getopts "hi:gt:" opt; do
    t )
      ts_offset=${OPTARG}
      ;;
+    P )
+      pps_opts+=" -P ${OPTARG} "
+      ;;
    *)
      usage
      ;;
@@ -54,7 +58,9 @@ ts=$($script_dir/get-ptp-time -m $ts_offset)
 echo "Timestamp: $ts";
 ssh $board1 "cd tsn-measures/software-pps/build;make";
-$script_dir/exec-ssh-nohup $board1 "tsn-measures/software-pps/build/software-pps -a1 -p97 -i $interval $pps_opts -s $ts";
+echo "$script_dir/exec-ssh-nohup $board1 \"tsn-measures/software-pps/build/software-pps -a1 -p97 -i $interval $pps_opts -s $ts\"" server_log;
+$script_dir/exec-ssh-nohup $board1 "tsn-measures/software-pps/build/software-pps -a1 -p97 -i $interval $pps_opts -s $ts" server_log;
 ssh $board2 "cd tsn-measures/software-pps/build;make";
-$script_dir/exec-ssh-nohup $board2 "tsn-measures/software-pps/build/software-pps -a1 -p97 -i $interval $pps_opts -s $ts";
+echo "$script_dir/exec-ssh-nohup $board2 \"tsn-measures/software-pps/build/software-pps -a1 -p97 -i $interval $pps_opts -s $ts\"" server_log;
+$script_dir/exec-ssh-nohup $board2 "tsn-measures/software-pps/build/software-pps -a1 -p97 -i $interval $pps_opts -s $ts" server_log;
--- a/scripts/test-board-synchro
+++ b/scripts/test-board-synchro
@@ -9,6 +9,8 @@ Usage: $0 [-h] [-i USEC -c USEC -t MSEC] [-T] BOARD1_HOSTNAME BOARD2_HOSTNAME
   -i USEC        Specify which interval to use in client
   -c USEC        Specify which offset to use for the timestamp in the packet
   -t MSEC        Set the start timestamp offset
+   -P USEC        Do polling to wakeup signal thread with specified margin
+   -X POLL_MODE   Use XDP with specified poll mode
   -T             Enable tracing on the boards
   BOARD_HOSTNAME Uses /etc/hosts to find the IP address associated to the hostname
 ENDUSAGE
@@ -21,7 +23,7 @@ interval=1000
 server_opts=""
 ts_offset=2000
-while getopts "hc:i:o:rt:T" opt; do
+while getopts "hc:i:o:rt:TP:X:" opt; do
  case "${opt}" in
    h )
        usage
@@ -38,6 +40,12 @@ while getopts "hc:i:o:rt:T" opt; do
    T )
        enable_tracing=1
        ;;
+    P )
+        server_opts+=" -P ${OPTARG} "
+      ;;
+    X )
+        server_opts+=" -X ${OPTARG} "
+      ;;
    * )
        usage
        ;;
@@ -62,8 +70,8 @@ $script_dir/sudossh $board2 "killall server";
 killall client;
 killall run-client;
-$script_dir/exec-ssh-nohup $board1 "run-server -c $server_opts";
+$script_dir/exec-ssh-nohup $board1 "run-server -c $interval $server_opts" server_log;
-$script_dir/exec-ssh-nohup $board2 "run-server -c $server_opts";
+$script_dir/exec-ssh-nohup $board2 "run-server -c $interval $server_opts" server_log;
 ts=$($script_dir/get-ptp-time -m $ts_offset)
@@ -76,8 +84,8 @@ if [ -z "$delay" ]; then
  delay=$((interval / 2))
 fi
-echo "$script_dir/run-client -a 1 -q -i $interval -p -I enp1s0 -c $delay -s $ts $board1 &> client_enp1s0_log&";
+echo "$script_dir/run-client -a 1 -i $interval -I enp1s0 -c $delay -s $ts $board1 &> client_enp1s0_log&";
-echo "$script_dir/run-client -a 2 -q -i $interval -p -I enp2s0 -c $delay -s $ts $board2 &> client_enp2s0_log&";
+echo "$script_dir/run-client -a 2 -i $interval -I enp2s0 -c $delay -s $ts $board2 &> client_enp2s0_log&";
-$script_dir/run-client -a 1 -q -i $interval -p -I enp1s0 -c $delay -s $ts $board1 &> client_enp1s0_log&
+$script_dir/run-client -a 1 -i $interval -I enp1s0 -c $delay -s $ts $board1 &> client_enp1s0_log&
-$script_dir/run-client -a 2 -q -i $interval -p -I enp2s0 -c $delay -s $ts $board2 &> client_enp2s0_log&
+$script_dir/run-client -a 2 -i $interval -I enp2s0 -c $delay -s $ts $board2 &> client_enp2s0_log&
--- a/software-pps/src/common.c
+++ b/software-pps/src/common.c
@@ -28,6 +28,15 @@ void add_ns(struct timespec *t, uint64_t ns) {
  }
 }
+void substract_ns(struct timespec *t, uint64_t ns) {
+  t->tv_nsec -= ns;
+  while (t->tv_nsec < 0) {
+    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);
@@ -35,6 +44,13 @@ uint64_t calcdiff_ns(struct timespec t1, struct timespec t2) {
  return diff;
 }
+int64_t calcdiff_ns_signed(struct timespec t1, struct timespec t2) {
+  int64_t diff;
+  diff = NSEC_PER_SEC * ((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; }
--- a/software-pps/src/common.h
+++ b/software-pps/src/common.h
@@ -41,7 +41,9 @@
 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);
+void substract_ns(struct timespec *t, uint64_t ns);
 uint64_t calcdiff_ns(struct timespec t1, struct timespec t2);
+int64_t calcdiff_ns_signed(struct timespec t1, struct timespec t2);
 void init_signals(void (*_sighand)(int));

--- a/software-pps/src/software-pps.c
+++ b/software-pps/src/software-pps.c
@@ -20,9 +20,9 @@
 #include <time.h>
 #include <unistd.h>
-#include "pulse.h"
-#include "gpio.h"
 #include "common.h"
+#include "gpio.h"
+#include "pulse.h"
 // Structs
@@ -31,6 +31,8 @@ typedef struct thread_param {
  unsigned int max_cycles;
  int priority;
  int affinity_cpu;
+  int poll;
+  int poll_margin;
  uint64_t start_ts;
 } thread_param_t;
@@ -62,11 +64,30 @@ static void help(char *argv[]) {
      "       -l N_CYCLES        RT thread cycles amount (default: infinite)\n"
      "       -s NS              Common start time reference\n"
      "       -g                 Use GPIO instead of serial\n"
+      "       -P USEC            Do polling to wakeup with specified margin\n"
      "       -v                 Verbose\n"
      "\n",
      argv[0]);
 }
+static void poll_wakeup(struct timespec ts, int margin) {
+  int ret;
+  struct timespec ts_prev, current;
+  ts_prev = ts;
+  substract_ns(&ts_prev, margin * 1000);
+  ret = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &ts_prev, NULL);
+  if (ret) {
+    fprintf(stderr, "clock_nanosleep returned error: %d, aborting...\n", ret);
+    exit(EXIT_FAILURE);
+  }
+  do {
+    clock_gettime(CLOCK_REALTIME, &current);
+  } while(calcdiff_ns_signed(ts, current) > 1000);
+}
 /*
 * Real-time thread:
 */
@@ -88,11 +109,11 @@ static void *pps_thread(void *p) {
  clock_gettime(CLOCK_REALTIME, &next);
  if (thread_params.start_ts) {
-    if(thread_params.start_ts < ts_to_uint(next)) {
+    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))) {
+    if (thread_params.start_ts > (ts_to_uint(next) + UINT64_C(3600000000000))) {
      fprintf(stderr, "start timestamp is too high, aborting...\n");
      exit(EXIT_FAILURE);
    }
@@ -105,20 +126,24 @@ static void *pps_thread(void *p) {
        nb_cycles >= ((unsigned int)thread_params.max_cycles))
      break;
+    if (use_gpio)
-    if(use_gpio)
      toggle_gpio();
    else
      send_pulse();
    add_ns(&next, thread_params.interval);
+    if(thread_params.poll) {
+      poll_wakeup(next, thread_params.poll_margin);
+    }
+    else {
      ret = clock_nanosleep(CLOCK_REALTIME, TIMER_ABSTIME, &next, NULL);
-    if(ret) {
+      if (ret) {
        fprintf(stderr, "clock_nanosleep returned error: %d, aborting...\n", ret);
        exit(EXIT_FAILURE);
      }
    }
+  }
  pthread_exit(NULL);
 }
@@ -139,6 +164,8 @@ int main(int argc, char *argv[]) {
  thread_params.priority = 98;
  thread_params.affinity_cpu = -1;
  thread_params.start_ts = 0;
+  thread_params.poll = 0;
+  thread_params.poll_margin = 75;
  main_params.refresh_rate = 50000;
  main_params.verbose = 0;
  use_gpio = 0;
@@ -153,7 +180,7 @@ int main(int argc, char *argv[]) {
  // Process bash options
  process_options(argc, argv);
-  if(use_gpio)
+  if (use_gpio)
    enable_gpio(86);
  else
    enable_pulse();
@@ -204,12 +231,11 @@ int main(int argc, char *argv[]) {
  exit(EXIT_SUCCESS);
 }
 /* Process bash options
 */
 static void process_options(int argc, char *argv[]) {
  for (;;) {
-    int c = getopt(argc, argv, "a:hgi:l:p:r:s:v");
+    int c = getopt(argc, argv, "a:hgi:l:p:r:s:vP:");
    if (c == -1) break;
@@ -242,6 +268,10 @@ static void process_options(int argc, char *argv[]) {
      case 'v':
        main_params.verbose = 1;
        break;
+      case 'P':
+        thread_params.poll = 1;
+        thread_params.poll_margin = atoi(optarg);
+        break;
    }
  }
 }
--- a/xdp-testing/Makefile
+++ b/xdp-testing/Makefile
+IFLAGS += -I ${HOME}/libbpf/include
+xdp_%.o: xdp_%.c
+	clang $(IFLAGS) -isystem /usr/include/arm-linux-gnueabihf -S -target bpf -D __BPF_TRACING__ -Wall -O2 -emit-llvm -c -g -o xdp_$*.ll $^
+	llc -march=bpf -filetype=obj -o $@ xdp_$*.ll
+clean:
+	$(RM) xdp_*.o xdp_*.ll
+.PHONY: clean
--- a/xdp-testing/bpf_helpers.h
+++ b/xdp-testing/bpf_helpers.h
--- a/xdp-testing/xdp_kern.c
+++ b/xdp-testing/xdp_kern.c
+#define KBUILD_MODNAME "blub"
+#include <linux/bpf.h>
+#include <linux/if_ether.h>
+#include <linux/if_link.h>
+#include <linux/if_xdp.h>
+#include <linux/in.h>
+#include <linux/ip.h>
+#include <linux/udp.h>
+#include "bpf_helpers.h"
+#define bpf_printk(fmt, ...)                                               \
+	({                                                                 \
+		char ____fmt[] = fmt;                                      \
+		bpf_trace_printk(____fmt, sizeof(____fmt), ##__VA_ARGS__); \
+	})
+struct bpf_map_def SEC("maps") xsks_map = {
+    .type = BPF_MAP_TYPE_XSKMAP,
+    .key_size = sizeof(int),
+    .value_size = sizeof(int),
+    .max_entries = 64,
+};
+static inline int parse_ipv4(void *data, unsigned long long nh_off,
+			     void *data_end) {
+	struct iphdr *iph = data + nh_off;
+	if ((void *)(iph + 1) > data_end) return 0;
+	return iph->protocol;
+}
+SEC("xdp_sock")
+int xdp_sock_prog(struct xdp_md *ctx) {
+	void *data_end = (void *)(long)ctx->data_end;
+	void *data = (void *)(long)ctx->data;
+	struct ethhdr *eth = data;
+	int idx = ctx->rx_queue_index;
+	unsigned int ipproto = 0;
+	unsigned long long nh_off;
+	/* Check if it's a UDP frame: If UDP -> Redirect to active xsk for user
+	 * space. If not -> pass to stack.
+	 */
+	nh_off = sizeof(*eth);
+	if (data + nh_off > data_end) return XDP_PASS;
+	if (eth->h_proto == __builtin_bswap16(ETH_P_IP))
+		ipproto = parse_ipv4(data, nh_off, data_end);
+	if (ipproto != IPPROTO_UDP) return XDP_PASS;
+	/* If socket bound to rx_queue then redirect to user space */
+	if (bpf_map_lookup_elem(&xsks_map, &idx))
+		return bpf_redirect_map(&xsks_map, idx, 0);
+	/* Else pass to Linux' network stack */
+	return XDP_PASS;
+}
+char _license[] SEC("license") = "GPL";
--- a/xdp-testing/xdp_pass_kern.c
+++ b/xdp-testing/xdp_pass_kern.c
+/* SPDX-License-Identifier: GPL-2.0 */
+#include <linux/bpf.h>
+#include <bpf/bpf_helpers.h>
+SEC("xdp")
+int  xdp_prog_simple(struct xdp_md *ctx)
+{
+	return XDP_PASS;
+}
+char _license[] SEC("license") = "GPL";