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