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