cangen: add support for SO_TXTIME

Signed-off-by: Marc Kleine-Budde <mkl@pengutronix.de>

cangen.c

@@ -2,6 +2,8 @@
 /*
  * cangen.c - CAN frames generator
  *
+ * Copyright (c) 2022 Pengutronix,
+ *		 Marc Kleine-Budde <kernel@pengutronix.de>
  * Copyright (c) 2002-2007 Volkswagen Group Electronic Research
  * All rights reserved.
  *
@@ -44,7 +46,9 @@
 
 #include <ctype.h>
 #include <errno.h>
+#include <getopt.h>
 #include <libgen.h>
+#include <limits.h>
 #include <poll.h>
 #include <signal.h>
 #include <stdbool.h>
@@ -64,11 +68,13 @@
 
 #include <linux/can.h>
 #include <linux/can/raw.h>
+#include <linux/net_tstamp.h>
 
 #include "lib.h"
 
 #define DEFAULT_GAP 200 /* ms */
 #define DEFAULT_BURST_COUNT 1
+#define DEFAULT_SO_MARK_VAL 1
 
 #define MODE_RANDOM 0
 #define MODE_INCREMENT 1
@@ -81,6 +87,12 @@ extern int optind, opterr, optopt;
 static volatile int running = 1;
 static unsigned long long enobufs_count;
 static bool ignore_enobufs;
+static bool use_so_txtime;
+
+static int clockid = CLOCK_TAI;
+static int clock_nanosleep_flags;
+static struct timespec ts, ts_gap;
+static int so_mark_val = DEFAULT_SO_MARK_VAL;
 
 #define NSEC_PER_SEC 1000000000LL
 
@@ -151,6 +163,9 @@ static void print_usage(char *prg)
 	fprintf(stderr, "Options:\n");
 	fprintf(stderr, "         -g <ms>       (gap in milli seconds - default: %d ms)\n", DEFAULT_GAP);
 	fprintf(stderr, "         -a            (use absolute time for gap)");
+	fprintf(stderr, "         -t            (use SO_TXTIME)");
+	fprintf(stderr, "         --start <ns>  (start time (UTC nanoseconds))");
+	fprintf(stderr, "         --mark <id>   (set SO_MARK to <id>, default %u)", DEFAULT_SO_MARK_VAL);
 	fprintf(stderr, "         -e            (generate extended frame mode (EFF) CAN frames)\n");
 	fprintf(stderr, "         -f            (generate CAN FD CAN frames)\n");
 	fprintf(stderr, "         -b            (generate CAN FD CAN frames with bitrate switch (BRS))\n");
@@ -198,8 +213,78 @@ static void sigterm(int signo)
 	running = 0;
 }
 
+static int setsockopt_txtime(int fd)
+{
+	const struct sock_txtime so_txtime_val = {
+		.clockid = clockid,
+		.flags = SOF_TXTIME_REPORT_ERRORS,
+	};
+	struct sock_txtime so_txtime_val_read;
+	int so_mark_val_read;
+	socklen_t vallen;
+	int ret;
+
+	/* SO_TXTIME */
+
+	ret = setsockopt(fd, SOL_SOCKET, SO_TXTIME,
+			 &so_txtime_val, sizeof(so_txtime_val));
+	if (ret) {
+		int err = errno;
+
+		perror("setsockopt() SO_TXTIME");
+		if (err == EPERM)
+			fprintf(stderr, "Run with CAP_NET_ADMIN or as root.\n");
+
+		return -err;
+	};
+
+	vallen = sizeof(so_txtime_val_read);
+	ret = getsockopt(fd, SOL_SOCKET, SO_TXTIME,
+			 &so_txtime_val_read, &vallen);
+	if (ret) {
+		perror("getsockopt() SO_TXTIME");
+		return -errno;
+	};
+
+	if (vallen != sizeof(so_txtime_val) ||
+	    memcmp(&so_txtime_val, &so_txtime_val_read, vallen)) {
+		perror("getsockopt() SO_TXTIME: mismatch");
+		return -EINVAL;
+	}
+
+	/* SO_MARK */
+
+	ret = setsockopt(fd, SOL_SOCKET, SO_MARK, &so_mark_val, sizeof(so_mark_val));
+	if (ret) {
+		int err = errno;
+
+		perror("setsockopt() SO_MARK");
+		if (err == EPERM)
+			fprintf(stderr, "Run with CAP_NET_ADMIN or as root.\n");
+
+		return -err;
+	};
+
+	vallen = sizeof(so_mark_val_read);
+	ret = getsockopt(fd, SOL_SOCKET, SO_MARK,
+			 &so_mark_val_read, &vallen);
+	if (ret) {
+		perror("getsockopt() SO_MARK");
+		return -errno;
+	};
+
+	if (vallen != sizeof(so_mark_val) ||
+	    memcmp(&so_mark_val, &so_mark_val_read, vallen)) {
+		perror("getsockopt() SO_MARK: mismatch");
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
 static int do_send_one(int fd, void *buf, size_t len, int timeout)
 {
+	uint8_t control[CMSG_SPACE(sizeof(uint64_t))] = { 0 };
 	struct iovec iov = {
 		.iov_base = buf,
 		.iov_len = len,
@@ -211,6 +296,23 @@ static int do_send_one(int fd, void *buf, size_t len, int timeout)
 	ssize_t nbytes;
 	int ret;
 
+	if (use_so_txtime) {
+		struct cmsghdr *cm;
+		uint64_t tdeliver;
+
+		msg.msg_control = control;
+		msg.msg_controllen = sizeof(control);
+
+		tdeliver = ts.tv_sec * NSEC_PER_SEC + ts.tv_nsec;
+		ts = timespec_add(ts, ts_gap);
+
+		cm = CMSG_FIRSTHDR(&msg);
+		cm->cmsg_level = SOL_SOCKET;
+		cm->cmsg_type = SCM_TXTIME;
+		cm->cmsg_len = CMSG_LEN(sizeof(tdeliver));
+		memcpy(CMSG_DATA(cm), &tdeliver, sizeof(tdeliver));
+	}
+
  resend:
 	nbytes = sendmsg(fd, &msg, 0);
 	if (nbytes < 0) {
@@ -249,6 +351,53 @@ static int do_send_one(int fd, void *buf, size_t len, int timeout)
 	return 0;
 }
 
+static int setup_time(void)
+{
+	int ret;
+
+	if (use_so_txtime) {
+		/* start time is defined */
+		if (ts.tv_sec || ts.tv_nsec)
+			return 0;
+
+		/* start time is now .... */
+		ret = clock_gettime(clockid, &ts);
+		if (ret) {
+			perror("clock_gettime");
+			return ret;
+		}
+
+		/* ... + gap */
+		ts = timespec_add(ts, ts_gap);
+
+		return 0;
+	}
+
+	if (ts.tv_sec || ts.tv_nsec) {
+		ret = clock_nanosleep(clockid, TIMER_ABSTIME, &ts, NULL);
+		if (ret != 0 && ret != EINTR) {
+			perror("clock_nanosleep");
+			return ret;
+		}
+	} else if (clock_nanosleep_flags == TIMER_ABSTIME) {
+		ret = clock_gettime(clockid, &ts);
+		if (ret)
+			perror("clock_gettime");
+
+		return ret;
+	}
+
+	if (clock_nanosleep_flags != TIMER_ABSTIME)
+		ts = ts_gap;
+
+	return 0;
+}
+
+enum {
+	OPT_MARK = UCHAR_MAX + 1,
+	OPT_START = UCHAR_MAX + 2,
+};
+
 int main(int argc, char **argv)
 {
 	double gap = DEFAULT_GAP;
@@ -283,9 +432,7 @@ int main(int argc, char **argv)
 	int i;
 	struct ifreq ifr;
 
-	struct timespec ts, ts_gap;
 	struct timeval now;
-	int clock_nanosleep_flags = 0;
 	int ret;
 
 	/* set seed value for pseudo random numbers */
@@ -296,7 +443,13 @@ int main(int argc, char **argv)
 	signal(SIGHUP, sigterm);
 	signal(SIGINT, sigterm);
 
-	while ((opt = getopt(argc, argv, "g:aefbER8mI:L:D:p:n:ixc:vh?")) != -1) {
+	const struct option long_options[] = {
+		{ "mark",	required_argument,	0, OPT_MARK, },
+		{ "start",	required_argument,	0, OPT_START, },
+		{ 0,		0,			0, 0 },
+	};
+
+	while ((opt = getopt_long(argc, argv, "g:atefbER8mI:L:D:p:n:ixc:vh?", long_options, NULL)) != -1) {
 		switch (opt) {
 
 		case 'g':
@@ -307,6 +460,22 @@ int main(int argc, char **argv)
 			clock_nanosleep_flags = TIMER_ABSTIME;
 			break;
 
+		case 't':
+			clock_nanosleep_flags = TIMER_ABSTIME;
+			use_so_txtime = true;
+			break;
+
+		case OPT_START: {
+			int64_t start_time_ns;
+
+			start_time_ns = strtoll(optarg, NULL, 0);
+			ts = ns_to_timespec(start_time_ns);
+
+			break;
+		}
+		case OPT_MARK:
+			so_mark_val = strtoul(optarg, NULL, 0);
+			break;
 		case 'e':
 			extended = 1;
 			break;
@@ -501,16 +670,16 @@ int main(int argc, char **argv)
 		return 1;
 	}
 
-	if (clock_nanosleep_flags == TIMER_ABSTIME) {
+	if (use_so_txtime) {
-		ret = clock_gettime(CLOCK_MONOTONIC, &ts);
+		ret = setsockopt_txtime(s);
-		if (ret) {
+		if (ret)
-			perror("clock_gettime");
 			return 1;
-		}
-	} else {
-		ts = ts_gap;
 	}
 
+	ret = setup_time();
+	if (ret)
+		return 1;
+
 	while (running) {
 		frame.flags = 0;
 
@@ -587,12 +756,13 @@ int main(int argc, char **argv)
 		if (frame.len < maxdlen)
 			memset(&frame.data[frame.len], 0, maxdlen - frame.len);
 
-		if ((ts.tv_sec || ts.tv_nsec) &&
+		if (!use_so_txtime &&
+		    (ts.tv_sec || ts.tv_nsec) &&
 		    burst_sent_count >= burst_count) {
 			if (clock_nanosleep_flags == TIMER_ABSTIME)
 				ts = timespec_add(ts, ts_gap);
 
-			ret = clock_nanosleep(CLOCK_MONOTONIC, clock_nanosleep_flags, &ts, NULL);
+			ret = clock_nanosleep(clockid, clock_nanosleep_flags, &ts, NULL);
 			if (ret != 0 && ret != EINTR) {
 				perror("clock_nanosleep");
 				return 1;