net/ipv6/seg6_local.c

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