From 13d7080ac19a5ac762022131e541ce27fc716c18 Mon Sep 17 00:00:00 2001
From: Samuel Thibault <samuel.thibault@ens-lyon.org>
Date: Thu, 18 May 2023 00:38:19 +0200
Subject: [PATCH] Add recvfromto6

---
 l2tpns.c |  4 +--
 util.c   | 93 +++++++++++++++++++++++++++++++++++++++++++-------------
 util.h   |  5 ++-
 3 files changed, 78 insertions(+), 24 deletions(-)

diff --git a/l2tpns.c b/l2tpns.c
index 209d637..1fa32a0 100644
--- a/l2tpns.c
+++ b/l2tpns.c
@@ -5205,14 +5205,14 @@ static void mainloop(void)
 
 				case FD_TYPE_CONTROL: // nsctl commands
 					alen = sizeof(addr);
-					s = recvfromto(controlfd, p, size_bufp, MSG_WAITALL, (struct sockaddr *) &addr, &alen, &local);
+					s = recvfromto(controlfd, p, size_bufp, MSG_WAITALL, (struct sockaddr *) &addr, &alen, &local, NULL);
 					if (s > 0) processcontrol(p, s, &addr, alen, &local);
 					n--;
 					break;
 
 				case FD_TYPE_DAE: // DAE requests
 					alen = sizeof(addr);
-					s = recvfromto(daefd, p, size_bufp, MSG_WAITALL, (struct sockaddr *) &addr, &alen, &local);
+					s = recvfromto(daefd, p, size_bufp, MSG_WAITALL, (struct sockaddr *) &addr, &alen, &local, NULL);
 					if (s > 0) processdae(p, s, &addr, alen, &local);
 					n--;
 					break;
diff --git a/util.c b/util.c
index 5c7c44b..6e50fce 100644
--- a/util.c
+++ b/util.c
@@ -1,9 +1,11 @@
 /* Misc util functions */
 
+#define _GNU_SOURCE
 #include <unistd.h>
 #include <errno.h>
 #include <sched.h>
 #include <sys/socket.h>
+#include <netinet/ip6.h>
 #include <netinet/in.h>
 #include <arpa/inet.h>
 #include <string.h>
@@ -132,41 +134,90 @@ pid_t fork_and_close()
     return pid;
 }
 
+static ssize_t recvfromtox(int s, void *buf, size_t len, int flags,
+    struct sockaddr *from, socklen_t *fromlen, char *cbuf, size_t cbuflen, struct msghdr *msg)
+{
+	ssize_t r;
+	struct iovec vec;
+
+	memset(msg, 0, sizeof(*msg));
+	msg->msg_name = from;
+	msg->msg_namelen = *fromlen;
+
+	vec.iov_base = buf;
+	vec.iov_len = len;
+	msg->msg_iov = &vec;
+	msg->msg_iovlen = 1;
+	msg->msg_flags = 0;
+
+	msg->msg_control = cbuf;
+	msg->msg_controllen = cbuflen;
+
+	if ((r = recvmsg(s, msg, flags)) < 0)
+		return r;
+
+	if (fromlen)
+		*fromlen = msg->msg_namelen;
+
+	return r;
+}
+
 ssize_t recvfromto(int s, void *buf, size_t len, int flags,
-	struct sockaddr *from, socklen_t *fromlen, struct in_addr *toaddr)
+    struct sockaddr *from, socklen_t *fromlen, struct in_addr *toaddr, int *ifidx)
 {
 	ssize_t r;
 	struct msghdr msg;
 	struct cmsghdr *cmsg;
-	struct iovec vec;
-	char cbuf[128];
+	char cbuf[BUFSIZ];
 
-	memset(&msg, 0, sizeof(msg));
-	msg.msg_name = from;
-	msg.msg_namelen = *fromlen;
-
-	vec.iov_base = buf;
-	vec.iov_len = len;
-	msg.msg_iov = &vec;
-	msg.msg_iovlen = 1;
-	msg.msg_flags = 0;
-
-	msg.msg_control = cbuf;
-	msg.msg_controllen = sizeof(cbuf);
-
-	if ((r = recvmsg(s, &msg, flags)) < 0)
+	if ((r = recvfromtox(s, buf, len, flags, from, fromlen, cbuf, sizeof(cbuf), &msg)) < 0)
 		return r;
 
-	if (fromlen)
-		*fromlen = msg.msg_namelen;
 
-	memset(toaddr, 0, sizeof(*toaddr));
+	if (toaddr)
+		memset(toaddr, 0, sizeof(*toaddr));
+	if (ifidx)
+		*ifidx = -1;
 	for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg))
 	{
 		if (cmsg->cmsg_level == SOL_IP && cmsg->cmsg_type == IP_PKTINFO)
 		{
 			struct in_pktinfo *i = (struct in_pktinfo *) CMSG_DATA(cmsg);
-			memcpy(toaddr, &i->ipi_addr, sizeof(*toaddr));
+			if (toaddr)
+				memcpy(toaddr, &i->ipi_addr, sizeof(*toaddr));
+			if (ifidx)
+				*ifidx = i->ipi_ifindex;
+			break;
+		}
+	}
+
+	return r;
+}
+
+ssize_t recvfromto6(int s, void *buf, size_t len, int flags,
+    struct sockaddr *from, socklen_t *fromlen, struct in6_addr *toaddr, int *ifidx)
+{
+	ssize_t r;
+	struct msghdr msg;
+	struct cmsghdr *cmsg;
+	char cbuf[BUFSIZ];
+
+	if ((r = recvfromtox(s, buf, len, flags, from, fromlen, cbuf, sizeof(cbuf), &msg)) < 0)
+		return r;
+
+	if (toaddr)
+		memset(toaddr, 0, sizeof(*toaddr));
+	if (ifidx)
+		*ifidx = -1;
+	for (cmsg = CMSG_FIRSTHDR(&msg); cmsg; cmsg = CMSG_NXTHDR(&msg, cmsg))
+	{
+		if (cmsg->cmsg_level == SOL_IPV6 && cmsg->cmsg_type == IPV6_PKTINFO)
+		{
+			struct in6_pktinfo *i = (struct in6_pktinfo *) CMSG_DATA(cmsg);
+			if (toaddr)
+				memcpy(toaddr, &i->ipi6_addr, sizeof(*toaddr));
+			if (ifidx)
+				*ifidx = i->ipi6_ifindex;
 			break;
 		}
 	}
diff --git a/util.h b/util.h
index 332c6f8..88ebeaf 100644
--- a/util.h
+++ b/util.h
@@ -10,6 +10,9 @@ ssize_t sendtofrom(int s, void const *buf, size_t len, int flags,
     struct sockaddr const *to, socklen_t tolen, struct in_addr const *from);
 
 ssize_t recvfromto(int s, void *buf, size_t len, int flags,
-    struct sockaddr *from, socklen_t *fromlen, struct in_addr *toaddr);
+    struct sockaddr *from, socklen_t *fromlen, struct in_addr *toaddr, int *ifidx);
+
+ssize_t recvfromto6(int s, void *buf, size_t len, int flags,
+    struct sockaddr *from, socklen_t *fromlen, struct in6_addr *toaddr, int *ifidx);
 
 #endif /* __UTIL_H__ */