LISP: add NSH support

Change-Id: I971c110ed126f1a24a963f9d3b88cf8f8c308816
Signed-off-by: Filip Tehlar <ftehlar@cisco.com>

12 files changed, 579 insertions, 32 deletions

diff --git a/src/vnet/lisp-cp/control.c b/src/vnet/lisp-cp/control.c
index 19bbd618cbc..db78678d9c6 100644
--- a/src/vnet/lisp-cp/control.c
+++ b/src/vnet/lisp-cp/control.c
@@ -28,6 +28,8 @@
 #include <openssl/evp.h>
 #include <openssl/hmac.h>
 
+#define MAX_VALUE_U24 0xffffff
+
 lisp_cp_main_t lisp_control_main;
 
 u8 *format_lisp_cp_input_trace (u8 * s, va_list * args);
@@ -697,6 +699,20 @@ vnet_lisp_map_cache_add_del (vnet_lisp_add_del_mapping_args_t * a,
   mapping_t *m, *old_map;
   u32 **eid_indexes;
 
+  if (gid_address_type (&a->eid) == GID_ADDR_NSH)
+    {
+      if (gid_address_vni (&a->eid) != 0)
+	{
+	  clib_warning ("Supported only default VNI for NSH!");
+	  return VNET_API_ERROR_INVALID_ARGUMENT;
+	}
+      if (gid_address_nsh_spi (&a->eid) > MAX_VALUE_U24)
+	{
+	  clib_warning ("SPI is greater than 24bit!");
+	  return VNET_API_ERROR_INVALID_ARGUMENT;
+	}
+    }
+
   mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &a->eid);
   old_map = mi != ~0 ? pool_elt_at_index (lcm->mapping_pool, mi) : 0;
   if (a->is_add)
@@ -812,7 +828,7 @@ vnet_lisp_add_del_local_mapping (vnet_lisp_add_del_mapping_args_t * a,
   else if (GID_ADDR_MAC == type)
     dp_table = hash_get (lcm->bd_id_by_vni, vni);
 
-  if (!dp_table)
+  if (!dp_table && GID_ADDR_NSH != type)
     {
       clib_warning ("vni %d not associated to a %s!", vni,
 		    GID_ADDR_IP_PREFIX == type ? "vrf" : "bd");
@@ -1329,8 +1345,23 @@ vnet_lisp_add_del_adjacency (vnet_lisp_add_del_adjacency_args_t * a)
     {
       /* check if source eid has an associated mapping. If pitr mode is on,
        * just use the pitr's mapping */
-      local_mi = lcm->lisp_pitr ? lcm->pitr_map_index :
-	gid_dictionary_lookup (&lcm->mapping_index_by_gid, &a->leid);
+      if (lcm->lisp_pitr)
+	local_mi = lcm->pitr_map_index;
+      else
+	{
+	  if (gid_address_type (&a->reid) == GID_ADDR_NSH)
+	    {
+	      if (lcm->nsh_map_index == ~0)
+		local_mi = GID_LOOKUP_MISS;
+	      else
+		local_mi = lcm->nsh_map_index;
+	    }
+	  else
+	    {
+	      local_mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid,
+						&a->leid);
+	    }
+	}
 
       if (GID_LOOKUP_MISS == local_mi)
 	{
@@ -1371,6 +1402,57 @@ vnet_lisp_set_map_request_mode (u8 mode)
 }
 
 int
+vnet_lisp_nsh_set_locator_set (u8 * locator_set_name, u8 is_add)
+{
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+  lisp_gpe_main_t *lgm = vnet_lisp_gpe_get_main ();
+  u32 locator_set_index = ~0;
+  mapping_t *m;
+  uword *p;
+
+  if (vnet_lisp_enable_disable_status () == 0)
+    {
+      clib_warning ("LISP is disabled!");
+      return VNET_API_ERROR_LISP_DISABLED;
+    }
+
+  if (is_add)
+    {
+      if (lcm->nsh_map_index == (u32) ~ 0)
+	{
+	  p = hash_get_mem (lcm->locator_set_index_by_name, locator_set_name);
+	  if (!p)
+	    {
+	      clib_warning ("locator-set %v doesn't exist", locator_set_name);
+	      return -1;
+	    }
+	  locator_set_index = p[0];
+
+	  pool_get (lcm->mapping_pool, m);
+	  memset (m, 0, sizeof *m);
+	  m->locator_set_index = locator_set_index;
+	  m->local = 1;
+	  m->nsh_set = 1;
+	  lcm->nsh_map_index = m - lcm->mapping_pool;
+
+	  if (~0 == vnet_lisp_gpe_add_nsh_iface (lgm))
+	    return -1;
+	}
+    }
+  else
+    {
+      if (lcm->nsh_map_index != (u32) ~ 0)
+	{
+	  /* remove NSH mapping */
+	  pool_put_index (lcm->mapping_pool, lcm->nsh_map_index);
+	  lcm->nsh_map_index = ~0;
+	  vnet_lisp_gpe_del_nsh_iface (lgm);
+	}
+    }
+  return 0;
+}
+
+int
 vnet_lisp_pitr_set_locator_set (u8 * locator_set_name, u8 is_add)
 {
   lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
@@ -2667,7 +2749,7 @@ _send_encapsulated_map_request (lisp_cp_main_t * lcm,
     }
 
   /* get locator-set for seid */
-  if (!lcm->lisp_pitr)
+  if (!lcm->lisp_pitr && gid_address_type (deid) != GID_ADDR_NSH)
     {
       map_index = gid_dictionary_lookup (&lcm->mapping_index_by_gid, seid);
       if (map_index == ~0)
@@ -2690,9 +2772,24 @@ _send_encapsulated_map_request (lisp_cp_main_t * lcm,
     }
   else
     {
-      map_index = lcm->pitr_map_index;
-      map = pool_elt_at_index (lcm->mapping_pool, lcm->pitr_map_index);
-      ls_index = map->locator_set_index;
+      if (lcm->lisp_pitr)
+	{
+	  map = pool_elt_at_index (lcm->mapping_pool, lcm->pitr_map_index);
+	  ls_index = map->locator_set_index;
+	}
+      else
+	{
+	  if (lcm->nsh_map_index == (u32) ~ 0)
+	    {
+	      clib_warning ("No locator-set defined for NSH!");
+	      return -1;
+	    }
+	  else
+	    {
+	      map = pool_elt_at_index (lcm->mapping_pool, lcm->nsh_map_index);
+	      ls_index = map->locator_set_index;
+	    }
+	}
     }
 
   /* overwrite locator set if map-request itr-rlocs configured */
@@ -2843,6 +2940,7 @@ get_src_and_dst_eids_from_buffer (lisp_cp_main_t * lcm, vlib_buffer_t * b,
 				  gid_address_t * src, gid_address_t * dst,
 				  u16 type)
 {
+  ethernet_header_t *eh;
   u32 vni = 0;
 
   memset (src, 0, sizeof (*src));
@@ -2873,7 +2971,6 @@ get_src_and_dst_eids_from_buffer (lisp_cp_main_t * lcm, vlib_buffer_t * b,
     }
   else if (LISP_AFI_MAC == type)
     {
-      ethernet_header_t *eh;
       ethernet_arp_header_t *ah;
 
       eh = vlib_buffer_get_current (b);
@@ -2906,8 +3003,19 @@ get_src_and_dst_eids_from_buffer (lisp_cp_main_t * lcm, vlib_buffer_t * b,
     }
   else if (LISP_AFI_LCAF == type)
     {
-      /* Eventually extend this to support NSH and other */
-      ASSERT (0);
+      lisp_nsh_hdr_t *nh;
+      eh = vlib_buffer_get_current (b);
+
+      if (clib_net_to_host_u16 (eh->type) == ETHERNET_TYPE_NSH)
+	{
+	  nh = (lisp_nsh_hdr_t *) (((u8 *) eh) + sizeof (*eh));
+	  u32 spi = clib_net_to_host_u32 (nh->spi_si << 8);
+	  u8 si = (u8) clib_net_to_host_u32 (nh->spi_si);
+	  gid_address_nsh_spi (dst) = spi;
+	  gid_address_nsh_si (dst) = si;
+
+	  gid_address_type (dst) = GID_ADDR_NSH;
+	}
     }
 }
 
@@ -3009,8 +3117,14 @@ lisp_cp_lookup_inline (vlib_main_t * vm,
 		}
 	      else
 		{
-		  si = gid_dictionary_lookup (&lcm->mapping_index_by_gid,
-					      &src);
+		  if (GID_ADDR_NSH != gid_address_type (&dst))
+		    {
+		      si = gid_dictionary_lookup (&lcm->mapping_index_by_gid,
+						  &src);
+		    }
+		  else
+		    si = lcm->nsh_map_index;
+
 		  if (~0 != si)
 		    {
 		      dp_add_fwd_entry_from_mt (si, di);
@@ -3862,6 +3976,7 @@ lisp_cp_init (vlib_main_t * vm)
 
   u64 now = clib_cpu_time_now ();
   timing_wheel_init (&lcm->wheel, now, vm->clib_time.clocks_per_second);
+  lcm->nsh_map_index = ~0;
   return 0;
 }
 
diff --git a/src/vnet/lisp-cp/control.h b/src/vnet/lisp-cp/control.h
index feb8cfa41a3..ad90b5269ff 100644
--- a/src/vnet/lisp-cp/control.h
+++ b/src/vnet/lisp-cp/control.h
@@ -211,6 +211,9 @@ typedef struct
   /* LISP PITR mode */
   u8 lisp_pitr;
 
+  /* mapping index for NSH */
+  u32 nsh_map_index;
+
   /* map request mode */
   u8 map_request_mode;
 
@@ -353,6 +356,7 @@ u8 vnet_lisp_rloc_probe_state_get (void);
 int vnet_lisp_add_del_l2_arp_entry (gid_address_t * key, u8 * mac, u8 is_add);
 u32 *vnet_lisp_l2_arp_bds_get (void);
 lisp_api_l2_arp_entry_t *vnet_lisp_l2_arp_entries_get_by_bd (u32 bd);
+int vnet_lisp_nsh_set_locator_set (u8 * locator_set_name, u8 is_add);
 
 map_records_arg_t *parse_map_reply (vlib_buffer_t * b);
 
diff --git a/src/vnet/lisp-cp/gid_dictionary.c b/src/vnet/lisp-cp/gid_dictionary.c
index 80d59fafdd7..cf9a741ae12 100644
--- a/src/vnet/lisp-cp/gid_dictionary.c
+++ b/src/vnet/lisp-cp/gid_dictionary.c
@@ -345,6 +345,14 @@ make_arp_key (BVT (clib_bihash_kv) * kv, u32 bd, ip4_address_t * addr)
   kv->key[2] = (u64) 0;
 }
 
+static void
+make_nsh_key (BVT (clib_bihash_kv) * kv, u32 vni, u32 spi, u8 si)
+{
+  kv->key[0] = (u64) vni;
+  kv->key[1] = (u64) spi;
+  kv->key[2] = (u64) si;
+}
+
 static u64
 arp_lookup (gid_l2_arp_table_t * db, u32 bd, ip4_address_t * key)
 {
@@ -360,6 +368,21 @@ arp_lookup (gid_l2_arp_table_t * db, u32 bd, ip4_address_t * key)
   return GID_LOOKUP_MISS_L2;
 }
 
+static u32
+nsh_lookup (gid_nsh_table_t * db, u32 vni, u32 spi, u8 si)
+{
+  int rv;
+  BVT (clib_bihash_kv) kv, value;
+
+  make_nsh_key (&kv, vni, spi, si);
+  rv = BV (clib_bihash_search_inline_2) (&db->nsh_lookup_table, &kv, &value);
+
+  if (rv == 0)
+    return value.value;
+
+  return GID_LOOKUP_MISS;
+}
+
 u64
 gid_dictionary_lookup (gid_dictionary_t * db, gid_address_t * key)
 {
@@ -393,6 +416,9 @@ gid_dictionary_lookup (gid_dictionary_t * db, gid_address_t * key)
     case GID_ADDR_ARP:
       return arp_lookup (&db->arp_table, gid_address_arp_bd (key),
 			 &gid_address_arp_ip4 (key));
+    case GID_ADDR_NSH:
+      return nsh_lookup (&db->nsh_table, gid_address_vni (key),
+			 gid_address_nsh_spi (key), gid_address_nsh_si (key));
     default:
       clib_warning ("address type %d not supported!", gid_address_type (key));
       break;
@@ -432,6 +458,9 @@ gid_dictionary_sd_lookup (gid_dictionary_t * db, gid_address_t * dst,
 	  break;
 	}
       break;
+    case GID_ADDR_NSH:
+      return gid_dictionary_lookup (db, dst);
+      break;
     default:
       clib_warning ("address type %d not supported!", gid_address_type (dst));
       break;
@@ -860,7 +889,7 @@ add_del_sd (gid_dictionary_t * db, u32 vni, source_dest_t * key, u32 value,
   return ~0;
 }
 
-static u32
+static u64
 add_del_arp (gid_l2_arp_table_t * db, u32 bd, ip4_address_t * key, u64 value,
 	     u8 is_add)
 {
@@ -885,6 +914,31 @@ add_del_arp (gid_l2_arp_table_t * db, u32 bd, ip4_address_t * key, u64 value,
   return old_val;
 }
 
+static u32
+add_del_nsh (gid_nsh_table_t * db, u32 vni, u32 spi, u8 si, u32 value,
+	     u8 is_add)
+{
+  BVT (clib_bihash_kv) kv, result;
+  u32 old_val = ~0;
+
+  make_nsh_key (&kv, vni, spi, si);
+  if (BV (clib_bihash_search) (&db->nsh_lookup_table, &kv, &result) == 0)
+    old_val = result.value;
+
+  if (is_add)
+    {
+      kv.value = value;
+      BV (clib_bihash_add_del) (&db->nsh_lookup_table, &kv, 1 /* is_add */ );
+      db->count++;
+    }
+  else
+    {
+      BV (clib_bihash_add_del) (&db->nsh_lookup_table, &kv, 0 /* is_add */ );
+      db->count--;
+    }
+  return old_val;
+}
+
 u32
 gid_dictionary_add_del (gid_dictionary_t * db, gid_address_t * key, u64 value,
 			u8 is_add)
@@ -903,6 +957,11 @@ gid_dictionary_add_del (gid_dictionary_t * db, gid_address_t * key, u64 value,
     case GID_ADDR_ARP:
       return add_del_arp (&db->arp_table, gid_address_arp_bd (key),
 			  &gid_address_arp_ip4 (key), value, is_add);
+    case GID_ADDR_NSH:
+      return add_del_nsh (&db->nsh_table, gid_address_vni (key),
+			  gid_address_nsh_spi (key), gid_address_nsh_si (key),
+			  value, is_add);
+
     default:
       clib_warning ("address type %d not supported!", gid_address_type (key));
       break;
@@ -944,6 +1003,23 @@ arp_lookup_init (gid_l2_arp_table_t * db)
 			 db->arp_lookup_table_size);
 }
 
+static void
+nsh_lookup_init (gid_nsh_table_t * db)
+{
+  if (db->nsh_lookup_table_nbuckets == 0)
+    db->nsh_lookup_table_nbuckets = MAC_LOOKUP_DEFAULT_HASH_NUM_BUCKETS;
+
+  db->nsh_lookup_table_nbuckets =
+    1 << max_log2 (db->nsh_lookup_table_nbuckets);
+
+  if (db->nsh_lookup_table_size == 0)
+    db->nsh_lookup_table_size = MAC_LOOKUP_DEFAULT_HASH_MEMORY_SIZE;
+
+  BV (clib_bihash_init) (&db->nsh_lookup_table, "nsh lookup table",
+			 db->nsh_lookup_table_nbuckets,
+			 db->nsh_lookup_table_size);
+}
+
 void
 gid_dictionary_init (gid_dictionary_t * db)
 {
@@ -951,6 +1027,7 @@ gid_dictionary_init (gid_dictionary_t * db)
   ip6_lookup_init (&db->dst_ip6_table);
   mac_lookup_init (&db->sd_mac_table);
   arp_lookup_init (&db->arp_table);
+  nsh_lookup_init (&db->nsh_table);
 }
 
 /*
diff --git a/src/vnet/lisp-cp/gid_dictionary.h b/src/vnet/lisp-cp/gid_dictionary.h
index 9612fb133fd..51806bd67f8 100644
--- a/src/vnet/lisp-cp/gid_dictionary.h
+++ b/src/vnet/lisp-cp/gid_dictionary.h
@@ -40,6 +40,10 @@
 #define ARP_LOOKUP_DEFAULT_HASH_NUM_BUCKETS (64 * 1024)
 #define ARP_LOOKUP_DEFAULT_HASH_MEMORY_SIZE (32<<20)
 
+/* Default size of the NSH hash table */
+#define NSH_LOOKUP_DEFAULT_HASH_NUM_BUCKETS (64 * 1024)
+#define NSH_LOOKUP_DEFAULT_HASH_MEMORY_SIZE (32<<20)
+
 typedef void (*foreach_subprefix_match_cb_t) (u32, void *);
 
 typedef struct
@@ -84,6 +88,16 @@ typedef struct gid_mac_table
   u64 count;
 } gid_mac_table_t;
 
+typedef struct gid_nsh_table
+{
+  BVT (clib_bihash) nsh_lookup_table;
+
+  /* nsh lookup table config parameters */
+  u32 nsh_lookup_table_nbuckets;
+  uword nsh_lookup_table_size;
+  u64 count;
+} gid_nsh_table_t;
+
 typedef struct
 {
   BVT (clib_bihash) arp_lookup_table;
@@ -97,6 +111,9 @@ typedef struct
   /** L2 ARP table */
   gid_l2_arp_table_t arp_table;
 
+  /** NSH lookup table */
+  gid_nsh_table_t nsh_table;
+
   /** destination IP LPM ip4 lookup table */
   gid_ip4_table_t dst_ip4_table;
 
diff --git a/src/vnet/lisp-cp/lisp_cp_messages.h b/src/vnet/lisp-cp/lisp_cp_messages.h
index 278f60e1726..69510a0e1ce 100644
--- a/src/vnet/lisp-cp/lisp_cp_messages.h
+++ b/src/vnet/lisp-cp/lisp_cp_messages.h
@@ -474,6 +474,22 @@ typedef struct _lcaf_src_dst_hdr_t
 #define LCAF_SD_DST_ML(_h) (_h)->dst_mask_len
 
 /*
+ * SPI LCAF
+ *
+ *    0                   1                   2                   3
+ *    0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ *   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *   |              Service Path ID                  | Service index |
+ *   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+typedef struct _lcaf_spi_hdr_t
+{
+  u32 spi_si;
+} __attribute__ ((__packed__)) lcaf_spi_hdr_t;
+
+#define LCAF_SPI_SI(_h) (_h)->spi_si
+
+/*
  * The Map-Register message format is:
  *
  *       0                   1                   2                   3
@@ -602,6 +618,21 @@ typedef struct
 #define MNOTIFY_AUTH_DATA_LEN(h_) (MREG_HDR_CAST(h_))->auth_data_len
 #define MNOTIFY_DATA(h_) (MREG_HDR_CAST(h_))->data
 
+/*
+ *   0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |Ver|O|C|R|R|R|R|R|R|   Length  |  MD type=0x1  | Next Protocol |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |          Service Path Identifer               | Service Index |
+ *  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+
+typedef struct
+{
+  u32 header;
+  u32 spi_si;
+} __attribute__ ((__packed__)) lisp_nsh_hdr_t;
+
 #endif /* VNET_LISP_GPE_LISP_CP_MESSAGES_H_ */
 
 /*
diff --git a/src/vnet/lisp-cp/lisp_types.c b/src/vnet/lisp-cp/lisp_types.c
index 85cefae05ab..e50c3aa1877 100644
--- a/src/vnet/lisp-cp/lisp_types.c
+++ b/src/vnet/lisp-cp/lisp_types.c
@@ -31,16 +31,28 @@ typedef int (*cmp_fct) (void *, void *);
 
 size_to_write_fct size_to_write_fcts[GID_ADDR_TYPES] =
   { ip_prefix_size_to_write, lcaf_size_to_write, mac_size_to_write,
-  sd_size_to_write, nsh_size_to_write
+  sd_size_to_write, nsh_size_to_write, 0 /* arp */ , no_addr_size_to_write
 };
+
 serdes_fct write_fcts[GID_ADDR_TYPES] =
-  { ip_prefix_write, lcaf_write, mac_write, sd_write, nsh_write };
+  { ip_prefix_write, lcaf_write, mac_write, sd_write, nsh_write, 0 /* arp */ ,
+  no_addr_write
+};
+
 cast_fct cast_fcts[GID_ADDR_TYPES] =
-  { ip_prefix_cast, lcaf_cast, mac_cast, sd_cast, nsh_cast };
+  { ip_prefix_cast, lcaf_cast, mac_cast, sd_cast, nsh_cast, 0 /* arp */ ,
+  no_addr_cast
+};
+
 addr_len_fct addr_len_fcts[GID_ADDR_TYPES] =
-  { ip_prefix_length, lcaf_length, mac_length, sd_length, nsh_length };
+  { ip_prefix_length, lcaf_length, mac_length, sd_length, nsh_length,
+  0 /* arp */ , no_addr_length
+};
+
 copy_fct copy_fcts[GID_ADDR_TYPES] =
-  { ip_prefix_copy, lcaf_copy, mac_copy, sd_copy, nsh_copy };
+  { ip_prefix_copy, lcaf_copy, mac_copy, sd_copy, nsh_copy, 0 /* arp */ ,
+  no_addr_copy
+};
 
 #define foreach_lcaf_type \
   _(1, no_addr)      \
@@ -55,7 +67,12 @@ copy_fct copy_fcts[GID_ADDR_TYPES] =
   _(0, NULL)         \
   _(0, NULL)         \
   _(0, NULL)         \
-  _(1, sd)
+  _(1, sd)           \
+  _(0, NULL)         \
+  _(0, NULL)         \
+  _(0, NULL)         \
+  _(0, NULL)         \
+  _(1, nsh)
 
 #define _(cond, name)                             \
   u16 name ## _write (u8 * p, void * a);          \
@@ -254,11 +271,12 @@ format_gid_address (u8 * s, va_list * args)
     case GID_ADDR_MAC:
       return format (s, "[%d] %U", gid_address_vni (a), format_mac_address,
 		     &gid_address_mac (a));
-    case GID_ADDR_NSH:
-      return format (s, "%U", format_nsh_address, &gid_address_nsh (a));
     case GID_ADDR_ARP:
       return format (s, "[%d, %U]", gid_address_arp_bd (a),
 		     format_ip4_address, &gid_address_arp_ip4 (a));
+    case GID_ADDR_NSH:
+      return format (s, "%U", format_nsh_address, &gid_address_nsh (a));
+
     default:
       clib_warning ("Can't format gid type %d", type);
       return 0;
@@ -287,7 +305,7 @@ unformat_fid_address (unformat_input_t * i, va_list * args)
   else if (unformat (i, "%U", unformat_nsh_address, &nsh))
     {
       fid_addr_type (a) = FID_ADDR_NSH;
-      nsh_copy (&fid_addr_nsh (a), mac);
+      nsh_copy (&fid_addr_nsh (a), &nsh);
     }
   else
     return 0;
@@ -673,6 +691,38 @@ do {                    \
   dst += _sum;          \
 } while (0);
 
+void
+nsh_free (void *a)
+{
+  /* nothing to do */
+}
+
+u16
+nsh_parse (u8 * p, void *a)
+{
+  lcaf_spi_hdr_t *h = (lcaf_spi_hdr_t *) p;
+  gid_address_t *g = a;
+
+  gid_address_type (g) = GID_ADDR_NSH;
+  gid_address_nsh_spi (g) = clib_net_to_host_u32 (LCAF_SPI_SI (h)) >> 8;
+  gid_address_nsh_si (g) = (u8) clib_net_to_host_u32 (LCAF_SPI_SI (h));
+
+  return sizeof (lcaf_spi_hdr_t);
+}
+
+int
+nsh_cmp (void *a1, void *a2)
+{
+  nsh_t *n1 = a1;
+  nsh_t *n2 = a2;
+
+  if (n1->spi != n2->spi)
+    return 1;
+  if (n1->si != n2->si)
+    return 1;
+  return 0;
+}
+
 u16
 sd_parse (u8 * p, void *a)
 {
@@ -1095,6 +1145,12 @@ mac_cast (gid_address_t * a)
 }
 
 void *
+no_addr_cast (gid_address_t * a)
+{
+  return (void *) a;
+}
+
+void *
 sd_cast (gid_address_t * a)
 {
   return &gid_address_sd (a);
@@ -1227,8 +1283,33 @@ sd_write (u8 * p, void *a)
 u16
 nsh_write (u8 * p, void *a)
 {
-  clib_warning ("not done");
-  return 0;
+  lcaf_spi_hdr_t spi;
+  lcaf_hdr_t lcaf;
+  gid_address_t *g = a;
+  u16 size = 0;
+
+  ASSERT (gid_address_type (g) == GID_ADDR_NSH);
+
+  memset (&lcaf, 0, sizeof (lcaf));
+  memset (&spi, 0, sizeof (spi));
+
+  LCAF_TYPE (&lcaf) = LCAF_NSH;
+  LCAF_LENGTH (&lcaf) = clib_host_to_net_u16 (sizeof (lcaf_spi_hdr_t));
+
+  u32 s = clib_host_to_net_u32 (gid_address_nsh_spi (g) << 8 |
+				gid_address_nsh_si (g));
+  LCAF_SPI_SI (&spi) = s;
+
+  *(u16 *) p = clib_host_to_net_u16 (LISP_AFI_LCAF);
+  size += sizeof (u16);
+
+  clib_memcpy (p + size, &lcaf, sizeof (lcaf));
+  size += sizeof (lcaf);
+
+  clib_memcpy (p + size, &spi, sizeof (spi));
+  size += sizeof (spi);
+
+  return size;
 }
 
 u16
@@ -1354,7 +1435,7 @@ mac_size_to_write (void *a)
 u16
 nsh_size_to_write (void *a)
 {
-  return sizeof (u16) + 4;
+  return sizeof (u16) + sizeof (lcaf_hdr_t) + sizeof (lcaf_spi_hdr_t);
 }
 
 u8
@@ -1581,6 +1662,9 @@ gid_address_cmp (gid_address_t * a1, gid_address_t * a2)
     case GID_ADDR_SRC_DST:
       cmp = sd_cmp (&gid_address_sd (a1), &gid_address_sd (a2));
       break;
+    case GID_ADDR_NSH:
+      cmp = nsh_cmp (&gid_address_nsh (a1), &gid_address_nsh (a2));
+      break;
     default:
       break;
     }
diff --git a/src/vnet/lisp-cp/lisp_types.h b/src/vnet/lisp-cp/lisp_types.h
index f5d2a676ff7..b7ad0f27dd0 100644
--- a/src/vnet/lisp-cp/lisp_types.h
+++ b/src/vnet/lisp-cp/lisp_types.h
@@ -102,6 +102,7 @@ typedef enum
   LCAF_AFI_LIST_TYPE,
   LCAF_INSTANCE_ID,
   LCAF_SOURCE_DEST = 12,
+  LCAF_NSH = 17,
   LCAF_TYPES
 } lcaf_type_t;
 
@@ -166,6 +167,9 @@ typedef struct
   u8 si;
 } nsh_t;
 
+#define nsh_spi(_a) (_a)->spi
+#define nsh_si(_a) (_a)->si
+
 typedef struct
 {
   ip4_address_t addr;
@@ -258,6 +262,8 @@ void gid_address_ip_set (gid_address_t * dst, void *src, u8 version);
 #define gid_address_lcaf(_a) (_a)->lcaf
 #define gid_address_mac(_a) (_a)->mac
 #define gid_address_nsh(_a) (_a)->nsh
+#define gid_address_nsh_spi(_a) nsh_spi(&gid_address_nsh(_a))
+#define gid_address_nsh_si(_a) nsh_si(&gid_address_nsh(_a))
 #define gid_address_vni(_a) (_a)->vni
 #define gid_address_vni_mask(_a) (_a)->vni_mask
 #define gid_address_sd_dst_ippref(_a) sd_dst_ippref(&(_a)->sd)
@@ -275,6 +281,7 @@ void gid_address_ip_set (gid_address_t * dst, void *src, u8 version);
 
 /* 'sub'address functions */
 #define foreach_gid_address_type_fcns  \
+  _(no_addr)                      \
   _(ip_prefix)                    \
   _(lcaf)                         \
   _(mac)                          \
@@ -350,7 +357,8 @@ typedef struct
   /* valid only for remote mappings */
   u8 is_static:1;
   u8 pitr_set:1;
-  u8 rsvd:4;
+  u8 nsh_set:1;
+  u8 rsvd:3;
 
 
   u8 *key;
diff --git a/src/vnet/lisp-cp/one.api b/src/vnet/lisp-cp/one.api
index 7d07cc47db5..31811a34bf5 100644
--- a/src/vnet/lisp-cp/one.api
+++ b/src/vnet/lisp-cp/one.api
@@ -146,6 +146,20 @@ autoreply define one_enable_disable
   u8 is_en;
 };
 
+/** \brief configure or delete ONE NSH mapping
+    @param client_index - opaque cookie to identify the sender
+    @param context - sender context, to match reply w/ request
+    @param ls_name - locator set name
+    @param is_add - add locator set if non-zero; delete otherwise
+*/
+autoreply define one_nsh_set_locator_set
+{
+  u32 client_index;
+  u32 context;
+  u8 is_add;
+  u8 ls_name[64];
+};
+
 /** \brief configure or disable ONE PITR node
     @param client_index - opaque cookie to identify the sender
     @param context - sender context, to match reply w/ request
@@ -325,6 +339,12 @@ typeonly manual_endian manual_print define one_remote_locator
       0 : ipv4
       1 : ipv6
       2 : mac
+      3 : NSH : both information (service path ID and service index) are
+        encoded in 'eid' field in a following way:
+
+        |4 B |1 B |
+        -----------
+        |SPI | SI |
     @param deid - dst EID
     @param seid - src EID, valid only if is_src_dst is enabled
     @param rloc_num - number of remote locators
@@ -596,6 +616,12 @@ define one_eid_table_details
         0: EID is IPv4
         1: EID is IPv6
         2: EID is ethernet address
+        3 : NSH : both information (service path ID and service index) are
+          encoded in 'eid' field in a following way:
+
+          |4 B |1 B |
+          -----------
+          |SPI | SI |
     @param eid - endpoint identifier
     @param filter - filter type;
       Support values:
@@ -787,6 +813,29 @@ define one_get_map_request_itr_rlocs_reply
   u8 locator_set_name[64];
 };
 
+/** \brief Request for ONE NSH mapping
+    @param client_index - opaque cookie to identify the sender
+    @param context - sender context, to match reply w/ request
+*/
+define show_one_nsh_mapping
+{
+  u32 client_index;
+  u32 context;
+};
+
+/** \brief Reply for ONE NSH mapping
+    @param context - sender context, to match reply w/ request
+    @param is_set - is ONE NSH mapping set
+    @param locator_set_name - name of the locator_set if NSH mapping is set
+*/
+define show_one_nsh_mapping_reply
+{
+  u32 context;
+  i32 retval;
+  u8 is_set;
+  u8 locator_set_name[64];
+};
+
 /** \brief Request for ONE PITR status
     @param client_index - opaque cookie to identify the sender
     @param context - sender context, to match reply w/ request
diff --git a/src/vnet/lisp-cp/one_api.c b/src/vnet/lisp-cp/one_api.c
index 724e58df839..18235fa4514 100644
--- a/src/vnet/lisp-cp/one_api.c
+++ b/src/vnet/lisp-cp/one_api.c
@@ -90,6 +90,7 @@ _(ONE_MAP_REGISTER_ENABLE_DISABLE, one_map_register_enable_disable)     \
 _(ONE_ADD_DEL_REMOTE_MAPPING, one_add_del_remote_mapping)               \
 _(ONE_ADD_DEL_ADJACENCY, one_add_del_adjacency)                         \
 _(ONE_PITR_SET_LOCATOR_SET, one_pitr_set_locator_set)                   \
+_(ONE_NSH_SET_LOCATOR_SET, one_nsh_set_locator_set)                     \
 _(ONE_MAP_REQUEST_MODE, one_map_request_mode)                           \
 _(ONE_EID_TABLE_ADD_DEL_MAP, one_eid_table_add_del_map)                 \
 _(ONE_LOCATOR_SET_DUMP, one_locator_set_dump)                           \
@@ -100,6 +101,7 @@ _(ONE_MAP_SERVER_DUMP, one_map_server_dump)                             \
 _(ONE_EID_TABLE_MAP_DUMP, one_eid_table_map_dump)                       \
 _(ONE_EID_TABLE_VNI_DUMP, one_eid_table_vni_dump)                       \
 _(ONE_ADJACENCIES_GET, one_adjacencies_get)                             \
+_(SHOW_ONE_NSH_MAPPING, show_one_nsh_mapping)                           \
 _(SHOW_ONE_RLOC_PROBE_STATE, show_one_rloc_probe_state)                 \
 _(SHOW_ONE_MAP_REGISTER_STATE, show_one_map_register_state)             \
 _(SHOW_ONE_STATUS, show_one_status)                                     \
@@ -225,10 +227,18 @@ vl_api_one_add_del_locator_t_handler (vl_api_one_add_del_locator_t * mp)
   REPLY_MACRO (VL_API_ONE_ADD_DEL_LOCATOR_REPLY);
 }
 
+typedef struct
+{
+  u32 spi;
+  u8 si;
+} __attribute__ ((__packed__)) lisp_nsh_api_t;
+
 static int
 unformat_one_eid_api (gid_address_t * dst, u32 vni, u8 type, void *src,
 		      u8 len)
 {
+  lisp_nsh_api_t *nsh;
+
   switch (type)
     {
     case 0:			/* ipv4 */
@@ -247,6 +257,12 @@ unformat_one_eid_api (gid_address_t * dst, u32 vni, u8 type, void *src,
       gid_address_type (dst) = GID_ADDR_MAC;
       clib_memcpy (&gid_address_mac (dst), src, 6);
       break;
+    case 3:			/* NSH */
+      gid_address_type (dst) = GID_ADDR_NSH;
+      nsh = src;
+      gid_address_nsh_spi (dst) = clib_net_to_host_u32 (nsh->spi);
+      gid_address_nsh_si (dst) = nsh->si;
+      break;
     default:
       /* unknown type */
       return VNET_API_ERROR_INVALID_VALUE;
@@ -276,6 +292,12 @@ vl_api_one_add_del_local_eid_t_handler (vl_api_one_add_del_local_eid_t * mp)
   if (rv)
     goto out;
 
+  if (gid_address_type (eid) == GID_ADDR_NSH)
+    {
+      rv = VNET_API_ERROR_INVALID_VALUE;
+      goto out;
+    }
+
   name = format (0, "%s", mp->locator_set_name);
   p = hash_get_mem (lcm->locator_set_index_by_name, name);
   if (!p)
@@ -409,6 +431,21 @@ vl_api_one_map_request_mode_t_handler (vl_api_one_map_request_mode_t * mp)
 }
 
 static void
+vl_api_one_nsh_set_locator_set_t_handler (vl_api_one_nsh_set_locator_set_t
+					  * mp)
+{
+  vl_api_one_nsh_set_locator_set_reply_t *rmp;
+  int rv = 0;
+  u8 *ls_name = 0;
+
+  ls_name = format (0, "%s", mp->ls_name);
+  rv = vnet_lisp_nsh_set_locator_set (ls_name, mp->is_add);
+  vec_free (ls_name);
+
+  REPLY_MACRO (VL_API_ONE_PITR_SET_LOCATOR_SET_REPLY);
+}
+
+static void
 vl_api_one_pitr_set_locator_set_t_handler (vl_api_one_pitr_set_locator_set_t
 					   * mp)
 {
@@ -781,7 +818,7 @@ send_one_eid_table_details (mapping_t * mapit,
   u8 *mac = 0;
   ip_prefix_t *ip_prefix = NULL;
 
-  if (mapit->pitr_set)
+  if (mapit->pitr_set || mapit->nsh_set)
     return;
 
   switch (filter)
@@ -851,6 +888,13 @@ send_one_eid_table_details (mapping_t * mapit,
       rmp->eid_type = 2;	/* l2 mac type */
       clib_memcpy (rmp->eid, mac, 6);
       break;
+    case GID_ADDR_NSH:
+      rmp->eid_type = 3;	/* NSH type */
+      lisp_nsh_api_t nsh;
+      nsh.spi = clib_host_to_net_u32 (gid_address_nsh_spi (gid));
+      nsh.si = gid_address_nsh_si (gid);
+      clib_memcpy (rmp->eid, &nsh, sizeof (nsh));
+      break;
     default:
       ASSERT (0);
     }
@@ -1071,6 +1115,7 @@ one_adjacency_copy (vl_api_one_adjacency_t * dst, lisp_adjacency_t * adjs)
   lisp_adjacency_t *adj;
   vl_api_one_adjacency_t a;
   u32 i, n = vec_len (adjs);
+  lisp_nsh_api_t nsh;
 
   for (i = 0; i < n; i++)
     {
@@ -1100,6 +1145,15 @@ one_adjacency_copy (vl_api_one_adjacency_t * dst, lisp_adjacency_t * adjs)
 	  mac_copy (a.reid, gid_address_mac (&adj->reid));
 	  mac_copy (a.leid, gid_address_mac (&adj->leid));
 	  break;
+	case GID_ADDR_NSH:
+	  a.eid_type = 3;	/* NSH type */
+	  nsh.spi = clib_host_to_net_u32 (gid_address_nsh_spi (&adj->reid));
+	  nsh.si = gid_address_nsh_si (&adj->reid);
+	  clib_memcpy (a.reid, &nsh, sizeof (nsh));
+
+	  nsh.spi = clib_host_to_net_u32 (gid_address_nsh_spi (&adj->leid));
+	  nsh.si = gid_address_nsh_si (&adj->leid);
+	  clib_memcpy (a.leid, &nsh, sizeof (nsh));
 	default:
 	  ASSERT (0);
 	}
@@ -1256,6 +1310,55 @@ static void
 }
 
 static void
+vl_api_show_one_nsh_mapping_t_handler (vl_api_show_one_nsh_mapping_t * mp)
+{
+  unix_shared_memory_queue_t *q = NULL;
+  vl_api_show_one_nsh_mapping_reply_t *rmp = NULL;
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+  mapping_t *m;
+  locator_set_t *ls = 0;
+  u8 *tmp_str = 0;
+  u8 is_set = 0;
+  int rv = 0;
+
+  q = vl_api_client_index_to_input_queue (mp->client_index);
+  if (q == 0)
+    {
+      return;
+    }
+
+  if (lcm->nsh_map_index == (u32) ~ 0)
+    {
+      tmp_str = format (0, "N/A");
+    }
+  else
+    {
+      m = pool_elt_at_index (lcm->mapping_pool, lcm->nsh_map_index);
+      if (~0 != m->locator_set_index)
+	{
+	  ls =
+	    pool_elt_at_index (lcm->locator_set_pool, m->locator_set_index);
+	  tmp_str = format (0, "%s", ls->name);
+	  is_set = 1;
+	}
+      else
+	{
+	  tmp_str = format (0, "N/A");
+	}
+    }
+  vec_add1 (tmp_str, 0);
+
+  /* *INDENT-OFF* */
+  REPLY_MACRO2(VL_API_SHOW_ONE_NSH_MAPPING_REPLY,
+  ({
+    rmp->is_set = is_set;
+    strncpy((char *) rmp->locator_set_name, (char *) tmp_str,
+            ARRAY_LEN(rmp->locator_set_name) - 1);
+  }));
+  /* *INDENT-ON* */
+}
+
+static void
 vl_api_show_one_pitr_t_handler (vl_api_show_one_pitr_t * mp)
 {
   unix_shared_memory_queue_t *q = NULL;
diff --git a/src/vnet/lisp-cp/one_cli.c b/src/vnet/lisp-cp/one_cli.c
index db6a6c833a9..e3fbf5a1f07 100644
--- a/src/vnet/lisp-cp/one_cli.c
+++ b/src/vnet/lisp-cp/one_cli.c
@@ -721,6 +721,62 @@ VLIB_CLI_COMMAND (one_show_map_resolvers_command) = {
 };
 /* *INDENT-ON* */
 
+static clib_error_t *
+lisp_nsh_set_locator_set_command_fn (vlib_main_t * vm,
+				     unformat_input_t * input,
+				     vlib_cli_command_t * cmd)
+{
+  u8 locator_name_set = 0;
+  u8 *locator_set_name = 0;
+  u8 is_add = 1;
+  unformat_input_t _line_input, *line_input = &_line_input;
+  clib_error_t *error = 0;
+  int rv = 0;
+
+  /* Get a line of input. */
+  if (!unformat_user (input, unformat_line_input, line_input))
+    return 0;
+
+  while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
+    {
+      if (unformat (line_input, "ls %_%v%_", &locator_set_name))
+	locator_name_set = 1;
+      else if (unformat (line_input, "disable"))
+	is_add = 0;
+      else
+	{
+	  error = clib_error_return (0, "parse error");
+	  goto done;
+	}
+    }
+
+  if (!locator_name_set)
+    {
+      clib_warning ("No locator set specified!");
+      goto done;
+    }
+
+  rv = vnet_lisp_nsh_set_locator_set (locator_set_name, is_add);
+  if (0 != rv)
+    {
+      error = clib_error_return (0, "failed to %s NSH mapping!",
+				 is_add ? "add" : "delete");
+    }
+
+done:
+  vec_free (locator_set_name);
+  unformat_free (line_input);
+  return error;
+}
+
+/* *INDENT-OFF* */
+VLIB_CLI_COMMAND (one_nsh_set_locator_set_command) = {
+    .path = "one nsh-mapping",
+    .short_help = "one nsh-mapping [del] ls <locator-set-name>",
+    .function = lisp_nsh_set_locator_set_command_fn,
+};
+/* *INDENT-ON* */
+
 
 static clib_error_t *
 lisp_pitr_set_locator_set_command_fn (vlib_main_t * vm,
@@ -923,7 +979,7 @@ lisp_show_eid_table_command_fn (vlib_main_t * vm,
       /* *INDENT-OFF* */
       pool_foreach (mapit, lcm->mapping_pool,
       ({
-        if (mapit->pitr_set)
+        if (mapit->pitr_set || mapit->nsh_set)
           continue;
 
         locator_set_t * ls = pool_elt_at_index (lcm->locator_set_pool,
diff --git a/src/vnet/lisp-gpe/interface.c b/src/vnet/lisp-gpe/interface.c
index 94703abc4cf..52b939f0e8b 100644
--- a/src/vnet/lisp-gpe/interface.c
+++ b/src/vnet/lisp-gpe/interface.c
@@ -742,7 +742,7 @@ lisp_gpe_del_l2_iface (lisp_gpe_main_t * lgm, u32 vni, u32 bd_id)
  * @return sw_if_index.
  */
 u32
-lisp_gpe_add_nsh_iface (lisp_gpe_main_t * lgm)
+vnet_lisp_gpe_add_nsh_iface (lisp_gpe_main_t * lgm)
 {
   vnet_main_t *vnm = lgm->vnet_main;
   tunnel_lookup_t *nsh_ifaces = &lgm->nsh_ifaces;
@@ -782,7 +782,7 @@ lisp_gpe_add_nsh_iface (lisp_gpe_main_t * lgm)
  *
  */
 void
-lisp_gpe_del_nsh_iface (lisp_gpe_main_t * lgm)
+vnet_lisp_gpe_del_nsh_iface (lisp_gpe_main_t * lgm)
 {
   tunnel_lookup_t *nsh_ifaces = &lgm->nsh_ifaces;
   uword *hip;
@@ -851,7 +851,7 @@ lisp_gpe_add_del_iface_command_fn (vlib_main_t * vm, unformat_input_t * input,
     {
       if (is_add)
 	{
-	  if (~0 == lisp_gpe_add_nsh_iface (&lisp_gpe_main))
+	  if (~0 == vnet_lisp_gpe_add_nsh_iface (&lisp_gpe_main))
 	    {
 	      error = clib_error_return (0, "NSH interface not created");
 	      goto done;
@@ -859,7 +859,7 @@ lisp_gpe_add_del_iface_command_fn (vlib_main_t * vm, unformat_input_t * input,
 	}
       else
 	{
-	  lisp_gpe_del_nsh_iface (&lisp_gpe_main);
+	  vnet_lisp_gpe_del_nsh_iface (&lisp_gpe_main);
 	}
       goto done;
     }
diff --git a/src/vnet/lisp-gpe/lisp_gpe.h b/src/vnet/lisp-gpe/lisp_gpe.h
index 660f8a6628b..5eafdd55ffc 100644
--- a/src/vnet/lisp-gpe/lisp_gpe.h
+++ b/src/vnet/lisp-gpe/lisp_gpe.h
@@ -324,6 +324,9 @@ vnet_api_error_t vnet_lisp_stats_enable_disable (u8 enable);
 lisp_api_stats_t *vnet_lisp_get_stats (void);
 int vnet_lisp_flush_stats (void);
 int vnet_gpe_add_del_native_fwd_rpath (vnet_gpe_native_fwd_rpath_args_t * a);
+u32 vnet_lisp_gpe_add_nsh_iface (lisp_gpe_main_t * lgm);
+void vnet_lisp_gpe_del_nsh_iface (lisp_gpe_main_t * lgm);
+
 #endif /* included_vnet_lisp_gpe_h */
 
 /*