518 lines · c
1// SPDX-License-Identifier: GPL-2.02/*3 * Test the SO_TXTIME API4 *5 * Takes a stream of { payload, delivery time }[], to be sent across two6 * processes. Start this program on two separate network namespaces or7 * connected hosts, one instance in transmit mode and the other in receive8 * mode using the '-r' option. Receiver will compare arrival timestamps to9 * the expected stream. Sender will read transmit timestamps from the error10 * queue. The streams can differ due to out-of-order delivery and drops.11 */12 13#define _GNU_SOURCE14 15#include <arpa/inet.h>16#include <error.h>17#include <errno.h>18#include <inttypes.h>19#include <linux/net_tstamp.h>20#include <linux/errqueue.h>21#include <linux/if_ether.h>22#include <linux/ipv6.h>23#include <linux/udp.h>24#include <stdbool.h>25#include <stdlib.h>26#include <stdio.h>27#include <string.h>28#include <sys/socket.h>29#include <sys/stat.h>30#include <sys/time.h>31#include <sys/types.h>32#include <time.h>33#include <unistd.h>34#include <poll.h>35 36static int cfg_clockid = CLOCK_TAI;37static uint16_t cfg_port = 8000;38static int cfg_variance_us = 4000;39static uint64_t cfg_start_time_ns;40static int cfg_mark;41static bool cfg_rx;42 43static uint64_t glob_tstart;44static uint64_t tdeliver_max;45 46/* encode one timed transmission (of a 1B payload) */47struct timed_send {48 char data;49 int64_t delay_us;50};51 52#define MAX_NUM_PKT 853static struct timed_send cfg_buf[MAX_NUM_PKT];54static int cfg_num_pkt;55 56static int cfg_errq_level;57static int cfg_errq_type;58 59static struct sockaddr_storage cfg_dst_addr;60static struct sockaddr_storage cfg_src_addr;61static socklen_t cfg_alen;62 63static uint64_t gettime_ns(clockid_t clock)64{65 struct timespec ts;66 67 if (clock_gettime(clock, &ts))68 error(1, errno, "gettime");69 70 return ts.tv_sec * (1000ULL * 1000 * 1000) + ts.tv_nsec;71}72 73static void do_send_one(int fdt, struct timed_send *ts)74{75 char control[CMSG_SPACE(sizeof(uint64_t))];76 struct msghdr msg = {0};77 struct iovec iov = {0};78 struct cmsghdr *cm;79 uint64_t tdeliver;80 int ret;81 82 iov.iov_base = &ts->data;83 iov.iov_len = 1;84 85 msg.msg_iov = &iov;86 msg.msg_iovlen = 1;87 msg.msg_name = (struct sockaddr *)&cfg_dst_addr;88 msg.msg_namelen = cfg_alen;89 90 if (ts->delay_us >= 0) {91 memset(control, 0, sizeof(control));92 msg.msg_control = &control;93 msg.msg_controllen = sizeof(control);94 95 tdeliver = glob_tstart + ts->delay_us * 1000;96 tdeliver_max = tdeliver_max > tdeliver ?97 tdeliver_max : tdeliver;98 99 cm = CMSG_FIRSTHDR(&msg);100 cm->cmsg_level = SOL_SOCKET;101 cm->cmsg_type = SCM_TXTIME;102 cm->cmsg_len = CMSG_LEN(sizeof(tdeliver));103 memcpy(CMSG_DATA(cm), &tdeliver, sizeof(tdeliver));104 }105 106 ret = sendmsg(fdt, &msg, 0);107 if (ret == -1)108 error(1, errno, "write");109 if (ret == 0)110 error(1, 0, "write: 0B");111 112}113 114static void do_recv_one(int fdr, struct timed_send *ts)115{116 int64_t tstop, texpect;117 char rbuf[2];118 int ret;119 120 ret = recv(fdr, rbuf, sizeof(rbuf), 0);121 if (ret == -1 && errno == EAGAIN)122 error(1, EAGAIN, "recv: timeout");123 if (ret == -1)124 error(1, errno, "read");125 if (ret != 1)126 error(1, 0, "read: %dB", ret);127 128 tstop = (gettime_ns(cfg_clockid) - glob_tstart) / 1000;129 texpect = ts->delay_us >= 0 ? ts->delay_us : 0;130 131 fprintf(stderr, "payload:%c delay:%lld expected:%lld (us)\n",132 rbuf[0], (long long)tstop, (long long)texpect);133 134 if (rbuf[0] != ts->data)135 error(1, 0, "payload mismatch. expected %c", ts->data);136 137 if (llabs(tstop - texpect) > cfg_variance_us) {138 fprintf(stderr, "exceeds variance (%d us)\n", cfg_variance_us);139 if (!getenv("KSFT_MACHINE_SLOW"))140 exit(1);141 }142}143 144static void do_recv_verify_empty(int fdr)145{146 char rbuf[1];147 int ret;148 149 ret = recv(fdr, rbuf, sizeof(rbuf), 0);150 if (ret != -1 || errno != EAGAIN)151 error(1, 0, "recv: not empty as expected (%d, %d)", ret, errno);152}153 154static int do_recv_errqueue_timeout(int fdt)155{156 char control[CMSG_SPACE(sizeof(struct sock_extended_err)) +157 CMSG_SPACE(sizeof(struct sockaddr_in6))] = {0};158 char data[sizeof(struct ethhdr) + sizeof(struct ipv6hdr) +159 sizeof(struct udphdr) + 1];160 struct sock_extended_err *err;161 int ret, num_tstamp = 0;162 struct msghdr msg = {0};163 struct iovec iov = {0};164 struct cmsghdr *cm;165 int64_t tstamp = 0;166 167 iov.iov_base = data;168 iov.iov_len = sizeof(data);169 170 msg.msg_iov = &iov;171 msg.msg_iovlen = 1;172 173 msg.msg_control = control;174 msg.msg_controllen = sizeof(control);175 176 while (1) {177 const char *reason;178 179 ret = recvmsg(fdt, &msg, MSG_ERRQUEUE);180 if (ret == -1 && errno == EAGAIN)181 break;182 if (ret == -1)183 error(1, errno, "errqueue");184 if (msg.msg_flags != MSG_ERRQUEUE)185 error(1, 0, "errqueue: flags 0x%x\n", msg.msg_flags);186 187 cm = CMSG_FIRSTHDR(&msg);188 if (cm->cmsg_level != cfg_errq_level ||189 cm->cmsg_type != cfg_errq_type)190 error(1, 0, "errqueue: type 0x%x.0x%x\n",191 cm->cmsg_level, cm->cmsg_type);192 193 err = (struct sock_extended_err *)CMSG_DATA(cm);194 if (err->ee_origin != SO_EE_ORIGIN_TXTIME)195 error(1, 0, "errqueue: origin 0x%x\n", err->ee_origin);196 197 switch (err->ee_errno) {198 case ECANCELED:199 if (err->ee_code != SO_EE_CODE_TXTIME_MISSED)200 error(1, 0, "errqueue: unknown ECANCELED %u\n",201 err->ee_code);202 reason = "missed txtime";203 break;204 case EINVAL:205 if (err->ee_code != SO_EE_CODE_TXTIME_INVALID_PARAM)206 error(1, 0, "errqueue: unknown EINVAL %u\n",207 err->ee_code);208 reason = "invalid txtime";209 break;210 default:211 error(1, 0, "errqueue: errno %u code %u\n",212 err->ee_errno, err->ee_code);213 }214 215 tstamp = ((int64_t) err->ee_data) << 32 | err->ee_info;216 tstamp -= (int64_t) glob_tstart;217 tstamp /= 1000 * 1000;218 fprintf(stderr, "send: pkt %c at %" PRId64 "ms dropped: %s\n",219 data[ret - 1], tstamp, reason);220 221 msg.msg_flags = 0;222 msg.msg_controllen = sizeof(control);223 num_tstamp++;224 }225 226 return num_tstamp;227}228 229static void recv_errqueue_msgs(int fdt)230{231 struct pollfd pfd = { .fd = fdt, .events = POLLERR };232 const int timeout_ms = 10;233 int ret, num_tstamp = 0;234 235 do {236 ret = poll(&pfd, 1, timeout_ms);237 if (ret == -1)238 error(1, errno, "poll");239 240 if (ret && (pfd.revents & POLLERR))241 num_tstamp += do_recv_errqueue_timeout(fdt);242 243 if (num_tstamp == cfg_num_pkt)244 break;245 246 } while (gettime_ns(cfg_clockid) < tdeliver_max);247}248 249static void start_time_wait(void)250{251 uint64_t now;252 int err;253 254 if (!cfg_start_time_ns)255 return;256 257 now = gettime_ns(CLOCK_REALTIME);258 if (cfg_start_time_ns < now)259 return;260 261 err = usleep((cfg_start_time_ns - now) / 1000);262 if (err)263 error(1, errno, "usleep");264}265 266static void setsockopt_txtime(int fd)267{268 struct sock_txtime so_txtime_val = { .clockid = cfg_clockid };269 struct sock_txtime so_txtime_val_read = { 0 };270 socklen_t vallen = sizeof(so_txtime_val);271 272 so_txtime_val.flags = SOF_TXTIME_REPORT_ERRORS;273 274 if (setsockopt(fd, SOL_SOCKET, SO_TXTIME,275 &so_txtime_val, sizeof(so_txtime_val)))276 error(1, errno, "setsockopt txtime");277 278 if (getsockopt(fd, SOL_SOCKET, SO_TXTIME,279 &so_txtime_val_read, &vallen))280 error(1, errno, "getsockopt txtime");281 282 if (vallen != sizeof(so_txtime_val) ||283 memcmp(&so_txtime_val, &so_txtime_val_read, vallen))284 error(1, 0, "getsockopt txtime: mismatch");285}286 287static int setup_tx(struct sockaddr *addr, socklen_t alen)288{289 int fd;290 291 fd = socket(addr->sa_family, SOCK_DGRAM, 0);292 if (fd == -1)293 error(1, errno, "socket t");294 295 if (connect(fd, addr, alen))296 error(1, errno, "connect");297 298 setsockopt_txtime(fd);299 300 if (cfg_mark &&301 setsockopt(fd, SOL_SOCKET, SO_MARK, &cfg_mark, sizeof(cfg_mark)))302 error(1, errno, "setsockopt mark");303 304 return fd;305}306 307static int setup_rx(struct sockaddr *addr, socklen_t alen)308{309 struct timeval tv = { .tv_usec = 100 * 1000 };310 int fd;311 312 fd = socket(addr->sa_family, SOCK_DGRAM, 0);313 if (fd == -1)314 error(1, errno, "socket r");315 316 if (bind(fd, addr, alen))317 error(1, errno, "bind");318 319 if (setsockopt(fd, SOL_SOCKET, SO_RCVTIMEO, &tv, sizeof(tv)))320 error(1, errno, "setsockopt rcv timeout");321 322 return fd;323}324 325static void do_test_tx(struct sockaddr *addr, socklen_t alen)326{327 int fdt, i;328 329 fprintf(stderr, "\nSO_TXTIME ipv%c clock %s\n",330 addr->sa_family == PF_INET ? '4' : '6',331 cfg_clockid == CLOCK_TAI ? "tai" : "monotonic");332 333 fdt = setup_tx(addr, alen);334 335 start_time_wait();336 glob_tstart = gettime_ns(cfg_clockid);337 338 for (i = 0; i < cfg_num_pkt; i++)339 do_send_one(fdt, &cfg_buf[i]);340 341 recv_errqueue_msgs(fdt);342 343 if (close(fdt))344 error(1, errno, "close t");345}346 347static void do_test_rx(struct sockaddr *addr, socklen_t alen)348{349 int fdr, i;350 351 fdr = setup_rx(addr, alen);352 353 start_time_wait();354 glob_tstart = gettime_ns(cfg_clockid);355 356 for (i = 0; i < cfg_num_pkt; i++)357 do_recv_one(fdr, &cfg_buf[i]);358 359 do_recv_verify_empty(fdr);360 361 if (close(fdr))362 error(1, errno, "close r");363}364 365static void setup_sockaddr(int domain, const char *str_addr,366 struct sockaddr_storage *sockaddr)367{368 struct sockaddr_in6 *addr6 = (void *) sockaddr;369 struct sockaddr_in *addr4 = (void *) sockaddr;370 371 switch (domain) {372 case PF_INET:373 memset(addr4, 0, sizeof(*addr4));374 addr4->sin_family = AF_INET;375 addr4->sin_port = htons(cfg_port);376 if (str_addr &&377 inet_pton(AF_INET, str_addr, &(addr4->sin_addr)) != 1)378 error(1, 0, "ipv4 parse error: %s", str_addr);379 break;380 case PF_INET6:381 memset(addr6, 0, sizeof(*addr6));382 addr6->sin6_family = AF_INET6;383 addr6->sin6_port = htons(cfg_port);384 if (str_addr &&385 inet_pton(AF_INET6, str_addr, &(addr6->sin6_addr)) != 1)386 error(1, 0, "ipv6 parse error: %s", str_addr);387 break;388 }389}390 391static int parse_io(const char *optarg, struct timed_send *array)392{393 char *arg, *tok;394 int aoff = 0;395 396 arg = strdup(optarg);397 if (!arg)398 error(1, errno, "strdup");399 400 while ((tok = strtok(arg, ","))) {401 arg = NULL; /* only pass non-zero on first call */402 403 if (aoff / 2 == MAX_NUM_PKT)404 error(1, 0, "exceeds max pkt count (%d)", MAX_NUM_PKT);405 406 if (aoff & 1) { /* parse delay */407 array->delay_us = strtol(tok, NULL, 0) * 1000;408 array++;409 } else { /* parse character */410 array->data = tok[0];411 }412 413 aoff++;414 }415 416 free(arg);417 418 return aoff / 2;419}420 421static void usage(const char *progname)422{423 fprintf(stderr, "\nUsage: %s [options] <payload>\n"424 "Options:\n"425 " -4 only IPv4\n"426 " -6 only IPv6\n"427 " -c <clock> monotonic or tai (default)\n"428 " -D <addr> destination IP address (server)\n"429 " -S <addr> source IP address (client)\n"430 " -r run rx mode\n"431 " -t <nsec> start time (UTC nanoseconds)\n"432 " -m <mark> socket mark\n"433 "\n",434 progname);435 exit(1);436}437 438static void parse_opts(int argc, char **argv)439{440 char *daddr = NULL, *saddr = NULL;441 int domain = PF_UNSPEC;442 int c;443 444 while ((c = getopt(argc, argv, "46c:S:D:rt:m:")) != -1) {445 switch (c) {446 case '4':447 if (domain != PF_UNSPEC)448 error(1, 0, "Pass one of -4 or -6");449 domain = PF_INET;450 cfg_alen = sizeof(struct sockaddr_in);451 cfg_errq_level = SOL_IP;452 cfg_errq_type = IP_RECVERR;453 break;454 case '6':455 if (domain != PF_UNSPEC)456 error(1, 0, "Pass one of -4 or -6");457 domain = PF_INET6;458 cfg_alen = sizeof(struct sockaddr_in6);459 cfg_errq_level = SOL_IPV6;460 cfg_errq_type = IPV6_RECVERR;461 break;462 case 'c':463 if (!strcmp(optarg, "tai"))464 cfg_clockid = CLOCK_TAI;465 else if (!strcmp(optarg, "monotonic") ||466 !strcmp(optarg, "mono"))467 cfg_clockid = CLOCK_MONOTONIC;468 else469 error(1, 0, "unknown clock id %s", optarg);470 break;471 case 'S':472 saddr = optarg;473 break;474 case 'D':475 daddr = optarg;476 break;477 case 'r':478 cfg_rx = true;479 break;480 case 't':481 cfg_start_time_ns = strtoll(optarg, NULL, 0);482 break;483 case 'm':484 cfg_mark = strtol(optarg, NULL, 0);485 break;486 default:487 usage(argv[0]);488 }489 }490 491 if (argc - optind != 1)492 usage(argv[0]);493 494 if (domain == PF_UNSPEC)495 error(1, 0, "Pass one of -4 or -6");496 if (!daddr)497 error(1, 0, "-D <server addr> required\n");498 if (!cfg_rx && !saddr)499 error(1, 0, "-S <client addr> required\n");500 501 setup_sockaddr(domain, daddr, &cfg_dst_addr);502 setup_sockaddr(domain, saddr, &cfg_src_addr);503 504 cfg_num_pkt = parse_io(argv[optind], cfg_buf);505}506 507int main(int argc, char **argv)508{509 parse_opts(argc, argv);510 511 if (cfg_rx)512 do_test_rx((void *)&cfg_dst_addr, cfg_alen);513 else514 do_test_tx((void *)&cfg_src_addr, cfg_alen);515 516 return 0;517}518