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