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