cmsg_sender.c

  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	ERN_SEND_MORE,
 37};
 38
 39struct option_cmsg_u32 {
 40	bool ena;
 41	unsigned int val;
 42};
 43
 44struct options {
 45	bool silent_send;
 46	const char *host;
 47	const char *service;
 48	unsigned int size;
 49	unsigned int num_pkt;
 50	bool msg_more;
 51	struct {
 52		unsigned int mark;
 53		unsigned int dontfrag;
 54		unsigned int tclass;
 55		unsigned int hlimit;
 56		unsigned int priority;
 57	} sockopt;
 58	struct {
 59		unsigned int family;
 60		unsigned int type;
 61		unsigned int proto;
 62	} sock;
 63	struct option_cmsg_u32 mark;
 64	struct option_cmsg_u32 priority;
 65	struct {
 66		bool ena;
 67		unsigned int delay;
 68	} txtime;
 69	struct {
 70		bool ena;
 71	} ts;
 72	struct {
 73		struct option_cmsg_u32 dontfrag;
 74		struct option_cmsg_u32 tclass;
 75		struct option_cmsg_u32 hlimit;
 76		struct option_cmsg_u32 exthdr;
 77	} cmsg;
 78} opt = {
 79	.size = 13,
 80	.num_pkt = 1,
 81	.sock = {
 82		.family	= AF_UNSPEC,
 83		.type	= SOCK_DGRAM,
 84		.proto	= IPPROTO_UDP,
 85	},
 86};
 87
 88static struct timespec time_start_real;
 89static struct timespec time_start_mono;
 90
 91static void __attribute__((noreturn)) cs_usage(const char *bin)
 92{
 93	printf("Usage: %s [opts] <dst host> <dst port / service>\n", bin);
 94	printf("Options:\n"
 95	       "\t\t-s      Silent send() failures\n"
 96	       "\t\t-S      send() size\n"
 97	       "\t\t-4/-6   Force IPv4 / IPv6 only\n"
 98	       "\t\t-p prot Socket protocol\n"
 99	       "\t\t        (u = UDP (default); i = ICMP; r = RAW;\n"
100	       "\t\t         U = UDP with MSG_MORE)\n"
101	       "\n"
102	       "\t\t-m val  Set SO_MARK with given value\n"
103	       "\t\t-M val  Set SO_MARK via setsockopt\n"
104	       "\t\t-P val  Set SO_PRIORITY via setsockopt\n"
105	       "\t\t-Q val  Set SO_PRIORITY via cmsg\n"
106	       "\t\t-d val  Set SO_TXTIME with given delay (usec)\n"
107	       "\t\t-t      Enable time stamp reporting\n"
108	       "\t\t-f val  Set don't fragment via cmsg\n"
109	       "\t\t-F val  Set don't fragment via setsockopt\n"
110	       "\t\t-c val  Set TOS/TCLASS via cmsg\n"
111	       "\t\t-C val  Set TOS/TCLASS via setsockopt\n"
112	       "\t\t-l val  Set TTL/HOPLIMIT via cmsg\n"
113	       "\t\t-L val  Set TTL/HOPLIMIT via setsockopt\n"
114	       "\t\t-H type Add an IPv6 header option\n"
115	       "\t\t        (h = HOP; d = DST; r = RTDST)\n"
116	       "\n");
117	exit(ERN_HELP);
118}
119
120static void cs_parse_args(int argc, char *argv[])
121{
122	int o;
123
124	while ((o = getopt(argc, argv, "46sS:p:P:m:M:n:d:tf:F:c:C:l:L:H:Q:")) != -1) {
125		switch (o) {
126		case 's':
127			opt.silent_send = true;
128			break;
129		case 'S':
130			opt.size = atoi(optarg);
131			break;
132		case '4':
133			opt.sock.family = AF_INET;
134			break;
135		case '6':
136			opt.sock.family = AF_INET6;
137			break;
138		case 'p':
139			if (*optarg == 'u') {
140				opt.sock.proto = IPPROTO_UDP;
141			} else if (*optarg == 'U') {
142				opt.sock.proto = IPPROTO_UDP;
143				opt.msg_more = true;
144			} else if (*optarg == 'i' || *optarg == 'I') {
145				opt.sock.proto = IPPROTO_ICMP;
146			} else if (*optarg == 'r') {
147				opt.sock.type = SOCK_RAW;
148			} else {
149				printf("Error: unknown protocol: %s\n", optarg);
150				cs_usage(argv[0]);
151			}
152			break;
153		case 'P':
154			opt.sockopt.priority = atoi(optarg);
155			break;
156		case 'm':
157			opt.mark.ena = true;
158			opt.mark.val = atoi(optarg);
159			break;
160		case 'Q':
161			opt.priority.ena = true;
162			opt.priority.val = atoi(optarg);
163			break;
164		case 'M':
165			opt.sockopt.mark = atoi(optarg);
166			break;
167		case 'n':
168			opt.num_pkt = atoi(optarg);
169			break;
170		case 'd':
171			opt.txtime.ena = true;
172			opt.txtime.delay = atoi(optarg);
173			break;
174		case 't':
175			opt.ts.ena = true;
176			break;
177		case 'f':
178			opt.cmsg.dontfrag.ena = true;
179			opt.cmsg.dontfrag.val = atoi(optarg);
180			break;
181		case 'F':
182			opt.sockopt.dontfrag = atoi(optarg);
183			break;
184		case 'c':
185			opt.cmsg.tclass.ena = true;
186			opt.cmsg.tclass.val = atoi(optarg);
187			break;
188		case 'C':
189			opt.sockopt.tclass = atoi(optarg);
190			break;
191		case 'l':
192			opt.cmsg.hlimit.ena = true;
193			opt.cmsg.hlimit.val = atoi(optarg);
194			break;
195		case 'L':
196			opt.sockopt.hlimit = atoi(optarg);
197			break;
198		case 'H':
199			opt.cmsg.exthdr.ena = true;
200			switch (optarg[0]) {
201			case 'h':
202				opt.cmsg.exthdr.val = IPV6_HOPOPTS;
203				break;
204			case 'd':
205				opt.cmsg.exthdr.val = IPV6_DSTOPTS;
206				break;
207			case 'r':
208				opt.cmsg.exthdr.val = IPV6_RTHDRDSTOPTS;
209				break;
210			default:
211				printf("Error: hdr type: %s\n", optarg);
212				break;
213			}
214			break;
215		}
216	}
217
218	if (optind != argc - 2)
219		cs_usage(argv[0]);
220
221	opt.host = argv[optind];
222	opt.service = argv[optind + 1];
223}
224
225static void memrnd(void *s, size_t n)
226{
227	int *dword = s;
228	char *byte;
229
230	for (; n >= 4; n -= 4)
231		*dword++ = rand();
232	byte = (void *)dword;
233	while (n--)
234		*byte++ = rand();
235}
236
237static void
238ca_write_cmsg_u32(char *cbuf, size_t cbuf_sz, size_t *cmsg_len,
239		  int level, int optname, struct option_cmsg_u32 *uopt)
240{
241	struct cmsghdr *cmsg;
242
243	if (!uopt->ena)
244		return;
245
246	cmsg = (struct cmsghdr *)(cbuf + *cmsg_len);
247	*cmsg_len += CMSG_SPACE(sizeof(__u32));
248	if (cbuf_sz < *cmsg_len)
249		error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
250
251	cmsg->cmsg_level = level;
252	cmsg->cmsg_type = optname;
253	cmsg->cmsg_len = CMSG_LEN(sizeof(__u32));
254	*(__u32 *)CMSG_DATA(cmsg) = uopt->val;
255}
256
257static void
258cs_write_cmsg(int fd, struct msghdr *msg, char *cbuf, size_t cbuf_sz)
259{
260	struct cmsghdr *cmsg;
261	size_t cmsg_len;
262
263	msg->msg_control = cbuf;
264	cmsg_len = 0;
265
266	ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
267			  SOL_SOCKET, SO_MARK, &opt.mark);
268	ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
269			  SOL_SOCKET, SO_PRIORITY, &opt.priority);
270
271	if (opt.sock.family == AF_INET) {
272		ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
273				  SOL_IP, IP_TOS, &opt.cmsg.tclass);
274		ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
275				  SOL_IP, IP_TTL, &opt.cmsg.hlimit);
276	} else {
277		ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
278				  SOL_IPV6, IPV6_DONTFRAG, &opt.cmsg.dontfrag);
279		ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
280				  SOL_IPV6, IPV6_TCLASS, &opt.cmsg.tclass);
281		ca_write_cmsg_u32(cbuf, cbuf_sz, &cmsg_len,
282				  SOL_IPV6, IPV6_HOPLIMIT, &opt.cmsg.hlimit);
283	}
284
285	if (opt.txtime.ena) {
286		__u64 txtime;
287
288		txtime = time_start_mono.tv_sec * (1000ULL * 1000 * 1000) +
289			 time_start_mono.tv_nsec +
290			 opt.txtime.delay * 1000;
291
292		cmsg = (struct cmsghdr *)(cbuf + cmsg_len);
293		cmsg_len += CMSG_SPACE(sizeof(txtime));
294		if (cbuf_sz < cmsg_len)
295			error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
296
297		cmsg->cmsg_level = SOL_SOCKET;
298		cmsg->cmsg_type = SCM_TXTIME;
299		cmsg->cmsg_len = CMSG_LEN(sizeof(txtime));
300		memcpy(CMSG_DATA(cmsg), &txtime, sizeof(txtime));
301	}
302	if (opt.ts.ena) {
303		cmsg = (struct cmsghdr *)(cbuf + cmsg_len);
304		cmsg_len += CMSG_SPACE(sizeof(__u32));
305		if (cbuf_sz < cmsg_len)
306			error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
307
308		cmsg->cmsg_level = SOL_SOCKET;
309		cmsg->cmsg_type = SO_TIMESTAMPING;
310		cmsg->cmsg_len = CMSG_LEN(sizeof(__u32));
311		*(__u32 *)CMSG_DATA(cmsg) = SOF_TIMESTAMPING_TX_SCHED |
312					    SOF_TIMESTAMPING_TX_SOFTWARE;
313	}
314	if (opt.cmsg.exthdr.ena) {
315		cmsg = (struct cmsghdr *)(cbuf + cmsg_len);
316		cmsg_len += CMSG_SPACE(8);
317		if (cbuf_sz < cmsg_len)
318			error(ERN_CMSG_WR, EFAULT, "cmsg buffer too small");
319
320		cmsg->cmsg_level = SOL_IPV6;
321		cmsg->cmsg_type = opt.cmsg.exthdr.val;
322		cmsg->cmsg_len = CMSG_LEN(8);
323		*(__u64 *)CMSG_DATA(cmsg) = 0;
324	}
325
326	if (cmsg_len)
327		msg->msg_controllen = cmsg_len;
328	else
329		msg->msg_control = NULL;
330}
331
332static const char *cs_ts_info2str(unsigned int info)
333{
334	static const char *names[] = {
335		[SCM_TSTAMP_SND]	= "SND",
336		[SCM_TSTAMP_SCHED]	= "SCHED",
337		[SCM_TSTAMP_ACK]	= "ACK",
338	};
339
340	if (info < ARRAY_SIZE(names))
341		return names[info];
342	return "unknown";
343}
344
345static unsigned long
346cs_read_cmsg(int fd, struct msghdr *msg, char *cbuf, size_t cbuf_sz)
347{
348	struct sock_extended_err *see;
349	struct scm_timestamping *ts;
350	unsigned long ts_seen = 0;
351	struct cmsghdr *cmsg;
352	int i, err;
353
354	if (!opt.ts.ena)
355		return 0;
356	msg->msg_control = cbuf;
357	msg->msg_controllen = cbuf_sz;
358
359	while (true) {
360		ts = NULL;
361		see = NULL;
362		memset(cbuf, 0, cbuf_sz);
363
364		err = recvmsg(fd, msg, MSG_ERRQUEUE);
365		if (err < 0) {
366			if (errno == EAGAIN)
367				break;
368			error(ERN_RECVERR, errno, "recvmsg ERRQ");
369		}
370
371		for (cmsg = CMSG_FIRSTHDR(msg); cmsg != NULL;
372		     cmsg = CMSG_NXTHDR(msg, cmsg)) {
373			if (cmsg->cmsg_level == SOL_SOCKET &&
374			    cmsg->cmsg_type == SO_TIMESTAMPING_OLD) {
375				if (cmsg->cmsg_len < sizeof(*ts))
376					error(ERN_CMSG_RD, EINVAL, "TS cmsg");
377
378				ts = (void *)CMSG_DATA(cmsg);
379			}
380			if ((cmsg->cmsg_level == SOL_IP &&
381			     cmsg->cmsg_type == IP_RECVERR) ||
382			    (cmsg->cmsg_level == SOL_IPV6 &&
383			     cmsg->cmsg_type == IPV6_RECVERR)) {
384				if (cmsg->cmsg_len < sizeof(*see))
385					error(ERN_CMSG_RD, EINVAL, "sock_err cmsg");
386
387				see = (void *)CMSG_DATA(cmsg);
388			}
389		}
390
391		if (!ts)
392			error(ERN_CMSG_RCV, ENOENT, "TS cmsg not found");
393		if (!see)
394			error(ERN_CMSG_RCV, ENOENT, "sock_err cmsg not found");
395
396		for (i = 0; i < 3; i++) {
397			unsigned long long rel_time;
398
399			if (!ts->ts[i].tv_sec && !ts->ts[i].tv_nsec)
400				continue;
401
402			rel_time = (ts->ts[i].tv_sec - time_start_real.tv_sec) *
403				(1000ULL * 1000) +
404				(ts->ts[i].tv_nsec - time_start_real.tv_nsec) /
405				1000;
406			printf(" %5s ts%d %lluus\n",
407			       cs_ts_info2str(see->ee_info),
408			       i, rel_time);
409			ts_seen |= 1 << see->ee_info;
410		}
411	}
412
413	return ts_seen;
414}
415
416static void ca_set_sockopts(int fd)
417{
418	if (opt.sockopt.mark &&
419	    setsockopt(fd, SOL_SOCKET, SO_MARK,
420		       &opt.sockopt.mark, sizeof(opt.sockopt.mark)))
421		error(ERN_SOCKOPT, errno, "setsockopt SO_MARK");
422	if (opt.sockopt.priority &&
423	    setsockopt(fd, SOL_SOCKET, SO_PRIORITY,
424		       &opt.sockopt.priority, sizeof(opt.sockopt.priority)))
425		error(ERN_SOCKOPT, errno, "setsockopt SO_PRIORITY");
426
427	if (opt.sock.family == AF_INET) {
428		if (opt.sockopt.tclass &&
429		    setsockopt(fd, SOL_IP, IP_TOS,
430			       &opt.sockopt.tclass, sizeof(opt.sockopt.tclass)))
431			error(ERN_SOCKOPT, errno, "setsockopt IP_TOS");
432		if (opt.sockopt.hlimit &&
433		    setsockopt(fd, SOL_IP, IP_TTL,
434			       &opt.sockopt.hlimit, sizeof(opt.sockopt.hlimit)))
435			error(ERN_SOCKOPT, errno, "setsockopt IP_TTL");
436	} else {
437		if (opt.sockopt.dontfrag &&
438		    setsockopt(fd, SOL_IPV6, IPV6_DONTFRAG,
439			       &opt.sockopt.dontfrag, sizeof(opt.sockopt.dontfrag)))
440			error(ERN_SOCKOPT, errno, "setsockopt IPV6_DONTFRAG");
441		if (opt.sockopt.tclass &&
442		    setsockopt(fd, SOL_IPV6, IPV6_TCLASS,
443			       &opt.sockopt.tclass, sizeof(opt.sockopt.tclass)))
444			error(ERN_SOCKOPT, errno, "setsockopt IPV6_TCLASS");
445		if (opt.sockopt.hlimit &&
446		    setsockopt(fd, SOL_IPV6, IPV6_UNICAST_HOPS,
447			       &opt.sockopt.hlimit, sizeof(opt.sockopt.hlimit)))
448			error(ERN_SOCKOPT, errno, "setsockopt IPV6_HOPLIMIT");
449	}
450
451	if (opt.txtime.ena) {
452		struct sock_txtime so_txtime = {
453			.clockid = CLOCK_MONOTONIC,
454		};
455
456		if (setsockopt(fd, SOL_SOCKET, SO_TXTIME,
457			       &so_txtime, sizeof(so_txtime)))
458			error(ERN_SOCKOPT, errno, "setsockopt TXTIME");
459	}
460	if (opt.ts.ena) {
461		__u32 val = SOF_TIMESTAMPING_SOFTWARE |
462			SOF_TIMESTAMPING_OPT_TSONLY;
463
464		if (setsockopt(fd, SOL_SOCKET, SO_TIMESTAMPING,
465			       &val, sizeof(val)))
466			error(ERN_SOCKOPT, errno, "setsockopt TIMESTAMPING");
467	}
468}
469
470int main(int argc, char *argv[])
471{
472	struct addrinfo hints, *ai;
473	struct iovec iov[1];
474	unsigned char *buf;
475	struct msghdr msg;
476	char cbuf[1024];
477	int err;
478	int fd;
479	int i;
480
481	cs_parse_args(argc, argv);
482
483	buf = malloc(opt.size);
484	memrnd(buf, opt.size);
485
486	memset(&hints, 0, sizeof(hints));
487	hints.ai_family = opt.sock.family;
488
489	ai = NULL;
490	err = getaddrinfo(opt.host, opt.service, &hints, &ai);
491	if (err) {
492		fprintf(stderr, "Can't resolve address [%s]:%s\n",
493			opt.host, opt.service);
494		err = ERN_SOCK_CREATE;
495		goto err_free_buff;
496	}
497
498	if (ai->ai_family == AF_INET6 && opt.sock.proto == IPPROTO_ICMP)
499		opt.sock.proto = IPPROTO_ICMPV6;
500
501	fd = socket(ai->ai_family, opt.sock.type, opt.sock.proto);
502	if (fd < 0) {
503		fprintf(stderr, "Can't open socket: %s\n", strerror(errno));
504		err = ERN_RESOLVE;
505		goto err_free_info;
506	}
507
508	if (opt.sock.proto == IPPROTO_ICMP) {
509		buf[0] = ICMP_ECHO;
510		buf[1] = 0;
511	} else if (opt.sock.proto == IPPROTO_ICMPV6) {
512		buf[0] = ICMPV6_ECHO_REQUEST;
513		buf[1] = 0;
514	} else if (opt.sock.type == SOCK_RAW) {
515		struct udphdr hdr = { 1, 2, htons(opt.size), 0 };
516		struct sockaddr_in6 *sin6 = (void *)ai->ai_addr;
517
518		memcpy(buf, &hdr, sizeof(hdr));
519		sin6->sin6_port = htons(opt.sock.proto);
520	}
521
522	ca_set_sockopts(fd);
523
524	if (clock_gettime(CLOCK_REALTIME, &time_start_real))
525		error(ERN_GETTIME, errno, "gettime REALTIME");
526	if (clock_gettime(CLOCK_MONOTONIC, &time_start_mono))
527		error(ERN_GETTIME, errno, "gettime MONOTONIC");
528
529	iov[0].iov_base = buf;
530	iov[0].iov_len = opt.size;
531
532	memset(&msg, 0, sizeof(msg));
533	msg.msg_name = ai->ai_addr;
534	msg.msg_namelen = ai->ai_addrlen;
535	msg.msg_iov = iov;
536	msg.msg_iovlen = 1;
537
538	cs_write_cmsg(fd, &msg, cbuf, sizeof(cbuf));
539
540	for (i = 0; i < opt.num_pkt; i++) {
541		err = sendmsg(fd, &msg, opt.msg_more ? MSG_MORE : 0);
542		if (err < 0) {
543			if (!opt.silent_send)
544				fprintf(stderr, "send failed: %s\n", strerror(errno));
545			err = ERN_SEND;
546			goto err_out;
547		} else if (err != (int)opt.size) {
548			fprintf(stderr, "short send\n");
549			err = ERN_SEND_SHORT;
550			goto err_out;
551		}
552		if (opt.msg_more) {
553			err = write(fd, NULL, 0);
554			if (err < 0) {
555				fprintf(stderr, "send more: %s\n", strerror(errno));
556				err = ERN_SEND_MORE;
557				goto err_out;
558			}
559		}
560	}
561	err = ERN_SUCCESS;
562
563	if (opt.ts.ena) {
564		unsigned long seen;
565		int i;
566
567		/* Make sure all timestamps have time to loop back */
568		for (i = 0; i < 40; i++) {
569			seen = cs_read_cmsg(fd, &msg, cbuf, sizeof(cbuf));
570			if (seen & (1 << SCM_TSTAMP_SND))
571				break;
572			usleep(opt.txtime.delay / 20);
573		}
574	}
575
576err_out:
577	close(fd);
578err_free_info:
579	freeaddrinfo(ai);
580err_free_buff:
581	free(buf);
582	return err;
583}