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