VPP-198: LISP map-resolver failover algorithm
[vpp.git] / vnet / vnet / lisp-cp / control.c
index 1e3b98f..1b129d2 100644 (file)
  * limitations under the License.
  */
 
+#include <vlibmemory/api.h>
 #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>
 
+typedef struct
+{
+  u8 is_resend;
+  gid_address_t seid;
+  gid_address_t deid;
+  u8 smr_invoked;
+} map_request_args_t;
+
+static int
+queue_map_request (gid_address_t * seid, gid_address_t * deid,
+                   u8 smr_invoked, u8 is_resend);
+
+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_address (ip_lookup_main_t * lm, u32 sw_if_index,
+                                u8 version)
+{
+  ip_interface_address_t * ia;
+
+  ia = ip_interface_get_first_interface_address (lm, sw_if_index, 1);
+  if (!ia)
+    return 0;
+  return ip_interface_address_get_address (lm, ia);
+}
+
+int
+ip_interface_get_first_ip_address (lisp_cp_main_t * lcm, u32 sw_if_index,
+                                   u8 version, ip_address_t * result)
+{
+  ip_lookup_main_t * lm;
+  void * addr;
+
+  lm = (version == IP4) ? &lcm->im4->lookup_main : &lcm->im6->lookup_main;
+  addr = ip_interface_get_first_address (lm, sw_if_index, version);
+  if (!addr)
+    return 0;
+
+  ip_address_set (result, addr, version);
+  return 1;
+}
+
+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));
+}
+
+u32
+ip_fib_get_egress_iface_for_dst_with_lm (lisp_cp_main_t * lcm,
+                                         ip_address_t * dst,
+                                         ip_lookup_main_t * lm)
+{
+  u32 adj_index;
+  ip_adjacency_t * adj;
+
+  adj_index = ip_fib_lookup_with_table (lcm, 0, dst);
+  adj = ip_get_adjacency (lm, adj_index);
+
+  if (adj == 0)
+    return ~0;
+
+  /* we only want outgoing routes */
+  if (adj->lookup_next_index != IP_LOOKUP_NEXT_ARP
+      && adj->lookup_next_index != IP_LOOKUP_NEXT_REWRITE)
+    return ~0;
+
+  return adj->rewrite_header.sw_if_index;
+}
+
+/**
+ * Find the sw_if_index of the interface that would be used to egress towards
+ * dst.
+ */
+u32
+ip_fib_get_egress_iface_for_dst (lisp_cp_main_t * lcm, ip_address_t * dst)
+{
+  ip_lookup_main_t * lm;
+
+  lm = ip_addr_version (dst) == IP4 ?
+      &lcm->im4->lookup_main : &lcm->im6->lookup_main;
+
+  return ip_fib_get_egress_iface_for_dst_with_lm (lcm, dst, lm);
+}
+
+/**
+ * Find first IP of the interface that would be used to egress towards dst.
+ * Returns 1 if the address is found 0 otherwise.
+ */
+int
+ip_fib_get_first_egress_ip_for_dst (lisp_cp_main_t * lcm, ip_address_t * dst,
+                                    ip_address_t * result)
+{
+  u32 si;
+  ip_lookup_main_t * lm;
+  void * addr = 0;
+  u8 ipver;
+
+  ASSERT(result != 0);
+
+  ipver = ip_addr_version(dst);
+
+  lm = (ipver == IP4) ? &lcm->im4->lookup_main : &lcm->im6->lookup_main;
+  si = ip_fib_get_egress_iface_for_dst_with_lm (lcm, dst, lm);
+
+  if ((u32) ~0 == si)
+    return 0;
+
+  /* find the first ip address */
+  addr = ip_interface_get_first_address (lm, si, ipver);
+  if (0 == addr)
+    return 0;
+
+  ip_address_set (result, addr, ipver);
+  return 1;
+}
+
+static int
+dp_add_del_iface (lisp_cp_main_t * lcm, u32 vni, u8 is_l2, u8 is_add)
+{
+  uword * dp_table, * intf;
+  vnet_lisp_gpe_add_del_iface_args_t _ai, *ai = &_ai;
+
+  if (!is_l2)
+    {
+      dp_table = hash_get(lcm->table_id_by_vni, vni);
+
+      if (!dp_table)
+        {
+          clib_warning("vni %d not associated to a vrf!", vni);
+          return VNET_API_ERROR_INVALID_VALUE;
+        }
+    }
+  else
+    {
+      dp_table = hash_get(lcm->bd_id_by_vni, vni);
+      if (!dp_table)
+        {
+          clib_warning("vni %d not associated to a bridge domain!", vni);
+          return VNET_API_ERROR_INVALID_VALUE;
+        }
+    }
+
+  intf = hash_get(is_l2 ? lcm->l2_dp_intf_by_vni :lcm->dp_intf_by_vni, vni);
+
+  /* enable/disable data-plane interface */
+  if (is_add)
+    {
+      /* create interface */
+      if (!intf)
+        {
+          ai->is_add = 1;
+          ai->vni = vni;
+          ai->is_l2 = is_l2;
+          ai->dp_table = dp_table[0];
+
+          vnet_lisp_gpe_add_del_iface (ai, 0);
+
+          /* keep track of vnis for which interfaces have been created */
+          hash_set(lcm->dp_intf_by_vni, vni, 1);
+        }
+    }
+  else
+    {
+      if (intf == 0)
+        {
+          clib_warning("interface for vni %d doesn't exist!", vni);
+          return VNET_API_ERROR_INVALID_VALUE;
+        }
+
+      ai->is_add = 0;
+      ai->vni = vni;
+      ai->dp_table = dp_table[0];
+      vnet_lisp_gpe_add_del_iface (ai, 0);
+      hash_unset(lcm->dp_intf_by_vni, vni);
+    }
+
+  return 0;
+}
+
 static void
-add_fwd_entry (lisp_cp_main_t* lcm, u32 src_map_index, u32 dst_map_index);
+dp_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->locator_pairs = fe->locator_pairs;
+  a->vni = gid_address_vni(&a->rmt_eid);
+  gid_address_copy(&a->rmt_eid, &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);
+  vec_free(fe->locator_pairs);
+  pool_put(lcm->fwd_entry_pool, fe);
+}
+
+/**
+ * Finds first remote locator with best (lowest) priority that has a local
+ * peer locator with an underlying route to it.
+ *
+ */
+static u32
+get_locator_pairs (lisp_cp_main_t* lcm, mapping_t * lcl_map,
+                   mapping_t * rmt_map, locator_pair_t ** locator_pairs)
+{
+  u32 i, limitp = 0, li, found = 0, esi;
+  locator_set_t * rmt_ls, * lcl_ls;
+  ip_address_t _lcl_addr, * lcl_addr = &_lcl_addr;
+  locator_t * lp, * rmt = 0;
+  uword * checked = 0;
+  locator_pair_t pair;
+
+  rmt_ls = pool_elt_at_index(lcm->locator_set_pool, rmt_map->locator_set_index);
+  lcl_ls = pool_elt_at_index(lcm->locator_set_pool, lcl_map->locator_set_index);
+
+  if (!rmt_ls || vec_len(rmt_ls->locator_indices) == 0)
+    return 0;
+
+  while (1)
+    {
+      rmt = 0;
+
+      /* find unvisited remote locator with best priority */
+      for (i = 0; i < vec_len(rmt_ls->locator_indices); i++)
+        {
+          if (0 != hash_get(checked, i))
+            continue;
+
+          li = vec_elt(rmt_ls->locator_indices, i);
+          lp = pool_elt_at_index(lcm->locator_pool, li);
+
+          /* we don't support non-IP locators for now */
+          if (gid_address_type(&lp->address) != GID_ADDR_IP_PREFIX)
+            continue;
+
+          if ((found && lp->priority == limitp)
+              || (!found && lp->priority >= limitp))
+            {
+              rmt = lp;
+
+              /* don't search for locators with lower priority and don't
+               * check this locator again*/
+              limitp = lp->priority;
+              hash_set(checked, i, 1);
+              break;
+            }
+        }
+      /* check if a local locator with a route to remote locator exists */
+      if (rmt != 0)
+        {
+          /* find egress sw_if_index for rmt locator */
+          esi = ip_fib_get_egress_iface_for_dst (
+              lcm, &gid_address_ip(&rmt->address));
+          if ((u32) ~0 == esi)
+            continue;
+
+          for (i = 0; i < vec_len(lcl_ls->locator_indices); i++)
+            {
+              li = vec_elt (lcl_ls->locator_indices, i);
+              locator_t * sl = pool_elt_at_index (lcm->locator_pool, li);
+
+              /* found local locator with the needed sw_if_index*/
+              if (sl->sw_if_index == esi)
+                {
+                  /* and it has an address */
+                  if (0 == ip_interface_get_first_ip_address (lcm,
+                             sl->sw_if_index,
+                             gid_address_ip_version(&rmt->address), lcl_addr))
+                    continue;
+
+                  memset(&pair, 0, sizeof(pair));
+                  ip_address_copy (&pair.rmt_loc,
+                                   &gid_address_ip(&rmt->address));
+                  ip_address_copy(&pair.lcl_loc, lcl_addr);
+                  pair.weight = rmt->weight;
+                  vec_add1(locator_pairs[0], pair);
+                  found = 1;
+                }
+            }
+        }
+      else
+        break;
+    }
+
+  hash_free(checked);
+  return found;
+}
 
 static void
-del_fwd_entry (lisp_cp_main_t * lcm, u32 src_map_index, u32 dst_map_index);
+dp_add_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;
+  mapping_t * src_map, * dst_map;
+  u32 sw_if_index;
+  uword * feip = 0, * dpid;
+  fwd_entry_t* fe;
+  u8 type;
 
-static u8
-compare_locators (lisp_cp_main_t *lcm, u32 * old_ls_indexes,
-                  locator_t * new_locators);
+  memset (a, 0, sizeof(*a));
 
-/* Stores mapping in map-cache. It does NOT program data plane forwarding for
- * remote/learned mappings. */
+  /* remove entry if it already exists */
+  feip = hash_get (lcm->fwd_entry_by_mapping_index, dst_map_index);
+  if (feip)
+    dp_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);
+
+  /* insert data plane forwarding entry */
+  a->is_add = 1;
+
+  gid_address_copy (&a->rmt_eid, &dst_map->eid);
+  a->vni = gid_address_vni(&a->rmt_eid);
+
+  /* get vrf or bd_index associated to vni */
+  type = gid_address_type(&dst_map->eid);
+  if (GID_ADDR_IP_PREFIX == type)
+    {
+      dpid = hash_get(lcm->table_id_by_vni, a->vni);
+      if (!dpid)
+        {
+          clib_warning("vni %d not associated to a vrf!", a->vni);
+          return;
+        }
+      a->table_id = dpid[0];
+    }
+  else if (GID_ADDR_MAC == type)
+    {
+      dpid = hash_get(lcm->bd_id_by_vni, a->vni);
+      if (!dpid)
+        {
+          clib_warning("vni %d not associated to a bridge domain !", a->vni);
+          return;
+        }
+      a->bd_id = dpid[0];
+    }
+
+  /* find best locator pair that 1) verifies LISP policy 2) are connected */
+  if (0 == get_locator_pairs (lcm, src_map, dst_map, &a->locator_pairs))
+    {
+      /* negative entry */
+      a->is_negative = 1;
+      a->action = dst_map->action;
+    }
+
+  /* TODO remove */
+  u8 ipver = ip_prefix_version(&gid_address_ippref(&a->rmt_eid));
+  a->decap_next_index = (ipver == IP4) ?
+          LISP_GPE_INPUT_NEXT_IP4_INPUT : LISP_GPE_INPUT_NEXT_IP6_INPUT;
+
+  vnet_lisp_gpe_add_del_fwd_entry (a, &sw_if_index);
+
+  /* add tunnel to fwd entry table XXX check return value from DP insertion */
+  pool_get (lcm->fwd_entry_pool, fe);
+  fe->locator_pairs = a->locator_pairs;
+  gid_address_copy (&fe->deid, &a->rmt_eid);
+  hash_set (lcm->fwd_entry_by_mapping_index, dst_map_index,
+            fe - lcm->fwd_entry_pool);
+}
+
+/**
+ * Add/remove mapping to/from map-cache. Overwriting not allowed.
+ */
 int
-vnet_lisp_add_del_mapping (vnet_lisp_add_del_mapping_args_t * a,
-                           u32 * map_index_result)
+vnet_lisp_map_cache_add_del (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, * old_map;
   u32 ** eid_indexes;
 
-  mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &a->deid);
+  mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &a->eid);
   old_map = mi != ~0 ? pool_elt_at_index (lcm->mapping_pool, mi) : 0;
   if (a->is_add)
     {
       /* TODO check if overwriting and take appropriate actions */
       if (mi != GID_LOOKUP_MISS && !gid_address_cmp (&old_map->eid,
-                                                     &a->deid))
+                                                     &a->eid))
         {
           clib_warning ("eid %U found in the eid-table", format_gid_address,
-                       &a->deid);
+                       &a->eid);
           return VNET_API_ERROR_VALUE_EXIST;
         }
 
       pool_get(lcm->mapping_pool, m);
-      gid_address_copy (&m->eid, &a->deid);
+      gid_address_copy (&m->eid, &a->eid);
       m->locator_set_index = a->locator_set_index;
       m->ttl = a->ttl;
       m->action = a->action;
       m->local = a->local;
 
       map_index = m - lcm->mapping_pool;
-      gid_dictionary_add_del (&lcm->mapping_index_by_gid, &a->deid, map_index,
+      gid_dictionary_add_del (&lcm->mapping_index_by_gid, &a->eid, map_index,
                               1);
 
       if (pool_is_free_index(lcm->locator_set_pool, a->locator_set_index))
@@ -88,7 +471,7 @@ vnet_lisp_add_del_mapping (vnet_lisp_add_del_mapping_args_t * a,
       if (mi == GID_LOOKUP_MISS)
         {
           clib_warning("eid %U not found in the eid-table", format_gid_address,
-                       &a->deid);
+                       &a->eid);
           return VNET_API_ERROR_INVALID_VALUE;
         }
 
@@ -116,13 +499,9 @@ vnet_lisp_add_del_mapping (vnet_lisp_add_del_mapping_args_t * a,
             }
           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);
+      gid_dictionary_add_del (&lcm->mapping_index_by_gid, &a->eid, 0, 0);
       gid_address_free (&m->eid);
       pool_put_index (lcm->mapping_pool, mi);
     }
@@ -130,14 +509,17 @@ vnet_lisp_add_del_mapping (vnet_lisp_add_del_mapping_args_t * a,
   return 0;
 }
 
-/* Stores mapping in map-cache and programs data plane for local mappings. */
+/**
+ *  Add/update/delete mapping to/in/from map-cache.
+ */
 int
 vnet_lisp_add_del_local_mapping (vnet_lisp_add_del_mapping_args_t * a,
                                  u32 * map_index_result)
 {
-  uword * table_id, * refc;
-  u32 rv, vni;
-  vnet_lisp_gpe_add_del_iface_args_t _ai, *ai = &_ai;
+  uword * dp_table = 0;
+  u32 vni;
+  u8 type;
+
   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
 
   if (vnet_lisp_enable_disable_status () == 0)
@@ -146,60 +528,22 @@ vnet_lisp_add_del_local_mapping (vnet_lisp_add_del_mapping_args_t * a,
       return VNET_API_ERROR_LISP_DISABLED;
     }
 
-  vni = gid_address_vni(&a->deid);
-
-  /* store/remove mapping from map-cache */
-  rv = vnet_lisp_add_del_mapping (a, map_index_result);
-  if (rv)
-    return rv;
-
-  table_id = hash_get(lcm->table_id_by_vni, vni);
+  vni = gid_address_vni(&a->eid);
+  type = gid_address_type(&a->eid);
+  if (GID_ADDR_IP_PREFIX == type)
+    dp_table = hash_get(lcm->table_id_by_vni, vni);
+  else if (GID_ADDR_MAC == type)
+    dp_table = hash_get(lcm->bd_id_by_vni, vni);
 
-  if (!table_id)
+  if (!dp_table)
     {
-      clib_warning ("vni %d not associated to a vrf!", vni);
+      clib_warning("vni %d not associated to a %s!", vni,
+                   GID_ADDR_IP_PREFIX == type ? "vrf" : "bd");
       return VNET_API_ERROR_INVALID_VALUE;
     }
 
-  refc = hash_get(lcm->dp_if_refcount_by_vni, vni);
-
-  /* enable/disable data-plane interface */
-  if (a->is_add)
-    {
-      /* create interface or update refcount */
-      if (!refc)
-        {
-          ai->is_add = 1;
-          ai->vni = vni;
-          ai->table_id = table_id[0];
-          vnet_lisp_gpe_add_del_iface (ai, 0);
-
-          /* counts the number of eids in a vni that use the interface */
-          hash_set(lcm->dp_if_refcount_by_vni, vni, 1);
-        }
-      else
-        {
-          refc[0]++;
-        }
-    }
-  else
-    {
-      /* since this is a remove for an existing eid, the iface should exist */
-      ASSERT(refc != 0);
-      refc[0]--;
-
-      /* remove iface if needed */
-      if (refc[0] == 0)
-        {
-          ai->is_add = 0;
-          ai->vni = vni;
-          ai->table_id = table_id[0];
-          vnet_lisp_gpe_add_del_iface (ai, 0);
-          hash_unset (lcm->dp_if_refcount_by_vni, vni);
-        }
-    }
-
-  return rv;
+  /* store/remove mapping from map-cache */
+  return vnet_lisp_map_cache_add_del (a, map_index_result);
 }
 
 static clib_error_t *
@@ -210,7 +554,6 @@ lisp_add_del_local_eid_command_fn (vlib_main_t * vm, unformat_input_t * input,
   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 = 0;
@@ -221,6 +564,8 @@ lisp_add_del_local_eid_command_fn (vlib_main_t * vm, unformat_input_t * input,
   u32 vni = 0;
 
   memset (&eid, 0, sizeof (eid));
+  memset (a, 0, sizeof (*a));
+
   /* Get a line of input. */
   if (! unformat_user (input, unformat_line_input, line_input))
     return 0;
@@ -231,12 +576,10 @@ lisp_add_del_local_eid_command_fn (vlib_main_t * vm, unformat_input_t * input,
         is_add = 1;
       else if (unformat (line_input, "del"))
         is_add = 0;
+      else if (unformat (line_input, "eid %U", unformat_gid_address, &eid))
+        ;
       else if (unformat (line_input, "vni %d", &vni))
         gid_address_vni (&eid) = vni;
-      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);
@@ -254,10 +597,15 @@ lisp_add_del_local_eid_command_fn (vlib_main_t * vm, unformat_input_t * input,
           goto done;
         }
     }
-  /* XXX treat batch configuration */
+  /* XXX treat batch configuration */
+
+  if (GID_ADDR_SRC_DST == gid_address_type(&eid))
+    {
+      error = clib_error_return(0, "src/dst is not supported for local EIDs!");
+      goto done;
+    }
 
-  gid_address_type (&eid) = GID_ADDR_IP_PREFIX;
-  a->deid = eid;
+  gid_address_copy(&a->eid, &eid);
   a->is_add = is_add;
   a->locator_set_index = locator_set_index;
   a->local = 1;
@@ -265,14 +613,14 @@ lisp_add_del_local_eid_command_fn (vlib_main_t * vm, unformat_input_t * input,
   rv = vnet_lisp_add_del_local_mapping (a, &map_index);
   if (0 != rv)
    {
-      error = clib_error_return(0, "failed to %s eid-table!",
+      error = clib_error_return(0, "failed to %s local mapping!",
                                 is_add ? "add" : "delete");
    }
  done:
   vec_free(eids);
   if (locator_set_name)
     vec_free (locator_set_name);
-  gid_address_free (&a->deid);
+  gid_address_free (&a->eid);
   return error;
 }
 
@@ -284,10 +632,10 @@ VLIB_CLI_COMMAND (lisp_add_del_local_eid_command) = {
 };
 
 int
-vnet_lisp_eid_table_map (u32 vni, u32 vrf, u8 is_add)
+vnet_lisp_eid_table_map (u32 vni, u32 dp_id, u8 is_l2, u8 is_add)
 {
   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
-  uword * table_id, * vnip;
+  uword * dp_idp, * vnip, ** dp_table_by_vni, ** vni_by_dp_table;
 
   if (vnet_lisp_enable_disable_status () == 0)
     {
@@ -295,38 +643,48 @@ vnet_lisp_eid_table_map (u32 vni, u32 vrf, u8 is_add)
       return -1;
     }
 
-  if (vni == 0 || vrf == 0)
+  dp_table_by_vni = is_l2 ? &lcm->bd_id_by_vni : &lcm->table_id_by_vni;
+  vni_by_dp_table = is_l2 ? &lcm->vni_by_bd_id : &lcm->vni_by_table_id;
+
+  if (!is_l2 && (vni == 0 || dp_id == 0))
     {
       clib_warning ("can't add/del default vni-vrf mapping!");
       return -1;
     }
 
-  table_id = hash_get (lcm->table_id_by_vni, vni);
-  vnip = hash_get (lcm->vni_by_table_id, vrf);
+  dp_idp = hash_get (dp_table_by_vni[0], vni);
+  vnip = hash_get (vni_by_dp_table[0], dp_id);
 
   if (is_add)
     {
-      if (table_id || vnip)
+      if (dp_idp || vnip)
         {
-          clib_warning ("vni %d or vrf %d already used in any vrf/vni "
-                        "mapping!", vni, vrf);
+          clib_warning ("vni %d or vrf %d already used in vrf/vni "
+                        "mapping!", vni, dp_id);
           return -1;
         }
-      hash_set (lcm->table_id_by_vni, vni, vrf);
-      hash_set (lcm->vni_by_table_id, vrf, vni);
+      hash_set (dp_table_by_vni[0], vni, dp_id);
+      hash_set (vni_by_dp_table[0], dp_id, vni);
+
+      /* create dp iface */
+      dp_add_del_iface (lcm, vni, is_l2, 1);
     }
   else
     {
-      if (!table_id || !vnip)
+      if (!dp_idp || !vnip)
         {
           clib_warning ("vni %d or vrf %d not used in any vrf/vni! "
-                        "mapping!", vni, vrf);
+                        "mapping!", vni, dp_id);
           return -1;
         }
-      hash_unset (lcm->table_id_by_vni, vni);
-      hash_unset (lcm->vni_by_table_id, vrf);
+      hash_unset (dp_table_by_vni[0], vni);
+      hash_unset (vni_by_dp_table[0], dp_id);
+
+      /* remove dp iface */
+      dp_add_del_iface (lcm, vni, is_l2, 0);
     }
   return 0;
+
 }
 
 static clib_error_t *
@@ -334,8 +692,8 @@ lisp_eid_table_map_command_fn (vlib_main_t * vm,
                                unformat_input_t * input,
                                vlib_cli_command_t * cmd)
 {
-  u8 is_add = 1;
-  u32 vni = 0, vrf = 0;
+  u8 is_add = 1, is_l2 = 0;
+  u32 vni = 0, dp_id = 0;
   unformat_input_t _line_input, * line_input = &_line_input;
 
   /* Get a line of input. */
@@ -348,140 +706,71 @@ lisp_eid_table_map_command_fn (vlib_main_t * vm,
         is_add = 0;
       else if (unformat (line_input, "vni %d", &vni))
         ;
-      else if (unformat (line_input, "vrf %d", &vrf))
+      else if (unformat (line_input, "vrf %d", &dp_id))
         ;
+      else if (unformat (line_input, "bd %d", &dp_id))
+        is_l2 = 1;
       else
         {
           return unformat_parse_error (line_input);
         }
     }
-  vnet_lisp_eid_table_map (vni, vrf, is_add);
+  vnet_lisp_eid_table_map (vni, dp_id, is_l2, is_add);
   return 0;
 }
 
 VLIB_CLI_COMMAND (lisp_eid_table_map_command) = {
     .path = "lisp eid-table map",
-    .short_help = "lisp eid-table map [del] vni <vni> vrf <vrf>",
+    .short_help = "lisp eid-table map [del] vni <vni> vrf <vrf> | bd <bdi>",
     .function = lisp_eid_table_map_command_fn,
 };
 
-static int
-lisp_add_del_negative_static_mapping (gid_address_t * deid,
-    vnet_lisp_add_del_locator_set_args_t * ls, u8 action, u8 is_add)
-{
-  uword * p;
-  mapping_t * map;
-  u32 mi = ~0;
-  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
-  uword * refc;
-  vnet_lisp_add_del_mapping_args_t _dm_args, * dm_args = &_dm_args;
-  int rv = 0;
-  u32 ls_index = 0, dst_map_index;
-  vnet_lisp_gpe_add_del_iface_args_t _ai, *ai = &_ai;
-
-  memset (dm_args, 0, sizeof (dm_args[0]));
-  u32 vni = gid_address_vni (deid);
-  refc = hash_get (lcm->dp_if_refcount_by_vni, vni);
-
-  p = hash_get (lcm->table_id_by_vni, vni);
-  if (!p)
-    {
-      clib_warning ("vni %d not associated to a vrf!", vni);
-      return VNET_API_ERROR_INVALID_VALUE;
-    }
-
-  if (is_add)
-    {
-      vnet_lisp_add_del_locator_set (ls, &ls_index);
-      /* add mapping */
-      gid_address_copy (&dm_args->deid, deid);
-      dm_args->is_add = 1;
-      dm_args->action = action;
-      dm_args->locator_set_index = ls_index;
 
-      /* create interface or update refcount */
-      if (!refc)
-        {
-          vnet_lisp_gpe_add_del_iface_args_t _ai, *ai = &_ai;
-          ai->is_add = 1;
-          ai->vni = vni;
-          ai->table_id = p[0];
-          vnet_lisp_gpe_add_del_iface (ai, 0);
+/* 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;
 
-          /* counts the number of eids in a vni that use the interface */
-          hash_set (lcm->dp_if_refcount_by_vni, vni, 1);
-        }
-      else
-        refc[0]++;
+  if (vec_len (old_ls_indexes) != vec_len(new_locators))
+    return 1;
 
-      rv = vnet_lisp_add_del_local_mapping (dm_args, &dst_map_index);
-      if (!rv)
-        add_fwd_entry (lcm, lcm->pitr_map_index, dst_map_index);
-    }
-  else
+  for (i = 0; i < vec_len(new_locators); i++)
     {
-      mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, deid);
-      if ((u32)~0 == mi)
-        {
-          clib_warning ("eid %U marked for removal, but not found in "
-                        "map-cache!", unformat_gid_address, deid);
-          return VNET_API_ERROR_INVALID_VALUE;
-        }
-
-      /* delete forwarding entry */
-      del_fwd_entry (lcm, 0, mi);
-
-      dm_args->is_add = 0;
-      gid_address_copy (&dm_args->deid, deid);
-      map = pool_elt_at_index (lcm->mapping_pool, mi);
-      dm_args->locator_set_index = map->locator_set_index;
-
-      /* delete mapping associated to fwd entry */
-      vnet_lisp_add_del_mapping (dm_args, 0);
+      old_li = vec_elt(old_ls_indexes, i);
+      old_loc = pool_elt_at_index(lcm->locator_pool, old_li);
 
-      refc = hash_get (lcm->dp_if_refcount_by_vni, vni);
-      ASSERT(refc != 0);
-      refc[0]--;
+      new_loc = vec_elt_at_index(new_locators, i);
 
-      /* remove iface if needed */
-      if (refc[0] == 0)
-        {
-          ai->is_add = 0;
-          ai->vni = vni;
-          ai->table_id = p[0];
-          vnet_lisp_gpe_add_del_iface (ai, 0);
-          hash_unset (lcm->dp_if_refcount_by_vni, vni);
-        }
+      if (locator_cmp (old_loc, new_loc))
+        return 1;
     }
-  return rv;
+  return 0;
 }
 
 /**
- * Adds/removes/updates static remote mapping.
- *
- * This function also modifies forwarding entries if needed.
+ * Adds/removes/updates mapping. Does not program forwarding.
  *
- * @param deid destination EID
- * @param seid source EID
+ * @param eid end-host identifier
  * @param rlocs vector of remote locators
  * @param action action for negative map-reply
  * @param is_add add mapping if non-zero, delete otherwise
- * @param del_all if set, delete all remote mappings
+ * @param res_map_index the map-index that was created/updated/removed. It is
+ *                      set to ~0 if no action is taken.
  * @return return code
  */
 int
-vnet_lisp_add_del_remote_mapping (gid_address_t * deid, gid_address_t * seid,
-                                  ip_address_t * rlocs, u8 action, u8 is_add,
-                                  u8 del_all)
+vnet_lisp_add_del_mapping (gid_address_t * eid, locator_t * rlocs, u8 action,
+                           u8 authoritative, u32 ttl, u8 is_add,
+                           u32 * res_map_index)
 {
-  vnet_lisp_add_del_mapping_args_t _dm_args, * dm_args = &_dm_args;
-  vnet_lisp_add_del_mapping_args_t _sm_args, * sm_args = &_sm_args;
-  vnet_lisp_add_del_locator_set_args_t _ls, * ls = &_ls;
+  vnet_lisp_add_del_mapping_args_t _m_args, * m_args = &_m_args;
+  vnet_lisp_add_del_locator_set_args_t _ls_args, * ls_args = &_ls_args;
   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
-  u32 mi, ls_index = 0, dst_map_index, src_map_index;
-  locator_t loc;
-  ip_address_t * dl;
-  int rc = -1;
+  u32 mi, ls_index = 0, dst_map_index;
+  mapping_t * old_map;
 
   if (vnet_lisp_enable_disable_status() == 0)
     {
@@ -489,117 +778,94 @@ vnet_lisp_add_del_remote_mapping (gid_address_t * deid, gid_address_t * seid,
       return VNET_API_ERROR_LISP_DISABLED;
     }
 
-  if (del_all)
-    return vnet_lisp_clear_all_remote_mappings ();
+  if (res_map_index)
+    res_map_index[0] = ~0;
 
-  memset (sm_args, 0, sizeof (sm_args[0]));
-  memset (dm_args, 0, sizeof (dm_args[0]));
-  memset (ls, 0, sizeof (ls[0]));
+  memset (m_args, 0, sizeof (m_args[0]));
+  memset (ls_args, 0, sizeof (ls_args[0]));
 
-  /* prepare remote locator set */
-  vec_foreach (dl, rlocs)
-    {
-      memset (&loc, 0, sizeof (loc));
-      gid_address_ip (&loc.address) = dl[0];
-      vec_add1 (ls->locators, loc);
-    }
-  src_map_index = gid_dictionary_lookup (&lcm->mapping_index_by_gid, seid);
+  ls_args->locators = rlocs;
 
-  mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, deid);
-  /* new mapping */
-  if ((u32)~0 == mi)
+  mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, eid);
+  old_map = ((u32) ~0 != mi) ? pool_elt_at_index(lcm->mapping_pool, mi) : 0;
+
+  if (is_add)
     {
-      ls->is_add = 1;
-      ls->index = ~0;
+      /* overwrite: if mapping already exists, decide if locators should be
+       * updated and be done */
+      if (old_map && gid_address_cmp (&old_map->eid, eid) == 0)
+        {
+          locator_set_t * old_ls;
 
-      /* process a negative mapping */
-      if (0 == vec_len (rlocs))
-        return lisp_add_del_negative_static_mapping (deid, ls,
-                                                     action, is_add);
+          /* update mapping attributes */
+          old_map->action = action;
+          old_map->authoritative = authoritative;
+          old_map->ttl = ttl;
 
-      if ((u32)~0 == src_map_index)
-        {
-          clib_warning ("seid %U not found!", format_gid_address, seid);
-          goto done;
+          old_ls = pool_elt_at_index(lcm->locator_set_pool,
+                                     old_map->locator_set_index);
+          if (compare_locators (lcm, old_ls->locator_indices,
+                                ls_args->locators))
+            {
+              /* set locator-set index to overwrite */
+              ls_args->is_add = 1;
+              ls_args->index = old_map->locator_set_index;
+              vnet_lisp_add_del_locator_set (ls_args, 0);
+              if (res_map_index)
+                res_map_index[0] = mi;
+            }
         }
-
-      if (!is_add)
+      /* new mapping */
+      else
         {
-          clib_warning ("deid %U marked for removal but not "
-                        "found!", format_gid_address, deid);
-          goto done;
-        }
+          ls_args->is_add = 1;
+          ls_args->index = ~0;
 
-      vnet_lisp_add_del_locator_set (ls, &ls_index);
+          vnet_lisp_add_del_locator_set (ls_args, &ls_index);
 
-      /* add mapping */
-      gid_address_copy (&dm_args->deid, deid);
-      dm_args->is_add = 1;
-      dm_args->action = action;
-      dm_args->locator_set_index = ls_index;
-      vnet_lisp_add_del_mapping (dm_args, &dst_map_index);
+          /* add mapping */
+          gid_address_copy (&m_args->eid, eid);
+          m_args->is_add = 1;
+          m_args->action = action;
+          m_args->locator_set_index = ls_index;
+          vnet_lisp_map_cache_add_del (m_args, &dst_map_index);
 
-      /* add fwd tunnel */
-      add_fwd_entry (lcm, src_map_index, dst_map_index);
+          if (res_map_index)
+            res_map_index[0] = dst_map_index;
+        }
     }
   else
     {
-      /* delete mapping */
-      if (!is_add)
+      if (old_map == 0 || gid_address_cmp (&old_map->eid, eid) != 0)
         {
-          /* delete forwarding entry */
-          del_fwd_entry (lcm, 0, mi);
-          dm_args->is_add = 0;
-          gid_address_copy (&dm_args->deid, deid);
-          mapping_t * map = pool_elt_at_index (lcm->mapping_pool, mi);
-          dm_args->locator_set_index = map->locator_set_index;
-
-          /* delete mapping associated to fwd entry */
-          vnet_lisp_add_del_mapping (dm_args, 0);
-
-          ls->is_add = 0;
-          ls->index = map->locator_set_index;
-          /* delete locator set */
-          vnet_lisp_add_del_locator_set (ls, 0);
+          clib_warning("cannot delete mapping for eid %U", format_gid_address,
+                       eid);
+          return -1;
         }
-      /* update existing locator set */
-      else
-        {
-          if ((u32)~0 == src_map_index)
-            {
-              clib_warning ("seid %U not found!", format_gid_address, seid);
-              goto done;
-            }
 
-          mapping_t * old_map;
-          locator_set_t * old_ls;
-          old_map = pool_elt_at_index (lcm->mapping_pool, mi);
+      m_args->is_add = 0;
+      gid_address_copy (&m_args->eid, eid);
+      m_args->locator_set_index = old_map->locator_set_index;
 
-          /* update mapping attributes */
-          old_map->action = action;
+      /* delete mapping associated from map-cache */
+      vnet_lisp_map_cache_add_del (m_args, 0);
 
-          old_ls = pool_elt_at_index(lcm->locator_set_pool,
-                                     old_map->locator_set_index);
-          if (compare_locators (lcm, old_ls->locator_indices,
-                                ls->locators))
-            {
-              /* set locator-set index to overwrite */
-              ls->is_add = 1;
-              ls->index = old_map->locator_set_index;
-              vnet_lisp_add_del_locator_set (ls, 0);
-              add_fwd_entry (lcm, src_map_index, mi);
-            }
-        }
+      ls_args->is_add = 0;
+      ls_args->index = old_map->locator_set_index;
+      /* delete locator set */
+      vnet_lisp_add_del_locator_set (ls_args, 0);
+
+      /* return old mapping index */
+      if (res_map_index)
+        res_map_index[0] = mi;
     }
+
   /* success */
-  rc = 0;
-done:
-  vec_free (ls->locators);
-  return rc;
+  return 0;
 }
 
 int
-vnet_lisp_clear_all_remote_mappings (void)
+vnet_lisp_clear_all_remote_adjacencies (void)
 {
   int rv = 0;
   u32 mi, * map_indices = 0, * map_indexp;
@@ -617,14 +883,14 @@ vnet_lisp_clear_all_remote_mappings (void)
       mapping_t * map = pool_elt_at_index (lcm->mapping_pool, map_indexp[0]);
       if (!map->local)
         {
-          del_fwd_entry (lcm, 0, map_indexp[0]);
+          dp_del_fwd_entry (lcm, 0, map_indexp[0]);
 
           dm_args->is_add = 0;
-          gid_address_copy (&dm_args->deid, &map->eid);
+          gid_address_copy (&dm_args->eid, &map->eid);
           dm_args->locator_set_index = map->locator_set_index;
 
           /* delete mapping associated to fwd entry */
-          vnet_lisp_add_del_mapping (dm_args, 0);
+          vnet_lisp_map_cache_add_del (dm_args, 0);
 
           ls->is_add = 0;
           ls->local = 0;
@@ -642,6 +908,67 @@ cleanup:
   return rv;
 }
 
+/**
+ * Adds adjacency or removes forwarding entry associated to remote mapping.
+ * Note that adjacencies are not stored, they only result in forwarding entries
+ * being created.
+ */
+int
+lisp_add_del_adjacency (lisp_cp_main_t * lcm, gid_address_t * local_eid,
+                        gid_address_t * remote_eid, u8 is_add)
+{
+  u32 local_mi, remote_mi = ~0;
+
+  if (vnet_lisp_enable_disable_status () == 0)
+    {
+      clib_warning ("LISP is disabled!");
+      return VNET_API_ERROR_LISP_DISABLED;
+    }
+
+  remote_mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, remote_eid);
+  if (GID_LOOKUP_MISS == remote_mi)
+    {
+      clib_warning("Remote eid %U not found. Cannot add adjacency!",
+                   format_gid_address, remote_eid);
+
+      return -1;
+    }
+
+  if (is_add)
+    {
+      /* TODO 1) check if src/dst 2) once we have src/dst working, use it in
+       * delete*/
+
+      /* check if source eid has an associated mapping. If pitr mode is on,
+       * just use the pitr's mapping */
+      local_mi = lcm->lisp_pitr ? lcm->pitr_map_index :
+              gid_dictionary_lookup (&lcm->mapping_index_by_gid, local_eid);
+
+
+      if (GID_LOOKUP_MISS == local_mi)
+        {
+          clib_warning("Local eid %U not found. Cannot add adjacency!",
+                       format_gid_address, local_eid);
+
+          return -1;
+        }
+
+      /* update forwarding */
+      dp_add_fwd_entry (lcm, local_mi, remote_mi);
+    }
+  else
+    dp_del_fwd_entry (lcm, 0, remote_mi);
+
+  return 0;
+}
+
+int
+vnet_lisp_add_del_adjacency (vnet_lisp_add_del_adjacency_args_t * a)
+{
+  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
+  return lisp_add_del_adjacency(lcm, &a->seid, &a->deid, a->is_add);
+}
+
 /**
  * Handler for add/del remote mapping CLI.
  *
@@ -658,37 +985,163 @@ lisp_add_del_remote_mapping_command_fn (vlib_main_t * vm,
   clib_error_t * error = 0;
   unformat_input_t _line_input, * line_input = &_line_input;
   u8 is_add = 1, del_all = 0;
-  ip_address_t rloc, * rlocs = 0;
+  locator_t rloc, * rlocs = 0, * curr_rloc = 0;
+  gid_address_t eid;
+  u8 eid_set = 0;
+  u32 vni, action = ~0, p, w;
+  int rv;
+
+  /* Get a line of input. */
+  if (! unformat_user (input, unformat_line_input, line_input))
+    return 0;
+
+  memset(&eid, 0, sizeof(eid));
+  memset(&rloc, 0, sizeof(rloc));
+
+  while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
+    {
+      if (unformat (line_input, "del-all"))
+        del_all = 1;
+      else if (unformat (line_input, "del"))
+        is_add = 0;
+      else if (unformat (line_input, "add"))
+        ;
+      else if (unformat (line_input, "eid %U", unformat_gid_address, &eid))
+        eid_set = 1;
+      else if (unformat (line_input, "vni %u", &vni))
+        {
+          gid_address_vni (&eid) = vni;
+        }
+      else if (unformat (line_input, "p %d w %d", &p, &w))
+        {
+          if (!curr_rloc)
+            {
+              clib_warning ("No RLOC configured for setting priority/weight!");
+              goto done;
+            }
+          curr_rloc->priority = p;
+          curr_rloc->weight = w;
+        }
+      else if (unformat (line_input, "rloc %U", unformat_ip_address,
+                         &gid_address_ip(&rloc.address)))
+        {
+          vec_add1 (rlocs, rloc);
+          curr_rloc = &rlocs[vec_len (rlocs) - 1];
+        }
+      else if (unformat (line_input, "action %U",
+                         unformat_negative_mapping_action, &action))
+        ;
+      else
+        {
+          clib_warning ("parse error");
+          goto done;
+        }
+    }
+
+  if (!eid_set)
+    {
+      clib_warning ("missing eid!");
+      goto done;
+    }
+
+  if (!del_all)
+    {
+      if (is_add && (~0 == action)
+          && 0 == vec_len (rlocs))
+        {
+          clib_warning ("no action set for negative map-reply!");
+          goto done;
+        }
+    }
+  else
+    {
+      vnet_lisp_clear_all_remote_adjacencies ();
+      goto done;
+    }
+
+  /* TODO build src/dst with seid*/
+
+  /* if it's a delete, clean forwarding */
+  if (!is_add)
+    {
+      lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
+      rv = lisp_add_del_adjacency (lcm, 0, &eid, /* is_add */ 0);
+      if (rv)
+        {
+          goto done;
+        }
+    }
+
+  /* add as static remote mapping, i.e., not authoritative and infinite
+   * ttl */
+  rv = vnet_lisp_add_del_mapping (&eid, rlocs, action, 0, ~0, is_add, 0);
+
+  if (rv)
+    clib_warning("failed to %s remote mapping!", is_add ? "add" : "delete");
+
+done:
+  vec_free (rlocs);
+  unformat_free (line_input);
+  return error;
+}
+
+VLIB_CLI_COMMAND (lisp_add_del_remote_mapping_command) = {
+    .path = "lisp remote-mapping",
+    .short_help = "lisp remote-mapping add|del [del-all] vni <vni> "
+     "eid <est-eid> [action <no-action|natively-forward|"
+     "send-map-request|drop>] rloc <dst-locator> p <prio> w <weight> "
+     "[rloc <dst-locator> ... ]",
+    .function = lisp_add_del_remote_mapping_command_fn,
+};
+
+/**
+ * Handler for add/del adjacency CLI.
+ */
+static clib_error_t *
+lisp_add_del_adjacency_command_fn (vlib_main_t * vm, unformat_input_t * input,
+                                   vlib_cli_command_t * cmd)
+{
+  clib_error_t * error = 0;
+  unformat_input_t _line_input, * line_input = &_line_input;
+  vnet_lisp_add_del_adjacency_args_t _a, * a = &_a;
+  u8 is_add = 1;
+  locator_t rloc, * rlocs = 0;
   ip_prefix_t * deid_ippref, * seid_ippref;
   gid_address_t seid, deid;
+  u8 * dmac = gid_address_mac (&deid);
+  u8 * smac = gid_address_mac (&seid);
   u8 deid_set = 0, seid_set = 0;
   u8 * s = 0;
   u32 vni, action = ~0;
+  int rv;
 
   /* Get a line of input. */
   if (! unformat_user (input, unformat_line_input, line_input))
     return 0;
 
-  memset (&deid, 0, sizeof (deid));
-  memset (&seid, 0, sizeof (seid));
+  memset(&deid, 0, sizeof(deid));
+  memset(&seid, 0, sizeof(seid));
+  memset(&rloc, 0, sizeof(rloc));
 
   seid_ippref = &gid_address_ippref(&seid);
   deid_ippref = &gid_address_ippref(&deid);
 
-  gid_address_type (&deid) = GID_ADDR_IP_PREFIX;
-  gid_address_type (&seid) = GID_ADDR_IP_PREFIX;
-
   while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
     {
-      if (unformat (line_input, "del-all"))
-        del_all = 1;
-      else if (unformat (line_input, "del"))
+      if (unformat (line_input, "del"))
         is_add = 0;
       else if (unformat (line_input, "add"))
         ;
       else if (unformat (line_input, "deid %U",
                          unformat_ip_prefix, deid_ippref))
         {
+          gid_address_type (&deid) = GID_ADDR_IP_PREFIX;
+          deid_set = 1;
+        }
+      else if (unformat (line_input, "deid %U",
+                         unformat_mac_address, dmac))
+        {
+          gid_address_type (&deid) = GID_ADDR_MAC;
           deid_set = 1;
         }
       else if (unformat (line_input, "vni %u", &vni))
@@ -699,25 +1152,14 @@ lisp_add_del_remote_mapping_command_fn (vlib_main_t * vm,
       else if (unformat (line_input, "seid %U",
                          unformat_ip_prefix, seid_ippref))
         {
+          gid_address_type (&seid) = GID_ADDR_IP_PREFIX;
           seid_set = 1;
         }
-      else if (unformat (line_input, "rloc %U", unformat_ip_address, &rloc))
-        vec_add1 (rlocs, rloc);
-      else if (unformat (line_input, "action %s", &s))
-        {
-          if (!strcmp ((char *)s, "no-action"))
-            action = ACTION_NONE;
-          if (!strcmp ((char *)s, "natively-forward"))
-            action = ACTION_NATIVELY_FORWARDED;
-          if (!strcmp ((char *)s, "send-map-request"))
-            action = ACTION_SEND_MAP_REQUEST;
-          else if (!strcmp ((char *)s, "drop"))
-            action = ACTION_DROP;
-          else
-            {
-              clib_warning ("invalid action: '%s'", s);
-              goto done;
-            }
+      else if (unformat (line_input, "seid %U",
+                         unformat_mac_address, smac))
+        {
+          gid_address_type (&seid) = GID_ADDR_MAC;
+          seid_set = 1;
         }
       else
         {
@@ -726,41 +1168,46 @@ lisp_add_del_remote_mapping_command_fn (vlib_main_t * vm,
         }
     }
 
-  if (!del_all)
-    {
-      if (!deid_set)
-        {
-          clib_warning ("missing deid!");
-          goto done;
-        }
+  if (!deid_set)
+    {
+      clib_warning ("missing deid!");
+      goto done;
+    }
 
-      /* if seid not set, make sure the ip version is the same as that of the
-       * deid. This ensures the seid to be configured will be either 0/0 or
-       * ::/0 */
+  if (GID_ADDR_IP_PREFIX == gid_address_type (&deid))
+    {
+      /* if seid not set, make sure the ip version is the same as that
+       * of the deid. This ensures the seid to be configured will be
+       * either 0/0 or ::/0 */
       if (!seid_set)
         ip_prefix_version(seid_ippref) = ip_prefix_version(deid_ippref);
 
       if (is_add &&
-          (ip_prefix_version (deid_ippref) != ip_prefix_version(seid_ippref)))
+          (ip_prefix_version (deid_ippref)
+           != ip_prefix_version(seid_ippref)))
         {
           clib_warning ("source and destination EIDs are not"
                         " in the same IP family!");
           goto done;
         }
+    }
 
-      if (is_add && (~0 == action)
-          && 0 == vec_len (rlocs))
-        {
-          clib_warning ("no action set for negative map-reply!");
-          goto done;
-        }
+  if (is_add && (~0 == action)
+      && 0 == vec_len (rlocs))
+    {
+      clib_warning ("no action set for negative map-reply!");
+      goto done;
     }
 
-  int rv = vnet_lisp_add_del_remote_mapping (&deid, &seid, rlocs,
-                                             action, is_add, del_all);
+  memset(a, 0, sizeof(a[0]));
+  gid_address_copy (&a->seid, &deid);
+  gid_address_copy (&a->deid, &seid);
+
+  a->is_add = is_add;
+  rv = vnet_lisp_add_del_adjacency (a);
+
   if (rv)
-    clib_warning ("failed to %s remote mapping!",
-                  is_add ? "add" : "delete");
+    clib_warning("failed to %s adjacency!", is_add ? "add" : "delete");
 
 done:
   unformat_free (line_input);
@@ -769,25 +1216,26 @@ done:
   return error;
 }
 
-VLIB_CLI_COMMAND (lisp_add_del_remote_mapping_command) = {
-    .path = "lisp remote-mapping",
-    .short_help = "lisp remote-mapping add|del [del-all] vni <vni>"
+VLIB_CLI_COMMAND (lisp_add_del_adjacency_command) = {
+    .path = "lisp adjacency",
+    .short_help = "lisp adjacency add|del vni <vni>"
      "deid <dest-eid> seid <src-eid> [action <no-action|natively-forward|"
      "send-map-request|drop>] rloc <dst-locator> [rloc <dst-locator> ... ]",
-    .function = lisp_add_del_remote_mapping_command_fn,
+    .function = lisp_add_del_adjacency_command_fn,
 };
 
+
 static clib_error_t *
 lisp_show_map_resolvers_command_fn (vlib_main_t * vm,
                                     unformat_input_t * input,
                                     vlib_cli_command_t * cmd)
 {
-  ip_address_t * addr;
+  map_resolver_t * mr;
   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
 
-  vec_foreach (addr, lcm->map_resolvers)
+  vec_foreach (mr, lcm->map_resolvers)
     {
-      vlib_cli_output (vm, "%U", format_ip_address, addr);
+      vlib_cli_output (vm, "%U", format_ip_address, &mr->address);
     }
   return 0;
 }
@@ -891,14 +1339,61 @@ VLIB_CLI_COMMAND (lisp_pitr_set_locator_set_command) = {
     .function = lisp_pitr_set_locator_set_command_fn,
 };
 
+static clib_error_t *
+lisp_show_pitr_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 * m;
+  locator_set_t * ls;
+  u8 * tmp_str = 0;
+
+  vlib_cli_output (vm, "%=20s%=16s",
+                   "pitr", lcm->lisp_pitr ? "locator-set" : "");
+
+  if (!lcm->lisp_pitr) {
+    vlib_cli_output (vm, "%=20s", "disable");
+    return 0;
+  }
+
+  if (~0 == lcm->pitr_map_index) {
+    tmp_str = format(0, "N/A");
+  } else {
+    m = pool_elt_at_index (lcm->mapping_pool, lcm->pitr_map_index);
+    if (~0 != m->locator_set_index) {
+      ls = pool_elt_at_index (lcm->locator_set_pool, m->locator_set_index);
+      tmp_str = format(0, "%s", ls->name);
+    } else {
+      tmp_str = format(0, "N/A");
+    }
+  }
+  vec_add1(tmp_str, 0);
+
+  vlib_cli_output (vm, "%=20s%=16s",
+                   "enable", tmp_str);
+
+  vec_free(tmp_str);
+
+  return 0;
+}
+
+VLIB_CLI_COMMAND (lisp_show_pitr_command) = {
+    .path = "show lisp pitr",
+    .short_help = "Show pitr",
+    .function = lisp_show_pitr_command_fn,
+};
 
 static u8 *
 format_eid_entry (u8 * s, va_list * args)
 {
   vnet_main_t * vnm = va_arg (*args, vnet_main_t *);
   lisp_cp_main_t * lcm = va_arg (*args, lisp_cp_main_t *);
-  gid_address_t * gid = va_arg (*args, gid_address_t *);
+  mapping_t * mapit = va_arg (*args, mapping_t *);
   locator_set_t * ls = va_arg (*args, locator_set_t *);
+  gid_address_t * gid = &mapit->eid;
+  u32 ttl = mapit->ttl;
+  u8 aut = mapit->authoritative;
   u32 * loc_index;
   u8 first_line = 1;
   u8 * loc;
@@ -908,7 +1403,8 @@ format_eid_entry (u8 * s, va_list * args)
 
   if (vec_len (ls->locator_indices) == 0)
     {
-      s = format (s, "%-35U%-20s", format_gid_address, gid, type);
+      s = format (s, "%-35U%-30s%-20u%-u", format_gid_address, gid,
+                                           type, ttl, aut);
     }
   else
     {
@@ -924,8 +1420,8 @@ format_eid_entry (u8 * s, va_list * args)
 
           if (first_line)
             {
-              s = format (s, "%-35U%-20s%-v\n", format_gid_address,
-                          gid, type, loc);
+              s = format (s, "%-35U%-20s%-30v%-20u%-u\n", format_gid_address,
+                          gid, type, loc, ttl, aut);
               first_line = 0;
             }
           else
@@ -942,15 +1438,68 @@ lisp_show_eid_table_command_fn (vlib_main_t * vm,
 {
   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
   mapping_t * mapit;
+  unformat_input_t _line_input, * line_input = &_line_input;
+  u32 mi;
+  gid_address_t eid;
+  u8 print_all = 1;
+  u8 filter = 0;
 
-  vlib_cli_output (vm, "%-35s%-20s%-s", "EID", "type", "locators");
-  pool_foreach (mapit, lcm->mapping_pool,
-  ({
-    locator_set_t * ls = pool_elt_at_index (lcm->locator_set_pool,
-                                            mapit->locator_set_index);
-    vlib_cli_output (vm, "%U", format_eid_entry, lcm->vnet_main,
-                     lcm, &mapit->eid, ls);
-  }));
+  memset (&eid, 0, sizeof(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, "eid %U", unformat_gid_address, &eid))
+        print_all = 0;
+      else if (unformat (line_input, "local"))
+        filter = 1;
+      else if (unformat(line_input, "remote"))
+        filter = 2;
+      else
+        return clib_error_return (0, "parse error: '%U'",
+                                  format_unformat_error, line_input);
+    }
+
+  vlib_cli_output (vm, "%-35s%-20s%-30s%-20s%-s",
+                   "EID", "type", "locators", "ttl", "autoritative");
+
+  if (print_all)
+    {
+      pool_foreach (mapit, lcm->mapping_pool,
+      ({
+        locator_set_t * ls = pool_elt_at_index (lcm->locator_set_pool,
+                                                mapit->locator_set_index);
+        if (filter && !((1 == filter && ls->local) ||
+          (2 == filter && !ls->local)))
+          {
+            continue;
+          }
+        vlib_cli_output (vm, "%U", format_eid_entry, lcm->vnet_main,
+                         lcm, mapit, ls);
+      }));
+    }
+  else
+    {
+      mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &eid);
+      if ((u32)~0 == mi)
+        return 0;
+
+      mapit = pool_elt_at_index (lcm->mapping_pool, mi);
+      locator_set_t * ls = pool_elt_at_index (lcm->locator_set_pool,
+                                              mapit->locator_set_index);
+
+      if (filter && !((1 == filter && ls->local) ||
+        (2 == filter && !ls->local)))
+        {
+          return 0;
+        }
+
+      vlib_cli_output (vm, "%U,", format_eid_entry, lcm->vnet_main,
+                       lcm, mapit, ls);
+    }
 
   return 0;
 }
@@ -1002,9 +1551,8 @@ clean_locator_to_locator_set (lisp_cp_main_t * lcm, u32 lsi)
     }
 }
 
-static inline
-uword *get_locator_set_index(vnet_lisp_add_del_locator_set_args_t * a,
-                             uword * p)
+static inline uword *
+get_locator_set_index (vnet_lisp_add_del_locator_set_args_t * a, uword * p)
 {
   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
 
@@ -1024,9 +1572,9 @@ uword *get_locator_set_index(vnet_lisp_add_del_locator_set_args_t * a,
   return p;
 }
 
-static inline
-int is_locator_in_locator_set(lisp_cp_main_t * lcm, locator_set_t * ls,
-                              locator_t * loc)
+static inline int
+is_locator_in_locator_set (lisp_cp_main_t * lcm, locator_set_t * ls,
+                           locator_t * loc)
 {
   locator_t * itloc;
   u32 * locit;
@@ -1037,8 +1585,8 @@ int is_locator_in_locator_set(lisp_cp_main_t * lcm, locator_set_t * ls,
   vec_foreach(locit, ls->locator_indices)
     {
       itloc = pool_elt_at_index(lcm->locator_pool, locit[0]);
-      if (itloc->sw_if_index == loc->sw_if_index ||
-          !gid_address_cmp(&itloc->address, &loc->address))
+      if ((ls->local && itloc->sw_if_index == loc->sw_if_index) ||
+          (!ls->local && !gid_address_cmp(&itloc->address, &loc->address)))
         {
           clib_warning("Duplicate locator");
           return VNET_API_ERROR_VALUE_EXIST;
@@ -1048,9 +1596,9 @@ int is_locator_in_locator_set(lisp_cp_main_t * lcm, locator_set_t * ls,
   return 0;
 }
 
-static inline
-void remove_locator_from_locator_set(locator_set_t * ls, u32 * locit,
-                                     u32 ls_index, u32 loc_id)
+static inline void
+remove_locator_from_locator_set (locator_set_t * ls, u32 * locit, u32 ls_index,
+                                 u32 loc_id)
 {
   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
   u32 ** ls_indexes = NULL;
@@ -1291,16 +1839,14 @@ vnet_lisp_add_del_locator_set (vnet_lisp_add_del_locator_set_args_t * a,
 }
 
 clib_error_t *
-vnet_lisp_enable_disable (u8 is_enabled)
+vnet_lisp_enable_disable (u8 is_enable)
 {
-  vnet_lisp_gpe_add_del_iface_args_t _ai, * ai= &_ai;
-  uword * table_id, * refc;
-  u32 i;
+  u32 vni, dp_table;
   clib_error_t * error = 0;
   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
   vnet_lisp_gpe_enable_disable_args_t _a, * a = &_a;
 
-  a->is_en = is_enabled;
+  a->is_en = is_enable;
   error = vnet_lisp_gpe_enable_disable (a);
   if (error)
     {
@@ -1308,54 +1854,34 @@ vnet_lisp_enable_disable (u8 is_enabled)
                                 a->is_en ? "enable" : "disable");
     }
 
-  if (is_enabled)
+  if (is_enable)
     {
-      /* enable all ifaces */
-      for (i = 0; i < vec_len (lcm->local_mappings_indexes); i++)
-        {
-          mapping_t * m = vec_elt_at_index (lcm->mapping_pool, i);
-          ai->is_add = 1;
-          ai->vni = gid_address_vni (&m->eid);
-
-          refc = hash_get (lcm->dp_if_refcount_by_vni, ai->vni);
-          if (!refc)
-            {
-              table_id = hash_get (lcm->table_id_by_vni, ai->vni);
-              if (table_id)
-                {
-                  ai->table_id = table_id[0];
-                  /* enables interface and adds defaults */
-                  vnet_lisp_gpe_add_del_iface (ai, 0);
-                }
-              else
-                return clib_error_return (0, "no table_id found for vni %u!",
-                                          ai->vni);
+      /* enable all l2 and l3 ifaces */
+      hash_foreach(vni, dp_table, lcm->table_id_by_vni, ({
+        dp_add_del_iface(lcm, vni, 0, 1);
+      }));
 
-              hash_set (lcm->dp_if_refcount_by_vni, ai->vni, 1);
-            }
-          else
-            {
-              refc[0]++;
-            }
-        }
+      hash_foreach(vni, dp_table, lcm->bd_id_by_vni, ({
+        dp_add_del_iface(lcm, vni, /* is_l2 */ 1, 1);
+      }));
     }
   else
     {
-      /* clear refcount table */
-      hash_free (lcm->dp_if_refcount_by_vni);
-      hash_free (lcm->fwd_entry_by_mapping_index);
-      pool_free (lcm->fwd_entry_pool);
+      /* clear interface table */
+      hash_free(lcm->dp_intf_by_vni);
+      hash_free(lcm->fwd_entry_by_mapping_index);
+      pool_free(lcm->fwd_entry_pool);
     }
 
   /* update global flag */
-  lcm->is_enabled = is_enabled;
+  lcm->is_enabled = is_enable;
 
   return 0;
 }
 
 static clib_error_t *
 lisp_enable_disable_command_fn (vlib_main_t * vm, unformat_input_t * input,
-                                   vlib_cli_command_t * cmd)
+                                vlib_cli_command_t * cmd)
 {
   unformat_input_t _line_input, * line_input = &_line_input;
   u8 is_enabled = 0;
@@ -1600,7 +2126,14 @@ lisp_cp_show_locator_sets_command_fn (vlib_main_t * vm,
   ({
     u8 * msg = 0;
     int next_line = 0;
-    msg = format (msg, "%=16v", lsit->name);
+    if (lsit->local)
+      {
+        msg = format (msg, "%=16v", lsit->name);
+      }
+    else
+      {
+        msg = format (msg, "%=16s", "remote");
+      }
     vec_foreach (locit, lsit->locator_indices)
       {
         if (next_line)
@@ -1613,7 +2146,7 @@ lisp_cp_show_locator_sets_command_fn (vlib_main_t * vm,
                         loc->weight);
         else
           msg = format (msg, "%16U%16d%16d\n", format_ip_address,
-                        gid_address_ip(&loc->address), loc->priority,
+                        &gid_address_ip(&loc->address), loc->priority,
                         loc->weight);
         next_line = 1;
       }
@@ -1629,12 +2162,28 @@ VLIB_CLI_COMMAND (lisp_cp_show_locator_sets_command) = {
     .function = lisp_cp_show_locator_sets_command_fn,
 };
 
+static map_resolver_t *
+get_map_resolver (ip_address_t * a)
+{
+  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
+  map_resolver_t * mr;
+
+  vec_foreach (mr, lcm->map_resolvers)
+    {
+      if (!ip_address_cmp (&mr->address, a))
+        {
+          return mr;
+        }
+    }
+  return 0;
+}
+
 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;
+  map_resolver_t _mr, * mr = &_mr;
 
   if (vnet_lisp_enable_disable_status () == 0)
     {
@@ -1644,25 +2193,32 @@ vnet_lisp_add_del_map_resolver (vnet_lisp_add_del_map_resolver_args_t * a)
 
   if (a->is_add)
     {
-      vec_foreach(addr, lcm->map_resolvers)
+
+      if (get_map_resolver (&a->address))
         {
-          if (!ip_address_cmp (addr, &a->address))
-            {
-              clib_warning("map-resolver %U already exists!", format_ip_address,
-                           &a->address);
-              return -1;
-            }
+          clib_warning("map-resolver %U already exists!", format_ip_address,
+                       &a->address);
+          return -1;
         }
-      vec_add1(lcm->map_resolvers, a->address);
+
+      memset (mr, 0, sizeof (*mr));
+      ip_address_copy(&mr->address, &a->address);
+      vec_add1(lcm->map_resolvers, *mr);
+
+      if (vec_len (lcm->map_resolvers) == 1)
+        lcm->do_map_resolver_election = 1;
     }
   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))
+          mr = vec_elt_at_index(lcm->map_resolvers, i);
+          if (!ip_address_cmp (&mr->address, &a->address))
             {
-              vec_delete(lcm->map_resolvers, 1, i);
+              if (!ip_address_cmp (&mr->address, &lcm->active_map_resolver))
+                lcm->do_map_resolver_election = 1;
+
+              vec_del1 (lcm->map_resolvers, i);
               break;
             }
         }
@@ -1672,11 +2228,11 @@ vnet_lisp_add_del_map_resolver (vnet_lisp_add_del_map_resolver_args_t * a)
 
 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 * input,
+                                      vlib_cli_command_t * cmd)
 {
   unformat_input_t _line_input, * line_input = &_line_input;
-  u8 is_add = 1;
+  u8 is_add = 1, addr_set = 0;
   ip_address_t ip_addr;
   clib_error_t * error = 0;
   int rv = 0;
@@ -1693,13 +2249,20 @@ lisp_add_del_map_resolver_command_fn (vlib_main_t * vm,
       else if (unformat (line_input, "del"))
         is_add = 0;
       else if (unformat (line_input, "%U", unformat_ip_address, &ip_addr))
-        ;
+        addr_set = 1;
       else
         {
           error = unformat_parse_error(line_input);
           goto done;
         }
     }
+
+  if (!addr_set)
+    {
+      error = clib_error_return(0, "Map-resolver address must be set!");
+      goto done;
+    }
+
   a->is_add = is_add;
   a->address = ip_addr;
   rv = vnet_lisp_add_del_map_resolver (a);
@@ -1725,12 +2288,11 @@ vnet_lisp_add_del_mreq_itr_rlocs (vnet_lisp_add_del_mreq_itr_rloc_args_t * a)
   lisp_cp_main_t * lcm = vnet_lisp_cp_get_main();
   uword * p = 0;
 
-  //TODO: Wait for merge https://gerrit.fd.io/r/#/c/1427/
-//   if (vnet_lisp_enable_disable_status () == 0)
-//     {
-//       clib_warning ("LISP is disabled!");
-//       return VNET_API_ERROR_LISP_DISABLED;
-//     }
+  if (vnet_lisp_enable_disable_status () == 0)
+    {
+      clib_warning("LISP is disabled!");
+      return VNET_API_ERROR_LISP_DISABLED;
+    }
 
   if (a->is_add)
     {
@@ -1752,9 +2314,9 @@ vnet_lisp_add_del_mreq_itr_rlocs (vnet_lisp_add_del_mreq_itr_rloc_args_t * a)
 }
 
 static clib_error_t *
-lisp_add_del_mreq_itr_rlocs_command_fn(vlib_main_t * vm,
-                                       unformat_input_t * input,
-                                       vlib_cli_command_t * cmd)
+lisp_add_del_mreq_itr_rlocs_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;
@@ -1858,152 +2420,30 @@ typedef enum
 } lisp_cp_lookup_next_t;
 
 typedef struct
-{
-  gid_address_t dst_eid;
-  ip_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_ip_address, &t->map_resolver_ip, format_gid_address,
-              &t->dst_eid);
-  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_address (ip_lookup_main_t * lm, u32 sw_if_index,
-                                u8 version)
-{
-  ip_interface_address_t * ia;
-
-  ia = ip_interface_get_first_interface_address (lm, sw_if_index, 1);
-  if (!ia)
-    return 0;
-  return ip_interface_address_get_address (lm, ia);
-}
-
-int
-ip_interface_get_first_ip_address (lisp_cp_main_t * lcm, u32 sw_if_index,
-                                   u8 version, ip_address_t * result)
-{
-  ip_lookup_main_t * lm;
-  void * addr;
-
-  lm = (version == IP4) ? &lcm->im4->lookup_main : &lcm->im6->lookup_main;
-  addr = ip_interface_get_first_address (lm, sw_if_index, version);
-  if (!addr)
-    return 0;
-
-  ip_address_set (result, addr, version);
-  return 1;
-}
-
-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));
-}
-
-u32
-ip_fib_get_egress_iface_for_dst_with_lm (lisp_cp_main_t * lcm,
-                                         ip_address_t * dst,
-                                         ip_lookup_main_t * lm)
-{
-  u32 adj_index;
-  ip_adjacency_t * adj;
-
-  adj_index = ip_fib_lookup_with_table (lcm, 0, dst);
-  adj = ip_get_adjacency (lm, adj_index);
-
-  if (adj == 0)
-    return ~0;
-
-  /* we only want outgoing routes */
-  if (adj->lookup_next_index != IP_LOOKUP_NEXT_ARP
-      && adj->lookup_next_index != IP_LOOKUP_NEXT_REWRITE)
-    return ~0;
-
-  return adj->rewrite_header.sw_if_index;
-}
-
-/**
- * Find the sw_if_index of the interface that would be used to egress towards
- * dst.
- */
-u32
-ip_fib_get_egress_iface_for_dst (lisp_cp_main_t * lcm, ip_address_t * dst)
-{
-  ip_lookup_main_t * lm;
-
-  lm = ip_addr_version (dst) == IP4 ?
-      &lcm->im4->lookup_main : &lcm->im6->lookup_main;
-
-  return ip_fib_get_egress_iface_for_dst_with_lm (lcm, dst, lm);
-}
-
-/**
- * Find first IP of the interface that would be used to egress towards dst.
- * Returns 1 if the address is found 0 otherwise.
- */
-int
-ip_fib_get_first_egress_ip_for_dst (lisp_cp_main_t * lcm, ip_address_t * dst,
-                                    ip_address_t * result)
-{
-  u32 si;
-  ip_lookup_main_t * lm;
-  void * addr = 0;
-  u8 ipver;
-
-  ASSERT(result != 0);
-
-  ipver = ip_addr_version(dst);
-
-  lm = (ipver == IP4) ? &lcm->im4->lookup_main : &lcm->im6->lookup_main;
-  si = ip_fib_get_egress_iface_for_dst_with_lm (lcm, dst, lm);
-
-  if ((u32) ~0 == si)
-    return 0;
+{
+  gid_address_t dst_eid;
+  ip_address_t map_resolver_ip;
+} lisp_cp_lookup_trace_t;
 
-  /* find the first ip address */
-  addr = ip_interface_get_first_address (lm, si, ipver);
-  if (0 == addr)
-    return 0;
+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 *);
 
-  ip_address_set (result, addr, ipver);
-  return 1;
+  s = format (s, "LISP-CP-LOOKUP: map-resolver: %U destination eid %U",
+              format_ip_address, &t->map_resolver_ip, format_gid_address,
+              &t->dst_eid);
+  return s;
 }
 
 int
 get_mr_and_local_iface_ip (lisp_cp_main_t * lcm, ip_address_t * mr_ip,
                            ip_address_t * sloc)
 {
-  ip_address_t * mrit;
+  map_resolver_t * mrit;
+  ip_address_t * a;
 
   if (vec_len(lcm->map_resolvers) == 0)
     {
@@ -2015,10 +2455,14 @@ get_mr_and_local_iface_ip (lisp_cp_main_t * lcm, ip_address_t * mr_ip,
    * iface that has a route to it */
   vec_foreach(mrit, lcm->map_resolvers)
     {
-      if (0 != ip_fib_get_first_egress_ip_for_dst (lcm, mrit, sloc)) {
-          ip_address_copy(mr_ip, mrit);
+      a = &mrit->address;
+      if (0 != ip_fib_get_first_egress_ip_for_dst (lcm, a, sloc))
+        {
+          ip_address_copy(mr_ip, a);
+
+          /* also update globals */
           return 1;
-      }
+        }
     }
 
   clib_warning("Can't find map-resolver and local interface ip!");
@@ -2052,6 +2496,7 @@ build_itr_rloc_list (lisp_cp_main_t * lcm, locator_set_t * loc_set)
        addr = ip_interface_address_get_address (&lcm->im4->lookup_main, ia);
        ip_address_set (rloc, addr, IP4);
         ip_prefix_len (ippref) = 32;
+        ip_prefix_normalize (ippref);
         vec_add1 (rlocs, gid[0]);
       }));
 
@@ -2062,6 +2507,7 @@ build_itr_rloc_list (lisp_cp_main_t * lcm, locator_set_t * loc_set)
         addr = ip_interface_address_get_address (&lcm->im6->lookup_main, ia);
         ip_address_set (rloc, addr, IP6);
         ip_prefix_len (ippref) = 128;
+        ip_prefix_normalize (ippref);
         vec_add1 (rlocs, gid[0]);
       }));
     }
@@ -2069,7 +2515,7 @@ build_itr_rloc_list (lisp_cp_main_t * lcm, locator_set_t * loc_set)
 }
 
 static vlib_buffer_t *
-build_encapsulated_map_request (vlib_main_t * vm, lisp_cp_main_t *lcm,
+build_encapsulated_map_request (lisp_cp_main_t *lcm,
                                 gid_address_t * seid, gid_address_t * deid,
                                 locator_set_t * loc_set, ip_address_t * mr_ip,
                                 ip_address_t * sloc, u8 is_smr_invoked,
@@ -2078,6 +2524,7 @@ build_encapsulated_map_request (vlib_main_t * vm, lisp_cp_main_t *lcm,
   vlib_buffer_t * b;
   u32 bi;
   gid_address_t * rlocs = 0;
+  vlib_main_t * vm = lcm->vlib_main;
 
   if (vlib_buffer_alloc (vm, &bi, 1) != 1)
     {
@@ -2110,20 +2557,70 @@ build_encapsulated_map_request (vlib_main_t * vm, lisp_cp_main_t *lcm,
 }
 
 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)
+reset_pending_mr_counters (pending_map_request_t * r)
+{
+  r->time_to_expire = PENDING_MREQ_EXPIRATION_TIME;
+  r->retries_num = 0;
+}
+
+static int
+elect_map_resolver (lisp_cp_main_t * lcm)
+{
+  map_resolver_t * mr;
+
+  vec_foreach (mr, lcm->map_resolvers)
+    {
+      if (!mr->is_down)
+        {
+          ip_address_copy (&lcm->active_map_resolver, &mr->address);
+          lcm->do_map_resolver_election = 0;
+          return 1;
+        }
+    }
+  return 0;
+}
+
+#define send_encapsulated_map_request(lcm, seid, deid, smr) \
+  _send_encapsulated_map_request(lcm, seid, deid, smr, 0)
+
+#define resend_encapsulated_map_request(lcm, seid, deid, smr) \
+  _send_encapsulated_map_request(lcm, seid, deid, smr, 1)
+
+static int
+_send_encapsulated_map_request (lisp_cp_main_t *lcm,
+                                gid_address_t * seid, gid_address_t * deid,
+                                u8 is_smr_invoked, u8 is_resend)
 {
+  map_resolver_t * mr;
   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;
-  ip_address_t mr_ip, sloc;
+  pending_map_request_t * pmr, * duplicate_pmr = 0;
+  ip_address_t sloc;
   u32 ls_index;
 
+  ASSERT (*lcm->pending_map_request_lock);
+
+  /* if there is already a pending request remember it */
+  pool_foreach(pmr, lcm->pending_map_requests_pool,
+  ({
+    if (!gid_address_cmp (&pmr->src, seid)
+        && !gid_address_cmp (&pmr->dst, deid))
+      {
+        duplicate_pmr = pmr;
+        break;
+      }
+  }));
+
+  if (!is_resend && duplicate_pmr)
+    {
+      /* don't send the request if there is a pending map request already */
+      return 0;
+    }
+
   /* get locator-set for seid */
   if (!lcm->lisp_pitr)
     {
@@ -2132,7 +2629,7 @@ send_encapsulated_map_request (vlib_main_t * vm, lisp_cp_main_t *lcm,
         {
           clib_warning("No local mapping found in eid-table for %U!",
                        format_gid_address, seid);
-          return;
+          return -1;
         }
 
       map = pool_elt_at_index (lcm->mapping_pool, map_index);
@@ -2141,7 +2638,7 @@ send_encapsulated_map_request (vlib_main_t * vm, lisp_cp_main_t *lcm,
         {
           clib_warning("Mapping found for src eid %U is not marked as local!",
                        format_gid_address, seid);
-          return;
+          return -1;
         }
       ls_index = map->locator_set_index;
     }
@@ -2160,41 +2657,80 @@ send_encapsulated_map_request (vlib_main_t * vm, lisp_cp_main_t *lcm,
 
   loc_set = pool_elt_at_index (lcm->locator_set_pool, ls_index);
 
-  /* get local iface ip to use in map-request */
-  if (0 == get_mr_and_local_iface_ip (lcm, &mr_ip, &sloc))
-    return;
+  while (lcm->do_map_resolver_election
+         | (0 == ip_fib_get_first_egress_ip_for_dst (lcm,
+                                                     &lcm->active_map_resolver,
+                                                     &sloc)))
+    {
+      if (0 == elect_map_resolver (lcm))
+        /* all Mrs are down */
+        {
+          if (duplicate_pmr)
+            duplicate_pmr->to_be_removed = 1;
+
+          /* restart MR checking by marking all of them up */
+          vec_foreach (mr, lcm->map_resolvers)
+          mr->is_down = 0;
+
+          return -1;
+        }
+    }
 
   /* build the encapsulated map request */
-  b = build_encapsulated_map_request (vm, lcm, seid, deid, loc_set, &mr_ip,
+  b = build_encapsulated_map_request (lcm, seid, deid, loc_set,
+                                      &lcm->active_map_resolver,
                                       &sloc, is_smr_invoked, &nonce, &bi);
 
   if (!b)
-    return;
+    return -1;
 
-  /* set fib index and lookup node */
-  vnet_buffer(b)->sw_if_index[VLIB_TX] = ~0;
-  next_index = (ip_addr_version(&mr_ip) == IP4) ?
+  /* set fib index to default and lookup node */
+  vnet_buffer(b)->sw_if_index[VLIB_TX] = 0;
+  next_index = (ip_addr_version(&lcm->active_map_resolver) == IP4) ?
       ip4_lookup_node.index : ip6_lookup_node.index;
 
-  f = vlib_get_frame_to_node (vm, next_index);
+  f = vlib_get_frame_to_node (lcm->vlib_main, 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);
+  vlib_put_frame_to_node (lcm->vlib_main, next_index, f);
+
+  if (duplicate_pmr)
+    /* if there is a pending request already update it */
+    {
+      if (vec_len (duplicate_pmr->nonces) >= PENDING_MREQ_QUEUE_LEN)
+        {
+          /* remove the oldest nonce */
+          u64 * nonce_del = vec_elt_at_index (duplicate_pmr->nonces, 0);
+          hash_unset (lcm->pending_map_requests_by_nonce, nonce_del[0]);
+          vec_del1 (duplicate_pmr->nonces, 0);
+        }
+
+      vec_add1 (duplicate_pmr->nonces, nonce);
+      hash_set (lcm->pending_map_requests_by_nonce, nonce,
+                duplicate_pmr - lcm->pending_map_requests_pool);
+    }
+  else
+    {
+      /* add map-request to pending requests table */
+      pool_get(lcm->pending_map_requests_pool, pmr);
+      memset (pmr, 0, sizeof (*pmr));
+      gid_address_copy (&pmr->src, seid);
+      gid_address_copy (&pmr->dst, deid);
+      vec_add1 (pmr->nonces, nonce);
+      pmr->is_smr_invoked = is_smr_invoked;
+      reset_pending_mr_counters (pmr);
+      hash_set (lcm->pending_map_requests_by_nonce, nonce,
+                pmr - lcm->pending_map_requests_pool);
+    }
 
-  /* 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);
+  return 0;
 }
 
 static void
-get_src_and_dst (void *hdr, ip_address_t * src, ip_address_t *dst)
+get_src_and_dst_ip (void *hdr, ip_address_t * src, ip_address_t *dst)
 {
   ip4_header_t * ip4 = hdr;
   ip6_header_t * ip6;
@@ -2213,11 +2749,11 @@ get_src_and_dst (void *hdr, ip_address_t * src, ip_address_t *dst)
 }
 
 static u32
-lisp_get_vni_from_buffer (vlib_buffer_t * b, u8 version)
+lisp_get_vni_from_buffer_ip (lisp_cp_main_t * lcm, vlib_buffer_t * b,
+                             u8 version)
 {
   uword * vnip;
   u32 vni = ~0, table_id = ~0, fib_index;
-  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
 
   if (version == IP4)
     {
@@ -2249,6 +2785,81 @@ lisp_get_vni_from_buffer (vlib_buffer_t * b, u8 version)
   return vni;
 }
 
+always_inline u32
+lisp_get_vni_from_buffer_eth (lisp_cp_main_t * lcm, vlib_buffer_t * b)
+{
+  uword * vnip;
+  u32 vni = ~0;
+  u32 sw_if_index0;
+
+  l2input_main_t * l2im = &l2input_main;
+  l2_input_config_t * config;
+  l2_bridge_domain_t * bd_config;
+
+  sw_if_index0 = vnet_buffer(b)->sw_if_index[VLIB_RX];
+  config = vec_elt_at_index(l2im->configs, sw_if_index0);
+  bd_config = vec_elt_at_index (l2im->bd_configs, config->bd_index);
+
+  vnip = hash_get (lcm->vni_by_bd_id, bd_config->bd_id);
+  if (vnip)
+    vni = vnip[0];
+  else
+    clib_warning("bridge domain %d is not mapped to any vni!",
+                 config->bd_index);
+
+  return vni;
+}
+
+always_inline void
+get_src_and_dst_eids_from_buffer (lisp_cp_main_t *lcm, vlib_buffer_t * b,
+                                  gid_address_t * src, gid_address_t * dst)
+{
+  u32 vni = 0;
+  u16 type;
+
+  memset (src, 0, sizeof (*src));
+  memset (dst, 0, sizeof (*dst));
+  type = vnet_buffer(b)->lisp.overlay_afi;
+
+  if (LISP_AFI_IP == type || LISP_AFI_IP6 == type)
+    {
+      ip4_header_t * ip;
+      u8 version, preflen;
+
+      gid_address_type(src) = GID_ADDR_IP_PREFIX;
+      gid_address_type(dst) = GID_ADDR_IP_PREFIX;
+
+      ip = vlib_buffer_get_current (b);
+      get_src_and_dst_ip (ip, &gid_address_ip(src), &gid_address_ip(dst));
+
+      version = gid_address_ip_version(src);
+      preflen = ip_address_max_len (version);
+      gid_address_ippref_len(src) = preflen;
+      gid_address_ippref_len(dst) = preflen;
+
+      vni = lisp_get_vni_from_buffer_ip (lcm, b, version);
+      gid_address_vni (dst) = vni;
+      gid_address_vni (src) = vni;
+    }
+  else if (LISP_AFI_MAC == type)
+    {
+      ethernet_header_t * eh;
+
+      eh = vlib_buffer_get_current (b);
+
+      gid_address_type(src) = GID_ADDR_MAC;
+      gid_address_type(dst) = GID_ADDR_MAC;
+      mac_copy(&gid_address_mac(src), eh->src_address);
+      mac_copy(&gid_address_mac(dst), eh->dst_address);
+
+      /* get vni */
+      vni = lisp_get_vni_from_buffer_eth (lcm, b);
+
+      gid_address_vni (dst) = vni;
+      gid_address_vni (src) = vni;
+    }
+}
+
 static uword
 lisp_cp_lookup (vlib_main_t * vm, vlib_node_runtime_t * node,
               vlib_frame_t * from_frame)
@@ -2268,16 +2879,9 @@ lisp_cp_lookup (vlib_main_t * vm, vlib_node_runtime_t * node,
 
       while (n_left_from > 0 && n_left_to_next_drop > 0)
         {
-          u32 pi0, vni;
-          vlib_buffer_t * p0;
-          ip4_header_t * ip0;
+          u32 pi0;
+          vlib_buffer_t * b0;
           gid_address_t src, dst;
-          ip_prefix_t * spref, * dpref;
-
-          gid_address_type (&src) = GID_ADDR_IP_PREFIX;
-          spref = &gid_address_ippref(&src);
-          gid_address_type (&dst) = GID_ADDR_IP_PREFIX;
-          dpref = &gid_address_ippref(&dst);
 
           pi0 = from[0];
           from += 1;
@@ -2286,18 +2890,11 @@ lisp_cp_lookup (vlib_main_t * vm, vlib_node_runtime_t * node,
           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];
+          b0 = vlib_get_buffer (vm, pi0);
+          b0->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));
-
-          vni = lisp_get_vni_from_buffer (p0, ip_prefix_version (spref));
-          gid_address_vni (&dst) = vni;
-          gid_address_vni (&src) = vni;
+          get_src_and_dst_eids_from_buffer (lcm, b0, &src, &dst);
 
           /* if we have remote mapping for destination already in map-chache
              add forwarding tunnel directly. If not send a map-request */
@@ -2307,333 +2904,145 @@ lisp_cp_lookup (vlib_main_t * vm, vlib_node_runtime_t * node,
               mapping_t * m =  vec_elt_at_index (lcm->mapping_pool, di);
               /* send a map-request also in case of negative mapping entry
                 with corresponding action */
-              if (m->action == ACTION_SEND_MAP_REQUEST)
+              if (m->action == LISP_SEND_MAP_REQUEST)
                 {
                   /* send map-request */
-                  send_encapsulated_map_request (vm, lcm, &src, &dst, 0);
+                  queue_map_request (&src, &dst, 0 /* smr_invoked */,
+                                     0 /* is_resend */);
                   pkts_mapped++;
                 }
               else
                 {
                   si =  gid_dictionary_lookup (&lcm->mapping_index_by_gid,
                                                &src);
-                  if (~0 != si)
-                    {
-                      add_fwd_entry (lcm, si, di);
-                    }
-                }
-            }
-          else
-            {
-              /* 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));
-
-              memset(tr, 0, sizeof(*tr));
-              gid_address_copy (&tr->dst_eid, &dst);
-              if (vec_len(lcm->map_resolvers) > 0)
-                {
-                  clib_memcpy (&tr->map_resolver_ip,
-                               vec_elt_at_index(lcm->map_resolvers, 0),
-                               sizeof(ip_address_t));
-                }
-            }
-          gid_address_free (&dst);
-          gid_address_free (&src);
-        }
-
-      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;
-}
-
-static 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->vni = gid_address_vni(&a->deid);
-  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);
-}
-
-/**
- * Finds first remote locator with best (lowest) priority that has a local
- * peer locator with an underlying route to it.
- *
- */
-static u32
-get_locator_pair (lisp_cp_main_t* lcm, mapping_t * lcl_map, mapping_t * rmt_map,
-                  ip_address_t * lcl_loc, ip_address_t * rmt_loc)
-{
-  u32 i, minp = ~0, limitp = 0, li, check_index = 0, done = 0, esi;
-  locator_set_t * rmt_ls, * lcl_ls;
-  ip_address_t _lcl, * lcl = &_lcl;
-  locator_t * l, * rmt = 0;
-  uword * checked = 0;
-
-  rmt_ls = pool_elt_at_index(lcm->locator_set_pool, rmt_map->locator_set_index);
-  lcl_ls = pool_elt_at_index(lcm->locator_set_pool, lcl_map->locator_set_index);
-
-  if (!rmt_ls || vec_len(rmt_ls->locator_indices) == 0)
-    return 0;
-
-  while (!done)
-    {
-      /* find unvisited remote locator with best priority */
-      for (i = 0; i < vec_len(rmt_ls->locator_indices); i++)
-        {
-          if (0 != hash_get(checked, i))
-            continue;
-
-          li = vec_elt(rmt_ls->locator_indices, i);
-          l = pool_elt_at_index(lcm->locator_pool, li);
-
-          /* we don't support non-IP locators for now */
-          if (gid_address_type(&l->address) != GID_ADDR_IP_PREFIX)
-            continue;
-
-          if (l->priority < minp && l->priority >= limitp)
+                  if (~0 != si)
+                    {
+                      dp_add_fwd_entry (lcm, si, di);
+                    }
+                }
+            }
+          else
             {
-              minp = l->priority;
-              rmt = l;
-              check_index = i;
+              /* send map-request */
+              queue_map_request (&src, &dst, 0 /* smr_invoked */,
+                                 0 /* is_resend */);
+              pkts_mapped++;
             }
-        }
-      /* check if a local locator with a route to remote locator exists */
-      if (rmt != 0)
-        {
-          esi = ip_fib_get_egress_iface_for_dst (
-              lcm, &gid_address_ip(&rmt->address));
-          if ((u32) ~0 == esi)
-            continue;
 
-          for (i = 0; i < vec_len(lcl_ls->locator_indices); i++)
+          if (PREDICT_FALSE(b0->flags & VLIB_BUFFER_IS_TRACED))
             {
-              li = vec_elt (lcl_ls->locator_indices, i);
-              locator_t * sl = pool_elt_at_index (lcm->locator_pool, li);
+              lisp_cp_lookup_trace_t *tr = vlib_add_trace (vm, node, b0,
+                                                          sizeof(*tr));
 
-              /* found local locator */
-              if (sl->sw_if_index == esi)
+              memset(tr, 0, sizeof(*tr));
+              gid_address_copy (&tr->dst_eid, &dst);
+              if (vec_len(lcm->map_resolvers) > 0)
                 {
-                  if (0 == ip_interface_get_first_ip_address (lcm,
-                             sl->sw_if_index,
-                             gid_address_ip_version(&rmt->address), lcl))
-                    continue;
-
-                  ip_address_copy(rmt_loc, &gid_address_ip(&rmt->address));
-                  ip_address_copy(lcl_loc, lcl);
-                  done = 2;
+                  clib_memcpy (&tr->map_resolver_ip,
+                               vec_elt_at_index(lcm->map_resolvers, 0),
+                               sizeof(ip_address_t));
                 }
             }
-
-          /* skip this remote locator in next searches */
-          limitp = minp;
-          hash_set(checked, check_index, 1);
+          gid_address_free (&dst);
+          gid_address_free (&src);
         }
-      else
-        done = 1;
+
+      vlib_put_next_frame (vm, node, LISP_CP_LOOKUP_NEXT_DROP, n_left_to_next_drop);
     }
-  hash_free(checked);
-  return (done == 2) ? 1 : 0;
+  vlib_node_increment_counter (vm, node->node_index,
+                               LISP_CP_LOOKUP_ERROR_MAP_REQUESTS_SENT,
+                               pkts_mapped);
+  return from_frame->n_vectors;
 }
 
-static void
-add_fwd_entry (lisp_cp_main_t* lcm, u32 src_map_index, u32 dst_map_index)
-{
-  mapping_t * src_map, * dst_map;
-  u32 sw_if_index;
-  uword * feip = 0, * tidp;
-  fwd_entry_t* fe;
-  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);
-
-  gid_address_copy (&a->deid, &dst_map->eid);
-  a->vni = gid_address_vni(&a->deid);
-
-  tidp = hash_get(lcm->table_id_by_vni, a->vni);
-  if (!tidp)
-    {
-      clib_warning("vni %d not associated to a vrf!", a->vni);
-      return;
-    }
-  a->table_id = tidp[0];
+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,
 
-  /* insert data plane forwarding entry */
-  a->is_add = 1;
+  .n_errors = LISP_CP_LOOKUP_N_ERROR,
+  .error_strings = lisp_cp_lookup_error_strings,
 
-  /* find best locator pair that 1) verifies LISP policy 2) are connected */
-  if (0 == get_locator_pair (lcm, src_map, dst_map, &a->slocator, &a->dlocator))
-    {
-      /* negative entry */
-      a->is_negative = 1;
-      a->action = dst_map->action;
-    }
+  .n_next_nodes = LISP_CP_LOOKUP_N_NEXT,
 
-  /* TODO remove */
-  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;
+  .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",
+  },
+};
 
-  vnet_lisp_gpe_add_del_fwd_entry (a, &sw_if_index);
+/* lisp_cp_input statistics */
+#define foreach_lisp_cp_input_error                     \
+_(DROP, "drop")                                         \
+_(MAP_REPLIES_RECEIVED, "map-replies received")
 
-  /* add tunnel to fwd entry table XXX check return value from DP insertion */
-  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);
-}
+static char * lisp_cp_input_error_strings[] = {
+#define _(sym,string) string,
+  foreach_lisp_cp_input_error
+#undef _
+};
 
-/* 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)
+typedef enum
 {
-  u32 i, old_li;
-  locator_t * old_loc, * new_loc;
+#define _(sym,str) LISP_CP_INPUT_ERROR_##sym,
+    foreach_lisp_cp_input_error
+#undef _
+    LISP_CP_INPUT_N_ERROR,
+} lisp_cp_input_error_t;
 
-  if (vec_len (old_ls_indexes) != vec_len(new_locators))
-    return 1;
+typedef enum
+{
+  LISP_CP_INPUT_NEXT_DROP,
+  LISP_CP_INPUT_N_NEXT,
+} lisp_cp_input_next_t;
 
-  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);
+typedef struct
+{
+  gid_address_t dst_eid;
+  ip4_address_t map_resolver_ip;
+} lisp_cp_input_trace_t;
 
-      new_loc = vec_elt_at_index(new_locators, i);
+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 *);
 
-      if (locator_cmp (old_loc, new_loc))
-        return 1;
-    }
-  return 0;
+  s = format (s, "LISP-CP-INPUT: TODO");
+  return s;
 }
 
-void
-process_map_reply (lisp_cp_main_t * lcm, vlib_buffer_t * b)
+void *
+process_map_reply (void * arg)
 {
-  mapping_t * old_map;
-  locator_t * loc;
-  u32 len = 0, i, ls_index = 0;
+  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
+  vlib_buffer_t * b = arg;
+  u32 len = 0, i, ttl, dst_map_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;
+  u64 nonce, * noncep;
+  gid_address_t deid;
   uword * pmr_index;
+  u8 authoritative, action;
+  locator_t * locators = 0, * loc;
 
   mrep_hdr = vlib_buffer_get_current (b);
 
+  lisp_pending_map_request_lock (lcm);
+
   /* 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;
+      goto done;
     }
   pmr = pool_elt_at_index(lcm->pending_map_requests_pool, pmr_index[0]);
 
@@ -2641,76 +3050,43 @@ process_map_reply (lisp_cp_main_t * lcm, vlib_buffer_t * b)
 
   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);
+      ttl = clib_net_to_host_u32 (MAP_REC_TTL(h));
+      action = MAP_REC_ACTION(h);
+      authoritative = MAP_REC_AUTH(h);
 
-      len = lisp_msg_parse_mapping_record (b, &m_args->deid, &ls_arg->locators,
-                                           &probed);
+      len = lisp_msg_parse_mapping_record (b, &deid, &locators, &probed);
       if (len == ~0)
         {
           clib_warning ("Failed to parse mapping record!");
-          vec_foreach (loc, ls_arg->locators)
+          vec_foreach (loc, locators)
             {
               locator_free (loc);
             }
-          vec_free(ls_arg->locators);
-          return;
+          vec_free(locators);
+          goto done;
         }
 
-      mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &m_args->deid);
-      old_map = mi != ~0 ? pool_elt_at_index(lcm->mapping_pool, mi) : 0;
-
-      /* if mapping already exists, decide if locators (and forwarding) should
-       * be updated and be done */
-      if (old_map != 0 && !gid_address_cmp (&old_map->eid, &m_args->deid))
-        {
-          locator_set_t * old_ls;
-
-          /* 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);
+      /* insert/update mappings cache */
+      vnet_lisp_add_del_mapping (&deid, locators, action, authoritative, ttl, 1,
+                                 &dst_map_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);
+      /* try to program forwarding only if mapping saved or updated*/
+      if ((u32) ~0 != dst_map_index)
+        lisp_add_del_adjacency (lcm, &pmr->src, &deid, 1);
 
-          /* add forwarding tunnel */
-          add_fwd_entry (lcm, pmr->src_mapping_index, dst_map_index);
-        }
-      vec_free(ls_arg->locators);
+      vec_free(locators);
     }
 
   /* remove pending map request entry */
-  hash_unset(lcm->pending_map_requests_by_nonce, nonce);
+  vec_foreach (noncep, pmr->nonces)
+    hash_unset(lcm->pending_map_requests_by_nonce, noncep[0]);
+  vec_free(pmr->nonces);
   pool_put(lcm->pending_map_requests_pool, pmr);
+
+done:
+  lisp_pending_map_request_unlock (lcm);
+  return 0;
 }
 
 void
@@ -2759,10 +3135,25 @@ process_map_request (vlib_main_t * vm, lisp_cp_main_t * lcm, vlib_buffer_t * b)
           return;
         }
       /* send SMR-invoked map-requests */
-      send_encapsulated_map_request (vm, lcm, &dst, &src, /* invoked */ 1);
+      queue_map_request (&dst, &src, 1 /* invoked */, 0 /* resend */);
     }
 }
 
+static void
+queue_map_reply (vlib_buffer_t * b)
+{
+  vlib_buffer_t * a = clib_mem_alloc (sizeof (a[0]) + b->current_length);
+
+  clib_memcpy (a->data, b->data + b->current_data,
+               b->current_length);
+  a->current_length = b->current_length;
+  a->current_data = 0;
+
+  vl_api_rpc_call_main_thread (process_map_reply, (u8 *) a, sizeof (a[0])
+                                + a->current_length);
+  clib_mem_free (a);
+}
+
 static uword
 lisp_cp_input (vlib_main_t * vm, vlib_node_runtime_t * node,
                vlib_frame_t * from_frame)
@@ -2799,7 +3190,7 @@ lisp_cp_input (vlib_main_t * vm, vlib_node_runtime_t * node,
           switch (type)
             {
             case LISP_MAP_REPLY:
-              process_map_reply (lcm, b0);
+              queue_map_reply (b0);
               break;
             case LISP_MAP_REQUEST:
               process_map_request(vm, lcm, b0);
@@ -2853,8 +3244,14 @@ lisp_cp_init (vlib_main_t *vm)
   lcm->vlib_main = vm;
   lcm->vnet_main = vnet_get_main();
   lcm->mreq_itr_rlocs = ~0;
+  lcm->lisp_pitr = 0;
+
+  lcm->pending_map_request_lock =
+    clib_mem_alloc_aligned (CLIB_CACHE_LINE_BYTES, CLIB_CACHE_LINE_BYTES);
 
+  lcm->pending_map_request_lock[0] = 0;
   gid_dictionary_init (&lcm->mapping_index_by_gid);
+  lcm->do_map_resolver_election = 1;
 
   /* default vrf mapped to vni 0 */
   hash_set(lcm->table_id_by_vni, 0, 0);
@@ -2868,4 +3265,157 @@ lisp_cp_init (vlib_main_t *vm)
   return 0;
 }
 
+static void *
+send_map_request_thread_fn (void * arg)
+{
+  map_request_args_t * a = arg;
+  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
+
+  lisp_pending_map_request_lock (lcm);
+
+  if (a->is_resend)
+    resend_encapsulated_map_request (lcm, &a->seid, &a->deid, a->smr_invoked);
+  else
+    send_encapsulated_map_request (lcm, &a->seid, &a->deid, a->smr_invoked);
+
+  lisp_pending_map_request_unlock (lcm);
+
+  return 0;
+}
+
+static int
+queue_map_request (gid_address_t * seid, gid_address_t * deid,
+                   u8 smr_invoked, u8 is_resend)
+{
+  map_request_args_t a;
+
+  a.is_resend = is_resend;
+  gid_address_copy (&a.seid, seid);
+  gid_address_copy (&a.deid, deid);
+  a.smr_invoked = smr_invoked;
+
+  vl_api_rpc_call_main_thread (send_map_request_thread_fn,
+                               (u8 *) &a, sizeof (a));
+  return 0;
+}
+
+/**
+ * Take an action with a pending map request depending on expiration time
+ * and re-try counters.
+ */
+static void
+update_pending_request (pending_map_request_t * r, f64 dt)
+{
+  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
+  map_resolver_t * mr;
+
+  if (r->time_to_expire - dt < 0)
+    /* it's time to decide what to do with this pending request */
+    {
+      if (r->retries_num >= NUMBER_OF_RETRIES)
+        /* too many retries -> assume current map resolver is not available */
+        {
+          mr = get_map_resolver (&lcm->active_map_resolver);
+          if (!mr)
+            {
+              clib_warning ("Map resolver %U not found - probably deleted "
+                            "by the user recently.", format_ip_address,
+                            &lcm->active_map_resolver);
+            }
+          else
+            {
+              clib_warning ("map resolver %U is unreachable, ignoring",
+                            format_ip_address, &lcm->active_map_resolver);
+
+              /* mark current map resolver unavailable so it won't be
+               * selected next time */
+              mr->is_down = 1;
+              mr->last_update = vlib_time_now (lcm->vlib_main);
+            }
+
+          reset_pending_mr_counters (r);
+          elect_map_resolver (lcm);
+
+          /* try to find a next eligible map resolver and re-send */
+          queue_map_request (&r->src, &r->dst, r->is_smr_invoked,
+                             1 /* resend */);
+        }
+      else
+        {
+          /* try again */
+          queue_map_request (&r->src, &r->dst, r->is_smr_invoked,
+                             1 /* resend */);
+          r->retries_num++;
+          r->time_to_expire = PENDING_MREQ_EXPIRATION_TIME;
+        }
+    }
+  else
+    r->time_to_expire -= dt;
+}
+
+static void
+remove_dead_pending_map_requests (lisp_cp_main_t * lcm)
+{
+  u64 * nonce;
+  pending_map_request_t * pmr;
+  u32 * to_be_removed = 0, * pmr_index;
+
+  ASSERT (*lcm->pending_map_request_lock);
+
+  pool_foreach (pmr, lcm->pending_map_requests_pool,
+    ({
+      if (pmr->to_be_removed)
+        {
+          vec_foreach (nonce, pmr->nonces)
+            hash_unset (lcm->pending_map_requests_by_nonce, nonce[0]);
+
+          vec_add1 (to_be_removed, pmr - lcm->pending_map_requests_pool);
+        }
+    }));
+
+  vec_foreach (pmr_index, to_be_removed)
+    pool_put_index (lcm->pending_map_requests_by_nonce, pmr_index[0]);
+
+  vec_free (to_be_removed);
+}
+
+static uword
+send_map_resolver_service (vlib_main_t * vm,
+                           vlib_node_runtime_t * rt,
+                           vlib_frame_t * f)
+{
+  f64 period = 2.0;
+  pending_map_request_t * pmr;
+  lisp_cp_main_t * lcm = vnet_lisp_cp_get_main ();
+
+  while (1)
+    {
+      vlib_process_wait_for_event_or_clock (vm, period);
+
+      /* currently no signals are expected - just wait for clock */
+      (void) vlib_process_get_events (vm, 0);
+
+      lisp_pending_map_request_lock (lcm);
+      pool_foreach (pmr, lcm->pending_map_requests_pool,
+        ({
+          if (!pmr->to_be_removed)
+            update_pending_request (pmr, period);
+        }));
+
+      remove_dead_pending_map_requests (lcm);
+
+      lisp_pending_map_request_unlock (lcm);
+    }
+
+  /* unreachable */
+  return 0;
+}
+
+VLIB_REGISTER_NODE (lisp_retry_service_node,static) = {
+    .function = send_map_resolver_service,
+    .type = VLIB_NODE_TYPE_PROCESS,
+    .name = "lisp-retry-service",
+    .process_log2_n_stack_bytes = 16,
+};
+
 VLIB_INIT_FUNCTION(lisp_cp_init);