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