1 // SPDX-License-Identifier: GPL-2.0-only
2 #include <linux/module.h>
3 #include <linux/errno.h>
4 #include <linux/socket.h>
5 #include <linux/skbuff.h>
6 #include <linux/ip.h>
7 #include <linux/icmp.h>
8 #include <linux/udp.h>
9 #include <linux/types.h>
10 #include <linux/kernel.h>
11 #include <net/genetlink.h>
12 #include <net/gro.h>
13 #include <net/gue.h>
14 #include <net/fou.h>
15 #include <net/ip.h>
16 #include <net/protocol.h>
17 #include <net/udp.h>
18 #include <net/udp_tunnel.h>
19 #include <uapi/linux/fou.h>
20 #include <uapi/linux/genetlink.h>
21 
22 #include "fou_nl.h"
23 
24 struct fou {
25 	struct socket *sock;
26 	u8 protocol;
27 	u8 flags;
28 	__be16 port;
29 	u8 family;
30 	u16 type;
31 	struct list_head list;
32 	struct rcu_head rcu;
33 };
34 
35 #define FOU_F_REMCSUM_NOPARTIAL BIT(0)
36 
37 struct fou_cfg {
38 	u16 type;
39 	u8 protocol;
40 	u8 flags;
41 	struct udp_port_cfg udp_config;
42 };
43 
44 static unsigned int fou_net_id;
45 
46 struct fou_net {
47 	struct list_head fou_list;
48 	struct mutex fou_lock;
49 };
50 
fou_from_sock(struct sock * sk)51 static inline struct fou *fou_from_sock(struct sock *sk)
52 {
53 	return rcu_dereference_sk_user_data(sk);
54 }
55 
fou_recv_pull(struct sk_buff * skb,struct fou * fou,size_t len)56 static int fou_recv_pull(struct sk_buff *skb, struct fou *fou, size_t len)
57 {
58 	/* Remove 'len' bytes from the packet (UDP header and
59 	 * FOU header if present).
60 	 */
61 	if (fou->family == AF_INET)
62 		ip_hdr(skb)->tot_len = htons(ntohs(ip_hdr(skb)->tot_len) - len);
63 	else
64 		ipv6_hdr(skb)->payload_len =
65 		    htons(ntohs(ipv6_hdr(skb)->payload_len) - len);
66 
67 	__skb_pull(skb, len);
68 	skb_postpull_rcsum(skb, udp_hdr(skb), len);
69 	skb_reset_transport_header(skb);
70 	return iptunnel_pull_offloads(skb);
71 }
72 
fou_udp_recv(struct sock * sk,struct sk_buff * skb)73 static int fou_udp_recv(struct sock *sk, struct sk_buff *skb)
74 {
75 	struct fou *fou = fou_from_sock(sk);
76 
77 	if (!fou)
78 		return 1;
79 
80 	if (fou_recv_pull(skb, fou, sizeof(struct udphdr)))
81 		goto drop;
82 
83 	return -fou->protocol;
84 
85 drop:
86 	kfree_skb(skb);
87 	return 0;
88 }
89 
gue_remcsum(struct sk_buff * skb,struct guehdr * guehdr,void * data,size_t hdrlen,u8 ipproto,bool nopartial)90 static struct guehdr *gue_remcsum(struct sk_buff *skb, struct guehdr *guehdr,
91 				  void *data, size_t hdrlen, u8 ipproto,
92 				  bool nopartial)
93 {
94 	__be16 *pd = data;
95 	size_t start = ntohs(pd[0]);
96 	size_t offset = ntohs(pd[1]);
97 	size_t plen = sizeof(struct udphdr) + hdrlen +
98 	    max_t(size_t, offset + sizeof(u16), start);
99 
100 	if (skb->remcsum_offload)
101 		return guehdr;
102 
103 	if (!pskb_may_pull(skb, plen))
104 		return NULL;
105 	guehdr = (struct guehdr *)&udp_hdr(skb)[1];
106 
107 	skb_remcsum_process(skb, (void *)guehdr + hdrlen,
108 			    start, offset, nopartial);
109 
110 	return guehdr;
111 }
112 
gue_control_message(struct sk_buff * skb,struct guehdr * guehdr)113 static int gue_control_message(struct sk_buff *skb, struct guehdr *guehdr)
114 {
115 	/* No support yet */
116 	kfree_skb(skb);
117 	return 0;
118 }
119 
gue_udp_recv(struct sock * sk,struct sk_buff * skb)120 static int gue_udp_recv(struct sock *sk, struct sk_buff *skb)
121 {
122 	struct fou *fou = fou_from_sock(sk);
123 	size_t len, optlen, hdrlen;
124 	struct guehdr *guehdr;
125 	void *data;
126 	u16 doffset = 0;
127 	u8 proto_ctype;
128 
129 	if (!fou)
130 		return 1;
131 
132 	len = sizeof(struct udphdr) + sizeof(struct guehdr);
133 	if (!pskb_may_pull(skb, len))
134 		goto drop;
135 
136 	guehdr = (struct guehdr *)&udp_hdr(skb)[1];
137 
138 	switch (guehdr->version) {
139 	case 0: /* Full GUE header present */
140 		break;
141 
142 	case 1: {
143 		/* Direct encapsulation of IPv4 or IPv6 */
144 
145 		int prot;
146 
147 		switch (((struct iphdr *)guehdr)->version) {
148 		case 4:
149 			prot = IPPROTO_IPIP;
150 			break;
151 		case 6:
152 			prot = IPPROTO_IPV6;
153 			break;
154 		default:
155 			goto drop;
156 		}
157 
158 		if (fou_recv_pull(skb, fou, sizeof(struct udphdr)))
159 			goto drop;
160 
161 		return -prot;
162 	}
163 
164 	default: /* Undefined version */
165 		goto drop;
166 	}
167 
168 	optlen = guehdr->hlen << 2;
169 	len += optlen;
170 
171 	if (!pskb_may_pull(skb, len))
172 		goto drop;
173 
174 	/* guehdr may change after pull */
175 	guehdr = (struct guehdr *)&udp_hdr(skb)[1];
176 
177 	if (validate_gue_flags(guehdr, optlen))
178 		goto drop;
179 
180 	hdrlen = sizeof(struct guehdr) + optlen;
181 
182 	if (fou->family == AF_INET)
183 		ip_hdr(skb)->tot_len = htons(ntohs(ip_hdr(skb)->tot_len) - len);
184 	else
185 		ipv6_hdr(skb)->payload_len =
186 		    htons(ntohs(ipv6_hdr(skb)->payload_len) - len);
187 
188 	/* Pull csum through the guehdr now . This can be used if
189 	 * there is a remote checksum offload.
190 	 */
191 	skb_postpull_rcsum(skb, udp_hdr(skb), len);
192 
193 	data = &guehdr[1];
194 
195 	if (guehdr->flags & GUE_FLAG_PRIV) {
196 		__be32 flags = *(__be32 *)(data + doffset);
197 
198 		doffset += GUE_LEN_PRIV;
199 
200 		if (flags & GUE_PFLAG_REMCSUM) {
201 			guehdr = gue_remcsum(skb, guehdr, data + doffset,
202 					     hdrlen, guehdr->proto_ctype,
203 					     !!(fou->flags &
204 						FOU_F_REMCSUM_NOPARTIAL));
205 			if (!guehdr)
206 				goto drop;
207 
208 			data = &guehdr[1];
209 
210 			doffset += GUE_PLEN_REMCSUM;
211 		}
212 	}
213 
214 	if (unlikely(guehdr->control))
215 		return gue_control_message(skb, guehdr);
216 
217 	proto_ctype = guehdr->proto_ctype;
218 	__skb_pull(skb, sizeof(struct udphdr) + hdrlen);
219 	skb_reset_transport_header(skb);
220 
221 	if (iptunnel_pull_offloads(skb))
222 		goto drop;
223 
224 	return -proto_ctype;
225 
226 drop:
227 	kfree_skb(skb);
228 	return 0;
229 }
230 
fou_gro_receive(struct sock * sk,struct list_head * head,struct sk_buff * skb)231 static struct sk_buff *fou_gro_receive(struct sock *sk,
232 				       struct list_head *head,
233 				       struct sk_buff *skb)
234 {
235 	const struct net_offload __rcu **offloads;
236 	struct fou *fou = fou_from_sock(sk);
237 	const struct net_offload *ops;
238 	struct sk_buff *pp = NULL;
239 	u8 proto;
240 
241 	if (!fou)
242 		goto out;
243 
244 	proto = fou->protocol;
245 
246 	/* We can clear the encap_mark for FOU as we are essentially doing
247 	 * one of two possible things.  We are either adding an L4 tunnel
248 	 * header to the outer L3 tunnel header, or we are simply
249 	 * treating the GRE tunnel header as though it is a UDP protocol
250 	 * specific header such as VXLAN or GENEVE.
251 	 */
252 	NAPI_GRO_CB(skb)->encap_mark = 0;
253 
254 	/* Flag this frame as already having an outer encap header */
255 	NAPI_GRO_CB(skb)->is_fou = 1;
256 
257 	offloads = NAPI_GRO_CB(skb)->is_ipv6 ? inet6_offloads : inet_offloads;
258 	ops = rcu_dereference(offloads[proto]);
259 	if (!ops || !ops->callbacks.gro_receive)
260 		goto out;
261 
262 	pp = call_gro_receive(ops->callbacks.gro_receive, head, skb);
263 
264 out:
265 	return pp;
266 }
267 
fou_gro_complete(struct sock * sk,struct sk_buff * skb,int nhoff)268 static int fou_gro_complete(struct sock *sk, struct sk_buff *skb,
269 			    int nhoff)
270 {
271 	const struct net_offload __rcu **offloads;
272 	struct fou *fou = fou_from_sock(sk);
273 	const struct net_offload *ops;
274 	u8 proto;
275 	int err;
276 
277 	if (!fou) {
278 		err = -ENOENT;
279 		goto out;
280 	}
281 
282 	proto = fou->protocol;
283 
284 	offloads = NAPI_GRO_CB(skb)->is_ipv6 ? inet6_offloads : inet_offloads;
285 	ops = rcu_dereference(offloads[proto]);
286 	if (WARN_ON(!ops || !ops->callbacks.gro_complete)) {
287 		err = -ENOSYS;
288 		goto out;
289 	}
290 
291 	err = ops->callbacks.gro_complete(skb, nhoff);
292 
293 	skb_set_inner_mac_header(skb, nhoff);
294 
295 out:
296 	return err;
297 }
298 
gue_gro_remcsum(struct sk_buff * skb,unsigned int off,struct guehdr * guehdr,void * data,size_t hdrlen,struct gro_remcsum * grc,bool nopartial)299 static struct guehdr *gue_gro_remcsum(struct sk_buff *skb, unsigned int off,
300 				      struct guehdr *guehdr, void *data,
301 				      size_t hdrlen, struct gro_remcsum *grc,
302 				      bool nopartial)
303 {
304 	__be16 *pd = data;
305 	size_t start = ntohs(pd[0]);
306 	size_t offset = ntohs(pd[1]);
307 
308 	if (skb->remcsum_offload)
309 		return guehdr;
310 
311 	if (!NAPI_GRO_CB(skb)->csum_valid)
312 		return NULL;
313 
314 	guehdr = skb_gro_remcsum_process(skb, (void *)guehdr, off, hdrlen,
315 					 start, offset, grc, nopartial);
316 
317 	skb->remcsum_offload = 1;
318 
319 	return guehdr;
320 }
321 
gue_gro_receive(struct sock * sk,struct list_head * head,struct sk_buff * skb)322 static struct sk_buff *gue_gro_receive(struct sock *sk,
323 				       struct list_head *head,
324 				       struct sk_buff *skb)
325 {
326 	const struct net_offload __rcu **offloads;
327 	const struct net_offload *ops;
328 	struct sk_buff *pp = NULL;
329 	struct sk_buff *p;
330 	struct guehdr *guehdr;
331 	size_t len, optlen, hdrlen, off;
332 	void *data;
333 	u16 doffset = 0;
334 	int flush = 1;
335 	struct fou *fou = fou_from_sock(sk);
336 	struct gro_remcsum grc;
337 	u8 proto;
338 
339 	skb_gro_remcsum_init(&grc);
340 
341 	if (!fou)
342 		goto out;
343 
344 	off = skb_gro_offset(skb);
345 	len = off + sizeof(*guehdr);
346 
347 	guehdr = skb_gro_header(skb, len, off);
348 	if (unlikely(!guehdr))
349 		goto out;
350 
351 	switch (guehdr->version) {
352 	case 0:
353 		break;
354 	case 1:
355 		switch (((struct iphdr *)guehdr)->version) {
356 		case 4:
357 			proto = IPPROTO_IPIP;
358 			break;
359 		case 6:
360 			proto = IPPROTO_IPV6;
361 			break;
362 		default:
363 			goto out;
364 		}
365 		goto next_proto;
366 	default:
367 		goto out;
368 	}
369 
370 	optlen = guehdr->hlen << 2;
371 	len += optlen;
372 
373 	if (!skb_gro_may_pull(skb, len)) {
374 		guehdr = skb_gro_header_slow(skb, len, off);
375 		if (unlikely(!guehdr))
376 			goto out;
377 	}
378 
379 	if (unlikely(guehdr->control) || guehdr->version != 0 ||
380 	    validate_gue_flags(guehdr, optlen))
381 		goto out;
382 
383 	hdrlen = sizeof(*guehdr) + optlen;
384 
385 	/* Adjust NAPI_GRO_CB(skb)->csum to account for guehdr,
386 	 * this is needed if there is a remote checkcsum offload.
387 	 */
388 	skb_gro_postpull_rcsum(skb, guehdr, hdrlen);
389 
390 	data = &guehdr[1];
391 
392 	if (guehdr->flags & GUE_FLAG_PRIV) {
393 		__be32 flags = *(__be32 *)(data + doffset);
394 
395 		doffset += GUE_LEN_PRIV;
396 
397 		if (flags & GUE_PFLAG_REMCSUM) {
398 			guehdr = gue_gro_remcsum(skb, off, guehdr,
399 						 data + doffset, hdrlen, &grc,
400 						 !!(fou->flags &
401 						    FOU_F_REMCSUM_NOPARTIAL));
402 
403 			if (!guehdr)
404 				goto out;
405 
406 			data = &guehdr[1];
407 
408 			doffset += GUE_PLEN_REMCSUM;
409 		}
410 	}
411 
412 	skb_gro_pull(skb, hdrlen);
413 
414 	list_for_each_entry(p, head, list) {
415 		const struct guehdr *guehdr2;
416 
417 		if (!NAPI_GRO_CB(p)->same_flow)
418 			continue;
419 
420 		guehdr2 = (struct guehdr *)(p->data + off);
421 
422 		/* Compare base GUE header to be equal (covers
423 		 * hlen, version, proto_ctype, and flags.
424 		 */
425 		if (guehdr->word != guehdr2->word) {
426 			NAPI_GRO_CB(p)->same_flow = 0;
427 			continue;
428 		}
429 
430 		/* Compare optional fields are the same. */
431 		if (guehdr->hlen && memcmp(&guehdr[1], &guehdr2[1],
432 					   guehdr->hlen << 2)) {
433 			NAPI_GRO_CB(p)->same_flow = 0;
434 			continue;
435 		}
436 	}
437 
438 	proto = guehdr->proto_ctype;
439 
440 next_proto:
441 
442 	/* We can clear the encap_mark for GUE as we are essentially doing
443 	 * one of two possible things.  We are either adding an L4 tunnel
444 	 * header to the outer L3 tunnel header, or we are simply
445 	 * treating the GRE tunnel header as though it is a UDP protocol
446 	 * specific header such as VXLAN or GENEVE.
447 	 */
448 	NAPI_GRO_CB(skb)->encap_mark = 0;
449 
450 	/* Flag this frame as already having an outer encap header */
451 	NAPI_GRO_CB(skb)->is_fou = 1;
452 
453 	offloads = NAPI_GRO_CB(skb)->is_ipv6 ? inet6_offloads : inet_offloads;
454 	ops = rcu_dereference(offloads[proto]);
455 	if (!ops || !ops->callbacks.gro_receive)
456 		goto out;
457 
458 	pp = call_gro_receive(ops->callbacks.gro_receive, head, skb);
459 	flush = 0;
460 
461 out:
462 	skb_gro_flush_final_remcsum(skb, pp, flush, &grc);
463 
464 	return pp;
465 }
466 
gue_gro_complete(struct sock * sk,struct sk_buff * skb,int nhoff)467 static int gue_gro_complete(struct sock *sk, struct sk_buff *skb, int nhoff)
468 {
469 	struct guehdr *guehdr = (struct guehdr *)(skb->data + nhoff);
470 	const struct net_offload __rcu **offloads;
471 	const struct net_offload *ops;
472 	unsigned int guehlen = 0;
473 	u8 proto;
474 	int err = -ENOENT;
475 
476 	switch (guehdr->version) {
477 	case 0:
478 		proto = guehdr->proto_ctype;
479 		guehlen = sizeof(*guehdr) + (guehdr->hlen << 2);
480 		break;
481 	case 1:
482 		switch (((struct iphdr *)guehdr)->version) {
483 		case 4:
484 			proto = IPPROTO_IPIP;
485 			break;
486 		case 6:
487 			proto = IPPROTO_IPV6;
488 			break;
489 		default:
490 			return err;
491 		}
492 		break;
493 	default:
494 		return err;
495 	}
496 
497 	offloads = NAPI_GRO_CB(skb)->is_ipv6 ? inet6_offloads : inet_offloads;
498 	ops = rcu_dereference(offloads[proto]);
499 	if (WARN_ON(!ops || !ops->callbacks.gro_complete))
500 		goto out;
501 
502 	err = ops->callbacks.gro_complete(skb, nhoff + guehlen);
503 
504 	skb_set_inner_mac_header(skb, nhoff + guehlen);
505 
506 out:
507 	return err;
508 }
509 
fou_cfg_cmp(struct fou * fou,struct fou_cfg * cfg)510 static bool fou_cfg_cmp(struct fou *fou, struct fou_cfg *cfg)
511 {
512 	struct sock *sk = fou->sock->sk;
513 	struct udp_port_cfg *udp_cfg = &cfg->udp_config;
514 
515 	if (fou->family != udp_cfg->family ||
516 	    fou->port != udp_cfg->local_udp_port ||
517 	    sk->sk_dport != udp_cfg->peer_udp_port ||
518 	    sk->sk_bound_dev_if != udp_cfg->bind_ifindex)
519 		return false;
520 
521 	if (fou->family == AF_INET) {
522 		if (sk->sk_rcv_saddr != udp_cfg->local_ip.s_addr ||
523 		    sk->sk_daddr != udp_cfg->peer_ip.s_addr)
524 			return false;
525 		else
526 			return true;
527 #if IS_ENABLED(CONFIG_IPV6)
528 	} else {
529 		if (ipv6_addr_cmp(&sk->sk_v6_rcv_saddr, &udp_cfg->local_ip6) ||
530 		    ipv6_addr_cmp(&sk->sk_v6_daddr, &udp_cfg->peer_ip6))
531 			return false;
532 		else
533 			return true;
534 #endif
535 	}
536 
537 	return false;
538 }
539 
fou_add_to_port_list(struct net * net,struct fou * fou,struct fou_cfg * cfg)540 static int fou_add_to_port_list(struct net *net, struct fou *fou,
541 				struct fou_cfg *cfg)
542 {
543 	struct fou_net *fn = net_generic(net, fou_net_id);
544 	struct fou *fout;
545 
546 	mutex_lock(&fn->fou_lock);
547 	list_for_each_entry(fout, &fn->fou_list, list) {
548 		if (fou_cfg_cmp(fout, cfg)) {
549 			mutex_unlock(&fn->fou_lock);
550 			return -EALREADY;
551 		}
552 	}
553 
554 	list_add(&fou->list, &fn->fou_list);
555 	mutex_unlock(&fn->fou_lock);
556 
557 	return 0;
558 }
559 
fou_release(struct fou * fou)560 static void fou_release(struct fou *fou)
561 {
562 	struct socket *sock = fou->sock;
563 
564 	list_del(&fou->list);
565 	udp_tunnel_sock_release(sock);
566 
567 	kfree_rcu(fou, rcu);
568 }
569 
fou_create(struct net * net,struct fou_cfg * cfg,struct socket ** sockp)570 static int fou_create(struct net *net, struct fou_cfg *cfg,
571 		      struct socket **sockp)
572 {
573 	struct socket *sock = NULL;
574 	struct fou *fou = NULL;
575 	struct sock *sk;
576 	struct udp_tunnel_sock_cfg tunnel_cfg;
577 	int err;
578 
579 	/* Open UDP socket */
580 	err = udp_sock_create(net, &cfg->udp_config, &sock);
581 	if (err < 0)
582 		goto error;
583 
584 	/* Allocate FOU port structure */
585 	fou = kzalloc(sizeof(*fou), GFP_KERNEL);
586 	if (!fou) {
587 		err = -ENOMEM;
588 		goto error;
589 	}
590 
591 	sk = sock->sk;
592 
593 	fou->port = cfg->udp_config.local_udp_port;
594 	fou->family = cfg->udp_config.family;
595 	fou->flags = cfg->flags;
596 	fou->type = cfg->type;
597 	fou->sock = sock;
598 
599 	memset(&tunnel_cfg, 0, sizeof(tunnel_cfg));
600 	tunnel_cfg.encap_type = 1;
601 	tunnel_cfg.sk_user_data = fou;
602 	tunnel_cfg.encap_destroy = NULL;
603 
604 	/* Initial for fou type */
605 	switch (cfg->type) {
606 	case FOU_ENCAP_DIRECT:
607 		tunnel_cfg.encap_rcv = fou_udp_recv;
608 		tunnel_cfg.gro_receive = fou_gro_receive;
609 		tunnel_cfg.gro_complete = fou_gro_complete;
610 		fou->protocol = cfg->protocol;
611 		break;
612 	case FOU_ENCAP_GUE:
613 		tunnel_cfg.encap_rcv = gue_udp_recv;
614 		tunnel_cfg.gro_receive = gue_gro_receive;
615 		tunnel_cfg.gro_complete = gue_gro_complete;
616 		break;
617 	default:
618 		err = -EINVAL;
619 		goto error;
620 	}
621 
622 	setup_udp_tunnel_sock(net, sock, &tunnel_cfg);
623 
624 	sk->sk_allocation = GFP_ATOMIC;
625 
626 	err = fou_add_to_port_list(net, fou, cfg);
627 	if (err)
628 		goto error;
629 
630 	if (sockp)
631 		*sockp = sock;
632 
633 	return 0;
634 
635 error:
636 	kfree(fou);
637 	if (sock)
638 		udp_tunnel_sock_release(sock);
639 
640 	return err;
641 }
642 
fou_destroy(struct net * net,struct fou_cfg * cfg)643 static int fou_destroy(struct net *net, struct fou_cfg *cfg)
644 {
645 	struct fou_net *fn = net_generic(net, fou_net_id);
646 	int err = -EINVAL;
647 	struct fou *fou;
648 
649 	mutex_lock(&fn->fou_lock);
650 	list_for_each_entry(fou, &fn->fou_list, list) {
651 		if (fou_cfg_cmp(fou, cfg)) {
652 			fou_release(fou);
653 			err = 0;
654 			break;
655 		}
656 	}
657 	mutex_unlock(&fn->fou_lock);
658 
659 	return err;
660 }
661 
662 static struct genl_family fou_nl_family;
663 
parse_nl_config(struct genl_info * info,struct fou_cfg * cfg)664 static int parse_nl_config(struct genl_info *info,
665 			   struct fou_cfg *cfg)
666 {
667 	bool has_local = false, has_peer = false;
668 	struct nlattr *attr;
669 	int ifindex;
670 	__be16 port;
671 
672 	memset(cfg, 0, sizeof(*cfg));
673 
674 	cfg->udp_config.family = AF_INET;
675 
676 	if (info->attrs[FOU_ATTR_AF]) {
677 		u8 family = nla_get_u8(info->attrs[FOU_ATTR_AF]);
678 
679 		switch (family) {
680 		case AF_INET:
681 			break;
682 		case AF_INET6:
683 			cfg->udp_config.ipv6_v6only = 1;
684 			break;
685 		default:
686 			return -EAFNOSUPPORT;
687 		}
688 
689 		cfg->udp_config.family = family;
690 	}
691 
692 	if (info->attrs[FOU_ATTR_PORT]) {
693 		port = nla_get_be16(info->attrs[FOU_ATTR_PORT]);
694 		cfg->udp_config.local_udp_port = port;
695 	}
696 
697 	if (info->attrs[FOU_ATTR_IPPROTO])
698 		cfg->protocol = nla_get_u8(info->attrs[FOU_ATTR_IPPROTO]);
699 
700 	if (info->attrs[FOU_ATTR_TYPE])
701 		cfg->type = nla_get_u8(info->attrs[FOU_ATTR_TYPE]);
702 
703 	if (info->attrs[FOU_ATTR_REMCSUM_NOPARTIAL])
704 		cfg->flags |= FOU_F_REMCSUM_NOPARTIAL;
705 
706 	if (cfg->udp_config.family == AF_INET) {
707 		if (info->attrs[FOU_ATTR_LOCAL_V4]) {
708 			attr = info->attrs[FOU_ATTR_LOCAL_V4];
709 			cfg->udp_config.local_ip.s_addr = nla_get_in_addr(attr);
710 			has_local = true;
711 		}
712 
713 		if (info->attrs[FOU_ATTR_PEER_V4]) {
714 			attr = info->attrs[FOU_ATTR_PEER_V4];
715 			cfg->udp_config.peer_ip.s_addr = nla_get_in_addr(attr);
716 			has_peer = true;
717 		}
718 #if IS_ENABLED(CONFIG_IPV6)
719 	} else {
720 		if (info->attrs[FOU_ATTR_LOCAL_V6]) {
721 			attr = info->attrs[FOU_ATTR_LOCAL_V6];
722 			cfg->udp_config.local_ip6 = nla_get_in6_addr(attr);
723 			has_local = true;
724 		}
725 
726 		if (info->attrs[FOU_ATTR_PEER_V6]) {
727 			attr = info->attrs[FOU_ATTR_PEER_V6];
728 			cfg->udp_config.peer_ip6 = nla_get_in6_addr(attr);
729 			has_peer = true;
730 		}
731 #endif
732 	}
733 
734 	if (has_peer) {
735 		if (info->attrs[FOU_ATTR_PEER_PORT]) {
736 			port = nla_get_be16(info->attrs[FOU_ATTR_PEER_PORT]);
737 			cfg->udp_config.peer_udp_port = port;
738 		} else {
739 			return -EINVAL;
740 		}
741 	}
742 
743 	if (info->attrs[FOU_ATTR_IFINDEX]) {
744 		if (!has_local)
745 			return -EINVAL;
746 
747 		ifindex = nla_get_s32(info->attrs[FOU_ATTR_IFINDEX]);
748 
749 		cfg->udp_config.bind_ifindex = ifindex;
750 	}
751 
752 	return 0;
753 }
754 
fou_nl_add_doit(struct sk_buff * skb,struct genl_info * info)755 int fou_nl_add_doit(struct sk_buff *skb, struct genl_info *info)
756 {
757 	struct net *net = genl_info_net(info);
758 	struct fou_cfg cfg;
759 	int err;
760 
761 	err = parse_nl_config(info, &cfg);
762 	if (err)
763 		return err;
764 
765 	return fou_create(net, &cfg, NULL);
766 }
767 
fou_nl_del_doit(struct sk_buff * skb,struct genl_info * info)768 int fou_nl_del_doit(struct sk_buff *skb, struct genl_info *info)
769 {
770 	struct net *net = genl_info_net(info);
771 	struct fou_cfg cfg;
772 	int err;
773 
774 	err = parse_nl_config(info, &cfg);
775 	if (err)
776 		return err;
777 
778 	return fou_destroy(net, &cfg);
779 }
780 
fou_fill_info(struct fou * fou,struct sk_buff * msg)781 static int fou_fill_info(struct fou *fou, struct sk_buff *msg)
782 {
783 	struct sock *sk = fou->sock->sk;
784 
785 	if (nla_put_u8(msg, FOU_ATTR_AF, fou->sock->sk->sk_family) ||
786 	    nla_put_be16(msg, FOU_ATTR_PORT, fou->port) ||
787 	    nla_put_be16(msg, FOU_ATTR_PEER_PORT, sk->sk_dport) ||
788 	    nla_put_u8(msg, FOU_ATTR_IPPROTO, fou->protocol) ||
789 	    nla_put_u8(msg, FOU_ATTR_TYPE, fou->type) ||
790 	    nla_put_s32(msg, FOU_ATTR_IFINDEX, sk->sk_bound_dev_if))
791 		return -1;
792 
793 	if (fou->flags & FOU_F_REMCSUM_NOPARTIAL)
794 		if (nla_put_flag(msg, FOU_ATTR_REMCSUM_NOPARTIAL))
795 			return -1;
796 
797 	if (fou->sock->sk->sk_family == AF_INET) {
798 		if (nla_put_in_addr(msg, FOU_ATTR_LOCAL_V4, sk->sk_rcv_saddr))
799 			return -1;
800 
801 		if (nla_put_in_addr(msg, FOU_ATTR_PEER_V4, sk->sk_daddr))
802 			return -1;
803 #if IS_ENABLED(CONFIG_IPV6)
804 	} else {
805 		if (nla_put_in6_addr(msg, FOU_ATTR_LOCAL_V6,
806 				     &sk->sk_v6_rcv_saddr))
807 			return -1;
808 
809 		if (nla_put_in6_addr(msg, FOU_ATTR_PEER_V6, &sk->sk_v6_daddr))
810 			return -1;
811 #endif
812 	}
813 
814 	return 0;
815 }
816 
fou_dump_info(struct fou * fou,u32 portid,u32 seq,u32 flags,struct sk_buff * skb,u8 cmd)817 static int fou_dump_info(struct fou *fou, u32 portid, u32 seq,
818 			 u32 flags, struct sk_buff *skb, u8 cmd)
819 {
820 	void *hdr;
821 
822 	hdr = genlmsg_put(skb, portid, seq, &fou_nl_family, flags, cmd);
823 	if (!hdr)
824 		return -ENOMEM;
825 
826 	if (fou_fill_info(fou, skb) < 0)
827 		goto nla_put_failure;
828 
829 	genlmsg_end(skb, hdr);
830 	return 0;
831 
832 nla_put_failure:
833 	genlmsg_cancel(skb, hdr);
834 	return -EMSGSIZE;
835 }
836 
fou_nl_get_doit(struct sk_buff * skb,struct genl_info * info)837 int fou_nl_get_doit(struct sk_buff *skb, struct genl_info *info)
838 {
839 	struct net *net = genl_info_net(info);
840 	struct fou_net *fn = net_generic(net, fou_net_id);
841 	struct sk_buff *msg;
842 	struct fou_cfg cfg;
843 	struct fou *fout;
844 	__be16 port;
845 	u8 family;
846 	int ret;
847 
848 	ret = parse_nl_config(info, &cfg);
849 	if (ret)
850 		return ret;
851 	port = cfg.udp_config.local_udp_port;
852 	if (port == 0)
853 		return -EINVAL;
854 
855 	family = cfg.udp_config.family;
856 	if (family != AF_INET && family != AF_INET6)
857 		return -EINVAL;
858 
859 	msg = nlmsg_new(NLMSG_DEFAULT_SIZE, GFP_KERNEL);
860 	if (!msg)
861 		return -ENOMEM;
862 
863 	ret = -ESRCH;
864 	mutex_lock(&fn->fou_lock);
865 	list_for_each_entry(fout, &fn->fou_list, list) {
866 		if (fou_cfg_cmp(fout, &cfg)) {
867 			ret = fou_dump_info(fout, info->snd_portid,
868 					    info->snd_seq, 0, msg,
869 					    info->genlhdr->cmd);
870 			break;
871 		}
872 	}
873 	mutex_unlock(&fn->fou_lock);
874 	if (ret < 0)
875 		goto out_free;
876 
877 	return genlmsg_reply(msg, info);
878 
879 out_free:
880 	nlmsg_free(msg);
881 	return ret;
882 }
883 
fou_nl_get_dumpit(struct sk_buff * skb,struct netlink_callback * cb)884 int fou_nl_get_dumpit(struct sk_buff *skb, struct netlink_callback *cb)
885 {
886 	struct net *net = sock_net(skb->sk);
887 	struct fou_net *fn = net_generic(net, fou_net_id);
888 	struct fou *fout;
889 	int idx = 0, ret;
890 
891 	mutex_lock(&fn->fou_lock);
892 	list_for_each_entry(fout, &fn->fou_list, list) {
893 		if (idx++ < cb->args[0])
894 			continue;
895 		ret = fou_dump_info(fout, NETLINK_CB(cb->skb).portid,
896 				    cb->nlh->nlmsg_seq, NLM_F_MULTI,
897 				    skb, FOU_CMD_GET);
898 		if (ret)
899 			break;
900 	}
901 	mutex_unlock(&fn->fou_lock);
902 
903 	cb->args[0] = idx;
904 	return skb->len;
905 }
906 
907 static struct genl_family fou_nl_family __ro_after_init = {
908 	.hdrsize	= 0,
909 	.name		= FOU_GENL_NAME,
910 	.version	= FOU_GENL_VERSION,
911 	.maxattr	= FOU_ATTR_MAX,
912 	.policy		= fou_nl_policy,
913 	.netnsok	= true,
914 	.module		= THIS_MODULE,
915 	.small_ops	= fou_nl_ops,
916 	.n_small_ops	= ARRAY_SIZE(fou_nl_ops),
917 	.resv_start_op	= FOU_CMD_GET + 1,
918 };
919 
fou_encap_hlen(struct ip_tunnel_encap * e)920 size_t fou_encap_hlen(struct ip_tunnel_encap *e)
921 {
922 	return sizeof(struct udphdr);
923 }
924 EXPORT_SYMBOL(fou_encap_hlen);
925 
gue_encap_hlen(struct ip_tunnel_encap * e)926 size_t gue_encap_hlen(struct ip_tunnel_encap *e)
927 {
928 	size_t len;
929 	bool need_priv = false;
930 
931 	len = sizeof(struct udphdr) + sizeof(struct guehdr);
932 
933 	if (e->flags & TUNNEL_ENCAP_FLAG_REMCSUM) {
934 		len += GUE_PLEN_REMCSUM;
935 		need_priv = true;
936 	}
937 
938 	len += need_priv ? GUE_LEN_PRIV : 0;
939 
940 	return len;
941 }
942 EXPORT_SYMBOL(gue_encap_hlen);
943 
__fou_build_header(struct sk_buff * skb,struct ip_tunnel_encap * e,u8 * protocol,__be16 * sport,int type)944 int __fou_build_header(struct sk_buff *skb, struct ip_tunnel_encap *e,
945 		       u8 *protocol, __be16 *sport, int type)
946 {
947 	int err;
948 
949 	err = iptunnel_handle_offloads(skb, type);
950 	if (err)
951 		return err;
952 
953 	*sport = e->sport ? : udp_flow_src_port(dev_net(skb->dev),
954 						skb, 0, 0, false);
955 
956 	return 0;
957 }
958 EXPORT_SYMBOL(__fou_build_header);
959 
__gue_build_header(struct sk_buff * skb,struct ip_tunnel_encap * e,u8 * protocol,__be16 * sport,int type)960 int __gue_build_header(struct sk_buff *skb, struct ip_tunnel_encap *e,
961 		       u8 *protocol, __be16 *sport, int type)
962 {
963 	struct guehdr *guehdr;
964 	size_t hdrlen, optlen = 0;
965 	void *data;
966 	bool need_priv = false;
967 	int err;
968 
969 	if ((e->flags & TUNNEL_ENCAP_FLAG_REMCSUM) &&
970 	    skb->ip_summed == CHECKSUM_PARTIAL) {
971 		optlen += GUE_PLEN_REMCSUM;
972 		type |= SKB_GSO_TUNNEL_REMCSUM;
973 		need_priv = true;
974 	}
975 
976 	optlen += need_priv ? GUE_LEN_PRIV : 0;
977 
978 	err = iptunnel_handle_offloads(skb, type);
979 	if (err)
980 		return err;
981 
982 	/* Get source port (based on flow hash) before skb_push */
983 	*sport = e->sport ? : udp_flow_src_port(dev_net(skb->dev),
984 						skb, 0, 0, false);
985 
986 	hdrlen = sizeof(struct guehdr) + optlen;
987 
988 	skb_push(skb, hdrlen);
989 
990 	guehdr = (struct guehdr *)skb->data;
991 
992 	guehdr->control = 0;
993 	guehdr->version = 0;
994 	guehdr->hlen = optlen >> 2;
995 	guehdr->flags = 0;
996 	guehdr->proto_ctype = *protocol;
997 
998 	data = &guehdr[1];
999 
1000 	if (need_priv) {
1001 		__be32 *flags = data;
1002 
1003 		guehdr->flags |= GUE_FLAG_PRIV;
1004 		*flags = 0;
1005 		data += GUE_LEN_PRIV;
1006 
1007 		if (type & SKB_GSO_TUNNEL_REMCSUM) {
1008 			u16 csum_start = skb_checksum_start_offset(skb);
1009 			__be16 *pd = data;
1010 
1011 			if (csum_start < hdrlen)
1012 				return -EINVAL;
1013 
1014 			csum_start -= hdrlen;
1015 			pd[0] = htons(csum_start);
1016 			pd[1] = htons(csum_start + skb->csum_offset);
1017 
1018 			if (!skb_is_gso(skb)) {
1019 				skb->ip_summed = CHECKSUM_NONE;
1020 				skb->encapsulation = 0;
1021 			}
1022 
1023 			*flags |= GUE_PFLAG_REMCSUM;
1024 			data += GUE_PLEN_REMCSUM;
1025 		}
1026 
1027 	}
1028 
1029 	return 0;
1030 }
1031 EXPORT_SYMBOL(__gue_build_header);
1032 
1033 #ifdef CONFIG_NET_FOU_IP_TUNNELS
1034 
fou_build_udp(struct sk_buff * skb,struct ip_tunnel_encap * e,struct flowi4 * fl4,u8 * protocol,__be16 sport)1035 static void fou_build_udp(struct sk_buff *skb, struct ip_tunnel_encap *e,
1036 			  struct flowi4 *fl4, u8 *protocol, __be16 sport)
1037 {
1038 	struct udphdr *uh;
1039 
1040 	skb_push(skb, sizeof(struct udphdr));
1041 	skb_reset_transport_header(skb);
1042 
1043 	uh = udp_hdr(skb);
1044 
1045 	uh->dest = e->dport;
1046 	uh->source = sport;
1047 	uh->len = htons(skb->len);
1048 	udp_set_csum(!(e->flags & TUNNEL_ENCAP_FLAG_CSUM), skb,
1049 		     fl4->saddr, fl4->daddr, skb->len);
1050 
1051 	*protocol = IPPROTO_UDP;
1052 }
1053 
fou_build_header(struct sk_buff * skb,struct ip_tunnel_encap * e,u8 * protocol,struct flowi4 * fl4)1054 static int fou_build_header(struct sk_buff *skb, struct ip_tunnel_encap *e,
1055 			    u8 *protocol, struct flowi4 *fl4)
1056 {
1057 	int type = e->flags & TUNNEL_ENCAP_FLAG_CSUM ? SKB_GSO_UDP_TUNNEL_CSUM :
1058 						       SKB_GSO_UDP_TUNNEL;
1059 	__be16 sport;
1060 	int err;
1061 
1062 	err = __fou_build_header(skb, e, protocol, &sport, type);
1063 	if (err)
1064 		return err;
1065 
1066 	fou_build_udp(skb, e, fl4, protocol, sport);
1067 
1068 	return 0;
1069 }
1070 
gue_build_header(struct sk_buff * skb,struct ip_tunnel_encap * e,u8 * protocol,struct flowi4 * fl4)1071 static int gue_build_header(struct sk_buff *skb, struct ip_tunnel_encap *e,
1072 			    u8 *protocol, struct flowi4 *fl4)
1073 {
1074 	int type = e->flags & TUNNEL_ENCAP_FLAG_CSUM ? SKB_GSO_UDP_TUNNEL_CSUM :
1075 						       SKB_GSO_UDP_TUNNEL;
1076 	__be16 sport;
1077 	int err;
1078 
1079 	err = __gue_build_header(skb, e, protocol, &sport, type);
1080 	if (err)
1081 		return err;
1082 
1083 	fou_build_udp(skb, e, fl4, protocol, sport);
1084 
1085 	return 0;
1086 }
1087 
gue_err_proto_handler(int proto,struct sk_buff * skb,u32 info)1088 static int gue_err_proto_handler(int proto, struct sk_buff *skb, u32 info)
1089 {
1090 	const struct net_protocol *ipprot = rcu_dereference(inet_protos[proto]);
1091 
1092 	if (ipprot && ipprot->err_handler) {
1093 		if (!ipprot->err_handler(skb, info))
1094 			return 0;
1095 	}
1096 
1097 	return -ENOENT;
1098 }
1099 
gue_err(struct sk_buff * skb,u32 info)1100 static int gue_err(struct sk_buff *skb, u32 info)
1101 {
1102 	int transport_offset = skb_transport_offset(skb);
1103 	struct guehdr *guehdr;
1104 	size_t len, optlen;
1105 	int ret;
1106 
1107 	len = sizeof(struct udphdr) + sizeof(struct guehdr);
1108 	if (!pskb_may_pull(skb, transport_offset + len))
1109 		return -EINVAL;
1110 
1111 	guehdr = (struct guehdr *)&udp_hdr(skb)[1];
1112 
1113 	switch (guehdr->version) {
1114 	case 0: /* Full GUE header present */
1115 		break;
1116 	case 1: {
1117 		/* Direct encapsulation of IPv4 or IPv6 */
1118 		skb_set_transport_header(skb, -(int)sizeof(struct icmphdr));
1119 
1120 		switch (((struct iphdr *)guehdr)->version) {
1121 		case 4:
1122 			ret = gue_err_proto_handler(IPPROTO_IPIP, skb, info);
1123 			goto out;
1124 #if IS_ENABLED(CONFIG_IPV6)
1125 		case 6:
1126 			ret = gue_err_proto_handler(IPPROTO_IPV6, skb, info);
1127 			goto out;
1128 #endif
1129 		default:
1130 			ret = -EOPNOTSUPP;
1131 			goto out;
1132 		}
1133 	}
1134 	default: /* Undefined version */
1135 		return -EOPNOTSUPP;
1136 	}
1137 
1138 	if (guehdr->control)
1139 		return -ENOENT;
1140 
1141 	optlen = guehdr->hlen << 2;
1142 
1143 	if (!pskb_may_pull(skb, transport_offset + len + optlen))
1144 		return -EINVAL;
1145 
1146 	guehdr = (struct guehdr *)&udp_hdr(skb)[1];
1147 	if (validate_gue_flags(guehdr, optlen))
1148 		return -EINVAL;
1149 
1150 	/* Handling exceptions for direct UDP encapsulation in GUE would lead to
1151 	 * recursion. Besides, this kind of encapsulation can't even be
1152 	 * configured currently. Discard this.
1153 	 */
1154 	if (guehdr->proto_ctype == IPPROTO_UDP ||
1155 	    guehdr->proto_ctype == IPPROTO_UDPLITE)
1156 		return -EOPNOTSUPP;
1157 
1158 	skb_set_transport_header(skb, -(int)sizeof(struct icmphdr));
1159 	ret = gue_err_proto_handler(guehdr->proto_ctype, skb, info);
1160 
1161 out:
1162 	skb_set_transport_header(skb, transport_offset);
1163 	return ret;
1164 }
1165 
1166 
1167 static const struct ip_tunnel_encap_ops fou_iptun_ops = {
1168 	.encap_hlen = fou_encap_hlen,
1169 	.build_header = fou_build_header,
1170 	.err_handler = gue_err,
1171 };
1172 
1173 static const struct ip_tunnel_encap_ops gue_iptun_ops = {
1174 	.encap_hlen = gue_encap_hlen,
1175 	.build_header = gue_build_header,
1176 	.err_handler = gue_err,
1177 };
1178 
ip_tunnel_encap_add_fou_ops(void)1179 static int ip_tunnel_encap_add_fou_ops(void)
1180 {
1181 	int ret;
1182 
1183 	ret = ip_tunnel_encap_add_ops(&fou_iptun_ops, TUNNEL_ENCAP_FOU);
1184 	if (ret < 0) {
1185 		pr_err("can't add fou ops\n");
1186 		return ret;
1187 	}
1188 
1189 	ret = ip_tunnel_encap_add_ops(&gue_iptun_ops, TUNNEL_ENCAP_GUE);
1190 	if (ret < 0) {
1191 		pr_err("can't add gue ops\n");
1192 		ip_tunnel_encap_del_ops(&fou_iptun_ops, TUNNEL_ENCAP_FOU);
1193 		return ret;
1194 	}
1195 
1196 	return 0;
1197 }
1198 
ip_tunnel_encap_del_fou_ops(void)1199 static void ip_tunnel_encap_del_fou_ops(void)
1200 {
1201 	ip_tunnel_encap_del_ops(&fou_iptun_ops, TUNNEL_ENCAP_FOU);
1202 	ip_tunnel_encap_del_ops(&gue_iptun_ops, TUNNEL_ENCAP_GUE);
1203 }
1204 
1205 #else
1206 
ip_tunnel_encap_add_fou_ops(void)1207 static int ip_tunnel_encap_add_fou_ops(void)
1208 {
1209 	return 0;
1210 }
1211 
ip_tunnel_encap_del_fou_ops(void)1212 static void ip_tunnel_encap_del_fou_ops(void)
1213 {
1214 }
1215 
1216 #endif
1217 
fou_init_net(struct net * net)1218 static __net_init int fou_init_net(struct net *net)
1219 {
1220 	struct fou_net *fn = net_generic(net, fou_net_id);
1221 
1222 	INIT_LIST_HEAD(&fn->fou_list);
1223 	mutex_init(&fn->fou_lock);
1224 	return 0;
1225 }
1226 
fou_exit_net(struct net * net)1227 static __net_exit void fou_exit_net(struct net *net)
1228 {
1229 	struct fou_net *fn = net_generic(net, fou_net_id);
1230 	struct fou *fou, *next;
1231 
1232 	/* Close all the FOU sockets */
1233 	mutex_lock(&fn->fou_lock);
1234 	list_for_each_entry_safe(fou, next, &fn->fou_list, list)
1235 		fou_release(fou);
1236 	mutex_unlock(&fn->fou_lock);
1237 }
1238 
1239 static struct pernet_operations fou_net_ops = {
1240 	.init = fou_init_net,
1241 	.exit = fou_exit_net,
1242 	.id   = &fou_net_id,
1243 	.size = sizeof(struct fou_net),
1244 };
1245 
fou_init(void)1246 static int __init fou_init(void)
1247 {
1248 	int ret;
1249 
1250 	ret = register_pernet_device(&fou_net_ops);
1251 	if (ret)
1252 		goto exit;
1253 
1254 	ret = genl_register_family(&fou_nl_family);
1255 	if (ret < 0)
1256 		goto unregister;
1257 
1258 	ret = register_fou_bpf();
1259 	if (ret < 0)
1260 		goto kfunc_failed;
1261 
1262 	ret = ip_tunnel_encap_add_fou_ops();
1263 	if (ret == 0)
1264 		return 0;
1265 
1266 kfunc_failed:
1267 	genl_unregister_family(&fou_nl_family);
1268 unregister:
1269 	unregister_pernet_device(&fou_net_ops);
1270 exit:
1271 	return ret;
1272 }
1273 
fou_fini(void)1274 static void __exit fou_fini(void)
1275 {
1276 	ip_tunnel_encap_del_fou_ops();
1277 	genl_unregister_family(&fou_nl_family);
1278 	unregister_pernet_device(&fou_net_ops);
1279 }
1280 
1281 module_init(fou_init);
1282 module_exit(fou_fini);
1283 MODULE_AUTHOR("Tom Herbert <therbert@google.com>");
1284 MODULE_LICENSE("GPL");
1285 MODULE_DESCRIPTION("Foo over UDP");
1286