X-Git-Url: https://gerrit.fd.io/r/gitweb?a=blobdiff_plain;f=src%2Fvnet%2Flisp-cp%2Flisp_types.c;h=971d3071b6a0aa52177f6fde134716aaf61bb1d4;hb=3ec09e9243dceb78e7548725281b0da8eddf72b1;hp=5ab4a5eb4496f4ffc2920f891ffb9635f130f0d4;hpb=7cd468a3d7dee7d6c92f69a0bb7061ae208ec727;p=vpp.git

diff --git a/src/vnet/lisp-cp/lisp_types.c b/src/vnet/lisp-cp/lisp_types.c
index 5ab4a5eb449..971d3071b6a 100644
--- a/src/vnet/lisp-cp/lisp_types.c
+++ b/src/vnet/lisp-cp/lisp_types.c
@@ -13,6 +13,7 @@
  * limitations under the License.
  */
 
+#include <vnet/ethernet/ethernet.h>
 #include <vnet/lisp-cp/lisp_types.h>
 
 static u16 gid_address_put_no_vni (u8 * b, gid_address_t * gid);
@@ -31,16 +32,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
+  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 };
+  { 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 };
+  { 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 };
+  { 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 };
+  { 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 +68,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);          \
@@ -123,83 +141,25 @@ serdes_fct lcaf_parse_fcts[LCAF_TYPES] = {
 #undef _
 };
 
-u8 *
-format_ip_address (u8 * s, va_list * args)
-{
-  ip_address_t *a = va_arg (*args, ip_address_t *);
-  u8 ver = ip_addr_version (a);
-  if (ver == IP4)
-    {
-      return format (s, "%U", format_ip4_address, &ip_addr_v4 (a));
-    }
-  else if (ver == IP6)
-    {
-      return format (s, "%U", format_ip6_address, &ip_addr_v6 (a));
-    }
-  else
-    {
-      clib_warning ("Can't format IP version %d!", ver);
-      return 0;
-    }
-}
-
 uword
-unformat_ip_address (unformat_input_t * input, va_list * args)
+unformat_nsh_address (unformat_input_t * input, va_list * args)
 {
-  ip_address_t *a = va_arg (*args, ip_address_t *);
-
-  memset (a, 0, sizeof (*a));
-  if (unformat (input, "%U", unformat_ip4_address, &ip_addr_v4 (a)))
-    ip_addr_version (a) = IP4;
-  else if (unformat_user (input, unformat_ip6_address, &ip_addr_v6 (a)))
-    ip_addr_version (a) = IP6;
-  else
-    return 0;
-  return 1;
+  nsh_t *a = va_arg (*args, nsh_t *);
+  return unformat (input, "SPI:%d SI:%d", &a->spi, &a->si);
 }
 
 u8 *
-format_ip_prefix (u8 * s, va_list * args)
+format_nsh_address (u8 * s, va_list * args)
 {
-  ip_prefix_t *a = va_arg (*args, ip_prefix_t *);
-  return format (s, "%U/%d", format_ip_address, &ip_prefix_addr (a),
-		 ip_prefix_len (a));
-}
-
-uword
-unformat_ip_prefix (unformat_input_t * input, va_list * args)
-{
-  ip_prefix_t *a = va_arg (*args, ip_prefix_t *);
-  if (unformat (input, "%U/%d", unformat_ip_address, &ip_prefix_addr (a),
-		&ip_prefix_len (a)))
-    {
-      if ((ip_prefix_version (a) == IP4 && 32 < ip_prefix_len (a)) ||
-	  (ip_prefix_version (a) == IP6 && 128 < ip_prefix_length (a)))
-	{
-	  clib_warning ("Prefix length to big: %d!", ip_prefix_len (a));
-	  return 0;
-	}
-      ip_prefix_normalize (a);
-    }
-  else
-    return 0;
-  return 1;
-}
-
-uword
-unformat_mac_address (unformat_input_t * input, va_list * args)
-{
-  u8 *a = va_arg (*args, u8 *);
-  return unformat (input, "%x:%x:%x:%x:%x:%x", &a[0], &a[1], &a[2], &a[3],
-		   &a[4], &a[5]);
+  nsh_t *a = va_arg (*args, nsh_t *);
+  return format (s, "SPI:%d SI:%d", a->spi, a->si);
 }
 
 u8 *
-format_mac_address (u8 * s, va_list * args)
+format_fid_nsh_address (u8 * s, va_list * args)
 {
-  u8 *a = va_arg (*args, u8 *);
-  return format (s, "%02x:%02x:%02x:%02x:%02x:%02x",
-		 a[0], a[1], a[2], a[3], a[4], a[5]);
+  u32 *a = va_arg (*args, u32 *);
+  return format (s, "SPI:%d SI:%d", *a >> 8, *a & 0xff);
 }
 
 u8 *
@@ -211,9 +171,10 @@ format_fid_address (u8 * s, va_list * args)
     {
     case FID_ADDR_IP_PREF:
       return format (s, "%U", format_ip_prefix, &fid_addr_ippref (a));
-
     case FID_ADDR_MAC:
       return format (s, "%U", format_mac_address, &fid_addr_mac (a));
+    case FID_ADDR_NSH:
+      return format (s, "%U", format_fid_nsh_address, &fid_addr_nsh (a));
 
     default:
       clib_warning ("Can't format fid address type %d!", fid_addr_type (a));
@@ -239,6 +200,13 @@ 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_ARP:
+    case GID_ADDR_NDP:
+      return format (s, "[%d, %U]", gid_address_arp_ndp_bd (a),
+		     format_ip_address, &gid_address_arp_ndp_ip (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;
@@ -252,6 +220,7 @@ unformat_fid_address (unformat_input_t * i, va_list * args)
   fid_address_t *a = va_arg (*args, fid_address_t *);
   ip_prefix_t ippref;
   u8 mac[6] = { 0 };
+  nsh_t nsh;
 
   if (unformat (i, "%U", unformat_ip_prefix, &ippref))
     {
@@ -263,6 +232,11 @@ unformat_fid_address (unformat_input_t * i, va_list * args)
       fid_addr_type (a) = FID_ADDR_MAC;
       mac_copy (fid_addr_mac (a), mac);
     }
+  else if (unformat (i, "%U", unformat_nsh_address, &nsh))
+    {
+      fid_addr_type (a) = FID_ADDR_NSH;
+      nsh_copy (&fid_addr_nsh (a), &nsh);
+    }
   else
     return 0;
 
@@ -301,10 +275,11 @@ unformat_gid_address (unformat_input_t * input, va_list * args)
   u8 mac[6] = { 0 };
   ip_prefix_t ippref;
   fid_address_t sim1, sim2;
+  nsh_t nsh;
 
-  memset (&ippref, 0, sizeof (ippref));
-  memset (&sim1, 0, sizeof (sim1));
-  memset (&sim2, 0, sizeof (sim2));
+  clib_memset (&ippref, 0, sizeof (ippref));
+  clib_memset (&sim1, 0, sizeof (sim1));
+  clib_memset (&sim2, 0, sizeof (sim2));
 
   if (unformat (input, "%U|%U", unformat_fid_address, &sim1,
 		unformat_fid_address, &sim2))
@@ -323,6 +298,11 @@ unformat_gid_address (unformat_input_t * input, va_list * args)
       mac_copy (gid_address_mac (a), mac);
       gid_address_type (a) = GID_ADDR_MAC;
     }
+  else if (unformat (input, "%U", unformat_nsh_address, &nsh))
+    {
+      nsh_copy (&gid_address_nsh (a), &nsh);
+      gid_address_type (a) = GID_ADDR_NSH;
+    }
   else
     return 0;
 
@@ -400,49 +380,10 @@ format_negative_mapping_action (u8 * s, va_list * args)
   return (s);
 }
 
-u16
-ip_address_size (const ip_address_t * a)
-{
-  switch (ip_addr_version (a))
-    {
-    case IP4:
-      return sizeof (ip4_address_t);
-      break;
-    case IP6:
-      return sizeof (ip6_address_t);
-      break;
-    }
-  return 0;
-}
-
-u16
-ip_version_to_size (u8 ver)
-{
-  switch (ver)
-    {
-    case IP4:
-      return sizeof (ip4_address_t);
-      break;
-    case IP6:
-      return sizeof (ip6_address_t);
-      break;
-    }
-  return 0;
-}
-
 u8
-ip_version_to_max_plen (u8 ver)
+ip_prefix_length (void *a)
 {
-  switch (ver)
-    {
-    case IP4:
-      return 32;
-      break;
-    case IP6:
-      return 128;
-      break;
-    }
-  return 0;
+  return ip_prefix_len ((ip_prefix_t *) a);
 }
 
 always_inline lisp_afi_e
@@ -450,9 +391,9 @@ ip_version_to_iana_afi (u16 version)
 {
   switch (version)
     {
-    case IP4:
+    case AF_IP4:
       return LISP_AFI_IP;
-    case IP6:
+    case AF_IP6:
       return LISP_AFI_IP6;
     default:
       return 0;
@@ -466,9 +407,9 @@ ip_iana_afi_to_version (lisp_afi_e afi)
   switch (afi)
     {
     case LISP_AFI_IP:
-      return IP4;
+      return AF_IP4;
     case LISP_AFI_IP6:
-      return IP6;
+      return AF_IP6;
     default:
       return 0;
     }
@@ -490,7 +431,7 @@ ip_address_iana_afi (ip_address_t * a)
 u8
 ip_address_max_len (u8 version)
 {
-  return version == IP4 ? 32 : 128;
+  return version == AF_IP4 ? 32 : 128;
 }
 
 u16
@@ -510,7 +451,7 @@ ip6_address_size_to_put ()
 u32
 ip4_address_put (u8 * b, ip4_address_t * a)
 {
-  *(u16 *) b = clib_host_to_net_u16 (ip_version_to_iana_afi (IP4));
+  *(u16 *) b = clib_host_to_net_u16 (ip_version_to_iana_afi (AF_IP4));
   u8 *p = b + sizeof (u16);
   clib_memcpy (p, a, sizeof (*a));
   return ip4_address_size_to_put ();
@@ -519,7 +460,7 @@ ip4_address_put (u8 * b, ip4_address_t * a)
 u32
 ip6_address_put (u8 * b, ip6_address_t * a)
 {
-  *(u16 *) b = clib_host_to_net_u16 (ip_version_to_iana_afi (IP6));
+  *(u16 *) b = clib_host_to_net_u16 (ip_version_to_iana_afi (AF_IP6));
   u8 *p = b + sizeof (u16);
   clib_memcpy (p, a, sizeof (*a));
   return ip6_address_size_to_put ();
@@ -531,7 +472,7 @@ ip_address_put (u8 * b, ip_address_t * a)
   u32 len = ip_address_size (a);
   *(u16 *) b = clib_host_to_net_u16 (ip_address_iana_afi (a));
   u8 *p = b + sizeof (u16);
-  clib_memcpy (p, &ip_addr_addr (a), len);
+  clib_memcpy (p, ip_addr_bytes (a), len);
   return (len + sizeof (u16));
 }
 
@@ -540,10 +481,48 @@ ip_address_parse (void *offset, u16 iana_afi, ip_address_t * dst)
 {
   ip_addr_version (dst) = ip_iana_afi_to_version (iana_afi);
   u8 size = ip_version_to_size (ip_addr_version (dst));
-  clib_memcpy (&ip_addr_addr (dst), offset + sizeof (u16), size);
+  clib_memcpy (ip_addr_bytes (dst), offset + sizeof (u16), size);
   return (sizeof (u16) + size);
 }
 
+void
+gid_to_dp_address (gid_address_t * g, dp_address_t * d)
+{
+  switch (gid_address_type (g))
+    {
+    case GID_ADDR_SRC_DST:
+      switch (gid_address_sd_dst_type (g))
+	{
+	case FID_ADDR_IP_PREF:
+	  ip_prefix_copy (&d->ippref, &gid_address_sd_dst_ippref (g));
+	  d->type = FID_ADDR_IP_PREF;
+	  break;
+	case FID_ADDR_MAC:
+	  mac_copy (&d->mac, &gid_address_sd_dst_mac (g));
+	  d->type = FID_ADDR_MAC;
+	  break;
+	default:
+	  clib_warning ("Source/Dest address type %d not supported!",
+			gid_address_sd_dst_type (g));
+	  break;
+	}
+      break;
+    case GID_ADDR_IP_PREFIX:
+      ip_prefix_copy (&d->ippref, &gid_address_ippref (g));
+      d->type = FID_ADDR_IP_PREF;
+      break;
+    case GID_ADDR_MAC:
+      mac_copy (&d->mac, &gid_address_mac (g));
+      d->type = FID_ADDR_MAC;
+      break;
+    case GID_ADDR_NSH:
+    default:
+      d->nsh = gid_address_nsh (g).spi << 8 | gid_address_nsh (g).si;
+      d->type = FID_ADDR_NSH;
+      break;
+    }
+}
+
 u32
 lcaf_hdr_parse (void *offset, lcaf_t * lcaf)
 {
@@ -588,10 +567,53 @@ fid_addr_parse (u8 * p, fid_address_t * a)
 
     case FID_ADDR_IP_PREF:
       return ip_address_parse (p, afi, ip_addr);
+
+    case FID_ADDR_NSH:
+      break;
     }
   return ~0;
 }
 
+#define INC(dst, exp)   \
+do {                    \
+  u16 _sum = (exp);     \
+  if ((u16)~0 == _sum)  \
+    return ~0;          \
+  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)
 {
@@ -606,8 +628,8 @@ sd_parse (u8 * p, void *a)
   sd_hdr = (lcaf_src_dst_hdr_t *) (p + size);
   size += sizeof (sd_hdr[0]);
 
-  size += fid_addr_parse (p + size, src);
-  size += fid_addr_parse (p + size, dst);
+  INC (size, fid_addr_parse (p + size, src));
+  INC (size, fid_addr_parse (p + size, dst));
 
   if (fid_addr_type (src) == FID_ADDR_IP_PREF)
     {
@@ -633,7 +655,7 @@ try_parse_src_dst_lcaf (u8 * p, gid_address_t * a)
   if (LCAF_SOURCE_DEST != lcaf_type (&lcaf))
     return ~0;
 
-  size += sd_parse (p + size, a);
+  INC (size, sd_parse (p + size, a));
   return size;
 }
 
@@ -653,13 +675,10 @@ vni_parse (u8 * p, void *a)
   u16 afi = clib_net_to_host_u16 (*((u16 *) (p + size)));
   if (LISP_AFI_LCAF == afi)
     {
-      u16 len = try_parse_src_dst_lcaf (p + size, g);
-      if ((u16) ~ 0 == len)
-	return ~0;
-      size += len;
+      INC (size, try_parse_src_dst_lcaf (p + size, g));
     }
   else
-    size += gid_address_parse (p + size, g);
+    INC (size, gid_address_parse (p + size, g));
 
   return size;
 }
@@ -686,7 +705,7 @@ lcaf_parse (void *offset, gid_address_t * addr)
       clib_warning ("Unsupported LCAF type: %u", type);
       return ~0;
     }
-  size += (*lcaf_parse_fcts[type]) (offset + size, lcaf);
+  INC (size, (*lcaf_parse_fcts[type]) (offset + size, lcaf));
   return sizeof (u16) + size;
 }
 
@@ -724,150 +743,11 @@ gid_address_free (gid_address_t * a)
 void
 gid_address_from_ip (gid_address_t * g, ip_address_t * ip)
 {
-  memset (g, 0, sizeof (g[0]));
+  clib_memset (g, 0, sizeof (g[0]));
   ip_address_set (&gid_address_ip (g), ip, ip_addr_version (ip));
   gid_address_ippref_len (g) = 32;
 }
 
-int
-ip_address_cmp (const ip_address_t * ip1, const ip_address_t * ip2)
-{
-  int res = 0;
-  if (ip_addr_version (ip1) != ip_addr_version (ip2))
-    return -1;
-  res =
-    memcmp (&ip_addr_addr (ip1), &ip_addr_addr (ip2), ip_address_size (ip1));
-
-  if (res < 0)
-    res = 2;
-  else if (res > 0)
-    res = 1;
-
-  return res;
-}
-
-void
-ip_address_copy (ip_address_t * dst, const ip_address_t * src)
-{
-  if (IP4 == ip_addr_version (src))
-    {
-      /* don't copy any garbe from the union */
-      memset (dst, 0, sizeof (*dst));
-      dst->ip.v4 = src->ip.v4;
-      dst->version = IP4;
-    }
-  else
-    {
-      clib_memcpy (dst, src, sizeof (ip_address_t));
-    }
-}
-
-void
-ip_address_copy_addr (void *dst, const ip_address_t * src)
-{
-  clib_memcpy (dst, src, ip_address_size (src));
-}
-
-void
-ip_address_set (ip_address_t * dst, const void *src, u8 version)
-{
-  clib_memcpy (dst, src, ip_version_to_size (version));
-  ip_addr_version (dst) = version;
-}
-
-void
-ip_address_to_46 (const ip_address_t * addr,
-		  ip46_address_t * a, fib_protocol_t * proto)
-{
-  *proto = (IP4 == ip_addr_version (addr) ?
-	    FIB_PROTOCOL_IP4 : FIB_PROTOCOL_IP6);
-  switch (*proto)
-    {
-    case FIB_PROTOCOL_IP4:
-      ip46_address_set_ip4 (a, &addr->ip.v4);
-      break;
-    case FIB_PROTOCOL_IP6:
-      a->ip6 = addr->ip.v6;
-      break;
-    default:
-      ASSERT (0);
-      break;
-    }
-}
-
-static void
-ip_prefix_normalize_ip4 (ip4_address_t * ip4, u8 preflen)
-{
-  u32 mask = ~0;
-
-  ASSERT (ip4);
-
-  if (32 <= preflen)
-    {
-      return;
-    }
-
-  mask = pow2_mask (preflen) << (32 - preflen);
-  mask = clib_host_to_net_u32 (mask);
-  ip4->data_u32 &= mask;
-}
-
-static void
-ip_prefix_normalize_ip6 (ip6_address_t * ip6, u8 preflen)
-{
-  u8 mask_6[16];
-  u32 *m;
-  u8 j, i0, i1;
-
-  ASSERT (ip6);
-
-  memset (mask_6, 0, sizeof (mask_6));
-
-  if (128 <= preflen)
-    {
-      return;
-    }
-
-  i1 = preflen % 32;
-  i0 = preflen / 32;
-  m = (u32 *) & mask_6[0];
-
-  for (j = 0; j < i0; j++)
-    {
-      m[j] = ~0;
-    }
-
-  if (i1)
-    {
-      m[i0] = clib_host_to_net_u32 (pow2_mask (i1) << (32 - i1));
-    }
-
-  for (j = 0; j < sizeof (mask_6); j++)
-    {
-      ip6->as_u8[j] &= mask_6[j];
-    }
-}
-
-void
-ip_prefix_normalize (ip_prefix_t * a)
-{
-  u8 preflen = ip_prefix_len (a);
-
-  switch (ip_prefix_version (a))
-    {
-    case IP4:
-      ip_prefix_normalize_ip4 (&ip_prefix_v4 (a), preflen);
-      break;
-
-    case IP6:
-      ip_prefix_normalize_ip6 (&ip_prefix_v6 (a), preflen);
-      break;
-
-    default:
-      ASSERT (0);
-    }
-}
-
 void *
 ip_prefix_cast (gid_address_t * a)
 {
@@ -889,28 +769,16 @@ ip_prefix_write (u8 * p, void *gid)
 
   switch (ip_prefix_version (a))
     {
-    case IP4:
+    case AF_IP4:
       return ip4_address_put (p, &ip_prefix_v4 (a));
       break;
-    case IP6:
+    case AF_IP6:
       return ip6_address_put (p, &ip_prefix_v6 (a));
       break;
     }
   return 0;
 }
 
-u8
-ip_prefix_length (void *a)
-{
-  return ip_prefix_len ((ip_prefix_t *) a);
-}
-
-void
-ip_prefix_copy (void *dst, void *src)
-{
-  clib_memcpy (dst, src, sizeof (ip_prefix_t));
-}
-
 void
 mac_copy (void *dst, void *src)
 {
@@ -923,28 +791,10 @@ sd_copy (void *dst, void *src)
   clib_memcpy (dst, src, sizeof (source_dest_t));
 }
 
-int
-ip_prefix_cmp (ip_prefix_t * p1, ip_prefix_t * p2)
+void
+nsh_copy (void *dst, void *src)
 {
-  int cmp = 0;
-
-  ip_prefix_normalize (p1);
-  ip_prefix_normalize (p2);
-
-  cmp = ip_address_cmp (&ip_prefix_addr (p1), &ip_prefix_addr (p2));
-  if (cmp == 0)
-    {
-      if (ip_prefix_len (p1) < ip_prefix_len (p2))
-	{
-	  cmp = 1;
-	}
-      else
-	{
-	  if (ip_prefix_len (p1) > ip_prefix_len (p2))
-	    cmp = 2;
-	}
-    }
-  return cmp;
+  clib_memcpy (dst, src, sizeof (nsh_t));
 }
 
 void
@@ -992,6 +842,12 @@ sd_length (void *a)
   return 0;
 }
 
+u8
+nsh_length (void *a)
+{
+  return 0;
+}
+
 void *
 lcaf_cast (gid_address_t * a)
 {
@@ -1004,12 +860,24 @@ mac_cast (gid_address_t * a)
   return &gid_address_mac (a);
 }
 
+void *
+no_addr_cast (gid_address_t * a)
+{
+  return (void *) a;
+}
+
 void *
 sd_cast (gid_address_t * a)
 {
   return &gid_address_sd (a);
 }
 
+void *
+nsh_cast (gid_address_t * a)
+{
+  return &gid_address_nsh (a);
+}
+
 u8
 no_addr_length (void *a)
 {
@@ -1034,7 +902,7 @@ lcaf_write (u8 * p, void *a)
 
   *(u16 *) p = clib_host_to_net_u16 (LISP_AFI_LCAF);
   size += sizeof (u16);
-  memset (h, 0, sizeof (h[0]));
+  clib_memset (h, 0, sizeof (h[0]));
   LCAF_TYPE (h) = type;
   u16 lcaf_len = (*lcaf_body_length_fcts[type]) (lcaf);
   LCAF_LENGTH (h) = clib_host_to_net_u16 (lcaf_len);
@@ -1083,6 +951,8 @@ fid_address_length (fid_address_t * a)
       return ip_prefix_length (&fid_addr_ippref (a));
     case FID_ADDR_MAC:
       return 0;
+    case FID_ADDR_NSH:
+      return 0;
     }
   return 0;
 }
@@ -1097,7 +967,7 @@ sd_write (u8 * p, void *a)
 
   *(u16 *) p = clib_host_to_net_u16 (LISP_AFI_LCAF);
   size += sizeof (u16);
-  memset (h, 0, sizeof (h[0]));
+  clib_memset (h, 0, sizeof (h[0]));
   LCAF_TYPE (h) = LCAF_SOURCE_DEST;
   u16 lcaf_len = sizeof (lcaf_src_dst_hdr_t)
     + fid_addr_size_to_write (&sd_src (sd))
@@ -1107,7 +977,7 @@ sd_write (u8 * p, void *a)
   clib_memcpy (p + size, h, sizeof (h[0]));
   size += sizeof (h[0]);
 
-  memset (&sd_hdr, 0, sizeof (sd_hdr));
+  clib_memset (&sd_hdr, 0, sizeof (sd_hdr));
   LCAF_SD_SRC_ML (&sd_hdr) = fid_address_length (&sd_src (sd));
   LCAF_SD_DST_ML (&sd_hdr) = fid_address_length (&sd_dst (sd));
   clib_memcpy (p + size, &sd_hdr, sizeof (sd_hdr));
@@ -1126,6 +996,38 @@ sd_write (u8 * p, void *a)
   return size;
 }
 
+u16
+nsh_write (u8 * p, void *a)
+{
+  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);
+
+  clib_memset (&lcaf, 0, sizeof (lcaf));
+  clib_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
 vni_write (u8 * p, void *a)
 {
@@ -1136,7 +1038,7 @@ vni_write (u8 * p, void *a)
   /* put lcaf header */
   *(u16 *) p = clib_host_to_net_u16 (LISP_AFI_LCAF);
   size += sizeof (u16);
-  memset (h, 0, sizeof (h[0]));
+  clib_memset (h, 0, sizeof (h[0]));
   LCAF_TYPE (h) = LCAF_INSTANCE_ID;
   u16 lcaf_len = sizeof (u32)	/* Instance ID size */
     + gid_address_size_to_put_no_vni (g);
@@ -1246,6 +1148,12 @@ mac_size_to_write (void *a)
   return sizeof (u16) + 6;
 }
 
+u16
+nsh_size_to_write (void *a)
+{
+  return sizeof (u16) + sizeof (lcaf_hdr_t) + sizeof (lcaf_spi_hdr_t);
+}
+
 u8
 gid_address_len (gid_address_t * a)
 {
@@ -1315,10 +1223,9 @@ u32
 gid_address_parse (u8 * offset, gid_address_t * a)
 {
   lisp_afi_e afi;
-  int len = 0;
+  u16 len = 0;
 
-  if (!a)
-    return 0;
+  ASSERT (a);
 
   /* NOTE: since gid_address_parse may be called by vni_parse, we can't 0
    * the gid address here */
@@ -1354,7 +1261,7 @@ gid_address_parse (u8 * offset, gid_address_t * a)
       clib_warning ("LISP AFI %d not supported!", afi);
       return ~0;
     }
-  return len;
+  return (len == (u16) ~ 0) ? ~0 : len;
 }
 
 void
@@ -1440,8 +1347,11 @@ gid_address_cmp (gid_address_t * a1, gid_address_t * a2)
     return -1;
   if (gid_address_vni (a1) != gid_address_vni (a2))
     return -1;
-  if (gid_address_vni_mask (a1) != gid_address_vni_mask (a2))
-    return -1;
+
+  /* TODO vni mask is not supported, disable comparing for now
+     if (gid_address_vni_mask (a1) != gid_address_vni_mask (a2))
+     return -1;
+   */
 
   switch (gid_address_type (a1))
     {
@@ -1468,6 +1378,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;
     }
@@ -1492,6 +1405,7 @@ locator_parse (void *b, locator_t * loc)
 
   loc->state = status;
   loc->local = 0;
+  loc->probed = 0;
   loc->priority = LOC_PRIORITY (h);
   loc->weight = LOC_WEIGHT (h);
   loc->mpriority = LOC_MPRIORITY (h);
@@ -1537,7 +1451,7 @@ locator_free (locator_t * l)
 void
 build_src_dst (gid_address_t * sd, gid_address_t * src, gid_address_t * dst)
 {
-  memset (sd, 0, sizeof (*sd));
+  clib_memset (sd, 0, sizeof (*sd));
   gid_address_type (sd) = GID_ADDR_SRC_DST;
   gid_address_vni (sd) = gid_address_vni (dst);
   gid_address_vni_mask (sd) = gid_address_vni_mask (dst);