[PATCH 1/3] udp: Use PKTINFO cmsgs to get destination address for received datagrams

[David Gibson <david@gibson.dropbear.id.au>]

Currently we get the source address for received datagrams from recvmsg(),
but we don't get the local destination address.  Sometimes we implicitly
know this because the receiving socket is bound to a specific address, but
when listening on 0.0.0.0 or ::, we don't.

We need this information to properly direct replies to flows which come in
to a non-default local address.  So, enable the IP_PKTINFO and IPV6_PKTINFO
control messages to obtain this information in udp_peek_addr().  For now
we log a trace messages but don't do anything more with the information.

Signed-off-by: David Gibson <david@gibson.dropbear.id.au>
---
 udp.c  | 37 +++++++++++++++++++++++++++++++++++--
 util.c |  8 ++++++--
 2 files changed, 41 insertions(+), 4 deletions(-)

diff --git a/udp.c b/udp.c
index ed6edc12..a71141a5 100644
--- a/udp.c
+++ b/udp.c
@@ -587,18 +587,29 @@ static int udp_sock_errs(const struct ctx *c, union epoll_ref ref)
 	return n_err;
 }
 
+#define PKTINFO_SPACE					\
+	MAX(CMSG_SPACE(sizeof(struct in_pktinfo)),	\
+	    CMSG_SPACE(sizeof(struct in6_pktinfo)))
+
 /**
  * udp_peek_addr() - Get source address for next packet
  * @s:		Socket to get information from
  * @src:	Socket address (output)
+ * @dst:	(Local) destination address (output)
  *
  * Return: 0 on success, -1 otherwise
  */
-static int udp_peek_addr(int s, union sockaddr_inany *src)
+static int udp_peek_addr(int s, union sockaddr_inany *src,
+			 union inany_addr *dst)
 {
+	char sastr[SOCKADDR_STRLEN], dstr[INANY_ADDRSTRLEN];
+	const struct cmsghdr *hdr;
+	char cmsg[PKTINFO_SPACE];
 	struct msghdr msg = {
 		.msg_name = src,
 		.msg_namelen = sizeof(*src),
+		.msg_control = cmsg,
+		.msg_controllen = sizeof(cmsg),
 	};
 	int rc;
 
@@ -608,6 +619,27 @@ static int udp_peek_addr(int s, union sockaddr_inany *src)
 			warn_perror("Error peeking at socket address");
 		return rc;
 	}
+
+	hdr = CMSG_FIRSTHDR(&msg);
+	if (hdr && hdr->cmsg_level == IPPROTO_IP &&
+	    hdr->cmsg_type == IP_PKTINFO) {
+		const struct in_pktinfo *info4 = (void *)CMSG_DATA(hdr);
+
+		*dst = inany_from_v4(info4->ipi_addr);
+	} else if (hdr && hdr->cmsg_level == IPPROTO_IPV6 &&
+		   hdr->cmsg_type == IPV6_PKTINFO) {
+		const struct in6_pktinfo *info6 = (void *)CMSG_DATA(hdr);
+
+		dst->a6 = info6->ipi6_addr;
+	} else {
+		debug("Unexpected cmsg on UDP datagram");
+		*dst = inany_any6;
+	}
+
+	trace("Peeked UDP datagram: %s -> %s",
+	      sockaddr_ntop(src, sastr, sizeof(sastr)),
+	      inany_ntop(dst, dstr, sizeof(dstr)));
+
 	return 0;
 }
 
@@ -702,8 +734,9 @@ void udp_sock_fwd(const struct ctx *c, int s, uint8_t frompif,
 		  in_port_t port, const struct timespec *now)
 {
 	union sockaddr_inany src;
+	union inany_addr dst;
 
-	while (udp_peek_addr(s, &src) == 0) {
+	while (udp_peek_addr(s, &src, &dst) == 0) {
 		flow_sidx_t tosidx = udp_flow_from_sock(c, frompif, port,
 							&src, now);
 		uint8_t topif = pif_at_sidx(tosidx);
diff --git a/util.c b/util.c
index 0f68cf57..62a6003e 100644
--- a/util.c
+++ b/util.c
@@ -109,11 +109,15 @@ int sock_l4_sa(const struct ctx *c, enum epoll_type type,
 		debug("Failed to set SO_REUSEADDR on socket %i", fd);
 
 	if (proto == IPPROTO_UDP) {
+		int pktinfo = af == AF_INET ? IP_PKTINFO : IPV6_RECVPKTINFO;
+		int recverr = af == AF_INET ? IP_RECVERR : IPV6_RECVERR;
 		int level = af == AF_INET ? IPPROTO_IP : IPPROTO_IPV6;
-		int opt = af == AF_INET ? IP_RECVERR : IPV6_RECVERR;
 
-		if (setsockopt(fd, level, opt, &y, sizeof(y)))
+		if (setsockopt(fd, level, recverr, &y, sizeof(y)))
 			die_perror("Failed to set RECVERR on socket %i", fd);
+
+		if (setsockopt(fd, level, pktinfo, &y, sizeof(y)))
+			die_perror("Failed to set PKTINFO on socket %i", fd);
 	}
 
 	if (ifname && *ifname) {
-- 
@@ -109,11 +109,15 @@ int sock_l4_sa(const struct ctx *c, enum epoll_type type,
 		debug("Failed to set SO_REUSEADDR on socket %i", fd);
 
 	if (proto == IPPROTO_UDP) {
+		int pktinfo = af == AF_INET ? IP_PKTINFO : IPV6_RECVPKTINFO;
+		int recverr = af == AF_INET ? IP_RECVERR : IPV6_RECVERR;
 		int level = af == AF_INET ? IPPROTO_IP : IPPROTO_IPV6;
-		int opt = af == AF_INET ? IP_RECVERR : IPV6_RECVERR;
 
-		if (setsockopt(fd, level, opt, &y, sizeof(y)))
+		if (setsockopt(fd, level, recverr, &y, sizeof(y)))
 			die_perror("Failed to set RECVERR on socket %i", fd);
+
+		if (setsockopt(fd, level, pktinfo, &y, sizeof(y)))
+			die_perror("Failed to set PKTINFO on socket %i", fd);
 	}
 
 	if (ifname && *ifname) {
-- 
2.49.0