From 7096e9622eff52002c823c802eb73f5a47697556 Mon Sep 17 00:00:00 2001
From: Pablo Neira Ayuso <pablo@netfilter.org>
Date: Thu, 30 Mar 2023 22:14:01 +0200
Subject: [PATCH 1/5] add IPv6 support

---
 include/linux/gtp.h | 16 ++++++--
 src/gtp-genl.c      | 96 +++++++++++++++++++++++++++++++++++----------
 src/internal.h      |  5 +++
 3 files changed, 92 insertions(+), 25 deletions(-)

diff --git a/include/linux/gtp.h b/include/linux/gtp.h
index c525cb3675e9..c2ecec2e39fa 100644
--- a/include/linux/gtp.h
+++ b/include/linux/gtp.h
@@ -1,10 +1,14 @@
-#ifndef _UAPI_LINUX_GTP_H_
-#define _UAPI_LINUX_GTP_H__
+/* SPDX-License-Identifier: GPL-2.0 WITH Linux-syscall-note */
+#ifndef __LINUX_GTP_H_
+#define __LINUX_GTP_H_
+
+#define GTP_GENL_MCGRP_NAME	"gtp"
 
 enum gtp_genl_cmds {
 	GTP_CMD_NEWPDP,
 	GTP_CMD_DELPDP,
 	GTP_CMD_GETPDP,
+	GTP_CMD_ECHOREQ,
 
 	GTP_CMD_MAX,
 };
@@ -19,15 +23,19 @@ enum gtp_attrs {
 	GTPA_LINK,
 	GTPA_VERSION,
 	GTPA_TID,	/* for GTPv0 only */
-	GTPA_PEER_ADDRESS,
+	GTPA_PEER_ADDRESS,	/* Remote GSN peer, either SGSN or GGSN */
+#define GTPA_SGSN_ADDRESS GTPA_PEER_ADDRESS /* maintain legacy attr name */
 	GTPA_MS_ADDRESS,
 	GTPA_FLOW,
 	GTPA_NET_NS_FD,
 	GTPA_I_TEI,	/* for GTPv1 only */
 	GTPA_O_TEI,	/* for GTPv1 only */
 	GTPA_PAD,
+	GTPA_PEER_ADDR6,	/* Remote GSN peer, either SGSN or GGSN */
+	GTPA_MS_ADDR6,
+	GTPA_FAMILY,
 	__GTPA_MAX,
 };
 #define GTPA_MAX (__GTPA_MAX + 1)
 
-#endif /* _UAPI_LINUX_GTP_H_ */
+#endif /* __LINUX_GTP_H_ */
diff --git a/src/gtp-genl.c b/src/gtp-genl.c
index 21297b7c3f9a..e4e73e23baaf 100644
--- a/src/gtp-genl.c
+++ b/src/gtp-genl.c
@@ -44,14 +44,23 @@
 
 static void gtp_build_payload(struct nlmsghdr *nlh, struct gtp_tunnel *t)
 {
+	mnl_attr_put_u8(nlh, GTPA_FAMILY, t->family);
 	mnl_attr_put_u32(nlh, GTPA_VERSION, t->gtp_version);
 	if (t->ifns >= 0)
 		mnl_attr_put_u32(nlh, GTPA_NET_NS_FD, t->ifns);
 	mnl_attr_put_u32(nlh, GTPA_LINK, t->ifidx);
-	if (t->ip.sgsn_addr.s_addr)
-		mnl_attr_put_u32(nlh, GTPA_PEER_ADDRESS, t->ip.sgsn_addr.s_addr);
-	if (t->ip.ms_addr.s_addr)
-		mnl_attr_put_u32(nlh, GTPA_MS_ADDRESS, t->ip.ms_addr.s_addr);
+	switch (t->family) {
+	case AF_INET:
+		if (t->ip.sgsn_addr.s_addr)
+			mnl_attr_put_u32(nlh, GTPA_PEER_ADDRESS, t->ip.sgsn_addr.s_addr);
+		if (t->ip.ms_addr.s_addr)
+			mnl_attr_put_u32(nlh, GTPA_MS_ADDRESS, t->ip.ms_addr.s_addr);
+		break;
+	case AF_INET6:
+		mnl_attr_put(nlh, GTPA_PEER_ADDR6, sizeof(t->ip6.sgsn_addr), &t->ip6.sgsn_addr);
+		mnl_attr_put(nlh, GTPA_MS_ADDR6, sizeof(t->ip6.ms_addr), &t->ip6.ms_addr);
+		break;
+	}
 	if (t->gtp_version == GTP_V0) {
 		mnl_attr_put_u64(nlh, GTPA_TID, t->u.v0.tid);
 		mnl_attr_put_u16(nlh, GTPA_FLOW, t->u.v0.flowid);
@@ -106,6 +115,7 @@ int gtp_del_tunnel(int genl_id, struct mnl_socket *nl, struct gtp_tunnel *t)
 EXPORT_SYMBOL(gtp_del_tunnel);
 
 struct gtp_pdp {
+	int 		family;
 	uint32_t	version;
 	union {
 		struct {
@@ -121,6 +131,10 @@ struct gtp_pdp {
 			struct in_addr	sgsn_addr;
 			struct in_addr	ms_addr;
 		} ip;
+		struct {
+			struct in6_addr	sgsn_addr;
+			struct in6_addr	ms_addr;
+		} ip6;
 	};
 };
 
@@ -133,6 +147,12 @@ static int genl_gtp_validate_cb(const struct nlattr *attr, void *data)
 		return MNL_CB_OK;
 
 	switch(type) {
+	case GTPA_FAMILY:
+		if (mnl_attr_validate(attr, MNL_TYPE_U8) < 0) {
+			perror("mnl_attr_validate");
+			return MNL_CB_ERROR;
+		}
+		break;
 	case GTPA_TID:
 		if (mnl_attr_validate(attr, MNL_TYPE_U64) < 0) {
 			perror("mnl_attr_validate");
@@ -149,6 +169,14 @@ static int genl_gtp_validate_cb(const struct nlattr *attr, void *data)
 			return MNL_CB_ERROR;
 		}
 		break;
+	case GTPA_PEER_ADDR6:
+	case GTPA_MS_ADDR6:
+		if (mnl_attr_validate2(attr, MNL_TYPE_BINARY,
+				       sizeof(struct in6_addr)) < 0) {
+			perror("mnl_attr_validate");
+			return MNL_CB_ERROR;
+		}
+		break;
 	default:
 		break;
 	}
@@ -164,35 +192,61 @@ static int genl_gtp_attr_cb(const struct nlmsghdr *nlh, void *data)
 	struct genlmsghdr *genl;
 
 	mnl_attr_parse(nlh, sizeof(*genl), genl_gtp_validate_cb, tb);
+	if (tb[GTPA_FAMILY])
+		pdp.family = mnl_attr_get_u32(tb[GTPA_FAMILY]);
+	else
+		pdp.family = AF_INET;
+
 	if (tb[GTPA_TID])
 		pdp.u.v0.tid = mnl_attr_get_u64(tb[GTPA_TID]);
 	if (tb[GTPA_I_TEI])
 		pdp.u.v1.i_tei = mnl_attr_get_u32(tb[GTPA_I_TEI]);
 	if (tb[GTPA_O_TEI])
 		pdp.u.v1.o_tei = mnl_attr_get_u32(tb[GTPA_O_TEI]);
-	if (tb[GTPA_PEER_ADDRESS]) {
-		pdp.ip.sgsn_addr.s_addr =
-			mnl_attr_get_u32(tb[GTPA_PEER_ADDRESS]);
-	}
-	if (tb[GTPA_MS_ADDRESS]) {
-		pdp.ip.ms_addr.s_addr = mnl_attr_get_u32(tb[GTPA_MS_ADDRESS]);
+
+	switch (pdp.family) {
+	case AF_INET:
+		if (tb[GTPA_PEER_ADDRESS]) {
+			pdp.ip.sgsn_addr.s_addr =
+				mnl_attr_get_u32(tb[GTPA_PEER_ADDRESS]);
+		}
+		if (tb[GTPA_MS_ADDRESS]) {
+			pdp.ip.ms_addr.s_addr = mnl_attr_get_u32(tb[GTPA_MS_ADDRESS]);
+		}
+		break;
+	case AF_INET6:
+		if (tb[GTPA_PEER_ADDR6])
+			memcpy(&pdp.ip6.sgsn_addr, mnl_attr_get_payload(tb[GTPA_PEER_ADDR6]), sizeof(struct in6_addr));
+		if (tb[GTPA_MS_ADDR6])
+			memcpy(&pdp.ip6.ms_addr, mnl_attr_get_payload(tb[GTPA_MS_ADDR6]), sizeof(struct in6_addr));
+		break;
 	}
-	if (tb[GTPA_VERSION]) {
+	if (tb[GTPA_VERSION])
 		pdp.version = mnl_attr_get_u32(tb[GTPA_VERSION]);
-	}
 
 	printf("version %u ", pdp.version);
-	if (pdp.version == GTP_V0) {
-		inet_ntop(AF_INET, &pdp.ip.ms_addr, buf, sizeof(buf));
-		printf("tid %"PRIu64" ms_addr %s ",
-		       pdp.u.v0.tid, buf);
-	} else if (pdp.version == GTP_V1) {
+
+	if (pdp.version == GTP_V0)
+		printf("tid %"PRIu64" ", pdp.u.v0.tid);
+	else if (pdp.version == GTP_V1)
+		printf("tei %u/%u ", pdp.u.v1.i_tei, pdp.u.v1.o_tei);
+
+	printf("family %s ", pdp.family == AF_INET6 ? "ip6" : "ip");
+
+	switch (pdp.family) {
+	case AF_INET:
 		inet_ntop(AF_INET, &pdp.ip.ms_addr, buf, sizeof(buf));
-		printf("tei %u/%u ms_addr %s ", pdp.u.v1.i_tei,
-		       pdp.u.v1.o_tei, buf);
+		printf("ms_addr %s ", buf);
+		inet_ntop(AF_INET, &pdp.ip.sgsn_addr, buf, sizeof(buf));
+		printf("sgsn_addr %s\n", buf);
+		break;
+	case AF_INET6:
+		inet_ntop(AF_INET6, &pdp.ip6.ms_addr, buf, sizeof(buf));
+		printf("ms_addr6 %s ", buf);
+		inet_ntop(AF_INET6, &pdp.ip6.sgsn_addr, buf, sizeof(buf));
+		printf("sgsn_addr6 %s\n", buf);
+		break;
 	}
-	inet_ntop(AF_INET, &pdp.ip.sgsn_addr, buf, sizeof(buf));
-	printf("sgsn_addr %s\n", buf);
 
 	return MNL_CB_OK;
 }
diff --git a/src/internal.h b/src/internal.h
index 559461f1b21e..8e8b3c5af975 100644
--- a/src/internal.h
+++ b/src/internal.h
@@ -13,6 +13,7 @@
 #include <netinet/in.h>
 
 struct gtp_tunnel {
+	uint8_t		family;
 	int             ifns;
 	uint32_t	ifidx;
 	union {
@@ -20,6 +21,10 @@ struct gtp_tunnel {
 			struct in_addr	ms_addr;
 			struct in_addr	sgsn_addr;
 		} ip;
+		struct {
+			struct in6_addr	ms_addr;
+			struct in6_addr	sgsn_addr;
+		} ip6;
 	};
 	int		gtp_version;
 	union {
-- 
2.30.2

