src/vnet/srv6/sr_policy_rewrite.c

   1 /*
   2  * sr_policy_rewrite.c: ipv6 sr policy creation
   3  *
   4  * Copyright (c) 2016 Cisco and/or its affiliates.
   5  * Licensed under the Apache License, Version 2.0 (the "License");
   6  * you may not use this file except in compliance with the License.
   7  * You may obtain a copy of the License at:
   8  *
   9  *     http://www.apache.org/licenses/LICENSE-2.0
  10  *
  11  * Unless required by applicable law or agreed to in writing, software
  12  * distributed under the License is distributed on an "AS IS" BASIS,
  13  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  14  * See the License for the specific language governing permissions and
  15  * limitations under the License.
  16  */
  17
  18 /**
  19  * @file
  20  * @brief SR policy creation and application
  21  *
  22  * Create an SR policy.
  23  * An SR policy can be either of 'default' type or 'spray' type
  24  * An SR policy has attached a list of SID lists.
  25  * In case the SR policy is a default one it will load balance among them.
  26  * An SR policy has associated a BindingSID.
  27  * In case any packet arrives with IPv6 DA == BindingSID then the SR policy
  28  * associated to such bindingSID will be applied to such packet.
  29  *
  30  * SR policies can be applied either by using IPv6 encapsulation or
  31  * SRH insertion. Both methods can be found on this file.
  32  *
  33  * Traffic input usually is IPv6 packets. However it is possible to have
  34  * IPv4 packets or L2 frames. (that are encapsulated into IPv6 with SRH)
  35  *
  36  * This file provides the appropriate VPP graph nodes to do any of these
  37  * methods.
  38  *
  39  */
  40
  41 #include <vlib/vlib.h>
  42 #include <vnet/vnet.h>
  43 #include <vnet/srv6/sr.h>
  44 #include <vnet/ip/ip4_inlines.h>
  45 #include <vnet/ip/ip6_inlines.h>
  46 #include <vnet/srv6/sr_packet.h>
  47 #include <vnet/fib/ip6_fib.h>
  48 #include <vnet/dpo/dpo.h>
  49 #include <vnet/dpo/replicate_dpo.h>
  50 #include <vnet/srv6/sr_pt.h>
  51
  52 #include <vppinfra/error.h>
  53 #include <vppinfra/elog.h>
  54
  55 /**
  56  * @brief SR policy rewrite trace
  57  */
  58 typedef struct
  59 {
  60   ip6_address_t src, dst;
  61 } sr_policy_rewrite_trace_t;
  62
  63 /* Graph arcs */
  64 #define foreach_sr_policy_rewrite_next     \
  65 _(IP6_LOOKUP, "ip6-lookup")         \
  66 _(ERROR, "error-drop")
  67
  68 typedef enum
  69 {
  70 #define _(s,n) SR_POLICY_REWRITE_NEXT_##s,
  71   foreach_sr_policy_rewrite_next
  72 #undef _
  73     SR_POLICY_REWRITE_N_NEXT,
  74 } sr_policy_rewrite_next_t;
  75
  76 /* SR rewrite errors */
  77 #define foreach_sr_policy_rewrite_error                     \
  78 _(INTERNAL_ERROR, "Segment Routing undefined error")        \
  79 _(BSID_ZERO, "BSID with SL = 0")                            \
  80 _(COUNTER_TOTAL, "SR steered IPv6 packets")                 \
  81 _(COUNTER_ENCAP, "SR: Encaps packets")                      \
  82 _(COUNTER_INSERT, "SR: SRH inserted packets")               \
  83 _(COUNTER_BSID, "SR: BindingSID steered packets")
  84
  85 typedef enum
  86 {
  87 #define _(sym,str) SR_POLICY_REWRITE_ERROR_##sym,
  88   foreach_sr_policy_rewrite_error
  89 #undef _
  90     SR_POLICY_REWRITE_N_ERROR,
  91 } sr_policy_rewrite_error_t;
  92
  93 static char *sr_policy_rewrite_error_strings[] = {
  94 #define _(sym,string) string,
  95   foreach_sr_policy_rewrite_error
  96 #undef _
  97 };
  98
  99 /**
 100  * @brief Dynamically added SR SL DPO type
 101  */
 102 static dpo_type_t sr_pr_encaps_dpo_type;
 103 static dpo_type_t sr_pr_insert_dpo_type;
 104 static dpo_type_t sr_pr_bsid_encaps_dpo_type;
 105 static dpo_type_t sr_pr_bsid_insert_dpo_type;
 106
 107 /**
 108  * @brief IPv6 SA for encapsulated packets
 109  */
 110 static ip6_address_t sr_pr_encaps_src;
 111 static u8 sr_pr_encaps_hop_limit = IPv6_DEFAULT_HOP_LIMIT;
 112
 113 /******************* SR rewrite set encaps IPv6 source addr *******************/
 114 /* Note:  This is temporal. We don't know whether to follow this path or
 115           take the ip address of a loopback interface or even the OIF         */
 116
 117 void
 118 sr_set_source (ip6_address_t * address)
 119 {
 120   clib_memcpy_fast (&sr_pr_encaps_src, address, sizeof (sr_pr_encaps_src));
 121 }
 122
 123 ip6_address_t *
 124 sr_get_encaps_source ()
 125 {
 126   return &sr_pr_encaps_src;
 127 }
 128
 129 static clib_error_t *
 130 set_sr_src_command_fn (vlib_main_t * vm, unformat_input_t * input,
 131                        vlib_cli_command_t * cmd)
 132 {
 133   while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
 134     {
 135       if (unformat
 136           (input, "addr %U", unformat_ip6_address, &sr_pr_encaps_src))
 137         return 0;
 138       else
 139         return clib_error_return (0, "No address specified");
 140     }
 141   return clib_error_return (0, "No address specified");
 142 }
 143
 144 /* *INDENT-OFF* */
 145 VLIB_CLI_COMMAND (set_sr_src_command, static) = {
 146   .path = "set sr encaps source",
 147   .short_help = "set sr encaps source addr <ip6_addr>",
 148   .function = set_sr_src_command_fn,
 149 };
 150 /* *INDENT-ON* */
 151
 152 /******************** SR rewrite set encaps IPv6 hop-limit ********************/
 153
 154 void
 155 sr_set_hop_limit (u8 hop_limit)
 156 {
 157   sr_pr_encaps_hop_limit = hop_limit;
 158 }
 159
 160 u8
 161 sr_get_hop_limit (void)
 162 {
 163   return sr_pr_encaps_hop_limit;
 164 }
 165
 166 static clib_error_t *
 167 set_sr_hop_limit_command_fn (vlib_main_t * vm, unformat_input_t * input,
 168                              vlib_cli_command_t * cmd)
 169 {
 170   int hop_limit = sr_get_hop_limit ();
 171
 172   if (unformat_check_input (input) == UNFORMAT_END_OF_INPUT)
 173     return clib_error_return (0, "No value specified");
 174   if (!unformat (input, "%d", &hop_limit))
 175     return clib_error_return (0, "Invalid value");
 176   if (hop_limit <= 0 || hop_limit > 255)
 177     return clib_error_return (0, "Value out of range [1-255]");
 178   sr_pr_encaps_hop_limit = (u8) hop_limit;
 179   return 0;
 180 }
 181
 182 /* *INDENT-OFF* */
 183 VLIB_CLI_COMMAND (set_sr_hop_limit_command, static) = {
 184   .path = "set sr encaps hop-limit",
 185   .short_help = "set sr encaps hop-limit <value>",
 186   .function = set_sr_hop_limit_command_fn,
 187 };
 188 /* *INDENT-ON* */
 189
 190 /*********************** SR rewrite string computation ************************/
 191 /**
 192  * @brief SR rewrite string computation for IPv6 encapsulation (inline)
 193  *
 194  * @param sl is a vector of IPv6 addresses composing the Segment List
 195  *
 196  * @return precomputed rewrite string for encapsulation
 197  */
 198 static inline u8 *
 199 compute_rewrite_encaps (ip6_address_t *sl, u8 type)
 200 {
 201   ip6_header_t *iph;
 202   ip6_sr_header_t *srh;
 203   ip6_sr_pt_tlv_t *srh_pt_tlv;
 204   ip6_address_t *addrp, *this_address;
 205   u32 header_length = 0;
 206   u8 *rs = NULL;
 207
 208   header_length = 0;
 209   header_length += IPv6_DEFAULT_HEADER_LENGTH;
 210   if (type == SR_POLICY_TYPE_TEF)
 211     {
 212       header_length += sizeof (ip6_sr_header_t);
 213       header_length += vec_len (sl) * sizeof (ip6_address_t);
 214       header_length += sizeof (ip6_sr_pt_tlv_t);
 215     }
 216   else if (vec_len (sl) > 1)
 217     {
 218       header_length += sizeof (ip6_sr_header_t);
 219       header_length += vec_len (sl) * sizeof (ip6_address_t);
 220     }
 221
 222   vec_validate (rs, header_length - 1);
 223
 224   iph = (ip6_header_t *) rs;
 225   iph->ip_version_traffic_class_and_flow_label =
 226     clib_host_to_net_u32 (0 | ((6 & 0xF) << 28));
 227   iph->src_address.as_u64[0] = sr_pr_encaps_src.as_u64[0];
 228   iph->src_address.as_u64[1] = sr_pr_encaps_src.as_u64[1];
 229   iph->payload_length = header_length - IPv6_DEFAULT_HEADER_LENGTH;
 230   iph->protocol = IP_PROTOCOL_IPV6;
 231   iph->hop_limit = sr_pr_encaps_hop_limit;
 232
 233   if (type == SR_POLICY_TYPE_TEF)
 234     {
 235       srh = (ip6_sr_header_t *) (iph + 1);
 236       iph->protocol = IP_PROTOCOL_IPV6_ROUTE;
 237       srh->protocol = IP_PROTOCOL_IPV6;
 238       srh->type = ROUTING_HEADER_TYPE_SR;
 239       srh->flags = 0x00;
 240       srh->tag = 0x0000;
 241       srh->segments_left = vec_len (sl) - 1;
 242       srh->last_entry = vec_len (sl) - 1;
 243       srh->length =
 244         ((sizeof (ip6_sr_header_t) + (vec_len (sl) * sizeof (ip6_address_t)) +
 245           sizeof (ip6_sr_pt_tlv_t)) /
 246          8) -
 247         1;
 248       addrp = srh->segments + vec_len (sl) - 1;
 249       vec_foreach (this_address, sl)
 250         {
 251           clib_memcpy_fast (addrp->as_u8, this_address->as_u8,
 252                             sizeof (ip6_address_t));
 253           addrp--;
 254         }
 255       srh_pt_tlv = (ip6_sr_pt_tlv_t *) (srh->segments + vec_len (sl));
 256       srh_pt_tlv->type = IP6_SRH_PT_TLV_TYPE;
 257       srh_pt_tlv->length = IP6_SRH_PT_TLV_LEN;
 258     }
 259   else if (vec_len (sl) > 1)
 260     {
 261       srh = (ip6_sr_header_t *) (iph + 1);
 262       iph->protocol = IP_PROTOCOL_IPV6_ROUTE;
 263       srh->protocol = IP_PROTOCOL_IPV6;
 264       srh->type = ROUTING_HEADER_TYPE_SR;
 265       srh->segments_left = vec_len (sl) - 1;
 266       srh->last_entry = vec_len (sl) - 1;
 267       srh->length = ((sizeof (ip6_sr_header_t) +
 268                       (vec_len (sl) * sizeof (ip6_address_t))) / 8) - 1;
 269       srh->flags = 0x00;
 270       srh->tag = 0x0000;
 271       addrp = srh->segments + vec_len (sl) - 1;
 272       vec_foreach (this_address, sl)
 273       {
 274         clib_memcpy_fast (addrp->as_u8, this_address->as_u8,
 275                           sizeof (ip6_address_t));
 276         addrp--;
 277       }
 278     }
 279   iph->dst_address.as_u64[0] = sl->as_u64[0];
 280   iph->dst_address.as_u64[1] = sl->as_u64[1];
 281   return rs;
 282 }
 283
 284 /**
 285  * @brief SR rewrite string computation for SRH insertion (inline)
 286  *
 287  * @param sl is a vector of IPv6 addresses composing the Segment List
 288  *
 289  * @return precomputed rewrite string for SRH insertion
 290  */
 291 static inline u8 *
 292 compute_rewrite_insert (ip6_address_t *sl, u8 type)
 293 {
 294   ip6_sr_header_t *srh;
 295   ip6_address_t *addrp, *this_address;
 296   u32 header_length = 0;
 297   u8 *rs = NULL;
 298
 299   header_length = 0;
 300   header_length += sizeof (ip6_sr_header_t);
 301   header_length += (vec_len (sl) + 1) * sizeof (ip6_address_t);
 302
 303   vec_validate (rs, header_length - 1);
 304
 305   srh = (ip6_sr_header_t *) rs;
 306   srh->type = ROUTING_HEADER_TYPE_SR;
 307   srh->segments_left = vec_len (sl);
 308   srh->last_entry = vec_len (sl);
 309   srh->length = ((sizeof (ip6_sr_header_t) +
 310                   ((vec_len (sl) + 1) * sizeof (ip6_address_t))) / 8) - 1;
 311   srh->flags = 0x00;
 312   srh->tag = 0x0000;
 313   addrp = srh->segments + vec_len (sl);
 314   vec_foreach (this_address, sl)
 315   {
 316     clib_memcpy_fast (addrp->as_u8, this_address->as_u8,
 317                       sizeof (ip6_address_t));
 318     addrp--;
 319   }
 320   return rs;
 321 }
 322
 323 /**
 324  * @brief SR rewrite string computation for SRH insertion with BSID (inline)
 325  *
 326  * @param sl is a vector of IPv6 addresses composing the Segment List
 327  *
 328  * @return precomputed rewrite string for SRH insertion with BSID
 329  */
 330 static inline u8 *
 331 compute_rewrite_bsid (ip6_address_t * sl)
 332 {
 333   ip6_sr_header_t *srh;
 334   ip6_address_t *addrp, *this_address;
 335   u32 header_length = 0;
 336   u8 *rs = NULL;
 337
 338   header_length = 0;
 339   header_length += sizeof (ip6_sr_header_t);
 340   header_length += vec_len (sl) * sizeof (ip6_address_t);
 341
 342   vec_validate (rs, header_length - 1);
 343
 344   srh = (ip6_sr_header_t *) rs;
 345   srh->type = ROUTING_HEADER_TYPE_SR;
 346   srh->segments_left = vec_len (sl) - 1;
 347   srh->last_entry = vec_len (sl) - 1;
 348   srh->length = ((sizeof (ip6_sr_header_t) +
 349                   (vec_len (sl) * sizeof (ip6_address_t))) / 8) - 1;
 350   srh->flags = 0x00;
 351   srh->tag = 0x0000;
 352   addrp = srh->segments + vec_len (sl) - 1;
 353   vec_foreach (this_address, sl)
 354   {
 355     clib_memcpy_fast (addrp->as_u8, this_address->as_u8,
 356                       sizeof (ip6_address_t));
 357     addrp--;
 358   }
 359   return rs;
 360 }
 361
 362 /***************************  SR LB helper functions **************************/
 363 /**
 364  * @brief Creates a Segment List and adds it to an SR policy
 365  *
 366  * Creates a Segment List and adds it to the SR policy. Notice that the SL are
 367  * not necessarily unique. Hence there might be two Segment List within the
 368  * same SR Policy with exactly the same segments and same weight.
 369  *
 370  * @param sr_policy is the SR policy where the SL will be added
 371  * @param sl is a vector of IPv6 addresses composing the Segment List
 372  * @param weight is the weight of the SegmentList (for load-balancing purposes)
 373  * @param is_encap represents the mode (SRH insertion vs Encapsulation)
 374  *
 375  * @return pointer to the just created segment list
 376  */
 377 static inline ip6_sr_sl_t *
 378 create_sl (ip6_sr_policy_t * sr_policy, ip6_address_t * sl, u32 weight,
 379            u8 is_encap)
 380 {
 381   ip6_sr_main_t *sm = &sr_main;
 382   ip6_sr_sl_t *segment_list;
 383   sr_policy_fn_registration_t *plugin = 0;
 384
 385   pool_get (sm->sid_lists, segment_list);
 386   clib_memset (segment_list, 0, sizeof (*segment_list));
 387
 388   vec_add1 (sr_policy->segments_lists, segment_list - sm->sid_lists);
 389
 390   /* Fill in segment list */
 391   segment_list->weight =
 392     (weight != (u32) ~ 0 ? weight : SR_SEGMENT_LIST_WEIGHT_DEFAULT);
 393
 394   segment_list->segments = vec_dup (sl);
 395   segment_list->policy_type = sr_policy->type;
 396
 397   segment_list->egress_fib_table =
 398     ip6_fib_index_from_table_id (sr_policy->fib_table);
 399
 400   if (is_encap)
 401     {
 402       segment_list->rewrite = compute_rewrite_encaps (sl, sr_policy->type);
 403       segment_list->rewrite_bsid = segment_list->rewrite;
 404     }
 405   else
 406     {
 407       segment_list->rewrite = compute_rewrite_insert (sl, sr_policy->type);
 408       segment_list->rewrite_bsid = compute_rewrite_bsid (sl);
 409     }
 410
 411   if (sr_policy->plugin)
 412     {
 413       plugin =
 414         pool_elt_at_index (sm->policy_plugin_functions,
 415                            sr_policy->plugin - SR_BEHAVIOR_LAST);
 416
 417       segment_list->plugin = sr_policy->plugin;
 418       segment_list->plugin_mem = sr_policy->plugin_mem;
 419
 420       plugin->creation (sr_policy);
 421     }
 422
 423   /* Create DPO */
 424   dpo_reset (&segment_list->bsid_dpo);
 425   dpo_reset (&segment_list->ip6_dpo);
 426   dpo_reset (&segment_list->ip4_dpo);
 427
 428   if (is_encap)
 429     {
 430       if (!sr_policy->plugin)
 431         {
 432           dpo_set (&segment_list->ip6_dpo, sr_pr_encaps_dpo_type,
 433                    DPO_PROTO_IP6, segment_list - sm->sid_lists);
 434           dpo_set (&segment_list->ip4_dpo, sr_pr_encaps_dpo_type,
 435                    DPO_PROTO_IP4, segment_list - sm->sid_lists);
 436           dpo_set (&segment_list->bsid_dpo, sr_pr_bsid_encaps_dpo_type,
 437                    DPO_PROTO_IP6, segment_list - sm->sid_lists);
 438         }
 439       else
 440         {
 441           dpo_set (&segment_list->ip6_dpo, plugin->dpo, DPO_PROTO_IP6,
 442                    segment_list - sm->sid_lists);
 443           dpo_set (&segment_list->ip4_dpo, plugin->dpo, DPO_PROTO_IP4,
 444                    segment_list - sm->sid_lists);
 445           dpo_set (&segment_list->bsid_dpo, plugin->dpo, DPO_PROTO_IP6,
 446                    segment_list - sm->sid_lists);
 447         }
 448     }
 449   else
 450     {
 451       if (!sr_policy->plugin)
 452         {
 453           dpo_set (&segment_list->ip6_dpo, sr_pr_insert_dpo_type,
 454                    DPO_PROTO_IP6, segment_list - sm->sid_lists);
 455           dpo_set (&segment_list->bsid_dpo, sr_pr_bsid_insert_dpo_type,
 456                    DPO_PROTO_IP6, segment_list - sm->sid_lists);
 457         }
 458       else
 459         {
 460           dpo_set (&segment_list->ip6_dpo, plugin->dpo, DPO_PROTO_IP6,
 461                    segment_list - sm->sid_lists);
 462           dpo_set (&segment_list->bsid_dpo, plugin->dpo, DPO_PROTO_IP6,
 463                    segment_list - sm->sid_lists);
 464         }
 465     }
 466
 467   return segment_list;
 468 }
 469
 470 /**
 471  * @brief Updates the Load-Balancer after an SR Policy change
 472  *
 473  * @param sr_policy is the modified SR Policy
 474  */
 475 static inline void
 476 update_lb (ip6_sr_policy_t * sr_policy)
 477 {
 478   flow_hash_config_t fhc;
 479   u32 *sl_index;
 480   ip6_sr_sl_t *segment_list;
 481   ip6_sr_main_t *sm = &sr_main;
 482   load_balance_path_t path;
 483   path.path_index = FIB_NODE_INDEX_INVALID;
 484   load_balance_path_t *ip4_path_vector = 0;
 485   load_balance_path_t *ip6_path_vector = 0;
 486   load_balance_path_t *b_path_vector = 0;
 487
 488   /* In case LB does not exist, create it */
 489   if (!dpo_id_is_valid (&sr_policy->bsid_dpo))
 490     {
 491       fib_prefix_t pfx = {
 492         .fp_proto = FIB_PROTOCOL_IP6,
 493         .fp_len = 128,
 494         .fp_addr = {
 495                     .ip6 = sr_policy->bsid,
 496                     }
 497       };
 498
 499       /* Add FIB entry for BSID */
 500       fhc = fib_table_get_flow_hash_config (sr_policy->fib_table,
 501                                             FIB_PROTOCOL_IP6);
 502
 503       dpo_set (&sr_policy->bsid_dpo, DPO_LOAD_BALANCE, DPO_PROTO_IP6,
 504                load_balance_create (0, DPO_PROTO_IP6, fhc));
 505
 506       dpo_set (&sr_policy->ip6_dpo, DPO_LOAD_BALANCE, DPO_PROTO_IP6,
 507                load_balance_create (0, DPO_PROTO_IP6, fhc));
 508
 509       /* Update FIB entry's to point to the LB DPO in the main FIB and hidden one */
 510       fib_table_entry_special_dpo_update (fib_table_find (FIB_PROTOCOL_IP6,
 511                                                           sr_policy->fib_table),
 512                                           &pfx, FIB_SOURCE_SR,
 513                                           FIB_ENTRY_FLAG_EXCLUSIVE,
 514                                           &sr_policy->bsid_dpo);
 515
 516       fib_table_entry_special_dpo_update (sm->fib_table_ip6,
 517                                           &pfx,
 518                                           FIB_SOURCE_SR,
 519                                           FIB_ENTRY_FLAG_EXCLUSIVE,
 520                                           &sr_policy->ip6_dpo);
 521
 522       if (sr_policy->is_encap)
 523         {
 524           dpo_set (&sr_policy->ip4_dpo, DPO_LOAD_BALANCE, DPO_PROTO_IP4,
 525                    load_balance_create (0, DPO_PROTO_IP4, fhc));
 526
 527           fib_table_entry_special_dpo_update (sm->fib_table_ip4,
 528                                               &pfx,
 529                                               FIB_SOURCE_SR,
 530                                               FIB_ENTRY_FLAG_EXCLUSIVE,
 531                                               &sr_policy->ip4_dpo);
 532         }
 533
 534     }
 535
 536   /* Create the LB path vector */
 537   vec_foreach (sl_index, sr_policy->segments_lists)
 538   {
 539     segment_list = pool_elt_at_index (sm->sid_lists, *sl_index);
 540     path.path_dpo = segment_list->bsid_dpo;
 541     path.path_weight = segment_list->weight;
 542     vec_add1 (b_path_vector, path);
 543     path.path_dpo = segment_list->ip6_dpo;
 544     vec_add1 (ip6_path_vector, path);
 545     if (sr_policy->is_encap)
 546       {
 547         path.path_dpo = segment_list->ip4_dpo;
 548         vec_add1 (ip4_path_vector, path);
 549       }
 550   }
 551
 552   /* Update LB multipath */
 553   load_balance_multipath_update (&sr_policy->bsid_dpo, b_path_vector,
 554                                  LOAD_BALANCE_FLAG_NONE);
 555   load_balance_multipath_update (&sr_policy->ip6_dpo, ip6_path_vector,
 556                                  LOAD_BALANCE_FLAG_NONE);
 557   if (sr_policy->is_encap)
 558     load_balance_multipath_update (&sr_policy->ip4_dpo, ip4_path_vector,
 559                                    LOAD_BALANCE_FLAG_NONE);
 560
 561   /* Cleanup */
 562   vec_free (b_path_vector);
 563   vec_free (ip6_path_vector);
 564   vec_free (ip4_path_vector);
 565 }
 566
 567 /**
 568  * @brief Updates the Replicate DPO after an SR Policy change
 569  *
 570  * @param sr_policy is the modified SR Policy (type spray)
 571  */
 572 static inline void
 573 update_replicate (ip6_sr_policy_t * sr_policy)
 574 {
 575   u32 *sl_index;
 576   ip6_sr_sl_t *segment_list;
 577   ip6_sr_main_t *sm = &sr_main;
 578   load_balance_path_t path;
 579   path.path_index = FIB_NODE_INDEX_INVALID;
 580   load_balance_path_t *b_path_vector = 0;
 581   load_balance_path_t *ip6_path_vector = 0;
 582   load_balance_path_t *ip4_path_vector = 0;
 583
 584   /* In case LB does not exist, create it */
 585   if (!dpo_id_is_valid (&sr_policy->bsid_dpo))
 586     {
 587       dpo_set (&sr_policy->bsid_dpo, DPO_REPLICATE,
 588                DPO_PROTO_IP6, replicate_create (0, DPO_PROTO_IP6));
 589
 590       dpo_set (&sr_policy->ip6_dpo, DPO_REPLICATE,
 591                DPO_PROTO_IP6, replicate_create (0, DPO_PROTO_IP6));
 592
 593       /* Update FIB entry's DPO to point to SR without LB */
 594       fib_prefix_t pfx = {
 595         .fp_proto = FIB_PROTOCOL_IP6,
 596         .fp_len = 128,
 597         .fp_addr = {
 598                     .ip6 = sr_policy->bsid,
 599                     }
 600       };
 601       fib_table_entry_special_dpo_update (fib_table_find (FIB_PROTOCOL_IP6,
 602                                                           sr_policy->fib_table),
 603                                           &pfx, FIB_SOURCE_SR,
 604                                           FIB_ENTRY_FLAG_EXCLUSIVE,
 605                                           &sr_policy->bsid_dpo);
 606
 607       fib_table_entry_special_dpo_update (sm->fib_table_ip6,
 608                                           &pfx,
 609                                           FIB_SOURCE_SR,
 610                                           FIB_ENTRY_FLAG_EXCLUSIVE,
 611                                           &sr_policy->ip6_dpo);
 612
 613       if (sr_policy->is_encap)
 614         {
 615           dpo_set (&sr_policy->ip4_dpo, DPO_REPLICATE, DPO_PROTO_IP4,
 616                    replicate_create (0, DPO_PROTO_IP4));
 617
 618           fib_table_entry_special_dpo_update (sm->fib_table_ip4,
 619                                               &pfx,
 620                                               FIB_SOURCE_SR,
 621                                               FIB_ENTRY_FLAG_EXCLUSIVE,
 622                                               &sr_policy->ip4_dpo);
 623         }
 624
 625     }
 626
 627   /* Create the replicate path vector */
 628   path.path_weight = 1;
 629   vec_foreach (sl_index, sr_policy->segments_lists)
 630   {
 631     segment_list = pool_elt_at_index (sm->sid_lists, *sl_index);
 632     path.path_dpo = segment_list->bsid_dpo;
 633     vec_add1 (b_path_vector, path);
 634     path.path_dpo = segment_list->ip6_dpo;
 635     vec_add1 (ip6_path_vector, path);
 636     if (sr_policy->is_encap)
 637       {
 638         path.path_dpo = segment_list->ip4_dpo;
 639         vec_add1 (ip4_path_vector, path);
 640       }
 641   }
 642
 643   /* Update replicate multipath */
 644   replicate_multipath_update (&sr_policy->bsid_dpo, b_path_vector);
 645   replicate_multipath_update (&sr_policy->ip6_dpo, ip6_path_vector);
 646   if (sr_policy->is_encap)
 647     replicate_multipath_update (&sr_policy->ip4_dpo, ip4_path_vector);
 648 }
 649
 650 /******************************* SR rewrite API *******************************/
 651 /* Three functions for handling sr policies:
 652  *   -> sr_policy_add
 653  *   -> sr_policy_del
 654  *   -> sr_policy_mod
 655  * All of them are API. CLI function on sr_policy_command_fn                  */
 656
 657 /**
 658  * @brief Create a new SR policy
 659  *
 660  * @param bsid is the bindingSID of the SR Policy
 661  * @param segments is a vector of IPv6 address composing the segment list
 662  * @param weight is the weight of the sid list. optional.
 663  * @param behavior is the behavior of the SR policy. (default//spray)
 664  * @param fib_table is the VRF where to install the FIB entry for the BSID
 665  * @param is_encap (bool) whether SR policy should behave as Encap/SRH Insertion
 666  *
 667  * @return 0 if correct, else error
 668  */
 669 int
 670 sr_policy_add (ip6_address_t *bsid, ip6_address_t *segments, u32 weight,
 671                u8 type, u32 fib_table, u8 is_encap, u16 plugin,
 672                void *ls_plugin_mem)
 673 {
 674   ip6_sr_main_t *sm = &sr_main;
 675   ip6_sr_policy_t *sr_policy = 0;
 676   uword *p;
 677
 678   /* Search for existing keys (BSID) */
 679   p = mhash_get (&sm->sr_policies_index_hash, bsid);
 680   if (p)
 681     {
 682       /* Add SR policy that already exists; complain */
 683       return -12;
 684     }
 685
 686   /* Search collision in FIB entries */
 687   /* Explanation: It might be possible that some other entity has already
 688    * created a route for the BSID. This in theory is impossible, but in
 689    * practise we could see it. Assert it and scream if needed */
 690   fib_prefix_t pfx = {
 691     .fp_proto = FIB_PROTOCOL_IP6,
 692     .fp_len = 128,
 693     .fp_addr = {
 694                 .ip6 = *bsid,
 695                 }
 696   };
 697
 698   /* Lookup the FIB index associated to the table selected */
 699   u32 fib_index = fib_table_find (FIB_PROTOCOL_IP6,
 700                                   (fib_table != (u32) ~ 0 ? fib_table : 0));
 701   if (fib_index == ~0)
 702     return -13;
 703
 704   /* Lookup whether there exists an entry for the BSID */
 705   fib_node_index_t fei = fib_table_lookup_exact_match (fib_index, &pfx);
 706   if (FIB_NODE_INDEX_INVALID != fei)
 707     return -12;                 //There is an entry for such lookup
 708
 709   /* Add an SR policy object */
 710   pool_get (sm->sr_policies, sr_policy);
 711   clib_memset (sr_policy, 0, sizeof (*sr_policy));
 712   clib_memcpy_fast (&sr_policy->bsid, bsid, sizeof (ip6_address_t));
 713   sr_policy->type = type;
 714   sr_policy->fib_table = (fib_table != (u32) ~ 0 ? fib_table : 0);      //Is default FIB 0 ?
 715   sr_policy->is_encap = is_encap;
 716
 717   if (plugin)
 718     {
 719       sr_policy->plugin = plugin;
 720       sr_policy->plugin_mem = ls_plugin_mem;
 721     }
 722
 723   /* Copy the key */
 724   mhash_set (&sm->sr_policies_index_hash, bsid, sr_policy - sm->sr_policies,
 725              NULL);
 726
 727   /* Create a segment list and add the index to the SR policy */
 728   create_sl (sr_policy, segments, weight, is_encap);
 729
 730   /* If FIB doesnt exist, create them */
 731   if (sm->fib_table_ip6 == (u32) ~ 0)
 732     {
 733       sm->fib_table_ip6 = fib_table_create_and_lock (FIB_PROTOCOL_IP6,
 734                                                      FIB_SOURCE_SR,
 735                                                      "SRv6 steering of IP6 prefixes through BSIDs");
 736       sm->fib_table_ip4 = fib_table_create_and_lock (FIB_PROTOCOL_IP6,
 737                                                      FIB_SOURCE_SR,
 738                                                      "SRv6 steering of IP4 prefixes through BSIDs");
 739     }
 740
 741   /* Create IPv6 FIB for the BindingSID attached to the DPO of the only SL */
 742   if (sr_policy->type == SR_POLICY_TYPE_DEFAULT ||
 743       sr_policy->type == SR_POLICY_TYPE_TEF)
 744     update_lb (sr_policy);
 745   else if (sr_policy->type == SR_POLICY_TYPE_SPRAY)
 746     update_replicate (sr_policy);
 747   return 0;
 748 }
 749
 750 /**
 751  * @brief Delete a SR policy
 752  *
 753  * @param bsid is the bindingSID of the SR Policy
 754  * @param index is the index of the SR policy
 755  *
 756  * @return 0 if correct, else error
 757  */
 758 int
 759 sr_policy_del (ip6_address_t * bsid, u32 index)
 760 {
 761   ip6_sr_main_t *sm = &sr_main;
 762   ip6_sr_policy_t *sr_policy = 0;
 763   ip6_sr_sl_t *segment_list;
 764   u32 *sl_index;
 765   uword *p;
 766
 767   if (bsid)
 768     {
 769       p = mhash_get (&sm->sr_policies_index_hash, bsid);
 770       if (p)
 771         sr_policy = pool_elt_at_index (sm->sr_policies, p[0]);
 772       else
 773         return -1;
 774     }
 775   else
 776     {
 777       sr_policy = pool_elt_at_index (sm->sr_policies, index);
 778     }
 779
 780   /* Remove BindingSID FIB entry */
 781   fib_prefix_t pfx = {
 782     .fp_proto = FIB_PROTOCOL_IP6,
 783     .fp_len = 128,
 784     .fp_addr = {
 785                 .ip6 = sr_policy->bsid,
 786                 }
 787     ,
 788   };
 789
 790   fib_table_entry_special_remove (fib_table_find (FIB_PROTOCOL_IP6,
 791                                                   sr_policy->fib_table),
 792                                   &pfx, FIB_SOURCE_SR);
 793
 794   fib_table_entry_special_remove (sm->fib_table_ip6, &pfx, FIB_SOURCE_SR);
 795
 796   if (sr_policy->is_encap)
 797     fib_table_entry_special_remove (sm->fib_table_ip4, &pfx, FIB_SOURCE_SR);
 798
 799   if (dpo_id_is_valid (&sr_policy->bsid_dpo))
 800     {
 801       dpo_reset (&sr_policy->bsid_dpo);
 802       dpo_reset (&sr_policy->ip4_dpo);
 803       dpo_reset (&sr_policy->ip6_dpo);
 804     }
 805
 806   /* Clean SID Lists */
 807   vec_foreach (sl_index, sr_policy->segments_lists)
 808   {
 809     segment_list = pool_elt_at_index (sm->sid_lists, *sl_index);
 810     vec_free (segment_list->segments);
 811     vec_free (segment_list->rewrite);
 812     if (!sr_policy->is_encap)
 813       vec_free (segment_list->rewrite_bsid);
 814     pool_put_index (sm->sid_lists, *sl_index);
 815   }
 816
 817   if (sr_policy->plugin)
 818     {
 819       sr_policy_fn_registration_t *plugin = 0;
 820
 821       plugin =
 822         pool_elt_at_index (sm->policy_plugin_functions,
 823                            sr_policy->plugin - SR_BEHAVIOR_LAST);
 824
 825       plugin->removal (sr_policy);
 826       sr_policy->plugin = 0;
 827       sr_policy->plugin_mem = NULL;
 828     }
 829
 830   /* Remove SR policy entry */
 831   mhash_unset (&sm->sr_policies_index_hash, &sr_policy->bsid, NULL);
 832   pool_put (sm->sr_policies, sr_policy);
 833
 834   /* If FIB empty unlock it */
 835   if (!pool_elts (sm->sr_policies) && !pool_elts (sm->steer_policies))
 836     {
 837       fib_table_unlock (sm->fib_table_ip6, FIB_PROTOCOL_IP6, FIB_SOURCE_SR);
 838       fib_table_unlock (sm->fib_table_ip4, FIB_PROTOCOL_IP6, FIB_SOURCE_SR);
 839       sm->fib_table_ip6 = (u32) ~ 0;
 840       sm->fib_table_ip4 = (u32) ~ 0;
 841     }
 842
 843   return 0;
 844 }
 845
 846 /**
 847  * @brief Modify an existing SR policy
 848  *
 849  * The possible modifications are adding a new Segment List, modifying an
 850  * existing Segment List (modify the weight only) and delete a given
 851  * Segment List from the SR Policy.
 852  *
 853  * @param bsid is the bindingSID of the SR Policy
 854  * @param index is the index of the SR policy
 855  * @param fib_table is the VRF where to install the FIB entry for the BSID
 856  * @param operation is the operation to perform (among the top ones)
 857  * @param segments is a vector of IPv6 address composing the segment list
 858  * @param sl_index is the index of the Segment List to modify/delete
 859  * @param weight is the weight of the sid list. optional.
 860  * @param is_encap Mode. Encapsulation or SRH insertion.
 861  *
 862  * @return 0 if correct, else error
 863  */
 864 int
 865 sr_policy_mod (ip6_address_t * bsid, u32 index, u32 fib_table,
 866                u8 operation, ip6_address_t * segments, u32 sl_index,
 867                u32 weight)
 868 {
 869   ip6_sr_main_t *sm = &sr_main;
 870   ip6_sr_policy_t *sr_policy = 0;
 871   ip6_sr_sl_t *segment_list;
 872   u32 *sl_index_iterate;
 873   uword *p;
 874
 875   if (bsid)
 876     {
 877       p = mhash_get (&sm->sr_policies_index_hash, bsid);
 878       if (p)
 879         sr_policy = pool_elt_at_index (sm->sr_policies, p[0]);
 880       else
 881         return -1;
 882     }
 883   else
 884     {
 885       sr_policy = pool_elt_at_index (sm->sr_policies, index);
 886     }
 887
 888   if (operation == 1)           /* Add SR List to an existing SR policy */
 889     {
 890       /* Create the new SL */
 891       segment_list =
 892         create_sl (sr_policy, segments, weight, sr_policy->is_encap);
 893
 894       /* Create a new LB DPO */
 895       if (sr_policy->type == SR_POLICY_TYPE_DEFAULT)
 896         update_lb (sr_policy);
 897       else if (sr_policy->type == SR_POLICY_TYPE_SPRAY)
 898         update_replicate (sr_policy);
 899     }
 900   else if (operation == 2)      /* Delete SR List from an existing SR policy */
 901     {
 902       /* Check that currently there are more than one SID list */
 903       if (vec_len (sr_policy->segments_lists) == 1)
 904         return -21;
 905
 906       /* Check that the SR list does exist and is assigned to the sr policy */
 907       vec_foreach (sl_index_iterate, sr_policy->segments_lists)
 908         if (*sl_index_iterate == sl_index)
 909         break;
 910
 911       if (*sl_index_iterate != sl_index)
 912         return -22;
 913
 914       /* Remove the lucky SR list that is being kicked out */
 915       segment_list = pool_elt_at_index (sm->sid_lists, sl_index);
 916       vec_free (segment_list->segments);
 917       vec_free (segment_list->rewrite);
 918       if (!sr_policy->is_encap)
 919         vec_free (segment_list->rewrite_bsid);
 920       pool_put_index (sm->sid_lists, sl_index);
 921       vec_del1 (sr_policy->segments_lists,
 922                 sl_index_iterate - sr_policy->segments_lists);
 923
 924       /* Create a new LB DPO */
 925       if (sr_policy->type == SR_POLICY_TYPE_DEFAULT)
 926         update_lb (sr_policy);
 927       else if (sr_policy->type == SR_POLICY_TYPE_SPRAY)
 928         update_replicate (sr_policy);
 929     }
 930   else if (operation == 3)      /* Modify the weight of an existing SR List */
 931     {
 932       /* Find the corresponding SL */
 933       vec_foreach (sl_index_iterate, sr_policy->segments_lists)
 934         if (*sl_index_iterate == sl_index)
 935         break;
 936
 937       if (*sl_index_iterate != sl_index)
 938         return -32;
 939
 940       /* Change the weight */
 941       segment_list = pool_elt_at_index (sm->sid_lists, sl_index);
 942       segment_list->weight = weight;
 943
 944       /* Update LB */
 945       if (sr_policy->type == SR_POLICY_TYPE_DEFAULT)
 946         update_lb (sr_policy);
 947     }
 948   else                          /* Incorrect op. */
 949     return -1;
 950
 951   return 0;
 952 }
 953
 954 /**
 955  * @brief CLI for 'sr policies' command family
 956  */
 957 static clib_error_t *
 958 sr_policy_command_fn (vlib_main_t * vm, unformat_input_t * input,
 959                       vlib_cli_command_t * cmd)
 960 {
 961   ip6_sr_main_t *sm = &sr_main;
 962   int rv = -1;
 963   char is_del = 0, is_add = 0, is_mod = 0;
 964   char policy_set = 0;
 965   ip6_address_t bsid, next_address;
 966   u32 sr_policy_index = (u32) ~ 0, sl_index = (u32) ~ 0;
 967   u32 weight = (u32) ~ 0, fib_table = (u32) ~ 0;
 968   ip6_address_t *segments = 0, *this_seg;
 969   u8 operation = 0;
 970   char is_encap = 1;
 971   u8 type = SR_POLICY_TYPE_DEFAULT;
 972   u16 behavior = 0;
 973   void *ls_plugin_mem = 0;
 974
 975   while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
 976     {
 977       if (!is_add && !is_mod && !is_del && unformat (input, "add"))
 978         is_add = 1;
 979       else if (!is_add && !is_mod && !is_del && unformat (input, "del"))
 980         is_del = 1;
 981       else if (!is_add && !is_mod && !is_del && unformat (input, "mod"))
 982         is_mod = 1;
 983       else if (!policy_set
 984                && unformat (input, "bsid %U", unformat_ip6_address, &bsid))
 985         policy_set = 1;
 986       else if (!is_add && !policy_set
 987                && unformat (input, "index %d", &sr_policy_index))
 988         policy_set = 1;
 989       else if (unformat (input, "weight %d", &weight));
 990       else
 991         if (unformat (input, "next %U", unformat_ip6_address, &next_address))
 992         {
 993           vec_add2 (segments, this_seg, 1);
 994           clib_memcpy_fast (this_seg->as_u8, next_address.as_u8,
 995                             sizeof (*this_seg));
 996         }
 997       else if (unformat (input, "add sl"))
 998         operation = 1;
 999       else if (unformat (input, "del sl index %d", &sl_index))
1000         operation = 2;
1001       else if (unformat (input, "mod sl index %d", &sl_index))
1002         operation = 3;
1003       else if (fib_table == (u32) ~ 0
1004                && unformat (input, "fib-table %d", &fib_table));
1005       else if (unformat (input, "encap"))
1006         is_encap = 1;
1007       else if (unformat (input, "insert"))
1008         is_encap = 0;
1009       else if (unformat (input, "spray"))
1010         type = SR_POLICY_TYPE_SPRAY;
1011       else if (unformat (input, "tef"))
1012         type = SR_POLICY_TYPE_TEF;
1013       else if (!behavior && unformat (input, "behavior"))
1014         {
1015           sr_policy_fn_registration_t *plugin = 0, **vec_plugins = 0;
1016           sr_policy_fn_registration_t **plugin_it = 0;
1017
1018           /* *INDENT-OFF* */
1019           pool_foreach (plugin, sm->policy_plugin_functions)
1020             {
1021               vec_add1 (vec_plugins, plugin);
1022             }
1023           /* *INDENT-ON* */
1024
1025           vec_foreach (plugin_it, vec_plugins)
1026           {
1027             if (unformat
1028                 (input, "%U", (*plugin_it)->ls_unformat, &ls_plugin_mem))
1029               {
1030                 behavior = (*plugin_it)->sr_policy_function_number;
1031                 break;
1032               }
1033           }
1034
1035           if (!behavior)
1036             {
1037               return clib_error_return (0, "Invalid behavior");
1038             }
1039         }
1040       else
1041         break;
1042     }
1043
1044   if (!is_add && !is_mod && !is_del)
1045     return clib_error_return (0, "Incorrect CLI");
1046
1047   if (!policy_set)
1048     return clib_error_return (0, "No SR policy BSID or index specified");
1049
1050   if (is_add)
1051     {
1052       if (behavior && vec_len (segments) == 0)
1053         {
1054           vec_add2 (segments, this_seg, 1);
1055           clib_memset (this_seg, 0, sizeof (*this_seg));
1056         }
1057
1058       if (vec_len (segments) == 0)
1059         return clib_error_return (0, "No Segment List specified");
1060
1061       rv = sr_policy_add (&bsid, segments, weight, type, fib_table, is_encap,
1062                           behavior, ls_plugin_mem);
1063
1064       vec_free (segments);
1065     }
1066   else if (is_del)
1067     rv = sr_policy_del ((sr_policy_index != (u32) ~ 0 ? NULL : &bsid),
1068                         sr_policy_index);
1069   else if (is_mod)
1070     {
1071       if (!operation)
1072         return clib_error_return (0, "No SL modification specified");
1073       if (operation != 1 && sl_index == (u32) ~ 0)
1074         return clib_error_return (0, "No Segment List index specified");
1075       if (operation == 1 && vec_len (segments) == 0)
1076         return clib_error_return (0, "No Segment List specified");
1077       if (operation == 3 && weight == (u32) ~ 0)
1078         return clib_error_return (0, "No new weight for the SL specified");
1079
1080       rv = sr_policy_mod ((sr_policy_index != (u32) ~ 0 ? NULL : &bsid),
1081                           sr_policy_index, fib_table, operation, segments,
1082                           sl_index, weight);
1083
1084       if (segments)
1085         vec_free (segments);
1086     }
1087
1088   switch (rv)
1089     {
1090     case 0:
1091       break;
1092     case 1:
1093       return 0;
1094     case -12:
1095       return clib_error_return (0,
1096                                 "There is already a FIB entry for the BindingSID address.\n"
1097                                 "The SR policy could not be created.");
1098     case -13:
1099       return clib_error_return (0, "The specified FIB table does not exist.");
1100     case -21:
1101       return clib_error_return (0,
1102                                 "The selected SR policy only contains ONE segment list. "
1103                                 "Please remove the SR policy instead");
1104     case -22:
1105       return clib_error_return (0,
1106                                 "Could not delete the segment list. "
1107                                 "It is not associated with that SR policy.");
1108     case -32:
1109       return clib_error_return (0,
1110                                 "Could not modify the segment list. "
1111                                 "The given SL is not associated with such SR policy.");
1112     default:
1113       return clib_error_return (0, "BUG: sr policy returns %d", rv);
1114     }
1115   return 0;
1116 }
1117
1118 /* *INDENT-OFF* */
1119 VLIB_CLI_COMMAND (sr_policy_command, static) = {
1120   .path = "sr policy",
1121   .short_help = "sr policy [add||del||mod] [bsid 2001::1||index 5] "
1122     "next A:: next B:: next C:: (weight 1) (fib-table 2) (encap|insert)",
1123   .long_help =
1124     "Manipulation of SR policies.\n"
1125     "A Segment Routing policy may contain several SID lists. Each SID list has\n"
1126     "an associated weight (default 1), which will result in wECMP (uECMP).\n"
1127     "Segment Routing policies might be of type encapsulation or srh insertion\n"
1128     "Each SR policy will be associated with a unique BindingSID.\n"
1129     "A BindingSID is a locally allocated SegmentID. For every packet that arrives\n"
1130     "with IPv6_DA:BSID such traffic will be steered into the SR policy.\n"
1131     "The add command will create a SR policy with its first segment list (sl)\n"
1132     "The mod command allows you to add, remove, or modify the existing segment lists\n"
1133     "within an SR policy.\n"
1134     "The del command allows you to delete a SR policy along with all its associated\n"
1135     "SID lists.\n",
1136   .function = sr_policy_command_fn,
1137 };
1138 /* *INDENT-ON* */
1139
1140 /**
1141  * @brief CLI to display onscreen all the SR policies
1142  */
1143 static clib_error_t *
1144 show_sr_policies_command_fn (vlib_main_t * vm, unformat_input_t * input,
1145                              vlib_cli_command_t * cmd)
1146 {
1147   ip6_sr_main_t *sm = &sr_main;
1148   u32 *sl_index;
1149   ip6_sr_sl_t *segment_list = 0;
1150   ip6_sr_policy_t *sr_policy = 0;
1151   ip6_sr_policy_t **vec_policies = 0;
1152   ip6_address_t *addr;
1153   u8 *s;
1154   int i = 0;
1155
1156   vlib_cli_output (vm, "SR policies:");
1157
1158   /* *INDENT-OFF* */
1159   pool_foreach (sr_policy, sm->sr_policies)
1160                 {vec_add1 (vec_policies, sr_policy); }
1161   /* *INDENT-ON* */
1162
1163   vec_foreach_index (i, vec_policies)
1164   {
1165     sr_policy = vec_policies[i];
1166     vlib_cli_output (vm, "[%u].-\tBSID: %U",
1167                      (u32) (sr_policy - sm->sr_policies),
1168                      format_ip6_address, &sr_policy->bsid);
1169     vlib_cli_output (vm, "\tBehavior: %s",
1170                      (sr_policy->is_encap ? "Encapsulation" :
1171                       "SRH insertion"));
1172     switch (sr_policy->type)
1173       {
1174       case SR_POLICY_TYPE_SPRAY:
1175         vlib_cli_output (vm, "\tType: %s", "Spray");
1176         break;
1177       case SR_POLICY_TYPE_TEF:
1178         vlib_cli_output (vm, "\tType: %s",
1179                          "TEF (Timestamp, Encapsulate, and Forward)");
1180         break;
1181       default:
1182         vlib_cli_output (vm, "\tType: %s", "Default");
1183         break;
1184       }
1185     vlib_cli_output (vm, "\tFIB table: %u",
1186                      (sr_policy->fib_table !=
1187                       (u32) ~ 0 ? sr_policy->fib_table : 0));
1188     vlib_cli_output (vm, "\tSegment Lists:");
1189     vec_foreach (sl_index, sr_policy->segments_lists)
1190     {
1191       s = NULL;
1192       s = format (s, "\t[%u].- ", *sl_index);
1193       segment_list = pool_elt_at_index (sm->sid_lists, *sl_index);
1194       s = format (s, "< ");
1195       vec_foreach (addr, segment_list->segments)
1196       {
1197         s = format (s, "%U, ", format_ip6_address, addr);
1198       }
1199       s = format (s, "\b\b > ");
1200       s = format (s, "weight: %u", segment_list->weight);
1201       vlib_cli_output (vm, "  %v", s);
1202     }
1203     vlib_cli_output (vm, "-----------");
1204   }
1205   return 0;
1206 }
1207
1208 /* *INDENT-OFF* */
1209 VLIB_CLI_COMMAND (show_sr_policies_command, static) = {
1210   .path = "show sr policies",
1211   .short_help = "show sr policies",
1212   .function = show_sr_policies_command_fn,
1213 };
1214 /* *INDENT-ON* */
1215
1216 /**
1217  * @brief CLI to display onscreen the SR encaps source addr
1218  */
1219 static clib_error_t *
1220 show_sr_encaps_source_command_fn (vlib_main_t * vm, unformat_input_t * input,
1221                                   vlib_cli_command_t * cmd)
1222 {
1223   vlib_cli_output (vm, "SR encaps source addr = %U", format_ip6_address,
1224                    sr_get_encaps_source ());
1225
1226   return 0;
1227 }
1228
1229 /* *INDENT-OFF* */
1230 VLIB_CLI_COMMAND (show_sr_encaps_source_command, static) = {
1231   .path = "show sr encaps source addr",
1232   .short_help = "show sr encaps source addr",
1233   .function = show_sr_encaps_source_command_fn,
1234 };
1235 /* *INDENT-ON* */
1236
1237 /**
1238  * @brief CLI to display onscreen the hop-limit value used for SRv6 encapsulation
1239  */
1240 static clib_error_t *
1241 show_sr_encaps_hop_limit_command_fn (vlib_main_t * vm,
1242                                      unformat_input_t * input,
1243                                      vlib_cli_command_t * cmd)
1244 {
1245   vlib_cli_output (vm, "SR encaps hop-limit = %u", sr_get_hop_limit ());
1246
1247   return 0;
1248 }
1249
1250 /* *INDENT-OFF* */
1251 VLIB_CLI_COMMAND (show_sr_encaps_hop_limit_command, static) = {
1252   .path = "show sr encaps hop-limit",
1253   .short_help = "show sr encaps hop-limit",
1254   .function = show_sr_encaps_hop_limit_command_fn,
1255 };
1256 /* *INDENT-ON* */
1257
1258 /*************************** SR rewrite graph node ****************************/
1259 /**
1260  * @brief Trace for the SR Policy Rewrite graph node
1261  */
1262 static u8 *
1263 format_sr_policy_rewrite_trace (u8 * s, va_list * args)
1264 {
1265   //TODO
1266   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
1267   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
1268   sr_policy_rewrite_trace_t *t = va_arg (*args, sr_policy_rewrite_trace_t *);
1269
1270   s = format
1271     (s, "SR-policy-rewrite: src %U dst %U",
1272      format_ip6_address, &t->src, format_ip6_address, &t->dst);
1273
1274   return s;
1275 }
1276 /**
1277  * @brief SRv6 TEF (Timestamp, Encapsulate, and Forward) behavior
1278  */
1279 static_always_inline void
1280 srv6_tef_behavior (vlib_node_runtime_t *node, vlib_buffer_t *b0,
1281                    ip6_header_t *ip0)
1282 {
1283   ip6_sr_header_t *srh;
1284   ip6_sr_pt_tlv_t *srh_pt_tlv;
1285   timestamp_64_t ts;
1286   sr_pt_iface_t *ls = 0;
1287   u16 id_ld = 0;
1288   srh = (ip6_sr_header_t *) (ip0 + 1);
1289
1290   srh_pt_tlv =
1291     (ip6_sr_pt_tlv_t *) ((u8 *) ip0 + sizeof (ip6_header_t) +
1292                          sizeof (ip6_sr_header_t) +
1293                          sizeof (ip6_address_t) * (srh->last_entry + 1));
1294
1295   unix_time_now_nsec_fraction (&ts.sec, &ts.nsec);
1296   srh_pt_tlv->t64.sec = htobe32 (ts.sec);
1297   srh_pt_tlv->t64.nsec = htobe32 (ts.nsec);
1298   ls = sr_pt_find_iface (vnet_buffer (b0)->sw_if_index[VLIB_RX]);
1299   if (ls)
1300     {
1301       id_ld = ls->id << 4;
1302       id_ld |= ls->ingress_load;
1303       srh_pt_tlv->id_ld = htobe16 (id_ld);
1304     }
1305 }
1306
1307 /**
1308  * @brief IPv6 encapsulation processing as per RFC2473
1309  */
1310 static_always_inline void
1311 encaps_processing_v6 (vlib_node_runtime_t *node, vlib_buffer_t *b0,
1312                       ip6_header_t *ip0, ip6_header_t *ip0_encap,
1313                       u8 policy_type)
1314 {
1315   u32 new_l0;
1316   u32 flow_label;
1317
1318   ip0_encap->hop_limit -= 1;
1319   new_l0 =
1320     ip0->payload_length + sizeof (ip6_header_t) +
1321     clib_net_to_host_u16 (ip0_encap->payload_length);
1322   ip0->payload_length = clib_host_to_net_u16 (new_l0);
1323
1324   flow_label = ip6_compute_flow_hash (ip0_encap, IP_FLOW_HASH_DEFAULT);
1325   ip0->ip_version_traffic_class_and_flow_label = clib_host_to_net_u32 (
1326     0 |
1327     (clib_net_to_host_u32 (
1328        ip0_encap->ip_version_traffic_class_and_flow_label) &
1329      0xfff00000) |
1330     (flow_label & 0x0000ffff));
1331   if (policy_type == SR_POLICY_TYPE_TEF)
1332     srv6_tef_behavior (node, b0, ip0);
1333 }
1334
1335 /**
1336  * @brief Graph node for applying a SR policy into an IPv6 packet. Encapsulation
1337  */
1338 static uword
1339 sr_policy_rewrite_encaps (vlib_main_t * vm, vlib_node_runtime_t * node,
1340                           vlib_frame_t * from_frame)
1341 {
1342   ip6_sr_main_t *sm = &sr_main;
1343   u32 n_left_from, next_index, *from, *to_next;
1344
1345   from = vlib_frame_vector_args (from_frame);
1346   n_left_from = from_frame->n_vectors;
1347
1348   next_index = node->cached_next_index;
1349
1350   int encap_pkts = 0, bsid_pkts = 0;
1351
1352   while (n_left_from > 0)
1353     {
1354       u32 n_left_to_next;
1355
1356       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
1357
1358       /* Quad - Loop */
1359       while (n_left_from >= 8 && n_left_to_next >= 4)
1360         {
1361           u32 bi0, bi1, bi2, bi3;
1362           vlib_buffer_t *b0, *b1, *b2, *b3;
1363           u32 next0, next1, next2, next3;
1364           next0 = next1 = next2 = next3 = SR_POLICY_REWRITE_NEXT_IP6_LOOKUP;
1365           ip6_header_t *ip0, *ip1, *ip2, *ip3;
1366           ip6_header_t *ip0_encap, *ip1_encap, *ip2_encap, *ip3_encap;
1367           ip6_sr_sl_t *sl0, *sl1, *sl2, *sl3;
1368
1369           /* Prefetch next iteration. */
1370           {
1371             vlib_buffer_t *p4, *p5, *p6, *p7;
1372
1373             p4 = vlib_get_buffer (vm, from[4]);
1374             p5 = vlib_get_buffer (vm, from[5]);
1375             p6 = vlib_get_buffer (vm, from[6]);
1376             p7 = vlib_get_buffer (vm, from[7]);
1377
1378             /* Prefetch the buffer header and packet for the N+2 loop iteration */
1379             vlib_prefetch_buffer_header (p4, LOAD);
1380             vlib_prefetch_buffer_header (p5, LOAD);
1381             vlib_prefetch_buffer_header (p6, LOAD);
1382             vlib_prefetch_buffer_header (p7, LOAD);
1383
1384             clib_prefetch_store (p4->data);
1385             clib_prefetch_store (p5->data);
1386             clib_prefetch_store (p6->data);
1387             clib_prefetch_store (p7->data);
1388           }
1389
1390           to_next[0] = bi0 = from[0];
1391           to_next[1] = bi1 = from[1];
1392           to_next[2] = bi2 = from[2];
1393           to_next[3] = bi3 = from[3];
1394           from += 4;
1395           to_next += 4;
1396           n_left_from -= 4;
1397           n_left_to_next -= 4;
1398
1399           b0 = vlib_get_buffer (vm, bi0);
1400           b1 = vlib_get_buffer (vm, bi1);
1401           b2 = vlib_get_buffer (vm, bi2);
1402           b3 = vlib_get_buffer (vm, bi3);
1403
1404           sl0 =
1405             pool_elt_at_index (sm->sid_lists,
1406                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
1407           sl1 =
1408             pool_elt_at_index (sm->sid_lists,
1409                                vnet_buffer (b1)->ip.adj_index[VLIB_TX]);
1410           sl2 =
1411             pool_elt_at_index (sm->sid_lists,
1412                                vnet_buffer (b2)->ip.adj_index[VLIB_TX]);
1413           sl3 =
1414             pool_elt_at_index (sm->sid_lists,
1415                                vnet_buffer (b3)->ip.adj_index[VLIB_TX]);
1416
1417           ASSERT (b0->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
1418                   vec_len (sl0->rewrite));
1419           ASSERT (b1->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
1420                   vec_len (sl1->rewrite));
1421           ASSERT (b2->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
1422                   vec_len (sl2->rewrite));
1423           ASSERT (b3->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
1424                   vec_len (sl3->rewrite));
1425
1426           ip0_encap = vlib_buffer_get_current (b0);
1427           ip1_encap = vlib_buffer_get_current (b1);
1428           ip2_encap = vlib_buffer_get_current (b2);
1429           ip3_encap = vlib_buffer_get_current (b3);
1430
1431           clib_memcpy_fast (((u8 *) ip0_encap) - vec_len (sl0->rewrite),
1432                             sl0->rewrite, vec_len (sl0->rewrite));
1433           clib_memcpy_fast (((u8 *) ip1_encap) - vec_len (sl1->rewrite),
1434                             sl1->rewrite, vec_len (sl1->rewrite));
1435           clib_memcpy_fast (((u8 *) ip2_encap) - vec_len (sl2->rewrite),
1436                             sl2->rewrite, vec_len (sl2->rewrite));
1437           clib_memcpy_fast (((u8 *) ip3_encap) - vec_len (sl3->rewrite),
1438                             sl3->rewrite, vec_len (sl3->rewrite));
1439
1440           vlib_buffer_advance (b0, -(word) vec_len (sl0->rewrite));
1441           vlib_buffer_advance (b1, -(word) vec_len (sl1->rewrite));
1442           vlib_buffer_advance (b2, -(word) vec_len (sl2->rewrite));
1443           vlib_buffer_advance (b3, -(word) vec_len (sl3->rewrite));
1444
1445           ip0 = vlib_buffer_get_current (b0);
1446           ip1 = vlib_buffer_get_current (b1);
1447           ip2 = vlib_buffer_get_current (b2);
1448           ip3 = vlib_buffer_get_current (b3);
1449
1450           encaps_processing_v6 (node, b0, ip0, ip0_encap, sl0->policy_type);
1451           encaps_processing_v6 (node, b1, ip1, ip1_encap, sl1->policy_type);
1452           encaps_processing_v6 (node, b2, ip2, ip2_encap, sl2->policy_type);
1453           encaps_processing_v6 (node, b3, ip3, ip3_encap, sl3->policy_type);
1454
1455           vnet_buffer (b0)->sw_if_index[VLIB_TX] = sl0->egress_fib_table;
1456           vnet_buffer (b1)->sw_if_index[VLIB_TX] = sl1->egress_fib_table;
1457           vnet_buffer (b2)->sw_if_index[VLIB_TX] = sl2->egress_fib_table;
1458           vnet_buffer (b3)->sw_if_index[VLIB_TX] = sl3->egress_fib_table;
1459
1460           if (PREDICT_FALSE ((node->flags & VLIB_NODE_FLAG_TRACE)))
1461             {
1462               if (PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
1463                 {
1464                   sr_policy_rewrite_trace_t *tr =
1465                     vlib_add_trace (vm, node, b0, sizeof (*tr));
1466                   clib_memcpy_fast (tr->src.as_u8, ip0->src_address.as_u8,
1467                                     sizeof (tr->src.as_u8));
1468                   clib_memcpy_fast (tr->dst.as_u8, ip0->dst_address.as_u8,
1469                                     sizeof (tr->dst.as_u8));
1470                 }
1471
1472               if (PREDICT_FALSE (b1->flags & VLIB_BUFFER_IS_TRACED))
1473                 {
1474                   sr_policy_rewrite_trace_t *tr =
1475                     vlib_add_trace (vm, node, b1, sizeof (*tr));
1476                   clib_memcpy_fast (tr->src.as_u8, ip1->src_address.as_u8,
1477                                     sizeof (tr->src.as_u8));
1478                   clib_memcpy_fast (tr->dst.as_u8, ip1->dst_address.as_u8,
1479                                     sizeof (tr->dst.as_u8));
1480                 }
1481
1482               if (PREDICT_FALSE (b2->flags & VLIB_BUFFER_IS_TRACED))
1483                 {
1484                   sr_policy_rewrite_trace_t *tr =
1485                     vlib_add_trace (vm, node, b2, sizeof (*tr));
1486                   clib_memcpy_fast (tr->src.as_u8, ip2->src_address.as_u8,
1487                                     sizeof (tr->src.as_u8));
1488                   clib_memcpy_fast (tr->dst.as_u8, ip2->dst_address.as_u8,
1489                                     sizeof (tr->dst.as_u8));
1490                 }
1491
1492               if (PREDICT_FALSE (b3->flags & VLIB_BUFFER_IS_TRACED))
1493                 {
1494                   sr_policy_rewrite_trace_t *tr =
1495                     vlib_add_trace (vm, node, b3, sizeof (*tr));
1496                   clib_memcpy_fast (tr->src.as_u8, ip3->src_address.as_u8,
1497                                     sizeof (tr->src.as_u8));
1498                   clib_memcpy_fast (tr->dst.as_u8, ip3->dst_address.as_u8,
1499                                     sizeof (tr->dst.as_u8));
1500                 }
1501             }
1502
1503           encap_pkts += 4;
1504           vlib_validate_buffer_enqueue_x4 (vm, node, next_index, to_next,
1505                                            n_left_to_next, bi0, bi1, bi2, bi3,
1506                                            next0, next1, next2, next3);
1507         }
1508
1509       /* Single loop for potentially the last three packets */
1510       while (n_left_from > 0 && n_left_to_next > 0)
1511         {
1512           u32 bi0;
1513           vlib_buffer_t *b0;
1514           ip6_header_t *ip0 = 0, *ip0_encap = 0;
1515           ip6_sr_sl_t *sl0;
1516           u32 next0 = SR_POLICY_REWRITE_NEXT_IP6_LOOKUP;
1517
1518           bi0 = from[0];
1519           to_next[0] = bi0;
1520           from += 1;
1521           to_next += 1;
1522           n_left_from -= 1;
1523           n_left_to_next -= 1;
1524           b0 = vlib_get_buffer (vm, bi0);
1525
1526           sl0 =
1527             pool_elt_at_index (sm->sid_lists,
1528                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
1529           ASSERT (b0->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
1530                   vec_len (sl0->rewrite));
1531
1532           ip0_encap = vlib_buffer_get_current (b0);
1533
1534           clib_memcpy_fast (((u8 *) ip0_encap) - vec_len (sl0->rewrite),
1535                             sl0->rewrite, vec_len (sl0->rewrite));
1536           vlib_buffer_advance (b0, -(word) vec_len (sl0->rewrite));
1537
1538           ip0 = vlib_buffer_get_current (b0);
1539
1540           encaps_processing_v6 (node, b0, ip0, ip0_encap, sl0->policy_type);
1541
1542           vnet_buffer (b0)->sw_if_index[VLIB_TX] = sl0->egress_fib_table;
1543
1544           if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
1545               PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
1546             {
1547               sr_policy_rewrite_trace_t *tr =
1548                 vlib_add_trace (vm, node, b0, sizeof (*tr));
1549               clib_memcpy_fast (tr->src.as_u8, ip0->src_address.as_u8,
1550                                 sizeof (tr->src.as_u8));
1551               clib_memcpy_fast (tr->dst.as_u8, ip0->dst_address.as_u8,
1552                                 sizeof (tr->dst.as_u8));
1553             }
1554
1555           encap_pkts++;
1556           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
1557                                            n_left_to_next, bi0, next0);
1558         }
1559
1560       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
1561     }
1562
1563   /* Update counters */
1564   vlib_node_increment_counter (vm, sr_policy_rewrite_encaps_node.index,
1565                                SR_POLICY_REWRITE_ERROR_COUNTER_TOTAL,
1566                                encap_pkts);
1567   vlib_node_increment_counter (vm, sr_policy_rewrite_encaps_node.index,
1568                                SR_POLICY_REWRITE_ERROR_COUNTER_BSID,
1569                                bsid_pkts);
1570
1571   return from_frame->n_vectors;
1572 }
1573
1574 /* *INDENT-OFF* */
1575 VLIB_REGISTER_NODE (sr_policy_rewrite_encaps_node) = {
1576   .function = sr_policy_rewrite_encaps,
1577   .name = "sr-pl-rewrite-encaps",
1578   .vector_size = sizeof (u32),
1579   .format_trace = format_sr_policy_rewrite_trace,
1580   .type = VLIB_NODE_TYPE_INTERNAL,
1581   .n_errors = SR_POLICY_REWRITE_N_ERROR,
1582   .error_strings = sr_policy_rewrite_error_strings,
1583   .n_next_nodes = SR_POLICY_REWRITE_N_NEXT,
1584   .next_nodes = {
1585 #define _(s,n) [SR_POLICY_REWRITE_NEXT_##s] = n,
1586     foreach_sr_policy_rewrite_next
1587 #undef _
1588   },
1589 };
1590 /* *INDENT-ON* */
1591
1592 /**
1593  * @brief IPv4 encapsulation processing as per RFC2473
1594  */
1595 static_always_inline void
1596 encaps_processing_v4 (vlib_node_runtime_t * node,
1597                       vlib_buffer_t * b0,
1598                       ip6_header_t * ip0, ip4_header_t * ip0_encap)
1599 {
1600   u32 new_l0;
1601   ip6_sr_header_t *sr0;
1602
1603   u32 checksum0;
1604   u32 flow_label;
1605
1606   /* Inner IPv4: Decrement TTL & update checksum */
1607   ip0_encap->ttl -= 1;
1608   checksum0 = ip0_encap->checksum + clib_host_to_net_u16 (0x0100);
1609   checksum0 += checksum0 >= 0xffff;
1610   ip0_encap->checksum = checksum0;
1611
1612   /* Outer IPv6: Update length, FL, proto */
1613   new_l0 = ip0->payload_length + clib_net_to_host_u16 (ip0_encap->length);
1614   ip0->payload_length = clib_host_to_net_u16 (new_l0);
1615   flow_label = ip4_compute_flow_hash (ip0_encap, IP_FLOW_HASH_DEFAULT);
1616   ip0->ip_version_traffic_class_and_flow_label = clib_host_to_net_u32 (
1617     0 | ((6 & 0xF) << 28) | ((ip0_encap->tos & 0xFF) << 20) |
1618     (flow_label & 0x0000ffff));
1619   if (ip0->protocol == IP_PROTOCOL_IPV6_ROUTE)
1620     {
1621       sr0 = (void *) (ip0 + 1);
1622       sr0->protocol = IP_PROTOCOL_IP_IN_IP;
1623     }
1624   else
1625     ip0->protocol = IP_PROTOCOL_IP_IN_IP;
1626 }
1627
1628 /**
1629  * @brief Graph node for applying a SR policy into an IPv4 packet. Encapsulation
1630  */
1631 static uword
1632 sr_policy_rewrite_encaps_v4 (vlib_main_t * vm, vlib_node_runtime_t * node,
1633                              vlib_frame_t * from_frame)
1634 {
1635   ip6_sr_main_t *sm = &sr_main;
1636   u32 n_left_from, next_index, *from, *to_next;
1637
1638   from = vlib_frame_vector_args (from_frame);
1639   n_left_from = from_frame->n_vectors;
1640
1641   next_index = node->cached_next_index;
1642
1643   int encap_pkts = 0, bsid_pkts = 0;
1644
1645   while (n_left_from > 0)
1646     {
1647       u32 n_left_to_next;
1648
1649       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
1650
1651       /* Quad - Loop */
1652       while (n_left_from >= 8 && n_left_to_next >= 4)
1653         {
1654           u32 bi0, bi1, bi2, bi3;
1655           vlib_buffer_t *b0, *b1, *b2, *b3;
1656           u32 next0, next1, next2, next3;
1657           next0 = next1 = next2 = next3 = SR_POLICY_REWRITE_NEXT_IP6_LOOKUP;
1658           ip6_header_t *ip0, *ip1, *ip2, *ip3;
1659           ip4_header_t *ip0_encap, *ip1_encap, *ip2_encap, *ip3_encap;
1660           ip6_sr_sl_t *sl0, *sl1, *sl2, *sl3;
1661
1662           /* Prefetch next iteration. */
1663           {
1664             vlib_buffer_t *p4, *p5, *p6, *p7;
1665
1666             p4 = vlib_get_buffer (vm, from[4]);
1667             p5 = vlib_get_buffer (vm, from[5]);
1668             p6 = vlib_get_buffer (vm, from[6]);
1669             p7 = vlib_get_buffer (vm, from[7]);
1670
1671             /* Prefetch the buffer header and packet for the N+2 loop iteration */
1672             vlib_prefetch_buffer_header (p4, LOAD);
1673             vlib_prefetch_buffer_header (p5, LOAD);
1674             vlib_prefetch_buffer_header (p6, LOAD);
1675             vlib_prefetch_buffer_header (p7, LOAD);
1676
1677             clib_prefetch_store (p4->data);
1678             clib_prefetch_store (p5->data);
1679             clib_prefetch_store (p6->data);
1680             clib_prefetch_store (p7->data);
1681           }
1682
1683           to_next[0] = bi0 = from[0];
1684           to_next[1] = bi1 = from[1];
1685           to_next[2] = bi2 = from[2];
1686           to_next[3] = bi3 = from[3];
1687           from += 4;
1688           to_next += 4;
1689           n_left_from -= 4;
1690           n_left_to_next -= 4;
1691
1692           b0 = vlib_get_buffer (vm, bi0);
1693           b1 = vlib_get_buffer (vm, bi1);
1694           b2 = vlib_get_buffer (vm, bi2);
1695           b3 = vlib_get_buffer (vm, bi3);
1696
1697           sl0 =
1698             pool_elt_at_index (sm->sid_lists,
1699                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
1700           sl1 =
1701             pool_elt_at_index (sm->sid_lists,
1702                                vnet_buffer (b1)->ip.adj_index[VLIB_TX]);
1703           sl2 =
1704             pool_elt_at_index (sm->sid_lists,
1705                                vnet_buffer (b2)->ip.adj_index[VLIB_TX]);
1706           sl3 =
1707             pool_elt_at_index (sm->sid_lists,
1708                                vnet_buffer (b3)->ip.adj_index[VLIB_TX]);
1709           ASSERT (b0->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
1710                   vec_len (sl0->rewrite));
1711           ASSERT (b1->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
1712                   vec_len (sl1->rewrite));
1713           ASSERT (b2->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
1714                   vec_len (sl2->rewrite));
1715           ASSERT (b3->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
1716                   vec_len (sl3->rewrite));
1717
1718           ip0_encap = vlib_buffer_get_current (b0);
1719           ip1_encap = vlib_buffer_get_current (b1);
1720           ip2_encap = vlib_buffer_get_current (b2);
1721           ip3_encap = vlib_buffer_get_current (b3);
1722
1723           clib_memcpy_fast (((u8 *) ip0_encap) - vec_len (sl0->rewrite),
1724                             sl0->rewrite, vec_len (sl0->rewrite));
1725           clib_memcpy_fast (((u8 *) ip1_encap) - vec_len (sl1->rewrite),
1726                             sl1->rewrite, vec_len (sl1->rewrite));
1727           clib_memcpy_fast (((u8 *) ip2_encap) - vec_len (sl2->rewrite),
1728                             sl2->rewrite, vec_len (sl2->rewrite));
1729           clib_memcpy_fast (((u8 *) ip3_encap) - vec_len (sl3->rewrite),
1730                             sl3->rewrite, vec_len (sl3->rewrite));
1731
1732           vlib_buffer_advance (b0, -(word) vec_len (sl0->rewrite));
1733           vlib_buffer_advance (b1, -(word) vec_len (sl1->rewrite));
1734           vlib_buffer_advance (b2, -(word) vec_len (sl2->rewrite));
1735           vlib_buffer_advance (b3, -(word) vec_len (sl3->rewrite));
1736
1737           ip0 = vlib_buffer_get_current (b0);
1738           ip1 = vlib_buffer_get_current (b1);
1739           ip2 = vlib_buffer_get_current (b2);
1740           ip3 = vlib_buffer_get_current (b3);
1741
1742           encaps_processing_v4 (node, b0, ip0, ip0_encap);
1743           encaps_processing_v4 (node, b1, ip1, ip1_encap);
1744           encaps_processing_v4 (node, b2, ip2, ip2_encap);
1745           encaps_processing_v4 (node, b3, ip3, ip3_encap);
1746
1747           vnet_buffer (b0)->sw_if_index[VLIB_TX] = sl0->egress_fib_table;
1748           vnet_buffer (b1)->sw_if_index[VLIB_TX] = sl1->egress_fib_table;
1749           vnet_buffer (b2)->sw_if_index[VLIB_TX] = sl2->egress_fib_table;
1750           vnet_buffer (b3)->sw_if_index[VLIB_TX] = sl3->egress_fib_table;
1751
1752           if (PREDICT_FALSE ((node->flags & VLIB_NODE_FLAG_TRACE)))
1753             {
1754               if (PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
1755                 {
1756                   sr_policy_rewrite_trace_t *tr =
1757                     vlib_add_trace (vm, node, b0, sizeof (*tr));
1758                   clib_memcpy_fast (tr->src.as_u8, ip0->src_address.as_u8,
1759                                     sizeof (tr->src.as_u8));
1760                   clib_memcpy_fast (tr->dst.as_u8, ip0->dst_address.as_u8,
1761                                     sizeof (tr->dst.as_u8));
1762                 }
1763
1764               if (PREDICT_FALSE (b1->flags & VLIB_BUFFER_IS_TRACED))
1765                 {
1766                   sr_policy_rewrite_trace_t *tr =
1767                     vlib_add_trace (vm, node, b1, sizeof (*tr));
1768                   clib_memcpy_fast (tr->src.as_u8, ip1->src_address.as_u8,
1769                                     sizeof (tr->src.as_u8));
1770                   clib_memcpy_fast (tr->dst.as_u8, ip1->dst_address.as_u8,
1771                                     sizeof (tr->dst.as_u8));
1772                 }
1773
1774               if (PREDICT_FALSE (b2->flags & VLIB_BUFFER_IS_TRACED))
1775                 {
1776                   sr_policy_rewrite_trace_t *tr =
1777                     vlib_add_trace (vm, node, b2, sizeof (*tr));
1778                   clib_memcpy_fast (tr->src.as_u8, ip2->src_address.as_u8,
1779                                     sizeof (tr->src.as_u8));
1780                   clib_memcpy_fast (tr->dst.as_u8, ip2->dst_address.as_u8,
1781                                     sizeof (tr->dst.as_u8));
1782                 }
1783
1784               if (PREDICT_FALSE (b3->flags & VLIB_BUFFER_IS_TRACED))
1785                 {
1786                   sr_policy_rewrite_trace_t *tr =
1787                     vlib_add_trace (vm, node, b3, sizeof (*tr));
1788                   clib_memcpy_fast (tr->src.as_u8, ip3->src_address.as_u8,
1789                                     sizeof (tr->src.as_u8));
1790                   clib_memcpy_fast (tr->dst.as_u8, ip3->dst_address.as_u8,
1791                                     sizeof (tr->dst.as_u8));
1792                 }
1793             }
1794
1795           encap_pkts += 4;
1796           vlib_validate_buffer_enqueue_x4 (vm, node, next_index, to_next,
1797                                            n_left_to_next, bi0, bi1, bi2, bi3,
1798                                            next0, next1, next2, next3);
1799         }
1800
1801       /* Single loop for potentially the last three packets */
1802       while (n_left_from > 0 && n_left_to_next > 0)
1803         {
1804           u32 bi0;
1805           vlib_buffer_t *b0;
1806           ip6_header_t *ip0 = 0;
1807           ip4_header_t *ip0_encap = 0;
1808           ip6_sr_sl_t *sl0;
1809           u32 next0 = SR_POLICY_REWRITE_NEXT_IP6_LOOKUP;
1810
1811           bi0 = from[0];
1812           to_next[0] = bi0;
1813           from += 1;
1814           to_next += 1;
1815           n_left_from -= 1;
1816           n_left_to_next -= 1;
1817           b0 = vlib_get_buffer (vm, bi0);
1818
1819           sl0 =
1820             pool_elt_at_index (sm->sid_lists,
1821                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
1822           ASSERT (b0->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
1823                   vec_len (sl0->rewrite));
1824
1825           ip0_encap = vlib_buffer_get_current (b0);
1826
1827           clib_memcpy_fast (((u8 *) ip0_encap) - vec_len (sl0->rewrite),
1828                             sl0->rewrite, vec_len (sl0->rewrite));
1829           vlib_buffer_advance (b0, -(word) vec_len (sl0->rewrite));
1830
1831           ip0 = vlib_buffer_get_current (b0);
1832
1833           encaps_processing_v4 (node, b0, ip0, ip0_encap);
1834
1835           vnet_buffer (b0)->sw_if_index[VLIB_TX] = sl0->egress_fib_table;
1836
1837           if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
1838               PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
1839             {
1840               sr_policy_rewrite_trace_t *tr =
1841                 vlib_add_trace (vm, node, b0, sizeof (*tr));
1842               clib_memcpy_fast (tr->src.as_u8, ip0->src_address.as_u8,
1843                                 sizeof (tr->src.as_u8));
1844               clib_memcpy_fast (tr->dst.as_u8, ip0->dst_address.as_u8,
1845                                 sizeof (tr->dst.as_u8));
1846             }
1847
1848           encap_pkts++;
1849           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
1850                                            n_left_to_next, bi0, next0);
1851         }
1852
1853       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
1854     }
1855
1856   /* Update counters */
1857   vlib_node_increment_counter (vm, sr_policy_rewrite_encaps_node.index,
1858                                SR_POLICY_REWRITE_ERROR_COUNTER_TOTAL,
1859                                encap_pkts);
1860   vlib_node_increment_counter (vm, sr_policy_rewrite_encaps_node.index,
1861                                SR_POLICY_REWRITE_ERROR_COUNTER_BSID,
1862                                bsid_pkts);
1863
1864   return from_frame->n_vectors;
1865 }
1866
1867 /* *INDENT-OFF* */
1868 VLIB_REGISTER_NODE (sr_policy_rewrite_encaps_v4_node) = {
1869   .function = sr_policy_rewrite_encaps_v4,
1870   .name = "sr-pl-rewrite-encaps-v4",
1871   .vector_size = sizeof (u32),
1872   .format_trace = format_sr_policy_rewrite_trace,
1873   .type = VLIB_NODE_TYPE_INTERNAL,
1874   .n_errors = SR_POLICY_REWRITE_N_ERROR,
1875   .error_strings = sr_policy_rewrite_error_strings,
1876   .n_next_nodes = SR_POLICY_REWRITE_N_NEXT,
1877   .next_nodes = {
1878 #define _(s,n) [SR_POLICY_REWRITE_NEXT_##s] = n,
1879     foreach_sr_policy_rewrite_next
1880 #undef _
1881   },
1882 };
1883 /* *INDENT-ON* */
1884
1885 always_inline u32
1886 ip_flow_hash (void *data)
1887 {
1888   ip4_header_t *iph = (ip4_header_t *) data;
1889
1890   if ((iph->ip_version_and_header_length & 0xF0) == 0x40)
1891     return ip4_compute_flow_hash (iph, IP_FLOW_HASH_DEFAULT);
1892   else
1893     return ip6_compute_flow_hash ((ip6_header_t *) iph, IP_FLOW_HASH_DEFAULT);
1894 }
1895
1896 always_inline u64
1897 mac_to_u64 (u8 * m)
1898 {
1899   return (*((u64 *) m) & 0xffffffffffff);
1900 }
1901
1902 always_inline u32
1903 l2_flow_hash (vlib_buffer_t * b0)
1904 {
1905   ethernet_header_t *eh;
1906   u64 a, b, c;
1907   uword is_ip, eh_size;
1908   u16 eh_type;
1909
1910   eh = vlib_buffer_get_current (b0);
1911   eh_type = clib_net_to_host_u16 (eh->type);
1912   eh_size = ethernet_buffer_header_size (b0);
1913
1914   is_ip = (eh_type == ETHERNET_TYPE_IP4 || eh_type == ETHERNET_TYPE_IP6);
1915
1916   /* since we have 2 cache lines, use them */
1917   if (is_ip)
1918     a = ip_flow_hash ((u8 *) vlib_buffer_get_current (b0) + eh_size);
1919   else
1920     a = eh->type;
1921
1922   b = mac_to_u64 ((u8 *) eh->dst_address);
1923   c = mac_to_u64 ((u8 *) eh->src_address);
1924   hash_mix64 (a, b, c);
1925
1926   return (u32) c;
1927 }
1928
1929 /**
1930  * @brief Graph node for applying a SR policy into a L2 frame
1931  */
1932 static uword
1933 sr_policy_rewrite_encaps_l2 (vlib_main_t * vm, vlib_node_runtime_t * node,
1934                              vlib_frame_t * from_frame)
1935 {
1936   ip6_sr_main_t *sm = &sr_main;
1937   u32 n_left_from, next_index, *from, *to_next;
1938
1939   from = vlib_frame_vector_args (from_frame);
1940   n_left_from = from_frame->n_vectors;
1941
1942   next_index = node->cached_next_index;
1943
1944   int encap_pkts = 0, bsid_pkts = 0;
1945
1946   while (n_left_from > 0)
1947     {
1948       u32 n_left_to_next;
1949
1950       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
1951
1952       /* Quad - Loop */
1953       while (n_left_from >= 8 && n_left_to_next >= 4)
1954         {
1955           u32 bi0, bi1, bi2, bi3;
1956           vlib_buffer_t *b0, *b1, *b2, *b3;
1957           u32 next0, next1, next2, next3;
1958           next0 = next1 = next2 = next3 = SR_POLICY_REWRITE_NEXT_IP6_LOOKUP;
1959           ethernet_header_t *en0, *en1, *en2, *en3;
1960           ip6_header_t *ip0, *ip1, *ip2, *ip3;
1961           ip6_sr_header_t *sr0, *sr1, *sr2, *sr3;
1962           ip6_sr_policy_t *sp0, *sp1, *sp2, *sp3;
1963           ip6_sr_sl_t *sl0, *sl1, *sl2, *sl3;
1964           u32 flow_label0, flow_label1, flow_label2, flow_label3;
1965
1966           /* Prefetch next iteration. */
1967           {
1968             vlib_buffer_t *p4, *p5, *p6, *p7;
1969
1970             p4 = vlib_get_buffer (vm, from[4]);
1971             p5 = vlib_get_buffer (vm, from[5]);
1972             p6 = vlib_get_buffer (vm, from[6]);
1973             p7 = vlib_get_buffer (vm, from[7]);
1974
1975             /* Prefetch the buffer header and packet for the N+2 loop iteration */
1976             vlib_prefetch_buffer_header (p4, LOAD);
1977             vlib_prefetch_buffer_header (p5, LOAD);
1978             vlib_prefetch_buffer_header (p6, LOAD);
1979             vlib_prefetch_buffer_header (p7, LOAD);
1980
1981             clib_prefetch_store (p4->data);
1982             clib_prefetch_store (p5->data);
1983             clib_prefetch_store (p6->data);
1984             clib_prefetch_store (p7->data);
1985           }
1986
1987           to_next[0] = bi0 = from[0];
1988           to_next[1] = bi1 = from[1];
1989           to_next[2] = bi2 = from[2];
1990           to_next[3] = bi3 = from[3];
1991           from += 4;
1992           to_next += 4;
1993           n_left_from -= 4;
1994           n_left_to_next -= 4;
1995
1996           b0 = vlib_get_buffer (vm, bi0);
1997           b1 = vlib_get_buffer (vm, bi1);
1998           b2 = vlib_get_buffer (vm, bi2);
1999           b3 = vlib_get_buffer (vm, bi3);
2000
2001           sp0 = pool_elt_at_index (sm->sr_policies,
2002                                    sm->sw_iface_sr_policies[vnet_buffer
2003                                                             (b0)->sw_if_index
2004                                                             [VLIB_RX]]);
2005
2006           sp1 = pool_elt_at_index (sm->sr_policies,
2007                                    sm->sw_iface_sr_policies[vnet_buffer
2008                                                             (b1)->sw_if_index
2009                                                             [VLIB_RX]]);
2010
2011           sp2 = pool_elt_at_index (sm->sr_policies,
2012                                    sm->sw_iface_sr_policies[vnet_buffer
2013                                                             (b2)->sw_if_index
2014                                                             [VLIB_RX]]);
2015
2016           sp3 = pool_elt_at_index (sm->sr_policies,
2017                                    sm->sw_iface_sr_policies[vnet_buffer
2018                                                             (b3)->sw_if_index
2019                                                             [VLIB_RX]]);
2020           flow_label0 = l2_flow_hash (b0);
2021           flow_label1 = l2_flow_hash (b1);
2022           flow_label2 = l2_flow_hash (b2);
2023           flow_label3 = l2_flow_hash (b3);
2024
2025           if (vec_len (sp0->segments_lists) == 1)
2026             vnet_buffer (b0)->ip.adj_index[VLIB_TX] = sp0->segments_lists[0];
2027           else
2028             {
2029               vnet_buffer (b0)->ip.flow_hash = flow_label0;
2030               vnet_buffer (b0)->ip.adj_index[VLIB_TX] =
2031                 sp0->segments_lists[(vnet_buffer (b0)->ip.flow_hash &
2032                                      (vec_len (sp0->segments_lists) - 1))];
2033             }
2034
2035           if (vec_len (sp1->segments_lists) == 1)
2036             vnet_buffer (b1)->ip.adj_index[VLIB_TX] = sp1->segments_lists[1];
2037           else
2038             {
2039               vnet_buffer (b1)->ip.flow_hash = flow_label1;
2040               vnet_buffer (b1)->ip.adj_index[VLIB_TX] =
2041                 sp1->segments_lists[(vnet_buffer (b1)->ip.flow_hash &
2042                                      (vec_len (sp1->segments_lists) - 1))];
2043             }
2044
2045           if (vec_len (sp2->segments_lists) == 1)
2046             vnet_buffer (b2)->ip.adj_index[VLIB_TX] = sp2->segments_lists[2];
2047           else
2048             {
2049               vnet_buffer (b2)->ip.flow_hash = flow_label2;
2050               vnet_buffer (b2)->ip.adj_index[VLIB_TX] =
2051                 sp2->segments_lists[(vnet_buffer (b2)->ip.flow_hash &
2052                                      (vec_len (sp2->segments_lists) - 1))];
2053             }
2054
2055           if (vec_len (sp3->segments_lists) == 1)
2056             vnet_buffer (b3)->ip.adj_index[VLIB_TX] = sp3->segments_lists[3];
2057           else
2058             {
2059               vnet_buffer (b3)->ip.flow_hash = flow_label3;
2060               vnet_buffer (b3)->ip.adj_index[VLIB_TX] =
2061                 sp3->segments_lists[(vnet_buffer (b3)->ip.flow_hash &
2062                                      (vec_len (sp3->segments_lists) - 1))];
2063             }
2064
2065           sl0 =
2066             pool_elt_at_index (sm->sid_lists,
2067                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
2068           sl1 =
2069             pool_elt_at_index (sm->sid_lists,
2070                                vnet_buffer (b1)->ip.adj_index[VLIB_TX]);
2071           sl2 =
2072             pool_elt_at_index (sm->sid_lists,
2073                                vnet_buffer (b2)->ip.adj_index[VLIB_TX]);
2074           sl3 =
2075             pool_elt_at_index (sm->sid_lists,
2076                                vnet_buffer (b3)->ip.adj_index[VLIB_TX]);
2077
2078           ASSERT (b0->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
2079                   vec_len (sl0->rewrite));
2080           ASSERT (b1->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
2081                   vec_len (sl1->rewrite));
2082           ASSERT (b2->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
2083                   vec_len (sl2->rewrite));
2084           ASSERT (b3->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
2085                   vec_len (sl3->rewrite));
2086
2087           en0 = vlib_buffer_get_current (b0);
2088           en1 = vlib_buffer_get_current (b1);
2089           en2 = vlib_buffer_get_current (b2);
2090           en3 = vlib_buffer_get_current (b3);
2091
2092           clib_memcpy_fast (((u8 *) en0) - vec_len (sl0->rewrite),
2093                             sl0->rewrite, vec_len (sl0->rewrite));
2094           clib_memcpy_fast (((u8 *) en1) - vec_len (sl1->rewrite),
2095                             sl1->rewrite, vec_len (sl1->rewrite));
2096           clib_memcpy_fast (((u8 *) en2) - vec_len (sl2->rewrite),
2097                             sl2->rewrite, vec_len (sl2->rewrite));
2098           clib_memcpy_fast (((u8 *) en3) - vec_len (sl3->rewrite),
2099                             sl3->rewrite, vec_len (sl3->rewrite));
2100
2101           vlib_buffer_advance (b0, -(word) vec_len (sl0->rewrite));
2102           vlib_buffer_advance (b1, -(word) vec_len (sl1->rewrite));
2103           vlib_buffer_advance (b2, -(word) vec_len (sl2->rewrite));
2104           vlib_buffer_advance (b3, -(word) vec_len (sl3->rewrite));
2105
2106           ip0 = vlib_buffer_get_current (b0);
2107           ip1 = vlib_buffer_get_current (b1);
2108           ip2 = vlib_buffer_get_current (b2);
2109           ip3 = vlib_buffer_get_current (b3);
2110
2111           ip0->payload_length =
2112             clib_host_to_net_u16 (b0->current_length - sizeof (ip6_header_t));
2113           ip1->payload_length =
2114             clib_host_to_net_u16 (b1->current_length - sizeof (ip6_header_t));
2115           ip2->payload_length =
2116             clib_host_to_net_u16 (b2->current_length - sizeof (ip6_header_t));
2117           ip3->payload_length =
2118             clib_host_to_net_u16 (b3->current_length - sizeof (ip6_header_t));
2119
2120           if (ip0->protocol == IP_PROTOCOL_IPV6_ROUTE)
2121             {
2122               sr0 = (void *) (ip0 + 1);
2123               sr0->protocol = IP_PROTOCOL_IP6_ETHERNET;
2124             }
2125           else
2126             ip0->protocol = IP_PROTOCOL_IP6_ETHERNET;
2127
2128           if (ip1->protocol == IP_PROTOCOL_IPV6_ROUTE)
2129             {
2130               sr1 = (void *) (ip1 + 1);
2131               sr1->protocol = IP_PROTOCOL_IP6_ETHERNET;
2132             }
2133           else
2134             ip1->protocol = IP_PROTOCOL_IP6_ETHERNET;
2135
2136           if (ip2->protocol == IP_PROTOCOL_IPV6_ROUTE)
2137             {
2138               sr2 = (void *) (ip2 + 1);
2139               sr2->protocol = IP_PROTOCOL_IP6_ETHERNET;
2140             }
2141           else
2142             ip2->protocol = IP_PROTOCOL_IP6_ETHERNET;
2143
2144           if (ip3->protocol == IP_PROTOCOL_IPV6_ROUTE)
2145             {
2146               sr3 = (void *) (ip3 + 1);
2147               sr3->protocol = IP_PROTOCOL_IP6_ETHERNET;
2148             }
2149           else
2150             ip3->protocol = IP_PROTOCOL_IP6_ETHERNET;
2151
2152           /* TC is set to 0 for all ethernet frames, should be taken from COS
2153            * od DSCP of encapsulated packet in the future */
2154           ip0->ip_version_traffic_class_and_flow_label = clib_host_to_net_u32 (
2155             0 | ((6 & 0xF) << 28) | ((0x00) << 20) | (flow_label0 & 0xffff));
2156           ip1->ip_version_traffic_class_and_flow_label = clib_host_to_net_u32 (
2157             0 | ((6 & 0xF) << 28) | ((0x00) << 20) | (flow_label1 & 0xffff));
2158           ip2->ip_version_traffic_class_and_flow_label = clib_host_to_net_u32 (
2159             0 | ((6 & 0xF) << 28) | ((0x00) << 20) | (flow_label2 & 0xffff));
2160           ip3->ip_version_traffic_class_and_flow_label = clib_host_to_net_u32 (
2161             0 | ((6 & 0xF) << 28) | ((0x00) << 20) | (flow_label3 & 0xffff));
2162
2163           if (PREDICT_FALSE ((node->flags & VLIB_NODE_FLAG_TRACE)))
2164             {
2165               if (PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
2166                 {
2167                   sr_policy_rewrite_trace_t *tr =
2168                     vlib_add_trace (vm, node, b0, sizeof (*tr));
2169                   clib_memcpy_fast (tr->src.as_u8, ip0->src_address.as_u8,
2170                                     sizeof (tr->src.as_u8));
2171                   clib_memcpy_fast (tr->dst.as_u8, ip0->dst_address.as_u8,
2172                                     sizeof (tr->dst.as_u8));
2173                 }
2174
2175               if (PREDICT_FALSE (b1->flags & VLIB_BUFFER_IS_TRACED))
2176                 {
2177                   sr_policy_rewrite_trace_t *tr =
2178                     vlib_add_trace (vm, node, b1, sizeof (*tr));
2179                   clib_memcpy_fast (tr->src.as_u8, ip1->src_address.as_u8,
2180                                     sizeof (tr->src.as_u8));
2181                   clib_memcpy_fast (tr->dst.as_u8, ip1->dst_address.as_u8,
2182                                     sizeof (tr->dst.as_u8));
2183                 }
2184
2185               if (PREDICT_FALSE (b2->flags & VLIB_BUFFER_IS_TRACED))
2186                 {
2187                   sr_policy_rewrite_trace_t *tr =
2188                     vlib_add_trace (vm, node, b2, sizeof (*tr));
2189                   clib_memcpy_fast (tr->src.as_u8, ip2->src_address.as_u8,
2190                                     sizeof (tr->src.as_u8));
2191                   clib_memcpy_fast (tr->dst.as_u8, ip2->dst_address.as_u8,
2192                                     sizeof (tr->dst.as_u8));
2193                 }
2194
2195               if (PREDICT_FALSE (b3->flags & VLIB_BUFFER_IS_TRACED))
2196                 {
2197                   sr_policy_rewrite_trace_t *tr =
2198                     vlib_add_trace (vm, node, b3, sizeof (*tr));
2199                   clib_memcpy_fast (tr->src.as_u8, ip3->src_address.as_u8,
2200                                     sizeof (tr->src.as_u8));
2201                   clib_memcpy_fast (tr->dst.as_u8, ip3->dst_address.as_u8,
2202                                     sizeof (tr->dst.as_u8));
2203                 }
2204             }
2205
2206           encap_pkts += 4;
2207           vlib_validate_buffer_enqueue_x4 (vm, node, next_index, to_next,
2208                                            n_left_to_next, bi0, bi1, bi2, bi3,
2209                                            next0, next1, next2, next3);
2210         }
2211
2212       /* Single loop for potentially the last three packets */
2213       while (n_left_from > 0 && n_left_to_next > 0)
2214         {
2215           u32 bi0;
2216           vlib_buffer_t *b0;
2217           ip6_header_t *ip0 = 0;
2218           ip6_sr_header_t *sr0;
2219           ethernet_header_t *en0;
2220           ip6_sr_policy_t *sp0;
2221           ip6_sr_sl_t *sl0;
2222           u32 next0 = SR_POLICY_REWRITE_NEXT_IP6_LOOKUP;
2223           u32 flow_label0;
2224
2225           bi0 = from[0];
2226           to_next[0] = bi0;
2227           from += 1;
2228           to_next += 1;
2229           n_left_from -= 1;
2230           n_left_to_next -= 1;
2231           b0 = vlib_get_buffer (vm, bi0);
2232
2233           /* Find the SR policy */
2234           sp0 = pool_elt_at_index (sm->sr_policies,
2235                                    sm->sw_iface_sr_policies[vnet_buffer
2236                                                             (b0)->sw_if_index
2237                                                             [VLIB_RX]]);
2238           flow_label0 = l2_flow_hash (b0);
2239
2240           /* In case there is more than one SL, LB among them */
2241           if (vec_len (sp0->segments_lists) == 1)
2242             vnet_buffer (b0)->ip.adj_index[VLIB_TX] = sp0->segments_lists[0];
2243           else
2244             {
2245               vnet_buffer (b0)->ip.flow_hash = flow_label0;
2246               vnet_buffer (b0)->ip.adj_index[VLIB_TX] =
2247                 sp0->segments_lists[(vnet_buffer (b0)->ip.flow_hash &
2248                                      (vec_len (sp0->segments_lists) - 1))];
2249             }
2250           sl0 =
2251             pool_elt_at_index (sm->sid_lists,
2252                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
2253           ASSERT (b0->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
2254                   vec_len (sl0->rewrite));
2255
2256           en0 = vlib_buffer_get_current (b0);
2257
2258           clib_memcpy_fast (((u8 *) en0) - vec_len (sl0->rewrite),
2259                             sl0->rewrite, vec_len (sl0->rewrite));
2260
2261           vlib_buffer_advance (b0, -(word) vec_len (sl0->rewrite));
2262
2263           ip0 = vlib_buffer_get_current (b0);
2264
2265           ip0->payload_length =
2266             clib_host_to_net_u16 (b0->current_length - sizeof (ip6_header_t));
2267
2268           if (ip0->protocol == IP_PROTOCOL_IPV6_ROUTE)
2269             {
2270               sr0 = (void *) (ip0 + 1);
2271               sr0->protocol = IP_PROTOCOL_IP6_ETHERNET;
2272             }
2273           else
2274             ip0->protocol = IP_PROTOCOL_IP6_ETHERNET;
2275
2276           ip0->ip_version_traffic_class_and_flow_label = clib_host_to_net_u32 (
2277             0 | ((6 & 0xF) << 28) | ((0x00) << 20) | (flow_label0 & 0xffff));
2278
2279           if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
2280               PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
2281             {
2282               sr_policy_rewrite_trace_t *tr =
2283                 vlib_add_trace (vm, node, b0, sizeof (*tr));
2284               clib_memcpy_fast (tr->src.as_u8, ip0->src_address.as_u8,
2285                                 sizeof (tr->src.as_u8));
2286               clib_memcpy_fast (tr->dst.as_u8, ip0->dst_address.as_u8,
2287                                 sizeof (tr->dst.as_u8));
2288             }
2289
2290           encap_pkts++;
2291           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
2292                                            n_left_to_next, bi0, next0);
2293         }
2294
2295       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
2296     }
2297
2298   /* Update counters */
2299   vlib_node_increment_counter (vm, sr_policy_rewrite_encaps_node.index,
2300                                SR_POLICY_REWRITE_ERROR_COUNTER_TOTAL,
2301                                encap_pkts);
2302   vlib_node_increment_counter (vm, sr_policy_rewrite_encaps_node.index,
2303                                SR_POLICY_REWRITE_ERROR_COUNTER_BSID,
2304                                bsid_pkts);
2305
2306   return from_frame->n_vectors;
2307 }
2308
2309 /* *INDENT-OFF* */
2310 VLIB_REGISTER_NODE (sr_policy_rewrite_encaps_l2_node) = {
2311   .function = sr_policy_rewrite_encaps_l2,
2312   .name = "sr-pl-rewrite-encaps-l2",
2313   .vector_size = sizeof (u32),
2314   .format_trace = format_sr_policy_rewrite_trace,
2315   .type = VLIB_NODE_TYPE_INTERNAL,
2316   .n_errors = SR_POLICY_REWRITE_N_ERROR,
2317   .error_strings = sr_policy_rewrite_error_strings,
2318   .n_next_nodes = SR_POLICY_REWRITE_N_NEXT,
2319   .next_nodes = {
2320 #define _(s,n) [SR_POLICY_REWRITE_NEXT_##s] = n,
2321     foreach_sr_policy_rewrite_next
2322 #undef _
2323   },
2324 };
2325 /* *INDENT-ON* */
2326
2327 /**
2328  * @brief Graph node for applying a SR policy into a packet. SRH insertion.
2329  */
2330 static uword
2331 sr_policy_rewrite_insert (vlib_main_t * vm, vlib_node_runtime_t * node,
2332                           vlib_frame_t * from_frame)
2333 {
2334   ip6_sr_main_t *sm = &sr_main;
2335   u32 n_left_from, next_index, *from, *to_next;
2336
2337   from = vlib_frame_vector_args (from_frame);
2338   n_left_from = from_frame->n_vectors;
2339
2340   next_index = node->cached_next_index;
2341
2342   int insert_pkts = 0, bsid_pkts = 0;
2343
2344   while (n_left_from > 0)
2345     {
2346       u32 n_left_to_next;
2347
2348       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
2349
2350       /* Quad - Loop */
2351       while (n_left_from >= 8 && n_left_to_next >= 4)
2352         {
2353           u32 bi0, bi1, bi2, bi3;
2354           vlib_buffer_t *b0, *b1, *b2, *b3;
2355           u32 next0, next1, next2, next3;
2356           next0 = next1 = next2 = next3 = SR_POLICY_REWRITE_NEXT_IP6_LOOKUP;
2357           ip6_header_t *ip0, *ip1, *ip2, *ip3;
2358           ip6_sr_header_t *sr0, *sr1, *sr2, *sr3;
2359           ip6_sr_sl_t *sl0, *sl1, *sl2, *sl3;
2360           u16 new_l0, new_l1, new_l2, new_l3;
2361
2362           /* Prefetch next iteration. */
2363           {
2364             vlib_buffer_t *p4, *p5, *p6, *p7;
2365
2366             p4 = vlib_get_buffer (vm, from[4]);
2367             p5 = vlib_get_buffer (vm, from[5]);
2368             p6 = vlib_get_buffer (vm, from[6]);
2369             p7 = vlib_get_buffer (vm, from[7]);
2370
2371             /* Prefetch the buffer header and packet for the N+2 loop iteration */
2372             vlib_prefetch_buffer_header (p4, LOAD);
2373             vlib_prefetch_buffer_header (p5, LOAD);
2374             vlib_prefetch_buffer_header (p6, LOAD);
2375             vlib_prefetch_buffer_header (p7, LOAD);
2376
2377             clib_prefetch_store (p4->data);
2378             clib_prefetch_store (p5->data);
2379             clib_prefetch_store (p6->data);
2380             clib_prefetch_store (p7->data);
2381           }
2382
2383           to_next[0] = bi0 = from[0];
2384           to_next[1] = bi1 = from[1];
2385           to_next[2] = bi2 = from[2];
2386           to_next[3] = bi3 = from[3];
2387           from += 4;
2388           to_next += 4;
2389           n_left_from -= 4;
2390           n_left_to_next -= 4;
2391
2392           b0 = vlib_get_buffer (vm, bi0);
2393           b1 = vlib_get_buffer (vm, bi1);
2394           b2 = vlib_get_buffer (vm, bi2);
2395           b3 = vlib_get_buffer (vm, bi3);
2396
2397           sl0 =
2398             pool_elt_at_index (sm->sid_lists,
2399                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
2400           sl1 =
2401             pool_elt_at_index (sm->sid_lists,
2402                                vnet_buffer (b1)->ip.adj_index[VLIB_TX]);
2403           sl2 =
2404             pool_elt_at_index (sm->sid_lists,
2405                                vnet_buffer (b2)->ip.adj_index[VLIB_TX]);
2406           sl3 =
2407             pool_elt_at_index (sm->sid_lists,
2408                                vnet_buffer (b3)->ip.adj_index[VLIB_TX]);
2409           ASSERT (b0->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
2410                   vec_len (sl0->rewrite));
2411           ASSERT (b1->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
2412                   vec_len (sl1->rewrite));
2413           ASSERT (b2->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
2414                   vec_len (sl2->rewrite));
2415           ASSERT (b3->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
2416                   vec_len (sl3->rewrite));
2417
2418           ip0 = vlib_buffer_get_current (b0);
2419           ip1 = vlib_buffer_get_current (b1);
2420           ip2 = vlib_buffer_get_current (b2);
2421           ip3 = vlib_buffer_get_current (b3);
2422
2423           if (ip0->protocol == IP_PROTOCOL_IP6_HOP_BY_HOP_OPTIONS)
2424             sr0 =
2425               (ip6_sr_header_t *) (((void *) (ip0 + 1)) +
2426                                    ip6_ext_header_len (ip0 + 1));
2427           else
2428             sr0 = (ip6_sr_header_t *) (ip0 + 1);
2429
2430           if (ip1->protocol == IP_PROTOCOL_IP6_HOP_BY_HOP_OPTIONS)
2431             sr1 =
2432               (ip6_sr_header_t *) (((void *) (ip1 + 1)) +
2433                                    ip6_ext_header_len (ip1 + 1));
2434           else
2435             sr1 = (ip6_sr_header_t *) (ip1 + 1);
2436
2437           if (ip2->protocol == IP_PROTOCOL_IP6_HOP_BY_HOP_OPTIONS)
2438             sr2 =
2439               (ip6_sr_header_t *) (((void *) (ip2 + 1)) +
2440                                    ip6_ext_header_len (ip2 + 1));
2441           else
2442             sr2 = (ip6_sr_header_t *) (ip2 + 1);
2443
2444           if (ip3->protocol == IP_PROTOCOL_IP6_HOP_BY_HOP_OPTIONS)
2445             sr3 =
2446               (ip6_sr_header_t *) (((void *) (ip3 + 1)) +
2447                                    ip6_ext_header_len (ip3 + 1));
2448           else
2449             sr3 = (ip6_sr_header_t *) (ip3 + 1);
2450
2451           clib_memcpy_fast ((u8 *) ip0 - vec_len (sl0->rewrite), (u8 *) ip0,
2452                             (void *) sr0 - (void *) ip0);
2453           clib_memcpy_fast ((u8 *) ip1 - vec_len (sl1->rewrite), (u8 *) ip1,
2454                             (void *) sr1 - (void *) ip1);
2455           clib_memcpy_fast ((u8 *) ip2 - vec_len (sl2->rewrite), (u8 *) ip2,
2456                             (void *) sr2 - (void *) ip2);
2457           clib_memcpy_fast ((u8 *) ip3 - vec_len (sl3->rewrite), (u8 *) ip3,
2458                             (void *) sr3 - (void *) ip3);
2459
2460           clib_memcpy_fast (((u8 *) sr0 - vec_len (sl0->rewrite)),
2461                             sl0->rewrite, vec_len (sl0->rewrite));
2462           clib_memcpy_fast (((u8 *) sr1 - vec_len (sl1->rewrite)),
2463                             sl1->rewrite, vec_len (sl1->rewrite));
2464           clib_memcpy_fast (((u8 *) sr2 - vec_len (sl2->rewrite)),
2465                             sl2->rewrite, vec_len (sl2->rewrite));
2466           clib_memcpy_fast (((u8 *) sr3 - vec_len (sl3->rewrite)),
2467                             sl3->rewrite, vec_len (sl3->rewrite));
2468
2469           vlib_buffer_advance (b0, -(word) vec_len (sl0->rewrite));
2470           vlib_buffer_advance (b1, -(word) vec_len (sl1->rewrite));
2471           vlib_buffer_advance (b2, -(word) vec_len (sl2->rewrite));
2472           vlib_buffer_advance (b3, -(word) vec_len (sl3->rewrite));
2473
2474           ip0 = ((void *) ip0) - vec_len (sl0->rewrite);
2475           ip1 = ((void *) ip1) - vec_len (sl1->rewrite);
2476           ip2 = ((void *) ip2) - vec_len (sl2->rewrite);
2477           ip3 = ((void *) ip3) - vec_len (sl3->rewrite);
2478
2479           ip0->hop_limit -= 1;
2480           ip1->hop_limit -= 1;
2481           ip2->hop_limit -= 1;
2482           ip3->hop_limit -= 1;
2483
2484           new_l0 =
2485             clib_net_to_host_u16 (ip0->payload_length) +
2486             vec_len (sl0->rewrite);
2487           new_l1 =
2488             clib_net_to_host_u16 (ip1->payload_length) +
2489             vec_len (sl1->rewrite);
2490           new_l2 =
2491             clib_net_to_host_u16 (ip2->payload_length) +
2492             vec_len (sl2->rewrite);
2493           new_l3 =
2494             clib_net_to_host_u16 (ip3->payload_length) +
2495             vec_len (sl3->rewrite);
2496
2497           ip0->payload_length = clib_host_to_net_u16 (new_l0);
2498           ip1->payload_length = clib_host_to_net_u16 (new_l1);
2499           ip2->payload_length = clib_host_to_net_u16 (new_l2);
2500           ip3->payload_length = clib_host_to_net_u16 (new_l3);
2501
2502           sr0 = ((void *) sr0) - vec_len (sl0->rewrite);
2503           sr1 = ((void *) sr1) - vec_len (sl1->rewrite);
2504           sr2 = ((void *) sr2) - vec_len (sl2->rewrite);
2505           sr3 = ((void *) sr3) - vec_len (sl3->rewrite);
2506
2507           sr0->segments->as_u64[0] = ip0->dst_address.as_u64[0];
2508           sr0->segments->as_u64[1] = ip0->dst_address.as_u64[1];
2509           sr1->segments->as_u64[0] = ip1->dst_address.as_u64[0];
2510           sr1->segments->as_u64[1] = ip1->dst_address.as_u64[1];
2511           sr2->segments->as_u64[0] = ip2->dst_address.as_u64[0];
2512           sr2->segments->as_u64[1] = ip2->dst_address.as_u64[1];
2513           sr3->segments->as_u64[0] = ip3->dst_address.as_u64[0];
2514           sr3->segments->as_u64[1] = ip3->dst_address.as_u64[1];
2515
2516           ip0->dst_address.as_u64[0] =
2517             (sr0->segments + sr0->segments_left)->as_u64[0];
2518           ip0->dst_address.as_u64[1] =
2519             (sr0->segments + sr0->segments_left)->as_u64[1];
2520           ip1->dst_address.as_u64[0] =
2521             (sr1->segments + sr1->segments_left)->as_u64[0];
2522           ip1->dst_address.as_u64[1] =
2523             (sr1->segments + sr1->segments_left)->as_u64[1];
2524           ip2->dst_address.as_u64[0] =
2525             (sr2->segments + sr2->segments_left)->as_u64[0];
2526           ip2->dst_address.as_u64[1] =
2527             (sr2->segments + sr2->segments_left)->as_u64[1];
2528           ip3->dst_address.as_u64[0] =
2529             (sr3->segments + sr3->segments_left)->as_u64[0];
2530           ip3->dst_address.as_u64[1] =
2531             (sr3->segments + sr3->segments_left)->as_u64[1];
2532
2533           ip6_ext_header_t *ip_ext;
2534           if (ip0 + 1 == (void *) sr0)
2535             {
2536               sr0->protocol = ip0->protocol;
2537               ip0->protocol = IP_PROTOCOL_IPV6_ROUTE;
2538             }
2539           else
2540             {
2541               ip_ext = (void *) (ip0 + 1);
2542               sr0->protocol = ip_ext->next_hdr;
2543               ip_ext->next_hdr = IP_PROTOCOL_IPV6_ROUTE;
2544             }
2545
2546           if (ip1 + 1 == (void *) sr1)
2547             {
2548               sr1->protocol = ip1->protocol;
2549               ip1->protocol = IP_PROTOCOL_IPV6_ROUTE;
2550             }
2551           else
2552             {
2553               ip_ext = (void *) (ip2 + 1);
2554               sr2->protocol = ip_ext->next_hdr;
2555               ip_ext->next_hdr = IP_PROTOCOL_IPV6_ROUTE;
2556             }
2557
2558           if (ip2 + 1 == (void *) sr2)
2559             {
2560               sr2->protocol = ip2->protocol;
2561               ip2->protocol = IP_PROTOCOL_IPV6_ROUTE;
2562             }
2563           else
2564             {
2565               ip_ext = (void *) (ip2 + 1);
2566               sr2->protocol = ip_ext->next_hdr;
2567               ip_ext->next_hdr = IP_PROTOCOL_IPV6_ROUTE;
2568             }
2569
2570           if (ip3 + 1 == (void *) sr3)
2571             {
2572               sr3->protocol = ip3->protocol;
2573               ip3->protocol = IP_PROTOCOL_IPV6_ROUTE;
2574             }
2575           else
2576             {
2577               ip_ext = (void *) (ip3 + 1);
2578               sr3->protocol = ip_ext->next_hdr;
2579               ip_ext->next_hdr = IP_PROTOCOL_IPV6_ROUTE;
2580             }
2581
2582           insert_pkts += 4;
2583
2584           if (PREDICT_FALSE ((node->flags & VLIB_NODE_FLAG_TRACE)))
2585             {
2586               if (PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
2587                 {
2588                   sr_policy_rewrite_trace_t *tr =
2589                     vlib_add_trace (vm, node, b0, sizeof (*tr));
2590                   clib_memcpy_fast (tr->src.as_u8, ip0->src_address.as_u8,
2591                                     sizeof (tr->src.as_u8));
2592                   clib_memcpy_fast (tr->dst.as_u8, ip0->dst_address.as_u8,
2593                                     sizeof (tr->dst.as_u8));
2594                 }
2595
2596               if (PREDICT_FALSE (b1->flags & VLIB_BUFFER_IS_TRACED))
2597                 {
2598                   sr_policy_rewrite_trace_t *tr =
2599                     vlib_add_trace (vm, node, b1, sizeof (*tr));
2600                   clib_memcpy_fast (tr->src.as_u8, ip1->src_address.as_u8,
2601                                     sizeof (tr->src.as_u8));
2602                   clib_memcpy_fast (tr->dst.as_u8, ip1->dst_address.as_u8,
2603                                     sizeof (tr->dst.as_u8));
2604                 }
2605
2606               if (PREDICT_FALSE (b2->flags & VLIB_BUFFER_IS_TRACED))
2607                 {
2608                   sr_policy_rewrite_trace_t *tr =
2609                     vlib_add_trace (vm, node, b2, sizeof (*tr));
2610                   clib_memcpy_fast (tr->src.as_u8, ip2->src_address.as_u8,
2611                                     sizeof (tr->src.as_u8));
2612                   clib_memcpy_fast (tr->dst.as_u8, ip2->dst_address.as_u8,
2613                                     sizeof (tr->dst.as_u8));
2614                 }
2615
2616               if (PREDICT_FALSE (b3->flags & VLIB_BUFFER_IS_TRACED))
2617                 {
2618                   sr_policy_rewrite_trace_t *tr =
2619                     vlib_add_trace (vm, node, b3, sizeof (*tr));
2620                   clib_memcpy_fast (tr->src.as_u8, ip3->src_address.as_u8,
2621                                     sizeof (tr->src.as_u8));
2622                   clib_memcpy_fast (tr->dst.as_u8, ip3->dst_address.as_u8,
2623                                     sizeof (tr->dst.as_u8));
2624                 }
2625             }
2626
2627           vlib_validate_buffer_enqueue_x4 (vm, node, next_index, to_next,
2628                                            n_left_to_next, bi0, bi1, bi2, bi3,
2629                                            next0, next1, next2, next3);
2630         }
2631
2632       /* Single loop for potentially the last three packets */
2633       while (n_left_from > 0 && n_left_to_next > 0)
2634         {
2635           u32 bi0;
2636           vlib_buffer_t *b0;
2637           ip6_header_t *ip0 = 0;
2638           ip6_sr_header_t *sr0 = 0;
2639           ip6_sr_sl_t *sl0;
2640           u32 next0 = SR_POLICY_REWRITE_NEXT_IP6_LOOKUP;
2641           u16 new_l0 = 0;
2642
2643           bi0 = from[0];
2644           to_next[0] = bi0;
2645           from += 1;
2646           to_next += 1;
2647           n_left_from -= 1;
2648           n_left_to_next -= 1;
2649
2650           b0 = vlib_get_buffer (vm, bi0);
2651           sl0 =
2652             pool_elt_at_index (sm->sid_lists,
2653                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
2654           ASSERT (b0->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
2655                   vec_len (sl0->rewrite));
2656
2657           ip0 = vlib_buffer_get_current (b0);
2658
2659           if (ip0->protocol == IP_PROTOCOL_IP6_HOP_BY_HOP_OPTIONS)
2660             sr0 =
2661               (ip6_sr_header_t *) (((void *) (ip0 + 1)) +
2662                                    ip6_ext_header_len (ip0 + 1));
2663           else
2664             sr0 = (ip6_sr_header_t *) (ip0 + 1);
2665
2666           clib_memcpy_fast ((u8 *) ip0 - vec_len (sl0->rewrite), (u8 *) ip0,
2667                             (void *) sr0 - (void *) ip0);
2668           clib_memcpy_fast (((u8 *) sr0 - vec_len (sl0->rewrite)),
2669                             sl0->rewrite, vec_len (sl0->rewrite));
2670
2671           vlib_buffer_advance (b0, -(word) vec_len (sl0->rewrite));
2672
2673           ip0 = ((void *) ip0) - vec_len (sl0->rewrite);
2674           ip0->hop_limit -= 1;
2675           new_l0 =
2676             clib_net_to_host_u16 (ip0->payload_length) +
2677             vec_len (sl0->rewrite);
2678           ip0->payload_length = clib_host_to_net_u16 (new_l0);
2679
2680           sr0 = ((void *) sr0) - vec_len (sl0->rewrite);
2681           sr0->segments->as_u64[0] = ip0->dst_address.as_u64[0];
2682           sr0->segments->as_u64[1] = ip0->dst_address.as_u64[1];
2683
2684           ip0->dst_address.as_u64[0] =
2685             (sr0->segments + sr0->segments_left)->as_u64[0];
2686           ip0->dst_address.as_u64[1] =
2687             (sr0->segments + sr0->segments_left)->as_u64[1];
2688
2689           if (ip0 + 1 == (void *) sr0)
2690             {
2691               sr0->protocol = ip0->protocol;
2692               ip0->protocol = IP_PROTOCOL_IPV6_ROUTE;
2693             }
2694           else
2695             {
2696               ip6_ext_header_t *ip_ext = (void *) (ip0 + 1);
2697               sr0->protocol = ip_ext->next_hdr;
2698               ip_ext->next_hdr = IP_PROTOCOL_IPV6_ROUTE;
2699             }
2700
2701           if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
2702               PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
2703             {
2704               sr_policy_rewrite_trace_t *tr =
2705                 vlib_add_trace (vm, node, b0, sizeof (*tr));
2706               clib_memcpy_fast (tr->src.as_u8, ip0->src_address.as_u8,
2707                                 sizeof (tr->src.as_u8));
2708               clib_memcpy_fast (tr->dst.as_u8, ip0->dst_address.as_u8,
2709                                 sizeof (tr->dst.as_u8));
2710             }
2711
2712           insert_pkts++;
2713
2714           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
2715                                            n_left_to_next, bi0, next0);
2716         }
2717
2718       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
2719     }
2720
2721   /* Update counters */
2722   vlib_node_increment_counter (vm, sr_policy_rewrite_insert_node.index,
2723                                SR_POLICY_REWRITE_ERROR_COUNTER_TOTAL,
2724                                insert_pkts);
2725   vlib_node_increment_counter (vm, sr_policy_rewrite_insert_node.index,
2726                                SR_POLICY_REWRITE_ERROR_COUNTER_BSID,
2727                                bsid_pkts);
2728   return from_frame->n_vectors;
2729 }
2730
2731 /* *INDENT-OFF* */
2732 VLIB_REGISTER_NODE (sr_policy_rewrite_insert_node) = {
2733   .function = sr_policy_rewrite_insert,
2734   .name = "sr-pl-rewrite-insert",
2735   .vector_size = sizeof (u32),
2736   .format_trace = format_sr_policy_rewrite_trace,
2737   .type = VLIB_NODE_TYPE_INTERNAL,
2738   .n_errors = SR_POLICY_REWRITE_N_ERROR,
2739   .error_strings = sr_policy_rewrite_error_strings,
2740   .n_next_nodes = SR_POLICY_REWRITE_N_NEXT,
2741   .next_nodes = {
2742 #define _(s,n) [SR_POLICY_REWRITE_NEXT_##s] = n,
2743     foreach_sr_policy_rewrite_next
2744 #undef _
2745   },
2746 };
2747 /* *INDENT-ON* */
2748
2749 /**
2750  * @brief Graph node for applying a SR policy into a packet. BSID - SRH insertion.
2751  */
2752 static uword
2753 sr_policy_rewrite_b_insert (vlib_main_t * vm, vlib_node_runtime_t * node,
2754                             vlib_frame_t * from_frame)
2755 {
2756   ip6_sr_main_t *sm = &sr_main;
2757   u32 n_left_from, next_index, *from, *to_next;
2758
2759   from = vlib_frame_vector_args (from_frame);
2760   n_left_from = from_frame->n_vectors;
2761
2762   next_index = node->cached_next_index;
2763
2764   int insert_pkts = 0, bsid_pkts = 0;
2765
2766   while (n_left_from > 0)
2767     {
2768       u32 n_left_to_next;
2769
2770       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
2771
2772       /* Quad - Loop */
2773       while (n_left_from >= 8 && n_left_to_next >= 4)
2774         {
2775           u32 bi0, bi1, bi2, bi3;
2776           vlib_buffer_t *b0, *b1, *b2, *b3;
2777           u32 next0, next1, next2, next3;
2778           next0 = next1 = next2 = next3 = SR_POLICY_REWRITE_NEXT_IP6_LOOKUP;
2779           ip6_header_t *ip0, *ip1, *ip2, *ip3;
2780           ip6_sr_header_t *sr0, *sr1, *sr2, *sr3;
2781           ip6_sr_sl_t *sl0, *sl1, *sl2, *sl3;
2782           u16 new_l0, new_l1, new_l2, new_l3;
2783
2784           /* Prefetch next iteration. */
2785           {
2786             vlib_buffer_t *p4, *p5, *p6, *p7;
2787
2788             p4 = vlib_get_buffer (vm, from[4]);
2789             p5 = vlib_get_buffer (vm, from[5]);
2790             p6 = vlib_get_buffer (vm, from[6]);
2791             p7 = vlib_get_buffer (vm, from[7]);
2792
2793             /* Prefetch the buffer header and packet for the N+2 loop iteration */
2794             vlib_prefetch_buffer_header (p4, LOAD);
2795             vlib_prefetch_buffer_header (p5, LOAD);
2796             vlib_prefetch_buffer_header (p6, LOAD);
2797             vlib_prefetch_buffer_header (p7, LOAD);
2798
2799             clib_prefetch_store (p4->data);
2800             clib_prefetch_store (p5->data);
2801             clib_prefetch_store (p6->data);
2802             clib_prefetch_store (p7->data);
2803           }
2804
2805           to_next[0] = bi0 = from[0];
2806           to_next[1] = bi1 = from[1];
2807           to_next[2] = bi2 = from[2];
2808           to_next[3] = bi3 = from[3];
2809           from += 4;
2810           to_next += 4;
2811           n_left_from -= 4;
2812           n_left_to_next -= 4;
2813
2814           b0 = vlib_get_buffer (vm, bi0);
2815           b1 = vlib_get_buffer (vm, bi1);
2816           b2 = vlib_get_buffer (vm, bi2);
2817           b3 = vlib_get_buffer (vm, bi3);
2818
2819           sl0 =
2820             pool_elt_at_index (sm->sid_lists,
2821                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
2822           sl1 =
2823             pool_elt_at_index (sm->sid_lists,
2824                                vnet_buffer (b1)->ip.adj_index[VLIB_TX]);
2825           sl2 =
2826             pool_elt_at_index (sm->sid_lists,
2827                                vnet_buffer (b2)->ip.adj_index[VLIB_TX]);
2828           sl3 =
2829             pool_elt_at_index (sm->sid_lists,
2830                                vnet_buffer (b3)->ip.adj_index[VLIB_TX]);
2831           ASSERT (b0->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
2832                   vec_len (sl0->rewrite_bsid));
2833           ASSERT (b1->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
2834                   vec_len (sl1->rewrite_bsid));
2835           ASSERT (b2->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
2836                   vec_len (sl2->rewrite_bsid));
2837           ASSERT (b3->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
2838                   vec_len (sl3->rewrite_bsid));
2839
2840           ip0 = vlib_buffer_get_current (b0);
2841           ip1 = vlib_buffer_get_current (b1);
2842           ip2 = vlib_buffer_get_current (b2);
2843           ip3 = vlib_buffer_get_current (b3);
2844
2845           if (ip0->protocol == IP_PROTOCOL_IP6_HOP_BY_HOP_OPTIONS)
2846             sr0 =
2847               (ip6_sr_header_t *) (((void *) (ip0 + 1)) +
2848                                    ip6_ext_header_len (ip0 + 1));
2849           else
2850             sr0 = (ip6_sr_header_t *) (ip0 + 1);
2851
2852           if (ip1->protocol == IP_PROTOCOL_IP6_HOP_BY_HOP_OPTIONS)
2853             sr1 =
2854               (ip6_sr_header_t *) (((void *) (ip1 + 1)) +
2855                                    ip6_ext_header_len (ip1 + 1));
2856           else
2857             sr1 = (ip6_sr_header_t *) (ip1 + 1);
2858
2859           if (ip2->protocol == IP_PROTOCOL_IP6_HOP_BY_HOP_OPTIONS)
2860             sr2 =
2861               (ip6_sr_header_t *) (((void *) (ip2 + 1)) +
2862                                    ip6_ext_header_len (ip2 + 1));
2863           else
2864             sr2 = (ip6_sr_header_t *) (ip2 + 1);
2865
2866           if (ip3->protocol == IP_PROTOCOL_IP6_HOP_BY_HOP_OPTIONS)
2867             sr3 =
2868               (ip6_sr_header_t *) (((void *) (ip3 + 1)) +
2869                                    ip6_ext_header_len (ip3 + 1));
2870           else
2871             sr3 = (ip6_sr_header_t *) (ip3 + 1);
2872
2873           clib_memcpy_fast ((u8 *) ip0 - vec_len (sl0->rewrite_bsid),
2874                             (u8 *) ip0, (void *) sr0 - (void *) ip0);
2875           clib_memcpy_fast ((u8 *) ip1 - vec_len (sl1->rewrite_bsid),
2876                             (u8 *) ip1, (void *) sr1 - (void *) ip1);
2877           clib_memcpy_fast ((u8 *) ip2 - vec_len (sl2->rewrite_bsid),
2878                             (u8 *) ip2, (void *) sr2 - (void *) ip2);
2879           clib_memcpy_fast ((u8 *) ip3 - vec_len (sl3->rewrite_bsid),
2880                             (u8 *) ip3, (void *) sr3 - (void *) ip3);
2881
2882           clib_memcpy_fast (((u8 *) sr0 - vec_len (sl0->rewrite_bsid)),
2883                             sl0->rewrite_bsid, vec_len (sl0->rewrite_bsid));
2884           clib_memcpy_fast (((u8 *) sr1 - vec_len (sl1->rewrite_bsid)),
2885                             sl1->rewrite_bsid, vec_len (sl1->rewrite_bsid));
2886           clib_memcpy_fast (((u8 *) sr2 - vec_len (sl2->rewrite_bsid)),
2887                             sl2->rewrite_bsid, vec_len (sl2->rewrite_bsid));
2888           clib_memcpy_fast (((u8 *) sr3 - vec_len (sl3->rewrite_bsid)),
2889                             sl3->rewrite_bsid, vec_len (sl3->rewrite_bsid));
2890
2891           vlib_buffer_advance (b0, -(word) vec_len (sl0->rewrite_bsid));
2892           vlib_buffer_advance (b1, -(word) vec_len (sl1->rewrite_bsid));
2893           vlib_buffer_advance (b2, -(word) vec_len (sl2->rewrite_bsid));
2894           vlib_buffer_advance (b3, -(word) vec_len (sl3->rewrite_bsid));
2895
2896           ip0 = ((void *) ip0) - vec_len (sl0->rewrite_bsid);
2897           ip1 = ((void *) ip1) - vec_len (sl1->rewrite_bsid);
2898           ip2 = ((void *) ip2) - vec_len (sl2->rewrite_bsid);
2899           ip3 = ((void *) ip3) - vec_len (sl3->rewrite_bsid);
2900
2901           ip0->hop_limit -= 1;
2902           ip1->hop_limit -= 1;
2903           ip2->hop_limit -= 1;
2904           ip3->hop_limit -= 1;
2905
2906           new_l0 =
2907             clib_net_to_host_u16 (ip0->payload_length) +
2908             vec_len (sl0->rewrite_bsid);
2909           new_l1 =
2910             clib_net_to_host_u16 (ip1->payload_length) +
2911             vec_len (sl1->rewrite_bsid);
2912           new_l2 =
2913             clib_net_to_host_u16 (ip2->payload_length) +
2914             vec_len (sl2->rewrite_bsid);
2915           new_l3 =
2916             clib_net_to_host_u16 (ip3->payload_length) +
2917             vec_len (sl3->rewrite_bsid);
2918
2919           ip0->payload_length = clib_host_to_net_u16 (new_l0);
2920           ip1->payload_length = clib_host_to_net_u16 (new_l1);
2921           ip2->payload_length = clib_host_to_net_u16 (new_l2);
2922           ip3->payload_length = clib_host_to_net_u16 (new_l3);
2923
2924           sr0 = ((void *) sr0) - vec_len (sl0->rewrite_bsid);
2925           sr1 = ((void *) sr1) - vec_len (sl1->rewrite_bsid);
2926           sr2 = ((void *) sr2) - vec_len (sl2->rewrite_bsid);
2927           sr3 = ((void *) sr3) - vec_len (sl3->rewrite_bsid);
2928
2929           ip0->dst_address.as_u64[0] =
2930             (sr0->segments + sr0->segments_left)->as_u64[0];
2931           ip0->dst_address.as_u64[1] =
2932             (sr0->segments + sr0->segments_left)->as_u64[1];
2933           ip1->dst_address.as_u64[0] =
2934             (sr1->segments + sr1->segments_left)->as_u64[0];
2935           ip1->dst_address.as_u64[1] =
2936             (sr1->segments + sr1->segments_left)->as_u64[1];
2937           ip2->dst_address.as_u64[0] =
2938             (sr2->segments + sr2->segments_left)->as_u64[0];
2939           ip2->dst_address.as_u64[1] =
2940             (sr2->segments + sr2->segments_left)->as_u64[1];
2941           ip3->dst_address.as_u64[0] =
2942             (sr3->segments + sr3->segments_left)->as_u64[0];
2943           ip3->dst_address.as_u64[1] =
2944             (sr3->segments + sr3->segments_left)->as_u64[1];
2945
2946           ip6_ext_header_t *ip_ext;
2947           if (ip0 + 1 == (void *) sr0)
2948             {
2949               sr0->protocol = ip0->protocol;
2950               ip0->protocol = IP_PROTOCOL_IPV6_ROUTE;
2951             }
2952           else
2953             {
2954               ip_ext = (void *) (ip0 + 1);
2955               sr0->protocol = ip_ext->next_hdr;
2956               ip_ext->next_hdr = IP_PROTOCOL_IPV6_ROUTE;
2957             }
2958
2959           if (ip1 + 1 == (void *) sr1)
2960             {
2961               sr1->protocol = ip1->protocol;
2962               ip1->protocol = IP_PROTOCOL_IPV6_ROUTE;
2963             }
2964           else
2965             {
2966               ip_ext = (void *) (ip2 + 1);
2967               sr2->protocol = ip_ext->next_hdr;
2968               ip_ext->next_hdr = IP_PROTOCOL_IPV6_ROUTE;
2969             }
2970
2971           if (ip2 + 1 == (void *) sr2)
2972             {
2973               sr2->protocol = ip2->protocol;
2974               ip2->protocol = IP_PROTOCOL_IPV6_ROUTE;
2975             }
2976           else
2977             {
2978               ip_ext = (void *) (ip2 + 1);
2979               sr2->protocol = ip_ext->next_hdr;
2980               ip_ext->next_hdr = IP_PROTOCOL_IPV6_ROUTE;
2981             }
2982
2983           if (ip3 + 1 == (void *) sr3)
2984             {
2985               sr3->protocol = ip3->protocol;
2986               ip3->protocol = IP_PROTOCOL_IPV6_ROUTE;
2987             }
2988           else
2989             {
2990               ip_ext = (void *) (ip3 + 1);
2991               sr3->protocol = ip_ext->next_hdr;
2992               ip_ext->next_hdr = IP_PROTOCOL_IPV6_ROUTE;
2993             }
2994
2995           insert_pkts += 4;
2996
2997           if (PREDICT_FALSE ((node->flags & VLIB_NODE_FLAG_TRACE)))
2998             {
2999               if (PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
3000                 {
3001                   sr_policy_rewrite_trace_t *tr =
3002                     vlib_add_trace (vm, node, b0, sizeof (*tr));
3003                   clib_memcpy_fast (tr->src.as_u8, ip0->src_address.as_u8,
3004                                     sizeof (tr->src.as_u8));
3005                   clib_memcpy_fast (tr->dst.as_u8, ip0->dst_address.as_u8,
3006                                     sizeof (tr->dst.as_u8));
3007                 }
3008
3009               if (PREDICT_FALSE (b1->flags & VLIB_BUFFER_IS_TRACED))
3010                 {
3011                   sr_policy_rewrite_trace_t *tr =
3012                     vlib_add_trace (vm, node, b1, sizeof (*tr));
3013                   clib_memcpy_fast (tr->src.as_u8, ip1->src_address.as_u8,
3014                                     sizeof (tr->src.as_u8));
3015                   clib_memcpy_fast (tr->dst.as_u8, ip1->dst_address.as_u8,
3016                                     sizeof (tr->dst.as_u8));
3017                 }
3018
3019               if (PREDICT_FALSE (b2->flags & VLIB_BUFFER_IS_TRACED))
3020                 {
3021                   sr_policy_rewrite_trace_t *tr =
3022                     vlib_add_trace (vm, node, b2, sizeof (*tr));
3023                   clib_memcpy_fast (tr->src.as_u8, ip2->src_address.as_u8,
3024                                     sizeof (tr->src.as_u8));
3025                   clib_memcpy_fast (tr->dst.as_u8, ip2->dst_address.as_u8,
3026                                     sizeof (tr->dst.as_u8));
3027                 }
3028
3029               if (PREDICT_FALSE (b3->flags & VLIB_BUFFER_IS_TRACED))
3030                 {
3031                   sr_policy_rewrite_trace_t *tr =
3032                     vlib_add_trace (vm, node, b3, sizeof (*tr));
3033                   clib_memcpy_fast (tr->src.as_u8, ip3->src_address.as_u8,
3034                                     sizeof (tr->src.as_u8));
3035                   clib_memcpy_fast (tr->dst.as_u8, ip3->dst_address.as_u8,
3036                                     sizeof (tr->dst.as_u8));
3037                 }
3038             }
3039
3040           vlib_validate_buffer_enqueue_x4 (vm, node, next_index, to_next,
3041                                            n_left_to_next, bi0, bi1, bi2, bi3,
3042                                            next0, next1, next2, next3);
3043         }
3044
3045       /* Single loop for potentially the last three packets */
3046       while (n_left_from > 0 && n_left_to_next > 0)
3047         {
3048           u32 bi0;
3049           vlib_buffer_t *b0;
3050           ip6_header_t *ip0 = 0;
3051           ip6_sr_header_t *sr0 = 0;
3052           ip6_sr_sl_t *sl0;
3053           u32 next0 = SR_POLICY_REWRITE_NEXT_IP6_LOOKUP;
3054           u16 new_l0 = 0;
3055
3056           bi0 = from[0];
3057           to_next[0] = bi0;
3058           from += 1;
3059           to_next += 1;
3060           n_left_from -= 1;
3061           n_left_to_next -= 1;
3062
3063           b0 = vlib_get_buffer (vm, bi0);
3064           sl0 =
3065             pool_elt_at_index (sm->sid_lists,
3066                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
3067           ASSERT (b0->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
3068                   vec_len (sl0->rewrite_bsid));
3069
3070           ip0 = vlib_buffer_get_current (b0);
3071
3072           if (ip0->protocol == IP_PROTOCOL_IP6_HOP_BY_HOP_OPTIONS)
3073             sr0 =
3074               (ip6_sr_header_t *) (((void *) (ip0 + 1)) +
3075                                    ip6_ext_header_len (ip0 + 1));
3076           else
3077             sr0 = (ip6_sr_header_t *) (ip0 + 1);
3078
3079           clib_memcpy_fast ((u8 *) ip0 - vec_len (sl0->rewrite_bsid),
3080                             (u8 *) ip0, (void *) sr0 - (void *) ip0);
3081           clib_memcpy_fast (((u8 *) sr0 - vec_len (sl0->rewrite_bsid)),
3082                             sl0->rewrite_bsid, vec_len (sl0->rewrite_bsid));
3083
3084           vlib_buffer_advance (b0, -(word) vec_len (sl0->rewrite_bsid));
3085
3086           ip0 = ((void *) ip0) - vec_len (sl0->rewrite_bsid);
3087           ip0->hop_limit -= 1;
3088           new_l0 =
3089             clib_net_to_host_u16 (ip0->payload_length) +
3090             vec_len (sl0->rewrite_bsid);
3091           ip0->payload_length = clib_host_to_net_u16 (new_l0);
3092
3093           sr0 = ((void *) sr0) - vec_len (sl0->rewrite_bsid);
3094
3095           ip0->dst_address.as_u64[0] =
3096             (sr0->segments + sr0->segments_left)->as_u64[0];
3097           ip0->dst_address.as_u64[1] =
3098             (sr0->segments + sr0->segments_left)->as_u64[1];
3099
3100           if (ip0 + 1 == (void *) sr0)
3101             {
3102               sr0->protocol = ip0->protocol;
3103               ip0->protocol = IP_PROTOCOL_IPV6_ROUTE;
3104             }
3105           else
3106             {
3107               ip6_ext_header_t *ip_ext = (void *) (ip0 + 1);
3108               sr0->protocol = ip_ext->next_hdr;
3109               ip_ext->next_hdr = IP_PROTOCOL_IPV6_ROUTE;
3110             }
3111
3112           if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
3113               PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
3114             {
3115               sr_policy_rewrite_trace_t *tr =
3116                 vlib_add_trace (vm, node, b0, sizeof (*tr));
3117               clib_memcpy_fast (tr->src.as_u8, ip0->src_address.as_u8,
3118                                 sizeof (tr->src.as_u8));
3119               clib_memcpy_fast (tr->dst.as_u8, ip0->dst_address.as_u8,
3120                                 sizeof (tr->dst.as_u8));
3121             }
3122
3123           insert_pkts++;
3124
3125           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
3126                                            n_left_to_next, bi0, next0);
3127         }
3128
3129       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
3130     }
3131
3132   /* Update counters */
3133   vlib_node_increment_counter (vm, sr_policy_rewrite_insert_node.index,
3134                                SR_POLICY_REWRITE_ERROR_COUNTER_TOTAL,
3135                                insert_pkts);
3136   vlib_node_increment_counter (vm, sr_policy_rewrite_insert_node.index,
3137                                SR_POLICY_REWRITE_ERROR_COUNTER_BSID,
3138                                bsid_pkts);
3139   return from_frame->n_vectors;
3140 }
3141
3142 /* *INDENT-OFF* */
3143 VLIB_REGISTER_NODE (sr_policy_rewrite_b_insert_node) = {
3144   .function = sr_policy_rewrite_b_insert,
3145   .name = "sr-pl-rewrite-b-insert",
3146   .vector_size = sizeof (u32),
3147   .format_trace = format_sr_policy_rewrite_trace,
3148   .type = VLIB_NODE_TYPE_INTERNAL,
3149   .n_errors = SR_POLICY_REWRITE_N_ERROR,
3150   .error_strings = sr_policy_rewrite_error_strings,
3151   .n_next_nodes = SR_POLICY_REWRITE_N_NEXT,
3152   .next_nodes = {
3153 #define _(s,n) [SR_POLICY_REWRITE_NEXT_##s] = n,
3154     foreach_sr_policy_rewrite_next
3155 #undef _
3156   },
3157 };
3158 /* *INDENT-ON* */
3159
3160 /**
3161  * @brief Function BSID encapsulation
3162  */
3163 static_always_inline void
3164 end_bsid_encaps_srh_processing (vlib_node_runtime_t *node, vlib_buffer_t *b0,
3165                                 ip6_header_t *ip0, ip6_sr_header_t *sr0,
3166                                 u32 *next0, u8 policy_type)
3167 {
3168   ip6_address_t *new_dst0;
3169
3170   if (PREDICT_FALSE (!sr0))
3171     goto error_bsid_encaps;
3172
3173   if (PREDICT_TRUE (sr0->type == ROUTING_HEADER_TYPE_SR))
3174     {
3175       if (PREDICT_TRUE (sr0->segments_left != 0))
3176         {
3177           sr0->segments_left -= 1;
3178           new_dst0 = (ip6_address_t *) (sr0->segments);
3179           new_dst0 += sr0->segments_left;
3180           ip0->dst_address.as_u64[0] = new_dst0->as_u64[0];
3181           ip0->dst_address.as_u64[1] = new_dst0->as_u64[1];
3182           return;
3183         }
3184       else if (sr0->segments_left == 0 && policy_type == SR_POLICY_TYPE_TEF)
3185         return;
3186     }
3187
3188 error_bsid_encaps:
3189   *next0 = SR_POLICY_REWRITE_NEXT_ERROR;
3190   b0->error = node->errors[SR_POLICY_REWRITE_ERROR_BSID_ZERO];
3191 }
3192
3193 /**
3194  * @brief Graph node for applying a SR policy BSID - Encapsulation
3195  */
3196 static uword
3197 sr_policy_rewrite_b_encaps (vlib_main_t * vm, vlib_node_runtime_t * node,
3198                             vlib_frame_t * from_frame)
3199 {
3200   ip6_sr_main_t *sm = &sr_main;
3201   u32 n_left_from, next_index, *from, *to_next;
3202
3203   from = vlib_frame_vector_args (from_frame);
3204   n_left_from = from_frame->n_vectors;
3205
3206   next_index = node->cached_next_index;
3207
3208   int encap_pkts = 0, bsid_pkts = 0;
3209
3210   while (n_left_from > 0)
3211     {
3212       u32 n_left_to_next;
3213
3214       vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
3215
3216       /* Quad - Loop */
3217       while (n_left_from >= 8 && n_left_to_next >= 4)
3218         {
3219           u32 bi0, bi1, bi2, bi3;
3220           vlib_buffer_t *b0, *b1, *b2, *b3;
3221           u32 next0, next1, next2, next3;
3222           next0 = next1 = next2 = next3 = SR_POLICY_REWRITE_NEXT_IP6_LOOKUP;
3223           ip6_header_t *ip0, *ip1, *ip2, *ip3;
3224           ip6_header_t *ip0_encap, *ip1_encap, *ip2_encap, *ip3_encap;
3225           ip6_sr_header_t *sr0, *sr1, *sr2, *sr3;
3226           ip6_sr_sl_t *sl0, *sl1, *sl2, *sl3;
3227
3228           /* Prefetch next iteration. */
3229           {
3230             vlib_buffer_t *p4, *p5, *p6, *p7;
3231
3232             p4 = vlib_get_buffer (vm, from[4]);
3233             p5 = vlib_get_buffer (vm, from[5]);
3234             p6 = vlib_get_buffer (vm, from[6]);
3235             p7 = vlib_get_buffer (vm, from[7]);
3236
3237             /* Prefetch the buffer header and packet for the N+2 loop iteration */
3238             vlib_prefetch_buffer_header (p4, LOAD);
3239             vlib_prefetch_buffer_header (p5, LOAD);
3240             vlib_prefetch_buffer_header (p6, LOAD);
3241             vlib_prefetch_buffer_header (p7, LOAD);
3242
3243             clib_prefetch_store (p4->data);
3244             clib_prefetch_store (p5->data);
3245             clib_prefetch_store (p6->data);
3246             clib_prefetch_store (p7->data);
3247           }
3248
3249           to_next[0] = bi0 = from[0];
3250           to_next[1] = bi1 = from[1];
3251           to_next[2] = bi2 = from[2];
3252           to_next[3] = bi3 = from[3];
3253           from += 4;
3254           to_next += 4;
3255           n_left_from -= 4;
3256           n_left_to_next -= 4;
3257
3258           b0 = vlib_get_buffer (vm, bi0);
3259           b1 = vlib_get_buffer (vm, bi1);
3260           b2 = vlib_get_buffer (vm, bi2);
3261           b3 = vlib_get_buffer (vm, bi3);
3262
3263           sl0 =
3264             pool_elt_at_index (sm->sid_lists,
3265                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
3266           sl1 =
3267             pool_elt_at_index (sm->sid_lists,
3268                                vnet_buffer (b1)->ip.adj_index[VLIB_TX]);
3269           sl2 =
3270             pool_elt_at_index (sm->sid_lists,
3271                                vnet_buffer (b2)->ip.adj_index[VLIB_TX]);
3272           sl3 =
3273             pool_elt_at_index (sm->sid_lists,
3274                                vnet_buffer (b3)->ip.adj_index[VLIB_TX]);
3275           ASSERT (b0->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
3276                   vec_len (sl0->rewrite));
3277           ASSERT (b1->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
3278                   vec_len (sl1->rewrite));
3279           ASSERT (b2->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
3280                   vec_len (sl2->rewrite));
3281           ASSERT (b3->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
3282                   vec_len (sl3->rewrite));
3283
3284           ip0_encap = vlib_buffer_get_current (b0);
3285           ip1_encap = vlib_buffer_get_current (b1);
3286           ip2_encap = vlib_buffer_get_current (b2);
3287           ip3_encap = vlib_buffer_get_current (b3);
3288
3289           sr0 =
3290             ip6_ext_header_find (vm, b0, ip0_encap, IP_PROTOCOL_IPV6_ROUTE,
3291                                  NULL);
3292           sr1 =
3293             ip6_ext_header_find (vm, b1, ip1_encap, IP_PROTOCOL_IPV6_ROUTE,
3294                                  NULL);
3295           sr2 =
3296             ip6_ext_header_find (vm, b2, ip2_encap, IP_PROTOCOL_IPV6_ROUTE,
3297                                  NULL);
3298           sr3 =
3299             ip6_ext_header_find (vm, b3, ip3_encap, IP_PROTOCOL_IPV6_ROUTE,
3300                                  NULL);
3301
3302           end_bsid_encaps_srh_processing (node, b0, ip0_encap, sr0, &next0,
3303                                           sl0->policy_type);
3304           end_bsid_encaps_srh_processing (node, b1, ip1_encap, sr1, &next1,
3305                                           sl1->policy_type);
3306           end_bsid_encaps_srh_processing (node, b2, ip2_encap, sr2, &next2,
3307                                           sl2->policy_type);
3308           end_bsid_encaps_srh_processing (node, b3, ip3_encap, sr3, &next3,
3309                                           sl3->policy_type);
3310
3311           clib_memcpy_fast (((u8 *) ip0_encap) - vec_len (sl0->rewrite),
3312                             sl0->rewrite, vec_len (sl0->rewrite));
3313           clib_memcpy_fast (((u8 *) ip1_encap) - vec_len (sl1->rewrite),
3314                             sl1->rewrite, vec_len (sl1->rewrite));
3315           clib_memcpy_fast (((u8 *) ip2_encap) - vec_len (sl2->rewrite),
3316                             sl2->rewrite, vec_len (sl2->rewrite));
3317           clib_memcpy_fast (((u8 *) ip3_encap) - vec_len (sl3->rewrite),
3318                             sl3->rewrite, vec_len (sl3->rewrite));
3319
3320           vlib_buffer_advance (b0, -(word) vec_len (sl0->rewrite));
3321           vlib_buffer_advance (b1, -(word) vec_len (sl1->rewrite));
3322           vlib_buffer_advance (b2, -(word) vec_len (sl2->rewrite));
3323           vlib_buffer_advance (b3, -(word) vec_len (sl3->rewrite));
3324
3325           ip0 = vlib_buffer_get_current (b0);
3326           ip1 = vlib_buffer_get_current (b1);
3327           ip2 = vlib_buffer_get_current (b2);
3328           ip3 = vlib_buffer_get_current (b3);
3329
3330           encaps_processing_v6 (node, b0, ip0, ip0_encap, sl0->policy_type);
3331           encaps_processing_v6 (node, b1, ip1, ip1_encap, sl1->policy_type);
3332           encaps_processing_v6 (node, b2, ip2, ip2_encap, sl2->policy_type);
3333           encaps_processing_v6 (node, b3, ip3, ip3_encap, sl3->policy_type);
3334
3335           if (PREDICT_FALSE ((node->flags & VLIB_NODE_FLAG_TRACE)))
3336             {
3337               if (PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
3338                 {
3339                   sr_policy_rewrite_trace_t *tr =
3340                     vlib_add_trace (vm, node, b0, sizeof (*tr));
3341                   clib_memcpy_fast (tr->src.as_u8, ip0->src_address.as_u8,
3342                                     sizeof (tr->src.as_u8));
3343                   clib_memcpy_fast (tr->dst.as_u8, ip0->dst_address.as_u8,
3344                                     sizeof (tr->dst.as_u8));
3345                 }
3346
3347               if (PREDICT_FALSE (b1->flags & VLIB_BUFFER_IS_TRACED))
3348                 {
3349                   sr_policy_rewrite_trace_t *tr =
3350                     vlib_add_trace (vm, node, b1, sizeof (*tr));
3351                   clib_memcpy_fast (tr->src.as_u8, ip1->src_address.as_u8,
3352                                     sizeof (tr->src.as_u8));
3353                   clib_memcpy_fast (tr->dst.as_u8, ip1->dst_address.as_u8,
3354                                     sizeof (tr->dst.as_u8));
3355                 }
3356
3357               if (PREDICT_FALSE (b2->flags & VLIB_BUFFER_IS_TRACED))
3358                 {
3359                   sr_policy_rewrite_trace_t *tr =
3360                     vlib_add_trace (vm, node, b2, sizeof (*tr));
3361                   clib_memcpy_fast (tr->src.as_u8, ip2->src_address.as_u8,
3362                                     sizeof (tr->src.as_u8));
3363                   clib_memcpy_fast (tr->dst.as_u8, ip2->dst_address.as_u8,
3364                                     sizeof (tr->dst.as_u8));
3365                 }
3366
3367               if (PREDICT_FALSE (b3->flags & VLIB_BUFFER_IS_TRACED))
3368                 {
3369                   sr_policy_rewrite_trace_t *tr =
3370                     vlib_add_trace (vm, node, b3, sizeof (*tr));
3371                   clib_memcpy_fast (tr->src.as_u8, ip3->src_address.as_u8,
3372                                     sizeof (tr->src.as_u8));
3373                   clib_memcpy_fast (tr->dst.as_u8, ip3->dst_address.as_u8,
3374                                     sizeof (tr->dst.as_u8));
3375                 }
3376             }
3377
3378           encap_pkts += 4;
3379           vlib_validate_buffer_enqueue_x4 (vm, node, next_index, to_next,
3380                                            n_left_to_next, bi0, bi1, bi2, bi3,
3381                                            next0, next1, next2, next3);
3382         }
3383
3384       /* Single loop for potentially the last three packets */
3385       while (n_left_from > 0 && n_left_to_next > 0)
3386         {
3387           u32 bi0;
3388           vlib_buffer_t *b0;
3389           ip6_header_t *ip0 = 0, *ip0_encap = 0;
3390           ip6_sr_header_t *sr0;
3391           ip6_sr_sl_t *sl0;
3392           u32 next0 = SR_POLICY_REWRITE_NEXT_IP6_LOOKUP;
3393
3394           bi0 = from[0];
3395           to_next[0] = bi0;
3396           from += 1;
3397           to_next += 1;
3398           n_left_from -= 1;
3399           n_left_to_next -= 1;
3400           b0 = vlib_get_buffer (vm, bi0);
3401
3402           sl0 =
3403             pool_elt_at_index (sm->sid_lists,
3404                                vnet_buffer (b0)->ip.adj_index[VLIB_TX]);
3405           ASSERT (b0->current_data + VLIB_BUFFER_PRE_DATA_SIZE >=
3406                   vec_len (sl0->rewrite));
3407
3408           ip0_encap = vlib_buffer_get_current (b0);
3409           sr0 =
3410             ip6_ext_header_find (vm, b0, ip0_encap, IP_PROTOCOL_IPV6_ROUTE,
3411                                  NULL);
3412           end_bsid_encaps_srh_processing (node, b0, ip0_encap, sr0, &next0,
3413                                           sl0->policy_type);
3414
3415           clib_memcpy_fast (((u8 *) ip0_encap) - vec_len (sl0->rewrite),
3416                             sl0->rewrite, vec_len (sl0->rewrite));
3417           vlib_buffer_advance (b0, -(word) vec_len (sl0->rewrite));
3418
3419           ip0 = vlib_buffer_get_current (b0);
3420
3421           encaps_processing_v6 (node, b0, ip0, ip0_encap, sl0->policy_type);
3422
3423           if (PREDICT_FALSE (node->flags & VLIB_NODE_FLAG_TRACE) &&
3424               PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
3425             {
3426               sr_policy_rewrite_trace_t *tr =
3427                 vlib_add_trace (vm, node, b0, sizeof (*tr));
3428               clib_memcpy_fast (tr->src.as_u8, ip0->src_address.as_u8,
3429                                 sizeof (tr->src.as_u8));
3430               clib_memcpy_fast (tr->dst.as_u8, ip0->dst_address.as_u8,
3431                                 sizeof (tr->dst.as_u8));
3432             }
3433
3434           encap_pkts++;
3435           vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
3436                                            n_left_to_next, bi0, next0);
3437         }
3438
3439       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
3440     }
3441
3442   /* Update counters */
3443   vlib_node_increment_counter (vm, sr_policy_rewrite_encaps_node.index,
3444                                SR_POLICY_REWRITE_ERROR_COUNTER_TOTAL,
3445                                encap_pkts);
3446   vlib_node_increment_counter (vm, sr_policy_rewrite_encaps_node.index,
3447                                SR_POLICY_REWRITE_ERROR_COUNTER_BSID,
3448                                bsid_pkts);
3449
3450   return from_frame->n_vectors;
3451 }
3452
3453 /* *INDENT-OFF* */
3454 VLIB_REGISTER_NODE (sr_policy_rewrite_b_encaps_node) = {
3455   .function = sr_policy_rewrite_b_encaps,
3456   .name = "sr-pl-rewrite-b-encaps",
3457   .vector_size = sizeof (u32),
3458   .format_trace = format_sr_policy_rewrite_trace,
3459   .type = VLIB_NODE_TYPE_INTERNAL,
3460   .n_errors = SR_POLICY_REWRITE_N_ERROR,
3461   .error_strings = sr_policy_rewrite_error_strings,
3462   .n_next_nodes = SR_POLICY_REWRITE_N_NEXT,
3463   .next_nodes = {
3464 #define _(s,n) [SR_POLICY_REWRITE_NEXT_##s] = n,
3465     foreach_sr_policy_rewrite_next
3466 #undef _
3467   },
3468 };
3469 /* *INDENT-ON* */
3470
3471 /*************************** SR Policy plugins ******************************/
3472 /**
3473  * @brief SR Policy plugin registry
3474  */
3475 int
3476 sr_policy_register_function (vlib_main_t * vm, u8 * fn_name,
3477                              u8 * keyword_str, u8 * def_str,
3478                              u8 * params_str, u8 prefix_length,
3479                              dpo_type_t * dpo,
3480                              format_function_t * ls_format,
3481                              unformat_function_t * ls_unformat,
3482                              sr_p_plugin_callback_t * creation_fn,
3483                              sr_p_plugin_callback_t * removal_fn)
3484 {
3485   ip6_sr_main_t *sm = &sr_main;
3486   uword *p;
3487
3488   sr_policy_fn_registration_t *plugin;
3489
3490   /* Did this function exist? If so update it */
3491   p = hash_get_mem (sm->policy_plugin_functions_by_key, fn_name);
3492   if (p)
3493     {
3494       plugin = pool_elt_at_index (sm->policy_plugin_functions, p[0]);
3495     }
3496   /* Else create a new one and set hash key */
3497   else
3498     {
3499       pool_get (sm->policy_plugin_functions, plugin);
3500       hash_set_mem (sm->policy_plugin_functions_by_key, fn_name,
3501                     plugin - sm->policy_plugin_functions);
3502     }
3503
3504   clib_memset (plugin, 0, sizeof (*plugin));
3505
3506   plugin->sr_policy_function_number = (plugin - sm->policy_plugin_functions);
3507   plugin->sr_policy_function_number += SR_BEHAVIOR_LAST;
3508   plugin->prefix_length = prefix_length;
3509   plugin->ls_format = ls_format;
3510   plugin->ls_unformat = ls_unformat;
3511   plugin->creation = creation_fn;
3512   plugin->removal = removal_fn;
3513   clib_memcpy (&plugin->dpo, dpo, sizeof (dpo_type_t));
3514   plugin->function_name = format (0, "%s%c", fn_name, 0);
3515   plugin->keyword_str = format (0, "%s%c", keyword_str, 0);
3516   plugin->def_str = format (0, "%s%c", def_str, 0);
3517   plugin->params_str = format (0, "%s%c", params_str, 0);
3518
3519   return plugin->sr_policy_function_number;
3520 }
3521
3522 /**
3523  * @brief CLI function to 'show' all available SR LocalSID behaviors
3524  */
3525 static clib_error_t *
3526 show_sr_policy_behaviors_command_fn (vlib_main_t * vm,
3527                                      unformat_input_t * input,
3528                                      vlib_cli_command_t * cmd)
3529 {
3530   ip6_sr_main_t *sm = &sr_main;
3531   sr_policy_fn_registration_t *plugin;
3532   sr_policy_fn_registration_t **plugins_vec = 0;
3533   int i;
3534
3535   vlib_cli_output (vm, "SR Policy behaviors:\n-----------------------\n\n");
3536
3537   /* *INDENT-OFF* */
3538   pool_foreach (plugin, sm->policy_plugin_functions)
3539      { vec_add1 (plugins_vec, plugin); }
3540   /* *INDENT-ON* */
3541
3542   vlib_cli_output (vm, "Plugin behaviors:\n");
3543   for (i = 0; i < vec_len (plugins_vec); i++)
3544     {
3545       plugin = plugins_vec[i];
3546       vlib_cli_output (vm, "\t%s\t-> %s.\n", plugin->keyword_str,
3547                        plugin->def_str);
3548       vlib_cli_output (vm, "\t\tParameters: '%s'\n", plugin->params_str);
3549     }
3550   return 0;
3551 }
3552
3553 /* *INDENT-OFF* */
3554 VLIB_CLI_COMMAND (show_sr_policy_behaviors_command, static) = {
3555   .path = "show sr policy behaviors",
3556   .short_help = "show sr policy behaviors",
3557   .function = show_sr_policy_behaviors_command_fn,
3558 };
3559 /* *INDENT-ON* */
3560
3561 /*************************** SR Segment Lists DPOs ****************************/
3562 static u8 *
3563 format_sr_segment_list_dpo (u8 * s, va_list * args)
3564 {
3565   ip6_sr_main_t *sm = &sr_main;
3566   ip6_address_t *addr;
3567   ip6_sr_sl_t *sl;
3568
3569   index_t index = va_arg (*args, index_t);
3570   CLIB_UNUSED (u32 indent) = va_arg (*args, u32);
3571   s = format (s, "SR: Segment List index:[%d]", index);
3572   s = format (s, "\n\tSegments:");
3573
3574   sl = pool_elt_at_index (sm->sid_lists, index);
3575
3576   s = format (s, "< ");
3577   vec_foreach (addr, sl->segments)
3578   {
3579     s = format (s, "%U, ", format_ip6_address, addr);
3580   }
3581   s = format (s, "\b\b > - ");
3582   s = format (s, "Weight: %u", sl->weight);
3583
3584   return s;
3585 }
3586
3587 const static dpo_vft_t sr_policy_rewrite_vft = {
3588   .dv_lock = sr_dpo_lock,
3589   .dv_unlock = sr_dpo_unlock,
3590   .dv_format = format_sr_segment_list_dpo,
3591 };
3592
3593 const static char *const sr_pr_encaps_ip6_nodes[] = {
3594   "sr-pl-rewrite-encaps",
3595   NULL,
3596 };
3597
3598 const static char *const sr_pr_encaps_ip4_nodes[] = {
3599   "sr-pl-rewrite-encaps-v4",
3600   NULL,
3601 };
3602
3603 const static char *const *const sr_pr_encaps_nodes[DPO_PROTO_NUM] = {
3604   [DPO_PROTO_IP6] = sr_pr_encaps_ip6_nodes,
3605   [DPO_PROTO_IP4] = sr_pr_encaps_ip4_nodes,
3606 };
3607
3608 const static char *const sr_pr_insert_ip6_nodes[] = {
3609   "sr-pl-rewrite-insert",
3610   NULL,
3611 };
3612
3613 const static char *const *const sr_pr_insert_nodes[DPO_PROTO_NUM] = {
3614   [DPO_PROTO_IP6] = sr_pr_insert_ip6_nodes,
3615 };
3616
3617 const static char *const sr_pr_bsid_insert_ip6_nodes[] = {
3618   "sr-pl-rewrite-b-insert",
3619   NULL,
3620 };
3621
3622 const static char *const *const sr_pr_bsid_insert_nodes[DPO_PROTO_NUM] = {
3623   [DPO_PROTO_IP6] = sr_pr_bsid_insert_ip6_nodes,
3624 };
3625
3626 const static char *const sr_pr_bsid_encaps_ip6_nodes[] = {
3627   "sr-pl-rewrite-b-encaps",
3628   NULL,
3629 };
3630
3631 const static char *const *const sr_pr_bsid_encaps_nodes[DPO_PROTO_NUM] = {
3632   [DPO_PROTO_IP6] = sr_pr_bsid_encaps_ip6_nodes,
3633 };
3634
3635 /********************* SR Policy Rewrite initialization ***********************/
3636 /**
3637  * @brief SR Policy Rewrite initialization
3638  */
3639 clib_error_t *
3640 sr_policy_rewrite_init (vlib_main_t * vm)
3641 {
3642   ip6_sr_main_t *sm = &sr_main;
3643
3644   /* Init memory for sr policy keys (bsid <-> ip6_address_t) */
3645   mhash_init (&sm->sr_policies_index_hash, sizeof (uword),
3646               sizeof (ip6_address_t));
3647
3648   /* Init SR VPO DPOs type */
3649   sr_pr_encaps_dpo_type =
3650     dpo_register_new_type (&sr_policy_rewrite_vft, sr_pr_encaps_nodes);
3651
3652   sr_pr_insert_dpo_type =
3653     dpo_register_new_type (&sr_policy_rewrite_vft, sr_pr_insert_nodes);
3654
3655   sr_pr_bsid_encaps_dpo_type =
3656     dpo_register_new_type (&sr_policy_rewrite_vft, sr_pr_bsid_encaps_nodes);
3657
3658   sr_pr_bsid_insert_dpo_type =
3659     dpo_register_new_type (&sr_policy_rewrite_vft, sr_pr_bsid_insert_nodes);
3660
3661   /* Register the L2 encaps node used in HW redirect */
3662   sm->l2_sr_policy_rewrite_index = sr_policy_rewrite_encaps_node.index;
3663
3664   sm->fib_table_ip6 = (u32) ~ 0;
3665   sm->fib_table_ip4 = (u32) ~ 0;
3666
3667   return 0;
3668 }
3669
3670 VLIB_INIT_FUNCTION (sr_policy_rewrite_init);
3671
3672
3673 /*
3674 * fd.io coding-style-patch-verification: ON
3675 *
3676 * Local Variables:
3677 * eval: (c-set-style "gnu")
3678 * End:
3679 */