summaryrefslogtreecommitdiff
path: root/drivers/net/gtp.c
diff options
context:
space:
mode:
Diffstat (limited to 'drivers/net/gtp.c')
-rw-r--r--drivers/net/gtp.c567
1 files changed, 523 insertions, 44 deletions
diff --git a/drivers/net/gtp.c b/drivers/net/gtp.c
index 24e5c54d06c1..a208e2b1a9af 100644
--- a/drivers/net/gtp.c
+++ b/drivers/net/gtp.c
@@ -66,13 +66,23 @@ struct gtp_dev {
struct sock *sk0;
struct sock *sk1u;
+ u8 sk_created;
struct net_device *dev;
+ struct net *net;
unsigned int role;
unsigned int hash_size;
struct hlist_head *tid_hash;
struct hlist_head *addr_hash;
+
+ u8 restart_count;
+};
+
+struct echo_info {
+ struct in_addr ms_addr_ip4;
+ struct in_addr peer_addr_ip4;
+ u8 gtp_version;
};
static unsigned int gtp_net_id __read_mostly;
@@ -83,6 +93,16 @@ struct gtp_net {
static u32 gtp_h_initval;
+static struct genl_family gtp_genl_family;
+
+enum gtp_multicast_groups {
+ GTP_GENL_MCGRP,
+};
+
+static const struct genl_multicast_group gtp_genl_mcgrps[] = {
+ [GTP_GENL_MCGRP] = { .name = GTP_GENL_MCGRP_NAME },
+};
+
static void pdp_context_delete(struct pdp_ctx *pctx);
static inline u32 gtp0_hashfn(u64 tid)
@@ -207,7 +227,7 @@ static int gtp_rx(struct pdp_ctx *pctx, struct sk_buff *skb,
dev_sw_netstats_rx_add(pctx->dev, skb->len);
- netif_rx(skb);
+ __netif_rx(skb);
return 0;
err:
@@ -215,6 +235,174 @@ err:
return -1;
}
+static struct rtable *ip4_route_output_gtp(struct flowi4 *fl4,
+ const struct sock *sk,
+ __be32 daddr, __be32 saddr)
+{
+ memset(fl4, 0, sizeof(*fl4));
+ fl4->flowi4_oif = sk->sk_bound_dev_if;
+ fl4->daddr = daddr;
+ fl4->saddr = saddr;
+ fl4->flowi4_tos = RT_CONN_FLAGS(sk);
+ fl4->flowi4_proto = sk->sk_protocol;
+
+ return ip_route_output_key(sock_net(sk), fl4);
+}
+
+/* GSM TS 09.60. 7.3
+ * In all Path Management messages:
+ * - TID: is not used and shall be set to 0.
+ * - Flow Label is not used and shall be set to 0
+ * In signalling messages:
+ * - number: this field is not yet used in signalling messages.
+ * It shall be set to 255 by the sender and shall be ignored
+ * by the receiver
+ * Returns true if the echo req was correct, false otherwise.
+ */
+static bool gtp0_validate_echo_hdr(struct gtp0_header *gtp0)
+{
+ return !(gtp0->tid || (gtp0->flags ^ 0x1e) ||
+ gtp0->number != 0xff || gtp0->flow);
+}
+
+/* msg_type has to be GTP_ECHO_REQ or GTP_ECHO_RSP */
+static void gtp0_build_echo_msg(struct gtp0_header *hdr, __u8 msg_type)
+{
+ int len_pkt, len_hdr;
+
+ hdr->flags = 0x1e; /* v0, GTP-non-prime. */
+ hdr->type = msg_type;
+ /* GSM TS 09.60. 7.3 In all Path Management Flow Label and TID
+ * are not used and shall be set to 0.
+ */
+ hdr->flow = 0;
+ hdr->tid = 0;
+ hdr->number = 0xff;
+ hdr->spare[0] = 0xff;
+ hdr->spare[1] = 0xff;
+ hdr->spare[2] = 0xff;
+
+ len_pkt = sizeof(struct gtp0_packet);
+ len_hdr = sizeof(struct gtp0_header);
+
+ if (msg_type == GTP_ECHO_RSP)
+ hdr->length = htons(len_pkt - len_hdr);
+ else
+ hdr->length = 0;
+}
+
+static int gtp0_send_echo_resp(struct gtp_dev *gtp, struct sk_buff *skb)
+{
+ struct gtp0_packet *gtp_pkt;
+ struct gtp0_header *gtp0;
+ struct rtable *rt;
+ struct flowi4 fl4;
+ struct iphdr *iph;
+ __be16 seq;
+
+ gtp0 = (struct gtp0_header *)(skb->data + sizeof(struct udphdr));
+
+ if (!gtp0_validate_echo_hdr(gtp0))
+ return -1;
+
+ seq = gtp0->seq;
+
+ /* pull GTP and UDP headers */
+ skb_pull_data(skb, sizeof(struct gtp0_header) + sizeof(struct udphdr));
+
+ gtp_pkt = skb_push(skb, sizeof(struct gtp0_packet));
+ memset(gtp_pkt, 0, sizeof(struct gtp0_packet));
+
+ gtp0_build_echo_msg(&gtp_pkt->gtp0_h, GTP_ECHO_RSP);
+
+ /* GSM TS 09.60. 7.3 The Sequence Number in a signalling response
+ * message shall be copied from the signalling request message
+ * that the GSN is replying to.
+ */
+ gtp_pkt->gtp0_h.seq = seq;
+
+ gtp_pkt->ie.tag = GTPIE_RECOVERY;
+ gtp_pkt->ie.val = gtp->restart_count;
+
+ iph = ip_hdr(skb);
+
+ /* find route to the sender,
+ * src address becomes dst address and vice versa.
+ */
+ rt = ip4_route_output_gtp(&fl4, gtp->sk0, iph->saddr, iph->daddr);
+ if (IS_ERR(rt)) {
+ netdev_dbg(gtp->dev, "no route for echo response from %pI4\n",
+ &iph->saddr);
+ return -1;
+ }
+
+ udp_tunnel_xmit_skb(rt, gtp->sk0, skb,
+ fl4.saddr, fl4.daddr,
+ iph->tos,
+ ip4_dst_hoplimit(&rt->dst),
+ 0,
+ htons(GTP0_PORT), htons(GTP0_PORT),
+ !net_eq(sock_net(gtp->sk1u),
+ dev_net(gtp->dev)),
+ false);
+ return 0;
+}
+
+static int gtp_genl_fill_echo(struct sk_buff *skb, u32 snd_portid, u32 snd_seq,
+ int flags, u32 type, struct echo_info echo)
+{
+ void *genlh;
+
+ genlh = genlmsg_put(skb, snd_portid, snd_seq, &gtp_genl_family, flags,
+ type);
+ if (!genlh)
+ goto failure;
+
+ if (nla_put_u32(skb, GTPA_VERSION, echo.gtp_version) ||
+ nla_put_be32(skb, GTPA_PEER_ADDRESS, echo.peer_addr_ip4.s_addr) ||
+ nla_put_be32(skb, GTPA_MS_ADDRESS, echo.ms_addr_ip4.s_addr))
+ goto failure;
+
+ genlmsg_end(skb, genlh);
+ return 0;
+
+failure:
+ genlmsg_cancel(skb, genlh);
+ return -EMSGSIZE;
+}
+
+static int gtp0_handle_echo_resp(struct gtp_dev *gtp, struct sk_buff *skb)
+{
+ struct gtp0_header *gtp0;
+ struct echo_info echo;
+ struct sk_buff *msg;
+ struct iphdr *iph;
+ int ret;
+
+ gtp0 = (struct gtp0_header *)(skb->data + sizeof(struct udphdr));
+
+ if (!gtp0_validate_echo_hdr(gtp0))
+ return -1;
+
+ iph = ip_hdr(skb);
+ echo.ms_addr_ip4.s_addr = iph->daddr;
+ echo.peer_addr_ip4.s_addr = iph->saddr;
+ echo.gtp_version = GTP_V0;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
+ if (!msg)
+ return -ENOMEM;
+
+ ret = gtp_genl_fill_echo(msg, 0, 0, 0, GTP_CMD_ECHOREQ, echo);
+ if (ret < 0) {
+ nlmsg_free(msg);
+ return ret;
+ }
+
+ return genlmsg_multicast_netns(&gtp_genl_family, dev_net(gtp->dev),
+ msg, 0, GTP_GENL_MCGRP, GFP_ATOMIC);
+}
+
/* 1 means pass up to the stack, -1 means drop and 0 means decapsulated. */
static int gtp0_udp_encap_recv(struct gtp_dev *gtp, struct sk_buff *skb)
{
@@ -231,6 +419,16 @@ static int gtp0_udp_encap_recv(struct gtp_dev *gtp, struct sk_buff *skb)
if ((gtp0->flags >> 5) != GTP_V0)
return 1;
+ /* If the sockets were created in kernel, it means that
+ * there is no daemon running in userspace which would
+ * handle echo request.
+ */
+ if (gtp0->type == GTP_ECHO_REQ && gtp->sk_created)
+ return gtp0_send_echo_resp(gtp, skb);
+
+ if (gtp0->type == GTP_ECHO_RSP && gtp->sk_created)
+ return gtp0_handle_echo_resp(gtp, skb);
+
if (gtp0->type != GTP_TPDU)
return 1;
@@ -243,6 +441,131 @@ static int gtp0_udp_encap_recv(struct gtp_dev *gtp, struct sk_buff *skb)
return gtp_rx(pctx, skb, hdrlen, gtp->role);
}
+/* msg_type has to be GTP_ECHO_REQ or GTP_ECHO_RSP */
+static void gtp1u_build_echo_msg(struct gtp1_header_long *hdr, __u8 msg_type)
+{
+ int len_pkt, len_hdr;
+
+ /* S flag must be set to 1 */
+ hdr->flags = 0x32; /* v1, GTP-non-prime. */
+ hdr->type = msg_type;
+ /* 3GPP TS 29.281 5.1 - TEID has to be set to 0 */
+ hdr->tid = 0;
+
+ /* seq, npdu and next should be counted to the length of the GTP packet
+ * that's why szie of gtp1_header should be subtracted,
+ * not size of gtp1_header_long.
+ */
+
+ len_hdr = sizeof(struct gtp1_header);
+
+ if (msg_type == GTP_ECHO_RSP) {
+ len_pkt = sizeof(struct gtp1u_packet);
+ hdr->length = htons(len_pkt - len_hdr);
+ } else {
+ /* GTP_ECHO_REQ does not carry GTP Information Element,
+ * the why gtp1_header_long is used here.
+ */
+ len_pkt = sizeof(struct gtp1_header_long);
+ hdr->length = htons(len_pkt - len_hdr);
+ }
+}
+
+static int gtp1u_send_echo_resp(struct gtp_dev *gtp, struct sk_buff *skb)
+{
+ struct gtp1_header_long *gtp1u;
+ struct gtp1u_packet *gtp_pkt;
+ struct rtable *rt;
+ struct flowi4 fl4;
+ struct iphdr *iph;
+
+ gtp1u = (struct gtp1_header_long *)(skb->data + sizeof(struct udphdr));
+
+ /* 3GPP TS 29.281 5.1 - For the Echo Request, Echo Response,
+ * Error Indication and Supported Extension Headers Notification
+ * messages, the S flag shall be set to 1 and TEID shall be set to 0.
+ */
+ if (!(gtp1u->flags & GTP1_F_SEQ) || gtp1u->tid)
+ return -1;
+
+ /* pull GTP and UDP headers */
+ skb_pull_data(skb,
+ sizeof(struct gtp1_header_long) + sizeof(struct udphdr));
+
+ gtp_pkt = skb_push(skb, sizeof(struct gtp1u_packet));
+ memset(gtp_pkt, 0, sizeof(struct gtp1u_packet));
+
+ gtp1u_build_echo_msg(&gtp_pkt->gtp1u_h, GTP_ECHO_RSP);
+
+ /* 3GPP TS 29.281 7.7.2 - The Restart Counter value in the
+ * Recovery information element shall not be used, i.e. it shall
+ * be set to zero by the sender and shall be ignored by the receiver.
+ * The Recovery information element is mandatory due to backwards
+ * compatibility reasons.
+ */
+ gtp_pkt->ie.tag = GTPIE_RECOVERY;
+ gtp_pkt->ie.val = 0;
+
+ iph = ip_hdr(skb);
+
+ /* find route to the sender,
+ * src address becomes dst address and vice versa.
+ */
+ rt = ip4_route_output_gtp(&fl4, gtp->sk1u, iph->saddr, iph->daddr);
+ if (IS_ERR(rt)) {
+ netdev_dbg(gtp->dev, "no route for echo response from %pI4\n",
+ &iph->saddr);
+ return -1;
+ }
+
+ udp_tunnel_xmit_skb(rt, gtp->sk1u, skb,
+ fl4.saddr, fl4.daddr,
+ iph->tos,
+ ip4_dst_hoplimit(&rt->dst),
+ 0,
+ htons(GTP1U_PORT), htons(GTP1U_PORT),
+ !net_eq(sock_net(gtp->sk1u),
+ dev_net(gtp->dev)),
+ false);
+ return 0;
+}
+
+static int gtp1u_handle_echo_resp(struct gtp_dev *gtp, struct sk_buff *skb)
+{
+ struct gtp1_header_long *gtp1u;
+ struct echo_info echo;
+ struct sk_buff *msg;
+ struct iphdr *iph;
+ int ret;
+
+ gtp1u = (struct gtp1_header_long *)(skb->data + sizeof(struct udphdr));
+
+ /* 3GPP TS 29.281 5.1 - For the Echo Request, Echo Response,
+ * Error Indication and Supported Extension Headers Notification
+ * messages, the S flag shall be set to 1 and TEID shall be set to 0.
+ */
+ if (!(gtp1u->flags & GTP1_F_SEQ) || gtp1u->tid)
+ return -1;
+
+ iph = ip_hdr(skb);
+ echo.ms_addr_ip4.s_addr = iph->daddr;
+ echo.peer_addr_ip4.s_addr = iph->saddr;
+ echo.gtp_version = GTP_V1;
+
+ msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);
+ if (!msg)
+ return -ENOMEM;
+
+ ret = gtp_genl_fill_echo(msg, 0, 0, 0, GTP_CMD_ECHOREQ, echo);
+ if (ret < 0) {
+ nlmsg_free(msg);
+ return ret;
+ }
+
+ return genlmsg_multicast_netns(&gtp_genl_family, dev_net(gtp->dev),
+ msg, 0, GTP_GENL_MCGRP, GFP_ATOMIC);
+}
+
static int gtp1u_udp_encap_recv(struct gtp_dev *gtp, struct sk_buff *skb)
{
unsigned int hdrlen = sizeof(struct udphdr) +
@@ -258,6 +581,16 @@ static int gtp1u_udp_encap_recv(struct gtp_dev *gtp, struct sk_buff *skb)
if ((gtp1->flags >> 5) != GTP_V1)
return 1;
+ /* If the sockets were created in kernel, it means that
+ * there is no daemon running in userspace which would
+ * handle echo request.
+ */
+ if (gtp1->type == GTP_ECHO_REQ && gtp->sk_created)
+ return gtp1u_send_echo_resp(gtp, skb);
+
+ if (gtp1->type == GTP_ECHO_RSP && gtp->sk_created)
+ return gtp1u_handle_echo_resp(gtp, skb);
+
if (gtp1->type != GTP_TPDU)
return 1;
@@ -320,8 +653,16 @@ static void gtp_encap_disable_sock(struct sock *sk)
static void gtp_encap_disable(struct gtp_dev *gtp)
{
- gtp_encap_disable_sock(gtp->sk0);
- gtp_encap_disable_sock(gtp->sk1u);
+ if (gtp->sk_created) {
+ udp_tunnel_sock_release(gtp->sk0->sk_socket);
+ udp_tunnel_sock_release(gtp->sk1u->sk_socket);
+ gtp->sk_created = false;
+ gtp->sk0 = NULL;
+ gtp->sk1u = NULL;
+ } else {
+ gtp_encap_disable_sock(gtp->sk0);
+ gtp_encap_disable_sock(gtp->sk1u);
+ }
}
/* UDP encapsulation receive handler. See net/ipv4/udp.c.
@@ -388,20 +729,6 @@ static void gtp_dev_uninit(struct net_device *dev)
free_percpu(dev->tstats);
}
-static struct rtable *ip4_route_output_gtp(struct flowi4 *fl4,
- const struct sock *sk,
- __be32 daddr)
-{
- memset(fl4, 0, sizeof(*fl4));
- fl4->flowi4_oif = sk->sk_bound_dev_if;
- fl4->daddr = daddr;
- fl4->saddr = inet_sk(sk)->inet_saddr;
- fl4->flowi4_tos = RT_CONN_FLAGS(sk);
- fl4->flowi4_proto = sk->sk_protocol;
-
- return ip_route_output_key(sock_net(sk), fl4);
-}
-
static inline void gtp0_push_header(struct sk_buff *skb, struct pdp_ctx *pctx)
{
int payload_len = skb->len;
@@ -507,7 +834,8 @@ static int gtp_build_skb_ip4(struct sk_buff *skb, struct net_device *dev,
}
netdev_dbg(dev, "found PDP context %p\n", pctx);
- rt = ip4_route_output_gtp(&fl4, pctx->sk, pctx->peer_addr_ip4.s_addr);
+ rt = ip4_route_output_gtp(&fl4, pctx->sk, pctx->peer_addr_ip4.s_addr,
+ inet_sk(pctx->sk)->inet_saddr);
if (IS_ERR(rt)) {
netdev_dbg(dev, "no route to SSGN %pI4\n",
&pctx->peer_addr_ip4.s_addr);
@@ -656,17 +984,69 @@ static void gtp_destructor(struct net_device *dev)
kfree(gtp->tid_hash);
}
+static struct sock *gtp_create_sock(int type, struct gtp_dev *gtp)
+{
+ struct udp_tunnel_sock_cfg tuncfg = {};
+ struct udp_port_cfg udp_conf = {
+ .local_ip.s_addr = htonl(INADDR_ANY),
+ .family = AF_INET,
+ };
+ struct net *net = gtp->net;
+ struct socket *sock;
+ int err;
+
+ if (type == UDP_ENCAP_GTP0)
+ udp_conf.local_udp_port = htons(GTP0_PORT);
+ else if (type == UDP_ENCAP_GTP1U)
+ udp_conf.local_udp_port = htons(GTP1U_PORT);
+ else
+ return ERR_PTR(-EINVAL);
+
+ err = udp_sock_create(net, &udp_conf, &sock);
+ if (err)
+ return ERR_PTR(err);
+
+ tuncfg.sk_user_data = gtp;
+ tuncfg.encap_type = type;
+ tuncfg.encap_rcv = gtp_encap_recv;
+ tuncfg.encap_destroy = NULL;
+
+ setup_udp_tunnel_sock(net, sock, &tuncfg);
+
+ return sock->sk;
+}
+
+static int gtp_create_sockets(struct gtp_dev *gtp, struct nlattr *data[])
+{
+ struct sock *sk1u = NULL;
+ struct sock *sk0 = NULL;
+
+ sk0 = gtp_create_sock(UDP_ENCAP_GTP0, gtp);
+ if (IS_ERR(sk0))
+ return PTR_ERR(sk0);
+
+ sk1u = gtp_create_sock(UDP_ENCAP_GTP1U, gtp);
+ if (IS_ERR(sk1u)) {
+ udp_tunnel_sock_release(sk0->sk_socket);
+ return PTR_ERR(sk1u);
+ }
+
+ gtp->sk_created = true;
+ gtp->sk0 = sk0;
+ gtp->sk1u = sk1u;
+
+ return 0;
+}
+
static int gtp_newlink(struct net *src_net, struct net_device *dev,
struct nlattr *tb[], struct nlattr *data[],
struct netlink_ext_ack *extack)
{
+ unsigned int role = GTP_ROLE_GGSN;
struct gtp_dev *gtp;
struct gtp_net *gn;
int hashsize, err;
- if (!data[IFLA_GTP_FD0] && !data[IFLA_GTP_FD1])
- return -EINVAL;
-
gtp = netdev_priv(dev);
if (!data[IFLA_GTP_PDP_HASHSIZE]) {
@@ -677,11 +1057,28 @@ static int gtp_newlink(struct net *src_net, struct net_device *dev,
hashsize = 1024;
}
+ if (data[IFLA_GTP_ROLE]) {
+ role = nla_get_u32(data[IFLA_GTP_ROLE]);
+ if (role > GTP_ROLE_SGSN)
+ return -EINVAL;
+ }
+ gtp->role = role;
+
+ if (!data[IFLA_GTP_RESTART_COUNT])
+ gtp->restart_count = 0;
+ else
+ gtp->restart_count = nla_get_u8(data[IFLA_GTP_RESTART_COUNT]);
+
+ gtp->net = src_net;
+
err = gtp_hashtable_new(gtp, hashsize);
if (err < 0)
return err;
- err = gtp_encap_enable(gtp, data);
+ if (data[IFLA_GTP_CREATE_SOCKETS])
+ err = gtp_create_sockets(gtp, data);
+ else
+ err = gtp_encap_enable(gtp, data);
if (err < 0)
goto out_hashtable;
@@ -726,6 +1123,8 @@ static const struct nla_policy gtp_policy[IFLA_GTP_MAX + 1] = {
[IFLA_GTP_FD1] = { .type = NLA_U32 },
[IFLA_GTP_PDP_HASHSIZE] = { .type = NLA_U32 },
[IFLA_GTP_ROLE] = { .type = NLA_U32 },
+ [IFLA_GTP_CREATE_SOCKETS] = { .type = NLA_U8 },
+ [IFLA_GTP_RESTART_COUNT] = { .type = NLA_U8 },
};
static int gtp_validate(struct nlattr *tb[], struct nlattr *data[],
@@ -740,7 +1139,8 @@ static int gtp_validate(struct nlattr *tb[], struct nlattr *data[],
static size_t gtp_get_size(const struct net_device *dev)
{
return nla_total_size(sizeof(__u32)) + /* IFLA_GTP_PDP_HASHSIZE */
- nla_total_size(sizeof(__u32)); /* IFLA_GTP_ROLE */
+ nla_total_size(sizeof(__u32)) + /* IFLA_GTP_ROLE */
+ nla_total_size(sizeof(__u8)); /* IFLA_GTP_RESTART_COUNT */
}
static int gtp_fill_info(struct sk_buff *skb, const struct net_device *dev)
@@ -751,6 +1151,8 @@ static int gtp_fill_info(struct sk_buff *skb, const struct net_device *dev)
goto nla_put_failure;
if (nla_put_u32(skb, IFLA_GTP_ROLE, gtp->role))
goto nla_put_failure;
+ if (nla_put_u8(skb, IFLA_GTP_RESTART_COUNT, gtp->restart_count))
+ goto nla_put_failure;
return 0;
@@ -848,7 +1250,9 @@ static int gtp_encap_enable(struct gtp_dev *gtp, struct nlattr *data[])
{
struct sock *sk1u = NULL;
struct sock *sk0 = NULL;
- unsigned int role = GTP_ROLE_GGSN;
+
+ if (!data[IFLA_GTP_FD0] && !data[IFLA_GTP_FD1])
+ return -EINVAL;
if (data[IFLA_GTP_FD0]) {
u32 fd0 = nla_get_u32(data[IFLA_GTP_FD0]);
@@ -868,18 +1272,8 @@ static int gtp_encap_enable(struct gtp_dev *gtp, struct nlattr *data[])
}
}
- if (data[IFLA_GTP_ROLE]) {
- role = nla_get_u32(data[IFLA_GTP_ROLE]);
- if (role > GTP_ROLE_SGSN) {
- gtp_encap_disable_sock(sk0);
- gtp_encap_disable_sock(sk1u);
- return -EINVAL;
- }
- }
-
gtp->sk0 = sk0;
gtp->sk1u = sk1u;
- gtp->role = role;
return 0;
}
@@ -1183,16 +1577,6 @@ out_unlock:
return err;
}
-static struct genl_family gtp_genl_family;
-
-enum gtp_multicast_groups {
- GTP_GENL_MCGRP,
-};
-
-static const struct genl_multicast_group gtp_genl_mcgrps[] = {
- [GTP_GENL_MCGRP] = { .name = GTP_GENL_MCGRP_NAME },
-};
-
static int gtp_genl_fill_info(struct sk_buff *skb, u32 snd_portid, u32 snd_seq,
int flags, u32 type, struct pdp_ctx *pctx)
{
@@ -1336,6 +1720,95 @@ out:
return skb->len;
}
+static int gtp_genl_send_echo_req(struct sk_buff *skb, struct genl_info *info)
+{
+ struct sk_buff *skb_to_send;
+ __be32 src_ip, dst_ip;
+ unsigned int version;
+ struct gtp_dev *gtp;
+ struct flowi4 fl4;
+ struct rtable *rt;
+ struct sock *sk;
+ __be16 port;
+ int len;
+
+ if (!info->attrs[GTPA_VERSION] ||
+ !info->attrs[GTPA_LINK] ||
+ !info->attrs[GTPA_PEER_ADDRESS] ||
+ !info->attrs[GTPA_MS_ADDRESS])
+ return -EINVAL;
+
+ version = nla_get_u32(info->attrs[GTPA_VERSION]);
+ dst_ip = nla_get_be32(info->attrs[GTPA_PEER_ADDRESS]);
+ src_ip = nla_get_be32(info->attrs[GTPA_MS_ADDRESS]);
+
+ gtp = gtp_find_dev(sock_net(skb->sk), info->attrs);
+ if (!gtp)
+ return -ENODEV;
+
+ if (!gtp->sk_created)
+ return -EOPNOTSUPP;
+ if (!(gtp->dev->flags & IFF_UP))
+ return -ENETDOWN;
+
+ if (version == GTP_V0) {
+ struct gtp0_header *gtp0_h;
+
+ len = LL_RESERVED_SPACE(gtp->dev) + sizeof(struct gtp0_header) +
+ sizeof(struct iphdr) + sizeof(struct udphdr);
+
+ skb_to_send = netdev_alloc_skb_ip_align(gtp->dev, len);
+ if (!skb_to_send)
+ return -ENOMEM;
+
+ sk = gtp->sk0;
+ port = htons(GTP0_PORT);
+
+ gtp0_h = skb_push(skb_to_send, sizeof(struct gtp0_header));
+ memset(gtp0_h, 0, sizeof(struct gtp0_header));
+ gtp0_build_echo_msg(gtp0_h, GTP_ECHO_REQ);
+ } else if (version == GTP_V1) {
+ struct gtp1_header_long *gtp1u_h;
+
+ len = LL_RESERVED_SPACE(gtp->dev) +
+ sizeof(struct gtp1_header_long) +
+ sizeof(struct iphdr) + sizeof(struct udphdr);
+
+ skb_to_send = netdev_alloc_skb_ip_align(gtp->dev, len);
+ if (!skb_to_send)
+ return -ENOMEM;
+
+ sk = gtp->sk1u;
+ port = htons(GTP1U_PORT);
+
+ gtp1u_h = skb_push(skb_to_send,
+ sizeof(struct gtp1_header_long));
+ memset(gtp1u_h, 0, sizeof(struct gtp1_header_long));
+ gtp1u_build_echo_msg(gtp1u_h, GTP_ECHO_REQ);
+ } else {
+ return -ENODEV;
+ }
+
+ rt = ip4_route_output_gtp(&fl4, sk, dst_ip, src_ip);
+ if (IS_ERR(rt)) {
+ netdev_dbg(gtp->dev, "no route for echo request to %pI4\n",
+ &dst_ip);
+ kfree_skb(skb_to_send);
+ return -ENODEV;
+ }
+
+ udp_tunnel_xmit_skb(rt, sk, skb_to_send,
+ fl4.saddr, fl4.daddr,
+ fl4.flowi4_tos,
+ ip4_dst_hoplimit(&rt->dst),
+ 0,
+ port, port,
+ !net_eq(sock_net(sk),
+ dev_net(gtp->dev)),
+ false);
+ return 0;
+}
+
static const struct nla_policy gtp_genl_policy[GTPA_MAX + 1] = {
[GTPA_LINK] = { .type = NLA_U32, },
[GTPA_VERSION] = { .type = NLA_U32, },
@@ -1368,6 +1841,12 @@ static const struct genl_small_ops gtp_genl_ops[] = {
.dumpit = gtp_genl_dump_pdp,
.flags = GENL_ADMIN_PERM,
},
+ {
+ .cmd = GTP_CMD_ECHOREQ,
+ .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,
+ .doit = gtp_genl_send_echo_req,
+ .flags = GENL_ADMIN_PERM,
+ },
};
static struct genl_family gtp_genl_family __ro_after_init = {