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 
cs_usage(const char * bin)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 
cs_parse_args(int argc,char * argv[])111 static void cs_parse_args(int argc, char *argv[])
112 {
113 	int 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 
memrnd(void * s,size_t n)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
ca_write_cmsg_u32(char * cbuf,size_t cbuf_sz,size_t * cmsg_len,int level,int optname,struct option_cmsg_u32 * uopt)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
cs_write_cmsg(int fd,struct msghdr * msg,char * cbuf,size_t cbuf_sz)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 
cs_ts_info2str(unsigned int info)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
cs_read_cmsg(int fd,struct msghdr * msg,char * cbuf,size_t cbuf_sz)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 
ca_set_sockopts(int fd)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 
main(int argc,char * argv[])415 int main(int argc, char *argv[])
416 {
417 	struct addrinfo hints, *ai;
418 	struct iovec iov[1];
419 	unsigned char *buf;
420 	struct msghdr msg;
421 	char cbuf[1024];
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