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