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