xref: /openbmc/linux/net/ipv6/seg6_local.c (revision 96ac6d43) 
1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  *  SR-IPv6 implementation
4  *
5  *  Authors:
6  *  David Lebrun <david.lebrun@uclouvain.be>
7  *  eBPF support: Mathieu Xhonneux <m.xhonneux@gmail.com>
8  */
9 
10 #include <linux/types.h>
11 #include <linux/skbuff.h>
12 #include <linux/net.h>
13 #include <linux/module.h>
14 #include <net/ip.h>
15 #include <net/lwtunnel.h>
16 #include <net/netevent.h>
17 #include <net/netns/generic.h>
18 #include <net/ip6_fib.h>
19 #include <net/route.h>
20 #include <net/seg6.h>
21 #include <linux/seg6.h>
22 #include <linux/seg6_local.h>
23 #include <net/addrconf.h>
24 #include <net/ip6_route.h>
25 #include <net/dst_cache.h>
26 #ifdef CONFIG_IPV6_SEG6_HMAC
27 #include <net/seg6_hmac.h>
28 #endif
29 #include <net/seg6_local.h>
30 #include <linux/etherdevice.h>
31 #include <linux/bpf.h>
32 
33 struct seg6_local_lwt;
34 
35 struct seg6_action_desc {
36 	int action;
37 	unsigned long attrs;
38 	int (*input)(struct sk_buff *skb, struct seg6_local_lwt *slwt);
39 	int static_headroom;
40 };
41 
42 struct bpf_lwt_prog {
43 	struct bpf_prog *prog;
44 	char *name;
45 };
46 
47 struct seg6_local_lwt {
48 	int action;
49 	struct ipv6_sr_hdr *srh;
50 	int table;
51 	struct in_addr nh4;
52 	struct in6_addr nh6;
53 	int iif;
54 	int oif;
55 	struct bpf_lwt_prog bpf;
56 
57 	int headroom;
58 	struct seg6_action_desc *desc;
59 };
60 
61 static struct seg6_local_lwt *seg6_local_lwtunnel(struct lwtunnel_state *lwt)
62 {
63 	return (struct seg6_local_lwt *)lwt->data;
64 }
65 
66 static struct ipv6_sr_hdr *get_srh(struct sk_buff *skb)
67 {
68 	struct ipv6_sr_hdr *srh;
69 	int len, srhoff = 0;
70 
71 	if (ipv6_find_hdr(skb, &srhoff, IPPROTO_ROUTING, NULL, NULL) < 0)
72 		return NULL;
73 
74 	if (!pskb_may_pull(skb, srhoff + sizeof(*srh)))
75 		return NULL;
76 
77 	srh = (struct ipv6_sr_hdr *)(skb->data + srhoff);
78 
79 	len = (srh->hdrlen + 1) << 3;
80 
81 	if (!pskb_may_pull(skb, srhoff + len))
82 		return NULL;
83 
84 	if (!seg6_validate_srh(srh, len))
85 		return NULL;
86 
87 	return srh;
88 }
89 
90 static struct ipv6_sr_hdr *get_and_validate_srh(struct sk_buff *skb)
91 {
92 	struct ipv6_sr_hdr *srh;
93 
94 	srh = get_srh(skb);
95 	if (!srh)
96 		return NULL;
97 
98 	if (srh->segments_left == 0)
99 		return NULL;
100 
101 #ifdef CONFIG_IPV6_SEG6_HMAC
102 	if (!seg6_hmac_validate_skb(skb))
103 		return NULL;
104 #endif
105 
106 	return srh;
107 }
108 
109 static bool decap_and_validate(struct sk_buff *skb, int proto)
110 {
111 	struct ipv6_sr_hdr *srh;
112 	unsigned int off = 0;
113 
114 	srh = get_srh(skb);
115 	if (srh && srh->segments_left > 0)
116 		return false;
117 
118 #ifdef CONFIG_IPV6_SEG6_HMAC
119 	if (srh && !seg6_hmac_validate_skb(skb))
120 		return false;
121 #endif
122 
123 	if (ipv6_find_hdr(skb, &off, proto, NULL, NULL) < 0)
124 		return false;
125 
126 	if (!pskb_pull(skb, off))
127 		return false;
128 
129 	skb_postpull_rcsum(skb, skb_network_header(skb), off);
130 
131 	skb_reset_network_header(skb);
132 	skb_reset_transport_header(skb);
133 	skb->encapsulation = 0;
134 
135 	return true;
136 }
137 
138 static void advance_nextseg(struct ipv6_sr_hdr *srh, struct in6_addr *daddr)
139 {
140 	struct in6_addr *addr;
141 
142 	srh->segments_left--;
143 	addr = srh->segments + srh->segments_left;
144 	*daddr = *addr;
145 }
146 
147 int seg6_lookup_nexthop(struct sk_buff *skb, struct in6_addr *nhaddr,
148 			u32 tbl_id)
149 {
150 	struct net *net = dev_net(skb->dev);
151 	struct ipv6hdr *hdr = ipv6_hdr(skb);
152 	int flags = RT6_LOOKUP_F_HAS_SADDR;
153 	struct dst_entry *dst = NULL;
154 	struct rt6_info *rt;
155 	struct flowi6 fl6;
156 
157 	fl6.flowi6_iif = skb->dev->ifindex;
158 	fl6.daddr = nhaddr ? *nhaddr : hdr->daddr;
159 	fl6.saddr = hdr->saddr;
160 	fl6.flowlabel = ip6_flowinfo(hdr);
161 	fl6.flowi6_mark = skb->mark;
162 	fl6.flowi6_proto = hdr->nexthdr;
163 
164 	if (nhaddr)
165 		fl6.flowi6_flags = FLOWI_FLAG_KNOWN_NH;
166 
167 	if (!tbl_id) {
168 		dst = ip6_route_input_lookup(net, skb->dev, &fl6, skb, flags);
169 	} else {
170 		struct fib6_table *table;
171 
172 		table = fib6_get_table(net, tbl_id);
173 		if (!table)
174 			goto out;
175 
176 		rt = ip6_pol_route(net, table, 0, &fl6, skb, flags);
177 		dst = &rt->dst;
178 	}
179 
180 	if (dst && dst->dev->flags & IFF_LOOPBACK && !dst->error) {
181 		dst_release(dst);
182 		dst = NULL;
183 	}
184 
185 out:
186 	if (!dst) {
187 		rt = net->ipv6.ip6_blk_hole_entry;
188 		dst = &rt->dst;
189 		dst_hold(dst);
190 	}
191 
192 	skb_dst_drop(skb);
193 	skb_dst_set(skb, dst);
194 	return dst->error;
195 }
196 
197 /* regular endpoint function */
198 static int input_action_end(struct sk_buff *skb, struct seg6_local_lwt *slwt)
199 {
200 	struct ipv6_sr_hdr *srh;
201 
202 	srh = get_and_validate_srh(skb);
203 	if (!srh)
204 		goto drop;
205 
206 	advance_nextseg(srh, &ipv6_hdr(skb)->daddr);
207 
208 	seg6_lookup_nexthop(skb, NULL, 0);
209 
210 	return dst_input(skb);
211 
212 drop:
213 	kfree_skb(skb);
214 	return -EINVAL;
215 }
216 
217 /* regular endpoint, and forward to specified nexthop */
218 static int input_action_end_x(struct sk_buff *skb, struct seg6_local_lwt *slwt)
219 {
220 	struct ipv6_sr_hdr *srh;
221 
222 	srh = get_and_validate_srh(skb);
223 	if (!srh)
224 		goto drop;
225 
226 	advance_nextseg(srh, &ipv6_hdr(skb)->daddr);
227 
228 	seg6_lookup_nexthop(skb, &slwt->nh6, 0);
229 
230 	return dst_input(skb);
231 
232 drop:
233 	kfree_skb(skb);
234 	return -EINVAL;
235 }
236 
237 static int input_action_end_t(struct sk_buff *skb, struct seg6_local_lwt *slwt)
238 {
239 	struct ipv6_sr_hdr *srh;
240 
241 	srh = get_and_validate_srh(skb);
242 	if (!srh)
243 		goto drop;
244 
245 	advance_nextseg(srh, &ipv6_hdr(skb)->daddr);
246 
247 	seg6_lookup_nexthop(skb, NULL, slwt->table);
248 
249 	return dst_input(skb);
250 
251 drop:
252 	kfree_skb(skb);
253 	return -EINVAL;
254 }
255 
256 /* decapsulate and forward inner L2 frame on specified interface */
257 static int input_action_end_dx2(struct sk_buff *skb,
258 				struct seg6_local_lwt *slwt)
259 {
260 	struct net *net = dev_net(skb->dev);
261 	struct net_device *odev;
262 	struct ethhdr *eth;
263 
264 	if (!decap_and_validate(skb, NEXTHDR_NONE))
265 		goto drop;
266 
267 	if (!pskb_may_pull(skb, ETH_HLEN))
268 		goto drop;
269 
270 	skb_reset_mac_header(skb);
271 	eth = (struct ethhdr *)skb->data;
272 
273 	/* To determine the frame's protocol, we assume it is 802.3. This avoids
274 	 * a call to eth_type_trans(), which is not really relevant for our
275 	 * use case.
276 	 */
277 	if (!eth_proto_is_802_3(eth->h_proto))
278 		goto drop;
279 
280 	odev = dev_get_by_index_rcu(net, slwt->oif);
281 	if (!odev)
282 		goto drop;
283 
284 	/* As we accept Ethernet frames, make sure the egress device is of
285 	 * the correct type.
286 	 */
287 	if (odev->type != ARPHRD_ETHER)
288 		goto drop;
289 
290 	if (!(odev->flags & IFF_UP) || !netif_carrier_ok(odev))
291 		goto drop;
292 
293 	skb_orphan(skb);
294 
295 	if (skb_warn_if_lro(skb))
296 		goto drop;
297 
298 	skb_forward_csum(skb);
299 
300 	if (skb->len - ETH_HLEN > odev->mtu)
301 		goto drop;
302 
303 	skb->dev = odev;
304 	skb->protocol = eth->h_proto;
305 
306 	return dev_queue_xmit(skb);
307 
308 drop:
309 	kfree_skb(skb);
310 	return -EINVAL;
311 }
312 
313 /* decapsulate and forward to specified nexthop */
314 static int input_action_end_dx6(struct sk_buff *skb,
315 				struct seg6_local_lwt *slwt)
316 {
317 	struct in6_addr *nhaddr = NULL;
318 
319 	/* this function accepts IPv6 encapsulated packets, with either
320 	 * an SRH with SL=0, or no SRH.
321 	 */
322 
323 	if (!decap_and_validate(skb, IPPROTO_IPV6))
324 		goto drop;
325 
326 	if (!pskb_may_pull(skb, sizeof(struct ipv6hdr)))
327 		goto drop;
328 
329 	/* The inner packet is not associated to any local interface,
330 	 * so we do not call netif_rx().
331 	 *
332 	 * If slwt->nh6 is set to ::, then lookup the nexthop for the
333 	 * inner packet's DA. Otherwise, use the specified nexthop.
334 	 */
335 
336 	if (!ipv6_addr_any(&slwt->nh6))
337 		nhaddr = &slwt->nh6;
338 
339 	seg6_lookup_nexthop(skb, nhaddr, 0);
340 
341 	return dst_input(skb);
342 drop:
343 	kfree_skb(skb);
344 	return -EINVAL;
345 }
346 
347 static int input_action_end_dx4(struct sk_buff *skb,
348 				struct seg6_local_lwt *slwt)
349 {
350 	struct iphdr *iph;
351 	__be32 nhaddr;
352 	int err;
353 
354 	if (!decap_and_validate(skb, IPPROTO_IPIP))
355 		goto drop;
356 
357 	if (!pskb_may_pull(skb, sizeof(struct iphdr)))
358 		goto drop;
359 
360 	skb->protocol = htons(ETH_P_IP);
361 
362 	iph = ip_hdr(skb);
363 
364 	nhaddr = slwt->nh4.s_addr ?: iph->daddr;
365 
366 	skb_dst_drop(skb);
367 
368 	err = ip_route_input(skb, nhaddr, iph->saddr, 0, skb->dev);
369 	if (err)
370 		goto drop;
371 
372 	return dst_input(skb);
373 
374 drop:
375 	kfree_skb(skb);
376 	return -EINVAL;
377 }
378 
379 static int input_action_end_dt6(struct sk_buff *skb,
380 				struct seg6_local_lwt *slwt)
381 {
382 	if (!decap_and_validate(skb, IPPROTO_IPV6))
383 		goto drop;
384 
385 	if (!pskb_may_pull(skb, sizeof(struct ipv6hdr)))
386 		goto drop;
387 
388 	seg6_lookup_nexthop(skb, NULL, slwt->table);
389 
390 	return dst_input(skb);
391 
392 drop:
393 	kfree_skb(skb);
394 	return -EINVAL;
395 }
396 
397 /* push an SRH on top of the current one */
398 static int input_action_end_b6(struct sk_buff *skb, struct seg6_local_lwt *slwt)
399 {
400 	struct ipv6_sr_hdr *srh;
401 	int err = -EINVAL;
402 
403 	srh = get_and_validate_srh(skb);
404 	if (!srh)
405 		goto drop;
406 
407 	err = seg6_do_srh_inline(skb, slwt->srh);
408 	if (err)
409 		goto drop;
410 
411 	ipv6_hdr(skb)->payload_len = htons(skb->len - sizeof(struct ipv6hdr));
412 	skb_set_transport_header(skb, sizeof(struct ipv6hdr));
413 
414 	seg6_lookup_nexthop(skb, NULL, 0);
415 
416 	return dst_input(skb);
417 
418 drop:
419 	kfree_skb(skb);
420 	return err;
421 }
422 
423 /* encapsulate within an outer IPv6 header and a specified SRH */
424 static int input_action_end_b6_encap(struct sk_buff *skb,
425 				     struct seg6_local_lwt *slwt)
426 {
427 	struct ipv6_sr_hdr *srh;
428 	int err = -EINVAL;
429 
430 	srh = get_and_validate_srh(skb);
431 	if (!srh)
432 		goto drop;
433 
434 	advance_nextseg(srh, &ipv6_hdr(skb)->daddr);
435 
436 	skb_reset_inner_headers(skb);
437 	skb->encapsulation = 1;
438 
439 	err = seg6_do_srh_encap(skb, slwt->srh, IPPROTO_IPV6);
440 	if (err)
441 		goto drop;
442 
443 	ipv6_hdr(skb)->payload_len = htons(skb->len - sizeof(struct ipv6hdr));
444 	skb_set_transport_header(skb, sizeof(struct ipv6hdr));
445 
446 	seg6_lookup_nexthop(skb, NULL, 0);
447 
448 	return dst_input(skb);
449 
450 drop:
451 	kfree_skb(skb);
452 	return err;
453 }
454 
455 DEFINE_PER_CPU(struct seg6_bpf_srh_state, seg6_bpf_srh_states);
456 
457 bool seg6_bpf_has_valid_srh(struct sk_buff *skb)
458 {
459 	struct seg6_bpf_srh_state *srh_state =
460 		this_cpu_ptr(&seg6_bpf_srh_states);
461 	struct ipv6_sr_hdr *srh = srh_state->srh;
462 
463 	if (unlikely(srh == NULL))
464 		return false;
465 
466 	if (unlikely(!srh_state->valid)) {
467 		if ((srh_state->hdrlen & 7) != 0)
468 			return false;
469 
470 		srh->hdrlen = (u8)(srh_state->hdrlen >> 3);
471 		if (!seg6_validate_srh(srh, (srh->hdrlen + 1) << 3))
472 			return false;
473 
474 		srh_state->valid = true;
475 	}
476 
477 	return true;
478 }
479 
480 static int input_action_end_bpf(struct sk_buff *skb,
481 				struct seg6_local_lwt *slwt)
482 {
483 	struct seg6_bpf_srh_state *srh_state =
484 		this_cpu_ptr(&seg6_bpf_srh_states);
485 	struct ipv6_sr_hdr *srh;
486 	int ret;
487 
488 	srh = get_and_validate_srh(skb);
489 	if (!srh) {
490 		kfree_skb(skb);
491 		return -EINVAL;
492 	}
493 	advance_nextseg(srh, &ipv6_hdr(skb)->daddr);
494 
495 	/* preempt_disable is needed to protect the per-CPU buffer srh_state,
496 	 * which is also accessed by the bpf_lwt_seg6_* helpers
497 	 */
498 	preempt_disable();
499 	srh_state->srh = srh;
500 	srh_state->hdrlen = srh->hdrlen << 3;
501 	srh_state->valid = true;
502 
503 	rcu_read_lock();
504 	bpf_compute_data_pointers(skb);
505 	ret = bpf_prog_run_save_cb(slwt->bpf.prog, skb);
506 	rcu_read_unlock();
507 
508 	switch (ret) {
509 	case BPF_OK:
510 	case BPF_REDIRECT:
511 		break;
512 	case BPF_DROP:
513 		goto drop;
514 	default:
515 		pr_warn_once("bpf-seg6local: Illegal return value %u\n", ret);
516 		goto drop;
517 	}
518 
519 	if (srh_state->srh && !seg6_bpf_has_valid_srh(skb))
520 		goto drop;
521 
522 	preempt_enable();
523 	if (ret != BPF_REDIRECT)
524 		seg6_lookup_nexthop(skb, NULL, 0);
525 
526 	return dst_input(skb);
527 
528 drop:
529 	preempt_enable();
530 	kfree_skb(skb);
531 	return -EINVAL;
532 }
533 
534 static struct seg6_action_desc seg6_action_table[] = {
535 	{
536 		.action		= SEG6_LOCAL_ACTION_END,
537 		.attrs		= 0,
538 		.input		= input_action_end,
539 	},
540 	{
541 		.action		= SEG6_LOCAL_ACTION_END_X,
542 		.attrs		= (1 << SEG6_LOCAL_NH6),
543 		.input		= input_action_end_x,
544 	},
545 	{
546 		.action		= SEG6_LOCAL_ACTION_END_T,
547 		.attrs		= (1 << SEG6_LOCAL_TABLE),
548 		.input		= input_action_end_t,
549 	},
550 	{
551 		.action		= SEG6_LOCAL_ACTION_END_DX2,
552 		.attrs		= (1 << SEG6_LOCAL_OIF),
553 		.input		= input_action_end_dx2,
554 	},
555 	{
556 		.action		= SEG6_LOCAL_ACTION_END_DX6,
557 		.attrs		= (1 << SEG6_LOCAL_NH6),
558 		.input		= input_action_end_dx6,
559 	},
560 	{
561 		.action		= SEG6_LOCAL_ACTION_END_DX4,
562 		.attrs		= (1 << SEG6_LOCAL_NH4),
563 		.input		= input_action_end_dx4,
564 	},
565 	{
566 		.action		= SEG6_LOCAL_ACTION_END_DT6,
567 		.attrs		= (1 << SEG6_LOCAL_TABLE),
568 		.input		= input_action_end_dt6,
569 	},
570 	{
571 		.action		= SEG6_LOCAL_ACTION_END_B6,
572 		.attrs		= (1 << SEG6_LOCAL_SRH),
573 		.input		= input_action_end_b6,
574 	},
575 	{
576 		.action		= SEG6_LOCAL_ACTION_END_B6_ENCAP,
577 		.attrs		= (1 << SEG6_LOCAL_SRH),
578 		.input		= input_action_end_b6_encap,
579 		.static_headroom	= sizeof(struct ipv6hdr),
580 	},
581 	{
582 		.action		= SEG6_LOCAL_ACTION_END_BPF,
583 		.attrs		= (1 << SEG6_LOCAL_BPF),
584 		.input		= input_action_end_bpf,
585 	},
586 
587 };
588 
589 static struct seg6_action_desc *__get_action_desc(int action)
590 {
591 	struct seg6_action_desc *desc;
592 	int i, count;
593 
594 	count = ARRAY_SIZE(seg6_action_table);
595 	for (i = 0; i < count; i++) {
596 		desc = &seg6_action_table[i];
597 		if (desc->action == action)
598 			return desc;
599 	}
600 
601 	return NULL;
602 }
603 
604 static int seg6_local_input(struct sk_buff *skb)
605 {
606 	struct dst_entry *orig_dst = skb_dst(skb);
607 	struct seg6_action_desc *desc;
608 	struct seg6_local_lwt *slwt;
609 
610 	if (skb->protocol != htons(ETH_P_IPV6)) {
611 		kfree_skb(skb);
612 		return -EINVAL;
613 	}
614 
615 	slwt = seg6_local_lwtunnel(orig_dst->lwtstate);
616 	desc = slwt->desc;
617 
618 	return desc->input(skb, slwt);
619 }
620 
621 static const struct nla_policy seg6_local_policy[SEG6_LOCAL_MAX + 1] = {
622 	[SEG6_LOCAL_ACTION]	= { .type = NLA_U32 },
623 	[SEG6_LOCAL_SRH]	= { .type = NLA_BINARY },
624 	[SEG6_LOCAL_TABLE]	= { .type = NLA_U32 },
625 	[SEG6_LOCAL_NH4]	= { .type = NLA_BINARY,
626 				    .len = sizeof(struct in_addr) },
627 	[SEG6_LOCAL_NH6]	= { .type = NLA_BINARY,
628 				    .len = sizeof(struct in6_addr) },
629 	[SEG6_LOCAL_IIF]	= { .type = NLA_U32 },
630 	[SEG6_LOCAL_OIF]	= { .type = NLA_U32 },
631 	[SEG6_LOCAL_BPF]	= { .type = NLA_NESTED },
632 };
633 
634 static int parse_nla_srh(struct nlattr **attrs, struct seg6_local_lwt *slwt)
635 {
636 	struct ipv6_sr_hdr *srh;
637 	int len;
638 
639 	srh = nla_data(attrs[SEG6_LOCAL_SRH]);
640 	len = nla_len(attrs[SEG6_LOCAL_SRH]);
641 
642 	/* SRH must contain at least one segment */
643 	if (len < sizeof(*srh) + sizeof(struct in6_addr))
644 		return -EINVAL;
645 
646 	if (!seg6_validate_srh(srh, len))
647 		return -EINVAL;
648 
649 	slwt->srh = kmemdup(srh, len, GFP_KERNEL);
650 	if (!slwt->srh)
651 		return -ENOMEM;
652 
653 	slwt->headroom += len;
654 
655 	return 0;
656 }
657 
658 static int put_nla_srh(struct sk_buff *skb, struct seg6_local_lwt *slwt)
659 {
660 	struct ipv6_sr_hdr *srh;
661 	struct nlattr *nla;
662 	int len;
663 
664 	srh = slwt->srh;
665 	len = (srh->hdrlen + 1) << 3;
666 
667 	nla = nla_reserve(skb, SEG6_LOCAL_SRH, len);
668 	if (!nla)
669 		return -EMSGSIZE;
670 
671 	memcpy(nla_data(nla), srh, len);
672 
673 	return 0;
674 }
675 
676 static int cmp_nla_srh(struct seg6_local_lwt *a, struct seg6_local_lwt *b)
677 {
678 	int len = (a->srh->hdrlen + 1) << 3;
679 
680 	if (len != ((b->srh->hdrlen + 1) << 3))
681 		return 1;
682 
683 	return memcmp(a->srh, b->srh, len);
684 }
685 
686 static int parse_nla_table(struct nlattr **attrs, struct seg6_local_lwt *slwt)
687 {
688 	slwt->table = nla_get_u32(attrs[SEG6_LOCAL_TABLE]);
689 
690 	return 0;
691 }
692 
693 static int put_nla_table(struct sk_buff *skb, struct seg6_local_lwt *slwt)
694 {
695 	if (nla_put_u32(skb, SEG6_LOCAL_TABLE, slwt->table))
696 		return -EMSGSIZE;
697 
698 	return 0;
699 }
700 
701 static int cmp_nla_table(struct seg6_local_lwt *a, struct seg6_local_lwt *b)
702 {
703 	if (a->table != b->table)
704 		return 1;
705 
706 	return 0;
707 }
708 
709 static int parse_nla_nh4(struct nlattr **attrs, struct seg6_local_lwt *slwt)
710 {
711 	memcpy(&slwt->nh4, nla_data(attrs[SEG6_LOCAL_NH4]),
712 	       sizeof(struct in_addr));
713 
714 	return 0;
715 }
716 
717 static int put_nla_nh4(struct sk_buff *skb, struct seg6_local_lwt *slwt)
718 {
719 	struct nlattr *nla;
720 
721 	nla = nla_reserve(skb, SEG6_LOCAL_NH4, sizeof(struct in_addr));
722 	if (!nla)
723 		return -EMSGSIZE;
724 
725 	memcpy(nla_data(nla), &slwt->nh4, sizeof(struct in_addr));
726 
727 	return 0;
728 }
729 
730 static int cmp_nla_nh4(struct seg6_local_lwt *a, struct seg6_local_lwt *b)
731 {
732 	return memcmp(&a->nh4, &b->nh4, sizeof(struct in_addr));
733 }
734 
735 static int parse_nla_nh6(struct nlattr **attrs, struct seg6_local_lwt *slwt)
736 {
737 	memcpy(&slwt->nh6, nla_data(attrs[SEG6_LOCAL_NH6]),
738 	       sizeof(struct in6_addr));
739 
740 	return 0;
741 }
742 
743 static int put_nla_nh6(struct sk_buff *skb, struct seg6_local_lwt *slwt)
744 {
745 	struct nlattr *nla;
746 
747 	nla = nla_reserve(skb, SEG6_LOCAL_NH6, sizeof(struct in6_addr));
748 	if (!nla)
749 		return -EMSGSIZE;
750 
751 	memcpy(nla_data(nla), &slwt->nh6, sizeof(struct in6_addr));
752 
753 	return 0;
754 }
755 
756 static int cmp_nla_nh6(struct seg6_local_lwt *a, struct seg6_local_lwt *b)
757 {
758 	return memcmp(&a->nh6, &b->nh6, sizeof(struct in6_addr));
759 }
760 
761 static int parse_nla_iif(struct nlattr **attrs, struct seg6_local_lwt *slwt)
762 {
763 	slwt->iif = nla_get_u32(attrs[SEG6_LOCAL_IIF]);
764 
765 	return 0;
766 }
767 
768 static int put_nla_iif(struct sk_buff *skb, struct seg6_local_lwt *slwt)
769 {
770 	if (nla_put_u32(skb, SEG6_LOCAL_IIF, slwt->iif))
771 		return -EMSGSIZE;
772 
773 	return 0;
774 }
775 
776 static int cmp_nla_iif(struct seg6_local_lwt *a, struct seg6_local_lwt *b)
777 {
778 	if (a->iif != b->iif)
779 		return 1;
780 
781 	return 0;
782 }
783 
784 static int parse_nla_oif(struct nlattr **attrs, struct seg6_local_lwt *slwt)
785 {
786 	slwt->oif = nla_get_u32(attrs[SEG6_LOCAL_OIF]);
787 
788 	return 0;
789 }
790 
791 static int put_nla_oif(struct sk_buff *skb, struct seg6_local_lwt *slwt)
792 {
793 	if (nla_put_u32(skb, SEG6_LOCAL_OIF, slwt->oif))
794 		return -EMSGSIZE;
795 
796 	return 0;
797 }
798 
799 static int cmp_nla_oif(struct seg6_local_lwt *a, struct seg6_local_lwt *b)
800 {
801 	if (a->oif != b->oif)
802 		return 1;
803 
804 	return 0;
805 }
806 
807 #define MAX_PROG_NAME 256
808 static const struct nla_policy bpf_prog_policy[SEG6_LOCAL_BPF_PROG_MAX + 1] = {
809 	[SEG6_LOCAL_BPF_PROG]	   = { .type = NLA_U32, },
810 	[SEG6_LOCAL_BPF_PROG_NAME] = { .type = NLA_NUL_STRING,
811 				       .len = MAX_PROG_NAME },
812 };
813 
814 static int parse_nla_bpf(struct nlattr **attrs, struct seg6_local_lwt *slwt)
815 {
816 	struct nlattr *tb[SEG6_LOCAL_BPF_PROG_MAX + 1];
817 	struct bpf_prog *p;
818 	int ret;
819 	u32 fd;
820 
821 	ret = nla_parse_nested_deprecated(tb, SEG6_LOCAL_BPF_PROG_MAX,
822 					  attrs[SEG6_LOCAL_BPF],
823 					  bpf_prog_policy, NULL);
824 	if (ret < 0)
825 		return ret;
826 
827 	if (!tb[SEG6_LOCAL_BPF_PROG] || !tb[SEG6_LOCAL_BPF_PROG_NAME])
828 		return -EINVAL;
829 
830 	slwt->bpf.name = nla_memdup(tb[SEG6_LOCAL_BPF_PROG_NAME], GFP_KERNEL);
831 	if (!slwt->bpf.name)
832 		return -ENOMEM;
833 
834 	fd = nla_get_u32(tb[SEG6_LOCAL_BPF_PROG]);
835 	p = bpf_prog_get_type(fd, BPF_PROG_TYPE_LWT_SEG6LOCAL);
836 	if (IS_ERR(p)) {
837 		kfree(slwt->bpf.name);
838 		return PTR_ERR(p);
839 	}
840 
841 	slwt->bpf.prog = p;
842 	return 0;
843 }
844 
845 static int put_nla_bpf(struct sk_buff *skb, struct seg6_local_lwt *slwt)
846 {
847 	struct nlattr *nest;
848 
849 	if (!slwt->bpf.prog)
850 		return 0;
851 
852 	nest = nla_nest_start_noflag(skb, SEG6_LOCAL_BPF);
853 	if (!nest)
854 		return -EMSGSIZE;
855 
856 	if (nla_put_u32(skb, SEG6_LOCAL_BPF_PROG, slwt->bpf.prog->aux->id))
857 		return -EMSGSIZE;
858 
859 	if (slwt->bpf.name &&
860 	    nla_put_string(skb, SEG6_LOCAL_BPF_PROG_NAME, slwt->bpf.name))
861 		return -EMSGSIZE;
862 
863 	return nla_nest_end(skb, nest);
864 }
865 
866 static int cmp_nla_bpf(struct seg6_local_lwt *a, struct seg6_local_lwt *b)
867 {
868 	if (!a->bpf.name && !b->bpf.name)
869 		return 0;
870 
871 	if (!a->bpf.name || !b->bpf.name)
872 		return 1;
873 
874 	return strcmp(a->bpf.name, b->bpf.name);
875 }
876 
877 struct seg6_action_param {
878 	int (*parse)(struct nlattr **attrs, struct seg6_local_lwt *slwt);
879 	int (*put)(struct sk_buff *skb, struct seg6_local_lwt *slwt);
880 	int (*cmp)(struct seg6_local_lwt *a, struct seg6_local_lwt *b);
881 };
882 
883 static struct seg6_action_param seg6_action_params[SEG6_LOCAL_MAX + 1] = {
884 	[SEG6_LOCAL_SRH]	= { .parse = parse_nla_srh,
885 				    .put = put_nla_srh,
886 				    .cmp = cmp_nla_srh },
887 
888 	[SEG6_LOCAL_TABLE]	= { .parse = parse_nla_table,
889 				    .put = put_nla_table,
890 				    .cmp = cmp_nla_table },
891 
892 	[SEG6_LOCAL_NH4]	= { .parse = parse_nla_nh4,
893 				    .put = put_nla_nh4,
894 				    .cmp = cmp_nla_nh4 },
895 
896 	[SEG6_LOCAL_NH6]	= { .parse = parse_nla_nh6,
897 				    .put = put_nla_nh6,
898 				    .cmp = cmp_nla_nh6 },
899 
900 	[SEG6_LOCAL_IIF]	= { .parse = parse_nla_iif,
901 				    .put = put_nla_iif,
902 				    .cmp = cmp_nla_iif },
903 
904 	[SEG6_LOCAL_OIF]	= { .parse = parse_nla_oif,
905 				    .put = put_nla_oif,
906 				    .cmp = cmp_nla_oif },
907 
908 	[SEG6_LOCAL_BPF]	= { .parse = parse_nla_bpf,
909 				    .put = put_nla_bpf,
910 				    .cmp = cmp_nla_bpf },
911 
912 };
913 
914 static int parse_nla_action(struct nlattr **attrs, struct seg6_local_lwt *slwt)
915 {
916 	struct seg6_action_param *param;
917 	struct seg6_action_desc *desc;
918 	int i, err;
919 
920 	desc = __get_action_desc(slwt->action);
921 	if (!desc)
922 		return -EINVAL;
923 
924 	if (!desc->input)
925 		return -EOPNOTSUPP;
926 
927 	slwt->desc = desc;
928 	slwt->headroom += desc->static_headroom;
929 
930 	for (i = 0; i < SEG6_LOCAL_MAX + 1; i++) {
931 		if (desc->attrs & (1 << i)) {
932 			if (!attrs[i])
933 				return -EINVAL;
934 
935 			param = &seg6_action_params[i];
936 
937 			err = param->parse(attrs, slwt);
938 			if (err < 0)
939 				return err;
940 		}
941 	}
942 
943 	return 0;
944 }
945 
946 static int seg6_local_build_state(struct nlattr *nla, unsigned int family,
947 				  const void *cfg, struct lwtunnel_state **ts,
948 				  struct netlink_ext_ack *extack)
949 {
950 	struct nlattr *tb[SEG6_LOCAL_MAX + 1];
951 	struct lwtunnel_state *newts;
952 	struct seg6_local_lwt *slwt;
953 	int err;
954 
955 	if (family != AF_INET6)
956 		return -EINVAL;
957 
958 	err = nla_parse_nested_deprecated(tb, SEG6_LOCAL_MAX, nla,
959 					  seg6_local_policy, extack);
960 
961 	if (err < 0)
962 		return err;
963 
964 	if (!tb[SEG6_LOCAL_ACTION])
965 		return -EINVAL;
966 
967 	newts = lwtunnel_state_alloc(sizeof(*slwt));
968 	if (!newts)
969 		return -ENOMEM;
970 
971 	slwt = seg6_local_lwtunnel(newts);
972 	slwt->action = nla_get_u32(tb[SEG6_LOCAL_ACTION]);
973 
974 	err = parse_nla_action(tb, slwt);
975 	if (err < 0)
976 		goto out_free;
977 
978 	newts->type = LWTUNNEL_ENCAP_SEG6_LOCAL;
979 	newts->flags = LWTUNNEL_STATE_INPUT_REDIRECT;
980 	newts->headroom = slwt->headroom;
981 
982 	*ts = newts;
983 
984 	return 0;
985 
986 out_free:
987 	kfree(slwt->srh);
988 	kfree(newts);
989 	return err;
990 }
991 
992 static void seg6_local_destroy_state(struct lwtunnel_state *lwt)
993 {
994 	struct seg6_local_lwt *slwt = seg6_local_lwtunnel(lwt);
995 
996 	kfree(slwt->srh);
997 
998 	if (slwt->desc->attrs & (1 << SEG6_LOCAL_BPF)) {
999 		kfree(slwt->bpf.name);
1000 		bpf_prog_put(slwt->bpf.prog);
1001 	}
1002 
1003 	return;
1004 }
1005 
1006 static int seg6_local_fill_encap(struct sk_buff *skb,
1007 				 struct lwtunnel_state *lwt)
1008 {
1009 	struct seg6_local_lwt *slwt = seg6_local_lwtunnel(lwt);
1010 	struct seg6_action_param *param;
1011 	int i, err;
1012 
1013 	if (nla_put_u32(skb, SEG6_LOCAL_ACTION, slwt->action))
1014 		return -EMSGSIZE;
1015 
1016 	for (i = 0; i < SEG6_LOCAL_MAX + 1; i++) {
1017 		if (slwt->desc->attrs & (1 << i)) {
1018 			param = &seg6_action_params[i];
1019 			err = param->put(skb, slwt);
1020 			if (err < 0)
1021 				return err;
1022 		}
1023 	}
1024 
1025 	return 0;
1026 }
1027 
1028 static int seg6_local_get_encap_size(struct lwtunnel_state *lwt)
1029 {
1030 	struct seg6_local_lwt *slwt = seg6_local_lwtunnel(lwt);
1031 	unsigned long attrs;
1032 	int nlsize;
1033 
1034 	nlsize = nla_total_size(4); /* action */
1035 
1036 	attrs = slwt->desc->attrs;
1037 
1038 	if (attrs & (1 << SEG6_LOCAL_SRH))
1039 		nlsize += nla_total_size((slwt->srh->hdrlen + 1) << 3);
1040 
1041 	if (attrs & (1 << SEG6_LOCAL_TABLE))
1042 		nlsize += nla_total_size(4);
1043 
1044 	if (attrs & (1 << SEG6_LOCAL_NH4))
1045 		nlsize += nla_total_size(4);
1046 
1047 	if (attrs & (1 << SEG6_LOCAL_NH6))
1048 		nlsize += nla_total_size(16);
1049 
1050 	if (attrs & (1 << SEG6_LOCAL_IIF))
1051 		nlsize += nla_total_size(4);
1052 
1053 	if (attrs & (1 << SEG6_LOCAL_OIF))
1054 		nlsize += nla_total_size(4);
1055 
1056 	if (attrs & (1 << SEG6_LOCAL_BPF))
1057 		nlsize += nla_total_size(sizeof(struct nlattr)) +
1058 		       nla_total_size(MAX_PROG_NAME) +
1059 		       nla_total_size(4);
1060 
1061 	return nlsize;
1062 }
1063 
1064 static int seg6_local_cmp_encap(struct lwtunnel_state *a,
1065 				struct lwtunnel_state *b)
1066 {
1067 	struct seg6_local_lwt *slwt_a, *slwt_b;
1068 	struct seg6_action_param *param;
1069 	int i;
1070 
1071 	slwt_a = seg6_local_lwtunnel(a);
1072 	slwt_b = seg6_local_lwtunnel(b);
1073 
1074 	if (slwt_a->action != slwt_b->action)
1075 		return 1;
1076 
1077 	if (slwt_a->desc->attrs != slwt_b->desc->attrs)
1078 		return 1;
1079 
1080 	for (i = 0; i < SEG6_LOCAL_MAX + 1; i++) {
1081 		if (slwt_a->desc->attrs & (1 << i)) {
1082 			param = &seg6_action_params[i];
1083 			if (param->cmp(slwt_a, slwt_b))
1084 				return 1;
1085 		}
1086 	}
1087 
1088 	return 0;
1089 }
1090 
1091 static const struct lwtunnel_encap_ops seg6_local_ops = {
1092 	.build_state	= seg6_local_build_state,
1093 	.destroy_state	= seg6_local_destroy_state,
1094 	.input		= seg6_local_input,
1095 	.fill_encap	= seg6_local_fill_encap,
1096 	.get_encap_size	= seg6_local_get_encap_size,
1097 	.cmp_encap	= seg6_local_cmp_encap,
1098 	.owner		= THIS_MODULE,
1099 };
1100 
1101 int __init seg6_local_init(void)
1102 {
1103 	return lwtunnel_encap_add_ops(&seg6_local_ops,
1104 				      LWTUNNEL_ENCAP_SEG6_LOCAL);
1105 }
1106 
1107 void seg6_local_exit(void)
1108 {
1109 	lwtunnel_encap_del_ops(&seg6_local_ops, LWTUNNEL_ENCAP_SEG6_LOCAL);
1110 }
1111