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