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
21enum {
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
38struct option_cmsg_u32 {
39	bool ena;
40	unsigned int val;
41};
42
43struct 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
82static struct timespec time_start_real;
83static struct timespec time_start_mono;
84
85static 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
111static 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
204static 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
216static void
217ca_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
236static void
237cs_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
315static 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
328static void
329cs_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
395static 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
415int 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
502err_out:
503	close(fd);
504	freeaddrinfo(ai);
505	return err;
506}
507