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	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
85static struct timespec time_start_real;
86static struct timespec time_start_mono;
87
88static 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
114static 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(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(argv[0]);
207
208	opt.host = argv[optind];
209	opt.service = argv[optind + 1];
210}
211
212static 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
224static void
225ca_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
244static void
245cs_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,
254			  SOL_SOCKET, SO_MARK, &opt.mark);
255	ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
256			  SOL_IPV6, IPV6_DONTFRAG, &opt.v6.dontfrag);
257	ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
258			  SOL_IPV6, IPV6_TCLASS, &opt.v6.tclass);
259	ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
260			  SOL_IPV6, IPV6_HOPLIMIT, &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
323static 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
336static void
337cs_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(see->ee_info),
398			       i, rel_time);
399		}
400	}
401}
402
403static 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
427int 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(buf, 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, cbuf, 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, cbuf, sizeof(cbuf));
516	}
517
518err_out:
519	close(fd);
520	freeaddrinfo(ai);
521	return err;
522}
523