Greatly improve handling of hosts with multiple IP addresses

1 files changed, 124 insertions, 21 deletions

diff --git a/src/fastd.c b/src/fastd.c
index d82589e..70bd99e 100644
--- a/src/fastd.c
+++ b/src/fastd.c
@@ -224,6 +224,19 @@ static int bind_socket(fastd_context_t *ctx, const fastd_bind_address_t *addr, b
 	fastd_setfd(ctx, fd, FD_CLOEXEC, 0);
 	fastd_setfl(ctx, fd, O_NONBLOCK, 0);
 
+	int one = 1;
+	if (setsockopt(fd, IPPROTO_IP, IP_PKTINFO, &one, sizeof(one))) {
+		pr_error_errno(ctx, "setsockopt: unable to set IP_PKTINFO");
+		goto error;
+	}
+
+	if (af == AF_INET6) {
+		if (setsockopt(fd, IPPROTO_IPV6, IPV6_RECVPKTINFO, &one, sizeof(one))) {
+			pr_error_errno(ctx, "setsockopt: unable to set IPV6_RECVPKTINFO");
+			goto error;
+		}
+	}
+
 	if (addr->bindtodev) {
 		if (setsockopt(fd, SOL_SOCKET, SO_BINDTODEVICE, addr->bindtodev, strlen(addr->bindtodev))) {
 			if (warn)
@@ -303,10 +316,14 @@ static bool bind_sockets(fastd_context_t *ctx) {
 				continue;
 			}
 
+			fastd_peer_address_t bound_addr = *ctx->socks[i].bound_addr;
+			if (!ctx->socks[i].addr->addr.sa.sa_family)
+				bound_addr.sa.sa_family = AF_UNSPEC;
+
 			if (ctx->socks[i].addr->bindtodev)
-				pr_info(ctx, "successfully bound to %B on `%s'", ctx->socks[i].bound_addr, ctx->socks[i].addr->bindtodev);
+				pr_info(ctx, "successfully bound to %B on `%s'", &bound_addr, ctx->socks[i].addr->bindtodev);
 			else
-				pr_info(ctx, "successfully bound to %B", ctx->socks[i].bound_addr);
+				pr_info(ctx, "successfully bound to %B", &bound_addr);
 		}
 	}
 
@@ -504,21 +521,21 @@ static size_t methods_min_decrypt_tail_space(fastd_context_t *ctx) {
 	return ret;
 }
 
-static void fastd_send_type(fastd_context_t *ctx, const fastd_socket_t *sock, const fastd_peer_address_t *address, uint8_t packet_type, fastd_buffer_t buffer) {
+static void fastd_send_type(fastd_context_t *ctx, const fastd_socket_t *sock, const fastd_peer_address_t *local_addr, const fastd_peer_address_t *remote_addr, uint8_t packet_type, fastd_buffer_t buffer) {
 	if (!sock)
 		exit_bug(ctx, "send: sock == NULL");
 
-	struct msghdr msg;
-	memset(&msg, 0, sizeof(msg));
+	struct msghdr msg = {};
+	char cbuf[1024] = {};
 
-	switch (address->sa.sa_family) {
+	switch (remote_addr->sa.sa_family) {
 	case AF_INET:
-		msg.msg_name = (void*)&address->in;
+		msg.msg_name = (void*)&remote_addr->in;
 		msg.msg_namelen = sizeof(struct sockaddr_in);
 		break;
 
 	case AF_INET6:
-		msg.msg_name = (void*)&address->in6;
+		msg.msg_name = (void*)&remote_addr->in6;
 		msg.msg_namelen = sizeof(struct sockaddr_in6);
 		break;
 
@@ -534,6 +551,36 @@ static void fastd_send_type(fastd_context_t *ctx, const fastd_socket_t *sock, co
 	msg.msg_iov = iov;
 	msg.msg_iovlen = buffer.len ? 2 : 1;
 
+	if (local_addr && (local_addr->sa.sa_family == AF_INET || local_addr->sa.sa_family == AF_INET6)) {
+		msg.msg_control = cbuf;
+		msg.msg_controllen = sizeof(cbuf);
+
+		struct cmsghdr *cmsg = CMSG_FIRSTHDR(&msg);
+
+		if (local_addr->sa.sa_family == AF_INET) {
+			cmsg->cmsg_level = IPPROTO_IP;
+			cmsg->cmsg_type = IP_PKTINFO;
+
+			msg.msg_controllen = cmsg->cmsg_len = CMSG_LEN(sizeof(struct in_pktinfo));
+
+			struct in_pktinfo *pktinfo = (struct in_pktinfo*)CMSG_DATA(cmsg);
+			pktinfo->ipi_addr = local_addr->in.sin_addr;
+		}
+		else {
+			cmsg->cmsg_level = IPPROTO_IPV6;
+			cmsg->cmsg_type = IPV6_PKTINFO;
+
+			msg.msg_controllen = cmsg->cmsg_len = CMSG_LEN(sizeof(struct in6_pktinfo));
+
+			struct in6_pktinfo *pktinfo = (struct in6_pktinfo*)CMSG_DATA(cmsg);
+
+			pktinfo->ipi6_addr = local_addr->in6.sin6_addr;
+
+			if (IN6_IS_ADDR_LINKLOCAL(&local_addr->in6.sin6_addr))
+				pktinfo->ipi6_ifindex = local_addr->in6.sin6_scope_id;
+		}
+	}
+
 	int ret;
 	do {
 		ret = sendmsg(sock->fd, &msg, 0);
@@ -545,12 +592,12 @@ static void fastd_send_type(fastd_context_t *ctx, const fastd_socket_t *sock, co
 	fastd_buffer_free(buffer);
 }
 
-void fastd_send(fastd_context_t *ctx, const fastd_socket_t *sock, const fastd_peer_address_t *address, fastd_buffer_t buffer) {
-	fastd_send_type(ctx, sock, address, PACKET_DATA, buffer);
+void fastd_send(fastd_context_t *ctx, const fastd_socket_t *sock, const fastd_peer_address_t *local_addr, const fastd_peer_address_t *remote_addr, fastd_buffer_t buffer) {
+	fastd_send_type(ctx, sock, local_addr, remote_addr, PACKET_DATA, buffer);
 }
 
-void fastd_send_handshake(fastd_context_t *ctx, const fastd_socket_t *sock, const fastd_peer_address_t *address, fastd_buffer_t buffer) {
-	fastd_send_type(ctx, sock, address, PACKET_HANDSHAKE, buffer);
+void fastd_send_handshake(fastd_context_t *ctx, const fastd_socket_t *sock, const fastd_peer_address_t *local_addr, const fastd_peer_address_t *remote_addr, fastd_buffer_t buffer) {
+	fastd_send_type(ctx, sock, local_addr, remote_addr, PACKET_HANDSHAKE, buffer);
 }
 
 void fastd_handle_receive(fastd_context_t *ctx, fastd_peer_t *peer, fastd_buffer_t buffer) {
@@ -739,7 +786,7 @@ static void send_handshake(fastd_context_t *ctx, fastd_peer_t *peer) {
 			pr_debug(ctx, "sending handshake to %P...", peer);
 			peer->last_handshake = ctx->now;
 			peer->last_handshake_address = peer->address;
-			ctx->conf->protocol->handshake_init(ctx, peer->sock, &peer->address, peer);
+			ctx->conf->protocol->handshake_init(ctx, peer->sock, &peer->local_address, &peer->address, peer);
 		}
 	}
 
@@ -828,15 +875,65 @@ static void handle_socket(fastd_context_t *ctx, fastd_socket_t *sock) {
 	size_t max_len = PACKET_TYPE_LEN + methods_max_packet_size(ctx);
 	fastd_buffer_t buffer = fastd_buffer_alloc(ctx, max_len, methods_min_decrypt_head_space(ctx), methods_min_decrypt_tail_space(ctx));
 	uint8_t *packet_type;
+	fastd_peer_address_t local_addr = {};
 
 	fastd_peer_address_t recvaddr;
-	socklen_t recvaddrlen = sizeof(recvaddr);
+	struct iovec buffer_vec = { .iov_base = buffer.data, .iov_len = buffer.len };
+	char cbuf[1024];
+
+	struct msghdr message = {
+		.msg_name = &recvaddr,
+		.msg_namelen = sizeof(recvaddr),
+		.msg_iov = &buffer_vec,
+		.msg_iovlen = 1,
+		.msg_control = cbuf,
+		.msg_controllen = sizeof(cbuf),
+	};
 
-	ssize_t len = recvfrom(sock->fd, buffer.data, buffer.len, 0, (struct sockaddr*)&recvaddr, &recvaddrlen);
+	ssize_t len = recvmsg(sock->fd, &message, 0);
 	if (len <= 0) {
 		if (len < 0 && errno != EINTR)
-			pr_warn(ctx, "recvfrom: %s", strerror(errno));
+			pr_warn_errno(ctx, "recvmsg");
+
+		fastd_buffer_free(buffer);
+		return;
+	}
+
+	struct cmsghdr *cmsg;
+	for (cmsg = CMSG_FIRSTHDR(&message); cmsg; cmsg = CMSG_NXTHDR(&message, cmsg)) {
+		if ((char*)cmsg + sizeof(*cmsg) > cbuf + sizeof(cbuf))
+			break;
+
+		if (cmsg->cmsg_level == IPPROTO_IP && cmsg->cmsg_type == IP_PKTINFO) {
+			struct in_pktinfo *pktinfo = (struct in_pktinfo*)CMSG_DATA(cmsg);
+			if ((char*)pktinfo + sizeof(*pktinfo) > cbuf + sizeof(cbuf))
+				break;
+
+			local_addr.in.sin_family = AF_INET;
+			local_addr.in.sin_addr = pktinfo->ipi_addr;
+			local_addr.in.sin_port = fastd_peer_address_get_port(sock->bound_addr);
+
+			break;
+		}
 
+		if (cmsg->cmsg_level == IPPROTO_IPV6 && cmsg->cmsg_type == IPV6_PKTINFO) {
+			struct in6_pktinfo *pktinfo = (struct in6_pktinfo*)CMSG_DATA(cmsg);
+			if ((char*)pktinfo + sizeof(*pktinfo) > cbuf + sizeof(cbuf))
+				break;
+
+			local_addr.in6.sin6_family = AF_INET6;
+			local_addr.in6.sin6_addr = pktinfo->ipi6_addr;
+			local_addr.in6.sin6_port = fastd_peer_address_get_port(sock->bound_addr);
+
+			if (IN6_IS_ADDR_LINKLOCAL(&local_addr.in6.sin6_addr))
+				local_addr.in6.sin6_scope_id = pktinfo->ipi6_ifindex;
+
+			break;
+		}
+	}
+
+	if (!local_addr.sa.sa_family) {
+		pr_error(ctx, "received packet without packet info");
 		fastd_buffer_free(buffer);
 		return;
 	}
@@ -870,11 +967,17 @@ static void handle_socket(fastd_context_t *ctx, fastd_socket_t *sock) {
 
 		switch (*packet_type) {
 		case PACKET_DATA:
-			ctx->conf->protocol->handle_recv(ctx, peer, buffer);
+			if (fastd_peer_is_established(peer) && fastd_peer_address_equal(&peer->local_address, &local_addr)) {
+				ctx->conf->protocol->handle_recv(ctx, peer, buffer);
+			}
+			else {
+				fastd_buffer_free(buffer);
+				ctx->conf->protocol->handshake_init(ctx, sock, &local_addr, &recvaddr, NULL);
+			}
 			break;
 
 		case PACKET_HANDSHAKE:
-			fastd_handshake_handle(ctx, sock, &recvaddr, peer, buffer);
+			fastd_handshake_handle(ctx, sock, &local_addr, &recvaddr, peer, buffer);
 			break;
 
 		default:
@@ -887,11 +990,11 @@ static void handle_socket(fastd_context_t *ctx, fastd_socket_t *sock) {
 		switch (*packet_type) {
 		case PACKET_DATA:
 			fastd_buffer_free(buffer);
-			ctx->conf->protocol->handshake_init(ctx, sock, &recvaddr, NULL);
+			ctx->conf->protocol->handshake_init(ctx, sock, &local_addr, &recvaddr, NULL);
 			break;
 
 		case PACKET_HANDSHAKE:
-			fastd_handshake_handle(ctx, sock, &recvaddr, NULL, buffer);
+			fastd_handshake_handle(ctx, sock, &local_addr, &recvaddr, NULL, buffer);
 			break;
 
 		default:
@@ -933,7 +1036,7 @@ static void handle_resolv_returns(fastd_context_t *ctx) {
 
 		peer->last_resolve_return = ctx->now;
 
-		if (fastd_peer_claim_address(ctx, peer, NULL, &resolve_return.addr)) {
+		if (fastd_peer_claim_address(ctx, peer, NULL, NULL, &resolve_return.addr)) {
 			send_handshake(ctx, peer);
 		}
 		else {