net/ipv6/seg6_local.c

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