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 { 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 85 static struct timespec time_start_real; 86 static struct timespec time_start_mono; 87 88 static 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 114 static 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 212 static 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 224 static void 225 ca_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 244 static void 245 cs_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 else 306 msg->msg_control = NULL; 307 } 308 309 static 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 322 static unsigned long 323 cs_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 393 static 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 435 int 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 532 err_out: 533 close(fd); 534 freeaddrinfo(ai); 535 return err; 536 } 537