tools: added stamp-frame

Signed-off-by: Alessandro Rubini <rubini@gnudd.com>

tools/.gitignore

@@ -7,3 +7,4 @@ wr-dio-cmd
 wr-dio-pps
 wr-dio-agent
 wr-dio-ruler
+stamp-frame

tools/Makefile

@@ -7,6 +7,7 @@ LIBOBJ = speclib.o loader-ll.o
 
 PROGS = spec-cl spec-fwloader spec-vuart specmem
 PROGS += wr-dio-cmd wr-dio-pps wr-dio-agent wr-dio-ruler
+PROGS += stamp-frame
 
 
 all: $(LIB) $(PROGS)

tools/stamp-frame.c

+/*
+ * Copyright (C) 2010,2012 CERN (www.cern.ch)
+ * Author: Alessandro Rubini <rubini@gnudd.com>
+ *
+ * Released to the public domain as sample code to be customized.
+ *
+ * This work is part of the White Rabbit project, a research effort led
+ * by CERN, the European Institute for Nuclear Research.
+ */
+#include <stdio.h>
+#include <stdlib.h>
+#include <unistd.h>
+#include <string.h>
+#include <errno.h>
+#include <sys/ioctl.h>
+#include <sys/socket.h>
+#include <netpacket/packet.h>
+#include <net/if.h>
+#include <net/ethernet.h>
+#include <arpa/inet.h>
+
+#include "net_tstamp.h" /* Actually, <linux/net_tstamp.h> */
+
+#ifndef SO_TIMESTAMPING
+# define SO_TIMESTAMPING         37
+# define SCM_TIMESTAMPING        SO_TIMESTAMPING
+#endif
+
+#ifndef SIOCSHWTSTAMP
+# define SIOCSHWTSTAMP 0x89b0
+#endif
+
+#ifndef ETH_P_1588
+# define ETH_P_1588   0x88F7
+#endif
+
+/* This structure is used to collect stamping information */
+struct ts_data {
+	struct timespec ns;
+	struct timespec hw[3]; /* software, hw-sys, hw-raw */
+	int error;
+};
+
+/* We can print such stamp info. Returns -1 with errno set on error */
+int print_stamp(FILE *out, char *prefix, struct ts_data *tstamp, FILE *err)
+{
+	int i;
+	static char *names[] = {"sw    ", "hw-sys", "hw-raw"};
+
+	if (tstamp->error) {
+		if (err)
+			fprintf(err, "%s: %s\n", prefix, strerror(errno));
+		errno = tstamp->error;
+		return -1;
+	}
+	fprintf(out, "%s     ns: %10li.%09li\n", prefix, tstamp->ns.tv_sec,
+		tstamp->ns.tv_nsec);
+	for (i = 0; i < 3; i++)
+		fprintf(out, "%s %s: %10li.%09li\n", prefix, names[i],
+			tstamp->hw[i].tv_sec,
+			tstamp->hw[i].tv_nsec);
+	fprintf(out, "\n");
+	return 0;
+}
+
+
+/*
+ * This function opens a socket and configures it for stamping.
+ * It is a library function, in a way, and was used as such
+ * when part of the "testing" programs of wr-switch-sw
+ */
+int make_stamping_socket(FILE *errchan, char *argv0, char *ifname,
+			 int tx_type, int rx_filter, int bits,
+			 unsigned char *macaddr)
+{
+	struct ifreq ifr;
+	struct sockaddr_ll addr;
+	struct hwtstamp_config hwconfig;
+	int sock, iindex, enable = 1;
+
+	sock = socket(PF_PACKET, SOCK_RAW, htons(ETH_P_ALL));
+	if (sock < 0 && errchan)
+		fprintf(errchan, "%s: socket(): %s\n", argv0,
+			strerror(errno));
+	if (sock < 0)
+		return sock;
+
+	memset (&ifr, 0, sizeof(ifr));
+	strcpy(ifr.ifr_name, ifname);
+
+	/* hw interface information */
+	if (ioctl(sock, SIOCGIFINDEX, &ifr) < 0) {
+		if (errchan)
+			fprintf(errchan, "%s: SIOCGIFINDEX(%s): %s\n", argv0,
+				ifname, strerror(errno));
+		close(sock);
+		return -1;
+	}
+
+	iindex = ifr.ifr_ifindex;
+	if (ioctl(sock, SIOCGIFHWADDR, &ifr) < 0) {
+		if (errchan)
+			fprintf(errchan, "%s: SIOCGIFHWADDR(%s): %s\n", argv0,
+				ifname, strerror(errno));
+		close(sock);
+		return -1;
+	}
+	memcpy(macaddr, ifr.ifr_hwaddr.sa_data, 6);
+
+	/* Also, enable stamping for the hw interface */
+	memset(&hwconfig, 0, sizeof(hwconfig));
+	hwconfig.tx_type = tx_type;
+	hwconfig.rx_filter = rx_filter;
+	ifr.ifr_data = (void *)&hwconfig;
+	if (ioctl(sock, SIOCSHWTSTAMP, &ifr) < 0) {
+		if (errchan)
+			fprintf(errchan, "%s: SIOCSHWSTAMP(%s): %s\n", argv0,
+				ifname, strerror(errno));
+		close(sock);
+		return -1;
+	}
+
+	/* bind and setsockopt */
+	memset(&addr, 0, sizeof(addr));
+	addr.sll_family = AF_PACKET;
+	addr.sll_protocol = htons(ETH_P_ALL);
+	addr.sll_ifindex = iindex;
+	addr.sll_pkttype = PACKET_OUTGOING;
+	if (bind(sock, (struct sockaddr *)&addr, sizeof(addr)) < 0) {
+		if (errchan)
+			fprintf(errchan, "%s: SIOCSHWSTAMP(%s): %s\n", argv0,
+				ifname, strerror(errno));
+		close(sock);
+		return -1;
+	}
+	if (setsockopt(sock, SOL_SOCKET, SO_TIMESTAMP,
+			       &enable, sizeof(enable)) < 0) {
+		if (errchan)
+			fprintf(errchan, "%s: setsockopt(TIMESTAMP): %s\n",
+				argv0, strerror(errno));
+		close(sock);
+		return -1;
+	}
+	if (setsockopt(sock, SOL_SOCKET, SO_TIMESTAMPING,
+			       &bits, sizeof(bits)) < 0) {
+		if (errchan)
+			fprintf(errchan, "%s: setsockopt(TIMESTAMPING): %s\n",
+				argv0, strerror(errno));
+		close(sock);
+		return -1;
+	}
+	return sock;
+}
+
+/*
+ * Another "library" function, actually only used by the following two
+ */
+static void __collect_data(struct msghdr *msgp, struct ts_data *tstamp)
+{
+	struct cmsghdr *cm;
+	struct timespec *tsptr;
+
+	if (!tstamp)
+		return;
+	memset(tstamp, 0, sizeof(*tstamp));
+
+	/* Extract data from the cmsg */
+	for (cm = CMSG_FIRSTHDR(msgp); cm; cm = CMSG_NXTHDR(msgp, cm)) {
+		tsptr = (struct timespec *)CMSG_DATA(cm);
+		if (0) {
+			printf("level %i, type %i, len %i\n", cm->cmsg_level,
+			       cm->cmsg_type, cm->cmsg_len);
+		}
+		if (cm->cmsg_level != SOL_SOCKET)
+			continue;
+		if (cm->cmsg_type == SO_TIMESTAMPNS)
+			tstamp->ns = *tsptr;
+		if (cm->cmsg_type == SO_TIMESTAMPING)
+			memcpy(tstamp->hw, tsptr, sizeof(tstamp->hw));
+	}
+}
+
+/*
+ * These functions are like send/recv but handle stamping too.
+ */
+ssize_t send_and_stamp(int sock, void *buf, size_t len, int flags,
+	struct ts_data *tstamp)
+{
+	struct msghdr msg; /* this line and more from timestamping.c */
+	struct iovec entry;
+	struct sockaddr_ll from_addr;
+	struct {
+		struct cmsghdr cm;
+		char control[512];
+	} control;
+	char data[3*1024];
+	int i, j, ret;
+
+	ret = send(sock, buf, len, flags);
+	if (ret < 0)
+		return ret;
+
+	/* Then, get back from the error queue */
+	memset(&msg, 0, sizeof(msg));
+	msg.msg_iov = &entry;
+	msg.msg_iovlen = 1;
+	entry.iov_base = data;
+	entry.iov_len = sizeof(data);
+	msg.msg_name = (caddr_t)&from_addr;
+	msg.msg_namelen = sizeof(from_addr);
+	msg.msg_control = &control;
+	msg.msg_controllen = sizeof(control);
+
+	j = 100; /* number of trials */
+	while ( (i = recvmsg(sock, &msg, MSG_ERRQUEUE)) < 0 && j--)
+		usleep(10000); /* retry for 1 second */
+	if (i < 0) {
+		memset(tstamp, 0, sizeof(*tstamp));
+		tstamp->error = ETIMEDOUT;
+		return ret;
+	}
+	if (getenv("STAMP_VERBOSE")) {
+		int b;
+		printf("send %i =", i);
+		for (b = 0; b < i && b < 20; b++)
+			printf(" %02x", data[b] & 0xff);
+		putchar('\n');
+	}
+
+	/* FIX<E: Check that the actual data is what we sent */
+
+	__collect_data(&msg, tstamp);
+
+	return ret;
+}
+
+ssize_t recv_and_stamp(int sock, void *buf, size_t len, int flags,
+	struct ts_data *tstamp)
+{
+	int ret;
+	struct msghdr msg;
+	struct iovec entry;
+	struct sockaddr_ll from_addr;
+	struct {
+		struct cmsghdr cm;
+		char control[512];
+	} control;
+
+	if (0) {
+		/* we can't really call recv, do it with cmsg alone */
+		ret = recv(sock, buf, len, flags);
+	} else {
+		memset(&msg, 0, sizeof(msg));
+		msg.msg_iov = &entry;
+		msg.msg_iovlen = 1;
+		entry.iov_base = buf;
+		entry.iov_len = len;
+		msg.msg_name = (caddr_t)&from_addr;
+		msg.msg_namelen = sizeof(from_addr);
+		msg.msg_control = &control;
+		msg.msg_controllen = sizeof(control);
+
+		ret = recvmsg(sock, &msg, 0);
+		if (ret < 0)
+			return ret;
+
+		if (getenv("STAMP_VERBOSE")) {
+			int b;
+			printf("recv %i =", ret);
+			for (b = 0; b < ret && b < 20; b++)
+				printf(" %02x", ((char *)buf)[b] & 0xff);
+			putchar('\n');
+		}
+		__collect_data(&msg, tstamp);
+	}
+	return ret;
+}
+
+/* Add and subtract timespec */
+void ts_add(struct timespec *t1, struct timespec *t2)
+{
+	t1->tv_sec += t2->tv_sec;
+	t1->tv_nsec += t2->tv_nsec;
+	if (t1->tv_nsec > 1000 * 1000 * 1000) {
+		t1->tv_nsec -= 1000 * 1000 * 1000;
+		t1->tv_sec++;
+	}
+}
+
+void ts_sub(struct timespec *t1, struct timespec *t2)
+{
+	t1->tv_sec -= t2->tv_sec;
+	t1->tv_nsec -= t2->tv_nsec;
+	if (t1->tv_nsec < 0) {
+		t1->tv_nsec += 1000 * 1000 * 1000;
+		t1->tv_sec--;
+	}
+}
+
+
+/*
+ * Ok, the library-like part is over, we are at main now
+ */
+#define STAMP_PROTO 0xf001
+
+/* This is the frame we are exchanging back and forth */
+struct frame {
+	struct ether_header h;
+	uint16_t phase; /* 0 = transmit, 1 = tx follow up */
+	struct timespec ts[4];
+	unsigned char filler[64]; /* to reach minimum size for sure */
+};
+
+void report_times(struct timespec *ts)
+{
+	struct timespec rtt, tmp, fw, bw;
+	int i;
+
+	for (i = 0; i < 4; i++)
+		printf("timestamp    T%i: %9li.%09li\n", i+1,
+		       ts[i].tv_sec, ts[i].tv_nsec);
+
+	/* calculate round trip time, forward, backward */
+	rtt = ts[3];
+	ts_sub(&rtt, &ts[0]);
+	tmp = ts[2];
+	ts_sub(&tmp, &ts[1]);
+	ts_sub(&rtt, &tmp);
+
+	fw = ts[1];
+	ts_sub(&fw, &ts[0]);
+	bw = ts[3];
+	ts_sub(&bw, &ts[2]);
+
+	printf("round trip time: %9li.%09li\n", rtt.tv_sec, rtt.tv_nsec);
+	printf("forward    time: %9li.%09li\n", fw.tv_sec, fw.tv_nsec);
+	printf("backward   time: %9li.%09li\n", bw.tv_sec, bw.tv_nsec);
+}
+
+/* send a frame, and then again after filling the tx time at offset given */
+void send_one_with_followup(int sock, struct frame *f, unsigned char *mac,
+			    int offset)
+{
+	struct ts_data stamp;
+
+	/* Fill ether header */
+	memset(&f->h.ether_dhost, 0xff, ETH_ALEN); /* broadcast */
+	memcpy(&f->h.ether_shost, mac, ETH_ALEN);
+	f->h.ether_type = htons(STAMP_PROTO);
+
+	f->phase = 0;
+	if (send_and_stamp(sock, f, sizeof(*f), 0, &stamp) < 0)
+		fprintf(stderr, "send_and_stamp: %s\n", strerror(errno));
+	f->phase = 1;
+	f->ts[offset] = stamp.hw[2]; /* hw raw */
+	if (send_and_stamp(sock, f, sizeof(*f), 0, NULL) < 0)
+		fprintf(stderr, "send_and_stamp: %s\n", strerror(errno));
+
+	if (getenv("STAMP_PRINT_ALL"))
+		print_stamp(stdout, "send", &stamp, stderr);
+}
+
+/* receive a frame, timestamping it, and receive the followup too; save ts */
+void recv_one_with_followup(int sock, struct frame *f, unsigned char *mac,
+			    int offset)
+{
+	struct ts_data stamp;
+	int ret;
+
+	while (1) { /* repeat until a good frame is received */
+		ret = recv_and_stamp(sock, f, sizeof(*f), MSG_TRUNC, &stamp);
+		if (ret < 0) {
+			fprintf(stderr, "recv_and_stamp: %s\n",
+				strerror(errno));
+			continue;
+		}
+		if (ret != sizeof(*f))
+			continue;
+		if (ntohs(f->h.ether_type) != STAMP_PROTO)
+			continue;
+		if (f->phase != 0)
+			continue;
+		break;
+	}
+	/* receive another one, lazily don't wait */
+	if (recv_and_stamp(sock, f, sizeof(*f), MSG_TRUNC, NULL) < 0)
+		fprintf(stderr, "recv_and_stamp: %s\n",
+			strerror(errno));
+	f->ts[offset] = stamp.hw[2];
+
+	if (getenv("STAMP_PRINT_ALL"))
+		print_stamp(stdout, "recv", &stamp, stderr);
+
+}
+
+int main(int argc, char **argv)
+{
+	static struct frame f;
+	int sock;
+	unsigned char macaddr[6];
+	int listenmode = 0;
+	int howto = SOF_TIMESTAMPING_MASK; /* everything */
+
+	/* From ./net_tstamp.h, these are the "howto" values
+	 *
+	 * SOF_TIMESTAMPING_TX_HARDWARE = 1,
+	 * SOF_TIMESTAMPING_TX_SOFTWARE = 2
+	 * SOF_TIMESTAMPING_RX_HARDWARE = 4,
+	 * SOF_TIMESTAMPING_RX_SOFTWARE = 8,
+	 * SOF_TIMESTAMPING_SOFTWARE =   16,
+	 * SOF_TIMESTAMPING_SYS_HARDWARE = 32,
+	 * SOF_TIMESTAMPING_RAW_HARDWARE = 64,
+	 */
+
+	if (argc == 3 && !strcmp(argv[2], "listen")) {
+		listenmode = 1;
+		argc--;
+	}
+	if (argc != 2) {
+		fprintf(stderr, "%s: Use \"%s <ifname> [listen]\n", argv[0],
+			argv[0]);
+		exit(1);
+	}
+
+	printf("%s: Using interface %s, with all timestamp options active\n",
+	       argv[0], argv[1]);
+
+	/* Create a socket to use for stamping, use the library code above */
+	sock = make_stamping_socket(stderr, argv[0], argv[1],
+				    HWTSTAMP_TX_ON, HWTSTAMP_FILTER_ALL,
+				    howto, macaddr);
+	if (sock < 0) /* message already printed */
+		exit(1);
+
+	if (listenmode) {
+		/* forever reply */
+		while (1) {
+			recv_one_with_followup(sock, &f, macaddr, 1);
+			send_one_with_followup(sock, &f, macaddr, 2);
+		}
+	}
+
+	/* send mode:  send first, then receive, then print */
+	send_one_with_followup(sock, &f, macaddr, 0);
+	recv_one_with_followup(sock, &f, macaddr, 3);
+
+	report_times(f.ts);
+
+	exit(0);
+}
+