Add motor-control

src/motor-control/build/Makefile

+SERVER_PROG = server
+CLIENT_PROG = client
+SRCDIR = ../src
+
+SERVER_SRCS  = server.c
+SERVER_SRCS  += recv_packet.c
+SERVER_SRCS  += send_packet.c
+SERVER_SRCS  += common.c
+SERVER_SRCS  += gpio.c
+
+CLIENT_SRCS  = client.c
+CLIENT_SRCS += recv_packet.c
+CLIENT_SRCS += send_packet.c
+CLIENT_SRCS += common.c
+
+SERVER_OBJS = $(SERVER_SRCS:%.c=%.o)
+CLIENT_OBJS = $(CLIENT_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
+
+ifneq ($(WITH_XDP),)
+CFLAGS += -D WITH_XDP
+LDFLAGS += -L/usr/lib -lbpf
+else ifneq ($(WITH_GIT_XDP),)
+IFLAGS += -I ${HOME}/libbpf/include
+CFLAGS += -D WITH_XDP $(IFLAGS)
+LDIRS += -L${HOME}/libbpf/src
+LLIBS += -lelf -lz -l:libbpf.a
+endif
+
+vpath %.c $(SRCDIR)
+
+all: links
+links: bin/$(SERVER_PROG) bin/$(CLIENT_PROG)
+bin/$(SERVER_PROG): $(SERVER_PROG)
+	mkdir -p bin
+	ln -fs $(realpath $(SERVER_PROG)) $@
+bin/$(CLIENT_PROG): $(CLIENT_PROG)
+	mkdir -p bin
+	ln -fs $(realpath $(CLIENT_PROG)) $@
+
+xdp_kern.o: xdp_kern.c
+	clang $(IFLAGS) -isystem /usr/include/arm-linux-gnueabihf -S -target bpf -D __BPF_TRACING__ -Wall -O2 -emit-llvm -c -g -o xdp_kern.ll $^
+	llc -march=bpf -filetype=obj -o $@ xdp_kern.ll
+
+ifneq ($(WITH_GIT_XDP),)
+$(SERVER_PROG): $(SERVER_OBJS) xdp_kern.o
+	$(CC) $(LDFLAGS) $(LDIRS) $(SERVER_OBJS) $(LLIBS) -o $@
+else
+$(SERVER_PROG): $(SERVER_OBJS)
+	$(CC) $(LDFLAGS) $(LDIRS) $^ $(LLIBS) -o $@
+endif
+
+$(CLIENT_PROG): $(CLIENT_OBJS)
+	$(CC) $(LDFLAGS) $(LDIRS) $^ $(LLIBS) -o $@
+
+
+-include $(subst .c,.d,$(SERVER_SRCS))
+-include $(subst .c,.d,$(CLIENT_SRCS))
+
+clean:
+	$(RM) -rf bin
+	$(RM) $(SERVER_OBJS) $(SERVER_PROG) $(subst .c,.d,$(SERVER_SRCS))
+	$(RM) $(CLIENT_OBJS) $(CLIENT_PROG) $(subst .c,.d,$(CLIENT_SRCS))
+
+.PHONY: clean all links

src/motor-control/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__ */
+

src/motor-control/src/common.c

+#define _GNU_SOURCE
+#include "common.h"
+
+#include <errno.h>
+#include <error.h>
+#include <fcntl.h>
+#include <inttypes.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 <sys/stat.h>
+#include <sys/types.h>
+#include <time.h>
+#include <unistd.h>
+
+static int latency_target_fd = -1;
+static int32_t latency_target_value = 0;
+
+void (*previous_handlers[NSIG])(int);
+static void (*sighand)(int);
+
+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;
+}
+
+/* Latency trick
+ * if the file /dev/cpu_dma_latency exists,
+ * open it and write a zero into it. This will tell
+ * the power management system not to transition to
+ * a high cstate (in fact, the system acts like idle=poll)
+ * When the fd to /dev/cpu_dma_latency is closed, the behavior
+ * goes back to the system default.
+ *
+ * Documentation/power/pm_qos_interface.txt
+ */
+void set_latency_target(void) {
+  struct stat s;
+  int err;
+
+  errno = 0;
+  err = stat("/dev/cpu_dma_latency", &s);
+  if (err == -1) {
+    error(EXIT_FAILURE, errno, "WARN: stat /dev/cpu_dma_latency failed");
+    return;
+  }
+
+  errno = 0;
+  latency_target_fd = open("/dev/cpu_dma_latency", O_RDWR);
+  if (latency_target_fd == -1) {
+    error(EXIT_FAILURE, errno, "WARN: open /dev/cpu_dma_latency");
+    return;
+  }
+
+  errno = 0;
+  err = write(latency_target_fd, &latency_target_value, 4);
+  if (err < 1) {
+    error(EXIT_FAILURE, errno, "# error setting cpu_dma_latency to %d!",
+          latency_target_value);
+    close(latency_target_fd);
+    return;
+  }
+  printf("# /dev/cpu_dma_latency set to %dus\n", latency_target_value);
+}
+
+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;
+  }
+}
+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);
+  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; }
+
+static void sighand_wrapper(int sig) {
+  // If we get un unexpected signal, report it, if not print the histogram
+  if (sig == SIGINT || sig == SIGTERM)
+    (*sighand)(sig);  // Will print the histogram
+  else
+    printf("Uknown signal interrupt: %s (%d)\n", strsignal(sig), sig);
+
+  // Execute the default handler
+  if (previous_handlers[sig] == SIG_DFL) {
+    signal(sig, SIG_DFL);
+    raise(sig);
+  } else if (previous_handlers[sig] == SIG_IGN) {
+    return;
+  } else {
+    (*previous_handlers[sig])(sig);
+  }
+}
+
+void init_signals(void (*_sighand)(int)) {
+  sighand = _sighand;
+
+  for (int i = 0; i < NSIG; i++) signal(i, sighand_wrapper);
+}
+
+static int get_bit(uint64_t t, int i) {
+  return (t & (((uint64_t) 1) << i)) > 0;
+}
+void encode(uint64_t t, char * b) {
+  for(int i = 0; i < 64; i++)
+    b[i] = get_bit(t, i) * 0xff;
+}
+uint64_t decode(char * b) {
+  uint64_t ret = 0;
+  for(int i = 0; i < 64; i++) {
+    int sum_bits = 0;
+    for(int j = 0; j < 8; j++)
+      sum_bits += (b[i] & (1 << j)) > 0;
+    ret |= (sum_bits >= 4) * (((uint64_t) 1) << i);
+  }
+  return ret;
+}

src/motor-control/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 MOTOR_STEPS 800
+
+#define NSEC_PER_SEC UINT64_C(1000000000)
+#define USEC_PER_SEC UINT64_C(1000000)
+#define SERVER_PORT "50000"
+#define SERVER_PORT_INT 50000
+
+#define MAX_BUFFER_SIZE 1024
+
+#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);
+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));
+
+int _min_(int a, int b);
+int _max_(int a, int b);
+
+extern void (*previous_handlers[NSIG])(int);
+
+void encode(uint64_t t, char * b);
+uint64_t decode(char * b);
+
+#endif

src/motor-control/src/gpio.c

+#include "gpio.h"
+#include <stdlib.h>
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/stat.h>
+#include <fcntl.h>
+
+static char path[64];
+static char cmd[128];
+static char * one = "1";
+static char * zero = "0";
+
+int enable_gpio(int * fd, int gpio_index) {
+  FILE *fp;
+  char gpio_state_str[16];
+  int gpio_state = 0;
+
+  sprintf(path, "/sys/class/gpio/gpio%d/value", gpio_index);
+  sprintf(cmd, "cat %s", path);
+
+  fp = popen(cmd, "r");
+  if (fp == NULL) {
+    fprintf(stderr, "Error when reading gpio\n");
+    exit(EXIT_FAILURE);
+  }
+
+  while (fgets(gpio_state_str, sizeof(gpio_state_str), fp) != NULL) {
+    gpio_state = atoi(gpio_state_str);
+  }
+
+  pclose(fp);
+
+  *fd = open(path, O_WRONLY);
+
+  return gpio_state;
+}
+
+int toggle_gpio(int fd, int gpio_state) {
+
+  if(gpio_state)
+        write(fd, zero, 1);
+  else
+        write(fd, one, 1);
+
+  gpio_state = !gpio_state;
+
+  return gpio_state;
+}

src/motor-control/src/gpio.h

+#ifndef GPIO_H
+#define GPIO_H
+
+int enable_gpio(int * fd, int gpio_index);
+int toggle_gpio(int fd, int gpio_state);
+
+#endif

src/motor-control/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 */

src/motor-control/src/recv_packet.c

+#define _GNU_SOURCE
+#include "recv_packet.h"
+
+#include <arpa/inet.h>
+#include <errno.h>
+#include <error.h>
+#include <fcntl.h>
+#include <getopt.h>
+#include <ifaddrs.h>
+#include <inttypes.h>
+#include <limits.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 <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/mman.h>
+#include <sys/socket.h>
+#include <sys/stat.h>
+#include <sys/types.h>
+#include <time.h>
+#include <unistd.h>
+
+#include "common.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
+
+static char rx_buffer[MAX_BUFFER_SIZE];
+static int sock_fd;
+
+static ingress_param_t *params;
+static ingress_stat_t *stats;
+
+// Sets the interface
+static int set_if(void) {
+  struct ifreq ifreq;
+
+  memset(&ifreq, 0, sizeof(ifreq));
+  strncpy(ifreq.ifr_name, params->network_if, sizeof(ifreq.ifr_name) - 1);
+
+  if (ioctl(sock_fd, SIOCGIFINDEX, &ifreq))
+    error(EXIT_FAILURE, errno, "ioctl SIOCGIFINDEX failed\n");
+
+  return ifreq.ifr_ifindex;
+}
+
+void init_udp_recv(ingress_param_t *_params, ingress_stat_t *_stats) {
+  int getaddrinfo_err;
+  int set_if_err;
+  struct addrinfo hints, *servinfo, *servinfo_it;
+
+  params = _params;
+  stats = _stats;
+
+  memset(&hints, 0, sizeof hints);
+  hints.ai_family = AF_UNSPEC;
+  hints.ai_socktype = SOCK_DGRAM;
+  hints.ai_flags = AI_PASSIVE;
+
+  getaddrinfo_err = getaddrinfo(NULL, SERVER_PORT, &hints, &servinfo);
+  if (getaddrinfo_err != 0) {
+    fprintf(stderr, "getaddrinfo: %s\n", gai_strerror(getaddrinfo_err));
+    exit(EXIT_FAILURE);
+  }
+
+  for (servinfo_it = servinfo; servinfo_it;
+       servinfo_it = servinfo_it->ai_next) {
+    sock_fd = socket(servinfo->ai_family, servinfo->ai_socktype,
+                     servinfo->ai_protocol);
+
+    if (bind(sock_fd, servinfo_it->ai_addr, servinfo_it->ai_addrlen) == -1) {
+      close(sock_fd);
+      continue;
+    }
+    break;
+  }
+
+  freeaddrinfo(servinfo);
+
+  if (sock_fd == -1)
+    error(EXIT_FAILURE, errno, "Couldn't create receive socket");
+
+  set_if_err = set_if();
+  if (set_if_err < 0) error(EXIT_FAILURE, errno, "Couldn't set interface\n");
+
+  if (setsockopt(sock_fd, SOL_SOCKET, SO_BINDTODEVICE, params->network_if,
+                 strlen(params->network_if)))
+    error(EXIT_FAILURE, errno, "setsockopt SO_BINDTODEVICE failed\n");
+}
+
+/*
+ * Receive UDP packet
+ */
+void recv_udp_packet() {
+  struct cmsghdr *cmsg;
+  struct msghdr msg;  // Message hardware, sent to the socket
+  struct iovec iov;   // The iovec structures stores the RX buffer
+  struct sockaddr_in sin;
+  struct {
+    struct cmsghdr cm;
+    char control[512];
+  } control;
+
+  int recvmsgerr;
+
+  iov.iov_base = &rx_buffer;
+  iov.iov_len = MAX_BUFFER_SIZE - 1;
+
+  memset(&msg, 0, sizeof(msg));
+  msg.msg_name = &sin;
+  msg.msg_namelen = sizeof(sin);
+  msg.msg_iov = &iov;
+  msg.msg_iovlen = 1;
+  msg.msg_control = &control;
+  msg.msg_controllen = sizeof(control);
+
+  recvmsgerr = recvmsg(sock_fd, &msg, 0);
+  if (recvmsgerr < 0)
+    error(EXIT_FAILURE, errno, "recvmsg failed, ret value: %d\n", recvmsgerr);
+
+  for (int i = 0; i < MAX_BUFFER_SIZE; i++) stats->data[i] = rx_buffer[i];
+}
+
+#ifdef WITH_XDP
+static int xdp_flags = XDP_FLAGS_DRV_MODE;
+static struct pollfd fds[1] = {0};
+static unsigned int ifindex;
+static struct xdpsock xdp_socket;
+
+static void open_xdp_socket(char *network_if) {
+  struct xsk_socket_config xsk_cfg;
+  uint32_t idx;
+  int ret, i;
+
+  /* Create XDP socket */
+  xsk_cfg.rx_size = XSK_RING_CONS__DEFAULT_NUM_DESCS;
+  xsk_cfg.tx_size = XSK_RING_PROD__DEFAULT_NUM_DESCS;
+  xsk_cfg.libbpf_flags = 0;
+  xsk_cfg.xdp_flags = xdp_flags;
+  xsk_cfg.bind_flags = 0;
+
+  ret = xsk_socket__create(&xdp_socket.xsk, network_if, 0, xdp_socket.umem.umem,
+                           &xdp_socket.rx, &xdp_socket.tx, &xsk_cfg);
+  if (ret) err("xsk_socket__create() failed");
+
+  /* Add some buffers */
+  ret = xsk_ring_prod__reserve(&xdp_socket.umem.fq,
+                               XSK_RING_PROD__DEFAULT_NUM_DESCS, &idx);
+
+  if (ret != XSK_RING_PROD__DEFAULT_NUM_DESCS)
+    err("xsk_ring_prod__reserve() failed");
+
+  for (i = 0; i < XSK_RING_PROD__DEFAULT_NUM_DESCS; i++)
+    *xsk_ring_prod__fill_addr(&xdp_socket.umem.fq, idx++) = i * FRAME_SIZE;
+
+  xsk_ring_prod__submit(&xdp_socket.umem.fq, XSK_RING_PROD__DEFAULT_NUM_DESCS);
+}
+
+/*
+ * Init XDP socket
+ */
+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/tsn-measures/packet-exchange/build/xdp_kern.o",
+  };
+  struct xsk_umem_config cfg = {
+      .fill_size = XSK_RING_PROD__DEFAULT_NUM_DESCS,
+      .comp_size = XSK_RING_CONS__DEFAULT_NUM_DESCS,
+      .frame_size = FRAME_SIZE,
+      .frame_headroom = XSK_UMEM__DEFAULT_FRAME_HEADROOM,
+      .flags = 0,
+  };
+  struct bpf_object *obj;
+  struct bpf_map *map;
+  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");
+
+  map = bpf_object__find_map_by_name(obj, "xsks_map");
+  xsks_map = bpf_map__fd(map);
+  if (xsks_map < 0) err("No xsks_map found!");
+
+  ifindex = if_nametoindex(params->network_if);
+  if (!ifindex) err_errno("if_nametoindex() failed");
+
+  /* Use XDP _only_ in conjuction with driver assisted mode */
+  ret = bpf_set_link_xdp_fd(ifindex, prog_fd, xdp_flags);
+  if (ret) err("bpf_set_link_xdp_fd() failed");
+
+  /* Allocate user space memory for xdp frames */
+  ret =
+      posix_memalign(&buffer, sysconf(_SC_PAGE_SIZE), NUM_FRAMES * FRAME_SIZE);
+  if (ret) err_errno("posix_memalign() failed");
+
+  ret = xsk_umem__create(&xdp_socket.umem.umem, buffer, NUM_FRAMES * FRAME_SIZE,
+                         &xdp_socket.umem.fq, &xdp_socket.umem.cq, &cfg);
+  if (ret) err("xsk_umem__create() failed");
+  xdp_socket.umem.buffer = buffer;
+
+  /* Open and bind socket */
+  open_xdp_socket(params->network_if);
+}
+
+void setup_poll_fd(void) {
+  fds[0].fd = xsk_socket__fd(xdp_socket.xsk);
+  fds[0].events = POLLIN;
+}
+
+static int received;
+static uint32_t idx_rx = 0, idx;
+
+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(struct timespec next) {
+  int ret;
+  uint64_t addr;
+  uint32_t len;
+  struct timespec next_pre, current;
+  int k = 0;
+
+  ret = poll(fds, 1, -1);
+
+  if (ret != 1)
+    error(EXIT_FAILURE, errno, "poll failed");
+
+  received = xsk_ring_cons__peek(&xdp_socket.rx, 1, &idx_rx);
+
+  if (received != 1)
+    error(EXIT_FAILURE, errno, "Received %d packets", received);
+
+  /* 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) {
+  uint64_t addr;
+  int ret;
+
+  /* Cleanup */
+  xsk_ring_cons__release(&xdp_socket.rx, received);
+
+  /* Add that particular buffer back to the fill queue */
+  if (xsk_prod_nb_free(&xdp_socket.umem.fq, received)) {
+    ret = xsk_ring_prod__reserve(&xdp_socket.umem.fq, received, &idx);
+
+    if (ret != received) err("xsk_ring_prod__reserve() failed");
+
+    *xsk_ring_prod__fill_addr(&xdp_socket.umem.fq, idx) =
+        xsk_umem__extract_addr(addr);
+
+    xsk_ring_prod__submit(&xdp_socket.umem.fq, received);
+  }
+}
+
+void close_xdp_socket(void) {
+  xsk_socket__delete(xdp_socket.xsk);
+  xsk_umem__delete(xdp_socket.umem.umem);
+  bpf_set_link_xdp_fd(ifindex, -1, xdp_flags);
+}
+
+#else
+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(struct timespec next) {
+  (void)next;
+  return 0;
+}
+void recv_xdp_cleanup(void) {}
+#endif
+

src/motor-control/src/recv_packet.h

+#ifndef RECV_PACKET
+#define RECV_PACKET
+
+#include "common.h"
+
+typedef struct ingress_param {
+	char network_if[16];
+
+	size_t tx_buffer_len;
+
+} ingress_param_t;
+
+typedef struct ingress_stat {
+	int min_interval;
+	int avg_interval;
+	int max_interval;
+
+	uint64_t packets_received;
+	uint64_t high_kernel_latency;
+	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);
+
+void recv_udp_packet(void);
+
+#ifdef WITH_XDP
+#define NUM_FRAMES 4096
+#define FRAME_SIZE XSK_UMEM__DEFAULT_FRAME_SIZE
+
+struct xsk_umem_info {
+	struct xsk_ring_prod fq;
+	struct xsk_ring_cons cq;
+	struct xsk_umem *umem;
+	void *buffer;
+};
+struct xdpsock {
+	struct xsk_ring_cons rx;
+	struct xsk_ring_prod tx;
+	struct xsk_umem_info umem;
+	struct xsk_socket *xsk;
+	int fd;
+};
+#endif
+
+void init_xdp_recv(ingress_param_t * _params, ingress_stat_t *_stats);
+int recv_xdp_packet();
+void recv_xdp_cleanup(void);
+void setup_poll_fd(void);
+void close_xdp_socket(void);
+
+#endif

src/motor-control/src/send_packet.c

+/*
+ *
+ * UDP packet sending functions
+ *
+ * Large portions taken from scheduled tx tools gist
+ *
+ */
+
+#define _GNU_SOURCE
+#include "send_packet.h"
+
+#include <arpa/inet.h>
+#include <errno.h>
+#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>
+#include <linux/sockios.h>
+#include <net/if.h>
+#include <netdb.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 <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include "common.h"
+
+static void *poll_thread(void *p);
+static void process_error_queue(egress_info_t * info);
+
+static void init_tx_buffer(egress_info_t * info);
+static int set_if(egress_info_t * info);
+
+static int so_timestamping_flags =
+    SOF_TIMESTAMPING_TX_SOFTWARE | SOF_TIMESTAMPING_SOFTWARE;
+
+/*
+ * Init UDP socket
+ */
+void init_udp_send(egress_info_t *info) {
+  int set_if_err;
+  pthread_t thread;
+
+  egress_param_t *params = info->params;
+  egress_stat_t *stats = info->stats;
+
+  stats->packets_sent = 0;
+
+  init_tx_buffer(info);
+
+  info->sock_fd = socket(PF_INET, SOCK_DGRAM, IPPROTO_UDP);
+  if (info->sock_fd < 0) error(EXIT_FAILURE, errno, "Socket creation failed\n");
+
+  set_if_err = set_if(info);
+  if (set_if_err < 0) error(EXIT_FAILURE, errno, "Couldn't set interface\n");
+
+  if (setsockopt(info->sock_fd, SOL_SOCKET, SO_PRIORITY, &params->packet_priority,
+                 sizeof(params->packet_priority)))
+    error(EXIT_FAILURE, errno, "Couldn't set socket priority\n");
+
+  if (setsockopt(info->sock_fd, SOL_SOCKET, SO_BINDTODEVICE, params->network_if,
+                 strlen(params->network_if)))
+    error(EXIT_FAILURE, errno, "setsockopt SO_BINDTODEVICE failed\n");
+
+  if (params->use_etf) {
+    info->sk_txtime.clockid = CLOCK_REALTIME;
+    info->sk_txtime.flags = SOF_TXTIME_REPORT_ERRORS;
+
+    if (setsockopt(info->sock_fd, SOL_SOCKET, SO_TXTIME, &info->sk_txtime,
+                   sizeof(info->sk_txtime)))
+      error(EXIT_FAILURE, errno, "setsockopt SO_TXTIME failed\n");
+  }
+
+  // Create poll thread
+  if (pthread_create(&thread, NULL, poll_thread, info))
+    error(EXIT_FAILURE, errno, "Couldn't create poll thread");
+}
+
+
+/*
+ * Sends udp packets
+ */
+void send_udp_packet(char *data, uint64_t txtime, egress_info_t * info) {
+  struct msghdr msg;     // Message hardware, sent to the socket
+  struct cmsghdr *cmsg;  // Control message hardware, for txtime
+  char control[CMSG_SPACE(sizeof(txtime))] = {};  // Stores txtime
+  struct iovec iov;  // The iovec structures stores the TX buffer
+  int sendmsgerr;
+  struct sockaddr_in sin;  // Server address
+
+  egress_param_t *params = info->params;
+  egress_stat_t *stats = info->stats;
+
+  stats->packets_sent++;
+
+  for(int i = 0; i < (int)params->tx_buffer_len; i++)
+    info->tx_buffer[i] = data[i];
+
+  memset(&sin, 0, sizeof(sin));
+  sin.sin_family = AF_INET;
+  sin.sin_addr.s_addr = inet_addr(params->server_ip);
+  sin.sin_port = htons(SERVER_PORT_INT);
+
+  iov.iov_base = info->tx_buffer;
+  iov.iov_len = params->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 (params->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(uint64_t));
+    *((uint64_t *)CMSG_DATA(cmsg)) = txtime;
+    msg.msg_controllen = cmsg->cmsg_len;
+  }
+
+  sendmsgerr = sendmsg(info->sock_fd, &msg, 0);
+  if (sendmsgerr < 0)
+    error(EXIT_FAILURE, errno, "sendmsg failed, ret value: %d\n", sendmsgerr);
+}
+
+static void *poll_thread(void *p) {
+  egress_info_t * info = (egress_info_t *) p;
+
+  // Poll file descriptor
+  struct pollfd poll_fd = {.fd = info->sock_fd};
+
+
+  while (1) {
+    int ret;
+
+    ret = poll(&poll_fd, 1, -1);
+    if (ret == 1 && poll_fd.revents & POLLERR) {
+      process_error_queue(info);
+    }
+  }
+
+  return NULL;
+}
+
+static void process_error_queue(egress_info_t * info) {
+  int recv_ret;
+
+  // IO vector
+  unsigned char data_buffer[256];  // Buffer in io vector
+  struct iovec iov = {.iov_base = data_buffer, .iov_len = sizeof(data_buffer)};
+
+  // Control data, will store error or timestamps
+  unsigned char msg_control[CMSG_SPACE(sizeof(struct sock_extended_err)) +
+                            CMSG_SPACE(sizeof(struct timespec))];
+
+  // Message hardware structure, containts IO vector and control message
+  // hardware
+  struct msghdr msg = {.msg_iov = &iov,
+                       .msg_iovlen = 1,
+                       .msg_control = msg_control,
+                       .msg_controllen = sizeof(msg_control)};
+
+  struct cmsghdr *cmsg;
+
+  egress_stat_t *stats = info->stats;
+
+
+  // Timestamps and errors are received in the error queue
+  recv_ret = recvmsg(info->sock_fd, &msg, MSG_ERRQUEUE | MSG_DONTWAIT);
+  if (recv_ret == -1) {
+    fprintf(stderr, "recvmsg() failed\n");
+    return;
+  }
+
+  for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) {
+    if (cmsg->cmsg_level == SOL_SOCKET && cmsg->cmsg_type == SO_TIMESTAMPING)
+      continue;
+    // If an error was received
+    else {
+      struct sock_extended_err *serr = (void *)CMSG_DATA(cmsg);
+
+      if (serr->ee_origin != SO_EE_ORIGIN_TXTIME) continue;
+
+      switch (serr->ee_code) {
+        case SO_EE_CODE_TXTIME_INVALID_PARAM:
+          stats->invalid_parameter++;
+          break;
+        case SO_EE_CODE_TXTIME_MISSED:
+          stats->missed_deadline++;
+          break;
+        default:
+          fprintf(stderr, "Uknown TxTime error\n");
+      }
+    }
+  }
+}
+
+// Sets the interface
+static int set_if(egress_info_t * info) {
+  struct ifreq ifreq;
+
+  egress_param_t * params = info->params;
+
+  memset(&ifreq, 0, sizeof(ifreq));
+  strncpy(ifreq.ifr_name, params->network_if, sizeof(ifreq.ifr_name) - 1);
+
+  if (ioctl(info->sock_fd, SIOCGIFINDEX, &ifreq))
+    error(EXIT_FAILURE, errno, "ioctl SIOCGIFINDEX failed\n");
+
+  return ifreq.ifr_ifindex;
+}
+
+static void init_tx_buffer(egress_info_t * info) {
+
+  egress_param_t * params = info->params;
+
+  if (params->tx_buffer_len < 1) {
+    fprintf(stderr, "tx buffer length should be greater than 1\n");
+    exit(EXIT_FAILURE);
+  }
+
+  info->tx_buffer = malloc(params->tx_buffer_len);
+}
+

src/motor-control/src/send_packet.h

+#ifndef SEND_PACKET_H
+#define SEND_PACKET_H
+
+#include <arpa/inet.h>
+#include <errno.h>
+#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>
+#include <linux/sockios.h>
+#include <net/if.h>
+#include <netdb.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 <sys/stat.h>
+#include <sys/types.h>
+#include <unistd.h>
+
+#include "common.h"
+
+typedef struct egress_param {
+  int packet_priority;
+  size_t tx_buffer_len;
+  char server_ip[45];
+  char network_if[16];
+
+  int use_etf;
+} egress_param_t;
+
+typedef struct egress_stat {
+
+  uint64_t packets_sent;
+
+  uint64_t high_kernel_latency;
+  uint64_t invalid_parameter;
+  uint64_t missed_deadline;
+
+  int min_interval;
+  int avg_interval;
+  int max_interval;
+
+} egress_stat_t;
+
+typedef struct egress_info {
+
+  int sock_fd;
+
+  struct sock_txtime sk_txtime;
+
+  char *tx_buffer;
+
+  egress_param_t *params;
+  egress_stat_t *stats;
+
+} egress_info_t;
+
+void init_udp_send(egress_info_t *info);
+void send_udp_packet(char *data, uint64_t txtime, egress_info_t * info);
+
+#endif

src/motor-control/src/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"
+#include "bpf_endian.h"
+#include "parsing_helpers.h"
+
+#define UDP_PORT 50000
+
+#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,
+};
+
+SEC("xdp_sock")
+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 hdr_cursor nh = { .pos = data };
+
+        index = ctx->rx_queue_index;
+
+	eth_type = parse_ethhdr(&nh, data_end, &eth);
+	if (eth_type < 0)
+		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;
+	}
+
+	// only support UDP for now
+	if (ip_type != 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, &index))
+		return bpf_redirect_map(&xsks_map, index, 0);
+
+	/* Else pass to Linux' network stack */
+	return XDP_PASS;
+}
+
+char _license[] SEC("license") = "GPL";
+