1 // SPDX-License-Identifier: GPL-2.0
2 /* nettest - used for functional tests of networking APIs
3  *
4  * Copyright (c) 2013-2019 David Ahern <dsahern@gmail.com>. All rights reserved.
5  */
6 
7 #define _GNU_SOURCE
8 #include <features.h>
9 #include <sys/types.h>
10 #include <sys/ioctl.h>
11 #include <sys/socket.h>
12 #include <sys/wait.h>
13 #include <linux/tcp.h>
14 #include <linux/udp.h>
15 #include <arpa/inet.h>
16 #include <net/if.h>
17 #include <netinet/in.h>
18 #include <netinet/ip.h>
19 #include <netdb.h>
20 #include <fcntl.h>
21 #include <libgen.h>
22 #include <limits.h>
23 #include <sched.h>
24 #include <stdarg.h>
25 #include <stdio.h>
26 #include <stdlib.h>
27 #include <string.h>
28 #include <unistd.h>
29 #include <time.h>
30 #include <errno.h>
31 #include <getopt.h>
32 
33 #include <linux/xfrm.h>
34 #include <linux/ipsec.h>
35 #include <linux/pfkeyv2.h>
36 
37 #ifndef IPV6_UNICAST_IF
38 #define IPV6_UNICAST_IF         76
39 #endif
40 #ifndef IPV6_MULTICAST_IF
41 #define IPV6_MULTICAST_IF       17
42 #endif
43 
44 #define DEFAULT_PORT 12345
45 
46 #define NS_PREFIX "/run/netns/"
47 
48 #ifndef MAX
49 #define MAX(a, b)  ((a) > (b) ? (a) : (b))
50 #endif
51 #ifndef MIN
52 #define MIN(a, b)  ((a) < (b) ? (a) : (b))
53 #endif
54 
55 struct sock_args {
56 	/* local address */
57 	const char *local_addr_str;
58 	const char *client_local_addr_str;
59 	union {
60 		struct in_addr  in;
61 		struct in6_addr in6;
62 	} local_addr;
63 
64 	/* remote address */
65 	const char *remote_addr_str;
66 	union {
67 		struct in_addr  in;
68 		struct in6_addr in6;
69 	} remote_addr;
70 	int scope_id;  /* remote scope; v6 send only */
71 
72 	struct in_addr grp;     /* multicast group */
73 
74 	unsigned int has_local_ip:1,
75 		     has_remote_ip:1,
76 		     has_grp:1,
77 		     has_expected_laddr:1,
78 		     has_expected_raddr:1,
79 		     bind_test_only:1;
80 
81 	unsigned short port;
82 
83 	int type;      /* DGRAM, STREAM, RAW */
84 	int protocol;
85 	int version;   /* AF_INET/AF_INET6 */
86 
87 	int use_setsockopt;
88 	int use_freebind;
89 	int use_cmsg;
90 	const char *dev;
91 	const char *server_dev;
92 	int ifindex;
93 
94 	const char *clientns;
95 	const char *serverns;
96 
97 	const char *password;
98 	const char *client_pw;
99 	/* prefix for MD5 password */
100 	const char *md5_prefix_str;
101 	union {
102 		struct sockaddr_in v4;
103 		struct sockaddr_in6 v6;
104 	} md5_prefix;
105 	unsigned int prefix_len;
106 	/* 0: default, -1: force off, +1: force on */
107 	int bind_key_ifindex;
108 
109 	/* expected addresses and device index for connection */
110 	const char *expected_dev;
111 	const char *expected_server_dev;
112 	int expected_ifindex;
113 
114 	/* local address */
115 	const char *expected_laddr_str;
116 	union {
117 		struct in_addr  in;
118 		struct in6_addr in6;
119 	} expected_laddr;
120 
121 	/* remote address */
122 	const char *expected_raddr_str;
123 	union {
124 		struct in_addr  in;
125 		struct in6_addr in6;
126 	} expected_raddr;
127 
128 	/* ESP in UDP encap test */
129 	int use_xfrm;
130 };
131 
132 static int server_mode;
133 static unsigned int prog_timeout = 5;
134 static unsigned int interactive;
135 static int iter = 1;
136 static char *msg = "Hello world!";
137 static int msglen;
138 static int quiet;
139 static int try_broadcast = 1;
140 
timestamp(char * timebuf,int buflen)141 static char *timestamp(char *timebuf, int buflen)
142 {
143 	time_t now;
144 
145 	now = time(NULL);
146 	if (strftime(timebuf, buflen, "%T", localtime(&now)) == 0) {
147 		memset(timebuf, 0, buflen);
148 		strncpy(timebuf, "00:00:00", buflen-1);
149 	}
150 
151 	return timebuf;
152 }
153 
log_msg(const char * format,...)154 static void log_msg(const char *format, ...)
155 {
156 	char timebuf[64];
157 	va_list args;
158 
159 	if (quiet)
160 		return;
161 
162 	fprintf(stdout, "%s %s:",
163 		timestamp(timebuf, sizeof(timebuf)),
164 		server_mode ? "server" : "client");
165 	va_start(args, format);
166 	vfprintf(stdout, format, args);
167 	va_end(args);
168 
169 	fflush(stdout);
170 }
171 
log_error(const char * format,...)172 static void log_error(const char *format, ...)
173 {
174 	char timebuf[64];
175 	va_list args;
176 
177 	if (quiet)
178 		return;
179 
180 	fprintf(stderr, "%s %s:",
181 		timestamp(timebuf, sizeof(timebuf)),
182 		server_mode ? "server" : "client");
183 	va_start(args, format);
184 	vfprintf(stderr, format, args);
185 	va_end(args);
186 
187 	fflush(stderr);
188 }
189 
log_err_errno(const char * fmt,...)190 static void log_err_errno(const char *fmt, ...)
191 {
192 	char timebuf[64];
193 	va_list args;
194 
195 	if (quiet)
196 		return;
197 
198 	fprintf(stderr, "%s %s: ",
199 		timestamp(timebuf, sizeof(timebuf)),
200 		server_mode ? "server" : "client");
201 	va_start(args, fmt);
202 	vfprintf(stderr, fmt, args);
203 	va_end(args);
204 
205 	fprintf(stderr, ": %d: %s\n", errno, strerror(errno));
206 	fflush(stderr);
207 }
208 
log_address(const char * desc,struct sockaddr * sa)209 static void log_address(const char *desc, struct sockaddr *sa)
210 {
211 	char addrstr[64];
212 
213 	if (quiet)
214 		return;
215 
216 	if (sa->sa_family == AF_INET) {
217 		struct sockaddr_in *s = (struct sockaddr_in *) sa;
218 
219 		log_msg("%s %s:%d\n",
220 			desc,
221 			inet_ntop(AF_INET, &s->sin_addr, addrstr,
222 				  sizeof(addrstr)),
223 			ntohs(s->sin_port));
224 
225 	} else if (sa->sa_family == AF_INET6) {
226 		struct sockaddr_in6 *s6 = (struct sockaddr_in6 *) sa;
227 
228 		log_msg("%s [%s]:%d\n",
229 			desc,
230 			inet_ntop(AF_INET6, &s6->sin6_addr, addrstr,
231 				  sizeof(addrstr)),
232 			ntohs(s6->sin6_port));
233 	}
234 
235 	fflush(stdout);
236 }
237 
switch_ns(const char * ns)238 static int switch_ns(const char *ns)
239 {
240 	char path[PATH_MAX];
241 	int fd, ret;
242 
243 	if (geteuid())
244 		log_error("warning: likely need root to set netns %s!\n", ns);
245 
246 	snprintf(path, sizeof(path), "%s%s", NS_PREFIX, ns);
247 	fd = open(path, 0);
248 	if (fd < 0) {
249 		log_err_errno("Failed to open netns path; can not switch netns");
250 		return 1;
251 	}
252 
253 	ret = setns(fd, CLONE_NEWNET);
254 	close(fd);
255 
256 	return ret;
257 }
258 
tcp_md5sig(int sd,void * addr,socklen_t alen,struct sock_args * args)259 static int tcp_md5sig(int sd, void *addr, socklen_t alen, struct sock_args *args)
260 {
261 	int keylen = strlen(args->password);
262 	struct tcp_md5sig md5sig = {};
263 	int opt = TCP_MD5SIG;
264 	int rc;
265 
266 	md5sig.tcpm_keylen = keylen;
267 	memcpy(md5sig.tcpm_key, args->password, keylen);
268 
269 	if (args->prefix_len) {
270 		opt = TCP_MD5SIG_EXT;
271 		md5sig.tcpm_flags |= TCP_MD5SIG_FLAG_PREFIX;
272 
273 		md5sig.tcpm_prefixlen = args->prefix_len;
274 		addr = &args->md5_prefix;
275 	}
276 	memcpy(&md5sig.tcpm_addr, addr, alen);
277 
278 	if ((args->ifindex && args->bind_key_ifindex >= 0) || args->bind_key_ifindex >= 1) {
279 		opt = TCP_MD5SIG_EXT;
280 		md5sig.tcpm_flags |= TCP_MD5SIG_FLAG_IFINDEX;
281 
282 		md5sig.tcpm_ifindex = args->ifindex;
283 		log_msg("TCP_MD5SIG_FLAG_IFINDEX set tcpm_ifindex=%d\n", md5sig.tcpm_ifindex);
284 	} else {
285 		log_msg("TCP_MD5SIG_FLAG_IFINDEX off\n", md5sig.tcpm_ifindex);
286 	}
287 
288 	rc = setsockopt(sd, IPPROTO_TCP, opt, &md5sig, sizeof(md5sig));
289 	if (rc < 0) {
290 		/* ENOENT is harmless. Returned when a password is cleared */
291 		if (errno == ENOENT)
292 			rc = 0;
293 		else
294 			log_err_errno("setsockopt(TCP_MD5SIG)");
295 	}
296 
297 	return rc;
298 }
299 
tcp_md5_remote(int sd,struct sock_args * args)300 static int tcp_md5_remote(int sd, struct sock_args *args)
301 {
302 	struct sockaddr_in sin = {
303 		.sin_family = AF_INET,
304 	};
305 	struct sockaddr_in6 sin6 = {
306 		.sin6_family = AF_INET6,
307 	};
308 	void *addr;
309 	int alen;
310 
311 	switch (args->version) {
312 	case AF_INET:
313 		sin.sin_port = htons(args->port);
314 		sin.sin_addr = args->md5_prefix.v4.sin_addr;
315 		addr = &sin;
316 		alen = sizeof(sin);
317 		break;
318 	case AF_INET6:
319 		sin6.sin6_port = htons(args->port);
320 		sin6.sin6_addr = args->md5_prefix.v6.sin6_addr;
321 		addr = &sin6;
322 		alen = sizeof(sin6);
323 		break;
324 	default:
325 		log_error("unknown address family\n");
326 		exit(1);
327 	}
328 
329 	if (tcp_md5sig(sd, addr, alen, args))
330 		return -1;
331 
332 	return 0;
333 }
334 
get_ifidx(const char * ifname)335 static int get_ifidx(const char *ifname)
336 {
337 	struct ifreq ifdata;
338 	int sd, rc;
339 
340 	if (!ifname || *ifname == '\0')
341 		return -1;
342 
343 	memset(&ifdata, 0, sizeof(ifdata));
344 
345 	strcpy(ifdata.ifr_name, ifname);
346 
347 	sd = socket(PF_INET, SOCK_DGRAM, IPPROTO_IP);
348 	if (sd < 0) {
349 		log_err_errno("socket failed");
350 		return -1;
351 	}
352 
353 	rc = ioctl(sd, SIOCGIFINDEX, (char *)&ifdata);
354 	close(sd);
355 	if (rc != 0) {
356 		log_err_errno("ioctl(SIOCGIFINDEX) failed");
357 		return -1;
358 	}
359 
360 	return ifdata.ifr_ifindex;
361 }
362 
bind_to_device(int sd,const char * name)363 static int bind_to_device(int sd, const char *name)
364 {
365 	int rc;
366 
367 	rc = setsockopt(sd, SOL_SOCKET, SO_BINDTODEVICE, name, strlen(name)+1);
368 	if (rc < 0)
369 		log_err_errno("setsockopt(SO_BINDTODEVICE)");
370 
371 	return rc;
372 }
373 
get_bind_to_device(int sd,char * name,size_t len)374 static int get_bind_to_device(int sd, char *name, size_t len)
375 {
376 	int rc;
377 	socklen_t optlen = len;
378 
379 	name[0] = '\0';
380 	rc = getsockopt(sd, SOL_SOCKET, SO_BINDTODEVICE, name, &optlen);
381 	if (rc < 0)
382 		log_err_errno("setsockopt(SO_BINDTODEVICE)");
383 
384 	return rc;
385 }
386 
check_device(int sd,struct sock_args * args)387 static int check_device(int sd, struct sock_args *args)
388 {
389 	int ifindex = 0;
390 	char name[32];
391 
392 	if (get_bind_to_device(sd, name, sizeof(name)))
393 		*name = '\0';
394 	else
395 		ifindex = get_ifidx(name);
396 
397 	log_msg("    bound to device %s/%d\n",
398 		*name ? name : "<none>", ifindex);
399 
400 	if (!args->expected_ifindex)
401 		return 0;
402 
403 	if (args->expected_ifindex != ifindex) {
404 		log_error("Device index mismatch: expected %d have %d\n",
405 			  args->expected_ifindex, ifindex);
406 		return 1;
407 	}
408 
409 	log_msg("Device index matches: expected %d have %d\n",
410 		args->expected_ifindex, ifindex);
411 
412 	return 0;
413 }
414 
set_pktinfo_v4(int sd)415 static int set_pktinfo_v4(int sd)
416 {
417 	int one = 1;
418 	int rc;
419 
420 	rc = setsockopt(sd, SOL_IP, IP_PKTINFO, &one, sizeof(one));
421 	if (rc < 0 && rc != -ENOTSUP)
422 		log_err_errno("setsockopt(IP_PKTINFO)");
423 
424 	return rc;
425 }
426 
set_recvpktinfo_v6(int sd)427 static int set_recvpktinfo_v6(int sd)
428 {
429 	int one = 1;
430 	int rc;
431 
432 	rc = setsockopt(sd, SOL_IPV6, IPV6_RECVPKTINFO, &one, sizeof(one));
433 	if (rc < 0 && rc != -ENOTSUP)
434 		log_err_errno("setsockopt(IPV6_RECVPKTINFO)");
435 
436 	return rc;
437 }
438 
set_recverr_v4(int sd)439 static int set_recverr_v4(int sd)
440 {
441 	int one = 1;
442 	int rc;
443 
444 	rc = setsockopt(sd, SOL_IP, IP_RECVERR, &one, sizeof(one));
445 	if (rc < 0 && rc != -ENOTSUP)
446 		log_err_errno("setsockopt(IP_RECVERR)");
447 
448 	return rc;
449 }
450 
set_recverr_v6(int sd)451 static int set_recverr_v6(int sd)
452 {
453 	int one = 1;
454 	int rc;
455 
456 	rc = setsockopt(sd, SOL_IPV6, IPV6_RECVERR, &one, sizeof(one));
457 	if (rc < 0 && rc != -ENOTSUP)
458 		log_err_errno("setsockopt(IPV6_RECVERR)");
459 
460 	return rc;
461 }
462 
set_unicast_if(int sd,int ifindex,int version)463 static int set_unicast_if(int sd, int ifindex, int version)
464 {
465 	int opt = IP_UNICAST_IF;
466 	int level = SOL_IP;
467 	int rc;
468 
469 	ifindex = htonl(ifindex);
470 
471 	if (version == AF_INET6) {
472 		opt = IPV6_UNICAST_IF;
473 		level = SOL_IPV6;
474 	}
475 	rc = setsockopt(sd, level, opt, &ifindex, sizeof(ifindex));
476 	if (rc < 0)
477 		log_err_errno("setsockopt(IP_UNICAST_IF)");
478 
479 	return rc;
480 }
481 
set_multicast_if(int sd,int ifindex)482 static int set_multicast_if(int sd, int ifindex)
483 {
484 	struct ip_mreqn mreq = { .imr_ifindex = ifindex };
485 	int rc;
486 
487 	rc = setsockopt(sd, SOL_IP, IP_MULTICAST_IF, &mreq, sizeof(mreq));
488 	if (rc < 0)
489 		log_err_errno("setsockopt(IP_MULTICAST_IF)");
490 
491 	return rc;
492 }
493 
set_membership(int sd,uint32_t grp,uint32_t addr,int ifindex)494 static int set_membership(int sd, uint32_t grp, uint32_t addr, int ifindex)
495 {
496 	uint32_t if_addr = addr;
497 	struct ip_mreqn mreq;
498 	int rc;
499 
500 	if (addr == htonl(INADDR_ANY) && !ifindex) {
501 		log_error("Either local address or device needs to be given for multicast membership\n");
502 		return -1;
503 	}
504 
505 	mreq.imr_multiaddr.s_addr = grp;
506 	mreq.imr_address.s_addr = if_addr;
507 	mreq.imr_ifindex = ifindex;
508 
509 	rc = setsockopt(sd, IPPROTO_IP, IP_ADD_MEMBERSHIP, &mreq, sizeof(mreq));
510 	if (rc < 0) {
511 		log_err_errno("setsockopt(IP_ADD_MEMBERSHIP)");
512 		return -1;
513 	}
514 
515 	return 0;
516 }
517 
set_freebind(int sd,int version)518 static int set_freebind(int sd, int version)
519 {
520 	unsigned int one = 1;
521 	int rc = 0;
522 
523 	switch (version) {
524 	case AF_INET:
525 		if (setsockopt(sd, SOL_IP, IP_FREEBIND, &one, sizeof(one))) {
526 			log_err_errno("setsockopt(IP_FREEBIND)");
527 			rc = -1;
528 		}
529 		break;
530 	case AF_INET6:
531 		if (setsockopt(sd, SOL_IPV6, IPV6_FREEBIND, &one, sizeof(one))) {
532 			log_err_errno("setsockopt(IPV6_FREEBIND");
533 			rc = -1;
534 		}
535 		break;
536 	}
537 
538 	return rc;
539 }
540 
set_broadcast(int sd)541 static int set_broadcast(int sd)
542 {
543 	unsigned int one = 1;
544 	int rc = 0;
545 
546 	if (setsockopt(sd, SOL_SOCKET, SO_BROADCAST, &one, sizeof(one)) != 0) {
547 		log_err_errno("setsockopt(SO_BROADCAST)");
548 		rc = -1;
549 	}
550 
551 	return rc;
552 }
553 
set_reuseport(int sd)554 static int set_reuseport(int sd)
555 {
556 	unsigned int one = 1;
557 	int rc = 0;
558 
559 	if (setsockopt(sd, SOL_SOCKET, SO_REUSEPORT, &one, sizeof(one)) != 0) {
560 		log_err_errno("setsockopt(SO_REUSEPORT)");
561 		rc = -1;
562 	}
563 
564 	return rc;
565 }
566 
set_reuseaddr(int sd)567 static int set_reuseaddr(int sd)
568 {
569 	unsigned int one = 1;
570 	int rc = 0;
571 
572 	if (setsockopt(sd, SOL_SOCKET, SO_REUSEADDR, &one, sizeof(one)) != 0) {
573 		log_err_errno("setsockopt(SO_REUSEADDR)");
574 		rc = -1;
575 	}
576 
577 	return rc;
578 }
579 
str_to_uint(const char * str,int min,int max,unsigned int * value)580 static int str_to_uint(const char *str, int min, int max, unsigned int *value)
581 {
582 	int number;
583 	char *end;
584 
585 	errno = 0;
586 	number = (unsigned int) strtoul(str, &end, 0);
587 
588 	/* entire string should be consumed by conversion
589 	 * and value should be between min and max
590 	 */
591 	if (((*end == '\0') || (*end == '\n')) && (end != str) &&
592 	    (errno != ERANGE) && (min <= number) && (number <= max)) {
593 		*value = number;
594 		return 0;
595 	}
596 
597 	return -1;
598 }
599 
resolve_devices(struct sock_args * args)600 static int resolve_devices(struct sock_args *args)
601 {
602 	if (args->dev) {
603 		args->ifindex = get_ifidx(args->dev);
604 		if (args->ifindex < 0) {
605 			log_error("Invalid device name\n");
606 			return 1;
607 		}
608 	}
609 
610 	if (args->expected_dev) {
611 		unsigned int tmp;
612 
613 		if (str_to_uint(args->expected_dev, 0, INT_MAX, &tmp) == 0) {
614 			args->expected_ifindex = (int)tmp;
615 		} else {
616 			args->expected_ifindex = get_ifidx(args->expected_dev);
617 			if (args->expected_ifindex < 0) {
618 				fprintf(stderr, "Invalid expected device\n");
619 				return 1;
620 			}
621 		}
622 	}
623 
624 	return 0;
625 }
626 
expected_addr_match(struct sockaddr * sa,void * expected,const char * desc)627 static int expected_addr_match(struct sockaddr *sa, void *expected,
628 			       const char *desc)
629 {
630 	char addrstr[64];
631 	int rc = 0;
632 
633 	if (sa->sa_family == AF_INET) {
634 		struct sockaddr_in *s = (struct sockaddr_in *) sa;
635 		struct in_addr *exp_in = (struct in_addr *) expected;
636 
637 		if (s->sin_addr.s_addr != exp_in->s_addr) {
638 			log_error("%s address does not match expected %s\n",
639 				  desc,
640 				  inet_ntop(AF_INET, exp_in,
641 					    addrstr, sizeof(addrstr)));
642 			rc = 1;
643 		}
644 	} else if (sa->sa_family == AF_INET6) {
645 		struct sockaddr_in6 *s6 = (struct sockaddr_in6 *) sa;
646 		struct in6_addr *exp_in = (struct in6_addr *) expected;
647 
648 		if (memcmp(&s6->sin6_addr, exp_in, sizeof(*exp_in))) {
649 			log_error("%s address does not match expected %s\n",
650 				  desc,
651 				  inet_ntop(AF_INET6, exp_in,
652 					    addrstr, sizeof(addrstr)));
653 			rc = 1;
654 		}
655 	} else {
656 		log_error("%s address does not match expected - unknown family\n",
657 			  desc);
658 		rc = 1;
659 	}
660 
661 	if (!rc)
662 		log_msg("%s address matches expected\n", desc);
663 
664 	return rc;
665 }
666 
show_sockstat(int sd,struct sock_args * args)667 static int show_sockstat(int sd, struct sock_args *args)
668 {
669 	struct sockaddr_in6 local_addr, remote_addr;
670 	socklen_t alen = sizeof(local_addr);
671 	struct sockaddr *sa;
672 	const char *desc;
673 	int rc = 0;
674 
675 	desc = server_mode ? "server local:" : "client local:";
676 	sa = (struct sockaddr *) &local_addr;
677 	if (getsockname(sd, sa, &alen) == 0) {
678 		log_address(desc, sa);
679 
680 		if (args->has_expected_laddr) {
681 			rc = expected_addr_match(sa, &args->expected_laddr,
682 						 "local");
683 		}
684 	} else {
685 		log_err_errno("getsockname failed");
686 	}
687 
688 	sa = (struct sockaddr *) &remote_addr;
689 	desc = server_mode ? "server peer:" : "client peer:";
690 	if (getpeername(sd, sa, &alen) == 0) {
691 		log_address(desc, sa);
692 
693 		if (args->has_expected_raddr) {
694 			rc |= expected_addr_match(sa, &args->expected_raddr,
695 						 "remote");
696 		}
697 	} else {
698 		log_err_errno("getpeername failed");
699 	}
700 
701 	return rc;
702 }
703 
704 enum addr_type {
705 	ADDR_TYPE_LOCAL,
706 	ADDR_TYPE_REMOTE,
707 	ADDR_TYPE_MCAST,
708 	ADDR_TYPE_EXPECTED_LOCAL,
709 	ADDR_TYPE_EXPECTED_REMOTE,
710 	ADDR_TYPE_MD5_PREFIX,
711 };
712 
convert_addr(struct sock_args * args,const char * _str,enum addr_type atype)713 static int convert_addr(struct sock_args *args, const char *_str,
714 			enum addr_type atype)
715 {
716 	int pfx_len_max = args->version == AF_INET6 ? 128 : 32;
717 	int family = args->version;
718 	char *str, *dev, *sep;
719 	struct in6_addr *in6;
720 	struct in_addr  *in;
721 	const char *desc;
722 	void *addr;
723 	int rc = 0;
724 
725 	str = strdup(_str);
726 	if (!str)
727 		return -ENOMEM;
728 
729 	switch (atype) {
730 	case ADDR_TYPE_LOCAL:
731 		desc = "local";
732 		addr = &args->local_addr;
733 		break;
734 	case ADDR_TYPE_REMOTE:
735 		desc = "remote";
736 		addr = &args->remote_addr;
737 		break;
738 	case ADDR_TYPE_MCAST:
739 		desc = "mcast grp";
740 		addr = &args->grp;
741 		break;
742 	case ADDR_TYPE_EXPECTED_LOCAL:
743 		desc = "expected local";
744 		addr = &args->expected_laddr;
745 		break;
746 	case ADDR_TYPE_EXPECTED_REMOTE:
747 		desc = "expected remote";
748 		addr = &args->expected_raddr;
749 		break;
750 	case ADDR_TYPE_MD5_PREFIX:
751 		desc = "md5 prefix";
752 		if (family == AF_INET) {
753 			args->md5_prefix.v4.sin_family = AF_INET;
754 			addr = &args->md5_prefix.v4.sin_addr;
755 		} else if (family == AF_INET6) {
756 			args->md5_prefix.v6.sin6_family = AF_INET6;
757 			addr = &args->md5_prefix.v6.sin6_addr;
758 		} else
759 			return 1;
760 
761 		sep = strchr(str, '/');
762 		if (sep) {
763 			*sep = '\0';
764 			sep++;
765 			if (str_to_uint(sep, 1, pfx_len_max,
766 					&args->prefix_len) != 0) {
767 				fprintf(stderr, "Invalid port\n");
768 				return 1;
769 			}
770 		} else {
771 			args->prefix_len = 0;
772 		}
773 		break;
774 	default:
775 		log_error("unknown address type\n");
776 		exit(1);
777 	}
778 
779 	switch (family) {
780 	case AF_INET:
781 		in  = (struct in_addr *) addr;
782 		if (str) {
783 			if (inet_pton(AF_INET, str, in) == 0) {
784 				log_error("Invalid %s IP address\n", desc);
785 				rc = -1;
786 				goto out;
787 			}
788 		} else {
789 			in->s_addr = htonl(INADDR_ANY);
790 		}
791 		break;
792 
793 	case AF_INET6:
794 		dev = strchr(str, '%');
795 		if (dev) {
796 			*dev = '\0';
797 			dev++;
798 		}
799 
800 		in6 = (struct in6_addr *) addr;
801 		if (str) {
802 			if (inet_pton(AF_INET6, str, in6) == 0) {
803 				log_error("Invalid %s IPv6 address\n", desc);
804 				rc = -1;
805 				goto out;
806 			}
807 		} else {
808 			*in6 = in6addr_any;
809 		}
810 		if (dev) {
811 			args->scope_id = get_ifidx(dev);
812 			if (args->scope_id < 0) {
813 				log_error("Invalid scope on %s IPv6 address\n",
814 					  desc);
815 				rc = -1;
816 				goto out;
817 			}
818 		}
819 		break;
820 
821 	default:
822 		log_error("Invalid address family\n");
823 	}
824 
825 out:
826 	free(str);
827 	return rc;
828 }
829 
validate_addresses(struct sock_args * args)830 static int validate_addresses(struct sock_args *args)
831 {
832 	if (args->local_addr_str &&
833 	    convert_addr(args, args->local_addr_str, ADDR_TYPE_LOCAL) < 0)
834 		return 1;
835 
836 	if (args->remote_addr_str &&
837 	    convert_addr(args, args->remote_addr_str, ADDR_TYPE_REMOTE) < 0)
838 		return 1;
839 
840 	if (args->md5_prefix_str &&
841 	    convert_addr(args, args->md5_prefix_str,
842 			 ADDR_TYPE_MD5_PREFIX) < 0)
843 		return 1;
844 
845 	if (args->expected_laddr_str &&
846 	    convert_addr(args, args->expected_laddr_str,
847 			 ADDR_TYPE_EXPECTED_LOCAL))
848 		return 1;
849 
850 	if (args->expected_raddr_str &&
851 	    convert_addr(args, args->expected_raddr_str,
852 			 ADDR_TYPE_EXPECTED_REMOTE))
853 		return 1;
854 
855 	return 0;
856 }
857 
get_index_from_cmsg(struct msghdr * m)858 static int get_index_from_cmsg(struct msghdr *m)
859 {
860 	struct cmsghdr *cm;
861 	int ifindex = 0;
862 	char buf[64];
863 
864 	for (cm = (struct cmsghdr *)CMSG_FIRSTHDR(m);
865 	     m->msg_controllen != 0 && cm;
866 	     cm = (struct cmsghdr *)CMSG_NXTHDR(m, cm)) {
867 
868 		if (cm->cmsg_level == SOL_IP &&
869 		    cm->cmsg_type == IP_PKTINFO) {
870 			struct in_pktinfo *pi;
871 
872 			pi = (struct in_pktinfo *)(CMSG_DATA(cm));
873 			inet_ntop(AF_INET, &pi->ipi_addr, buf, sizeof(buf));
874 			ifindex = pi->ipi_ifindex;
875 		} else if (cm->cmsg_level == SOL_IPV6 &&
876 			   cm->cmsg_type == IPV6_PKTINFO) {
877 			struct in6_pktinfo *pi6;
878 
879 			pi6 = (struct in6_pktinfo *)(CMSG_DATA(cm));
880 			inet_ntop(AF_INET6, &pi6->ipi6_addr, buf, sizeof(buf));
881 			ifindex = pi6->ipi6_ifindex;
882 		}
883 	}
884 
885 	if (ifindex) {
886 		log_msg("    pktinfo: ifindex %d dest addr %s\n",
887 			ifindex, buf);
888 	}
889 	return ifindex;
890 }
891 
send_msg_no_cmsg(int sd,void * addr,socklen_t alen)892 static int send_msg_no_cmsg(int sd, void *addr, socklen_t alen)
893 {
894 	int err;
895 
896 again:
897 	err = sendto(sd, msg, msglen, 0, addr, alen);
898 	if (err < 0) {
899 		if (errno == EACCES && try_broadcast) {
900 			try_broadcast = 0;
901 			if (!set_broadcast(sd))
902 				goto again;
903 			errno = EACCES;
904 		}
905 
906 		log_err_errno("sendto failed");
907 		return 1;
908 	}
909 
910 	return 0;
911 }
912 
send_msg_cmsg(int sd,void * addr,socklen_t alen,int ifindex,int version)913 static int send_msg_cmsg(int sd, void *addr, socklen_t alen,
914 			 int ifindex, int version)
915 {
916 	unsigned char cmsgbuf[64];
917 	struct iovec iov[2];
918 	struct cmsghdr *cm;
919 	struct msghdr m;
920 	int err;
921 
922 	iov[0].iov_base = msg;
923 	iov[0].iov_len = msglen;
924 	m.msg_iov = iov;
925 	m.msg_iovlen = 1;
926 	m.msg_name = (caddr_t)addr;
927 	m.msg_namelen = alen;
928 
929 	memset(cmsgbuf, 0, sizeof(cmsgbuf));
930 	cm = (struct cmsghdr *)cmsgbuf;
931 	m.msg_control = (caddr_t)cm;
932 
933 	if (version == AF_INET) {
934 		struct in_pktinfo *pi;
935 
936 		cm->cmsg_level = SOL_IP;
937 		cm->cmsg_type = IP_PKTINFO;
938 		cm->cmsg_len = CMSG_LEN(sizeof(struct in_pktinfo));
939 		pi = (struct in_pktinfo *)(CMSG_DATA(cm));
940 		pi->ipi_ifindex = ifindex;
941 
942 		m.msg_controllen = cm->cmsg_len;
943 
944 	} else if (version == AF_INET6) {
945 		struct in6_pktinfo *pi6;
946 
947 		cm->cmsg_level = SOL_IPV6;
948 		cm->cmsg_type = IPV6_PKTINFO;
949 		cm->cmsg_len = CMSG_LEN(sizeof(struct in6_pktinfo));
950 
951 		pi6 = (struct in6_pktinfo *)(CMSG_DATA(cm));
952 		pi6->ipi6_ifindex = ifindex;
953 
954 		m.msg_controllen = cm->cmsg_len;
955 	}
956 
957 again:
958 	err = sendmsg(sd, &m, 0);
959 	if (err < 0) {
960 		if (errno == EACCES && try_broadcast) {
961 			try_broadcast = 0;
962 			if (!set_broadcast(sd))
963 				goto again;
964 			errno = EACCES;
965 		}
966 
967 		log_err_errno("sendmsg failed");
968 		return 1;
969 	}
970 
971 	return 0;
972 }
973 
974 
send_msg(int sd,void * addr,socklen_t alen,struct sock_args * args)975 static int send_msg(int sd, void *addr, socklen_t alen, struct sock_args *args)
976 {
977 	if (args->type == SOCK_STREAM) {
978 		if (write(sd, msg, msglen) < 0) {
979 			log_err_errno("write failed sending msg to peer");
980 			return 1;
981 		}
982 	} else if (args->ifindex && args->use_cmsg) {
983 		if (send_msg_cmsg(sd, addr, alen, args->ifindex, args->version))
984 			return 1;
985 	} else {
986 		if (send_msg_no_cmsg(sd, addr, alen))
987 			return 1;
988 	}
989 
990 	log_msg("Sent message:\n");
991 	log_msg("    %.24s%s\n", msg, msglen > 24 ? " ..." : "");
992 
993 	return 0;
994 }
995 
socket_read_dgram(int sd,struct sock_args * args)996 static int socket_read_dgram(int sd, struct sock_args *args)
997 {
998 	unsigned char addr[sizeof(struct sockaddr_in6)];
999 	struct sockaddr *sa = (struct sockaddr *) addr;
1000 	socklen_t alen = sizeof(addr);
1001 	struct iovec iov[2];
1002 	struct msghdr m = {
1003 		.msg_name = (caddr_t)addr,
1004 		.msg_namelen = alen,
1005 		.msg_iov = iov,
1006 		.msg_iovlen = 1,
1007 	};
1008 	unsigned char cmsgbuf[256];
1009 	struct cmsghdr *cm = (struct cmsghdr *)cmsgbuf;
1010 	char buf[16*1024];
1011 	int ifindex;
1012 	int len;
1013 
1014 	iov[0].iov_base = (caddr_t)buf;
1015 	iov[0].iov_len = sizeof(buf);
1016 
1017 	memset(cmsgbuf, 0, sizeof(cmsgbuf));
1018 	m.msg_control = (caddr_t)cm;
1019 	m.msg_controllen = sizeof(cmsgbuf);
1020 
1021 	len = recvmsg(sd, &m, 0);
1022 	if (len == 0) {
1023 		log_msg("peer closed connection.\n");
1024 		return 0;
1025 	} else if (len < 0) {
1026 		log_msg("failed to read message: %d: %s\n",
1027 			errno, strerror(errno));
1028 		return -1;
1029 	}
1030 
1031 	buf[len] = '\0';
1032 
1033 	log_address("Message from:", sa);
1034 	log_msg("    %.24s%s\n", buf, len > 24 ? " ..." : "");
1035 
1036 	ifindex = get_index_from_cmsg(&m);
1037 	if (args->expected_ifindex) {
1038 		if (args->expected_ifindex != ifindex) {
1039 			log_error("Device index mismatch: expected %d have %d\n",
1040 				  args->expected_ifindex, ifindex);
1041 			return -1;
1042 		}
1043 		log_msg("Device index matches: expected %d have %d\n",
1044 			args->expected_ifindex, ifindex);
1045 	}
1046 
1047 	if (!interactive && server_mode) {
1048 		if (sa->sa_family == AF_INET6) {
1049 			struct sockaddr_in6 *s6 = (struct sockaddr_in6 *) sa;
1050 			struct in6_addr *in6 = &s6->sin6_addr;
1051 
1052 			if (IN6_IS_ADDR_V4MAPPED(in6)) {
1053 				const uint32_t *pa = (uint32_t *) &in6->s6_addr;
1054 				struct in_addr in4;
1055 				struct sockaddr_in *sin;
1056 
1057 				sin = (struct sockaddr_in *) addr;
1058 				pa += 3;
1059 				in4.s_addr = *pa;
1060 				sin->sin_addr = in4;
1061 				sin->sin_family = AF_INET;
1062 				if (send_msg_cmsg(sd, addr, alen,
1063 						  ifindex, AF_INET) < 0)
1064 					goto out_err;
1065 			}
1066 		}
1067 again:
1068 		iov[0].iov_len = len;
1069 
1070 		if (args->version == AF_INET6) {
1071 			struct sockaddr_in6 *s6 = (struct sockaddr_in6 *) sa;
1072 
1073 			if (args->dev) {
1074 				/* avoid PKTINFO conflicts with bindtodev */
1075 				if (sendto(sd, buf, len, 0,
1076 					   (void *) addr, alen) < 0)
1077 					goto out_err;
1078 			} else {
1079 				/* kernel is allowing scope_id to be set to VRF
1080 				 * index for LLA. for sends to global address
1081 				 * reset scope id
1082 				 */
1083 				s6->sin6_scope_id = ifindex;
1084 				if (sendmsg(sd, &m, 0) < 0)
1085 					goto out_err;
1086 			}
1087 		} else {
1088 			int err;
1089 
1090 			err = sendmsg(sd, &m, 0);
1091 			if (err < 0) {
1092 				if (errno == EACCES && try_broadcast) {
1093 					try_broadcast = 0;
1094 					if (!set_broadcast(sd))
1095 						goto again;
1096 					errno = EACCES;
1097 				}
1098 				goto out_err;
1099 			}
1100 		}
1101 		log_msg("Sent message:\n");
1102 		log_msg("    %.24s%s\n", buf, len > 24 ? " ..." : "");
1103 	}
1104 
1105 	return 1;
1106 out_err:
1107 	log_err_errno("failed to send msg to peer");
1108 	return -1;
1109 }
1110 
socket_read_stream(int sd)1111 static int socket_read_stream(int sd)
1112 {
1113 	char buf[1024];
1114 	int len;
1115 
1116 	len = read(sd, buf, sizeof(buf)-1);
1117 	if (len == 0) {
1118 		log_msg("client closed connection.\n");
1119 		return 0;
1120 	} else if (len < 0) {
1121 		log_msg("failed to read message\n");
1122 		return -1;
1123 	}
1124 
1125 	buf[len] = '\0';
1126 	log_msg("Incoming message:\n");
1127 	log_msg("    %.24s%s\n", buf, len > 24 ? " ..." : "");
1128 
1129 	if (!interactive && server_mode) {
1130 		if (write(sd, buf, len) < 0) {
1131 			log_err_errno("failed to send buf");
1132 			return -1;
1133 		}
1134 		log_msg("Sent message:\n");
1135 		log_msg("     %.24s%s\n", buf, len > 24 ? " ..." : "");
1136 	}
1137 
1138 	return 1;
1139 }
1140 
socket_read(int sd,struct sock_args * args)1141 static int socket_read(int sd, struct sock_args *args)
1142 {
1143 	if (args->type == SOCK_STREAM)
1144 		return socket_read_stream(sd);
1145 
1146 	return socket_read_dgram(sd, args);
1147 }
1148 
stdin_to_socket(int sd,int type,void * addr,socklen_t alen)1149 static int stdin_to_socket(int sd, int type, void *addr, socklen_t alen)
1150 {
1151 	char buf[1024];
1152 	int len;
1153 
1154 	if (fgets(buf, sizeof(buf), stdin) == NULL)
1155 		return 0;
1156 
1157 	len = strlen(buf);
1158 	if (type == SOCK_STREAM) {
1159 		if (write(sd, buf, len) < 0) {
1160 			log_err_errno("failed to send buf");
1161 			return -1;
1162 		}
1163 	} else {
1164 		int err;
1165 
1166 again:
1167 		err = sendto(sd, buf, len, 0, addr, alen);
1168 		if (err < 0) {
1169 			if (errno == EACCES && try_broadcast) {
1170 				try_broadcast = 0;
1171 				if (!set_broadcast(sd))
1172 					goto again;
1173 				errno = EACCES;
1174 			}
1175 			log_err_errno("failed to send msg to peer");
1176 			return -1;
1177 		}
1178 	}
1179 	log_msg("Sent message:\n");
1180 	log_msg("    %.24s%s\n", buf, len > 24 ? " ..." : "");
1181 
1182 	return 1;
1183 }
1184 
set_recv_attr(int sd,int version)1185 static void set_recv_attr(int sd, int version)
1186 {
1187 	if (version == AF_INET6) {
1188 		set_recvpktinfo_v6(sd);
1189 		set_recverr_v6(sd);
1190 	} else {
1191 		set_pktinfo_v4(sd);
1192 		set_recverr_v4(sd);
1193 	}
1194 }
1195 
msg_loop(int client,int sd,void * addr,socklen_t alen,struct sock_args * args)1196 static int msg_loop(int client, int sd, void *addr, socklen_t alen,
1197 		    struct sock_args *args)
1198 {
1199 	struct timeval timeout = { .tv_sec = prog_timeout }, *ptval = NULL;
1200 	fd_set rfds;
1201 	int nfds;
1202 	int rc;
1203 
1204 	if (args->type != SOCK_STREAM)
1205 		set_recv_attr(sd, args->version);
1206 
1207 	if (msg) {
1208 		msglen = strlen(msg);
1209 
1210 		/* client sends first message */
1211 		if (client) {
1212 			if (send_msg(sd, addr, alen, args))
1213 				return 1;
1214 		}
1215 		if (!interactive) {
1216 			ptval = &timeout;
1217 			if (!prog_timeout)
1218 				timeout.tv_sec = 5;
1219 		}
1220 	}
1221 
1222 	nfds = interactive ? MAX(fileno(stdin), sd)  + 1 : sd + 1;
1223 	while (1) {
1224 		FD_ZERO(&rfds);
1225 		FD_SET(sd, &rfds);
1226 		if (interactive)
1227 			FD_SET(fileno(stdin), &rfds);
1228 
1229 		rc = select(nfds, &rfds, NULL, NULL, ptval);
1230 		if (rc < 0) {
1231 			if (errno == EINTR)
1232 				continue;
1233 
1234 			rc = 1;
1235 			log_err_errno("select failed");
1236 			break;
1237 		} else if (rc == 0) {
1238 			log_error("Timed out waiting for response\n");
1239 			rc = 2;
1240 			break;
1241 		}
1242 
1243 		if (FD_ISSET(sd, &rfds)) {
1244 			rc = socket_read(sd, args);
1245 			if (rc < 0) {
1246 				rc = 1;
1247 				break;
1248 			}
1249 			if (rc == 0)
1250 				break;
1251 		}
1252 
1253 		rc = 0;
1254 
1255 		if (FD_ISSET(fileno(stdin), &rfds)) {
1256 			if (stdin_to_socket(sd, args->type, addr, alen) <= 0)
1257 				break;
1258 		}
1259 
1260 		if (interactive)
1261 			continue;
1262 
1263 		if (iter != -1) {
1264 			--iter;
1265 			if (iter == 0)
1266 				break;
1267 		}
1268 
1269 		log_msg("Going into quiet mode\n");
1270 		quiet = 1;
1271 
1272 		if (client) {
1273 			if (send_msg(sd, addr, alen, args)) {
1274 				rc = 1;
1275 				break;
1276 			}
1277 		}
1278 	}
1279 
1280 	return rc;
1281 }
1282 
msock_init(struct sock_args * args,int server)1283 static int msock_init(struct sock_args *args, int server)
1284 {
1285 	uint32_t if_addr = htonl(INADDR_ANY);
1286 	struct sockaddr_in laddr = {
1287 		.sin_family = AF_INET,
1288 		.sin_port = htons(args->port),
1289 	};
1290 	int one = 1;
1291 	int sd;
1292 
1293 	if (!server && args->has_local_ip)
1294 		if_addr = args->local_addr.in.s_addr;
1295 
1296 	sd = socket(PF_INET, SOCK_DGRAM, 0);
1297 	if (sd < 0) {
1298 		log_err_errno("socket");
1299 		return -1;
1300 	}
1301 
1302 	if (setsockopt(sd, SOL_SOCKET, SO_REUSEADDR,
1303 		       (char *)&one, sizeof(one)) < 0) {
1304 		log_err_errno("Setting SO_REUSEADDR error");
1305 		goto out_err;
1306 	}
1307 
1308 	if (setsockopt(sd, SOL_SOCKET, SO_BROADCAST,
1309 		       (char *)&one, sizeof(one)) < 0)
1310 		log_err_errno("Setting SO_BROADCAST error");
1311 
1312 	if (args->dev && bind_to_device(sd, args->dev) != 0)
1313 		goto out_err;
1314 	else if (args->use_setsockopt &&
1315 		 set_multicast_if(sd, args->ifindex))
1316 		goto out_err;
1317 
1318 	laddr.sin_addr.s_addr = if_addr;
1319 
1320 	if (bind(sd, (struct sockaddr *) &laddr, sizeof(laddr)) < 0) {
1321 		log_err_errno("bind failed");
1322 		goto out_err;
1323 	}
1324 
1325 	if (server &&
1326 	    set_membership(sd, args->grp.s_addr,
1327 			   args->local_addr.in.s_addr, args->ifindex))
1328 		goto out_err;
1329 
1330 	return sd;
1331 out_err:
1332 	close(sd);
1333 	return -1;
1334 }
1335 
msock_server(struct sock_args * args)1336 static int msock_server(struct sock_args *args)
1337 {
1338 	return msock_init(args, 1);
1339 }
1340 
msock_client(struct sock_args * args)1341 static int msock_client(struct sock_args *args)
1342 {
1343 	return msock_init(args, 0);
1344 }
1345 
bind_socket(int sd,struct sock_args * args)1346 static int bind_socket(int sd, struct sock_args *args)
1347 {
1348 	struct sockaddr_in serv_addr = {
1349 		.sin_family = AF_INET,
1350 	};
1351 	struct sockaddr_in6 serv6_addr = {
1352 		.sin6_family = AF_INET6,
1353 	};
1354 	void *addr;
1355 	socklen_t alen;
1356 
1357 	if (!args->has_local_ip && args->type == SOCK_RAW)
1358 		return 0;
1359 
1360 	switch (args->version) {
1361 	case AF_INET:
1362 		serv_addr.sin_port = htons(args->port);
1363 		serv_addr.sin_addr = args->local_addr.in;
1364 		addr = &serv_addr;
1365 		alen = sizeof(serv_addr);
1366 		break;
1367 
1368 	case AF_INET6:
1369 		serv6_addr.sin6_port = htons(args->port);
1370 		serv6_addr.sin6_addr = args->local_addr.in6;
1371 		addr = &serv6_addr;
1372 		alen = sizeof(serv6_addr);
1373 		break;
1374 
1375 	default:
1376 		log_error("Invalid address family\n");
1377 		return -1;
1378 	}
1379 
1380 	if (bind(sd, addr, alen) < 0) {
1381 		log_err_errno("error binding socket");
1382 		return -1;
1383 	}
1384 
1385 	return 0;
1386 }
1387 
config_xfrm_policy(int sd,struct sock_args * args)1388 static int config_xfrm_policy(int sd, struct sock_args *args)
1389 {
1390 	struct xfrm_userpolicy_info policy = {};
1391 	int type = UDP_ENCAP_ESPINUDP;
1392 	int xfrm_af = IP_XFRM_POLICY;
1393 	int level = SOL_IP;
1394 
1395 	if (args->type != SOCK_DGRAM) {
1396 		log_error("Invalid socket type. Only DGRAM could be used for XFRM\n");
1397 		return 1;
1398 	}
1399 
1400 	policy.action = XFRM_POLICY_ALLOW;
1401 	policy.sel.family = args->version;
1402 	if (args->version == AF_INET6) {
1403 		xfrm_af = IPV6_XFRM_POLICY;
1404 		level = SOL_IPV6;
1405 	}
1406 
1407 	policy.dir = XFRM_POLICY_OUT;
1408 	if (setsockopt(sd, level, xfrm_af, &policy, sizeof(policy)) < 0)
1409 		return 1;
1410 
1411 	policy.dir = XFRM_POLICY_IN;
1412 	if (setsockopt(sd, level, xfrm_af, &policy, sizeof(policy)) < 0)
1413 		return 1;
1414 
1415 	if (setsockopt(sd, IPPROTO_UDP, UDP_ENCAP, &type, sizeof(type)) < 0) {
1416 		log_err_errno("Failed to set xfrm encap");
1417 		return 1;
1418 	}
1419 
1420 	return 0;
1421 }
1422 
lsock_init(struct sock_args * args)1423 static int lsock_init(struct sock_args *args)
1424 {
1425 	long flags;
1426 	int sd;
1427 
1428 	sd = socket(args->version, args->type, args->protocol);
1429 	if (sd < 0) {
1430 		log_err_errno("Error opening socket");
1431 		return  -1;
1432 	}
1433 
1434 	if (set_reuseaddr(sd) != 0)
1435 		goto err;
1436 
1437 	if (set_reuseport(sd) != 0)
1438 		goto err;
1439 
1440 	if (args->dev && bind_to_device(sd, args->dev) != 0)
1441 		goto err;
1442 	else if (args->use_setsockopt &&
1443 		 set_unicast_if(sd, args->ifindex, args->version))
1444 		goto err;
1445 
1446 	if (args->use_freebind && set_freebind(sd, args->version))
1447 		goto err;
1448 
1449 	if (bind_socket(sd, args))
1450 		goto err;
1451 
1452 	if (args->bind_test_only)
1453 		goto out;
1454 
1455 	if (args->type == SOCK_STREAM && listen(sd, 1) < 0) {
1456 		log_err_errno("listen failed");
1457 		goto err;
1458 	}
1459 
1460 	flags = fcntl(sd, F_GETFL);
1461 	if ((flags < 0) || (fcntl(sd, F_SETFL, flags|O_NONBLOCK) < 0)) {
1462 		log_err_errno("Failed to set non-blocking option");
1463 		goto err;
1464 	}
1465 
1466 	if (fcntl(sd, F_SETFD, FD_CLOEXEC) < 0)
1467 		log_err_errno("Failed to set close-on-exec flag");
1468 
1469 	if (args->use_xfrm && config_xfrm_policy(sd, args)) {
1470 		log_err_errno("Failed to set xfrm policy");
1471 		goto err;
1472 	}
1473 
1474 out:
1475 	return sd;
1476 
1477 err:
1478 	close(sd);
1479 	return -1;
1480 }
1481 
ipc_write(int fd,int message)1482 static void ipc_write(int fd, int message)
1483 {
1484 	/* Not in both_mode, so there's no process to signal */
1485 	if (fd < 0)
1486 		return;
1487 
1488 	if (write(fd, &message, sizeof(message)) < 0)
1489 		log_err_errno("Failed to send client status");
1490 }
1491 
do_server(struct sock_args * args,int ipc_fd)1492 static int do_server(struct sock_args *args, int ipc_fd)
1493 {
1494 	/* ipc_fd = -1 if no parent process to signal */
1495 	struct timeval timeout = { .tv_sec = prog_timeout }, *ptval = NULL;
1496 	unsigned char addr[sizeof(struct sockaddr_in6)] = {};
1497 	socklen_t alen = sizeof(addr);
1498 	int lsd, csd = -1;
1499 
1500 	fd_set rfds;
1501 	int rc;
1502 
1503 	if (args->serverns) {
1504 		if (switch_ns(args->serverns)) {
1505 			log_error("Could not set server netns to %s\n",
1506 				  args->serverns);
1507 			goto err_exit;
1508 		}
1509 		log_msg("Switched server netns\n");
1510 	}
1511 
1512 	args->dev = args->server_dev;
1513 	args->expected_dev = args->expected_server_dev;
1514 	if (resolve_devices(args) || validate_addresses(args))
1515 		goto err_exit;
1516 
1517 	if (prog_timeout)
1518 		ptval = &timeout;
1519 
1520 	if (args->has_grp)
1521 		lsd = msock_server(args);
1522 	else
1523 		lsd = lsock_init(args);
1524 
1525 	if (lsd < 0)
1526 		goto err_exit;
1527 
1528 	if (args->bind_test_only) {
1529 		close(lsd);
1530 		ipc_write(ipc_fd, 1);
1531 		return 0;
1532 	}
1533 
1534 	if (args->type != SOCK_STREAM) {
1535 		ipc_write(ipc_fd, 1);
1536 		rc = msg_loop(0, lsd, (void *) addr, alen, args);
1537 		close(lsd);
1538 		return rc;
1539 	}
1540 
1541 	if (args->password && tcp_md5_remote(lsd, args)) {
1542 		close(lsd);
1543 		goto err_exit;
1544 	}
1545 
1546 	ipc_write(ipc_fd, 1);
1547 	while (1) {
1548 		log_msg("waiting for client connection.\n");
1549 		FD_ZERO(&rfds);
1550 		FD_SET(lsd, &rfds);
1551 
1552 		rc = select(lsd+1, &rfds, NULL, NULL, ptval);
1553 		if (rc == 0) {
1554 			rc = 2;
1555 			break;
1556 		}
1557 
1558 		if (rc < 0) {
1559 			if (errno == EINTR)
1560 				continue;
1561 
1562 			log_err_errno("select failed");
1563 			break;
1564 		}
1565 
1566 		if (FD_ISSET(lsd, &rfds)) {
1567 
1568 			csd = accept(lsd, (void *) addr, &alen);
1569 			if (csd < 0) {
1570 				log_err_errno("accept failed");
1571 				break;
1572 			}
1573 
1574 			rc = show_sockstat(csd, args);
1575 			if (rc)
1576 				break;
1577 
1578 			rc = check_device(csd, args);
1579 			if (rc)
1580 				break;
1581 		}
1582 
1583 		rc = msg_loop(0, csd, (void *) addr, alen, args);
1584 		close(csd);
1585 
1586 		if (!interactive)
1587 			break;
1588 	}
1589 
1590 	close(lsd);
1591 
1592 	return rc;
1593 err_exit:
1594 	ipc_write(ipc_fd, 0);
1595 	return 1;
1596 }
1597 
wait_for_connect(int sd)1598 static int wait_for_connect(int sd)
1599 {
1600 	struct timeval _tv = { .tv_sec = prog_timeout }, *tv = NULL;
1601 	fd_set wfd;
1602 	int val = 0, sz = sizeof(val);
1603 	int rc;
1604 
1605 	FD_ZERO(&wfd);
1606 	FD_SET(sd, &wfd);
1607 
1608 	if (prog_timeout)
1609 		tv = &_tv;
1610 
1611 	rc = select(FD_SETSIZE, NULL, &wfd, NULL, tv);
1612 	if (rc == 0) {
1613 		log_error("connect timed out\n");
1614 		return -2;
1615 	} else if (rc < 0) {
1616 		log_err_errno("select failed");
1617 		return -3;
1618 	}
1619 
1620 	if (getsockopt(sd, SOL_SOCKET, SO_ERROR, &val, (socklen_t *)&sz) < 0) {
1621 		log_err_errno("getsockopt(SO_ERROR) failed");
1622 		return -4;
1623 	}
1624 
1625 	if (val != 0) {
1626 		log_error("connect failed: %d: %s\n", val, strerror(val));
1627 		return -1;
1628 	}
1629 
1630 	return 0;
1631 }
1632 
connectsock(void * addr,socklen_t alen,struct sock_args * args)1633 static int connectsock(void *addr, socklen_t alen, struct sock_args *args)
1634 {
1635 	int sd, rc = -1;
1636 	long flags;
1637 
1638 	sd = socket(args->version, args->type, args->protocol);
1639 	if (sd < 0) {
1640 		log_err_errno("Failed to create socket");
1641 		return -1;
1642 	}
1643 
1644 	flags = fcntl(sd, F_GETFL);
1645 	if ((flags < 0) || (fcntl(sd, F_SETFL, flags|O_NONBLOCK) < 0)) {
1646 		log_err_errno("Failed to set non-blocking option");
1647 		goto err;
1648 	}
1649 
1650 	if (set_reuseport(sd) != 0)
1651 		goto err;
1652 
1653 	if (args->dev && bind_to_device(sd, args->dev) != 0)
1654 		goto err;
1655 	else if (args->use_setsockopt &&
1656 		 set_unicast_if(sd, args->ifindex, args->version))
1657 		goto err;
1658 
1659 	if (args->has_local_ip && bind_socket(sd, args))
1660 		goto err;
1661 
1662 	if (args->type != SOCK_STREAM)
1663 		goto out;
1664 
1665 	if (args->password && tcp_md5sig(sd, addr, alen, args))
1666 		goto err;
1667 
1668 	if (args->bind_test_only)
1669 		goto out;
1670 
1671 	if (connect(sd, addr, alen) < 0) {
1672 		if (errno != EINPROGRESS) {
1673 			log_err_errno("Failed to connect to remote host");
1674 			rc = -1;
1675 			goto err;
1676 		}
1677 		rc = wait_for_connect(sd);
1678 		if (rc < 0)
1679 			goto err;
1680 	}
1681 out:
1682 	return sd;
1683 
1684 err:
1685 	close(sd);
1686 	return rc;
1687 }
1688 
do_client(struct sock_args * args)1689 static int do_client(struct sock_args *args)
1690 {
1691 	struct sockaddr_in sin = {
1692 		.sin_family = AF_INET,
1693 	};
1694 	struct sockaddr_in6 sin6 = {
1695 		.sin6_family = AF_INET6,
1696 	};
1697 	void *addr;
1698 	int alen;
1699 	int rc = 0;
1700 	int sd;
1701 
1702 	if (!args->has_remote_ip && !args->has_grp) {
1703 		fprintf(stderr, "remote IP or multicast group not given\n");
1704 		return 1;
1705 	}
1706 
1707 	if (args->clientns) {
1708 		if (switch_ns(args->clientns)) {
1709 			log_error("Could not set client netns to %s\n",
1710 				  args->clientns);
1711 			return 1;
1712 		}
1713 		log_msg("Switched client netns\n");
1714 	}
1715 
1716 	args->local_addr_str = args->client_local_addr_str;
1717 	if (resolve_devices(args) || validate_addresses(args))
1718 		return 1;
1719 
1720 	if ((args->use_setsockopt || args->use_cmsg) && !args->ifindex) {
1721 		fprintf(stderr, "Device binding not specified\n");
1722 		return 1;
1723 	}
1724 	if (args->use_setsockopt || args->use_cmsg)
1725 		args->dev = NULL;
1726 
1727 	switch (args->version) {
1728 	case AF_INET:
1729 		sin.sin_port = htons(args->port);
1730 		if (args->has_grp)
1731 			sin.sin_addr = args->grp;
1732 		else
1733 			sin.sin_addr = args->remote_addr.in;
1734 		addr = &sin;
1735 		alen = sizeof(sin);
1736 		break;
1737 	case AF_INET6:
1738 		sin6.sin6_port = htons(args->port);
1739 		sin6.sin6_addr = args->remote_addr.in6;
1740 		sin6.sin6_scope_id = args->scope_id;
1741 		addr = &sin6;
1742 		alen = sizeof(sin6);
1743 		break;
1744 	}
1745 
1746 	args->password = args->client_pw;
1747 
1748 	if (args->has_grp)
1749 		sd = msock_client(args);
1750 	else
1751 		sd = connectsock(addr, alen, args);
1752 
1753 	if (sd < 0)
1754 		return -sd;
1755 
1756 	if (args->bind_test_only)
1757 		goto out;
1758 
1759 	if (args->type == SOCK_STREAM) {
1760 		rc = show_sockstat(sd, args);
1761 		if (rc != 0)
1762 			goto out;
1763 	}
1764 
1765 	rc = msg_loop(1, sd, addr, alen, args);
1766 
1767 out:
1768 	close(sd);
1769 
1770 	return rc;
1771 }
1772 
random_msg(int len)1773 static char *random_msg(int len)
1774 {
1775 	int i, n = 0, olen = len + 1;
1776 	char *m;
1777 
1778 	if (len <= 0)
1779 		return NULL;
1780 
1781 	m = malloc(olen);
1782 	if (!m)
1783 		return NULL;
1784 
1785 	while (len > 26) {
1786 		i = snprintf(m + n, olen - n, "%.26s",
1787 			     "abcdefghijklmnopqrstuvwxyz");
1788 		n += i;
1789 		len -= i;
1790 	}
1791 	i = snprintf(m + n, olen - n, "%.*s", len,
1792 		     "abcdefghijklmnopqrstuvwxyz");
1793 	return m;
1794 }
1795 
ipc_child(int fd,struct sock_args * args)1796 static int ipc_child(int fd, struct sock_args *args)
1797 {
1798 	char *outbuf, *errbuf;
1799 	int rc = 1;
1800 
1801 	outbuf = malloc(4096);
1802 	errbuf = malloc(4096);
1803 	if (!outbuf || !errbuf) {
1804 		fprintf(stderr, "server: Failed to allocate buffers for stdout and stderr\n");
1805 		goto out;
1806 	}
1807 
1808 	setbuffer(stdout, outbuf, 4096);
1809 	setbuffer(stderr, errbuf, 4096);
1810 
1811 	server_mode = 1; /* to tell log_msg in case we are in both_mode */
1812 
1813 	/* when running in both mode, address validation applies
1814 	 * solely to client side
1815 	 */
1816 	args->has_expected_laddr = 0;
1817 	args->has_expected_raddr = 0;
1818 
1819 	rc = do_server(args, fd);
1820 
1821 out:
1822 	free(outbuf);
1823 	free(errbuf);
1824 
1825 	return rc;
1826 }
1827 
ipc_parent(int cpid,int fd,struct sock_args * args)1828 static int ipc_parent(int cpid, int fd, struct sock_args *args)
1829 {
1830 	int client_status;
1831 	int status;
1832 	int buf;
1833 
1834 	/* do the client-side function here in the parent process,
1835 	 * waiting to be told when to continue
1836 	 */
1837 	if (read(fd, &buf, sizeof(buf)) <= 0) {
1838 		log_err_errno("Failed to read IPC status from status");
1839 		return 1;
1840 	}
1841 	if (!buf) {
1842 		log_error("Server failed; can not continue\n");
1843 		return 1;
1844 	}
1845 	log_msg("Server is ready\n");
1846 
1847 	client_status = do_client(args);
1848 	log_msg("parent is done!\n");
1849 
1850 	if (kill(cpid, 0) == 0)
1851 		kill(cpid, SIGKILL);
1852 
1853 	wait(&status);
1854 	return client_status;
1855 }
1856 
1857 #define GETOPT_STR  "sr:l:c:p:t:g:P:DRn:M:X:m:d:I:BN:O:SCi6xL:0:1:2:3:Fbqf"
1858 #define OPT_FORCE_BIND_KEY_IFINDEX 1001
1859 #define OPT_NO_BIND_KEY_IFINDEX 1002
1860 
1861 static struct option long_opts[] = {
1862 	{"force-bind-key-ifindex", 0, 0, OPT_FORCE_BIND_KEY_IFINDEX},
1863 	{"no-bind-key-ifindex", 0, 0, OPT_NO_BIND_KEY_IFINDEX},
1864 	{0, 0, 0, 0}
1865 };
1866 
print_usage(char * prog)1867 static void print_usage(char *prog)
1868 {
1869 	printf(
1870 	"usage: %s OPTS\n"
1871 	"Required:\n"
1872 	"    -r addr       remote address to connect to (client mode only)\n"
1873 	"    -p port       port to connect to (client mode)/listen on (server mode)\n"
1874 	"                  (default: %d)\n"
1875 	"    -s            server mode (default: client mode)\n"
1876 	"    -t            timeout seconds (default: none)\n"
1877 	"\n"
1878 	"Optional:\n"
1879 	"    -B            do both client and server via fork and IPC\n"
1880 	"    -N ns         set client to network namespace ns (requires root)\n"
1881 	"    -O ns         set server to network namespace ns (requires root)\n"
1882 	"    -F            Restart server loop\n"
1883 	"    -6            IPv6 (default is IPv4)\n"
1884 	"    -P proto      protocol for socket: icmp, ospf (default: none)\n"
1885 	"    -D|R          datagram (D) / raw (R) socket (default stream)\n"
1886 	"    -l addr       local address to bind to in server mode\n"
1887 	"    -c addr       local address to bind to in client mode\n"
1888 	"    -x            configure XFRM policy on socket\n"
1889 	"\n"
1890 	"    -d dev        bind socket to given device name\n"
1891 	"    -I dev        bind socket to given device name - server mode\n"
1892 	"    -S            use setsockopt (IP_UNICAST_IF or IP_MULTICAST_IF)\n"
1893 	"                  to set device binding\n"
1894 	"    -f            bind socket with the IP[V6]_FREEBIND option\n"
1895 	"    -C            use cmsg and IP_PKTINFO to specify device binding\n"
1896 	"\n"
1897 	"    -L len        send random message of given length\n"
1898 	"    -n num        number of times to send message\n"
1899 	"\n"
1900 	"    -M password   use MD5 sum protection\n"
1901 	"    -X password   MD5 password for client mode\n"
1902 	"    -m prefix/len prefix and length to use for MD5 key\n"
1903 	"    --no-bind-key-ifindex: Force TCP_MD5SIG_FLAG_IFINDEX off\n"
1904 	"    --force-bind-key-ifindex: Force TCP_MD5SIG_FLAG_IFINDEX on\n"
1905 	"        (default: only if -I is passed)\n"
1906 	"\n"
1907 	"    -g grp        multicast group (e.g., 239.1.1.1)\n"
1908 	"    -i            interactive mode (default is echo and terminate)\n"
1909 	"\n"
1910 	"    -0 addr       Expected local address\n"
1911 	"    -1 addr       Expected remote address\n"
1912 	"    -2 dev        Expected device name (or index) to receive packet\n"
1913 	"    -3 dev        Expected device name (or index) to receive packets - server mode\n"
1914 	"\n"
1915 	"    -b            Bind test only.\n"
1916 	"    -q            Be quiet. Run test without printing anything.\n"
1917 	, prog, DEFAULT_PORT);
1918 }
1919 
main(int argc,char * argv[])1920 int main(int argc, char *argv[])
1921 {
1922 	struct sock_args args = {
1923 		.version = AF_INET,
1924 		.type    = SOCK_STREAM,
1925 		.port    = DEFAULT_PORT,
1926 	};
1927 	struct protoent *pe;
1928 	int both_mode = 0;
1929 	unsigned int tmp;
1930 	int forever = 0;
1931 	int fd[2];
1932 	int cpid;
1933 
1934 	/* process inputs */
1935 	extern char *optarg;
1936 	int rc = 0;
1937 
1938 	/*
1939 	 * process input args
1940 	 */
1941 
1942 	while ((rc = getopt_long(argc, argv, GETOPT_STR, long_opts, NULL)) != -1) {
1943 		switch (rc) {
1944 		case 'B':
1945 			both_mode = 1;
1946 			break;
1947 		case 's':
1948 			server_mode = 1;
1949 			break;
1950 		case 'F':
1951 			forever = 1;
1952 			break;
1953 		case 'l':
1954 			args.has_local_ip = 1;
1955 			args.local_addr_str = optarg;
1956 			break;
1957 		case 'r':
1958 			args.has_remote_ip = 1;
1959 			args.remote_addr_str = optarg;
1960 			break;
1961 		case 'c':
1962 			args.has_local_ip = 1;
1963 			args.client_local_addr_str = optarg;
1964 			break;
1965 		case 'p':
1966 			if (str_to_uint(optarg, 1, 65535, &tmp) != 0) {
1967 				fprintf(stderr, "Invalid port\n");
1968 				return 1;
1969 			}
1970 			args.port = (unsigned short) tmp;
1971 			break;
1972 		case 't':
1973 			if (str_to_uint(optarg, 0, INT_MAX,
1974 					&prog_timeout) != 0) {
1975 				fprintf(stderr, "Invalid timeout\n");
1976 				return 1;
1977 			}
1978 			break;
1979 		case 'D':
1980 			args.type = SOCK_DGRAM;
1981 			break;
1982 		case 'R':
1983 			args.type = SOCK_RAW;
1984 			args.port = 0;
1985 			if (!args.protocol)
1986 				args.protocol = IPPROTO_RAW;
1987 			break;
1988 		case 'P':
1989 			pe = getprotobyname(optarg);
1990 			if (pe) {
1991 				args.protocol = pe->p_proto;
1992 			} else {
1993 				if (str_to_uint(optarg, 0, 0xffff, &tmp) != 0) {
1994 					fprintf(stderr, "Invalid protocol\n");
1995 					return 1;
1996 				}
1997 				args.protocol = tmp;
1998 			}
1999 			break;
2000 		case 'n':
2001 			iter = atoi(optarg);
2002 			break;
2003 		case 'N':
2004 			args.clientns = optarg;
2005 			break;
2006 		case 'O':
2007 			args.serverns = optarg;
2008 			break;
2009 		case 'L':
2010 			msg = random_msg(atoi(optarg));
2011 			break;
2012 		case 'M':
2013 			args.password = optarg;
2014 			break;
2015 		case OPT_FORCE_BIND_KEY_IFINDEX:
2016 			args.bind_key_ifindex = 1;
2017 			break;
2018 		case OPT_NO_BIND_KEY_IFINDEX:
2019 			args.bind_key_ifindex = -1;
2020 			break;
2021 		case 'X':
2022 			args.client_pw = optarg;
2023 			break;
2024 		case 'm':
2025 			args.md5_prefix_str = optarg;
2026 			break;
2027 		case 'S':
2028 			args.use_setsockopt = 1;
2029 			break;
2030 		case 'f':
2031 			args.use_freebind = 1;
2032 			break;
2033 		case 'C':
2034 			args.use_cmsg = 1;
2035 			break;
2036 		case 'd':
2037 			args.dev = optarg;
2038 			break;
2039 		case 'I':
2040 			args.server_dev = optarg;
2041 			break;
2042 		case 'i':
2043 			interactive = 1;
2044 			break;
2045 		case 'g':
2046 			args.has_grp = 1;
2047 			if (convert_addr(&args, optarg, ADDR_TYPE_MCAST) < 0)
2048 				return 1;
2049 			args.type = SOCK_DGRAM;
2050 			break;
2051 		case '6':
2052 			args.version = AF_INET6;
2053 			break;
2054 		case 'b':
2055 			args.bind_test_only = 1;
2056 			break;
2057 		case '0':
2058 			args.has_expected_laddr = 1;
2059 			args.expected_laddr_str = optarg;
2060 			break;
2061 		case '1':
2062 			args.has_expected_raddr = 1;
2063 			args.expected_raddr_str = optarg;
2064 			break;
2065 		case '2':
2066 			args.expected_dev = optarg;
2067 			break;
2068 		case '3':
2069 			args.expected_server_dev = optarg;
2070 			break;
2071 		case 'q':
2072 			quiet = 1;
2073 			break;
2074 		case 'x':
2075 			args.use_xfrm = 1;
2076 			break;
2077 		default:
2078 			print_usage(argv[0]);
2079 			return 1;
2080 		}
2081 	}
2082 
2083 	if (args.password &&
2084 	    ((!args.has_remote_ip && !args.md5_prefix_str) ||
2085 	      args.type != SOCK_STREAM)) {
2086 		log_error("MD5 passwords apply to TCP only and require a remote ip for the password\n");
2087 		return 1;
2088 	}
2089 
2090 	if (args.md5_prefix_str && !args.password) {
2091 		log_error("Prefix range for MD5 protection specified without a password\n");
2092 		return 1;
2093 	}
2094 
2095 	if (iter == 0) {
2096 		fprintf(stderr, "Invalid number of messages to send\n");
2097 		return 1;
2098 	}
2099 
2100 	if (args.type == SOCK_STREAM && !args.protocol)
2101 		args.protocol = IPPROTO_TCP;
2102 	if (args.type == SOCK_DGRAM && !args.protocol)
2103 		args.protocol = IPPROTO_UDP;
2104 
2105 	if ((args.type == SOCK_STREAM || args.type == SOCK_DGRAM) &&
2106 	     args.port == 0) {
2107 		fprintf(stderr, "Invalid port number\n");
2108 		return 1;
2109 	}
2110 
2111 	if ((both_mode || !server_mode) && !args.has_grp &&
2112 	    !args.has_remote_ip && !args.has_local_ip) {
2113 		fprintf(stderr,
2114 			"Local (server mode) or remote IP (client IP) required\n");
2115 		return 1;
2116 	}
2117 
2118 	if (interactive) {
2119 		prog_timeout = 0;
2120 		msg = NULL;
2121 	}
2122 
2123 	if (both_mode) {
2124 		if (pipe(fd) < 0) {
2125 			perror("pipe");
2126 			exit(1);
2127 		}
2128 
2129 		cpid = fork();
2130 		if (cpid < 0) {
2131 			perror("fork");
2132 			exit(1);
2133 		}
2134 		if (cpid)
2135 			return ipc_parent(cpid, fd[0], &args);
2136 
2137 		return ipc_child(fd[1], &args);
2138 	}
2139 
2140 	if (server_mode) {
2141 		do {
2142 			rc = do_server(&args, -1);
2143 		} while (forever);
2144 
2145 		return rc;
2146 	}
2147 	return do_client(&args);
2148 }
2149