X-Git-Url: https://gerrit.fd.io/r/gitweb?a=blobdiff_plain;f=vnet%2Fvnet%2Flisp-cp%2Fcontrol.c;h=de048a4144a6d087816c0366a22c2496163bc634;hb=fb9931ff1687e79461df5f92210d81d96fffa3ad;hp=8dc4207847ab4c4d673ee27cdb1b171288b789d5;hpb=69a9b76d648a108e0a7fad4404b218ebdd6a8ea5;p=vpp.git

diff --git a/vnet/vnet/lisp-cp/control.c b/vnet/vnet/lisp-cp/control.c
index 8dc4207847a..de048a4144a 100644
--- a/vnet/vnet/lisp-cp/control.c
+++ b/vnet/vnet/lisp-cp/control.c
@@ -23,6 +23,9 @@
 #include <vnet/fib/fib_entry.h>
 #include <vnet/fib/fib_table.h>
 
+#include <openssl/evp.h>
+#include <openssl/hmac.h>
+
 typedef struct
 {
   u8 is_resend;
@@ -31,6 +34,17 @@ typedef struct
   u8 smr_invoked;
 } map_request_args_t;
 
+typedef struct
+{
+  u64 nonce;
+  u8 is_rloc_probe;
+  mapping_t *mappings;
+} map_records_arg_t;
+
+static int
+lisp_add_del_adjacency (lisp_cp_main_t * lcm, gid_address_t * local_eid,
+			gid_address_t * remote_eid, u8 is_add);
+
 u8
 vnet_lisp_get_map_request_mode (void)
 {
@@ -38,6 +52,38 @@ vnet_lisp_get_map_request_mode (void)
   return lcm->map_request_mode;
 }
 
+static u16
+auth_data_len_by_key_id (lisp_key_type_t key_id)
+{
+  switch (key_id)
+    {
+    case HMAC_SHA_1_96:
+      return SHA1_AUTH_DATA_LEN;
+    case HMAC_SHA_256_128:
+      return SHA256_AUTH_DATA_LEN;
+    default:
+      clib_warning ("unsupported key type: %d!", key_id);
+      return (u16) ~ 0;
+    }
+  return (u16) ~ 0;
+}
+
+static const EVP_MD *
+get_encrypt_fcn (lisp_key_type_t key_id)
+{
+  switch (key_id)
+    {
+    case HMAC_SHA_1_96:
+      return EVP_sha1 ();
+    case HMAC_SHA_256_128:
+      return EVP_sha256 ();
+    default:
+      clib_warning ("unsupported encryption key type: %d!", key_id);
+      break;
+    }
+  return 0;
+}
+
 static int
 queue_map_request (gid_address_t * seid, gid_address_t * deid,
 		   u8 smr_invoked, u8 is_resend);
@@ -230,6 +276,8 @@ dp_del_fwd_entry (lisp_cp_main_t * lcm, u32 src_map_index, u32 dst_map_index)
   a->locator_pairs = fe->locator_pairs;
   a->vni = gid_address_vni (&fe->reid);
   gid_address_copy (&a->rmt_eid, &fe->reid);
+  if (fe->is_src_dst)
+    gid_address_copy (&a->lcl_eid, &fe->leid);
 
   vnet_lisp_gpe_add_del_fwd_entry (a, &sw_if_index);
 
@@ -323,6 +371,7 @@ get_locator_pairs (lisp_cp_main_t * lcm, mapping_t * lcl_map,
 				   &gid_address_ip (&rmt->address));
 		  ip_address_copy (&pair.lcl_loc, lcl_addr);
 		  pair.weight = rmt->weight;
+		  pair.priority = rmt->priority;
 		  vec_add1 (locator_pairs[0], pair);
 		  found = 1;
 		}
@@ -360,6 +409,20 @@ gid_address_sd_to_flat (gid_address_t * dst, gid_address_t * src,
     }
 }
 
+u8
+vnet_lisp_map_register_state_get (void)
+{
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+  return lcm->map_registering;
+}
+
+u8
+vnet_lisp_rloc_probe_state_get (void)
+{
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+  return lcm->rloc_probing;
+}
+
 static void
 dp_add_fwd_entry (lisp_cp_main_t * lcm, u32 src_map_index, u32 dst_map_index)
 {
@@ -386,12 +449,20 @@ dp_add_fwd_entry (lisp_cp_main_t * lcm, u32 src_map_index, u32 dst_map_index)
   /* insert data plane forwarding entry */
   a->is_add = 1;
 
-  if (GID_ADDR_SRC_DST == gid_address_type (&dst_map->eid))
+  if (MR_MODE_SRC_DST == lcm->map_request_mode)
     {
-      gid_address_sd_to_flat (&a->rmt_eid, &dst_map->eid,
-			      &gid_address_sd_dst (&dst_map->eid));
-      gid_address_sd_to_flat (&a->lcl_eid, &dst_map->eid,
-			      &gid_address_sd_src (&dst_map->eid));
+      if (GID_ADDR_SRC_DST == gid_address_type (&dst_map->eid))
+	{
+	  gid_address_sd_to_flat (&a->rmt_eid, &dst_map->eid,
+				  &gid_address_sd_dst (&dst_map->eid));
+	  gid_address_sd_to_flat (&a->lcl_eid, &dst_map->eid,
+				  &gid_address_sd_src (&dst_map->eid));
+	}
+      else
+	{
+	  gid_address_copy (&a->rmt_eid, &dst_map->eid);
+	  gid_address_copy (&a->lcl_eid, &src_map->eid);
+	}
       is_src_dst = 1;
     }
   else
@@ -529,6 +600,132 @@ VLIB_CLI_COMMAND (lisp_show_adjacencies_command) = {
 };
 /* *INDENT-ON* */
 
+static lisp_msmr_t *
+get_map_server (ip_address_t * a)
+{
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+  lisp_msmr_t *m;
+
+  vec_foreach (m, lcm->map_servers)
+  {
+    if (!ip_address_cmp (&m->address, a))
+      {
+	return m;
+      }
+  }
+  return 0;
+}
+
+static lisp_msmr_t *
+get_map_resolver (ip_address_t * a)
+{
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+  lisp_msmr_t *m;
+
+  vec_foreach (m, lcm->map_resolvers)
+  {
+    if (!ip_address_cmp (&m->address, a))
+      {
+	return m;
+      }
+  }
+  return 0;
+}
+
+int
+vnet_lisp_add_del_map_server (ip_address_t * addr, u8 is_add)
+{
+  u32 i;
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+  lisp_msmr_t _ms, *ms = &_ms;
+
+  if (vnet_lisp_enable_disable_status () == 0)
+    {
+      clib_warning ("LISP is disabled!");
+      return VNET_API_ERROR_LISP_DISABLED;
+    }
+
+  if (is_add)
+    {
+      if (get_map_server (addr))
+	{
+	  clib_warning ("map-server %U already exists!", format_ip_address,
+			addr);
+	  return -1;
+	}
+
+      memset (ms, 0, sizeof (*ms));
+      ip_address_copy (&ms->address, addr);
+      vec_add1 (lcm->map_servers, ms[0]);
+    }
+  else
+    {
+      for (i = 0; i < vec_len (lcm->map_servers); i++)
+	{
+	  ms = vec_elt_at_index (lcm->map_servers, i);
+	  if (!ip_address_cmp (&ms->address, addr))
+	    {
+	      vec_del1 (lcm->map_servers, i);
+	      break;
+	    }
+	}
+    }
+
+  return 0;
+}
+
+static clib_error_t *
+lisp_add_del_map_server_command_fn (vlib_main_t * vm,
+				    unformat_input_t * input,
+				    vlib_cli_command_t * cmd)
+{
+  int rv = 0;
+  u8 is_add = 1, ip_set = 0;
+  ip_address_t ip;
+  unformat_input_t _line_input, *line_input = &_line_input;
+
+  /* Get a line of input. */
+  if (!unformat_user (input, unformat_line_input, line_input))
+    return 0;
+
+  while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
+    {
+      if (unformat (line_input, "add"))
+	is_add = 1;
+      else if (unformat (line_input, "del"))
+	is_add = 0;
+      else if (unformat (line_input, "%U", unformat_ip_address, &ip))
+	ip_set = 1;
+      else
+	{
+	  vlib_cli_output (vm, "parse error: '%U'",
+			   format_unformat_error, line_input);
+	  return 0;
+	}
+    }
+
+  if (!ip_set)
+    {
+      vlib_cli_output (vm, "map-server ip address not set!");
+      return 0;
+    }
+
+  rv = vnet_lisp_add_del_map_server (&ip, is_add);
+  if (!rv)
+    vlib_cli_output (vm, "failed to %s map-server!",
+		     is_add ? "add" : "delete");
+
+  return 0;
+}
+
+/* *INDENT-OFF* */
+VLIB_CLI_COMMAND (lisp_add_del_map_server_command) = {
+    .path = "lisp map-server",
+    .short_help = "lisp map-server add|del <ip>",
+    .function = lisp_add_del_map_server_command_fn,
+};
+/* *INDENT-ON* */
+
 /**
  * Add/remove mapping to/from map-cache. Overwriting not allowed.
  */
@@ -560,6 +757,8 @@ vnet_lisp_map_cache_add_del (vnet_lisp_add_del_mapping_args_t * a,
       m->action = a->action;
       m->local = a->local;
       m->is_static = a->is_static;
+      m->key = vec_dup (a->key);
+      m->key_id = a->key_id;
 
       map_index = m - lcm->mapping_pool;
       gid_dictionary_add_del (&lcm->mapping_index_by_gid, &a->eid, map_index,
@@ -681,6 +880,8 @@ lisp_add_del_local_eid_command_fn (vlib_main_t * vm, unformat_input_t * input,
   vnet_lisp_add_del_mapping_args_t _a, *a = &_a;
   int rv = 0;
   u32 vni = 0;
+  u8 *key = 0;
+  u32 key_id = 0;
 
   memset (&eid, 0, sizeof (eid));
   memset (a, 0, sizeof (*a));
@@ -699,6 +900,11 @@ lisp_add_del_local_eid_command_fn (vlib_main_t * vm, unformat_input_t * input,
 	;
       else if (unformat (line_input, "vni %d", &vni))
 	gid_address_vni (&eid) = vni;
+      else if (unformat (line_input, "secret-key %_%v%_", &key))
+	;
+      else if (unformat (line_input, "key-id %U", unformat_hmac_key_id,
+			 &key_id))
+	;
       else if (unformat (line_input, "locator-set %_%v%_", &locator_set_name))
 	{
 	  p = hash_get_mem (lcm->locator_set_index_by_name, locator_set_name);
@@ -725,10 +931,18 @@ lisp_add_del_local_eid_command_fn (vlib_main_t * vm, unformat_input_t * input,
       goto done;
     }
 
+  if (key && (0 == key_id))
+    {
+      vlib_cli_output (vm, "invalid key_id!");
+      return 0;
+    }
+
   gid_address_copy (&a->eid, &eid);
   a->is_add = is_add;
   a->locator_set_index = locator_set_index;
   a->local = 1;
+  a->key = key;
+  a->key_id = key_id;
 
   rv = vnet_lisp_add_del_local_mapping (a, &map_index);
   if (0 != rv)
@@ -741,6 +955,7 @@ done:
   if (locator_set_name)
     vec_free (locator_set_name);
   gid_address_free (&a->eid);
+  vec_free (a->key);
   return error;
 }
 
@@ -748,7 +963,7 @@ done:
 VLIB_CLI_COMMAND (lisp_add_del_local_eid_command) = {
     .path = "lisp eid-table",
     .short_help = "lisp eid-table add/del [vni <vni>] eid <eid> "
-      "locator-set <locator-set>",
+      "locator-set <locator-set> [key <secret-key> key-id sha1|sha256 ]",
     .function = lisp_add_del_local_eid_command_fn,
 };
 /* *INDENT-ON* */
@@ -873,6 +1088,94 @@ compare_locators (lisp_cp_main_t * lcm, u32 * old_ls_indexes,
   return 0;
 }
 
+typedef struct
+{
+  u8 is_negative;
+  void *lcm;
+  gid_address_t *eids_to_be_deleted;
+} remove_mapping_args_t;
+
+/**
+ * Callback invoked when a sub-prefix is found
+ */
+static void
+remove_mapping_if_needed (u32 mi, void *arg)
+{
+  u8 delete = 0;
+  remove_mapping_args_t *a = arg;
+  lisp_cp_main_t *lcm = a->lcm;
+  mapping_t *m;
+  locator_set_t *ls;
+
+  m = pool_elt_at_index (lcm->mapping_pool, mi);
+  if (!m)
+    return;
+
+  ls = pool_elt_at_index (lcm->locator_set_pool, m->locator_set_index);
+
+  if (a->is_negative)
+    {
+      if (0 != vec_len (ls->locator_indices))
+	delete = 1;
+    }
+  else
+    {
+      if (0 == vec_len (ls->locator_indices))
+	delete = 1;
+    }
+
+  if (delete)
+    vec_add1 (a->eids_to_be_deleted, m->eid);
+}
+
+/**
+ * This function searches map cache and looks for IP prefixes that are subset
+ * of the provided one. If such prefix is found depending on 'is_negative'
+ * it does follows:
+ *
+ * 1) if is_negative is true and found prefix points to positive mapping,
+ *    then the mapping is removed
+ * 2) if is_negative is false and found prefix points to negative mapping,
+ *    then the mapping is removed
+ */
+static void
+remove_overlapping_sub_prefixes (lisp_cp_main_t * lcm, gid_address_t * eid,
+				 u8 is_negative)
+{
+  gid_address_t *e;
+  remove_mapping_args_t a;
+  memset (&a, 0, sizeof (a));
+
+  /* do this only in src/dst mode ... */
+  if (MR_MODE_SRC_DST != lcm->map_request_mode)
+    return;
+
+  /* ... and  only for IP prefix */
+  if (GID_ADDR_SRC_DST != gid_address_type (eid)
+      || (FID_ADDR_IP_PREF != gid_address_sd_dst_type (eid)))
+    return;
+
+  a.is_negative = is_negative;
+  a.lcm = lcm;
+
+  gid_dict_foreach_subprefix (&lcm->mapping_index_by_gid, eid,
+			      remove_mapping_if_needed, &a);
+
+  vec_foreach (e, a.eids_to_be_deleted)
+  {
+    lisp_add_del_adjacency (lcm, 0, e, 0 /* is_add */ );
+    vnet_lisp_add_del_mapping (e, 0, 0, 0, 0, 0 /* is add */ , 0, 0);
+  }
+
+  vec_free (a.eids_to_be_deleted);
+}
+
+static void
+mapping_delete_timer (lisp_cp_main_t * lcm, u32 mi)
+{
+  timing_wheel_delete (&lcm->wheel, mi);
+}
+
 /**
  * Adds/removes/updates mapping. Does not program forwarding.
  *
@@ -925,7 +1228,7 @@ vnet_lisp_add_del_mapping (gid_address_t * eid, locator_t * rlocs, u8 action,
 	      /* do not overwrite local or static remote mappings */
 	      clib_warning ("mapping %U rejected due to collision with local "
 			    "or static remote mapping!", format_gid_address,
-			    &eid);
+			    eid);
 	      return 0;
 	    }
 
@@ -952,6 +1255,8 @@ vnet_lisp_add_del_mapping (gid_address_t * eid, locator_t * rlocs, u8 action,
       /* new mapping */
       else
 	{
+	  remove_overlapping_sub_prefixes (lcm, eid, 0 == ls_args->locators);
+
 	  ls_args->is_add = 1;
 	  ls_args->index = ~0;
 
@@ -963,6 +1268,7 @@ vnet_lisp_add_del_mapping (gid_address_t * eid, locator_t * rlocs, u8 action,
 	  m_args->action = action;
 	  m_args->locator_set_index = ls_index;
 	  m_args->is_static = is_static;
+	  m_args->ttl = ttl;
 	  vnet_lisp_map_cache_add_del (m_args, &dst_map_index);
 
 	  if (res_map_index)
@@ -990,6 +1296,10 @@ vnet_lisp_add_del_mapping (gid_address_t * eid, locator_t * rlocs, u8 action,
       /* delete locator set */
       vnet_lisp_add_del_locator_set (ls_args, 0);
 
+      /* delete timer associated to the mapping if any */
+      if (old_map->timer_set)
+	mapping_delete_timer (lcm, mi);
+
       /* return old mapping index */
       if (res_map_index)
 	res_map_index[0] = mi;
@@ -1050,7 +1360,7 @@ cleanup:
  * Note that adjacencies are not stored, they only result in forwarding entries
  * being created.
  */
-int
+static int
 lisp_add_del_adjacency (lisp_cp_main_t * lcm, gid_address_t * local_eid,
 			gid_address_t * remote_eid, u8 is_add)
 {
@@ -1450,7 +1760,7 @@ lisp_show_map_resolvers_command_fn (vlib_main_t * vm,
 				    unformat_input_t * input,
 				    vlib_cli_command_t * cmd)
 {
-  map_resolver_t *mr;
+  lisp_msmr_t *mr;
   lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
 
   vec_foreach (mr, lcm->map_resolvers)
@@ -2078,6 +2388,24 @@ vnet_lisp_add_del_locator_set (vnet_lisp_add_del_locator_set_args_t * a,
   return 0;
 }
 
+int
+vnet_lisp_rloc_probe_enable_disable (u8 is_enable)
+{
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+
+  lcm->rloc_probing = is_enable;
+  return 0;
+}
+
+int
+vnet_lisp_map_register_enable_disable (u8 is_enable)
+{
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+
+  lcm->map_registering = is_enable;
+  return 0;
+}
+
 clib_error_t *
 vnet_lisp_enable_disable (u8 is_enable)
 {
@@ -2151,7 +2479,8 @@ lisp_enable_disable_command_fn (vlib_main_t * vm, unformat_input_t * input,
   if (!is_set)
     return clib_error_return (0, "state not set");
 
-  return vnet_lisp_enable_disable (is_enabled);
+  vnet_lisp_enable_disable (is_enabled);
+  return 0;
 }
 
 /* *INDENT-OFF* */
@@ -2162,50 +2491,62 @@ VLIB_CLI_COMMAND (lisp_cp_enable_disable_command) = {
 };
 /* *INDENT-ON* */
 
-u8
-vnet_lisp_enable_disable_status (void)
+static clib_error_t *
+lisp_map_register_enable_disable_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 ();
-  return lcm->is_enabled;
-}
+  unformat_input_t _line_input, *line_input = &_line_input;
+  u8 is_enabled = 0;
+  u8 is_set = 0;
 
-static u8 *
-format_lisp_status (u8 * s, va_list * args)
-{
-  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
-  return format (s, "%s", lcm->is_enabled ? "enabled" : "disabled");
-}
+  /* Get a line of input. */
+  if (!unformat_user (input, unformat_line_input, line_input))
+    return 0;
 
-static clib_error_t *
-lisp_show_status_command_fn (vlib_main_t * vm, unformat_input_t * input,
-			     vlib_cli_command_t * cmd)
-{
-  u8 *msg = 0;
-  msg = format (msg, "feature: %U\ngpe: %U\n",
-		format_lisp_status, format_vnet_lisp_gpe_status);
-  vlib_cli_output (vm, "%v", msg);
-  vec_free (msg);
+  while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
+    {
+      if (unformat (line_input, "enable"))
+	{
+	  is_set = 1;
+	  is_enabled = 1;
+	}
+      else if (unformat (line_input, "disable"))
+	is_set = 1;
+      else
+	{
+	  vlib_cli_output (vm, "parse error: '%U'", format_unformat_error,
+			   line_input);
+	  return 0;
+	}
+    }
+
+  if (!is_set)
+    {
+      vlib_cli_output (vm, "state not set!");
+      return 0;
+    }
+
+  vnet_lisp_map_register_enable_disable (is_enabled);
   return 0;
 }
 
 /* *INDENT-OFF* */
-VLIB_CLI_COMMAND (lisp_show_status_command) = {
-    .path = "show lisp status",
-    .short_help = "show lisp status",
-    .function = lisp_show_status_command_fn,
+VLIB_CLI_COMMAND (lisp_map_register_enable_disable_command) = {
+    .path = "lisp map-register",
+    .short_help = "lisp map-register [enable|disable]",
+    .function = lisp_map_register_enable_disable_command_fn,
 };
 /* *INDENT-ON* */
 
 static clib_error_t *
-lisp_show_eid_table_map_command_fn (vlib_main_t * vm,
-				    unformat_input_t * input,
-				    vlib_cli_command_t * cmd)
+lisp_rloc_probe_enable_disable_command_fn (vlib_main_t * vm,
+					   unformat_input_t * input,
+					   vlib_cli_command_t * cmd)
 {
-  hash_pair_t *p;
   unformat_input_t _line_input, *line_input = &_line_input;
-  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
-  uword *vni_table = 0;
-  u8 is_l2 = 0;
+  u8 is_enabled = 0;
+  u8 is_set = 0;
 
   /* Get a line of input. */
   if (!unformat_user (input, unformat_line_input, line_input))
@@ -2213,10 +2554,94 @@ lisp_show_eid_table_map_command_fn (vlib_main_t * vm,
 
   while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
     {
-      if (unformat (line_input, "l2"))
+      if (unformat (line_input, "enable"))
 	{
-	  vni_table = lcm->bd_id_by_vni;
-	  is_l2 = 1;
+	  is_set = 1;
+	  is_enabled = 1;
+	}
+      else if (unformat (line_input, "disable"))
+	is_set = 1;
+      else
+	{
+	  vlib_cli_output (vm, "parse error: '%U'", format_unformat_error,
+			   line_input);
+	  return 0;
+	}
+    }
+
+  if (!is_set)
+    {
+      vlib_cli_output (vm, "state not set!");
+      return 0;
+    }
+
+  vnet_lisp_rloc_probe_enable_disable (is_enabled);
+  return 0;
+}
+
+/* *INDENT-OFF* */
+VLIB_CLI_COMMAND (lisp_rloc_probe_enable_disable_command) = {
+    .path = "lisp rloc-probe",
+    .short_help = "lisp rloc-probe [enable|disable]",
+    .function = lisp_rloc_probe_enable_disable_command_fn,
+};
+/* *INDENT-ON* */
+
+u8
+vnet_lisp_enable_disable_status (void)
+{
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+  return lcm->is_enabled;
+}
+
+static u8 *
+format_lisp_status (u8 * s, va_list * args)
+{
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+  return format (s, "%s", lcm->is_enabled ? "enabled" : "disabled");
+}
+
+static clib_error_t *
+lisp_show_status_command_fn (vlib_main_t * vm, unformat_input_t * input,
+			     vlib_cli_command_t * cmd)
+{
+  u8 *msg = 0;
+  msg = format (msg, "feature: %U\ngpe: %U\n",
+		format_lisp_status, format_vnet_lisp_gpe_status);
+  vlib_cli_output (vm, "%v", msg);
+  vec_free (msg);
+  return 0;
+}
+
+/* *INDENT-OFF* */
+VLIB_CLI_COMMAND (lisp_show_status_command) = {
+    .path = "show lisp status",
+    .short_help = "show lisp status",
+    .function = lisp_show_status_command_fn,
+};
+/* *INDENT-ON* */
+
+static clib_error_t *
+lisp_show_eid_table_map_command_fn (vlib_main_t * vm,
+				    unformat_input_t * input,
+				    vlib_cli_command_t * cmd)
+{
+  hash_pair_t *p;
+  unformat_input_t _line_input, *line_input = &_line_input;
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+  uword *vni_table = 0;
+  u8 is_l2 = 0;
+
+  /* Get a line of input. */
+  if (!unformat_user (input, unformat_line_input, line_input))
+    return 0;
+
+  while (unformat_check_input (line_input) != UNFORMAT_END_OF_INPUT)
+    {
+      if (unformat (line_input, "l2"))
+	{
+	  vni_table = lcm->bd_id_by_vni;
+	  is_l2 = 1;
 	}
       else if (unformat (line_input, "l3"))
 	{
@@ -2457,28 +2882,12 @@ VLIB_CLI_COMMAND (lisp_cp_show_locator_sets_command) = {
 };
 /* *INDENT-ON* */
 
-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 ();
   u32 i;
-  map_resolver_t _mr, *mr = &_mr;
+  lisp_msmr_t _mr, *mr = &_mr;
 
   if (vnet_lisp_enable_disable_status () == 0)
     {
@@ -2741,7 +3150,7 @@ int
 get_mr_and_local_iface_ip (lisp_cp_main_t * lcm, ip_address_t * mr_ip,
 			   ip_address_t * sloc)
 {
-  map_resolver_t *mrit;
+  lisp_msmr_t *mrit;
   ip_address_t *a;
 
   if (vec_len (lcm->map_resolvers) == 0)
@@ -2817,6 +3226,39 @@ build_itr_rloc_list (lisp_cp_main_t * lcm, locator_set_t * loc_set)
   return rlocs;
 }
 
+static vlib_buffer_t *
+build_map_request (lisp_cp_main_t * lcm, gid_address_t * deid,
+		   ip_address_t * sloc, ip_address_t * rloc,
+		   gid_address_t * itr_rlocs, u64 * nonce_res, u32 * bi_res)
+{
+  vlib_buffer_t *b;
+  u32 bi;
+  vlib_main_t *vm = lcm->vlib_main;
+
+  if (vlib_buffer_alloc (vm, &bi, 1) != 1)
+    {
+      clib_warning ("Can't allocate buffer for Map-Request!");
+      return 0;
+    }
+
+  b = vlib_get_buffer (vm, bi);
+
+  /* leave some space for the encap headers */
+  vlib_buffer_make_headroom (b, MAX_LISP_MSG_ENCAP_LEN);
+
+  /* put lisp msg */
+  lisp_msg_put_mreq (lcm, b, NULL, deid, itr_rlocs, 0 /* smr invoked */ ,
+		     1 /* rloc probe */ , nonce_res);
+
+  /* push outer ip header */
+  pkt_push_udp_and_ip (vm, b, LISP_CONTROL_PORT, LISP_CONTROL_PORT, sloc,
+		       rloc);
+
+  bi_res[0] = bi;
+
+  return b;
+}
+
 static vlib_buffer_t *
 build_encapsulated_map_request (lisp_cp_main_t * lcm,
 				gid_address_t * seid, gid_address_t * deid,
@@ -2843,18 +3285,20 @@ build_encapsulated_map_request (lisp_cp_main_t * lcm,
   /* get rlocs */
   rlocs = build_itr_rloc_list (lcm, loc_set);
 
-  if (MR_MODE_SRC_DST == lcm->map_request_mode)
+  if (MR_MODE_SRC_DST == lcm->map_request_mode
+      && GID_ADDR_SRC_DST != gid_address_type (deid))
     {
       gid_address_t sd;
       memset (&sd, 0, sizeof (sd));
       build_src_dst (&sd, seid, deid);
-      lisp_msg_put_mreq (lcm, b, seid, &sd, rlocs, is_smr_invoked, nonce_res);
+      lisp_msg_put_mreq (lcm, b, seid, &sd, rlocs, is_smr_invoked,
+			 0 /* rloc probe */ , nonce_res);
     }
   else
     {
       /* put lisp msg */
       lisp_msg_put_mreq (lcm, b, seid, deid, rlocs, is_smr_invoked,
-			 nonce_res);
+			 0 /* rloc probe */ , nonce_res);
     }
 
   /* push ecm: udp-ip-lisp */
@@ -2880,7 +3324,7 @@ reset_pending_mr_counters (pending_map_request_t * r)
 static int
 elect_map_resolver (lisp_cp_main_t * lcm)
 {
-  map_resolver_t *mr;
+  lisp_msmr_t *mr;
 
   vec_foreach (mr, lcm->map_resolvers)
   {
@@ -2894,115 +3338,178 @@ elect_map_resolver (lisp_cp_main_t * lcm)
   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)
+static void
+free_map_register_records (mapping_t * maps)
 {
-  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, *duplicate_pmr = 0;
-  ip_address_t sloc;
-  u32 ls_index;
+  vec_foreach (map, maps) vec_free (map->locators);
 
-  ASSERT (*lcm->pending_map_request_lock);
+  vec_free (maps);
+}
 
-  /* if there is already a pending request remember it */
+static void
+add_locators (lisp_cp_main_t * lcm, mapping_t * m, u32 locator_set_index,
+	      ip_address_t * probed_loc)
+{
+  u32 *li;
+  locator_t *loc, new;
+  ip_interface_address_t *ia = 0;
+  void *addr;
+  ip_address_t *new_ip = &gid_address_ip (&new.address);
 
-  /* *INDENT-OFF* */
-  pool_foreach(pmr, lcm->pending_map_requests_pool,
-  ({
-    if (!gid_address_cmp (&pmr->src, seid)
-        && !gid_address_cmp (&pmr->dst, deid))
+  m->locators = 0;
+  locator_set_t *ls = pool_elt_at_index (lcm->locator_set_pool,
+					 locator_set_index);
+  vec_foreach (li, ls->locator_indices)
+  {
+    loc = pool_elt_at_index (lcm->locator_pool, li[0]);
+    new = loc[0];
+    if (loc->local)
       {
-        duplicate_pmr = pmr;
-        break;
+          /* *INDENT-OFF* */
+          foreach_ip_interface_address (&lcm->im4->lookup_main, ia,
+                                        loc->sw_if_index, 1 /* unnumbered */,
+          ({
+            addr = ip_interface_address_get_address (&lcm->im4->lookup_main,
+                                                     ia);
+            ip_address_set (new_ip, addr, IP4);
+          }));
+
+          /* Add ipv6 locators */
+          foreach_ip_interface_address (&lcm->im6->lookup_main, ia,
+                                        loc->sw_if_index, 1 /* unnumbered */,
+          ({
+            addr = ip_interface_address_get_address (&lcm->im6->lookup_main,
+                                                     ia);
+            ip_address_set (new_ip, addr, IP6);
+          }));
+          /* *INDENT-ON* */
+
+	if (probed_loc && ip_address_cmp (probed_loc, new_ip) == 0)
+	  new.probed = 1;
       }
-  }));
+    vec_add1 (m->locators, new);
+  }
+}
+
+static mapping_t *
+build_map_register_record_list (lisp_cp_main_t * lcm)
+{
+  mapping_t *recs = 0, rec, *m;
+
+  /* *INDENT-OFF* */
+  pool_foreach(m, lcm->mapping_pool,
+  {
+    /* for now build only local mappings */
+    if (!m->local)
+      continue;
+
+    rec = m[0];
+    add_locators (lcm, &rec, m->locator_set_index, NULL);
+    vec_add1 (recs, rec);
+  });
   /* *INDENT-ON* */
 
-  if (!is_resend && duplicate_pmr)
+  return recs;
+}
+
+static int
+update_map_register_auth_data (map_register_hdr_t * map_reg_hdr,
+			       lisp_key_type_t key_id, u8 * key,
+			       u16 auth_data_len, u32 msg_len)
+{
+  MREG_KEY_ID (map_reg_hdr) = clib_host_to_net_u16 (key_id);
+  MREG_AUTH_DATA_LEN (map_reg_hdr) = clib_host_to_net_u16 (auth_data_len);
+
+  unsigned char *result = HMAC (get_encrypt_fcn (key_id), key, vec_len (key),
+				(unsigned char *) map_reg_hdr, msg_len, NULL,
+				NULL);
+  clib_memcpy (MREG_DATA (map_reg_hdr), result, auth_data_len);
+
+  return 0;
+}
+
+static vlib_buffer_t *
+build_map_register (lisp_cp_main_t * lcm, ip_address_t * sloc,
+		    ip_address_t * ms_ip, u64 * nonce_res, u8 want_map_notif,
+		    mapping_t * records, lisp_key_type_t key_id, u8 * key,
+		    u32 * bi_res)
+{
+  void *map_reg_hdr;
+  vlib_buffer_t *b;
+  u32 bi, auth_data_len = 0, msg_len = 0;
+  vlib_main_t *vm = lcm->vlib_main;
+
+  if (vlib_buffer_alloc (vm, &bi, 1) != 1)
     {
-      /* don't send the request if there is a pending map request already */
+      clib_warning ("Can't allocate buffer for Map-Register!");
       return 0;
     }
 
-  /* get locator-set for seid */
-  if (!lcm->lisp_pitr)
-    {
-      map_index = gid_dictionary_lookup (&lcm->mapping_index_by_gid, seid);
-      if (map_index == ~0)
-	{
-	  clib_warning ("No local mapping found in eid-table for %U!",
-			format_gid_address, seid);
-	  return -1;
-	}
+  b = vlib_get_buffer (vm, bi);
 
-      map = pool_elt_at_index (lcm->mapping_pool, map_index);
+  /* leave some space for the encap headers */
+  vlib_buffer_make_headroom (b, MAX_LISP_MSG_ENCAP_LEN);
 
-      if (!map->local)
-	{
-	  clib_warning
-	    ("Mapping found for src eid %U is not marked as local!",
-	     format_gid_address, seid);
-	  return -1;
-	}
-      ls_index = map->locator_set_index;
-    }
-  else
-    {
-      map_index = lcm->pitr_map_index;
-      map = pool_elt_at_index (lcm->mapping_pool, lcm->pitr_map_index);
-      ls_index = map->locator_set_index;
-    }
+  auth_data_len = auth_data_len_by_key_id (key_id);
+  map_reg_hdr = lisp_msg_put_map_register (b, records, want_map_notif,
+					   auth_data_len, nonce_res,
+					   &msg_len);
 
-  /* overwrite locator set if map-request itr-rlocs configured */
-  if (~0 != lcm->mreq_itr_rlocs)
-    {
-      ls_index = lcm->mreq_itr_rlocs;
-    }
+  update_map_register_auth_data (map_reg_hdr, key_id, key, auth_data_len,
+				 msg_len);
 
-  loc_set = pool_elt_at_index (lcm->locator_set_pool, ls_index);
+  /* push outer ip header */
+  pkt_push_udp_and_ip (vm, b, LISP_CONTROL_PORT, LISP_CONTROL_PORT, sloc,
+		       ms_ip);
+
+  bi_res[0] = bi;
+  return b;
+}
 
+static int
+get_egress_map_resolver_ip (lisp_cp_main_t * lcm, ip_address_t * ip)
+{
+  lisp_msmr_t *mr;
   while (lcm->do_map_resolver_election
 	 | (0 == ip_fib_get_first_egress_ip_for_dst (lcm,
 						     &lcm->active_map_resolver,
-						     &sloc)))
+						     ip)))
     {
       if (0 == elect_map_resolver (lcm))
-	/* all Mrs are down */
+	/* all map resolvers 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;
 	}
     }
+  return 0;
+}
 
-  /* build the encapsulated map request */
-  b = build_encapsulated_map_request (lcm, seid, deid, loc_set,
-				      &lcm->active_map_resolver,
-				      &sloc, is_smr_invoked, &nonce, &bi);
+static int
+send_rloc_probe (lisp_cp_main_t * lcm, gid_address_t * deid,
+		 u32 local_locator_set_index, ip_address_t * sloc,
+		 ip_address_t * rloc)
+{
+  locator_set_t *ls;
+  u32 bi;
+  vlib_buffer_t *b;
+  vlib_frame_t *f;
+  u64 nonce = 0;
+  u32 next_index, *to_next;
+  gid_address_t *itr_rlocs;
+
+  ls = pool_elt_at_index (lcm->locator_set_pool, local_locator_set_index);
+  itr_rlocs = build_itr_rloc_list (lcm, ls);
 
+  b = build_map_request (lcm, deid, sloc, rloc, itr_rlocs, &nonce, &bi);
+  vec_free (itr_rlocs);
   if (!b)
     return -1;
 
-  /* 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;
 
@@ -3014,23 +3521,242 @@ _send_encapsulated_map_request (lisp_cp_main_t * lcm,
   f->n_vectors = 1;
   vlib_put_frame_to_node (lcm->vlib_main, next_index, f);
 
-  if (duplicate_pmr)
-    /* if there is a pending request already update it */
-    {
-      if (clib_fifo_elts (duplicate_pmr->nonces) >= PENDING_MREQ_QUEUE_LEN)
-	{
-	  /* remove the oldest nonce */
-	  u64 CLIB_UNUSED (tmp), *nonce_del;
-	  nonce_del = clib_fifo_head (duplicate_pmr->nonces);
-	  hash_unset (lcm->pending_map_requests_by_nonce, nonce_del[0]);
-	  clib_fifo_sub1 (duplicate_pmr->nonces, tmp);
-	}
+  hash_set (lcm->map_register_messages_by_nonce, nonce, 0);
+  return 0;
+}
 
-      clib_fifo_add1 (duplicate_pmr->nonces, nonce);
-      hash_set (lcm->pending_map_requests_by_nonce, nonce,
-		duplicate_pmr - lcm->pending_map_requests_pool);
-    }
-  else
+static int
+send_rloc_probes (lisp_cp_main_t * lcm)
+{
+  u8 lprio = 0;
+  mapping_t *lm;
+  fwd_entry_t *e;
+  locator_pair_t *lp;
+  u32 si;
+
+  /* *INDENT-OFF* */
+  pool_foreach (e, lcm->fwd_entry_pool,
+  {
+    if (vec_len (e->locator_pairs) == 0)
+      continue;
+
+    si = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &e->leid);
+    if (~0 == si)
+      {
+        clib_warning ("internal error: cannot find local eid %U in "
+                      "map-cache!", format_gid_address, &e->leid);
+        continue;
+      }
+    lm = pool_elt_at_index (lcm->mapping_pool, si);
+
+    /* get the best (lowest) priority */
+    lprio = e->locator_pairs[0].priority;
+
+    /* send rloc-probe for pair(s) with the best remote locator priority */
+    vec_foreach (lp, e->locator_pairs)
+      {
+        if (lp->priority != lprio)
+          break;
+
+        /* get first remote locator */
+        send_rloc_probe (lcm, &e->reid, lm->locator_set_index, &lp->lcl_loc,
+                         &lp->rmt_loc);
+      }
+  });
+  /* *INDENT-ON* */
+
+  return 0;
+}
+
+static int
+send_map_register (lisp_cp_main_t * lcm, u8 want_map_notif)
+{
+  u32 bi;
+  vlib_buffer_t *b;
+  ip_address_t sloc;
+  vlib_frame_t *f;
+  u64 nonce = 0;
+  u32 next_index, *to_next;
+  ip_address_t *ms = 0;
+  mapping_t *records, *r, *g;
+
+  // TODO: support multiple map servers and do election
+  if (0 == vec_len (lcm->map_servers))
+    return -1;
+
+  ms = &lcm->map_servers[0].address;
+
+  if (0 == ip_fib_get_first_egress_ip_for_dst (lcm, ms, &sloc))
+    {
+      clib_warning ("no eligible interface address found for %U!",
+		    format_ip_address, &lcm->map_servers[0]);
+      return -1;
+    }
+
+  records = build_map_register_record_list (lcm);
+  if (!records)
+    return -1;
+
+  vec_foreach (r, records)
+  {
+    u8 *key = r->key;
+    u8 key_id = r->key_id;
+
+    if (!key)
+      continue;			/* no secret key -> map-register cannot be sent */
+
+    g = 0;
+    // TODO: group mappings that share common key
+    vec_add1 (g, r[0]);
+    b = build_map_register (lcm, &sloc, ms, &nonce, want_map_notif, g,
+			    key_id, key, &bi);
+    vec_free (g);
+    if (!b)
+      continue;
+
+    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 (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 (lcm->vlib_main, next_index, f);
+
+    hash_set (lcm->map_register_messages_by_nonce, nonce, 0);
+  }
+  free_map_register_records (records);
+
+  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)
+{
+  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, *duplicate_pmr = 0;
+  ip_address_t sloc;
+  u32 ls_index;
+
+  /* if there is already a pending request remember it */
+
+  /* *INDENT-OFF* */
+  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;
+      }
+  }));
+  /* *INDENT-ON* */
+
+  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)
+    {
+      map_index = gid_dictionary_lookup (&lcm->mapping_index_by_gid, seid);
+      if (map_index == ~0)
+	{
+	  clib_warning ("No local mapping found in eid-table for %U!",
+			format_gid_address, seid);
+	  return -1;
+	}
+
+      map = pool_elt_at_index (lcm->mapping_pool, map_index);
+
+      if (!map->local)
+	{
+	  clib_warning
+	    ("Mapping found for src eid %U is not marked as local!",
+	     format_gid_address, seid);
+	  return -1;
+	}
+      ls_index = map->locator_set_index;
+    }
+  else
+    {
+      map_index = lcm->pitr_map_index;
+      map = pool_elt_at_index (lcm->mapping_pool, lcm->pitr_map_index);
+      ls_index = map->locator_set_index;
+    }
+
+  /* overwrite locator set if map-request itr-rlocs configured */
+  if (~0 != lcm->mreq_itr_rlocs)
+    {
+      ls_index = lcm->mreq_itr_rlocs;
+    }
+
+  loc_set = pool_elt_at_index (lcm->locator_set_pool, ls_index);
+
+  if (get_egress_map_resolver_ip (lcm, &sloc) < 0)
+    {
+      if (duplicate_pmr)
+	duplicate_pmr->to_be_removed = 1;
+      return -1;
+    }
+
+  /* build the encapsulated map request */
+  b = build_encapsulated_map_request (lcm, seid, deid, loc_set,
+				      &lcm->active_map_resolver,
+				      &sloc, is_smr_invoked, &nonce, &bi);
+
+  if (!b)
+    return -1;
+
+  /* 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 (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 (lcm->vlib_main, next_index, f);
+
+  if (duplicate_pmr)
+    /* if there is a pending request already update it */
+    {
+      if (clib_fifo_elts (duplicate_pmr->nonces) >= PENDING_MREQ_QUEUE_LEN)
+	{
+	  /* remove the oldest nonce */
+	  u64 CLIB_UNUSED (tmp), *nonce_del;
+	  nonce_del = clib_fifo_head (duplicate_pmr->nonces);
+	  hash_unset (lcm->pending_map_requests_by_nonce, nonce_del[0]);
+	  clib_fifo_sub1 (duplicate_pmr->nonces, tmp);
+	}
+
+      clib_fifo_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);
@@ -3380,81 +4106,354 @@ format_lisp_cp_input_trace (u8 * s, va_list * args)
   return s;
 }
 
+static void
+remove_expired_mapping (lisp_cp_main_t * lcm, u32 mi)
+{
+  mapping_t *m;
+
+  m = pool_elt_at_index (lcm->mapping_pool, mi);
+  lisp_add_del_adjacency (lcm, 0, &m->eid, 0 /* is_add */ );
+  vnet_lisp_add_del_mapping (&m->eid, 0, 0, 0, ~0, 0 /* is_add */ ,
+			     0 /* is_static */ , 0);
+  mapping_delete_timer (lcm, mi);
+}
+
+static void
+mapping_start_expiration_timer (lisp_cp_main_t * lcm, u32 mi,
+				f64 expiration_time)
+{
+  mapping_t *m;
+  u64 now = clib_cpu_time_now ();
+  u64 cpu_cps = lcm->vlib_main->clib_time.clocks_per_second;
+  u64 exp_clock_time = now + expiration_time * cpu_cps;
+
+  m = pool_elt_at_index (lcm->mapping_pool, mi);
+
+  m->timer_set = 1;
+  timing_wheel_insert (&lcm->wheel, exp_clock_time, mi);
+}
+
+static void
+map_records_arg_free (map_records_arg_t * a)
+{
+  mapping_t *m;
+  vec_foreach (m, a->mappings)
+  {
+    vec_free (m->locators);
+    gid_address_free (&m->eid);
+  }
+
+  clib_mem_free (a);
+}
+
 void *
-process_map_reply (void *arg)
+process_map_reply (map_records_arg_t * a)
 {
+  mapping_t *m;
   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;
+  u32 dst_map_index = 0;
   pending_map_request_t *pmr;
-  locator_t probed;
-  map_reply_hdr_t *mrep_hdr;
-  u64 nonce, *noncep;
-  gid_address_t deid;
+  u64 *noncep;
   uword *pmr_index;
-  u8 authoritative, action;
-  locator_t *locators = 0, *loc;
 
-  mrep_hdr = vlib_buffer_get_current (b);
-
-  lisp_pending_map_request_lock (lcm);
+  if (a->is_rloc_probe)
+    goto done;
 
   /* Check pending requests table and nonce */
-  nonce = MREP_NONCE (mrep_hdr);
-  pmr_index = hash_get (lcm->pending_map_requests_by_nonce, nonce);
+  pmr_index = hash_get (lcm->pending_map_requests_by_nonce, a->nonce);
   if (!pmr_index)
     {
-      clib_warning ("No pending map-request entry with nonce %lu!", nonce);
+      clib_warning ("No pending map-request entry with nonce %lu!", a->nonce);
       goto done;
     }
   pmr = pool_elt_at_index (lcm->pending_map_requests_pool, pmr_index[0]);
 
-  vlib_buffer_pull (b, sizeof (*mrep_hdr));
+  vec_foreach (m, a->mappings)
+  {
+    /* insert/update mappings cache */
+    vnet_lisp_add_del_mapping (&m->eid, m->locators, m->action,
+			       m->authoritative, m->ttl,
+			       1, 0 /* is_static */ , &dst_map_index);
 
-  for (i = 0; i < MREP_REC_COUNT (mrep_hdr); i++)
+    /* try to program forwarding only if mapping saved or updated */
+    if ((u32) ~ 0 != dst_map_index)
+      {
+	lisp_add_del_adjacency (lcm, &pmr->src, &m->eid, 1);
+	if ((u32) ~ 0 != m->ttl)
+	  mapping_start_expiration_timer (lcm, dst_map_index, m->ttl * 60);
+      }
+  }
+
+  /* remove pending map request entry */
+
+  /* *INDENT-OFF* */
+  clib_fifo_foreach (noncep, pmr->nonces, ({
+    hash_unset(lcm->pending_map_requests_by_nonce, noncep[0]);
+  }));
+  /* *INDENT-ON* */
+
+  clib_fifo_free (pmr->nonces);
+  pool_put (lcm->pending_map_requests_pool, pmr);
+
+done:
+  map_records_arg_free (a);
+  return 0;
+}
+
+static int
+is_auth_data_valid (map_notify_hdr_t * h, u32 msg_len,
+		    lisp_key_type_t key_id, u8 * key)
+{
+  u8 *auth_data = 0;
+  u16 auth_data_len;
+  int result;
+
+  auth_data_len = auth_data_len_by_key_id (key_id);
+  if ((u16) ~ 0 == auth_data_len)
     {
-      h = vlib_buffer_get_current (b);
-      ttl = clib_net_to_host_u32 (MAP_REC_TTL (h));
-      action = MAP_REC_ACTION (h);
-      authoritative = MAP_REC_AUTH (h);
+      clib_warning ("invalid length for key_id %d!", key_id);
+      return 0;
+    }
 
-      len = lisp_msg_parse_mapping_record (b, &deid, &locators, &probed);
+  /* save auth data */
+  vec_validate (auth_data, auth_data_len - 1);
+  clib_memcpy (auth_data, MNOTIFY_DATA (h), auth_data_len);
+
+  /* clear auth data */
+  memset (MNOTIFY_DATA (h), 0, auth_data_len);
+
+  /* get hash of the message */
+  unsigned char *code = HMAC (get_encrypt_fcn (key_id), key, vec_len (key),
+			      (unsigned char *) h, msg_len, NULL, NULL);
+
+  result = memcmp (code, auth_data, auth_data_len);
+
+  vec_free (auth_data);
+
+  return !result;
+}
+
+static void
+process_map_notify (map_records_arg_t * a)
+{
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+  uword *pmr_index;
+
+  pmr_index = hash_get (lcm->map_register_messages_by_nonce, a->nonce);
+  if (!pmr_index)
+    {
+      clib_warning ("No pending map-register entry with nonce %lu!",
+		    a->nonce);
+      return;
+    }
+
+  map_records_arg_free (a);
+  hash_unset (lcm->map_register_messages_by_nonce, a->nonce);
+}
+
+static mapping_t *
+get_mapping (lisp_cp_main_t * lcm, gid_address_t * e)
+{
+  u32 mi;
+
+  mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, e);
+  if (~0 == mi)
+    {
+      clib_warning ("eid %U not found in map-cache!", unformat_gid_address,
+		    e);
+      return 0;
+    }
+  return pool_elt_at_index (lcm->mapping_pool, mi);
+}
+
+/**
+ * When map-notify is received it is necessary that all EIDs in the record
+ * list share common key. The key is then used to verify authentication
+ * data in map-notify message.
+ */
+static int
+map_record_integrity_check (lisp_cp_main_t * lcm, mapping_t * maps,
+			    u32 key_id, u8 ** key_out)
+{
+  u32 i, len = vec_len (maps);
+  mapping_t *m;
+
+  /* get key of the first mapping */
+  m = get_mapping (lcm, &maps[0].eid);
+  if (!m || !m->key)
+    return -1;
+
+  key_out[0] = m->key;
+
+  for (i = 1; i < len; i++)
+    {
+      m = get_mapping (lcm, &maps[i].eid);
+      if (!m || !m->key)
+	return -1;
+
+      if (key_id != m->key_id || vec_cmp (m->key, key_out[0]))
+	{
+	  clib_warning ("keys does not match! %v, %v", key_out[0], m->key);
+	  return -1;
+	}
+    }
+  return 0;
+}
+
+static int
+parse_map_records (vlib_buffer_t * b, map_records_arg_t * a, u8 count)
+{
+  locator_t *locators = 0;
+  u32 i, len;
+  gid_address_t deid;
+  mapping_t m;
+  locator_t *loc;
+
+  /* parse record eid */
+  for (i = 0; i < count; i++)
+    {
+      len = lisp_msg_parse_mapping_record (b, &deid, &locators, NULL);
       if (len == ~0)
 	{
 	  clib_warning ("Failed to parse mapping record!");
-	  vec_foreach (loc, locators)
-	  {
-	    locator_free (loc);
-	  }
+	  vec_foreach (loc, locators) locator_free (loc);
 	  vec_free (locators);
-	  goto done;
+	  return -1;
 	}
 
-      /* insert/update mappings cache */
-      vnet_lisp_add_del_mapping (&deid, locators, action, authoritative, ttl,
-				 1, 0 /* is_static */ , &dst_map_index);
+      m.locators = locators;
+      gid_address_copy (&m.eid, &deid);
+      vec_add1 (a->mappings, m);
+    }
 
-      /* 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);
+  return 0;
+}
+
+static map_records_arg_t *
+parse_map_notify (vlib_buffer_t * b)
+{
+  int rc = 0;
+  map_notify_hdr_t *mnotif_hdr;
+  lisp_key_type_t key_id;
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+  u8 *key = 0;
+  gid_address_t deid;
+  u16 auth_data_len = 0;
+  u8 record_count;
+  map_records_arg_t *a = clib_mem_alloc (sizeof (*a));
+
+  memset (a, 0, sizeof (*a));
+  mnotif_hdr = vlib_buffer_get_current (b);
+  vlib_buffer_pull (b, sizeof (*mnotif_hdr));
+  memset (&deid, 0, sizeof (deid));
 
-      vec_free (locators);
+  a->nonce = MNOTIFY_NONCE (mnotif_hdr);
+  key_id = clib_net_to_host_u16 (MNOTIFY_KEY_ID (mnotif_hdr));
+  auth_data_len = auth_data_len_by_key_id (key_id);
+
+  /* advance buffer by authentication data */
+  vlib_buffer_pull (b, auth_data_len);
+
+  record_count = MNOTIFY_REC_COUNT (mnotif_hdr);
+  rc = parse_map_records (b, a, record_count);
+  if (rc != 0)
+    {
+      map_records_arg_free (a);
+      return 0;
     }
 
-  /* remove pending map request entry */
+  rc = map_record_integrity_check (lcm, a->mappings, key_id, &key);
+  if (rc != 0)
+    {
+      map_records_arg_free (a);
+      return 0;
+    }
 
-  /* *INDENT-OFF* */
-  clib_fifo_foreach (noncep, pmr->nonces, ({
-    hash_unset(lcm->pending_map_requests_by_nonce, noncep[0]);
-  }));
-  /* *INDENT-ON* */
+  /* verify authentication data */
+  if (!is_auth_data_valid (mnotif_hdr, vlib_buffer_get_tail (b)
+			   - (u8 *) mnotif_hdr, key_id, key))
+    {
+      clib_warning ("Map-notify auth data verification failed for nonce %lu!",
+		    a->nonce);
+      map_records_arg_free (a);
+      return 0;
+    }
+  return a;
+}
 
-  clib_fifo_free (pmr->nonces);
-  pool_put (lcm->pending_map_requests_pool, pmr);
+static vlib_buffer_t *
+build_map_reply (lisp_cp_main_t * lcm, ip_address_t * sloc,
+		 ip_address_t * dst, u64 nonce, u8 probe_bit,
+		 mapping_t * records, u16 dst_port, u32 * bi_res)
+{
+  vlib_buffer_t *b;
+  u32 bi;
+  vlib_main_t *vm = lcm->vlib_main;
 
-done:
-  lisp_pending_map_request_unlock (lcm);
+  if (vlib_buffer_alloc (vm, &bi, 1) != 1)
+    {
+      clib_warning ("Can't allocate buffer for Map-Register!");
+      return 0;
+    }
+
+  b = vlib_get_buffer (vm, bi);
+
+  /* leave some space for the encap headers */
+  vlib_buffer_make_headroom (b, MAX_LISP_MSG_ENCAP_LEN);
+
+  lisp_msg_put_map_reply (b, records, nonce, probe_bit);
+
+  /* push outer ip header */
+  pkt_push_udp_and_ip (vm, b, LISP_CONTROL_PORT, dst_port, sloc, dst);
+
+  bi_res[0] = bi;
+  return b;
+}
+
+static int
+send_map_reply (lisp_cp_main_t * lcm, u32 mi, ip_address_t * dst,
+		u8 probe_bit, u64 nonce, u16 dst_port,
+		ip_address_t * probed_loc)
+{
+  ip_address_t src;
+  u32 bi;
+  vlib_buffer_t *b;
+  vlib_frame_t *f;
+  u32 next_index, *to_next;
+  mapping_t *records = 0, *m;
+
+  m = pool_elt_at_index (lcm->mapping_pool, mi);
+  if (!m)
+    return -1;
+
+  vec_add1 (records, m[0]);
+  add_locators (lcm, &records[0], m->locator_set_index, probed_loc);
+  memset (&src, 0, sizeof (src));
+
+  if (!ip_fib_get_first_egress_ip_for_dst (lcm, dst, &src))
+    {
+      clib_warning ("can't find inteface address for %U", format_ip_address,
+		    dst);
+      return -1;
+    }
+
+  b = build_map_reply (lcm, &src, dst, nonce, probe_bit, records, dst_port,
+		       &bi);
+  if (!b)
+    return -1;
+  free_map_register_records (records);
+
+  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 (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 (lcm->vlib_main, next_index, f);
   return 0;
 }
 
@@ -3462,20 +4461,49 @@ void
 process_map_request (vlib_main_t * vm, lisp_cp_main_t * lcm,
 		     vlib_buffer_t * b)
 {
+  u8 *ip_hdr = 0, *udp_hdr;
+  ip4_header_t *ip4;
+  ip6_header_t *ip6;
+  ip_address_t *dst_loc = 0, probed_loc, src_loc;
+  mapping_t m;
   map_request_hdr_t *mreq_hdr;
   gid_address_t src, dst;
-//  u64 nonce;
+  u64 nonce;
   u32 i, len = 0;
-  gid_address_t *itr_rlocs = 0, *rloc;
+  gid_address_t *itr_rlocs = 0;
 
   mreq_hdr = vlib_buffer_get_current (b);
+
+  // TODO ugly workaround to find out whether LISP is carried by ip4 or 6
+  // and needs to be fixed
+  udp_hdr = (u8 *) vlib_buffer_get_current (b) - sizeof (udp_header_t);
+  ip4 = (ip4_header_t *) (udp_hdr - sizeof (ip4_header_t));
+  ip6 = (ip6_header_t *) (udp_hdr - sizeof (ip6_header_t));
+
+  if ((ip4->ip_version_and_header_length & 0xF0) == 0x40)
+    ip_hdr = (u8 *) ip4;
+  else
+    {
+      u32 flags = clib_net_to_host_u32
+	(ip6->ip_version_traffic_class_and_flow_label);
+      if ((flags & 0xF0000000) == 0x60000000)
+	ip_hdr = (u8 *) ip6;
+      else
+	{
+	  clib_warning ("internal error: cannot determine whether packet "
+			"is ip4 or 6!");
+	  return;
+	}
+    }
+
   vlib_buffer_pull (b, sizeof (*mreq_hdr));
 
-//  nonce = MREQ_NONCE(mreq_hdr);
+  nonce = MREQ_NONCE (mreq_hdr);
 
-  if (!MREQ_SMR (mreq_hdr))
+  if (!MREQ_SMR (mreq_hdr) && !MREQ_RLOC_PROBE (mreq_hdr))
     {
-      clib_warning ("Only SMR Map-Requests supported for now!");
+      clib_warning
+	("Only SMR Map-Requests and RLOC probe supported for now!");
       return;
     }
 
@@ -3484,19 +4512,11 @@ process_map_request (vlib_main_t * vm, lisp_cp_main_t * lcm,
   if (len == ~0)
     return;
 
-  /* for now we don't do anything with the itr's rlocs */
-  len =
-    lisp_msg_parse_itr_rlocs (b, &itr_rlocs,
-			      MREQ_ITR_RLOC_COUNT (mreq_hdr) + 1);
+  len = lisp_msg_parse_itr_rlocs (b, &itr_rlocs,
+				  MREQ_ITR_RLOC_COUNT (mreq_hdr) + 1);
   if (len == ~0)
     return;
 
-  /* TODO: RLOCs are currently unused, so free them for now */
-  vec_foreach (rloc, itr_rlocs)
-  {
-    gid_address_free (rloc);
-  }
-
   /* parse eid records and send SMR-invoked map-requests */
   for (i = 0; i < MREQ_REC_COUNT (mreq_hdr); i++)
     {
@@ -3505,25 +4525,90 @@ process_map_request (vlib_main_t * vm, lisp_cp_main_t * lcm,
       if (len == ~0)
 	{
 	  clib_warning ("Can't parse map-request EID-record");
-	  return;
+	  goto done;
+	}
+
+      if (MREQ_SMR (mreq_hdr))
+	{
+	  /* send SMR-invoked map-requests */
+	  queue_map_request (&dst, &src, 1 /* invoked */ , 0 /* resend */ );
+	}
+      else if (MREQ_RLOC_PROBE (mreq_hdr))
+	{
+	  memset (&m, 0, sizeof (m));
+	  u32 mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &dst);
+
+	  // TODO: select best locator; for now use the first one
+	  dst_loc = &gid_address_ip (&itr_rlocs[0]);
+
+	  /* get src/dst IP addresses */
+	  get_src_and_dst_ip (ip_hdr, &src_loc, &probed_loc);
+
+	  // TODO get source port from buffer
+	  u16 src_port = LISP_CONTROL_PORT;
+
+	  send_map_reply (lcm, mi, dst_loc, 1 /* probe-bit */ , nonce,
+			  src_port, &probed_loc);
 	}
-      /* send SMR-invoked map-requests */
-      queue_map_request (&dst, &src, 1 /* invoked */ , 0 /* resend */ );
     }
+
+done:
+  vec_free (itr_rlocs);
 }
 
-static void
-queue_map_reply (vlib_buffer_t * b)
+static map_records_arg_t *
+parse_map_reply (vlib_buffer_t * b)
 {
-  vlib_buffer_t *a = clib_mem_alloc (sizeof (a[0]) + b->current_length);
+  locator_t probed;
+  gid_address_t deid;
+  void *h;
+  u32 i, len = 0;
+  mapping_t m;
+  map_reply_hdr_t *mrep_hdr;
+  map_records_arg_t *a = clib_mem_alloc (sizeof (*a));
+  memset (a, 0, sizeof (*a));
+  locator_t *locators;
 
-  clib_memcpy (a->data, b->data + b->current_data, b->current_length);
-  a->current_length = b->current_length;
-  a->current_data = 0;
+  mrep_hdr = vlib_buffer_get_current (b);
+  a->nonce = MREP_NONCE (mrep_hdr);
+  a->is_rloc_probe = MREP_RLOC_PROBE (mrep_hdr);
+  vlib_buffer_pull (b, sizeof (*mrep_hdr));
 
-  vl_api_rpc_call_main_thread (process_map_reply, (u8 *) a, sizeof (a[0])
-			       + a->current_length);
-  clib_mem_free (a);
+  for (i = 0; i < MREP_REC_COUNT (mrep_hdr); i++)
+    {
+      memset (&m, 0, sizeof (m));
+      locators = 0;
+      h = vlib_buffer_get_current (b);
+
+      m.ttl = clib_net_to_host_u32 (MAP_REC_TTL (h));
+      m.action = MAP_REC_ACTION (h);
+      m.authoritative = MAP_REC_AUTH (h);
+
+      len = lisp_msg_parse_mapping_record (b, &deid, &locators, &probed);
+      if (len == ~0)
+	{
+	  clib_warning ("Failed to parse mapping record!");
+	  map_records_arg_free (a);
+	  return 0;
+	}
+
+      m.locators = locators;
+      gid_address_copy (&m.eid, &deid);
+      vec_add1 (a->mappings, m);
+    }
+  return a;
+}
+
+static void
+queue_map_reply_for_processing (map_records_arg_t * a)
+{
+  vl_api_rpc_call_main_thread (process_map_reply, (u8 *) a, sizeof (a));
+}
+
+static void
+queue_map_notify_for_processing (map_records_arg_t * a)
+{
+  vl_api_rpc_call_main_thread (process_map_notify, (u8 *) a, sizeof (a[0]));
 }
 
 static uword
@@ -3533,6 +4618,7 @@ lisp_cp_input (vlib_main_t * vm, vlib_node_runtime_t * node,
   u32 n_left_from, *from, *to_next_drop;
   lisp_msg_type_e type;
   lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+  map_records_arg_t *a;
 
   from = vlib_frame_vector_args (from_frame);
   n_left_from = from_frame->n_vectors;
@@ -3562,11 +4648,18 @@ lisp_cp_input (vlib_main_t * vm, vlib_node_runtime_t * node,
 	  switch (type)
 	    {
 	    case LISP_MAP_REPLY:
-	      queue_map_reply (b0);
+	      a = parse_map_reply (b0);
+	      if (a)
+		queue_map_reply_for_processing (a);
 	      break;
 	    case LISP_MAP_REQUEST:
 	      process_map_request (vm, lcm, b0);
 	      break;
+	    case LISP_MAP_NOTIFY:
+	      a = parse_map_notify (b0);
+	      if (a)
+		queue_map_notify_for_processing (a);
+	      break;
 	    default:
 	      clib_warning ("Unsupported LISP message type %d", type);
 	      break;
@@ -3622,10 +4715,6 @@ lisp_cp_init (vlib_main_t * vm)
   lcm->lisp_pitr = 0;
   memset (&lcm->active_map_resolver, 0, sizeof (lcm->active_map_resolver));
 
-  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;
   lcm->map_request_mode = MR_MODE_DST_ONLY;
@@ -3639,6 +4728,8 @@ lisp_cp_init (vlib_main_t * vm)
   udp_register_dst_port (vm, UDP_DST_PORT_lisp_cp6,
 			 lisp_cp_input_node.index, 0 /* is_ip4 */ );
 
+  u64 now = clib_cpu_time_now ();
+  timing_wheel_init (&lcm->wheel, now, vm->clib_time.clocks_per_second);
   return 0;
 }
 
@@ -3649,16 +4740,9 @@ send_map_request_thread_fn (void *arg)
   lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
 
   if (a->is_resend)
-    /* if resending, we already have the lock */
     resend_encapsulated_map_request (lcm, &a->seid, &a->deid, a->smr_invoked);
   else
-    {
-      /* get lock before sending map-request */
-      lisp_pending_map_request_lock (lcm);
-      send_encapsulated_map_request (lcm, &a->seid, &a->deid, a->smr_invoked);
-      lisp_pending_map_request_unlock (lcm);
-    }
-
+    send_encapsulated_map_request (lcm, &a->seid, &a->deid, a->smr_invoked);
 
   return 0;
 }
@@ -3687,7 +4771,7 @@ 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;
+  lisp_msmr_t *mr;
 
   if (r->time_to_expire - dt < 0)
     /* it's time to decide what to do with this pending request */
@@ -3740,8 +4824,6 @@ remove_dead_pending_map_requests (lisp_cp_main_t * lcm)
   pending_map_request_t *pmr;
   u32 *to_be_removed = 0, *pmr_index;
 
-  ASSERT (*lcm->pending_map_request_lock);
-
   /* *INDENT-OFF* */
   pool_foreach (pmr, lcm->pending_map_requests_pool,
   ({
@@ -3762,10 +4844,50 @@ remove_dead_pending_map_requests (lisp_cp_main_t * lcm)
   vec_free (to_be_removed);
 }
 
+static void
+update_rloc_probing (lisp_cp_main_t * lcm, f64 dt)
+{
+  static f64 time_left = RLOC_PROBING_INTERVAL;
+
+  if (!lcm->is_enabled || !lcm->rloc_probing)
+    return;
+
+  time_left -= dt;
+  if (time_left <= 0)
+    {
+      time_left = RLOC_PROBING_INTERVAL;
+      send_rloc_probes (lcm);
+    }
+}
+
+static void
+update_map_register (lisp_cp_main_t * lcm, f64 dt)
+{
+  static f64 time_left = QUICK_MAP_REGISTER_INTERVAL;
+  static u64 mreg_sent_counter = 0;
+
+  if (!lcm->is_enabled || !lcm->map_registering)
+    return;
+
+  time_left -= dt;
+  if (time_left <= 0)
+    {
+      if (mreg_sent_counter >= QUICK_MAP_REGISTER_MSG_COUNT)
+	time_left = MAP_REGISTER_INTERVAL;
+      else
+	{
+	  mreg_sent_counter++;
+	  time_left = QUICK_MAP_REGISTER_INTERVAL;
+	}
+      send_map_register (lcm, 1 /* want map notify */ );
+    }
+}
+
 static uword
 send_map_resolver_service (vlib_main_t * vm,
 			   vlib_node_runtime_t * rt, vlib_frame_t * f)
 {
+  u32 *expired = 0;
   f64 period = 2.0;
   pending_map_request_t *pmr;
   lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
@@ -3777,8 +4899,6 @@ send_map_resolver_service (vlib_main_t * vm,
       /* currently no signals are expected - just wait for clock */
       (void) vlib_process_get_events (vm, 0);
 
-      lisp_pending_map_request_lock (lcm);
-
       /* *INDENT-OFF* */
       pool_foreach (pmr, lcm->pending_map_requests_pool,
       ({
@@ -3789,7 +4909,21 @@ send_map_resolver_service (vlib_main_t * vm,
 
       remove_dead_pending_map_requests (lcm);
 
-      lisp_pending_map_request_unlock (lcm);
+      update_map_register (lcm, period);
+      update_rloc_probing (lcm, period);
+
+      u64 now = clib_cpu_time_now ();
+
+      expired = timing_wheel_advance (&lcm->wheel, now, expired, 0);
+      if (vec_len (expired) > 0)
+	{
+	  u32 *mi = 0;
+	  vec_foreach (mi, expired)
+	  {
+	    remove_expired_mapping (lcm, mi[0]);
+	  }
+	  _vec_len (expired) = 0;
+	}
     }
 
   /* unreachable */