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(bin: 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(bin: 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: &cmsg_len, |
254 | SOL_SOCKET, SO_MARK, uopt: &opt.mark); |
255 | ca_write_cmsg_u32(cbuf, cbuf_sz, cmsg_len: &cmsg_len, |
256 | SOL_IPV6, IPV6_DONTFRAG, uopt: &opt.v6.dontfrag); |
257 | ca_write_cmsg_u32(cbuf, cbuf_sz, cmsg_len: &cmsg_len, |
258 | SOL_IPV6, IPV6_TCLASS, uopt: &opt.v6.tclass); |
259 | ca_write_cmsg_u32(cbuf, cbuf_sz, cmsg_len: &cmsg_len, |
260 | SOL_IPV6, IPV6_HOPLIMIT, uopt: &opt.v6.hlimit); |
261 | |
262 | if (opt.txtime.ena) { |
263 | struct sock_txtime so_txtime = { |
264 | .clockid = CLOCK_MONOTONIC, |
265 | }; |
266 | __u64 txtime; |
267 | |
268 | if (setsockopt(fd, SOL_SOCKET, SO_TXTIME, |
269 | &so_txtime, sizeof(so_txtime))) |
270 | error(ERN_SOCKOPT, errno, "setsockopt TXTIME" ); |
271 | |
272 | txtime = time_start_mono.tv_sec * (1000ULL * 1000 * 1000) + |
273 | time_start_mono.tv_nsec + |
274 | opt.txtime.delay * 1000; |
275 | |
276 | cmsg = (struct cmsghdr *)(cbuf + cmsg_len); |
277 | cmsg_len += CMSG_SPACE(sizeof(txtime)); |
278 | if (cbuf_sz < cmsg_len) |
279 | error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small" ); |
280 | |
281 | cmsg->cmsg_level = SOL_SOCKET; |
282 | cmsg->cmsg_type = SCM_TXTIME; |
283 | cmsg->cmsg_len = CMSG_LEN(sizeof(txtime)); |
284 | memcpy(CMSG_DATA(cmsg), &txtime, sizeof(txtime)); |
285 | } |
286 | if (opt.ts.ena) { |
287 | __u32 val = SOF_TIMESTAMPING_SOFTWARE | |
288 | SOF_TIMESTAMPING_OPT_TSONLY; |
289 | |
290 | if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING, |
291 | &val, sizeof(val))) |
292 | error(ERN_SOCKOPT, errno, "setsockopt TIMESTAMPING" ); |
293 | |
294 | cmsg = (struct cmsghdr *)(cbuf + cmsg_len); |
295 | cmsg_len += CMSG_SPACE(sizeof(__u32)); |
296 | if (cbuf_sz < cmsg_len) |
297 | error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small" ); |
298 | |
299 | cmsg->cmsg_level = SOL_SOCKET; |
300 | cmsg->cmsg_type = SO_TIMESTAMPING; |
301 | cmsg->cmsg_len = CMSG_LEN(sizeof(__u32)); |
302 | *(__u32 *)CMSG_DATA(cmsg) = SOF_TIMESTAMPING_TX_SCHED | |
303 | SOF_TIMESTAMPING_TX_SOFTWARE; |
304 | } |
305 | if (opt.v6.exthdr.ena) { |
306 | cmsg = (struct cmsghdr *)(cbuf + cmsg_len); |
307 | cmsg_len += CMSG_SPACE(8); |
308 | if (cbuf_sz < cmsg_len) |
309 | error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small" ); |
310 | |
311 | cmsg->cmsg_level = SOL_IPV6; |
312 | cmsg->cmsg_type = opt.v6.exthdr.val; |
313 | cmsg->cmsg_len = CMSG_LEN(8); |
314 | *(__u64 *)CMSG_DATA(cmsg) = 0; |
315 | } |
316 | |
317 | if (cmsg_len) |
318 | msg->msg_controllen = cmsg_len; |
319 | else |
320 | msg->msg_control = NULL; |
321 | } |
322 | |
323 | static const char *cs_ts_info2str(unsigned int info) |
324 | { |
325 | static const char *names[] = { |
326 | [SCM_TSTAMP_SND] = "SND" , |
327 | [SCM_TSTAMP_SCHED] = "SCHED" , |
328 | [SCM_TSTAMP_ACK] = "ACK" , |
329 | }; |
330 | |
331 | if (info < ARRAY_SIZE(names)) |
332 | return names[info]; |
333 | return "unknown" ; |
334 | } |
335 | |
336 | static void |
337 | cs_read_cmsg(int fd, struct msghdr *msg, char *cbuf, size_t cbuf_sz) |
338 | { |
339 | struct sock_extended_err *see; |
340 | struct scm_timestamping *ts; |
341 | struct cmsghdr *cmsg; |
342 | int i, err; |
343 | |
344 | if (!opt.ts.ena) |
345 | return; |
346 | msg->msg_control = cbuf; |
347 | msg->msg_controllen = cbuf_sz; |
348 | |
349 | while (true) { |
350 | ts = NULL; |
351 | see = NULL; |
352 | memset(cbuf, 0, cbuf_sz); |
353 | |
354 | err = recvmsg(fd, msg, MSG_ERRQUEUE); |
355 | if (err < 0) { |
356 | if (errno == EAGAIN) |
357 | break; |
358 | error(ERN_RECVERR, errno, "recvmsg ERRQ" ); |
359 | } |
360 | |
361 | for (cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL; |
362 | cmsg = CMSG_NXTHDR(msg, cmsg)) { |
363 | if (cmsg->cmsg_level == SOL_SOCKET && |
364 | cmsg->cmsg_type == SO_TIMESTAMPING_OLD) { |
365 | if (cmsg->cmsg_len < sizeof(*ts)) |
366 | error(ERN_CMSG_RD, EINVAL, "TS cmsg" ); |
367 | |
368 | ts = (void *)CMSG_DATA(cmsg); |
369 | } |
370 | if ((cmsg->cmsg_level == SOL_IP && |
371 | cmsg->cmsg_type == IP_RECVERR) || |
372 | (cmsg->cmsg_level == SOL_IPV6 && |
373 | cmsg->cmsg_type == IPV6_RECVERR)) { |
374 | if (cmsg->cmsg_len < sizeof(*see)) |
375 | error(ERN_CMSG_RD, EINVAL, "sock_err cmsg" ); |
376 | |
377 | see = (void *)CMSG_DATA(cmsg); |
378 | } |
379 | } |
380 | |
381 | if (!ts) |
382 | error(ERN_CMSG_RCV, ENOENT, "TS cmsg not found" ); |
383 | if (!see) |
384 | error(ERN_CMSG_RCV, ENOENT, "sock_err cmsg not found" ); |
385 | |
386 | for (i = 0; i < 3; i++) { |
387 | unsigned long long rel_time; |
388 | |
389 | if (!ts->ts[i].tv_sec && !ts->ts[i].tv_nsec) |
390 | continue; |
391 | |
392 | rel_time = (ts->ts[i].tv_sec - time_start_real.tv_sec) * |
393 | (1000ULL * 1000) + |
394 | (ts->ts[i].tv_nsec - time_start_real.tv_nsec) / |
395 | 1000; |
396 | printf(" %5s ts%d %lluus\n" , |
397 | cs_ts_info2str(info: see->ee_info), |
398 | i, rel_time); |
399 | } |
400 | } |
401 | } |
402 | |
403 | static void ca_set_sockopts(int fd) |
404 | { |
405 | if (opt.sockopt.mark && |
406 | setsockopt(fd, SOL_SOCKET, SO_MARK, |
407 | &opt.sockopt.mark, sizeof(opt.sockopt.mark))) |
408 | error(ERN_SOCKOPT, errno, "setsockopt SO_MARK" ); |
409 | if (opt.sockopt.dontfrag && |
410 | setsockopt(fd, SOL_IPV6, IPV6_DONTFRAG, |
411 | &opt.sockopt.dontfrag, sizeof(opt.sockopt.dontfrag))) |
412 | error(ERN_SOCKOPT, errno, "setsockopt IPV6_DONTFRAG" ); |
413 | if (opt.sockopt.tclass && |
414 | setsockopt(fd, SOL_IPV6, IPV6_TCLASS, |
415 | &opt.sockopt.tclass, sizeof(opt.sockopt.tclass))) |
416 | error(ERN_SOCKOPT, errno, "setsockopt IPV6_TCLASS" ); |
417 | if (opt.sockopt.hlimit && |
418 | setsockopt(fd, SOL_IPV6, IPV6_UNICAST_HOPS, |
419 | &opt.sockopt.hlimit, sizeof(opt.sockopt.hlimit))) |
420 | error(ERN_SOCKOPT, errno, "setsockopt IPV6_HOPLIMIT" ); |
421 | if (opt.sockopt.priority && |
422 | setsockopt(fd, SOL_SOCKET, SO_PRIORITY, |
423 | &opt.sockopt.priority, sizeof(opt.sockopt.priority))) |
424 | error(ERN_SOCKOPT, errno, "setsockopt SO_PRIORITY" ); |
425 | } |
426 | |
427 | int main(int argc, char *argv[]) |
428 | { |
429 | struct addrinfo hints, *ai; |
430 | struct iovec iov[1]; |
431 | unsigned char *buf; |
432 | struct msghdr msg; |
433 | char cbuf[1024]; |
434 | int err; |
435 | int fd; |
436 | int i; |
437 | |
438 | cs_parse_args(argc, argv); |
439 | |
440 | buf = malloc(opt.size); |
441 | memrnd(s: buf, n: opt.size); |
442 | |
443 | memset(&hints, 0, sizeof(hints)); |
444 | hints.ai_family = opt.sock.family; |
445 | |
446 | ai = NULL; |
447 | err = getaddrinfo(opt.host, opt.service, &hints, &ai); |
448 | if (err) { |
449 | fprintf(stderr, "Can't resolve address [%s]:%s\n" , |
450 | opt.host, opt.service); |
451 | return ERN_SOCK_CREATE; |
452 | } |
453 | |
454 | if (ai->ai_family == AF_INET6 && opt.sock.proto == IPPROTO_ICMP) |
455 | opt.sock.proto = IPPROTO_ICMPV6; |
456 | |
457 | fd = socket(ai->ai_family, opt.sock.type, opt.sock.proto); |
458 | if (fd < 0) { |
459 | fprintf(stderr, "Can't open socket: %s\n" , strerror(errno)); |
460 | freeaddrinfo(ai); |
461 | return ERN_RESOLVE; |
462 | } |
463 | |
464 | if (opt.sock.proto == IPPROTO_ICMP) { |
465 | buf[0] = ICMP_ECHO; |
466 | buf[1] = 0; |
467 | } else if (opt.sock.proto == IPPROTO_ICMPV6) { |
468 | buf[0] = ICMPV6_ECHO_REQUEST; |
469 | buf[1] = 0; |
470 | } else if (opt.sock.type == SOCK_RAW) { |
471 | struct udphdr hdr = { 1, 2, htons(opt.size), 0 }; |
472 | struct sockaddr_in6 *sin6 = (void *)ai->ai_addr; |
473 | |
474 | memcpy(buf, &hdr, sizeof(hdr)); |
475 | sin6->sin6_port = htons(opt.sock.proto); |
476 | } |
477 | |
478 | ca_set_sockopts(fd); |
479 | |
480 | if (clock_gettime(CLOCK_REALTIME, &time_start_real)) |
481 | error(ERN_GETTIME, errno, "gettime REALTIME" ); |
482 | if (clock_gettime(CLOCK_MONOTONIC, &time_start_mono)) |
483 | error(ERN_GETTIME, errno, "gettime MONOTONIC" ); |
484 | |
485 | iov[0].iov_base = buf; |
486 | iov[0].iov_len = opt.size; |
487 | |
488 | memset(&msg, 0, sizeof(msg)); |
489 | msg.msg_name = ai->ai_addr; |
490 | msg.msg_namelen = ai->ai_addrlen; |
491 | msg.msg_iov = iov; |
492 | msg.msg_iovlen = 1; |
493 | |
494 | cs_write_cmsg(fd, msg: &msg, cbuf, cbuf_sz: sizeof(cbuf)); |
495 | |
496 | for (i = 0; i < opt.num_pkt; i++) { |
497 | err = sendmsg(fd, &msg, 0); |
498 | if (err < 0) { |
499 | if (!opt.silent_send) |
500 | fprintf(stderr, "send failed: %s\n" , strerror(errno)); |
501 | err = ERN_SEND; |
502 | goto err_out; |
503 | } else if (err != (int)opt.size) { |
504 | fprintf(stderr, "short send\n" ); |
505 | err = ERN_SEND_SHORT; |
506 | goto err_out; |
507 | } |
508 | } |
509 | err = ERN_SUCCESS; |
510 | |
511 | if (opt.ts.ena) { |
512 | /* Make sure all timestamps have time to loop back */ |
513 | usleep(opt.txtime.delay); |
514 | |
515 | cs_read_cmsg(fd, msg: &msg, cbuf, cbuf_sz: sizeof(cbuf)); |
516 | } |
517 | |
518 | err_out: |
519 | close(fd); |
520 | freeaddrinfo(ai); |
521 | return err; |
522 | } |
523 | |