selftests/net/so_txtime.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Test the SO_TXTIME API
0004  *
0005  * Takes a stream of { payload, delivery time }[], to be sent across two
0006  * processes. Start this program on two separate network namespaces or
0007  * connected hosts, one instance in transmit mode and the other in receive
0008  * mode using the '-r' option. Receiver will compare arrival timestamps to
0009  * the expected stream. Sender will read transmit timestamps from the error
0010  * queue. The streams can differ due to out-of-order delivery and drops.
0011  */
0012
0013 #define _GNU_SOURCE
0014
0015 #include <arpa/inet.h>
0016 #include <error.h>
0017 #include <errno.h>
0018 #include <inttypes.h>
0019 #include <linux/net_tstamp.h>
0020 #include <linux/errqueue.h>
0021 #include <linux/if_ether.h>
0022 #include <linux/ipv6.h>
0023 #include <linux/udp.h>
0024 #include <stdbool.h>
0025 #include <stdlib.h>
0026 #include <stdio.h>
0027 #include <string.h>
0028 #include <sys/socket.h>
0029 #include <sys/stat.h>
0030 #include <sys/time.h>
0031 #include <sys/types.h>
0032 #include <time.h>
0033 #include <unistd.h>
0034 #include <poll.h>
0035
0036 static int  cfg_clockid = CLOCK_TAI;
0037 static uint16_t cfg_port    = 8000;
0038 static int  cfg_variance_us = 4000;
0039 static uint64_t cfg_start_time_ns;
0040 static int  cfg_mark;
0041 static bool cfg_rx;
0042
0043 static uint64_t glob_tstart;
0044 static uint64_t tdeliver_max;
0045
0046 /* encode one timed transmission (of a 1B payload) */
0047 struct timed_send {
0048     char    data;
0049     int64_t delay_us;
0050 };
0051
0052 #define MAX_NUM_PKT 8
0053 static struct timed_send cfg_buf[MAX_NUM_PKT];
0054 static int cfg_num_pkt;
0055
0056 static int cfg_errq_level;
0057 static int cfg_errq_type;
0058
0059 static struct sockaddr_storage cfg_dst_addr;
0060 static struct sockaddr_storage cfg_src_addr;
0061 static socklen_t cfg_alen;
0062
0063 static uint64_t gettime_ns(clockid_t clock)
0064 {
0065     struct timespec ts;
0066
0067     if (clock_gettime(clock, &ts))
0068         error(1, errno, "gettime");
0069
0070     return ts.tv_sec * (1000ULL * 1000 * 1000) + ts.tv_nsec;
0071 }
0072
0073 static void do_send_one(int fdt, struct timed_send *ts)
0074 {
0075     char control[CMSG_SPACE(sizeof(uint64_t))];
0076     struct msghdr msg = {0};
0077     struct iovec iov = {0};
0078     struct cmsghdr *cm;
0079     uint64_t tdeliver;
0080     int ret;
0081
0082     iov.iov_base = &ts->data;
0083     iov.iov_len = 1;
0084
0085     msg.msg_iov = &iov;
0086     msg.msg_iovlen = 1;
0087     msg.msg_name = (struct sockaddr *)&cfg_dst_addr;
0088     msg.msg_namelen = cfg_alen;
0089
0090     if (ts->delay_us >= 0) {
0091         memset(control, 0, sizeof(control));
0092         msg.msg_control = &control;
0093         msg.msg_controllen = sizeof(control);
0094
0095         tdeliver = glob_tstart + ts->delay_us * 1000;
0096         tdeliver_max = tdeliver_max > tdeliver ?
0097                    tdeliver_max : tdeliver;
0098
0099         cm = CMSG_FIRSTHDR(&msg);
0100         cm->cmsg_level = SOL_SOCKET;
0101         cm->cmsg_type = SCM_TXTIME;
0102         cm->cmsg_len = CMSG_LEN(sizeof(tdeliver));
0103         memcpy(CMSG_DATA(cm), &tdeliver, sizeof(tdeliver));
0104     }
0105
0106     ret = sendmsg(fdt, &msg, 0);
0107     if (ret == -1)
0108         error(1, errno, "write");
0109     if (ret == 0)
0110         error(1, 0, "write: 0B");
0111
0112 }
0113
0114 static void do_recv_one(int fdr, struct timed_send *ts)
0115 {
0116     int64_t tstop, texpect;
0117     char rbuf[2];
0118     int ret;
0119
0120     ret = recv(fdr, rbuf, sizeof(rbuf), 0);
0121     if (ret == -1 && errno == EAGAIN)
0122         error(1, EAGAIN, "recv: timeout");
0123     if (ret == -1)
0124         error(1, errno, "read");
0125     if (ret != 1)
0126         error(1, 0, "read: %dB", ret);
0127
0128     tstop = (gettime_ns(cfg_clockid) - glob_tstart) / 1000;
0129     texpect = ts->delay_us >= 0 ? ts->delay_us : 0;
0130
0131     fprintf(stderr, "payload:%c delay:%lld expected:%lld (us)\n",
0132             rbuf[0], (long long)tstop, (long long)texpect);
0133
0134     if (rbuf[0] != ts->data)
0135         error(1, 0, "payload mismatch. expected %c", ts->data);
0136
0137     if (llabs(tstop - texpect) > cfg_variance_us)
0138         error(1, 0, "exceeds variance (%d us)", cfg_variance_us);
0139 }
0140
0141 static void do_recv_verify_empty(int fdr)
0142 {
0143     char rbuf[1];
0144     int ret;
0145
0146     ret = recv(fdr, rbuf, sizeof(rbuf), 0);
0147     if (ret != -1 || errno != EAGAIN)
0148         error(1, 0, "recv: not empty as expected (%d, %d)", ret, errno);
0149 }
0150
0151 static int do_recv_errqueue_timeout(int fdt)
0152 {
0153     char control[CMSG_SPACE(sizeof(struct sock_extended_err)) +
0154              CMSG_SPACE(sizeof(struct sockaddr_in6))] = {0};
0155     char data[sizeof(struct ethhdr) + sizeof(struct ipv6hdr) +
0156           sizeof(struct udphdr) + 1];
0157     struct sock_extended_err *err;
0158     int ret, num_tstamp = 0;
0159     struct msghdr msg = {0};
0160     struct iovec iov = {0};
0161     struct cmsghdr *cm;
0162     int64_t tstamp = 0;
0163
0164     iov.iov_base = data;
0165     iov.iov_len = sizeof(data);
0166
0167     msg.msg_iov = &iov;
0168     msg.msg_iovlen = 1;
0169
0170     msg.msg_control = control;
0171     msg.msg_controllen = sizeof(control);
0172
0173     while (1) {
0174         const char *reason;
0175
0176         ret = recvmsg(fdt, &msg, MSG_ERRQUEUE);
0177         if (ret == -1 && errno == EAGAIN)
0178             break;
0179         if (ret == -1)
0180             error(1, errno, "errqueue");
0181         if (msg.msg_flags != MSG_ERRQUEUE)
0182             error(1, 0, "errqueue: flags 0x%x\n", msg.msg_flags);
0183
0184         cm = CMSG_FIRSTHDR(&msg);
0185         if (cm->cmsg_level != cfg_errq_level ||
0186             cm->cmsg_type != cfg_errq_type)
0187             error(1, 0, "errqueue: type 0x%x.0x%x\n",
0188                     cm->cmsg_level, cm->cmsg_type);
0189
0190         err = (struct sock_extended_err *)CMSG_DATA(cm);
0191         if (err->ee_origin != SO_EE_ORIGIN_TXTIME)
0192             error(1, 0, "errqueue: origin 0x%x\n", err->ee_origin);
0193
0194         switch (err->ee_errno) {
0195         case ECANCELED:
0196             if (err->ee_code != SO_EE_CODE_TXTIME_MISSED)
0197                 error(1, 0, "errqueue: unknown ECANCELED %u\n",
0198                       err->ee_code);
0199             reason = "missed txtime";
0200         break;
0201         case EINVAL:
0202             if (err->ee_code != SO_EE_CODE_TXTIME_INVALID_PARAM)
0203                 error(1, 0, "errqueue: unknown EINVAL %u\n",
0204                       err->ee_code);
0205             reason = "invalid txtime";
0206         break;
0207         default:
0208             error(1, 0, "errqueue: errno %u code %u\n",
0209                   err->ee_errno, err->ee_code);
0210         }
0211
0212         tstamp = ((int64_t) err->ee_data) << 32 | err->ee_info;
0213         tstamp -= (int64_t) glob_tstart;
0214         tstamp /= 1000 * 1000;
0215         fprintf(stderr, "send: pkt %c at %" PRId64 "ms dropped: %s\n",
0216             data[ret - 1], tstamp, reason);
0217
0218         msg.msg_flags = 0;
0219         msg.msg_controllen = sizeof(control);
0220         num_tstamp++;
0221     }
0222
0223     return num_tstamp;
0224 }
0225
0226 static void recv_errqueue_msgs(int fdt)
0227 {
0228     struct pollfd pfd = { .fd = fdt, .events = POLLERR };
0229     const int timeout_ms = 10;
0230     int ret, num_tstamp = 0;
0231
0232     do {
0233         ret = poll(&pfd, 1, timeout_ms);
0234         if (ret == -1)
0235             error(1, errno, "poll");
0236
0237         if (ret && (pfd.revents & POLLERR))
0238             num_tstamp += do_recv_errqueue_timeout(fdt);
0239
0240         if (num_tstamp == cfg_num_pkt)
0241             break;
0242
0243     } while (gettime_ns(cfg_clockid) < tdeliver_max);
0244 }
0245
0246 static void start_time_wait(void)
0247 {
0248     uint64_t now;
0249     int err;
0250
0251     if (!cfg_start_time_ns)
0252         return;
0253
0254     now = gettime_ns(CLOCK_REALTIME);
0255     if (cfg_start_time_ns < now)
0256         return;
0257
0258     err = usleep((cfg_start_time_ns - now) / 1000);
0259     if (err)
0260         error(1, errno, "usleep");
0261 }
0262
0263 static void setsockopt_txtime(int fd)
0264 {
0265     struct sock_txtime so_txtime_val = { .clockid = cfg_clockid };
0266     struct sock_txtime so_txtime_val_read = { 0 };
0267     socklen_t vallen = sizeof(so_txtime_val);
0268
0269     so_txtime_val.flags = SOF_TXTIME_REPORT_ERRORS;
0270
0271     if (setsockopt(fd, SOL_SOCKET, SO_TXTIME,
0272                &so_txtime_val, sizeof(so_txtime_val)))
0273         error(1, errno, "setsockopt txtime");
0274
0275     if (getsockopt(fd, SOL_SOCKET, SO_TXTIME,
0276                &so_txtime_val_read, &vallen))
0277         error(1, errno, "getsockopt txtime");
0278
0279     if (vallen != sizeof(so_txtime_val) ||
0280         memcmp(&so_txtime_val, &so_txtime_val_read, vallen))
0281         error(1, 0, "getsockopt txtime: mismatch");
0282 }
0283
0284 static int setup_tx(struct sockaddr *addr, socklen_t alen)
0285 {
0286     int fd;
0287
0288     fd = socket(addr->sa_family, SOCK_DGRAM, 0);
0289     if (fd == -1)
0290         error(1, errno, "socket t");
0291
0292     if (connect(fd, addr, alen))
0293         error(1, errno, "connect");
0294
0295     setsockopt_txtime(fd);
0296
0297     if (cfg_mark &&
0298         setsockopt(fd, SOL_SOCKET, SO_MARK, &cfg_mark, sizeof(cfg_mark)))
0299         error(1, errno, "setsockopt mark");
0300
0301     return fd;
0302 }
0303
0304 static int setup_rx(struct sockaddr *addr, socklen_t alen)
0305 {
0306     struct timeval tv = { .tv_usec = 100 * 1000 };
0307     int fd;
0308
0309     fd = socket(addr->sa_family, SOCK_DGRAM, 0);
0310     if (fd == -1)
0311         error(1, errno, "socket r");
0312
0313     if (bind(fd, addr, alen))
0314         error(1, errno, "bind");
0315
0316     if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)))
0317         error(1, errno, "setsockopt rcv timeout");
0318
0319     return fd;
0320 }
0321
0322 static void do_test_tx(struct sockaddr *addr, socklen_t alen)
0323 {
0324     int fdt, i;
0325
0326     fprintf(stderr, "\nSO_TXTIME ipv%c clock %s\n",
0327             addr->sa_family == PF_INET ? '4' : '6',
0328             cfg_clockid == CLOCK_TAI ? "tai" : "monotonic");
0329
0330     fdt = setup_tx(addr, alen);
0331
0332     start_time_wait();
0333     glob_tstart = gettime_ns(cfg_clockid);
0334
0335     for (i = 0; i < cfg_num_pkt; i++)
0336         do_send_one(fdt, &cfg_buf[i]);
0337
0338     recv_errqueue_msgs(fdt);
0339
0340     if (close(fdt))
0341         error(1, errno, "close t");
0342 }
0343
0344 static void do_test_rx(struct sockaddr *addr, socklen_t alen)
0345 {
0346     int fdr, i;
0347
0348     fdr = setup_rx(addr, alen);
0349
0350     start_time_wait();
0351     glob_tstart = gettime_ns(cfg_clockid);
0352
0353     for (i = 0; i < cfg_num_pkt; i++)
0354         do_recv_one(fdr, &cfg_buf[i]);
0355
0356     do_recv_verify_empty(fdr);
0357
0358     if (close(fdr))
0359         error(1, errno, "close r");
0360 }
0361
0362 static void setup_sockaddr(int domain, const char *str_addr,
0363                struct sockaddr_storage *sockaddr)
0364 {
0365     struct sockaddr_in6 *addr6 = (void *) sockaddr;
0366     struct sockaddr_in *addr4 = (void *) sockaddr;
0367
0368     switch (domain) {
0369     case PF_INET:
0370         memset(addr4, 0, sizeof(*addr4));
0371         addr4->sin_family = AF_INET;
0372         addr4->sin_port = htons(cfg_port);
0373         if (str_addr &&
0374             inet_pton(AF_INET, str_addr, &(addr4->sin_addr)) != 1)
0375             error(1, 0, "ipv4 parse error: %s", str_addr);
0376         break;
0377     case PF_INET6:
0378         memset(addr6, 0, sizeof(*addr6));
0379         addr6->sin6_family = AF_INET6;
0380         addr6->sin6_port = htons(cfg_port);
0381         if (str_addr &&
0382             inet_pton(AF_INET6, str_addr, &(addr6->sin6_addr)) != 1)
0383             error(1, 0, "ipv6 parse error: %s", str_addr);
0384         break;
0385     }
0386 }
0387
0388 static int parse_io(const char *optarg, struct timed_send *array)
0389 {
0390     char *arg, *tok;
0391     int aoff = 0;
0392
0393     arg = strdup(optarg);
0394     if (!arg)
0395         error(1, errno, "strdup");
0396
0397     while ((tok = strtok(arg, ","))) {
0398         arg = NULL; /* only pass non-zero on first call */
0399
0400         if (aoff / 2 == MAX_NUM_PKT)
0401             error(1, 0, "exceeds max pkt count (%d)", MAX_NUM_PKT);
0402
0403         if (aoff & 1) { /* parse delay */
0404             array->delay_us = strtol(tok, NULL, 0) * 1000;
0405             array++;
0406         } else {    /* parse character */
0407             array->data = tok[0];
0408         }
0409
0410         aoff++;
0411     }
0412
0413     free(arg);
0414
0415     return aoff / 2;
0416 }
0417
0418 static void usage(const char *progname)
0419 {
0420     fprintf(stderr, "\nUsage: %s [options] <payload>\n"
0421             "Options:\n"
0422             "  -4            only IPv4\n"
0423             "  -6            only IPv6\n"
0424             "  -c <clock>    monotonic or tai (default)\n"
0425             "  -D <addr>     destination IP address (server)\n"
0426             "  -S <addr>     source IP address (client)\n"
0427             "  -r            run rx mode\n"
0428             "  -t <nsec>     start time (UTC nanoseconds)\n"
0429             "  -m <mark>     socket mark\n"
0430             "\n",
0431             progname);
0432     exit(1);
0433 }
0434
0435 static void parse_opts(int argc, char **argv)
0436 {
0437     char *daddr = NULL, *saddr = NULL;
0438     int domain = PF_UNSPEC;
0439     int c;
0440
0441     while ((c = getopt(argc, argv, "46c:S:D:rt:m:")) != -1) {
0442         switch (c) {
0443         case '4':
0444             if (domain != PF_UNSPEC)
0445                 error(1, 0, "Pass one of -4 or -6");
0446             domain = PF_INET;
0447             cfg_alen = sizeof(struct sockaddr_in);
0448             cfg_errq_level = SOL_IP;
0449             cfg_errq_type = IP_RECVERR;
0450             break;
0451         case '6':
0452             if (domain != PF_UNSPEC)
0453                 error(1, 0, "Pass one of -4 or -6");
0454             domain = PF_INET6;
0455             cfg_alen = sizeof(struct sockaddr_in6);
0456             cfg_errq_level = SOL_IPV6;
0457             cfg_errq_type = IPV6_RECVERR;
0458             break;
0459         case 'c':
0460             if (!strcmp(optarg, "tai"))
0461                 cfg_clockid = CLOCK_TAI;
0462             else if (!strcmp(optarg, "monotonic") ||
0463                  !strcmp(optarg, "mono"))
0464                 cfg_clockid = CLOCK_MONOTONIC;
0465             else
0466                 error(1, 0, "unknown clock id %s", optarg);
0467             break;
0468         case 'S':
0469             saddr = optarg;
0470             break;
0471         case 'D':
0472             daddr = optarg;
0473             break;
0474         case 'r':
0475             cfg_rx = true;
0476             break;
0477         case 't':
0478             cfg_start_time_ns = strtoll(optarg, NULL, 0);
0479             break;
0480         case 'm':
0481             cfg_mark = strtol(optarg, NULL, 0);
0482             break;
0483         default:
0484             usage(argv[0]);
0485         }
0486     }
0487
0488     if (argc - optind != 1)
0489         usage(argv[0]);
0490
0491     if (domain == PF_UNSPEC)
0492         error(1, 0, "Pass one of -4 or -6");
0493     if (!daddr)
0494         error(1, 0, "-D <server addr> required\n");
0495     if (!cfg_rx && !saddr)
0496         error(1, 0, "-S <client addr> required\n");
0497
0498     setup_sockaddr(domain, daddr, &cfg_dst_addr);
0499     setup_sockaddr(domain, saddr, &cfg_src_addr);
0500
0501     cfg_num_pkt = parse_io(argv[optind], cfg_buf);
0502 }
0503
0504 int main(int argc, char **argv)
0505 {
0506     parse_opts(argc, argv);
0507
0508     if (cfg_rx)
0509         do_test_rx((void *)&cfg_dst_addr, cfg_alen);
0510     else
0511         do_test_tx((void *)&cfg_src_addr, cfg_alen);
0512
0513     return 0;
0514 }