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