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 *src_net, struct net_device *dev, 1466 struct nlattr *tb[], struct nlattr *data[], 1467 struct netlink_ext_ack *extack) 1468 { 1469 unsigned int role = GTP_ROLE_GGSN; 1470 struct gtp_dev *gtp; 1471 struct gtp_net *gn; 1472 int hashsize, err; 1473 1474 #if !IS_ENABLED(CONFIG_IPV6) 1475 if (data[IFLA_GTP_LOCAL6]) 1476 return -EAFNOSUPPORT; 1477 #endif 1478 1479 gtp = netdev_priv(dev); 1480 1481 if (!data[IFLA_GTP_PDP_HASHSIZE]) { 1482 hashsize = 1024; 1483 } else { 1484 hashsize = nla_get_u32(data[IFLA_GTP_PDP_HASHSIZE]); 1485 if (!hashsize) 1486 hashsize = 1024; 1487 } 1488 1489 if (data[IFLA_GTP_ROLE]) { 1490 role = nla_get_u32(data[IFLA_GTP_ROLE]); 1491 if (role > GTP_ROLE_SGSN) 1492 return -EINVAL; 1493 } 1494 gtp->role = role; 1495 1496 gtp->restart_count = nla_get_u8_default(data[IFLA_GTP_RESTART_COUNT], 1497 0); 1498 1499 gtp->net = src_net; 1500 1501 err = gtp_hashtable_new(gtp, hashsize); 1502 if (err < 0) 1503 return err; 1504 1505 if (data[IFLA_GTP_CREATE_SOCKETS]) { 1506 if (data[IFLA_GTP_LOCAL6]) 1507 err = gtp_create_sockets(gtp, data[IFLA_GTP_LOCAL6], AF_INET6); 1508 else 1509 err = gtp_create_sockets(gtp, data[IFLA_GTP_LOCAL], AF_INET); 1510 } else { 1511 err = gtp_encap_enable(gtp, data); 1512 } 1513 1514 if (err < 0) 1515 goto out_hashtable; 1516 1517 if ((gtp->sk0 && gtp->sk0->sk_family == AF_INET6) || 1518 (gtp->sk1u && gtp->sk1u->sk_family == AF_INET6)) { 1519 dev->mtu = ETH_DATA_LEN - GTP_IPV6_MAXLEN; 1520 dev->needed_headroom = LL_MAX_HEADER + GTP_IPV6_MAXLEN; 1521 } 1522 1523 err = register_netdevice(dev); 1524 if (err < 0) { 1525 netdev_dbg(dev, "failed to register new netdev %d\n", err); 1526 goto out_encap; 1527 } 1528 1529 gn = net_generic(src_net, gtp_net_id); 1530 list_add(>p->list, &gn->gtp_dev_list); 1531 dev->priv_destructor = gtp_destructor; 1532 1533 netdev_dbg(dev, "registered new GTP interface\n"); 1534 1535 return 0; 1536 1537 out_encap: 1538 gtp_encap_disable(gtp); 1539 out_hashtable: 1540 kfree(gtp->addr_hash); 1541 kfree(gtp->tid_hash); 1542 return err; 1543 } 1544 1545 static void gtp_dellink(struct net_device *dev, struct list_head *head) 1546 { 1547 struct gtp_dev *gtp = netdev_priv(dev); 1548 struct hlist_node *next; 1549 struct pdp_ctx *pctx; 1550 int i; 1551 1552 for (i = 0; i < gtp->hash_size; i++) 1553 hlist_for_each_entry_safe(pctx, next, >p->tid_hash[i], hlist_tid) 1554 pdp_context_delete(pctx); 1555 1556 list_del(>p->list); 1557 unregister_netdevice_queue(dev, head); 1558 } 1559 1560 static const struct nla_policy gtp_policy[IFLA_GTP_MAX + 1] = { 1561 [IFLA_GTP_FD0] = { .type = NLA_U32 }, 1562 [IFLA_GTP_FD1] = { .type = NLA_U32 }, 1563 [IFLA_GTP_PDP_HASHSIZE] = { .type = NLA_U32 }, 1564 [IFLA_GTP_ROLE] = { .type = NLA_U32 }, 1565 [IFLA_GTP_CREATE_SOCKETS] = { .type = NLA_U8 }, 1566 [IFLA_GTP_RESTART_COUNT] = { .type = NLA_U8 }, 1567 [IFLA_GTP_LOCAL] = { .type = NLA_U32 }, 1568 [IFLA_GTP_LOCAL6] = { .len = sizeof(struct in6_addr) }, 1569 }; 1570 1571 static int gtp_validate(struct nlattr *tb[], struct nlattr *data[], 1572 struct netlink_ext_ack *extack) 1573 { 1574 if (!data) 1575 return -EINVAL; 1576 1577 return 0; 1578 } 1579 1580 static size_t gtp_get_size(const struct net_device *dev) 1581 { 1582 return nla_total_size(sizeof(__u32)) + /* IFLA_GTP_PDP_HASHSIZE */ 1583 nla_total_size(sizeof(__u32)) + /* IFLA_GTP_ROLE */ 1584 nla_total_size(sizeof(__u8)); /* IFLA_GTP_RESTART_COUNT */ 1585 } 1586 1587 static int gtp_fill_info(struct sk_buff *skb, const struct net_device *dev) 1588 { 1589 struct gtp_dev *gtp = netdev_priv(dev); 1590 1591 if (nla_put_u32(skb, IFLA_GTP_PDP_HASHSIZE, gtp->hash_size)) 1592 goto nla_put_failure; 1593 if (nla_put_u32(skb, IFLA_GTP_ROLE, gtp->role)) 1594 goto nla_put_failure; 1595 if (nla_put_u8(skb, IFLA_GTP_RESTART_COUNT, gtp->restart_count)) 1596 goto nla_put_failure; 1597 1598 return 0; 1599 1600 nla_put_failure: 1601 return -EMSGSIZE; 1602 } 1603 1604 static struct rtnl_link_ops gtp_link_ops __read_mostly = { 1605 .kind = "gtp", 1606 .maxtype = IFLA_GTP_MAX, 1607 .policy = gtp_policy, 1608 .priv_size = sizeof(struct gtp_dev), 1609 .setup = gtp_link_setup, 1610 .validate = gtp_validate, 1611 .newlink = gtp_newlink, 1612 .dellink = gtp_dellink, 1613 .get_size = gtp_get_size, 1614 .fill_info = gtp_fill_info, 1615 }; 1616 1617 static int gtp_hashtable_new(struct gtp_dev *gtp, int hsize) 1618 { 1619 int i; 1620 1621 gtp->addr_hash = kmalloc_array(hsize, sizeof(struct hlist_head), 1622 GFP_KERNEL | __GFP_NOWARN); 1623 if (gtp->addr_hash == NULL) 1624 return -ENOMEM; 1625 1626 gtp->tid_hash = kmalloc_array(hsize, sizeof(struct hlist_head), 1627 GFP_KERNEL | __GFP_NOWARN); 1628 if (gtp->tid_hash == NULL) 1629 goto err1; 1630 1631 gtp->hash_size = hsize; 1632 1633 for (i = 0; i < hsize; i++) { 1634 INIT_HLIST_HEAD(>p->addr_hash[i]); 1635 INIT_HLIST_HEAD(>p->tid_hash[i]); 1636 } 1637 return 0; 1638 err1: 1639 kfree(gtp->addr_hash); 1640 return -ENOMEM; 1641 } 1642 1643 static struct sock *gtp_encap_enable_socket(int fd, int type, 1644 struct gtp_dev *gtp) 1645 { 1646 struct udp_tunnel_sock_cfg tuncfg = {NULL}; 1647 struct socket *sock; 1648 struct sock *sk; 1649 int err; 1650 1651 pr_debug("enable gtp on %d, %d\n", fd, type); 1652 1653 sock = sockfd_lookup(fd, &err); 1654 if (!sock) { 1655 pr_debug("gtp socket fd=%d not found\n", fd); 1656 return ERR_PTR(err); 1657 } 1658 1659 sk = sock->sk; 1660 if (sk->sk_protocol != IPPROTO_UDP || 1661 sk->sk_type != SOCK_DGRAM || 1662 (sk->sk_family != AF_INET && sk->sk_family != AF_INET6)) { 1663 pr_debug("socket fd=%d not UDP\n", fd); 1664 sk = ERR_PTR(-EINVAL); 1665 goto out_sock; 1666 } 1667 1668 if (sk->sk_family == AF_INET6 && 1669 !sk->sk_ipv6only) { 1670 sk = ERR_PTR(-EADDRNOTAVAIL); 1671 goto out_sock; 1672 } 1673 1674 lock_sock(sk); 1675 if (sk->sk_user_data) { 1676 sk = ERR_PTR(-EBUSY); 1677 goto out_rel_sock; 1678 } 1679 1680 sock_hold(sk); 1681 1682 tuncfg.sk_user_data = gtp; 1683 tuncfg.encap_type = type; 1684 tuncfg.encap_rcv = gtp_encap_recv; 1685 tuncfg.encap_destroy = gtp_encap_destroy; 1686 1687 setup_udp_tunnel_sock(sock_net(sock->sk), sock, &tuncfg); 1688 1689 out_rel_sock: 1690 release_sock(sock->sk); 1691 out_sock: 1692 sockfd_put(sock); 1693 return sk; 1694 } 1695 1696 static int gtp_encap_enable(struct gtp_dev *gtp, struct nlattr *data[]) 1697 { 1698 struct sock *sk1u = NULL; 1699 struct sock *sk0 = NULL; 1700 1701 if (!data[IFLA_GTP_FD0] && !data[IFLA_GTP_FD1]) 1702 return -EINVAL; 1703 1704 if (data[IFLA_GTP_FD0]) { 1705 int fd0 = nla_get_u32(data[IFLA_GTP_FD0]); 1706 1707 if (fd0 >= 0) { 1708 sk0 = gtp_encap_enable_socket(fd0, UDP_ENCAP_GTP0, gtp); 1709 if (IS_ERR(sk0)) 1710 return PTR_ERR(sk0); 1711 } 1712 } 1713 1714 if (data[IFLA_GTP_FD1]) { 1715 int fd1 = nla_get_u32(data[IFLA_GTP_FD1]); 1716 1717 if (fd1 >= 0) { 1718 sk1u = gtp_encap_enable_socket(fd1, UDP_ENCAP_GTP1U, gtp); 1719 if (IS_ERR(sk1u)) { 1720 gtp_encap_disable_sock(sk0); 1721 return PTR_ERR(sk1u); 1722 } 1723 } 1724 } 1725 1726 gtp->sk0 = sk0; 1727 gtp->sk1u = sk1u; 1728 1729 if (sk0 && sk1u && 1730 sk0->sk_family != sk1u->sk_family) { 1731 gtp_encap_disable_sock(sk0); 1732 gtp_encap_disable_sock(sk1u); 1733 return -EINVAL; 1734 } 1735 1736 return 0; 1737 } 1738 1739 static struct gtp_dev *gtp_find_dev(struct net *src_net, struct nlattr *nla[]) 1740 { 1741 struct gtp_dev *gtp = NULL; 1742 struct net_device *dev; 1743 struct net *net; 1744 1745 /* Examine the link attributes and figure out which network namespace 1746 * we are talking about. 1747 */ 1748 if (nla[GTPA_NET_NS_FD]) 1749 net = get_net_ns_by_fd(nla_get_u32(nla[GTPA_NET_NS_FD])); 1750 else 1751 net = get_net(src_net); 1752 1753 if (IS_ERR(net)) 1754 return NULL; 1755 1756 /* Check if there's an existing gtpX device to configure */ 1757 dev = dev_get_by_index_rcu(net, nla_get_u32(nla[GTPA_LINK])); 1758 if (dev && dev->netdev_ops == >p_netdev_ops) 1759 gtp = netdev_priv(dev); 1760 1761 put_net(net); 1762 return gtp; 1763 } 1764 1765 static void gtp_pdp_fill(struct pdp_ctx *pctx, struct genl_info *info) 1766 { 1767 pctx->gtp_version = nla_get_u32(info->attrs[GTPA_VERSION]); 1768 1769 switch (pctx->gtp_version) { 1770 case GTP_V0: 1771 /* According to TS 09.60, sections 7.5.1 and 7.5.2, the flow 1772 * label needs to be the same for uplink and downlink packets, 1773 * so let's annotate this. 1774 */ 1775 pctx->u.v0.tid = nla_get_u64(info->attrs[GTPA_TID]); 1776 pctx->u.v0.flow = nla_get_u16(info->attrs[GTPA_FLOW]); 1777 break; 1778 case GTP_V1: 1779 pctx->u.v1.i_tei = nla_get_u32(info->attrs[GTPA_I_TEI]); 1780 pctx->u.v1.o_tei = nla_get_u32(info->attrs[GTPA_O_TEI]); 1781 break; 1782 default: 1783 break; 1784 } 1785 } 1786 1787 static void ip_pdp_peer_fill(struct pdp_ctx *pctx, struct genl_info *info) 1788 { 1789 if (info->attrs[GTPA_PEER_ADDRESS]) { 1790 pctx->peer.addr.s_addr = 1791 nla_get_be32(info->attrs[GTPA_PEER_ADDRESS]); 1792 } else if (info->attrs[GTPA_PEER_ADDR6]) { 1793 pctx->peer.addr6 = nla_get_in6_addr(info->attrs[GTPA_PEER_ADDR6]); 1794 } 1795 } 1796 1797 static void ipv4_pdp_fill(struct pdp_ctx *pctx, struct genl_info *info) 1798 { 1799 ip_pdp_peer_fill(pctx, info); 1800 pctx->ms.addr.s_addr = 1801 nla_get_be32(info->attrs[GTPA_MS_ADDRESS]); 1802 gtp_pdp_fill(pctx, info); 1803 } 1804 1805 static bool ipv6_pdp_fill(struct pdp_ctx *pctx, struct genl_info *info) 1806 { 1807 ip_pdp_peer_fill(pctx, info); 1808 pctx->ms.addr6 = nla_get_in6_addr(info->attrs[GTPA_MS_ADDR6]); 1809 if (pctx->ms.addr6.s6_addr32[2] || 1810 pctx->ms.addr6.s6_addr32[3]) 1811 return false; 1812 1813 gtp_pdp_fill(pctx, info); 1814 1815 return true; 1816 } 1817 1818 static struct pdp_ctx *gtp_pdp_add(struct gtp_dev *gtp, struct sock *sk, 1819 struct genl_info *info) 1820 { 1821 struct pdp_ctx *pctx, *pctx_tid = NULL; 1822 struct net_device *dev = gtp->dev; 1823 u32 hash_ms, hash_tid = 0; 1824 struct in6_addr ms_addr6; 1825 unsigned int version; 1826 bool found = false; 1827 __be32 ms_addr; 1828 int family; 1829 1830 version = nla_get_u32(info->attrs[GTPA_VERSION]); 1831 1832 family = nla_get_u8_default(info->attrs[GTPA_FAMILY], AF_INET); 1833 1834 #if !IS_ENABLED(CONFIG_IPV6) 1835 if (family == AF_INET6) 1836 return ERR_PTR(-EAFNOSUPPORT); 1837 #endif 1838 if (!info->attrs[GTPA_PEER_ADDRESS] && 1839 !info->attrs[GTPA_PEER_ADDR6]) 1840 return ERR_PTR(-EINVAL); 1841 1842 if ((info->attrs[GTPA_PEER_ADDRESS] && 1843 sk->sk_family == AF_INET6) || 1844 (info->attrs[GTPA_PEER_ADDR6] && 1845 sk->sk_family == AF_INET)) 1846 return ERR_PTR(-EAFNOSUPPORT); 1847 1848 switch (family) { 1849 case AF_INET: 1850 if (!info->attrs[GTPA_MS_ADDRESS] || 1851 info->attrs[GTPA_MS_ADDR6]) 1852 return ERR_PTR(-EINVAL); 1853 1854 ms_addr = nla_get_be32(info->attrs[GTPA_MS_ADDRESS]); 1855 hash_ms = ipv4_hashfn(ms_addr) % gtp->hash_size; 1856 pctx = ipv4_pdp_find(gtp, ms_addr); 1857 break; 1858 case AF_INET6: 1859 if (!info->attrs[GTPA_MS_ADDR6] || 1860 info->attrs[GTPA_MS_ADDRESS]) 1861 return ERR_PTR(-EINVAL); 1862 1863 ms_addr6 = nla_get_in6_addr(info->attrs[GTPA_MS_ADDR6]); 1864 hash_ms = ipv6_hashfn(&ms_addr6) % gtp->hash_size; 1865 pctx = ipv6_pdp_find(gtp, &ms_addr6); 1866 break; 1867 default: 1868 return ERR_PTR(-EAFNOSUPPORT); 1869 } 1870 if (pctx) 1871 found = true; 1872 if (version == GTP_V0) 1873 pctx_tid = gtp0_pdp_find(gtp, 1874 nla_get_u64(info->attrs[GTPA_TID]), 1875 family); 1876 else if (version == GTP_V1) 1877 pctx_tid = gtp1_pdp_find(gtp, 1878 nla_get_u32(info->attrs[GTPA_I_TEI]), 1879 family); 1880 if (pctx_tid) 1881 found = true; 1882 1883 if (found) { 1884 if (info->nlhdr->nlmsg_flags & NLM_F_EXCL) 1885 return ERR_PTR(-EEXIST); 1886 if (info->nlhdr->nlmsg_flags & NLM_F_REPLACE) 1887 return ERR_PTR(-EOPNOTSUPP); 1888 1889 if (pctx && pctx_tid) 1890 return ERR_PTR(-EEXIST); 1891 if (!pctx) 1892 pctx = pctx_tid; 1893 1894 switch (pctx->af) { 1895 case AF_INET: 1896 ipv4_pdp_fill(pctx, info); 1897 break; 1898 case AF_INET6: 1899 if (!ipv6_pdp_fill(pctx, info)) 1900 return ERR_PTR(-EADDRNOTAVAIL); 1901 break; 1902 } 1903 1904 if (pctx->gtp_version == GTP_V0) 1905 netdev_dbg(dev, "GTPv0-U: update tunnel id = %llx (pdp %p)\n", 1906 pctx->u.v0.tid, pctx); 1907 else if (pctx->gtp_version == GTP_V1) 1908 netdev_dbg(dev, "GTPv1-U: update tunnel id = %x/%x (pdp %p)\n", 1909 pctx->u.v1.i_tei, pctx->u.v1.o_tei, pctx); 1910 1911 return pctx; 1912 1913 } 1914 1915 pctx = kmalloc(sizeof(*pctx), GFP_ATOMIC); 1916 if (pctx == NULL) 1917 return ERR_PTR(-ENOMEM); 1918 1919 sock_hold(sk); 1920 pctx->sk = sk; 1921 pctx->dev = gtp->dev; 1922 pctx->af = family; 1923 1924 switch (pctx->af) { 1925 case AF_INET: 1926 if (!info->attrs[GTPA_MS_ADDRESS]) { 1927 sock_put(sk); 1928 kfree(pctx); 1929 return ERR_PTR(-EINVAL); 1930 } 1931 1932 ipv4_pdp_fill(pctx, info); 1933 break; 1934 case AF_INET6: 1935 if (!info->attrs[GTPA_MS_ADDR6]) { 1936 sock_put(sk); 1937 kfree(pctx); 1938 return ERR_PTR(-EINVAL); 1939 } 1940 1941 if (!ipv6_pdp_fill(pctx, info)) { 1942 sock_put(sk); 1943 kfree(pctx); 1944 return ERR_PTR(-EADDRNOTAVAIL); 1945 } 1946 break; 1947 } 1948 atomic_set(&pctx->tx_seq, 0); 1949 1950 switch (pctx->gtp_version) { 1951 case GTP_V0: 1952 /* TS 09.60: "The flow label identifies unambiguously a GTP 1953 * flow.". We use the tid for this instead, I cannot find a 1954 * situation in which this doesn't unambiguosly identify the 1955 * PDP context. 1956 */ 1957 hash_tid = gtp0_hashfn(pctx->u.v0.tid) % gtp->hash_size; 1958 break; 1959 case GTP_V1: 1960 hash_tid = gtp1u_hashfn(pctx->u.v1.i_tei) % gtp->hash_size; 1961 break; 1962 } 1963 1964 hlist_add_head_rcu(&pctx->hlist_addr, >p->addr_hash[hash_ms]); 1965 hlist_add_head_rcu(&pctx->hlist_tid, >p->tid_hash[hash_tid]); 1966 1967 switch (pctx->gtp_version) { 1968 case GTP_V0: 1969 netdev_dbg(dev, "GTPv0-U: new PDP ctx id=%llx ssgn=%pI4 ms=%pI4 (pdp=%p)\n", 1970 pctx->u.v0.tid, &pctx->peer.addr, 1971 &pctx->ms.addr, pctx); 1972 break; 1973 case GTP_V1: 1974 netdev_dbg(dev, "GTPv1-U: new PDP ctx id=%x/%x ssgn=%pI4 ms=%pI4 (pdp=%p)\n", 1975 pctx->u.v1.i_tei, pctx->u.v1.o_tei, 1976 &pctx->peer.addr, &pctx->ms.addr, pctx); 1977 break; 1978 } 1979 1980 return pctx; 1981 } 1982 1983 static void pdp_context_free(struct rcu_head *head) 1984 { 1985 struct pdp_ctx *pctx = container_of(head, struct pdp_ctx, rcu_head); 1986 1987 sock_put(pctx->sk); 1988 kfree(pctx); 1989 } 1990 1991 static void pdp_context_delete(struct pdp_ctx *pctx) 1992 { 1993 hlist_del_rcu(&pctx->hlist_tid); 1994 hlist_del_rcu(&pctx->hlist_addr); 1995 call_rcu(&pctx->rcu_head, pdp_context_free); 1996 } 1997 1998 static int gtp_tunnel_notify(struct pdp_ctx *pctx, u8 cmd, gfp_t allocation); 1999 2000 static int gtp_genl_new_pdp(struct sk_buff *skb, struct genl_info *info) 2001 { 2002 unsigned int version; 2003 struct pdp_ctx *pctx; 2004 struct gtp_dev *gtp; 2005 struct sock *sk; 2006 int err; 2007 2008 if (!info->attrs[GTPA_VERSION] || 2009 !info->attrs[GTPA_LINK]) 2010 return -EINVAL; 2011 2012 version = nla_get_u32(info->attrs[GTPA_VERSION]); 2013 2014 switch (version) { 2015 case GTP_V0: 2016 if (!info->attrs[GTPA_TID] || 2017 !info->attrs[GTPA_FLOW]) 2018 return -EINVAL; 2019 break; 2020 case GTP_V1: 2021 if (!info->attrs[GTPA_I_TEI] || 2022 !info->attrs[GTPA_O_TEI]) 2023 return -EINVAL; 2024 break; 2025 2026 default: 2027 return -EINVAL; 2028 } 2029 2030 rtnl_lock(); 2031 2032 gtp = gtp_find_dev(sock_net(skb->sk), info->attrs); 2033 if (!gtp) { 2034 err = -ENODEV; 2035 goto out_unlock; 2036 } 2037 2038 if (version == GTP_V0) 2039 sk = gtp->sk0; 2040 else if (version == GTP_V1) 2041 sk = gtp->sk1u; 2042 else 2043 sk = NULL; 2044 2045 if (!sk) { 2046 err = -ENODEV; 2047 goto out_unlock; 2048 } 2049 2050 pctx = gtp_pdp_add(gtp, sk, info); 2051 if (IS_ERR(pctx)) { 2052 err = PTR_ERR(pctx); 2053 } else { 2054 gtp_tunnel_notify(pctx, GTP_CMD_NEWPDP, GFP_KERNEL); 2055 err = 0; 2056 } 2057 2058 out_unlock: 2059 rtnl_unlock(); 2060 return err; 2061 } 2062 2063 static struct pdp_ctx *gtp_find_pdp_by_link(struct net *net, 2064 struct nlattr *nla[]) 2065 { 2066 struct gtp_dev *gtp; 2067 int family; 2068 2069 family = nla_get_u8_default(nla[GTPA_FAMILY], AF_INET); 2070 2071 gtp = gtp_find_dev(net, nla); 2072 if (!gtp) 2073 return ERR_PTR(-ENODEV); 2074 2075 if (nla[GTPA_MS_ADDRESS]) { 2076 __be32 ip = nla_get_be32(nla[GTPA_MS_ADDRESS]); 2077 2078 if (family != AF_INET) 2079 return ERR_PTR(-EINVAL); 2080 2081 return ipv4_pdp_find(gtp, ip); 2082 } else if (nla[GTPA_MS_ADDR6]) { 2083 struct in6_addr addr = nla_get_in6_addr(nla[GTPA_MS_ADDR6]); 2084 2085 if (family != AF_INET6) 2086 return ERR_PTR(-EINVAL); 2087 2088 if (addr.s6_addr32[2] || 2089 addr.s6_addr32[3]) 2090 return ERR_PTR(-EADDRNOTAVAIL); 2091 2092 return ipv6_pdp_find(gtp, &addr); 2093 } else if (nla[GTPA_VERSION]) { 2094 u32 gtp_version = nla_get_u32(nla[GTPA_VERSION]); 2095 2096 if (gtp_version == GTP_V0 && nla[GTPA_TID]) { 2097 return gtp0_pdp_find(gtp, nla_get_u64(nla[GTPA_TID]), 2098 family); 2099 } else if (gtp_version == GTP_V1 && nla[GTPA_I_TEI]) { 2100 return gtp1_pdp_find(gtp, nla_get_u32(nla[GTPA_I_TEI]), 2101 family); 2102 } 2103 } 2104 2105 return ERR_PTR(-EINVAL); 2106 } 2107 2108 static struct pdp_ctx *gtp_find_pdp(struct net *net, struct nlattr *nla[]) 2109 { 2110 struct pdp_ctx *pctx; 2111 2112 if (nla[GTPA_LINK]) 2113 pctx = gtp_find_pdp_by_link(net, nla); 2114 else 2115 pctx = ERR_PTR(-EINVAL); 2116 2117 if (!pctx) 2118 pctx = ERR_PTR(-ENOENT); 2119 2120 return pctx; 2121 } 2122 2123 static int gtp_genl_del_pdp(struct sk_buff *skb, struct genl_info *info) 2124 { 2125 struct pdp_ctx *pctx; 2126 int err = 0; 2127 2128 if (!info->attrs[GTPA_VERSION]) 2129 return -EINVAL; 2130 2131 rcu_read_lock(); 2132 2133 pctx = gtp_find_pdp(sock_net(skb->sk), info->attrs); 2134 if (IS_ERR(pctx)) { 2135 err = PTR_ERR(pctx); 2136 goto out_unlock; 2137 } 2138 2139 if (pctx->gtp_version == GTP_V0) 2140 netdev_dbg(pctx->dev, "GTPv0-U: deleting tunnel id = %llx (pdp %p)\n", 2141 pctx->u.v0.tid, pctx); 2142 else if (pctx->gtp_version == GTP_V1) 2143 netdev_dbg(pctx->dev, "GTPv1-U: deleting tunnel id = %x/%x (pdp %p)\n", 2144 pctx->u.v1.i_tei, pctx->u.v1.o_tei, pctx); 2145 2146 gtp_tunnel_notify(pctx, GTP_CMD_DELPDP, GFP_ATOMIC); 2147 pdp_context_delete(pctx); 2148 2149 out_unlock: 2150 rcu_read_unlock(); 2151 return err; 2152 } 2153 2154 static int gtp_genl_fill_info(struct sk_buff *skb, u32 snd_portid, u32 snd_seq, 2155 int flags, u32 type, struct pdp_ctx *pctx) 2156 { 2157 void *genlh; 2158 2159 genlh = genlmsg_put(skb, snd_portid, snd_seq, >p_genl_family, flags, 2160 type); 2161 if (genlh == NULL) 2162 goto nlmsg_failure; 2163 2164 if (nla_put_u32(skb, GTPA_VERSION, pctx->gtp_version) || 2165 nla_put_u32(skb, GTPA_LINK, pctx->dev->ifindex) || 2166 nla_put_u8(skb, GTPA_FAMILY, pctx->af)) 2167 goto nla_put_failure; 2168 2169 switch (pctx->af) { 2170 case AF_INET: 2171 if (nla_put_be32(skb, GTPA_MS_ADDRESS, pctx->ms.addr.s_addr)) 2172 goto nla_put_failure; 2173 break; 2174 case AF_INET6: 2175 if (nla_put_in6_addr(skb, GTPA_MS_ADDR6, &pctx->ms.addr6)) 2176 goto nla_put_failure; 2177 break; 2178 } 2179 2180 switch (pctx->sk->sk_family) { 2181 case AF_INET: 2182 if (nla_put_be32(skb, GTPA_PEER_ADDRESS, pctx->peer.addr.s_addr)) 2183 goto nla_put_failure; 2184 break; 2185 case AF_INET6: 2186 if (nla_put_in6_addr(skb, GTPA_PEER_ADDR6, &pctx->peer.addr6)) 2187 goto nla_put_failure; 2188 break; 2189 } 2190 2191 switch (pctx->gtp_version) { 2192 case GTP_V0: 2193 if (nla_put_u64_64bit(skb, GTPA_TID, pctx->u.v0.tid, GTPA_PAD) || 2194 nla_put_u16(skb, GTPA_FLOW, pctx->u.v0.flow)) 2195 goto nla_put_failure; 2196 break; 2197 case GTP_V1: 2198 if (nla_put_u32(skb, GTPA_I_TEI, pctx->u.v1.i_tei) || 2199 nla_put_u32(skb, GTPA_O_TEI, pctx->u.v1.o_tei)) 2200 goto nla_put_failure; 2201 break; 2202 } 2203 genlmsg_end(skb, genlh); 2204 return 0; 2205 2206 nlmsg_failure: 2207 nla_put_failure: 2208 genlmsg_cancel(skb, genlh); 2209 return -EMSGSIZE; 2210 } 2211 2212 static int gtp_tunnel_notify(struct pdp_ctx *pctx, u8 cmd, gfp_t allocation) 2213 { 2214 struct sk_buff *msg; 2215 int ret; 2216 2217 msg = nlmsg_new(NLMSG_DEFAULT_SIZE, allocation); 2218 if (!msg) 2219 return -ENOMEM; 2220 2221 ret = gtp_genl_fill_info(msg, 0, 0, 0, cmd, pctx); 2222 if (ret < 0) { 2223 nlmsg_free(msg); 2224 return ret; 2225 } 2226 2227 ret = genlmsg_multicast_netns(>p_genl_family, dev_net(pctx->dev), msg, 2228 0, GTP_GENL_MCGRP, GFP_ATOMIC); 2229 return ret; 2230 } 2231 2232 static int gtp_genl_get_pdp(struct sk_buff *skb, struct genl_info *info) 2233 { 2234 struct pdp_ctx *pctx = NULL; 2235 struct sk_buff *skb2; 2236 int err; 2237 2238 if (!info->attrs[GTPA_VERSION]) 2239 return -EINVAL; 2240 2241 rcu_read_lock(); 2242 2243 pctx = gtp_find_pdp(sock_net(skb->sk), info->attrs); 2244 if (IS_ERR(pctx)) { 2245 err = PTR_ERR(pctx); 2246 goto err_unlock; 2247 } 2248 2249 skb2 = genlmsg_new(NLMSG_GOODSIZE, GFP_ATOMIC); 2250 if (skb2 == NULL) { 2251 err = -ENOMEM; 2252 goto err_unlock; 2253 } 2254 2255 err = gtp_genl_fill_info(skb2, NETLINK_CB(skb).portid, info->snd_seq, 2256 0, info->nlhdr->nlmsg_type, pctx); 2257 if (err < 0) 2258 goto err_unlock_free; 2259 2260 rcu_read_unlock(); 2261 return genlmsg_unicast(genl_info_net(info), skb2, info->snd_portid); 2262 2263 err_unlock_free: 2264 kfree_skb(skb2); 2265 err_unlock: 2266 rcu_read_unlock(); 2267 return err; 2268 } 2269 2270 static int gtp_genl_dump_pdp(struct sk_buff *skb, 2271 struct netlink_callback *cb) 2272 { 2273 struct gtp_dev *last_gtp = (struct gtp_dev *)cb->args[2], *gtp; 2274 int i, j, bucket = cb->args[0], skip = cb->args[1]; 2275 struct net *net = sock_net(skb->sk); 2276 struct net_device *dev; 2277 struct pdp_ctx *pctx; 2278 2279 if (cb->args[4]) 2280 return 0; 2281 2282 rcu_read_lock(); 2283 for_each_netdev_rcu(net, dev) { 2284 if (dev->rtnl_link_ops != >p_link_ops) 2285 continue; 2286 2287 gtp = netdev_priv(dev); 2288 2289 if (last_gtp && last_gtp != gtp) 2290 continue; 2291 else 2292 last_gtp = NULL; 2293 2294 for (i = bucket; i < gtp->hash_size; i++) { 2295 j = 0; 2296 hlist_for_each_entry_rcu(pctx, >p->tid_hash[i], 2297 hlist_tid) { 2298 if (j >= skip && 2299 gtp_genl_fill_info(skb, 2300 NETLINK_CB(cb->skb).portid, 2301 cb->nlh->nlmsg_seq, 2302 NLM_F_MULTI, 2303 cb->nlh->nlmsg_type, pctx)) { 2304 cb->args[0] = i; 2305 cb->args[1] = j; 2306 cb->args[2] = (unsigned long)gtp; 2307 goto out; 2308 } 2309 j++; 2310 } 2311 skip = 0; 2312 } 2313 bucket = 0; 2314 } 2315 cb->args[4] = 1; 2316 out: 2317 rcu_read_unlock(); 2318 return skb->len; 2319 } 2320 2321 static int gtp_genl_send_echo_req(struct sk_buff *skb, struct genl_info *info) 2322 { 2323 struct sk_buff *skb_to_send; 2324 __be32 src_ip, dst_ip; 2325 unsigned int version; 2326 struct gtp_dev *gtp; 2327 struct flowi4 fl4; 2328 struct rtable *rt; 2329 struct sock *sk; 2330 __be16 port; 2331 int len; 2332 2333 if (!info->attrs[GTPA_VERSION] || 2334 !info->attrs[GTPA_LINK] || 2335 !info->attrs[GTPA_PEER_ADDRESS] || 2336 !info->attrs[GTPA_MS_ADDRESS]) 2337 return -EINVAL; 2338 2339 version = nla_get_u32(info->attrs[GTPA_VERSION]); 2340 dst_ip = nla_get_be32(info->attrs[GTPA_PEER_ADDRESS]); 2341 src_ip = nla_get_be32(info->attrs[GTPA_MS_ADDRESS]); 2342 2343 gtp = gtp_find_dev(sock_net(skb->sk), info->attrs); 2344 if (!gtp) 2345 return -ENODEV; 2346 2347 if (!gtp->sk_created) 2348 return -EOPNOTSUPP; 2349 if (!(gtp->dev->flags & IFF_UP)) 2350 return -ENETDOWN; 2351 2352 if (version == GTP_V0) { 2353 struct gtp0_header *gtp0_h; 2354 2355 len = LL_RESERVED_SPACE(gtp->dev) + sizeof(struct gtp0_header) + 2356 sizeof(struct iphdr) + sizeof(struct udphdr); 2357 2358 skb_to_send = netdev_alloc_skb_ip_align(gtp->dev, len); 2359 if (!skb_to_send) 2360 return -ENOMEM; 2361 2362 sk = gtp->sk0; 2363 port = htons(GTP0_PORT); 2364 2365 gtp0_h = skb_push(skb_to_send, sizeof(struct gtp0_header)); 2366 memset(gtp0_h, 0, sizeof(struct gtp0_header)); 2367 gtp0_build_echo_msg(gtp0_h, GTP_ECHO_REQ); 2368 } else if (version == GTP_V1) { 2369 struct gtp1_header_long *gtp1u_h; 2370 2371 len = LL_RESERVED_SPACE(gtp->dev) + 2372 sizeof(struct gtp1_header_long) + 2373 sizeof(struct iphdr) + sizeof(struct udphdr); 2374 2375 skb_to_send = netdev_alloc_skb_ip_align(gtp->dev, len); 2376 if (!skb_to_send) 2377 return -ENOMEM; 2378 2379 sk = gtp->sk1u; 2380 port = htons(GTP1U_PORT); 2381 2382 gtp1u_h = skb_push(skb_to_send, 2383 sizeof(struct gtp1_header_long)); 2384 memset(gtp1u_h, 0, sizeof(struct gtp1_header_long)); 2385 gtp1u_build_echo_msg(gtp1u_h, GTP_ECHO_REQ); 2386 } else { 2387 return -ENODEV; 2388 } 2389 2390 rt = ip4_route_output_gtp(&fl4, sk, dst_ip, src_ip); 2391 if (IS_ERR(rt)) { 2392 netdev_dbg(gtp->dev, "no route for echo request to %pI4\n", 2393 &dst_ip); 2394 kfree_skb(skb_to_send); 2395 return -ENODEV; 2396 } 2397 2398 udp_tunnel_xmit_skb(rt, sk, skb_to_send, 2399 fl4.saddr, fl4.daddr, 2400 fl4.flowi4_tos, 2401 ip4_dst_hoplimit(&rt->dst), 2402 0, 2403 port, port, 2404 !net_eq(sock_net(sk), 2405 dev_net(gtp->dev)), 2406 false); 2407 return 0; 2408 } 2409 2410 static const struct nla_policy gtp_genl_policy[GTPA_MAX + 1] = { 2411 [GTPA_LINK] = { .type = NLA_U32, }, 2412 [GTPA_VERSION] = { .type = NLA_U32, }, 2413 [GTPA_TID] = { .type = NLA_U64, }, 2414 [GTPA_PEER_ADDRESS] = { .type = NLA_U32, }, 2415 [GTPA_MS_ADDRESS] = { .type = NLA_U32, }, 2416 [GTPA_FLOW] = { .type = NLA_U16, }, 2417 [GTPA_NET_NS_FD] = { .type = NLA_U32, }, 2418 [GTPA_I_TEI] = { .type = NLA_U32, }, 2419 [GTPA_O_TEI] = { .type = NLA_U32, }, 2420 [GTPA_PEER_ADDR6] = { .len = sizeof(struct in6_addr), }, 2421 [GTPA_MS_ADDR6] = { .len = sizeof(struct in6_addr), }, 2422 [GTPA_FAMILY] = { .type = NLA_U8, }, 2423 }; 2424 2425 static const struct genl_small_ops gtp_genl_ops[] = { 2426 { 2427 .cmd = GTP_CMD_NEWPDP, 2428 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 2429 .doit = gtp_genl_new_pdp, 2430 .flags = GENL_ADMIN_PERM, 2431 }, 2432 { 2433 .cmd = GTP_CMD_DELPDP, 2434 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 2435 .doit = gtp_genl_del_pdp, 2436 .flags = GENL_ADMIN_PERM, 2437 }, 2438 { 2439 .cmd = GTP_CMD_GETPDP, 2440 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 2441 .doit = gtp_genl_get_pdp, 2442 .dumpit = gtp_genl_dump_pdp, 2443 .flags = GENL_ADMIN_PERM, 2444 }, 2445 { 2446 .cmd = GTP_CMD_ECHOREQ, 2447 .validate = GENL_DONT_VALIDATE_STRICT | GENL_DONT_VALIDATE_DUMP, 2448 .doit = gtp_genl_send_echo_req, 2449 .flags = GENL_ADMIN_PERM, 2450 }, 2451 }; 2452 2453 static struct genl_family gtp_genl_family __ro_after_init = { 2454 .name = "gtp", 2455 .version = 0, 2456 .hdrsize = 0, 2457 .maxattr = GTPA_MAX, 2458 .policy = gtp_genl_policy, 2459 .netnsok = true, 2460 .module = THIS_MODULE, 2461 .small_ops = gtp_genl_ops, 2462 .n_small_ops = ARRAY_SIZE(gtp_genl_ops), 2463 .resv_start_op = GTP_CMD_ECHOREQ + 1, 2464 .mcgrps = gtp_genl_mcgrps, 2465 .n_mcgrps = ARRAY_SIZE(gtp_genl_mcgrps), 2466 }; 2467 2468 static int __net_init gtp_net_init(struct net *net) 2469 { 2470 struct gtp_net *gn = net_generic(net, gtp_net_id); 2471 2472 INIT_LIST_HEAD(&gn->gtp_dev_list); 2473 return 0; 2474 } 2475 2476 static void __net_exit gtp_net_exit_batch_rtnl(struct list_head *net_list, 2477 struct list_head *dev_to_kill) 2478 { 2479 struct net *net; 2480 2481 list_for_each_entry(net, net_list, exit_list) { 2482 struct gtp_net *gn = net_generic(net, gtp_net_id); 2483 struct gtp_dev *gtp, *gtp_next; 2484 struct net_device *dev; 2485 2486 for_each_netdev(net, dev) 2487 if (dev->rtnl_link_ops == >p_link_ops) 2488 gtp_dellink(dev, dev_to_kill); 2489 2490 list_for_each_entry_safe(gtp, gtp_next, &gn->gtp_dev_list, list) 2491 gtp_dellink(gtp->dev, dev_to_kill); 2492 } 2493 } 2494 2495 static struct pernet_operations gtp_net_ops = { 2496 .init = gtp_net_init, 2497 .exit_batch_rtnl = gtp_net_exit_batch_rtnl, 2498 .id = >p_net_id, 2499 .size = sizeof(struct gtp_net), 2500 }; 2501 2502 static int __init gtp_init(void) 2503 { 2504 int err; 2505 2506 get_random_bytes(>p_h_initval, sizeof(gtp_h_initval)); 2507 2508 err = register_pernet_subsys(>p_net_ops); 2509 if (err < 0) 2510 goto error_out; 2511 2512 err = rtnl_link_register(>p_link_ops); 2513 if (err < 0) 2514 goto unreg_pernet_subsys; 2515 2516 err = genl_register_family(>p_genl_family); 2517 if (err < 0) 2518 goto unreg_rtnl_link; 2519 2520 pr_info("GTP module loaded (pdp ctx size %zd bytes)\n", 2521 sizeof(struct pdp_ctx)); 2522 return 0; 2523 2524 unreg_rtnl_link: 2525 rtnl_link_unregister(>p_link_ops); 2526 unreg_pernet_subsys: 2527 unregister_pernet_subsys(>p_net_ops); 2528 error_out: 2529 pr_err("error loading GTP module loaded\n"); 2530 return err; 2531 } 2532 late_initcall(gtp_init); 2533 2534 static void __exit gtp_fini(void) 2535 { 2536 genl_unregister_family(>p_genl_family); 2537 rtnl_link_unregister(>p_link_ops); 2538 unregister_pernet_subsys(>p_net_ops); 2539 2540 pr_info("GTP module unloaded\n"); 2541 } 2542 module_exit(gtp_fini); 2543 2544 MODULE_LICENSE("GPL"); 2545 MODULE_AUTHOR("Harald Welte <hwelte@sysmocom.de>"); 2546 MODULE_DESCRIPTION("Interface driver for GTP encapsulated traffic"); 2547 MODULE_ALIAS_RTNL_LINK("gtp"); 2548 MODULE_ALIAS_GENL_FAMILY("gtp"); 2549