drivers/net/gtp.c

   1 /* GTP according to GSM TS 09.60 / 3GPP TS 29.060
   2  *
   3  * (C) 2012-2014 by sysmocom - s.f.m.c. GmbH
   4  * (C) 2016 by Pablo Neira Ayuso <[email protected]>
   5  *
   6  * Author: Harald Welte <[email protected]>
   7  *         Pablo Neira Ayuso <[email protected]>
   8  *         Andreas Schultz <[email protected]>
   9  *
  10  * This program is free software; you can redistribute it and/or
  11  * modify it under the terms of the GNU General Public License
  12  * as published by the Free Software Foundation; either version
  13  * 2 of the License, or (at your option) any later version.
  14  */
  15
  16 #define pr_fmt(fmt) KBUILD_MODNAME ": " fmt
  17
  18 #include <linux/module.h>
  19 #include <linux/version.h>
  20 #include <linux/skbuff.h>
  21 #include <linux/udp.h>
  22 #include <linux/rculist.h>
  23 #include <linux/jhash.h>
  24 #include <linux/if_tunnel.h>
  25 #include <linux/net.h>
  26 #include <linux/file.h>
  27 #include <linux/gtp.h>
  28
  29 #include <net/net_namespace.h>
  30 #include <net/protocol.h>
  31 #include <net/ip.h>
  32 #include <net/udp.h>
  33 #include <net/udp_tunnel.h>
  34 #include <net/icmp.h>
  35 #include <net/xfrm.h>
  36 #include <net/genetlink.h>
  37 #include <net/netns/generic.h>
  38 #include <net/gtp.h>
  39
  40 /* An active session for the subscriber. */
  41 struct pdp_ctx {
  42         struct hlist_node       hlist_tid;
  43         struct hlist_node       hlist_addr;
  44
  45         union {
  46                 u64             tid;
  47                 struct {
  48                         u64     tid;
  49                         u16     flow;
  50                 } v0;
  51                 struct {
  52                         u32     i_tei;
  53                         u32     o_tei;
  54                 } v1;
  55         } u;
  56         u8                      gtp_version;
  57         u16                     af;
  58
  59         struct in_addr          ms_addr_ip4;
  60         struct in_addr          sgsn_addr_ip4;
  61
  62         atomic_t                tx_seq;
  63         struct rcu_head         rcu_head;
  64 };
  65
  66 /* One instance of the GTP device. */
  67 struct gtp_dev {
  68         struct list_head        list;
  69
  70         struct socket           *sock0;
  71         struct socket           *sock1u;
  72
  73         struct net              *net;
  74         struct net_device       *dev;
  75
  76         unsigned int            hash_size;
  77         struct hlist_head       *tid_hash;
  78         struct hlist_head       *addr_hash;
  79 };
  80
  81 static int gtp_net_id __read_mostly;
  82
  83 struct gtp_net {
  84         struct list_head gtp_dev_list;
  85 };
  86
  87 static u32 gtp_h_initval;
  88
  89 static inline u32 gtp0_hashfn(u64 tid)
  90 {
  91         u32 *tid32 = (u32 *) &tid;
  92         return jhash_2words(tid32[0], tid32[1], gtp_h_initval);
  93 }
  94
  95 static inline u32 gtp1u_hashfn(u32 tid)
  96 {
  97         return jhash_1word(tid, gtp_h_initval);
  98 }
  99
 100 static inline u32 ipv4_hashfn(__be32 ip)
 101 {
 102         return jhash_1word((__force u32)ip, gtp_h_initval);
 103 }
 104
 105 /* Resolve a PDP context structure based on the 64bit TID. */
 106 static struct pdp_ctx *gtp0_pdp_find(struct gtp_dev *gtp, u64 tid)
 107 {
 108         struct hlist_head *head;
 109         struct pdp_ctx *pdp;
 110
 111         head = &gtp->tid_hash[gtp0_hashfn(tid) % gtp->hash_size];
 112
 113         hlist_for_each_entry_rcu(pdp, head, hlist_tid) {
 114                 if (pdp->gtp_version == GTP_V0 &&
 115                     pdp->u.v0.tid == tid)
 116                         return pdp;
 117         }
 118         return NULL;
 119 }
 120
 121 /* Resolve a PDP context structure based on the 32bit TEI. */
 122 static struct pdp_ctx *gtp1_pdp_find(struct gtp_dev *gtp, u32 tid)
 123 {
 124         struct hlist_head *head;
 125         struct pdp_ctx *pdp;
 126
 127         head = &gtp->tid_hash[gtp1u_hashfn(tid) % gtp->hash_size];
 128
 129         hlist_for_each_entry_rcu(pdp, head, hlist_tid) {
 130                 if (pdp->gtp_version == GTP_V1 &&
 131                     pdp->u.v1.i_tei == tid)
 132                         return pdp;
 133         }
 134         return NULL;
 135 }
 136
 137 /* Resolve a PDP context based on IPv4 address of MS. */
 138 static struct pdp_ctx *ipv4_pdp_find(struct gtp_dev *gtp, __be32 ms_addr)
 139 {
 140         struct hlist_head *head;
 141         struct pdp_ctx *pdp;
 142
 143         head = &gtp->addr_hash[ipv4_hashfn(ms_addr) % gtp->hash_size];
 144
 145         hlist_for_each_entry_rcu(pdp, head, hlist_addr) {
 146                 if (pdp->af == AF_INET &&
 147                     pdp->ms_addr_ip4.s_addr == ms_addr)
 148                         return pdp;
 149         }
 150
 151         return NULL;
 152 }
 153
 154 static bool gtp_check_src_ms_ipv4(struct sk_buff *skb, struct pdp_ctx *pctx,
 155                                   unsigned int hdrlen)
 156 {
 157         struct iphdr *iph;
 158
 159         if (!pskb_may_pull(skb, hdrlen + sizeof(struct iphdr)))
 160                 return false;
 161
 162         iph = (struct iphdr *)(skb->data + hdrlen + sizeof(struct iphdr));
 163
 164         return iph->saddr != pctx->ms_addr_ip4.s_addr;
 165 }
 166
 167 /* Check if the inner IP source address in this packet is assigned to any
 168  * existing mobile subscriber.
 169  */
 170 static bool gtp_check_src_ms(struct sk_buff *skb, struct pdp_ctx *pctx,
 171                              unsigned int hdrlen)
 172 {
 173         switch (ntohs(skb->protocol)) {
 174         case ETH_P_IP:
 175                 return gtp_check_src_ms_ipv4(skb, pctx, hdrlen);
 176         }
 177         return false;
 178 }
 179
 180 /* 1 means pass up to the stack, -1 means drop and 0 means decapsulated. */
 181 static int gtp0_udp_encap_recv(struct gtp_dev *gtp, struct sk_buff *skb,
 182                                bool xnet)
 183 {
 184         unsigned int hdrlen = sizeof(struct udphdr) +
 185                               sizeof(struct gtp0_header);
 186         struct gtp0_header *gtp0;
 187         struct pdp_ctx *pctx;
 188         int ret = 0;
 189
 190         if (!pskb_may_pull(skb, hdrlen))
 191                 return -1;
 192
 193         gtp0 = (struct gtp0_header *)(skb->data + sizeof(struct udphdr));
 194
 195         if ((gtp0->flags >> 5) != GTP_V0)
 196                 return 1;
 197
 198         if (gtp0->type != GTP_TPDU)
 199                 return 1;
 200
 201         rcu_read_lock();
 202         pctx = gtp0_pdp_find(gtp, be64_to_cpu(gtp0->tid));
 203         if (!pctx) {
 204                 netdev_dbg(gtp->dev, "No PDP ctx to decap skb=%p\n", skb);
 205                 ret = -1;
 206                 goto out_rcu;
 207         }
 208
 209         if (!gtp_check_src_ms(skb, pctx, hdrlen)) {
 210                 netdev_dbg(gtp->dev, "No PDP ctx for this MS\n");
 211                 ret = -1;
 212                 goto out_rcu;
 213         }
 214         rcu_read_unlock();
 215
 216         /* Get rid of the GTP + UDP headers. */
 217         return iptunnel_pull_header(skb, hdrlen, skb->protocol, xnet);
 218 out_rcu:
 219         rcu_read_unlock();
 220         return ret;
 221 }
 222
 223 static int gtp1u_udp_encap_recv(struct gtp_dev *gtp, struct sk_buff *skb,
 224                                 bool xnet)
 225 {
 226         unsigned int hdrlen = sizeof(struct udphdr) +
 227                               sizeof(struct gtp1_header);
 228         struct gtp1_header *gtp1;
 229         struct pdp_ctx *pctx;
 230         int ret = 0;
 231
 232         if (!pskb_may_pull(skb, hdrlen))
 233                 return -1;
 234
 235         gtp1 = (struct gtp1_header *)(skb->data + sizeof(struct udphdr));
 236
 237         if ((gtp1->flags >> 5) != GTP_V1)
 238                 return 1;
 239
 240         if (gtp1->type != GTP_TPDU)
 241                 return 1;
 242
 243         /* From 29.060: "This field shall be present if and only if any one or
 244          * more of the S, PN and E flags are set.".
 245          *
 246          * If any of the bit is set, then the remaining ones also have to be
 247          * set.
 248          */
 249         if (gtp1->flags & GTP1_F_MASK)
 250                 hdrlen += 4;
 251
 252         /* Make sure the header is larger enough, including extensions. */
 253         if (!pskb_may_pull(skb, hdrlen))
 254                 return -1;
 255
 256         gtp1 = (struct gtp1_header *)(skb->data + sizeof(struct udphdr));
 257
 258         rcu_read_lock();
 259         pctx = gtp1_pdp_find(gtp, ntohl(gtp1->tid));
 260         if (!pctx) {
 261                 netdev_dbg(gtp->dev, "No PDP ctx to decap skb=%p\n", skb);
 262                 ret = -1;
 263                 goto out_rcu;
 264         }
 265
 266         if (!gtp_check_src_ms(skb, pctx, hdrlen)) {
 267                 netdev_dbg(gtp->dev, "No PDP ctx for this MS\n");
 268                 ret = -1;
 269                 goto out_rcu;
 270         }
 271         rcu_read_unlock();
 272
 273         /* Get rid of the GTP + UDP headers. */
 274         return iptunnel_pull_header(skb, hdrlen, skb->protocol, xnet);
 275 out_rcu:
 276         rcu_read_unlock();
 277         return ret;
 278 }
 279
 280 static void gtp_encap_disable(struct gtp_dev *gtp)
 281 {
 282         if (gtp->sock0 && gtp->sock0->sk) {
 283                 udp_sk(gtp->sock0->sk)->encap_type = 0;
 284                 rcu_assign_sk_user_data(gtp->sock0->sk, NULL);
 285         }
 286         if (gtp->sock1u && gtp->sock1u->sk) {
 287                 udp_sk(gtp->sock1u->sk)->encap_type = 0;
 288                 rcu_assign_sk_user_data(gtp->sock1u->sk, NULL);
 289         }
 290
 291         gtp->sock0 = NULL;
 292         gtp->sock1u = NULL;
 293 }
 294
 295 static void gtp_encap_destroy(struct sock *sk)
 296 {
 297         struct gtp_dev *gtp;
 298
 299         gtp = rcu_dereference_sk_user_data(sk);
 300         if (gtp)
 301                 gtp_encap_disable(gtp);
 302 }
 303
 304 /* UDP encapsulation receive handler. See net/ipv4/udp.c.
 305  * Return codes: 0: success, <0: error, >0: pass up to userspace UDP socket.
 306  */
 307 static int gtp_encap_recv(struct sock *sk, struct sk_buff *skb)
 308 {
 309         struct pcpu_sw_netstats *stats;
 310         struct gtp_dev *gtp;
 311         bool xnet;
 312         int ret;
 313
 314         gtp = rcu_dereference_sk_user_data(sk);
 315         if (!gtp)
 316                 return 1;
 317
 318         netdev_dbg(gtp->dev, "encap_recv sk=%p\n", sk);
 319
 320         xnet = !net_eq(gtp->net, dev_net(gtp->dev));
 321
 322         switch (udp_sk(sk)->encap_type) {
 323         case UDP_ENCAP_GTP0:
 324                 netdev_dbg(gtp->dev, "received GTP0 packet\n");
 325                 ret = gtp0_udp_encap_recv(gtp, skb, xnet);
 326                 break;
 327         case UDP_ENCAP_GTP1U:
 328                 netdev_dbg(gtp->dev, "received GTP1U packet\n");
 329                 ret = gtp1u_udp_encap_recv(gtp, skb, xnet);
 330                 break;
 331         default:
 332                 ret = -1; /* Shouldn't happen. */
 333         }
 334
 335         switch (ret) {
 336         case 1:
 337                 netdev_dbg(gtp->dev, "pass up to the process\n");
 338                 return 1;
 339         case 0:
 340                 netdev_dbg(gtp->dev, "forwarding packet from GGSN to uplink\n");
 341                 break;
 342         case -1:
 343                 netdev_dbg(gtp->dev, "GTP packet has been dropped\n");
 344                 kfree_skb(skb);
 345                 return 0;
 346         }
 347
 348         /* Now that the UDP and the GTP header have been removed, set up the
 349          * new network header. This is required by the upper layer to
 350          * calculate the transport header.
 351          */
 352         skb_reset_network_header(skb);
 353
 354         skb->dev = gtp->dev;
 355
 356         stats = this_cpu_ptr(gtp->dev->tstats);
 357         u64_stats_update_begin(&stats->syncp);
 358         stats->rx_packets++;
 359         stats->rx_bytes += skb->len;
 360         u64_stats_update_end(&stats->syncp);
 361
 362         netif_rx(skb);
 363
 364         return 0;
 365 }
 366
 367 static int gtp_dev_init(struct net_device *dev)
 368 {
 369         struct gtp_dev *gtp = netdev_priv(dev);
 370
 371         gtp->dev = dev;
 372
 373         dev->tstats = alloc_percpu(struct pcpu_sw_netstats);
 374         if (!dev->tstats)
 375                 return -ENOMEM;
 376
 377         return 0;
 378 }
 379
 380 static void gtp_dev_uninit(struct net_device *dev)
 381 {
 382         struct gtp_dev *gtp = netdev_priv(dev);
 383
 384         gtp_encap_disable(gtp);
 385         free_percpu(dev->tstats);
 386 }
 387
 388 static struct rtable *ip4_route_output_gtp(struct net *net, struct flowi4 *fl4,
 389                                            const struct sock *sk, __be32 daddr)
 390 {
 391         memset(fl4, 0, sizeof(*fl4));
 392         fl4->flowi4_oif         = sk->sk_bound_dev_if;
 393         fl4->daddr              = daddr;
 394         fl4->saddr              = inet_sk(sk)->inet_saddr;
 395         fl4->flowi4_tos         = RT_CONN_FLAGS(sk);
 396         fl4->flowi4_proto       = sk->sk_protocol;
 397
 398         return ip_route_output_key(net, fl4);
 399 }
 400
 401 static inline void gtp0_push_header(struct sk_buff *skb, struct pdp_ctx *pctx)
 402 {
 403         int payload_len = skb->len;
 404         struct gtp0_header *gtp0;
 405
 406         gtp0 = (struct gtp0_header *) skb_push(skb, sizeof(*gtp0));
 407
 408         gtp0->flags     = 0x1e; /* v0, GTP-non-prime. */
 409         gtp0->type      = GTP_TPDU;
 410         gtp0->length    = htons(payload_len);
 411         gtp0->seq       = htons((atomic_inc_return(&pctx->tx_seq) - 1) % 0xffff);
 412         gtp0->flow      = htons(pctx->u.v0.flow);
 413         gtp0->number    = 0xff;
 414         gtp0->spare[0]  = gtp0->spare[1] = gtp0->spare[2] = 0xff;
 415         gtp0->tid       = cpu_to_be64(pctx->u.v0.tid);
 416 }
 417
 418 static inline void gtp1_push_header(struct sk_buff *skb, struct pdp_ctx *pctx)
 419 {
 420         int payload_len = skb->len;
 421         struct gtp1_header *gtp1;
 422
 423         gtp1 = (struct gtp1_header *) skb_push(skb, sizeof(*gtp1));
 424
 425         /* Bits    8  7  6  5  4  3  2  1
 426          *        +--+--+--+--+--+--+--+--+
 427          *        |version |PT| 1| E| S|PN|
 428          *        +--+--+--+--+--+--+--+--+
 429          *          0  0  1  1  1  0  0  0
 430          */
 431         gtp1->flags     = 0x38; /* v1, GTP-non-prime. */
 432         gtp1->type      = GTP_TPDU;
 433         gtp1->length    = htons(payload_len);
 434         gtp1->tid       = htonl(pctx->u.v1.o_tei);
 435
 436         /* TODO: Suppport for extension header, sequence number and N-PDU.
 437          *       Update the length field if any of them is available.
 438          */
 439 }
 440
 441 struct gtp_pktinfo {
 442         struct sock             *sk;
 443         struct iphdr            *iph;
 444         struct flowi4           fl4;
 445         struct rtable           *rt;
 446         struct pdp_ctx          *pctx;
 447         struct net_device       *dev;
 448         __be16                  gtph_port;
 449 };
 450
 451 static void gtp_push_header(struct sk_buff *skb, struct gtp_pktinfo *pktinfo)
 452 {
 453         switch (pktinfo->pctx->gtp_version) {
 454         case GTP_V0:
 455                 pktinfo->gtph_port = htons(GTP0_PORT);
 456                 gtp0_push_header(skb, pktinfo->pctx);
 457                 break;
 458         case GTP_V1:
 459                 pktinfo->gtph_port = htons(GTP1U_PORT);
 460                 gtp1_push_header(skb, pktinfo->pctx);
 461                 break;
 462         }
 463 }
 464
 465 static inline void gtp_set_pktinfo_ipv4(struct gtp_pktinfo *pktinfo,
 466                                         struct sock *sk, struct iphdr *iph,
 467                                         struct pdp_ctx *pctx, struct rtable *rt,
 468                                         struct flowi4 *fl4,
 469                                         struct net_device *dev)
 470 {
 471         pktinfo->sk     = sk;
 472         pktinfo->iph    = iph;
 473         pktinfo->pctx   = pctx;
 474         pktinfo->rt     = rt;
 475         pktinfo->fl4    = *fl4;
 476         pktinfo->dev    = dev;
 477 }
 478
 479 static int gtp_build_skb_ip4(struct sk_buff *skb, struct net_device *dev,
 480                              struct gtp_pktinfo *pktinfo)
 481 {
 482         struct gtp_dev *gtp = netdev_priv(dev);
 483         struct pdp_ctx *pctx;
 484         struct rtable *rt;
 485         struct flowi4 fl4;
 486         struct iphdr *iph;
 487         struct sock *sk;
 488         __be16 df;
 489         int mtu;
 490
 491         /* Read the IP destination address and resolve the PDP context.
 492          * Prepend PDP header with TEI/TID from PDP ctx.
 493          */
 494         iph = ip_hdr(skb);
 495         pctx = ipv4_pdp_find(gtp, iph->daddr);
 496         if (!pctx) {
 497                 netdev_dbg(dev, "no PDP ctx found for %pI4, skip\n",
 498                            &iph->daddr);
 499                 return -ENOENT;
 500         }
 501         netdev_dbg(dev, "found PDP context %p\n", pctx);
 502
 503         switch (pctx->gtp_version) {
 504         case GTP_V0:
 505                 if (gtp->sock0)
 506                         sk = gtp->sock0->sk;
 507                 else
 508                         sk = NULL;
 509                 break;
 510         case GTP_V1:
 511                 if (gtp->sock1u)
 512                         sk = gtp->sock1u->sk;
 513                 else
 514                         sk = NULL;
 515                 break;
 516         default:
 517                 return -ENOENT;
 518         }
 519
 520         if (!sk) {
 521                 netdev_dbg(dev, "no userspace socket is available, skip\n");
 522                 return -ENOENT;
 523         }
 524
 525         rt = ip4_route_output_gtp(sock_net(sk), &fl4, gtp->sock0->sk,
 526                                   pctx->sgsn_addr_ip4.s_addr);
 527         if (IS_ERR(rt)) {
 528                 netdev_dbg(dev, "no route to SSGN %pI4\n",
 529                            &pctx->sgsn_addr_ip4.s_addr);
 530                 dev->stats.tx_carrier_errors++;
 531                 goto err;
 532         }
 533
 534         if (rt->dst.dev == dev) {
 535                 netdev_dbg(dev, "circular route to SSGN %pI4\n",
 536                            &pctx->sgsn_addr_ip4.s_addr);
 537                 dev->stats.collisions++;
 538                 goto err_rt;
 539         }
 540
 541         skb_dst_drop(skb);
 542
 543         /* This is similar to tnl_update_pmtu(). */
 544         df = iph->frag_off;
 545         if (df) {
 546                 mtu = dst_mtu(&rt->dst) - dev->hard_header_len -
 547                         sizeof(struct iphdr) - sizeof(struct udphdr);
 548                 switch (pctx->gtp_version) {
 549                 case GTP_V0:
 550                         mtu -= sizeof(struct gtp0_header);
 551                         break;
 552                 case GTP_V1:
 553                         mtu -= sizeof(struct gtp1_header);
 554                         break;
 555                 }
 556         } else {
 557                 mtu = dst_mtu(&rt->dst);
 558         }
 559
 560         rt->dst.ops->update_pmtu(&rt->dst, NULL, skb, mtu);
 561
 562         if (!skb_is_gso(skb) && (iph->frag_off & htons(IP_DF)) &&
 563             mtu < ntohs(iph->tot_len)) {
 564                 netdev_dbg(dev, "packet too big, fragmentation needed\n");
 565                 memset(IPCB(skb), 0, sizeof(*IPCB(skb)));
 566                 icmp_send(skb, ICMP_DEST_UNREACH, ICMP_FRAG_NEEDED,
 567                           htonl(mtu));
 568                 goto err_rt;
 569         }
 570
 571         gtp_set_pktinfo_ipv4(pktinfo, sk, iph, pctx, rt, &fl4, dev);
 572         gtp_push_header(skb, pktinfo);
 573
 574         return 0;
 575 err_rt:
 576         ip_rt_put(rt);
 577 err:
 578         return -EBADMSG;
 579 }
 580
 581 static netdev_tx_t gtp_dev_xmit(struct sk_buff *skb, struct net_device *dev)
 582 {
 583         unsigned int proto = ntohs(skb->protocol);
 584         struct gtp_pktinfo pktinfo;
 585         int err;
 586
 587         /* Ensure there is sufficient headroom. */
 588         if (skb_cow_head(skb, dev->needed_headroom))
 589                 goto tx_err;
 590
 591         skb_reset_inner_headers(skb);
 592
 593         /* PDP context lookups in gtp_build_skb_*() need rcu read-side lock. */
 594         rcu_read_lock();
 595         switch (proto) {
 596         case ETH_P_IP:
 597                 err = gtp_build_skb_ip4(skb, dev, &pktinfo);
 598                 break;
 599         default:
 600                 err = -EOPNOTSUPP;
 601                 break;
 602         }
 603         rcu_read_unlock();
 604
 605         if (err < 0)
 606                 goto tx_err;
 607
 608         switch (proto) {
 609         case ETH_P_IP:
 610                 netdev_dbg(pktinfo.dev, "gtp -> IP src: %pI4 dst: %pI4\n",
 611                            &pktinfo.iph->saddr, &pktinfo.iph->daddr);
 612                 udp_tunnel_xmit_skb(pktinfo.rt, pktinfo.sk, skb,
 613                                     pktinfo.fl4.saddr, pktinfo.fl4.daddr,
 614                                     pktinfo.iph->tos,
 615                                     ip4_dst_hoplimit(&pktinfo.rt->dst),
 616                                     htons(IP_DF),
 617                                     pktinfo.gtph_port, pktinfo.gtph_port,
 618                                     true, false);
 619                 break;
 620         }
 621
 622         return NETDEV_TX_OK;
 623 tx_err:
 624         dev->stats.tx_errors++;
 625         dev_kfree_skb(skb);
 626         return NETDEV_TX_OK;
 627 }
 628
 629 static const struct net_device_ops gtp_netdev_ops = {
 630         .ndo_init               = gtp_dev_init,
 631         .ndo_uninit             = gtp_dev_uninit,
 632         .ndo_start_xmit         = gtp_dev_xmit,
 633         .ndo_get_stats64        = ip_tunnel_get_stats64,
 634 };
 635
 636 static void gtp_link_setup(struct net_device *dev)
 637 {
 638         dev->netdev_ops         = &gtp_netdev_ops;
 639         dev->destructor         = free_netdev;
 640
 641         dev->hard_header_len = 0;
 642         dev->addr_len = 0;
 643
 644         /* Zero header length. */
 645         dev->type = ARPHRD_NONE;
 646         dev->flags = IFF_POINTOPOINT | IFF_NOARP | IFF_MULTICAST;
 647
 648         dev->priv_flags |= IFF_NO_QUEUE;
 649         dev->features   |= NETIF_F_LLTX;
 650         netif_keep_dst(dev);
 651
 652         /* Assume largest header, ie. GTPv0. */
 653         dev->needed_headroom    = LL_MAX_HEADER +
 654                                   sizeof(struct iphdr) +
 655                                   sizeof(struct udphdr) +
 656                                   sizeof(struct gtp0_header);
 657 }
 658
 659 static int gtp_hashtable_new(struct gtp_dev *gtp, int hsize);
 660 static void gtp_hashtable_free(struct gtp_dev *gtp);
 661 static int gtp_encap_enable(struct net_device *dev, struct gtp_dev *gtp,
 662                             int fd_gtp0, int fd_gtp1, struct net *src_net);
 663
 664 static int gtp_newlink(struct net *src_net, struct net_device *dev,
 665                         struct nlattr *tb[], struct nlattr *data[])
 666 {
 667         int hashsize, err, fd0, fd1;
 668         struct gtp_dev *gtp;
 669         struct gtp_net *gn;
 670
 671         if (!data[IFLA_GTP_FD0] || !data[IFLA_GTP_FD1])
 672                 return -EINVAL;
 673
 674         gtp = netdev_priv(dev);
 675
 676         fd0 = nla_get_u32(data[IFLA_GTP_FD0]);
 677         fd1 = nla_get_u32(data[IFLA_GTP_FD1]);
 678
 679         err = gtp_encap_enable(dev, gtp, fd0, fd1, src_net);
 680         if (err < 0)
 681                 goto out_err;
 682
 683         if (!data[IFLA_GTP_PDP_HASHSIZE])
 684                 hashsize = 1024;
 685         else
 686                 hashsize = nla_get_u32(data[IFLA_GTP_PDP_HASHSIZE]);
 687
 688         err = gtp_hashtable_new(gtp, hashsize);
 689         if (err < 0)
 690                 goto out_encap;
 691
 692         err = register_netdevice(dev);
 693         if (err < 0) {
 694                 netdev_dbg(dev, "failed to register new netdev %d\n", err);
 695                 goto out_hashtable;
 696         }
 697
 698         gn = net_generic(dev_net(dev), gtp_net_id);
 699         list_add_rcu(&gtp->list, &gn->gtp_dev_list);
 700
 701         netdev_dbg(dev, "registered new GTP interface\n");
 702
 703         return 0;
 704
 705 out_hashtable:
 706         gtp_hashtable_free(gtp);
 707 out_encap:
 708         gtp_encap_disable(gtp);
 709 out_err:
 710         return err;
 711 }
 712
 713 static void gtp_dellink(struct net_device *dev, struct list_head *head)
 714 {
 715         struct gtp_dev *gtp = netdev_priv(dev);
 716
 717         gtp_encap_disable(gtp);
 718         gtp_hashtable_free(gtp);
 719         list_del_rcu(&gtp->list);
 720         unregister_netdevice_queue(dev, head);
 721 }
 722
 723 static const struct nla_policy gtp_policy[IFLA_GTP_MAX + 1] = {
 724         [IFLA_GTP_FD0]                  = { .type = NLA_U32 },
 725         [IFLA_GTP_FD1]                  = { .type = NLA_U32 },
 726         [IFLA_GTP_PDP_HASHSIZE]         = { .type = NLA_U32 },
 727 };
 728
 729 static int gtp_validate(struct nlattr *tb[], struct nlattr *data[])
 730 {
 731         if (!data)
 732                 return -EINVAL;
 733
 734         return 0;
 735 }
 736
 737 static size_t gtp_get_size(const struct net_device *dev)
 738 {
 739         return nla_total_size(sizeof(__u32));   /* IFLA_GTP_PDP_HASHSIZE */
 740 }
 741
 742 static int gtp_fill_info(struct sk_buff *skb, const struct net_device *dev)
 743 {
 744         struct gtp_dev *gtp = netdev_priv(dev);
 745
 746         if (nla_put_u32(skb, IFLA_GTP_PDP_HASHSIZE, gtp->hash_size))
 747                 goto nla_put_failure;
 748
 749         return 0;
 750
 751 nla_put_failure:
 752         return -EMSGSIZE;
 753 }
 754
 755 static struct rtnl_link_ops gtp_link_ops __read_mostly = {
 756         .kind           = "gtp",
 757         .maxtype        = IFLA_GTP_MAX,
 758         .policy         = gtp_policy,
 759         .priv_size      = sizeof(struct gtp_dev),
 760         .setup          = gtp_link_setup,
 761         .validate       = gtp_validate,
 762         .newlink        = gtp_newlink,
 763         .dellink        = gtp_dellink,
 764         .get_size       = gtp_get_size,
 765         .fill_info      = gtp_fill_info,
 766 };
 767
 768 static struct net *gtp_genl_get_net(struct net *src_net, struct nlattr *tb[])
 769 {
 770         struct net *net;
 771
 772         /* Examine the link attributes and figure out which network namespace
 773          * we are talking about.
 774          */
 775         if (tb[GTPA_NET_NS_FD])
 776                 net = get_net_ns_by_fd(nla_get_u32(tb[GTPA_NET_NS_FD]));
 777         else
 778                 net = get_net(src_net);
 779
 780         return net;
 781 }
 782
 783 static int gtp_hashtable_new(struct gtp_dev *gtp, int hsize)
 784 {
 785         int i;
 786
 787         gtp->addr_hash = kmalloc(sizeof(struct hlist_head) * hsize, GFP_KERNEL);
 788         if (gtp->addr_hash == NULL)
 789                 return -ENOMEM;
 790
 791         gtp->tid_hash = kmalloc(sizeof(struct hlist_head) * hsize, GFP_KERNEL);
 792         if (gtp->tid_hash == NULL)
 793                 goto err1;
 794
 795         gtp->hash_size = hsize;
 796
 797         for (i = 0; i < hsize; i++) {
 798                 INIT_HLIST_HEAD(&gtp->addr_hash[i]);
 799                 INIT_HLIST_HEAD(&gtp->tid_hash[i]);
 800         }
 801         return 0;
 802 err1:
 803         kfree(gtp->addr_hash);
 804         return -ENOMEM;
 805 }
 806
 807 static void gtp_hashtable_free(struct gtp_dev *gtp)
 808 {
 809         struct pdp_ctx *pctx;
 810         int i;
 811
 812         for (i = 0; i < gtp->hash_size; i++) {
 813                 hlist_for_each_entry_rcu(pctx, &gtp->tid_hash[i], hlist_tid) {
 814                         hlist_del_rcu(&pctx->hlist_tid);
 815                         hlist_del_rcu(&pctx->hlist_addr);
 816                         kfree_rcu(pctx, rcu_head);
 817                 }
 818         }
 819         synchronize_rcu();
 820         kfree(gtp->addr_hash);
 821         kfree(gtp->tid_hash);
 822 }
 823
 824 static int gtp_encap_enable(struct net_device *dev, struct gtp_dev *gtp,
 825                             int fd_gtp0, int fd_gtp1, struct net *src_net)
 826 {
 827         struct udp_tunnel_sock_cfg tuncfg = {NULL};
 828         struct socket *sock0, *sock1u;
 829         int err;
 830
 831         netdev_dbg(dev, "enable gtp on %d, %d\n", fd_gtp0, fd_gtp1);
 832
 833         sock0 = sockfd_lookup(fd_gtp0, &err);
 834         if (sock0 == NULL) {
 835                 netdev_dbg(dev, "socket fd=%d not found (gtp0)\n", fd_gtp0);
 836                 return -ENOENT;
 837         }
 838
 839         if (sock0->sk->sk_protocol != IPPROTO_UDP) {
 840                 netdev_dbg(dev, "socket fd=%d not UDP\n", fd_gtp0);
 841                 err = -EINVAL;
 842                 goto err1;
 843         }
 844
 845         sock1u = sockfd_lookup(fd_gtp1, &err);
 846         if (sock1u == NULL) {
 847                 netdev_dbg(dev, "socket fd=%d not found (gtp1u)\n", fd_gtp1);
 848                 err = -ENOENT;
 849                 goto err1;
 850         }
 851
 852         if (sock1u->sk->sk_protocol != IPPROTO_UDP) {
 853                 netdev_dbg(dev, "socket fd=%d not UDP\n", fd_gtp1);
 854                 err = -EINVAL;
 855                 goto err2;
 856         }
 857
 858         netdev_dbg(dev, "enable gtp on %p, %p\n", sock0, sock1u);
 859
 860         gtp->sock0 = sock0;
 861         gtp->sock1u = sock1u;
 862         gtp->net = src_net;
 863
 864         tuncfg.sk_user_data = gtp;
 865         tuncfg.encap_rcv = gtp_encap_recv;
 866         tuncfg.encap_destroy = gtp_encap_destroy;
 867
 868         tuncfg.encap_type = UDP_ENCAP_GTP0;
 869         setup_udp_tunnel_sock(sock_net(gtp->sock0->sk), gtp->sock0, &tuncfg);
 870
 871         tuncfg.encap_type = UDP_ENCAP_GTP1U;
 872         setup_udp_tunnel_sock(sock_net(gtp->sock1u->sk), gtp->sock1u, &tuncfg);
 873
 874         err = 0;
 875 err2:
 876         sockfd_put(sock1u);
 877 err1:
 878         sockfd_put(sock0);
 879         return err;
 880 }
 881
 882 static struct net_device *gtp_find_dev(struct net *net, int ifindex)
 883 {
 884         struct gtp_net *gn = net_generic(net, gtp_net_id);
 885         struct gtp_dev *gtp;
 886
 887         list_for_each_entry_rcu(gtp, &gn->gtp_dev_list, list) {
 888                 if (ifindex == gtp->dev->ifindex)
 889                         return gtp->dev;
 890         }
 891         return NULL;
 892 }
 893
 894 static void ipv4_pdp_fill(struct pdp_ctx *pctx, struct genl_info *info)
 895 {
 896         pctx->gtp_version = nla_get_u32(info->attrs[GTPA_VERSION]);
 897         pctx->af = AF_INET;
 898         pctx->sgsn_addr_ip4.s_addr =
 899                 nla_get_be32(info->attrs[GTPA_SGSN_ADDRESS]);
 900         pctx->ms_addr_ip4.s_addr =
 901                 nla_get_be32(info->attrs[GTPA_MS_ADDRESS]);
 902
 903         switch (pctx->gtp_version) {
 904         case GTP_V0:
 905                 /* According to TS 09.60, sections 7.5.1 and 7.5.2, the flow
 906                  * label needs to be the same for uplink and downlink packets,
 907                  * so let's annotate this.
 908                  */
 909                 pctx->u.v0.tid = nla_get_u64(info->attrs[GTPA_TID]);
 910                 pctx->u.v0.flow = nla_get_u16(info->attrs[GTPA_FLOW]);
 911                 break;
 912         case GTP_V1:
 913                 pctx->u.v1.i_tei = nla_get_u32(info->attrs[GTPA_I_TEI]);
 914                 pctx->u.v1.o_tei = nla_get_u32(info->attrs[GTPA_O_TEI]);
 915                 break;
 916         default:
 917                 break;
 918         }
 919 }
 920
 921 static int ipv4_pdp_add(struct net_device *dev, struct genl_info *info)
 922 {
 923         struct gtp_dev *gtp = netdev_priv(dev);
 924         u32 hash_ms, hash_tid = 0;
 925         struct pdp_ctx *pctx;
 926         bool found = false;
 927         __be32 ms_addr;
 928
 929         ms_addr = nla_get_be32(info->attrs[GTPA_MS_ADDRESS]);
 930         hash_ms = ipv4_hashfn(ms_addr) % gtp->hash_size;
 931
 932         hlist_for_each_entry_rcu(pctx, &gtp->addr_hash[hash_ms], hlist_addr) {
 933                 if (pctx->ms_addr_ip4.s_addr == ms_addr) {
 934                         found = true;
 935                         break;
 936                 }
 937         }
 938
 939         if (found) {
 940                 if (info->nlhdr->nlmsg_flags & NLM_F_EXCL)
 941                         return -EEXIST;
 942                 if (info->nlhdr->nlmsg_flags & NLM_F_REPLACE)
 943                         return -EOPNOTSUPP;
 944
 945                 ipv4_pdp_fill(pctx, info);
 946
 947                 if (pctx->gtp_version == GTP_V0)
 948                         netdev_dbg(dev, "GTPv0-U: update tunnel id = %llx (pdp %p)\n",
 949                                    pctx->u.v0.tid, pctx);
 950                 else if (pctx->gtp_version == GTP_V1)
 951                         netdev_dbg(dev, "GTPv1-U: update tunnel id = %x/%x (pdp %p)\n",
 952                                    pctx->u.v1.i_tei, pctx->u.v1.o_tei, pctx);
 953
 954                 return 0;
 955
 956         }
 957
 958         pctx = kmalloc(sizeof(struct pdp_ctx), GFP_KERNEL);
 959         if (pctx == NULL)
 960                 return -ENOMEM;
 961
 962         ipv4_pdp_fill(pctx, info);
 963         atomic_set(&pctx->tx_seq, 0);
 964
 965         switch (pctx->gtp_version) {
 966         case GTP_V0:
 967                 /* TS 09.60: "The flow label identifies unambiguously a GTP
 968                  * flow.". We use the tid for this instead, I cannot find a
 969                  * situation in which this doesn't unambiguosly identify the
 970                  * PDP context.
 971                  */
 972                 hash_tid = gtp0_hashfn(pctx->u.v0.tid) % gtp->hash_size;
 973                 break;
 974         case GTP_V1:
 975                 hash_tid = gtp1u_hashfn(pctx->u.v1.i_tei) % gtp->hash_size;
 976                 break;
 977         }
 978
 979         hlist_add_head_rcu(&pctx->hlist_addr, &gtp->addr_hash[hash_ms]);
 980         hlist_add_head_rcu(&pctx->hlist_tid, &gtp->tid_hash[hash_tid]);
 981
 982         switch (pctx->gtp_version) {
 983         case GTP_V0:
 984                 netdev_dbg(dev, "GTPv0-U: new PDP ctx id=%llx ssgn=%pI4 ms=%pI4 (pdp=%p)\n",
 985                            pctx->u.v0.tid, &pctx->sgsn_addr_ip4,
 986                            &pctx->ms_addr_ip4, pctx);
 987                 break;
 988         case GTP_V1:
 989                 netdev_dbg(dev, "GTPv1-U: new PDP ctx id=%x/%x ssgn=%pI4 ms=%pI4 (pdp=%p)\n",
 990                            pctx->u.v1.i_tei, pctx->u.v1.o_tei,
 991                            &pctx->sgsn_addr_ip4, &pctx->ms_addr_ip4, pctx);
 992                 break;
 993         }
 994
 995         return 0;
 996 }
 997
 998 static int gtp_genl_new_pdp(struct sk_buff *skb, struct genl_info *info)
 999 {
1000         struct net_device *dev;
1001         struct net *net;
1002
1003         if (!info->attrs[GTPA_VERSION] ||
1004             !info->attrs[GTPA_LINK] ||
1005             !info->attrs[GTPA_SGSN_ADDRESS] ||
1006             !info->attrs[GTPA_MS_ADDRESS])
1007                 return -EINVAL;
1008
1009         switch (nla_get_u32(info->attrs[GTPA_VERSION])) {
1010         case GTP_V0:
1011                 if (!info->attrs[GTPA_TID] ||
1012                     !info->attrs[GTPA_FLOW])
1013                         return -EINVAL;
1014                 break;
1015         case GTP_V1:
1016                 if (!info->attrs[GTPA_I_TEI] ||
1017                     !info->attrs[GTPA_O_TEI])
1018                         return -EINVAL;
1019                 break;
1020
1021         default:
1022                 return -EINVAL;
1023         }
1024
1025         net = gtp_genl_get_net(sock_net(skb->sk), info->attrs);
1026         if (IS_ERR(net))
1027                 return PTR_ERR(net);
1028
1029         /* Check if there's an existing gtpX device to configure */
1030         dev = gtp_find_dev(net, nla_get_u32(info->attrs[GTPA_LINK]));
1031         if (dev == NULL) {
1032                 put_net(net);
1033                 return -ENODEV;
1034         }
1035         put_net(net);
1036
1037         return ipv4_pdp_add(dev, info);
1038 }
1039
1040 static int gtp_genl_del_pdp(struct sk_buff *skb, struct genl_info *info)
1041 {
1042         struct net_device *dev;
1043         struct pdp_ctx *pctx;
1044         struct gtp_dev *gtp;
1045         struct net *net;
1046
1047         if (!info->attrs[GTPA_VERSION] ||
1048             !info->attrs[GTPA_LINK])
1049                 return -EINVAL;
1050
1051         net = gtp_genl_get_net(sock_net(skb->sk), info->attrs);
1052         if (IS_ERR(net))
1053                 return PTR_ERR(net);
1054
1055         /* Check if there's an existing gtpX device to configure */
1056         dev = gtp_find_dev(net, nla_get_u32(info->attrs[GTPA_LINK]));
1057         if (dev == NULL) {
1058                 put_net(net);
1059                 return -ENODEV;
1060         }
1061         put_net(net);
1062
1063         gtp = netdev_priv(dev);
1064
1065         switch (nla_get_u32(info->attrs[GTPA_VERSION])) {
1066         case GTP_V0:
1067                 if (!info->attrs[GTPA_TID])
1068                         return -EINVAL;
1069                 pctx = gtp0_pdp_find(gtp, nla_get_u64(info->attrs[GTPA_TID]));
1070                 break;
1071         case GTP_V1:
1072                 if (!info->attrs[GTPA_I_TEI])
1073                         return -EINVAL;
1074                 pctx = gtp1_pdp_find(gtp, nla_get_u64(info->attrs[GTPA_I_TEI]));
1075                 break;
1076
1077         default:
1078                 return -EINVAL;
1079         }
1080
1081         if (pctx == NULL)
1082                 return -ENOENT;
1083
1084         if (pctx->gtp_version == GTP_V0)
1085                 netdev_dbg(dev, "GTPv0-U: deleting tunnel id = %llx (pdp %p)\n",
1086                            pctx->u.v0.tid, pctx);
1087         else if (pctx->gtp_version == GTP_V1)
1088                 netdev_dbg(dev, "GTPv1-U: deleting tunnel id = %x/%x (pdp %p)\n",
1089                            pctx->u.v1.i_tei, pctx->u.v1.o_tei, pctx);
1090
1091         hlist_del_rcu(&pctx->hlist_tid);
1092         hlist_del_rcu(&pctx->hlist_addr);
1093         kfree_rcu(pctx, rcu_head);
1094
1095         return 0;
1096 }
1097
1098 static struct genl_family gtp_genl_family = {
1099         .id             = GENL_ID_GENERATE,
1100         .name           = "gtp",
1101         .version        = 0,
1102         .hdrsize        = 0,
1103         .maxattr        = GTPA_MAX,
1104         .netnsok        = true,
1105 };
1106
1107 static int gtp_genl_fill_info(struct sk_buff *skb, u32 snd_portid, u32 snd_seq,
1108                               u32 type, struct pdp_ctx *pctx)
1109 {
1110         void *genlh;
1111
1112         genlh = genlmsg_put(skb, snd_portid, snd_seq, &gtp_genl_family, 0,
1113                             type);
1114         if (genlh == NULL)
1115                 goto nlmsg_failure;
1116
1117         if (nla_put_u32(skb, GTPA_VERSION, pctx->gtp_version) ||
1118             nla_put_be32(skb, GTPA_SGSN_ADDRESS, pctx->sgsn_addr_ip4.s_addr) ||
1119             nla_put_be32(skb, GTPA_MS_ADDRESS, pctx->ms_addr_ip4.s_addr))
1120                 goto nla_put_failure;
1121
1122         switch (pctx->gtp_version) {
1123         case GTP_V0:
1124                 if (nla_put_u64_64bit(skb, GTPA_TID, pctx->u.v0.tid, GTPA_PAD) ||
1125                     nla_put_u16(skb, GTPA_FLOW, pctx->u.v0.flow))
1126                         goto nla_put_failure;
1127                 break;
1128         case GTP_V1:
1129                 if (nla_put_u32(skb, GTPA_I_TEI, pctx->u.v1.i_tei) ||
1130                     nla_put_u32(skb, GTPA_O_TEI, pctx->u.v1.o_tei))
1131                         goto nla_put_failure;
1132                 break;
1133         }
1134         genlmsg_end(skb, genlh);
1135         return 0;
1136
1137 nlmsg_failure:
1138 nla_put_failure:
1139         genlmsg_cancel(skb, genlh);
1140         return -EMSGSIZE;
1141 }
1142
1143 static int gtp_genl_get_pdp(struct sk_buff *skb, struct genl_info *info)
1144 {
1145         struct pdp_ctx *pctx = NULL;
1146         struct net_device *dev;
1147         struct sk_buff *skb2;
1148         struct gtp_dev *gtp;
1149         u32 gtp_version;
1150         struct net *net;
1151         int err;
1152
1153         if (!info->attrs[GTPA_VERSION] ||
1154             !info->attrs[GTPA_LINK])
1155                 return -EINVAL;
1156
1157         gtp_version = nla_get_u32(info->attrs[GTPA_VERSION]);
1158         switch (gtp_version) {
1159         case GTP_V0:
1160         case GTP_V1:
1161                 break;
1162         default:
1163                 return -EINVAL;
1164         }
1165
1166         net = gtp_genl_get_net(sock_net(skb->sk), info->attrs);
1167         if (IS_ERR(net))
1168                 return PTR_ERR(net);
1169
1170         /* Check if there's an existing gtpX device to configure */
1171         dev = gtp_find_dev(net, nla_get_u32(info->attrs[GTPA_LINK]));
1172         if (dev == NULL) {
1173                 put_net(net);
1174                 return -ENODEV;
1175         }
1176         put_net(net);
1177
1178         gtp = netdev_priv(dev);
1179
1180         rcu_read_lock();
1181         if (gtp_version == GTP_V0 &&
1182             info->attrs[GTPA_TID]) {
1183                 u64 tid = nla_get_u64(info->attrs[GTPA_TID]);
1184
1185                 pctx = gtp0_pdp_find(gtp, tid);
1186         } else if (gtp_version == GTP_V1 &&
1187                  info->attrs[GTPA_I_TEI]) {
1188                 u32 tid = nla_get_u32(info->attrs[GTPA_I_TEI]);
1189
1190                 pctx = gtp1_pdp_find(gtp, tid);
1191         } else if (info->attrs[GTPA_MS_ADDRESS]) {
1192                 __be32 ip = nla_get_be32(info->attrs[GTPA_MS_ADDRESS]);
1193
1194                 pctx = ipv4_pdp_find(gtp, ip);
1195         }
1196
1197         if (pctx == NULL) {
1198                 err = -ENOENT;
1199                 goto err_unlock;
1200         }
1201
1202         skb2 = genlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC);
1203         if (skb2 == NULL) {
1204                 err = -ENOMEM;
1205                 goto err_unlock;
1206         }
1207
1208         err = gtp_genl_fill_info(skb2, NETLINK_CB(skb).portid,
1209                                  info->snd_seq, info->nlhdr->nlmsg_type, pctx);
1210         if (err < 0)
1211                 goto err_unlock_free;
1212
1213         rcu_read_unlock();
1214         return genlmsg_unicast(genl_info_net(info), skb2, info->snd_portid);
1215
1216 err_unlock_free:
1217         kfree_skb(skb2);
1218 err_unlock:
1219         rcu_read_unlock();
1220         return err;
1221 }
1222
1223 static int gtp_genl_dump_pdp(struct sk_buff *skb,
1224                                 struct netlink_callback *cb)
1225 {
1226         struct gtp_dev *last_gtp = (struct gtp_dev *)cb->args[2], *gtp;
1227         struct net *net = sock_net(skb->sk);
1228         struct gtp_net *gn = net_generic(net, gtp_net_id);
1229         unsigned long tid = cb->args[1];
1230         int i, k = cb->args[0], ret;
1231         struct pdp_ctx *pctx;
1232
1233         if (cb->args[4])
1234                 return 0;
1235
1236         list_for_each_entry_rcu(gtp, &gn->gtp_dev_list, list) {
1237                 if (last_gtp && last_gtp != gtp)
1238                         continue;
1239                 else
1240                         last_gtp = NULL;
1241
1242                 for (i = k; i < gtp->hash_size; i++) {
1243                         hlist_for_each_entry_rcu(pctx, &gtp->tid_hash[i], hlist_tid) {
1244                                 if (tid && tid != pctx->u.tid)
1245                                         continue;
1246                                 else
1247                                         tid = 0;
1248
1249                                 ret = gtp_genl_fill_info(skb,
1250                                                          NETLINK_CB(cb->skb).portid,
1251                                                          cb->nlh->nlmsg_seq,
1252                                                          cb->nlh->nlmsg_type, pctx);
1253                                 if (ret < 0) {
1254                                         cb->args[0] = i;
1255                                         cb->args[1] = pctx->u.tid;
1256                                         cb->args[2] = (unsigned long)gtp;
1257                                         goto out;
1258                                 }
1259                         }
1260                 }
1261         }
1262         cb->args[4] = 1;
1263 out:
1264         return skb->len;
1265 }
1266
1267 static struct nla_policy gtp_genl_policy[GTPA_MAX + 1] = {
1268         [GTPA_LINK]             = { .type = NLA_U32, },
1269         [GTPA_VERSION]          = { .type = NLA_U32, },
1270         [GTPA_TID]              = { .type = NLA_U64, },
1271         [GTPA_SGSN_ADDRESS]     = { .type = NLA_U32, },
1272         [GTPA_MS_ADDRESS]       = { .type = NLA_U32, },
1273         [GTPA_FLOW]             = { .type = NLA_U16, },
1274         [GTPA_NET_NS_FD]        = { .type = NLA_U32, },
1275         [GTPA_I_TEI]            = { .type = NLA_U32, },
1276         [GTPA_O_TEI]            = { .type = NLA_U32, },
1277 };
1278
1279 static const struct genl_ops gtp_genl_ops[] = {
1280         {
1281                 .cmd = GTP_CMD_NEWPDP,
1282                 .doit = gtp_genl_new_pdp,
1283                 .policy = gtp_genl_policy,
1284                 .flags = GENL_ADMIN_PERM,
1285         },
1286         {
1287                 .cmd = GTP_CMD_DELPDP,
1288                 .doit = gtp_genl_del_pdp,
1289                 .policy = gtp_genl_policy,
1290                 .flags = GENL_ADMIN_PERM,
1291         },
1292         {
1293                 .cmd = GTP_CMD_GETPDP,
1294                 .doit = gtp_genl_get_pdp,
1295                 .dumpit = gtp_genl_dump_pdp,
1296                 .policy = gtp_genl_policy,
1297                 .flags = GENL_ADMIN_PERM,
1298         },
1299 };
1300
1301 static int __net_init gtp_net_init(struct net *net)
1302 {
1303         struct gtp_net *gn = net_generic(net, gtp_net_id);
1304
1305         INIT_LIST_HEAD(&gn->gtp_dev_list);
1306         return 0;
1307 }
1308
1309 static void __net_exit gtp_net_exit(struct net *net)
1310 {
1311         struct gtp_net *gn = net_generic(net, gtp_net_id);
1312         struct gtp_dev *gtp;
1313         LIST_HEAD(list);
1314
1315         rtnl_lock();
1316         list_for_each_entry(gtp, &gn->gtp_dev_list, list)
1317                 gtp_dellink(gtp->dev, &list);
1318
1319         unregister_netdevice_many(&list);
1320         rtnl_unlock();
1321 }
1322
1323 static struct pernet_operations gtp_net_ops = {
1324         .init   = gtp_net_init,
1325         .exit   = gtp_net_exit,
1326         .id     = &gtp_net_id,
1327         .size   = sizeof(struct gtp_net),
1328 };
1329
1330 static int __init gtp_init(void)
1331 {
1332         int err;
1333
1334         get_random_bytes(&gtp_h_initval, sizeof(gtp_h_initval));
1335
1336         err = rtnl_link_register(&gtp_link_ops);
1337         if (err < 0)
1338                 goto error_out;
1339
1340         err = genl_register_family_with_ops(&gtp_genl_family, gtp_genl_ops);
1341         if (err < 0)
1342                 goto unreg_rtnl_link;
1343
1344         err = register_pernet_subsys(&gtp_net_ops);
1345         if (err < 0)
1346                 goto unreg_genl_family;
1347
1348         pr_info("GTP module loaded (pdp ctx size %Zd bytes)\n",
1349                 sizeof(struct pdp_ctx));
1350         return 0;
1351
1352 unreg_genl_family:
1353         genl_unregister_family(&gtp_genl_family);
1354 unreg_rtnl_link:
1355         rtnl_link_unregister(&gtp_link_ops);
1356 error_out:
1357         pr_err("error loading GTP module loaded\n");
1358         return err;
1359 }
1360 late_initcall(gtp_init);
1361
1362 static void __exit gtp_fini(void)
1363 {
1364         unregister_pernet_subsys(&gtp_net_ops);
1365         genl_unregister_family(&gtp_genl_family);
1366         rtnl_link_unregister(&gtp_link_ops);
1367
1368         pr_info("GTP module unloaded\n");
1369 }
1370 module_exit(gtp_fini);
1371
1372 MODULE_LICENSE("GPL");
1373 MODULE_AUTHOR("Harald Welte <[email protected]>");
1374 MODULE_DESCRIPTION("Interface driver for GTP encapsulated traffic");
1375 MODULE_ALIAS_RTNL_LINK("gtp");