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