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