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