Add support for iOAM (sub-TLV) over NSH-MD2. iOAM trace

records are carried as MD2 TLV data.

Change-Id: Id6fe6c961aa3879df5ae476f13b0e39bcc6c0dfe
Signed-off-by: Vengada <venggovi@cisco.com>

8 files changed, 1512 insertions, 3 deletions

diff --git a/nsh-plugin/Makefile.am b/nsh-plugin/Makefile.am
index ffbfaa3..2da0460 100644
--- a/nsh-plugin/Makefile.am
+++ b/nsh-plugin/Makefile.am
@@ -22,7 +22,12 @@ CPPFLAGS += -DDEBUG -g
 
 lib_LTLIBRARIES = nsh_plugin.la nsh_test_plugin.la
 nsh_plugin_la_SOURCES = nsh/nsh.c  \
-	vpp-api/nsh.api.h
+	vpp-api/nsh.api.h \
+	nsh-md2-ioam/nsh_md2_ioam.c \
+	nsh-md2-ioam/nsh_md2_ioam_trace.c \
+	nsh-md2-ioam/md2_ioam_transit.c \
+	nsh-md2-ioam/nsh_md2_ioam_api.c
+
 nsh_plugin_la_LDFLAGS = -module
 
 BUILT_SOURCES = vpp-api/nsh.api.h
diff --git a/nsh-plugin/nsh-md2-ioam/md2_ioam_transit.c b/nsh-plugin/nsh-md2-ioam/md2_ioam_transit.c
new file mode 100644
index 0000000..72176a8
--- /dev/null
+++ b/nsh-plugin/nsh-md2-ioam/md2_ioam_transit.c
@@ -0,0 +1,191 @@
+ /*
+  * Copyright (c) 2017 Cisco and/or its affiliates.
+  * Licensed under the Apache License, Version 2.0 (the "License");
+  * you may not use this file except in compliance with the License.
+  * You may obtain a copy of the License at:
+  *
+  *     http://www.apache.org/licenses/LICENSE-2.0
+  *
+  * Unless required by applicable law or agreed to in writing, software
+  * distributed under the License is distributed on an "AS IS" BASIS,
+  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+  * See the License for the specific language governing permissions and
+  * limitations under the License.
+  */
+#include <vppinfra/error.h>
+#include <vppinfra/hash.h>
+#include <vnet/vnet.h>
+#include <vnet/ip/ip.h>
+#include <vnet/udp/udp.h>
+#include <vnet/ethernet/ethernet.h>
+#include <vnet/lisp-gpe/lisp_gpe.h>
+#include <vnet/lisp-gpe/lisp_gpe_packet.h>
+#include <nsh/nsh.h>
+#include <nsh/nsh_packet.h>
+#include <nsh-md2-ioam/nsh_md2_ioam.h>
+#include <nsh-md2-ioam/nsh_md2_ioam_util.h>
+#include <vnet/fib/ip6_fib.h>
+#include <vnet/fib/ip4_fib.h>
+#include <vnet/fib/fib_entry.h>
+
+/* Statistics (not really errors) */
+#define foreach_nsh_md2_ioam_encap_transit_error    \
+_(ENCAPSULATED, "good packets encapsulated")
+
+static char *nsh_md2_ioam_encap_transit_error_strings[] = {
+#define _(sym,string) string,
+  foreach_nsh_md2_ioam_encap_transit_error
+#undef _
+};
+
+typedef enum
+{
+#define _(sym,str) NSH_MD2_IOAM_ENCAP_TRANSIT_IOAM_ERROR_##sym,
+  foreach_nsh_md2_ioam_encap_transit_error
+#undef _
+    NSH_MD2_IOAM_ENCAP_TRANSIT_IOAM_N_ERROR,
+} nsh_md2_ioam_encap_transit_error_t;
+
+typedef enum
+{
+  NSH_MD2_IOAM_ENCAP_TRANSIT_IOAM_NEXT_OUTPUT,
+  NSH_MD2_IOAM_ENCAP_TRANSIT_IOAM_NEXT_DROP,
+  NSH_MD2_IOAM_ENCAP_TRANSIT_IOAM_N_NEXT
+} nsh_md2_ioam_encap_transit_next_t;
+
+
+/* *INDENT-OFF* */
+VNET_FEATURE_INIT (nsh_md2_ioam_encap_transit, static) =
+{
+  .arc_name = "ip4-output",
+  .node_name = "nsh-md2-ioam-encap-transit",
+  .runs_before = VNET_FEATURES ("adj-midchain-tx"),
+};
+/* *INDENT-ON* */
+
+
+static uword
+nsh_md2_ioam_encap_transit (vlib_main_t * vm,
+			vlib_node_runtime_t * node, vlib_frame_t * from_frame)
+{
+  u32 n_left_from, next_index, *from, *to_next;
+
+  from = vlib_frame_vector_args (from_frame);
+  n_left_from = from_frame->n_vectors;
+
+  next_index = node->cached_next_index;
+
+  while (n_left_from > 0)
+    {
+      u32 n_left_to_next;
+
+      vlib_get_next_frame (vm, node, next_index, to_next, n_left_to_next);
+
+
+      while (n_left_from > 0 && n_left_to_next > 0)
+	{
+	  u32 bi0;
+	  vlib_buffer_t *b0;
+	  u32 next0 = NSH_MD2_IOAM_ENCAP_TRANSIT_IOAM_NEXT_OUTPUT;
+
+	  bi0 = from[0];
+	  to_next[0] = bi0;
+	  from += 1;
+	  to_next += 1;
+	  n_left_from -= 1;
+	  n_left_to_next -= 1;
+	  ip4_header_t *ip0;
+	  u32 iph_offset = 0;
+
+	  b0 = vlib_get_buffer (vm, bi0);
+	  iph_offset = vnet_buffer (b0)->ip.save_rewrite_length;
+	  ip0 = (ip4_header_t *) ((u8 *) vlib_buffer_get_current (b0)
+				  + iph_offset);
+
+	  /* just forward non ipv4 packets */
+	  if (PREDICT_FALSE
+	      ((ip0->ip_version_and_header_length & 0xF0) == 0x40))
+	    {
+	      /* ipv4 packets */
+	      udp_header_t *udp_hdr0 = (udp_header_t *) (ip0 + 1);
+	      if (PREDICT_FALSE
+		  ((ip0->protocol == IP_PROTOCOL_UDP) &&
+		   (clib_net_to_host_u16 (udp_hdr0->dst_port) ==
+		    UDP_DST_PORT_lisp_gpe)))
+		{
+
+		  /* Check the iOAM header */
+		  lisp_gpe_header_t *lisp_gpe_hdr0 = (lisp_gpe_header_t *) (udp_hdr0 + 1);
+                  nsh_base_header_t *nsh_hdr = (nsh_base_header_t *)(lisp_gpe_hdr0 + 1);
+
+		  if (PREDICT_FALSE
+		      (lisp_gpe_hdr0->next_protocol == LISP_GPE_NEXT_PROTO_NSH) &&
+                      (nsh_hdr->md_type == 2 ))
+		    {
+		      uword *t = NULL;
+		      nsh_md2_ioam_main_t *hm = &nsh_md2_ioam_main;
+		      fib_prefix_t key4;
+		      memset (&key4, 0, sizeof (key4));
+		      key4.fp_proto = FIB_PROTOCOL_IP4;
+		      key4.fp_addr.ip4.as_u32 = ip0->dst_address.as_u32;
+		      t = hash_get_mem (hm->dst_by_ip4, &key4);
+		      if (t)
+			{
+			  vlib_buffer_advance (b0,
+					       (word) (sizeof
+						       (ethernet_header_t)));
+			  nsh_md2_ioam_encap_decap_ioam_v4_one_inline (vm, node,
+								    b0,
+								    &next0,
+								    NSH_MD2_IOAM_ENCAP_TRANSIT_IOAM_NEXT_DROP,
+								    1
+								    /* use_adj */
+			    );
+			  vlib_buffer_advance (b0,
+					       -(word) (sizeof
+							(ethernet_header_t)));
+			}
+		    }
+		}
+	    }
+
+	  vlib_validate_buffer_enqueue_x1 (vm, node, next_index, to_next,
+					   n_left_to_next, bi0, next0);
+	}
+
+      vlib_put_next_frame (vm, node, next_index, n_left_to_next);
+    }
+
+  return from_frame->n_vectors;
+}
+
+extern u8 * format_nsh_node_map_trace (u8 * s, va_list * args);
+/* *INDENT-OFF* */
+VLIB_REGISTER_NODE (nsh_md2_ioam_encap_transit_node) = {
+  .function = nsh_md2_ioam_encap_transit,
+  .name = "nsh-md2-ioam-encap-transit",
+  .vector_size = sizeof (u32),
+  .format_trace = format_nsh_node_map_trace,
+  .type = VLIB_NODE_TYPE_INTERNAL,
+
+  .n_errors = ARRAY_LEN(nsh_md2_ioam_encap_transit_error_strings),
+  .error_strings = nsh_md2_ioam_encap_transit_error_strings,
+
+  .n_next_nodes = NSH_MD2_IOAM_ENCAP_TRANSIT_IOAM_N_NEXT,
+
+  .next_nodes = {
+        [NSH_MD2_IOAM_ENCAP_TRANSIT_IOAM_NEXT_OUTPUT] = "interface-output",
+        [NSH_MD2_IOAM_ENCAP_TRANSIT_IOAM_NEXT_DROP] = "error-drop",
+  },
+
+};
+/* *INDENT-ON* */
+
+
+/*
+ * fd.io coding-style-patch-verification: ON
+ *
+ * Local Variables:
+ * eval: (c-set-style "gnu")
+ * End:
+ */
diff --git a/nsh-plugin/nsh-md2-ioam/nsh_md2_ioam.c b/nsh-plugin/nsh-md2-ioam/nsh_md2_ioam.c
new file mode 100644
index 0000000..8f96759
--- /dev/null
+++ b/nsh-plugin/nsh-md2-ioam/nsh_md2_ioam.c
@@ -0,0 +1,513 @@
+/*
+ * nsh_md2_ioam.c - NSH iOAM functions for MD type 2
+ *
+ * Copyright (c) 2017 Cisco and/or its affiliates.
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at:
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include <vnet/vnet.h>
+#include <vnet/plugin/plugin.h>
+#include <nsh/nsh.h>
+#include <nsh/nsh_packet.h>
+#include <vnet/ip/ip.h>
+#include <nsh-md2-ioam/nsh_md2_ioam.h>
+
+#include <vlibapi/api.h>
+#include <vlibmemory/api.h>
+#include <vlibsocket/api.h>
+
+#include <vnet/fib/ip6_fib.h>
+#include <vnet/fib/ip4_fib.h>
+#include <vnet/fib/fib_entry.h>
+
+/* define message IDs */
+#include <vpp-api/nsh_msg_enum.h>
+
+/* define message structures */
+#define vl_typedefs
+#include <vpp-api/nsh_all_api_h.h>
+#undef vl_typedefs
+
+/* define generated endian-swappers */
+#define vl_endianfun
+#include <vpp-api/nsh_all_api_h.h>
+#undef vl_endianfun
+
+nsh_md2_ioam_main_t nsh_md2_ioam_main;
+
+static void
+nsh_md2_ioam_set_clear_output_feature_on_intf (vlib_main_t * vm,
+						    u32 sw_if_index0,
+						    u8 is_add)
+{
+
+
+
+  vnet_feature_enable_disable ("ip4-output",
+			       "nsh-md2-ioam-encap-transit",
+			       sw_if_index0, is_add,
+			       0 /* void *feature_config */ ,
+			       0 /* u32 n_feature_config_bytes */ );
+  return;
+}
+
+void
+nsh_md2_ioam_clear_output_feature_on_all_intfs (vlib_main_t * vm)
+{
+  vnet_sw_interface_t *si = 0;
+  vnet_main_t *vnm = vnet_get_main ();
+  vnet_interface_main_t *im = &vnm->interface_main;
+
+  pool_foreach (si, im->sw_interfaces, (
+					 {
+					 nsh_md2_ioam_set_clear_output_feature_on_intf
+					 (vm, si->sw_if_index, 0);
+					 }));
+  return;
+}
+
+
+extern fib_forward_chain_type_t
+fib_entry_get_default_chain_type (const fib_entry_t * fib_entry);
+
+int
+nsh_md2_ioam_enable_disable_for_dest (vlib_main_t * vm,
+					   ip46_address_t dst_addr,
+					   u32 outer_fib_index,
+					   u8 is_ipv4, u8 is_add)
+{
+  nsh_md2_ioam_main_t *hm = &nsh_md2_ioam_main;
+  u32 fib_index0 = 0;
+
+  fib_node_index_t fei = ~0;
+  u32 *sw_if_index0 = NULL;
+#if 0
+  fib_entry_t *fib_entry;
+  u32 adj_index0;
+  ip_adjacency_t *adj0;
+  load_balance_t *lb_m, *lb_b;
+  const dpo_id_t *dpo0, *dpo1;
+  u32 i, j, k;
+#endif
+  u32 *intf_list = NULL;
+  fib_prefix_t fib_prefix;
+
+  if (is_ipv4)
+    {
+      memset (&fib_prefix, 0, sizeof (fib_prefix_t));
+      fib_prefix.fp_len = 32;
+      fib_prefix.fp_proto = FIB_PROTOCOL_IP4;
+#define  TRANSIT_UNIT_TEST_HACK 1
+#ifdef TRANSIT_UNIT_TEST_HACK
+      memset(&dst_addr, 0, sizeof(dst_addr));
+      dst_addr.ip4.as_u32 = clib_net_to_host_u32(0x14020102);
+#endif
+      fib_prefix.fp_addr = dst_addr;
+    }
+  else
+    {
+      return 0;
+    }
+
+  fei = fib_table_lookup (fib_index0, &fib_prefix);
+#if 0
+  fib_entry = fib_entry_get (fei);
+
+
+  if (!dpo_id_is_valid (&fib_entry->fe_lb))
+    {
+      return (-1);
+    }
+
+  lb_m = load_balance_get (fib_entry->fe_lb.dpoi_index);
+
+  for (i = 0; i < lb_m->lb_n_buckets; i++)
+    {
+      dpo0 = load_balance_get_bucket_i (lb_m, i);
+
+      if (dpo0->dpoi_type == DPO_LOAD_BALANCE ||
+	  dpo0->dpoi_type == DPO_ADJACENCY)
+	{
+	  if (dpo0->dpoi_type == DPO_ADJACENCY)
+	    {
+	      k = 1;
+	    }
+	  else
+	    {
+	      lb_b = load_balance_get (dpo0->dpoi_index);
+	      k = lb_b->lb_n_buckets;
+	    }
+
+	  for (j = 0; j < k; j++)
+	    {
+	      if (dpo0->dpoi_type == DPO_ADJACENCY)
+		{
+		  dpo1 = dpo0;
+		}
+	      else
+		{
+		  dpo1 = load_balance_get_bucket_i (lb_b, j);
+		}
+
+	      if (dpo1->dpoi_type == DPO_ADJACENCY)
+		{
+		  adj_index0 = dpo1->dpoi_index;
+
+		  if (ADJ_INDEX_INVALID == adj_index0)
+		    {
+		      continue;
+		    }
+
+		  adj0 =
+		    ip_get_adjacency (&(ip4_main.lookup_main), adj_index0);
+		  sw_if_index0 = adj0->rewrite_header.sw_if_index;
+
+		  if (~0 == sw_if_index0)
+		    {
+		      continue;
+		    }
+
+
+		  if (is_add)
+		    {
+		      vnet_feature_enable_disable ("ip4-output",
+						   "nsh-md2-ioam-encap-transit",
+						   sw_if_index0, is_add, 0,
+						   /* void *feature_config */
+						   0	/* u32 n_feature_config_bytes */
+			);
+
+		      vec_validate_init_empty (hm->bool_ref_by_sw_if_index,
+					       sw_if_index0, ~0);
+		      hm->bool_ref_by_sw_if_index[sw_if_index0] = 1;
+		    }
+		  else
+		    {
+		      hm->bool_ref_by_sw_if_index[sw_if_index0] = ~0;
+		    }
+		}
+	    }
+	}
+    }
+#else
+
+u32 fib_path_get_resolving_interface (fib_node_index_t path_index);
+    vec_add1(intf_list, fib_path_get_resolving_interface(fei));
+    vec_foreach(sw_if_index0, intf_list)
+    if (is_add)
+      {
+        vnet_feature_enable_disable ("ip4-output",
+                         "nsh-md2-ioam-encap-transit",
+                         *sw_if_index0, is_add, 0,
+                         /* void *feature_config */
+                         0    /* u32 n_feature_config_bytes */
+          );
+
+        vec_validate_init_empty (hm->bool_ref_by_sw_if_index,
+                         *sw_if_index0, ~0);
+        hm->bool_ref_by_sw_if_index[*sw_if_index0] = 1;
+      }
+    else
+      {
+        hm->bool_ref_by_sw_if_index[*sw_if_index0] = ~0;
+      }
+
+#endif
+
+  if (is_ipv4)
+    {
+      uword *t = NULL;
+      nsh_md2_ioam_dest_tunnels_t *t1;
+      fib_prefix_t key4, *key4_copy;
+      hash_pair_t *hp;
+      memset (&key4, 0, sizeof (key4));
+      key4.fp_proto = FIB_PROTOCOL_IP4;
+      key4.fp_addr.ip4.as_u32 = fib_prefix.fp_addr.ip4.as_u32;
+      t = hash_get_mem (hm->dst_by_ip4, &key4);
+      if (is_add)
+	{
+	  if (t)
+	    {
+	      return 0;
+	    }
+	  pool_get_aligned (hm->dst_tunnels, t1, CLIB_CACHE_LINE_BYTES);
+	  memset (t1, 0, sizeof (*t1));
+	  t1->fp_proto = FIB_PROTOCOL_IP4;
+	  t1->dst_addr.ip4.as_u32 = fib_prefix.fp_addr.ip4.as_u32;
+	  key4_copy = clib_mem_alloc (sizeof (*key4_copy));
+          memset(key4_copy, 0, sizeof(*key4_copy));
+	  clib_memcpy (key4_copy, &key4, sizeof (*key4_copy));
+	  hash_set_mem (hm->dst_by_ip4, key4_copy, t1 - hm->dst_tunnels);
+	  /*
+	   * Attach to the FIB entry for the VxLAN-GPE destination
+	   * and become its child. The dest route will invoke a callback
+	   * when the fib entry changes, it can be used to
+	   * re-program the output feature on the egress interface.
+	   */
+
+	  const fib_prefix_t tun_dst_pfx = {
+	    .fp_len = 32,
+	    .fp_proto = FIB_PROTOCOL_IP4,
+	    .fp_addr = {.ip4 = t1->dst_addr.ip4,}
+	  };
+
+	  t1->fib_entry_index =
+	    fib_table_entry_special_add (outer_fib_index,
+					 &tun_dst_pfx,
+					 FIB_SOURCE_RR,
+					 FIB_ENTRY_FLAG_NONE,
+					 ADJ_INDEX_INVALID);
+	  t1->sibling_index =
+	    fib_entry_child_add (t1->fib_entry_index,
+				 hm->fib_entry_type, t1 - hm->dst_tunnels);
+	  t1->outer_fib_index = outer_fib_index;
+
+	}
+      else
+	{
+	  if (!t)
+	    {
+	      return 0;
+	    }
+	  t1 = pool_elt_at_index (hm->dst_tunnels, t[0]);
+	  hp = hash_get_pair (hm->dst_by_ip4, &key4);
+	  key4_copy = (void *) (hp->key);
+	  hash_unset_mem (hm->dst_by_ip4, &key4);
+	  clib_mem_free (key4_copy);
+	  pool_put (hm->dst_tunnels, t1);
+	}
+    }
+  else
+    {
+      // TBD for IPv6
+    }
+
+  return 0;
+}
+
+void
+nsh_md2_ioam_refresh_output_feature_on_all_dest (void)
+{
+  nsh_md2_ioam_main_t *hm = &nsh_md2_ioam_main;
+  nsh_main_t *gm = &nsh_main;
+  nsh_md2_ioam_dest_tunnels_t *t;
+  u32 i;
+
+  if (pool_elts (hm->dst_tunnels) == 0)
+    return;
+
+  nsh_md2_ioam_clear_output_feature_on_all_intfs (gm->vlib_main);
+  i = vec_len (hm->bool_ref_by_sw_if_index);
+  vec_free (hm->bool_ref_by_sw_if_index);
+  vec_validate_init_empty (hm->bool_ref_by_sw_if_index, i, ~0);
+  pool_foreach (t, hm->dst_tunnels, (
+				      {
+				      nsh_md2_ioam_enable_disable_for_dest
+				      (gm->vlib_main,
+				       t->dst_addr,
+				       t->outer_fib_index,
+				       (t->fp_proto == FIB_PROTOCOL_IP4), 1
+				       /* is_add */
+				      );
+				      }
+		));
+  return;
+}
+
+void
+nsh_md2_ioam_clear_output_feature_on_select_intfs (void)
+{
+  nsh_md2_ioam_main_t *hm = &nsh_md2_ioam_main;
+  nsh_main_t *gm = &nsh_main;
+
+  u32 sw_if_index0 = 0;
+  for (sw_if_index0 = 0;
+       sw_if_index0 < vec_len (hm->bool_ref_by_sw_if_index); sw_if_index0++)
+    {
+      if (hm->bool_ref_by_sw_if_index[sw_if_index0] == 0xFF)
+	{
+	  nsh_md2_ioam_set_clear_output_feature_on_intf
+	    (gm->vlib_main, sw_if_index0, 0);
+	}
+    }
+
+  return;
+}
+
+
+
+
+clib_error_t *
+nsh_md2_ioam_enable_disable (int has_trace_option, int has_pot_option,
+				  int has_ppc_option)
+{
+  nsh_md2_ioam_main_t *hm = &nsh_md2_ioam_main;
+
+  hm->has_trace_option = has_trace_option;
+  hm->has_pot_option = has_pot_option;
+  hm->has_ppc_option = has_ppc_option;
+
+  if (hm->has_trace_option)
+    {
+      nsh_md2_ioam_trace_profile_setup ();
+    }
+  else if (!hm->has_trace_option)
+    {
+      nsh_md2_ioam_trace_profile_cleanup ();
+    }
+
+  return 0;
+}
+
+
+int nsh_md2_ioam_disable_for_dest
+  (vlib_main_t * vm, ip46_address_t dst_addr, u32 outer_fib_index,
+   u8 ipv4_set)
+{
+  nsh_md2_ioam_dest_tunnels_t *t;
+  nsh_md2_ioam_main_t *hm = &nsh_md2_ioam_main;
+  nsh_main_t *gm = &nsh_main;
+
+  nsh_md2_ioam_enable_disable_for_dest (gm->vlib_main,
+					     dst_addr, outer_fib_index,
+					     ipv4_set, 0);
+  if (pool_elts (hm->dst_tunnels) == 0)
+    {
+      nsh_md2_ioam_clear_output_feature_on_select_intfs ();
+      return 0;
+    }
+
+  pool_foreach (t, hm->dst_tunnels, (
+				      {
+				      nsh_md2_ioam_enable_disable_for_dest
+				      (gm->vlib_main,
+				       t->dst_addr,
+				       t->outer_fib_index,
+				       (t->fp_proto ==
+					FIB_PROTOCOL_IP4), 1 /* is_add */ );
+				      }
+		));
+  nsh_md2_ioam_clear_output_feature_on_select_intfs ();
+  return (0);
+
+}
+
+static clib_error_t *nsh_md2_ioam_set_transit_rewrite_command_fn
+  (vlib_main_t * vm, unformat_input_t * input, vlib_cli_command_t * cmd)
+{
+  nsh_main_t *gm = &nsh_main;
+  ip46_address_t dst_addr;
+  u8 dst_addr_set = 0;
+  u8 ipv4_set = 0;
+  u8 ipv6_set = 0;
+  u8 disable = 0;
+  clib_error_t *rv = 0;
+  u32 outer_fib_index = 0;
+  while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
+    {
+      if (unformat (input, "dst-ip %U", unformat_ip4_address, &dst_addr.ip4))
+	{
+	  dst_addr_set = 1;
+	  ipv4_set = 1;
+	}
+      else
+	if (unformat
+	    (input, "dst-ip %U", unformat_ip6_address, &dst_addr.ip6))
+	{
+	  dst_addr_set = 1;
+	  ipv6_set = 1;
+	}
+      else if (unformat (input, "outer-fib-index %d", &outer_fib_index))
+	{
+	}
+
+      else if (unformat (input, "disable"))
+	disable = 1;
+      else
+	break;
+    }
+
+  if (dst_addr_set == 0)
+    return clib_error_return (0,
+			      "LISP-GPE Tunnel destination address not specified");
+  if (ipv4_set && ipv6_set)
+    return clib_error_return (0, "both IPv4 and IPv6 addresses specified");
+  if (!disable)
+    {
+      nsh_md2_ioam_enable_disable_for_dest (gm->vlib_main,
+						 dst_addr, outer_fib_index,
+						 ipv4_set, 1);
+    }
+  else
+    {
+      nsh_md2_ioam_disable_for_dest
+	(vm, dst_addr, outer_fib_index, ipv4_set);
+    }
+  return rv;
+}
+
+/* *INDENT-OFF* */
+VLIB_CLI_COMMAND (nsh_md2_ioam_set_transit_rewrite_cmd, static) = {
+  .path = "set nsh-md2-ioam-transit",
+  .short_help = "set nsh-ioam-lisp-gpe-transit dst-ip <dst_ip> [outer-fib-index <outer_fib_index>] [disable]",
+  .function = nsh_md2_ioam_set_transit_rewrite_command_fn,
+};
+
+/**
+ * Function definition to backwalk a FIB node
+ */
+static fib_node_back_walk_rc_t
+nsh_md2_ioam_back_walk (fib_node_t * node, fib_node_back_walk_ctx_t * ctx)
+{
+  nsh_md2_ioam_refresh_output_feature_on_all_dest ();
+  return (FIB_NODE_BACK_WALK_CONTINUE);
+}
+
+/**
+ * Function definition to get a FIB node from its index
+ */
+static fib_node_t *
+nsh_md2_ioam_fib_node_get (fib_node_index_t index)
+{
+  nsh_md2_ioam_main_t *hm = &nsh_md2_ioam_main;
+  return (&hm->node);
+}
+
+/**
+ * Function definition to inform the FIB node that its last lock has gone.
+ */
+static void
+nsh_md2_ioam_last_lock_gone (fib_node_t * node)
+{
+  ASSERT (0);
+}
+
+
+/*
+ * Virtual function table registered by MPLS GRE tunnels
+ * for participation in the FIB object graph.
+ */
+const static fib_node_vft_t nsh_md2_ioam_vft = {
+  .fnv_get = nsh_md2_ioam_fib_node_get,
+  .fnv_last_lock = nsh_md2_ioam_last_lock_gone,
+  .fnv_back_walk = nsh_md2_ioam_back_walk,
+};
+
+void
+nsh_md2_ioam_interface_init (void)
+{
+  nsh_md2_ioam_main_t *hm = &nsh_md2_ioam_main;
+  hm->fib_entry_type = fib_node_register_new_type (&nsh_md2_ioam_vft);
+  return;
+}
+
diff --git a/nsh-plugin/nsh-md2-ioam/nsh_md2_ioam.h b/nsh-plugin/nsh-md2-ioam/nsh_md2_ioam.h
new file mode 100644
index 0000000..3d48fde
--- /dev/null
+++ b/nsh-plugin/nsh-md2-ioam/nsh_md2_ioam.h
@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 2017 Cisco and/or its affiliates.
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at:
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef __included_nsh_md2_ioam_h__
+#define __included_nsh_md2_ioam_h__
+
+#include <nsh/nsh.h>
+#include <nsh/nsh_packet.h>
+#include <vnet/ip/ip.h>
+#include <vnet/ip/ip4_packet.h>
+#include <vnet/ip/ip6_packet.h>
+
+
+typedef struct nsh_md2_ioam_sw_interface_
+{
+  u32 sw_if_index;
+} nsh_md2_ioam_sw_interface_t;
+
+typedef struct nsh_md2_ioam_dest_tunnels_
+{
+  ip46_address_t dst_addr;
+  u32 fp_proto;
+  u32 sibling_index;
+  fib_node_index_t fib_entry_index;
+  u32 outer_fib_index;
+} nsh_md2_ioam_dest_tunnels_t;
+
+typedef struct nsh_md2_ioam_main_
+{
+  /**
+   * Linkage into the FIB object graph
+   */
+  fib_node_t node;
+
+  /* time scale transform. Joy. */
+  u32 unix_time_0;
+  f64 vlib_time_0;
+
+
+  /* Trace option */
+  u8 has_trace_option;
+
+  /* Pot option */
+  u8 has_pot_option;
+
+#define PPC_NONE  0
+#define PPC_ENCAP 1
+#define PPC_DECAP 2
+  u8 has_ppc_option;
+
+#define TSP_SECONDS              0
+#define TSP_MILLISECONDS         1
+#define TSP_MICROSECONDS         2
+#define TSP_NANOSECONDS          3
+
+
+  /* API message ID base */
+  u16 msg_id_base;
+
+  /* Override to export for iOAM */
+  uword decap_v4_next_override;
+  uword decap_v6_next_override;
+
+  /* sequence of node graph for encap */
+  uword encap_v4_next_node;
+  uword encap_v6_next_node;
+
+  /* Software interfaces. */
+  nsh_md2_ioam_sw_interface_t *sw_interfaces;
+
+  /* hash ip4/ip6 -> list of destinations for doing transit iOAM operation */
+  nsh_md2_ioam_dest_tunnels_t *dst_tunnels;
+  uword *dst_by_ip4;
+  uword *dst_by_ip6;
+
+  /** per sw_if_index, to maintain bitmap */
+  u8 *bool_ref_by_sw_if_index;
+  fib_node_type_t fib_entry_type;
+
+
+} nsh_md2_ioam_main_t;
+extern nsh_md2_ioam_main_t nsh_md2_ioam_main;
+
+/*
+ * Primary h-b-h handler trace support
+ */
+typedef struct
+{
+  u32 next_index;
+  u32 trace_len;
+  u8 option_data[256];
+} ioam_trace_t;
+
+
+clib_error_t *nsh_md2_ioam_enable_disable (int has_trace_option,
+						int has_pot_option,
+						int has_ppc_option);
+
+
+
+int nsh_md2_ioam_trace_profile_setup (void);
+
+int nsh_md2_ioam_trace_profile_cleanup (void);
+extern void nsh_md2_ioam_interface_init (void);
+
+
+
+#endif
diff --git a/nsh-plugin/nsh-md2-ioam/nsh_md2_ioam_api.c b/nsh-plugin/nsh-md2-ioam/nsh_md2_ioam_api.c
new file mode 100644
index 0000000..42172cb
--- /dev/null
+++ b/nsh-plugin/nsh-md2-ioam/nsh_md2_ioam_api.c
@@ -0,0 +1,88 @@
+/*
+ * Copyright (c) 2016 Cisco and/or its affiliates.
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at:
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+/*
+ *-----------------------------------------------------------------------
+ * nsh_md2_ioam_api.c - iOAM for NSH/LISP-GPE related APIs to create
+ *               and maintain profiles
+ *-----------------------------------------------------------------------
+ */
+
+#include <vnet/vnet.h>
+#include <vlib/unix/plugin.h>
+#include <vnet/plugin/plugin.h>
+#include <nsh-md2-ioam/nsh_md2_ioam.h>
+
+#include <vlibapi/api.h>
+#include <vlibmemory/api.h>
+#include <vlibsocket/api.h>
+
+/* define message IDs */
+#include <vpp-api/nsh_msg_enum.h>
+
+/* define message structures */
+#define vl_typedefs
+#include <vpp-api/nsh_all_api_h.h>
+#undef vl_typedefs
+
+/* define generated endian-swappers */
+#define vl_endianfun
+#include <vpp-api/nsh_all_api_h.h>
+#undef vl_endianfun
+
+/* instantiate all the print functions we know about */
+#define vl_print(handle, ...) vlib_cli_output (handle, __VA_ARGS__)
+#define vl_printfun
+#include <vpp-api/nsh_all_api_h.h>
+#undef vl_printfun
+
+/* Get the API version number */
+#if 0
+#define vl_api_version(n,v) static u32 api_version=(v);
+#include <vpp-api/nsh_all_api_h.h>
+#undef vl_api_version
+#endif
+
+u8 *nsh_trace_main = NULL;
+static clib_error_t *
+nsh_md2_ioam_init (vlib_main_t * vm)
+{
+  nsh_md2_ioam_main_t *sm = &nsh_md2_ioam_main;
+  clib_error_t *error = 0;
+
+  nsh_trace_main =
+    (u8 *) vlib_get_plugin_symbol ("ioam_plugin.so", "trace_main");
+
+  if (!nsh_trace_main)
+    return error;
+
+  vec_new (nsh_md2_ioam_sw_interface_t, pool_elts (sm->sw_interfaces));
+  sm->dst_by_ip4 = hash_create_mem (0, sizeof (fib_prefix_t), sizeof (uword));
+
+  sm->dst_by_ip6 = hash_create_mem (0, sizeof (fib_prefix_t), sizeof (uword));
+
+  nsh_md2_ioam_interface_init ();
+
+  return error;
+}
+
+VLIB_INIT_FUNCTION (nsh_md2_ioam_init);
+
+/*
+ * fd.io coding-style-patch-verification: ON
+ *
+ * Local Variables:
+ * eval: (c-set-style "gnu")
+ * End:
+ */
diff --git a/nsh-plugin/nsh-md2-ioam/nsh_md2_ioam_trace.c b/nsh-plugin/nsh-md2-ioam/nsh_md2_ioam_trace.c
new file mode 100644
index 0000000..13bf433
--- /dev/null
+++ b/nsh-plugin/nsh-md2-ioam/nsh_md2_ioam_trace.c
@@ -0,0 +1,471 @@
+/*
+ * Copyright (c) 2017 Cisco and/or its affiliates.
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at:
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#include <vlib/vlib.h>
+#include <vnet/vnet.h>
+#include <vnet/pg/pg.h>
+#include <vppinfra/error.h>
+
+#include <vppinfra/hash.h>
+#include <vppinfra/error.h>
+#include <vppinfra/elog.h>
+
+#include <ioam/lib-trace/trace_util.h>
+#include <nsh-md2-ioam/nsh_md2_ioam.h>
+#include <nsh/nsh_packet.h>
+
+/* Timestamp precision multipliers for seconds, milliseconds, microseconds
+ * and nanoseconds respectively.
+ */
+static f64 trace_tsp_mul[4] = { 1, 1e3, 1e6, 1e9 };
+#define NSH_MD2_IOAM_TRACE_SIZE_DUMMY 20
+
+typedef union
+{
+  u64 as_u64;
+  u32 as_u32[2];
+} time_u64_t;
+
+
+/* *INDENT-OFF* */
+typedef CLIB_PACKED(struct {
+  u16 class;
+  u8 type;
+  u8 length;
+  u8 data_list_elts_left;
+  u16 ioam_trace_type;
+  u8 reserve;
+  u32 elts[0]; /* Variable type. So keep it generic */
+}) nsh_md2_ioam_trace_option_t;
+/* *INDENT-ON* */
+
+
+#define foreach_nsh_md2_ioam_trace_stats				\
+  _(SUCCESS, "Pkts updated with TRACE records")					\
+  _(FAILED, "Errors in TRACE due to lack of TRACE records")
+
+static char *nsh_md2_ioam_trace_stats_strings[] = {
+#define _(sym,string) string,
+  foreach_nsh_md2_ioam_trace_stats
+#undef _
+};
+
+typedef enum
+{
+#define _(sym,str) NSH_MD2_IOAM_TRACE_##sym,
+  foreach_nsh_md2_ioam_trace_stats
+#undef _
+    NSH_MD2_IOAM_TRACE_N_STATS,
+} nsh_md2_ioam_trace_stats_t;
+
+
+typedef struct
+{
+  /* stats */
+  u64 counters[ARRAY_LEN (nsh_md2_ioam_trace_stats_strings)];
+
+  /* convenience */
+  vlib_main_t *vlib_main;
+  vnet_main_t *vnet_main;
+} nsh_md2_ioam_trace_main_t;
+
+nsh_md2_ioam_trace_main_t nsh_md2_ioam_trace_main;
+
+/*
+ * Find a trace profile
+ */
+
+extern u8 *nsh_trace_main;
+always_inline trace_profile *
+nsh_trace_profile_find (void)
+{
+  trace_main_t *sm = (trace_main_t *) nsh_trace_main;
+
+  return (&(sm->profile));
+}
+
+
+always_inline void
+nsh_md2_ioam_trace_stats_increment_counter (u32 counter_index,
+						 u64 increment)
+{
+  nsh_md2_ioam_trace_main_t *hm = &nsh_md2_ioam_trace_main;
+
+  hm->counters[counter_index] += increment;
+}
+
+
+static u8 *
+format_ioam_data_list_element (u8 * s, va_list * args)
+{
+  u32 *elt = va_arg (*args, u32 *);
+  u8 *trace_type_p = va_arg (*args, u8 *);
+  u8 trace_type = *trace_type_p;
+
+
+  if (trace_type & BIT_TTL_NODEID)
+    {
+      u32 ttl_node_id_host_byte_order = clib_net_to_host_u32 (*elt);
+      s = format (s, "ttl 0x%x node id 0x%x ",
+		  ttl_node_id_host_byte_order >> 24,
+		  ttl_node_id_host_byte_order & 0x00FFFFFF);
+
+      elt++;
+    }
+
+  if (trace_type & BIT_ING_INTERFACE && trace_type & BIT_ING_INTERFACE)
+    {
+      u32 ingress_host_byte_order = clib_net_to_host_u32 (*elt);
+      s = format (s, "ingress 0x%x egress 0x%x ",
+		  ingress_host_byte_order >> 16,
+		  ingress_host_byte_order & 0xFFFF);
+      elt++;
+    }
+
+  if (trace_type & BIT_TIMESTAMP)
+    {
+      u32 ts_in_host_byte_order = clib_net_to_host_u32 (*elt);
+      s = format (s, "ts 0x%x \n", ts_in_host_byte_order);
+      elt++;
+    }
+
+  if (trace_type & BIT_APPDATA)
+    {
+      u32 appdata_in_host_byte_order = clib_net_to_host_u32 (*elt);
+      s = format (s, "app 0x%x ", appdata_in_host_byte_order);
+      elt++;
+    }
+
+  return s;
+}
+
+
+
+int
+nsh_md2_ioam_trace_rewrite_handler (u8 * rewrite_string,
+					 u8 * rewrite_size)
+{
+  nsh_md2_ioam_trace_option_t *trace_option = NULL;
+  u8 trace_data_size = 0;
+  u8 trace_option_elts = 0;
+  trace_profile *profile = NULL;
+
+  profile = nsh_trace_profile_find ();
+
+  if (PREDICT_FALSE (!profile))
+    {
+      return (-1);
+    }
+
+  if (PREDICT_FALSE (!rewrite_string))
+    return -1;
+
+  trace_option_elts = profile->num_elts;
+  trace_data_size = fetch_trace_data_size (profile->trace_type);
+
+  trace_option = (nsh_md2_ioam_trace_option_t *) rewrite_string;
+  trace_option->class = clib_host_to_net_u16(0x9);
+  trace_option->type = NSH_MD2_IOAM_OPTION_TYPE_TRACE;
+  trace_option->length = (trace_option_elts * trace_data_size) + 4;
+  trace_option->data_list_elts_left = trace_option_elts;
+  trace_option->ioam_trace_type =
+        clib_host_to_net_u16(profile->trace_type & TRACE_TYPE_MASK);
+
+  *rewrite_size =
+    sizeof (nsh_md2_ioam_trace_option_t) +
+    (trace_option_elts * trace_data_size);
+
+  return 0;
+}
+
+
+int
+nsh_md2_ioam_trace_data_list_handler (vlib_buffer_t * b,
+					   nsh_tlv_header_t * opt)
+{
+  u8 elt_index = 0;
+  nsh_md2_ioam_trace_option_t *trace =
+    (nsh_md2_ioam_trace_option_t *) ((u8 *)opt);
+  time_u64_t time_u64;
+  u32 *elt;
+  int rv = 0;
+  trace_profile *profile = NULL;
+  nsh_md2_ioam_main_t *hm = &nsh_md2_ioam_main;
+  nsh_main_t *gm = &nsh_main;
+  u16 ioam_trace_type = 0;
+
+  profile = nsh_trace_profile_find ();
+
+  if (PREDICT_FALSE (!profile))
+    {
+      return (-1);
+    }
+
+
+  ioam_trace_type = profile->trace_type & TRACE_TYPE_MASK;
+  time_u64.as_u64 = 0;
+
+  if (PREDICT_TRUE (trace->data_list_elts_left))
+    {
+      trace->data_list_elts_left--;
+      /* fetch_trace_data_size returns in bytes. Convert it to 4-bytes
+       * to skip to this node's location.
+       */
+      elt_index =
+	trace->data_list_elts_left *
+	fetch_trace_data_size (ioam_trace_type) / 4;
+      elt = &trace->elts[elt_index];
+	  if (ioam_trace_type & BIT_TTL_NODEID)
+	    {
+	      ip4_header_t *ip0 = vlib_buffer_get_current (b);
+	      *elt = clib_host_to_net_u32 (((ip0->ttl - 1) << 24) |
+					   profile->node_id);
+	      elt++;
+	    }
+
+	  if (ioam_trace_type & BIT_ING_INTERFACE)
+	    {
+	      u16 tx_if = vnet_buffer(b)->sw_if_index[VLIB_TX];
+
+	      *elt =
+		(vnet_buffer (b)->sw_if_index[VLIB_RX] & 0xFFFF) << 16 |
+		tx_if;
+	      *elt = clib_host_to_net_u32 (*elt);
+	      elt++;
+	    }
+
+
+      if (ioam_trace_type & BIT_TIMESTAMP)
+	{
+	  /* Send least significant 32 bits */
+	  f64 time_f64 =
+	    (f64) (((f64) hm->unix_time_0) +
+		   (vlib_time_now (gm->vlib_main) - hm->vlib_time_0));
+
+	  time_u64.as_u64 = time_f64 * trace_tsp_mul[profile->trace_tsp];
+	  *elt = clib_host_to_net_u32 (time_u64.as_u32[0]);
+	  elt++;
+	}
+
+      if (ioam_trace_type & BIT_APPDATA)
+	{
+	  /* $$$ set elt0->app_data */
+	  *elt = clib_host_to_net_u32 (profile->app_data);
+	  elt++;
+	}
+      nsh_md2_ioam_trace_stats_increment_counter
+	(NSH_MD2_IOAM_TRACE_SUCCESS, 1);
+    }
+  else
+    {
+      nsh_md2_ioam_trace_stats_increment_counter
+	(NSH_MD2_IOAM_TRACE_FAILED, 1);
+    }
+  return (rv);
+}
+
+
+
+u8 *
+nsh_md2_ioam_trace_data_list_trace_handler (u8 * s,
+					    nsh_tlv_header_t * opt)
+{
+  nsh_md2_ioam_trace_option_t *trace;
+  u8 trace_data_size_in_words = 0;
+  u32 *elt;
+  int elt_index = 0;
+  u16 ioam_trace_type = 0;
+
+  trace = (nsh_md2_ioam_trace_option_t *) ((u8 *)opt);
+  ioam_trace_type = clib_net_to_host_u16(trace->ioam_trace_type);
+  trace_data_size_in_words =
+    fetch_trace_data_size (ioam_trace_type) / 4;
+  elt = &trace->elts[0];
+  s =
+    format (s, "  Trace Type 0x%x , %d elts left\n", ioam_trace_type,
+	    trace->data_list_elts_left);
+  while ((u8 *) elt < ((u8 *) (&trace->elts[0]) + trace->length  - 4
+		       /* -2 accounts for ioam_trace_type,elts_left */ ))
+    {
+      s = format (s, "    [%d] %U\n", elt_index,
+		  format_ioam_data_list_element,
+		  elt, &ioam_trace_type);
+      elt_index++;
+      elt += trace_data_size_in_words;
+    }
+  return (s);
+}
+
+int
+nsh_md2_ioam_trace_swap_handler (vlib_buffer_t * b,
+				 nsh_tlv_header_t * old_opt,
+				 nsh_tlv_header_t * new_opt)
+{
+
+  clib_memcpy(new_opt, old_opt, new_opt->length + sizeof(nsh_tlv_header_t));
+  return nsh_md2_ioam_trace_data_list_handler (b, new_opt);
+}
+
+static clib_error_t *
+nsh_md2_ioam_show_ioam_trace_cmd_fn (vlib_main_t * vm,
+					  unformat_input_t * input,
+					  vlib_cli_command_t * cmd)
+{
+  nsh_md2_ioam_trace_main_t *hm = &nsh_md2_ioam_trace_main;
+  u8 *s = 0;
+  int i = 0;
+
+  for (i = 0; i < NSH_MD2_IOAM_TRACE_N_STATS; i++)
+    {
+      s = format (s, " %s - %lu\n", nsh_md2_ioam_trace_stats_strings[i],
+		  hm->counters[i]);
+    }
+
+  vlib_cli_output (vm, "%v", s);
+  vec_free (s);
+  return 0;
+}
+
+
+/* *INDENT-OFF* */
+VLIB_CLI_COMMAND (nsh_md2_ioam_show_ioam_trace_cmd, static) = {
+  .path = "show ioam nsh-lisp-gpe trace",
+  .short_help = "iOAM trace statistics",
+  .function = nsh_md2_ioam_show_ioam_trace_cmd_fn,
+};
+/* *INDENT-ON* */
+
+
+int 
+nsh_md2_ioam_trace_pop_handler (vlib_buffer_t * b,
+		     nsh_tlv_header_t * opt)
+{
+  return nsh_md2_ioam_trace_data_list_handler (b, opt);
+}
+
+static clib_error_t *
+nsh_md2_ioam_trace_init (vlib_main_t * vm)
+{
+  nsh_md2_ioam_trace_main_t *hm = &nsh_md2_ioam_trace_main;
+  nsh_md2_ioam_main_t *gm = &nsh_md2_ioam_main;
+  clib_error_t *error;
+
+  if ((error = vlib_call_init_function (vm, nsh_init)))
+    return (error);
+
+  if ((error = vlib_call_init_function (vm, nsh_md2_ioam_init)))
+    return (error);
+
+  hm->vlib_main = vm;
+  hm->vnet_main = vnet_get_main ();
+  gm->unix_time_0 = (u32) time (0);     /* Store starting time */
+  gm->vlib_time_0 = vlib_time_now (vm);
+
+  memset (hm->counters, 0, sizeof (hm->counters));
+
+  if (nsh_md2_register_option
+      (clib_host_to_net_u16(0x9), 
+       NSH_MD2_IOAM_OPTION_TYPE_TRACE, 
+       NSH_MD2_IOAM_TRACE_SIZE_DUMMY,
+       nsh_md2_ioam_trace_rewrite_handler, 
+       nsh_md2_ioam_trace_data_list_handler,
+       nsh_md2_ioam_trace_swap_handler,
+       nsh_md2_ioam_trace_pop_handler,
+       nsh_md2_ioam_trace_data_list_trace_handler) < 0)
+    return (clib_error_create
+	    ("registration of NSH_MD2_IOAM_OPTION_TYPE_TRACE failed"));
+
+  return (0);
+}
+
+VLIB_INIT_FUNCTION (nsh_md2_ioam_trace_init);
+
+int
+nsh_md2_ioam_trace_profile_cleanup (void)
+{
+  nsh_main_t *hm = &nsh_main;
+
+  hm->options_size[NSH_MD2_IOAM_OPTION_TYPE_TRACE] = 0;
+
+  return 0;
+
+}
+
+static int
+nsh_md2_ioam_trace_get_sizeof_handler (u32 * result)
+{
+  u16 size = 0;
+  u8 trace_data_size = 0;
+  trace_profile *profile = NULL;
+
+  *result = 0;
+
+  profile = nsh_trace_profile_find ();
+
+  if (PREDICT_FALSE (!profile))
+    {
+      return (-1);
+    }
+
+  trace_data_size = fetch_trace_data_size (profile->trace_type);
+  if (PREDICT_FALSE (trace_data_size == 0))
+    return VNET_API_ERROR_INVALID_VALUE;
+
+  if (PREDICT_FALSE (profile->num_elts * trace_data_size > 254))
+    return VNET_API_ERROR_INVALID_VALUE;
+
+  size +=
+    sizeof (nsh_md2_ioam_trace_option_t) +
+    profile->num_elts * trace_data_size;
+  *result = size;
+
+  return 0;
+}
+
+
+int
+nsh_md2_ioam_trace_profile_setup (void)
+{
+  u32 trace_size = 0;
+  nsh_main_t *hm = &nsh_main;
+
+  trace_profile *profile = NULL;
+
+
+  profile = nsh_trace_profile_find ();
+
+  if (PREDICT_FALSE (!profile))
+    {
+      return (-1);
+    }
+
+
+  if (nsh_md2_ioam_trace_get_sizeof_handler (&trace_size) < 0)
+    return (-1);
+
+  hm->options_size[NSH_MD2_IOAM_OPTION_TYPE_TRACE] = trace_size;
+
+  return (0);
+}
+
+
+
+/*
+ * fd.io coding-style-patch-verification: ON
+ *
+ * Local Variables:
+ * eval: (c-set-style "gnu")
+ * End:
+ */
+
diff --git a/nsh-plugin/nsh-md2-ioam/nsh_md2_ioam_util.h b/nsh-plugin/nsh-md2-ioam/nsh_md2_ioam_util.h
new file mode 100644
index 0000000..a4fb874
--- /dev/null
+++ b/nsh-plugin/nsh-md2-ioam/nsh_md2_ioam_util.h
@@ -0,0 +1,119 @@
+/*
+ * Copyright (c) 2017 Cisco and/or its affiliates.
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at:
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+#ifndef __included_nsh_md2_ioam_util_h__
+#define __included_nsh_md2_ioam_util_h__
+
+#include <vnet/lisp-gpe/lisp_gpe.h>
+#include <vnet/lisp-gpe/lisp_gpe_packet.h>
+#include <vnet/ip/ip.h>
+#include <nsh/nsh.h>
+#include <nsh-md2-ioam/nsh_md2_ioam.h>
+#include <nsh/nsh_packet.h>
+
+
+extern nsh_option_map_t * nsh_md2_lookup_option (u16 class, u8 type);
+
+
+typedef struct {
+   u8 trace_data[256];
+} nsh_transit_trace_t;
+
+always_inline void
+nsh_md2_ioam_encap_decap_ioam_v4_one_inline (vlib_main_t * vm,
+					  vlib_node_runtime_t * node,
+					  vlib_buffer_t * b0,
+					  u32 * next0, u32 drop_node_val,
+					  u8 use_adj)
+{
+  ip4_header_t *ip0;
+  udp_header_t *udp_hdr0;
+  lisp_gpe_header_t *lisp_gpe_hdr0;
+  nsh_base_header_t *nsh_hdr;
+  nsh_tlv_header_t *opt0;
+  nsh_tlv_header_t *limit0;
+  nsh_main_t *hm = &nsh_main;
+  nsh_option_map_t *nsh_option;
+
+  /* Populate the iOAM header */
+  ip0 = vlib_buffer_get_current (b0);
+  udp_hdr0 = (udp_header_t *) (ip0 + 1);
+  lisp_gpe_hdr0 = (lisp_gpe_header_t *) (udp_hdr0 + 1);
+  nsh_hdr = (nsh_base_header_t *)(lisp_gpe_hdr0 + 1);
+  opt0 = (nsh_tlv_header_t *) (nsh_hdr + 1);
+  limit0 = (nsh_tlv_header_t *) ((u8 *) opt0 + (nsh_hdr->length *4) - sizeof(nsh_base_header_t));
+
+  /*
+   * Basic validity checks
+   */
+  if ((nsh_hdr->length*4) > clib_net_to_host_u16 (ip0->length))
+    {
+      *next0 = drop_node_val;
+      return;
+    }
+
+  if (nsh_hdr->md_type != 2)
+    {
+      *next0 = drop_node_val;
+      return;
+    }
+
+  /* Scan the set of h-b-h options, process ones that we understand */
+  while (opt0 < limit0)
+    {
+      u8 type0;
+      type0 = opt0->type;
+      switch (type0)
+	{
+	case 0:		/* Pad1 */
+	  opt0 = (nsh_tlv_header_t *) ((u8 *) opt0) + 1;
+	  continue;
+	case 1:		/* PadN */
+	  break;
+	default:
+          nsh_option = nsh_md2_lookup_option(opt0->class, opt0->type);
+          if(( nsh_option != NULL) && (hm->options[nsh_option->option_id]))
+	    {
+	       if ((*hm->options[nsh_option->option_id]) (b0, opt0) < 0)
+		  {
+		     *next0 = drop_node_val;
+		     return;
+		  }
+	    }
+	  break;
+	}
+      opt0 =
+	(nsh_tlv_header_t *) (((u8 *) opt0) + opt0->length +
+				     sizeof (nsh_tlv_header_t));
+    }
+
+
+  if (PREDICT_FALSE (b0->flags & VLIB_BUFFER_IS_TRACED))
+    {
+      nsh_transit_trace_t *tr = vlib_add_trace(vm, node, b0, sizeof(*tr));
+      clib_memcpy ( &(tr->trace_data), nsh_hdr, (nsh_hdr->length*4) );
+    }
+  return;
+}
+
+
+#endif
+
+/*
+ * fd.io coding-style-patch-verification: ON
+ *
+ * Local Variables:
+ * eval: (c-set-style "gnu")
+ * End:
+ */
diff --git a/nsh-plugin/nsh/nsh.c b/nsh-plugin/nsh/nsh.c
index 1699126..4102c15 100644
--- a/nsh-plugin/nsh/nsh.c
+++ b/nsh-plugin/nsh/nsh.c
@@ -583,7 +583,7 @@ nsh_add_del_map_command_fn (vlib_main_t * vm,
       next_node = NSH_NODE_NEXT_ENCAP_VXLAN4;
     else if (unformat (line_input, "encap-vxlan6-intf %d", &sw_if_index))
       next_node = NSH_NODE_NEXT_ENCAP_VXLAN6;
-    else if (unformat (line_input, "encap-none"))
+    else if (unformat (line_input, "encap-none %d", &sw_if_index))
       next_node = NSH_NODE_NEXT_DECAP_ETH_INPUT;
     else
       return clib_error_return (0, "parse error: '%U'",
@@ -602,7 +602,7 @@ nsh_add_del_map_command_fn (vlib_main_t * vm,
     return clib_error_return (0, "nsh_action required: swap|push|pop.");
 
   if (next_node == ~0)
-    return clib_error_return (0, "must specific action: [encap-gre-intf <nn> | encap-vxlan-gpe-intf <nn> | encap-lisp-gpe-intf <nn> | encap-none]");
+    return clib_error_return (0, "must specific action: [encap-gre-intf <nn> | encap-vxlan-gpe-intf <nn> | encap-lisp-gpe-intf <nn> | encap-none <rx_sw_if_index>]");
 
   memset (a, 0, sizeof (*a));
 
@@ -1611,6 +1611,7 @@ nsh_input_map (vlib_main_t * vm,
 	      /* Manipulate MD2 */
               if(PREDICT_FALSE(hdr0->md_type == 2))
         	{
+	          vnet_buffer(b0)->sw_if_index[VLIB_RX] = map0->sw_if_index;
         	  nsh_md2_decap(b0, hdr0, header_len0, &next0, NSH_NODE_NEXT_DROP);
         	  if (PREDICT_FALSE(next0 == NSH_NODE_NEXT_DROP))
         	    {
@@ -1713,6 +1714,7 @@ nsh_input_map (vlib_main_t * vm,
 	      /* Manipulate MD2 */
               if(PREDICT_FALSE(hdr1->md_type == 2))
         	{
+	          vnet_buffer(b1)->sw_if_index[VLIB_RX] = map1->sw_if_index;
         	  nsh_md2_decap(b1, hdr1, header_len1, &next1, NSH_NODE_NEXT_DROP);
         	  if (PREDICT_FALSE(next1 == NSH_NODE_NEXT_DROP))
         	    {
@@ -1881,6 +1883,7 @@ nsh_input_map (vlib_main_t * vm,
 	      /* Manipulate MD2 */
               if(PREDICT_FALSE(hdr0->md_type == 2))
         	{
+	          vnet_buffer(b0)->sw_if_index[VLIB_RX] = map0->sw_if_index;
         	  nsh_md2_decap(b0, hdr0, header_len0, &next0, NSH_NODE_NEXT_DROP);
         	  if (PREDICT_FALSE(next0 == NSH_NODE_NEXT_DROP))
         	    {