From ad947bd1e598d87c69f7b2e540d9b7348401fe00 Mon Sep 17 00:00:00 2001
From: Pablo Neira Ayuso <pablo@netfilter.org>
Date: Fri, 3 Nov 2023 17:58:43 +0100
Subject: [PATCH 1/7] gtp-link: add IPv6 support

Update tool to allow to specify family that is used for the outer network
header in GTP tunnels.

./gtp-link add gtp0 ip 			# IPv4 GGSN
./gtp-link add gtp0 ip --sgsn		# IPv4 SGSN
./gtp-link add gtp0 ip6 		# IPv6 GGSN
./gtp-link add gtp0 ip6 --sgsn		# IPv6 SGSN
---
 tools/gtp-link.c | 122 +++++++++++++++++++++++++++++++++++------------
 1 file changed, 91 insertions(+), 31 deletions(-)

diff --git a/tools/gtp-link.c b/tools/gtp-link.c
index d342f2ba4b86..3e98f56e5c0e 100644
--- a/tools/gtp-link.c
+++ b/tools/gtp-link.c
@@ -40,13 +40,75 @@
 
 #include <libgtpnl/gtpnl.h>
 
+struct gtp_server_sock {
+	int			family;
+	int			fd1;
+	int			fd2;
+	socklen_t		len;
+	struct {
+		union {
+			struct sockaddr_in	in;
+			struct sockaddr_in6	in6;
+		} fd1;
+		union {
+			struct sockaddr_in	in;
+			struct sockaddr_in6	in6;
+		} fd2;
+	} sockaddr;
+};
+
+static void setup_sockaddr_in(struct sockaddr_in *sockaddr, uint16_t port)
+{
+	sockaddr->sin_family = AF_INET;
+	sockaddr->sin_port = htons(port);
+	sockaddr->sin_addr.s_addr = INADDR_ANY;
+}
+
+static void setup_sockaddr_in6(struct sockaddr_in6 *sockaddr, uint16_t port)
+{
+	sockaddr->sin6_family = AF_INET6;
+	sockaddr->sin6_port = htons(port);
+	sockaddr->sin6_addr = in6addr_any;
+}
+
+static int setup_socket(struct gtp_server_sock *gtp_sock, int family)
+{
+	int fd1 = socket(family, SOCK_DGRAM, 0);
+	int fd2 = socket(family, SOCK_DGRAM, 0);
+
+	if (fd1 < 0 || fd2 < 0)
+		return -1;
+
+	memset(gtp_sock, 0, sizeof(*gtp_sock));
+
+	gtp_sock->family = family;
+	gtp_sock->fd1 = fd1;
+	gtp_sock->fd2 = fd2;
+
+	switch (family) {
+	case AF_INET:
+		gtp_sock->len = sizeof(struct sockaddr_in);
+		setup_sockaddr_in(&gtp_sock->sockaddr.fd1.in, 3386);
+		setup_sockaddr_in(&gtp_sock->sockaddr.fd2.in, 2152);
+		break;
+	case AF_INET6:
+		gtp_sock->len = sizeof(struct sockaddr_in6);
+		setup_sockaddr_in6(&gtp_sock->sockaddr.fd1.in6, 3386);
+		setup_sockaddr_in6(&gtp_sock->sockaddr.fd2.in6, 2152);
+		break;
+	}
+
+	return 0;
+}
+
 int main(int argc, char *argv[])
 {
 	char buf[MNL_SOCKET_BUFFER_SIZE];
-	int ret, sgsn_mode = 0;
+	struct gtp_server_sock gtp_sock;
+	int ret, sgsn_mode = 0, family;
 
-	if (argc < 3) {
-		printf("Usage: %s add <device> [--sgsn]\n", argv[0]);
+	if (argc < 4) {
+		printf("Usage: %s add <device> <family> [--sgsn]\n", argv[0]);
 		printf("       %s del <device>\n", argv[0]);
 		exit(EXIT_FAILURE);
 	}
@@ -59,41 +121,37 @@ int main(int argc, char *argv[])
 		return 0;
 	}
 
-	if (argc > 3 && !strcmp(argv[3], "--sgsn"))
+	if (argc > 4 && !strcmp(argv[4], "--sgsn"))
 		sgsn_mode = 1;
 
-	int fd1 = socket(AF_INET, SOCK_DGRAM, 0);
-	int fd2 = socket(AF_INET, SOCK_DGRAM, 0);
-	struct sockaddr_in sockaddr_fd1 = {
-		.sin_family	= AF_INET,
-		.sin_port	= htons(3386),
-		.sin_addr	= {
-			.s_addr 	= INADDR_ANY,
-		},
-	};
-	struct sockaddr_in sockaddr_fd2 = {
-		.sin_family	= AF_INET,
-		.sin_port	= htons(2152),
-		.sin_addr	= {
-			.s_addr 	= INADDR_ANY,
-		},
-	};
-
-	if (bind(fd1, (struct sockaddr *) &sockaddr_fd1,
-		 sizeof(sockaddr_fd1)) < 0) {
+	if (!strcmp(argv[3], "ip"))
+		family = AF_INET;
+	else if (!strcmp(argv[3], "ip6"))
+		family = AF_INET6;
+	else {
+		fprintf(stderr, "unsupport family `%s', expecting `ip' or `ip6'\n",
+			argv[3]);
+		exit(EXIT_FAILURE);
+	}
+
+	if (setup_socket(&gtp_sock, family) < 0) {
+		perror("socket");
+		exit(EXIT_FAILURE);
+	}
+
+	if (bind(gtp_sock.fd1, (struct sockaddr *) &gtp_sock.sockaddr.fd1, gtp_sock.len) < 0) {
 		perror("bind");
 		exit(EXIT_FAILURE);
 	}
-	if (bind(fd2, (struct sockaddr *) &sockaddr_fd2,
-		 sizeof(sockaddr_fd2)) < 0) {
+	if (bind(gtp_sock.fd2, (struct sockaddr *) &gtp_sock.sockaddr.fd2, gtp_sock.len) < 0) {
 		perror("bind");
 		exit(EXIT_FAILURE);
 	}
 
 	if (sgsn_mode)
-		ret = gtp_dev_create_sgsn(-1, argv[2], fd1, fd2);
+		ret = gtp_dev_create_sgsn(-1, argv[2], gtp_sock.fd1, gtp_sock.fd2);
 	else
-		ret = gtp_dev_create(-1, argv[2], fd1, fd2);
+		ret = gtp_dev_create(-1, argv[2], gtp_sock.fd1, gtp_sock.fd2);
 	if (ret < 0) {
 		perror("cannot create GTP device\n");
 		exit(EXIT_FAILURE);
@@ -103,11 +161,13 @@ int main(int argc, char *argv[])
 			"this process running for testing purposes.\n");
 
 	while (1) {
-		struct sockaddr_in addr;
-		socklen_t len = sizeof(addr);
+		union {
+			struct sockaddr_in addr;
+			struct sockaddr_in6 addr6;
+		} sock;
 
-		ret = recvfrom(fd1, buf, sizeof(buf), 0,
-			       (struct sockaddr *)&addr, &len);
+		ret = recvfrom(gtp_sock.fd1, buf, sizeof(buf), 0,
+			       (struct sockaddr *)&sock, &gtp_sock.len);
 		printf("received %d bytes via UDP socket\n", ret);
 	}
 
-- 
2.30.2

