2547 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/* GTP according to GSM TS 09.60 / 3GPP TS 29.0603 *4 * (C) 2012-2014 by sysmocom - s.f.m.c. GmbH5 * (C) 2016 by Pablo Neira Ayuso <pablo@netfilter.org>6 *7 * Author: Harald Welte <hwelte@sysmocom.de>8 * Pablo Neira Ayuso <pablo@netfilter.org>9 * Andreas Schultz <aschultz@travelping.com>10 */11 12#define pr_fmt(fmt) KBUILD_MODNAME ": " fmt13 14#include <linux/module.h>15#include <linux/skbuff.h>16#include <linux/udp.h>17#include <linux/rculist.h>18#include <linux/jhash.h>19#include <linux/if_tunnel.h>20#include <linux/net.h>21#include <linux/file.h>22#include <linux/gtp.h>23 24#include <net/net_namespace.h>25#include <net/protocol.h>26#include <net/ip.h>27#include <net/ipv6.h>28#include <net/udp.h>29#include <net/udp_tunnel.h>30#include <net/icmp.h>31#include <net/xfrm.h>32#include <net/genetlink.h>33#include <net/netns/generic.h>34#include <net/gtp.h>35 36/* An active session for the subscriber. */37struct pdp_ctx {38 struct hlist_node hlist_tid;39 struct hlist_node hlist_addr;40 41 union {42 struct {43 u64 tid;44 u16 flow;45 } v0;46 struct {47 u32 i_tei;48 u32 o_tei;49 } v1;50 } u;51 u8 gtp_version;52 u16 af;53 54 union {55 struct in_addr addr;56 struct in6_addr addr6;57 } ms;58 union {59 struct in_addr addr;60 struct in6_addr addr6;61 } peer;62 63 struct sock *sk;64 struct net_device *dev;65 66 atomic_t tx_seq;67 struct rcu_head rcu_head;68};69 70/* One instance of the GTP device. */71struct gtp_dev {72 struct list_head list;73 74 struct sock *sk0;75 struct sock *sk1u;76 u8 sk_created;77 78 struct net_device *dev;79 struct net *net;80 81 unsigned int role;82 unsigned int hash_size;83 struct hlist_head *tid_hash;84 struct hlist_head *addr_hash;85 86 u8 restart_count;87};88 89struct echo_info {90 u16 af;91 u8 gtp_version;92 93 union {94 struct in_addr addr;95 } ms;96 union {97 struct in_addr addr;98 } peer;99};100 101static unsigned int gtp_net_id __read_mostly;102 103struct gtp_net {104 struct list_head gtp_dev_list;105};106 107static u32 gtp_h_initval;108 109static struct genl_family gtp_genl_family;110 111enum gtp_multicast_groups {112 GTP_GENL_MCGRP,113};114 115static const struct genl_multicast_group gtp_genl_mcgrps[] = {116 [GTP_GENL_MCGRP] = { .name = GTP_GENL_MCGRP_NAME },117};118 119static void pdp_context_delete(struct pdp_ctx *pctx);120 121static inline u32 gtp0_hashfn(u64 tid)122{123 u32 *tid32 = (u32 *) &tid;124 return jhash_2words(tid32[0], tid32[1], gtp_h_initval);125}126 127static inline u32 gtp1u_hashfn(u32 tid)128{129 return jhash_1word(tid, gtp_h_initval);130}131 132static inline u32 ipv4_hashfn(__be32 ip)133{134 return jhash_1word((__force u32)ip, gtp_h_initval);135}136 137static u32 ipv6_hashfn(const struct in6_addr *ip6)138{139 return jhash_2words((__force u32)ip6->s6_addr32[0],140 (__force u32)ip6->s6_addr32[1], gtp_h_initval);141}142 143/* Resolve a PDP context structure based on the 64bit TID. */144static struct pdp_ctx *gtp0_pdp_find(struct gtp_dev *gtp, u64 tid, u16 family)145{146 struct hlist_head *head;147 struct pdp_ctx *pdp;148 149 head = >p->tid_hash[gtp0_hashfn(tid) % gtp->hash_size];150 151 hlist_for_each_entry_rcu(pdp, head, hlist_tid) {152 if (pdp->af == family &&153 pdp->gtp_version == GTP_V0 &&154 pdp->u.v0.tid == tid)155 return pdp;156 }157 return NULL;158}159 160/* Resolve a PDP context structure based on the 32bit TEI. */161static struct pdp_ctx *gtp1_pdp_find(struct gtp_dev *gtp, u32 tid, u16 family)162{163 struct hlist_head *head;164 struct pdp_ctx *pdp;165 166 head = >p->tid_hash[gtp1u_hashfn(tid) % gtp->hash_size];167 168 hlist_for_each_entry_rcu(pdp, head, hlist_tid) {169 if (pdp->af == family &&170 pdp->gtp_version == GTP_V1 &&171 pdp->u.v1.i_tei == tid)172 return pdp;173 }174 return NULL;175}176 177/* Resolve a PDP context based on IPv4 address of MS. */178static struct pdp_ctx *ipv4_pdp_find(struct gtp_dev *gtp, __be32 ms_addr)179{180 struct hlist_head *head;181 struct pdp_ctx *pdp;182 183 head = >p->addr_hash[ipv4_hashfn(ms_addr) % gtp->hash_size];184 185 hlist_for_each_entry_rcu(pdp, head, hlist_addr) {186 if (pdp->af == AF_INET &&187 pdp->ms.addr.s_addr == ms_addr)188 return pdp;189 }190 191 return NULL;192}193 194/* 3GPP TS 29.060: PDN Connection: the association between a MS represented by195 * [...] one IPv6 *prefix* and a PDN represented by an APN.196 *197 * Then, 3GPP TS 29.061, Section 11.2.1.3 says: The size of the prefix shall be198 * according to the maximum prefix length for a global IPv6 address as199 * specified in the IPv6 Addressing Architecture, see RFC 4291.200 *201 * Finally, RFC 4291 section 2.5.4 states: All Global Unicast addresses other202 * than those that start with binary 000 have a 64-bit interface ID field203 * (i.e., n + m = 64).204 */205static bool ipv6_pdp_addr_equal(const struct in6_addr *a,206 const struct in6_addr *b)207{208 return a->s6_addr32[0] == b->s6_addr32[0] &&209 a->s6_addr32[1] == b->s6_addr32[1];210}211 212static struct pdp_ctx *ipv6_pdp_find(struct gtp_dev *gtp,213 const struct in6_addr *ms_addr)214{215 struct hlist_head *head;216 struct pdp_ctx *pdp;217 218 head = >p->addr_hash[ipv6_hashfn(ms_addr) % gtp->hash_size];219 220 hlist_for_each_entry_rcu(pdp, head, hlist_addr) {221 if (pdp->af == AF_INET6 &&222 ipv6_pdp_addr_equal(&pdp->ms.addr6, ms_addr))223 return pdp;224 }225 226 return NULL;227}228 229static bool gtp_check_ms_ipv4(struct sk_buff *skb, struct pdp_ctx *pctx,230 unsigned int hdrlen, unsigned int role)231{232 struct iphdr *iph;233 234 if (!pskb_may_pull(skb, hdrlen + sizeof(struct iphdr)))235 return false;236 237 iph = (struct iphdr *)(skb->data + hdrlen);238 239 if (role == GTP_ROLE_SGSN)240 return iph->daddr == pctx->ms.addr.s_addr;241 else242 return iph->saddr == pctx->ms.addr.s_addr;243}244 245static bool gtp_check_ms_ipv6(struct sk_buff *skb, struct pdp_ctx *pctx,246 unsigned int hdrlen, unsigned int role)247{248 struct ipv6hdr *ip6h;249 int ret;250 251 if (!pskb_may_pull(skb, hdrlen + sizeof(struct ipv6hdr)))252 return false;253 254 ip6h = (struct ipv6hdr *)(skb->data + hdrlen);255 256 if ((ipv6_addr_type(&ip6h->saddr) & IPV6_ADDR_LINKLOCAL) ||257 (ipv6_addr_type(&ip6h->daddr) & IPV6_ADDR_LINKLOCAL))258 return false;259 260 if (role == GTP_ROLE_SGSN) {261 ret = ipv6_pdp_addr_equal(&ip6h->daddr, &pctx->ms.addr6);262 } else {263 ret = ipv6_pdp_addr_equal(&ip6h->saddr, &pctx->ms.addr6);264 }265 266 return ret;267}268 269/* Check if the inner IP address in this packet is assigned to any270 * existing mobile subscriber.271 */272static bool gtp_check_ms(struct sk_buff *skb, struct pdp_ctx *pctx,273 unsigned int hdrlen, unsigned int role,274 __u16 inner_proto)275{276 switch (inner_proto) {277 case ETH_P_IP:278 return gtp_check_ms_ipv4(skb, pctx, hdrlen, role);279 case ETH_P_IPV6:280 return gtp_check_ms_ipv6(skb, pctx, hdrlen, role);281 }282 return false;283}284 285static int gtp_inner_proto(struct sk_buff *skb, unsigned int hdrlen,286 __u16 *inner_proto)287{288 __u8 *ip_version, _ip_version;289 290 ip_version = skb_header_pointer(skb, hdrlen, sizeof(*ip_version),291 &_ip_version);292 if (!ip_version)293 return -1;294 295 switch (*ip_version & 0xf0) {296 case 0x40:297 *inner_proto = ETH_P_IP;298 break;299 case 0x60:300 *inner_proto = ETH_P_IPV6;301 break;302 default:303 return -1;304 }305 306 return 0;307}308 309static int gtp_rx(struct pdp_ctx *pctx, struct sk_buff *skb,310 unsigned int hdrlen, unsigned int role, __u16 inner_proto)311{312 if (!gtp_check_ms(skb, pctx, hdrlen, role, inner_proto)) {313 netdev_dbg(pctx->dev, "No PDP ctx for this MS\n");314 return 1;315 }316 317 /* Get rid of the GTP + UDP headers. */318 if (iptunnel_pull_header(skb, hdrlen, htons(inner_proto),319 !net_eq(sock_net(pctx->sk), dev_net(pctx->dev)))) {320 pctx->dev->stats.rx_length_errors++;321 goto err;322 }323 324 netdev_dbg(pctx->dev, "forwarding packet from GGSN to uplink\n");325 326 /* Now that the UDP and the GTP header have been removed, set up the327 * new network header. This is required by the upper layer to328 * calculate the transport header.329 */330 skb_reset_network_header(skb);331 skb_reset_mac_header(skb);332 333 skb->dev = pctx->dev;334 335 dev_sw_netstats_rx_add(pctx->dev, skb->len);336 337 __netif_rx(skb);338 return 0;339 340err:341 pctx->dev->stats.rx_dropped++;342 return -1;343}344 345static struct rtable *ip4_route_output_gtp(struct flowi4 *fl4,346 const struct sock *sk,347 __be32 daddr, __be32 saddr)348{349 memset(fl4, 0, sizeof(*fl4));350 fl4->flowi4_oif = sk->sk_bound_dev_if;351 fl4->daddr = daddr;352 fl4->saddr = saddr;353 fl4->flowi4_tos = ip_sock_rt_tos(sk);354 fl4->flowi4_scope = ip_sock_rt_scope(sk);355 fl4->flowi4_proto = sk->sk_protocol;356 357 return ip_route_output_key(sock_net(sk), fl4);358}359 360static struct rt6_info *ip6_route_output_gtp(struct net *net,361 struct flowi6 *fl6,362 const struct sock *sk,363 const struct in6_addr *daddr,364 struct in6_addr *saddr)365{366 struct dst_entry *dst;367 368 memset(fl6, 0, sizeof(*fl6));369 fl6->flowi6_oif = sk->sk_bound_dev_if;370 fl6->daddr = *daddr;371 fl6->saddr = *saddr;372 fl6->flowi6_proto = sk->sk_protocol;373 374 dst = ipv6_stub->ipv6_dst_lookup_flow(net, sk, fl6, NULL);375 if (IS_ERR(dst))376 return ERR_PTR(-ENETUNREACH);377 378 return (struct rt6_info *)dst;379}380 381/* GSM TS 09.60. 7.3382 * In all Path Management messages:383 * - TID: is not used and shall be set to 0.384 * - Flow Label is not used and shall be set to 0385 * In signalling messages:386 * - number: this field is not yet used in signalling messages.387 * It shall be set to 255 by the sender and shall be ignored388 * by the receiver389 * Returns true if the echo req was correct, false otherwise.390 */391static bool gtp0_validate_echo_hdr(struct gtp0_header *gtp0)392{393 return !(gtp0->tid || (gtp0->flags ^ 0x1e) ||394 gtp0->number != 0xff || gtp0->flow);395}396 397/* msg_type has to be GTP_ECHO_REQ or GTP_ECHO_RSP */398static void gtp0_build_echo_msg(struct gtp0_header *hdr, __u8 msg_type)399{400 int len_pkt, len_hdr;401 402 hdr->flags = 0x1e; /* v0, GTP-non-prime. */403 hdr->type = msg_type;404 /* GSM TS 09.60. 7.3 In all Path Management Flow Label and TID405 * are not used and shall be set to 0.406 */407 hdr->flow = 0;408 hdr->tid = 0;409 hdr->number = 0xff;410 hdr->spare[0] = 0xff;411 hdr->spare[1] = 0xff;412 hdr->spare[2] = 0xff;413 414 len_pkt = sizeof(struct gtp0_packet);415 len_hdr = sizeof(struct gtp0_header);416 417 if (msg_type == GTP_ECHO_RSP)418 hdr->length = htons(len_pkt - len_hdr);419 else420 hdr->length = 0;421}422 423static int gtp0_send_echo_resp_ip(struct gtp_dev *gtp, struct sk_buff *skb)424{425 struct iphdr *iph = ip_hdr(skb);426 struct flowi4 fl4;427 struct rtable *rt;428 429 /* find route to the sender,430 * src address becomes dst address and vice versa.431 */432 rt = ip4_route_output_gtp(&fl4, gtp->sk0, iph->saddr, iph->daddr);433 if (IS_ERR(rt)) {434 netdev_dbg(gtp->dev, "no route for echo response from %pI4\n",435 &iph->saddr);436 return -1;437 }438 439 udp_tunnel_xmit_skb(rt, gtp->sk0, skb,440 fl4.saddr, fl4.daddr,441 iph->tos,442 ip4_dst_hoplimit(&rt->dst),443 0,444 htons(GTP0_PORT), htons(GTP0_PORT),445 !net_eq(sock_net(gtp->sk1u),446 dev_net(gtp->dev)),447 false);448 449 return 0;450}451 452static int gtp0_send_echo_resp(struct gtp_dev *gtp, struct sk_buff *skb)453{454 struct gtp0_packet *gtp_pkt;455 struct gtp0_header *gtp0;456 __be16 seq;457 458 gtp0 = (struct gtp0_header *)(skb->data + sizeof(struct udphdr));459 460 if (!gtp0_validate_echo_hdr(gtp0))461 return -1;462 463 seq = gtp0->seq;464 465 /* pull GTP and UDP headers */466 skb_pull_data(skb, sizeof(struct gtp0_header) + sizeof(struct udphdr));467 468 gtp_pkt = skb_push(skb, sizeof(struct gtp0_packet));469 memset(gtp_pkt, 0, sizeof(struct gtp0_packet));470 471 gtp0_build_echo_msg(>p_pkt->gtp0_h, GTP_ECHO_RSP);472 473 /* GSM TS 09.60. 7.3 The Sequence Number in a signalling response474 * message shall be copied from the signalling request message475 * that the GSN is replying to.476 */477 gtp_pkt->gtp0_h.seq = seq;478 479 gtp_pkt->ie.tag = GTPIE_RECOVERY;480 gtp_pkt->ie.val = gtp->restart_count;481 482 switch (gtp->sk0->sk_family) {483 case AF_INET:484 if (gtp0_send_echo_resp_ip(gtp, skb) < 0)485 return -1;486 break;487 case AF_INET6:488 return -1;489 }490 491 return 0;492}493 494static int gtp_genl_fill_echo(struct sk_buff *skb, u32 snd_portid, u32 snd_seq,495 int flags, u32 type, struct echo_info echo)496{497 void *genlh;498 499 genlh = genlmsg_put(skb, snd_portid, snd_seq, >p_genl_family, flags,500 type);501 if (!genlh)502 goto failure;503 504 if (nla_put_u32(skb, GTPA_VERSION, echo.gtp_version) ||505 nla_put_be32(skb, GTPA_PEER_ADDRESS, echo.peer.addr.s_addr) ||506 nla_put_be32(skb, GTPA_MS_ADDRESS, echo.ms.addr.s_addr))507 goto failure;508 509 genlmsg_end(skb, genlh);510 return 0;511 512failure:513 genlmsg_cancel(skb, genlh);514 return -EMSGSIZE;515}516 517static void gtp0_handle_echo_resp_ip(struct sk_buff *skb, struct echo_info *echo)518{519 struct iphdr *iph = ip_hdr(skb);520 521 echo->ms.addr.s_addr = iph->daddr;522 echo->peer.addr.s_addr = iph->saddr;523 echo->gtp_version = GTP_V0;524}525 526static int gtp0_handle_echo_resp(struct gtp_dev *gtp, struct sk_buff *skb)527{528 struct gtp0_header *gtp0;529 struct echo_info echo;530 struct sk_buff *msg;531 int ret;532 533 gtp0 = (struct gtp0_header *)(skb->data + sizeof(struct udphdr));534 535 if (!gtp0_validate_echo_hdr(gtp0))536 return -1;537 538 switch (gtp->sk0->sk_family) {539 case AF_INET:540 gtp0_handle_echo_resp_ip(skb, &echo);541 break;542 case AF_INET6:543 return -1;544 }545 546 msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);547 if (!msg)548 return -ENOMEM;549 550 ret = gtp_genl_fill_echo(msg, 0, 0, 0, GTP_CMD_ECHOREQ, echo);551 if (ret < 0) {552 nlmsg_free(msg);553 return ret;554 }555 556 return genlmsg_multicast_netns(>p_genl_family, dev_net(gtp->dev),557 msg, 0, GTP_GENL_MCGRP, GFP_ATOMIC);558}559 560static int gtp_proto_to_family(__u16 proto)561{562 switch (proto) {563 case ETH_P_IP:564 return AF_INET;565 case ETH_P_IPV6:566 return AF_INET6;567 default:568 WARN_ON_ONCE(1);569 break;570 }571 572 return AF_UNSPEC;573}574 575/* 1 means pass up to the stack, -1 means drop and 0 means decapsulated. */576static int gtp0_udp_encap_recv(struct gtp_dev *gtp, struct sk_buff *skb)577{578 unsigned int hdrlen = sizeof(struct udphdr) +579 sizeof(struct gtp0_header);580 struct gtp0_header *gtp0;581 struct pdp_ctx *pctx;582 __u16 inner_proto;583 584 if (!pskb_may_pull(skb, hdrlen))585 return -1;586 587 gtp0 = (struct gtp0_header *)(skb->data + sizeof(struct udphdr));588 589 if ((gtp0->flags >> 5) != GTP_V0)590 return 1;591 592 /* If the sockets were created in kernel, it means that593 * there is no daemon running in userspace which would594 * handle echo request.595 */596 if (gtp0->type == GTP_ECHO_REQ && gtp->sk_created)597 return gtp0_send_echo_resp(gtp, skb);598 599 if (gtp0->type == GTP_ECHO_RSP && gtp->sk_created)600 return gtp0_handle_echo_resp(gtp, skb);601 602 if (gtp0->type != GTP_TPDU)603 return 1;604 605 if (gtp_inner_proto(skb, hdrlen, &inner_proto) < 0) {606 netdev_dbg(gtp->dev, "GTP packet does not encapsulate an IP packet\n");607 return -1;608 }609 610 pctx = gtp0_pdp_find(gtp, be64_to_cpu(gtp0->tid),611 gtp_proto_to_family(inner_proto));612 if (!pctx) {613 netdev_dbg(gtp->dev, "No PDP ctx to decap skb=%p\n", skb);614 return 1;615 }616 617 return gtp_rx(pctx, skb, hdrlen, gtp->role, inner_proto);618}619 620/* msg_type has to be GTP_ECHO_REQ or GTP_ECHO_RSP */621static void gtp1u_build_echo_msg(struct gtp1_header_long *hdr, __u8 msg_type)622{623 int len_pkt, len_hdr;624 625 /* S flag must be set to 1 */626 hdr->flags = 0x32; /* v1, GTP-non-prime. */627 hdr->type = msg_type;628 /* 3GPP TS 29.281 5.1 - TEID has to be set to 0 */629 hdr->tid = 0;630 631 /* seq, npdu and next should be counted to the length of the GTP packet632 * that's why szie of gtp1_header should be subtracted,633 * not size of gtp1_header_long.634 */635 636 len_hdr = sizeof(struct gtp1_header);637 638 if (msg_type == GTP_ECHO_RSP) {639 len_pkt = sizeof(struct gtp1u_packet);640 hdr->length = htons(len_pkt - len_hdr);641 } else {642 /* GTP_ECHO_REQ does not carry GTP Information Element,643 * the why gtp1_header_long is used here.644 */645 len_pkt = sizeof(struct gtp1_header_long);646 hdr->length = htons(len_pkt - len_hdr);647 }648}649 650static int gtp1u_send_echo_resp(struct gtp_dev *gtp, struct sk_buff *skb)651{652 struct gtp1_header_long *gtp1u;653 struct gtp1u_packet *gtp_pkt;654 struct rtable *rt;655 struct flowi4 fl4;656 struct iphdr *iph;657 658 gtp1u = (struct gtp1_header_long *)(skb->data + sizeof(struct udphdr));659 660 /* 3GPP TS 29.281 5.1 - For the Echo Request, Echo Response,661 * Error Indication and Supported Extension Headers Notification662 * messages, the S flag shall be set to 1 and TEID shall be set to 0.663 */664 if (!(gtp1u->flags & GTP1_F_SEQ) || gtp1u->tid)665 return -1;666 667 /* pull GTP and UDP headers */668 skb_pull_data(skb,669 sizeof(struct gtp1_header_long) + sizeof(struct udphdr));670 671 gtp_pkt = skb_push(skb, sizeof(struct gtp1u_packet));672 memset(gtp_pkt, 0, sizeof(struct gtp1u_packet));673 674 gtp1u_build_echo_msg(>p_pkt->gtp1u_h, GTP_ECHO_RSP);675 676 /* 3GPP TS 29.281 7.7.2 - The Restart Counter value in the677 * Recovery information element shall not be used, i.e. it shall678 * be set to zero by the sender and shall be ignored by the receiver.679 * The Recovery information element is mandatory due to backwards680 * compatibility reasons.681 */682 gtp_pkt->ie.tag = GTPIE_RECOVERY;683 gtp_pkt->ie.val = 0;684 685 iph = ip_hdr(skb);686 687 /* find route to the sender,688 * src address becomes dst address and vice versa.689 */690 rt = ip4_route_output_gtp(&fl4, gtp->sk1u, iph->saddr, iph->daddr);691 if (IS_ERR(rt)) {692 netdev_dbg(gtp->dev, "no route for echo response from %pI4\n",693 &iph->saddr);694 return -1;695 }696 697 udp_tunnel_xmit_skb(rt, gtp->sk1u, skb,698 fl4.saddr, fl4.daddr,699 iph->tos,700 ip4_dst_hoplimit(&rt->dst),701 0,702 htons(GTP1U_PORT), htons(GTP1U_PORT),703 !net_eq(sock_net(gtp->sk1u),704 dev_net(gtp->dev)),705 false);706 return 0;707}708 709static int gtp1u_handle_echo_resp(struct gtp_dev *gtp, struct sk_buff *skb)710{711 struct gtp1_header_long *gtp1u;712 struct echo_info echo;713 struct sk_buff *msg;714 struct iphdr *iph;715 int ret;716 717 gtp1u = (struct gtp1_header_long *)(skb->data + sizeof(struct udphdr));718 719 /* 3GPP TS 29.281 5.1 - For the Echo Request, Echo Response,720 * Error Indication and Supported Extension Headers Notification721 * messages, the S flag shall be set to 1 and TEID shall be set to 0.722 */723 if (!(gtp1u->flags & GTP1_F_SEQ) || gtp1u->tid)724 return -1;725 726 iph = ip_hdr(skb);727 echo.ms.addr.s_addr = iph->daddr;728 echo.peer.addr.s_addr = iph->saddr;729 echo.gtp_version = GTP_V1;730 731 msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_ATOMIC);732 if (!msg)733 return -ENOMEM;734 735 ret = gtp_genl_fill_echo(msg, 0, 0, 0, GTP_CMD_ECHOREQ, echo);736 if (ret < 0) {737 nlmsg_free(msg);738 return ret;739 }740 741 return genlmsg_multicast_netns(>p_genl_family, dev_net(gtp->dev),742 msg, 0, GTP_GENL_MCGRP, GFP_ATOMIC);743}744 745static int gtp_parse_exthdrs(struct sk_buff *skb, unsigned int *hdrlen)746{747 struct gtp_ext_hdr *gtp_exthdr, _gtp_exthdr;748 unsigned int offset = *hdrlen;749 __u8 *next_type, _next_type;750 751 /* From 29.060: "The Extension Header Length field specifies the length752 * of the particular Extension header in 4 octets units."753 *754 * This length field includes length field size itself (1 byte),755 * payload (variable length) and next type (1 byte). The extension756 * header is aligned to to 4 bytes.757 */758 759 do {760 gtp_exthdr = skb_header_pointer(skb, offset, sizeof(*gtp_exthdr),761 &_gtp_exthdr);762 if (!gtp_exthdr || !gtp_exthdr->len)763 return -1;764 765 offset += gtp_exthdr->len * 4;766 767 /* From 29.060: "If no such Header follows, then the value of768 * the Next Extension Header Type shall be 0."769 */770 next_type = skb_header_pointer(skb, offset - 1,771 sizeof(_next_type), &_next_type);772 if (!next_type)773 return -1;774 775 } while (*next_type != 0);776 777 *hdrlen = offset;778 779 return 0;780}781 782static int gtp1u_udp_encap_recv(struct gtp_dev *gtp, struct sk_buff *skb)783{784 unsigned int hdrlen = sizeof(struct udphdr) +785 sizeof(struct gtp1_header);786 struct gtp1_header *gtp1;787 struct pdp_ctx *pctx;788 __u16 inner_proto;789 790 if (!pskb_may_pull(skb, hdrlen))791 return -1;792 793 gtp1 = (struct gtp1_header *)(skb->data + sizeof(struct udphdr));794 795 if ((gtp1->flags >> 5) != GTP_V1)796 return 1;797 798 /* If the sockets were created in kernel, it means that799 * there is no daemon running in userspace which would800 * handle echo request.801 */802 if (gtp1->type == GTP_ECHO_REQ && gtp->sk_created)803 return gtp1u_send_echo_resp(gtp, skb);804 805 if (gtp1->type == GTP_ECHO_RSP && gtp->sk_created)806 return gtp1u_handle_echo_resp(gtp, skb);807 808 if (gtp1->type != GTP_TPDU)809 return 1;810 811 /* From 29.060: "This field shall be present if and only if any one or812 * more of the S, PN and E flags are set.".813 *814 * If any of the bit is set, then the remaining ones also have to be815 * set.816 */817 if (gtp1->flags & GTP1_F_MASK)818 hdrlen += 4;819 820 /* Make sure the header is larger enough, including extensions. */821 if (!pskb_may_pull(skb, hdrlen))822 return -1;823 824 if (gtp_inner_proto(skb, hdrlen, &inner_proto) < 0) {825 netdev_dbg(gtp->dev, "GTP packet does not encapsulate an IP packet\n");826 return -1;827 }828 829 gtp1 = (struct gtp1_header *)(skb->data + sizeof(struct udphdr));830 831 pctx = gtp1_pdp_find(gtp, ntohl(gtp1->tid),832 gtp_proto_to_family(inner_proto));833 if (!pctx) {834 netdev_dbg(gtp->dev, "No PDP ctx to decap skb=%p\n", skb);835 return 1;836 }837 838 if (gtp1->flags & GTP1_F_EXTHDR &&839 gtp_parse_exthdrs(skb, &hdrlen) < 0)840 return -1;841 842 return gtp_rx(pctx, skb, hdrlen, gtp->role, inner_proto);843}844 845static void __gtp_encap_destroy(struct sock *sk)846{847 struct gtp_dev *gtp;848 849 lock_sock(sk);850 gtp = sk->sk_user_data;851 if (gtp) {852 if (gtp->sk0 == sk)853 gtp->sk0 = NULL;854 else855 gtp->sk1u = NULL;856 WRITE_ONCE(udp_sk(sk)->encap_type, 0);857 rcu_assign_sk_user_data(sk, NULL);858 release_sock(sk);859 sock_put(sk);860 return;861 }862 release_sock(sk);863}864 865static void gtp_encap_destroy(struct sock *sk)866{867 rtnl_lock();868 __gtp_encap_destroy(sk);869 rtnl_unlock();870}871 872static void gtp_encap_disable_sock(struct sock *sk)873{874 if (!sk)875 return;876 877 __gtp_encap_destroy(sk);878}879 880static void gtp_encap_disable(struct gtp_dev *gtp)881{882 if (gtp->sk_created) {883 udp_tunnel_sock_release(gtp->sk0->sk_socket);884 udp_tunnel_sock_release(gtp->sk1u->sk_socket);885 gtp->sk_created = false;886 gtp->sk0 = NULL;887 gtp->sk1u = NULL;888 } else {889 gtp_encap_disable_sock(gtp->sk0);890 gtp_encap_disable_sock(gtp->sk1u);891 }892}893 894/* UDP encapsulation receive handler. See net/ipv4/udp.c.895 * Return codes: 0: success, <0: error, >0: pass up to userspace UDP socket.896 */897static int gtp_encap_recv(struct sock *sk, struct sk_buff *skb)898{899 struct gtp_dev *gtp;900 int ret = 0;901 902 gtp = rcu_dereference_sk_user_data(sk);903 if (!gtp)904 return 1;905 906 netdev_dbg(gtp->dev, "encap_recv sk=%p\n", sk);907 908 switch (READ_ONCE(udp_sk(sk)->encap_type)) {909 case UDP_ENCAP_GTP0:910 netdev_dbg(gtp->dev, "received GTP0 packet\n");911 ret = gtp0_udp_encap_recv(gtp, skb);912 break;913 case UDP_ENCAP_GTP1U:914 netdev_dbg(gtp->dev, "received GTP1U packet\n");915 ret = gtp1u_udp_encap_recv(gtp, skb);916 break;917 default:918 ret = -1; /* Shouldn't happen. */919 }920 921 switch (ret) {922 case 1:923 netdev_dbg(gtp->dev, "pass up to the process\n");924 break;925 case 0:926 break;927 case -1:928 netdev_dbg(gtp->dev, "GTP packet has been dropped\n");929 kfree_skb(skb);930 ret = 0;931 break;932 }933 934 return ret;935}936 937static void gtp_dev_uninit(struct net_device *dev)938{939 struct gtp_dev *gtp = netdev_priv(dev);940 941 gtp_encap_disable(gtp);942}943 944static inline void gtp0_push_header(struct sk_buff *skb, struct pdp_ctx *pctx)945{946 int payload_len = skb->len;947 struct gtp0_header *gtp0;948 949 gtp0 = skb_push(skb, sizeof(*gtp0));950 951 gtp0->flags = 0x1e; /* v0, GTP-non-prime. */952 gtp0->type = GTP_TPDU;953 gtp0->length = htons(payload_len);954 gtp0->seq = htons((atomic_inc_return(&pctx->tx_seq) - 1) % 0xffff);955 gtp0->flow = htons(pctx->u.v0.flow);956 gtp0->number = 0xff;957 gtp0->spare[0] = gtp0->spare[1] = gtp0->spare[2] = 0xff;958 gtp0->tid = cpu_to_be64(pctx->u.v0.tid);959}960 961static inline void gtp1_push_header(struct sk_buff *skb, struct pdp_ctx *pctx)962{963 int payload_len = skb->len;964 struct gtp1_header *gtp1;965 966 gtp1 = skb_push(skb, sizeof(*gtp1));967 968 /* Bits 8 7 6 5 4 3 2 1969 * +--+--+--+--+--+--+--+--+970 * |version |PT| 0| E| S|PN|971 * +--+--+--+--+--+--+--+--+972 * 0 0 1 1 1 0 0 0973 */974 gtp1->flags = 0x30; /* v1, GTP-non-prime. */975 gtp1->type = GTP_TPDU;976 gtp1->length = htons(payload_len);977 gtp1->tid = htonl(pctx->u.v1.o_tei);978 979 /* TODO: Support for extension header, sequence number and N-PDU.980 * Update the length field if any of them is available.981 */982}983 984struct gtp_pktinfo {985 struct sock *sk;986 union {987 struct flowi4 fl4;988 struct flowi6 fl6;989 };990 union {991 struct rtable *rt;992 struct rt6_info *rt6;993 };994 struct pdp_ctx *pctx;995 struct net_device *dev;996 __u8 tos;997 __be16 gtph_port;998};999 1000static void gtp_push_header(struct sk_buff *skb, struct gtp_pktinfo *pktinfo)1001{1002 switch (pktinfo->pctx->gtp_version) {1003 case GTP_V0:1004 pktinfo->gtph_port = htons(GTP0_PORT);1005 gtp0_push_header(skb, pktinfo->pctx);1006 break;1007 case GTP_V1:1008 pktinfo->gtph_port = htons(GTP1U_PORT);1009 gtp1_push_header(skb, pktinfo->pctx);1010 break;1011 }1012}1013 1014static inline void gtp_set_pktinfo_ipv4(struct gtp_pktinfo *pktinfo,1015 struct sock *sk, __u8 tos,1016 struct pdp_ctx *pctx, struct rtable *rt,1017 struct flowi4 *fl4,1018 struct net_device *dev)1019{1020 pktinfo->sk = sk;1021 pktinfo->tos = tos;1022 pktinfo->pctx = pctx;1023 pktinfo->rt = rt;1024 pktinfo->fl4 = *fl4;1025 pktinfo->dev = dev;1026}1027 1028static void gtp_set_pktinfo_ipv6(struct gtp_pktinfo *pktinfo,1029 struct sock *sk, __u8 tos,1030 struct pdp_ctx *pctx, struct rt6_info *rt6,1031 struct flowi6 *fl6,1032 struct net_device *dev)1033{1034 pktinfo->sk = sk;1035 pktinfo->tos = tos;1036 pktinfo->pctx = pctx;1037 pktinfo->rt6 = rt6;1038 pktinfo->fl6 = *fl6;1039 pktinfo->dev = dev;1040}1041 1042static int gtp_build_skb_outer_ip4(struct sk_buff *skb, struct net_device *dev,1043 struct gtp_pktinfo *pktinfo,1044 struct pdp_ctx *pctx, __u8 tos,1045 __be16 frag_off)1046{1047 struct rtable *rt;1048 struct flowi4 fl4;1049 __be16 df;1050 int mtu;1051 1052 rt = ip4_route_output_gtp(&fl4, pctx->sk, pctx->peer.addr.s_addr,1053 inet_sk(pctx->sk)->inet_saddr);1054 if (IS_ERR(rt)) {1055 netdev_dbg(dev, "no route to SSGN %pI4\n",1056 &pctx->peer.addr.s_addr);1057 dev->stats.tx_carrier_errors++;1058 goto err;1059 }1060 1061 if (rt->dst.dev == dev) {1062 netdev_dbg(dev, "circular route to SSGN %pI4\n",1063 &pctx->peer.addr.s_addr);1064 dev->stats.collisions++;1065 goto err_rt;1066 }1067 1068 /* This is similar to tnl_update_pmtu(). */1069 df = frag_off;1070 if (df) {1071 mtu = dst_mtu(&rt->dst) - dev->hard_header_len -1072 sizeof(struct iphdr) - sizeof(struct udphdr);1073 switch (pctx->gtp_version) {1074 case GTP_V0:1075 mtu -= sizeof(struct gtp0_header);1076 break;1077 case GTP_V1:1078 mtu -= sizeof(struct gtp1_header);1079 break;1080 }1081 } else {1082 mtu = dst_mtu(&rt->dst);1083 }1084 1085 skb_dst_update_pmtu_no_confirm(skb, mtu);1086 1087 if (frag_off & htons(IP_DF) &&1088 ((!skb_is_gso(skb) && skb->len > mtu) ||1089 (skb_is_gso(skb) && !skb_gso_validate_network_len(skb, mtu)))) {1090 netdev_dbg(dev, "packet too big, fragmentation needed\n");1091 icmp_ndo_send(skb, ICMP_DEST_UNREACH, ICMP_FRAG_NEEDED,1092 htonl(mtu));1093 goto err_rt;1094 }1095 1096 gtp_set_pktinfo_ipv4(pktinfo, pctx->sk, tos, pctx, rt, &fl4, dev);1097 gtp_push_header(skb, pktinfo);1098 1099 return 0;1100err_rt:1101 ip_rt_put(rt);1102err:1103 return -EBADMSG;1104}1105 1106static int gtp_build_skb_outer_ip6(struct net *net, struct sk_buff *skb,1107 struct net_device *dev,1108 struct gtp_pktinfo *pktinfo,1109 struct pdp_ctx *pctx, __u8 tos)1110{1111 struct dst_entry *dst;1112 struct rt6_info *rt;1113 struct flowi6 fl6;1114 int mtu;1115 1116 rt = ip6_route_output_gtp(net, &fl6, pctx->sk, &pctx->peer.addr6,1117 &inet6_sk(pctx->sk)->saddr);1118 if (IS_ERR(rt)) {1119 netdev_dbg(dev, "no route to SSGN %pI6\n",1120 &pctx->peer.addr6);1121 dev->stats.tx_carrier_errors++;1122 goto err;1123 }1124 dst = &rt->dst;1125 1126 if (rt->dst.dev == dev) {1127 netdev_dbg(dev, "circular route to SSGN %pI6\n",1128 &pctx->peer.addr6);1129 dev->stats.collisions++;1130 goto err_rt;1131 }1132 1133 mtu = dst_mtu(&rt->dst) - dev->hard_header_len -1134 sizeof(struct ipv6hdr) - sizeof(struct udphdr);1135 switch (pctx->gtp_version) {1136 case GTP_V0:1137 mtu -= sizeof(struct gtp0_header);1138 break;1139 case GTP_V1:1140 mtu -= sizeof(struct gtp1_header);1141 break;1142 }1143 1144 skb_dst_update_pmtu_no_confirm(skb, mtu);1145 1146 if ((!skb_is_gso(skb) && skb->len > mtu) ||1147 (skb_is_gso(skb) && !skb_gso_validate_network_len(skb, mtu))) {1148 netdev_dbg(dev, "packet too big, fragmentation needed\n");1149 icmpv6_ndo_send(skb, ICMPV6_PKT_TOOBIG, 0, mtu);1150 goto err_rt;1151 }1152 1153 gtp_set_pktinfo_ipv6(pktinfo, pctx->sk, tos, pctx, rt, &fl6, dev);1154 gtp_push_header(skb, pktinfo);1155 1156 return 0;1157err_rt:1158 dst_release(dst);1159err:1160 return -EBADMSG;1161}1162 1163static int gtp_build_skb_ip4(struct sk_buff *skb, struct net_device *dev,1164 struct gtp_pktinfo *pktinfo)1165{1166 struct gtp_dev *gtp = netdev_priv(dev);1167 struct net *net = gtp->net;1168 struct pdp_ctx *pctx;1169 struct iphdr *iph;1170 int ret;1171 1172 /* Read the IP destination address and resolve the PDP context.1173 * Prepend PDP header with TEI/TID from PDP ctx.1174 */1175 iph = ip_hdr(skb);1176 if (gtp->role == GTP_ROLE_SGSN)1177 pctx = ipv4_pdp_find(gtp, iph->saddr);1178 else1179 pctx = ipv4_pdp_find(gtp, iph->daddr);1180 1181 if (!pctx) {1182 netdev_dbg(dev, "no PDP ctx found for %pI4, skip\n",1183 &iph->daddr);1184 return -ENOENT;1185 }1186 netdev_dbg(dev, "found PDP context %p\n", pctx);1187 1188 switch (pctx->sk->sk_family) {1189 case AF_INET:1190 ret = gtp_build_skb_outer_ip4(skb, dev, pktinfo, pctx,1191 iph->tos, iph->frag_off);1192 break;1193 case AF_INET6:1194 ret = gtp_build_skb_outer_ip6(net, skb, dev, pktinfo, pctx,1195 iph->tos);1196 break;1197 default:1198 ret = -1;1199 WARN_ON_ONCE(1);1200 break;1201 }1202 1203 if (ret < 0)1204 return ret;1205 1206 netdev_dbg(dev, "gtp -> IP src: %pI4 dst: %pI4\n",1207 &iph->saddr, &iph->daddr);1208 1209 return 0;1210}1211 1212static int gtp_build_skb_ip6(struct sk_buff *skb, struct net_device *dev,1213 struct gtp_pktinfo *pktinfo)1214{1215 struct gtp_dev *gtp = netdev_priv(dev);1216 struct net *net = gtp->net;1217 struct pdp_ctx *pctx;1218 struct ipv6hdr *ip6h;1219 __u8 tos;1220 int ret;1221 1222 /* Read the IP destination address and resolve the PDP context.1223 * Prepend PDP header with TEI/TID from PDP ctx.1224 */1225 ip6h = ipv6_hdr(skb);1226 if (gtp->role == GTP_ROLE_SGSN)1227 pctx = ipv6_pdp_find(gtp, &ip6h->saddr);1228 else1229 pctx = ipv6_pdp_find(gtp, &ip6h->daddr);1230 1231 if (!pctx) {1232 netdev_dbg(dev, "no PDP ctx found for %pI6, skip\n",1233 &ip6h->daddr);1234 return -ENOENT;1235 }1236 netdev_dbg(dev, "found PDP context %p\n", pctx);1237 1238 tos = ipv6_get_dsfield(ip6h);1239 1240 switch (pctx->sk->sk_family) {1241 case AF_INET:1242 ret = gtp_build_skb_outer_ip4(skb, dev, pktinfo, pctx, tos, 0);1243 break;1244 case AF_INET6:1245 ret = gtp_build_skb_outer_ip6(net, skb, dev, pktinfo, pctx, tos);1246 break;1247 default:1248 ret = -1;1249 WARN_ON_ONCE(1);1250 break;1251 }1252 1253 if (ret < 0)1254 return ret;1255 1256 netdev_dbg(dev, "gtp -> IP src: %pI6 dst: %pI6\n",1257 &ip6h->saddr, &ip6h->daddr);1258 1259 return 0;1260}1261 1262static netdev_tx_t gtp_dev_xmit(struct sk_buff *skb, struct net_device *dev)1263{1264 unsigned int proto = ntohs(skb->protocol);1265 struct gtp_pktinfo pktinfo;1266 int err;1267 1268 /* Ensure there is sufficient headroom. */1269 if (skb_cow_head(skb, dev->needed_headroom))1270 goto tx_err;1271 1272 if (!pskb_inet_may_pull(skb))1273 goto tx_err;1274 1275 skb_reset_inner_headers(skb);1276 1277 /* PDP context lookups in gtp_build_skb_*() need rcu read-side lock. */1278 rcu_read_lock();1279 switch (proto) {1280 case ETH_P_IP:1281 err = gtp_build_skb_ip4(skb, dev, &pktinfo);1282 break;1283 case ETH_P_IPV6:1284 err = gtp_build_skb_ip6(skb, dev, &pktinfo);1285 break;1286 default:1287 err = -EOPNOTSUPP;1288 break;1289 }1290 rcu_read_unlock();1291 1292 if (err < 0)1293 goto tx_err;1294 1295 switch (pktinfo.pctx->sk->sk_family) {1296 case AF_INET:1297 udp_tunnel_xmit_skb(pktinfo.rt, pktinfo.sk, skb,1298 pktinfo.fl4.saddr, pktinfo.fl4.daddr,1299 pktinfo.tos,1300 ip4_dst_hoplimit(&pktinfo.rt->dst),1301 0,1302 pktinfo.gtph_port, pktinfo.gtph_port,1303 !net_eq(sock_net(pktinfo.pctx->sk),1304 dev_net(dev)),1305 false);1306 break;1307 case AF_INET6:1308#if IS_ENABLED(CONFIG_IPV6)1309 udp_tunnel6_xmit_skb(&pktinfo.rt6->dst, pktinfo.sk, skb, dev,1310 &pktinfo.fl6.saddr, &pktinfo.fl6.daddr,1311 pktinfo.tos,1312 ip6_dst_hoplimit(&pktinfo.rt->dst),1313 0,1314 pktinfo.gtph_port, pktinfo.gtph_port,1315 false);1316#else1317 goto tx_err;1318#endif1319 break;1320 }1321 1322 return NETDEV_TX_OK;1323tx_err:1324 dev->stats.tx_errors++;1325 dev_kfree_skb(skb);1326 return NETDEV_TX_OK;1327}1328 1329static const struct net_device_ops gtp_netdev_ops = {1330 .ndo_uninit = gtp_dev_uninit,1331 .ndo_start_xmit = gtp_dev_xmit,1332};1333 1334static const struct device_type gtp_type = {1335 .name = "gtp",1336};1337 1338#define GTP_TH_MAXLEN (sizeof(struct udphdr) + sizeof(struct gtp0_header))1339#define GTP_IPV4_MAXLEN (sizeof(struct iphdr) + GTP_TH_MAXLEN)1340 1341static void gtp_link_setup(struct net_device *dev)1342{1343 struct gtp_dev *gtp = netdev_priv(dev);1344 1345 dev->netdev_ops = >p_netdev_ops;1346 dev->needs_free_netdev = true;1347 SET_NETDEV_DEVTYPE(dev, >p_type);1348 1349 dev->hard_header_len = 0;1350 dev->addr_len = 0;1351 dev->mtu = ETH_DATA_LEN - GTP_IPV4_MAXLEN;1352 1353 /* Zero header length. */1354 dev->type = ARPHRD_NONE;1355 dev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;1356 1357 dev->pcpu_stat_type = NETDEV_PCPU_STAT_TSTATS;1358 dev->priv_flags |= IFF_NO_QUEUE;1359 dev->lltx = true;1360 netif_keep_dst(dev);1361 1362 dev->needed_headroom = LL_MAX_HEADER + GTP_IPV4_MAXLEN;1363 gtp->dev = dev;1364}1365 1366static int gtp_hashtable_new(struct gtp_dev *gtp, int hsize);1367static int gtp_encap_enable(struct gtp_dev *gtp, struct nlattr *data[]);1368 1369static void gtp_destructor(struct net_device *dev)1370{1371 struct gtp_dev *gtp = netdev_priv(dev);1372 1373 kfree(gtp->addr_hash);1374 kfree(gtp->tid_hash);1375}1376 1377static int gtp_sock_udp_config(struct udp_port_cfg *udp_conf,1378 const struct nlattr *nla, int family)1379{1380 udp_conf->family = family;1381 1382 switch (udp_conf->family) {1383 case AF_INET:1384 udp_conf->local_ip.s_addr = nla_get_be32(nla);1385 break;1386#if IS_ENABLED(CONFIG_IPV6)1387 case AF_INET6:1388 udp_conf->local_ip6 = nla_get_in6_addr(nla);1389 break;1390#endif1391 default:1392 return -EOPNOTSUPP;1393 }1394 1395 return 0;1396}1397 1398static struct sock *gtp_create_sock(int type, struct gtp_dev *gtp,1399 const struct nlattr *nla, int family)1400{1401 struct udp_tunnel_sock_cfg tuncfg = {};1402 struct udp_port_cfg udp_conf = {};1403 struct net *net = gtp->net;1404 struct socket *sock;1405 int err;1406 1407 if (nla) {1408 err = gtp_sock_udp_config(&udp_conf, nla, family);1409 if (err < 0)1410 return ERR_PTR(err);1411 } else {1412 udp_conf.local_ip.s_addr = htonl(INADDR_ANY);1413 udp_conf.family = AF_INET;1414 }1415 1416 if (type == UDP_ENCAP_GTP0)1417 udp_conf.local_udp_port = htons(GTP0_PORT);1418 else if (type == UDP_ENCAP_GTP1U)1419 udp_conf.local_udp_port = htons(GTP1U_PORT);1420 else1421 return ERR_PTR(-EINVAL);1422 1423 err = udp_sock_create(net, &udp_conf, &sock);1424 if (err)1425 return ERR_PTR(err);1426 1427 tuncfg.sk_user_data = gtp;1428 tuncfg.encap_type = type;1429 tuncfg.encap_rcv = gtp_encap_recv;1430 tuncfg.encap_destroy = NULL;1431 1432 setup_udp_tunnel_sock(net, sock, &tuncfg);1433 1434 return sock->sk;1435}1436 1437static int gtp_create_sockets(struct gtp_dev *gtp, const struct nlattr *nla,1438 int family)1439{1440 struct sock *sk1u;1441 struct sock *sk0;1442 1443 sk0 = gtp_create_sock(UDP_ENCAP_GTP0, gtp, nla, family);1444 if (IS_ERR(sk0))1445 return PTR_ERR(sk0);1446 1447 sk1u = gtp_create_sock(UDP_ENCAP_GTP1U, gtp, nla, family);1448 if (IS_ERR(sk1u)) {1449 udp_tunnel_sock_release(sk0->sk_socket);1450 return PTR_ERR(sk1u);1451 }1452 1453 gtp->sk_created = true;1454 gtp->sk0 = sk0;1455 gtp->sk1u = sk1u;1456 1457 return 0;1458}1459 1460#define GTP_TH_MAXLEN (sizeof(struct udphdr) + sizeof(struct gtp0_header))1461#define GTP_IPV6_MAXLEN (sizeof(struct ipv6hdr) + GTP_TH_MAXLEN)1462 1463static int gtp_newlink(struct net *src_net, struct net_device *dev,1464 struct nlattr *tb[], struct nlattr *data[],1465 struct netlink_ext_ack *extack)1466{1467 unsigned int role = GTP_ROLE_GGSN;1468 struct gtp_dev *gtp;1469 struct gtp_net *gn;1470 int hashsize, err;1471 1472#if !IS_ENABLED(CONFIG_IPV6)1473 if (data[IFLA_GTP_LOCAL6])1474 return -EAFNOSUPPORT;1475#endif1476 1477 gtp = netdev_priv(dev);1478 1479 if (!data[IFLA_GTP_PDP_HASHSIZE]) {1480 hashsize = 1024;1481 } else {1482 hashsize = nla_get_u32(data[IFLA_GTP_PDP_HASHSIZE]);1483 if (!hashsize)1484 hashsize = 1024;1485 }1486 1487 if (data[IFLA_GTP_ROLE]) {1488 role = nla_get_u32(data[IFLA_GTP_ROLE]);1489 if (role > GTP_ROLE_SGSN)1490 return -EINVAL;1491 }1492 gtp->role = role;1493 1494 if (!data[IFLA_GTP_RESTART_COUNT])1495 gtp->restart_count = 0;1496 else1497 gtp->restart_count = nla_get_u8(data[IFLA_GTP_RESTART_COUNT]);1498 1499 gtp->net = src_net;1500 1501 err = gtp_hashtable_new(gtp, hashsize);1502 if (err < 0)1503 return err;1504 1505 if (data[IFLA_GTP_CREATE_SOCKETS]) {1506 if (data[IFLA_GTP_LOCAL6])1507 err = gtp_create_sockets(gtp, data[IFLA_GTP_LOCAL6], AF_INET6);1508 else1509 err = gtp_create_sockets(gtp, data[IFLA_GTP_LOCAL], AF_INET);1510 } else {1511 err = gtp_encap_enable(gtp, data);1512 }1513 1514 if (err < 0)1515 goto out_hashtable;1516 1517 if ((gtp->sk0 && gtp->sk0->sk_family == AF_INET6) ||1518 (gtp->sk1u && gtp->sk1u->sk_family == AF_INET6)) {1519 dev->mtu = ETH_DATA_LEN - GTP_IPV6_MAXLEN;1520 dev->needed_headroom = LL_MAX_HEADER + GTP_IPV6_MAXLEN;1521 }1522 1523 err = register_netdevice(dev);1524 if (err < 0) {1525 netdev_dbg(dev, "failed to register new netdev %d\n", err);1526 goto out_encap;1527 }1528 1529 gn = net_generic(dev_net(dev), gtp_net_id);1530 list_add_rcu(>p->list, &gn->gtp_dev_list);1531 dev->priv_destructor = gtp_destructor;1532 1533 netdev_dbg(dev, "registered new GTP interface\n");1534 1535 return 0;1536 1537out_encap:1538 gtp_encap_disable(gtp);1539out_hashtable:1540 kfree(gtp->addr_hash);1541 kfree(gtp->tid_hash);1542 return err;1543}1544 1545static void gtp_dellink(struct net_device *dev, struct list_head *head)1546{1547 struct gtp_dev *gtp = netdev_priv(dev);1548 struct hlist_node *next;1549 struct pdp_ctx *pctx;1550 int i;1551 1552 for (i = 0; i < gtp->hash_size; i++)1553 hlist_for_each_entry_safe(pctx, next, >p->tid_hash[i], hlist_tid)1554 pdp_context_delete(pctx);1555 1556 list_del_rcu(>p->list);1557 unregister_netdevice_queue(dev, head);1558}1559 1560static const struct nla_policy gtp_policy[IFLA_GTP_MAX + 1] = {1561 [IFLA_GTP_FD0] = { .type = NLA_U32 },1562 [IFLA_GTP_FD1] = { .type = NLA_U32 },1563 [IFLA_GTP_PDP_HASHSIZE] = { .type = NLA_U32 },1564 [IFLA_GTP_ROLE] = { .type = NLA_U32 },1565 [IFLA_GTP_CREATE_SOCKETS] = { .type = NLA_U8 },1566 [IFLA_GTP_RESTART_COUNT] = { .type = NLA_U8 },1567 [IFLA_GTP_LOCAL] = { .type = NLA_U32 },1568 [IFLA_GTP_LOCAL6] = { .len = sizeof(struct in6_addr) },1569};1570 1571static int gtp_validate(struct nlattr *tb[], struct nlattr *data[],1572 struct netlink_ext_ack *extack)1573{1574 if (!data)1575 return -EINVAL;1576 1577 return 0;1578}1579 1580static size_t gtp_get_size(const struct net_device *dev)1581{1582 return nla_total_size(sizeof(__u32)) + /* IFLA_GTP_PDP_HASHSIZE */1583 nla_total_size(sizeof(__u32)) + /* IFLA_GTP_ROLE */1584 nla_total_size(sizeof(__u8)); /* IFLA_GTP_RESTART_COUNT */1585}1586 1587static int gtp_fill_info(struct sk_buff *skb, const struct net_device *dev)1588{1589 struct gtp_dev *gtp = netdev_priv(dev);1590 1591 if (nla_put_u32(skb, IFLA_GTP_PDP_HASHSIZE, gtp->hash_size))1592 goto nla_put_failure;1593 if (nla_put_u32(skb, IFLA_GTP_ROLE, gtp->role))1594 goto nla_put_failure;1595 if (nla_put_u8(skb, IFLA_GTP_RESTART_COUNT, gtp->restart_count))1596 goto nla_put_failure;1597 1598 return 0;1599 1600nla_put_failure:1601 return -EMSGSIZE;1602}1603 1604static struct rtnl_link_ops gtp_link_ops __read_mostly = {1605 .kind = "gtp",1606 .maxtype = IFLA_GTP_MAX,1607 .policy = gtp_policy,1608 .priv_size = sizeof(struct gtp_dev),1609 .setup = gtp_link_setup,1610 .validate = gtp_validate,1611 .newlink = gtp_newlink,1612 .dellink = gtp_dellink,1613 .get_size = gtp_get_size,1614 .fill_info = gtp_fill_info,1615};1616 1617static int gtp_hashtable_new(struct gtp_dev *gtp, int hsize)1618{1619 int i;1620 1621 gtp->addr_hash = kmalloc_array(hsize, sizeof(struct hlist_head),1622 GFP_KERNEL | __GFP_NOWARN);1623 if (gtp->addr_hash == NULL)1624 return -ENOMEM;1625 1626 gtp->tid_hash = kmalloc_array(hsize, sizeof(struct hlist_head),1627 GFP_KERNEL | __GFP_NOWARN);1628 if (gtp->tid_hash == NULL)1629 goto err1;1630 1631 gtp->hash_size = hsize;1632 1633 for (i = 0; i < hsize; i++) {1634 INIT_HLIST_HEAD(>p->addr_hash[i]);1635 INIT_HLIST_HEAD(>p->tid_hash[i]);1636 }1637 return 0;1638err1:1639 kfree(gtp->addr_hash);1640 return -ENOMEM;1641}1642 1643static struct sock *gtp_encap_enable_socket(int fd, int type,1644 struct gtp_dev *gtp)1645{1646 struct udp_tunnel_sock_cfg tuncfg = {NULL};1647 struct socket *sock;1648 struct sock *sk;1649 int err;1650 1651 pr_debug("enable gtp on %d, %d\n", fd, type);1652 1653 sock = sockfd_lookup(fd, &err);1654 if (!sock) {1655 pr_debug("gtp socket fd=%d not found\n", fd);1656 return ERR_PTR(err);1657 }1658 1659 sk = sock->sk;1660 if (sk->sk_protocol != IPPROTO_UDP ||1661 sk->sk_type != SOCK_DGRAM ||1662 (sk->sk_family != AF_INET && sk->sk_family != AF_INET6)) {1663 pr_debug("socket fd=%d not UDP\n", fd);1664 sk = ERR_PTR(-EINVAL);1665 goto out_sock;1666 }1667 1668 if (sk->sk_family == AF_INET6 &&1669 !sk->sk_ipv6only) {1670 sk = ERR_PTR(-EADDRNOTAVAIL);1671 goto out_sock;1672 }1673 1674 lock_sock(sk);1675 if (sk->sk_user_data) {1676 sk = ERR_PTR(-EBUSY);1677 goto out_rel_sock;1678 }1679 1680 sock_hold(sk);1681 1682 tuncfg.sk_user_data = gtp;1683 tuncfg.encap_type = type;1684 tuncfg.encap_rcv = gtp_encap_recv;1685 tuncfg.encap_destroy = gtp_encap_destroy;1686 1687 setup_udp_tunnel_sock(sock_net(sock->sk), sock, &tuncfg);1688 1689out_rel_sock:1690 release_sock(sock->sk);1691out_sock:1692 sockfd_put(sock);1693 return sk;1694}1695 1696static int gtp_encap_enable(struct gtp_dev *gtp, struct nlattr *data[])1697{1698 struct sock *sk1u = NULL;1699 struct sock *sk0 = NULL;1700 1701 if (!data[IFLA_GTP_FD0] && !data[IFLA_GTP_FD1])1702 return -EINVAL;1703 1704 if (data[IFLA_GTP_FD0]) {1705 int fd0 = nla_get_u32(data[IFLA_GTP_FD0]);1706 1707 if (fd0 >= 0) {1708 sk0 = gtp_encap_enable_socket(fd0, UDP_ENCAP_GTP0, gtp);1709 if (IS_ERR(sk0))1710 return PTR_ERR(sk0);1711 }1712 }1713 1714 if (data[IFLA_GTP_FD1]) {1715 int fd1 = nla_get_u32(data[IFLA_GTP_FD1]);1716 1717 if (fd1 >= 0) {1718 sk1u = gtp_encap_enable_socket(fd1, UDP_ENCAP_GTP1U, gtp);1719 if (IS_ERR(sk1u)) {1720 gtp_encap_disable_sock(sk0);1721 return PTR_ERR(sk1u);1722 }1723 }1724 }1725 1726 gtp->sk0 = sk0;1727 gtp->sk1u = sk1u;1728 1729 if (sk0 && sk1u &&1730 sk0->sk_family != sk1u->sk_family) {1731 gtp_encap_disable_sock(sk0);1732 gtp_encap_disable_sock(sk1u);1733 return -EINVAL;1734 }1735 1736 return 0;1737}1738 1739static struct gtp_dev *gtp_find_dev(struct net *src_net, struct nlattr *nla[])1740{1741 struct gtp_dev *gtp = NULL;1742 struct net_device *dev;1743 struct net *net;1744 1745 /* Examine the link attributes and figure out which network namespace1746 * we are talking about.1747 */1748 if (nla[GTPA_NET_NS_FD])1749 net = get_net_ns_by_fd(nla_get_u32(nla[GTPA_NET_NS_FD]));1750 else1751 net = get_net(src_net);1752 1753 if (IS_ERR(net))1754 return NULL;1755 1756 /* Check if there's an existing gtpX device to configure */1757 dev = dev_get_by_index_rcu(net, nla_get_u32(nla[GTPA_LINK]));1758 if (dev && dev->netdev_ops == >p_netdev_ops)1759 gtp = netdev_priv(dev);1760 1761 put_net(net);1762 return gtp;1763}1764 1765static void gtp_pdp_fill(struct pdp_ctx *pctx, struct genl_info *info)1766{1767 pctx->gtp_version = nla_get_u32(info->attrs[GTPA_VERSION]);1768 1769 switch (pctx->gtp_version) {1770 case GTP_V0:1771 /* According to TS 09.60, sections 7.5.1 and 7.5.2, the flow1772 * label needs to be the same for uplink and downlink packets,1773 * so let's annotate this.1774 */1775 pctx->u.v0.tid = nla_get_u64(info->attrs[GTPA_TID]);1776 pctx->u.v0.flow = nla_get_u16(info->attrs[GTPA_FLOW]);1777 break;1778 case GTP_V1:1779 pctx->u.v1.i_tei = nla_get_u32(info->attrs[GTPA_I_TEI]);1780 pctx->u.v1.o_tei = nla_get_u32(info->attrs[GTPA_O_TEI]);1781 break;1782 default:1783 break;1784 }1785}1786 1787static void ip_pdp_peer_fill(struct pdp_ctx *pctx, struct genl_info *info)1788{1789 if (info->attrs[GTPA_PEER_ADDRESS]) {1790 pctx->peer.addr.s_addr =1791 nla_get_be32(info->attrs[GTPA_PEER_ADDRESS]);1792 } else if (info->attrs[GTPA_PEER_ADDR6]) {1793 pctx->peer.addr6 = nla_get_in6_addr(info->attrs[GTPA_PEER_ADDR6]);1794 }1795}1796 1797static void ipv4_pdp_fill(struct pdp_ctx *pctx, struct genl_info *info)1798{1799 ip_pdp_peer_fill(pctx, info);1800 pctx->ms.addr.s_addr =1801 nla_get_be32(info->attrs[GTPA_MS_ADDRESS]);1802 gtp_pdp_fill(pctx, info);1803}1804 1805static bool ipv6_pdp_fill(struct pdp_ctx *pctx, struct genl_info *info)1806{1807 ip_pdp_peer_fill(pctx, info);1808 pctx->ms.addr6 = nla_get_in6_addr(info->attrs[GTPA_MS_ADDR6]);1809 if (pctx->ms.addr6.s6_addr32[2] ||1810 pctx->ms.addr6.s6_addr32[3])1811 return false;1812 1813 gtp_pdp_fill(pctx, info);1814 1815 return true;1816}1817 1818static struct pdp_ctx *gtp_pdp_add(struct gtp_dev *gtp, struct sock *sk,1819 struct genl_info *info)1820{1821 struct pdp_ctx *pctx, *pctx_tid = NULL;1822 struct net_device *dev = gtp->dev;1823 u32 hash_ms, hash_tid = 0;1824 struct in6_addr ms_addr6;1825 unsigned int version;1826 bool found = false;1827 __be32 ms_addr;1828 int family;1829 1830 version = nla_get_u32(info->attrs[GTPA_VERSION]);1831 1832 if (info->attrs[GTPA_FAMILY])1833 family = nla_get_u8(info->attrs[GTPA_FAMILY]);1834 else1835 family = AF_INET;1836 1837#if !IS_ENABLED(CONFIG_IPV6)1838 if (family == AF_INET6)1839 return ERR_PTR(-EAFNOSUPPORT);1840#endif1841 if (!info->attrs[GTPA_PEER_ADDRESS] &&1842 !info->attrs[GTPA_PEER_ADDR6])1843 return ERR_PTR(-EINVAL);1844 1845 if ((info->attrs[GTPA_PEER_ADDRESS] &&1846 sk->sk_family == AF_INET6) ||1847 (info->attrs[GTPA_PEER_ADDR6] &&1848 sk->sk_family == AF_INET))1849 return ERR_PTR(-EAFNOSUPPORT);1850 1851 switch (family) {1852 case AF_INET:1853 if (!info->attrs[GTPA_MS_ADDRESS] ||1854 info->attrs[GTPA_MS_ADDR6])1855 return ERR_PTR(-EINVAL);1856 1857 ms_addr = nla_get_be32(info->attrs[GTPA_MS_ADDRESS]);1858 hash_ms = ipv4_hashfn(ms_addr) % gtp->hash_size;1859 pctx = ipv4_pdp_find(gtp, ms_addr);1860 break;1861 case AF_INET6:1862 if (!info->attrs[GTPA_MS_ADDR6] ||1863 info->attrs[GTPA_MS_ADDRESS])1864 return ERR_PTR(-EINVAL);1865 1866 ms_addr6 = nla_get_in6_addr(info->attrs[GTPA_MS_ADDR6]);1867 hash_ms = ipv6_hashfn(&ms_addr6) % gtp->hash_size;1868 pctx = ipv6_pdp_find(gtp, &ms_addr6);1869 break;1870 default:1871 return ERR_PTR(-EAFNOSUPPORT);1872 }1873 if (pctx)1874 found = true;1875 if (version == GTP_V0)1876 pctx_tid = gtp0_pdp_find(gtp,1877 nla_get_u64(info->attrs[GTPA_TID]),1878 family);1879 else if (version == GTP_V1)1880 pctx_tid = gtp1_pdp_find(gtp,1881 nla_get_u32(info->attrs[GTPA_I_TEI]),1882 family);1883 if (pctx_tid)1884 found = true;1885 1886 if (found) {1887 if (info->nlhdr->nlmsg_flags & NLM_F_EXCL)1888 return ERR_PTR(-EEXIST);1889 if (info->nlhdr->nlmsg_flags & NLM_F_REPLACE)1890 return ERR_PTR(-EOPNOTSUPP);1891 1892 if (pctx && pctx_tid)1893 return ERR_PTR(-EEXIST);1894 if (!pctx)1895 pctx = pctx_tid;1896 1897 switch (pctx->af) {1898 case AF_INET:1899 ipv4_pdp_fill(pctx, info);1900 break;1901 case AF_INET6:1902 if (!ipv6_pdp_fill(pctx, info))1903 return ERR_PTR(-EADDRNOTAVAIL);1904 break;1905 }1906 1907 if (pctx->gtp_version == GTP_V0)1908 netdev_dbg(dev, "GTPv0-U: update tunnel id = %llx (pdp %p)\n",1909 pctx->u.v0.tid, pctx);1910 else if (pctx->gtp_version == GTP_V1)1911 netdev_dbg(dev, "GTPv1-U: update tunnel id = %x/%x (pdp %p)\n",1912 pctx->u.v1.i_tei, pctx->u.v1.o_tei, pctx);1913 1914 return pctx;1915 1916 }1917 1918 pctx = kmalloc(sizeof(*pctx), GFP_ATOMIC);1919 if (pctx == NULL)1920 return ERR_PTR(-ENOMEM);1921 1922 sock_hold(sk);1923 pctx->sk = sk;1924 pctx->dev = gtp->dev;1925 pctx->af = family;1926 1927 switch (pctx->af) {1928 case AF_INET:1929 if (!info->attrs[GTPA_MS_ADDRESS]) {1930 sock_put(sk);1931 kfree(pctx);1932 return ERR_PTR(-EINVAL);1933 }1934 1935 ipv4_pdp_fill(pctx, info);1936 break;1937 case AF_INET6:1938 if (!info->attrs[GTPA_MS_ADDR6]) {1939 sock_put(sk);1940 kfree(pctx);1941 return ERR_PTR(-EINVAL);1942 }1943 1944 if (!ipv6_pdp_fill(pctx, info)) {1945 sock_put(sk);1946 kfree(pctx);1947 return ERR_PTR(-EADDRNOTAVAIL);1948 }1949 break;1950 }1951 atomic_set(&pctx->tx_seq, 0);1952 1953 switch (pctx->gtp_version) {1954 case GTP_V0:1955 /* TS 09.60: "The flow label identifies unambiguously a GTP1956 * flow.". We use the tid for this instead, I cannot find a1957 * situation in which this doesn't unambiguosly identify the1958 * PDP context.1959 */1960 hash_tid = gtp0_hashfn(pctx->u.v0.tid) % gtp->hash_size;1961 break;1962 case GTP_V1:1963 hash_tid = gtp1u_hashfn(pctx->u.v1.i_tei) % gtp->hash_size;1964 break;1965 }1966 1967 hlist_add_head_rcu(&pctx->hlist_addr, >p->addr_hash[hash_ms]);1968 hlist_add_head_rcu(&pctx->hlist_tid, >p->tid_hash[hash_tid]);1969 1970 switch (pctx->gtp_version) {1971 case GTP_V0:1972 netdev_dbg(dev, "GTPv0-U: new PDP ctx id=%llx ssgn=%pI4 ms=%pI4 (pdp=%p)\n",1973 pctx->u.v0.tid, &pctx->peer.addr,1974 &pctx->ms.addr, pctx);1975 break;1976 case GTP_V1:1977 netdev_dbg(dev, "GTPv1-U: new PDP ctx id=%x/%x ssgn=%pI4 ms=%pI4 (pdp=%p)\n",1978 pctx->u.v1.i_tei, pctx->u.v1.o_tei,1979 &pctx->peer.addr, &pctx->ms.addr, pctx);1980 break;1981 }1982 1983 return pctx;1984}1985 1986static void pdp_context_free(struct rcu_head *head)1987{1988 struct pdp_ctx *pctx = container_of(head, struct pdp_ctx, rcu_head);1989 1990 sock_put(pctx->sk);1991 kfree(pctx);1992}1993 1994static void pdp_context_delete(struct pdp_ctx *pctx)1995{1996 hlist_del_rcu(&pctx->hlist_tid);1997 hlist_del_rcu(&pctx->hlist_addr);1998 call_rcu(&pctx->rcu_head, pdp_context_free);1999}2000 2001static int gtp_tunnel_notify(struct pdp_ctx *pctx, u8 cmd, gfp_t allocation);2002 2003static int gtp_genl_new_pdp(struct sk_buff *skb, struct genl_info *info)2004{2005 unsigned int version;2006 struct pdp_ctx *pctx;2007 struct gtp_dev *gtp;2008 struct sock *sk;2009 int err;2010 2011 if (!info->attrs[GTPA_VERSION] ||2012 !info->attrs[GTPA_LINK])2013 return -EINVAL;2014 2015 version = nla_get_u32(info->attrs[GTPA_VERSION]);2016 2017 switch (version) {2018 case GTP_V0:2019 if (!info->attrs[GTPA_TID] ||2020 !info->attrs[GTPA_FLOW])2021 return -EINVAL;2022 break;2023 case GTP_V1:2024 if (!info->attrs[GTPA_I_TEI] ||2025 !info->attrs[GTPA_O_TEI])2026 return -EINVAL;2027 break;2028 2029 default:2030 return -EINVAL;2031 }2032 2033 rtnl_lock();2034 2035 gtp = gtp_find_dev(sock_net(skb->sk), info->attrs);2036 if (!gtp) {2037 err = -ENODEV;2038 goto out_unlock;2039 }2040 2041 if (version == GTP_V0)2042 sk = gtp->sk0;2043 else if (version == GTP_V1)2044 sk = gtp->sk1u;2045 else2046 sk = NULL;2047 2048 if (!sk) {2049 err = -ENODEV;2050 goto out_unlock;2051 }2052 2053 pctx = gtp_pdp_add(gtp, sk, info);2054 if (IS_ERR(pctx)) {2055 err = PTR_ERR(pctx);2056 } else {2057 gtp_tunnel_notify(pctx, GTP_CMD_NEWPDP, GFP_KERNEL);2058 err = 0;2059 }2060 2061out_unlock:2062 rtnl_unlock();2063 return err;2064}2065 2066static struct pdp_ctx *gtp_find_pdp_by_link(struct net *net,2067 struct nlattr *nla[])2068{2069 struct gtp_dev *gtp;2070 int family;2071 2072 if (nla[GTPA_FAMILY])2073 family = nla_get_u8(nla[GTPA_FAMILY]);2074 else2075 family = AF_INET;2076 2077 gtp = gtp_find_dev(net, nla);2078 if (!gtp)2079 return ERR_PTR(-ENODEV);2080 2081 if (nla[GTPA_MS_ADDRESS]) {2082 __be32 ip = nla_get_be32(nla[GTPA_MS_ADDRESS]);2083 2084 if (family != AF_INET)2085 return ERR_PTR(-EINVAL);2086 2087 return ipv4_pdp_find(gtp, ip);2088 } else if (nla[GTPA_MS_ADDR6]) {2089 struct in6_addr addr = nla_get_in6_addr(nla[GTPA_MS_ADDR6]);2090 2091 if (family != AF_INET6)2092 return ERR_PTR(-EINVAL);2093 2094 if (addr.s6_addr32[2] ||2095 addr.s6_addr32[3])2096 return ERR_PTR(-EADDRNOTAVAIL);2097 2098 return ipv6_pdp_find(gtp, &addr);2099 } else if (nla[GTPA_VERSION]) {2100 u32 gtp_version = nla_get_u32(nla[GTPA_VERSION]);2101 2102 if (gtp_version == GTP_V0 && nla[GTPA_TID]) {2103 return gtp0_pdp_find(gtp, nla_get_u64(nla[GTPA_TID]),2104 family);2105 } else if (gtp_version == GTP_V1 && nla[GTPA_I_TEI]) {2106 return gtp1_pdp_find(gtp, nla_get_u32(nla[GTPA_I_TEI]),2107 family);2108 }2109 }2110 2111 return ERR_PTR(-EINVAL);2112}2113 2114static struct pdp_ctx *gtp_find_pdp(struct net *net, struct nlattr *nla[])2115{2116 struct pdp_ctx *pctx;2117 2118 if (nla[GTPA_LINK])2119 pctx = gtp_find_pdp_by_link(net, nla);2120 else2121 pctx = ERR_PTR(-EINVAL);2122 2123 if (!pctx)2124 pctx = ERR_PTR(-ENOENT);2125 2126 return pctx;2127}2128 2129static int gtp_genl_del_pdp(struct sk_buff *skb, struct genl_info *info)2130{2131 struct pdp_ctx *pctx;2132 int err = 0;2133 2134 if (!info->attrs[GTPA_VERSION])2135 return -EINVAL;2136 2137 rcu_read_lock();2138 2139 pctx = gtp_find_pdp(sock_net(skb->sk), info->attrs);2140 if (IS_ERR(pctx)) {2141 err = PTR_ERR(pctx);2142 goto out_unlock;2143 }2144 2145 if (pctx->gtp_version == GTP_V0)2146 netdev_dbg(pctx->dev, "GTPv0-U: deleting tunnel id = %llx (pdp %p)\n",2147 pctx->u.v0.tid, pctx);2148 else if (pctx->gtp_version == GTP_V1)2149 netdev_dbg(pctx->dev, "GTPv1-U: deleting tunnel id = %x/%x (pdp %p)\n",2150 pctx->u.v1.i_tei, pctx->u.v1.o_tei, pctx);2151 2152 gtp_tunnel_notify(pctx, GTP_CMD_DELPDP, GFP_ATOMIC);2153 pdp_context_delete(pctx);2154 2155out_unlock:2156 rcu_read_unlock();2157 return err;2158}2159 2160static int gtp_genl_fill_info(struct sk_buff *skb, u32 snd_portid, u32 snd_seq,2161 int flags, u32 type, struct pdp_ctx *pctx)2162{2163 void *genlh;2164 2165 genlh = genlmsg_put(skb, snd_portid, snd_seq, >p_genl_family, flags,2166 type);2167 if (genlh == NULL)2168 goto nlmsg_failure;2169 2170 if (nla_put_u32(skb, GTPA_VERSION, pctx->gtp_version) ||2171 nla_put_u32(skb, GTPA_LINK, pctx->dev->ifindex) ||2172 nla_put_u8(skb, GTPA_FAMILY, pctx->af))2173 goto nla_put_failure;2174 2175 switch (pctx->af) {2176 case AF_INET:2177 if (nla_put_be32(skb, GTPA_MS_ADDRESS, pctx->ms.addr.s_addr))2178 goto nla_put_failure;2179 break;2180 case AF_INET6:2181 if (nla_put_in6_addr(skb, GTPA_MS_ADDR6, &pctx->ms.addr6))2182 goto nla_put_failure;2183 break;2184 }2185 2186 switch (pctx->sk->sk_family) {2187 case AF_INET:2188 if (nla_put_be32(skb, GTPA_PEER_ADDRESS, pctx->peer.addr.s_addr))2189 goto nla_put_failure;2190 break;2191 case AF_INET6:2192 if (nla_put_in6_addr(skb, GTPA_PEER_ADDR6, &pctx->peer.addr6))2193 goto nla_put_failure;2194 break;2195 }2196 2197 switch (pctx->gtp_version) {2198 case GTP_V0:2199 if (nla_put_u64_64bit(skb, GTPA_TID, pctx->u.v0.tid, GTPA_PAD) ||2200 nla_put_u16(skb, GTPA_FLOW, pctx->u.v0.flow))2201 goto nla_put_failure;2202 break;2203 case GTP_V1:2204 if (nla_put_u32(skb, GTPA_I_TEI, pctx->u.v1.i_tei) ||2205 nla_put_u32(skb, GTPA_O_TEI, pctx->u.v1.o_tei))2206 goto nla_put_failure;2207 break;2208 }2209 genlmsg_end(skb, genlh);2210 return 0;2211 2212nlmsg_failure:2213nla_put_failure:2214 genlmsg_cancel(skb, genlh);2215 return -EMSGSIZE;2216}2217 2218static int gtp_tunnel_notify(struct pdp_ctx *pctx, u8 cmd, gfp_t allocation)2219{2220 struct sk_buff *msg;2221 int ret;2222 2223 msg = nlmsg_new(NLMSG_DEFAULT_SIZE, allocation);2224 if (!msg)2225 return -ENOMEM;2226 2227 ret = gtp_genl_fill_info(msg, 0, 0, 0, cmd, pctx);2228 if (ret < 0) {2229 nlmsg_free(msg);2230 return ret;2231 }2232 2233 ret = genlmsg_multicast_netns(>p_genl_family, dev_net(pctx->dev), msg,2234 0, GTP_GENL_MCGRP, GFP_ATOMIC);2235 return ret;2236}2237 2238static int gtp_genl_get_pdp(struct sk_buff *skb, struct genl_info *info)2239{2240 struct pdp_ctx *pctx = NULL;2241 struct sk_buff *skb2;2242 int err;2243 2244 if (!info->attrs[GTPA_VERSION])2245 return -EINVAL;2246 2247 rcu_read_lock();2248 2249 pctx = gtp_find_pdp(sock_net(skb->sk), info->attrs);2250 if (IS_ERR(pctx)) {2251 err = PTR_ERR(pctx);2252 goto err_unlock;2253 }2254 2255 skb2 = genlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC);2256 if (skb2 == NULL) {2257 err = -ENOMEM;2258 goto err_unlock;2259 }2260 2261 err = gtp_genl_fill_info(skb2, NETLINK_CB(skb).portid, info->snd_seq,2262 0, info->nlhdr->nlmsg_type, pctx);2263 if (err < 0)2264 goto err_unlock_free;2265 2266 rcu_read_unlock();2267 return genlmsg_unicast(genl_info_net(info), skb2, info->snd_portid);2268 2269err_unlock_free:2270 kfree_skb(skb2);2271err_unlock:2272 rcu_read_unlock();2273 return err;2274}2275 2276static int gtp_genl_dump_pdp(struct sk_buff *skb,2277 struct netlink_callback *cb)2278{2279 struct gtp_dev *last_gtp = (struct gtp_dev *)cb->args[2], *gtp;2280 int i, j, bucket = cb->args[0], skip = cb->args[1];2281 struct net *net = sock_net(skb->sk);2282 struct pdp_ctx *pctx;2283 struct gtp_net *gn;2284 2285 gn = net_generic(net, gtp_net_id);2286 2287 if (cb->args[4])2288 return 0;2289 2290 rcu_read_lock();2291 list_for_each_entry_rcu(gtp, &gn->gtp_dev_list, list) {2292 if (last_gtp && last_gtp != gtp)2293 continue;2294 else2295 last_gtp = NULL;2296 2297 for (i = bucket; i < gtp->hash_size; i++) {2298 j = 0;2299 hlist_for_each_entry_rcu(pctx, >p->tid_hash[i],2300 hlist_tid) {2301 if (j >= skip &&2302 gtp_genl_fill_info(skb,2303 NETLINK_CB(cb->skb).portid,2304 cb->nlh->nlmsg_seq,2305 NLM_F_MULTI,2306 cb->nlh->nlmsg_type, pctx)) {2307 cb->args[0] = i;2308 cb->args[1] = j;2309 cb->args[2] = (unsigned long)gtp;2310 goto out;2311 }2312 j++;2313 }2314 skip = 0;2315 }2316 bucket = 0;2317 }2318 cb->args[4] = 1;2319out:2320 rcu_read_unlock();2321 return skb->len;2322}2323 2324static int gtp_genl_send_echo_req(struct sk_buff *skb, struct genl_info *info)2325{2326 struct sk_buff *skb_to_send;2327 __be32 src_ip, dst_ip;2328 unsigned int version;2329 struct gtp_dev *gtp;2330 struct flowi4 fl4;2331 struct rtable *rt;2332 struct sock *sk;2333 __be16 port;2334 int len;2335 2336 if (!info->attrs[GTPA_VERSION] ||2337 !info->attrs[GTPA_LINK] ||2338 !info->attrs[GTPA_PEER_ADDRESS] ||2339 !info->attrs[GTPA_MS_ADDRESS])2340 return -EINVAL;2341 2342 version = nla_get_u32(info->attrs[GTPA_VERSION]);2343 dst_ip = nla_get_be32(info->attrs[GTPA_PEER_ADDRESS]);2344 src_ip = nla_get_be32(info->attrs[GTPA_MS_ADDRESS]);2345 2346 gtp = gtp_find_dev(sock_net(skb->sk), info->attrs);2347 if (!gtp)2348 return -ENODEV;2349 2350 if (!gtp->sk_created)2351 return -EOPNOTSUPP;2352 if (!(gtp->dev->flags & IFF_UP))2353 return -ENETDOWN;2354 2355 if (version == GTP_V0) {2356 struct gtp0_header *gtp0_h;2357 2358 len = LL_RESERVED_SPACE(gtp->dev) + sizeof(struct gtp0_header) +2359 sizeof(struct iphdr) + sizeof(struct udphdr);2360 2361 skb_to_send = netdev_alloc_skb_ip_align(gtp->dev, len);2362 if (!skb_to_send)2363 return -ENOMEM;2364 2365 sk = gtp->sk0;2366 port = htons(GTP0_PORT);2367 2368 gtp0_h = skb_push(skb_to_send, sizeof(struct gtp0_header));2369 memset(gtp0_h, 0, sizeof(struct gtp0_header));2370 gtp0_build_echo_msg(gtp0_h, GTP_ECHO_REQ);2371 } else if (version == GTP_V1) {2372 struct gtp1_header_long *gtp1u_h;2373 2374 len = LL_RESERVED_SPACE(gtp->dev) +2375 sizeof(struct gtp1_header_long) +2376 sizeof(struct iphdr) + sizeof(struct udphdr);2377 2378 skb_to_send = netdev_alloc_skb_ip_align(gtp->dev, len);2379 if (!skb_to_send)2380 return -ENOMEM;2381 2382 sk = gtp->sk1u;2383 port = htons(GTP1U_PORT);2384 2385 gtp1u_h = skb_push(skb_to_send,2386 sizeof(struct gtp1_header_long));2387 memset(gtp1u_h, 0, sizeof(struct gtp1_header_long));2388 gtp1u_build_echo_msg(gtp1u_h, GTP_ECHO_REQ);2389 } else {2390 return -ENODEV;2391 }2392 2393 rt = ip4_route_output_gtp(&fl4, sk, dst_ip, src_ip);2394 if (IS_ERR(rt)) {2395 netdev_dbg(gtp->dev, "no route for echo request to %pI4\n",2396 &dst_ip);2397 kfree_skb(skb_to_send);2398 return -ENODEV;2399 }2400 2401 udp_tunnel_xmit_skb(rt, sk, skb_to_send,2402 fl4.saddr, fl4.daddr,2403 fl4.flowi4_tos,2404 ip4_dst_hoplimit(&rt->dst),2405 0,2406 port, port,2407 !net_eq(sock_net(sk),2408 dev_net(gtp->dev)),2409 false);2410 return 0;2411}2412 2413static const struct nla_policy gtp_genl_policy[GTPA_MAX + 1] = {2414 [GTPA_LINK] = { .type = NLA_U32, },2415 [GTPA_VERSION] = { .type = NLA_U32, },2416 [GTPA_TID] = { .type = NLA_U64, },2417 [GTPA_PEER_ADDRESS] = { .type = NLA_U32, },2418 [GTPA_MS_ADDRESS] = { .type = NLA_U32, },2419 [GTPA_FLOW] = { .type = NLA_U16, },2420 [GTPA_NET_NS_FD] = { .type = NLA_U32, },2421 [GTPA_I_TEI] = { .type = NLA_U32, },2422 [GTPA_O_TEI] = { .type = NLA_U32, },2423 [GTPA_PEER_ADDR6] = { .len = sizeof(struct in6_addr), },2424 [GTPA_MS_ADDR6] = { .len = sizeof(struct in6_addr), },2425 [GTPA_FAMILY] = { .type = NLA_U8, },2426};2427 2428static const struct genl_small_ops gtp_genl_ops[] = {2429 {2430 .cmd = GTP_CMD_NEWPDP,2431 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,2432 .doit = gtp_genl_new_pdp,2433 .flags = GENL_ADMIN_PERM,2434 },2435 {2436 .cmd = GTP_CMD_DELPDP,2437 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,2438 .doit = gtp_genl_del_pdp,2439 .flags = GENL_ADMIN_PERM,2440 },2441 {2442 .cmd = GTP_CMD_GETPDP,2443 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,2444 .doit = gtp_genl_get_pdp,2445 .dumpit = gtp_genl_dump_pdp,2446 .flags = GENL_ADMIN_PERM,2447 },2448 {2449 .cmd = GTP_CMD_ECHOREQ,2450 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP,2451 .doit = gtp_genl_send_echo_req,2452 .flags = GENL_ADMIN_PERM,2453 },2454};2455 2456static struct genl_family gtp_genl_family __ro_after_init = {2457 .name = "gtp",2458 .version = 0,2459 .hdrsize = 0,2460 .maxattr = GTPA_MAX,2461 .policy = gtp_genl_policy,2462 .netnsok = true,2463 .module = THIS_MODULE,2464 .small_ops = gtp_genl_ops,2465 .n_small_ops = ARRAY_SIZE(gtp_genl_ops),2466 .resv_start_op = GTP_CMD_ECHOREQ + 1,2467 .mcgrps = gtp_genl_mcgrps,2468 .n_mcgrps = ARRAY_SIZE(gtp_genl_mcgrps),2469};2470 2471static int __net_init gtp_net_init(struct net *net)2472{2473 struct gtp_net *gn = net_generic(net, gtp_net_id);2474 2475 INIT_LIST_HEAD(&gn->gtp_dev_list);2476 return 0;2477}2478 2479static void __net_exit gtp_net_exit_batch_rtnl(struct list_head *net_list,2480 struct list_head *dev_to_kill)2481{2482 struct net *net;2483 2484 list_for_each_entry(net, net_list, exit_list) {2485 struct gtp_net *gn = net_generic(net, gtp_net_id);2486 struct gtp_dev *gtp;2487 2488 list_for_each_entry(gtp, &gn->gtp_dev_list, list)2489 gtp_dellink(gtp->dev, dev_to_kill);2490 }2491}2492 2493static struct pernet_operations gtp_net_ops = {2494 .init = gtp_net_init,2495 .exit_batch_rtnl = gtp_net_exit_batch_rtnl,2496 .id = >p_net_id,2497 .size = sizeof(struct gtp_net),2498};2499 2500static int __init gtp_init(void)2501{2502 int err;2503 2504 get_random_bytes(>p_h_initval, sizeof(gtp_h_initval));2505 2506 err = register_pernet_subsys(>p_net_ops);2507 if (err < 0)2508 goto error_out;2509 2510 err = rtnl_link_register(>p_link_ops);2511 if (err < 0)2512 goto unreg_pernet_subsys;2513 2514 err = genl_register_family(>p_genl_family);2515 if (err < 0)2516 goto unreg_rtnl_link;2517 2518 pr_info("GTP module loaded (pdp ctx size %zd bytes)\n",2519 sizeof(struct pdp_ctx));2520 return 0;2521 2522unreg_rtnl_link:2523 rtnl_link_unregister(>p_link_ops);2524unreg_pernet_subsys:2525 unregister_pernet_subsys(>p_net_ops);2526error_out:2527 pr_err("error loading GTP module loaded\n");2528 return err;2529}2530late_initcall(gtp_init);2531 2532static void __exit gtp_fini(void)2533{2534 genl_unregister_family(>p_genl_family);2535 rtnl_link_unregister(>p_link_ops);2536 unregister_pernet_subsys(>p_net_ops);2537 2538 pr_info("GTP module unloaded\n");2539}2540module_exit(gtp_fini);2541 2542MODULE_LICENSE("GPL");2543MODULE_AUTHOR("Harald Welte <hwelte@sysmocom.de>");2544MODULE_DESCRIPTION("Interface driver for GTP encapsulated traffic");2545MODULE_ALIAS_RTNL_LINK("gtp");2546MODULE_ALIAS_GENL_FAMILY("gtp");2547