1 // SPDX-License-Identifier: GPL-2.0
2 /*
3  *	XFRM virtual interface
4  *
5  *	Copyright (C) 2018 secunet Security Networks AG
6  *
7  *	Author:
8  *	Steffen Klassert <steffen.klassert@secunet.com>
9  */
10 
11 #include <linux/module.h>
12 #include <linux/capability.h>
13 #include <linux/errno.h>
14 #include <linux/types.h>
15 #include <linux/sockios.h>
16 #include <linux/icmp.h>
17 #include <linux/if.h>
18 #include <linux/in.h>
19 #include <linux/ip.h>
20 #include <linux/net.h>
21 #include <linux/in6.h>
22 #include <linux/netdevice.h>
23 #include <linux/if_link.h>
24 #include <linux/if_arp.h>
25 #include <linux/icmpv6.h>
26 #include <linux/init.h>
27 #include <linux/route.h>
28 #include <linux/rtnetlink.h>
29 #include <linux/netfilter_ipv6.h>
30 #include <linux/slab.h>
31 #include <linux/hash.h>
32 
33 #include <linux/uaccess.h>
34 #include <linux/atomic.h>
35 
36 #include <net/icmp.h>
37 #include <net/ip.h>
38 #include <net/ipv6.h>
39 #include <net/ip6_route.h>
40 #include <net/ip_tunnels.h>
41 #include <net/addrconf.h>
42 #include <net/xfrm.h>
43 #include <net/net_namespace.h>
44 #include <net/dst_metadata.h>
45 #include <net/netns/generic.h>
46 #include <linux/etherdevice.h>
47 
48 static int xfrmi_dev_init(struct net_device *dev);
49 static void xfrmi_dev_setup(struct net_device *dev);
50 static struct rtnl_link_ops xfrmi_link_ops __read_mostly;
51 static unsigned int xfrmi_net_id __read_mostly;
52 static const struct net_device_ops xfrmi_netdev_ops;
53 
54 #define XFRMI_HASH_BITS	8
55 #define XFRMI_HASH_SIZE	BIT(XFRMI_HASH_BITS)
56 
57 struct xfrmi_net {
58 	/* lists for storing interfaces in use */
59 	struct xfrm_if __rcu *xfrmi[XFRMI_HASH_SIZE];
60 	struct xfrm_if __rcu *collect_md_xfrmi;
61 };
62 
63 static const struct nla_policy xfrm_lwt_policy[LWT_XFRM_MAX + 1] = {
64 	[LWT_XFRM_IF_ID]	= NLA_POLICY_MIN(NLA_U32, 1),
65 	[LWT_XFRM_LINK]		= NLA_POLICY_MIN(NLA_U32, 1),
66 };
67 
68 static void xfrmi_destroy_state(struct lwtunnel_state *lwt)
69 {
70 }
71 
72 static int xfrmi_build_state(struct net *net, struct nlattr *nla,
73 			     unsigned int family, const void *cfg,
74 			     struct lwtunnel_state **ts,
75 			     struct netlink_ext_ack *extack)
76 {
77 	struct nlattr *tb[LWT_XFRM_MAX + 1];
78 	struct lwtunnel_state *new_state;
79 	struct xfrm_md_info *info;
80 	int ret;
81 
82 	ret = nla_parse_nested(tb, LWT_XFRM_MAX, nla, xfrm_lwt_policy, extack);
83 	if (ret < 0)
84 		return ret;
85 
86 	if (!tb[LWT_XFRM_IF_ID]) {
87 		NL_SET_ERR_MSG(extack, "if_id must be set");
88 		return -EINVAL;
89 	}
90 
91 	new_state = lwtunnel_state_alloc(sizeof(*info));
92 	if (!new_state) {
93 		NL_SET_ERR_MSG(extack, "failed to create encap info");
94 		return -ENOMEM;
95 	}
96 
97 	new_state->type = LWTUNNEL_ENCAP_XFRM;
98 
99 	info = lwt_xfrm_info(new_state);
100 
101 	info->if_id = nla_get_u32(tb[LWT_XFRM_IF_ID]);
102 
103 	if (tb[LWT_XFRM_LINK])
104 		info->link = nla_get_u32(tb[LWT_XFRM_LINK]);
105 
106 	*ts = new_state;
107 	return 0;
108 }
109 
110 static int xfrmi_fill_encap_info(struct sk_buff *skb,
111 				 struct lwtunnel_state *lwt)
112 {
113 	struct xfrm_md_info *info = lwt_xfrm_info(lwt);
114 
115 	if (nla_put_u32(skb, LWT_XFRM_IF_ID, info->if_id) ||
116 	    (info->link && nla_put_u32(skb, LWT_XFRM_LINK, info->link)))
117 		return -EMSGSIZE;
118 
119 	return 0;
120 }
121 
122 static int xfrmi_encap_nlsize(struct lwtunnel_state *lwtstate)
123 {
124 	return nla_total_size(sizeof(u32)) + /* LWT_XFRM_IF_ID */
125 		nla_total_size(sizeof(u32)); /* LWT_XFRM_LINK */
126 }
127 
128 static int xfrmi_encap_cmp(struct lwtunnel_state *a, struct lwtunnel_state *b)
129 {
130 	struct xfrm_md_info *a_info = lwt_xfrm_info(a);
131 	struct xfrm_md_info *b_info = lwt_xfrm_info(b);
132 
133 	return memcmp(a_info, b_info, sizeof(*a_info));
134 }
135 
136 static const struct lwtunnel_encap_ops xfrmi_encap_ops = {
137 	.build_state	= xfrmi_build_state,
138 	.destroy_state	= xfrmi_destroy_state,
139 	.fill_encap	= xfrmi_fill_encap_info,
140 	.get_encap_size = xfrmi_encap_nlsize,
141 	.cmp_encap	= xfrmi_encap_cmp,
142 	.owner		= THIS_MODULE,
143 };
144 
145 #define for_each_xfrmi_rcu(start, xi) \
146 	for (xi = rcu_dereference(start); xi; xi = rcu_dereference(xi->next))
147 
148 static u32 xfrmi_hash(u32 if_id)
149 {
150 	return hash_32(if_id, XFRMI_HASH_BITS);
151 }
152 
153 static struct xfrm_if *xfrmi_lookup(struct net *net, struct xfrm_state *x)
154 {
155 	struct xfrmi_net *xfrmn = net_generic(net, xfrmi_net_id);
156 	struct xfrm_if *xi;
157 
158 	for_each_xfrmi_rcu(xfrmn->xfrmi[xfrmi_hash(x->if_id)], xi) {
159 		if (x->if_id == xi->p.if_id &&
160 		    (xi->dev->flags & IFF_UP))
161 			return xi;
162 	}
163 
164 	xi = rcu_dereference(xfrmn->collect_md_xfrmi);
165 	if (xi && (xi->dev->flags & IFF_UP))
166 		return xi;
167 
168 	return NULL;
169 }
170 
171 static bool xfrmi_decode_session(struct sk_buff *skb,
172 				 unsigned short family,
173 				 struct xfrm_if_decode_session_result *res)
174 {
175 	struct net_device *dev;
176 	struct xfrm_if *xi;
177 	int ifindex = 0;
178 
179 	if (!secpath_exists(skb) || !skb->dev)
180 		return false;
181 
182 	switch (family) {
183 	case AF_INET6:
184 		ifindex = inet6_sdif(skb);
185 		break;
186 	case AF_INET:
187 		ifindex = inet_sdif(skb);
188 		break;
189 	}
190 
191 	if (ifindex) {
192 		struct net *net = xs_net(xfrm_input_state(skb));
193 
194 		dev = dev_get_by_index_rcu(net, ifindex);
195 	} else {
196 		dev = skb->dev;
197 	}
198 
199 	if (!dev || !(dev->flags & IFF_UP))
200 		return false;
201 	if (dev->netdev_ops != &xfrmi_netdev_ops)
202 		return false;
203 
204 	xi = netdev_priv(dev);
205 	res->net = xi->net;
206 
207 	if (xi->p.collect_md)
208 		res->if_id = xfrm_input_state(skb)->if_id;
209 	else
210 		res->if_id = xi->p.if_id;
211 	return true;
212 }
213 
214 static void xfrmi_link(struct xfrmi_net *xfrmn, struct xfrm_if *xi)
215 {
216 	struct xfrm_if __rcu **xip = &xfrmn->xfrmi[xfrmi_hash(xi->p.if_id)];
217 
218 	rcu_assign_pointer(xi->next , rtnl_dereference(*xip));
219 	rcu_assign_pointer(*xip, xi);
220 }
221 
222 static void xfrmi_unlink(struct xfrmi_net *xfrmn, struct xfrm_if *xi)
223 {
224 	struct xfrm_if __rcu **xip;
225 	struct xfrm_if *iter;
226 
227 	for (xip = &xfrmn->xfrmi[xfrmi_hash(xi->p.if_id)];
228 	     (iter = rtnl_dereference(*xip)) != NULL;
229 	     xip = &iter->next) {
230 		if (xi == iter) {
231 			rcu_assign_pointer(*xip, xi->next);
232 			break;
233 		}
234 	}
235 }
236 
237 static void xfrmi_dev_free(struct net_device *dev)
238 {
239 	struct xfrm_if *xi = netdev_priv(dev);
240 
241 	gro_cells_destroy(&xi->gro_cells);
242 	free_percpu(dev->tstats);
243 }
244 
245 static int xfrmi_create(struct net_device *dev)
246 {
247 	struct xfrm_if *xi = netdev_priv(dev);
248 	struct net *net = dev_net(dev);
249 	struct xfrmi_net *xfrmn = net_generic(net, xfrmi_net_id);
250 	int err;
251 
252 	dev->rtnl_link_ops = &xfrmi_link_ops;
253 	err = register_netdevice(dev);
254 	if (err < 0)
255 		goto out;
256 
257 	if (xi->p.collect_md)
258 		rcu_assign_pointer(xfrmn->collect_md_xfrmi, xi);
259 	else
260 		xfrmi_link(xfrmn, xi);
261 
262 	return 0;
263 
264 out:
265 	return err;
266 }
267 
268 static struct xfrm_if *xfrmi_locate(struct net *net, struct xfrm_if_parms *p)
269 {
270 	struct xfrm_if __rcu **xip;
271 	struct xfrm_if *xi;
272 	struct xfrmi_net *xfrmn = net_generic(net, xfrmi_net_id);
273 
274 	for (xip = &xfrmn->xfrmi[xfrmi_hash(p->if_id)];
275 	     (xi = rtnl_dereference(*xip)) != NULL;
276 	     xip = &xi->next)
277 		if (xi->p.if_id == p->if_id)
278 			return xi;
279 
280 	return NULL;
281 }
282 
283 static void xfrmi_dev_uninit(struct net_device *dev)
284 {
285 	struct xfrm_if *xi = netdev_priv(dev);
286 	struct xfrmi_net *xfrmn = net_generic(xi->net, xfrmi_net_id);
287 
288 	if (xi->p.collect_md)
289 		RCU_INIT_POINTER(xfrmn->collect_md_xfrmi, NULL);
290 	else
291 		xfrmi_unlink(xfrmn, xi);
292 }
293 
294 static void xfrmi_scrub_packet(struct sk_buff *skb, bool xnet)
295 {
296 	skb_clear_tstamp(skb);
297 	skb->pkt_type = PACKET_HOST;
298 	skb->skb_iif = 0;
299 	skb->ignore_df = 0;
300 	skb_dst_drop(skb);
301 	nf_reset_ct(skb);
302 	nf_reset_trace(skb);
303 
304 	if (!xnet)
305 		return;
306 
307 	ipvs_reset(skb);
308 	secpath_reset(skb);
309 	skb_orphan(skb);
310 	skb->mark = 0;
311 }
312 
313 static int xfrmi_input(struct sk_buff *skb, int nexthdr, __be32 spi,
314 		       int encap_type, unsigned short family)
315 {
316 	struct sec_path *sp;
317 
318 	sp = skb_sec_path(skb);
319 	if (sp && (sp->len || sp->olen) &&
320 	    !xfrm_policy_check(NULL, XFRM_POLICY_IN, skb, family))
321 		goto discard;
322 
323 	XFRM_SPI_SKB_CB(skb)->family = family;
324 	if (family == AF_INET) {
325 		XFRM_SPI_SKB_CB(skb)->daddroff = offsetof(struct iphdr, daddr);
326 		XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip4 = NULL;
327 	} else {
328 		XFRM_SPI_SKB_CB(skb)->daddroff = offsetof(struct ipv6hdr, daddr);
329 		XFRM_TUNNEL_SKB_CB(skb)->tunnel.ip6 = NULL;
330 	}
331 
332 	return xfrm_input(skb, nexthdr, spi, encap_type);
333 discard:
334 	kfree_skb(skb);
335 	return 0;
336 }
337 
338 static int xfrmi4_rcv(struct sk_buff *skb)
339 {
340 	return xfrmi_input(skb, ip_hdr(skb)->protocol, 0, 0, AF_INET);
341 }
342 
343 static int xfrmi6_rcv(struct sk_buff *skb)
344 {
345 	return xfrmi_input(skb, skb_network_header(skb)[IP6CB(skb)->nhoff],
346 			   0, 0, AF_INET6);
347 }
348 
349 static int xfrmi4_input(struct sk_buff *skb, int nexthdr, __be32 spi, int encap_type)
350 {
351 	return xfrmi_input(skb, nexthdr, spi, encap_type, AF_INET);
352 }
353 
354 static int xfrmi6_input(struct sk_buff *skb, int nexthdr, __be32 spi, int encap_type)
355 {
356 	return xfrmi_input(skb, nexthdr, spi, encap_type, AF_INET6);
357 }
358 
359 static int xfrmi_rcv_cb(struct sk_buff *skb, int err)
360 {
361 	const struct xfrm_mode *inner_mode;
362 	struct net_device *dev;
363 	struct xfrm_state *x;
364 	struct xfrm_if *xi;
365 	bool xnet;
366 	int link;
367 
368 	if (err && !secpath_exists(skb))
369 		return 0;
370 
371 	x = xfrm_input_state(skb);
372 
373 	xi = xfrmi_lookup(xs_net(x), x);
374 	if (!xi)
375 		return 1;
376 
377 	link = skb->dev->ifindex;
378 	dev = xi->dev;
379 	skb->dev = dev;
380 
381 	if (err) {
382 		dev->stats.rx_errors++;
383 		dev->stats.rx_dropped++;
384 
385 		return 0;
386 	}
387 
388 	xnet = !net_eq(xi->net, dev_net(skb->dev));
389 
390 	if (xnet) {
391 		inner_mode = &x->inner_mode;
392 
393 		if (x->sel.family == AF_UNSPEC) {
394 			inner_mode = xfrm_ip2inner_mode(x, XFRM_MODE_SKB_CB(skb)->protocol);
395 			if (inner_mode == NULL) {
396 				XFRM_INC_STATS(dev_net(skb->dev),
397 					       LINUX_MIB_XFRMINSTATEMODEERROR);
398 				return -EINVAL;
399 			}
400 		}
401 
402 		if (!xfrm_policy_check(NULL, XFRM_POLICY_IN, skb,
403 				       inner_mode->family))
404 			return -EPERM;
405 	}
406 
407 	xfrmi_scrub_packet(skb, xnet);
408 	if (xi->p.collect_md) {
409 		struct metadata_dst *md_dst;
410 
411 		md_dst = metadata_dst_alloc(0, METADATA_XFRM, GFP_ATOMIC);
412 		if (!md_dst)
413 			return -ENOMEM;
414 
415 		md_dst->u.xfrm_info.if_id = x->if_id;
416 		md_dst->u.xfrm_info.link = link;
417 		skb_dst_set(skb, (struct dst_entry *)md_dst);
418 	}
419 	dev_sw_netstats_rx_add(dev, skb->len);
420 
421 	return 0;
422 }
423 
424 static int
425 xfrmi_xmit2(struct sk_buff *skb, struct net_device *dev, struct flowi *fl)
426 {
427 	struct xfrm_if *xi = netdev_priv(dev);
428 	struct net_device_stats *stats = &xi->dev->stats;
429 	struct dst_entry *dst = skb_dst(skb);
430 	unsigned int length = skb->len;
431 	struct net_device *tdev;
432 	struct xfrm_state *x;
433 	int err = -1;
434 	u32 if_id;
435 	int mtu;
436 
437 	if (xi->p.collect_md) {
438 		struct xfrm_md_info *md_info = skb_xfrm_md_info(skb);
439 
440 		if (unlikely(!md_info))
441 			return -EINVAL;
442 
443 		if_id = md_info->if_id;
444 		fl->flowi_oif = md_info->link;
445 		if (md_info->dst_orig) {
446 			struct dst_entry *tmp_dst = dst;
447 
448 			dst = md_info->dst_orig;
449 			skb_dst_set(skb, dst);
450 			md_info->dst_orig = NULL;
451 			dst_release(tmp_dst);
452 		}
453 	} else {
454 		if_id = xi->p.if_id;
455 	}
456 
457 	dst_hold(dst);
458 	dst = xfrm_lookup_with_ifid(xi->net, dst, fl, NULL, 0, if_id);
459 	if (IS_ERR(dst)) {
460 		err = PTR_ERR(dst);
461 		dst = NULL;
462 		goto tx_err_link_failure;
463 	}
464 
465 	x = dst->xfrm;
466 	if (!x)
467 		goto tx_err_link_failure;
468 
469 	if (x->if_id != if_id)
470 		goto tx_err_link_failure;
471 
472 	tdev = dst->dev;
473 
474 	if (tdev == dev) {
475 		stats->collisions++;
476 		net_warn_ratelimited("%s: Local routing loop detected!\n",
477 				     dev->name);
478 		goto tx_err_dst_release;
479 	}
480 
481 	mtu = dst_mtu(dst);
482 	if ((!skb_is_gso(skb) && skb->len > mtu) ||
483 	    (skb_is_gso(skb) && !skb_gso_validate_network_len(skb, mtu))) {
484 		skb_dst_update_pmtu_no_confirm(skb, mtu);
485 
486 		if (skb->protocol == htons(ETH_P_IPV6)) {
487 			if (mtu < IPV6_MIN_MTU)
488 				mtu = IPV6_MIN_MTU;
489 
490 			if (skb->len > 1280)
491 				icmpv6_ndo_send(skb, ICMPV6_PKT_TOOBIG, 0, mtu);
492 			else
493 				goto xmit;
494 		} else {
495 			if (!(ip_hdr(skb)->frag_off & htons(IP_DF)))
496 				goto xmit;
497 			icmp_ndo_send(skb, ICMP_DEST_UNREACH, ICMP_FRAG_NEEDED,
498 				      htonl(mtu));
499 		}
500 
501 		dst_release(dst);
502 		return -EMSGSIZE;
503 	}
504 
505 xmit:
506 	xfrmi_scrub_packet(skb, !net_eq(xi->net, dev_net(dev)));
507 	skb_dst_set(skb, dst);
508 	skb->dev = tdev;
509 
510 	err = dst_output(xi->net, skb->sk, skb);
511 	if (net_xmit_eval(err) == 0) {
512 		dev_sw_netstats_tx_add(dev, 1, length);
513 	} else {
514 		stats->tx_errors++;
515 		stats->tx_aborted_errors++;
516 	}
517 
518 	return 0;
519 tx_err_link_failure:
520 	stats->tx_carrier_errors++;
521 	dst_link_failure(skb);
522 tx_err_dst_release:
523 	dst_release(dst);
524 	return err;
525 }
526 
527 static netdev_tx_t xfrmi_xmit(struct sk_buff *skb, struct net_device *dev)
528 {
529 	struct xfrm_if *xi = netdev_priv(dev);
530 	struct net_device_stats *stats = &xi->dev->stats;
531 	struct dst_entry *dst = skb_dst(skb);
532 	struct flowi fl;
533 	int ret;
534 
535 	memset(&fl, 0, sizeof(fl));
536 
537 	switch (skb->protocol) {
538 	case htons(ETH_P_IPV6):
539 		xfrm_decode_session(skb, &fl, AF_INET6);
540 		memset(IP6CB(skb), 0, sizeof(*IP6CB(skb)));
541 		if (!dst) {
542 			fl.u.ip6.flowi6_oif = dev->ifindex;
543 			fl.u.ip6.flowi6_flags |= FLOWI_FLAG_ANYSRC;
544 			dst = ip6_route_output(dev_net(dev), NULL, &fl.u.ip6);
545 			if (dst->error) {
546 				dst_release(dst);
547 				stats->tx_carrier_errors++;
548 				goto tx_err;
549 			}
550 			skb_dst_set(skb, dst);
551 		}
552 		break;
553 	case htons(ETH_P_IP):
554 		xfrm_decode_session(skb, &fl, AF_INET);
555 		memset(IPCB(skb), 0, sizeof(*IPCB(skb)));
556 		if (!dst) {
557 			struct rtable *rt;
558 
559 			fl.u.ip4.flowi4_oif = dev->ifindex;
560 			fl.u.ip4.flowi4_flags |= FLOWI_FLAG_ANYSRC;
561 			rt = __ip_route_output_key(dev_net(dev), &fl.u.ip4);
562 			if (IS_ERR(rt)) {
563 				stats->tx_carrier_errors++;
564 				goto tx_err;
565 			}
566 			skb_dst_set(skb, &rt->dst);
567 		}
568 		break;
569 	default:
570 		goto tx_err;
571 	}
572 
573 	fl.flowi_oif = xi->p.link;
574 
575 	ret = xfrmi_xmit2(skb, dev, &fl);
576 	if (ret < 0)
577 		goto tx_err;
578 
579 	return NETDEV_TX_OK;
580 
581 tx_err:
582 	stats->tx_errors++;
583 	stats->tx_dropped++;
584 	kfree_skb(skb);
585 	return NETDEV_TX_OK;
586 }
587 
588 static int xfrmi4_err(struct sk_buff *skb, u32 info)
589 {
590 	const struct iphdr *iph = (const struct iphdr *)skb->data;
591 	struct net *net = dev_net(skb->dev);
592 	int protocol = iph->protocol;
593 	struct ip_comp_hdr *ipch;
594 	struct ip_esp_hdr *esph;
595 	struct ip_auth_hdr *ah ;
596 	struct xfrm_state *x;
597 	struct xfrm_if *xi;
598 	__be32 spi;
599 
600 	switch (protocol) {
601 	case IPPROTO_ESP:
602 		esph = (struct ip_esp_hdr *)(skb->data+(iph->ihl<<2));
603 		spi = esph->spi;
604 		break;
605 	case IPPROTO_AH:
606 		ah = (struct ip_auth_hdr *)(skb->data+(iph->ihl<<2));
607 		spi = ah->spi;
608 		break;
609 	case IPPROTO_COMP:
610 		ipch = (struct ip_comp_hdr *)(skb->data+(iph->ihl<<2));
611 		spi = htonl(ntohs(ipch->cpi));
612 		break;
613 	default:
614 		return 0;
615 	}
616 
617 	switch (icmp_hdr(skb)->type) {
618 	case ICMP_DEST_UNREACH:
619 		if (icmp_hdr(skb)->code != ICMP_FRAG_NEEDED)
620 			return 0;
621 		break;
622 	case ICMP_REDIRECT:
623 		break;
624 	default:
625 		return 0;
626 	}
627 
628 	x = xfrm_state_lookup(net, skb->mark, (const xfrm_address_t *)&iph->daddr,
629 			      spi, protocol, AF_INET);
630 	if (!x)
631 		return 0;
632 
633 	xi = xfrmi_lookup(net, x);
634 	if (!xi) {
635 		xfrm_state_put(x);
636 		return -1;
637 	}
638 
639 	if (icmp_hdr(skb)->type == ICMP_DEST_UNREACH)
640 		ipv4_update_pmtu(skb, net, info, 0, protocol);
641 	else
642 		ipv4_redirect(skb, net, 0, protocol);
643 	xfrm_state_put(x);
644 
645 	return 0;
646 }
647 
648 static int xfrmi6_err(struct sk_buff *skb, struct inet6_skb_parm *opt,
649 		    u8 type, u8 code, int offset, __be32 info)
650 {
651 	const struct ipv6hdr *iph = (const struct ipv6hdr *)skb->data;
652 	struct net *net = dev_net(skb->dev);
653 	int protocol = iph->nexthdr;
654 	struct ip_comp_hdr *ipch;
655 	struct ip_esp_hdr *esph;
656 	struct ip_auth_hdr *ah;
657 	struct xfrm_state *x;
658 	struct xfrm_if *xi;
659 	__be32 spi;
660 
661 	switch (protocol) {
662 	case IPPROTO_ESP:
663 		esph = (struct ip_esp_hdr *)(skb->data + offset);
664 		spi = esph->spi;
665 		break;
666 	case IPPROTO_AH:
667 		ah = (struct ip_auth_hdr *)(skb->data + offset);
668 		spi = ah->spi;
669 		break;
670 	case IPPROTO_COMP:
671 		ipch = (struct ip_comp_hdr *)(skb->data + offset);
672 		spi = htonl(ntohs(ipch->cpi));
673 		break;
674 	default:
675 		return 0;
676 	}
677 
678 	if (type != ICMPV6_PKT_TOOBIG &&
679 	    type != NDISC_REDIRECT)
680 		return 0;
681 
682 	x = xfrm_state_lookup(net, skb->mark, (const xfrm_address_t *)&iph->daddr,
683 			      spi, protocol, AF_INET6);
684 	if (!x)
685 		return 0;
686 
687 	xi = xfrmi_lookup(net, x);
688 	if (!xi) {
689 		xfrm_state_put(x);
690 		return -1;
691 	}
692 
693 	if (type == NDISC_REDIRECT)
694 		ip6_redirect(skb, net, skb->dev->ifindex, 0,
695 			     sock_net_uid(net, NULL));
696 	else
697 		ip6_update_pmtu(skb, net, info, 0, 0, sock_net_uid(net, NULL));
698 	xfrm_state_put(x);
699 
700 	return 0;
701 }
702 
703 static int xfrmi_change(struct xfrm_if *xi, const struct xfrm_if_parms *p)
704 {
705 	if (xi->p.link != p->link)
706 		return -EINVAL;
707 
708 	xi->p.if_id = p->if_id;
709 
710 	return 0;
711 }
712 
713 static int xfrmi_update(struct xfrm_if *xi, struct xfrm_if_parms *p)
714 {
715 	struct net *net = xi->net;
716 	struct xfrmi_net *xfrmn = net_generic(net, xfrmi_net_id);
717 	int err;
718 
719 	xfrmi_unlink(xfrmn, xi);
720 	synchronize_net();
721 	err = xfrmi_change(xi, p);
722 	xfrmi_link(xfrmn, xi);
723 	netdev_state_change(xi->dev);
724 	return err;
725 }
726 
727 static int xfrmi_get_iflink(const struct net_device *dev)
728 {
729 	struct xfrm_if *xi = netdev_priv(dev);
730 
731 	return xi->p.link;
732 }
733 
734 static const struct net_device_ops xfrmi_netdev_ops = {
735 	.ndo_init	= xfrmi_dev_init,
736 	.ndo_uninit	= xfrmi_dev_uninit,
737 	.ndo_start_xmit = xfrmi_xmit,
738 	.ndo_get_stats64 = dev_get_tstats64,
739 	.ndo_get_iflink = xfrmi_get_iflink,
740 };
741 
742 static void xfrmi_dev_setup(struct net_device *dev)
743 {
744 	dev->netdev_ops 	= &xfrmi_netdev_ops;
745 	dev->header_ops		= &ip_tunnel_header_ops;
746 	dev->type		= ARPHRD_NONE;
747 	dev->mtu		= ETH_DATA_LEN;
748 	dev->min_mtu		= ETH_MIN_MTU;
749 	dev->max_mtu		= IP_MAX_MTU;
750 	dev->flags 		= IFF_NOARP;
751 	dev->needs_free_netdev	= true;
752 	dev->priv_destructor	= xfrmi_dev_free;
753 	netif_keep_dst(dev);
754 
755 	eth_broadcast_addr(dev->broadcast);
756 }
757 
758 #define XFRMI_FEATURES (NETIF_F_SG |		\
759 			NETIF_F_FRAGLIST |	\
760 			NETIF_F_GSO_SOFTWARE |	\
761 			NETIF_F_HW_CSUM)
762 
763 static int xfrmi_dev_init(struct net_device *dev)
764 {
765 	struct xfrm_if *xi = netdev_priv(dev);
766 	struct net_device *phydev = __dev_get_by_index(xi->net, xi->p.link);
767 	int err;
768 
769 	dev->tstats = netdev_alloc_pcpu_stats(struct pcpu_sw_netstats);
770 	if (!dev->tstats)
771 		return -ENOMEM;
772 
773 	err = gro_cells_init(&xi->gro_cells, dev);
774 	if (err) {
775 		free_percpu(dev->tstats);
776 		return err;
777 	}
778 
779 	dev->features |= NETIF_F_LLTX;
780 	dev->features |= XFRMI_FEATURES;
781 	dev->hw_features |= XFRMI_FEATURES;
782 
783 	if (phydev) {
784 		dev->needed_headroom = phydev->needed_headroom;
785 		dev->needed_tailroom = phydev->needed_tailroom;
786 
787 		if (is_zero_ether_addr(dev->dev_addr))
788 			eth_hw_addr_inherit(dev, phydev);
789 		if (is_zero_ether_addr(dev->broadcast))
790 			memcpy(dev->broadcast, phydev->broadcast,
791 			       dev->addr_len);
792 	} else {
793 		eth_hw_addr_random(dev);
794 		eth_broadcast_addr(dev->broadcast);
795 	}
796 
797 	return 0;
798 }
799 
800 static int xfrmi_validate(struct nlattr *tb[], struct nlattr *data[],
801 			 struct netlink_ext_ack *extack)
802 {
803 	return 0;
804 }
805 
806 static void xfrmi_netlink_parms(struct nlattr *data[],
807 			       struct xfrm_if_parms *parms)
808 {
809 	memset(parms, 0, sizeof(*parms));
810 
811 	if (!data)
812 		return;
813 
814 	if (data[IFLA_XFRM_LINK])
815 		parms->link = nla_get_u32(data[IFLA_XFRM_LINK]);
816 
817 	if (data[IFLA_XFRM_IF_ID])
818 		parms->if_id = nla_get_u32(data[IFLA_XFRM_IF_ID]);
819 
820 	if (data[IFLA_XFRM_COLLECT_METADATA])
821 		parms->collect_md = true;
822 }
823 
824 static int xfrmi_newlink(struct net *src_net, struct net_device *dev,
825 			struct nlattr *tb[], struct nlattr *data[],
826 			struct netlink_ext_ack *extack)
827 {
828 	struct net *net = dev_net(dev);
829 	struct xfrm_if_parms p = {};
830 	struct xfrm_if *xi;
831 	int err;
832 
833 	xfrmi_netlink_parms(data, &p);
834 	if (p.collect_md) {
835 		struct xfrmi_net *xfrmn = net_generic(net, xfrmi_net_id);
836 
837 		if (p.link || p.if_id) {
838 			NL_SET_ERR_MSG(extack, "link and if_id must be zero");
839 			return -EINVAL;
840 		}
841 
842 		if (rtnl_dereference(xfrmn->collect_md_xfrmi))
843 			return -EEXIST;
844 
845 	} else {
846 		if (!p.if_id) {
847 			NL_SET_ERR_MSG(extack, "if_id must be non zero");
848 			return -EINVAL;
849 		}
850 
851 		xi = xfrmi_locate(net, &p);
852 		if (xi)
853 			return -EEXIST;
854 	}
855 
856 	xi = netdev_priv(dev);
857 	xi->p = p;
858 	xi->net = net;
859 	xi->dev = dev;
860 
861 	err = xfrmi_create(dev);
862 	return err;
863 }
864 
865 static void xfrmi_dellink(struct net_device *dev, struct list_head *head)
866 {
867 	unregister_netdevice_queue(dev, head);
868 }
869 
870 static int xfrmi_changelink(struct net_device *dev, struct nlattr *tb[],
871 			   struct nlattr *data[],
872 			   struct netlink_ext_ack *extack)
873 {
874 	struct xfrm_if *xi = netdev_priv(dev);
875 	struct net *net = xi->net;
876 	struct xfrm_if_parms p = {};
877 
878 	xfrmi_netlink_parms(data, &p);
879 	if (!p.if_id) {
880 		NL_SET_ERR_MSG(extack, "if_id must be non zero");
881 		return -EINVAL;
882 	}
883 
884 	if (p.collect_md) {
885 		NL_SET_ERR_MSG(extack, "collect_md can't be changed");
886 		return -EINVAL;
887 	}
888 
889 	xi = xfrmi_locate(net, &p);
890 	if (!xi) {
891 		xi = netdev_priv(dev);
892 	} else {
893 		if (xi->dev != dev)
894 			return -EEXIST;
895 		if (xi->p.collect_md) {
896 			NL_SET_ERR_MSG(extack,
897 				       "device can't be changed to collect_md");
898 			return -EINVAL;
899 		}
900 	}
901 
902 	return xfrmi_update(xi, &p);
903 }
904 
905 static size_t xfrmi_get_size(const struct net_device *dev)
906 {
907 	return
908 		/* IFLA_XFRM_LINK */
909 		nla_total_size(4) +
910 		/* IFLA_XFRM_IF_ID */
911 		nla_total_size(4) +
912 		/* IFLA_XFRM_COLLECT_METADATA */
913 		nla_total_size(0) +
914 		0;
915 }
916 
917 static int xfrmi_fill_info(struct sk_buff *skb, const struct net_device *dev)
918 {
919 	struct xfrm_if *xi = netdev_priv(dev);
920 	struct xfrm_if_parms *parm = &xi->p;
921 
922 	if (nla_put_u32(skb, IFLA_XFRM_LINK, parm->link) ||
923 	    nla_put_u32(skb, IFLA_XFRM_IF_ID, parm->if_id) ||
924 	    (xi->p.collect_md && nla_put_flag(skb, IFLA_XFRM_COLLECT_METADATA)))
925 		goto nla_put_failure;
926 	return 0;
927 
928 nla_put_failure:
929 	return -EMSGSIZE;
930 }
931 
932 static struct net *xfrmi_get_link_net(const struct net_device *dev)
933 {
934 	struct xfrm_if *xi = netdev_priv(dev);
935 
936 	return xi->net;
937 }
938 
939 static const struct nla_policy xfrmi_policy[IFLA_XFRM_MAX + 1] = {
940 	[IFLA_XFRM_UNSPEC]		= { .strict_start_type = IFLA_XFRM_COLLECT_METADATA },
941 	[IFLA_XFRM_LINK]		= { .type = NLA_U32 },
942 	[IFLA_XFRM_IF_ID]		= { .type = NLA_U32 },
943 	[IFLA_XFRM_COLLECT_METADATA]	= { .type = NLA_FLAG },
944 };
945 
946 static struct rtnl_link_ops xfrmi_link_ops __read_mostly = {
947 	.kind		= "xfrm",
948 	.maxtype	= IFLA_XFRM_MAX,
949 	.policy		= xfrmi_policy,
950 	.priv_size	= sizeof(struct xfrm_if),
951 	.setup		= xfrmi_dev_setup,
952 	.validate	= xfrmi_validate,
953 	.newlink	= xfrmi_newlink,
954 	.dellink	= xfrmi_dellink,
955 	.changelink	= xfrmi_changelink,
956 	.get_size	= xfrmi_get_size,
957 	.fill_info	= xfrmi_fill_info,
958 	.get_link_net	= xfrmi_get_link_net,
959 };
960 
961 static void __net_exit xfrmi_exit_batch_net(struct list_head *net_exit_list)
962 {
963 	struct net *net;
964 	LIST_HEAD(list);
965 
966 	rtnl_lock();
967 	list_for_each_entry(net, net_exit_list, exit_list) {
968 		struct xfrmi_net *xfrmn = net_generic(net, xfrmi_net_id);
969 		struct xfrm_if __rcu **xip;
970 		struct xfrm_if *xi;
971 		int i;
972 
973 		for (i = 0; i < XFRMI_HASH_SIZE; i++) {
974 			for (xip = &xfrmn->xfrmi[i];
975 			     (xi = rtnl_dereference(*xip)) != NULL;
976 			     xip = &xi->next)
977 				unregister_netdevice_queue(xi->dev, &list);
978 		}
979 		xi = rtnl_dereference(xfrmn->collect_md_xfrmi);
980 		if (xi)
981 			unregister_netdevice_queue(xi->dev, &list);
982 	}
983 	unregister_netdevice_many(&list);
984 	rtnl_unlock();
985 }
986 
987 static struct pernet_operations xfrmi_net_ops = {
988 	.exit_batch = xfrmi_exit_batch_net,
989 	.id   = &xfrmi_net_id,
990 	.size = sizeof(struct xfrmi_net),
991 };
992 
993 static struct xfrm6_protocol xfrmi_esp6_protocol __read_mostly = {
994 	.handler	=	xfrmi6_rcv,
995 	.input_handler	=	xfrmi6_input,
996 	.cb_handler	=	xfrmi_rcv_cb,
997 	.err_handler	=	xfrmi6_err,
998 	.priority	=	10,
999 };
1000 
1001 static struct xfrm6_protocol xfrmi_ah6_protocol __read_mostly = {
1002 	.handler	=	xfrm6_rcv,
1003 	.input_handler	=	xfrm_input,
1004 	.cb_handler	=	xfrmi_rcv_cb,
1005 	.err_handler	=	xfrmi6_err,
1006 	.priority	=	10,
1007 };
1008 
1009 static struct xfrm6_protocol xfrmi_ipcomp6_protocol __read_mostly = {
1010 	.handler	=	xfrm6_rcv,
1011 	.input_handler	=	xfrm_input,
1012 	.cb_handler	=	xfrmi_rcv_cb,
1013 	.err_handler	=	xfrmi6_err,
1014 	.priority	=	10,
1015 };
1016 
1017 #if IS_REACHABLE(CONFIG_INET6_XFRM_TUNNEL)
1018 static int xfrmi6_rcv_tunnel(struct sk_buff *skb)
1019 {
1020 	const xfrm_address_t *saddr;
1021 	__be32 spi;
1022 
1023 	saddr = (const xfrm_address_t *)&ipv6_hdr(skb)->saddr;
1024 	spi = xfrm6_tunnel_spi_lookup(dev_net(skb->dev), saddr);
1025 
1026 	return xfrm6_rcv_spi(skb, IPPROTO_IPV6, spi, NULL);
1027 }
1028 
1029 static struct xfrm6_tunnel xfrmi_ipv6_handler __read_mostly = {
1030 	.handler	=	xfrmi6_rcv_tunnel,
1031 	.cb_handler	=	xfrmi_rcv_cb,
1032 	.err_handler	=	xfrmi6_err,
1033 	.priority	=	2,
1034 };
1035 
1036 static struct xfrm6_tunnel xfrmi_ip6ip_handler __read_mostly = {
1037 	.handler	=	xfrmi6_rcv_tunnel,
1038 	.cb_handler	=	xfrmi_rcv_cb,
1039 	.err_handler	=	xfrmi6_err,
1040 	.priority	=	2,
1041 };
1042 #endif
1043 
1044 static struct xfrm4_protocol xfrmi_esp4_protocol __read_mostly = {
1045 	.handler	=	xfrmi4_rcv,
1046 	.input_handler	=	xfrmi4_input,
1047 	.cb_handler	=	xfrmi_rcv_cb,
1048 	.err_handler	=	xfrmi4_err,
1049 	.priority	=	10,
1050 };
1051 
1052 static struct xfrm4_protocol xfrmi_ah4_protocol __read_mostly = {
1053 	.handler	=	xfrm4_rcv,
1054 	.input_handler	=	xfrm_input,
1055 	.cb_handler	=	xfrmi_rcv_cb,
1056 	.err_handler	=	xfrmi4_err,
1057 	.priority	=	10,
1058 };
1059 
1060 static struct xfrm4_protocol xfrmi_ipcomp4_protocol __read_mostly = {
1061 	.handler	=	xfrm4_rcv,
1062 	.input_handler	=	xfrm_input,
1063 	.cb_handler	=	xfrmi_rcv_cb,
1064 	.err_handler	=	xfrmi4_err,
1065 	.priority	=	10,
1066 };
1067 
1068 #if IS_REACHABLE(CONFIG_INET_XFRM_TUNNEL)
1069 static int xfrmi4_rcv_tunnel(struct sk_buff *skb)
1070 {
1071 	return xfrm4_rcv_spi(skb, IPPROTO_IPIP, ip_hdr(skb)->saddr);
1072 }
1073 
1074 static struct xfrm_tunnel xfrmi_ipip_handler __read_mostly = {
1075 	.handler	=	xfrmi4_rcv_tunnel,
1076 	.cb_handler	=	xfrmi_rcv_cb,
1077 	.err_handler	=	xfrmi4_err,
1078 	.priority	=	3,
1079 };
1080 
1081 static struct xfrm_tunnel xfrmi_ipip6_handler __read_mostly = {
1082 	.handler	=	xfrmi4_rcv_tunnel,
1083 	.cb_handler	=	xfrmi_rcv_cb,
1084 	.err_handler	=	xfrmi4_err,
1085 	.priority	=	2,
1086 };
1087 #endif
1088 
1089 static int __init xfrmi4_init(void)
1090 {
1091 	int err;
1092 
1093 	err = xfrm4_protocol_register(&xfrmi_esp4_protocol, IPPROTO_ESP);
1094 	if (err < 0)
1095 		goto xfrm_proto_esp_failed;
1096 	err = xfrm4_protocol_register(&xfrmi_ah4_protocol, IPPROTO_AH);
1097 	if (err < 0)
1098 		goto xfrm_proto_ah_failed;
1099 	err = xfrm4_protocol_register(&xfrmi_ipcomp4_protocol, IPPROTO_COMP);
1100 	if (err < 0)
1101 		goto xfrm_proto_comp_failed;
1102 #if IS_REACHABLE(CONFIG_INET_XFRM_TUNNEL)
1103 	err = xfrm4_tunnel_register(&xfrmi_ipip_handler, AF_INET);
1104 	if (err < 0)
1105 		goto xfrm_tunnel_ipip_failed;
1106 	err = xfrm4_tunnel_register(&xfrmi_ipip6_handler, AF_INET6);
1107 	if (err < 0)
1108 		goto xfrm_tunnel_ipip6_failed;
1109 #endif
1110 
1111 	return 0;
1112 
1113 #if IS_REACHABLE(CONFIG_INET_XFRM_TUNNEL)
1114 xfrm_tunnel_ipip6_failed:
1115 	xfrm4_tunnel_deregister(&xfrmi_ipip_handler, AF_INET);
1116 xfrm_tunnel_ipip_failed:
1117 	xfrm4_protocol_deregister(&xfrmi_ipcomp4_protocol, IPPROTO_COMP);
1118 #endif
1119 xfrm_proto_comp_failed:
1120 	xfrm4_protocol_deregister(&xfrmi_ah4_protocol, IPPROTO_AH);
1121 xfrm_proto_ah_failed:
1122 	xfrm4_protocol_deregister(&xfrmi_esp4_protocol, IPPROTO_ESP);
1123 xfrm_proto_esp_failed:
1124 	return err;
1125 }
1126 
1127 static void xfrmi4_fini(void)
1128 {
1129 #if IS_REACHABLE(CONFIG_INET_XFRM_TUNNEL)
1130 	xfrm4_tunnel_deregister(&xfrmi_ipip6_handler, AF_INET6);
1131 	xfrm4_tunnel_deregister(&xfrmi_ipip_handler, AF_INET);
1132 #endif
1133 	xfrm4_protocol_deregister(&xfrmi_ipcomp4_protocol, IPPROTO_COMP);
1134 	xfrm4_protocol_deregister(&xfrmi_ah4_protocol, IPPROTO_AH);
1135 	xfrm4_protocol_deregister(&xfrmi_esp4_protocol, IPPROTO_ESP);
1136 }
1137 
1138 static int __init xfrmi6_init(void)
1139 {
1140 	int err;
1141 
1142 	err = xfrm6_protocol_register(&xfrmi_esp6_protocol, IPPROTO_ESP);
1143 	if (err < 0)
1144 		goto xfrm_proto_esp_failed;
1145 	err = xfrm6_protocol_register(&xfrmi_ah6_protocol, IPPROTO_AH);
1146 	if (err < 0)
1147 		goto xfrm_proto_ah_failed;
1148 	err = xfrm6_protocol_register(&xfrmi_ipcomp6_protocol, IPPROTO_COMP);
1149 	if (err < 0)
1150 		goto xfrm_proto_comp_failed;
1151 #if IS_REACHABLE(CONFIG_INET6_XFRM_TUNNEL)
1152 	err = xfrm6_tunnel_register(&xfrmi_ipv6_handler, AF_INET6);
1153 	if (err < 0)
1154 		goto xfrm_tunnel_ipv6_failed;
1155 	err = xfrm6_tunnel_register(&xfrmi_ip6ip_handler, AF_INET);
1156 	if (err < 0)
1157 		goto xfrm_tunnel_ip6ip_failed;
1158 #endif
1159 
1160 	return 0;
1161 
1162 #if IS_REACHABLE(CONFIG_INET6_XFRM_TUNNEL)
1163 xfrm_tunnel_ip6ip_failed:
1164 	xfrm6_tunnel_deregister(&xfrmi_ipv6_handler, AF_INET6);
1165 xfrm_tunnel_ipv6_failed:
1166 	xfrm6_protocol_deregister(&xfrmi_ipcomp6_protocol, IPPROTO_COMP);
1167 #endif
1168 xfrm_proto_comp_failed:
1169 	xfrm6_protocol_deregister(&xfrmi_ah6_protocol, IPPROTO_AH);
1170 xfrm_proto_ah_failed:
1171 	xfrm6_protocol_deregister(&xfrmi_esp6_protocol, IPPROTO_ESP);
1172 xfrm_proto_esp_failed:
1173 	return err;
1174 }
1175 
1176 static void xfrmi6_fini(void)
1177 {
1178 #if IS_REACHABLE(CONFIG_INET6_XFRM_TUNNEL)
1179 	xfrm6_tunnel_deregister(&xfrmi_ip6ip_handler, AF_INET);
1180 	xfrm6_tunnel_deregister(&xfrmi_ipv6_handler, AF_INET6);
1181 #endif
1182 	xfrm6_protocol_deregister(&xfrmi_ipcomp6_protocol, IPPROTO_COMP);
1183 	xfrm6_protocol_deregister(&xfrmi_ah6_protocol, IPPROTO_AH);
1184 	xfrm6_protocol_deregister(&xfrmi_esp6_protocol, IPPROTO_ESP);
1185 }
1186 
1187 static const struct xfrm_if_cb xfrm_if_cb = {
1188 	.decode_session =	xfrmi_decode_session,
1189 };
1190 
1191 static int __init xfrmi_init(void)
1192 {
1193 	const char *msg;
1194 	int err;
1195 
1196 	pr_info("IPsec XFRM device driver\n");
1197 
1198 	msg = "tunnel device";
1199 	err = register_pernet_device(&xfrmi_net_ops);
1200 	if (err < 0)
1201 		goto pernet_dev_failed;
1202 
1203 	msg = "xfrm4 protocols";
1204 	err = xfrmi4_init();
1205 	if (err < 0)
1206 		goto xfrmi4_failed;
1207 
1208 	msg = "xfrm6 protocols";
1209 	err = xfrmi6_init();
1210 	if (err < 0)
1211 		goto xfrmi6_failed;
1212 
1213 
1214 	msg = "netlink interface";
1215 	err = rtnl_link_register(&xfrmi_link_ops);
1216 	if (err < 0)
1217 		goto rtnl_link_failed;
1218 
1219 	err = register_xfrm_interface_bpf();
1220 	if (err < 0)
1221 		goto kfunc_failed;
1222 
1223 	lwtunnel_encap_add_ops(&xfrmi_encap_ops, LWTUNNEL_ENCAP_XFRM);
1224 
1225 	xfrm_if_register_cb(&xfrm_if_cb);
1226 
1227 	return err;
1228 
1229 kfunc_failed:
1230 	rtnl_link_unregister(&xfrmi_link_ops);
1231 rtnl_link_failed:
1232 	xfrmi6_fini();
1233 xfrmi6_failed:
1234 	xfrmi4_fini();
1235 xfrmi4_failed:
1236 	unregister_pernet_device(&xfrmi_net_ops);
1237 pernet_dev_failed:
1238 	pr_err("xfrmi init: failed to register %s\n", msg);
1239 	return err;
1240 }
1241 
1242 static void __exit xfrmi_fini(void)
1243 {
1244 	xfrm_if_unregister_cb();
1245 	lwtunnel_encap_del_ops(&xfrmi_encap_ops, LWTUNNEL_ENCAP_XFRM);
1246 	rtnl_link_unregister(&xfrmi_link_ops);
1247 	xfrmi4_fini();
1248 	xfrmi6_fini();
1249 	unregister_pernet_device(&xfrmi_net_ops);
1250 }
1251 
1252 module_init(xfrmi_init);
1253 module_exit(xfrmi_fini);
1254 MODULE_LICENSE("GPL");
1255 MODULE_ALIAS_RTNL_LINK("xfrm");
1256 MODULE_ALIAS_NETDEV("xfrm0");
1257 MODULE_AUTHOR("Steffen Klassert");
1258 MODULE_DESCRIPTION("XFRM virtual interface");
1259