Raw export of IP6 packet with iOAM metadata

- is enabled on iOAM pop nodes with "set ioam export ipfix collector"
- ioam_export_node Hooks into vlib graph b/n ip6-hop-by-hop node and ip6-pop-hop-by-hop node
- A buffer per worker thread is created for collecting packet data to be exported
- ioam_export_node exports first 3 cachelines by collecting it in a MTU sized frame, slaps on ipfix header for export
- ioam_export_thread process node - checks for unsent record buffers for longer than 20 seconds and exports it
- Added dual loop and prefetch in add, hop-by-hop and pop functions

To be done:
- IPfix template
- Multi collector distribution of ipfix packets
- Port to be configurable

Change-Id: I959b4253036551382562bdaf10a83fd6f2f1c88b
Signed-off-by: Shwetha <shwethab@cisco.com>

3 files changed, 341 insertions, 124 deletions

diff --git a/vnet/vnet/ip/ip6.h b/vnet/vnet/ip/ip6.h
index 0d5d8d04bf8..b43e2dac791 100644
--- a/vnet/vnet/ip/ip6.h
+++ b/vnet/vnet/ip/ip6.h
@@ -578,6 +578,7 @@ typedef struct {
   /* Array of function pointers to HBH option handling routines */
   int (*options[256])(vlib_buffer_t *b, ip6_header_t *ip, ip6_hop_by_hop_option_t *opt);
   u8 *(*trace[256])(u8 *s, ip6_hop_by_hop_option_t *opt);
+  uword next_override;
 } ip6_hop_by_hop_main_t;
 
 extern ip6_hop_by_hop_main_t ip6_hop_by_hop_main;
@@ -586,6 +587,7 @@ int ip6_hbh_register_option (u8 option,
 			     int options(vlib_buffer_t *b, ip6_header_t *ip, ip6_hop_by_hop_option_t *opt),
 			     u8 *trace(u8 *s, ip6_hop_by_hop_option_t *opt));
 int ip6_hbh_unregister_option (u8 option);
+void ip6_hbh_set_next_override (uword next);
 
 /* Flag used by IOAM code. Classifier sets it pop-hop-by-hop checks it */
 #define OI_DECAP   100
diff --git a/vnet/vnet/ip/ip6_forward.c b/vnet/vnet/ip/ip6_forward.c
index f79acf74d7b..723b09041b8 100644
--- a/vnet/vnet/ip/ip6_forward.c
+++ b/vnet/vnet/ip/ip6_forward.c
@@ -765,7 +765,7 @@ ip6_lookup_inline (vlib_main_t * vm,
 	  ip0 = vlib_buffer_get_current (p0);
 	  ip1 = vlib_buffer_get_current (p1);
 
-	  if (is_indirect)
+	  if (PREDICT_FALSE(is_indirect))
 	    {
 	      ip_adjacency_t * iadj0, * iadj1;
 	      iadj0 = ip_get_adjacency (lm, vnet_buffer(p0)->ip.adj_index[VLIB_TX]);
@@ -911,7 +911,7 @@ ip6_lookup_inline (vlib_main_t * vm,
 
 	  ip0 = vlib_buffer_get_current (p0);
 
-	  if (is_indirect)
+	  if (PREDICT_FALSE(is_indirect))
 	    {
 	      ip_adjacency_t * iadj0;
 	      iadj0 = ip_get_adjacency (lm, vnet_buffer(p0)->ip.adj_index[VLIB_TX]);
@@ -2813,6 +2813,76 @@ format_ip6_hop_by_hop_trace (u8 * s, va_list * args)
   return s;
 }
 
+always_inline u8 ip6_scan_hbh_options (
+				       vlib_buffer_t * b0,
+				       ip6_header_t *ip0,
+				       ip6_hop_by_hop_header_t *hbh0,
+				       ip6_hop_by_hop_option_t *opt0,
+				       ip6_hop_by_hop_option_t *limit0,
+				       u32 *next0)
+{
+  ip6_hop_by_hop_main_t *hm = &ip6_hop_by_hop_main;
+  u8 type0;
+  u8 error0 = 0;
+
+  while (opt0 < limit0)
+    {
+      type0 = opt0->type;
+      switch (type0)
+	{
+	case 0: /* Pad1 */
+	  opt0 = (ip6_hop_by_hop_option_t *) ((u8 *)opt0) + 1;
+	  continue;
+	case 1: /* PadN */
+	  break;
+	default:
+	  if (hm->options[type0])
+	    {
+	      if ((*hm->options[type0])(b0, ip0, opt0) < 0)
+	        {
+		  error0 = IP6_HOP_BY_HOP_ERROR_FORMAT;
+		  return(error0);
+	        }
+	    }
+	  else
+	    {
+	      /* Unrecognized mandatory option, check the two high order bits */
+	      switch (opt0->type & HBH_OPTION_TYPE_HIGH_ORDER_BITS)
+		{
+		case HBH_OPTION_TYPE_SKIP_UNKNOWN:
+		  break;
+		case HBH_OPTION_TYPE_DISCARD_UNKNOWN:
+		  error0 = IP6_HOP_BY_HOP_ERROR_UNKNOWN_OPTION;
+		  *next0 = IP_LOOKUP_NEXT_DROP;
+		  break;
+		case HBH_OPTION_TYPE_DISCARD_UNKNOWN_ICMP:
+		  error0 = IP6_HOP_BY_HOP_ERROR_UNKNOWN_OPTION;
+		  *next0 = IP_LOOKUP_NEXT_ICMP_ERROR;
+		  icmp6_error_set_vnet_buffer(b0, ICMP6_parameter_problem,
+					      ICMP6_parameter_problem_unrecognized_option, (u8 *)opt0 - (u8 *)ip0);
+		  break;
+		case HBH_OPTION_TYPE_DISCARD_UNKNOWN_ICMP_NOT_MCAST:
+		  error0 = IP6_HOP_BY_HOP_ERROR_UNKNOWN_OPTION;
+		  if (!ip6_address_is_multicast(&ip0->dst_address))
+		    {
+		      *next0 =  IP_LOOKUP_NEXT_ICMP_ERROR;
+		      icmp6_error_set_vnet_buffer(b0, ICMP6_parameter_problem,
+						  ICMP6_parameter_problem_unrecognized_option, (u8 *)opt0 - (u8 *)ip0);
+		    }
+		  else
+		    {
+		      *next0 =  IP_LOOKUP_NEXT_DROP;
+		    }
+		  break;
+		}
+	      return(error0);
+	    }
+	}
+      opt0 = (ip6_hop_by_hop_option_t *) (((u8 *)opt0) + opt0->length + sizeof (ip6_hop_by_hop_option_t));
+    }
+  return(error0);
+}
+
 /*
  * Process the Hop-by-Hop Options header
  */
@@ -2837,6 +2907,116 @@ ip6_hop_by_hop (vlib_main_t * vm,
 
     vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
 
+    while (n_left_from >= 4 && n_left_to_next >= 2) {
+      u32 bi0, bi1;
+      vlib_buffer_t * b0, *b1;
+      u32 next0, next1;
+      ip6_header_t * ip0, *ip1;
+      ip6_hop_by_hop_header_t *hbh0, *hbh1;
+      ip6_hop_by_hop_option_t *opt0, *limit0, *opt1, *limit1;
+      u8 error0 = 0, error1 = 0;
+
+      /* Prefetch next iteration. */
+      {
+	vlib_buffer_t * p2, * p3;
+
+	p2 = vlib_get_buffer (vm, from[2]);
+	p3 = vlib_get_buffer (vm, from[3]);
+
+	vlib_prefetch_buffer_header (p2, LOAD);
+	vlib_prefetch_buffer_header (p3, LOAD);
+
+	CLIB_PREFETCH (p2->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
+	CLIB_PREFETCH (p3->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
+      }
+
+      /* Speculatively enqueue b0, b1 to the current next frame */
+      to_next[0] = bi0 = from[0];
+      to_next[1] = bi1 = from[1];
+      from += 2;
+      to_next += 2;
+      n_left_from -= 2;
+      n_left_to_next -= 2;
+
+      b0 = vlib_get_buffer (vm, bi0);
+      b1 = vlib_get_buffer (vm, bi1);
+      u32 adj_index0 = vnet_buffer(b0)->ip.adj_index[VLIB_TX];
+      ip_adjacency_t *adj0 = ip_get_adjacency(lm, adj_index0);
+      u32 adj_index1 = vnet_buffer(b1)->ip.adj_index[VLIB_TX];
+      ip_adjacency_t *adj1 = ip_get_adjacency(lm, adj_index1);
+
+      /* Default use the next_index from the adjacency. A HBH option rarely redirects to a different node */
+      next0 = adj0->lookup_next_index;
+      next1 = adj1->lookup_next_index;
+
+      ip0 = vlib_buffer_get_current (b0);
+      ip1 = vlib_buffer_get_current (b1);
+      hbh0 = (ip6_hop_by_hop_header_t *)(ip0+1);
+      hbh1 = (ip6_hop_by_hop_header_t *)(ip1+1);
+      opt0 = (ip6_hop_by_hop_option_t *)(hbh0+1);
+      opt1 = (ip6_hop_by_hop_option_t *)(hbh1+1);
+      limit0 = (ip6_hop_by_hop_option_t *)((u8 *)hbh0 + ((hbh0->length + 1) << 3));
+      limit1 = (ip6_hop_by_hop_option_t *)((u8 *)hbh1 + ((hbh1->length + 1) << 3));
+
+      /*
+       * Basic validity checks
+       */
+      if ((hbh0->length + 1) << 3 > clib_net_to_host_u16(ip0->payload_length)) {
+	error0 = IP6_HOP_BY_HOP_ERROR_FORMAT;
+	next0 = IP_LOOKUP_NEXT_DROP;
+	goto outdual;
+      }
+      /* Scan the set of h-b-h options, process ones that we understand */
+      error0 = ip6_scan_hbh_options(b0, ip0, hbh0, opt0, limit0, &next0);
+
+      if ((hbh1->length + 1) << 3 > clib_net_to_host_u16(ip1->payload_length)) {
+	error1 = IP6_HOP_BY_HOP_ERROR_FORMAT;
+	next1 = IP_LOOKUP_NEXT_DROP;
+	goto outdual;
+      }
+      /* Scan the set of h-b-h options, process ones that we understand */
+      error1 = ip6_scan_hbh_options(b1,ip1,hbh1,opt1,limit1, &next1);
+
+    outdual:
+      /* Has the classifier flagged this buffer for special treatment? */
+      if ((error0 == 0) && (vnet_buffer(b0)->l2_classify.opaque_index == OI_DECAP))
+	next0 = hm->next_override;
+
+      /* Has the classifier flagged this buffer for special treatment? */
+      if ((error1 == 0) && (vnet_buffer(b1)->l2_classify.opaque_index == OI_DECAP))
+	next1 = hm->next_override;
+
+      if (PREDICT_FALSE((node->flags & VLIB_NODE_FLAG_TRACE)))
+	{
+	  if (b0->flags & VLIB_BUFFER_IS_TRACED) {
+	    ip6_hop_by_hop_trace_t *t = vlib_add_trace(vm, node, b0, sizeof (*t));
+	    u32 trace_len = (hbh0->length + 1) << 3;
+	    t->next_index = next0;
+	    /* Capture the h-b-h option verbatim */
+	    trace_len = trace_len < ARRAY_LEN(t->option_data) ? trace_len : ARRAY_LEN(t->option_data);
+	    t->trace_len = trace_len;
+	    clib_memcpy(t->option_data, hbh0, trace_len);
+	  }
+	  if (b1->flags & VLIB_BUFFER_IS_TRACED) {
+	    ip6_hop_by_hop_trace_t *t = vlib_add_trace(vm, node, b1, sizeof (*t));
+	    u32 trace_len = (hbh1->length + 1) << 3;
+	    t->next_index = next1;
+	    /* Capture the h-b-h option verbatim */
+	    trace_len = trace_len < ARRAY_LEN(t->option_data) ? trace_len : ARRAY_LEN(t->option_data);
+	    t->trace_len = trace_len;
+	    clib_memcpy(t->option_data, hbh1, trace_len);
+	  }
+
+	}
+
+      b0->error = error_node->errors[error0];
+      b1->error = error_node->errors[error1];
+
+      /* verify speculative enqueue, maybe switch current next frame */
+      vlib_validate_buffer_enqueue_x2 (vm, node, next_index, to_next, n_left_to_next, bi0,
+				       bi1,next0, next1);
+    }
+
     while (n_left_from > 0 && n_left_to_next > 0) {
       u32 bi0;
       vlib_buffer_t * b0;
@@ -2844,7 +3024,6 @@ ip6_hop_by_hop (vlib_main_t * vm,
       ip6_header_t * ip0;
       ip6_hop_by_hop_header_t *hbh0;
       ip6_hop_by_hop_option_t *opt0, *limit0;
-      u8 type0;
       u8 error0 = 0;
 
       /* Speculatively enqueue b0 to the current next frame */
@@ -2876,54 +3055,12 @@ ip6_hop_by_hop (vlib_main_t * vm,
       }
 
       /* Scan the set of h-b-h options, process ones that we understand */
-      while (opt0 < limit0) {
-	type0 = opt0->type;
-	switch (type0) {
-	case 0: /* Pad1 */
-	  opt0 = (ip6_hop_by_hop_option_t *) ((u8 *)opt0) + 1;
-	  continue;
-	case 1: /* PadN */
-	  break;
-	default:
-	  if (hm->options[type0]) {
-	    if ((*hm->options[type0])(b0, ip0, opt0) < 0) {
-	      error0 = IP6_HOP_BY_HOP_ERROR_FORMAT;
-	      goto out0;
-	    }
-	  } else {
-	    /* Unrecognized mandatory option, check the two high order bits */
-	    switch (opt0->type & HBH_OPTION_TYPE_HIGH_ORDER_BITS) {
-	    case HBH_OPTION_TYPE_SKIP_UNKNOWN:
-	      break;
-	    case HBH_OPTION_TYPE_DISCARD_UNKNOWN:
-	      next0 = IP_LOOKUP_NEXT_DROP;
-	      break;
-	    case HBH_OPTION_TYPE_DISCARD_UNKNOWN_ICMP:
-	      next0 = IP_LOOKUP_NEXT_ICMP_ERROR;
-	      icmp6_error_set_vnet_buffer(b0, ICMP6_parameter_problem,
-					  ICMP6_parameter_problem_unrecognized_option, (u8 *)opt0 - (u8 *)ip0);
-	      break;
-	    case HBH_OPTION_TYPE_DISCARD_UNKNOWN_ICMP_NOT_MCAST:
-	      if (!ip6_address_is_multicast(&ip0->dst_address)) {
-		next0 =  IP_LOOKUP_NEXT_ICMP_ERROR;
-		icmp6_error_set_vnet_buffer(b0, ICMP6_parameter_problem,
-					    ICMP6_parameter_problem_unrecognized_option, (u8 *)opt0 - (u8 *)ip0);
-	      } else {
-		next0 =  IP_LOOKUP_NEXT_DROP;
-	      }
-	      break;
-	    }
-	    error0 = IP6_HOP_BY_HOP_ERROR_UNKNOWN_OPTION;
-	    goto out0;
-	  }
-	}
-	opt0 = (ip6_hop_by_hop_option_t *) (((u8 *)opt0) + opt0->length + sizeof (ip6_hop_by_hop_option_t));
-      }
+      error0 = ip6_scan_hbh_options(b0, ip0, hbh0, opt0, limit0, &next0);
 
     out0:
       /* Has the classifier flagged this buffer for special treatment? */
       if ((error0 == 0) && (vnet_buffer(b0)->l2_classify.opaque_index == OI_DECAP))
-	next0 = IP6_LOOKUP_NEXT_POP_HOP_BY_HOP;
+	next0 = hm->next_override;
 
       if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED)) {
 	ip6_hop_by_hop_trace_t *t = vlib_add_trace(vm, node, b0, sizeof (*t));
@@ -2965,12 +3102,19 @@ ip6_hop_by_hop_init (vlib_main_t * vm)
   ip6_hop_by_hop_main_t * hm = &ip6_hop_by_hop_main;
   memset(hm->options, 0, sizeof(hm->options));
   memset(hm->trace, 0, sizeof(hm->trace));
-
+  hm->next_override = IP6_LOOKUP_NEXT_POP_HOP_BY_HOP;
   return (0);
 }
 
 VLIB_INIT_FUNCTION (ip6_hop_by_hop_init);
 
+void ip6_hbh_set_next_override (uword next)
+{
+  ip6_hop_by_hop_main_t * hm = &ip6_hop_by_hop_main;
+
+  hm->next_override = next;
+}
+
 int
 ip6_hbh_register_option (u8 option,
 			 int options(vlib_buffer_t *b, ip6_header_t *ip, ip6_hop_by_hop_option_t *opt),
diff --git a/vnet/vnet/ip/ip6_hop_by_hop.c b/vnet/vnet/ip/ip6_hop_by_hop.c
index c9b151599f0..7038556848c 100644
--- a/vnet/vnet/ip/ip6_hop_by_hop.c
+++ b/vnet/vnet/ip/ip6_hop_by_hop.c
@@ -285,80 +285,114 @@ ip6_add_hop_by_hop_node_fn (vlib_main_t * vm,
 
       vlib_get_next_frame (vm, node, next_index,
 			   to_next, n_left_to_next);
-
-#if 0
       while (n_left_from >= 4 && n_left_to_next >= 2)
-	{
-          u32 next0 = IP6_ADD_HOP_BY_HOP_NEXT_INTERFACE_OUTPUT;
-          u32 next1 = IP6_ADD_HOP_BY_HOP_NEXT_INTERFACE_OUTPUT;
-          u32 sw_if_index0, sw_if_index1;
-          u8 tmp0[6], tmp1[6];
-          ethernet_header_t *en0, *en1;
+        {
           u32 bi0, bi1;
-	  vlib_buffer_t * b0, * b1;
-          
-	  /* Prefetch next iteration. */
-	  {
-	    vlib_buffer_t * p2, * p3;
-            
-	    p2 = vlib_get_buffer (vm, from[2]);
-	    p3 = vlib_get_buffer (vm, from[3]);
-            
-	    vlib_prefetch_buffer_header (p2, LOAD);
-	    vlib_prefetch_buffer_header (p3, LOAD);
+          vlib_buffer_t * b0, *b1;
+          u32 next0, next1;
+          ip6_header_t * ip0, *ip1;
+          ip6_hop_by_hop_header_t * hbh0, *hbh1;
+          u64 * copy_src0, * copy_dst0, *copy_src1, *copy_dst1;
+          u16 new_l0, new_l1;
 
-	    CLIB_PREFETCH (p2->data, CLIB_CACHE_LINE_BYTES, STORE);
-	    CLIB_PREFETCH (p3->data, CLIB_CACHE_LINE_BYTES, STORE);
-	  }
+          /* Prefetch next iteration. */
+          {
+            vlib_buffer_t * p2, * p3;
+
+            p2 = vlib_get_buffer (vm, from[2]);
+            p3 = vlib_get_buffer (vm, from[3]);
+
+            vlib_prefetch_buffer_header (p2, LOAD);
+            vlib_prefetch_buffer_header (p3, LOAD);
+
+            CLIB_PREFETCH (p2->data - rewrite_length, 2 * CLIB_CACHE_LINE_BYTES, STORE);
+            CLIB_PREFETCH (p3->data - rewrite_length, 2 * CLIB_CACHE_LINE_BYTES, STORE);
+          }
 
           /* speculatively enqueue b0 and b1 to the current next frame */
-	  to_next[0] = bi0 = from[0];
-	  to_next[1] = bi1 = from[1];
-	  from += 2;
-	  to_next += 2;
-	  n_left_from -= 2;
-	  n_left_to_next -= 2;
+          to_next[0] = bi0 = from[0];
+          to_next[1] = bi1 = from[1];
+          from += 2;
+          to_next += 2;
+          n_left_from -= 2;
+          n_left_to_next -= 2;
 
-	  b0 = vlib_get_buffer (vm, bi0);
-	  b1 = vlib_get_buffer (vm, bi1);
+          b0 = vlib_get_buffer (vm, bi0);
+          b1 = vlib_get_buffer (vm, bi1);
 
           /* $$$$$ Dual loop: process 2 x packets here $$$$$ */
           ASSERT (b0->current_data == 0);
           ASSERT (b1->current_data == 0);
-          
+
+          ip0 = vlib_buffer_get_current (b0);
+          ip1 = vlib_buffer_get_current (b1);
+
+          /* Copy the ip header left by the required amount */
+          copy_dst0 = (u64 *)(((u8 *)ip0) - rewrite_length);
+          copy_dst1 = (u64 *)(((u8 *)ip1) - rewrite_length);
+          copy_src0 = (u64 *) ip0;
+          copy_src1 = (u64 *) ip1;
+
+          copy_dst0 [0] = copy_src0 [0];
+          copy_dst0 [1] = copy_src0 [1];
+          copy_dst0 [2] = copy_src0 [2];
+          copy_dst0 [3] = copy_src0 [3];
+          copy_dst0 [4] = copy_src0 [4];
+
+          copy_dst1 [0] = copy_src1 [0];
+          copy_dst1 [1] = copy_src1 [1];
+          copy_dst1 [2] = copy_src1 [2];
+          copy_dst1 [3] = copy_src1 [3];
+          copy_dst1 [4] = copy_src1 [4];
+
+          vlib_buffer_advance (b0, - (word)rewrite_length);
+          vlib_buffer_advance (b1, - (word)rewrite_length);
           ip0 = vlib_buffer_get_current (b0);
-          ip1 = vlib_buffer_get_current (b0);
+          ip1 = vlib_buffer_get_current (b1);
+
+          hbh0 = (ip6_hop_by_hop_header_t *)(ip0 + 1);
+          hbh1 = (ip6_hop_by_hop_header_t *)(ip1 + 1);
+          /* $$$ tune, rewrite_length is a multiple of 8 */
+          clib_memcpy (hbh0, rewrite, rewrite_length);
+          clib_memcpy (hbh1, rewrite, rewrite_length);
+          /* Patch the protocol chain, insert the h-b-h (type 0) header */
+          hbh0->protocol = ip0->protocol;
+          hbh1->protocol = ip1->protocol;
+          ip0->protocol = 0;
+          ip1->protocol = 0;
+          new_l0 = clib_net_to_host_u16 (ip0->payload_length) + rewrite_length;
+          new_l1 = clib_net_to_host_u16 (ip1->payload_length) + rewrite_length;
+          ip0->payload_length = clib_host_to_net_u16 (new_l0);
+          ip1->payload_length = clib_host_to_net_u16 (new_l1);
+
+          /* Populate the (first) h-b-h list elt */
+          next0 = IP6_HBYH_IOAM_INPUT_NEXT_IP6_LOOKUP;
+          next1 = IP6_HBYH_IOAM_INPUT_NEXT_IP6_LOOKUP;
 
-          sw_if_index0 = vnet_buffer(b0)->sw_if_index[VLIB_RX];
-          sw_if_index1 = vnet_buffer(b1)->sw_if_index[VLIB_RX];
 
           /* $$$$$ End of processing 2 x packets $$$$$ */
 
           if (PREDICT_FALSE((node->flags & VLIB_NODE_FLAG_TRACE)))
             {
-              if (b0->flags & VLIB_BUFFER_IS_TRACED) 
+              if (b0->flags & VLIB_BUFFER_IS_TRACED)
                 {
-                    ip6_add_hop_by_hop_trace_t *t = 
-                      vlib_add_trace (vm, node, b0, sizeof (*t));
-                    t->sw_if_index = sw_if_index0;
-                    t->next_index = next0;
-                  }
-                if (b1->flags & VLIB_BUFFER_IS_TRACED) 
-                  {
-                    ip6_add_hop_by_hop_trace_t *t = 
-                      vlib_add_trace (vm, node, b1, sizeof (*t));
-                    t->sw_if_index = sw_if_index1;
-                    t->next_index = next1;
-                  }
-              }
-            
-            /* verify speculative enqueues, maybe switch current next frame */
-            vlib_validate_buffer_enqueue_x2 (vm, node, next_index,
-                                             to_next, n_left_to_next,
-                                             bi0, bi1, next0, next1);
+		  ip6_add_hop_by_hop_trace_t *t =
+		    vlib_add_trace (vm, node, b0, sizeof (*t));
+		  t->next_index = next0;
+		}
+	      if (b1->flags & VLIB_BUFFER_IS_TRACED)
+		{
+		  ip6_add_hop_by_hop_trace_t *t =
+		    vlib_add_trace (vm, node, b1, sizeof (*t));
+		  t->next_index = next1;
+		}
+	    }
+	  processed+=2;
+	  /* verify speculative enqueues, maybe switch current next frame */
+	  vlib_validate_buffer_enqueue_x2 (vm, node, next_index,
+					   to_next, n_left_to_next,
+					   bi0, bi1, next0, next1);
         }
-#endif
-
       while (n_left_from > 0 && n_left_to_next > 0)
 	{
           u32 bi0;
@@ -581,24 +615,25 @@ ip6_pop_hop_by_hop_node_fn (vlib_main_t * vm,
       vlib_get_next_frame (vm, node, next_index,
 			   to_next, n_left_to_next);
 
-#if 0
       while (n_left_from >= 4 && n_left_to_next >= 2)
 	{
-          u32 next0 = IP6_POP_HOP_BY_HOP_NEXT_INTERFACE_OUTPUT;
-          u32 next1 = IP6_POP_HOP_BY_HOP_NEXT_INTERFACE_OUTPUT;
-          u32 sw_if_index0, sw_if_index1;
-          u8 tmp0[6], tmp1[6];
-          ethernet_header_t *en0, *en1;
           u32 bi0, bi1;
 	  vlib_buffer_t * b0, * b1;
-          
+	  u32 next0, next1;
+	  u32 adj_index0, adj_index1;
+	  ip6_header_t * ip0, *ip1;
+	  ip_adjacency_t * adj0, *adj1;
+	  ip6_hop_by_hop_header_t *hbh0, *hbh1;
+	  u64 *copy_dst0, *copy_src0, *copy_dst1, *copy_src1;
+	  u16 new_l0, new_l1;
+
 	  /* Prefetch next iteration. */
 	  {
 	    vlib_buffer_t * p2, * p3;
-            
+
 	    p2 = vlib_get_buffer (vm, from[2]);
 	    p3 = vlib_get_buffer (vm, from[3]);
-            
+
 	    vlib_prefetch_buffer_header (p2, LOAD);
 	    vlib_prefetch_buffer_header (p3, LOAD);
 
@@ -620,39 +655,75 @@ ip6_pop_hop_by_hop_node_fn (vlib_main_t * vm,
           /* $$$$$ Dual loop: process 2 x packets here $$$$$ */
           ASSERT (b0->current_data == 0);
           ASSERT (b1->current_data == 0);
-          
+
           ip0 = vlib_buffer_get_current (b0);
-          ip1 = vlib_buffer_get_current (b0);
+          ip1 = vlib_buffer_get_current (b1);
+	  adj_index0 = vnet_buffer (b0)->ip.adj_index[VLIB_TX];
+	  adj_index1 = vnet_buffer (b1)->ip.adj_index[VLIB_TX];
+	  adj0 = ip_get_adjacency (lm, adj_index0);
+	  adj1 = ip_get_adjacency (lm, adj_index1);
+
+	  next0 = adj0->lookup_next_index;
+	  next1 = adj1->lookup_next_index;
+
+	  hbh0 = (ip6_hop_by_hop_header_t *)(ip0+1);
+	  hbh1 = (ip6_hop_by_hop_header_t *)(ip1+1);
+
+	  ioam_pop_hop_by_hop_processing(vm, ip0, hbh0);
+	  ioam_pop_hop_by_hop_processing(vm, ip1, hbh1);
+
+	  vlib_buffer_advance (b0, (hbh0->length+1)<<3);
+	  vlib_buffer_advance (b1, (hbh1->length+1)<<3);
+
+	  new_l0 = clib_net_to_host_u16 (ip0->payload_length) -
+	    ((hbh0->length+1)<<3);
+	  new_l1 = clib_net_to_host_u16 (ip1->payload_length) -
+	    ((hbh1->length+1)<<3);
 
-          sw_if_index0 = vnet_buffer(b0)->sw_if_index[VLIB_RX];
-          sw_if_index1 = vnet_buffer(b1)->sw_if_index[VLIB_RX];
+	  ip0->payload_length = clib_host_to_net_u16 (new_l0);
+	  ip1->payload_length = clib_host_to_net_u16 (new_l1);
+
+	  ip0->protocol = hbh0->protocol;
+	  ip1->protocol = hbh1->protocol;
 
+	  copy_src0 = (u64 *)ip0;
+	  copy_src1 = (u64 *)ip1;
+	  copy_dst0 = copy_src0 + (hbh0->length+1);
+	  copy_dst0 [4] = copy_src0[4];
+	  copy_dst0 [3] = copy_src0[3];
+	  copy_dst0 [2] = copy_src0[2];
+	  copy_dst0 [1] = copy_src0[1];
+	  copy_dst0 [0] = copy_src0[0];
+	  copy_dst1 = copy_src1 + (hbh1->length+1);
+	  copy_dst1 [4] = copy_src1[4];
+	  copy_dst1 [3] = copy_src1[3];
+	  copy_dst1 [2] = copy_src1[2];
+	  copy_dst1 [1] = copy_src1[1];
+	  copy_dst1 [0] = copy_src1[0];
+	  processed+=2;
           /* $$$$$ End of processing 2 x packets $$$$$ */
 
           if (PREDICT_FALSE((node->flags & VLIB_NODE_FLAG_TRACE)))
             {
-              if (b0->flags & VLIB_BUFFER_IS_TRACED) 
+              if (b0->flags & VLIB_BUFFER_IS_TRACED)
                 {
-                    ip6_pop_hop_by_hop_trace_t *t = 
+                    ip6_pop_hop_by_hop_trace_t *t =
                       vlib_add_trace (vm, node, b0, sizeof (*t));
-                    t->sw_if_index = sw_if_index0;
                     t->next_index = next0;
                   }
-                if (b1->flags & VLIB_BUFFER_IS_TRACED) 
+                if (b1->flags & VLIB_BUFFER_IS_TRACED)
                   {
-                    ip6_pop_hop_by_hop_trace_t *t = 
+                    ip6_pop_hop_by_hop_trace_t *t =
                       vlib_add_trace (vm, node, b1, sizeof (*t));
-                    t->sw_if_index = sw_if_index1;
                     t->next_index = next1;
                   }
               }
-            
+
             /* verify speculative enqueues, maybe switch current next frame */
             vlib_validate_buffer_enqueue_x2 (vm, node, next_index,
                                              to_next, n_left_to_next,
                                              bi0, bi1, next0, next1);
         }
-#endif
 
       while (n_left_from > 0 && n_left_to_next > 0)
 	{