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