drivers/net/gtp.c

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