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