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