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