llmnrd: Add IPv6 support

Closes #5

Signed-off-by: Tobias Klauser <tklauser@distanz.ch>

5 files changed, 121 insertions, 55 deletions

diff --git a/llmnr.c b/llmnr.c
index 01f25d9..f08c5d1 100644
--- a/llmnr.c
+++ b/llmnr.c
@@ -16,6 +16,7 @@
  * along with llmnrd.  If not, see <http://www.gnu.org/licenses/>.
  */
 
+#define _GNU_SOURCE
 #include <ctype.h>
 #include <errno.h>
 #include <stdbool.h>
@@ -36,8 +37,8 @@
 #include "llmnr-packet.h"
 #include "llmnr.h"
 
-static int llmnr_sock = -1;
-static int llmnr_sock_v6 = -1;
+static int llmnr_sock_ipv4 = -1;
+static int llmnr_sock_ipv6 = -1;
 static bool llmnr_running = true;
 /*
  * Host name in DNS name format (length octet + name + 0 byte)
@@ -48,10 +49,10 @@ static void llmnr_iface_event_handle(enum iface_event_type type, int af, unsigne
 {
 	switch (af) {
 	case AF_INET:
-		socket_mcast_group_ipv4(llmnr_sock, ifindex, type == IFACE_ADD);
+		socket_mcast_group_ipv4(llmnr_sock_ipv4, ifindex, type == IFACE_ADD);
 		break;
 	case AF_INET6:
-		socket_mcast_group_ipv6(llmnr_sock_v6, ifindex, type == IFACE_ADD);
+		socket_mcast_group_ipv6(llmnr_sock_ipv6, ifindex, type == IFACE_ADD);
 		break;
 	default:
 		/* ignore */
@@ -59,17 +60,23 @@ static void llmnr_iface_event_handle(enum iface_event_type type, int af, unsigne
 	}
 }
 
-int llmnr_init(const char *hostname, uint16_t port)
+int llmnr_init(const char *hostname, uint16_t port, bool ipv6)
 {
 	llmnr_hostname[0] = strlen(hostname);
 	strncpy(&llmnr_hostname[1], hostname, LLMNR_LABEL_MAX_SIZE);
 	llmnr_hostname[LLMNR_LABEL_MAX_SIZE + 1] = '\0';
 	log_info("Starting llmnrd on port %u, hostname %s\n", port, hostname);
 
-	llmnr_sock = socket_open_ipv4(port);
-	if (llmnr_sock < 0)
+	llmnr_sock_ipv4 = socket_open_ipv4(port);
+	if (llmnr_sock_ipv4 < 0)
 		return -1;
 
+	if (ipv6) {
+		llmnr_sock_ipv6 = socket_open_ipv6(port);
+		if (llmnr_sock_ipv6 < 0)
+			return -1;
+	}
+
 	iface_register_event_handler(&llmnr_iface_event_handle);
 
 	return 0;
@@ -94,7 +101,7 @@ static bool llmnr_name_matches(const uint8_t *query)
 
 static void llmnr_respond(unsigned int ifindex, const struct llmnr_hdr *hdr,
 			  const uint8_t *query, size_t query_len, int sock,
-			  const struct sockaddr *sa)
+			  const struct sockaddr_storage *sst)
 {
 	uint16_t qtype, qclass;
 	uint8_t name_len = query[0];
@@ -196,14 +203,14 @@ static void llmnr_respond(unsigned int ifindex, const struct llmnr_hdr *hdr,
 		memcpy(pkt_put(p, addr_size), addr, addr_size);
 	}
 
-	if (sendto(sock, p->data, pkt_len(p), 0, sa, sizeof(struct sockaddr_in)) < 0)
+	if (sendto(sock, p->data, pkt_len(p), 0, (struct sockaddr *)sst, sizeof(*sst)) < 0)
 		log_err("Failed to send response: %s\n", strerror(errno));
 
 	pkt_free(p);
 }
 
 static void llmnr_packet_process(unsigned int ifindex, const uint8_t *pktbuf, size_t len,
-				 int sock, const struct sockaddr *sa)
+				 int sock, const struct sockaddr_storage *sst)
 {
 	const struct llmnr_hdr *hdr = (const struct llmnr_hdr *)pktbuf;
 	uint16_t flags, qdcount;
@@ -232,7 +239,51 @@ static void llmnr_packet_process(unsigned int ifindex, const uint8_t *pktbuf, si
 
 	/* Authoritative? */
 	if (llmnr_name_matches(query))
-		llmnr_respond(ifindex, hdr, query, query_len, sock, sa);
+		llmnr_respond(ifindex, hdr, query, query_len, sock, sst);
+}
+
+static void llmnr_recv(int sock)
+{
+	uint8_t pktbuf[2048], aux[128];
+	struct msghdr msg;
+	struct iovec io;
+	struct sockaddr_storage sin_r;
+	struct cmsghdr *cmsg;
+	ssize_t recvlen;
+	int ifindex = -1;
+
+	io.iov_base = pktbuf;
+	io.iov_len = sizeof(pktbuf);
+
+	memset(&msg, 0, sizeof(msg));
+	msg.msg_name = &sin_r;
+	msg.msg_namelen = sizeof(sin_r);
+	msg.msg_iov = &io;
+	msg.msg_iovlen = 1;
+	msg.msg_control = aux;
+	msg.msg_controllen = sizeof(aux);
+
+	if ((recvlen = recvmsg(sock, &msg, 0)) < 0) {
+		if (errno != EINTR)
+			log_err("Failed to receive packet: %s\n", strerror(errno));
+		return;
+	}
+
+	for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) {
+		if (cmsg->cmsg_level == IPPROTO_IP && cmsg->cmsg_type == IP_PKTINFO) {
+			struct in_pktinfo *in = (struct in_pktinfo *)CMSG_DATA(cmsg);
+			ifindex = in->ipi_ifindex;
+		} else if (cmsg->cmsg_level == IPPROTO_IPV6 && cmsg->cmsg_type == IPV6_PKTINFO) {
+			struct in6_pktinfo *in6 = (struct in6_pktinfo *)CMSG_DATA(cmsg);
+			ifindex = in6->ipi6_ifindex;
+		}
+	}
+
+	if (ifindex >= 0)
+		llmnr_packet_process(ifindex, pktbuf, recvlen, sock,
+				     (const struct sockaddr_storage *)&sin_r);
+	else
+		log_warn("Could not get interface of incoming packet\n");
 }
 
 int llmnr_run(void)
@@ -240,44 +291,39 @@ int llmnr_run(void)
 	int ret = -1;
 
 	while (llmnr_running) {
-		uint8_t pktbuf[2048], aux[128];
-		struct msghdr msg;
-		struct iovec io;
-		struct sockaddr_in saddr_r;
-		struct cmsghdr *cmsg;
-		ssize_t recvlen;
-		unsigned int ifindex = 0;
-
-		io.iov_base = pktbuf;
-		io.iov_len = sizeof(pktbuf);
-
-		memset(&msg, 0, sizeof(msg));
-		msg.msg_name = &saddr_r;
-		msg.msg_namelen = sizeof(saddr_r);
-		msg.msg_iov = &io;
-		msg.msg_iovlen = 1;
-		msg.msg_control = aux;
-		msg.msg_controllen = sizeof(aux);
-
-		if ((recvlen = recvmsg(llmnr_sock, &msg, 0)) < 0) {
+		fd_set rfds;
+		struct timeval tv;
+		int nfds, ret;
+
+		FD_ZERO(&rfds);
+		FD_SET(llmnr_sock_ipv4, &rfds);
+		if (llmnr_sock_ipv6 >= 0) {
+			FD_SET(llmnr_sock_ipv6, &rfds);
+			nfds = max(llmnr_sock_ipv4, llmnr_sock_ipv6) + 1;
+		} else
+			nfds = llmnr_sock_ipv4 + 1;
+
+		tv.tv_sec = 0;
+		tv.tv_usec = 200;
+
+		ret = select(nfds, &rfds, NULL, NULL, &tv);
+		if (ret < 0) {
 			if (errno != EINTR)
-				log_err("Failed to receive packet: %s\n", strerror(errno));
+				log_err("Failed to select() on socket: %s\n", strerror(errno));
 			goto out;
+		} else if (ret) {
+			if (FD_ISSET(llmnr_sock_ipv4, &rfds))
+				llmnr_recv(llmnr_sock_ipv4);
+			if (llmnr_sock_ipv6 >= 0 && FD_ISSET(llmnr_sock_ipv6, &rfds))
+				llmnr_recv(llmnr_sock_ipv6);
 		}
-
-		for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg)) {
-			if (cmsg->cmsg_level == IPPROTO_IP && cmsg->cmsg_type == IP_PKTINFO) {
-				struct in_pktinfo *info = (struct in_pktinfo *)CMSG_DATA(cmsg);
-				ifindex = info->ipi_ifindex;
-			}
-		}
-
-		llmnr_packet_process(ifindex, pktbuf, recvlen, llmnr_sock, (const struct sockaddr *)&saddr_r);
 	}
 
 	ret = 0;
 out:
-	close(llmnr_sock);
+	close(llmnr_sock_ipv4);
+	if (llmnr_sock_ipv6 >= 0)
+		close(llmnr_sock_ipv6);
 	return ret;
 }
 
diff --git a/llmnr.h b/llmnr.h
index cc515f1..da86440 100644
--- a/llmnr.h
+++ b/llmnr.h
@@ -19,9 +19,10 @@
 #ifndef LLMNR_H
 #define LLMNR_H
 
+#include <stdbool.h>
 #include <stdint.h>
 
-int llmnr_init(const char *hostname, uint16_t port);
+int llmnr_init(const char *hostname, uint16_t port, bool ipv6);
 int llmnr_run(void);
 void llmnr_stop(void);
 
diff --git a/llmnrd.c b/llmnrd.c
index 4da5c4e..fb56b30 100644
--- a/llmnrd.c
+++ b/llmnrd.c
@@ -38,10 +38,11 @@
 #include "llmnr.h"
 #include "llmnr-packet.h"
 
-static const char *short_opts = "H:p:dhV";
+static const char *short_opts = "H:p:6dhV";
 static const struct option long_opts[] = {
 	{ "hostname",	required_argument,	NULL, 'H' },
 	{ "port",	required_argument,	NULL, 'p' },
+	{ "ipv6",	no_argument,		NULL, '6' },
 	{ "daemonize",	no_argument,		NULL, 'd' },
 	{ "help",	no_argument,		NULL, 'h' },
 	{ "version",	no_argument,		NULL, 'V' },
@@ -54,6 +55,7 @@ static void __noreturn usage_and_exit(int status)
 			"Options:\n"
 			"  -H, --hostname NAME  set hostname to respond with (default: system hostname)\n"
 			"  -p, --port NUM       set port number to listen on (default: %d)\n"
+			"  -6, --ipv6           enable LLMNR name resolution over IPv6\n"
 			"  -d, --daemonize      run as daemon in the background\n"
 			"  -h, --help           show this help and exit\n"
 			"  -V, --version        show version information and exit\n",
@@ -107,7 +109,7 @@ int main(int argc, char **argv)
 {
 	int c, ret = EXIT_FAILURE;
 	long num_arg;
-	bool daemonize = false;
+	bool daemonize = false, ipv6 = false;
 	char *hostname = NULL;
 	uint16_t port = LLMNR_UDP_PORT;
 
@@ -126,6 +128,9 @@ int main(int argc, char **argv)
 				return EXIT_FAILURE;
 			}
 			port = num_arg;
+		case '6':
+			ipv6 = true;
+			break;
 		case 'V':
 			version_and_exit();
 		case 'h':
@@ -156,7 +161,7 @@ int main(int argc, char **argv)
 		}
 	}
 
-	if (llmnr_init(hostname, port) < 0)
+	if (llmnr_init(hostname, port, ipv6) < 0)
 		goto out;
 
 	if (iface_start_thread() < 0)
diff --git a/socket.c b/socket.c
index 2e2b28e..6e9b035 100644
--- a/socket.c
+++ b/socket.c
@@ -88,7 +88,13 @@ int socket_open_ipv6(uint16_t port)
 	opt_pktinfo = IPV6_PKTINFO;
 #endif
 	if (setsockopt(sock, IPPROTO_IPV6, opt_pktinfo, &YES, sizeof(YES)) < 0) {
-		log_err("Failed to set IPv4 packet info socket option: %s\n", strerror(errno));
+		log_err("Failed to set IPv6 packet info socket option: %s\n", strerror(errno));
+		goto err;
+	}
+
+	/* IPv6 only socket */
+	if (setsockopt(sock, IPPROTO_IPV6, IPV6_V6ONLY, &YES, sizeof(YES)) < 0) {
+		log_err("Failed to set IPv6 only socket option: %s\n", strerror(errno));
 		goto err;
 	}
 
@@ -156,7 +162,7 @@ int socket_mcast_group_ipv4(int sock, unsigned int ifindex, bool join)
 
 	if (setsockopt(sock, IPPROTO_IP, join ? IP_ADD_MEMBERSHIP : IP_DROP_MEMBERSHIP,
 		       &mreq, sizeof(mreq)) < 0) {
-		log_err("Failed to join multicast group on interface %s: %s\n",
+		log_err("Failed to join IPv4 multicast group on interface %s: %s\n",
 			if_indextoname(ifindex, ifname), strerror(errno));
 		return -1;
 	}
@@ -166,19 +172,20 @@ int socket_mcast_group_ipv4(int sock, unsigned int ifindex, bool join)
 
 int socket_mcast_group_ipv6(int sock, unsigned int ifindex, bool join)
 {
-	struct ipv6_mreq mreq;
+	struct ipv6_mreq mreq6;
 	char ifname[IF_NAMESIZE];
 
 	/* silently ignore, we might not be listening on an IPv6 socket */
 	if (sock < 0)
 		return -1;
 
-	memset(&mreq, 0, sizeof(mreq));
-	inet_pton(AF_INET6, LLMNR_IPV6_MCAST_ADDR, &mreq.ipv6mr_multiaddr);
+	memset(&mreq6, 0, sizeof(mreq6));
+	mreq6.ipv6mr_interface = ifindex;
+	inet_pton(AF_INET6, LLMNR_IPV6_MCAST_ADDR, &mreq6.ipv6mr_multiaddr);
 
-	if (setsockopt(sock, IPPROTO_IP, join ? IP_ADD_MEMBERSHIP : IP_DROP_MEMBERSHIP,
-		       &mreq, sizeof(mreq)) < 0) {
-		log_err("Failed to join multicast group on interface %s: %s\n",
+	if (setsockopt(sock, IPPROTO_IPV6, join ? IPV6_ADD_MEMBERSHIP : IPV6_DROP_MEMBERSHIP,
+		       &mreq6, sizeof(mreq6)) < 0) {
+		log_err("Failed to join IPv6 multicast group on interface %s: %s\n",
 			if_indextoname(ifindex, ifname), strerror(errno));
 		return -1;
 	}
diff --git a/util.h b/util.h
index 13a8637..d4933c4 100644
--- a/util.h
+++ b/util.h
@@ -30,7 +30,7 @@
 #define ARRAY_SIZE(x)	(sizeof(x) / sizeof((x)[0]))
 
 /*
- * min() macro with strict type-checking.
+ * min()/max() macros with strict type-checking.
  * Taken from linux/kernel.h
  */
 #undef min
@@ -40,6 +40,13 @@
 	(void) (&_min1 == &_min2);	\
 	_min1 < _min2 ? _min1 : _min2; })
 
+#undef max
+#define max(x, y) ({			\
+	typeof(x) _max1 = (x);		\
+	typeof(y) _max2 = (y);		\
+	(void) (&_max1 == &_max2);	\
+	_max1 > _max2 ? _max1 : _max2; })
+
 static inline void __noreturn panic(const char *fmt, ...)
 {
 	va_list vl;