Basic support for LISP-GPE encapsulated NSH packets

Change-Id: I97fedb0f70dd18ed9bbe985407cc5fe714e8a2e2
Signed-off-by: Florin Coras <fcoras@cisco.com>
diff --git a/src/vnet/lisp-cp/control.c b/src/vnet/lisp-cp/control.c
index cc73dfc..f0383e1 100644
--- a/src/vnet/lisp-cp/control.c
+++ b/src/vnet/lisp-cp/control.c
@@ -2700,6 +2700,11 @@
       gid_address_vni (dst) = vni;
       gid_address_vni (src) = vni;
     }
+  else if (LISP_AFI_LCAF == type)
+    {
+      /* Eventually extend this to support NSH and other */
+      ASSERT (0);
+    }
 }
 
 static uword
@@ -2818,6 +2823,14 @@
   return (lisp_cp_lookup_inline (vm, node, from_frame, LISP_AFI_MAC));
 }
 
+static uword
+lisp_cp_lookup_nsh (vlib_main_t * vm,
+		    vlib_node_runtime_t * node, vlib_frame_t * from_frame)
+{
+  /* TODO decide if NSH should be propagated as LCAF or not */
+  return (lisp_cp_lookup_inline (vm, node, from_frame, LISP_AFI_LCAF));
+}
+
 /* *INDENT-OFF* */
 VLIB_REGISTER_NODE (lisp_cp_lookup_ip4_node) = {
   .function = lisp_cp_lookup_ip4,
@@ -2875,6 +2888,25 @@
 };
 /* *INDENT-ON* */
 
+/* *INDENT-OFF* */
+VLIB_REGISTER_NODE (lisp_cp_lookup_nsh_node) = {
+  .function = lisp_cp_lookup_nsh,
+  .name = "lisp-cp-lookup-nsh",
+  .vector_size = sizeof (u32),
+  .format_trace = format_lisp_cp_lookup_trace,
+  .type = VLIB_NODE_TYPE_INTERNAL,
+
+  .n_errors = LISP_CP_LOOKUP_N_ERROR,
+  .error_strings = lisp_cp_lookup_error_strings,
+
+  .n_next_nodes = LISP_CP_LOOKUP_N_NEXT,
+
+  .next_nodes = {
+      [LISP_CP_LOOKUP_NEXT_DROP] = "error-drop",
+  },
+};
+/* *INDENT-ON* */
+
 /* lisp_cp_input statistics */
 #define foreach_lisp_cp_input_error                               \
 _(DROP, "drop")                                                   \
diff --git a/src/vnet/lisp-cp/lisp_api.c b/src/vnet/lisp-cp/lisp_api.c
index a877540..78d32e1 100644
--- a/src/vnet/lisp-cp/lisp_api.c
+++ b/src/vnet/lisp-cp/lisp_api.c
@@ -714,6 +714,8 @@
 
     case FID_ADDR_MAC:
       return 2;
+    case FID_ADDR_NSH:
+      return 3;
     }
 
   return ~0;
diff --git a/src/vnet/lisp-cp/lisp_cp_dpo.c b/src/vnet/lisp-cp/lisp_cp_dpo.c
index 185b07a..848f621 100644
--- a/src/vnet/lisp-cp/lisp_cp_dpo.c
+++ b/src/vnet/lisp-cp/lisp_cp_dpo.c
@@ -79,12 +79,17 @@
   NULL,
 };
 
+const static char *const lisp_cp_nsh_nodes[] = {
+  "lisp-cp-lookup-nsh",
+  NULL,
+};
 
 const static char *const *const lisp_cp_nodes[DPO_PROTO_NUM] = {
   [DPO_PROTO_IP4] = lisp_cp_ip4_nodes,
   [DPO_PROTO_IP6] = lisp_cp_ip6_nodes,
   [DPO_PROTO_ETHERNET] = lisp_cp_ethernet_nodes,
   [DPO_PROTO_MPLS] = NULL,
+  [DPO_PROTO_NSH] = lisp_cp_nsh_nodes,
 };
 
 clib_error_t *
diff --git a/src/vnet/lisp-cp/lisp_types.c b/src/vnet/lisp-cp/lisp_types.c
index 748905d..4a3d05b 100644
--- a/src/vnet/lisp-cp/lisp_types.c
+++ b/src/vnet/lisp-cp/lisp_types.c
@@ -202,6 +202,20 @@
 		 a[0], a[1], a[2], a[3], a[4], a[5]);
 }
 
+uword
+unformat_nsh_address (unformat_input_t * input, va_list * args)
+{
+  nsh_t *a = va_arg (*args, nsh_t *);
+  return unformat (input, "SPI:%d SI:%d", &a->spi, &a->si);
+}
+
+u8 *
+format_nsh_address (u8 * s, va_list * args)
+{
+  nsh_t *a = va_arg (*args, nsh_t *);
+  return format (s, "SPI:%d SI:%d", a->spi, a->si);
+}
+
 u8 *
 format_fid_address (u8 * s, va_list * args)
 {
@@ -211,9 +225,10 @@
     {
     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_nsh_address, &fid_addr_nsh (a));
 
     default:
       clib_warning ("Can't format fid address type %d!", fid_addr_type (a));
@@ -239,6 +254,8 @@
     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));
     default:
       clib_warning ("Can't format gid type %d", type);
       return 0;
@@ -252,6 +269,7 @@
   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 +281,11 @@
       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), mac);
+    }
   else
     return 0;
 
@@ -301,6 +324,7 @@
   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));
@@ -323,6 +347,11 @@
       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;
 
@@ -588,6 +617,10 @@
 
     case FID_ADDR_IP_PREF:
       return ip_address_parse (p, afi, ip_addr);
+
+    case FID_ADDR_NSH:
+      ASSERT (0);
+      break;
     }
   return ~0;
 }
@@ -918,6 +951,12 @@
 }
 
 void
+nsh_copy (void *dst, void *src)
+{
+  clib_memcpy (dst, src, sizeof (nsh_t));
+}
+
+void
 sd_copy (void *dst, void *src)
 {
   clib_memcpy (dst, src, sizeof (source_dest_t));
@@ -1083,6 +1122,8 @@
       return ip_prefix_length (&fid_addr_ippref (a));
     case FID_ADDR_MAC:
       return 0;
+    case FID_ADDR_NSH:
+      return 0;
     }
   return 0;
 }
diff --git a/src/vnet/lisp-cp/lisp_types.h b/src/vnet/lisp-cp/lisp_types.h
index ac58b89..e43f5ab 100644
--- a/src/vnet/lisp-cp/lisp_types.h
+++ b/src/vnet/lisp-cp/lisp_types.h
@@ -89,6 +89,7 @@
   GID_ADDR_LCAF,
   GID_ADDR_MAC,
   GID_ADDR_SRC_DST,
+  GID_ADDR_NSH,
   GID_ADDR_NO_ADDRESS,
   GID_ADDR_TYPES
 } gid_address_type_t;
@@ -106,7 +107,8 @@
 typedef enum fid_addr_type_t_
 {
   FID_ADDR_IP_PREF,
-  FID_ADDR_MAC
+  FID_ADDR_MAC,
+  FID_ADDR_NSH
 } __attribute__ ((packed)) fid_addr_type_t;
 
 /* flat address type */
@@ -116,6 +118,7 @@
   {
     ip_prefix_t ippref;
     u8 mac[6];
+    u32 nsh;
   };
   fid_addr_type_t type;
 } fid_address_t;
@@ -124,6 +127,7 @@
 
 #define fid_addr_ippref(_a) (_a)->ippref
 #define fid_addr_mac(_a) (_a)->mac
+#define fid_addr_nsh(_a) (_a)->nsh
 #define fid_addr_type(_a) (_a)->type
 u8 *format_fid_address (u8 * s, va_list * args);
 
@@ -155,6 +159,12 @@
 
 typedef struct
 {
+  u32 spi;
+  u8 si;
+} nsh_t;
+
+typedef struct
+{
   /* the union needs to be at the beginning! */
   union
   {
@@ -177,6 +187,7 @@
     lcaf_t lcaf;
     u8 mac[6];
     source_dest_t sd;
+    nsh_t nsh;
   };
   u8 type;
   u32 vni;
@@ -232,6 +243,7 @@
 #define gid_address_ip_version(_a) ip_addr_version(&gid_address_ip(_a))
 #define gid_address_lcaf(_a) (_a)->lcaf
 #define gid_address_mac(_a) (_a)->mac
+#define gid_address_nsh(_a) (_a)->nsh
 #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)
@@ -249,6 +261,7 @@
   _(ip_prefix)                    \
   _(lcaf)                         \
   _(mac)                          \
+  _(nsh)                          \
   _(sd)
 
 /* *INDENT-OFF* */