net/ipv6/seg6_local.c

   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 }