LISP GPE: initial CP commit and DP improvements

Control Plane
-------------
In essence, this introduces basic support for map-request/reply
processing, the logic to generate and consume such messages, including
SMRs, a control-plane backend, consisting of an eid-table, locator and
locator-set tables, and CLI to interact with it. Naturally, we can now
serialize/deserialize LISP specific types: addresses, locators,
mappings, messages. An important caveat is that IPv6 support is not
complete, both for EIDs and RLOCs.

Functionally, the DP forwards all packets it can't handle to the CP
(lisp_cp_lookup node) which takes care of obtaining a mapping for the
packet's destination from a pre-configured map-resolver using the LISP
protocol. The CP then caches this information and programs the DP such
that all new packets with the same destination (or within the covering
prefix) are encapsulated to one of the locators retrieved in the
mapping. Ingress traffic-engineering is not yet supported.

Data Plane
----------
First of all, to enable punting to the CP, when LISP GPE is turned on a
default route that points to lisp_cp_lookup is now inserted. The DP
also exposes an API the CP can use to program forwarding for a given
mapping. This mainly consists in allocating a tunnel and programming the
FIB such that all packets destined to the mapping's prefix are forwarded
to a lisp-gpe encapsulating node.

Another important change done for lisp forwarding is that both source
and destination IP addresses are considered when encapsulating a packet.
To this end, a new FIB/mtrie is introduced as a second stage, src
lookup, post dst lookup. The latter is still done in the IP FIB but for
source-dest entries, in the dest adjacency the lookup_next_index points
to a lisp lookup node and the rewrite_header.sw_if_index points to the
src FIB. This is read by the lisp lookup node which then walks the src
mtrie, finds the associated adjacency, marks the buffer with the index
and forwards the packet to the appropriate next node (typically,
lisp-gpe-encap).

Change-Id: Ibdf52fdc1f89311854621403ccdd66f90e2522fd
Signed-off-by: Florin Coras <fcoras@cisco.com>

19 files changed, 5136 insertions, 705 deletions

diff --git a/vnet/Makefile.am b/vnet/Makefile.am
index b254d80ad48..52b5a6d0779 100644
--- a/vnet/Makefile.am
+++ b/vnet/Makefile.am
@@ -425,18 +425,37 @@ nobase_include_HEADERS +=			\
  vnet/nsh-vxlan-gpe/nsh_vxlan_gpe_error.def
 
 ########################################
+# LISP control plane: lisp-cp
+########################################
+
+libvnet_la_SOURCES +=				\
+ vnet/lisp-cp/lisp_types.c			\
+ vnet/lisp-cp/control.c				\
+ vnet/lisp-cp/gid_dictionary.c			\
+ vnet/lisp-cp/lisp_msg_serdes.c			\
+ vnet/lisp-cp/packets.c			
+
+nobase_include_HEADERS +=			\
+ vnet/lisp-cp/lisp_types.h			\
+ vnet/lisp-cp/packets.h				\
+ vnet/lisp-cp/gid_dictionary.h			\
+ vnet/lisp-cp/lisp_cp_messages.h		\
+ vnet/lisp-cp/lisp_msg_serdes.h			\
+ vnet/lisp-cp/control.h				
+
+########################################
 # Tunnel protocol: lisp-gpe
 ########################################
 
 libvnet_la_SOURCES +=				\
  vnet/lisp-gpe/lisp_gpe.c			\
  vnet/lisp-gpe/encap.c				\
- vnet/lisp-gpe/decap.c
+ vnet/lisp-gpe/decap.c				
 
 nobase_include_HEADERS +=			\
  vnet/lisp-gpe/lisp_gpe.h			\
  vnet/lisp-gpe/lisp_gpe_packet.h		\
- vnet/lisp-gpe/lisp_gpe_error.def
+ vnet/lisp-gpe/lisp_gpe_error.def		
 
 ########################################
 # DHCP client
diff --git a/vnet/vnet/ip/udp.h b/vnet/vnet/ip/udp.h
index 65eef29cb10..e9ee1e32faa 100644
--- a/vnet/vnet/ip/udp.h
+++ b/vnet/vnet/ip/udp.h
@@ -38,6 +38,7 @@ _ (67, dhcp_to_server)                          \
 _ (68, dhcp_to_client)                          \
 _ (500, ikev2)                                  \
 _ (4341, lisp_gpe)                              \
+_ (4342, lisp_cp)                          	\
 _ (4739, ipfix)                                 \
 _ (4789, vxlan)					\
 _ (4790, vxlan_gpe)				\
@@ -47,6 +48,7 @@ _ (6633, vpath_3)
 #define foreach_udp6_dst_port                   \
 _ (547, dhcpv6_to_server)                       \
 _ (546, dhcpv6_to_client)			\
+_ (4342, lisp_cp6)                          	\
 _ (6633, vpath6_3)
 
 typedef enum {
@@ -109,5 +111,100 @@ void udp_register_dst_port (vlib_main_t * vm,
                             udp_dst_port_t dst_port,
                             u32 node_index, u8 is_ip4);
 
-#endif /* included_udp_h */
+always_inline void
+ip4_udp_encap_one (vlib_main_t * vm, vlib_buffer_t * b0, u8 * ec0, u32 ec_len)
+{
+  ip4_header_t * ip0;
+  ip_csum_t sum0;
+  u16 old_l0 = 0;
+  u16 new_l0;
+  udp_header_t * udp0;
+
+  vlib_buffer_advance (b0, - ec_len);
+  ip0 = vlib_buffer_get_current(b0);
+
+  /* Apply the encap string. */
+#if DPDK
+  rte_memcpy(ip0, ec0, ec_len);
+#else
+  memcpy(ip0, ec0, ec_len);
+#endif
+
+  /* fix the <bleep>ing outer-IP checksum */
+  sum0 = ip0->checksum;
+  /* old_l0 always 0, see the rewrite setup */
+  new_l0 = clib_host_to_net_u16 (vlib_buffer_length_in_chain (vm, b0));
+
+  sum0 = ip_csum_update(sum0, old_l0, new_l0, ip4_header_t,
+                        length /* changed member */);
+  ip0->checksum = ip_csum_fold (sum0);
+  ip0->length = new_l0;
+
+  /* Fix UDP length */
+  udp0 = (udp_header_t *)(ip0+1);
+  new_l0 = clib_host_to_net_u16 (vlib_buffer_length_in_chain (vm, b0)
+                                 - sizeof (*ip0));
+
+  udp0->length = new_l0;
+}
 
+always_inline void
+ip4_udp_encap_two (vlib_main_t * vm, vlib_buffer_t * b0, vlib_buffer_t * b1,
+                   u8 * ec0, u8 * ec1, u32 ec_len)
+{
+  ip4_header_t * ip0, *ip1;
+  ip_csum_t sum0, sum1;
+  u16 old_l0 = 0, old_l1 = 0;
+  u16 new_l0, new_l1;
+  udp_header_t * udp0, *udp1;
+
+  ASSERT(_vec_len(ec0) == _vec_len(ec1));
+
+  vlib_buffer_advance (b0, -ec_len);
+  vlib_buffer_advance (b1, -ec_len);
+
+  ip0 = vlib_buffer_get_current (b0);
+  ip1 = vlib_buffer_get_current (b1);
+
+  /* Apply the encap string */
+#ifdef DPDK
+  rte_memcpy (ip0, ec0, ec_len);
+  rte_memcpy (ip1, ec1, ec_len);
+#else
+  memcpy (ip0, ec0, ec_len);
+  memcpy (ip1, ec1, ec_len);
+#endif
+
+  /* fix the <bleep>ing outer-IP checksum */
+  sum0 = ip0->checksum;
+  sum1 = ip1->checksum;
+
+  /* old_l0 always 0, see the rewrite setup */
+  new_l0 = clib_host_to_net_u16 (vlib_buffer_length_in_chain (vm, b0));
+  new_l1 = clib_host_to_net_u16 (vlib_buffer_length_in_chain (vm, b1));
+
+  sum0 = ip_csum_update(sum0, old_l0, new_l0, ip4_header_t,
+                        length /* changed member */);
+  sum1 = ip_csum_update(sum1, old_l1, new_l1, ip4_header_t,
+                        length /* changed member */);
+
+  ip0->checksum = ip_csum_fold (sum0);
+  ip1->checksum = ip_csum_fold (sum1);
+
+  ip0->length = new_l0;
+  ip1->length = new_l1;
+
+  /* Fix UDP length */
+  udp0 = (udp_header_t *) (ip0 + 1);
+  udp1 = (udp_header_t *) (ip1 + 1);
+
+  new_l0 = clib_host_to_net_u16 (
+      vlib_buffer_length_in_chain (vm, b0) - sizeof(*ip0));
+  new_l1 = clib_host_to_net_u16 (
+      vlib_buffer_length_in_chain (vm, b1) - sizeof(*ip1));
+  udp0->length = new_l0;
+  udp1->length = new_l1;
+  return;
+}
+
+#endif /* included_udp_h */
diff --git a/vnet/vnet/lisp-cp/control.c b/vnet/vnet/lisp-cp/control.c
new file mode 100644
index 00000000000..f4cb16fa016
--- /dev/null
+++ b/vnet/vnet/lisp-cp/control.c
@@ -0,0 +1,1362 @@
+/*
+ * 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.
+ */
+
+#include <vnet/lisp-cp/control.h>
+#include <vnet/lisp-cp/packets.h>
+#include <vnet/lisp-cp/lisp_msg_serdes.h>
+#include <vnet/lisp-gpe/lisp_gpe.h>
+
+/* Adds mapping to map-cache but does NOT program LISP forwarding */
+int
+vnet_lisp_add_del_mapping (vnet_lisp_add_del_mapping_args_t * a,
+                           u32 * map_index_result)
+{
+  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
+  u32 mi, * map_indexp, map_index, i;
+  mapping_t * m;
+  u32 ** eid_indexes;
+
+  mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &a->deid);
+  if (a->is_add)
+    {
+      /* TODO check if overwriting and take appropriate actions */
+      if (mi != GID_LOOKUP_MISS) {
+          clib_warning("eid %U found in the eid-table", format_ip_address,
+                       &a->deid);
+          return -1;
+      }
+
+      pool_get(lcm->mapping_pool, m);
+      m->eid = a->deid;
+      m->locator_set_index = a->locator_set_index;
+      m->ttl = a->ttl;
+      m->local = a->local;
+
+      map_index = m - lcm->mapping_pool;
+      gid_dictionary_add_del (&lcm->mapping_index_by_gid, &a->deid, map_index,
+                              1);
+
+      if (pool_is_free_index(lcm->locator_set_pool, a->locator_set_index))
+        {
+          clib_warning("Locator set with index %d doesn't exist",
+                       a->locator_set_index);
+          return -1;
+        }
+
+      /* add eid to list of eids supported by locator-set */
+      vec_validate (lcm->locator_set_to_eids, a->locator_set_index);
+      eid_indexes = vec_elt_at_index(lcm->locator_set_to_eids,
+                                     a->locator_set_index);
+      vec_add1(eid_indexes[0], map_index);
+
+      if (a->local)
+        {
+          /* mark as local */
+          vec_add1(lcm->local_mappings_indexes, map_index);
+
+          /* XXX do something else? */
+        }
+      map_index_result[0] = map_index;
+    }
+  else
+    {
+      if (mi != ~0) {
+          clib_warning("eid %U not found in the eid-table", format_ip_address,
+                       &a->deid);
+          return -1;
+      }
+
+      /* clear locator-set to eids binding */
+      eid_indexes = vec_elt_at_index(lcm->locator_set_to_eids,
+                                     a->locator_set_index);
+      for (i = 0; i < vec_len(eid_indexes[0]); i++)
+        {
+          map_indexp = vec_elt_at_index(eid_indexes[0], i);
+          if (map_indexp[0] == mi)
+              break;
+        }
+      vec_del1(eid_indexes[0], i);
+
+      /* remove local mark if needed */
+      m = pool_elt_at_index(lcm->mapping_pool, mi);
+      if (m->local)
+        {
+          u32 k, * lm_indexp;
+          for (k = 0; k < vec_len(lcm->local_mappings_indexes); k++)
+            {
+              lm_indexp = vec_elt_at_index(lcm->local_mappings_indexes, k);
+              if (lm_indexp[0] == mi)
+                break;
+            }
+          vec_del1(lcm->local_mappings_indexes, k);
+        }
+      else
+        {
+          /* remove tunnel ??? */
+        }
+
+      /* remove mapping from dictionary */
+      gid_dictionary_add_del (&lcm->mapping_index_by_gid, &a->deid, 0, 0);
+      pool_put_index (lcm->mapping_pool, mi);
+    }
+
+  return 0;
+}
+
+static clib_error_t *
+lisp_add_del_local_eid_command_fn (vlib_main_t * vm, unformat_input_t * input,
+                                   vlib_cli_command_t * cmd)
+{
+  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
+  unformat_input_t _line_input, * line_input = &_line_input;
+  u8 is_add = 1;
+  gid_address_t eid;
+  ip_prefix_t * prefp = &gid_address_ippref(&eid);
+  gid_address_t * eids = 0;
+  clib_error_t * error = 0;
+  u8 * locator_set_name;
+  u32 locator_set_index = 0, map_index = 0;
+  uword * p;
+  vnet_lisp_add_del_mapping_args_t _a, * a = &_a;
+
+  gid_address_type (&eid) = IP_PREFIX;
+
+  /* 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, "add"))
+        is_add = 1;
+      else if (unformat (line_input, "del"))
+        is_add = 0;
+      else if (unformat (line_input, "eid %U", unformat_ip_prefix, prefp))
+        {
+          vec_add1(eids, eid);
+        }
+      else if (unformat (line_input, "locator-set %_%v%_", &locator_set_name))
+        {
+          p = hash_get_mem(lcm->locator_set_index_by_name, locator_set_name);
+          if (!p)
+            {
+              error = clib_error_return(0, "locator-set %s doesn't exist",
+                                        locator_set_name);
+              goto done;
+            }
+          locator_set_index = p[0];
+        }
+      else
+        {
+          error = unformat_parse_error(line_input);
+          goto done;
+        }
+    }
+
+  /* XXX treat batch configuration */
+  a->deid = eid;
+  a->is_add = is_add;
+  a->locator_set_index = locator_set_index;
+  a->local = 1;
+
+  vnet_lisp_add_del_mapping (a, &map_index);
+ done:
+  vec_free(eids);
+  return error;
+}
+
+VLIB_CLI_COMMAND (lisp_add_del_local_eid_command) = {
+    .path = "lisp eid-table",
+    .short_help = "lisp eid-table add/del eid <eid> locator-set <locator-set>",
+    .function = lisp_add_del_local_eid_command_fn,
+};
+
+static clib_error_t *
+lisp_show_local_eid_table_command_fn (vlib_main_t * vm,
+                                      unformat_input_t * input,
+                                      vlib_cli_command_t * cmd)
+{
+  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
+  mapping_t * mapit;
+
+  vlib_cli_output (vm, "%=20s%=16s", "EID", "Locator");
+  pool_foreach (mapit, lcm->mapping_pool,
+  ({
+    u8 * msg = 0;
+    locator_set_t * ls = pool_elt_at_index (lcm->locator_set_pool,
+                                            mapit->locator_set_index);
+    vlib_cli_output (vm, "%-16U%16v", format_gid_address, &mapit->eid,
+                     ls->name);
+    vec_free (msg);
+  }));
+
+  return 0;
+}
+
+VLIB_CLI_COMMAND (lisp_cp_show_local_eid_table_command) = {
+    .path = "show lisp eid-table",
+    .short_help = "Shows local EID table",
+    .function = lisp_show_local_eid_table_command_fn,
+};
+
+/* cleans locator to locator-set data and removes locators not part of
+ * any locator-set */
+static void
+clean_locator_to_locator_set (lisp_cp_main_t * lcm, u32 lsi)
+{
+  u32 i, j, *loc_indexp, *ls_indexp, **ls_indexes;
+  locator_set_t * ls = pool_elt_at_index(lcm->locator_set_pool, lsi);
+  for (i = 0; i < vec_len(ls->locator_indices); i++)
+    {
+      loc_indexp = vec_elt_at_index(ls->locator_indices, i);
+      ls_indexes = vec_elt_at_index(lcm->locator_to_locator_sets,
+                                    loc_indexp[0]);
+      for (j = 0; j < vec_len(ls_indexes[0]); j++)
+        {
+          ls_indexp = vec_elt_at_index(ls_indexes[0], j);
+          if (ls_indexp[0] == lsi)
+            break;
+        }
+
+      /* delete index for removed locator-set*/
+      vec_del1(ls_indexes[0], j);
+
+      /* delete locator if it's part of no locator-set */
+      if (vec_len (ls_indexes[0]) == 0)
+        pool_put_index(lcm->locator_pool, loc_indexp[0]);
+
+      /* remove from local locator-set vector */
+      if (ls->local)
+        {
+          u32 k, *llocp;
+          for (k = 0; k < vec_len(lcm->local_locator_set_indexes); k++)
+            {
+              llocp = vec_elt_at_index(lcm->local_locator_set_indexes, k);
+              if (llocp[0] == lsi)
+                break;
+            }
+          vec_del1(lcm->local_locator_set_indexes, k);
+        }
+    }
+}
+int
+vnet_lisp_add_del_locator_set (vnet_lisp_add_del_locator_set_args_t * a,
+                               u32 * ls_result)
+{
+  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
+  locator_set_t * ls;
+  locator_t * loc, * itloc;
+  uword _p = (u32)~0, * p = &_p;
+  u32 loc_index, ls_index, ** ls_indexes;
+
+  if (a->is_add)
+    {
+      /* check if overwrite */
+      if (a->local)
+        p = hash_get_mem(lcm->locator_set_index_by_name, a->name);
+      else
+        *p = a->index;
+
+      /* overwrite */
+      if (p && p[0] != (u32)~0)
+        {
+          ls = pool_elt_at_index(lcm->locator_set_pool, p[0]);
+          if (!ls)
+            {
+              clib_warning("locator-set %d to be overwritten doesn't exist!",
+                           p[0]);
+              return -1;
+            }
+
+          /* clean locator to locator-set vectors and remove locators if
+           * they're not part of another locator-set */
+          clean_locator_to_locator_set (lcm, p[0]);
+
+          /* remove locator indices from locator set */
+          vec_free(ls->locator_indices);
+
+          ls_index = p[0];
+
+          if (ls_result)
+            ls_result[0] = p[0];
+        }
+      /* new locator-set */
+      else
+        {
+          pool_get(lcm->locator_set_pool, ls);
+          ls_index = ls - lcm->locator_set_pool;
+
+          if (a->local)
+            {
+              ls->name = vec_dup(a->name);
+
+              if (!lcm->locator_set_index_by_name)
+                lcm->locator_set_index_by_name = hash_create_vec(
+                    /* size */0, sizeof(ls->name[0]), sizeof(uword));
+              hash_set_mem(lcm->locator_set_index_by_name, ls->name, ls_index);
+
+              /* mark as local locator-set */
+              vec_add1(lcm->local_locator_set_indexes, ls_index);
+            }
+          ls->local = a->local;
+          if (ls_result)
+            ls_result[0] = ls_index;
+        }
+
+      /* allocate locators */
+      vec_foreach (itloc, a->locators)
+        {
+          pool_get(lcm->locator_pool, loc);
+          loc[0] = itloc[0];
+          loc_index = loc - lcm->locator_pool;
+
+          vec_add1(ls->locator_indices, loc_index);
+
+          vec_validate (lcm->locator_to_locator_sets, loc_index);
+          ls_indexes = vec_elt_at_index(lcm->locator_to_locator_sets,
+                                        loc_index);
+          vec_add1(ls_indexes[0], ls_index);
+        }
+    }
+  else
+    {
+      /* find locator-set */
+      if (a->local)
+        {
+          p = hash_get_mem(lcm->locator_set_index_by_name, a->name);
+          if (!p)
+            {
+              clib_warning("locator-set %v doesn't exists", a->name);
+              return -1;
+            }
+        }
+      else
+        *p = a->index;
+
+      ls = pool_elt_at_index(lcm->locator_set_pool, p[0]);
+      if (!ls)
+        {
+          clib_warning("locator-set with index %d doesn't exists", p[0]);
+          return -1;
+        }
+//      /* XXX what happens when a mapping is configured to use the loc-set ? */
+//      if (vec_len (vec_elt_at_index(lcm->locator_set_to_eids, p[0])) != 0)
+//        {
+//          clib_warning ("Can't delete a locator that supports a mapping!");
+//          return -1;
+//        }
+
+      /* clean locator to locator-sets data */
+      clean_locator_to_locator_set (lcm, p[0]);
+
+      if (ls->local)
+        {
+          u32 it, lsi;
+
+          vec_foreach_index(it, lcm->local_locator_set_indexes)
+            {
+              lsi = vec_elt(lcm->local_locator_set_indexes, it);
+              if (lsi == p[0])
+                {
+                  vec_del1(lcm->local_locator_set_indexes, it);
+                  break;
+                }
+            }
+          hash_unset_mem(lcm->locator_set_index_by_name, ls->name);
+          vec_free(ls->name);
+        }
+      pool_put(lcm->locator_set_pool, ls);
+    }
+  return 0;
+}
+
+static clib_error_t *
+lisp_add_del_locator_set_command_fn (vlib_main_t * vm, unformat_input_t * input,
+                                     vlib_cli_command_t * cmd)
+{
+  lisp_gpe_main_t * lgm = &lisp_gpe_main;
+  vnet_main_t * vnm = lgm->vnet_main;
+  unformat_input_t _line_input, * line_input = &_line_input;
+  u8 is_add = 1;
+  clib_error_t * error = 0;
+  u8 * locator_set_name = 0;
+  locator_t locator, * locators = 0;
+  vnet_lisp_add_del_locator_set_args_t _a, * a = &_a;
+  u32 ls_index = 0;
+
+  memset(&locator, 0, sizeof(locator));
+  memset(a, 0, sizeof(a[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, "add %_%v%_", &locator_set_name))
+        is_add = 1;
+      else if (unformat (line_input, "del %_%v%_", &locator_set_name))
+        is_add = 0;
+      else if (unformat (line_input, "iface %U p %d w %d",
+                         unformat_vnet_sw_interface, vnm, &locator.sw_if_index,
+                         &locator.priority, &locator.weight))
+        {
+          locator.local = 1;
+          vec_add1(locators, locator);
+        }
+      else
+        {
+          error = unformat_parse_error(line_input);
+          goto done;
+        }
+    }
+
+  a->name = locator_set_name;
+  a->locators = locators;
+  a->is_add = is_add;
+  a->local = 1;
+
+  vnet_lisp_add_del_locator_set(a, &ls_index);
+
+ done:
+  vec_free(locators);
+  vec_free(locator_set_name);
+  return error;
+}
+
+VLIB_CLI_COMMAND (lisp_cp_add_del_locator_set_command) = {
+    .path = "lisp locator-set",
+    .short_help = "lisp locator-set add/del <name> <iface-name> <priority> <weight>",
+    .function = lisp_add_del_locator_set_command_fn,
+};
+
+static clib_error_t *
+lisp_cp_show_locator_sets_command_fn (vlib_main_t * vm,
+                                      unformat_input_t * input,
+                                      vlib_cli_command_t * cmd)
+{
+  locator_set_t * lsit;
+  locator_t * loc;
+  u32 * locit;
+  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
+
+  vlib_cli_output (vm, "%=20s%=16s%=16s%=16s", "Locator-set", "Locator",
+                   "Priority", "Weight");
+  pool_foreach (lsit, lcm->locator_set_pool,
+  ({
+    u8 * msg = 0;
+    msg = format (msg, "%-16v", lsit->name);
+    vec_foreach (locit, lsit->locator_indices)
+      {
+        loc = pool_elt_at_index (lcm->locator_pool, locit[0]);
+        if (loc->local)
+          msg = format (msg, "%16d%16d%16d", loc->sw_if_index, loc->priority,
+                        loc->weight);
+        else
+          msg = format (msg, "%16U%16d%16d", format_gid_address, &loc->address,
+                        loc->priority, loc->weight);
+      }
+    vlib_cli_output (vm, "%v", msg);
+    vec_free (msg);
+  }));
+  return 0;
+}
+
+VLIB_CLI_COMMAND (lisp_cp_show_locator_sets_command) = {
+    .path = "show lisp locator-set",
+    .short_help = "Shows locator-sets",
+    .function = lisp_cp_show_locator_sets_command_fn,
+};
+
+int
+vnet_lisp_add_del_map_resolver (vnet_lisp_add_del_map_resolver_args_t * a)
+{
+  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
+  ip_address_t * addr;
+  u32 i;
+
+  if (a->is_add)
+    {
+      vec_foreach(addr, lcm->map_resolvers)
+        {
+          if (!ip_address_cmp (addr, &a->address))
+            {
+              clib_warning("map-resolver %U already exists!", format_ip_address,
+                           &a->address);
+              return -1;
+            }
+        }
+      vec_add1(lcm->map_resolvers, a->address);
+    }
+  else
+    {
+      for (i = 0; i < vec_len(lcm->map_resolvers); i++)
+        {
+          addr = vec_elt_at_index(lcm->map_resolvers, i);
+          if (!ip_address_cmp (addr, &a->address))
+            {
+              vec_delete(lcm->map_resolvers, 1, i);
+              break;
+            }
+        }
+    }
+  return 0;
+}
+
+static clib_error_t *
+lisp_add_del_map_resolver_command_fn (vlib_main_t * vm,
+                                         unformat_input_t * input,
+                                         vlib_cli_command_t * cmd)
+{
+  unformat_input_t _line_input, * line_input = &_line_input;
+  u8 is_add = 1;
+  ip_address_t ip_addr;
+  clib_error_t * error = 0;
+  vnet_lisp_add_del_map_resolver_args_t _a, * a = &_a;
+
+  /* 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, "add"))
+        is_add = 1;
+      else if (unformat (line_input, "del"))
+        is_add = 0;
+      else if (unformat (line_input, "%U", unformat_ip_address, &ip_addr))
+        ;
+      else
+        {
+          error = unformat_parse_error(line_input);
+          goto done;
+        }
+    }
+  a->is_add = is_add;
+  a->address = ip_addr;
+  vnet_lisp_add_del_map_resolver (a);
+
+ done:
+  return error;
+}
+
+VLIB_CLI_COMMAND (lisp_add_del_map_resolver_command) = {
+    .path = "lisp map-resolver",
+    .short_help = "lisp map-resolver add/del <ip_address>",
+    .function = lisp_add_del_map_resolver_command_fn,
+};
+
+/* Statistics (not really errors) */
+#define foreach_lisp_cp_lookup_error           \
+_(DROP, "drop")                                \
+_(MAP_REQUESTS_SENT, "map-request sent")
+
+static char * lisp_cp_lookup_error_strings[] = {
+#define _(sym,string) string,
+  foreach_lisp_cp_lookup_error
+#undef _
+};
+
+typedef enum
+{
+#define _(sym,str) LISP_CP_LOOKUP_ERROR_##sym,
+    foreach_lisp_cp_lookup_error
+#undef _
+    LISP_CP_LOOKUP_N_ERROR,
+} lisp_cp_lookup_error_t;
+
+typedef enum
+{
+  LISP_CP_LOOKUP_NEXT_DROP,
+  LISP_CP_LOOKUP_NEXT_IP4_LOOKUP,
+  LISP_CP_LOOKUP_NEXT_IP6_LOOKUP,
+  LISP_CP_LOOKUP_N_NEXT,
+} lisp_cp_lookup_next_t;
+
+typedef struct
+{
+  gid_address_t dst_eid;
+  ip4_address_t map_resolver_ip;
+} lisp_cp_lookup_trace_t;
+
+u8 *
+format_lisp_cp_lookup_trace (u8 * s, va_list * args)
+{
+  CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
+  CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
+  lisp_cp_lookup_trace_t * t = va_arg (*args, lisp_cp_lookup_trace_t *);
+
+  s = format (s, "LISP-CP-LOOKUP: map-resolver: %U destination eid %U",
+              format_ip4_address, &t->map_resolver_ip, format_gid_address,
+              &t->dst_eid);
+  return s;
+}
+
+static u32
+ip_fib_lookup_with_table (lisp_cp_main_t * lcm, u32 fib_index,
+                          ip_address_t * dst)
+{
+  if (ip_addr_version (dst) == IP4)
+      return ip4_fib_lookup_with_table (lcm->im4, fib_index, &ip_addr_v4(dst),
+                                        0);
+  else
+      return ip6_fib_lookup_with_table (lcm->im6, fib_index, &ip_addr_v6(dst));
+}
+
+void
+get_local_iface_ip_for_dst (lisp_cp_main_t *lcm, ip_address_t * dst,
+                            ip_address_t * sloc)
+{
+  u32 adj_index;
+  ip_adjacency_t * adj;
+  ip_interface_address_t * ia = 0;
+  ip_lookup_main_t * lm = &lcm->im4->lookup_main;
+  ip4_address_t * l4 = 0;
+  ip6_address_t * l6 = 0;
+
+  adj_index = ip_fib_lookup_with_table (lcm, 0, dst);
+  adj = ip_get_adjacency (lm, adj_index);
+
+  if (adj->lookup_next_index == IP_LOOKUP_NEXT_ARP)
+    {
+      ia = pool_elt_at_index(lm->if_address_pool, adj->if_address_index);
+      if (ip_addr_version(dst) == IP4)
+        {
+          l4 = ip_interface_address_get_address (lm, ia);
+        }
+      else
+        {
+          l6 = ip_interface_address_get_address (lm, ia);
+        }
+    }
+  else if (adj->lookup_next_index == IP_LOOKUP_NEXT_REWRITE)
+    {
+      /* find sw_if_index in rewrite header */
+      u32 sw_if_index = adj->rewrite_header.sw_if_index;
+
+      /* find suitable address */
+      if (ip_addr_version(dst) == IP4)
+        {
+          /* find the first ip address */
+          foreach_ip_interface_address (&lcm->im4->lookup_main, ia,
+                                        sw_if_index, 1 /* unnumbered */,
+          ({
+            l4 = ip_interface_address_get_address (&lcm->im4->lookup_main, ia);
+            break;
+          }));
+        }
+      else
+        {
+          /* find the first ip address */
+          foreach_ip_interface_address (&lcm->im6->lookup_main, ia,
+                                        sw_if_index, 1 /* unnumbered */,
+          ({
+            l6 = ip_interface_address_get_address (&lcm->im6->lookup_main, ia);
+            break;
+          }));
+        }
+    }
+  else
+    {
+      clib_warning("Can't find local local interface ip for dst %U",
+                   format_ip_address, dst);
+      return;
+    }
+
+  if (l4)
+    {
+      ip_addr_v4(sloc).as_u32 = l4->as_u32;
+      ip_addr_version(sloc) = IP4;
+    }
+  else if (l6)
+    {
+      memcpy (&ip_addr_v6(sloc), l6, sizeof(*l6));
+      ip_addr_version(sloc) = IP6;
+    }
+  else
+    {
+      clib_warning("Can't find local interface addr for dst %U",
+                   format_ip_address, dst);
+    }
+}
+
+static vlib_buffer_t *
+build_encapsulated_map_request (vlib_main_t * vm, lisp_cp_main_t *lcm,
+                                gid_address_t * seid, gid_address_t * deid,
+                                locator_set_t * loc_set, u8 is_smr_invoked,
+                                u64 *nonce_res, u32 * bi_res)
+{
+  vlib_buffer_t * b;
+  u32 bi;
+  ip_address_t * mr_ip, sloc;
+
+  if (vlib_buffer_alloc (vm, &bi, 1) != 1)
+    {
+      clib_warning ("Can't allocate buffer for Map-Request!");
+      return 0;
+    }
+
+  b = vlib_get_buffer (vm, bi);
+
+  /* leave some space for the encap headers */
+  vlib_buffer_make_headroom (b, MAX_LISP_MSG_ENCAP_LEN);
+
+  /* put lisp msg */
+  lisp_msg_put_mreq (lcm, b, seid, deid, loc_set, is_smr_invoked, nonce_res);
+
+  /* push ecm: udp-ip-lisp */
+  lisp_msg_push_ecm (vm, b, LISP_CONTROL_PORT, LISP_CONTROL_PORT, seid, deid);
+
+  /* get map-resolver ip XXX use first*/
+  mr_ip = vec_elt_at_index(lcm->map_resolvers, 0);
+
+  /* get local iface ip to use in map-request XXX fib 0 for now*/
+  get_local_iface_ip_for_dst (lcm, mr_ip, &sloc);
+
+  /* push outer ip header */
+  pkt_push_udp_and_ip (vm, b, LISP_CONTROL_PORT, LISP_CONTROL_PORT, &sloc,
+                       mr_ip);
+
+  bi_res[0] = bi;
+  return b;
+}
+
+static void
+send_encapsulated_map_request (vlib_main_t * vm, lisp_cp_main_t *lcm,
+                               gid_address_t * seid, gid_address_t * deid,
+                               u8 is_smr_invoked)
+{
+  u32 next_index, bi = 0, * to_next, map_index;
+  vlib_buffer_t * b;
+  vlib_frame_t * f;
+  u64 nonce = 0;
+  locator_set_t * loc_set;
+  mapping_t * map;
+  pending_map_request_t * pmr;
+
+  /* get locator-set for seid */
+  map_index = gid_dictionary_lookup (&lcm->mapping_index_by_gid, seid);
+  if (map_index == ~0)
+    {
+      clib_warning("No local mapping found in eid-table for %U!",
+                   format_gid_address, seid);
+      return;
+    }
+
+  map = pool_elt_at_index (lcm->mapping_pool, map_index);
+
+  if (!map->local)
+    {
+      clib_warning("Mapping found for src eid %U is not marked as local!",
+                   format_gid_address, seid);
+      return;
+    }
+  loc_set = pool_elt_at_index (lcm->locator_set_pool, map->locator_set_index);
+
+  /* build the encapsulated map request */
+  b = build_encapsulated_map_request (vm, lcm, seid, deid, loc_set,
+                                      is_smr_invoked, &nonce, &bi);
+
+  if (!b)
+    return;
+
+  vnet_buffer(b)->sw_if_index[VLIB_TX] = ~0;
+  next_index = (ip_prefix_version(&gid_address_ippref(seid)) == IP4) ?
+      ip4_lookup_node.index : ip6_lookup_node.index;
+
+  f = vlib_get_frame_to_node (vm, next_index);
+
+  /* Enqueue the packet */
+  to_next = vlib_frame_vector_args (f);
+  to_next[0] = bi;
+  f->n_vectors = 1;
+  vlib_put_frame_to_node (vm, next_index, f);
+
+  /* add map-request to pending requests table */
+  pool_get(lcm->pending_map_requests_pool, pmr);
+  gid_address_copy (&pmr->src, seid);
+  gid_address_copy (&pmr->dst, deid);
+  pmr->src_mapping_index = map_index;
+  hash_set(lcm->pending_map_requests_by_nonce, nonce,
+           pmr - lcm->pending_map_requests_pool);
+}
+
+static void
+get_src_and_dst (void *hdr, ip_address_t * src, ip_address_t *dst)
+{
+  ip4_header_t * ip4 = hdr;
+  ip6_header_t * ip6;
+
+  if ((ip4->ip_version_and_header_length & 0xF0) == 0x40)
+    {
+      ip_addr_v4(src).as_u32 = ip4->src_address.as_u32;
+      ip_addr_version(src) = IP4;
+      ip_addr_v4(dst).as_u32 = ip4->dst_address.as_u32;
+      ip_addr_version(dst) = IP4;
+    }
+  else
+    {
+      ip6 = hdr;
+      memcpy (&ip_addr_v6(src), &ip6->src_address, sizeof(ip6->src_address));
+      ip_addr_version(src) = IP6;
+      memcpy (&ip_addr_v6(dst), &ip6->dst_address, sizeof(ip6->dst_address));
+      ip_addr_version(dst) = IP6;
+    }
+}
+
+static uword
+lisp_cp_lookup (vlib_main_t * vm, vlib_node_runtime_t * node,
+              vlib_frame_t * from_frame)
+{
+  u32 * from, * to_next_drop;
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main();
+  u32 pkts_mapped = 0;
+  uword n_left_from, n_left_to_next_drop;
+
+  from = vlib_frame_vector_args (from_frame);
+  n_left_from = from_frame->n_vectors;
+
+  while (n_left_from > 0)
+    {
+      vlib_get_next_frame (vm, node, LISP_CP_LOOKUP_NEXT_DROP,
+                           to_next_drop, n_left_to_next_drop);
+
+      while (n_left_from > 0 && n_left_to_next_drop > 0)
+        {
+          u32 pi0;
+          vlib_buffer_t * p0;
+          ip4_header_t * ip0;
+          gid_address_t src, dst;
+          ip_prefix_t * spref, * dpref;
+
+          gid_address_type (&src) = IP_PREFIX;
+          spref = &gid_address_ippref(&src);
+          gid_address_type (&dst) = IP_PREFIX;
+          dpref = &gid_address_ippref(&dst);
+
+          pi0 = from[0];
+          from += 1;
+          n_left_from -= 1;
+          to_next_drop[0] = pi0;
+          to_next_drop += 1;
+          n_left_to_next_drop -= 1;
+
+          p0 = vlib_get_buffer (vm, pi0);
+          p0->error = node->errors[LISP_CP_LOOKUP_ERROR_DROP];
+
+          /* src/dst eid pair */
+          ip0 = vlib_buffer_get_current (p0);
+          get_src_and_dst (ip0, &ip_prefix_addr(spref), &ip_prefix_addr(dpref));
+          ip_prefix_len(spref) = ip_address_max_len (ip_prefix_version(spref));
+          ip_prefix_len(dpref) = ip_address_max_len (ip_prefix_version(dpref));
+
+          /* send map-request */
+          send_encapsulated_map_request (vm, lcm, &src, &dst, 0);
+
+          pkts_mapped++;
+
+          if (PREDICT_FALSE(p0->flags & VLIB_BUFFER_IS_TRACED))
+            {
+              lisp_cp_lookup_trace_t *tr = vlib_add_trace (vm, node, p0,
+                                                          sizeof(*tr));
+              gid_address_copy (&tr->dst_eid, &dst);
+              memcpy (&tr->map_resolver_ip,
+                      vec_elt_at_index(lcm->map_resolvers, 0),
+                      sizeof(ip_address_t));
+            }
+        }
+
+      vlib_put_next_frame (vm, node, LISP_CP_LOOKUP_NEXT_DROP, n_left_to_next_drop);
+    }
+  vlib_node_increment_counter (vm, node->node_index,
+                               LISP_CP_LOOKUP_ERROR_MAP_REQUESTS_SENT,
+                               pkts_mapped);
+  return from_frame->n_vectors;
+}
+
+VLIB_REGISTER_NODE (lisp_cp_lookup_node) = {
+  .function = lisp_cp_lookup,
+  .name = "lisp-cp-lookup",
+  .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",
+      [LISP_CP_LOOKUP_NEXT_IP4_LOOKUP] = "ip4-lookup",
+      [LISP_CP_LOOKUP_NEXT_IP6_LOOKUP] = "ip6-lookup",
+  },
+};
+
+/* lisp_cp_input statistics */
+#define foreach_lisp_cp_input_error                   \
+_(DROP, "drop")                                        \
+_(MAP_REPLIES_RECEIVED, "map-replies received")
+
+static char * lisp_cp_input_error_strings[] = {
+#define _(sym,string) string,
+  foreach_lisp_cp_input_error
+#undef _
+};
+
+typedef enum
+{
+#define _(sym,str) LISP_CP_INPUT_ERROR_##sym,
+    foreach_lisp_cp_input_error
+#undef _
+    LISP_CP_INPUT_N_ERROR,
+} lisp_cp_input_error_t;
+
+typedef enum
+{
+  LISP_CP_INPUT_NEXT_DROP,
+  LISP_CP_INPUT_N_NEXT,
+} lisp_cp_input_next_t;
+
+typedef struct
+{
+  gid_address_t dst_eid;
+  ip4_address_t map_resolver_ip;
+} lisp_cp_input_trace_t;
+
+u8 *
+format_lisp_cp_input_trace (u8 * s, va_list * args)
+{
+  CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
+  CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
+  CLIB_UNUSED(lisp_cp_input_trace_t * t) = va_arg (*args, lisp_cp_input_trace_t *);
+
+  s = format (s, "LISP-CP-INPUT: TODO");
+  return s;
+}
+
+ip_interface_address_t *
+ip_interface_get_first_interface_address (ip_lookup_main_t *lm, u32 sw_if_index,
+                                          u8 loop)
+{
+  vnet_main_t *vnm = vnet_get_main ();
+  vnet_sw_interface_t * swif = vnet_get_sw_interface (vnm, sw_if_index);
+  if (loop && swif->flags & VNET_SW_INTERFACE_FLAG_UNNUMBERED)
+    sw_if_index = swif->unnumbered_sw_if_index;
+  u32 ia =
+      (vec_len((lm)->if_address_pool_index_by_sw_if_index) > (sw_if_index)) ?
+          vec_elt((lm)->if_address_pool_index_by_sw_if_index, (sw_if_index)) :
+          (u32) ~0;
+  return pool_elt_at_index((lm)->if_address_pool, ia);
+}
+
+void *
+ip_interface_get_first_ip_addres (ip_lookup_main_t *lm, u32 sw_if_index,
+                                   u8 loop)
+{
+  ip_interface_address_t * ia = ip_interface_get_first_interface_address (
+      lm, sw_if_index, loop);
+  return ip_interface_address_get_address (lm, ia);
+}
+
+void
+del_fwd_entry (lisp_cp_main_t * lcm, u32 src_map_index,
+               u32 dst_map_index)
+{
+  vnet_lisp_gpe_add_del_fwd_entry_args_t _a, * a = &_a;
+  fwd_entry_t * fe = 0;
+  uword * feip = 0;
+  memset(a, 0, sizeof(*a));
+
+  feip = hash_get(lcm->fwd_entry_by_mapping_index, dst_map_index);
+  if (!feip)
+    return;
+
+  fe = pool_elt_at_index(lcm->fwd_entry_pool, feip[0]);
+
+  /* delete dp fwd entry */
+  u32 sw_if_index;
+  a->is_add = 0;
+  a->dlocator = fe->dst_loc;
+  a->slocator = fe->src_loc;
+  a->iid = 0; // XXX should be part of mapping/eid
+  gid_address_copy(&a->deid, &fe->deid);
+
+  vnet_lisp_gpe_add_del_fwd_entry (a, &sw_if_index);
+
+  /* delete entry in fwd table */
+  hash_unset(lcm->fwd_entry_by_mapping_index, dst_map_index);
+  pool_put(lcm->fwd_entry_pool, fe);
+}
+
+void
+add_fwd_entry (lisp_cp_main_t* lcm, u32 src_map_index, u32 dst_map_index)
+{
+  mapping_t * src_map, * dst_map;
+  locator_set_t * dst_ls, * src_ls;
+  u32 i, minp = ~0;
+  locator_t * dl = 0;
+  uword * feip = 0;
+  vnet_lisp_gpe_add_del_fwd_entry_args_t _a, * a = &_a;
+  memset (a, 0, sizeof(*a));
+
+  /* remove entry if it already exists */
+  feip = hash_get (lcm->fwd_entry_by_mapping_index, dst_map_index);
+  if (feip)
+    del_fwd_entry (lcm, src_map_index, dst_map_index);
+
+  src_map = pool_elt_at_index (lcm->mapping_pool, src_map_index);
+  dst_map = pool_elt_at_index (lcm->mapping_pool, dst_map_index);
+
+  /* XXX simple forwarding policy: first lowest (value) priority locator */
+  dst_ls = pool_elt_at_index (lcm->locator_set_pool,
+                              dst_map->locator_set_index);
+  for (i = 0; i < vec_len (dst_ls->locator_indices); i++)
+    {
+      u32 li = vec_elt (dst_ls->locator_indices, i);
+      locator_t * l = pool_elt_at_index (lcm->locator_pool, li);
+      if (l->priority < minp && gid_address_type(&l->address) == IP_PREFIX)
+        {
+          minp = l->priority;
+          dl = l;
+        }
+    }
+  if (dl)
+    {
+      src_ls = pool_elt_at_index (lcm->locator_set_pool,
+                                  src_map->locator_set_index);
+      for (i = 0; i < vec_len (src_ls->locator_indices); i++)
+        {
+          u32 li = vec_elt (src_ls->locator_indices, i);
+          locator_t * sl = pool_elt_at_index (lcm->locator_pool, li);
+
+          if (ip_addr_version(&gid_address_ip(&dl->address)) == IP4)
+            {
+              ip4_address_t * l4;
+              l4 = ip_interface_get_first_ip_addres (&lcm->im4->lookup_main,
+                                                     sl->sw_if_index,
+                                                     1 /* unnumbered */);
+              ip_addr_v4(&a->slocator) = *l4;
+              ip_addr_version(&a->slocator) = IP4;
+            }
+          else
+            {
+              ip6_address_t * l6;
+              l6 = ip_interface_get_first_ip_addres (&lcm->im6->lookup_main,
+                                                     sl->sw_if_index,
+                                                     1 /* unnumbered */);
+              ip_addr_v6(&a->slocator) = *l6;
+              ip_addr_version(&a->slocator) = IP6;
+            }
+        }
+    }
+  /* insert data plane forwarding entry */
+  u32 sw_if_index;
+  a->is_add = 1;
+  if (dl)
+    a->dlocator = gid_address_ip(&dl->address);
+  else
+    {
+      a->is_negative = 1;
+      a->action = dst_map->action;
+    }
+
+  gid_address_copy (&a->deid, &dst_map->eid);
+  a->iid = 0; // XXX should be part of mapping/eid
+  u8 ipver = ip_prefix_version(&gid_address_ippref(&a->deid));
+  a->decap_next_index = (ipver == IP4) ?
+          LISP_GPE_INPUT_NEXT_IP4_INPUT : LISP_GPE_INPUT_NEXT_IP6_INPUT;
+  /* XXX tunnels work only with IP4 now */
+  vnet_lisp_gpe_add_del_fwd_entry (a, &sw_if_index);
+
+  /* add tunnel to fwd entry table XXX check return value from DP insertion */
+  fwd_entry_t* fe;
+  pool_get (lcm->fwd_entry_pool, fe);
+  fe->dst_loc = a->dlocator;
+  fe->src_loc = a->slocator;
+  gid_address_copy (&fe->deid, &a->deid);
+  hash_set (lcm->fwd_entry_by_mapping_index, dst_map_index,
+            fe - lcm->fwd_entry_pool);
+}
+
+/* return 0 if the two locator sets are identical 1 otherwise */
+static u8
+compare_locators (lisp_cp_main_t *lcm, u32 * old_ls_indexes,
+                  locator_t * new_locators)
+{
+  u32 i, old_li;
+  locator_t * old_loc, * new_loc;
+
+  if (vec_len (old_ls_indexes) != vec_len(new_locators))
+    return 1;
+
+  for (i = 0; i < vec_len(new_locators); i++)
+    {
+      old_li = vec_elt(old_ls_indexes, i);
+      old_loc = pool_elt_at_index(lcm->locator_pool, old_li);
+
+      new_loc = vec_elt_at_index(new_locators, i);
+
+      if (locator_cmp (old_loc, new_loc))
+        return 1;
+    }
+  return 0;
+}
+
+void
+process_map_reply (lisp_cp_main_t * lcm, vlib_buffer_t * b)
+{
+  u32 len = 0, i, ls_index = 0;
+  void * h;
+  vnet_lisp_add_del_locator_set_args_t _ls_arg, * ls_arg = &_ls_arg;
+  vnet_lisp_add_del_mapping_args_t _m_args, * m_args = &_m_args;
+  pending_map_request_t * pmr;
+  locator_t probed;
+  map_reply_hdr_t * mrep_hdr;
+  u64 nonce;
+  u32 dst_map_index, mi;
+  uword * pmr_index;
+
+  mrep_hdr = vlib_buffer_get_current (b);
+
+  /* Check pending requests table and nonce */
+  nonce = MREP_NONCE(mrep_hdr);
+  pmr_index = hash_get(lcm->pending_map_requests_by_nonce, nonce);
+  if (!pmr_index)
+    {
+      clib_warning("No pending map-request entry with nonce %lu!", nonce);
+      return;
+    }
+  pmr = pool_elt_at_index(lcm->pending_map_requests_pool, pmr_index[0]);
+
+  vlib_buffer_pull (b, sizeof(*mrep_hdr));
+
+  for (i = 0; i < MREP_REC_COUNT(mrep_hdr); i++)
+    {
+      memset (ls_arg, 0, sizeof(*ls_arg));
+      memset (m_args, 0, sizeof(*m_args));
+
+      h = vlib_buffer_get_current (b);
+      m_args->ttl = clib_net_to_host_u32 (MAP_REC_TTL(h));
+      m_args->action = MAP_REC_ACTION(h);
+      m_args->authoritative = MAP_REC_AUTH(h);
+
+      len = lisp_msg_parse_mapping_record (b, &m_args->deid, &ls_arg->locators,
+                                           &probed);
+      if (len == ~0)
+        {
+          clib_warning ("Failed to parse mapping record!");
+          vec_free(ls_arg->locators);
+          return;
+        }
+
+      mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &m_args->deid);
+
+      /* if mapping already exists, decide if locators (and forwarding) should
+       * be updated and be done */
+      if (mi != ~0)
+        {
+          mapping_t * old_map;
+          locator_set_t * old_ls;
+          old_map = pool_elt_at_index(lcm->mapping_pool, mi);
+
+          /* update mapping attributes */
+          old_map->action = m_args->action;
+          old_map->authoritative = m_args->authoritative;
+          old_map->ttl = m_args->ttl;
+
+          old_ls = pool_elt_at_index(lcm->locator_set_pool,
+                                     old_map->locator_set_index);
+          /* if the two locators are not equal, update them and forwarding
+           * otherwise there's nothing to be done */
+          if (compare_locators (lcm, old_ls->locator_indices, ls_arg->locators))
+            {
+              /* set locator-set index to overwrite */
+              ls_arg->is_add = 1;
+              ls_arg->index = old_map->locator_set_index;
+              vnet_lisp_add_del_locator_set (ls_arg, 0);
+              add_fwd_entry (lcm, pmr->src_mapping_index, mi);
+            }
+        }
+      /* new mapping */
+      else
+        {
+          /* add locator-set */
+          ls_arg->is_add = 1;
+          ls_arg->index = ~0;
+          vnet_lisp_add_del_locator_set (ls_arg, &ls_index);
+
+          /* add mapping */
+          m_args->is_add = 1;
+          m_args->locator_set_index = ls_index;
+          vnet_lisp_add_del_mapping (m_args, &dst_map_index);
+
+          /* add forwarding tunnel */
+          add_fwd_entry (lcm, pmr->src_mapping_index, dst_map_index);
+        }
+      vec_free(ls_arg->locators);
+    }
+
+  /* remove pending map request entry */
+  hash_unset(lcm->pending_map_requests_by_nonce, nonce);
+  pool_put(lcm->pending_map_requests_pool, pmr);
+}
+
+void
+process_map_request (vlib_main_t * vm, lisp_cp_main_t * lcm, vlib_buffer_t * b)
+{
+  map_request_hdr_t * mreq_hdr;
+  gid_address_t src, dst;
+//  u64 nonce;
+  u32 i, len = 0;
+  gid_address_t * itr_rlocs = 0;
+
+  mreq_hdr = vlib_buffer_get_current (b);
+  vlib_buffer_pull (b, sizeof(*mreq_hdr));
+
+//  nonce = MREQ_NONCE(mreq_hdr);
+
+  if (!MREQ_SMR(mreq_hdr)) {
+      clib_warning("Only SMR Map-Requests supported for now!");
+      return;
+  }
+
+  /* parse src eid */
+  len = lisp_msg_parse_addr (b, &src);
+  if (len == ~0)
+    return;
+
+  /* for now we don't do anything with the itr's rlocs */
+  len = lisp_msg_parse_itr_rlocs (b, &itr_rlocs, MREQ_ITR_RLOC_COUNT(mreq_hdr) + 1);
+  if (len == ~0)
+    return;
+
+  /* parse eid records and send SMR-invoked map-requests */
+  for (i = 0; i < MREQ_REC_COUNT(mreq_hdr); i++)
+    {
+      memset(&dst, 0, sizeof(dst));
+      len = lisp_msg_parse_eid_rec (b, &dst);
+      if (len == ~0)
+        {
+          clib_warning("Can't parse map-request EID-record");
+          return;
+        }
+      /* send SMR-invoked map-requests */
+      send_encapsulated_map_request (vm, lcm, &dst, &src, /* invoked */ 1);
+    }
+}
+
+static uword
+lisp_cp_input (vlib_main_t * vm, vlib_node_runtime_t * node,
+               vlib_frame_t * from_frame)
+{
+  u32 n_left_from, * from, * to_next_drop;
+  lisp_msg_type_e type;
+  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
+
+  from = vlib_frame_vector_args (from_frame);
+  n_left_from = from_frame->n_vectors;
+
+
+  while (n_left_from > 0)
+    {
+      u32 n_left_to_next_drop;
+
+      vlib_get_next_frame (vm, node, LISP_CP_INPUT_NEXT_DROP,
+                           to_next_drop, n_left_to_next_drop);
+      while (n_left_from > 0 && n_left_to_next_drop > 0)
+        {
+          u32 bi0;
+          vlib_buffer_t * b0;
+
+          bi0 = from[0];
+          from += 1;
+          n_left_from -= 1;
+          to_next_drop[0] = bi0;
+          to_next_drop += 1;
+          n_left_to_next_drop -= 1;
+
+          b0 = vlib_get_buffer (vm, bi0);
+
+          type = lisp_msg_type(vlib_buffer_get_current (b0));
+          switch (type)
+            {
+            case LISP_MAP_REPLY:
+              process_map_reply (lcm, b0);
+              break;
+            case LISP_MAP_REQUEST:
+              process_map_request(vm, lcm, b0);
+              break;
+            default:
+              clib_warning("Unsupported LISP message type %d", type);
+              break;
+            }
+
+          b0->error = node->errors[LISP_CP_INPUT_ERROR_DROP];
+
+          if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED))
+            {
+
+            }
+        }
+
+      vlib_put_next_frame (vm, node, LISP_CP_INPUT_NEXT_DROP, n_left_to_next_drop);
+    }
+  return from_frame->n_vectors;
+}
+
+VLIB_REGISTER_NODE (lisp_cp_input_node) = {
+  .function = lisp_cp_input,
+  .name = "lisp-cp-input",
+  .vector_size = sizeof (u32),
+  .format_trace = format_lisp_cp_input_trace,
+  .type = VLIB_NODE_TYPE_INTERNAL,
+
+  .n_errors = LISP_CP_INPUT_N_ERROR,
+  .error_strings = lisp_cp_input_error_strings,
+
+  .n_next_nodes = LISP_CP_INPUT_N_NEXT,
+
+  .next_nodes = {
+      [LISP_CP_INPUT_NEXT_DROP] = "error-drop",
+  },
+};
+
+clib_error_t *
+lisp_cp_init (vlib_main_t *vm)
+{
+  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
+  clib_error_t * error = 0;
+
+  if ((error = vlib_call_init_function (vm, lisp_gpe_init)))
+    return error;
+
+  lcm->im4 = &ip4_main;
+  lcm->im6 = &ip6_main;
+  lcm->vlib_main = vm;
+  lcm->vnet_main = vnet_get_main();
+
+  gid_dictionary_init (&lcm->mapping_index_by_gid);
+  gid_dictionary_init (&lcm->mapping_index_by_gid);
+
+  udp_register_dst_port (vm, UDP_DST_PORT_lisp_cp,
+                         lisp_cp_input_node.index, 1 /* is_ip4 */);
+  udp_register_dst_port (vm, UDP_DST_PORT_lisp_cp6,
+                         lisp_cp_input_node.index, 0 /* is_ip4 */);
+
+  return 0;
+}
+
+VLIB_INIT_FUNCTION(lisp_cp_init);
diff --git a/vnet/vnet/lisp-cp/control.h b/vnet/vnet/lisp-cp/control.h
new file mode 100644
index 00000000000..713dce6a5b8
--- /dev/null
+++ b/vnet/vnet/lisp-cp/control.h
@@ -0,0 +1,157 @@
+/*
+ * 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.
+ */
+
+#ifndef VNET_CONTROL_H_
+#define VNET_CONTROL_H_
+
+#include <vnet/vnet.h>
+#include <vnet/lisp-cp/gid_dictionary.h>
+#include <vnet/lisp-cp/lisp_types.h>
+
+typedef struct
+{
+  gid_address_t src;
+  gid_address_t dst;
+  u32 src_mapping_index;
+} pending_map_request_t;
+
+typedef struct
+{
+  gid_address_t seid;
+  gid_address_t deid;
+  ip_address_t src_loc;
+  ip_address_t dst_loc;
+} fwd_entry_t;
+
+typedef enum
+{
+  IP4_MISS_PACKET,
+  IP6_MISS_PACKET
+} miss_packet_type_t;
+
+typedef struct
+{
+  /* headers */
+  u8 data[100];
+  u32 length;
+  miss_packet_type_t type;
+} miss_packet_t;
+
+typedef struct
+{
+  /* eid table */
+  gid_dictionary_t mapping_index_by_gid;
+
+  /* pool of mappings */
+  mapping_t * mapping_pool;
+
+  /* pool of locators */
+  locator_t * locator_pool;
+
+  /* pool of locator-sets */
+  locator_set_t * locator_set_pool;
+
+  /* vector of locator-set vectors composed of and indexed by locator index */
+  u32 ** locator_to_locator_sets;
+
+  /* hash map of locators by name */
+  uword * locator_set_index_by_name;
+
+  /* vector of eid index vectors supported and indexed by locator-set index */
+  u32 ** locator_set_to_eids;
+
+  /* vectors of indexes for local locator-sets and mappings */
+  u32 * local_mappings_indexes;
+  u32 * local_locator_set_indexes;
+
+  /* hash map of forwarding entries by mapping index */
+  u32 * fwd_entry_by_mapping_index;
+
+  /* forwarding entries pool */
+  fwd_entry_t * fwd_entry_pool;
+
+  /* hash map keyed by nonce of pending map-requests */
+  uword * pending_map_requests_by_nonce;
+
+  /* pool of pending map requests */
+  pending_map_request_t * pending_map_requests_pool;
+
+  /* vector of map-resolver addresses */
+  ip_address_t * map_resolvers;
+
+  /* commodity */
+  ip4_main_t * im4;
+  ip6_main_t * im6;
+  vlib_main_t * vlib_main;
+  vnet_main_t * vnet_main;
+} lisp_cp_main_t;
+
+/* lisp-gpe control plane */
+lisp_cp_main_t lisp_control_main;
+
+extern vlib_node_registration_t lisp_cp_input_node;
+extern vlib_node_registration_t lisp_cp_lookup_node;
+
+clib_error_t *
+lisp_cp_init ();
+
+typedef struct
+{
+  u8 is_add;
+  union
+  {
+    u8 * name;
+    u32 index;
+  };
+  locator_t * locators;
+  u8 local;
+} vnet_lisp_add_del_locator_set_args_t;
+
+int
+vnet_lisp_add_del_locator_set (vnet_lisp_add_del_locator_set_args_t * a,
+			       u32 * ls_index);
+
+typedef struct
+{
+  u8 is_add;
+  gid_address_t deid;
+  u32 locator_set_index;
+
+  u32 ttl;
+  u8 action;
+  u8 authoritative;
+
+  u8 local;
+} vnet_lisp_add_del_mapping_args_t;
+
+int
+vnet_lisp_add_del_mapping (vnet_lisp_add_del_mapping_args_t *a,
+			   u32 * map_index);
+
+typedef struct
+{
+  u8 is_add;
+  ip_address_t address;
+} vnet_lisp_add_del_map_resolver_args_t;
+
+int
+vnet_lisp_add_del_map_resolver (vnet_lisp_add_del_map_resolver_args_t * a);
+
+always_inline lisp_cp_main_t *
+vnet_lisp_cp_get_main() {
+  return &lisp_control_main;
+}
+
+#endif /* VNET_CONTROL_H_ */
diff --git a/vnet/vnet/lisp-cp/gid_dictionary.c b/vnet/vnet/lisp-cp/gid_dictionary.c
new file mode 100644
index 00000000000..dd10856c5ec
--- /dev/null
+++ b/vnet/vnet/lisp-cp/gid_dictionary.c
@@ -0,0 +1,347 @@
+/*
+ * 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.
+ */
+
+#include <vnet/lisp-cp/gid_dictionary.h>
+
+static u32
+ip4_lookup (gid_dictionary_t * db, u32 vni, ip_prefix_t *key)
+{
+  int i, len;
+  int rv;
+  BVT(clib_bihash_kv) kv, value;
+
+  len = vec_len (db->ip4_prefix_lengths_in_search_order);
+
+  for (i = 0; i < len; i++)
+    {
+      int dst_address_length = db->ip4_prefix_lengths_in_search_order[i];
+      ip4_address_t * mask;
+
+      ASSERT(dst_address_length >= 0 && dst_address_length <= 32);
+
+      mask = &db->ip4_fib_masks[dst_address_length];
+
+      kv.key[0] = ((u64) vni << 32) | (ip_prefix_v4(key).as_u32 & mask->as_u32);
+      kv.key[1] = 0;
+      kv.key[2] = 0;
+
+      rv = BV(clib_bihash_search_inline_2)(&db->ip4_lookup_table, &kv, &value);
+      if (rv == 0)
+        return value.value;
+    }
+
+  return GID_LOOKUP_MISS;
+}
+
+static u32
+ip6_lookup (gid_dictionary_t * db, u32 vni, ip_prefix_t *key)
+{
+  int i, len;
+  int rv;
+  BVT(clib_bihash_kv) kv, value;
+
+  len = vec_len (db->ip6_prefix_lengths_in_search_order);
+
+  for (i = 0; i < len; i++)
+    {
+      int dst_address_length = db->ip6_prefix_lengths_in_search_order[i];
+      ip6_address_t * mask;
+
+      ASSERT(dst_address_length >= 0 && dst_address_length <= 128);
+
+      mask = &db->ip6_fib_masks[dst_address_length];
+
+      kv.key[0] = ip_prefix_v6(key).as_u64[0] & mask->as_u64[0];
+      kv.key[1] = ip_prefix_v6(key).as_u64[1] & mask->as_u64[1];
+      kv.key[2] = (u64)vni;
+
+      rv = BV(clib_bihash_search_inline_2)(&db->ip6_lookup_table, &kv, &value);
+      if (rv == 0)
+        return value.value;
+    }
+
+  return GID_LOOKUP_MISS;
+}
+
+static u32
+ip_lookup (gid_dictionary_t * db, u32 vni, ip_prefix_t *key)
+{
+  /* XXX for now this only works with ip-prefixes, no lcafs */
+  switch (ip_prefix_version (key))
+    {
+    case IP4:
+      return ip4_lookup (db, vni, key);
+      break;
+    case IP6:
+      return ip6_lookup (db, vni, key);
+      break;
+    default:
+      clib_warning ("address type %d not supported!", ip_prefix_version(key));
+      break;
+    }
+  return ~0;
+}
+
+u32
+gid_dictionary_lookup (gid_dictionary_t * db, gid_address_t * key)
+{
+  /* XXX for now this only works with ip-prefixes, no lcafs */
+  switch (gid_address_type (key))
+    {
+    case IP_PREFIX:
+      return ip_lookup (db, 0, &gid_address_ippref(key));
+      break;
+    default:
+      clib_warning ("address type %d not supported!", gid_address_type(key));
+      break;
+    }
+  return ~0;
+}
+
+static void
+ip4_compute_prefix_lengths_in_search_order (gid_dictionary_t * db)
+{
+  int i;
+  vec_reset_length (db->ip4_prefix_lengths_in_search_order);
+  /* Note: bitmap reversed so this is in fact a longest prefix match */
+  clib_bitmap_foreach (i, db->ip4_non_empty_dst_address_length_bitmap,
+  ({
+    int dst_address_length = 32 - i;
+    vec_add1 (db->ip4_prefix_lengths_in_search_order, dst_address_length);
+  }));
+}
+
+static u32
+add_del_ip4_key (gid_dictionary_t *db, u32 vni, ip_prefix_t * pref, u32 val,
+                 u8 is_add)
+{
+  BVT(clib_bihash_kv) kv, value;
+  u32 old_val = ~0;
+  ip4_address_t key;
+  u8 plen = ip_prefix_len (pref);
+
+  memcpy (&key, &ip_prefix_v4(pref), sizeof(key));
+  key.as_u32 &= db->ip4_fib_masks[plen].as_u32;
+  if (is_add)
+    {
+      db->ip4_non_empty_dst_address_length_bitmap = clib_bitmap_set (
+          db->ip4_non_empty_dst_address_length_bitmap, 32 - plen,
+          1);
+      ip4_compute_prefix_lengths_in_search_order (db);
+
+      db->ip4_prefix_len_refcount[plen]++;
+    }
+  else
+    {
+      ASSERT(db->ip4_prefix_len_refcount[plen] != 0);
+
+      db->ip4_prefix_len_refcount[plen]--;
+
+      if (db->ip4_prefix_len_refcount[plen] == 0)
+        {
+            db->ip4_non_empty_dst_address_length_bitmap = clib_bitmap_set (
+                db->ip4_non_empty_dst_address_length_bitmap, 32 - plen,
+                0);
+            ip4_compute_prefix_lengths_in_search_order (db);
+        }
+    }
+
+  kv.key[0] = ((u64) vni << 32) | key.as_u32;
+  kv.key[1] = 0;
+  kv.key[2] = 0;
+
+  if (BV(clib_bihash_search)(&db->ip4_lookup_table, &kv, &value) == 0)
+    old_val = value.value;
+
+  if (!is_add)
+    BV(clib_bihash_add_del) (&db->ip4_lookup_table, &kv, 0 /* is_add */);
+  else
+    {
+      kv.value = val;
+      BV(clib_bihash_add_del) (&db->ip4_lookup_table, &kv, 1 /* is_add */);
+    }
+  return old_val;
+}
+
+static void
+ip6_compute_prefix_lengths_in_search_order (gid_dictionary_t * db)
+{
+  int i;
+  vec_reset_length (db->ip6_prefix_lengths_in_search_order);
+  /* Note: bitmap reversed so this is in fact a longest prefix match */
+  clib_bitmap_foreach (i, db->ip6_non_empty_dst_address_length_bitmap,
+  ({
+    int dst_address_length = 128 - i;
+    vec_add1 (db->ip6_prefix_lengths_in_search_order, dst_address_length);
+  }));
+}
+
+static u32
+add_del_ip6_key (gid_dictionary_t *db, u32 vni, ip_prefix_t *pref, u32 val,
+                 u8 is_add)
+{
+  BVT(clib_bihash_kv) kv, value;
+  u32 old_val = ~0;
+  ip6_address_t key;
+  u8 plen = ip_prefix_len (pref);
+
+  memcpy (&key, &ip_prefix_v6(pref), sizeof(key));
+  ip6_address_mask (&key, &db->ip6_fib_masks[plen]);
+  if (is_add)
+    {
+      db->ip6_non_empty_dst_address_length_bitmap = clib_bitmap_set (
+          db->ip6_non_empty_dst_address_length_bitmap, 128 - plen, 1);
+      ip6_compute_prefix_lengths_in_search_order (db);
+      db->ip6_prefix_len_refcount[plen]++;
+    }
+  else
+    {
+      ASSERT(db->ip6_prefix_len_refcount[plen] != 0);
+
+      db->ip6_prefix_len_refcount[plen]--;
+
+      if (db->ip6_prefix_len_refcount[plen] == 0)
+        {
+          db->ip6_non_empty_dst_address_length_bitmap = clib_bitmap_set (
+              db->ip6_non_empty_dst_address_length_bitmap, 128 - plen, 0);
+          ip6_compute_prefix_lengths_in_search_order (db);
+        }
+    }
+
+  kv.key[0] = key.as_u64[0];
+  kv.key[1] = key.as_u64[1];
+  kv.key[2] = (u64) vni;
+//  kv.key[2] = ((u64)((fib - im->fibs))<<32) | ip_prefix_len(key);
+
+  if (BV(clib_bihash_search)(&db->ip6_lookup_table, &kv, &value) == 0)
+    old_val = value.value;
+
+  if (!is_add)
+    BV(clib_bihash_add_del) (&db->ip6_lookup_table, &kv, 0 /* is_add */);
+  else
+    {
+      kv.value = val;
+      BV(clib_bihash_add_del) (&db->ip6_lookup_table, &kv, 1 /* is_add */);
+    }
+  return old_val;
+}
+
+static u32
+gid_dictionary_add_del_ip (gid_dictionary_t *db, u32 iid, ip_prefix_t *key,
+                           u32 value, u8 is_add)
+{
+  switch (ip_prefix_version (key))
+    {
+    case IP4:
+      return add_del_ip4_key (db, iid, key, value, is_add);
+      break;
+    case IP6:
+      return add_del_ip6_key (db, iid, key, value, is_add);
+      break;
+    default:
+      clib_warning("address type %d not supported!", ip_prefix_version (key));
+      break;
+    }
+  return ~0;
+}
+
+u32
+gid_dictionary_add_del (gid_dictionary_t *db, gid_address_t *key, u32 value,
+                        u8 is_add)
+{
+  /* XXX for now this only works with ip-prefixes, no lcafs */
+  switch (gid_address_type (key))
+    {
+    case IP_PREFIX:
+      return gid_dictionary_add_del_ip (db, 0, &gid_address_ippref(key), value,
+                                        is_add);
+      break;
+    default:
+      clib_warning ("address type %d not supported!", gid_address_type (key));
+      break;
+    }
+  return ~0;
+}
+
+static void
+ip4_lookup_init (gid_dictionary_t * db)
+{
+  uword i;
+
+  memset(db->ip4_prefix_len_refcount, 0, sizeof(db->ip4_prefix_len_refcount));
+
+  for (i = 0; i < ARRAY_LEN (db->ip4_fib_masks); i++)
+    {
+      u32 m;
+
+      if (i < 32)
+        m = pow2_mask (i) << (32 - i);
+      else
+        m = ~0;
+      db->ip4_fib_masks[i].as_u32 = clib_host_to_net_u32 (m);
+    }
+  if (db->ip4_lookup_table_nbuckets == 0)
+    db->ip4_lookup_table_nbuckets = IP4_LOOKUP_DEFAULT_HASH_NUM_BUCKETS;
+
+  db->ip4_lookup_table_nbuckets = 1 << max_log2 (db->ip4_lookup_table_nbuckets);
+
+  if (db->ip4_lookup_table_size == 0)
+    db->ip4_lookup_table_size = IP4_LOOKUP_DEFAULT_HASH_MEMORY_SIZE;
+
+  BV(clib_bihash_init) (&db->ip4_lookup_table, "ip4 lookup table",
+                         db->ip4_lookup_table_nbuckets, db->ip4_lookup_table_size);
+}
+
+static void
+ip6_lookup_init (gid_dictionary_t * db)
+{
+  uword i;
+
+  memset(db->ip6_prefix_len_refcount, 0, sizeof(db->ip6_prefix_len_refcount));
+
+  for (i = 0; i < ARRAY_LEN(db->ip6_fib_masks); i++)
+    {
+      u32 j, i0, i1;
+
+      i0 = i / 32;
+      i1 = i % 32;
+
+      for (j = 0; j < i0; j++)
+        db->ip6_fib_masks[i].as_u32[j] = ~0;
+
+      if (i1)
+        db->ip6_fib_masks[i].as_u32[i0] = clib_host_to_net_u32 (
+            pow2_mask (i1) << (32 - i1));
+    }
+
+  if (db->ip6_lookup_table_nbuckets == 0)
+    db->ip6_lookup_table_nbuckets = IP6_LOOKUP_DEFAULT_HASH_NUM_BUCKETS;
+
+  db->ip6_lookup_table_nbuckets = 1 << max_log2 (db->ip6_lookup_table_nbuckets);
+
+  if (db->ip6_lookup_table_size == 0)
+    db->ip6_lookup_table_size = IP6_LOOKUP_DEFAULT_HASH_MEMORY_SIZE;
+
+  BV(clib_bihash_init) (&db->ip6_lookup_table, "ip6 lookup table",
+                         db->ip6_lookup_table_nbuckets, db->ip6_lookup_table_size);
+}
+
+void
+gid_dictionary_init (gid_dictionary_t * db)
+{
+  ip4_lookup_init (db);
+  ip6_lookup_init (db);
+}
+
diff --git a/vnet/vnet/lisp-cp/gid_dictionary.h b/vnet/vnet/lisp-cp/gid_dictionary.h
new file mode 100644
index 00000000000..5b1a59b0161
--- /dev/null
+++ b/vnet/vnet/lisp-cp/gid_dictionary.h
@@ -0,0 +1,72 @@
+/*
+ * 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.
+ */
+
+#ifndef VNET_LISP_GPE_GID_DICTIONARY_H_
+#define VNET_LISP_GPE_GID_DICTIONARY_H_
+
+#include <vnet/vnet.h>
+#include <vnet/lisp-cp/lisp_types.h>
+#include <vppinfra/bihash_24_8.h>
+#include <vppinfra/bihash_template.h>
+
+#define GID_LOOKUP_MISS ((u32)~0)
+
+/* Default size of the ip4 hash table */
+#define IP4_LOOKUP_DEFAULT_HASH_NUM_BUCKETS (64 * 1024)
+#define IP4_LOOKUP_DEFAULT_HASH_MEMORY_SIZE (32<<20)
+
+/* Default size of the ip6 hash table */
+#define IP6_LOOKUP_DEFAULT_HASH_NUM_BUCKETS (64 * 1024)
+#define IP6_LOOKUP_DEFAULT_HASH_MEMORY_SIZE (32<<20)
+
+typedef struct
+{
+  BVT(clib_bihash) ip4_lookup_table;
+
+  /* bitmap/vector of mask widths to search */
+  uword * ip4_non_empty_dst_address_length_bitmap;
+  u8 * ip4_prefix_lengths_in_search_order;
+  ip4_address_t ip4_fib_masks[33];
+  u32 ip4_prefix_len_refcount[33];
+
+  /* ip4 lookup table config parameters */
+  u32 ip4_lookup_table_nbuckets;
+  uword ip4_lookup_table_size;
+
+  BVT(clib_bihash) ip6_lookup_table;
+
+  /* bitmap/vector of mask widths to search */
+  uword * ip6_non_empty_dst_address_length_bitmap;
+  u8 * ip6_prefix_lengths_in_search_order;
+  ip6_address_t ip6_fib_masks[129];
+  u64 ip6_prefix_len_refcount[129];
+
+  /* ip6 lookup table config parameters */
+  u32 ip6_lookup_table_nbuckets;
+  uword ip6_lookup_table_size;
+
+} gid_dictionary_t;
+
+u32
+gid_dictionary_add_del (gid_dictionary_t *db, gid_address_t *key, u32 value,
+			u8 is_add);
+
+u32
+gid_dictionary_lookup (gid_dictionary_t * db, gid_address_t *key);
+
+void
+gid_dictionary_init (gid_dictionary_t * db);
+
+#endif /* VNET_LISP_GPE_GID_DICTIONARY_H_ */
diff --git a/vnet/vnet/lisp-cp/lisp_cp_messages.h b/vnet/vnet/lisp-cp/lisp_cp_messages.h
new file mode 100644
index 00000000000..6142a0479d3
--- /dev/null
+++ b/vnet/vnet/lisp-cp/lisp_cp_messages.h
@@ -0,0 +1,433 @@
+/*
+ * 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.
+ */
+
+#ifndef VNET_LISP_GPE_LISP_CP_MESSAGES_H_
+#define VNET_LISP_GPE_LISP_CP_MESSAGES_H_
+
+#include <vnet/vnet.h>
+
+#define MAX_IP_PKT_LEN 4096
+#define MAX_IP_HDR_LEN 40  /* without options or IPv6 hdr extensions */
+#define UDP_HDR_LEN 8
+#define LISP_DATA_HDR_LEN 8
+#define LISP_ECM_HDR_LEN 4
+#define MAX_LISP_MSG_ENCAP_LEN  2*(MAX_IP_HDR_LEN + UDP_HDR_LEN)+ LISP_ECM_HDR_LEN
+#define MAX_LISP_PKT_ENCAP_LEN  MAX_IP_HDR_LEN + UDP_HDR_LEN + LISP_DATA_HDR_LEN
+
+#define LISP_CONTROL_PORT 4342
+
+/*
+ * EID RECORD FIELD
+ */
+
+/*
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *    / |   Reserved    | EID mask-len  |        EID-prefix-AFI         |
+ *  Rec +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *    \ |                       EID-prefix  ...                         |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+
+
+typedef struct _eid_prefix_record_hdr {
+    u8 reserved;
+    u8 eid_prefix_length;
+} __attribute__ ((__packed__)) eid_record_hdr_t;
+
+void eid_rec_hdr_init(eid_record_hdr_t *ptr);
+
+#define EID_REC_CAST(h_) ((eid_record_hdr_t *)(h_))
+#define EID_REC_MLEN(h_) EID_REC_CAST((h_))->eid_prefix_length
+#define EID_REC_ADDR(h) (u8 *)(h) + sizeof(eid_record_hdr_t)
+
+/* LISP Types */
+typedef enum
+{
+  NOT_LISP_MSG,
+  LISP_MAP_REQUEST = 1,
+  LISP_MAP_REPLY,
+  LISP_MAP_REGISTER,
+  LISP_MAP_NOTIFY,
+  LISP_INFO_NAT = 7,
+  LISP_ENCAP_CONTROL_TYPE = 8,
+  LISP_MSG_TYPES
+} lisp_msg_type_e;
+
+/*
+ * ENCAPSULATED CONTROL MESSAGE
+ */
+
+/*
+ *     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
+ *     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *   / |                       IPv4 or IPv6 Header                     |
+ * OH  |                      (uses RLOC addresses)                    |
+ *   \ |                                                               |
+ *     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *   / |       Source Port = xxxx      |       Dest Port = 4342        |
+ * UDP +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *   \ |           UDP Length          |        UDP Checksum           |
+ *     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * LH  |Type=8 |S|                  Reserved                           |
+ *     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *   / |                       IPv4 or IPv6 Header                     |
+ * IH  |                  (uses RLOC or EID addresses)                 |
+ *   \ |                                                               |
+ *     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *   / |       Source Port = xxxx      |       Dest Port = yyyy        |
+ * UDP +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *   \ |           UDP Length          |        UDP Checksum           |
+ *     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ * LCM |                      LISP Control Message                     |
+ *     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+
+
+/*
+ * Encapsulated control message header. This is followed by the IP
+ * header of the encapsulated LISP control message.
+ *
+ *    +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *    |Type=8 |S|                 Reserved                            |
+ *    +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+
+typedef struct
+{
+#if CLIB_ARCH_IS_LITTLE_ENDIAN
+  u8 reserved:3;
+  u8 s_bit:1;
+  u8 type:4;
+#else
+  u8 type:4;
+  u8 s_bit:1;
+  u8 reserved:3;
+#endif
+  u8 reserved2[3];
+} ecm_hdr_t;
+
+char *ecm_hdr_to_char(ecm_hdr_t *h);
+
+#define ECM_TYPE(h_) ((ecm_hdr_t *)(h_))->type
+
+/*
+ * MAP-REQUEST MESSAGE
+ */
+
+/*
+ * Map-Request Message Format
+ *
+ *       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
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |Type=1 |A|M|P|S|p|s|    Reserved     |   IRC   | Record Count  |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |                         Nonce . . .                           |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |                         . . . Nonce                           |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |         Source-EID-AFI        |   Source EID Address  ...     |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |         ITR-RLOC-AFI 1        |    ITR-RLOC Address 1  ...    |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |                              ...                              |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |         ITR-RLOC-AFI n        |    ITR-RLOC Address n  ...    |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *    / |   Reserved    | EID mask-len  |        EID-prefix-AFI         |
+ *  Rec +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *    \ |                       EID-prefix  ...                         |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |                   Map-Reply Record  ...                       |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |                     Mapping Protocol Data                     |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+
+
+/*
+ * Fixed size portion of the map request. Variable size source EID
+ * address, originating ITR RLOC AFIs and addresses and then map
+ * request records follow.
+ */
+typedef struct
+{
+#if CLIB_ARCH_IS_LITTLE_ENDIAN
+    u8 solicit_map_request:1;
+    u8 rloc_probe:1;
+    u8 map_data_present:1;
+    u8 authoritative:1;
+    u8 type:4;
+#else
+    u8 type:4;
+    u8 authoritative:1;
+    u8 map_data_present:1;
+    u8 rloc_probe:1;
+    u8 solicit_map_request:1;
+#endif
+#if CLIB_ARCH_IS_LITTLE_ENDIAN
+    u8 reserved1:6;
+    u8 smr_invoked:1;
+    u8 pitr:1;
+#else
+    u8 pitr:1;
+    u8 smr_invoked:1;
+    u8 reserved1:6;
+#endif
+#if CLIB_ARCH_IS_LITTLE_ENDIAN
+    u8 additional_itr_rloc_count:5;
+    u8 reserved2:3;
+#else
+    u8 reserved2:3;
+    u8 additional_itr_rloc_count:5;
+#endif
+    u8 record_count;
+    u64 nonce;
+}__attribute__ ((__packed__)) map_request_hdr_t;
+
+void map_request_hdr_init(void *ptr);
+char *map_request_hdr_to_char(map_request_hdr_t *h);
+
+#define MREQ_TYPE(h_) (h_)->type
+#define MREQ_HDR_CAST(h_) ((map_request_hdr_t *)(h_))
+#define MREQ_REC_COUNT(h_) (MREQ_HDR_CAST(h_))->record_count
+#define MREQ_RLOC_PROBE(h_) (MREQ_HDR_CAST(h_))->rloc_probe
+#define MREQ_ITR_RLOC_COUNT(h_) (MREQ_HDR_CAST(h_))->additional_itr_rloc_count
+#define MREQ_NONCE(h_) (MREQ_HDR_CAST(h_))->nonce
+#define MREQ_SMR(h_) (MREQ_HDR_CAST(h_))->solicit_map_request
+#define MREQ_SMR_INVOKED(h_) (MREQ_HDR_CAST(h_))->smr_invoked
+
+/*
+ * MAP-REPLY MESSAGE
+ */
+
+ /*  Map Reply action codes */
+ #define LISP_ACTION_NO_ACTION           0
+ #define LISP_ACTION_FORWARD             1
+ #define LISP_ACTION_DROP                2
+ #define LISP_ACTION_SEND_MAP_REQUEST    3
+
+ /*
+  * Map-Reply Message Format
+  *
+  *       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
+  *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+  *      |Type=2 |P|E|S|         Reserved                | Record Count  |
+  *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+  *      |                         Nonce . . .                           |
+  *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+  *      |                         . . . Nonce                           |
+  *  +-> +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+  *  |   |                          Record  TTL                          |
+  *  |   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+  *  R   | Locator Count | EID mask-len  | ACT |A|      Reserved         |
+  *  e   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+  *  c   | Rsvd  |  Map-Version Number   |            EID-AFI            |
+  *  o   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+  *  r   |                          EID-prefix                           |
+  *  d   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+  *  |  /|    Priority   |    Weight     |  M Priority   |   M Weight    |
+  *  | L +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+  *  | o |        Unused Flags     |L|p|R|           Loc-AFI             |
+  *  | c +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+  *  |  \|                            Locator                            |
+  *  +-> +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+  *      |                     Mapping Protocol Data                     |
+  *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+  */
+
+ /*
+  * Fixed size portion of the map reply.
+  */
+typedef struct
+{
+#if CLIB_ARCH_IS_LITTLE_ENDIAN
+    u8 reserved1:1;
+    u8 security:1;
+    u8 echo_nonce:1;
+    u8 rloc_probe:1;
+    u8 type:4;
+#else
+    u8 type:4;
+    u8 rloc_probe:1;
+    u8 echo_nonce:1;
+    u8 security:1;
+    u8 reserved1:1;
+#endif
+    u8 reserved2;
+    u8 reserved3;
+    u8 record_count;
+    u64 nonce;
+} __attribute__ ((__packed__)) map_reply_hdr_t;
+
+ void map_reply_hdr_init(void *ptr);
+ char *map_reply_hdr_to_char(map_reply_hdr_t *h);
+
+#define MREP_HDR_CAST(h_) ((map_reply_hdr_t *)(h_))
+#define MREP_REC_COUNT(h_) MREP_HDR_CAST(h_)->record_count
+#define MREP_RLOC_PROBE(h_) MREP_HDR_CAST(h_)->rloc_probe
+#define MREP_NONCE(h_) MREP_HDR_CAST(h_)->nonce
+
+
+always_inline lisp_msg_type_e
+lisp_msg_type (void * b)
+{
+  ecm_hdr_t * hdr = b;
+  if (!hdr)
+    {
+      return (NOT_LISP_MSG);
+    }
+  return (hdr->type);
+}
+
+always_inline void
+increment_record_count (void * b)
+{
+  switch (lisp_msg_type (b))
+    {
+    case LISP_MAP_REQUEST:
+      MREQ_REC_COUNT(b) += 1;
+      break;
+    case LISP_MAP_REPLY:
+      MREP_REC_COUNT(b) += 1;
+      break;
+    default:
+      return;
+    }
+}
+
+
+/*
+ * LOCATOR FIELD
+ *
+ *     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *    /|    Priority   |    Weight     |  M Priority   |   M Weight    |
+ *   L +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *   o |        Unused Flags     |L|p|R|           Loc-AFI             |
+ *   c +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *    \|                            Locator                            |
+ *     +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *
+ * Fixed portion of the mapping record locator. Variable length
+ * locator address follows.
+ */
+typedef struct _locator_hdr {
+    u8 priority;
+    u8 weight;
+    u8 mpriority;
+    u8 mweight;
+    u8 unused1;
+#ifdef CLIB_ARCH_IS_LITTLE_ENDIAN
+    u8 reachable:1;
+    u8 probed:1;
+    u8 local:1;
+    u8 unused2:5;
+#else
+    u8 unused2:5;
+    u8 local:1;
+    u8 probed:1;
+    u8 reachable:1;
+#endif
+} __attribute__ ((__packed__)) locator_hdr_t;
+
+#define LOC_CAST(h_) ((locator_hdr_t *)(h_))
+#define LOC_PROBED(h_) LOC_CAST(h_)->probed
+#define LOC_PRIORITY(h_) LOC_CAST(h_)->priority
+#define LOC_WEIGHT(h_) LOC_CAST(h_)->weight
+#define LOC_MPRIORITY(h_) LOC_CAST(h_)->mpriority
+#define LOC_MWEIGHT(h_) LOC_CAST(h_)->mweight
+#define LOC_REACHABLE(h_) LOC_CAST(h_)->reachable
+#define LOC_LOCAL(h_) LOC_CAST(h_)->local
+#define LOC_ADDR(h_) ((u8 *)(h_)  + sizeof(locator_hdr_t))
+
+/*
+ * MAPPING RECORD
+ *
+ * Mapping record used in all LISP control messages.
+ *
+ *  +--->  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |      |                          Record  TTL                          |
+ *  |      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  R      | Locator Count | EID mask-len  | ACT |A|       Reserved        |
+ *  e      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  c      | Rsvd  |  Map-Version Number   |            EID-AFI            |
+ *  o      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  r      |                          EID-prefix                           |
+ *  d      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |     /|    Priority   |    Weight     |  M Priority   |   M Weight    |
+ *  |    / +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |  Loc |         Unused Flags    |L|p|R|           Loc-AFI             |
+ *  |    \ +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |     \|                             Locator                           |
+ *  +--->  +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+
+/*
+ * Fixed portion of the mapping record. EID prefix address and
+ * locators follow.
+ */
+
+typedef struct _mapping_record_hdr_t
+{
+  u32 ttl;
+  u8 locator_count;
+  u8 eid_prefix_length;
+#ifdef CLIB_ARCH_IS_LITTLE_ENDIAN
+  u8 reserved1:4;
+  u8 authoritative:1;
+  u8 action:3;
+#else
+  u8 action :3;
+  u8 authoritative :1;
+  u8 reserved1 :4;
+#endif
+  u8 reserved2;
+#ifdef CLIB_ARCH_IS_LITTLE_ENDIAN
+  u8 version_hi:4;
+  u8 reserved3:4;
+#else
+  u8 reserved3 :4;
+  u8 version_hi :4;
+#endif
+  u8 version_low;
+}__attribute__ ((__packed__)) mapping_record_hdr_t;
+
+void mapping_record_init_hdr(mapping_record_hdr_t *h);
+
+#define MAP_REC_EID_PLEN(h) ((mapping_record_hdr_t *)(h))->eid_prefix_length
+#define MAP_REC_LOC_COUNT(h) ((mapping_record_hdr_t *)(h))->locator_count
+#define MAP_REC_ACTION(h) ((mapping_record_hdr_t *)(h))->action
+#define MAP_REC_AUTH(h) ((mapping_record_hdr_t *)(h))->authoritative
+#define MAP_REC_TTL(h) ((mapping_record_hdr_t *)(h))->ttl
+#define MAP_REC_EID(h) (u8 *)(h)+sizeof(mapping_record_hdr_t)
+#define MAP_REC_VERSION(h) (h)->version_hi << 8 | (h)->version_low
+
+typedef enum lisp_actions
+{
+  ACT_NO_ACTION = 0,
+  ACT_NATIVE_FWD,
+  ACT_SEND_MREQ,
+  ACT_DROP
+} lisp_action_e;
+
+typedef enum lisp_authoritative
+{
+  A_NO_AUTHORITATIVE = 0,
+  A_AUTHORITATIVE
+} lisp_authoritative_e;
+
+#endif /* VNET_LISP_GPE_LISP_CP_MESSAGES_H_ */
diff --git a/vnet/vnet/lisp-cp/lisp_msg_serdes.c b/vnet/vnet/lisp-cp/lisp_msg_serdes.c
new file mode 100644
index 00000000000..0e5ba73d0db
--- /dev/null
+++ b/vnet/vnet/lisp-cp/lisp_msg_serdes.c
@@ -0,0 +1,270 @@
+/*
+ * 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.
+ */
+
+#include <vnet/lisp-cp/lisp_msg_serdes.h>
+#include <vnet/lisp-cp/packets.h>
+#include <vppinfra/time.h>
+
+void *
+lisp_msg_put_gid (vlib_buffer_t * b, gid_address_t * gid)
+{
+  u8 * p = vlib_buffer_put_uninit (b, gid_address_size_to_put (gid));
+  gid_address_put (p, gid);
+  return p;
+}
+
+void *
+lisp_msg_put_itr_rlocs (lisp_cp_main_t * lcm, vlib_buffer_t * b,
+                        locator_set_t * loc_set, u8 * locs_put)
+{
+  ip_interface_address_t * ia = 0;
+  ip4_address_t * l4;
+  ip6_address_t * l6;
+  u32 * loc_indexp;
+  locator_t * loc;
+  u32 i;
+  u8 * p, * bp, count = 0;
+
+  bp = vlib_buffer_get_current(b);
+  for (i = 0; i < vec_len(loc_set->locator_indices); i++)
+    {
+      loc_indexp = vec_elt_at_index(loc_set->locator_indices, i);
+      loc = pool_elt_at_index (lcm->locator_pool, loc_indexp[0]);
+
+      /* Add ipv4 locators first TODO sort them */
+      foreach_ip_interface_address (&lcm->im4->lookup_main, ia,
+                                    loc->sw_if_index, 1 /* unnumbered */,
+      ({
+        l4 = ip_interface_address_get_address (&lcm->im4->lookup_main, ia);
+        p = vlib_buffer_put_uninit (b, ip4_address_size_to_put());
+        ip4_address_put (p, l4);
+        count++;
+      }));
+
+      /* Add ipv6 locators */
+      foreach_ip_interface_address (&lcm->im6->lookup_main, ia,
+                                    loc->sw_if_index, 1 /* unnumbered */,
+      ({
+        l6 = ip_interface_address_get_address (&lcm->im6->lookup_main, ia);
+        p = vlib_buffer_put_uninit (b, ip6_address_size_to_put());
+        ip6_address_put (p, l6);
+        count++;
+      }));
+    }
+  *locs_put = count-1;
+  return bp;
+}
+
+void *
+lisp_msg_put_eid_rec (vlib_buffer_t * b, gid_address_t * eid)
+{
+  eid_record_hdr_t * h = vlib_buffer_put_uninit (b, sizeof (*h));
+
+  memset(h, 0, sizeof (*h));
+  EID_REC_MLEN (h) = gid_address_len (eid);
+  lisp_msg_put_gid (b, eid);
+  return h;
+}
+
+u64
+nonce_build (u32 seed)
+{
+  u64 nonce;
+  u32 nonce_lower;
+  u32 nonce_upper;
+  struct timespec ts;
+
+  /* Put nanosecond clock in lower 32-bits and put an XOR of the nanosecond
+   * clock with the seond clock in the upper 32-bits. */
+  syscall (SYS_clock_gettime, CLOCK_REALTIME, &ts);
+  nonce_lower = ts.tv_nsec;
+  nonce_upper = ts.tv_sec ^ clib_host_to_net_u32(nonce_lower);
+
+  /* OR in a caller provided seed to the low-order 32-bits. */
+  nonce_lower |= seed;
+
+  /* Return 64-bit nonce. */
+  nonce = nonce_upper;
+  nonce = (nonce << 32) | nonce_lower;
+  return nonce;
+}
+
+void *
+lisp_msg_put_mreq (lisp_cp_main_t * lcm, vlib_buffer_t * b,
+                   gid_address_t * seid, gid_address_t * deid,
+                   locator_set_t * loc_set, u8 is_smr_invoked, u64 * nonce)
+{
+  u8 loc_count = 0;
+
+  /* Basic header init */
+  map_request_hdr_t * h = vlib_buffer_put_uninit (b, sizeof(h[0]));
+
+  memset(h, 0, sizeof(h[0]));
+  MREQ_TYPE(h) = LISP_MAP_REQUEST;
+  MREQ_NONCE(h) = nonce_build(0);
+  MREQ_SMR_INVOKED(h) = is_smr_invoked ? 1 : 0;
+
+  /* We're adding one eid record */
+  increment_record_count (h);
+
+  /* Fill source eid */
+  lisp_msg_put_gid (b, seid);
+
+  /* Put itr rlocs */
+  lisp_msg_put_itr_rlocs(lcm, b, loc_set, &loc_count);
+  MREQ_ITR_RLOC_COUNT(h) = loc_count;
+
+  /* Put eid record */
+  lisp_msg_put_eid_rec(b, deid);
+
+  nonce[0] = MREQ_NONCE(h);
+  return h;
+}
+
+void *
+lisp_msg_push_ecm (vlib_main_t * vm, vlib_buffer_t *b, int lp, int rp,
+                   gid_address_t *la, gid_address_t *ra)
+{
+  ecm_hdr_t *h;
+  ASSERT(gid_address_type(la) == IP_PREFIX);
+
+  /* Push inner ip and udp */
+  pkt_push_udp_and_ip (vm, b, lp, rp, &gid_address_ip(la),
+                              &gid_address_ip(ra));
+
+  /* Push lisp ecm hdr */
+  h = pkt_push_ecm_hdr (b);
+
+  return h;
+}
+
+static u32
+msg_type_to_hdr_len (lisp_msg_type_e type)
+{
+  switch (type)
+    {
+    case LISP_MAP_REQUEST:
+      return (sizeof(map_request_hdr_t));
+    case LISP_MAP_REPLY:
+      return (sizeof(map_reply_hdr_t));
+    default:
+      return (0);
+    }
+}
+
+void *
+lisp_msg_pull_hdr (vlib_buffer_t * b, lisp_msg_type_e type)
+{
+  return vlib_buffer_pull (b, msg_type_to_hdr_len (type));
+}
+
+u32
+lisp_msg_parse_addr (vlib_buffer_t * b, gid_address_t * eid)
+{
+  u32 len = gid_address_parse (vlib_buffer_get_current (b), eid);
+  if (len != ~0)
+    vlib_buffer_pull (b, len);
+  return len;
+}
+
+u32
+lisp_msg_parse_eid_rec (vlib_buffer_t * b, gid_address_t * eid)
+{
+  eid_record_hdr_t * h = vlib_buffer_get_current (b);
+  u32 len = gid_address_parse (EID_REC_ADDR(h), eid);
+  if (len == ~0)
+    return len;
+
+  gid_address_ippref_len(eid) = EID_REC_MLEN(h);
+  vlib_buffer_pull (b, len + sizeof(eid_record_hdr_t));
+
+  return len + sizeof(eid_record_hdr_t);
+}
+
+u32
+lisp_msg_parse_itr_rlocs (vlib_buffer_t * b, gid_address_t ** rlocs,
+                          u8 rloc_count)
+{
+  gid_address_t tloc;
+  u32 i, len = 0, tlen = 0;
+
+  //MREQ_ITR_RLOC_COUNT(mreq_hdr) + 1
+  for (i = 0; i < rloc_count; i++)
+    {
+      len = lisp_msg_parse_addr (b, &tloc);
+      if (len == ~0)
+        return len;
+      vec_add1(*rlocs, tloc);
+      tlen += len;
+    }
+  return tlen;
+}
+
+u32
+lisp_msg_parse_loc (vlib_buffer_t * b, locator_t * loc)
+{
+  int len;
+
+  len = locator_parse (vlib_buffer_get_current (b), loc);
+  if (len == ~0)
+      return ~0;
+
+  vlib_buffer_pull (b, len);
+
+  return len;
+}
+
+u32
+lisp_msg_parse_mapping_record (vlib_buffer_t * b, gid_address_t * eid,
+                               locator_t ** locs, locator_t * probed_)
+{
+  void * h = 0, * loc_hdr = 0;
+  locator_t loc, * probed = 0;
+  int i = 0, len = 0, llen = 0;
+
+  h = vlib_buffer_get_current (b);
+  vlib_buffer_pull (b, sizeof(mapping_record_hdr_t));
+
+  len = gid_address_parse (vlib_buffer_get_current (b), eid);
+  if (len == ~0)
+    return len;
+
+  vlib_buffer_pull (b, len);
+  gid_address_ippref_len(eid) = MAP_REC_EID_PLEN(h);
+
+  for (i = 0; i < MAP_REC_LOC_COUNT(h); i++)
+    {
+      loc_hdr = vlib_buffer_get_current (b);
+
+      llen = lisp_msg_parse_loc (b, &loc);
+      if (llen == ~0)
+        return llen;
+      vec_add1(*locs, loc);
+      len += llen;
+
+      if (LOC_PROBED(loc_hdr))
+        {
+          if (probed != 0)
+            clib_warning("Multiple locators probed! Probing only the first!");
+          else
+            probed = &loc;
+        }
+    }
+  /* XXX */
+  if (probed_ != 0 && probed)
+    *probed_ = *probed;
+
+  return len + sizeof(map_reply_hdr_t);
+}
diff --git a/vnet/vnet/lisp-cp/lisp_msg_serdes.h b/vnet/vnet/lisp-cp/lisp_msg_serdes.h
new file mode 100644
index 00000000000..dddae0747ba
--- /dev/null
+++ b/vnet/vnet/lisp-cp/lisp_msg_serdes.h
@@ -0,0 +1,46 @@
+/*
+ * 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.
+ */
+
+#ifndef VNET_LISP_GPE_LISP_MSG_BUILDER_H_
+#define VNET_LISP_GPE_LISP_MSG_BUILDER_H_
+
+#include <vnet/vnet.h>
+#include <vnet/lisp-cp/lisp_cp_messages.h>
+#include <vnet/lisp-cp/control.h>
+
+void *
+lisp_msg_put_mreq (lisp_cp_main_t * lcm, vlib_buffer_t * b,
+		   gid_address_t * seid, gid_address_t * deid,
+		   locator_set_t * loc_set, u8 is_smr_invoked, u64 * nonce);
+
+void *
+lisp_msg_push_ecm (vlib_main_t * vm, vlib_buffer_t *b, int lp, int rp,
+		   gid_address_t *la, gid_address_t *ra);
+
+u32
+lisp_msg_parse_mapping_record (vlib_buffer_t * b, gid_address_t * eid,
+			       locator_t ** locs, locator_t * probed_);
+
+u32
+lisp_msg_parse_addr (vlib_buffer_t * b, gid_address_t * eid);
+
+u32
+lisp_msg_parse_eid_rec (vlib_buffer_t *b, gid_address_t * eid);
+
+u32
+lisp_msg_parse_itr_rlocs (vlib_buffer_t * b, gid_address_t ** rlocs,
+			  u8 rloc_count);
+
+#endif /* VNET_LISP_GPE_LISP_MSG_BUILDER_H_ */
diff --git a/vnet/vnet/lisp-cp/lisp_types.c b/vnet/vnet/lisp-cp/lisp_types.c
new file mode 100644
index 00000000000..a04d36ff8d0
--- /dev/null
+++ b/vnet/vnet/lisp-cp/lisp_types.c
@@ -0,0 +1,475 @@
+/*
+ * 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.
+ */
+
+#include <vnet/lisp-cp/lisp_types.h>
+
+typedef u16 (*size_to_write_fct)(void *);
+typedef void * (*cast_fct)(gid_address_t *);
+typedef u16 (*write_fct)(u8 *, void *);
+typedef u8 (*addr_len_fct)(void *);
+typedef void (*copy_fct)(void *, void *);
+
+size_to_write_fct size_to_write_fcts[GID_ADDR_TYPES] =
+  { ip_prefix_size_to_write };
+write_fct write_fcts[GID_ADDR_TYPES] =
+  { ip_prefix_write };
+cast_fct cast_fcts[GID_ADDR_TYPES] =
+  { ip_prefix_cast };
+addr_len_fct addr_len_fcts[GID_ADDR_TYPES] =
+  { ip_prefix_length };
+copy_fct copy_fcts[GID_ADDR_TYPES] =
+  { ip_prefix_copy };
+
+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)
+{
+  ip_address_t * a = va_arg(*args, ip_address_t *);
+  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;
+}
+
+u8 *
+format_ip_prefix (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 *);
+  return unformat (input, "%U/%d", unformat_ip_address, &ip_prefix_addr(a),
+                   &ip_prefix_len(a));
+}
+
+u8 *
+format_gid_address (u8 * s, va_list * args)
+{
+  gid_address_t * a = va_arg(*args, gid_address_t *);
+  u8 type = gid_address_type(a);
+  switch (type)
+    {
+    case IP_PREFIX:
+      return format (s, "%U", format_ip_prefix, &gid_address_ippref(a));
+    default:
+      clib_warning("Can't format gid type %d", type);
+      return 0;
+    }
+}
+
+uword
+unformat_gid_address (unformat_input_t * input, va_list * args)
+{
+  gid_address_t * a = va_arg(*args, gid_address_t *);
+  if (unformat (input, "%U", unformat_ip_prefix, &gid_address_ippref(a)))
+    gid_address_type(a) = IP_PREFIX;
+  else
+    return 0;
+  return 1;
+}
+
+u16
+ip_address_size (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)
+{
+  switch (ver)
+  {
+    case IP4:
+      return 32;
+      break;
+    case IP6:
+      return 128;
+      break;
+  }
+  return 0;
+}
+
+always_inline lisp_afi_e
+ip_version_to_iana_afi (u16 version)
+{
+  switch (version)
+    {
+    case IP4:
+      return LISP_AFI_IP;
+    case IP6:
+      return LISP_AFI_IP6;
+    default:
+      return 0;
+    }
+  return 0;
+}
+
+always_inline u8
+ip_iana_afi_to_version (lisp_afi_e afi)
+{
+  switch (afi)
+    {
+    case LISP_AFI_IP:
+      return IP4;
+    case LISP_AFI_IP6:
+      return IP6;
+    default:
+      return 0;
+    }
+  return 0;
+}
+
+u16
+ip_address_size_to_write (ip_address_t * a)
+{
+  return ip_address_size (a) + sizeof (u16);
+}
+
+u16
+ip_address_iana_afi(ip_address_t *a)
+{
+    return ip_version_to_iana_afi(ip_addr_version(a));
+}
+
+u8
+ip_address_max_len (u8 version)
+{
+  return version == IP4 ? 32 : 128;
+}
+
+u16
+ip4_address_size_to_put ()
+{
+  // return sizeof(u16) + sizeof (ip4_address_t);
+  return 6;
+}
+
+u16
+ip6_address_size_to_put ()
+{
+  //return sizeof(u16) + sizeof (ip6_address_t);
+  return 18;
+}
+
+u32
+ip4_address_put (u8 * b, ip4_address_t * a)
+{
+  *(u16 *)b = clib_host_to_net_u16(ip_version_to_iana_afi(IP4));
+  u8 *p = b + sizeof (u16);
+  memcpy (p, a, sizeof(*a));
+  return ip4_address_size_to_put();
+}
+
+u32
+ip6_address_put (u8 * b, ip6_address_t * a)
+{
+  *(u16 *)b = clib_host_to_net_u16(ip_version_to_iana_afi(IP6));
+  u8 *p = b + sizeof (u16);
+  memcpy (p, a, sizeof(*a));
+  return ip6_address_size_to_put();
+}
+
+u32
+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);
+  memcpy (p, &ip_addr_addr (a), len);
+  return (len + sizeof (u16));
+}
+
+u32
+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));
+  memcpy (&ip_addr_addr(dst), offset + sizeof(u16), size);
+  return(sizeof(u16) + size);
+}
+
+int
+ip_address_cmp (ip_address_t * ip1, 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_prefix_cast (gid_address_t * a)
+{
+  return &gid_address_ippref(a);
+}
+
+u16
+ip_prefix_size_to_write (void * pref)
+{
+  ip_prefix_t *a = (ip_prefix_t *) pref;
+  return ip_address_size_to_write (&ip_prefix_addr (a));
+}
+
+u16
+ip_prefix_write (u8 * p, void * pref)
+{
+  ip_prefix_t *a = (ip_prefix_t *) pref;
+  switch (ip_prefix_version (a))
+  {
+    case IP4:
+      return ip4_address_put (p, &ip_prefix_v4 (a));
+      break;
+    case 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)
+{
+  memcpy (dst, src, sizeof (ip_prefix_t));
+}
+
+int
+ip_prefix_cmp(ip_prefix_t * p1, ip_prefix_t * p2)
+{
+  int cmp = 0;
+  cmp = ip_address_cmp (&ip_prefix_addr(p1), &ip_prefix_addr(p2));
+  if (cmp == 0)
+    cmp = ip_prefix_len(p1) < ip_prefix_len(p2) ? 1 : 2; /* XXX ? */
+  return cmp;
+}
+
+u8
+gid_address_len (gid_address_t *a)
+{
+  gid_address_type_t type = gid_address_type (a);
+  return (*addr_len_fcts[type])((*cast_fcts[type])(a));
+}
+
+u16
+gid_address_put (u8 * b, gid_address_t * gid)
+{
+  gid_address_type_t type = gid_address_type (gid);
+  return (*write_fcts[type])(b, (*cast_fcts[type])(gid));
+}
+
+u16
+gid_address_size_to_put (gid_address_t * gid)
+{
+  gid_address_type_t type = gid_address_type (gid);
+  return (*size_to_write_fcts[type])((*cast_fcts[type])(gid));
+}
+
+void *
+gid_address_cast (gid_address_t * gid, gid_address_type_t type)
+{
+  return (*cast_fcts[type])(gid);
+}
+
+void
+gid_address_copy(gid_address_t * dst, gid_address_t * src)
+{
+  gid_address_type_t type = gid_address_type(src);
+  (*copy_fcts[type])((*cast_fcts[type])(dst), (*cast_fcts[type])(src));
+  gid_address_type(dst) = type;
+}
+
+u32
+gid_address_parse (u8 * offset, gid_address_t *a)
+{
+  lisp_afi_e afi;
+  int len = 0;
+
+  if (!a)
+    return 0;
+
+  afi = clib_net_to_host_u16 (*((u16 *) offset));
+
+  switch (afi)
+    {
+    case LISP_AFI_NO_ADDR:
+      len = sizeof(u16);
+      gid_address_type(a) = NO_ADDRESS;
+      break;
+    case LISP_AFI_IP:
+      len = ip_address_parse (offset, afi, &gid_address_ip(a));
+      gid_address_type(a) = IP_PREFIX;
+      /* this should be modified outside if needed*/
+      gid_address_ippref_len(a) = 32;
+      break;
+    case LISP_AFI_IP6:
+      len = ip_address_parse (offset, afi, &gid_address_ip(a));
+      gid_address_type(a) = IP_PREFIX;
+      /* this should be modified outside if needed*/
+      gid_address_ippref_len(a) = 128;
+      break;
+    case LISP_AFI_LCAF:
+    default:
+      clib_warning("LISP AFI %d not supported!", afi);
+      return ~0;
+    }
+  return len;
+}
+
+/* Compare two gid_address_t.
+ * Returns:
+ *        -1: If they are from different afi
+ *             0: Both address are the same
+ *             1: Addr1 is bigger than addr2
+ *             2: Addr2 is bigger than addr1
+ */
+int
+gid_address_cmp (gid_address_t * a1, gid_address_t * a2)
+{
+  int cmp = -1;
+  if (!a1 || !a2)
+    return -1;
+  if (gid_address_type(a1) != gid_address_type(a2))
+    return -1;
+
+  switch (gid_address_type(a1))
+    {
+    case NO_ADDRESS:
+      if (a1 == a2)
+        cmp = 0;
+      else
+        cmp = 2;
+      break;
+    case IP_PREFIX:
+      cmp = ip_prefix_cmp (&gid_address_ippref(a1), &gid_address_ippref(a2));
+      break;
+    default:
+      break;
+    }
+
+  return cmp;
+}
+
+
+u32
+locator_parse (void * b, locator_t * loc)
+{
+  locator_hdr_t * h;
+  u8 status = 1; /* locator up */
+  int len;
+
+  h = b;
+  if (!LOC_REACHABLE(h) && LOC_LOCAL(h))
+    status = 0;
+
+  len = gid_address_parse (LOC_ADDR(h), &loc->address);
+  if (len == ~0)
+    return len;
+
+  loc->state = status;
+  loc->local = 0;
+  loc->priority = LOC_PRIORITY(h);
+  loc->weight = LOC_WEIGHT(h);
+  loc->mpriority = LOC_MPRIORITY(h);
+  loc->mweight = LOC_MWEIGHT(h);
+
+  return sizeof(locator_hdr_t) + len;
+}
+
+void
+locator_copy (locator_t * dst, locator_t * src)
+{
+  /* TODO if gid become more complex, this will need to be changed! */
+  memcpy (dst, src, sizeof(*dst));
+}
+
+u32
+locator_cmp (locator_t * l1, locator_t * l2)
+{
+  u32 ret = 0;
+  if ((ret = gid_address_cmp (&l1->address, &l2->address)) != 0)
+    return 1;
+
+  if (l1->priority != l2->priority)
+    return 1;
+  if (l1->weight != l2->weight)
+    return 1;
+  if (l1->mpriority != l2->mpriority)
+    return 1;
+  if (l1->mweight != l2->mweight)
+    return 1;
+  return 0;
+}
diff --git a/vnet/vnet/lisp-cp/lisp_types.h b/vnet/vnet/lisp-cp/lisp_types.h
new file mode 100644
index 00000000000..9602387fd9b
--- /dev/null
+++ b/vnet/vnet/lisp-cp/lisp_types.h
@@ -0,0 +1,163 @@
+/*
+ * 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.
+ */
+
+#ifndef VNET_LISP_GPE_LISP_TYPES_H_
+#define VNET_LISP_GPE_LISP_TYPES_H_
+
+#include <vnet/ip/ip.h>
+#include <vnet/lisp-cp/lisp_cp_messages.h>
+
+typedef enum
+{
+  IP4,
+  IP6
+} ip_address_type_t;
+
+typedef struct
+{
+  union
+  {
+    ip4_address_t v4;
+    ip6_address_t v6;
+  } ip;
+  ip_address_type_t version;
+} ip_address_t;
+
+typedef struct
+{
+  ip_address_t addr;
+  u8 len;
+} ip_prefix_t;
+
+#define ip_addr_addr(_a) (_a)->ip
+#define ip_addr_v4(_a) (_a)->ip.v4
+#define ip_addr_v6(_a) (_a)->ip.v6
+#define ip_addr_version(_a) (_a)->version
+
+#define ip_prefix_addr(_a) (_a)->addr
+#define ip_prefix_version(_a) ip_addr_version(&ip_prefix_addr(_a))
+#define ip_prefix_len(_a) (_a)->len
+#define ip_prefix_v4(_a) ip_addr_v4(&ip_prefix_addr(_a))
+#define ip_prefix_v6(_a) ip_addr_v6(&ip_prefix_addr(_a))
+
+typedef enum
+{
+  /* NOTE: ip addresses are left out on purpose. Use max masked ip-prefixes
+   * instead */
+  IP_PREFIX,
+  NO_ADDRESS,
+  GID_ADDR_TYPES
+} gid_address_type_t;
+
+/* might want to expand this in the future :) */
+typedef struct
+{
+  union
+  {
+    ip_prefix_t ippref;
+  };
+  u8 type;
+} gid_address_t;
+
+u8 * format_ip_address (u8 * s, va_list * args);
+uword unformat_ip_address (unformat_input_t * input, va_list * args);
+u8 * format_ip_prefix (u8 * s, va_list * args);
+uword unformat_ip_prefix (unformat_input_t * input, va_list * args);
+
+u16 ip4_address_size_to_put ();
+u16 ip6_address_size_to_put ();
+u32 ip4_address_put (u8 * b, ip4_address_t * a);
+u32 ip6_address_put (u8 * b, ip6_address_t * a);
+
+u16 ip_address_size_to_write (ip_address_t * a);
+u16 ip_address_iana_afi(ip_address_t *a);
+u8 ip_address_max_len (u8 ver);
+u32 ip_address_put (u8 * b, ip_address_t * a);
+
+/* LISP AFI codes  */
+typedef enum {
+    LISP_AFI_NO_ADDR,
+    LISP_AFI_IP,
+    LISP_AFI_IP6,
+    LISP_AFI_LCAF = 16387
+} lisp_afi_e;
+
+u8 *format_gid_address (u8 * s, va_list * args);
+uword unformat_gid_address (unformat_input_t * input, va_list * args);
+
+u16 gid_address_size_to_put (gid_address_t * a);
+u16 gid_address_put (u8 * b, gid_address_t * gid);
+u8 gid_address_len (gid_address_t *a);
+void * gid_address_cast (gid_address_t * gid, gid_address_type_t type);
+void gid_address_copy(gid_address_t * dst, gid_address_t * src);
+u32 gid_address_parse (u8 * offset, gid_address_t *a);
+
+#define gid_address_type(_a) (_a)->type
+#define gid_address_ippref(_a) (_a)->ippref
+#define gid_address_ippref_len(_a) (_a)->ippref.len
+#define gid_address_ip(_a) ip_prefix_addr(&gid_address_ippref(_a))
+
+/* 'sub'address functions */
+int ip_address_cmp (ip_address_t * ip1, ip_address_t * ip2);
+u16 ip_prefix_size_to_write (void * pref);
+u16 ip_prefix_write (u8 * p, void * pref);
+u8 ip_prefix_length (void *a);
+void *ip_prefix_cast (gid_address_t * a);
+void ip_prefix_copy (void * dst , void * src);
+
+typedef struct
+{
+  /* mark locator as local as opposed to remote */
+  u8 local;
+  u8 state;
+  union {
+    u32 sw_if_index;
+    gid_address_t address;
+  };
+  u8 priority;
+  u8 weight;
+  u8 mpriority;
+  u8 mweight;
+} locator_t;
+
+u32 locator_parse (void * ptr, locator_t * loc);
+void locator_copy (locator_t * dst, locator_t * src);
+u32 locator_cmp (locator_t * l1, locator_t * l2);
+
+typedef struct
+{
+  /* locator-set name */
+  u8 * name;
+
+  /* vector of locator indices */
+  u32 * locator_indices;
+  u8 local;
+} locator_set_t;
+
+typedef struct
+{
+  gid_address_t eid;
+
+  /* index of local locator set */
+  u32 locator_set_index;
+
+  u32 ttl;
+  u8 action;
+  u8 authoritative;
+
+  u8 local;
+} mapping_t;
+
+#endif /* VNET_LISP_GPE_LISP_TYPES_H_ */
diff --git a/vnet/vnet/lisp-cp/packets.c b/vnet/vnet/lisp-cp/packets.c
new file mode 100644
index 00000000000..e3e006b3ef7
--- /dev/null
+++ b/vnet/vnet/lisp-cp/packets.c
@@ -0,0 +1,248 @@
+/*
+ * 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.
+ */
+
+#include <vnet/lisp-cp/packets.h>
+#include <vnet/lisp-cp/lisp_cp_messages.h>
+#include <vnet/ip/udp_packet.h>
+
+///* Returns IP ID for the packet */
+//static u16 ip_id = 0;
+//static inline u16
+//get_IP_ID()
+//{
+//    ip_id++;
+//    return (ip_id);
+//}
+
+u16
+udp_ip4_checksum (const void *b, u32 len, u8 *src, u8 *dst)
+{
+  const u16 *buf = b;
+  u16 *ip_src = (u16 *) src;
+  u16 *ip_dst = (u16 *) dst;
+  u32 length = len;
+  u32 sum = 0;
+
+  while (len > 1)
+    {
+      sum += *buf++;
+      if (sum & 0x80000000)
+        sum = (sum & 0xFFFF) + (sum >> 16);
+      len -= 2;
+    }
+
+  /* Add the padding if the packet length is odd */
+  if (len & 1)
+    sum += *((u8 *) buf);
+
+  /* Add the pseudo-header */
+  sum += *(ip_src++);
+  sum += *ip_src;
+
+  sum += *(ip_dst++);
+  sum += *ip_dst;
+
+  sum += clib_host_to_net_u16 (IP_PROTOCOL_UDP);
+  sum += clib_host_to_net_u16 (length);
+
+  /* Add the carries */
+  while (sum >> 16)
+    sum = (sum & 0xFFFF) + (sum >> 16);
+
+  /* Return the one's complement of sum */
+  return ((u16) (~sum));
+}
+
+u16
+udp_ip6_checksum (ip6_header_t *ip6, udp_header_t *up, u32 len)
+{
+  size_t i;
+  register const u16 *sp;
+  u32 sum;
+  union
+  {
+    struct
+    {
+      ip6_address_t ph_src;
+      ip6_address_t ph_dst;
+      u32 ph_len;
+      u8 ph_zero[3];
+      u8 ph_nxt;
+    } ph;
+    u16 pa[20];
+  } phu;
+
+  /* pseudo-header */
+  memset (&phu, 0, sizeof(phu));
+  phu.ph.ph_src = ip6->src_address;
+  phu.ph.ph_dst = ip6->dst_address;
+  phu.ph.ph_len = clib_host_to_net_u32 (len);
+  phu.ph.ph_nxt = IP_PROTOCOL_UDP;
+
+  sum = 0;
+  for (i = 0; i < sizeof(phu.pa) / sizeof(phu.pa[0]); i++)
+    sum += phu.pa[i];
+
+  sp = (const u16 *) up;
+
+  for (i = 0; i < (len & ~1); i += 2)
+    sum += *sp++;
+
+  if (len & 1)
+    sum += clib_host_to_net_u16 ((*(const u8 *) sp) << 8);
+
+  while (sum > 0xffff)
+    sum = (sum & 0xffff) + (sum >> 16);
+  sum = ~sum & 0xffff;
+
+  return (sum);
+}
+
+u16
+udp_checksum (udp_header_t *uh, u32 udp_len, void *ih, u8 version)
+{
+  switch (version)
+    {
+    case IP4:
+      return (udp_ip4_checksum (uh, udp_len,
+                                ((ip4_header_t *) ih)->src_address.as_u8,
+                                ((ip4_header_t *) ih)->dst_address.as_u8));
+    case IP6:
+      return (udp_ip6_checksum (ih, uh, udp_len));
+    default:
+      return ~0;
+    }
+}
+
+void *
+pkt_push_udp (vlib_main_t * vm, vlib_buffer_t *b, u16 sp, u16 dp)
+{
+  udp_header_t * uh;
+  u16 udp_len = sizeof(udp_header_t) + vlib_buffer_length_in_chain (vm, b);
+
+  uh = vlib_buffer_push_uninit (b, sizeof(*uh));
+
+  uh->src_port = clib_host_to_net_u16 (sp);
+  uh->dst_port = clib_host_to_net_u16 (dp);
+  uh->length = clib_host_to_net_u16 (udp_len);
+  uh->checksum = 0;
+  return uh;
+}
+
+void *
+pkt_push_ipv4 (vlib_main_t * vm, vlib_buffer_t *b, ip4_address_t *src,
+               ip4_address_t *dst, int proto)
+{
+  ip4_header_t * ih;
+
+  /* make some room */
+  ih = vlib_buffer_push_uninit(b, sizeof(ip4_header_t));
+
+  ih->ip_version_and_header_length = 0x45;
+  ih->tos = 0;
+  ih->length = clib_host_to_net_u16(vlib_buffer_length_in_chain (vm, b));
+
+  // iph->fragment_id = clib_host_to_net_u16(get_IP_ID ());
+
+  /* TODO: decide if we allow fragments in case of control */
+  ih->flags_and_fragment_offset = clib_host_to_net_u16(IP_DF);
+  ih->ttl = 255;
+  ih->protocol = proto;
+  ih->src_address.as_u32 = src->as_u32;
+  ih->dst_address.as_u32 = dst->as_u32;
+
+  ih->checksum = ip4_header_checksum (ih);
+  return ih;
+}
+
+void *
+pkt_push_ipv6 (vlib_main_t * vm, vlib_buffer_t *b, ip6_address_t *src,
+               ip6_address_t *dst, int proto)
+{
+  return 0;
+//    struct ip6_hdr *ip6h;
+//    int len;
+//
+//    len = lbuf_size(b);
+//    ip6h = lbuf_push_uninit(b, sizeof(struct ip6_hdr));
+//
+//    ip6h->ip6_hops = 255;
+//    ip6h->ip6_vfc = (IP6VERSION << 4);
+//    ip6h->ip6_nxt = proto;
+//    ip6h->ip6_plen = clib_host_to_net_u16(len);
+//    memcpy(ip6h->ip6_src.s6_addr, src->s6_addr, sizeof(struct in6_addr));
+//    memcpy(ip6h->ip6_dst.s6_addr, dst->s6_addr, sizeof(struct in6_addr));
+//    return(ip6h);
+}
+
+void *
+pkt_push_ip (vlib_main_t * vm, vlib_buffer_t *b, ip_address_t *src,
+             ip_address_t *dst, u32 proto)
+{
+  if (ip_addr_version (src) != ip_addr_version(dst))
+    {
+      clib_warning("src %s and dst %s IP have different AFI! Discarding!",
+                   format_ip_address, src, format_ip_address, dst);
+      return 0;
+    }
+
+  switch (ip_addr_version(src))
+    {
+    case IP4:
+      return pkt_push_ipv4 (vm, b, &ip_addr_v4(src), &ip_addr_v4(dst), proto);
+      break;
+    case IP6:
+      return pkt_push_ipv6 (vm, b, &ip_addr_v6(src), &ip_addr_v6(dst), proto);
+      break;
+    }
+
+  return 0;
+}
+
+void *
+pkt_push_udp_and_ip (vlib_main_t * vm, vlib_buffer_t *b, u16 sp, u16 dp,
+                     ip_address_t *sip, ip_address_t *dip)
+{
+  u16 udpsum;
+  udp_header_t * uh;
+  void * ih;
+
+  uh = pkt_push_udp (vm, b, sp, dp);
+
+  ih = pkt_push_ip (vm, b, sip, dip, IP_PROTOCOL_UDP);
+
+  udpsum = udp_checksum (uh, clib_net_to_host_u16 (uh->length), ih,
+                         ip_addr_version(sip));
+  if (udpsum == -1)
+    {
+      clib_warning("Failed UDP checksum! Discarding");
+      return 0;
+    }
+  uh->checksum = udpsum;
+  return ih;
+}
+
+void *
+pkt_push_ecm_hdr (vlib_buffer_t *b)
+{
+  ecm_hdr_t * h;
+  h = vlib_buffer_push_uninit (b, sizeof (h[0]));
+
+  memset(h, 0, sizeof(h[0]));
+  h->type = LISP_ENCAP_CONTROL_TYPE;
+  memset (h->reserved2, 0, sizeof(h->reserved2));
+
+  return h;
+}
diff --git a/vnet/vnet/lisp-cp/packets.h b/vnet/vnet/lisp-cp/packets.h
new file mode 100644
index 00000000000..a9f9a109958
--- /dev/null
+++ b/vnet/vnet/lisp-cp/packets.h
@@ -0,0 +1,72 @@
+/*
+ * 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.
+ */
+
+#include <vnet/vnet.h>
+#include <vnet/lisp-cp/lisp_types.h>
+
+#define IP_DF 0x4000 /* don't fragment */
+
+void *
+pkt_push_ip (vlib_main_t * vm, vlib_buffer_t *b, ip_address_t *src,
+	     ip_address_t *dst, u32 proto);
+
+void *
+pkt_push_udp_and_ip (vlib_main_t * vm, vlib_buffer_t *b, u16 sp, u16 dp,
+		     ip_address_t *sip, ip_address_t *dip);
+
+void *
+pkt_push_ecm_hdr (vlib_buffer_t *b);
+
+always_inline u8 *
+vlib_buffer_get_tail (vlib_buffer_t *b)
+{
+  return b->data + b->current_data + b->current_length;
+}
+
+always_inline void *
+vlib_buffer_put_uninit (vlib_buffer_t *b, u8 size)
+{
+  /* XXX should make sure there's enough space! */
+  void * p = vlib_buffer_get_tail (b);
+  b->current_length += size;
+  return p;
+}
+
+always_inline void *
+vlib_buffer_push_uninit (vlib_buffer_t *b, u8 size)
+{
+  /* XXX should make sure there's enough space! */
+  ASSERT (b->current_data > size);
+  b->current_data -= size;
+  b->current_length += size;
+
+  return vlib_buffer_get_current(b);
+}
+
+always_inline void *
+vlib_buffer_make_headroom (vlib_buffer_t *b, u8 size)
+{
+  /* XXX should make sure there's enough space! */
+  b->current_data += size;
+  return vlib_buffer_get_current (b);
+}
+
+always_inline void *
+vlib_buffer_pull (vlib_buffer_t * b, u8 size)
+{
+  void * data = vlib_buffer_get_current (b);
+  vlib_buffer_advance (b, size);
+  return data;
+}
diff --git a/vnet/vnet/lisp-gpe/decap.c b/vnet/vnet/lisp-gpe/decap.c
index e10f1f2e399..356dbf2e858 100644
--- a/vnet/vnet/lisp-gpe/decap.c
+++ b/vnet/vnet/lisp-gpe/decap.c
@@ -1,7 +1,5 @@
 /*
- * decap.c: lisp-gpe decap processing
- *
- * Copyright (c) 2014 Cisco and/or its affiliates.
+ * 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:
@@ -19,14 +17,16 @@
 #include <vnet/pg/pg.h>
 #include <vnet/lisp-gpe/lisp_gpe.h>
 
-typedef struct {
+typedef struct
+{
   u32 next_index;
   u32 tunnel_index;
   u32 error;
   lisp_gpe_header_t h;
 } lisp_gpe_rx_trace_t;
 
-static u8 * format_lisp_gpe_rx_trace (u8 * s, va_list * args)
+static u8 *
+format_lisp_gpe_rx_trace (u8 * s, va_list * args)
 {
   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
@@ -34,27 +34,55 @@ static u8 * format_lisp_gpe_rx_trace (u8 * s, va_list * args)
 
   if (t->tunnel_index != ~0)
     {
-      s = format (s, "NSH-VXLAN: tunnel %d next %d error %d", t->tunnel_index, 
-                  t->next_index, t->error);
+      s = format (s, "LISP-GPE: tunnel %d next %d error %d", t->tunnel_index,
+          t->next_index, t->error);
     }
   else
     {
-      s = format (s, "NSH-VXLAN: no tunnel next %d error %d\n", t->next_index, 
-                  t->error);
+      s = format (s, "LISP-GPE: no tunnel next %d error %d\n", t->next_index,
+          t->error);
     }
-  s = format (s, "\n  %U", format_lisp_gpe_header_with_length, &t->h, 
-              (u32) sizeof (t->h) /* max size */);
+  s = format (s, "\n  %U", format_lisp_gpe_header_with_length, &t->h,
+      (u32) sizeof (t->h) /* max size */);
   return s;
 }
 
+static u32
+next_proto_to_next_index[LISP_GPE_NEXT_PROTOS] ={
+    LISP_GPE_INPUT_NEXT_DROP,
+    LISP_GPE_INPUT_NEXT_IP4_INPUT,
+    LISP_GPE_INPUT_NEXT_IP6_INPUT,
+    LISP_GPE_INPUT_NEXT_DROP,
+    LISP_GPE_INPUT_NEXT_DROP
+};
+
+static u32
+next_protocol_to_next_index (lisp_gpe_header_t * lgh, u8 * next_header)
+{
+  /* legay lisp router */
+  if (PREDICT_FALSE((lgh->flags & LISP_GPE_FLAGS_P) == 0))
+    {
+      ip4_header_t * iph = (ip4_header_t *) next_header;
+      if ((iph->ip_version_and_header_length & 0xF0) == 0x40)
+        return LISP_GPE_INPUT_NEXT_IP4_INPUT;
+      else if ((iph->ip_version_and_header_length & 0xF0) == 0x60)
+        return LISP_GPE_INPUT_NEXT_IP6_INPUT;
+      else
+        return LISP_GPE_INPUT_NEXT_DROP;
+    }
+  /* lisp-gpe router */
+  else if ((lgh->flags & LISP_GPE_FLAGS_P)
+      && lgh->next_protocol < LISP_GPE_NEXT_PROTOS)
+    return next_proto_to_next_index[lgh->next_protocol];
+  else
+    return LISP_GPE_INPUT_NEXT_DROP;
+}
+
 static uword
-lisp_gpe_input (vlib_main_t * vm,
-                     vlib_node_runtime_t * node,
-                     vlib_frame_t * from_frame)
+lisp_gpe_input (vlib_main_t * vm, vlib_node_runtime_t * node,
+                vlib_frame_t * from_frame)
 {
   u32 n_left_from, next_index, * from, * to_next;
-  lisp_gpe_main_t * ngm = &lisp_gpe_main;
-  u32 last_tunnel_index = ~0;
   lisp_gpe_tunnel_key_t last_key;
   u32 pkts_decapsulated = 0;
 
@@ -69,182 +97,143 @@ lisp_gpe_input (vlib_main_t * vm,
     {
       u32 n_left_to_next;
 
-      vlib_get_next_frame (vm, node, next_index,
-			   to_next, n_left_to_next);
+      vlib_get_next_frame(vm, node, next_index, to_next, n_left_to_next);
 
-#if 0
       while (n_left_from >= 4 && n_left_to_next >= 2)
-	{
-	  u32 bi0, bi1;
-	  vlib_buffer_t * b0, * b1;
-	  nsh_unicast_header_t * h0, * h1;
-          u32 label0, label1;
-	  u32 next0, next1;
-	  uword * p0, * p1;
-
-	  /* Prefetch next iteration. */
-	  {
-	    vlib_buffer_t * p2, * p3;
-
-	    p2 = vlib_get_buffer (vm, from[2]);
-	    p3 = vlib_get_buffer (vm, from[3]);
-
-	    vlib_prefetch_buffer_header (p2, LOAD);
-	    vlib_prefetch_buffer_header (p3, LOAD);
-
-	    CLIB_PREFETCH (p2->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
-	    CLIB_PREFETCH (p3->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
-	  }
-
-	  bi0 = from[0];
-	  bi1 = from[1];
-	  to_next[0] = bi0;
-	  to_next[1] = bi1;
-	  from += 2;
-	  to_next += 2;
-	  n_left_to_next -= 2;
-	  n_left_from -= 2;
-
-	  b0 = vlib_get_buffer (vm, bi0);
-	  b1 = vlib_get_buffer (vm, bi1);
-
-          h0 = vlib_buffer_get_current (b0);
-          h1 = vlib_buffer_get_current (b1);
-          
+        {
+          u32 bi0, bi1;
+          vlib_buffer_t * b0, * b1;
+          ip4_udp_lisp_gpe_header_t * iul0, * iul1;
+          u32 error0, error1;
+          u32 next0, next1;
+
           next0 = next1 = LISP_GPE_INPUT_NEXT_IP4_INPUT;
 
-          label0 = clib_net_to_host_u32 (h0->label_exp_s_ttl);
-          label1 = clib_net_to_host_u32 (h1->label_exp_s_ttl);
+          /* Prefetch next iteration. */
+          {
+            vlib_buffer_t * p2, * p3;
 
-	  /* 
-	   * Translate label contents into a fib index.
-	   * This is a decent sanity check, and guarantees
-	   * a sane FIB for the downstream lookup
-	   */
-          label0 = vnet_nsh_uc_get_label (label0);
-          label1 = vnet_nsh_uc_get_label (label1);
+            p2 = vlib_get_buffer (vm, from[2]);
+            p3 = vlib_get_buffer (vm, from[3]);
 
-          /* If 2xlabels match, and match the 1-wide cache, use it */
-          if (label0 == label1 && rt->last_label == label0)
-            {
-              vnet_buffer(b0)->sw_if_index[VLIB_TX] = rt->last_fib_index;
-              vnet_buffer(b1)->sw_if_index[VLIB_TX] = rt->last_fib_index;
-            }
-          else
-            {
-              p0 = hash_get (rt->mm->fib_index_by_nsh_label, label0);
-              if (PREDICT_FALSE (p0 == 0))
-                {
-                  next0 = LISP_GPE_INPUT_NEXT_DROP;
-                  b0->error = node->errors[NSH_ERROR_BAD_LABEL];
-                }
-              else
-                vnet_buffer(b0)->sw_if_index[VLIB_TX] = p0[0];
-              
-              p1 = hash_get (rt->mm->fib_index_by_nsh_label, label1);
-              if (PREDICT_FALSE (p1 == 0))
-                {
-                  next1 = LISP_GPE_INPUT_NEXT_DROP;
-                  b1->error = node->errors[NSH_ERROR_BAD_LABEL];
-                }
-              else
-                {
-                  vnet_buffer(b1)->sw_if_index[VLIB_TX] = p1[0];
-                  rt->last_fib_index = p1[0];
-                  rt->last_label = label1;
-                }
-            }
+            vlib_prefetch_buffer_header (p2, LOAD);
+            vlib_prefetch_buffer_header (p3, LOAD);
 
-          if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED)) 
+            CLIB_PREFETCH (p2->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
+            CLIB_PREFETCH (p3->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
+          }
+
+          bi0 = from[0];
+          bi1 = from[1];
+          to_next[0] = bi0;
+          to_next[1] = bi1;
+          from += 2;
+          to_next += 2;
+          n_left_to_next -= 2;
+          n_left_from -= 2;
+
+          b0 = vlib_get_buffer (vm, bi0);
+          b1 = vlib_get_buffer (vm, bi1);
+
+          /* udp leaves current_data pointing at the lisp header */
+          vlib_buffer_advance (b0, - IP_UDP_HDR_LEN);
+          vlib_buffer_advance (b1, - IP_UDP_HDR_LEN);
+
+          iul0 = vlib_buffer_get_current (b0);
+          iul1 = vlib_buffer_get_current (b1);
+
+          /* pop (ip, udp, lisp-gpe) */
+          vlib_buffer_advance (b0, sizeof (*iul0));
+          vlib_buffer_advance (b1, sizeof (*iul1));
+
+          /* determine next_index from lisp-gpe header */
+          next0 = next_protocol_to_next_index (&iul0->lisp,
+                                               vlib_buffer_get_current (b0));
+          next1 = next_protocol_to_next_index (&iul1->lisp,
+                                               vlib_buffer_get_current (b1));
+
+          /* Required to make the l2 tag push / pop code work on l2 subifs */
+          vnet_update_l2_len (b0);
+          vnet_update_l2_len (b1);
+
+          /* TODO hash to map iid to fib */
+          vnet_buffer(b0)->sw_if_index[VLIB_TX] = iul0->lisp.iid;
+          vnet_buffer(b1)->sw_if_index[VLIB_TX] = iul1->lisp.iid;
+
+          pkts_decapsulated += 2;
+
+          /* TODO error handling if security is implemented */
+          error0 = error1 = 0;
+          b0->error = error0 ? node->errors[error0] : 0;
+          b1->error = error1 ? node->errors[error1] : 0;
+
+          if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED))
             {
-              nsh_rx_trace_t *tr = vlib_add_trace (vm, node, 
-                                                   b0, sizeof (*tr));
-              tr->label_exp_s_ttl = label0;
+              lisp_gpe_rx_trace_t *tr = vlib_add_trace (vm, node, b0,
+                                                        sizeof(*tr));
+              tr->next_index = next0;
+              tr->error = error0;
+              tr->h = iul0->lisp;
             }
-          if (PREDICT_FALSE(b1->flags & VLIB_BUFFER_IS_TRACED)) 
+
+          if (PREDICT_FALSE(b1->flags & VLIB_BUFFER_IS_TRACED))
             {
-              nsh_rx_trace_t *tr = vlib_add_trace (vm, node, 
-                                                   b1, sizeof (*tr));
-              tr->label_exp_s_ttl = label1;
+              lisp_gpe_rx_trace_t *tr = vlib_add_trace (vm, node, b1,
+                                                        sizeof(*tr));
+              tr->next_index = next1;
+              tr->error = error1;
+              tr->h = iul1->lisp;
             }
 
-          vlib_buffer_advance (b0, sizeof (*h0));
-          vlib_buffer_advance (b1, sizeof (*h1));
-
-	  vlib_validate_buffer_enqueue_x2 (vm, node, next_index,
-					   to_next, n_left_to_next,
-					   bi0, bi1, next0, next1);
-	}
-#endif 
+          vlib_validate_buffer_enqueue_x2(vm, node, next_index, to_next,
+                                          n_left_to_next, bi0, bi1, next0,
+                                          next1);
+        }
     
       while (n_left_from > 0 && n_left_to_next > 0)
-	{
-	  u32 bi0;
-	  vlib_buffer_t * b0;
-	  u32 next0;
+        {
+          u32 bi0;
+          vlib_buffer_t * b0;
+          u32 next0;
           ip4_udp_lisp_gpe_header_t * iul0;
-	  uword * p0;
-          u32 tunnel_index0;
-          lisp_gpe_tunnel_t * t0;
-          lisp_gpe_tunnel_key_t key0;
           u32 error0;
 
-	  bi0 = from[0];
-	  to_next[0] = bi0;
-	  from += 1;
-	  to_next += 1;
-	  n_left_from -= 1;
-	  n_left_to_next -= 1;
+          bi0 = from[0];
+          to_next[0] = bi0;
+          from += 1;
+          to_next += 1;
+          n_left_from -= 1;
+          n_left_to_next -= 1;
 
-	  b0 = vlib_get_buffer (vm, bi0);
+          b0 = vlib_get_buffer (vm, bi0);
 
           /* udp leaves current_data pointing at the lisp header */
-          vlib_buffer_advance 
-            (b0, -(word)(sizeof(udp_header_t)+sizeof(ip4_header_t)));
+          vlib_buffer_advance (b0, - IP_UDP_HDR_LEN);
 
           iul0 = vlib_buffer_get_current (b0);
 
           /* pop (ip, udp, lisp-gpe) */
           vlib_buffer_advance (b0, sizeof (*iul0));
 
-          tunnel_index0 = ~0;
-          error0 = 0;
-          next0 = LISP_GPE_INPUT_NEXT_DROP;
-
-          key0.src = iul0->ip4.src_address.as_u32;
-          key0.iid = iul0->lisp.iid;
-
-          if (PREDICT_FALSE ((key0.as_u64[0] != last_key.as_u64[0])))
-            {
-              p0 = hash_get_mem (ngm->lisp_gpe_tunnel_by_key, &key0);
-
-              if (p0 == 0)
-                {
-                  error0 = LISP_GPE_ERROR_NO_SUCH_TUNNEL;
-                  goto trace0;
-                }
-
-              last_key.as_u64[0] = key0.as_u64[0];
-              tunnel_index0 = last_tunnel_index = p0[0];
-            }
-          else
-            tunnel_index0 = last_tunnel_index;
-
-          t0 = pool_elt_at_index (ngm->tunnels, tunnel_index0);
+          /* TODO if security is to be implemented, something similar to RPF,
+           * probably we'd like to check that the peer is allowed to send us
+           * packets. For this, we should use the tunnel table OR check that
+           * we have a mapping for the source eid and that the outer source of
+           * the packet is one of its locators */
 
-          next0 = t0->decap_next_index;
+          /* determine next_index from lisp-gpe header */
+          next0 = next_protocol_to_next_index (&iul0->lisp,
+                                               vlib_buffer_get_current (b0));
 
           /* Required to make the l2 tag push / pop code work on l2 subifs */
           vnet_update_l2_len (b0);
 
-          /* 
-           * ip[46] lookup in the configured FIB
-           * lisp-gpe-encap, here's the encap tunnel sw_if_index
-           */
-          vnet_buffer(b0)->sw_if_index[VLIB_TX] = t0->decap_fib_index;
+          /* TODO hash to map iid to fib */
+          vnet_buffer(b0)->sw_if_index[VLIB_TX] = iul0->lisp.iid;
           pkts_decapsulated ++;
 
-        trace0:
+          /* TODO error handling if security is implemented */
+          error0 = 0;
           b0->error = error0 ? node->errors[error0] : 0;
 
           if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED)) 
@@ -253,13 +242,12 @@ lisp_gpe_input (vlib_main_t * vm,
                 = vlib_add_trace (vm, node, b0, sizeof (*tr));
               tr->next_index = next0;
               tr->error = error0;
-              tr->tunnel_index = tunnel_index0;
               tr->h = iul0->lisp;
             }
-	  vlib_validate_buffer_enqueue_x1 (vm, node, next_index,
-					   to_next, n_left_to_next,
-					   bi0, next0);
-	}
+
+          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);
     }
diff --git a/vnet/vnet/lisp-gpe/encap.c b/vnet/vnet/lisp-gpe/encap.c
index b3a52c464be..a8158782d8c 100644
--- a/vnet/vnet/lisp-gpe/encap.c
+++ b/vnet/vnet/lisp-gpe/encap.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015 Cisco and/or its affiliates.
+ * 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:
@@ -12,10 +12,12 @@
  * 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/ip/udp.h>
 #include <vnet/ethernet/ethernet.h>
 #include <vnet/lisp-gpe/lisp_gpe.h>
 
@@ -31,45 +33,41 @@ static char * lisp_gpe_encap_error_strings[] = {
 
 typedef enum {
 #define _(sym,str) LISP_GPE_ENCAP_ERROR_##sym,
-    foreach_lisp_gpe_encap_error
+  foreach_lisp_gpe_encap_error
 #undef _
-    LISP_GPE_ENCAP_N_ERROR,
+  LISP_GPE_ENCAP_N_ERROR,
 } lisp_gpe_encap_error_t;
 
-typedef enum {
-    LISP_GPE_ENCAP_NEXT_IP4_LOOKUP,
-    LISP_GPE_ENCAP_NEXT_DROP,
-    LISP_GPE_ENCAP_N_NEXT,
+typedef enum
+{
+  LISP_GPE_ENCAP_NEXT_DROP,
+  LISP_GPE_ENCAP_NEXT_IP4_LOOKUP,
+  LISP_GPE_ENCAP_N_NEXT,
 } lisp_gpe_encap_next_t;
 
-typedef struct {
+typedef struct
+{
   u32 tunnel_index;
 } lisp_gpe_encap_trace_t;
 
-u8 * format_lisp_gpe_encap_trace (u8 * s, va_list * args)
+u8 *
+format_lisp_gpe_encap_trace (u8 * s, va_list * args)
 {
   CLIB_UNUSED (vlib_main_t * vm) = va_arg (*args, vlib_main_t *);
   CLIB_UNUSED (vlib_node_t * node) = va_arg (*args, vlib_node_t *);
-  lisp_gpe_encap_trace_t * t 
-      = va_arg (*args, lisp_gpe_encap_trace_t *);
+  lisp_gpe_encap_trace_t * t = va_arg (*args, lisp_gpe_encap_trace_t *);
 
   s = format (s, "LISP-GPE-ENCAP: tunnel %d", t->tunnel_index);
   return s;
 }
 
-#define foreach_fixed_header_offset             \
-_(0) _(1) _(2) _(3) 
-
 static uword
-lisp_gpe_encap (vlib_main_t * vm,
-               vlib_node_runtime_t * node,
-               vlib_frame_t * from_frame)
+lisp_gpe_encap (vlib_main_t * vm, vlib_node_runtime_t * node,
+                vlib_frame_t * from_frame)
 {
   u32 n_left_from, next_index, * from, * to_next;
-  lisp_gpe_main_t * ngm = &lisp_gpe_main;
-  vnet_main_t * vnm = ngm->vnet_main;
+  lisp_gpe_main_t * lgm = &lisp_gpe_main;
   u32 pkts_encapsulated = 0;
-  u16 old_l0 = 0;
 
   from = vlib_frame_vector_args (from_frame);
   n_left_from = from_frame->n_vectors;
@@ -81,196 +79,131 @@ lisp_gpe_encap (vlib_main_t * vm,
       u32 n_left_to_next;
 
       vlib_get_next_frame (vm, node, next_index,
-			   to_next, n_left_to_next);
+                           to_next, n_left_to_next);
 
-#if 0
       while (n_left_from >= 4 && n_left_to_next >= 2)
-	{
-	  u32 bi0, bi1;
-	  vlib_buffer_t * b0, * b1;
-	  nsh_unicast_header_t * h0, * h1;
-          u32 label0, label1;
-	  u32 next0, next1;
-	  uword * p0, * p1;
-
-	  /* Prefetch next iteration. */
-	  {
-	    vlib_buffer_t * p2, * p3;
-
-	    p2 = vlib_get_buffer (vm, from[2]);
-	    p3 = vlib_get_buffer (vm, from[3]);
-
-	    vlib_prefetch_buffer_header (p2, LOAD);
-	    vlib_prefetch_buffer_header (p3, LOAD);
-
-	    CLIB_PREFETCH (p2->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
-	    CLIB_PREFETCH (p3->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
-	  }
-
-	  bi0 = from[0];
-	  bi1 = from[1];
-	  to_next[0] = bi0;
-	  to_next[1] = bi1;
-	  from += 2;
-	  to_next += 2;
-	  n_left_to_next -= 2;
-	  n_left_from -= 2;
-
-	  b0 = vlib_get_buffer (vm, bi0);
-	  b1 = vlib_get_buffer (vm, bi1);
-
-          h0 = vlib_buffer_get_current (b0);
-          h1 = vlib_buffer_get_current (b1);
-          
-          next0 = next1 = NSH_INPUT_NEXT_IP4_INPUT;
-
-          label0 = clib_net_to_host_u32 (h0->label_exp_s_ttl);
-          label1 = clib_net_to_host_u32 (h1->label_exp_s_ttl);
-
-	  /* 
-	   * Translate label contents into a fib index.
-	   * This is a decent sanity check, and guarantees
-	   * a sane FIB for the downstream lookup
-	   */
-          label0 = vnet_nsh_uc_get_label (label0);
-          label1 = vnet_nsh_uc_get_label (label1);
-
-          /* If 2xlabels match, and match the 1-wide cache, use it */
-          if (label0 == label1 && rt->last_label == label0)
-            {
-              vnet_buffer(b0)->sw_if_index[VLIB_TX] = rt->last_fib_index;
-              vnet_buffer(b1)->sw_if_index[VLIB_TX] = rt->last_fib_index;
-            }
-          else
+        {
+          u32 bi0, bi1;
+          vlib_buffer_t * b0, * b1;
+          u32 next0, next1;
+          u32 adj_index0, adj_index1, tunnel_index0, tunnel_index1;
+          ip_adjacency_t * adj0, * adj1;
+          lisp_gpe_tunnel_t * t0, * t1;
+
+          next0 = next1 = LISP_GPE_ENCAP_NEXT_IP4_LOOKUP;
+
+          /* Prefetch next iteration. */
             {
-              p0 = hash_get (rt->mm->fib_index_by_nsh_label, label0);
-              if (PREDICT_FALSE (p0 == 0))
-                {
-                  next0 = NSH_INPUT_NEXT_DROP;
-                  b0->error = node->errors[NSH_ERROR_BAD_LABEL];
-                }
-              else
-                vnet_buffer(b0)->sw_if_index[VLIB_TX] = p0[0];
-              
-              p1 = hash_get (rt->mm->fib_index_by_nsh_label, label1);
-              if (PREDICT_FALSE (p1 == 0))
-                {
-                  next1 = NSH_INPUT_NEXT_DROP;
-                  b1->error = node->errors[NSH_ERROR_BAD_LABEL];
-                }
-              else
-                {
-                  vnet_buffer(b1)->sw_if_index[VLIB_TX] = p1[0];
-                  rt->last_fib_index = p1[0];
-                  rt->last_label = label1;
-                }
+              vlib_buffer_t * p2, *p3;
+
+              p2 = vlib_get_buffer (vm, from[2]);
+              p3 = vlib_get_buffer (vm, from[3]);
+
+              vlib_prefetch_buffer_header(p2, LOAD);
+              vlib_prefetch_buffer_header(p3, LOAD);
+
+              CLIB_PREFETCH(p2->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
+              CLIB_PREFETCH(p3->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
             }
 
-          if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED)) 
+          bi0 = from[0];
+          bi1 = from[1];
+          to_next[0] = bi0;
+          to_next[1] = bi1;
+          from += 2;
+          to_next += 2;
+          n_left_to_next -= 2;
+          n_left_from -= 2;
+
+          b0 = vlib_get_buffer (vm, bi0);
+          b1 = vlib_get_buffer (vm, bi1);
+
+          /* Get adjacency and from it the tunnel_index */
+          adj_index0 = vnet_buffer(b0)->ip.adj_index[VLIB_TX];
+          adj_index1 = vnet_buffer(b1)->ip.adj_index[VLIB_TX];
+
+          adj0 = ip_get_adjacency (lgm->lookup_main, adj_index0);
+          adj1 = ip_get_adjacency (lgm->lookup_main, adj_index1);
+
+          tunnel_index0 = adj0->rewrite_header.sw_if_index;
+          tunnel_index1 = adj1->rewrite_header.sw_if_index;
+
+          t0 = pool_elt_at_index (lgm->tunnels, tunnel_index0);
+          t1 = pool_elt_at_index (lgm->tunnels, tunnel_index1);
+
+          ASSERT(t0 != 0);
+          ASSERT(t1 != 0);
+
+          ASSERT (sizeof(ip4_udp_lisp_gpe_header_t) == 36);
+          ip4_udp_encap_two (vm, b0, b1, t0->rewrite, t1->rewrite, 36);
+
+          /* Reset to look up tunnel partner in the configured FIB */
+          vnet_buffer(b0)->sw_if_index[VLIB_TX] = t0->encap_fib_index;
+          vnet_buffer(b1)->sw_if_index[VLIB_TX] = t1->encap_fib_index;
+
+          if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED))
             {
-              nsh_rx_trace_t *tr = vlib_add_trace (vm, node, 
-                                                   b0, sizeof (*tr));
-              tr->label_exp_s_ttl = label0;
+              lisp_gpe_encap_trace_t *tr = vlib_add_trace (vm, node, b0,
+                                                           sizeof(*tr));
+              tr->tunnel_index = t0 - lgm->tunnels;
             }
-          if (PREDICT_FALSE(b1->flags & VLIB_BUFFER_IS_TRACED)) 
+          if (PREDICT_FALSE(b1->flags & VLIB_BUFFER_IS_TRACED))
             {
-              nsh_rx_trace_t *tr = vlib_add_trace (vm, node, 
-                                                   b1, sizeof (*tr));
-              tr->label_exp_s_ttl = label1;
+              lisp_gpe_encap_trace_t *tr = vlib_add_trace (vm, node, b1,
+                                                           sizeof(*tr));
+              tr->tunnel_index = t1 - lgm->tunnels;
             }
 
-          vlib_buffer_advance (b0, sizeof (*h0));
-          vlib_buffer_advance (b1, sizeof (*h1));
+          pkts_encapsulated += 2;
+
+          vlib_validate_buffer_enqueue_x2(vm, node, next_index, to_next,
+                                          n_left_to_next, bi0, bi1, next0,
+                                          next1);
+        }
 
-	  vlib_validate_buffer_enqueue_x2 (vm, node, next_index,
-					   to_next, n_left_to_next,
-					   bi0, bi1, next0, next1);
-	}
-#endif 
-    
       while (n_left_from > 0 && n_left_to_next > 0)
-	{
-	  u32 bi0;
-	  vlib_buffer_t * b0;
-	  u32 next0 = LISP_GPE_ENCAP_NEXT_IP4_LOOKUP;
-          vnet_hw_interface_t * hi0;
-          ip4_header_t * ip0;
-          udp_header_t * udp0;
-          u64 * copy_src0, * copy_dst0;
-          u32 * copy_src_last0, * copy_dst_last0;
-          lisp_gpe_tunnel_t * t0;
-          u16 new_l0;
-          ip_csum_t sum0;
-
-	  bi0 = from[0];
-	  to_next[0] = bi0;
-	  from += 1;
-	  to_next += 1;
-	  n_left_from -= 1;
-	  n_left_to_next -= 1;
-
-	  b0 = vlib_get_buffer (vm, bi0);
-
-          /* 1-wide cache? */
-          hi0 = vnet_get_sup_hw_interface 
-            (vnm, vnet_buffer(b0)->sw_if_index[VLIB_TX]);
-
-          t0 = pool_elt_at_index (ngm->tunnels, hi0->dev_instance);
-
-          ASSERT(vec_len(t0->rewrite) >= 24);
-
-          /* Apply the rewrite string. $$$$ vnet_rewrite? */
-          vlib_buffer_advance (b0, -(word)_vec_len(t0->rewrite));
-
-          ip0 = vlib_buffer_get_current(b0);
-          /* Copy the fixed header */
-          copy_dst0 = (u64 *) ip0;
-          copy_src0 = (u64 *) t0->rewrite;
-
-          ASSERT (sizeof (ip4_udp_lisp_gpe_header_t) == 36);
-
-          /* Copy first 32 octets 8-bytes at a time */
-#define _(offs) copy_dst0[offs] = copy_src0[offs];
-          foreach_fixed_header_offset;
-#undef _
-          /* Last 4 octets. Hopefully gcc will be our friend */
-          copy_dst_last0 = (u32 *)(&copy_dst0[4]);
-          copy_src_last0 = (u32 *)(&copy_src0[4]);
-          
-          copy_dst_last0[0] = copy_src_last0[0];
-
-          /* fix the <bleep>ing outer-IP checksum */
-          sum0 = ip0->checksum;
-          /* old_l0 always 0, see the rewrite setup */
-          new_l0 = 
-            clib_host_to_net_u16 (vlib_buffer_length_in_chain (vm, b0));
-          
-          sum0 = ip_csum_update (sum0, old_l0, new_l0, ip4_header_t,
-                                 length /* changed member */);
-          ip0->checksum = ip_csum_fold (sum0);
-          ip0->length = new_l0;
-          
-          /* Fix UDP length */
-          udp0 = (udp_header_t *)(ip0+1);
-          new_l0 = clib_host_to_net_u16 (vlib_buffer_length_in_chain (vm, b0)
-                                         - sizeof (*ip0));
-          
-          udp0->length = new_l0;
+        {
+          vlib_buffer_t * b0;
+          u32 bi0, adj_index0, tunnel_index0;
+          u32 next0 = LISP_GPE_ENCAP_NEXT_IP4_LOOKUP;
+          lisp_gpe_tunnel_t * t0 = 0;
+          ip_adjacency_t * adj0;
+
+          bi0 = from[0];
+          to_next[0] = bi0;
+          from += 1;
+          to_next += 1;
+          n_left_from -= 1;
+          n_left_to_next -= 1;
+
+          b0 = vlib_get_buffer (vm, bi0);
+
+          /* Get adjacency and from it the tunnel_index */
+          adj_index0 = vnet_buffer(b0)->ip.adj_index[VLIB_TX];
+          adj0 = ip_get_adjacency (lgm->lookup_main, adj_index0);
+
+          tunnel_index0 = adj0->rewrite_header.sw_if_index;
+          t0 = pool_elt_at_index (lgm->tunnels, tunnel_index0);
+
+          ASSERT(t0 != 0);
+
+          ASSERT (sizeof(ip4_udp_lisp_gpe_header_t) == 36);
+          ip4_udp_encap_one (vm, b0, t0->rewrite, 36);
 
           /* Reset to look up tunnel partner in the configured FIB */
           vnet_buffer(b0)->sw_if_index[VLIB_TX] = t0->encap_fib_index;
-          pkts_encapsulated ++;
+
+          pkts_encapsulated++;
 
           if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED)) 
             {
-              lisp_gpe_encap_trace_t *tr = 
-                vlib_add_trace (vm, node, b0, sizeof (*tr));
-              tr->tunnel_index = t0 - ngm->tunnels;
+              lisp_gpe_encap_trace_t *tr = vlib_add_trace (vm, node, b0,
+                                                           sizeof(*tr));
+              tr->tunnel_index = t0 - lgm->tunnels;
             }
-	  vlib_validate_buffer_enqueue_x1 (vm, node, next_index,
-					   to_next, n_left_to_next,
-					   bi0, next0);
-	}
+          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);
     }
@@ -293,7 +226,7 @@ VLIB_REGISTER_NODE (lisp_gpe_encap_node) = {
   .n_next_nodes = LISP_GPE_ENCAP_N_NEXT,
 
   .next_nodes = {
-        [LISP_GPE_ENCAP_NEXT_IP4_LOOKUP] = "ip4-lookup",
-        [LISP_GPE_ENCAP_NEXT_DROP] = "error-drop",
+      [LISP_GPE_ENCAP_NEXT_DROP] = "error-drop",
+      [LISP_GPE_ENCAP_NEXT_IP4_LOOKUP] = "ip4-lookup",
   },
 };
diff --git a/vnet/vnet/lisp-gpe/lisp_gpe.c b/vnet/vnet/lisp-gpe/lisp_gpe.c
index eb4ca919b20..b8072494c2c 100644
--- a/vnet/vnet/lisp-gpe/lisp_gpe.c
+++ b/vnet/vnet/lisp-gpe/lisp_gpe.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015 Cisco and/or its affiliates.
+ * 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:
@@ -12,129 +12,551 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
+
 #include <vnet/lisp-gpe/lisp_gpe.h>
 
 lisp_gpe_main_t lisp_gpe_main;
 
-static u8 * format_decap_next (u8 * s, va_list * args)
+/* avoids calling route callbacks for src fib */
+static void
+ip4_sd_fib_set_adj_index (lisp_gpe_main_t * lgm, ip4_fib_t * fib, u32 flags,
+                           u32 dst_address_u32, u32 dst_address_length,
+                           u32 adj_index)
 {
-  u32 next_index = va_arg (*args, u32);
+  ip_lookup_main_t * lm = lgm->lookup_main;
+  uword * hash;
 
-  switch (next_index)
+  if (vec_bytes(fib->old_hash_values))
+    memset (fib->old_hash_values, ~0, vec_bytes (fib->old_hash_values));
+  if (vec_bytes(fib->new_hash_values))
+    memset (fib->new_hash_values, ~0, vec_bytes (fib->new_hash_values));
+  fib->new_hash_values[0] = adj_index;
+
+  /* Make sure adj index is valid. */
+  if (CLIB_DEBUG > 0)
+    (void) ip_get_adjacency (lm, adj_index);
+
+  hash = fib->adj_index_by_dst_address[dst_address_length];
+
+  hash = _hash_set3 (hash, dst_address_u32,
+                     fib->new_hash_values,
+                     fib->old_hash_values);
+
+  fib->adj_index_by_dst_address[dst_address_length] = hash;
+}
+
+/* copied from ip4_forward since it's static */
+static void
+ip4_fib_init_adj_index_by_dst_address (ip_lookup_main_t * lm,
+                                       ip4_fib_t * fib,
+                                       u32 address_length)
+{
+  hash_t * h;
+  uword max_index;
+
+  ASSERT (lm->fib_result_n_bytes >= sizeof (uword));
+  lm->fib_result_n_words = round_pow2 (lm->fib_result_n_bytes, sizeof (uword)) / sizeof (uword);
+
+  fib->adj_index_by_dst_address[address_length] =
+    hash_create (32 /* elts */, lm->fib_result_n_words * sizeof (uword));
+
+  hash_set_flags (fib->adj_index_by_dst_address[address_length],
+                  HASH_FLAG_NO_AUTO_SHRINK);
+
+  h = hash_header (fib->adj_index_by_dst_address[address_length]);
+  max_index = (hash_value_bytes (h) / sizeof (fib->new_hash_values[0])) - 1;
+
+  /* Initialize new/old hash value vectors. */
+  vec_validate_init_empty (fib->new_hash_values, max_index, ~0);
+  vec_validate_init_empty (fib->old_hash_values, max_index, ~0);
+}
+
+void
+ip4_sd_fib_add_del_src_route (lisp_gpe_main_t * lgm,
+                              ip4_add_del_route_args_t * a)
+{
+  ip_lookup_main_t * lm = lgm->lookup_main;
+  ip4_fib_t * fib;
+  u32 dst_address, dst_address_length, adj_index, old_adj_index;
+  uword * hash, is_del;
+
+  /* Either create new adjacency or use given one depending on arguments. */
+  if (a->n_add_adj > 0)
+      ip_add_adjacency (lm, a->add_adj, a->n_add_adj, &adj_index);
+  else
+    adj_index = a->adj_index;
+
+  dst_address = a->dst_address.data_u32;
+  dst_address_length = a->dst_address_length;
+
+  fib = pool_elt_at_index(lgm->src_fibs, a->table_index_or_table_id);
+
+  if (! fib->adj_index_by_dst_address[dst_address_length])
+    ip4_fib_init_adj_index_by_dst_address (lm, fib, dst_address_length);
+
+  hash = fib->adj_index_by_dst_address[dst_address_length];
+
+  is_del = (a->flags & IP4_ROUTE_FLAG_DEL) != 0;
+
+  if (is_del)
     {
-    case LISP_GPE_INPUT_NEXT_DROP:
-      return format (s, "drop");
-    case LISP_GPE_INPUT_NEXT_IP4_INPUT:
-      return format (s, "ip4");
-    case LISP_GPE_INPUT_NEXT_IP6_INPUT:
-      return format (s, "ip6");
-    case LISP_GPE_INPUT_NEXT_LISP_GPE_ENCAP:
-      return format (s, "nsh-lisp-gpe");
-    default:
-      return format (s, "unknown %d", next_index);
+      fib->old_hash_values[0] = ~0;
+      hash = _hash_unset (hash, dst_address, fib->old_hash_values);
+      fib->adj_index_by_dst_address[dst_address_length] = hash;
     }
-  return s;
+  else
+    ip4_sd_fib_set_adj_index (lgm, fib, a->flags, dst_address,
+                              dst_address_length, adj_index);
+
+  old_adj_index = fib->old_hash_values[0];
+
+  ip4_fib_mtrie_add_del_route (fib, a->dst_address, dst_address_length,
+                               is_del ? old_adj_index : adj_index,
+                               is_del);
+
+  /* Delete old adjacency index if present and changed. */
+  if (! (a->flags & IP4_ROUTE_FLAG_KEEP_OLD_ADJACENCY)
+      && old_adj_index != ~0
+      && old_adj_index != adj_index)
+    ip_del_adjacency (lm, old_adj_index);
 }
 
-u8 * format_lisp_gpe_tunnel (u8 * s, va_list * args)
+void *
+ip4_sd_get_src_route (lisp_gpe_main_t * lgm, u32 src_fib_index,
+                      ip4_address_t * src, u32 address_length)
 {
-  lisp_gpe_tunnel_t * t = va_arg (*args, lisp_gpe_tunnel_t *);
-  lisp_gpe_main_t * ngm = &lisp_gpe_main;
+  ip4_fib_t * fib = pool_elt_at_index (lgm->src_fibs, src_fib_index);
+  uword * hash, * p;
 
-  s = format (s, 
-              "[%d] %U (src) %U (dst) fibs: encap %d, decap %d",
-              t - ngm->tunnels,
-              format_ip4_address, &t->src,
-              format_ip4_address, &t->dst,
-              t->encap_fib_index,
-              t->decap_fib_index);
+  hash = fib->adj_index_by_dst_address[address_length];
+  p = hash_get (hash, src->as_u32);
+  return (void *) p;
+}
 
-  s = format (s, " decap next %U\n", format_decap_next, t->decap_next_index);
-  s = format (s, "lisp ver %d ", (t->ver_res>>6));
+typedef CLIB_PACKED (struct {
+  ip4_address_t address;
+  u32 address_length : 6;
+  u32 index : 26;
+}) ip4_route_t;
 
-#define _(n,v) if (t->flags & v) s = format (s, "%s-bit ", #n);
-  foreach_lisp_gpe_flag_bit;
-#undef _
+static void
+ip4_sd_fib_clear_src_fib (lisp_gpe_main_t * lgm, ip4_fib_t * fib)
+{
+  ip4_route_t * routes = 0, * r;
+  u32 i;
 
-  s = format (s, "next_protocol %d ver_res %x res %x\n",
-              t->next_protocol, t->ver_res, t->res);
-  
-  s = format (s, "iid %d (0x%x)\n", t->iid, t->iid);
-  return s;
+  vec_reset_length (routes);
+
+  for (i = 0; i < ARRAY_LEN (fib->adj_index_by_dst_address); i++) {
+      uword * hash = fib->adj_index_by_dst_address[i];
+      hash_pair_t * p;
+      ip4_route_t x;
+
+      x.address_length = i;
+
+      hash_foreach_pair (p, hash,
+      ({
+          x.address.data_u32 = p->key;
+          vec_add1 (routes, x);
+      }));
+  }
+
+  vec_foreach (r, routes) {
+      ip4_add_del_route_args_t a;
+
+      memset (&a, 0, sizeof (a));
+      a.flags = IP4_ROUTE_FLAG_FIB_INDEX | IP4_ROUTE_FLAG_DEL;
+      a.table_index_or_table_id = fib - lgm->src_fibs;
+      a.dst_address = r->address;
+      a.dst_address_length = r->address_length;
+      a.adj_index = ~0;
+
+      ip4_sd_fib_add_del_src_route (lgm, &a);
+  }
 }
 
-static u8 * format_lisp_gpe_name (u8 * s, va_list * args)
+int
+ip4_sd_fib_add_del_route (lisp_gpe_main_t * lgm, ip_prefix_t * dst_prefix,
+                          ip_prefix_t * src_prefix, u32 table_index,
+                          ip_adjacency_t * add_adj, u8 is_add)
 {
-  u32 dev_instance = va_arg (*args, u32);
-  return format (s, "lisp_gpe_tunnel%d", dev_instance);
+  uword * p;
+  ip4_add_del_route_args_t a;
+  ip_adjacency_t * dst_adjp, dst_adj;
+  ip4_address_t dst = ip_prefix_v4(dst_prefix), src;
+  u32 dst_address_length = ip_prefix_len(dst_prefix), src_address_length = 0;
+  ip4_fib_t * src_fib;
+
+  if (src_prefix)
+    {
+      src = ip_prefix_v4(src_prefix);
+      src_address_length = ip_prefix_len(src_prefix);
+    }
+  else
+    memset(&src, 0, sizeof(src));
+
+  /* lookup dst adj */
+  p = ip4_get_route (lgm->im4, table_index, 0, dst.as_u8, dst_address_length);
+
+  if (is_add)
+    {
+      /* insert dst prefix to ip4 fib, if it's not in yet */
+      if (p == 0)
+        {
+          /* dst adj should point to lisp gpe lookup */
+          dst_adj = add_adj[0];
+          dst_adj.lookup_next_index = lgm->ip4_lookup_next_lgpe_ip4_lookup;
+
+          memset(&a, 0, sizeof(a));
+          a.flags = IP4_ROUTE_FLAG_TABLE_ID;
+          a.table_index_or_table_id = table_index; /* vrf */
+          a.adj_index = ~0;
+          a.dst_address_length = dst_address_length;
+          a.dst_address = dst;
+          a.flags |= IP4_ROUTE_FLAG_ADD;
+          a.add_adj = &dst_adj;
+          a.n_add_adj = 1;
+
+          ip4_add_del_route (lgm->im4, &a);
+
+          /* lookup dst adj to obtain the adj index */
+          p = ip4_get_route (lgm->im4, table_index, 0, dst.as_u8,
+                             dst_address_length);
+          if (p == 0)
+            {
+              clib_warning("Failed to insert dst route for eid %U!",
+                           format_ip4_address_and_length, dst.as_u8,
+                           dst_address_length);
+              return -1;
+            }
+
+          /* allocate and init src ip4 fib */
+          pool_get(lgm->src_fibs, src_fib);
+          ip4_mtrie_init (&src_fib->mtrie);
+
+          /* reuse rewrite header to store pointer to src fib */
+          dst_adjp = ip_get_adjacency (lgm->lookup_main, p[0]);
+          dst_adjp->rewrite_header.sw_if_index = src_fib - lgm->src_fibs;
+        }
+    }
+  else
+    {
+      if (p == 0)
+        {
+          clib_warning("Trying to delete inexistent dst route for %U. Aborting",
+                       format_ip4_address_and_length, dst.as_u8,
+                       dst_address_length);
+          return -1;
+        }
+    }
+
+  dst_adjp = ip_get_adjacency (lgm->lookup_main, p[0]);
+
+  /* add/del src prefix to src fib */
+  memset(&a, 0, sizeof(a));
+  a.flags = IP4_ROUTE_FLAG_TABLE_ID;
+  a.table_index_or_table_id = dst_adjp->rewrite_header.sw_if_index;
+  a.adj_index = ~0;
+  a.flags |= is_add ? IP4_ROUTE_FLAG_ADD : IP4_ROUTE_FLAG_DEL;
+  a.add_adj = add_adj;
+  a.n_add_adj = 1;
+  /* if src prefix is null, add 0/0 */
+  a.dst_address_length = src_address_length;
+  a.dst_address = src;
+  ip4_sd_fib_add_del_src_route (lgm, &a);
+
+  /* if a delete, check if there are elements left in the src fib */
+  if (!is_add)
+    {
+      src_fib = pool_elt_at_index(lgm->src_fibs,
+                                  dst_adjp->rewrite_header.sw_if_index);
+      if (!src_fib)
+        return 0;
+
+      /* if there's nothing left, clear src fib .. */
+      if (ARRAY_LEN(src_fib->adj_index_by_dst_address) == 0)
+        {
+          ip4_sd_fib_clear_src_fib (lgm, src_fib);
+          pool_put(lgm->src_fibs, src_fib);
+        }
+
+      /* .. and remove dst route */
+      memset(&a, 0, sizeof(a));
+      a.flags = IP4_ROUTE_FLAG_TABLE_ID;
+      a.table_index_or_table_id = table_index; /* vrf */
+      a.adj_index = ~0;
+      a.dst_address_length = dst_address_length;
+      a.dst_address = dst;
+      a.flags |= IP4_ROUTE_FLAG_DEL;
+
+      ip4_add_del_route (lgm->im4, &a);
+    }
+
+  return 0;
 }
 
-static uword dummy_interface_tx (vlib_main_t * vm,
-                                 vlib_node_runtime_t * node,
-                                 vlib_frame_t * frame)
+static void *
+ip4_sd_fib_get_route (lisp_gpe_main_t * lgm, ip_prefix_t * dst_prefix,
+                      ip_prefix_t * src_prefix, u32 table_index)
 {
-  clib_warning ("you shouldn't be here, leaking buffers...");
-  return frame->n_vectors;
+  uword * p;
+  ip4_address_t dst = ip_prefix_v4(dst_prefix), src;
+  u32 dst_address_length = ip_prefix_len(dst_prefix), src_address_length = 0;
+  ip_adjacency_t * dst_adj;
+
+  if (src_prefix)
+    {
+      src = ip_prefix_v4(src_prefix);
+      src_address_length = ip_prefix_len(src_prefix);
+    }
+  else
+    memset(&src, 0, sizeof(src));
+
+  /* lookup dst adj */
+  p = ip4_get_route (lgm->im4, table_index, 0, dst.as_u8, dst_address_length);
+  if (p == 0)
+      return p;
+
+  dst_adj = ip_get_adjacency (lgm->lookup_main, p[0]);
+  return ip4_sd_get_src_route (lgm, dst_adj->rewrite_header.sw_if_index, &src,
+                               src_address_length);
 }
 
-VNET_DEVICE_CLASS (lisp_gpe_device_class,static) = {
-  .name = "LISP_GPE",
-  .format_device_name = format_lisp_gpe_name,
-  .format_tx_trace = format_lisp_gpe_encap_trace,
-  .tx_function = dummy_interface_tx,
-};
+typedef enum
+{
+  LGPE_IP4_LOOKUP_NEXT_DROP,
+  LGPE_IP4_LOOKUP_NEXT_LISP_CP_LOOKUP,
+  LGPE_IP4_LOOKUP_NEXT_LGPE_ENCAP,
+  LGPE_IP4_LOOKUP_N_NEXT,
+} lgpe_ip4_lookup_next_t;
 
-static uword dummy_set_rewrite (vnet_main_t * vnm,
-                                u32 sw_if_index,
-                                u32 l3_type,
-                                void * dst_address,
-                                void * rewrite,
-                                uword max_rewrite_bytes)
+always_inline void
+ip4_src_fib_lookup_one (lisp_gpe_main_t * lgm, u32 src_fib_index0,
+                        ip4_address_t * addr0, u32 * src_adj_index0)
 {
-  return 0;
+  ip4_fib_mtrie_leaf_t leaf0, leaf1;
+  ip4_fib_mtrie_t * mtrie0;
+
+  mtrie0 = &vec_elt_at_index(lgm->src_fibs, src_fib_index0)->mtrie;
+
+  leaf0 = leaf1 = IP4_FIB_MTRIE_LEAF_ROOT;
+  leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 0);
+  leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 1);
+  leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 2);
+  leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 3);
+
+  /* Handle default route. */
+  leaf0 = (leaf0 == IP4_FIB_MTRIE_LEAF_EMPTY ? mtrie0->default_leaf : leaf0);
+  src_adj_index0[0] = ip4_fib_mtrie_leaf_get_adj_index (leaf0);
 }
 
-u8 * format_lisp_gpe_header_with_length (u8 * s, va_list * args)
+always_inline void
+ip4_src_fib_lookup_two (lisp_gpe_main_t * lgm, u32 src_fib_index0,
+                        u32 src_fib_index1, ip4_address_t * addr0,
+                        ip4_address_t * addr1, u32 * src_adj_index0,
+                        u32 * src_adj_index1)
 {
-  lisp_gpe_header_t * h = va_arg (*args, lisp_gpe_header_t *);
-  u32 max_header_bytes = va_arg (*args, u32);
-  u32 header_bytes;
+  ip4_fib_mtrie_leaf_t leaf0, leaf1;
+  ip4_fib_mtrie_t * mtrie0, * mtrie1;
 
-  header_bytes = sizeof (h[0]);
-  if (max_header_bytes != 0 && header_bytes > max_header_bytes)
-    return format (s, "gre-nsh header truncated");
+  mtrie0 = &vec_elt_at_index(lgm->src_fibs, src_fib_index0)->mtrie;
+  mtrie1 = &vec_elt_at_index(lgm->src_fibs, src_fib_index1)->mtrie;
 
-  s = format (s, "flags: ");
-#define _(n,v) if (h->flags & v) s = format (s, "%s ", #n);
-  foreach_lisp_gpe_flag_bit;
-#undef _
+  leaf0 = leaf1 = IP4_FIB_MTRIE_LEAF_ROOT;
 
-  s = format (s, "\n  ver_res %d res %d next_protocol %d iid %d(%x)",
-              h->ver_res, h->res, h->next_protocol, 
-              clib_net_to_host_u32 (h->iid),
-              clib_net_to_host_u32 (h->iid));
-  return s;
+  leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 0);
+  leaf1 = ip4_fib_mtrie_lookup_step (mtrie1, leaf1, addr1, 0);
+
+  leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 1);
+  leaf1 = ip4_fib_mtrie_lookup_step (mtrie1, leaf1, addr1, 1);
+
+  leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 2);
+  leaf1 = ip4_fib_mtrie_lookup_step (mtrie1, leaf1, addr1, 2);
+
+  leaf0 = ip4_fib_mtrie_lookup_step (mtrie0, leaf0, addr0, 3);
+  leaf1 = ip4_fib_mtrie_lookup_step (mtrie1, leaf1, addr1, 3);
+
+  /* Handle default route. */
+  leaf0 = (leaf0 == IP4_FIB_MTRIE_LEAF_EMPTY ? mtrie0->default_leaf : leaf0);
+  leaf1 = (leaf1 == IP4_FIB_MTRIE_LEAF_EMPTY ? mtrie1->default_leaf : leaf1);
+  src_adj_index0[0] = ip4_fib_mtrie_leaf_get_adj_index (leaf0);
+  src_adj_index1[0] = ip4_fib_mtrie_leaf_get_adj_index (leaf1);
 }
 
-VNET_HW_INTERFACE_CLASS (lisp_gpe_hw_class) = {
-  .name = "LISP_GPE",
-  .format_header = format_lisp_gpe_header_with_length,
-  .set_rewrite = dummy_set_rewrite,
-};
+always_inline uword
+lgpe_ip4_lookup (vlib_main_t * vm, vlib_node_runtime_t * node,
+                 vlib_frame_t * from_frame)
+{
+  u32 n_left_from, next_index, * from, * to_next;
+  lisp_gpe_main_t * lgm = &lisp_gpe_main;
 
-#define foreach_copy_field                      \
-_(src.as_u32)                                   \
-_(dst.as_u32)                                   \
-_(encap_fib_index)                              \
-_(decap_fib_index)                              \
-_(decap_next_index)                             \
-_(flags)                                        \
-_(next_protocol)				\
-_(ver_res)                                      \
-_(res)                                          \
-_(iid)
+  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 >= 4 && n_left_to_next >= 2)
+        {
+          u32 bi0, bi1;
+          vlib_buffer_t * b0, * b1;
+          ip4_header_t * ip0, * ip1;
+          u32 dst_adj_index0, src_adj_index0, src_fib_index0, dst_adj_index1,
+              src_adj_index1, src_fib_index1;
+          ip_adjacency_t * dst_adj0, * src_adj0, * dst_adj1, * src_adj1;
+          u32 next0, next1;
+
+          next0 = next1 = LGPE_IP4_LOOKUP_NEXT_LISP_CP_LOOKUP;
 
-static int lisp_gpe_rewrite (lisp_gpe_tunnel_t * t)
+          /* Prefetch next iteration. */
+          {
+            vlib_buffer_t * p2, * p3;
+
+            p2 = vlib_get_buffer (vm, from[2]);
+            p3 = vlib_get_buffer (vm, from[3]);
+
+            vlib_prefetch_buffer_header (p2, LOAD);
+            vlib_prefetch_buffer_header (p3, LOAD);
+
+            CLIB_PREFETCH (p2->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
+            CLIB_PREFETCH (p3->data, 2*CLIB_CACHE_LINE_BYTES, LOAD);
+          }
+
+          bi0 = from[0];
+          bi1 = from[1];
+          to_next[0] = bi0;
+          to_next[1] = bi1;
+          from += 2;
+          to_next += 2;
+          n_left_to_next -= 2;
+          n_left_from -= 2;
+
+          b0 = vlib_get_buffer (vm, bi0);
+          b1 = vlib_get_buffer (vm, bi1);
+
+          ip0 = vlib_buffer_get_current (b0);
+          ip1 = vlib_buffer_get_current (b1);
+
+          /* dst lookup was done by ip4 lookup */
+          dst_adj_index0 = vnet_buffer(b0)->ip.adj_index[VLIB_TX];
+          dst_adj_index1 = vnet_buffer(b1)->ip.adj_index[VLIB_TX];
+
+          dst_adj0 = ip_get_adjacency (lgm->lookup_main, dst_adj_index0);
+          dst_adj1 = ip_get_adjacency (lgm->lookup_main, dst_adj_index1);
+
+          src_fib_index0 = dst_adj0->rewrite_header.sw_if_index;
+          src_fib_index1 = dst_adj1->rewrite_header.sw_if_index;
+
+          /* if default route not hit in ip4 lookup */
+          if (PREDICT_TRUE(src_fib_index0 != (u32) ~0
+                           && src_fib_index1 != (u32) ~0))
+            {
+              ip4_src_fib_lookup_two (lgm, src_fib_index0, src_fib_index1,
+                                      &ip0->src_address, &ip1->src_address,
+                                      &src_adj_index0, &src_adj_index1);
+
+              vnet_buffer(b0)->ip.adj_index[VLIB_TX] = src_adj_index0;
+              vnet_buffer(b1)->ip.adj_index[VLIB_TX] = src_adj_index1;
+
+              src_adj0 = ip_get_adjacency (lgm->lookup_main, src_adj_index0);
+              src_adj1 = ip_get_adjacency (lgm->lookup_main, src_adj_index1);
+
+              next0 = src_adj0->lookup_next_index;
+              next1 = src_adj1->lookup_next_index;
+            }
+          else
+            {
+              if (src_fib_index0 != (u32) ~0)
+                {
+                  ip4_src_fib_lookup_one (lgm, src_fib_index0,
+                                          &ip0->src_address, &src_adj_index0);
+                  vnet_buffer(b0)->ip.adj_index[VLIB_TX] = src_adj_index0;
+                  src_adj0 = ip_get_adjacency (lgm->lookup_main,
+                                               src_adj_index0);
+                  next0 = src_adj0->lookup_next_index;
+                }
+              if (src_fib_index1 != (u32) ~0)
+                {
+                  ip4_src_fib_lookup_one (lgm, src_fib_index1,
+                                          &ip1->src_address, &src_adj_index1);
+                  vnet_buffer(b1)->ip.adj_index[VLIB_TX] = src_adj_index1;
+                  src_adj1 = ip_get_adjacency (lgm->lookup_main,
+                                               src_adj_index1);
+                  next1 = src_adj1->lookup_next_index;
+                }
+            }
+
+          vlib_validate_buffer_enqueue_x2(vm, node, next_index, to_next,
+                                          n_left_to_next, bi0, bi1, next0,
+                                          next1);
+        }
+
+      while (n_left_from > 0 && n_left_to_next > 0)
+        {
+          vlib_buffer_t * b0;
+          ip4_header_t * ip0;
+          u32 bi0, dst_adj_index0, src_adj_index0, src_fib_index0;
+          u32 next0 = LGPE_IP4_LOOKUP_NEXT_LISP_CP_LOOKUP;
+          ip_adjacency_t * dst_adj0, * src_adj0;
+
+          bi0 = from[0];
+          to_next[0] = bi0;
+          from += 1;
+          to_next += 1;
+          n_left_from -= 1;
+          n_left_to_next -= 1;
+
+          b0 = vlib_get_buffer (vm, bi0);
+          ip0 = vlib_buffer_get_current (b0);
+
+          /* dst lookup was done by ip4 lookup */
+          dst_adj_index0 = vnet_buffer(b0)->ip.adj_index[VLIB_TX];
+          dst_adj0 = ip_get_adjacency (lgm->lookup_main, dst_adj_index0);
+          src_fib_index0 = dst_adj0->rewrite_header.sw_if_index;
+
+          /* default route hit in ip4 lookup, send to lisp control plane */
+          if (src_fib_index0 == (u32) ~0)
+            goto done;
+
+          /* src lookup we do here */
+          ip4_src_fib_lookup_one (lgm, src_fib_index0, &ip0->src_address,
+                                  &src_adj_index0);
+          vnet_buffer(b0)->ip.adj_index[VLIB_TX] = src_adj_index0;
+          src_adj0 = ip_get_adjacency (lgm->lookup_main, src_adj_index0);
+          next0 = src_adj0->lookup_next_index;
+
+        done:
+          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;
+}
+
+
+VLIB_REGISTER_NODE (lgpe_ip4_lookup_node) = {
+  .function = lgpe_ip4_lookup,
+  .name = "lgpe-ip4-lookup",
+  .vector_size = sizeof (u32),
+
+  .type = VLIB_NODE_TYPE_INTERNAL,
+
+  .n_next_nodes = LGPE_IP4_LOOKUP_N_NEXT,
+  .next_nodes = {
+      [LGPE_IP4_LOOKUP_NEXT_DROP] = "error-drop",
+      [LGPE_IP4_LOOKUP_NEXT_LISP_CP_LOOKUP] = "lisp-cp-lookup",
+      [LGPE_IP4_LOOKUP_NEXT_LGPE_ENCAP] = "lisp-gpe-encap",
+  },
+};
+
+static int
+lisp_gpe_rewrite (lisp_gpe_tunnel_t * t)
 {
   u8 *rw = 0;
   ip4_header_t * ip0;
@@ -142,9 +564,9 @@ static int lisp_gpe_rewrite (lisp_gpe_tunnel_t * t)
   ip4_udp_lisp_gpe_header_t * h0;
   int len;
 
-  len = sizeof (*h0);
+  len = sizeof(*h0);
 
-  vec_validate_aligned (rw, len-1, CLIB_CACHE_LINE_BYTES);
+  vec_validate_aligned(rw, len - 1, CLIB_CACHE_LINE_BYTES);
 
   h0 = (ip4_udp_lisp_gpe_header_t *) rw;
 
@@ -165,308 +587,416 @@ static int lisp_gpe_rewrite (lisp_gpe_tunnel_t * t)
 
   /* LISP-gpe header */
   lisp0 = &h0->lisp;
-  
+
   lisp0->flags = t->flags;
   lisp0->ver_res = t->ver_res;
   lisp0->res = t->res;
   lisp0->next_protocol = t->next_protocol;
   lisp0->iid = clib_host_to_net_u32 (t->iid);
-  
+
   t->rewrite = rw;
-  return (0);
+  return 0;
+}
+
+/* TODO remove */
+int
+vnet_lisp_gpe_add_del_tunnel (vnet_lisp_gpe_add_del_tunnel_args_t *a,
+                              u32 * sw_if_indexp)
+{
+  clib_warning ("UNSUPPORTED! Use vnet_lisp_gpe_add_del_fwd_entry");
+  return 0;
 }
 
-int vnet_lisp_gpe_add_del_tunnel 
-(vnet_lisp_gpe_add_del_tunnel_args_t *a, u32 * sw_if_indexp)
+#define foreach_copy_field                      \
+_(encap_fib_index)                              \
+_(decap_fib_index)                              \
+_(decap_next_index)                             \
+_(flags)                                        \
+_(next_protocol)                                \
+_(ver_res)                                      \
+_(res)                                          \
+_(iid)
+
+static u32
+add_del_tunnel (vnet_lisp_gpe_add_del_fwd_entry_args_t *a, u32 * tun_index_res)
 {
-  lisp_gpe_main_t * ngm = &lisp_gpe_main;
+  lisp_gpe_main_t * lgm = &lisp_gpe_main;
   lisp_gpe_tunnel_t *t = 0;
-  vnet_main_t * vnm = ngm->vnet_main;
-  vnet_hw_interface_t * hi;
   uword * p;
-  u32 hw_if_index = ~0;
-  u32 sw_if_index = ~0;
   int rv;
-  lisp_gpe_tunnel_key_t key, *key_copy;
-  hash_pair_t *hp;
-  
-  key.src = a->src.as_u32;
-  key.iid = clib_host_to_net_u32(a->iid);
+  lisp_gpe_tunnel_key_t key;
+
+  memset(&key, 0, sizeof(key));
+  gid_address_copy(&key.eid, &a->deid);
+  key.dst_loc = ip_addr_v4(&a->dlocator).as_u32;
+  key.iid = clib_host_to_net_u32 (a->iid);
+
+  p = mhash_get (&lgm->lisp_gpe_tunnel_by_key, &key);
 
-  p = hash_get_mem (ngm->lisp_gpe_tunnel_by_key, &key);
-  
   if (a->is_add)
     {
       /* adding a tunnel: tunnel must not already exist */
-      if (p) 
+      if (p)
         return VNET_API_ERROR_INVALID_VALUE;
-      
+
       if (a->decap_next_index >= LISP_GPE_INPUT_N_NEXT)
         return VNET_API_ERROR_INVALID_DECAP_NEXT;
-      
-      pool_get_aligned (ngm->tunnels, t, CLIB_CACHE_LINE_BYTES);
+
+      pool_get_aligned (lgm->tunnels, t, CLIB_CACHE_LINE_BYTES);
       memset (t, 0, sizeof (*t));
-      
+
       /* copy from arg structure */
 #define _(x) t->x = a->x;
       foreach_copy_field;
 #undef _
-      
+
+      t->src = ip_addr_v4(&a->slocator);
+      t->dst = ip_addr_v4(&a->dlocator);
+
       rv = lisp_gpe_rewrite (t);
 
       if (rv)
         {
-          pool_put (ngm->tunnels, t);
+          pool_put(lgm->tunnels, t);
           return rv;
         }
 
-      key_copy = clib_mem_alloc (sizeof (*key_copy));
-      memcpy (key_copy, &key, sizeof (*key_copy));
+      mhash_set(&lgm->lisp_gpe_tunnel_by_key, &key, t - lgm->tunnels, 0);
 
-      hash_set_mem (ngm->lisp_gpe_tunnel_by_key, key_copy, 
-                    t - ngm->tunnels);
-      
-      if (vec_len (ngm->free_lisp_gpe_tunnel_hw_if_indices) > 0)
-        {
-          hw_if_index = ngm->free_lisp_gpe_tunnel_hw_if_indices
-            [vec_len (ngm->free_lisp_gpe_tunnel_hw_if_indices)-1];
-          _vec_len (ngm->free_lisp_gpe_tunnel_hw_if_indices) -= 1;
-          
-          hi = vnet_get_hw_interface (vnm, hw_if_index);
-          hi->dev_instance = t - ngm->tunnels;
-          hi->hw_instance = hi->dev_instance;
-        }
-      else 
-        {
-          hw_if_index = vnet_register_interface
-            (vnm, lisp_gpe_device_class.index, t - ngm->tunnels,
-             lisp_gpe_hw_class.index, t - ngm->tunnels);
-          hi = vnet_get_hw_interface (vnm, hw_if_index);
-          hi->output_node_index = lisp_gpe_encap_node.index;
-        }
-      
-      t->hw_if_index = hw_if_index;
-      t->sw_if_index = sw_if_index = hi->sw_if_index;
-      
-      vnet_sw_interface_set_flags (vnm, hi->sw_if_index, 
-                                   VNET_SW_INTERFACE_FLAG_ADMIN_UP);
+      /* return tunnel index */
+      if (tun_index_res)
+        tun_index_res[0] = t - lgm->tunnels;
     }
   else
     {
       /* deleting a tunnel: tunnel must exist */
-      if (!p) 
-        return VNET_API_ERROR_NO_SUCH_ENTRY;
-
-      t = pool_elt_at_index (ngm->tunnels, p[0]);
+      if (!p)
+        {
+          clib_warning("Tunnel for eid %U doesn't exist!", format_gid_address,
+                       &a->deid);
+          return VNET_API_ERROR_NO_SUCH_ENTRY;
+        }
 
-      vnet_sw_interface_set_flags (vnm, t->sw_if_index, 0 /* down */);
-      vec_add1 (ngm->free_lisp_gpe_tunnel_hw_if_indices, t->hw_if_index);
+      t = pool_elt_at_index(lgm->tunnels, p[0]);
 
-      hp = hash_get_pair (ngm->lisp_gpe_tunnel_by_key, &key);
-      key_copy = (void *)(hp->key);
-      hash_unset_mem (ngm->lisp_gpe_tunnel_by_key, &key);
-      clib_mem_free (key_copy);
+      mhash_unset(&lgm->lisp_gpe_tunnel_by_key, &key, 0);
 
-      vec_free (t->rewrite);
-      pool_put (ngm->tunnels, t);
+      vec_free(t->rewrite);
+      pool_put(lgm->tunnels, t);
     }
 
-  if (sw_if_indexp)
-      *sw_if_indexp = sw_if_index;
-
   return 0;
 }
 
-static u32 fib_index_from_fib_id (u32 fib_id)
+int
+vnet_lisp_gpe_add_del_fwd_entry (vnet_lisp_gpe_add_del_fwd_entry_args_t * a,
+                                 u32 * hw_if_indexp)
 {
-  ip4_main_t * im = &ip4_main;
-  uword * p;
+  lisp_gpe_main_t * lgm = &lisp_gpe_main;
+  ip_adjacency_t adj, * adjp;
+  u32 * adj_index, rv, tun_index = ~0;
+  ip_prefix_t * dpref = &gid_address_ippref(&a->deid);
+  ip_prefix_t * spref = &gid_address_ippref(&a->seid);
 
-  p = hash_get (im->fib_index_by_table_id, fib_id);
-  if (!p)
-    return ~0;
+  /* setup adjacency for eid */
+  memset (&adj, 0, sizeof(adj));
+  adj.n_adj = 1;
+  adj.explicit_fib_index = ~0;
 
-  return p[0];
-}
+  /* treat negative fwd entries separately */
+  if (a->is_negative)
+    {
+      switch (a->action)
+        {
+        case NO_ACTION:
+          /* TODO update timers? */
+        case FORWARD_NATIVE:
+          /* TODO check if route/next-hop for eid exists in fib and add
+           * more specific for the eid with the next-hop found */
+        case SEND_MAP_REQUEST:
+          /* TODO insert tunnel that always sends map-request */
+        case DROP:
+          /* for drop fwd entries, just add route, no need to add encap tunnel */
+          adj.lookup_next_index = LGPE_IP4_LOOKUP_NEXT_DROP;
 
-static uword unformat_decap_next (unformat_input_t * input, va_list * args)
-{
-  u32 * result = va_arg (*args, u32 *);
-  u32 tmp;
-  
-  if (unformat (input, "drop"))
-    *result = LISP_GPE_INPUT_NEXT_DROP;
-  else if (unformat (input, "ip4"))
-    *result = LISP_GPE_INPUT_NEXT_IP4_INPUT;
-  else if (unformat (input, "ip6"))
-    *result = LISP_GPE_INPUT_NEXT_IP6_INPUT;
-  else if (unformat (input, "ethernet"))
-    *result = LISP_GPE_INPUT_NEXT_IP6_INPUT;
-  else if (unformat (input, "lisp-gpe"))
-    *result = LISP_GPE_INPUT_NEXT_LISP_GPE_ENCAP;
-  else if (unformat (input, "%d", &tmp))
-    *result = tmp;
-  else
-    return 0;
-  return 1;
+          /* add/delete route for prefix */
+          rv = ip4_sd_fib_add_del_route (lgm, dpref, spref, a->iid, &adj,
+                                         a->is_add);
+          return rv;
+          break;
+        default:
+          return -1;
+        }
+    }
+
+  /* send packets that hit this adj to lisp-gpe encap */
+  adj.lookup_next_index = LGPE_IP4_LOOKUP_NEXT_LGPE_ENCAP;
+
+  /* add/delete route for prefix
+   * TODO use hash to decide fib instead of using iid in clear */
+  rv = ip4_sd_fib_add_del_route (lgm, dpref, spref, a->iid, &adj, a->is_add);
+
+  if (rv)
+    return rv;
+
+  /* add/del tunnel to tunnels pool */
+  rv = add_del_tunnel (a, &tun_index);
+
+  /* reuse sw_if_index for storing the tunnel index */
+  if (a->is_add)
+    {
+      adj_index = ip4_sd_fib_get_route(lgm, dpref, spref, a->iid);
+      if (!adj_index)
+        {
+          clib_warning("Failed to insert fwd entry! For %U",
+                       format_ip4_address_and_length, ip_prefix_v4(dpref),
+                       ip_prefix_len(dpref));
+          return -1;
+        }
+      adjp = ip_get_adjacency (lgm->lookup_main, adj_index[0]);
+      adjp->rewrite_header.sw_if_index = tun_index;
+    }
+
+  return rv;
 }
 
 static clib_error_t *
-lisp_gpe_add_del_tunnel_command_fn (vlib_main_t * vm,
-                                   unformat_input_t * input,
-                                   vlib_cli_command_t * cmd)
+lisp_gpe_add_del_fwd_entry_command_fn (vlib_main_t * vm,
+                                       unformat_input_t * input,
+                                       vlib_cli_command_t * cmd)
 {
   unformat_input_t _line_input, * line_input = &_line_input;
-  ip4_address_t src, dst;
   u8 is_add = 1;
-  u8 src_set = 0;
-  u8 dst_set = 0;
-  u32 encap_fib_index = 0;
-  u32 decap_fib_index = 0;
-  u8 next_protocol = LISP_GPE_NEXT_PROTOCOL_IP4;
-  u32 decap_next_index = LISP_GPE_INPUT_NEXT_IP4_INPUT;
-  u8 flags = LISP_GPE_FLAGS_P;
-  u8 ver_res = 0;
-  u8 res = 0;
-  u32 iid = 0;
-  u8 iid_set = 0;
-  u32 tmp;
-  int rv;
-  vnet_lisp_gpe_add_del_tunnel_args_t _a, * a = &_a;
-  
+  ip_address_t slocator, dlocator, *slocators = 0, *dlocators = 0;
+  ip_prefix_t * prefp;
+  gid_address_t * eids = 0, eid;
+  clib_error_t * error = 0;
+  u32 i;
+
+  prefp = &gid_address_ippref(&eid);
+
   /* 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, "del"))
-      is_add = 0;
-    else if (unformat (line_input, "src %U", 
-                       unformat_ip4_address, &src))
-      src_set = 1;
-    else if (unformat (line_input, "dst %U",
-                       unformat_ip4_address, &dst))
-      dst_set = 1;
-    else if (unformat (line_input, "encap-vrf-id %d", &tmp))
-      {
-        encap_fib_index = fib_index_from_fib_id (tmp);
-        if (encap_fib_index == ~0)
-          return clib_error_return (0, "nonexistent encap fib id %d", tmp);
-      }
-    else if (unformat (line_input, "decap-vrf-id %d", &tmp))
-      {
-        decap_fib_index = fib_index_from_fib_id (tmp);
-        if (decap_fib_index == ~0)
-          return clib_error_return (0, "nonexistent decap fib id %d", tmp);
-      }
-    else if (unformat (line_input, "decap-next %U", unformat_decap_next, 
-                       &decap_next_index))
-      ;
-    else if (unformat(line_input, "next-ip4"))
-      next_protocol = 1;
-    else if (unformat(line_input, "next-ip6"))
-      next_protocol = 2;
-    else if (unformat(line_input, "next-ethernet"))
-      next_protocol = 3;
-    else if (unformat(line_input, "next-nsh"))
-      next_protocol = 4;
-    /* Allow the user to specify anything they want in the LISP hdr */
-    else if (unformat (line_input, "ver_res %x", &tmp))
-      ver_res = tmp;
-    else if (unformat (line_input, "res %x", &tmp))
-      res = tmp;
-    else if (unformat (line_input, "flags %x", &tmp))
-      flags = tmp;
-    else if (unformat (line_input, "n-bit"))
-      flags |= LISP_GPE_FLAGS_N;
-    else if (unformat (line_input, "l-bit"))
-      flags |= LISP_GPE_FLAGS_L;
-    else if (unformat (line_input, "e-bit"))
-      flags |= LISP_GPE_FLAGS_E;
-    else if (unformat (line_input, "v-bit"))
-      flags |= LISP_GPE_FLAGS_V;
-    else if (unformat (line_input, "i-bit"))
-      flags |= LISP_GPE_FLAGS_V;
-    else if (unformat (line_input, "not-p-bit"))
-      flags &= ~LISP_GPE_FLAGS_P;
-    else if (unformat (line_input, "p-bit"))
-      flags |= LISP_GPE_FLAGS_P;
-    else if (unformat (line_input, "o-bit"))
-      flags |= LISP_GPE_FLAGS_O;
-    else if (unformat (line_input, "iidx %x", &iid))
-      iid_set = 1;
-    else if (unformat (line_input, "iid %d", &iid))
-      iid_set = 1;
-    else 
-      return clib_error_return (0, "parse error: '%U'", 
-                                format_unformat_error, line_input);
-  }
-
+  while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
+    {
+      if (unformat (line_input, "del"))
+        is_add = 0;
+      else if (unformat (line_input, "add"))
+        is_add = 1;
+      else if (unformat (line_input, "eid %U slocator %U dlocator %U",
+                         unformat_ip_prefix, prefp,
+                         unformat_ip_address, &slocator,
+                         unformat_ip_address, &dlocator))
+        {
+          vec_add1 (eids, eid);
+          vec_add1 (slocators, slocator);
+          vec_add1 (dlocators, dlocator);
+        }
+      else
+        {
+          error = unformat_parse_error (line_input);
+          goto done;
+        }
+    }
   unformat_free (line_input);
 
-  if (src_set == 0)
-    return clib_error_return (0, "tunnel src address not specified");
+  if (vec_len (eids) + vec_len (slocators) == 0)
+    {
+      error = clib_error_return (0, "expected ip4/ip6 eids/locators.");
+      goto done;
+    }
 
-  if (dst_set == 0)
-    return clib_error_return (0, "tunnel dst address not specified");
+  if (vec_len (eids) != vec_len (slocators))
+    {
+      error = clib_error_return (0, "number of eids not equal to that of locators.");
+      goto done;
+    }
 
-  if (iid_set == 0)
-    return clib_error_return (0, "iid not specified");
+  for (i = 0; i < vec_len(eids); i++)
+    {
+      vnet_lisp_gpe_add_del_fwd_entry_args_t a;
+      memset (&a, 0, sizeof(a));
 
-  memset (a, 0, sizeof (*a));
+      a.is_add = is_add;
+      a.deid = eids[i];
+      a.slocator = slocators[i];
+      a.dlocator = dlocators[i];
+      prefp = &gid_address_ippref(&a.deid);
+      a.decap_next_index = (ip_prefix_version(prefp) == IP4) ?
+              LISP_GPE_INPUT_NEXT_IP4_INPUT : LISP_GPE_INPUT_NEXT_IP6_INPUT;
+      vnet_lisp_gpe_add_del_fwd_entry (&a, 0);
+    }
 
-  a->is_add = is_add;
+ done:
+  vec_free(eids);
+  vec_free(slocators);
+  return error;
+}
 
-#define _(x) a->x = x;
-  foreach_copy_field;
-#undef _
-  
-  rv = vnet_lisp_gpe_add_del_tunnel (a, 0 /* hw_if_indexp */);
+VLIB_CLI_COMMAND (add_del_lisp_gpe_mapping_tunnel_command, static) = {
+  .path = "lisp gpe maptunnel",
+  .short_help = "lisp gpe maptunnel eid <eid> sloc <src-locator> dloc <dst-locator> [del]",
+  .function = lisp_gpe_add_del_fwd_entry_command_fn,
+};
 
-  switch(rv)
-    {
-    case 0:
-      break;
-    case VNET_API_ERROR_INVALID_DECAP_NEXT:
-      return clib_error_return (0, "invalid decap-next...");
+int
+add_del_ip_prefix_route (ip_prefix_t * dst_prefix, u32 table_id,
+                         ip_adjacency_t * add_adj, u8 is_add, u32 * adj_index)
+{
+  uword * p;
 
-    case VNET_API_ERROR_TUNNEL_EXIST:
-      return clib_error_return (0, "tunnel already exists...");
+  if (ip_prefix_version(dst_prefix) == IP4)
+    {
+      ip4_main_t * im4 = &ip4_main;
+      ip4_add_del_route_args_t a;
+      ip4_address_t addr = ip_prefix_v4(dst_prefix);
 
-    case VNET_API_ERROR_NO_SUCH_ENTRY:
-      return clib_error_return (0, "tunnel does not exist...");
+      memset(&a, 0, sizeof(a));
+      a.flags = IP4_ROUTE_FLAG_TABLE_ID;
+      a.table_index_or_table_id = table_id;
+      a.adj_index = ~0;
+      a.dst_address_length = ip_prefix_len(dst_prefix);
+      a.dst_address = addr;
+      a.flags |= is_add ? IP4_ROUTE_FLAG_ADD : IP4_ROUTE_FLAG_DEL;
+      a.add_adj = add_adj;
+      a.n_add_adj = 1;
+      ip4_add_del_route (im4, &a);
 
-    default:
-      return clib_error_return 
-        (0, "vnet_lisp_gpe_add_del_tunnel returned %d", rv);
+      if (is_add)
+        {
+          p = ip4_get_route (im4, table_id, 0, addr.as_u8,
+                             ip_prefix_len(dst_prefix));
+          if (p == 0)
+            {
+              clib_warning("Failed to insert route for eid %U!",
+                           format_ip4_address_and_length, addr.as_u8,
+                           ip_prefix_len(dst_prefix));
+              return -1;
+            }
+          adj_index[0] = p[0];
+        }
     }
+  else
+    {
+      ip6_main_t * im6 = &ip6_main;
+      ip6_add_del_route_args_t a;
+      ip6_address_t addr = ip_prefix_v6(dst_prefix);
 
+      memset(&a, 0, sizeof(a));
+      a.flags = IP6_ROUTE_FLAG_TABLE_ID;
+      a.table_index_or_table_id = table_id;
+      a.adj_index = ~0;
+      a.dst_address_length = ip_prefix_len(dst_prefix);
+      a.dst_address = addr;
+      a.flags |= is_add ? IP6_ROUTE_FLAG_ADD : IP6_ROUTE_FLAG_DEL;
+      a.add_adj = add_adj;
+      a.n_add_adj = 1;
+
+      ip6_add_del_route (im6, &a);
+
+      if (is_add)
+        {
+          adj_index[0] = ip6_get_route (im6, table_id, 0, &addr,
+                                        ip_prefix_len(dst_prefix));
+          if (adj_index[0] == 0)
+            {
+              clib_warning("Failed to insert route for eid %U!",
+                           format_ip6_address_and_length, addr.as_u8,
+                           ip_prefix_len(dst_prefix));
+              return -1;
+            }
+        }
+    }
   return 0;
 }
 
-VLIB_CLI_COMMAND (create_lisp_gpe_tunnel_command, static) = {
-  .path = "lisp gpe tunnel",
-  .short_help = 
-  "lisp gpe tunnel src <ip4-addr> dst <ip4-addr> iidx <0xnn> | iid <nn>\n"
-  "    [encap-fib-id <nn>] [decap-fib-id <nn>]\n"
-  "    [n-bit][l-bit][e-bit][v-bit][i-bit][p-bit][not-p-bit][o-bit]\n"
-  "    [next-ip4][next-ip6][next-ethernet][next-nsh]\n"
-  "    [decap-next [ip4|ip6|ethernet|nsh-encap|<nn>]][del]\n",
-  .function = lisp_gpe_add_del_tunnel_command_fn,
-};
+static void
+add_del_lisp_gpe_default_route (u8 is_v4, u8 is_add)
+{
+  lisp_gpe_main_t * lgm = &lisp_gpe_main;
+  ip_adjacency_t adj;
+  ip_prefix_t prefix;
+  u32 adj_index = 0;
+
+  /* setup adjacency */
+  memset (&adj, 0, sizeof(adj));
+  adj.n_adj = 1;
+  adj.explicit_fib_index = ~0;
+  adj.lookup_next_index = lgm->ip4_lookup_next_lgpe_ip4_lookup;
+  /* default route has tunnel_index ~0 */
+  adj.rewrite_header.sw_if_index = ~0;
+
+  /* set prefix to 0/0 */
+  memset(&prefix, 0, sizeof(prefix));
+  ip_prefix_version(&prefix) = is_v4 ? IP4 : IP6;
+
+  /* add/delete route for prefix XXX default table only */
+  add_del_ip_prefix_route (&prefix, 0, &adj, is_add, &adj_index);
+}
+
+static u8 *
+format_decap_next (u8 * s, va_list * args)
+{
+  u32 next_index = va_arg (*args, u32);
+
+  switch (next_index)
+    {
+    case LISP_GPE_INPUT_NEXT_DROP:
+      return format (s, "drop");
+    case LISP_GPE_INPUT_NEXT_IP4_INPUT:
+      return format (s, "ip4");
+    case LISP_GPE_INPUT_NEXT_IP6_INPUT:
+      return format (s, "ip6");
+    case LISP_GPE_INPUT_NEXT_LISP_GPE_ENCAP:
+      return format (s, "nsh-lisp-gpe");
+    default:
+      return format (s, "unknown %d", next_index);
+    }
+  return s;
+}
+
+u8 *
+format_lisp_gpe_tunnel (u8 * s, va_list * args)
+{
+  lisp_gpe_tunnel_t * t = va_arg (*args, lisp_gpe_tunnel_t *);
+  lisp_gpe_main_t * lgm = &lisp_gpe_main;
+
+  s = format (s,
+              "[%d] %U (src) %U (dst) fibs: encap %d, decap %d",
+              t - lgm->tunnels,
+              format_ip4_address, &t->src,
+              format_ip4_address, &t->dst,
+              t->encap_fib_index,
+              t->decap_fib_index);
+
+  s = format (s, " decap next %U\n", format_decap_next, t->decap_next_index);
+  s = format (s, "lisp ver %d ", (t->ver_res>>6));
+
+#define _(n,v) if (t->flags & v) s = format (s, "%s-bit ", #n);
+  foreach_lisp_gpe_flag_bit;
+#undef _
+
+  s = format (s, "next_protocol %d ver_res %x res %x\n",
+              t->next_protocol, t->ver_res, t->res);
+
+  s = format (s, "iid %d (0x%x)\n", t->iid, t->iid);
+  return s;
+}
 
 static clib_error_t *
 show_lisp_gpe_tunnel_command_fn (vlib_main_t * vm,
                                 unformat_input_t * input,
                                 vlib_cli_command_t * cmd)
 {
-  lisp_gpe_main_t * ngm = &lisp_gpe_main;
+  lisp_gpe_main_t * lgm = &lisp_gpe_main;
   lisp_gpe_tunnel_t * t;
   
-  if (pool_elts (ngm->tunnels) == 0)
+  if (pool_elts (lgm->tunnels) == 0)
     vlib_cli_output (vm, "No lisp-gpe tunnels configured...");
 
-  pool_foreach (t, ngm->tunnels,
+  pool_foreach (t, lgm->tunnels,
   ({
     vlib_cli_output (vm, "%U", format_lisp_gpe_tunnel, t);
   }));
@@ -479,15 +1009,154 @@ VLIB_CLI_COMMAND (show_lisp_gpe_tunnel_command, static) = {
     .function = show_lisp_gpe_tunnel_command_fn,
 };
 
-clib_error_t *lisp_gpe_init (vlib_main_t *vm)
+static u8 *
+format_lisp_gpe_name (u8 * s, va_list * args)
 {
-  lisp_gpe_main_t *ngm = &lisp_gpe_main;
-  
-  ngm->vnet_main = vnet_get_main();
-  ngm->vlib_main = vm;
+  u32 dev_instance = va_arg (*args, u32);
+  return format (s, "lisp_gpe_tunnel%d", dev_instance);
+}
+
+static uword
+dummy_interface_tx (vlib_main_t * vm, vlib_node_runtime_t * node,
+                    vlib_frame_t * frame)
+{
+  clib_warning("you shouldn't be here, leaking buffers...");
+  return frame->n_vectors;
+}
+
+VNET_DEVICE_CLASS (lisp_gpe_device_class,static) = {
+  .name = "LISP_GPE",
+  .format_device_name = format_lisp_gpe_name,
+  .format_tx_trace = format_lisp_gpe_encap_trace,
+  .tx_function = dummy_interface_tx,
+};
+
+static uword
+dummy_set_rewrite (vnet_main_t * vnm, u32 sw_if_index, u32 l3_type,
+                   void * dst_address, void * rewrite, uword max_rewrite_bytes)
+{
+  return 0;
+}
+
+u8 *
+format_lisp_gpe_header_with_length (u8 * s, va_list * args)
+{
+  lisp_gpe_header_t * h = va_arg (*args, lisp_gpe_header_t *);
+  u32 max_header_bytes = va_arg (*args, u32);
+  u32 header_bytes;
+
+  header_bytes = sizeof (h[0]);
+  if (max_header_bytes != 0 && header_bytes > max_header_bytes)
+    return format (s, "gre-nsh header truncated");
+
+  s = format (s, "flags: ");
+#define _(n,v) if (h->flags & v) s = format (s, "%s ", #n);
+  foreach_lisp_gpe_flag_bit;
+#undef _
+
+  s = format (s, "\n  ver_res %d res %d next_protocol %d iid %d(%x)",
+              h->ver_res, h->res, h->next_protocol,
+              clib_net_to_host_u32 (h->iid),
+              clib_net_to_host_u32 (h->iid));
+  return s;
+}
+
+VNET_HW_INTERFACE_CLASS (lisp_gpe_hw_class) = {
+  .name = "LISP_GPE",
+  .format_header = format_lisp_gpe_header_with_length,
+  .set_rewrite = dummy_set_rewrite,
+};
+
+void
+vnet_lisp_gpe_add_del_iface (vnet_lisp_gpe_add_del_iface_args_t * a,
+                             u32 * hw_if_indexp)
+{
+  lisp_gpe_main_t * lgm = &lisp_gpe_main;
+  vnet_main_t * vnm = lgm->vnet_main;
+  vnet_hw_interface_t * hi;
+  u32 hw_if_index = ~0;
+
+  if (a->is_add)
+    {
+      /* create hw lisp_gpe0 iface */
+      hw_if_index = vnet_register_interface (vnm, lisp_gpe_device_class.index, 0,
+                                             lisp_gpe_hw_class.index, 0);
+
+      hi = vnet_get_hw_interface (vnm, hw_if_index);
+      hi->output_node_index = lisp_gpe_encap_node.index;
+      lgm->lisp_gpe_hw_if_index = hw_if_index;
+
+      /* add lgpe_ip4_lookup as possible next_node for ip4 lookup */
+      lgm->ip4_lookup_next_lgpe_ip4_lookup = vlib_node_add_next (
+          vnm->vlib_main, ip4_lookup_node.index, lgpe_ip4_lookup_node.index);
+
+      /* insert default routes that points at lisp-gpe-encap */
+      add_del_lisp_gpe_default_route(/* is_v4 */1, 1);
+      add_del_lisp_gpe_default_route(/* is_v4 */0, 1);
+    }
+  else
+    {
+      vnet_sw_interface_set_flags (vnm, lgm->lisp_gpe_hw_if_index,
+                                   0 /* down */);
+    }
+}
+
+static clib_error_t *
+lisp_gpe_add_del_iface_command_fn (vlib_main_t * vm,
+                                   unformat_input_t * input,
+                                   vlib_cli_command_t * cmd)
+{
+  unformat_input_t _line_input, * line_input = &_line_input;
+  u8 is_add = 1;
+  vnet_lisp_gpe_add_del_iface_args_t _a, * a = &_a;
+
+  /* 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, "up"))
+        is_add = 1;
+      else if (unformat (line_input, "down"))
+        is_add = 0;
+      else
+        {
+          return clib_error_return (0, "parse error: '%U'",
+                                   format_unformat_error, line_input);
+        }
+    }
+
+  a->is_add = is_add;
+  vnet_lisp_gpe_add_del_iface (a, 0);
+  return 0;
+}
+
+VLIB_CLI_COMMAND (add_del_lisp_gpe_iface_command, static) = {
+  .path = "lisp gpe iface",
+  .short_help = "lisp gpe iface [del]",
+  .function = lisp_gpe_add_del_iface_command_fn,
+};
+
+clib_error_t *
+lisp_gpe_init (vlib_main_t *vm)
+{
+  lisp_gpe_main_t * lgm = &lisp_gpe_main;
+  clib_error_t * error = 0;
+
+  if ((error = vlib_call_init_function (vm, ip_main_init)))
+    return error;
+
+  if ((error = vlib_call_init_function (vm, ip4_lookup_init)))
+    return error;
+
+  lgm->vnet_main = vnet_get_main();
+  lgm->vlib_main = vm;
+  lgm->im4 = &ip4_main;
+  lgm->lookup_main = &ip4_main.lookup_main;
   
-  ngm->lisp_gpe_tunnel_by_key 
-    = hash_create_mem (0, sizeof(lisp_gpe_tunnel_key_t), sizeof (uword));
+  mhash_init (&lgm->lisp_gpe_tunnel_by_key, sizeof(uword),
+              sizeof(lisp_gpe_tunnel_key_t));
 
   udp_register_dst_port (vm, UDP_DST_PORT_lisp_gpe, 
                          lisp_gpe_input_node.index, 1 /* is_ip4 */);
@@ -495,4 +1164,3 @@ clib_error_t *lisp_gpe_init (vlib_main_t *vm)
 }
 
 VLIB_INIT_FUNCTION(lisp_gpe_init);
-  
diff --git a/vnet/vnet/lisp-gpe/lisp_gpe.h b/vnet/vnet/lisp-gpe/lisp_gpe.h
index 12c4ebce38c..7e86d57fc1a 100644
--- a/vnet/vnet/lisp-gpe/lisp_gpe.h
+++ b/vnet/vnet/lisp-gpe/lisp_gpe.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015 Cisco and/or its affiliates.
+ * 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:
@@ -12,18 +12,22 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
+
 #ifndef included_vnet_lisp_gpe_h
 #define included_vnet_lisp_gpe_h
 
 #include <vppinfra/error.h>
-#include <vppinfra/hash.h>
+#include <vppinfra/mhash.h>
 #include <vnet/vnet.h>
 #include <vnet/ip/ip.h>
 #include <vnet/l2/l2_input.h>
 #include <vnet/ethernet/ethernet.h>
-#include <vnet/lisp-gpe/lisp_gpe_packet.h>
 #include <vnet/ip/ip4_packet.h>
 #include <vnet/ip/udp.h>
+#include <vnet/lisp-cp/lisp_types.h>
+#include <vnet/lisp-gpe/lisp_gpe_packet.h>
+
+#define IP_UDP_HDR_LEN (word) (sizeof(udp_header_t) + sizeof(ip4_header_t))
 
 typedef CLIB_PACKED (struct {
   ip4_header_t ip4;             /* 20 bytes */
@@ -31,21 +35,22 @@ typedef CLIB_PACKED (struct {
   lisp_gpe_header_t lisp;       /* 8 bytes */
 }) ip4_udp_lisp_gpe_header_t;
 
-typedef CLIB_PACKED(struct {
-  /* 
-   * Key fields: ip src, LISP iid, ??? $$$$$$$$$ correct answer ???
-   * all fields in NET byte order
-   */
-  union {
-    struct {
-      u32 src;
-      u32 iid;
-    };
-    u64 as_u64[1];
+typedef struct
+{
+  union
+  {
+    struct
+      {
+	gid_address_t eid;
+	u32 dst_loc;
+	u32 iid;
+      };
+    u8 as_u8[6];
   };
-}) lisp_gpe_tunnel_key_t;
+} lisp_gpe_tunnel_key_t;
 
-typedef struct {
+typedef struct
+{
   /* Rewrite string. $$$$ embed vnet_rewrite header */
   u8 * rewrite;
 
@@ -93,30 +98,53 @@ typedef enum {
   LISP_GPE_N_ERROR,
 } lisp_gpe_input_error_t;
 
-typedef struct {
+/* As a first step, reuse v4 fib. The goal of the typedef is to shield
+ * consumers from future updates that may result in the lisp ip4 fib diverging
+ * from ip4 fib */
+typedef ip4_fib_t lisp_ip4_fib_t;
+
+typedef struct lisp_gpe_main
+{
+  /* Pool of src fibs that are paired with dst fibs */
+  ip4_fib_t * src_fibs;
+
   /* vector of encap tunnel instances */
-  lisp_gpe_tunnel_t *tunnels;
+  lisp_gpe_tunnel_t * tunnels;
 
   /* lookup tunnel by key */
-  uword * lisp_gpe_tunnel_by_key;
+  mhash_t lisp_gpe_tunnel_by_key;
+
+  /* lookup tunnel by adjacency index */
+  uword * lisp_gpe_tunnel_by_adj_index;
 
   /* Free vlib hw_if_indices */
   u32 * free_lisp_gpe_tunnel_hw_if_indices;
 
+  u32 lisp_gpe_hw_if_index;
+
+  /* next node indexes that points ip4 lookup to lisp gpe lookup and lisp cp */
+  u32 ip4_lookup_next_lgpe_ip4_lookup;
+
   /* convenience */
   vlib_main_t * vlib_main;
   vnet_main_t * vnet_main;
+  ip_lookup_main_t * lookup_main;
+  ip4_main_t * im4;
 } lisp_gpe_main_t;
 
 lisp_gpe_main_t lisp_gpe_main;
 
+extern vlib_node_registration_t lgpe_ip4_lookup_node;
 extern vlib_node_registration_t lisp_gpe_input_node;
 extern vlib_node_registration_t lisp_gpe_encap_node;
 
-u8 * format_lisp_gpe_encap_trace (u8 * s, va_list * args);
-u8 * format_lisp_gpe_header_with_length (u8 * s, va_list * args);
+u8 *
+format_lisp_gpe_encap_trace (u8 * s, va_list * args);
+u8 *
+format_lisp_gpe_header_with_length (u8 * s, va_list * args);
 
-typedef struct {
+typedef struct
+{
   u8 is_add;
   ip4_address_t src, dst;
   u32 encap_fib_index;
@@ -126,12 +154,54 @@ typedef struct {
   u8 ver_res;
   u8 res;
   u8 next_protocol;
-  u32 iid;                      /* host byte order */
+  u32 iid; /* host byte order */
 } vnet_lisp_gpe_add_del_tunnel_args_t;
 
-int vnet_lisp_gpe_add_del_tunnel 
-(vnet_lisp_gpe_add_del_tunnel_args_t *a, u32 * sw_if_indexp);
+int
+vnet_lisp_gpe_add_del_tunnel (vnet_lisp_gpe_add_del_tunnel_args_t *a,
+			      u32 * sw_if_indexp);
+
+typedef struct
+{
+  u8 is_add;
+} vnet_lisp_gpe_add_del_iface_args_t;
+
+void
+vnet_lisp_gpe_add_del_iface (vnet_lisp_gpe_add_del_iface_args_t *a,
+			     u32 * hw_if_indexp);
+
+typedef enum
+{
+  NO_ACTION,
+  FORWARD_NATIVE,
+  SEND_MAP_REQUEST,
+  DROP
+} negative_fwd_actions_e;
+
+typedef struct
+{
+  u8 is_add;
+  u8 is_negative;
+  negative_fwd_actions_e action;
+  gid_address_t seid; /* TODO convert to ip4, ip6, mac ? */
+  gid_address_t deid;
+  ip_address_t slocator;
+  ip_address_t dlocator;
+  u32 encap_fib_index;
+  u32 decap_fib_index;
+  u32 decap_next_index;
+  u8 flags;
+  u8 ver_res;
+  u8 res;
+  u8 next_protocol;
+  u32 iid; /* host byte order */
+} vnet_lisp_gpe_add_del_fwd_entry_args_t;
+
+int
+vnet_lisp_gpe_add_del_fwd_entry (vnet_lisp_gpe_add_del_fwd_entry_args_t *a,
+				 u32 * hw_if_indexp);
 
-u8 * format_lisp_gpe_header_with_length (u8 * s, va_list * args);
+u8 *
+format_lisp_gpe_header_with_length (u8 * s, va_list * args);
 
 #endif /* included_vnet_lisp_gpe_h */
diff --git a/vnet/vnet/lisp-gpe/lisp_gpe_error.def b/vnet/vnet/lisp-gpe/lisp_gpe_error.def
index 6ef894f474d..f002ecaeac0 100644
--- a/vnet/vnet/lisp-gpe/lisp_gpe_error.def
+++ b/vnet/vnet/lisp-gpe/lisp_gpe_error.def
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015 Cisco and/or its affiliates.
+ * 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:
@@ -12,5 +12,6 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
+ 
 lisp_gpe_error (DECAPSULATED, "good packets decapsulated")
 lisp_gpe_error (NO_SUCH_TUNNEL, "no such tunnel packets")
diff --git a/vnet/vnet/lisp-gpe/lisp_gpe_packet.h b/vnet/vnet/lisp-gpe/lisp_gpe_packet.h
index b3d96ed9d44..360157b700c 100644
--- a/vnet/vnet/lisp-gpe/lisp_gpe_packet.h
+++ b/vnet/vnet/lisp-gpe/lisp_gpe_packet.h
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2015 Cisco and/or its affiliates.
+ * 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:
@@ -12,6 +12,7 @@
  * See the License for the specific language governing permissions and
  * limitations under the License.
  */
+
 #ifndef included_lisp_gpe_packet_h
 #define included_lisp_gpe_packet_h
 
@@ -120,4 +121,13 @@ foreach_lisp_gpe_flag_bit
 #define LISP_GPE_NEXT_PROTOCOL_ETHERNET 0x3
 #define LISP_GPE_NEXT_PROTOCOL_NSH 0x4
 
+typedef enum
+{
+  LISP_GPE_NEXT_PROTO_IP4 = 1,
+  LISP_GPE_NEXT_PROTO_IP6,
+  LISP_GPE_NEXT_PROTO_ETHERNET,
+  LISP_GPE_NEXT_PROTO_NSH,
+  LISP_GPE_NEXT_PROTOS
+} lisp_gpe_next_protocol_e;
+
 #endif /* included_lisp_gpe_packet_h */