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 enum {
20 	ERN_SUCCESS = 0,
21 	/* Well defined errors, callers may depend on these */
22 	ERN_SEND = 1,
23 	/* Informational, can reorder */
24 	ERN_HELP,
25 	ERN_SEND_SHORT,
26 	ERN_SOCK_CREATE,
27 	ERN_RESOLVE,
28 	ERN_CMSG_WR,
29 	ERN_SOCKOPT,
30 	ERN_GETTIME,
31 	ERN_RECVERR,
32 	ERN_CMSG_RD,
33 	ERN_CMSG_RCV,
34 };
35 
36 struct option_cmsg_u32 {
37 	bool ena;
38 	unsigned int val;
39 };
40 
41 struct options {
42 	bool silent_send;
43 	const char *host;
44 	const char *service;
45 	unsigned int size;
46 	struct {
47 		unsigned int mark;
48 		unsigned int dontfrag;
49 	} sockopt;
50 	struct {
51 		unsigned int family;
52 		unsigned int type;
53 		unsigned int proto;
54 	} sock;
55 	struct option_cmsg_u32 mark;
56 	struct {
57 		bool ena;
58 		unsigned int delay;
59 	} txtime;
60 	struct {
61 		bool ena;
62 	} ts;
63 	struct {
64 		struct option_cmsg_u32 dontfrag;
65 	} v6;
66 } opt = {
67 	.size = 13,
68 	.sock = {
69 		.family	= AF_UNSPEC,
70 		.type	= SOCK_DGRAM,
71 		.proto	= IPPROTO_UDP,
72 	},
73 };
74 
75 static struct timespec time_start_real;
76 static struct timespec time_start_mono;
77 
78 static void __attribute__((noreturn)) cs_usage(const char *bin)
79 {
80 	printf("Usage: %s [opts] <dst host> <dst port / service>\n", bin);
81 	printf("Options:\n"
82 	       "\t\t-s      Silent send() failures\n"
83 	       "\t\t-S      send() size\n"
84 	       "\t\t-4/-6   Force IPv4 / IPv6 only\n"
85 	       "\t\t-p prot Socket protocol\n"
86 	       "\t\t        (u = UDP (default); i = ICMP; r = RAW)\n"
87 	       "\n"
88 	       "\t\t-m val  Set SO_MARK with given value\n"
89 	       "\t\t-M val  Set SO_MARK via setsockopt\n"
90 	       "\t\t-d val  Set SO_TXTIME with given delay (usec)\n"
91 	       "\t\t-t      Enable time stamp reporting\n"
92 	       "\t\t-f val  Set don't fragment via cmsg\n"
93 	       "\t\t-F val  Set don't fragment via setsockopt\n"
94 	       "");
95 	exit(ERN_HELP);
96 }
97 
98 static void cs_parse_args(int argc, char *argv[])
99 {
100 	char o;
101 
102 	while ((o = getopt(argc, argv, "46sS:p:m:M:d:tf:F:")) != -1) {
103 		switch (o) {
104 		case 's':
105 			opt.silent_send = true;
106 			break;
107 		case 'S':
108 			opt.size = atoi(optarg);
109 			break;
110 		case '4':
111 			opt.sock.family = AF_INET;
112 			break;
113 		case '6':
114 			opt.sock.family = AF_INET6;
115 			break;
116 		case 'p':
117 			if (*optarg == 'u' || *optarg == 'U') {
118 				opt.sock.proto = IPPROTO_UDP;
119 			} else if (*optarg == 'i' || *optarg == 'I') {
120 				opt.sock.proto = IPPROTO_ICMP;
121 			} else if (*optarg == 'r') {
122 				opt.sock.type = SOCK_RAW;
123 			} else {
124 				printf("Error: unknown protocol: %s\n", optarg);
125 				cs_usage(argv[0]);
126 			}
127 			break;
128 
129 		case 'm':
130 			opt.mark.ena = true;
131 			opt.mark.val = atoi(optarg);
132 			break;
133 		case 'M':
134 			opt.sockopt.mark = atoi(optarg);
135 			break;
136 		case 'd':
137 			opt.txtime.ena = true;
138 			opt.txtime.delay = atoi(optarg);
139 			break;
140 		case 't':
141 			opt.ts.ena = true;
142 			break;
143 		case 'f':
144 			opt.v6.dontfrag.ena = true;
145 			opt.v6.dontfrag.val = atoi(optarg);
146 			break;
147 		case 'F':
148 			opt.sockopt.dontfrag = atoi(optarg);
149 			break;
150 		}
151 	}
152 
153 	if (optind != argc - 2)
154 		cs_usage(argv[0]);
155 
156 	opt.host = argv[optind];
157 	opt.service = argv[optind + 1];
158 }
159 
160 static void memrnd(void *s, size_t n)
161 {
162 	int *dword = s;
163 	char *byte;
164 
165 	for (; n >= 4; n -= 4)
166 		*dword++ = rand();
167 	byte = (void *)dword;
168 	while (n--)
169 		*byte++ = rand();
170 }
171 
172 static void
173 ca_write_cmsg_u32(char *cbuf, size_t cbuf_sz, size_t *cmsg_len,
174 		  int level, int optname, struct option_cmsg_u32 *uopt)
175 {
176 	struct cmsghdr *cmsg;
177 
178 	if (!uopt->ena)
179 		return;
180 
181 	cmsg = (struct cmsghdr *)(cbuf + *cmsg_len);
182 	*cmsg_len += CMSG_SPACE(sizeof(__u32));
183 	if (cbuf_sz < *cmsg_len)
184 		error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
185 
186 	cmsg->cmsg_level = level;
187 	cmsg->cmsg_type = optname;
188 	cmsg->cmsg_len = CMSG_LEN(sizeof(__u32));
189 	*(__u32 *)CMSG_DATA(cmsg) = uopt->val;
190 }
191 
192 static void
193 cs_write_cmsg(int fd, struct msghdr *msg, char *cbuf, size_t cbuf_sz)
194 {
195 	struct cmsghdr *cmsg;
196 	size_t cmsg_len;
197 
198 	msg->msg_control = cbuf;
199 	cmsg_len = 0;
200 
201 	ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
202 			  SOL_SOCKET, SO_MARK, &opt.mark);
203 	ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
204 			  SOL_IPV6, IPV6_DONTFRAG, &opt.v6.dontfrag);
205 
206 	if (opt.txtime.ena) {
207 		struct sock_txtime so_txtime = {
208 			.clockid = CLOCK_MONOTONIC,
209 		};
210 		__u64 txtime;
211 
212 		if (setsockopt(fd, SOL_SOCKET, SO_TXTIME,
213 			       &so_txtime, sizeof(so_txtime)))
214 			error(ERN_SOCKOPT, errno, "setsockopt TXTIME");
215 
216 		txtime = time_start_mono.tv_sec * (1000ULL * 1000 * 1000) +
217 			 time_start_mono.tv_nsec +
218 			 opt.txtime.delay * 1000;
219 
220 		cmsg = (struct cmsghdr *)(cbuf + cmsg_len);
221 		cmsg_len += CMSG_SPACE(sizeof(txtime));
222 		if (cbuf_sz < cmsg_len)
223 			error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
224 
225 		cmsg->cmsg_level = SOL_SOCKET;
226 		cmsg->cmsg_type = SCM_TXTIME;
227 		cmsg->cmsg_len = CMSG_LEN(sizeof(txtime));
228 		memcpy(CMSG_DATA(cmsg), &txtime, sizeof(txtime));
229 	}
230 	if (opt.ts.ena) {
231 		__u32 val = SOF_TIMESTAMPING_SOFTWARE |
232 			    SOF_TIMESTAMPING_OPT_TSONLY;
233 
234 		if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING,
235 			       &val, sizeof(val)))
236 			error(ERN_SOCKOPT, errno, "setsockopt TIMESTAMPING");
237 
238 		cmsg = (struct cmsghdr *)(cbuf + cmsg_len);
239 		cmsg_len += CMSG_SPACE(sizeof(__u32));
240 		if (cbuf_sz < cmsg_len)
241 			error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
242 
243 		cmsg->cmsg_level = SOL_SOCKET;
244 		cmsg->cmsg_type = SO_TIMESTAMPING;
245 		cmsg->cmsg_len = CMSG_LEN(sizeof(__u32));
246 		*(__u32 *)CMSG_DATA(cmsg) = SOF_TIMESTAMPING_TX_SCHED |
247 					    SOF_TIMESTAMPING_TX_SOFTWARE;
248 	}
249 
250 	if (cmsg_len)
251 		msg->msg_controllen = cmsg_len;
252 	else
253 		msg->msg_control = NULL;
254 }
255 
256 static const char *cs_ts_info2str(unsigned int info)
257 {
258 	static const char *names[] = {
259 		[SCM_TSTAMP_SND]	= "SND",
260 		[SCM_TSTAMP_SCHED]	= "SCHED",
261 		[SCM_TSTAMP_ACK]	= "ACK",
262 	};
263 
264 	if (info < sizeof(names) / sizeof(names[0]))
265 		return names[info];
266 	return "unknown";
267 }
268 
269 static void
270 cs_read_cmsg(int fd, struct msghdr *msg, char *cbuf, size_t cbuf_sz)
271 {
272 	struct sock_extended_err *see;
273 	struct scm_timestamping *ts;
274 	struct cmsghdr *cmsg;
275 	int i, err;
276 
277 	if (!opt.ts.ena)
278 		return;
279 	msg->msg_control = cbuf;
280 	msg->msg_controllen = cbuf_sz;
281 
282 	while (true) {
283 		ts = NULL;
284 		see = NULL;
285 		memset(cbuf, 0, cbuf_sz);
286 
287 		err = recvmsg(fd, msg, MSG_ERRQUEUE);
288 		if (err < 0) {
289 			if (errno == EAGAIN)
290 				break;
291 			error(ERN_RECVERR, errno, "recvmsg ERRQ");
292 		}
293 
294 		for (cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL;
295 		     cmsg = CMSG_NXTHDR(msg, cmsg)) {
296 			if (cmsg->cmsg_level == SOL_SOCKET &&
297 			    cmsg->cmsg_type == SO_TIMESTAMPING_OLD) {
298 				if (cmsg->cmsg_len < sizeof(*ts))
299 					error(ERN_CMSG_RD, EINVAL, "TS cmsg");
300 
301 				ts = (void *)CMSG_DATA(cmsg);
302 			}
303 			if ((cmsg->cmsg_level == SOL_IP &&
304 			     cmsg->cmsg_type == IP_RECVERR) ||
305 			    (cmsg->cmsg_level == SOL_IPV6 &&
306 			     cmsg->cmsg_type == IPV6_RECVERR)) {
307 				if (cmsg->cmsg_len < sizeof(*see))
308 					error(ERN_CMSG_RD, EINVAL, "sock_err cmsg");
309 
310 				see = (void *)CMSG_DATA(cmsg);
311 			}
312 		}
313 
314 		if (!ts)
315 			error(ERN_CMSG_RCV, ENOENT, "TS cmsg not found");
316 		if (!see)
317 			error(ERN_CMSG_RCV, ENOENT, "sock_err cmsg not found");
318 
319 		for (i = 0; i < 3; i++) {
320 			unsigned long long rel_time;
321 
322 			if (!ts->ts[i].tv_sec && !ts->ts[i].tv_nsec)
323 				continue;
324 
325 			rel_time = (ts->ts[i].tv_sec - time_start_real.tv_sec) *
326 				(1000ULL * 1000) +
327 				(ts->ts[i].tv_nsec - time_start_real.tv_nsec) /
328 				1000;
329 			printf(" %5s ts%d %lluus\n",
330 			       cs_ts_info2str(see->ee_info),
331 			       i, rel_time);
332 		}
333 	}
334 }
335 
336 static void ca_set_sockopts(int fd)
337 {
338 	if (opt.sockopt.mark &&
339 	    setsockopt(fd, SOL_SOCKET, SO_MARK,
340 		       &opt.sockopt.mark, sizeof(opt.sockopt.mark)))
341 		error(ERN_SOCKOPT, errno, "setsockopt SO_MARK");
342 	if (opt.sockopt.dontfrag &&
343 	    setsockopt(fd, SOL_IPV6, IPV6_DONTFRAG,
344 		       &opt.sockopt.dontfrag, sizeof(opt.sockopt.dontfrag)))
345 		error(ERN_SOCKOPT, errno, "setsockopt IPV6_DONTFRAG");
346 }
347 
348 int main(int argc, char *argv[])
349 {
350 	struct addrinfo hints, *ai;
351 	struct iovec iov[1];
352 	struct msghdr msg;
353 	char cbuf[1024];
354 	char *buf;
355 	int err;
356 	int fd;
357 
358 	cs_parse_args(argc, argv);
359 
360 	buf = malloc(opt.size);
361 	memrnd(buf, opt.size);
362 
363 	memset(&hints, 0, sizeof(hints));
364 	hints.ai_family = opt.sock.family;
365 
366 	ai = NULL;
367 	err = getaddrinfo(opt.host, opt.service, &hints, &ai);
368 	if (err) {
369 		fprintf(stderr, "Can't resolve address [%s]:%s\n",
370 			opt.host, opt.service);
371 		return ERN_SOCK_CREATE;
372 	}
373 
374 	if (ai->ai_family == AF_INET6 && opt.sock.proto == IPPROTO_ICMP)
375 		opt.sock.proto = IPPROTO_ICMPV6;
376 
377 	fd = socket(ai->ai_family, opt.sock.type, opt.sock.proto);
378 	if (fd < 0) {
379 		fprintf(stderr, "Can't open socket: %s\n", strerror(errno));
380 		freeaddrinfo(ai);
381 		return ERN_RESOLVE;
382 	}
383 
384 	if (opt.sock.proto == IPPROTO_ICMP) {
385 		buf[0] = ICMP_ECHO;
386 		buf[1] = 0;
387 	} else if (opt.sock.proto == IPPROTO_ICMPV6) {
388 		buf[0] = ICMPV6_ECHO_REQUEST;
389 		buf[1] = 0;
390 	} else if (opt.sock.type == SOCK_RAW) {
391 		struct udphdr hdr = { 1, 2, htons(opt.size), 0 };
392 		struct sockaddr_in6 *sin6 = (void *)ai->ai_addr;;
393 
394 		memcpy(buf, &hdr, sizeof(hdr));
395 		sin6->sin6_port = htons(opt.sock.proto);
396 	}
397 
398 	ca_set_sockopts(fd);
399 
400 	if (clock_gettime(CLOCK_REALTIME, &time_start_real))
401 		error(ERN_GETTIME, errno, "gettime REALTIME");
402 	if (clock_gettime(CLOCK_MONOTONIC, &time_start_mono))
403 		error(ERN_GETTIME, errno, "gettime MONOTONIC");
404 
405 	iov[0].iov_base = buf;
406 	iov[0].iov_len = opt.size;
407 
408 	memset(&msg, 0, sizeof(msg));
409 	msg.msg_name = ai->ai_addr;
410 	msg.msg_namelen = ai->ai_addrlen;
411 	msg.msg_iov = iov;
412 	msg.msg_iovlen = 1;
413 
414 	cs_write_cmsg(fd, &msg, cbuf, sizeof(cbuf));
415 
416 	err = sendmsg(fd, &msg, 0);
417 	if (err < 0) {
418 		if (!opt.silent_send)
419 			fprintf(stderr, "send failed: %s\n", strerror(errno));
420 		err = ERN_SEND;
421 		goto err_out;
422 	} else if (err != (int)opt.size) {
423 		fprintf(stderr, "short send\n");
424 		err = ERN_SEND_SHORT;
425 		goto err_out;
426 	} else {
427 		err = ERN_SUCCESS;
428 	}
429 
430 	/* Make sure all timestamps have time to loop back */
431 	usleep(opt.txtime.delay);
432 
433 	cs_read_cmsg(fd, &msg, cbuf, sizeof(cbuf));
434 
435 err_out:
436 	close(fd);
437 	freeaddrinfo(ai);
438 	return err;
439 }
440