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