From c6f2cf7c2f2786b0a769534d0a1f0716326a3494 Mon Sep 17 00:00:00 2001 From: Marc Kleine-Budde Date: Thu, 21 Apr 2022 13:55:47 +0200 Subject: [PATCH] cangen: add support for SO_TXTIME Signed-off-by: Marc Kleine-Budde --- cangen.c | 194 +++++++++++++++++++++++++++++++++++++++++++++++++++---- 1 file changed, 182 insertions(+), 12 deletions(-) diff --git a/cangen.c b/cangen.c index 85b8021..ae6de4a 100644 --- a/cangen.c +++ b/cangen.c @@ -2,6 +2,8 @@ /* * cangen.c - CAN frames generator * + * Copyright (c) 2022 Pengutronix, + * Marc Kleine-Budde * Copyright (c) 2002-2007 Volkswagen Group Electronic Research * All rights reserved. * @@ -44,7 +46,9 @@ #include #include +#include #include +#include #include #include #include @@ -64,11 +68,13 @@ #include #include +#include #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 (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 (start time (UTC nanoseconds))"); + fprintf(stderr, " --mark (set SO_MARK to , 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) { - ret = clock_gettime(CLOCK_MONOTONIC, &ts); - if (ret) { - perror("clock_gettime"); + if (use_so_txtime) { + ret = setsockopt_txtime(s); + if (ret) 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;