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