src/vnet/srv6/sr_policy_rewrite.c

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