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