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