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