537 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2#include <errno.h>3#include <error.h>4#include <netdb.h>5#include <stdbool.h>6#include <stdio.h>7#include <stdlib.h>8#include <string.h>9#include <time.h>10#include <unistd.h>11#include <linux/errqueue.h>12#include <linux/icmp.h>13#include <linux/icmpv6.h>14#include <linux/net_tstamp.h>15#include <linux/types.h>16#include <linux/udp.h>17#include <sys/socket.h>18 19#include "../kselftest.h"20 21enum {22 ERN_SUCCESS = 0,23 /* Well defined errors, callers may depend on these */24 ERN_SEND = 1,25 /* Informational, can reorder */26 ERN_HELP,27 ERN_SEND_SHORT,28 ERN_SOCK_CREATE,29 ERN_RESOLVE,30 ERN_CMSG_WR,31 ERN_SOCKOPT,32 ERN_GETTIME,33 ERN_RECVERR,34 ERN_CMSG_RD,35 ERN_CMSG_RCV,36};37 38struct option_cmsg_u32 {39 bool ena;40 unsigned int val;41};42 43struct options {44 bool silent_send;45 const char *host;46 const char *service;47 unsigned int size;48 unsigned int num_pkt;49 struct {50 unsigned int mark;51 unsigned int dontfrag;52 unsigned int tclass;53 unsigned int hlimit;54 unsigned int priority;55 } sockopt;56 struct {57 unsigned int family;58 unsigned int type;59 unsigned int proto;60 } sock;61 struct option_cmsg_u32 mark;62 struct {63 bool ena;64 unsigned int delay;65 } txtime;66 struct {67 bool ena;68 } ts;69 struct {70 struct option_cmsg_u32 dontfrag;71 struct option_cmsg_u32 tclass;72 struct option_cmsg_u32 hlimit;73 struct option_cmsg_u32 exthdr;74 } v6;75} opt = {76 .size = 13,77 .num_pkt = 1,78 .sock = {79 .family = AF_UNSPEC,80 .type = SOCK_DGRAM,81 .proto = IPPROTO_UDP,82 },83};84 85static struct timespec time_start_real;86static struct timespec time_start_mono;87 88static void __attribute__((noreturn)) cs_usage(const char *bin)89{90 printf("Usage: %s [opts] <dst host> <dst port / service>\n", bin);91 printf("Options:\n"92 "\t\t-s Silent send() failures\n"93 "\t\t-S send() size\n"94 "\t\t-4/-6 Force IPv4 / IPv6 only\n"95 "\t\t-p prot Socket protocol\n"96 "\t\t (u = UDP (default); i = ICMP; r = RAW)\n"97 "\n"98 "\t\t-m val Set SO_MARK with given value\n"99 "\t\t-M val Set SO_MARK via setsockopt\n"100 "\t\t-d val Set SO_TXTIME with given delay (usec)\n"101 "\t\t-t Enable time stamp reporting\n"102 "\t\t-f val Set don't fragment via cmsg\n"103 "\t\t-F val Set don't fragment via setsockopt\n"104 "\t\t-c val Set TCLASS via cmsg\n"105 "\t\t-C val Set TCLASS via setsockopt\n"106 "\t\t-l val Set HOPLIMIT via cmsg\n"107 "\t\t-L val Set HOPLIMIT via setsockopt\n"108 "\t\t-H type Add an IPv6 header option\n"109 "\t\t (h = HOP; d = DST; r = RTDST)"110 "");111 exit(ERN_HELP);112}113 114static void cs_parse_args(int argc, char *argv[])115{116 int o;117 118 while ((o = getopt(argc, argv, "46sS:p:P:m:M:n:d:tf:F:c:C:l:L:H:")) != -1) {119 switch (o) {120 case 's':121 opt.silent_send = true;122 break;123 case 'S':124 opt.size = atoi(optarg);125 break;126 case '4':127 opt.sock.family = AF_INET;128 break;129 case '6':130 opt.sock.family = AF_INET6;131 break;132 case 'p':133 if (*optarg == 'u' || *optarg == 'U') {134 opt.sock.proto = IPPROTO_UDP;135 } else if (*optarg == 'i' || *optarg == 'I') {136 opt.sock.proto = IPPROTO_ICMP;137 } else if (*optarg == 'r') {138 opt.sock.type = SOCK_RAW;139 } else {140 printf("Error: unknown protocol: %s\n", optarg);141 cs_usage(argv[0]);142 }143 break;144 case 'P':145 opt.sockopt.priority = atoi(optarg);146 break;147 case 'm':148 opt.mark.ena = true;149 opt.mark.val = atoi(optarg);150 break;151 case 'M':152 opt.sockopt.mark = atoi(optarg);153 break;154 case 'n':155 opt.num_pkt = atoi(optarg);156 break;157 case 'd':158 opt.txtime.ena = true;159 opt.txtime.delay = atoi(optarg);160 break;161 case 't':162 opt.ts.ena = true;163 break;164 case 'f':165 opt.v6.dontfrag.ena = true;166 opt.v6.dontfrag.val = atoi(optarg);167 break;168 case 'F':169 opt.sockopt.dontfrag = atoi(optarg);170 break;171 case 'c':172 opt.v6.tclass.ena = true;173 opt.v6.tclass.val = atoi(optarg);174 break;175 case 'C':176 opt.sockopt.tclass = atoi(optarg);177 break;178 case 'l':179 opt.v6.hlimit.ena = true;180 opt.v6.hlimit.val = atoi(optarg);181 break;182 case 'L':183 opt.sockopt.hlimit = atoi(optarg);184 break;185 case 'H':186 opt.v6.exthdr.ena = true;187 switch (optarg[0]) {188 case 'h':189 opt.v6.exthdr.val = IPV6_HOPOPTS;190 break;191 case 'd':192 opt.v6.exthdr.val = IPV6_DSTOPTS;193 break;194 case 'r':195 opt.v6.exthdr.val = IPV6_RTHDRDSTOPTS;196 break;197 default:198 printf("Error: hdr type: %s\n", optarg);199 break;200 }201 break;202 }203 }204 205 if (optind != argc - 2)206 cs_usage(argv[0]);207 208 opt.host = argv[optind];209 opt.service = argv[optind + 1];210}211 212static void memrnd(void *s, size_t n)213{214 int *dword = s;215 char *byte;216 217 for (; n >= 4; n -= 4)218 *dword++ = rand();219 byte = (void *)dword;220 while (n--)221 *byte++ = rand();222}223 224static void225ca_write_cmsg_u32(char *cbuf, size_t cbuf_sz, size_t *cmsg_len,226 int level, int optname, struct option_cmsg_u32 *uopt)227{228 struct cmsghdr *cmsg;229 230 if (!uopt->ena)231 return;232 233 cmsg = (struct cmsghdr *)(cbuf + *cmsg_len);234 *cmsg_len += CMSG_SPACE(sizeof(__u32));235 if (cbuf_sz < *cmsg_len)236 error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");237 238 cmsg->cmsg_level = level;239 cmsg->cmsg_type = optname;240 cmsg->cmsg_len = CMSG_LEN(sizeof(__u32));241 *(__u32 *)CMSG_DATA(cmsg) = uopt->val;242}243 244static void245cs_write_cmsg(int fd, struct msghdr *msg, char *cbuf, size_t cbuf_sz)246{247 struct cmsghdr *cmsg;248 size_t cmsg_len;249 250 msg->msg_control = cbuf;251 cmsg_len = 0;252 253 ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,254 SOL_SOCKET, SO_MARK, &opt.mark);255 ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,256 SOL_IPV6, IPV6_DONTFRAG, &opt.v6.dontfrag);257 ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,258 SOL_IPV6, IPV6_TCLASS, &opt.v6.tclass);259 ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,260 SOL_IPV6, IPV6_HOPLIMIT, &opt.v6.hlimit);261 262 if (opt.txtime.ena) {263 __u64 txtime;264 265 txtime = time_start_mono.tv_sec * (1000ULL * 1000 * 1000) +266 time_start_mono.tv_nsec +267 opt.txtime.delay * 1000;268 269 cmsg = (struct cmsghdr *)(cbuf + cmsg_len);270 cmsg_len += CMSG_SPACE(sizeof(txtime));271 if (cbuf_sz < cmsg_len)272 error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");273 274 cmsg->cmsg_level = SOL_SOCKET;275 cmsg->cmsg_type = SCM_TXTIME;276 cmsg->cmsg_len = CMSG_LEN(sizeof(txtime));277 memcpy(CMSG_DATA(cmsg), &txtime, sizeof(txtime));278 }279 if (opt.ts.ena) {280 cmsg = (struct cmsghdr *)(cbuf + cmsg_len);281 cmsg_len += CMSG_SPACE(sizeof(__u32));282 if (cbuf_sz < cmsg_len)283 error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");284 285 cmsg->cmsg_level = SOL_SOCKET;286 cmsg->cmsg_type = SO_TIMESTAMPING;287 cmsg->cmsg_len = CMSG_LEN(sizeof(__u32));288 *(__u32 *)CMSG_DATA(cmsg) = SOF_TIMESTAMPING_TX_SCHED |289 SOF_TIMESTAMPING_TX_SOFTWARE;290 }291 if (opt.v6.exthdr.ena) {292 cmsg = (struct cmsghdr *)(cbuf + cmsg_len);293 cmsg_len += CMSG_SPACE(8);294 if (cbuf_sz < cmsg_len)295 error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");296 297 cmsg->cmsg_level = SOL_IPV6;298 cmsg->cmsg_type = opt.v6.exthdr.val;299 cmsg->cmsg_len = CMSG_LEN(8);300 *(__u64 *)CMSG_DATA(cmsg) = 0;301 }302 303 if (cmsg_len)304 msg->msg_controllen = cmsg_len;305 else306 msg->msg_control = NULL;307}308 309static const char *cs_ts_info2str(unsigned int info)310{311 static const char *names[] = {312 [SCM_TSTAMP_SND] = "SND",313 [SCM_TSTAMP_SCHED] = "SCHED",314 [SCM_TSTAMP_ACK] = "ACK",315 };316 317 if (info < ARRAY_SIZE(names))318 return names[info];319 return "unknown";320}321 322static unsigned long323cs_read_cmsg(int fd, struct msghdr *msg, char *cbuf, size_t cbuf_sz)324{325 struct sock_extended_err *see;326 struct scm_timestamping *ts;327 unsigned long ts_seen = 0;328 struct cmsghdr *cmsg;329 int i, err;330 331 if (!opt.ts.ena)332 return 0;333 msg->msg_control = cbuf;334 msg->msg_controllen = cbuf_sz;335 336 while (true) {337 ts = NULL;338 see = NULL;339 memset(cbuf, 0, cbuf_sz);340 341 err = recvmsg(fd, msg, MSG_ERRQUEUE);342 if (err < 0) {343 if (errno == EAGAIN)344 break;345 error(ERN_RECVERR, errno, "recvmsg ERRQ");346 }347 348 for (cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL;349 cmsg = CMSG_NXTHDR(msg, cmsg)) {350 if (cmsg->cmsg_level == SOL_SOCKET &&351 cmsg->cmsg_type == SO_TIMESTAMPING_OLD) {352 if (cmsg->cmsg_len < sizeof(*ts))353 error(ERN_CMSG_RD, EINVAL, "TS cmsg");354 355 ts = (void *)CMSG_DATA(cmsg);356 }357 if ((cmsg->cmsg_level == SOL_IP &&358 cmsg->cmsg_type == IP_RECVERR) ||359 (cmsg->cmsg_level == SOL_IPV6 &&360 cmsg->cmsg_type == IPV6_RECVERR)) {361 if (cmsg->cmsg_len < sizeof(*see))362 error(ERN_CMSG_RD, EINVAL, "sock_err cmsg");363 364 see = (void *)CMSG_DATA(cmsg);365 }366 }367 368 if (!ts)369 error(ERN_CMSG_RCV, ENOENT, "TS cmsg not found");370 if (!see)371 error(ERN_CMSG_RCV, ENOENT, "sock_err cmsg not found");372 373 for (i = 0; i < 3; i++) {374 unsigned long long rel_time;375 376 if (!ts->ts[i].tv_sec && !ts->ts[i].tv_nsec)377 continue;378 379 rel_time = (ts->ts[i].tv_sec - time_start_real.tv_sec) *380 (1000ULL * 1000) +381 (ts->ts[i].tv_nsec - time_start_real.tv_nsec) /382 1000;383 printf(" %5s ts%d %lluus\n",384 cs_ts_info2str(see->ee_info),385 i, rel_time);386 ts_seen |= 1 << see->ee_info;387 }388 }389 390 return ts_seen;391}392 393static void ca_set_sockopts(int fd)394{395 if (opt.sockopt.mark &&396 setsockopt(fd, SOL_SOCKET, SO_MARK,397 &opt.sockopt.mark, sizeof(opt.sockopt.mark)))398 error(ERN_SOCKOPT, errno, "setsockopt SO_MARK");399 if (opt.sockopt.dontfrag &&400 setsockopt(fd, SOL_IPV6, IPV6_DONTFRAG,401 &opt.sockopt.dontfrag, sizeof(opt.sockopt.dontfrag)))402 error(ERN_SOCKOPT, errno, "setsockopt IPV6_DONTFRAG");403 if (opt.sockopt.tclass &&404 setsockopt(fd, SOL_IPV6, IPV6_TCLASS,405 &opt.sockopt.tclass, sizeof(opt.sockopt.tclass)))406 error(ERN_SOCKOPT, errno, "setsockopt IPV6_TCLASS");407 if (opt.sockopt.hlimit &&408 setsockopt(fd, SOL_IPV6, IPV6_UNICAST_HOPS,409 &opt.sockopt.hlimit, sizeof(opt.sockopt.hlimit)))410 error(ERN_SOCKOPT, errno, "setsockopt IPV6_HOPLIMIT");411 if (opt.sockopt.priority &&412 setsockopt(fd, SOL_SOCKET, SO_PRIORITY,413 &opt.sockopt.priority, sizeof(opt.sockopt.priority)))414 error(ERN_SOCKOPT, errno, "setsockopt SO_PRIORITY");415 416 if (opt.txtime.ena) {417 struct sock_txtime so_txtime = {418 .clockid = CLOCK_MONOTONIC,419 };420 421 if (setsockopt(fd, SOL_SOCKET, SO_TXTIME,422 &so_txtime, sizeof(so_txtime)))423 error(ERN_SOCKOPT, errno, "setsockopt TXTIME");424 }425 if (opt.ts.ena) {426 __u32 val = SOF_TIMESTAMPING_SOFTWARE |427 SOF_TIMESTAMPING_OPT_TSONLY;428 429 if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING,430 &val, sizeof(val)))431 error(ERN_SOCKOPT, errno, "setsockopt TIMESTAMPING");432 }433}434 435int main(int argc, char *argv[])436{437 struct addrinfo hints, *ai;438 struct iovec iov[1];439 unsigned char *buf;440 struct msghdr msg;441 char cbuf[1024];442 int err;443 int fd;444 int i;445 446 cs_parse_args(argc, argv);447 448 buf = malloc(opt.size);449 memrnd(buf, opt.size);450 451 memset(&hints, 0, sizeof(hints));452 hints.ai_family = opt.sock.family;453 454 ai = NULL;455 err = getaddrinfo(opt.host, opt.service, &hints, &ai);456 if (err) {457 fprintf(stderr, "Can't resolve address [%s]:%s\n",458 opt.host, opt.service);459 return ERN_SOCK_CREATE;460 }461 462 if (ai->ai_family == AF_INET6 && opt.sock.proto == IPPROTO_ICMP)463 opt.sock.proto = IPPROTO_ICMPV6;464 465 fd = socket(ai->ai_family, opt.sock.type, opt.sock.proto);466 if (fd < 0) {467 fprintf(stderr, "Can't open socket: %s\n", strerror(errno));468 freeaddrinfo(ai);469 return ERN_RESOLVE;470 }471 472 if (opt.sock.proto == IPPROTO_ICMP) {473 buf[0] = ICMP_ECHO;474 buf[1] = 0;475 } else if (opt.sock.proto == IPPROTO_ICMPV6) {476 buf[0] = ICMPV6_ECHO_REQUEST;477 buf[1] = 0;478 } else if (opt.sock.type == SOCK_RAW) {479 struct udphdr hdr = { 1, 2, htons(opt.size), 0 };480 struct sockaddr_in6 *sin6 = (void *)ai->ai_addr;481 482 memcpy(buf, &hdr, sizeof(hdr));483 sin6->sin6_port = htons(opt.sock.proto);484 }485 486 ca_set_sockopts(fd);487 488 if (clock_gettime(CLOCK_REALTIME, &time_start_real))489 error(ERN_GETTIME, errno, "gettime REALTIME");490 if (clock_gettime(CLOCK_MONOTONIC, &time_start_mono))491 error(ERN_GETTIME, errno, "gettime MONOTONIC");492 493 iov[0].iov_base = buf;494 iov[0].iov_len = opt.size;495 496 memset(&msg, 0, sizeof(msg));497 msg.msg_name = ai->ai_addr;498 msg.msg_namelen = ai->ai_addrlen;499 msg.msg_iov = iov;500 msg.msg_iovlen = 1;501 502 cs_write_cmsg(fd, &msg, cbuf, sizeof(cbuf));503 504 for (i = 0; i < opt.num_pkt; i++) {505 err = sendmsg(fd, &msg, 0);506 if (err < 0) {507 if (!opt.silent_send)508 fprintf(stderr, "send failed: %s\n", strerror(errno));509 err = ERN_SEND;510 goto err_out;511 } else if (err != (int)opt.size) {512 fprintf(stderr, "short send\n");513 err = ERN_SEND_SHORT;514 goto err_out;515 }516 }517 err = ERN_SUCCESS;518 519 if (opt.ts.ena) {520 unsigned long seen;521 int i;522 523 /* Make sure all timestamps have time to loop back */524 for (i = 0; i < 40; i++) {525 seen = cs_read_cmsg(fd, &msg, cbuf, sizeof(cbuf));526 if (seen & (1 << SCM_TSTAMP_SND))527 break;528 usleep(opt.txtime.delay / 20);529 }530 }531 532err_out:533 close(fd);534 freeaddrinfo(ai);535 return err;536}537