projects/linux·blob master · refs

2567 lines · 59.5 KiB

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