xref: /linux/tools/testing/selftests/net/cmsg_sender.c (revision 5027ec19f1049a07df5b0a37b1f462514cf2724b)
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 {
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 
85 static struct timespec time_start_real;
86 static struct timespec time_start_mono;
87 
88 static 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 
114 static 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 
212 static 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 
224 static void
225 ca_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 
244 static void
245 cs_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 
323 static 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 
336 static void
337 cs_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 
403 static 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 
427 int main(int argc, char *argv[])
428 {
429 	struct addrinfo hints, *ai;
430 	struct iovec iov[1];
431 	struct msghdr msg;
432 	char cbuf[1024];
433 	char *buf;
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 
518 err_out:
519 	close(fd);
520 	freeaddrinfo(ai);
521 	return err;
522 }
523