vnet/vnet/sr/sr.c

   1 /*
   2  * sr.c: ipv6 segment routing
   3  *
   4  * Copyright (c) 2013 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 #include <vnet/vnet.h>
  19 #include <vnet/sr/sr.h>
  20
  21 #include <openssl/hmac.h>
  22
  23 ip6_sr_main_t sr_main;
  24 static vlib_node_registration_t sr_local_node;
  25
  26 static void sr_fix_hmac (ip6_sr_main_t * sm, ip6_header_t * ip,
  27                          ip6_sr_header_t * sr)
  28 {
  29   u32 key_index;
  30   static u8 * keybuf;
  31   u8 * copy_target;
  32   int first_segment;
  33   ip6_address_t *addrp;
  34   int i;
  35   ip6_sr_hmac_key_t * hmac_key;
  36   u32 sig_len;
  37
  38   key_index = sr->hmac_key;
  39
  40   /* No signature? Pass... */
  41   if (key_index == 0)
  42     return;
  43
  44   /* We don't know about this key? Fail... */
  45   if (key_index >= vec_len (sm->hmac_keys))
  46     return;
  47
  48   hmac_key = sm->hmac_keys + key_index;
  49
  50   vec_reset_length (keybuf);
  51
  52   /* pkt ip6 src address */
  53   vec_add2 (keybuf, copy_target, sizeof (ip6_address_t));
  54   clib_memcpy (copy_target, ip->src_address.as_u8, sizeof (ip6_address_t));
  55
  56   /* first segment */
  57   vec_add2 (keybuf, copy_target, 1);
  58   copy_target[0] = sr->first_segment;
  59
  60   /* octet w/ bit 0 = "clean" flag */
  61   vec_add2 (keybuf, copy_target, 1);
  62   copy_target[0]
  63     = (sr->flags & clib_host_to_net_u16 (IP6_SR_HEADER_FLAG_CLEANUP))
  64     ? 0x80 : 0;
  65
  66   /* hmac key id */
  67   vec_add2 (keybuf, copy_target, 1);
  68   copy_target[0] = sr->hmac_key;
  69
  70   first_segment = sr->first_segment;
  71
  72   addrp = sr->segments;
  73
  74   /* segments */
  75   for (i = 0; i <= first_segment; i++)
  76     {
  77       vec_add2 (keybuf, copy_target, sizeof (ip6_address_t));
  78       clib_memcpy (copy_target, addrp->as_u8, sizeof (ip6_address_t));
  79       addrp++;
  80     }
  81
  82   addrp++;
  83
  84   HMAC_CTX_init(sm->hmac_ctx);
  85   if (!HMAC_Init(sm->hmac_ctx, hmac_key->shared_secret,
  86                  vec_len(hmac_key->shared_secret),sm->md))
  87       clib_warning ("barf1");
  88   if (!HMAC_Update(sm->hmac_ctx,keybuf,vec_len(keybuf)))
  89       clib_warning ("barf2");
  90   if (!HMAC_Final(sm->hmac_ctx, (unsigned char *) addrp, &sig_len))
  91       clib_warning ("barf3");
  92   HMAC_CTX_cleanup(sm->hmac_ctx);
  93 }
  94
  95 u8 * format_ip6_sr_header_flags (u8 * s, va_list * args)
  96 {
  97   u16 flags = (u16) va_arg (*args, int);
  98   u8 pl_flag;
  99   int bswap_needed = va_arg (*args, int);
 100   int i;
 101
 102   if (bswap_needed)
 103       flags = clib_host_to_net_u16 (flags);
 104
 105   if (flags & IP6_SR_HEADER_FLAG_CLEANUP)
 106       s = format (s, "cleanup ");
 107
 108   if (flags & IP6_SR_HEADER_FLAG_PROTECTED)
 109       s = format (s, "reroute ");
 110
 111   s = format (s, "pl: ");
 112   for (i = 1; i <= 4; i++)
 113     {
 114       pl_flag = ip6_sr_policy_list_flags (flags, i);
 115       s = format (s, "[%d] ", i);
 116
 117       switch (pl_flag)
 118         {
 119         case IP6_SR_HEADER_FLAG_PL_ELT_NOT_PRESENT:
 120           s = format (s, "NotPr ");
 121           break;
 122         case IP6_SR_HEADER_FLAG_PL_ELT_INGRESS_PE:
 123           s = format (s, "InPE ");
 124           break;
 125         case IP6_SR_HEADER_FLAG_PL_ELT_EGRESS_PE:
 126           s = format (s, "EgPE ");
 127           break;
 128
 129         case IP6_SR_HEADER_FLAG_PL_ELT_ORIG_SRC_ADDR:
 130           s = format (s, "OrgSrc ");
 131           break;
 132         }
 133     }
 134   return s;
 135 }
 136
 137 u8 * format_ip6_sr_header (u8 * s, va_list * args)
 138 {
 139   ip6_sr_header_t * h = va_arg (*args, ip6_sr_header_t *);
 140   int print_hmac = va_arg (*args, int);
 141   int i, pl_index, max_segs;
 142   int flags_host_byte_order = clib_net_to_host_u16(h->flags);
 143
 144   s = format (s, "next proto %d, len %d, type %d",
 145               h->protocol, (h->length<<3)+8, h->type);
 146   s = format (s, "\n      segs left %d, first_segment %d, hmac key %d",
 147               h->segments_left, h->first_segment, h->hmac_key);
 148   s = format (s, "\n      flags %U", format_ip6_sr_header_flags,
 149               flags_host_byte_order, 0 /* bswap needed */ );
 150
 151   /*
 152    * Header length is in 8-byte units (minus one), so
 153    * divide by 2 to ascertain the number of ip6 addresses in the
 154    * segment list
 155    */
 156   max_segs = (h->length>>1);
 157
 158   if (!print_hmac && h->hmac_key)
 159     max_segs -= 2;
 160
 161   s = format (s, "\n  Segments (in processing order):");
 162
 163   for (i = h->first_segment; i >= 0; i--)
 164     s = format (s, "\n  %U", format_ip6_address, h->segments + i);
 165
 166   s = format (s, "\n  Policy List:");
 167
 168   pl_index = 1;                 /* to match the RFC text */
 169   for (i = (h->first_segment+1); i < max_segs; i++, pl_index++)
 170     {
 171       char * tag;
 172       char * tags[] = {" ", "InPE: ", "EgPE: ", "OrgSrc: "};
 173
 174       tag = tags[0];
 175       if (pl_index >=1 && pl_index <= 4)
 176         {
 177           int this_pl_flag = ip6_sr_policy_list_flags
 178             (flags_host_byte_order, pl_index);
 179           tag = tags[this_pl_flag];
 180         }
 181
 182       s = format (s, "\n  %s%U", tag, format_ip6_address, h->segments + i);
 183     }
 184
 185   return s;
 186 }
 187
 188 u8 * format_ip6_sr_header_with_length (u8 * s, va_list * args)
 189 {
 190   ip6_header_t * h = va_arg (*args, ip6_header_t *);
 191   u32 max_header_bytes = va_arg (*args, u32);
 192   uword header_bytes;
 193
 194   header_bytes = sizeof (h[0]) + sizeof (ip6_sr_header_t);
 195   if (max_header_bytes != 0 && header_bytes > max_header_bytes)
 196     return format (s, "ip6_sr header truncated");
 197
 198   s = format (s, "IP6: %U\n", format_ip6_header, h, max_header_bytes);
 199   s = format (s, "SR: %U\n", format_ip6_sr_header, (ip6_sr_header_t *)(h+1),
 200               0 /* print_hmac */, max_header_bytes);
 201   return s;
 202 }
 203
 204 #define foreach_sr_rewrite_next                 \
 205 _(ERROR, "error-drop")                          \
 206 _(IP6_LOOKUP, "ip6-lookup")                     \
 207 _(SR_LOCAL, "sr-local")
 208
 209 typedef enum {
 210 #define _(s,n) SR_REWRITE_NEXT_##s,
 211   foreach_sr_rewrite_next
 212 #undef _
 213   SR_REWRITE_N_NEXT,
 214 } sr_rewrite_next_t;
 215
 216 typedef struct {
 217   ip6_address_t src, dst;
 218   u16 length;
 219   u32 next_index;
 220   u32 tunnel_index;
 221   u8 sr[256];
 222 } sr_rewrite_trace_t;
 223
 224 static char * sr_rewrite_error_strings[] = {
 225 #define sr_error(n,s) s,
 226 #include "sr_error.def"
 227 #undef sr_error
 228 };
 229
 230 typedef enum {
 231 #define sr_error(n,s) SR_REWRITE_ERROR_##n,
 232 #include "sr_error.def"
 233 #undef sr_error
 234   SR_REWRITE_N_ERROR,
 235 } sr_rewrite_error_t;
 236
 237
 238 u8 * format_sr_rewrite_trace (u8 * s, va_list * args)
 239 {
 240   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
 241   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
 242   sr_rewrite_trace_t * t = va_arg (*args, sr_rewrite_trace_t *);
 243   ip6_main_t * im = &ip6_main;
 244   ip6_sr_main_t * sm = &sr_main;
 245   ip6_sr_tunnel_t *tun = pool_elt_at_index (sm->tunnels, t->tunnel_index);
 246   ip6_fib_t * rx_fib, * tx_fib;
 247
 248   rx_fib = find_ip6_fib_by_table_index_or_id (im, tun->rx_fib_index,
 249                                               IP6_ROUTE_FLAG_FIB_INDEX);
 250
 251   tx_fib = find_ip6_fib_by_table_index_or_id (im, tun->tx_fib_index,
 252                                               IP6_ROUTE_FLAG_FIB_INDEX);
 253
 254   s = format
 255     (s, "SR-REWRITE: next %s ip6 src %U dst %U len %u\n"
 256      "           rx-fib-id %d tx-fib-id %d\n%U",
 257      (t->next_index == SR_REWRITE_NEXT_SR_LOCAL)
 258      ? "sr-local" : "ip6-lookup",
 259      format_ip6_address, &t->src,
 260      format_ip6_address, &t->dst, t->length,
 261      rx_fib->table_id, tx_fib->table_id,
 262      format_ip6_sr_header, t->sr, 0 /* print_hmac */);
 263   return s;
 264 }
 265
 266 static uword
 267 sr_rewrite (vlib_main_t * vm,
 268                    vlib_node_runtime_t * node,
 269                    vlib_frame_t * from_frame)
 270 {
 271   u32 n_left_from, next_index, * from, * to_next;
 272   ip6_main_t * im = &ip6_main;
 273   ip_lookup_main_t * lm = &im->lookup_main;
 274   ip6_sr_main_t * sm = &sr_main;
 275   u32 (*sr_local_cb) (vlib_main_t *, vlib_node_runtime_t *,
 276                       vlib_buffer_t *, ip6_header_t *,
 277                       ip6_sr_header_t *);
 278   sr_local_cb = sm->sr_local_cb;
 279
 280   from = vlib_frame_vector_args (from_frame);
 281   n_left_from = from_frame->n_vectors;
 282
 283   next_index = node->cached_next_index;
 284
 285   while (n_left_from > 0)
 286     {
 287       u32 n_left_to_next;
 288
 289       vlib_get_next_frame (vm, node, next_index,
 290                            to_next, n_left_to_next);
 291
 292       while (n_left_from >= 4 && n_left_to_next >= 2)
 293         {
 294           u32 bi0, bi1;
 295           vlib_buffer_t * b0, * b1;
 296           ip6_header_t * ip0, * ip1;
 297           ip_adjacency_t * adj0, * adj1;
 298           ip6_sr_header_t * sr0, * sr1;
 299           ip6_sr_tunnel_t * t0, *t1;
 300           u64 * copy_src0, * copy_dst0;
 301           u64 * copy_src1, * copy_dst1;
 302           u32 next0 = SR_REWRITE_NEXT_IP6_LOOKUP;
 303           u32 next1 = SR_REWRITE_NEXT_IP6_LOOKUP;
 304           u16 new_l0, new_l1;
 305
 306           /* Prefetch next iteration. */
 307           {
 308             vlib_buffer_t * p2, * p3;
 309
 310             p2 = vlib_get_buffer (vm, from[2]);
 311             p3 = vlib_get_buffer (vm, from[3]);
 312
 313             vlib_prefetch_buffer_header (p2, LOAD);
 314             vlib_prefetch_buffer_header (p3, LOAD);
 315           }
 316
 317           bi0 = from[0];
 318           bi1 = from[1];
 319           to_next[0] = bi0;
 320           to_next[1] = bi1;
 321           from += 2;
 322           to_next += 2;
 323           n_left_to_next -= 2;
 324           n_left_from -= 2;
 325
 326           b0 = vlib_get_buffer (vm, bi0);
 327           b1 = vlib_get_buffer (vm, bi1);
 328
 329           /*
 330            * $$$ parse through header(s) to pick the point
 331            * where we punch in the SR extention header
 332            */
 333
 334           adj0 = ip_get_adjacency (lm, vnet_buffer(b0)->ip.adj_index[VLIB_TX]);
 335           adj1 = ip_get_adjacency (lm, vnet_buffer(b1)->ip.adj_index[VLIB_TX]);
 336           t0 = pool_elt_at_index (sm->tunnels,
 337                                   adj0->rewrite_header.sw_if_index);
 338           t1 = pool_elt_at_index (sm->tunnels,
 339                                   adj1->rewrite_header.sw_if_index);
 340
 341           ASSERT (VLIB_BUFFER_PRE_DATA_SIZE
 342                   >= ((word) vec_len (t0->rewrite)) + b0->current_data);
 343           ASSERT (VLIB_BUFFER_PRE_DATA_SIZE
 344                   >= ((word) vec_len (t1->rewrite)) + b1->current_data);
 345
 346           vnet_buffer(b0)->sw_if_index[VLIB_TX] = t0->tx_fib_index;
 347           vnet_buffer(b1)->sw_if_index[VLIB_TX] = t1->tx_fib_index;
 348
 349           ip0 = vlib_buffer_get_current (b0);
 350           ip1 = vlib_buffer_get_current (b1);
 351
 352           /*
 353            * SR-unaware service chaining case: pkt coming back from
 354            * service has the original dst address, and will already
 355            * have an SR header. If so, send it to sr-local
 356            */
 357           if (PREDICT_FALSE(ip0->protocol == 43))
 358             {
 359               vlib_buffer_advance (b0, sizeof(ip0));
 360               sr0 = (ip6_sr_header_t *) (ip0+1);
 361               new_l0 = clib_net_to_host_u16(ip0->payload_length);
 362               next0 = SR_REWRITE_NEXT_SR_LOCAL;
 363             }
 364           else
 365             {
 366               copy_dst0 = (u64 *)(((u8 *)ip0) - vec_len (t0->rewrite));
 367               copy_src0 = (u64 *) ip0;
 368
 369               /*
 370                * Copy data before the punch-in point left by the
 371                * required amount. Assume (for the moment) that only
 372                * the main packet header needs to be copied.
 373                */
 374               copy_dst0 [0] = copy_src0 [0];
 375               copy_dst0 [1] = copy_src0 [1];
 376               copy_dst0 [2] = copy_src0 [2];
 377               copy_dst0 [3] = copy_src0 [3];
 378               copy_dst0 [4] = copy_src0 [4];
 379               vlib_buffer_advance (b0, - (word) vec_len(t0->rewrite));
 380               ip0 = vlib_buffer_get_current (b0);
 381               sr0 = (ip6_sr_header_t *) (ip0+1);
 382               /* $$$ tune */
 383               clib_memcpy (sr0, t0->rewrite, vec_len (t0->rewrite));
 384               /* Fix the next header chain */
 385               sr0->protocol = ip0->protocol;
 386               ip0->protocol = 43; /* routing extension header */
 387               new_l0 = clib_net_to_host_u16(ip0->payload_length) +
 388                 vec_len (t0->rewrite);
 389               ip0->payload_length = clib_host_to_net_u16(new_l0);
 390               /* Rewrite the ip6 dst address */
 391               ip0->dst_address.as_u64[0] = t0->first_hop.as_u64[0];
 392               ip0->dst_address.as_u64[1] = t0->first_hop.as_u64[1];
 393
 394               sr_fix_hmac (sm, ip0, sr0);
 395
 396               next0 = sr_local_cb ? sr_local_cb (vm, node, b0, ip0, sr0) :
 397                   next0;
 398
 399               /*
 400                * Ignore "do not rewrite" shtik in this path
 401                */
 402               if (PREDICT_FALSE (next0 & 0x80000000))
 403               {
 404                   next0 ^= 0xFFFFFFFF;
 405                   if (PREDICT_FALSE(next0 == SR_REWRITE_NEXT_ERROR))
 406                       b0->error =
 407                           node->errors[SR_REWRITE_ERROR_APP_CALLBACK];
 408               }
 409             }
 410
 411           if (PREDICT_FALSE(ip1->protocol == 43))
 412             {
 413               vlib_buffer_advance (b1, sizeof(ip1));
 414               sr1 = (ip6_sr_header_t *) (ip1+1);
 415               new_l1 = clib_net_to_host_u16(ip1->payload_length);
 416               next1 = SR_REWRITE_NEXT_SR_LOCAL;
 417             }
 418           else
 419             {
 420               copy_dst1 = (u64 *)(((u8 *)ip1) - vec_len (t1->rewrite));
 421               copy_src1 = (u64 *) ip1;
 422
 423               copy_dst1 [0] = copy_src1 [0];
 424               copy_dst1 [1] = copy_src1 [1];
 425               copy_dst1 [2] = copy_src1 [2];
 426               copy_dst1 [3] = copy_src1 [3];
 427               copy_dst1 [4] = copy_src1 [4];
 428               vlib_buffer_advance (b1, - (word) vec_len(t1->rewrite));
 429               ip1 = vlib_buffer_get_current (b1);
 430               sr1 = (ip6_sr_header_t *) (ip1+1);
 431               clib_memcpy (sr1, t1->rewrite, vec_len (t1->rewrite));
 432               sr1->protocol = ip1->protocol;
 433               ip1->protocol = 43;
 434               new_l1 = clib_net_to_host_u16(ip1->payload_length) +
 435                 vec_len (t1->rewrite);
 436               ip1->payload_length = clib_host_to_net_u16(new_l1);
 437               ip1->dst_address.as_u64[0] = t1->first_hop.as_u64[0];
 438               ip1->dst_address.as_u64[1] = t1->first_hop.as_u64[1];
 439
 440               sr_fix_hmac (sm, ip1, sr1);
 441
 442               next1 = sr_local_cb ? sr_local_cb (vm, node, b1, ip1, sr1) :
 443                   next1;
 444
 445               /*
 446                * Ignore "do not rewrite" shtik in this path
 447                */
 448               if (PREDICT_FALSE (next1 & 0x80000000))
 449               {
 450                   next1 ^= 0xFFFFFFFF;
 451                   if (PREDICT_FALSE(next1 == SR_REWRITE_NEXT_ERROR))
 452                       b1->error =
 453                           node->errors[SR_REWRITE_ERROR_APP_CALLBACK];
 454               }
 455             }
 456
 457           if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED))
 458             {
 459               sr_rewrite_trace_t *tr = vlib_add_trace (vm, node,
 460                                                        b0, sizeof (*tr));
 461               tr->tunnel_index = t0 - sm->tunnels;
 462               clib_memcpy (tr->src.as_u8, ip0->src_address.as_u8,
 463                       sizeof (tr->src.as_u8));
 464               clib_memcpy (tr->dst.as_u8, ip0->dst_address.as_u8,
 465                       sizeof (tr->dst.as_u8));
 466               tr->length = new_l0;
 467               tr->next_index = next0;
 468               clib_memcpy (tr->sr, sr0, sizeof (tr->sr));
 469             }
 470           if (PREDICT_FALSE(b1->flags & VLIB_BUFFER_IS_TRACED))
 471             {
 472               sr_rewrite_trace_t *tr = vlib_add_trace (vm, node,
 473                                                        b1, sizeof (*tr));
 474               tr->tunnel_index = t1 - sm->tunnels;
 475               clib_memcpy (tr->src.as_u8, ip1->src_address.as_u8,
 476                       sizeof (tr->src.as_u8));
 477               clib_memcpy (tr->dst.as_u8, ip1->dst_address.as_u8,
 478                       sizeof (tr->dst.as_u8));
 479               tr->length = new_l1;
 480               tr->next_index = next1;
 481               clib_memcpy (tr->sr, sr1, sizeof (tr->sr));
 482             }
 483
 484           vlib_validate_buffer_enqueue_x2 (vm, node, next_index,
 485                                            to_next, n_left_to_next,
 486                                            bi0, bi1, next0, next1);
 487         }
 488
 489       while (n_left_from > 0 && n_left_to_next > 0)
 490         {
 491           u32 bi0;
 492           vlib_buffer_t * b0;
 493           ip6_header_t * ip0;
 494           ip_adjacency_t * adj0;
 495           ip6_sr_header_t * sr0;
 496           ip6_sr_tunnel_t * t0;
 497           u64 * copy_src0, * copy_dst0;
 498           u32 next0 = SR_REWRITE_NEXT_IP6_LOOKUP;
 499           u16 new_l0;
 500
 501           bi0 = from[0];
 502           to_next[0] = bi0;
 503           from += 1;
 504           to_next += 1;
 505           n_left_from -= 1;
 506           n_left_to_next -= 1;
 507
 508           b0 = vlib_get_buffer (vm, bi0);
 509
 510           /*
 511            * $$$ parse through header(s) to pick the point
 512            * where we punch in the SR extention header
 513            */
 514
 515           adj0 = ip_get_adjacency (lm, vnet_buffer(b0)->ip.adj_index[VLIB_TX]);
 516           t0 = pool_elt_at_index (sm->tunnels,
 517                                   adj0->rewrite_header.sw_if_index);
 518
 519           ASSERT (VLIB_BUFFER_PRE_DATA_SIZE
 520                   >= ((word) vec_len (t0->rewrite)) + b0->current_data);
 521
 522           vnet_buffer(b0)->sw_if_index[VLIB_TX] = t0->tx_fib_index;
 523
 524           ip0 = vlib_buffer_get_current (b0);
 525
 526           /*
 527            * SR-unaware service chaining case: pkt coming back from
 528            * service has the original dst address, and will already
 529            * have an SR header. If so, send it to sr-local
 530            */
 531           if (PREDICT_FALSE(ip0->protocol == 43))
 532             {
 533               vlib_buffer_advance (b0, sizeof(ip0));
 534               sr0 = (ip6_sr_header_t *) (ip0+1);
 535               new_l0 = clib_net_to_host_u16(ip0->payload_length);
 536               next0 = SR_REWRITE_NEXT_SR_LOCAL;
 537             }
 538           else
 539             {
 540               copy_dst0 = (u64 *)(((u8 *)ip0) - vec_len (t0->rewrite));
 541               copy_src0 = (u64 *) ip0;
 542
 543               /*
 544                * Copy data before the punch-in point left by the
 545                * required amount. Assume (for the moment) that only
 546                * the main packet header needs to be copied.
 547                */
 548               copy_dst0 [0] = copy_src0 [0];
 549               copy_dst0 [1] = copy_src0 [1];
 550               copy_dst0 [2] = copy_src0 [2];
 551               copy_dst0 [3] = copy_src0 [3];
 552               copy_dst0 [4] = copy_src0 [4];
 553               vlib_buffer_advance (b0, - (word) vec_len(t0->rewrite));
 554               ip0 = vlib_buffer_get_current (b0);
 555               sr0 = (ip6_sr_header_t *) (ip0+1);
 556               /* $$$ tune */
 557               clib_memcpy (sr0, t0->rewrite, vec_len (t0->rewrite));
 558               /* Fix the next header chain */
 559               sr0->protocol = ip0->protocol;
 560               ip0->protocol = 43; /* routing extension header */
 561               new_l0 = clib_net_to_host_u16(ip0->payload_length) +
 562                 vec_len (t0->rewrite);
 563               ip0->payload_length = clib_host_to_net_u16(new_l0);
 564               /* Rewrite the ip6 dst address */
 565               ip0->dst_address.as_u64[0] = t0->first_hop.as_u64[0];
 566               ip0->dst_address.as_u64[1] = t0->first_hop.as_u64[1];
 567
 568               sr_fix_hmac (sm, ip0, sr0);
 569
 570               next0 = sr_local_cb ? sr_local_cb (vm, node, b0, ip0, sr0) :
 571                   next0;
 572
 573               /*
 574                * Ignore "do not rewrite" shtik in this path
 575                */
 576               if (PREDICT_FALSE (next0 & 0x80000000))
 577               {
 578                   next0 ^= 0xFFFFFFFF;
 579                   if (PREDICT_FALSE(next0 == SR_REWRITE_NEXT_ERROR))
 580                       b0->error =
 581                           node->errors[SR_REWRITE_ERROR_APP_CALLBACK];
 582               }
 583             }
 584
 585           if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED))
 586             {
 587               sr_rewrite_trace_t *tr = vlib_add_trace (vm, node,
 588                                                        b0, sizeof (*tr));
 589               tr->tunnel_index = t0 - sm->tunnels;
 590               clib_memcpy (tr->src.as_u8, ip0->src_address.as_u8,
 591                       sizeof (tr->src.as_u8));
 592               clib_memcpy (tr->dst.as_u8, ip0->dst_address.as_u8,
 593                       sizeof (tr->dst.as_u8));
 594               tr->length = new_l0;
 595               tr->next_index = next0;
 596               clib_memcpy (tr->sr, sr0, sizeof (tr->sr));
 597             }
 598
 599           vlib_validate_buffer_enqueue_x1 (vm, node, next_index,
 600                                            to_next, n_left_to_next,
 601                                            bi0, next0);
 602         }
 603
 604       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
 605     }
 606   return from_frame->n_vectors;
 607 }
 608
 609 VLIB_REGISTER_NODE (sr_rewrite_node) = {
 610   .function = sr_rewrite,
 611   .name = "sr-rewrite",
 612   /* Takes a vector of packets. */
 613   .vector_size = sizeof (u32),
 614   .format_trace = format_sr_rewrite_trace,
 615   .format_buffer = format_ip6_sr_header_with_length,
 616
 617   .n_errors = SR_REWRITE_N_ERROR,
 618   .error_strings = sr_rewrite_error_strings,
 619
 620   .runtime_data_bytes = 0,
 621
 622   .n_next_nodes = SR_REWRITE_N_NEXT,
 623   .next_nodes = {
 624 #define _(s,n) [SR_REWRITE_NEXT_##s] = n,
 625     foreach_sr_rewrite_next
 626 #undef _
 627   },
 628 };
 629
 630 static int ip6_delete_route_no_next_hop (ip6_address_t *dst_address_arg,
 631                                          u32 dst_address_length,
 632                                          u32 rx_table_id)
 633 {
 634   ip6_add_del_route_args_t a;
 635   ip6_address_t dst_address;
 636   ip6_fib_t * fib;
 637   ip6_main_t * im6 = &ip6_main;
 638   BVT(clib_bihash_kv) kv, value;
 639
 640   fib = find_ip6_fib_by_table_index_or_id (im6, rx_table_id,
 641                                            IP6_ROUTE_FLAG_TABLE_ID);
 642   memset (&a, 0, sizeof (a));
 643   a.flags |= IP4_ROUTE_FLAG_DEL;
 644   a.dst_address_length = dst_address_length;
 645
 646   dst_address = *dst_address_arg;
 647
 648   ip6_address_mask (&dst_address,
 649                     &im6->fib_masks[dst_address_length]);
 650
 651   kv.key[0] = dst_address.as_u64[0];
 652   kv.key[1] = dst_address.as_u64[1];
 653   kv.key[2] = ((u64)((fib - im6->fibs))<<32) | dst_address_length;
 654
 655   if (BV(clib_bihash_search)(&im6->ip6_lookup_table, &kv, &value) < 0)
 656     {
 657       clib_warning ("%U/%d not in FIB",
 658                     format_ip6_address, &a.dst_address,
 659                     a.dst_address_length);
 660       return -10;
 661     }
 662
 663   a.adj_index = value.value;
 664   a.dst_address = dst_address;
 665
 666   ip6_add_del_route (im6, &a);
 667   ip6_maybe_remap_adjacencies (im6, rx_table_id, IP6_ROUTE_FLAG_TABLE_ID);
 668   return 0;
 669 }
 670
 671 static ip6_sr_hmac_key_t *
 672 find_or_add_shared_secret (ip6_sr_main_t * sm, u8 * secret, u32 * indexp)
 673 {
 674   uword * p;
 675   ip6_sr_hmac_key_t * key = 0;
 676   int i;
 677
 678   p = hash_get_mem (sm->hmac_key_by_shared_secret, secret);
 679
 680   if (p)
 681     {
 682       key = vec_elt_at_index (sm->hmac_keys, p[0]);
 683       if (indexp)
 684         *indexp = p[0];
 685       return (key);
 686     }
 687
 688   /* Specific key ID? */
 689   if (indexp && *indexp)
 690     {
 691       vec_validate (sm->hmac_keys, *indexp);
 692       key = sm->hmac_keys + *indexp;
 693     }
 694   else
 695     {
 696       for (i = 0; i < vec_len (sm->hmac_keys); i++)
 697         {
 698           if (sm->hmac_keys[i].shared_secret == 0)
 699             key = sm->hmac_keys + i;
 700           goto found;
 701         }
 702       vec_validate (sm->hmac_keys, i);
 703       key = sm->hmac_keys + i;
 704     found:
 705       ;
 706     }
 707
 708   key->shared_secret = vec_dup (secret);
 709
 710   hash_set_mem (sm->hmac_key_by_shared_secret, key->shared_secret,
 711                 key - sm->hmac_keys);
 712
 713   if (indexp)
 714     *indexp = key - sm->hmac_keys;
 715   return (key);
 716 }
 717
 718 int ip6_sr_add_del_tunnel (ip6_sr_add_del_tunnel_args_t * a)
 719 {
 720   ip6_main_t * im = &ip6_main;
 721   ip_lookup_main_t * lm = &im->lookup_main;
 722   ip6_sr_tunnel_key_t key;
 723   ip6_sr_tunnel_t * t;
 724   uword * p;
 725   ip6_sr_header_t * h = 0;
 726   u32 header_length;
 727   ip6_address_t * addrp, *this_address;
 728   ip_adjacency_t adj, * ap, * add_adj = 0;
 729   u32 adj_index;
 730   ip6_sr_main_t * sm = &sr_main;
 731   u8 * key_copy;
 732   u32 rx_fib_index, tx_fib_index;
 733   ip6_add_del_route_args_t aa;
 734   u32 hmac_key_index_u32;
 735   u8 hmac_key_index = 0;
 736
 737   /* Make sure that the rx FIB exists */
 738   p = hash_get (im->fib_index_by_table_id, a->rx_table_id);
 739
 740   if (p == 0)
 741     return -3;
 742
 743   /* remember the FIB index */
 744   rx_fib_index = p[0];
 745
 746   /* Make sure that the supplied FIB exists */
 747   p = hash_get (im->fib_index_by_table_id, a->tx_table_id);
 748
 749   if (p == 0)
 750     return -4;
 751
 752   /* remember the FIB index */
 753   tx_fib_index = p[0];
 754
 755   clib_memcpy (key.src.as_u8, a->src_address->as_u8, sizeof (key.src));
 756   clib_memcpy (key.dst.as_u8, a->dst_address->as_u8, sizeof (key.dst));
 757
 758   p = hash_get_mem (sm->tunnel_index_by_key, &key);
 759
 760   if (p)
 761     {
 762       if (a->is_del)
 763         {
 764           hash_pair_t *hp;
 765
 766           /* Delete existing tunnel */
 767           t = pool_elt_at_index (sm->tunnels, p[0]);
 768
 769           ip6_delete_route_no_next_hop (&t->key.dst, t->dst_mask_width,
 770                                         a->rx_table_id);
 771           vec_free (t->rewrite);
 772           pool_put (sm->tunnels, t);
 773           hp = hash_get_pair (sm->tunnel_index_by_key, &key);
 774           key_copy = (void *)(hp->key);
 775           hash_unset_mem (sm->tunnel_index_by_key, &key);
 776           vec_free (key_copy);
 777           return 0;
 778         }
 779       else /* create; tunnel already exists; complain */
 780         return -1;
 781     }
 782   else
 783     {
 784       /* delete; tunnel does not exist; complain */
 785       if (a->is_del)
 786         return -2;
 787     }
 788
 789   /* create a new tunnel */
 790   pool_get (sm->tunnels, t);
 791   memset (t, 0, sizeof (*t));
 792
 793   clib_memcpy (&t->key, &key, sizeof (t->key));
 794   t->dst_mask_width = a->dst_mask_width;
 795   t->rx_fib_index = rx_fib_index;
 796   t->tx_fib_index = tx_fib_index;
 797
 798   /* The first specified hop goes right into the dst address */
 799   if (vec_len(a->segments))
 800     {
 801       t->first_hop = a->segments[0];
 802       /* It won't feel nice if we do it twice */
 803       vec_delete (a->segments, 1, 0);
 804     }
 805   else  /* there must be at least one segment... */
 806       return -4;
 807
 808   /*
 809    * Create the sr header rewrite string
 810    * We append the dst address to the set of next hops
 811    * so the ultimate recipient can tell where the
 812    * packet entered the SR domain
 813    */
 814   header_length = sizeof (*h) +
 815     sizeof (ip6_address_t) * (vec_len (a->segments) + vec_len (a->tags));
 816
 817   if (a->shared_secret)
 818     {
 819       /* Allocate a new key slot if we don't find the secret key */
 820       hmac_key_index_u32 = 0;
 821       (void) find_or_add_shared_secret (sm, a->shared_secret,
 822                                         &hmac_key_index_u32);
 823
 824       /* Hey Vinz Clortho: Gozzer is pissed.. you're out of keys! */
 825       if (hmac_key_index_u32 >= 256)
 826         return -5;
 827       hmac_key_index = hmac_key_index_u32;
 828       header_length += SHA256_DIGEST_LENGTH;
 829     }
 830
 831   vec_validate (t->rewrite, header_length-1);
 832
 833   h = (ip6_sr_header_t *) t->rewrite;
 834
 835   h->protocol = 0xFF; /* we don't know yet */
 836
 837   h->length = (header_length/8) - 1;
 838   h->type = ROUTING_HEADER_TYPE_SR;
 839   h->segments_left = vec_len (a->segments);
 840   h->first_segment = vec_len(a->segments) -1;
 841   if (a->shared_secret)
 842     h->hmac_key = hmac_key_index & 0xFF;
 843
 844   h->flags = a->flags_net_byte_order;
 845
 846   /* Paint on the segment list, in reverse */
 847   addrp = h->segments + (vec_len (a->segments) - 1);
 848
 849   vec_foreach (this_address, a->segments)
 850     {
 851       clib_memcpy (addrp->as_u8, this_address->as_u8, sizeof (ip6_address_t));
 852       addrp--;
 853     }
 854
 855   /* Paint on the tag list, not reversed */
 856   addrp = h->segments + vec_len(a->segments);
 857
 858   vec_foreach (this_address, a->tags)
 859     {
 860       clib_memcpy (addrp->as_u8, this_address->as_u8, sizeof (ip6_address_t));
 861       addrp++;
 862     }
 863
 864   key_copy = vec_new (ip6_sr_tunnel_key_t, 1);
 865   clib_memcpy (key_copy, &key, sizeof (ip6_sr_tunnel_key_t));
 866   hash_set_mem (sm->tunnel_index_by_key, key_copy, t - sm->tunnels);
 867
 868   memset(&adj, 0, sizeof (adj));
 869
 870   /* Create an adjacency and add to v6 fib */
 871   adj.lookup_next_index = IP_LOOKUP_NEXT_REWRITE;
 872   adj.lookup_next_index = sm->ip6_lookup_sr_next_index;
 873   adj.explicit_fib_index = ~0;
 874
 875   ap = ip_add_adjacency (lm, &adj, 1 /* one adj */,
 876                          &adj_index);
 877
 878   /*
 879    * Stick the tunnel index into the rewrite header.
 880    *
 881    * Unfortunately, inserting an SR header according to the various
 882    * RFC's requires parsing through the ip6 header, perhaps consing a
 883    * buffer onto the head of the vlib_buffer_t, etc. We don't use the
 884    * normal reverse bcopy rewrite code.
 885    *
 886    * We don't handle ugly RFC-related cases yet, but I'm sure PL will complain
 887    * at some point...
 888    */
 889   ap->rewrite_header.sw_if_index = t - sm->tunnels;
 890
 891   vec_add1 (add_adj, ap[0]);
 892
 893   clib_memcpy (aa.dst_address.as_u8, a->dst_address, sizeof (aa.dst_address.as_u8));
 894   aa.dst_address_length = a->dst_mask_width;
 895
 896   aa.flags = (a->is_del ? IP6_ROUTE_FLAG_DEL : IP6_ROUTE_FLAG_ADD);
 897   aa.flags |= IP6_ROUTE_FLAG_FIB_INDEX;
 898   aa.table_index_or_table_id = rx_fib_index;
 899   aa.add_adj = add_adj;
 900   aa.adj_index = adj_index;
 901   aa.n_add_adj = 1;
 902   ip6_add_del_route (im, &aa);
 903   vec_free (add_adj);
 904
 905   return 0;
 906 }
 907
 908 static clib_error_t *
 909 sr_add_del_tunnel_command_fn (vlib_main_t * vm,
 910                               unformat_input_t * input,
 911                               vlib_cli_command_t * cmd)
 912 {
 913   int is_del = 0;
 914   ip6_address_t src_address;
 915   int src_address_set = 0;
 916   ip6_address_t dst_address;
 917   u32 dst_mask_width;
 918   int dst_address_set = 0;
 919   u16 flags = 0;
 920   u8 *shared_secret = 0;
 921   u32 rx_table_id = 0;
 922   u32 tx_table_id = 0;
 923   ip6_address_t * segments = 0;
 924   ip6_address_t * this_seg;
 925   ip6_address_t * tags = 0;
 926   ip6_address_t * this_tag;
 927   ip6_sr_add_del_tunnel_args_t _a, *a=&_a;
 928   ip6_address_t next_address, tag;
 929   int pl_index;
 930   int rv;
 931
 932   while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
 933     {
 934       if (unformat (input, "del"))
 935         is_del = 1;
 936       else if (unformat (input, "rx-fib-id %d", &rx_table_id))
 937         ;
 938       else if (unformat (input, "tx-fib-id %d", &tx_table_id))
 939         ;
 940       else if (unformat (input, "src %U", unformat_ip6_address, &src_address))
 941         src_address_set = 1;
 942       else if (unformat (input, "dst %U/%d",
 943                          unformat_ip6_address, &dst_address,
 944                          &dst_mask_width))
 945         dst_address_set = 1;
 946       else if (unformat (input, "next %U", unformat_ip6_address,
 947                          &next_address))
 948         {
 949           vec_add2 (segments, this_seg, 1);
 950           clib_memcpy (this_seg->as_u8, next_address.as_u8, sizeof (*this_seg));
 951         }
 952       else if (unformat (input, "tag %U", unformat_ip6_address,
 953                          &tag))
 954         {
 955           vec_add2 (tags, this_tag, 1);
 956           clib_memcpy (this_tag->as_u8, tag.as_u8, sizeof (*this_tag));
 957         }
 958       else if (unformat (input, "clean"))
 959         flags |= IP6_SR_HEADER_FLAG_CLEANUP;
 960       else if (unformat (input, "protected"))
 961         flags |= IP6_SR_HEADER_FLAG_PROTECTED;
 962       else if (unformat (input, "key %s", &shared_secret))
 963           /* Do not include the trailing NULL byte. Guaranteed interop issue */
 964           _vec_len (shared_secret) -= 1;
 965       else if (unformat (input, "InPE %d", &pl_index))
 966         {
 967           if (pl_index <= 0 || pl_index > 4)
 968             {
 969             pl_index_range_error:
 970               return clib_error_return
 971                 (0, "Policy List Element Index %d out of range (1-4)", pl_index);
 972
 973             }
 974           flags |= IP6_SR_HEADER_FLAG_PL_ELT_INGRESS_PE
 975             << ip6_sr_policy_list_shift_from_index (pl_index);
 976         }
 977       else if (unformat (input, "EgPE %d", &pl_index))
 978         {
 979           if (pl_index <= 0 || pl_index > 4)
 980             goto pl_index_range_error;
 981           flags |= IP6_SR_HEADER_FLAG_PL_ELT_EGRESS_PE
 982             << ip6_sr_policy_list_shift_from_index (pl_index);
 983         }
 984       else if (unformat (input, "OrgSrc %d", &pl_index))
 985         {
 986           if (pl_index <= 0 || pl_index > 4)
 987             goto pl_index_range_error;
 988           flags |= IP6_SR_HEADER_FLAG_PL_ELT_ORIG_SRC_ADDR
 989             << ip6_sr_policy_list_shift_from_index (pl_index);
 990         }
 991       else
 992         break;
 993     }
 994
 995   if (!src_address_set)
 996     return clib_error_return (0, "src address required");
 997
 998   if (!dst_address_set)
 999     return clib_error_return (0, "dst address required");
1000
1001   if (!segments)
1002     return clib_error_return (0, "at least one sr segment required");
1003
1004   memset (a, 0, sizeof (*a));
1005   a->src_address = &src_address;
1006   a->dst_address = &dst_address;
1007   a->dst_mask_width = dst_mask_width;
1008   a->segments = segments;
1009   a->tags = tags;
1010   a->flags_net_byte_order = clib_host_to_net_u16(flags);
1011   a->is_del = is_del;
1012   a->rx_table_id = rx_table_id;
1013   a->tx_table_id = tx_table_id;
1014   a->shared_secret = shared_secret;
1015
1016   rv = ip6_sr_add_del_tunnel (a);
1017
1018   vec_free (segments);
1019   vec_free (tags);
1020   vec_free (shared_secret);
1021
1022   switch (rv)
1023     {
1024     case 0:
1025       break;
1026
1027     case -1:
1028       return clib_error_return (0, "SR tunnel src %U dst %U already exists",
1029                                 format_ip6_address, &src_address,
1030                                 format_ip6_address, &dst_address);
1031
1032     case -2:
1033       return clib_error_return (0, "SR tunnel src %U dst %U does not exist",
1034                                 format_ip6_address, &src_address,
1035                                 format_ip6_address, &dst_address);
1036
1037     case -3:
1038       return clib_error_return (0, "FIB table %d does not exist", rx_table_id);
1039
1040     case -4:
1041       return clib_error_return (0, "At least one segment is required");
1042
1043     default:
1044       return clib_error_return (0, "BUG: ip6_sr_add_del_tunnel returns %d",
1045                                 rv);
1046     }
1047
1048   return 0;
1049 }
1050
1051 VLIB_CLI_COMMAND (sr_tunnel_command, static) = {
1052     .path = "sr tunnel",
1053     .short_help =
1054     "sr tunnel [del] <src> <dst> [next <addr>] [cleanup] [reroute] [key %s]",
1055     .function = sr_add_del_tunnel_command_fn,
1056 };
1057
1058
1059 static clib_error_t *
1060 show_sr_tunnel_fn (vlib_main_t * vm,
1061                    unformat_input_t * input,
1062                    vlib_cli_command_t * cmd)
1063 {
1064   static ip6_sr_tunnel_t ** tunnels;
1065   ip6_sr_tunnel_t * t;
1066   ip6_sr_main_t * sm = &sr_main;
1067   ip6_main_t * im = &ip6_main;
1068   ip6_fib_t * rx_fib, * tx_fib;
1069   int i;
1070
1071   vec_reset_length (tunnels);
1072
1073   pool_foreach (t, sm->tunnels,
1074   ({
1075     vec_add1 (tunnels, t);
1076   }));
1077
1078   if (vec_len (tunnels) == 0)
1079     vlib_cli_output (vm, "No SR tunnels configured");
1080
1081   for (i = 0; i < vec_len (tunnels); i++)
1082     {
1083       t = tunnels [i];
1084
1085       rx_fib = find_ip6_fib_by_table_index_or_id (im, t->rx_fib_index,
1086                                                   IP6_ROUTE_FLAG_FIB_INDEX);
1087
1088       tx_fib = find_ip6_fib_by_table_index_or_id (im, t->tx_fib_index,
1089                                                   IP6_ROUTE_FLAG_FIB_INDEX);
1090
1091       vlib_cli_output (vm, "src %U dst %U first hop %U",
1092                        format_ip6_address, &t->key.src,
1093                        format_ip6_address, &t->key.dst,
1094                        format_ip6_address, &t->first_hop);
1095       vlib_cli_output (vm, "    rx-fib-id %d tx-fib-id %d",
1096                        rx_fib->table_id, tx_fib->table_id);
1097       vlib_cli_output (vm, "  sr: %U", format_ip6_sr_header, t->rewrite,
1098                        0 /* print_hmac */);
1099       vlib_cli_output (vm, "-------");
1100     }
1101
1102   return 0;
1103 }
1104
1105 VLIB_CLI_COMMAND (show_sr_tunnel_command, static) = {
1106     .path = "show sr tunnel",
1107     .short_help = "show sr tunnel",
1108     .function = show_sr_tunnel_fn,
1109 };
1110
1111 #define foreach_sr_fix_dst_addr_next            \
1112 _(DROP, "error-drop")
1113
1114 typedef enum {
1115 #define _(s,n) SR_FIX_DST_ADDR_NEXT_##s,
1116 foreach_sr_fix_dst_addr_next
1117 #undef _
1118   SR_FIX_DST_ADDR_N_NEXT,
1119 } sr_fix_dst_addr_next_t;
1120
1121 static char * sr_fix_dst_error_strings[] = {
1122 #define sr_fix_dst_error(n,s) s,
1123 #include "sr_fix_dst_error.def"
1124 #undef sr_fix_dst_error
1125 };
1126
1127 typedef enum {
1128 #define sr_fix_dst_error(n,s) SR_FIX_DST_ERROR_##n,
1129 #include "sr_fix_dst_error.def"
1130 #undef sr_fix_dst_error
1131   SR_FIX_DST_N_ERROR,
1132 } sr_fix_dst_error_t;
1133
1134 typedef struct {
1135   ip6_address_t src, dst;
1136   u32 next_index;
1137   u32 adj_index;
1138   u8 sr[256];
1139 } sr_fix_addr_trace_t;
1140
1141 u8 * format_sr_fix_addr_trace (u8 * s, va_list * args)
1142 {
1143   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
1144   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
1145   sr_fix_addr_trace_t * t = va_arg (*args, sr_fix_addr_trace_t *);
1146   vnet_hw_interface_t * hi = 0;
1147   ip_adjacency_t * adj;
1148   ip6_main_t * im = &ip6_main;
1149   ip_lookup_main_t * lm = &im->lookup_main;
1150   vnet_main_t * vnm = vnet_get_main();
1151
1152   if (t->adj_index != ~0)
1153     {
1154       adj = ip_get_adjacency (lm, t->adj_index);
1155       hi = vnet_get_sup_hw_interface (vnm, adj->rewrite_header.sw_if_index);
1156     }
1157
1158   s = format (s, "SR-FIX_ADDR: next %s ip6 src %U dst %U\n",
1159               (t->next_index == SR_FIX_DST_ADDR_NEXT_DROP)
1160               ? "drop" : "output",
1161               format_ip6_address, &t->src,
1162               format_ip6_address, &t->dst);
1163   if (t->next_index != SR_FIX_DST_ADDR_NEXT_DROP)
1164     {
1165       s = format (s, "%U\n", format_ip6_sr_header, t->sr, 1 /* print_hmac */);
1166       s = format (s, "   output via %s", hi ? (char *)(hi->name)
1167                   : "Invalid adj");
1168     }
1169   return s;
1170 }
1171
1172 static uword
1173 sr_fix_dst_addr (vlib_main_t * vm,
1174                    vlib_node_runtime_t * node,
1175                    vlib_frame_t * from_frame)
1176 {
1177   u32 n_left_from, next_index, * from, * to_next;
1178   ip6_main_t * im = &ip6_main;
1179   ip_lookup_main_t * lm = &im->lookup_main;
1180
1181   from = vlib_frame_vector_args (from_frame);
1182   n_left_from = from_frame->n_vectors;
1183
1184   next_index = node->cached_next_index;
1185
1186   while (n_left_from > 0)
1187     {
1188       u32 n_left_to_next;
1189
1190       vlib_get_next_frame (vm, node, next_index,
1191                            to_next, n_left_to_next);
1192
1193 #if 0
1194       while (0 && n_left_from >= 4 && n_left_to_next >= 2)
1195         {
1196           u32 bi0, bi1;
1197           __attribute__((unused)) vlib_buffer_t * b0, * b1;
1198           u32 next0 = SR_FIX_DST_ADDR_NEXT_DROP;
1199           u32 next1 = SR_FIX_DST_ADDR_NEXT_DROP;
1200
1201           /* Prefetch next iteration. */
1202           {
1203             vlib_buffer_t * p2, * p3;
1204
1205             p2 = vlib_get_buffer (vm, from[2]);
1206             p3 = vlib_get_buffer (vm, from[3]);
1207
1208             vlib_prefetch_buffer_header (p2, LOAD);
1209             vlib_prefetch_buffer_header (p3, LOAD);
1210           }
1211
1212           bi0 = from[0];
1213           bi1 = from[1];
1214           to_next[0] = bi0;
1215           to_next[1] = bi1;
1216           from += 2;
1217           to_next += 2;
1218           n_left_to_next -= 2;
1219           n_left_from -= 2;
1220
1221           b0 = vlib_get_buffer (vm, bi0);
1222           b1 = vlib_get_buffer (vm, bi1);
1223
1224
1225           vlib_validate_buffer_enqueue_x2 (vm, node, next_index,
1226                                            to_next, n_left_to_next,
1227                                            bi0, bi1, next0, next1);
1228         }
1229 #endif
1230
1231       while (n_left_from > 0 && n_left_to_next > 0)
1232         {
1233           u32 bi0;
1234           vlib_buffer_t * b0;
1235           ip6_header_t * ip0;
1236           ip_adjacency_t * adj0;
1237           ip6_sr_header_t * sr0;
1238           u32 next0 = SR_FIX_DST_ADDR_NEXT_DROP;
1239           ip6_address_t *new_dst0;
1240           ethernet_header_t * eh0;
1241
1242           bi0 = from[0];
1243           to_next[0] = bi0;
1244           from += 1;
1245           to_next += 1;
1246           n_left_from -= 1;
1247           n_left_to_next -= 1;
1248
1249           b0 = vlib_get_buffer (vm, bi0);
1250
1251           adj0 = ip_get_adjacency (lm, vnet_buffer(b0)->ip.adj_index[VLIB_TX]);
1252           next0 = adj0->mcast_group_index;
1253
1254           /* We should be pointing at an Ethernet header... */
1255           eh0 = vlib_buffer_get_current (b0);
1256           ip0 = (ip6_header_t *)(eh0+1);
1257           sr0 = (ip6_sr_header_t *) (ip0+1);
1258
1259           /* We'd better find an SR header... */
1260           if (PREDICT_FALSE(ip0->protocol != 43))
1261             {
1262               b0->error = node->errors[SR_FIX_DST_ERROR_NO_SR_HEADER];
1263               goto do_trace0;
1264             }
1265           else
1266             {
1267               /*
1268                * We get here from sr_rewrite or sr_local, with
1269                * sr->segments_left pointing at the (copy of the original) dst
1270                * address. Use it, then increment sr0->segments_left.
1271                */
1272
1273               /* Out of segments? Turf the packet */
1274               if (PREDICT_FALSE (sr0->segments_left == 0))
1275                 {
1276                   b0->error = node->errors[SR_FIX_DST_ERROR_NO_MORE_SEGMENTS];
1277                   goto do_trace0;
1278                 }
1279
1280               /*
1281                * Rewrite the packet with the original dst address
1282                * We assume that the last segment (in processing order) contains
1283                * the original dst address. The list is reversed, so sr0->segments
1284                * contains the original dst address.
1285                */
1286               new_dst0 = sr0->segments;
1287               ip0->dst_address.as_u64[0] = new_dst0->as_u64[0];
1288               ip0->dst_address.as_u64[1] = new_dst0->as_u64[1];
1289             }
1290
1291         do_trace0:
1292
1293           if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED))
1294             {
1295               sr_fix_addr_trace_t *t = vlib_add_trace (vm, node,
1296                                                        b0, sizeof (*t));
1297               t->next_index = next0;
1298               t->adj_index = ~0;
1299
1300               if (next0 != SR_FIX_DST_ADDR_NEXT_DROP)
1301                 {
1302                   t->adj_index = vnet_buffer(b0)->ip.adj_index[VLIB_TX];
1303                   clib_memcpy (t->src.as_u8, ip0->src_address.as_u8,
1304                           sizeof (t->src.as_u8));
1305                   clib_memcpy (t->dst.as_u8, ip0->dst_address.as_u8,
1306                           sizeof (t->dst.as_u8));
1307                   clib_memcpy (t->sr, sr0, sizeof (t->sr));
1308                 }
1309             }
1310
1311           vlib_validate_buffer_enqueue_x1 (vm, node, next_index,
1312                                            to_next, n_left_to_next,
1313                                            bi0, next0);
1314         }
1315
1316       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
1317     }
1318   return from_frame->n_vectors;
1319 }
1320
1321
1322 VLIB_REGISTER_NODE (sr_fix_dst_addr_node) = {
1323   .function = sr_fix_dst_addr,
1324   .name = "sr-fix-dst-addr",
1325   /* Takes a vector of packets. */
1326   .vector_size = sizeof (u32),
1327   .format_trace = format_sr_fix_addr_trace,
1328   .format_buffer = format_ip6_sr_header_with_length,
1329
1330   .runtime_data_bytes = 0,
1331
1332   .n_errors = SR_FIX_DST_N_ERROR,
1333   .error_strings = sr_fix_dst_error_strings,
1334
1335   .n_next_nodes = SR_FIX_DST_ADDR_N_NEXT,
1336   .next_nodes = {
1337 #define _(s,n) [SR_FIX_DST_ADDR_NEXT_##s] = n,
1338     foreach_sr_fix_dst_addr_next
1339 #undef _
1340   },
1341 };
1342
1343 static clib_error_t * sr_init (vlib_main_t * vm)
1344 {
1345   ip6_sr_main_t * sm = &sr_main;
1346   clib_error_t * error = 0;
1347   vlib_node_t * ip6_lookup_node, * ip6_rewrite_node;
1348   vlib_node_t * ip6_rewrite_local_node;
1349   u32 verify_next_index;
1350
1351   if ((error = vlib_call_init_function (vm, ip_main_init)))
1352     return error;
1353
1354   if ((error = vlib_call_init_function (vm, ip6_lookup_init)))
1355     return error;
1356
1357   sm->vlib_main = vm;
1358   sm->vnet_main = vnet_get_main();
1359
1360   vec_validate (sm->hmac_keys, 0);
1361   sm->hmac_keys[0].shared_secret = (u8 *) 0xdeadbeef;
1362
1363   sm->tunnel_index_by_key =
1364     hash_create_mem (0, sizeof (ip6_sr_tunnel_key_t), sizeof (uword));
1365
1366   sm->hmac_key_by_shared_secret = hash_create_string (0, sizeof(uword));
1367
1368   ip6_register_protocol (43, sr_local_node.index);
1369
1370   ip6_lookup_node = vlib_get_node_by_name (vm, (u8 *)"ip6-lookup");
1371   ASSERT(ip6_lookup_node);
1372
1373   ip6_rewrite_node = vlib_get_node_by_name (vm, (u8 *)"ip6-rewrite");
1374   ASSERT(ip6_rewrite_node);
1375
1376   ip6_rewrite_local_node = vlib_get_node_by_name (vm,
1377                                                   (u8 *)"ip6-rewrite-local");
1378   ASSERT(ip6_rewrite_local_node);
1379
1380   /* Add a disposition to ip6_lookup for the sr rewrite node */
1381   sm->ip6_lookup_sr_next_index =
1382     vlib_node_add_next (vm, ip6_lookup_node->index, sr_rewrite_node.index);
1383
1384   /* Add a disposition to ip6_rewrite for the sr dst address hack node */
1385   sm->ip6_rewrite_sr_next_index =
1386     vlib_node_add_next (vm, ip6_rewrite_node->index,
1387                         sr_fix_dst_addr_node.index);
1388   /*
1389    * Fix ip6-rewrite-local, sibling of the above. The sibling bitmap
1390    * isn't set up at this point, so we have to do it manually
1391    */
1392   verify_next_index = vlib_node_add_next
1393       (vm, ip6_rewrite_local_node->index,
1394        sr_fix_dst_addr_node.index);
1395
1396   ASSERT(sm->ip6_rewrite_sr_next_index == verify_next_index);
1397
1398   OpenSSL_add_all_digests();
1399
1400   sm->md = (void *) EVP_get_digestbyname ("sha1");
1401   sm->hmac_ctx = clib_mem_alloc (sizeof (HMAC_CTX));
1402
1403   return error;
1404 }
1405
1406 VLIB_INIT_FUNCTION (sr_init);
1407
1408 #define foreach_sr_local_next                   \
1409   _ (ERROR, "error-drop")                       \
1410   _ (IP6_LOOKUP, "ip6-lookup")
1411
1412 typedef enum {
1413 #define _(s,n) SR_LOCAL_NEXT_##s,
1414   foreach_sr_local_next
1415 #undef _
1416   SR_LOCAL_N_NEXT,
1417 } sr_local_next_t;
1418
1419 typedef struct {
1420   u8 next_index;
1421   u8 sr_valid;
1422   ip6_address_t src, dst;
1423   u16 length;
1424   u8 sr[256];
1425 } sr_local_trace_t;
1426
1427 static char * sr_local_error_strings[] = {
1428 #define sr_error(n,s) s,
1429 #include "sr_error.def"
1430 #undef sr_error
1431 };
1432
1433 typedef enum {
1434 #define sr_error(n,s) SR_LOCAL_ERROR_##n,
1435 #include "sr_error.def"
1436 #undef sr_error
1437   SR_LOCAL_N_ERROR,
1438 } sr_local_error_t;
1439
1440 u8 * format_sr_local_trace (u8 * s, va_list * args)
1441 {
1442   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
1443   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
1444   sr_local_trace_t * t = va_arg (*args, sr_local_trace_t *);
1445
1446   s = format (s, "SR-LOCAL: src %U dst %U len %u next_index %d",
1447               format_ip6_address, &t->src,
1448               format_ip6_address, &t->dst, t->length, t->next_index);
1449   if (t->sr_valid)
1450     s = format (s, "\n  %U", format_ip6_sr_header, t->sr, 1 /* print_hmac */);
1451   else
1452     s = format (s, "\n  popped SR header");
1453
1454   return s;
1455 }
1456
1457
1458 /* $$$$ fixme: smp, don't copy data, cache input, output (maybe) */
1459
1460 static int sr_validate_hmac (ip6_sr_main_t * sm, ip6_header_t * ip,
1461                              ip6_sr_header_t * sr)
1462 {
1463   u32 key_index;
1464   static u8 * keybuf;
1465   u8 * copy_target;
1466   int first_segment;
1467   ip6_address_t *addrp;
1468   int i;
1469   ip6_sr_hmac_key_t * hmac_key;
1470   static u8 * signature;
1471   u32 sig_len;
1472
1473   key_index = sr->hmac_key;
1474
1475   /* No signature? Pass... */
1476   if (key_index == 0)
1477     return 0;
1478
1479   /* We don't know about this key? Fail... */
1480   if (key_index >= vec_len (sm->hmac_keys))
1481     return 1;
1482
1483   vec_validate (signature, SHA256_DIGEST_LENGTH-1);
1484
1485   hmac_key = sm->hmac_keys + key_index;
1486
1487   vec_reset_length (keybuf);
1488
1489   /* pkt ip6 src address */
1490   vec_add2 (keybuf, copy_target, sizeof (ip6_address_t));
1491   clib_memcpy (copy_target, ip->src_address.as_u8, sizeof (ip6_address_t));
1492
1493   /* last segment */
1494   vec_add2 (keybuf, copy_target, 1);
1495   copy_target[0] = sr->first_segment;
1496
1497   /* octet w/ bit 0 = "clean" flag */
1498   vec_add2 (keybuf, copy_target, 1);
1499   copy_target[0]
1500     = (sr->flags & clib_host_to_net_u16 (IP6_SR_HEADER_FLAG_CLEANUP))
1501     ? 0x80 : 0;
1502
1503   /* hmac key id */
1504   vec_add2 (keybuf, copy_target, 1);
1505   copy_target[0] = sr->hmac_key;
1506
1507   first_segment = sr->first_segment;
1508
1509   addrp = sr->segments;
1510
1511   /* segments */
1512   for (i = 0; i <= first_segment; i++)
1513     {
1514       vec_add2 (keybuf, copy_target, sizeof (ip6_address_t));
1515       clib_memcpy (copy_target, addrp->as_u8, sizeof (ip6_address_t));
1516       addrp++;
1517     }
1518
1519   if (sm->is_debug)
1520       clib_warning ("verify key index %d keybuf: %U", key_index,
1521                     format_hex_bytes, keybuf, vec_len(keybuf));
1522
1523   /* shared secret */
1524
1525   /* SHA1 is shorter than SHA-256 */
1526   memset (signature, 0, vec_len(signature));
1527
1528   HMAC_CTX_init(sm->hmac_ctx);
1529   if (!HMAC_Init(sm->hmac_ctx, hmac_key->shared_secret,
1530                  vec_len(hmac_key->shared_secret),sm->md))
1531       clib_warning ("barf1");
1532   if (!HMAC_Update(sm->hmac_ctx,keybuf,vec_len(keybuf)))
1533       clib_warning ("barf2");
1534   if (!HMAC_Final(sm->hmac_ctx,signature,&sig_len))
1535       clib_warning ("barf3");
1536   HMAC_CTX_cleanup(sm->hmac_ctx);
1537
1538   if (sm->is_debug)
1539       clib_warning ("computed signature len %d, value %U", sig_len,
1540                     format_hex_bytes, signature, vec_len(signature));
1541
1542   /* Point at the SHA signature in the packet */
1543   addrp++;
1544   if (sm->is_debug)
1545       clib_warning ("read signature %U", format_hex_bytes, addrp,
1546                     SHA256_DIGEST_LENGTH);
1547
1548   return memcmp (signature, addrp, SHA256_DIGEST_LENGTH);
1549 }
1550
1551 static uword
1552 sr_local (vlib_main_t * vm,
1553           vlib_node_runtime_t * node,
1554           vlib_frame_t * from_frame)
1555 {
1556   u32 n_left_from, next_index, * from, * to_next;
1557   ip6_sr_main_t * sm = &sr_main;
1558   u32 (*sr_local_cb) (vlib_main_t *, vlib_node_runtime_t *,
1559                       vlib_buffer_t *, ip6_header_t *,
1560                       ip6_sr_header_t *);
1561   sr_local_cb = sm->sr_local_cb;
1562
1563   from = vlib_frame_vector_args (from_frame);
1564   n_left_from = from_frame->n_vectors;
1565
1566   next_index = node->cached_next_index;
1567
1568   while (n_left_from > 0)
1569     {
1570       u32 n_left_to_next;
1571
1572       vlib_get_next_frame (vm, node, next_index,
1573                            to_next, n_left_to_next);
1574
1575       while (n_left_from >= 4 && n_left_to_next >= 2)
1576         {
1577           u32 bi0, bi1;
1578           vlib_buffer_t * b0, * b1;
1579           ip6_header_t * ip0, *ip1;
1580           ip6_sr_header_t * sr0, *sr1;
1581           ip6_address_t * new_dst0, * new_dst1;
1582           u32 next0 = SR_LOCAL_NEXT_IP6_LOOKUP;
1583           u32 next1 = SR_LOCAL_NEXT_IP6_LOOKUP;
1584           /* Prefetch next iteration. */
1585           {
1586             vlib_buffer_t * p2, * p3;
1587
1588             p2 = vlib_get_buffer (vm, from[2]);
1589             p3 = vlib_get_buffer (vm, from[3]);
1590
1591             vlib_prefetch_buffer_header (p2, LOAD);
1592             vlib_prefetch_buffer_header (p3, LOAD);
1593
1594             CLIB_PREFETCH (p2->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
1595             CLIB_PREFETCH (p3->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
1596           }
1597
1598           bi0 = from[0];
1599           bi1 = from[1];
1600           to_next[0] = bi0;
1601           to_next[1] = bi1;
1602           from += 2;
1603           to_next += 2;
1604           n_left_to_next -= 2;
1605           n_left_from -= 2;
1606
1607
1608           b0 = vlib_get_buffer (vm, bi0);
1609           ip0 = vlib_buffer_get_current (b0);
1610           sr0 = (ip6_sr_header_t *)(ip0+1);
1611
1612           if (PREDICT_FALSE(sr0->type != ROUTING_HEADER_TYPE_SR))
1613             {
1614               next0 = SR_LOCAL_NEXT_ERROR;
1615               b0->error = node->errors[SR_LOCAL_ERROR_BAD_ROUTING_HEADER_TYPE];
1616               goto do_trace0;
1617             }
1618
1619           /* Out of segments? Turf the packet */
1620           if (PREDICT_FALSE (sr0->segments_left == 0))
1621             {
1622               next0 = SR_LOCAL_NEXT_ERROR;
1623               b0->error = node->errors[SR_LOCAL_ERROR_NO_MORE_SEGMENTS];
1624               goto do_trace0;
1625             }
1626
1627           if (PREDICT_FALSE(sm->validate_hmac))
1628             {
1629               if (sr_validate_hmac (sm, ip0, sr0))
1630                 {
1631                   next0 = SR_LOCAL_NEXT_ERROR;
1632                   b0->error = node->errors[SR_LOCAL_ERROR_HMAC_INVALID];
1633                   goto do_trace0;
1634                 }
1635             }
1636
1637           next0 = sr_local_cb ? sr_local_cb (vm, node, b0, ip0, sr0) :
1638               next0;
1639
1640           /*
1641            * To suppress rewrite, return ~SR_LOCAL_NEXT_xxx
1642            */
1643           if (PREDICT_FALSE (next0 & 0x80000000))
1644           {
1645               next0 ^= 0xFFFFFFFF;
1646               if (PREDICT_FALSE(next0 == SR_LOCAL_NEXT_ERROR))
1647                   b0->error =
1648                   node->errors[SR_LOCAL_ERROR_APP_CALLBACK];
1649             }
1650           else
1651             {
1652               u32 segment_index0;
1653
1654               segment_index0 = sr0->segments_left - 1;
1655
1656               /* Rewrite the packet */
1657               new_dst0 = (ip6_address_t *)(sr0->segments + segment_index0);
1658               ip0->dst_address.as_u64[0] = new_dst0->as_u64[0];
1659               ip0->dst_address.as_u64[1] = new_dst0->as_u64[1];
1660
1661               if (PREDICT_TRUE (sr0->segments_left > 0))
1662                   sr0->segments_left -= 1;
1663             }
1664
1665           /* End of the path. Clean up the SR header, or not */
1666           if (PREDICT_FALSE
1667               (sr0->segments_left == 0 &&
1668                (sr0->flags & clib_host_to_net_u16(IP6_SR_HEADER_FLAG_CLEANUP))))
1669           {
1670               u64 *copy_dst0, *copy_src0;
1671               u16 new_l0;
1672               /*
1673                * Copy the ip6 header right by the (real) length of the
1674                * sr header. Here's another place which assumes that
1675                * the sr header is the only extention header.
1676                */
1677
1678               ip0->protocol = sr0->protocol;
1679               vlib_buffer_advance (b0, (sr0->length+1)*8);
1680
1681               new_l0 = clib_net_to_host_u16(ip0->payload_length) -
1682                   (sr0->length+1)*8;
1683               ip0->payload_length = clib_host_to_net_u16(new_l0);
1684
1685               copy_src0 = (u64 *)ip0;
1686               copy_dst0 = copy_src0 + (sr0->length + 1);
1687
1688               copy_dst0 [4] = copy_src0[4];
1689               copy_dst0 [3] = copy_src0[3];
1690               copy_dst0 [2] = copy_src0[2];
1691               copy_dst0 [1] = copy_src0[1];
1692               copy_dst0 [0] = copy_src0[0];
1693
1694               sr0 = 0;
1695             }
1696
1697         do_trace0:
1698           if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED))
1699             {
1700               sr_local_trace_t *tr = vlib_add_trace (vm, node,
1701                                                      b0, sizeof (*tr));
1702               clib_memcpy (tr->src.as_u8, ip0->src_address.as_u8,
1703                       sizeof (tr->src.as_u8));
1704               clib_memcpy (tr->dst.as_u8, ip0->dst_address.as_u8,
1705                       sizeof (tr->dst.as_u8));
1706               tr->length = vlib_buffer_length_in_chain (vm, b0);
1707               tr->next_index = next0;
1708               tr->sr_valid = sr0 != 0;
1709               if (tr->sr_valid)
1710                 clib_memcpy (tr->sr, sr0, sizeof (tr->sr));
1711             }
1712
1713           b1 = vlib_get_buffer (vm, bi1);
1714           ip1 = vlib_buffer_get_current (b1);
1715           sr1 = (ip6_sr_header_t *)(ip1+1);
1716
1717           if (PREDICT_FALSE(sr1->type != ROUTING_HEADER_TYPE_SR))
1718             {
1719               next1 = SR_LOCAL_NEXT_ERROR;
1720               b1->error = node->errors[SR_LOCAL_ERROR_BAD_ROUTING_HEADER_TYPE];
1721               goto do_trace1;
1722             }
1723
1724           /* Out of segments? Turf the packet */
1725           if (PREDICT_FALSE (sr1->segments_left == 0))
1726             {
1727               next1 = SR_LOCAL_NEXT_ERROR;
1728               b1->error = node->errors[SR_LOCAL_ERROR_NO_MORE_SEGMENTS];
1729               goto do_trace1;
1730             }
1731
1732           if (PREDICT_FALSE(sm->validate_hmac))
1733             {
1734               if (sr_validate_hmac (sm, ip1, sr1))
1735                 {
1736                   next1 = SR_LOCAL_NEXT_ERROR;
1737                   b1->error = node->errors[SR_LOCAL_ERROR_HMAC_INVALID];
1738                   goto do_trace1;
1739                 }
1740             }
1741
1742           next1 = sr_local_cb ? sr_local_cb (vm, node, b1, ip1, sr1) :
1743               next1;
1744
1745           /*
1746            * To suppress rewrite, return ~SR_LOCAL_NEXT_xxx
1747            */
1748           if (PREDICT_FALSE (next1 & 0x80000000))
1749           {
1750               next1 ^= 0xFFFFFFFF;
1751               if (PREDICT_FALSE(next1 == SR_LOCAL_NEXT_ERROR))
1752                   b1->error =
1753                   node->errors[SR_LOCAL_ERROR_APP_CALLBACK];
1754             }
1755           else
1756             {
1757               u32 segment_index1;
1758
1759               segment_index1 = sr1->segments_left - 1;
1760
1761               /* Rewrite the packet */
1762               new_dst1 = (ip6_address_t *)(sr1->segments + segment_index1);
1763               ip1->dst_address.as_u64[0] = new_dst1->as_u64[0];
1764               ip1->dst_address.as_u64[1] = new_dst1->as_u64[1];
1765
1766               if (PREDICT_TRUE (sr1->segments_left > 0))
1767                   sr1->segments_left -= 1;
1768             }
1769
1770           /* End of the path. Clean up the SR header, or not */
1771           if (PREDICT_FALSE
1772               (sr1->segments_left == 0 &&
1773                (sr1->flags & clib_host_to_net_u16(IP6_SR_HEADER_FLAG_CLEANUP))))
1774             {
1775               u64 *copy_dst1, *copy_src1;
1776               u16 new_l1;
1777               /*
1778                * Copy the ip6 header right by the (real) length of the
1779                * sr header. Here's another place which assumes that
1780                * the sr header is the only extention header.
1781                */
1782
1783               ip1->protocol = sr1->protocol;
1784               vlib_buffer_advance (b1, (sr1->length+1)*8);
1785
1786               new_l1 = clib_net_to_host_u16(ip1->payload_length) -
1787                   (sr1->length+1)*8;
1788               ip1->payload_length = clib_host_to_net_u16(new_l1);
1789
1790               copy_src1 = (u64 *)ip1;
1791               copy_dst1 = copy_src1 + (sr1->length + 1);
1792
1793               copy_dst1 [4] = copy_src1[4];
1794               copy_dst1 [3] = copy_src1[3];
1795               copy_dst1 [2] = copy_src1[2];
1796               copy_dst1 [1] = copy_src1[1];
1797               copy_dst1 [0] = copy_src1[0];
1798
1799               sr1 = 0;
1800             }
1801
1802         do_trace1:
1803           if (PREDICT_FALSE(b1->flags & VLIB_BUFFER_IS_TRACED))
1804             {
1805               sr_local_trace_t *tr = vlib_add_trace (vm, node,
1806                                                      b1, sizeof (*tr));
1807               clib_memcpy (tr->src.as_u8, ip1->src_address.as_u8,
1808                       sizeof (tr->src.as_u8));
1809               clib_memcpy (tr->dst.as_u8, ip1->dst_address.as_u8,
1810                       sizeof (tr->dst.as_u8));
1811               tr->length = vlib_buffer_length_in_chain (vm, b1);
1812               tr->next_index = next1;
1813               tr->sr_valid = sr1 != 0;
1814               if (tr->sr_valid)
1815                 clib_memcpy (tr->sr, sr1, sizeof (tr->sr));
1816             }
1817
1818           vlib_validate_buffer_enqueue_x2 (vm, node, next_index,
1819                                            to_next, n_left_to_next,
1820                                            bi0, bi1, next0, next1);
1821         }
1822
1823       while (n_left_from > 0 && n_left_to_next > 0)
1824         {
1825           u32 bi0;
1826           vlib_buffer_t * b0;
1827           ip6_header_t * ip0;
1828           ip6_sr_header_t * sr0;
1829           ip6_address_t * new_dst0;
1830           u32 next0 = SR_LOCAL_NEXT_IP6_LOOKUP;
1831
1832           bi0 = from[0];
1833           to_next[0] = bi0;
1834           from += 1;
1835           to_next += 1;
1836           n_left_from -= 1;
1837           n_left_to_next -= 1;
1838
1839           b0 = vlib_get_buffer (vm, bi0);
1840           ip0 = vlib_buffer_get_current (b0);
1841           sr0 = (ip6_sr_header_t *)(ip0+1);
1842
1843           if (PREDICT_FALSE(sr0->type != ROUTING_HEADER_TYPE_SR))
1844             {
1845               next0 = SR_LOCAL_NEXT_ERROR;
1846               b0->error = node->errors[SR_LOCAL_ERROR_BAD_ROUTING_HEADER_TYPE];
1847               goto do_trace;
1848             }
1849
1850           /* Out of segments? Turf the packet */
1851           if (PREDICT_FALSE (sr0->segments_left == 0))
1852             {
1853               next0 = SR_LOCAL_NEXT_ERROR;
1854               b0->error = node->errors[SR_LOCAL_ERROR_NO_MORE_SEGMENTS];
1855               goto do_trace;
1856             }
1857
1858           if (PREDICT_FALSE(sm->validate_hmac))
1859             {
1860               if (sr_validate_hmac (sm, ip0, sr0))
1861                 {
1862                   next0 = SR_LOCAL_NEXT_ERROR;
1863                   b0->error = node->errors[SR_LOCAL_ERROR_HMAC_INVALID];
1864                   goto do_trace;
1865                 }
1866             }
1867
1868           next0 = sr_local_cb ? sr_local_cb (vm, node, b0, ip0, sr0) :
1869             next0;
1870
1871           /*
1872            * To suppress rewrite, return ~SR_LOCAL_NEXT_xxx
1873            */
1874           if (PREDICT_FALSE (next0 & 0x80000000))
1875             {
1876               next0 ^= 0xFFFFFFFF;
1877               if (PREDICT_FALSE(next0 == SR_LOCAL_NEXT_ERROR))
1878                 b0->error =
1879                   node->errors[SR_LOCAL_ERROR_APP_CALLBACK];
1880             }
1881           else
1882             {
1883               u32 segment_index0;
1884
1885               segment_index0 = sr0->segments_left - 1;
1886
1887               /* Rewrite the packet */
1888               new_dst0 = (ip6_address_t *)(sr0->segments + segment_index0);
1889               ip0->dst_address.as_u64[0] = new_dst0->as_u64[0];
1890               ip0->dst_address.as_u64[1] = new_dst0->as_u64[1];
1891
1892               if (PREDICT_TRUE (sr0->segments_left > 0))
1893                   sr0->segments_left -= 1;
1894             }
1895
1896           /* End of the path. Clean up the SR header, or not */
1897           if (PREDICT_FALSE
1898               (sr0->segments_left == 0 &&
1899                (sr0->flags & clib_host_to_net_u16(IP6_SR_HEADER_FLAG_CLEANUP))))
1900             {
1901               u64 *copy_dst0, *copy_src0;
1902               u16 new_l0;
1903               /*
1904                * Copy the ip6 header right by the (real) length of the
1905                * sr header. Here's another place which assumes that
1906                * the sr header is the only extention header.
1907                */
1908
1909               ip0->protocol = sr0->protocol;
1910               vlib_buffer_advance (b0, (sr0->length+1)*8);
1911
1912               new_l0 = clib_net_to_host_u16(ip0->payload_length) -
1913                   (sr0->length+1)*8;
1914               ip0->payload_length = clib_host_to_net_u16(new_l0);
1915
1916               copy_src0 = (u64 *)ip0;
1917               copy_dst0 = copy_src0 + (sr0->length + 1);
1918
1919               copy_dst0 [4] = copy_src0[4];
1920               copy_dst0 [3] = copy_src0[3];
1921               copy_dst0 [2] = copy_src0[2];
1922               copy_dst0 [1] = copy_src0[1];
1923               copy_dst0 [0] = copy_src0[0];
1924
1925               sr0 = 0;
1926             }
1927
1928         do_trace:
1929           if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED))
1930             {
1931               sr_local_trace_t *tr = vlib_add_trace (vm, node,
1932                                                      b0, sizeof (*tr));
1933               clib_memcpy (tr->src.as_u8, ip0->src_address.as_u8,
1934                       sizeof (tr->src.as_u8));
1935               clib_memcpy (tr->dst.as_u8, ip0->dst_address.as_u8,
1936                       sizeof (tr->dst.as_u8));
1937               tr->length = vlib_buffer_length_in_chain (vm, b0);
1938               tr->next_index = next0;
1939               tr->sr_valid = sr0 != 0;
1940               if (tr->sr_valid)
1941                 clib_memcpy (tr->sr, sr0, sizeof (tr->sr));
1942             }
1943
1944           vlib_validate_buffer_enqueue_x1 (vm, node, next_index,
1945                                            to_next, n_left_to_next,
1946                                            bi0, next0);
1947         }
1948
1949       vlib_put_next_frame (vm, node, next_index, n_left_to_next);
1950     }
1951   vlib_node_increment_counter (vm, sr_local_node.index,
1952                                SR_LOCAL_ERROR_PKTS_PROCESSED,
1953                                from_frame->n_vectors);
1954   return from_frame->n_vectors;
1955 }
1956
1957 VLIB_REGISTER_NODE (sr_local_node, static) = {
1958   .function = sr_local,
1959   .name = "sr-local",
1960   /* Takes a vector of packets. */
1961   .vector_size = sizeof (u32),
1962   .format_trace = format_sr_local_trace,
1963
1964   .runtime_data_bytes = 0,
1965
1966   .n_errors = SR_LOCAL_N_ERROR,
1967   .error_strings = sr_local_error_strings,
1968
1969   .n_next_nodes = SR_LOCAL_N_NEXT,
1970   .next_nodes = {
1971 #define _(s,n) [SR_LOCAL_NEXT_##s] = n,
1972     foreach_sr_local_next
1973 #undef _
1974   },
1975 };
1976
1977 ip6_sr_main_t * sr_get_main (vlib_main_t * vm)
1978 {
1979   vlib_call_init_function (vm, sr_init);
1980   ASSERT(sr_local_node.index);
1981   return &sr_main;
1982 }
1983
1984
1985 static clib_error_t *
1986 set_ip6_sr_rewrite_fn (vlib_main_t * vm,
1987                        unformat_input_t * input,
1988                        vlib_cli_command_t * cmd)
1989 {
1990   ip6_address_t a;
1991   ip6_main_t * im = &ip6_main;
1992   ip_lookup_main_t * lm = &im->lookup_main;
1993   u32 fib_index = 0;
1994   u32 fib_id = 0;
1995   u32 adj_index;
1996   uword * p;
1997   ip_adjacency_t * adj;
1998   vnet_hw_interface_t * hi;
1999   u32 sw_if_index;
2000   ip6_sr_main_t * sm = &sr_main;
2001   vnet_main_t * vnm = vnet_get_main();
2002
2003   if (!unformat (input, "%U", unformat_ip6_address, &a))
2004     return clib_error_return (0, "ip6 address missing in '%U'",
2005                               format_unformat_error, input);
2006
2007   if (unformat (input, "rx-table-id %d", &fib_id))
2008     {
2009       p = hash_get (im->fib_index_by_table_id, fib_id);
2010       if (p == 0)
2011         return clib_error_return (0, "fib-id %d not found");
2012       fib_index = p[0];
2013     }
2014
2015   adj_index = ip6_fib_lookup_with_table (im, fib_index, &a);
2016
2017   if (adj_index == lm->miss_adj_index)
2018     return clib_error_return (0, "no match for %U",
2019                               format_ip6_address, &a);
2020
2021   adj = ip_get_adjacency (lm, adj_index);
2022
2023   if (adj->lookup_next_index != IP_LOOKUP_NEXT_REWRITE)
2024     return clib_error_return (0, "%U unresolved (not a rewrite adj)",
2025                               format_ip6_address, &a);
2026
2027   adj->rewrite_header.next_index = sm->ip6_rewrite_sr_next_index;
2028
2029   sw_if_index = adj->rewrite_header.sw_if_index;
2030   hi = vnet_get_sup_hw_interface (vnm, sw_if_index);
2031   adj->rewrite_header.node_index = sr_fix_dst_addr_node.index;
2032
2033   /* $$$$$ hack... steal the mcast group index */
2034   adj->mcast_group_index =
2035     vlib_node_add_next (vm, sr_fix_dst_addr_node.index, hi->output_node_index);
2036
2037   return 0;
2038 }
2039
2040 VLIB_CLI_COMMAND (set_ip6_sr_rewrite, static) = {
2041     .path = "set ip6 sr rewrite",
2042     .short_help = "set ip6 sr rewrite <ip6-address> [fib-id <id>]",
2043     .function = set_ip6_sr_rewrite_fn,
2044 };
2045
2046 void vnet_register_sr_app_callback (void *cb)
2047 {
2048   ip6_sr_main_t * sm = &sr_main;
2049
2050   sm->sr_local_cb = cb;
2051 }
2052
2053 static clib_error_t *
2054 test_sr_hmac_validate_fn (vlib_main_t * vm,
2055                     unformat_input_t * input,
2056                     vlib_cli_command_t * cmd)
2057 {
2058   ip6_sr_main_t * sm = &sr_main;
2059
2060   if (unformat (input, "validate on"))
2061     sm->validate_hmac = 1;
2062   else if (unformat (input, "chunk-offset off"))
2063     sm->validate_hmac = 0;
2064   else
2065     return clib_error_return (0, "expected validate on|off in '%U'",
2066                               format_unformat_error, input);
2067
2068   vlib_cli_output (vm, "hmac signature validation %s",
2069                    sm->validate_hmac ?
2070                    "on" : "off");
2071   return 0;
2072 }
2073
2074 VLIB_CLI_COMMAND (test_sr_hmac_validate, static) = {
2075     .path = "test sr hmac",
2076     .short_help = "test sr hmac validate [on|off]",
2077     .function = test_sr_hmac_validate_fn,
2078 };
2079
2080 i32 sr_hmac_add_del_key (ip6_sr_main_t * sm, u32 key_id, u8 * shared_secret,
2081                          u8 is_del)
2082 {
2083   u32 index;
2084   ip6_sr_hmac_key_t * key;
2085
2086   if (is_del == 0)
2087     {
2088       /* Specific key in use? Fail. */
2089       if (key_id && vec_len (sm->hmac_keys) > key_id
2090           && sm->hmac_keys[key_id].shared_secret)
2091         return -2;
2092
2093       index = key_id;
2094       key = find_or_add_shared_secret (sm, shared_secret, &index);
2095       ASSERT(index == key_id);
2096       return 0;
2097     }
2098
2099   /* delete */
2100
2101   if (key_id)                   /* delete by key ID */
2102     {
2103       if (vec_len (sm->hmac_keys) <= key_id)
2104         return -3;
2105
2106       key = sm->hmac_keys + key_id;
2107
2108       hash_unset_mem (sm->hmac_key_by_shared_secret, key->shared_secret);
2109       vec_free (key->shared_secret);
2110       return 0;
2111     }
2112
2113   index = 0;
2114   key = find_or_add_shared_secret (sm, shared_secret, &index);
2115   hash_unset_mem (sm->hmac_key_by_shared_secret, key->shared_secret);
2116   vec_free (key->shared_secret);
2117   return 0;
2118 }
2119
2120
2121 static clib_error_t *
2122 sr_hmac_add_del_key_fn (vlib_main_t * vm,
2123                         unformat_input_t * input,
2124                         vlib_cli_command_t * cmd)
2125 {
2126   ip6_sr_main_t * sm = &sr_main;
2127   u8 is_del = 0;
2128   u32 key_id = 0;
2129   u8 key_id_set = 0;
2130   u8 * shared_secret = 0;
2131   i32 rv;
2132
2133   while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
2134     {
2135       if (unformat (input, "del"))
2136         is_del = 1;
2137       else if (unformat (input, "id %d", &key_id))
2138           key_id_set = 1;
2139       else if (unformat (input, "key %s", &shared_secret))
2140         {
2141           /* Do not include the trailing NULL byte. Guaranteed interop issue */
2142           _vec_len (shared_secret) -= 1;
2143         }
2144       else
2145         break;
2146     }
2147
2148   if (is_del == 0 && shared_secret == 0)
2149     return clib_error_return (0, "shared secret must be set to add a key");
2150
2151   if (shared_secret == 0 && key_id_set == 0)
2152     return clib_error_return (0, "shared secret and key id both unset");
2153
2154   rv = sr_hmac_add_del_key (sm, key_id, shared_secret, is_del);
2155
2156   vec_free (shared_secret);
2157
2158   switch (rv)
2159     {
2160     case 0:
2161       break;
2162
2163     default:
2164       return clib_error_return (0, "sr_hmac_add_del_key returned %d",
2165                                 rv);
2166     }
2167
2168   return 0;
2169 }
2170
2171 VLIB_CLI_COMMAND (sr_hmac, static) = {
2172     .path = "sr hmac",
2173     .short_help = "sr hmac [del] id <nn> key <str>",
2174     .function = sr_hmac_add_del_key_fn,
2175 };
2176
2177
2178 static clib_error_t *
2179 show_sr_hmac_fn (vlib_main_t * vm,
2180                  unformat_input_t * input,
2181                  vlib_cli_command_t * cmd)
2182 {
2183   ip6_sr_main_t * sm = &sr_main;
2184   int i;
2185
2186   for (i = 1; i < vec_len (sm->hmac_keys); i++)
2187     {
2188       if (sm->hmac_keys[i].shared_secret)
2189           vlib_cli_output (vm, "[%d]: %v", i, sm->hmac_keys[i].shared_secret);
2190     }
2191
2192   return 0;
2193 }
2194
2195 VLIB_CLI_COMMAND (show_sr_hmac, static) = {
2196     .path = "show sr hmac",
2197     .short_help = "show sr hmac",
2198     .function = show_sr_hmac_fn,
2199 };
2200
2201 static clib_error_t *
2202 test_sr_debug_fn (vlib_main_t * vm,
2203                     unformat_input_t * input,
2204                     vlib_cli_command_t * cmd)
2205 {
2206   ip6_sr_main_t * sm = &sr_main;
2207
2208   if (unformat (input, "on"))
2209     sm->is_debug = 1;
2210   else if (unformat (input, "off"))
2211     sm->is_debug = 0;
2212   else
2213     return clib_error_return (0, "expected on|off in '%U'",
2214                               format_unformat_error, input);
2215
2216   vlib_cli_output (vm, "debug trace now %s", sm->is_debug ? "on" : "off");
2217
2218   return 0;
2219 }
2220
2221 VLIB_CLI_COMMAND (test_sr_debug, static) = {
2222     .path = "test sr debug",
2223     .short_help = "test sr debug on|off",
2224     .function = test_sr_debug_fn,
2225 };