Head

GCC Code Coverage Report

Directory:	./		Exec	Total	Coverage
File:	sbin/iked/util.c	Lines:	23	354	6.5 %
Date:	2017-11-13	Branches:	12	203	5.9 %


/*	$OpenBSD: util.c,v 1.33 2017/01/09 14:49:21 reyk Exp $	*/

/*
 * Copyright (c) 2010-2013 Reyk Floeter <reyk@openbsd.org>
 *
 * Permission to use, copy, modify, and distribute this software for any
 * purpose with or without fee is hereby granted, provided that the above
 * copyright notice and this permission notice appear in all copies.
 *
 * THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
 * WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
 * MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
 * ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
 * WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
 * ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
 * OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
 */

#include <sys/types.h>
#include <sys/queue.h>
#include <sys/socket.h>
#include <sys/uio.h>

#include <netdb.h>
#include <stdio.h>
#include <stdlib.h>
#include <unistd.h>
#include <string.h>
#include <errno.h>
#include <limits.h>
#include <fcntl.h>
#include <ctype.h>
#include <event.h>

#include "iked.h"
#include "ikev2.h"

int
socket_af(struct sockaddr *sa, in_port_t port)
{
	errno = 0;
	switch (sa->sa_family) {
	case AF_INET:
		((struct sockaddr_in *)sa)->sin_port = port;
		((struct sockaddr_in *)sa)->sin_len =
		    sizeof(struct sockaddr_in);
		break;
	case AF_INET6:
		((struct sockaddr_in6 *)sa)->sin6_port = port;
		((struct sockaddr_in6 *)sa)->sin6_len =
		    sizeof(struct sockaddr_in6);
		break;
	default:
		errno = EPFNOSUPPORT;
		return (-1);
	}

	return (0);
}

in_port_t
socket_getport(struct sockaddr *sa)
{
	switch (sa->sa_family) {
	case AF_INET:
		return (ntohs(((struct sockaddr_in *)sa)->sin_port));
	case AF_INET6:
		return (ntohs(((struct sockaddr_in6 *)sa)->sin6_port));
	default:
		return (0);
	}

	/* NOTREACHED */
	return (0);
}

int
socket_setport(struct sockaddr *sa, in_port_t port)
{
	switch (sa->sa_family) {
	case AF_INET:
		((struct sockaddr_in *)sa)->sin_port = htons(port);
		break;
	case AF_INET6:
		((struct sockaddr_in6 *)sa)->sin6_port = htons(port);
		break;
	default:
		return (-1);
	}
	return (0);
}

int
socket_getaddr(int s, struct sockaddr_storage *ss)
{
	socklen_t sslen;

	return (getsockname(s, (struct sockaddr *)ss, &sslen));
}

int
socket_bypass(int s, struct sockaddr *sa)
{
	int	 v, *a;
	int	 a4[] = {
		    IPPROTO_IP,
		    IP_AUTH_LEVEL,
		    IP_ESP_TRANS_LEVEL,
		    IP_ESP_NETWORK_LEVEL,
#ifdef IPV6_IPCOMP_LEVEL
		    IP_IPCOMP_LEVEL
#endif
	};
	int	 a6[] = {
		    IPPROTO_IPV6,
		    IPV6_AUTH_LEVEL,
		    IPV6_ESP_TRANS_LEVEL,
		    IPV6_ESP_NETWORK_LEVEL,
#ifdef IPV6_IPCOMP_LEVEL
		    IPV6_IPCOMP_LEVEL
#endif
	};

	switch (sa->sa_family) {
	case AF_INET:
		a = a4;
		break;
	case AF_INET6:
		a = a6;
		break;
	default:
		log_warn("%s: invalid address family", __func__);
		return (-1);
	}

	v = IPSEC_LEVEL_BYPASS;
	if (setsockopt(s, a[0], a[1], &v, sizeof(v)) == -1) {
		log_warn("%s: AUTH_LEVEL", __func__);
		return (-1);
	}
	if (setsockopt(s, a[0], a[2], &v, sizeof(v)) == -1) {
		log_warn("%s: ESP_TRANS_LEVEL", __func__);
		return (-1);
	}
	if (setsockopt(s, a[0], a[3], &v, sizeof(v)) == -1) {
		log_warn("%s: ESP_NETWORK_LEVEL", __func__);
		return (-1);
	}
#ifdef IP_IPCOMP_LEVEL
	if (setsockopt(s, a[0], a[4], &v, sizeof(v)) == -1) {
		log_warn("%s: IPCOMP_LEVEL", __func__);
		return (-1);
	}
#endif

	return (0);
}

int
udp_bind(struct sockaddr *sa, in_port_t port)
{
	int	 s, val;

	if (socket_af(sa, port) == -1) {
		log_warn("%s: failed to set UDP port", __func__);
		return (-1);
	}

	if ((s = socket(sa->sa_family,
	    SOCK_DGRAM | SOCK_NONBLOCK, IPPROTO_UDP)) == -1) {
		log_warn("%s: failed to get UDP socket", __func__);
		return (-1);
	}

	/* Skip IPsec processing (don't encrypt) for IKE messages */
	if (socket_bypass(s, sa) == -1) {
		log_warn("%s: failed to bypass IPsec on IKE socket",
		    __func__);
		goto bad;
	}

	val = 1;
	if (setsockopt(s, SOL_SOCKET, SO_REUSEPORT, &val, sizeof(int)) == -1) {
		log_warn("%s: failed to set reuseport", __func__);
		goto bad;
	}
	val = 1;
	if (setsockopt(s, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(int)) == -1) {
		log_warn("%s: failed to set reuseaddr", __func__);
		goto bad;
	}

	if (sa->sa_family == AF_INET) {
		val = 1;
		if (setsockopt(s, IPPROTO_IP, IP_RECVDSTADDR,
		    &val, sizeof(int)) == -1) {
			log_warn("%s: failed to set IPv4 packet info",
			    __func__);
			goto bad;
		}
	} else {
		val = 1;
		if (setsockopt(s, IPPROTO_IPV6, IPV6_RECVPKTINFO,
		    &val, sizeof(int)) == -1) {
			log_warn("%s: failed to set IPv6 packet info",
			    __func__);
			goto bad;
		}
	}

	if (bind(s, sa, sa->sa_len) == -1) {
		log_warn("%s: failed to bind UDP socket", __func__);
		goto bad;
	}

	return (s);
 bad:
	close(s);
	return (-1);
}

int
sockaddr_cmp(struct sockaddr *a, struct sockaddr *b, int prefixlen)
{
	struct sockaddr_in	*a4, *b4;
	struct sockaddr_in6	*a6, *b6;
	uint32_t		 av[4], bv[4], mv[4];

	if (a->sa_family == AF_UNSPEC || b->sa_family == AF_UNSPEC)
		return (0);
	else if (a->sa_family > b->sa_family)
		return (1);
	else if (a->sa_family < b->sa_family)
		return (-1);

	if (prefixlen == -1)
		memset(&mv, 0xff, sizeof(mv));

	switch (a->sa_family) {
	case AF_INET:
		a4 = (struct sockaddr_in *)a;
		b4 = (struct sockaddr_in *)b;

		av[0] = a4->sin_addr.s_addr;
		bv[0] = b4->sin_addr.s_addr;
		if (prefixlen != -1)
			mv[0] = prefixlen2mask(prefixlen);

		if ((av[0] & mv[0]) > (bv[0] & mv[0]))
			return (1);
		if ((av[0] & mv[0]) < (bv[0] & mv[0]))
			return (-1);
		break;
	case AF_INET6:
		a6 = (struct sockaddr_in6 *)a;
		b6 = (struct sockaddr_in6 *)b;

		memcpy(&av, &a6->sin6_addr.s6_addr, 16);
		memcpy(&bv, &b6->sin6_addr.s6_addr, 16);
		if (prefixlen != -1)
			prefixlen2mask6(prefixlen, mv);

		if ((av[3] & mv[3]) > (bv[3] & mv[3]))
			return (1);
		if ((av[3] & mv[3]) < (bv[3] & mv[3]))
			return (-1);
		if ((av[2] & mv[2]) > (bv[2] & mv[2]))
			return (1);
		if ((av[2] & mv[2]) < (bv[2] & mv[2]))
			return (-1);
		if ((av[1] & mv[1]) > (bv[1] & mv[1]))
			return (1);
		if ((av[1] & mv[1]) < (bv[1] & mv[1]))
			return (-1);
		if ((av[0] & mv[0]) > (bv[0] & mv[0]))
			return (1);
		if ((av[0] & mv[0]) < (bv[0] & mv[0]))
			return (-1);
		break;
	}

	return (0);
}

ssize_t
sendtofrom(int s, void *buf, size_t len, int flags, struct sockaddr *to,
    socklen_t tolen, struct sockaddr *from, socklen_t fromlen)
{
	struct iovec		 iov;
	struct msghdr		 msg;
	struct cmsghdr		*cmsg;
	struct in6_pktinfo	*pkt6;
	struct sockaddr_in	*in;
	struct sockaddr_in6	*in6;
	union {
		struct cmsghdr	hdr;
		char		inbuf[CMSG_SPACE(sizeof(struct in_addr))];
		char		in6buf[CMSG_SPACE(sizeof(struct in6_pktinfo))];
	} cmsgbuf;

	bzero(&msg, sizeof(msg));
	bzero(&cmsgbuf, sizeof(cmsgbuf));

	iov.iov_base = buf;
	iov.iov_len = len;
	msg.msg_iov = &iov;
	msg.msg_iovlen = 1;
	msg.msg_name = to;
	msg.msg_namelen = tolen;
	msg.msg_control = &cmsgbuf;
	msg.msg_controllen = sizeof(cmsgbuf);

	cmsg = CMSG_FIRSTHDR(&msg);
	switch (to->sa_family) {
	case AF_INET:
		msg.msg_controllen = sizeof(cmsgbuf.inbuf);
		cmsg->cmsg_len = CMSG_LEN(sizeof(struct in_addr));
		cmsg->cmsg_level = IPPROTO_IP;
		cmsg->cmsg_type = IP_SENDSRCADDR;
		in = (struct sockaddr_in *)from;
		memcpy(CMSG_DATA(cmsg), &in->sin_addr, sizeof(struct in_addr));
		break;
	case AF_INET6:
		msg.msg_controllen = sizeof(cmsgbuf.in6buf);
		cmsg->cmsg_len = CMSG_LEN(sizeof(struct in6_pktinfo));
		cmsg->cmsg_level = IPPROTO_IPV6;
		cmsg->cmsg_type = IPV6_PKTINFO;
		in6 = (struct sockaddr_in6 *)from;
		pkt6 = (struct in6_pktinfo *)CMSG_DATA(cmsg);
		pkt6->ipi6_addr = in6->sin6_addr;
		break;
	}

	return sendmsg(s, &msg, flags);
}

ssize_t
recvfromto(int s, void *buf, size_t len, int flags, struct sockaddr *from,
    socklen_t *fromlen, struct sockaddr *to, socklen_t *tolen)
{
	struct iovec		 iov;
	struct msghdr		 msg;
	struct cmsghdr		*cmsg;
	struct in6_pktinfo	*pkt6;
	struct sockaddr_in	*in;
	struct sockaddr_in6	*in6;
	ssize_t			 ret;
	union {
		struct cmsghdr hdr;
		char	buf[CMSG_SPACE(sizeof(struct sockaddr_storage))];
	} cmsgbuf;

	bzero(&msg, sizeof(msg));
	bzero(&cmsgbuf.buf, sizeof(cmsgbuf.buf));

	iov.iov_base = buf;
	iov.iov_len = len;
	msg.msg_iov = &iov;
	msg.msg_iovlen = 1;
	msg.msg_name = from;
	msg.msg_namelen = *fromlen;
	msg.msg_control = &cmsgbuf.buf;
	msg.msg_controllen = sizeof(cmsgbuf.buf);

	if ((ret = recvmsg(s, &msg, flags)) == -1)
		return (-1);

	*fromlen = from->sa_len;
	*tolen = 0;

	if (getsockname(s, to, tolen) != 0)
		*tolen = 0;

	for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
	    cmsg = CMSG_NXTHDR(&msg, cmsg)) {
		switch (from->sa_family) {
		case AF_INET:
			if (cmsg->cmsg_level == IPPROTO_IP &&
			    cmsg->cmsg_type == IP_RECVDSTADDR) {
				in = (struct sockaddr_in *)to;
				in->sin_family = AF_INET;
				in->sin_len = *tolen = sizeof(*in);
				memcpy(&in->sin_addr, CMSG_DATA(cmsg),
				    sizeof(struct in_addr));
			}
			break;
		case AF_INET6:
			if (cmsg->cmsg_level == IPPROTO_IPV6 &&
			    cmsg->cmsg_type == IPV6_PKTINFO) {
				in6 = (struct sockaddr_in6 *)to;
				in6->sin6_family = AF_INET6;
				in6->sin6_len = *tolen = sizeof(*in6);
				pkt6 = (struct in6_pktinfo *)CMSG_DATA(cmsg);
				memcpy(&in6->sin6_addr, &pkt6->ipi6_addr,
				    sizeof(struct in6_addr));
				if (IN6_IS_ADDR_LINKLOCAL(&in6->sin6_addr))
					in6->sin6_scope_id =
					    pkt6->ipi6_ifindex;
			}
			break;
		}
	}

	return (ret);
}

const char *
print_spi(uint64_t spi, int size)
{
	static char		 buf[IKED_CYCLE_BUFFERS][32];
	static int		 i = 0;
	char			*ptr;

	ptr = buf[i];

	switch (size) {
	case 2:
		snprintf(ptr, 32, "0x%04x", (uint16_t)spi);
		break;
	case 4:
		snprintf(ptr, 32, "0x%08x", (uint32_t)spi);
		break;
	case 8:
		snprintf(ptr, 32, "0x%016llx", spi);
		break;
	default:
		snprintf(ptr, 32, "%llu", spi);
		break;
	}

	if (++i >= IKED_CYCLE_BUFFERS)
		i = 0;

	return (ptr);
}

const char *
print_map(unsigned int type, struct iked_constmap *map)
{
	unsigned int		 i;
	static char		 buf[IKED_CYCLE_BUFFERS][32];
	static int		 idx = 0;
	const char		*name = NULL;

	if (idx >= IKED_CYCLE_BUFFERS)
		idx = 0;
	bzero(buf[idx], sizeof(buf[idx]));

	for (i = 0; map[i].cm_name != NULL; i++) {
		if (map[i].cm_type == type)
			name = map[i].cm_name;
	}

	if (name == NULL)
		snprintf(buf[idx], sizeof(buf[idx]), "<UNKNOWN:%u>", type);
	else
		strlcpy(buf[idx], name, sizeof(buf[idx]));

	return (buf[idx++]);
}

void
lc_string(char *str)
{
	for (; *str != '\0'; str++)
		*str = tolower((unsigned char)*str);
}

void
print_hex(uint8_t *buf, off_t offset, size_t length)
{
	unsigned int	 i;

	if (log_getverbose() < 3 || !length)
		return;

	for (i = 0; i < length; i++) {
		if (i && (i % 4) == 0) {
			if ((i % 32) == 0)
				print_debug("\n");
			else
				print_debug(" ");
		}
		print_debug("%02x", buf[offset + i]);
	}
	print_debug("\n");
}

void
print_hexval(uint8_t *buf, off_t offset, size_t length)
{
	unsigned int	 i;

	if (log_getverbose() < 2 || !length)
		return;

	print_debug("0x");
	for (i = 0; i < length; i++)
		print_debug("%02x", buf[offset + i]);
	print_debug("\n");
}

const char *
print_bits(unsigned short v, unsigned char *bits)
{
	static char	 buf[IKED_CYCLE_BUFFERS][BUFSIZ];
	static int	 idx = 0;
	unsigned int	 i, any = 0, j = 0;
	unsigned char	 c;

	if (!bits)
		return ("");

	if (++idx >= IKED_CYCLE_BUFFERS)
		idx = 0;

	bzero(buf[idx], sizeof(buf[idx]));

	bits++;
	while ((i = *bits++)) {
		if (v & (1 << (i-1))) {
			if (any) {
				buf[idx][j++] = ',';
				if (j >= sizeof(buf[idx]))
					return (buf[idx]);
			}
			any = 1;
			for (; (c = *bits) > 32; bits++) {
				buf[idx][j++] = tolower((unsigned char)c);
				if (j >= sizeof(buf[idx]))
					return (buf[idx]);
			}
		} else
			for (; *bits > 32; bits++)
				;
	}

	return (buf[idx]);
}

uint8_t
mask2prefixlen(struct sockaddr *sa)
{
	struct sockaddr_in	*sa_in = (struct sockaddr_in *)sa;
	in_addr_t		 ina = sa_in->sin_addr.s_addr;

	if (ina == 0)
		return (0);
	else
		return (33 - ffs(ntohl(ina)));
}

uint8_t
mask2prefixlen6(struct sockaddr *sa)
{
	struct sockaddr_in6	*sa_in6 = (struct sockaddr_in6 *)sa;
	uint8_t			 l = 0, *ap, *ep;

	/*
	 * sin6_len is the size of the sockaddr so substract the offset of
	 * the possibly truncated sin6_addr struct.
	 */
	ap = (uint8_t *)&sa_in6->sin6_addr;
	ep = (uint8_t *)sa_in6 + sa_in6->sin6_len;
	for (; ap < ep; ap++) {
		/* this "beauty" is adopted from sbin/route/show.c ... */
		switch (*ap) {
		case 0xff:
			l += 8;
			break;
		case 0xfe:
			l += 7;
			return (l);
		case 0xfc:
			l += 6;
			return (l);
		case 0xf8:
			l += 5;
			return (l);
		case 0xf0:
			l += 4;
			return (l);
		case 0xe0:
			l += 3;
			return (l);
		case 0xc0:
			l += 2;
			return (l);
		case 0x80:
			l += 1;
			return (l);
		case 0x00:
			return (l);
		default:
			return (0);
		}
	}

	return (l);
}

uint32_t
prefixlen2mask(uint8_t prefixlen)
{
	if (prefixlen == 0)
		return (0);

	if (prefixlen > 32)
		prefixlen = 32;

	return (htonl(0xffffffff << (32 - prefixlen)));
}

struct in6_addr *
prefixlen2mask6(uint8_t prefixlen, uint32_t *mask)
{
	static struct in6_addr  s6;
	int			i;

	if (prefixlen > 128)
		prefixlen = 128;

	bzero(&s6, sizeof(s6));
	for (i = 0; i < prefixlen / 8; i++)
		s6.s6_addr[i] = 0xff;
	i = prefixlen % 8;
	if (i)
		s6.s6_addr[prefixlen / 8] = 0xff00 >> i;

	memcpy(mask, &s6, sizeof(s6));

	return (&s6);
}

const char *
print_host(struct sockaddr *sa, char *buf, size_t len)
{
	static char	sbuf[IKED_CYCLE_BUFFERS][NI_MAXHOST + 7];
	static int	idx = 0;
	char		pbuf[7];
	in_port_t	port;

	if (buf == NULL) {
		buf = sbuf[idx];
		len = sizeof(sbuf[idx]);
		if (++idx >= IKED_CYCLE_BUFFERS)
			idx = 0;
	}

	if (sa->sa_family == AF_UNSPEC) {
		strlcpy(buf, "any", len);
		return (buf);
	}

	if (getnameinfo(sa, sa->sa_len,
	    buf, len, NULL, 0, NI_NUMERICHOST) != 0) {
		buf[0] = '\0';
		return (NULL);
	}

	if ((port = socket_getport(sa)) != 0) {
		snprintf(pbuf, sizeof(pbuf), ":%d", port);
		(void)strlcat(buf, pbuf, len);
	}

	return (buf);
}

char *
get_string(uint8_t *ptr, size_t len)
{
	size_t	 i;

	for (i = 0; i < len; i++)
		if (!isprint(ptr[i]))
			break;

	return strndup(ptr, i);
}

const char *
print_proto(uint8_t proto)
{
	struct protoent *p;
	static char	 buf[IKED_CYCLE_BUFFERS][BUFSIZ];
	static int	 idx = 0;

	if (idx >= IKED_CYCLE_BUFFERS)
		idx = 0;

	if ((p = getprotobynumber(proto)) != NULL)
		strlcpy(buf[idx], p->p_name, sizeof(buf[idx]));
	else
		snprintf(buf[idx], sizeof(buf), "%u", proto);


	return (buf[idx++]);
}

int
expand_string(char *label, size_t len, const char *srch, const char *repl)
{
	char *tmp;
	char *p, *q;

	if ((tmp = calloc(1, len)) == NULL) {
		log_debug("expand_string: calloc");
		return (-1);
	}
	p = q = label;
	while ((q = strstr(p, srch)) != NULL) {
		*q = '\0';
		if ((strlcat(tmp, p, len) >= len) ||
		    (strlcat(tmp, repl, len) >= len)) {
			log_debug("expand_string: string too long");
			free(tmp);
			return (-1);
		}
		q += strlen(srch);
		p = q;
	}
	if (strlcat(tmp, p, len) >= len) {
		log_debug("expand_string: string too long");
		free(tmp);
		return (-1);
	}
	strlcpy(label, tmp, len);	/* always fits */
	free(tmp);

	return (0);
}

uint8_t *
string2unicode(const char *ascii, size_t *outlen)
{
	uint8_t		*uc = NULL;
	size_t		 i, len = strlen(ascii);

	if ((uc = calloc(1, (len * 2) + 2)) == NULL)
		return (NULL);

	for (i = 0; i < len; i++) {
		/* XXX what about the byte order? */
		uc[i * 2] = ascii[i];
	}
	*outlen = len * 2;

	return (uc);
}

void
print_debug(const char *emsg, ...)
{
	va_list	 ap;

	if (log_getverbose() > 2) {
		va_start(ap, emsg);
		vfprintf(stderr, emsg, ap);
		va_end(ap);
	}
}

void
print_verbose(const char *emsg, ...)
{
	va_list	 ap;

	if (log_getverbose()) {
		va_start(ap, emsg);
		vfprintf(stderr, emsg, ap);
		va_end(ap);
	}
}


Generated by: GCOVR (Version 3.3)

Line	Branch	Exec	Source
1			/* $OpenBSD: util.c,v 1.33 2017/01/09 14:49:21 reyk Exp $ */
2
3			/*
4			* Copyright (c) 2010-2013 Reyk Floeter <reyk@openbsd.org>
5			*
6			* Permission to use, copy, modify, and distribute this software for any
7			* purpose with or without fee is hereby granted, provided that the above
8			* copyright notice and this permission notice appear in all copies.
9			*
10			* THE SOFTWARE IS PROVIDED "AS IS" AND THE AUTHOR DISCLAIMS ALL WARRANTIES
11			* WITH REGARD TO THIS SOFTWARE INCLUDING ALL IMPLIED WARRANTIES OF
12			* MERCHANTABILITY AND FITNESS. IN NO EVENT SHALL THE AUTHOR BE LIABLE FOR
13			* ANY SPECIAL, DIRECT, INDIRECT, OR CONSEQUENTIAL DAMAGES OR ANY DAMAGES
14			* WHATSOEVER RESULTING FROM LOSS OF USE, DATA OR PROFITS, WHETHER IN AN
15			* ACTION OF CONTRACT, NEGLIGENCE OR OTHER TORTIOUS ACTION, ARISING OUT OF
16			* OR IN CONNECTION WITH THE USE OR PERFORMANCE OF THIS SOFTWARE.
17			*/
18
19			#include <sys/types.h>
20			#include <sys/queue.h>
21			#include <sys/socket.h>
22			#include <sys/uio.h>
23
24			#include <netdb.h>
25			#include <stdio.h>
26			#include <stdlib.h>
27			#include <unistd.h>
28			#include <string.h>
29			#include <errno.h>
30			#include <limits.h>
31			#include <fcntl.h>
32			#include <ctype.h>
33			#include <event.h>
34
35			#include "iked.h"
36			#include "ikev2.h"
37
38			int
39			socket_af(struct sockaddr *sa, in_port_t port)
40			{
41			errno = 0;
42			switch (sa->sa_family) {
43			case AF_INET:
44			((struct sockaddr_in *)sa)->sin_port = port;
45			((struct sockaddr_in *)sa)->sin_len =
46			sizeof(struct sockaddr_in);
47			break;
48			case AF_INET6:
49			((struct sockaddr_in6 *)sa)->sin6_port = port;
50			((struct sockaddr_in6 *)sa)->sin6_len =
51			sizeof(struct sockaddr_in6);
52			break;
53			default:
54			errno = EPFNOSUPPORT;
55			return (-1);
56			}
57
58			return (0);
59			}
60
61			in_port_t
62			socket_getport(struct sockaddr *sa)
63			{
64			switch (sa->sa_family) {
65			case AF_INET:
66			return (ntohs(((struct sockaddr_in *)sa)->sin_port));
67			case AF_INET6:
68			return (ntohs(((struct sockaddr_in6 *)sa)->sin6_port));
69			default:
70			return (0);
71			}
72
73			/* NOTREACHED */
74			return (0);
75			}
76
77			int
78			socket_setport(struct sockaddr *sa, in_port_t port)
79			{
80			switch (sa->sa_family) {
81			case AF_INET:
82			((struct sockaddr_in *)sa)->sin_port = htons(port);
83			break;
84			case AF_INET6:
85			((struct sockaddr_in6 *)sa)->sin6_port = htons(port);
86			break;
87			default:
88			return (-1);
89			}
90			return (0);
91			}
92
93			int
94			socket_getaddr(int s, struct sockaddr_storage *ss)
95			{
96			socklen_t sslen;
97
98			return (getsockname(s, (struct sockaddr *)ss, &sslen));
99			}
100
101			int
102			socket_bypass(int s, struct sockaddr *sa)
103			{
104			int v, *a;
105			int a4[] = {
106			IPPROTO_IP,
107			IP_AUTH_LEVEL,
108			IP_ESP_TRANS_LEVEL,
109			IP_ESP_NETWORK_LEVEL,
110			#ifdef IPV6_IPCOMP_LEVEL
111			IP_IPCOMP_LEVEL
112			#endif
113			};
114			int a6[] = {
115			IPPROTO_IPV6,
116			IPV6_AUTH_LEVEL,
117			IPV6_ESP_TRANS_LEVEL,
118			IPV6_ESP_NETWORK_LEVEL,
119			#ifdef IPV6_IPCOMP_LEVEL
120			IPV6_IPCOMP_LEVEL
121			#endif
122			};
123
124			switch (sa->sa_family) {
125			case AF_INET:
126			a = a4;
127			break;
128			case AF_INET6:
129			a = a6;
130			break;
131			default:
132			log_warn("%s: invalid address family", __func__);
133			return (-1);
134			}
135
136			v = IPSEC_LEVEL_BYPASS;
137			if (setsockopt(s, a[0], a[1], &v, sizeof(v)) == -1) {
138			log_warn("%s: AUTH_LEVEL", __func__);
139			return (-1);
140			}
141			if (setsockopt(s, a[0], a[2], &v, sizeof(v)) == -1) {
142			log_warn("%s: ESP_TRANS_LEVEL", __func__);
143			return (-1);
144			}
145			if (setsockopt(s, a[0], a[3], &v, sizeof(v)) == -1) {
146			log_warn("%s: ESP_NETWORK_LEVEL", __func__);
147			return (-1);
148			}
149			#ifdef IP_IPCOMP_LEVEL
150			if (setsockopt(s, a[0], a[4], &v, sizeof(v)) == -1) {
151			log_warn("%s: IPCOMP_LEVEL", __func__);
152			return (-1);
153			}
154			#endif
155
156			return (0);
157			}
158
159			int
160			udp_bind(struct sockaddr *sa, in_port_t port)
161			{
162			int s, val;
163
164			if (socket_af(sa, port) == -1) {
165			log_warn("%s: failed to set UDP port", __func__);
166			return (-1);
167			}
168
169			if ((s = socket(sa->sa_family,
170			SOCK_DGRAM \| SOCK_NONBLOCK, IPPROTO_UDP)) == -1) {
171			log_warn("%s: failed to get UDP socket", __func__);
172			return (-1);
173			}
174
175			/* Skip IPsec processing (don't encrypt) for IKE messages */
176			if (socket_bypass(s, sa) == -1) {
177			log_warn("%s: failed to bypass IPsec on IKE socket",
178			__func__);
179			goto bad;
180			}
181
182			val = 1;
183			if (setsockopt(s, SOL_SOCKET, SO_REUSEPORT, &val, sizeof(int)) == -1) {
184			log_warn("%s: failed to set reuseport", __func__);
185			goto bad;
186			}
187			val = 1;
188			if (setsockopt(s, SOL_SOCKET, SO_REUSEADDR, &val, sizeof(int)) == -1) {
189			log_warn("%s: failed to set reuseaddr", __func__);
190			goto bad;
191			}
192
193			if (sa->sa_family == AF_INET) {
194			val = 1;
195			if (setsockopt(s, IPPROTO_IP, IP_RECVDSTADDR,
196			&val, sizeof(int)) == -1) {
197			log_warn("%s: failed to set IPv4 packet info",
198			__func__);
199			goto bad;
200			}
201			} else {
202			val = 1;
203			if (setsockopt(s, IPPROTO_IPV6, IPV6_RECVPKTINFO,
204			&val, sizeof(int)) == -1) {
205			log_warn("%s: failed to set IPv6 packet info",
206			__func__);
207			goto bad;
208			}
209			}
210
211			if (bind(s, sa, sa->sa_len) == -1) {
212			log_warn("%s: failed to bind UDP socket", __func__);
213			goto bad;
214			}
215
216			return (s);
217			bad:
218			close(s);
219			return (-1);
220			}
221
222			int
223			sockaddr_cmp(struct sockaddr a, struct sockaddr b, int prefixlen)
224			{
225			struct sockaddr_in a4, b4;
226			struct sockaddr_in6 a6, b6;
227			uint32_t av[4], bv[4], mv[4];
228
229			if (a->sa_family == AF_UNSPEC \|\| b->sa_family == AF_UNSPEC)
230			return (0);
231			else if (a->sa_family > b->sa_family)
232			return (1);
233			else if (a->sa_family < b->sa_family)
234			return (-1);
235
236			if (prefixlen == -1)
237			memset(&mv, 0xff, sizeof(mv));
238
239			switch (a->sa_family) {
240			case AF_INET:
241			a4 = (struct sockaddr_in *)a;
242			b4 = (struct sockaddr_in *)b;
243
244			av[0] = a4->sin_addr.s_addr;
245			bv[0] = b4->sin_addr.s_addr;
246			if (prefixlen != -1)
247			mv[0] = prefixlen2mask(prefixlen);
248
249			if ((av[0] & mv[0]) > (bv[0] & mv[0]))
250			return (1);
251			if ((av[0] & mv[0]) < (bv[0] & mv[0]))
252			return (-1);
253			break;
254			case AF_INET6:
255			a6 = (struct sockaddr_in6 *)a;
256			b6 = (struct sockaddr_in6 *)b;
257
258			memcpy(&av, &a6->sin6_addr.s6_addr, 16);
259			memcpy(&bv, &b6->sin6_addr.s6_addr, 16);
260			if (prefixlen != -1)
261			prefixlen2mask6(prefixlen, mv);
262
263			if ((av[3] & mv[3]) > (bv[3] & mv[3]))
264			return (1);
265			if ((av[3] & mv[3]) < (bv[3] & mv[3]))
266			return (-1);
267			if ((av[2] & mv[2]) > (bv[2] & mv[2]))
268			return (1);
269			if ((av[2] & mv[2]) < (bv[2] & mv[2]))
270			return (-1);
271			if ((av[1] & mv[1]) > (bv[1] & mv[1]))
272			return (1);
273			if ((av[1] & mv[1]) < (bv[1] & mv[1]))
274			return (-1);
275			if ((av[0] & mv[0]) > (bv[0] & mv[0]))
276			return (1);
277			if ((av[0] & mv[0]) < (bv[0] & mv[0]))
278			return (-1);
279			break;
280			}
281
282			return (0);
283			}
284
285			ssize_t
286			sendtofrom(int s, void buf, size_t len, int flags, struct sockaddr to,
287			socklen_t tolen, struct sockaddr *from, socklen_t fromlen)
288			{
289			struct iovec iov;
290			struct msghdr msg;
291			struct cmsghdr *cmsg;
292			struct in6_pktinfo *pkt6;
293			struct sockaddr_in *in;
294			struct sockaddr_in6 *in6;
295			union {
296			struct cmsghdr hdr;
297			char inbuf[CMSG_SPACE(sizeof(struct in_addr))];
298			char in6buf[CMSG_SPACE(sizeof(struct in6_pktinfo))];
299			} cmsgbuf;
300
301			bzero(&msg, sizeof(msg));
302			bzero(&cmsgbuf, sizeof(cmsgbuf));
303
304			iov.iov_base = buf;
305			iov.iov_len = len;
306			msg.msg_iov = &iov;
307			msg.msg_iovlen = 1;
308			msg.msg_name = to;
309			msg.msg_namelen = tolen;
310			msg.msg_control = &cmsgbuf;
311			msg.msg_controllen = sizeof(cmsgbuf);
312
313			cmsg = CMSG_FIRSTHDR(&msg);
314			switch (to->sa_family) {
315			case AF_INET:
316			msg.msg_controllen = sizeof(cmsgbuf.inbuf);
317			cmsg->cmsg_len = CMSG_LEN(sizeof(struct in_addr));
318			cmsg->cmsg_level = IPPROTO_IP;
319			cmsg->cmsg_type = IP_SENDSRCADDR;
320			in = (struct sockaddr_in *)from;
321			memcpy(CMSG_DATA(cmsg), &in->sin_addr, sizeof(struct in_addr));
322			break;
323			case AF_INET6:
324			msg.msg_controllen = sizeof(cmsgbuf.in6buf);
325			cmsg->cmsg_len = CMSG_LEN(sizeof(struct in6_pktinfo));
326			cmsg->cmsg_level = IPPROTO_IPV6;
327			cmsg->cmsg_type = IPV6_PKTINFO;
328			in6 = (struct sockaddr_in6 *)from;
329			pkt6 = (struct in6_pktinfo *)CMSG_DATA(cmsg);
330			pkt6->ipi6_addr = in6->sin6_addr;
331			break;
332			}
333
334			return sendmsg(s, &msg, flags);
335			}
336
337			ssize_t
338			recvfromto(int s, void buf, size_t len, int flags, struct sockaddr from,
339			socklen_t fromlen, struct sockaddr to, socklen_t *tolen)
340			{
341			struct iovec iov;
342			struct msghdr msg;
343			struct cmsghdr *cmsg;
344			struct in6_pktinfo *pkt6;
345			struct sockaddr_in *in;
346			struct sockaddr_in6 *in6;
347			ssize_t ret;
348			union {
349			struct cmsghdr hdr;
350			char buf[CMSG_SPACE(sizeof(struct sockaddr_storage))];
351			} cmsgbuf;
352
353			bzero(&msg, sizeof(msg));
354			bzero(&cmsgbuf.buf, sizeof(cmsgbuf.buf));
355
356			iov.iov_base = buf;
357			iov.iov_len = len;
358			msg.msg_iov = &iov;
359			msg.msg_iovlen = 1;
360			msg.msg_name = from;
361			msg.msg_namelen = *fromlen;
362			msg.msg_control = &cmsgbuf.buf;
363			msg.msg_controllen = sizeof(cmsgbuf.buf);
364
365			if ((ret = recvmsg(s, &msg, flags)) == -1)
366			return (-1);
367
368			*fromlen = from->sa_len;
369			*tolen = 0;
370
371			if (getsockname(s, to, tolen) != 0)
372			*tolen = 0;
373
374			for (cmsg = CMSG_FIRSTHDR(&msg); cmsg != NULL;
375			cmsg = CMSG_NXTHDR(&msg, cmsg)) {
376			switch (from->sa_family) {
377			case AF_INET:
378			if (cmsg->cmsg_level == IPPROTO_IP &&
379			cmsg->cmsg_type == IP_RECVDSTADDR) {
380			in = (struct sockaddr_in *)to;
381			in->sin_family = AF_INET;
382			in->sin_len = tolen = sizeof(in);
383			memcpy(&in->sin_addr, CMSG_DATA(cmsg),
384			sizeof(struct in_addr));
385			}
386			break;
387			case AF_INET6:
388			if (cmsg->cmsg_level == IPPROTO_IPV6 &&
389			cmsg->cmsg_type == IPV6_PKTINFO) {
390			in6 = (struct sockaddr_in6 *)to;
391			in6->sin6_family = AF_INET6;
392			in6->sin6_len = tolen = sizeof(in6);
393			pkt6 = (struct in6_pktinfo *)CMSG_DATA(cmsg);
394			memcpy(&in6->sin6_addr, &pkt6->ipi6_addr,
395			sizeof(struct in6_addr));
396			if (IN6_IS_ADDR_LINKLOCAL(&in6->sin6_addr))
397			in6->sin6_scope_id =
398			pkt6->ipi6_ifindex;
399			}
400			break;
401			}
402			}
403
404			return (ret);
405			}
406
407			const char *
408			print_spi(uint64_t spi, int size)
409			{
410			static char buf[IKED_CYCLE_BUFFERS][32];
411			static int i = 0;
412			char *ptr;
413
414		1115025730	ptr = buf[i];
415
416	✗✓✓✓	557512865	switch (size) {
417			case 2:
418			snprintf(ptr, 32, "0x%04x", (uint16_t)spi);
419			break;
420			case 4:
421		58072	snprintf(ptr, 32, "0x%08x", (uint32_t)spi);
422		58072	break;
423			case 8:
424		466758824	snprintf(ptr, 32, "0x%016llx", spi);
425		466758824	break;
426			default:
427		90695969	snprintf(ptr, 32, "%llu", spi);
428		90695969	break;
429			}
430
431		557512865	if (++i >= IKED_CYCLE_BUFFERS)
432			i = 0;
433
434		557512865	return (ptr);
435			}
436
437			const char *
438			print_map(unsigned int type, struct iked_constmap *map)
439			{
440			unsigned int i;
441			static char buf[IKED_CYCLE_BUFFERS][32];
442			static int idx = 0;
443			const char *name = NULL;
444
445	✓✓	6645652594	if (idx >= IKED_CYCLE_BUFFERS)
446		415353281	idx = 0;
447		3322826297	bzero(buf[idx], sizeof(buf[idx]));
448
449	✓✓	112503972168	for (i = 0; map[i].cm_name != NULL; i++) {
450	✓✓	52929159787	if (map[i].cm_type == type)
451		3303033195	name = map[i].cm_name;
452			}
453
454	✓✓	3322826297	if (name == NULL)
455		19793102	snprintf(buf[idx], sizeof(buf[idx]), "<UNKNOWN:%u>", type);
456			else
457		3303033195	strlcpy(buf[idx], name, sizeof(buf[idx]));
458
459		3322826297	return (buf[idx++]);
460			}
461
462			void
463			lc_string(char *str)
464			{
465			for (; *str != '\0'; str++)
466			str = tolower((unsigned char)str);
467			}
468
469			void
470			print_hex(uint8_t *buf, off_t offset, size_t length)
471			{
472			unsigned int i;
473
474	✓✗	1449340956	if (log_getverbose() < 3 \|\| !length)
475		724670478	return;
476
477			for (i = 0; i < length; i++) {
478			if (i && (i % 4) == 0) {
479			if ((i % 32) == 0)
480			print_debug("\n");
481			else
482			print_debug(" ");
483			}
484			print_debug("%02x", buf[offset + i]);
485			}
486			print_debug("\n");
487		724670478	}
488
489			void
490			print_hexval(uint8_t *buf, off_t offset, size_t length)
491			{
492			unsigned int i;
493
494			if (log_getverbose() < 2 \|\| !length)
495			return;
496
497			print_debug("0x");
498			for (i = 0; i < length; i++)
499			print_debug("%02x", buf[offset + i]);
500			print_debug("\n");
501			}
502
503			const char *
504			print_bits(unsigned short v, unsigned char *bits)
505			{
506			static char buf[IKED_CYCLE_BUFFERS][BUFSIZ];
507			static int idx = 0;
508			unsigned int i, any = 0, j = 0;
509			unsigned char c;
510
511			if (!bits)
512			return ("");
513
514			if (++idx >= IKED_CYCLE_BUFFERS)
515			idx = 0;
516
517			bzero(buf[idx], sizeof(buf[idx]));
518
519			bits++;
520			while ((i = *bits++)) {
521			if (v & (1 << (i-1))) {
522			if (any) {
523			buf[idx][j++] = ',';
524			if (j >= sizeof(buf[idx]))
525			return (buf[idx]);
526			}
527			any = 1;
528			for (; (c = *bits) > 32; bits++) {
529			buf[idx][j++] = tolower((unsigned char)c);
530			if (j >= sizeof(buf[idx]))
531			return (buf[idx]);
532			}
533			} else
534			for (; *bits > 32; bits++)
535			;
536			}
537
538			return (buf[idx]);
539			}
540
541			uint8_t
542			mask2prefixlen(struct sockaddr *sa)
543			{
544			struct sockaddr_in sa_in = (struct sockaddr_in )sa;
545			in_addr_t ina = sa_in->sin_addr.s_addr;
546
547			if (ina == 0)
548			return (0);
549			else
550			return (33 - ffs(ntohl(ina)));
551			}
552
553			uint8_t
554			mask2prefixlen6(struct sockaddr *sa)
555			{
556			struct sockaddr_in6 sa_in6 = (struct sockaddr_in6 )sa;
557			uint8_t l = 0, ap, ep;
558
559			/*
560			* sin6_len is the size of the sockaddr so substract the offset of
561			* the possibly truncated sin6_addr struct.
562			*/
563			ap = (uint8_t *)&sa_in6->sin6_addr;
564			ep = (uint8_t *)sa_in6 + sa_in6->sin6_len;
565			for (; ap < ep; ap++) {
566			/* this "beauty" is adopted from sbin/route/show.c ... */
567			switch (*ap) {
568			case 0xff:
569			l += 8;
570			break;
571			case 0xfe:
572			l += 7;
573			return (l);
574			case 0xfc:
575			l += 6;
576			return (l);
577			case 0xf8:
578			l += 5;
579			return (l);
580			case 0xf0:
581			l += 4;
582			return (l);
583			case 0xe0:
584			l += 3;
585			return (l);
586			case 0xc0:
587			l += 2;
588			return (l);
589			case 0x80:
590			l += 1;
591			return (l);
592			case 0x00:
593			return (l);
594			default:
595			return (0);
596			}
597			}
598
599			return (l);
600			}
601
602			uint32_t
603			prefixlen2mask(uint8_t prefixlen)
604			{
605			if (prefixlen == 0)
606			return (0);
607
608			if (prefixlen > 32)
609			prefixlen = 32;
610
611			return (htonl(0xffffffff << (32 - prefixlen)));
612			}
613
614			struct in6_addr *
615			prefixlen2mask6(uint8_t prefixlen, uint32_t *mask)
616			{
617			static struct in6_addr s6;
618			int i;
619
620			if (prefixlen > 128)
621			prefixlen = 128;
622
623			bzero(&s6, sizeof(s6));
624			for (i = 0; i < prefixlen / 8; i++)
625			s6.s6_addr[i] = 0xff;
626			i = prefixlen % 8;
627			if (i)
628			s6.s6_addr[prefixlen / 8] = 0xff00 >> i;
629
630			memcpy(mask, &s6, sizeof(s6));
631
632			return (&s6);
633			}
634
635			const char *
636			print_host(struct sockaddr sa, char buf, size_t len)
637			{
638			static char sbuf[IKED_CYCLE_BUFFERS][NI_MAXHOST + 7];
639			static int idx = 0;
640			char pbuf[7];
641			in_port_t port;
642
643			if (buf == NULL) {
644			buf = sbuf[idx];
645			len = sizeof(sbuf[idx]);
646			if (++idx >= IKED_CYCLE_BUFFERS)
647			idx = 0;
648			}
649
650			if (sa->sa_family == AF_UNSPEC) {
651			strlcpy(buf, "any", len);
652			return (buf);
653			}
654
655			if (getnameinfo(sa, sa->sa_len,
656			buf, len, NULL, 0, NI_NUMERICHOST) != 0) {
657			buf[0] = '\0';
658			return (NULL);
659			}
660
661			if ((port = socket_getport(sa)) != 0) {
662			snprintf(pbuf, sizeof(pbuf), ":%d", port);
663			(void)strlcat(buf, pbuf, len);
664			}
665
666			return (buf);
667			}
668
669			char *
670			get_string(uint8_t *ptr, size_t len)
671			{
672			size_t i;
673
674			for (i = 0; i < len; i++)
675			if (!isprint(ptr[i]))
676			break;
677
678			return strndup(ptr, i);
679			}
680
681			const char *
682			print_proto(uint8_t proto)
683			{
684			struct protoent *p;
685			static char buf[IKED_CYCLE_BUFFERS][BUFSIZ];
686			static int idx = 0;
687
688			if (idx >= IKED_CYCLE_BUFFERS)
689			idx = 0;
690
691			if ((p = getprotobynumber(proto)) != NULL)
692			strlcpy(buf[idx], p->p_name, sizeof(buf[idx]));
693			else
694			snprintf(buf[idx], sizeof(buf), "%u", proto);
695
696
697			return (buf[idx++]);
698			}
699
700			int
701			expand_string(char label, size_t len, const char srch, const char *repl)
702			{
703			char *tmp;
704			char p, q;
705
706			if ((tmp = calloc(1, len)) == NULL) {
707			log_debug("expand_string: calloc");
708			return (-1);
709			}
710			p = q = label;
711			while ((q = strstr(p, srch)) != NULL) {
712			*q = '\0';
713			if ((strlcat(tmp, p, len) >= len) \|\|
714			(strlcat(tmp, repl, len) >= len)) {
715			log_debug("expand_string: string too long");
716			free(tmp);
717			return (-1);
718			}
719			q += strlen(srch);
720			p = q;
721			}
722			if (strlcat(tmp, p, len) >= len) {
723			log_debug("expand_string: string too long");
724			free(tmp);
725			return (-1);
726			}
727			strlcpy(label, tmp, len); /* always fits */
728			free(tmp);
729
730			return (0);
731			}
732
733			uint8_t *
734			string2unicode(const char ascii, size_t outlen)
735			{
736			uint8_t *uc = NULL;
737			size_t i, len = strlen(ascii);
738
739			if ((uc = calloc(1, (len * 2) + 2)) == NULL)
740			return (NULL);
741
742			for (i = 0; i < len; i++) {
743			/* XXX what about the byte order? */
744			uc[i * 2] = ascii[i];
745			}
746			outlen = len 2;
747
748			return (uc);
749			}
750
751			void
752			print_debug(const char *emsg, ...)
753			{
754			va_list ap;
755
756			if (log_getverbose() > 2) {
757			va_start(ap, emsg);
758			vfprintf(stderr, emsg, ap);
759			va_end(ap);
760			}
761			}
762
763			void
764			print_verbose(const char *emsg, ...)
765			{
766			va_list ap;
767
768			if (log_getverbose()) {
769			va_start(ap, emsg);
770			vfprintf(stderr, emsg, ap);
771			va_end(ap);
772			}
773			}