13 files changed, 2131 insertions, 73 deletions

diff --git a/vnet/test/lisp-cp/test_cp_serdes.c b/vnet/test/lisp-cp/test_cp_serdes.c
index 7bfe6e35378..2d1cf06269b 100644
--- a/vnet/test/lisp-cp/test_cp_serdes.c
+++ b/vnet/test/lisp-cp/test_cp_serdes.c
@@ -181,6 +181,7 @@ build_map_request (lisp_cp_main_t * lcm, vlib_buffer_t * b,
   gid_address_t _seid, * seid = &_seid;
   gid_address_t _deid, * deid = &_deid;
   u8 is_smr_invoked = 1;
+  u8 rloc_probe_set = 0;
   u64 nonce = 0;
   map_request_hdr_t * h = 0;
   memset (deid, 0, sizeof (deid[0]));
@@ -198,7 +199,7 @@ build_map_request (lisp_cp_main_t * lcm, vlib_buffer_t * b,
   gid_address_ippref_len (deid) = 24;
 
   h = lisp_msg_put_mreq (lcm, b, seid, deid, rlocs,
-                     is_smr_invoked, &nonce);
+                     is_smr_invoked, rloc_probe_set, &nonce);
   vec_free(rlocs);
   return h;
 }
@@ -384,6 +385,107 @@ done:
   return error;
 }
 
+/* generate a vector of eid records */
+static mapping_t *
+build_test_map_records ()
+{
+  mapping_t * records = 0;
+
+  mapping_t r = {
+    .ttl = 0x44332211,
+    .eid = {
+      .type = GID_ADDR_MAC,
+      .mac = {1, 2, 3, 4, 5, 6},
+      .vni = 0x0
+    }
+  };
+
+  locator_t loc = {
+    .weight = 1,
+    .priority = 2,
+    .local = 1,
+    .address = {
+      .type = GID_ADDR_IP_PREFIX,
+      .ippref = {
+        .addr = {
+          .ip.v4.as_u32 = 0x99887766,
+          .version = IP4
+        }
+      }
+    }
+  };
+  vec_add1 (r.locators, loc);
+  vec_add1 (records, r);
+
+  return records;
+}
+
+static void
+free_test_map_records (mapping_t * maps)
+{
+  mapping_t * map;
+  vec_foreach (map, maps)
+    {
+      vec_free (map->locators);
+    }
+  vec_free (maps);
+}
+
+static clib_error_t *
+test_lisp_map_register ()
+{
+  vlib_buffer_t *b;
+  clib_error_t * error = 0;
+  u64 nonce;
+  u32 msg_len = 0;
+  mapping_t * records = build_test_map_records ();
+
+  u8 * data = clib_mem_alloc(500);
+  memset(data, 0, 500);
+  b = (vlib_buffer_t *) data;
+
+  lisp_msg_put_map_register (b, records, 1 /* want map notify */,
+                            20 /* length of HMAC_SHA_1_96 */,
+                            &nonce, &msg_len);
+  free_test_map_records (records);
+
+  /* clear Nonce to simplify comparison */
+  memset((u8 *)b->data + 4, 0, 8);
+
+  /* clear authentication data */
+  memset ((u8 *)b->data + 16, 0, 20);
+
+  u8 expected_data[] = {
+    0x30, 0x00, 0x01, 0x01, /* type; rsvd; want notify; REC count */
+    0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, /* nonce */
+    0x00, 0x00, 0x00, 0x00, /* key id, auth data length:
+                              both are zeroes because those are set in another
+                              function (see auth_data_len_by_key_id())*/
+    0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00,
+    0x00, 0x00, 0x00, 0x00, /* auth data */
+
+    /* first record */
+    0x44, 0x33, 0x22, 0x11, /* ttl */
+    0x01, 0x00, 0x00, 0x00, /* loc count, eid len, ACT, A */
+    0x00, 0x00, 0x40, 0x05, /* rsvd, map ver num, AFI = MAC */
+    0x01, 0x02, 0x03, 0x04,
+    0x05, 0x06,             /* MAC EID */
+
+    /* locator 1 */
+    0x02, 0x01, 0x00, 0x00, /* prio, weight, mprio, mweight */
+    0x00, 0x04, 0x00, 0x01, /* flags, AFI = ipv4 */
+    0x66, 0x77, 0x88, 0x99, /* ipv4 locator address */
+  };
+  _assert (0 == memcmp (expected_data, b->data, sizeof (expected_data)));
+done:
+  clib_mem_free (data);
+  return error;
+}
+
 static clib_error_t *
 test_lisp_parse_lcaf ()
 {
@@ -505,7 +607,8 @@ done:
   _(lisp_msg_push_ecm)                    \
   _(lisp_msg_parse)                       \
   _(lisp_msg_parse_mapping_record)        \
-  _(lisp_parse_lcaf)
+  _(lisp_parse_lcaf)                      \
+  _(lisp_map_register)
 
 int run_tests (void)
 {
diff --git a/vnet/vnet/lisp-cp/control.c b/vnet/vnet/lisp-cp/control.c
index 3b843a0158b..40ce6b0c4da 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;
@@ -38,6 +41,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);
@@ -362,6 +397,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)
 {
@@ -539,6 +588,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.
  */
@@ -570,6 +745,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,
@@ -691,6 +868,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));
@@ -709,6 +888,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);
@@ -735,10 +919,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)
@@ -751,6 +943,7 @@ done:
   if (locator_set_name)
     vec_free (locator_set_name);
   gid_address_free (&a->eid);
+  vec_free (a->key);
   return error;
 }
 
@@ -758,7 +951,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* */
@@ -1541,7 +1734,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)
@@ -2169,6 +2362,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)
 {
@@ -2242,7 +2453,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* */
@@ -2253,6 +2465,102 @@ VLIB_CLI_COMMAND (lisp_cp_enable_disable_command) = {
 };
 /* *INDENT-ON* */
 
+static clib_error_t *
+lisp_map_register_enable_disable_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_enabled = 0;
+  u8 is_set = 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, "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_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_rloc_probe_enable_disable_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_enabled = 0;
+  u8 is_set = 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, "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_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)
 {
@@ -2548,28 +2856,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)
     {
@@ -2832,7 +3124,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)
@@ -2909,6 +3201,39 @@ build_itr_rloc_list (lisp_cp_main_t * lcm, locator_set_t * loc_set)
 }
 
 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,
 				locator_set_t * loc_set, ip_address_t * mr_ip,
@@ -2940,13 +3265,14 @@ build_encapsulated_map_request (lisp_cp_main_t * lcm,
       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 */
@@ -2972,7 +3298,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)
   {
@@ -2986,6 +3312,287 @@ elect_map_resolver (lisp_cp_main_t * lcm)
   return 0;
 }
 
+static void
+free_map_register_records (mapping_t * maps)
+{
+  mapping_t *map;
+  vec_foreach (map, maps) vec_free (map->locators);
+
+  vec_free (maps);
+}
+
+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);
+
+  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)
+      {
+          /* *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* */
+
+  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)
+    {
+      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);
+
+  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);
+
+  update_map_register_auth_data (map_reg_hdr, key_id, key, auth_data_len,
+				 msg_len);
+
+  /* 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,
+						     ip)))
+    {
+      if (0 == elect_map_resolver (lcm))
+	/* all map resolvers are down */
+	{
+	  /* restart MR checking by marking all of them up */
+	  vec_foreach (mr, lcm->map_resolvers) mr->is_down = 0;
+	  return -1;
+	}
+    }
+  return 0;
+}
+
+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;
+
+  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);
+  return 0;
+}
+
+static int
+send_rloc_probes (lisp_cp_main_t * lcm)
+{
+  mapping_t *lm;
+  fwd_entry_t *e;
+  u32 si;
+
+  /* *INDENT-OFF* */
+  pool_foreach (e, lcm->fwd_entry_pool,
+  {
+    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);
+
+    if (vec_len (e->locator_pairs) == 0)
+      continue;
+
+    /* TODO send rloc-probe for each pair? for now assume there is only one
+      pair at a time */
+    locator_pair_t *lp = &e->locator_pairs[0];
+
+    /* 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;
+
+  // 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;
+    }
+
+  /* TODO build mapping records grouped by secret key and send one map-register
+     per group. Currently only one secret key per vpp is supported  */
+  mapping_t *records = build_map_register_record_list (lcm);
+  if (!records)
+    return -1;
+
+  u8 *key = records[0].key;
+  u8 key_id = records[0].key_id;
+
+  if (!key)
+    return 0;			/* no secret key -> map-register cannot be sent */
+
+  b = build_map_register (lcm, &sloc, ms, &nonce, want_map_notif, records,
+			  key_id, key, &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);
+
+  hash_set (lcm->map_register_messages_by_nonce, nonce, 0);
+  return 0;
+}
+
 #define send_encapsulated_map_request(lcm, seid, deid, smr) \
   _send_encapsulated_map_request(lcm, seid, deid, smr, 0)
 
@@ -2997,7 +3604,6 @@ _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;
@@ -3067,22 +3673,11 @@ _send_encapsulated_map_request (lisp_cp_main_t * lcm,
 
   loc_set = pool_elt_at_index (lcm->locator_set_pool, ls_index);
 
-  while (lcm->do_map_resolver_election
-	 | (0 == ip_fib_get_first_egress_ip_for_dst (lcm,
-						     &lcm->active_map_resolver,
-						     &sloc)))
+  if (get_egress_map_resolver_ip (lcm, &sloc) < 0)
     {
-      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;
-	}
+      if (duplicate_pmr)
+	duplicate_pmr->to_be_removed = 1;
+      return -1;
     }
 
   /* build the encapsulated map request */
@@ -3550,24 +4145,242 @@ done:
   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)
+    {
+      clib_warning ("invalid length for key_id %d!", key_id);
+      return 0;
+    }
+
+  /* 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 (vlib_main_t * vm, lisp_cp_main_t * lcm, vlib_buffer_t * b)
+{
+  lisp_key_type_t key_id;
+  gid_address_t deid;
+  mapping_t *m;
+  u32 i, mi, len;
+  map_notify_hdr_t *mnotif_hdr;
+  u64 nonce;
+  uword *pmr_index;
+  locator_t *locators = 0, *loc;
+  u16 auth_data_len = 0;
+
+  mnotif_hdr = vlib_buffer_get_current (b);
+  vlib_buffer_pull (b, sizeof (*mnotif_hdr));
+  memset (&deid, 0, sizeof (deid));
+
+  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);
+
+  pmr_index = hash_get (lcm->map_register_messages_by_nonce, nonce);
+  if (!pmr_index)
+    {
+      clib_warning ("No pending map-register entry with nonce %lu!", nonce);
+      return;
+    }
+
+  /* advance buffer by authentication data */
+  vlib_buffer_pull (b, auth_data_len);
+
+  /* parse record eid */
+  for (i = 0; i < MNOTIFY_REC_COUNT (mnotif_hdr); 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_free (locators);
+	  return;
+	}
+
+      vec_free (locators);
+    }
+
+  /* lookup eid in map-cache */
+  mi = gid_dictionary_lookup (&lcm->mapping_index_by_gid, &deid);
+  if (~0 == mi)
+    {
+      clib_warning ("eid %U not found in map-cache!", unformat_gid_address,
+		    &deid);
+      return;
+    }
+
+  m = pool_elt_at_index (lcm->mapping_pool, mi);
+  if (!m->key)
+    {
+      clib_warning ("There is no secret key assigned to %U!",
+		    format_gid_address, &deid);
+      goto done;
+    }
+
+  /* verify authentication data */
+  if (!is_auth_data_valid (mnotif_hdr, vlib_buffer_get_tail (b)
+			   - (u8 *) mnotif_hdr, m->key_id, m->key))
+    {
+      clib_warning ("Map-notify auth data verification failed for nonce %lu!",
+		    nonce);
+      goto done;
+    }
+
+done:
+  hash_unset (lcm->map_register_messages_by_nonce, nonce);
+  gid_address_free (&deid);
+}
+
+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;
+
+  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;
+}
+
 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;
 
   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;
     }
 
@@ -3583,12 +4396,6 @@ process_map_request (vlib_main_t * vm, lisp_cp_main_t * lcm,
   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++)
     {
@@ -3597,11 +4404,38 @@ 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_foreach (rloc, itr_rlocs)
+  {
+    gid_address_free (rloc);
+  }
 }
 
 static void
@@ -3659,6 +4493,9 @@ lisp_cp_input (vlib_main_t * vm, vlib_node_runtime_t * node,
 	    case LISP_MAP_REQUEST:
 	      process_map_request (vm, lcm, b0);
 	      break;
+	    case LISP_MAP_NOTIFY:
+	      process_map_notify (vm, lcm, b0);
+	      break;
 	    default:
 	      clib_warning ("Unsupported LISP message type %d", type);
 	      break;
@@ -3779,7 +4616,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 */
@@ -3854,6 +4691,45 @@ 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)
@@ -3880,8 +4756,10 @@ send_map_resolver_service (vlib_main_t * vm,
       /* *INDENT-ON* */
 
       remove_dead_pending_map_requests (lcm);
-
       lisp_pending_map_request_unlock (lcm);
+
+      update_map_register (lcm, period);
+      update_rloc_probing (lcm, period);
     }
 
   /* unreachable */
diff --git a/vnet/vnet/lisp-cp/control.h b/vnet/vnet/lisp-cp/control.h
index 7a56d4f981f..14b54606079 100644
--- a/vnet/vnet/lisp-cp/control.h
+++ b/vnet/vnet/lisp-cp/control.h
@@ -24,6 +24,19 @@
 #define PENDING_MREQ_EXPIRATION_TIME        3.0	/* seconds */
 #define PENDING_MREQ_QUEUE_LEN              5
 
+#define PENDING_MREG_EXPIRATION_TIME        3.0	/* seconds */
+#define RLOC_PROBING_INTERVAL               60.0
+
+/* when map-registration is enabled "quick registration" takes place first.
+   In this mode ETR sends map-register messages at an increased frequency
+   until specified message count is reached */
+#define QUICK_MAP_REGISTER_MSG_COUNT        3
+#define QUICK_MAP_REGISTER_INTERVAL         3.0
+
+/* normal map-register period */
+#define MAP_REGISTER_INTERVAL               60.0
+
+
 typedef struct
 {
   gid_address_t src;
@@ -55,12 +68,14 @@ typedef enum
   IP6_MISS_PACKET
 } miss_packet_type_t;
 
+/* map-server/map-resolver structure */
 typedef struct
 {
   u8 is_down;
   f64 last_update;
   ip_address_t address;
-} map_resolver_t;
+  char *key;
+} lisp_msmr_t;
 
 typedef struct
 {
@@ -88,6 +103,9 @@ typedef struct
   /* pool of mappings */
   mapping_t *mapping_pool;
 
+  /* hash map of secret keys by mapping index */
+  u8 *key_by_mapping_index;
+
   /* pool of locators */
   locator_t *locator_pool;
 
@@ -120,8 +138,14 @@ typedef struct
   pending_map_request_t *pending_map_requests_pool;
   volatile u32 *pending_map_request_lock;
 
+  /* hash map of sent map register messages */
+  uword *map_register_messages_by_nonce;
+
   /* vector of map-resolvers */
-  map_resolver_t *map_resolvers;
+  lisp_msmr_t *map_resolvers;
+
+  /* vector of map-servers */
+  lisp_msmr_t *map_servers;
 
   /* map resolver address currently being used for sending requests.
    * This has to be an actual address and not an index to map_resolvers vector
@@ -154,6 +178,12 @@ typedef struct
   /* map request mode */
   u8 map_request_mode;
 
+  /* enable/disable map registering */
+  u8 map_registering;
+
+  /* enable/disable rloc-probing */
+  u8 rloc_probing;
+
   /* commodity */
   ip4_main_t *im4;
   ip6_main_t *im6;
@@ -207,6 +237,8 @@ typedef struct
 
   u8 local;
   u8 is_static;
+  u8 *key;
+  u8 key_id;
 } vnet_lisp_add_del_mapping_args_t;
 
 int
@@ -238,6 +270,7 @@ typedef struct
 
 int
 vnet_lisp_add_del_map_resolver (vnet_lisp_add_del_map_resolver_args_t * a);
+int vnet_lisp_add_del_map_server (ip_address_t * addr, u8 is_add);
 
 clib_error_t *vnet_lisp_enable_disable (u8 is_enabled);
 u8 vnet_lisp_enable_disable_status (void);
@@ -256,9 +289,15 @@ vnet_lisp_add_del_mreq_itr_rlocs (vnet_lisp_add_del_mreq_itr_rloc_args_t * a);
 int vnet_lisp_clear_all_remote_adjacencies (void);
 
 int vnet_lisp_eid_table_map (u32 vni, u32 vrf, u8 is_l2, u8 is_add);
+int vnet_lisp_add_del_map_table_key (gid_address_t * eid, char *key,
+				     u8 is_add);
 int vnet_lisp_set_map_request_mode (u8 mode);
 u8 vnet_lisp_get_map_request_mode (void);
 lisp_adjacency_t *vnet_lisp_adjacencies_get_by_vni (u32 vni);
+int vnet_lisp_rloc_probe_enable_disable (u8 is_enable);
+int vnet_lisp_map_register_enable_disable (u8 is_enable);
+u8 vnet_lisp_map_register_state_get (void);
+u8 vnet_lisp_rloc_probe_state_get (void);
 
 static inline void
 lisp_pending_map_request_lock (lisp_cp_main_t * lcm)
diff --git a/vnet/vnet/lisp-cp/lisp_cp_messages.h b/vnet/vnet/lisp-cp/lisp_cp_messages.h
index d52f37b6011..278f60e1726 100644
--- a/vnet/vnet/lisp-cp/lisp_cp_messages.h
+++ b/vnet/vnet/lisp-cp/lisp_cp_messages.h
@@ -272,6 +272,7 @@ typedef struct
 void map_reply_hdr_init (void *ptr);
 char *map_reply_hdr_to_char (map_reply_hdr_t * h);
 
+#define MREP_TYPE(h_) MREP_HDR_CAST(h_)->type
 #define MREP_HDR_CAST(h_) ((map_reply_hdr_t *)(h_))
 #define MREP_REC_COUNT(h_) MREP_HDR_CAST(h_)->record_count
 #define MREP_RLOC_PROBE(h_) MREP_HDR_CAST(h_)->rloc_probe
@@ -472,6 +473,135 @@ typedef struct _lcaf_src_dst_hdr_t
 #define LCAF_SD_SRC_ML(_h) (_h)->src_mask_len
 #define LCAF_SD_DST_ML(_h) (_h)->dst_mask_len
 
+/*
+ * The Map-Register message format is:
+ *
+ *       0                   1                   2                   3
+ *       0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |Type=3 |P|            Reserved               |M| Record Count  |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |                         Nonce . . .                           |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |                         . . . Nonce                           |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |            Key ID             |  Authentication Data Length   |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      ~                     Authentication Data                       ~
+ *  +-> +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |   |                          Record TTL                           |
+ *  |   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  R   | Locator Count | EID mask-len  | ACT |A|      Reserved         |
+ *  e   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  c   | Rsvd  |  Map-Version Number   |        EID-Prefix-AFI         |
+ *  o   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  r   |                          EID-Prefix                           |
+ *  d   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |  /|    Priority   |    Weight     |  M Priority   |   M Weight    |
+ *  | L +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  | o |        Unused Flags     |L|p|R|           Loc-AFI             |
+ *  | c +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |  \|                             Locator                           |
+ *  +-> +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ */
+typedef struct
+{
+#if CLIB_ARCH_IS_LITTLE_ENDIAN
+  u8 res1:3;
+  u8 proxy_map_reply:1;
+  u8 type:4;
+#else
+  u8 type:4;
+  u8 proxy_map_reply:1;
+  u8 res1:3;
+#endif
+
+  u8 res2;
+
+#if CLIB_ARCH_IS_LITTLE_ENDIAN
+  u8 want_map_notify:1;
+  u8 res3:7;
+#else
+  u8 res3:7;
+  u8 want_map_notify:1;
+#endif
+
+  u8 record_count;
+  u64 nonce;
+  u16 key_id;
+  u16 auth_data_len;
+  u8 data[0];
+} __attribute__ ((__packed__)) map_register_hdr_t;
+
+#define MREG_TYPE(h_) (h_)->type
+#define MREG_HDR_CAST(h_) ((map_register_hdr_t *)(h_))
+#define MREG_PROXY_MR(h_) (MREG_HDR_CAST(h_))->proxy_map_reply
+#define MREG_WANT_MAP_NOTIFY(h_) (MREG_HDR_CAST(h_))->want_map_notify
+#define MREG_REC_COUNT(h_) (MREG_HDR_CAST(h_))->record_count
+#define MREG_NONCE(h_) (MREG_HDR_CAST(h_))->nonce
+#define MREG_KEY_ID(h_) (MREG_HDR_CAST(h_))->key_id
+#define MREG_AUTH_DATA_LEN(h_) (MREG_HDR_CAST(h_))->auth_data_len
+#define MREG_DATA(h_) (MREG_HDR_CAST(h_))->data
+
+/*
+ * The Map-Notify message format is:
+ *
+ *       0                   1                   2                   3
+ *       0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1 2 3 4 5 6 7 8 9 0 1
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |Type=4 |              Reserved                 | Record Count  |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |                         Nonce . . .                           |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |                         . . . Nonce                           |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      |            Key ID             |  Authentication Data Length   |
+ *      +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *      ~                     Authentication Data                       ~
+ *  +-> +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |   |                          Record TTL                           |
+ *  |   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  R   | Locator Count | EID mask-len  | ACT |A|      Reserved         |
+ *  e   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  c   | Rsvd  |  Map-Version Number   |         EID-Prefix-AFI        |
+ *  o   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  r   |                          EID-Prefix                           |
+ *  d   +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |  /|    Priority   |    Weight     |  M Priority   |   M Weight    |
+ *  | L +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  | o |        Unused Flags     |L|p|R|           Loc-AFI             |
+ *  | c +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+ *  |  \|                             Locator                           |
+ *  +-> +-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+
+*/
+
+typedef struct
+{
+#if CLIB_ARCH_IS_LITTLE_ENDIAN
+  u8 res1:4;
+  u8 type:4;
+#else
+  u8 type:4;
+  u8 res1:4;
+#endif
+
+  u16 res2;
+
+  u8 record_count;
+  u64 nonce;
+  u16 key_id;
+  u16 auth_data_len;
+  u8 data[0];
+} __attribute__ ((__packed__)) map_notify_hdr_t;
+
+#define MNOTIFY_TYPE(h_) (h_)->type
+#define MNOTIFY_HDR_CAST(h_) ((map_register_hdr_t *)(h_))
+#define MNOTIFY_REC_COUNT(h_) (MREG_HDR_CAST(h_))->record_count
+#define MNOTIFY_NONCE(h_) (MREG_HDR_CAST(h_))->nonce
+#define MNOTIFY_KEY_ID(h_) (MREG_HDR_CAST(h_))->key_id
+#define MNOTIFY_AUTH_DATA_LEN(h_) (MREG_HDR_CAST(h_))->auth_data_len
+#define MNOTIFY_DATA(h_) (MREG_HDR_CAST(h_))->data
+
 #endif /* VNET_LISP_GPE_LISP_CP_MESSAGES_H_ */
 
 /*
diff --git a/vnet/vnet/lisp-cp/lisp_msg_serdes.c b/vnet/vnet/lisp-cp/lisp_msg_serdes.c
index de9a4f5b121..57a04547967 100644
--- a/vnet/vnet/lisp-cp/lisp_msg_serdes.c
+++ b/vnet/vnet/lisp-cp/lisp_msg_serdes.c
@@ -17,11 +17,67 @@
 #include <vnet/lisp-cp/packets.h>
 #include <vppinfra/time.h>
 
+void *lisp_msg_put_gid (vlib_buffer_t * b, gid_address_t * gid);
+
+static void
+lisp_msg_put_locators (vlib_buffer_t * b, locator_t * locators)
+{
+  locator_t *loc;
+
+  vec_foreach (loc, locators)
+  {
+    u8 *p = vlib_buffer_put_uninit (b, sizeof (locator_hdr_t));
+    memset (p, 0, sizeof (locator_hdr_t));
+    LOC_PRIORITY (p) = loc->priority;
+    LOC_MPRIORITY (p) = loc->mpriority;
+    LOC_WEIGHT (p) = loc->weight;
+    LOC_MWEIGHT (p) = loc->mweight;
+    LOC_LOCAL (p) = loc->local;
+    LOC_PROBED (p) = loc->probed ? 1 : 0;
+    lisp_msg_put_gid (b, &loc->address);
+  }
+}
+
+static void
+lisp_msg_put_mapping_record (vlib_buffer_t * b, mapping_t * record)
+{
+  mapping_record_hdr_t *p =
+    vlib_buffer_put_uninit (b, sizeof (mapping_record_hdr_t));
+  gid_address_t *eid = &record->eid;
+
+  memset (p, 0, sizeof (*p));
+  MAP_REC_EID_PLEN (p) = gid_address_len (eid);
+  MAP_REC_TTL (p) = clib_host_to_net_u32 (record->ttl);
+  MAP_REC_AUTH (p) = record->authoritative ? 1 : 0;
+  MAP_REC_LOC_COUNT (p) = vec_len (record->locators);
+
+  lisp_msg_put_gid (b, eid);
+  lisp_msg_put_locators (b, record->locators);
+}
+
+static void
+lisp_msg_put_mreg_records (vlib_buffer_t * b, mapping_t * records)
+{
+  u32 i;
+  for (i = 0; i < vec_len (records); i++)
+    lisp_msg_put_mapping_record (b, &records[i]);
+}
+
 void *
 lisp_msg_put_gid (vlib_buffer_t * b, gid_address_t * gid)
 {
-  u8 *p = vlib_buffer_put_uninit (b, gid_address_size_to_put (gid));
-  gid_address_put (p, gid);
+  u8 *p = 0;
+  if (!gid)
+    {
+      /* insert only src-eid-afi field set to 0 */
+      p = vlib_buffer_put_uninit (b, sizeof (u16));
+      *(u16 *) p = 0;
+    }
+  else
+    {
+      p = vlib_buffer_put_uninit (b, gid_address_size_to_put (gid));
+      gid_address_put (p, gid);
+    }
   return p;
 }
 
@@ -78,9 +134,53 @@ nonce_build (u32 seed)
 }
 
 void *
+lisp_msg_put_map_reply (vlib_buffer_t * b, mapping_t * records, u64 nonce,
+			u8 probe_bit)
+{
+  map_reply_hdr_t *h = vlib_buffer_put_uninit (b, sizeof (h[0]));
+
+  memset (h, 0, sizeof (h[0]));
+  MREP_TYPE (h) = LISP_MAP_REPLY;
+  MREP_NONCE (h) = nonce;
+  MREP_REC_COUNT (h) = 1;
+  MREP_RLOC_PROBE (h) = probe_bit;
+
+  lisp_msg_put_mreg_records (b, records);
+  return h;
+}
+
+void *
+lisp_msg_put_map_register (vlib_buffer_t * b, mapping_t * records,
+			   u8 want_map_notify, u16 auth_data_len, u64 * nonce,
+			   u32 * msg_len)
+{
+  u8 *auth_data = 0;
+
+  /* Basic header init */
+  map_register_hdr_t *h = vlib_buffer_put_uninit (b, sizeof (h[0]));
+
+  memset (h, 0, sizeof (h[0]));
+  MREG_TYPE (h) = LISP_MAP_REGISTER;
+  MREG_NONCE (h) = nonce_build (0);
+  MREG_WANT_MAP_NOTIFY (h) = want_map_notify ? 1 : 0;
+  MREG_REC_COUNT (h) = vec_len (records);
+
+  auth_data = vlib_buffer_put_uninit (b, auth_data_len);
+  memset (auth_data, 0, auth_data_len);
+
+  /* Put map register records */
+  lisp_msg_put_mreg_records (b, records);
+
+  nonce[0] = MREG_NONCE (h);
+  msg_len[0] = vlib_buffer_get_tail (b) - (u8 *) h;
+  return h;
+}
+
+void *
 lisp_msg_put_mreq (lisp_cp_main_t * lcm, vlib_buffer_t * b,
 		   gid_address_t * seid, gid_address_t * deid,
-		   gid_address_t * rlocs, u8 is_smr_invoked, u64 * nonce)
+		   gid_address_t * rlocs, u8 is_smr_invoked,
+		   u8 rloc_probe_set, u64 * nonce)
 {
   u8 loc_count = 0;
 
@@ -91,6 +191,7 @@ lisp_msg_put_mreq (lisp_cp_main_t * lcm, vlib_buffer_t * b,
   MREQ_TYPE (h) = LISP_MAP_REQUEST;
   MREQ_NONCE (h) = nonce_build (0);
   MREQ_SMR_INVOKED (h) = is_smr_invoked ? 1 : 0;
+  MREQ_RLOC_PROBE (h) = rloc_probe_set ? 1 : 0;
 
   /* We're adding one eid record */
   increment_record_count (h);
diff --git a/vnet/vnet/lisp-cp/lisp_msg_serdes.h b/vnet/vnet/lisp-cp/lisp_msg_serdes.h
index 5f9ca7183e8..d794eff6340 100644
--- a/vnet/vnet/lisp-cp/lisp_msg_serdes.h
+++ b/vnet/vnet/lisp-cp/lisp_msg_serdes.h
@@ -23,11 +23,18 @@
 void *lisp_msg_put_mreq (lisp_cp_main_t * lcm, vlib_buffer_t * b,
 			 gid_address_t * seid, gid_address_t * deid,
 			 gid_address_t * rlocs, u8 is_smr_invoked,
-			 u64 * nonce);
+			 u8 rloc_probe_set, u64 * nonce);
+
+void *lisp_msg_put_map_register (vlib_buffer_t * b, mapping_t * records,
+				 u8 want_map_notify, u16 auth_data_len,
+				 u64 * nonce, u32 * msg_len);
 
 void *lisp_msg_push_ecm (vlib_main_t * vm, vlib_buffer_t * b, int lp, int rp,
 			 gid_address_t * la, gid_address_t * ra);
 
+void *lisp_msg_put_map_reply (vlib_buffer_t * b, mapping_t * record,
+			      u64 nonce, u8 probe_bit);
+
 u32
 lisp_msg_parse_mapping_record (vlib_buffer_t * b, gid_address_t * eid,
 			       locator_t ** locs, locator_t * probed_);
diff --git a/vnet/vnet/lisp-cp/lisp_types.c b/vnet/vnet/lisp-cp/lisp_types.c
index 04b8462e4fc..d206e6bccef 100644
--- a/vnet/vnet/lisp-cp/lisp_types.c
+++ b/vnet/vnet/lisp-cp/lisp_types.c
@@ -270,6 +270,31 @@ unformat_fid_address (unformat_input_t * i, va_list * args)
 }
 
 uword
+unformat_hmac_key_id (unformat_input_t * input, va_list * args)
+{
+  u32 *key_id = va_arg (*args, u32 *);
+  u8 *s = 0;
+
+  if (unformat (input, "%s", &s))
+    {
+      if (!strcmp ((char *) s, "sha1"))
+	key_id[0] = HMAC_SHA_1_96;
+      else if (!strcmp ((char *) s, "sha256"))
+	key_id[0] = HMAC_SHA_256_128;
+      else
+	{
+	  clib_warning ("invalid key_id: '%s'", s);
+	  key_id[0] = HMAC_NO_KEY;
+	}
+    }
+  else
+    return 0;
+
+  vec_free (s);
+  return 1;
+}
+
+uword
 unformat_gid_address (unformat_input_t * input, va_list * args)
 {
   gid_address_t *a = va_arg (*args, gid_address_t *);
@@ -334,6 +359,24 @@ unformat_negative_mapping_action (unformat_input_t * input, va_list * args)
 }
 
 u8 *
+format_hmac_key_id (u8 * s, va_list * args)
+{
+  lisp_key_type_t key_id = va_arg (*args, lisp_key_type_t);
+
+  switch (key_id)
+    {
+    case HMAC_SHA_1_96:
+      return format (0, "sha1");
+    case HMAC_SHA_256_128:
+      return format (0, "sha256");
+    default:
+      return 0;
+    }
+
+  return 0;
+}
+
+u8 *
 format_negative_mapping_action (u8 * s, va_list * args)
 {
   lisp_action_e action = va_arg (*args, lisp_action_e);
@@ -678,6 +721,14 @@ gid_address_free (gid_address_t * a)
   (*lcaf_free_fcts[lcaf_type]) (lcaf);
 }
 
+void
+gid_address_from_ip (gid_address_t * g, ip_address_t * ip)
+{
+  memset (g, 0, sizeof (g[0]));
+  ip_address_set (&gid_address_ip (g), ip, ip_addr_version (ip));
+  gid_address_ippref_len (g) = 32;
+}
+
 int
 ip_address_cmp (const ip_address_t * ip1, const ip_address_t * ip2)
 {
diff --git a/vnet/vnet/lisp-cp/lisp_types.h b/vnet/vnet/lisp-cp/lisp_types.h
index dd7a53ee548..7df0e653f52 100644
--- a/vnet/vnet/lisp-cp/lisp_types.h
+++ b/vnet/vnet/lisp-cp/lisp_types.h
@@ -19,6 +19,19 @@
 #include <vnet/ip/ip.h>
 #include <vnet/lisp-cp/lisp_cp_messages.h>
 
+#define SHA1_AUTH_DATA_LEN                  20
+#define SHA256_AUTH_DATA_LEN                32
+
+typedef enum
+{
+  HMAC_NO_KEY = 0,
+  HMAC_SHA_1_96,
+  HMAC_SHA_256_128
+} lisp_key_type_t;
+
+uword unformat_hmac_key_id (unformat_input_t * input, va_list * args);
+u8 *format_hmac_key_id (u8 * s, va_list * args);
+
 typedef enum
 {
   IP4,
@@ -270,6 +283,7 @@ typedef struct
   u8 weight;
   u8 mpriority;
   u8 mweight;
+  u8 probed;
 } locator_t;
 
 u32 locator_parse (void *ptr, locator_t * loc);
@@ -292,7 +306,11 @@ typedef struct
   gid_address_t eid;
 
   /* index of local locator set */
-  u32 locator_set_index;
+  union
+  {
+    u32 locator_set_index;
+    locator_t *locators;	/* used for map register message */
+  };
 
   u32 ttl;
   u8 action;
@@ -301,6 +319,8 @@ typedef struct
   u8 local;
   /* valid only for remote mappings */
   u8 is_static;
+  u8 *key;
+  lisp_key_type_t key_id;
 } mapping_t;
 
 uword
@@ -320,6 +340,8 @@ typedef struct locator_pair
 void
 build_src_dst (gid_address_t * sd, gid_address_t * src, gid_address_t * dst);
 
+void gid_address_from_ip (gid_address_t * g, ip_address_t * ip);
+
 #endif /* VNET_LISP_GPE_LISP_TYPES_H_ */
 
 /*
diff --git a/vpp-api-test/vat/api_format.c b/vpp-api-test/vat/api_format.c
index 63928063137..003fd723714 100644
--- a/vpp-api-test/vat/api_format.c
+++ b/vpp-api-test/vat/api_format.c
@@ -2337,11 +2337,13 @@ vl_api_lisp_eid_table_details_t_handler (vl_api_lisp_eid_table_details_t * mp)
 		mp->seid, mp->seid_prefix_len, mp->is_src_dst);
   vec_add1 (eid, 0);
 
-  fformat (vam->ofp, "[%d] %-35s%-20s%-30s%-20d%-d\n",
+  fformat (vam->ofp, "[%d] %-35s%-20s%-30s%-20d%-20d%-10d%-20s\n",
 	   clib_net_to_host_u32 (mp->vni),
 	   eid,
 	   mp->is_local ? "local" : "remote",
-	   s, clib_net_to_host_u32 (mp->ttl), mp->authoritative);
+	   s, clib_net_to_host_u32 (mp->ttl), mp->authoritative,
+	   clib_net_to_host_u16 (mp->key_id), mp->key);
+
   vec_free (s);
   vec_free (eid);
 }
@@ -2379,6 +2381,13 @@ vl_api_lisp_eid_table_details_t_handler_json (vl_api_lisp_eid_table_details_t
   vat_json_object_add_uint (node, "vni", clib_net_to_host_u32 (mp->vni));
   vat_json_object_add_uint (node, "ttl", clib_net_to_host_u32 (mp->ttl));
   vat_json_object_add_uint (node, "authoritative", (mp->authoritative));
+
+  if (mp->key_id)
+    {
+      vat_json_object_add_uint (node, "key_id",
+				clib_net_to_host_u16 (mp->key_id));
+      vat_json_object_add_string_copy (node, "key", mp->key);
+    }
   vec_free (eid);
 }
 
@@ -2540,6 +2549,76 @@ static void
 }
 
 static void
+  vl_api_show_lisp_map_register_state_reply_t_handler
+  (vl_api_show_lisp_map_register_state_reply_t * mp)
+{
+  vat_main_t *vam = &vat_main;
+  int retval = clib_net_to_host_u32 (mp->retval);
+
+  fformat (vam->ofp, "%s\n", mp->is_enabled ? "enabled" : "disabled");
+
+  vam->retval = retval;
+  vam->result_ready = 1;
+}
+
+static void
+  vl_api_show_lisp_map_register_state_reply_t_handler_json
+  (vl_api_show_lisp_map_register_state_reply_t * mp)
+{
+  vat_main_t *vam = &vat_main;
+  vat_json_node_t _node, *node = &_node;
+  int retval = clib_net_to_host_u32 (mp->retval);
+
+  u8 *s = format (0, "%s", mp->is_enabled ? "enabled" : "disabled");
+
+  vat_json_init_object (node);
+  vat_json_object_add_string_copy (node, "state", s);
+
+  vat_json_print (vam->ofp, node);
+  vat_json_free (node);
+
+  vam->retval = retval;
+  vam->result_ready = 1;
+  vec_free (s);
+}
+
+static void
+  vl_api_show_lisp_rloc_probe_state_reply_t_handler
+  (vl_api_show_lisp_rloc_probe_state_reply_t * mp)
+{
+  vat_main_t *vam = &vat_main;
+  int retval = clib_net_to_host_u32 (mp->retval);
+
+  if (retval)
+    goto end;
+
+  fformat (vam->ofp, "%s\n", mp->is_enabled ? "enabled" : "disabled");
+end:
+  vam->retval = retval;
+  vam->result_ready = 1;
+}
+
+static void
+  vl_api_show_lisp_rloc_probe_state_reply_t_handler_json
+  (vl_api_show_lisp_rloc_probe_state_reply_t * mp)
+{
+  vat_main_t *vam = &vat_main;
+  vat_json_node_t _node, *node = &_node;
+  int retval = clib_net_to_host_u32 (mp->retval);
+
+  u8 *s = format (0, "%s", mp->is_enabled ? "enabled" : "disabled");
+  vat_json_init_object (node);
+  vat_json_object_add_string_copy (node, "state", s);
+
+  vat_json_print (vam->ofp, node);
+  vat_json_free (node);
+
+  vam->retval = retval;
+  vam->result_ready = 1;
+  vec_free (s);
+}
+
+static void
   vl_api_lisp_adjacencies_get_reply_t_handler
   (vl_api_lisp_adjacencies_get_reply_t * mp)
 {
@@ -2612,6 +2691,46 @@ end:
 }
 
 static void
+vl_api_lisp_map_server_details_t_handler (vl_api_lisp_map_server_details_t
+					  * mp)
+{
+  vat_main_t *vam = &vat_main;
+
+  fformat (vam->ofp, "%=20U\n",
+	   mp->is_ipv6 ? format_ip6_address : format_ip4_address,
+	   mp->ip_address);
+}
+
+static void
+  vl_api_lisp_map_server_details_t_handler_json
+  (vl_api_lisp_map_server_details_t * mp)
+{
+  vat_main_t *vam = &vat_main;
+  vat_json_node_t *node = NULL;
+  struct in6_addr ip6;
+  struct in_addr ip4;
+
+  if (VAT_JSON_ARRAY != vam->json_tree.type)
+    {
+      ASSERT (VAT_JSON_NONE == vam->json_tree.type);
+      vat_json_init_array (&vam->json_tree);
+    }
+  node = vat_json_array_add (&vam->json_tree);
+
+  vat_json_init_object (node);
+  if (mp->is_ipv6)
+    {
+      clib_memcpy (&ip6, mp->ip_address, sizeof (ip6));
+      vat_json_object_add_ip6 (node, "map-server", ip6);
+    }
+  else
+    {
+      clib_memcpy (&ip4, mp->ip_address, sizeof (ip4));
+      vat_json_object_add_ip4 (node, "map-server", ip4);
+    }
+}
+
+static void
 vl_api_lisp_map_resolver_details_t_handler (vl_api_lisp_map_resolver_details_t
 					    * mp)
 {
@@ -3521,9 +3640,12 @@ _(lisp_add_del_remote_mapping_reply)                    \
 _(lisp_add_del_adjacency_reply)                         \
 _(lisp_gpe_add_del_fwd_entry_reply)                     \
 _(lisp_add_del_map_resolver_reply)                      \
+_(lisp_add_del_map_server_reply)                        \
 _(lisp_gpe_enable_disable_reply)                        \
 _(lisp_gpe_add_del_iface_reply)                         \
 _(lisp_enable_disable_reply)                            \
+_(lisp_rloc_probe_enable_disable_reply)                 \
+_(lisp_map_register_enable_disable_reply)               \
 _(lisp_pitr_set_locator_set_reply)                      \
 _(lisp_map_request_mode_reply)                          \
 _(lisp_add_del_map_request_itr_rlocs_reply)             \
@@ -3726,8 +3848,13 @@ _(LISP_ADD_DEL_REMOTE_MAPPING_REPLY, lisp_add_del_remote_mapping_reply) \
 _(LISP_ADD_DEL_ADJACENCY_REPLY, lisp_add_del_adjacency_reply)           \
 _(LISP_GPE_ADD_DEL_FWD_ENTRY_REPLY, lisp_gpe_add_del_fwd_entry_reply)   \
 _(LISP_ADD_DEL_MAP_RESOLVER_REPLY, lisp_add_del_map_resolver_reply)     \
+_(LISP_ADD_DEL_MAP_SERVER_REPLY, lisp_add_del_map_server_reply)         \
 _(LISP_GPE_ENABLE_DISABLE_REPLY, lisp_gpe_enable_disable_reply)         \
 _(LISP_ENABLE_DISABLE_REPLY, lisp_enable_disable_reply)                 \
+_(LISP_MAP_REGISTER_ENABLE_DISABLE_REPLY,                               \
+  lisp_map_register_enable_disable_reply)                               \
+_(LISP_RLOC_PROBE_ENABLE_DISABLE_REPLY,                                 \
+  lisp_rloc_probe_enable_disable_reply)                                 \
 _(LISP_PITR_SET_LOCATOR_SET_REPLY, lisp_pitr_set_locator_set_reply)     \
 _(LISP_MAP_REQUEST_MODE_REPLY, lisp_map_request_mode_reply)             \
 _(LISP_EID_TABLE_ADD_DEL_MAP_REPLY, lisp_eid_table_add_del_map_reply)   \
@@ -3739,6 +3866,7 @@ _(LISP_EID_TABLE_MAP_DETAILS, lisp_eid_table_map_details)               \
 _(LISP_EID_TABLE_VNI_DETAILS, lisp_eid_table_vni_details)               \
 _(LISP_GPE_TUNNEL_DETAILS, lisp_gpe_tunnel_details)                     \
 _(LISP_MAP_RESOLVER_DETAILS, lisp_map_resolver_details)                 \
+_(LISP_MAP_SERVER_DETAILS, lisp_map_server_details)                     \
 _(LISP_ADJACENCIES_GET_REPLY, lisp_adjacencies_get_reply)               \
 _(SHOW_LISP_STATUS_REPLY, show_lisp_status_reply)                       \
 _(LISP_ADD_DEL_MAP_REQUEST_ITR_RLOCS_REPLY,                             \
@@ -3747,6 +3875,9 @@ _(LISP_GET_MAP_REQUEST_ITR_RLOCS_REPLY,                                 \
   lisp_get_map_request_itr_rlocs_reply)                                 \
 _(SHOW_LISP_PITR_REPLY, show_lisp_pitr_reply)                           \
 _(SHOW_LISP_MAP_REQUEST_MODE_REPLY, show_lisp_map_request_mode_reply)   \
+_(SHOW_LISP_RLOC_PROBE_STATE_REPLY, show_lisp_rloc_probe_state_reply)   \
+_(SHOW_LISP_MAP_REGISTER_STATE_REPLY,                                   \
+  show_lisp_map_register_state_reply)                                   \
 _(AF_PACKET_CREATE_REPLY, af_packet_create_reply)                       \
 _(AF_PACKET_DELETE_REPLY, af_packet_delete_reply)                       \
 _(POLICER_ADD_DEL_REPLY, policer_add_del_reply)                         \
@@ -12849,6 +12980,31 @@ api_lisp_add_del_locator (vat_main_t * vam)
   return 0;
 }
 
+uword
+unformat_hmac_key_id (unformat_input_t * input, va_list * args)
+{
+  u32 *key_id = va_arg (*args, u32 *);
+  u8 *s = 0;
+
+  if (unformat (input, "%s", &s))
+    {
+      if (!strcmp ((char *) s, "sha1"))
+	key_id[0] = HMAC_SHA_1_96;
+      else if (!strcmp ((char *) s, "sha256"))
+	key_id[0] = HMAC_SHA_256_128;
+      else
+	{
+	  clib_warning ("invalid key_id: '%s'", s);
+	  key_id[0] = HMAC_NO_KEY;
+	}
+    }
+  else
+    return 0;
+
+  vec_free (s);
+  return 1;
+}
+
 static int
 api_lisp_add_del_local_eid (vat_main_t * vam)
 {
@@ -12861,6 +13017,8 @@ api_lisp_add_del_local_eid (vat_main_t * vam)
   u8 *locator_set_name = 0;
   u8 locator_set_name_set = 0;
   u32 vni = 0;
+  u16 key_id = 0;
+  u8 *key = 0;
 
   /* Parse args required to build the message */
   while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
@@ -12881,6 +13039,10 @@ api_lisp_add_del_local_eid (vat_main_t * vam)
 	{
 	  locator_set_name_set = 1;
 	}
+      else if (unformat (input, "key-id %U", unformat_hmac_key_id, &key_id))
+	;
+      else if (unformat (input, "secret-key %_%v%_", &key))
+	;
       else
 	break;
     }
@@ -12898,6 +13060,19 @@ api_lisp_add_del_local_eid (vat_main_t * vam)
       return -99;
     }
 
+  if (key && (0 == key_id))
+    {
+      errmsg ("invalid key_id!");
+      return -99;
+    }
+
+  if (vec_len (key) > 64)
+    {
+      errmsg ("key too long");
+      vec_free (key);
+      return -99;
+    }
+
   if (vec_len (locator_set_name) > 64)
     {
       errmsg ("locator-set name too long\n");
@@ -12914,10 +13089,13 @@ api_lisp_add_del_local_eid (vat_main_t * vam)
   mp->eid_type = eid->type;
   mp->prefix_len = eid->len;
   mp->vni = clib_host_to_net_u32 (vni);
+  mp->key_id = clib_host_to_net_u16 (key_id);
   clib_memcpy (mp->locator_set_name, locator_set_name,
 	       vec_len (locator_set_name));
+  clib_memcpy (mp->key, key, vec_len (key));
 
   vec_free (locator_set_name);
+  vec_free (key);
 
   /* send it... */
   S;
@@ -13071,6 +13249,74 @@ api_lisp_gpe_add_del_fwd_entry (vat_main_t * vam)
 }
 
 static int
+api_lisp_add_del_map_server (vat_main_t * vam)
+{
+  unformat_input_t *input = vam->input;
+  vl_api_lisp_add_del_map_server_t *mp;
+  f64 timeout = ~0;
+  u8 is_add = 1;
+  u8 ipv4_set = 0;
+  u8 ipv6_set = 0;
+  ip4_address_t ipv4;
+  ip6_address_t ipv6;
+
+  /* Parse args required to build the message */
+  while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
+    {
+      if (unformat (input, "del"))
+	{
+	  is_add = 0;
+	}
+      else if (unformat (input, "%U", unformat_ip4_address, &ipv4))
+	{
+	  ipv4_set = 1;
+	}
+      else if (unformat (input, "%U", unformat_ip6_address, &ipv6))
+	{
+	  ipv6_set = 1;
+	}
+      else
+	break;
+    }
+
+  if (ipv4_set && ipv6_set)
+    {
+      errmsg ("both eid v4 and v6 addresses set\n");
+      return -99;
+    }
+
+  if (!ipv4_set && !ipv6_set)
+    {
+      errmsg ("eid addresses not set\n");
+      return -99;
+    }
+
+  /* Construct the API message */
+  M (LISP_ADD_DEL_MAP_SERVER, lisp_add_del_map_server);
+
+  mp->is_add = is_add;
+  if (ipv6_set)
+    {
+      mp->is_ipv6 = 1;
+      clib_memcpy (mp->ip_address, &ipv6, sizeof (ipv6));
+    }
+  else
+    {
+      mp->is_ipv6 = 0;
+      clib_memcpy (mp->ip_address, &ipv4, sizeof (ipv4));
+    }
+
+  /* send it... */
+  S;
+
+  /* Wait for a reply... */
+  W;
+
+  /* NOTREACHED */
+  return 0;
+}
+
+static int
 api_lisp_add_del_map_resolver (vat_main_t * vam)
 {
   unformat_input_t *input = vam->input;
@@ -13186,6 +13432,94 @@ api_lisp_gpe_enable_disable (vat_main_t * vam)
 }
 
 static int
+api_lisp_rloc_probe_enable_disable (vat_main_t * vam)
+{
+  unformat_input_t *input = vam->input;
+  vl_api_lisp_rloc_probe_enable_disable_t *mp;
+  f64 timeout = ~0;
+  u8 is_set = 0;
+  u8 is_en = 0;
+
+  /* Parse args required to build the message */
+  while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
+    {
+      if (unformat (input, "enable"))
+	{
+	  is_set = 1;
+	  is_en = 1;
+	}
+      else if (unformat (input, "disable"))
+	is_set = 1;
+      else
+	break;
+    }
+
+  if (!is_set)
+    {
+      errmsg ("Value not set\n");
+      return -99;
+    }
+
+  /* Construct the API message */
+  M (LISP_RLOC_PROBE_ENABLE_DISABLE, lisp_rloc_probe_enable_disable);
+
+  mp->is_enabled = is_en;
+
+  /* send it... */
+  S;
+
+  /* Wait for a reply... */
+  W;
+
+  /* NOTREACHED */
+  return 0;
+}
+
+static int
+api_lisp_map_register_enable_disable (vat_main_t * vam)
+{
+  unformat_input_t *input = vam->input;
+  vl_api_lisp_map_register_enable_disable_t *mp;
+  f64 timeout = ~0;
+  u8 is_set = 0;
+  u8 is_en = 0;
+
+  /* Parse args required to build the message */
+  while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
+    {
+      if (unformat (input, "enable"))
+	{
+	  is_set = 1;
+	  is_en = 1;
+	}
+      else if (unformat (input, "disable"))
+	is_set = 1;
+      else
+	break;
+    }
+
+  if (!is_set)
+    {
+      errmsg ("Value not set\n");
+      return -99;
+    }
+
+  /* Construct the API message */
+  M (LISP_MAP_REGISTER_ENABLE_DISABLE, lisp_map_register_enable_disable);
+
+  mp->is_enabled = is_en;
+
+  /* send it... */
+  S;
+
+  /* Wait for a reply... */
+  W;
+
+  /* NOTREACHED */
+  return 0;
+}
+
+static int
 api_lisp_enable_disable (vat_main_t * vam)
 {
   unformat_input_t *input = vam->input;
@@ -13232,6 +13566,40 @@ api_lisp_enable_disable (vat_main_t * vam)
 }
 
 static int
+api_show_lisp_map_register_state (vat_main_t * vam)
+{
+  f64 timeout = ~0;
+  vl_api_show_lisp_map_register_state_t *mp;
+
+  M (SHOW_LISP_MAP_REGISTER_STATE, show_lisp_map_register_state);
+
+  /* send */
+  S;
+
+  /* wait for reply */
+  W;
+
+  return 0;
+}
+
+static int
+api_show_lisp_rloc_probe_state (vat_main_t * vam)
+{
+  f64 timeout = ~0;
+  vl_api_show_lisp_rloc_probe_state_t *mp;
+
+  M (SHOW_LISP_RLOC_PROBE_STATE, show_lisp_rloc_probe_state);
+
+  /* send */
+  S;
+
+  /* wait for reply */
+  W;
+
+  return 0;
+}
+
+static int
 api_show_lisp_map_request_mode (vat_main_t * vam)
 {
   f64 timeout = ~0;
@@ -14107,8 +14475,8 @@ api_lisp_eid_table_dump (vat_main_t * vam)
 
   if (!vam->json_output)
     {
-      fformat (vam->ofp, "%-35s%-20s%-30s%-20s%-s\n", "EID", "type",
-	       "ls_index", "ttl", "authoritative");
+      fformat (vam->ofp, "%-35s%-20s%-30s%-20s%-20s%-10s%-20s\n", "EID",
+	       "type", "ls_index", "ttl", "authoritative", "key_id", "key");
     }
 
   M (LISP_EID_TABLE_DUMP, lisp_eid_table_dump);
@@ -14234,6 +14602,34 @@ api_lisp_adjacencies_get (vat_main_t * vam)
 }
 
 static int
+api_lisp_map_server_dump (vat_main_t * vam)
+{
+  vl_api_lisp_map_server_dump_t *mp;
+  f64 timeout = ~0;
+
+  if (!vam->json_output)
+    {
+      fformat (vam->ofp, "%=20s\n", "Map server");
+    }
+
+  M (LISP_MAP_SERVER_DUMP, lisp_map_server_dump);
+  /* send it... */
+  S;
+
+  /* Use a control ping for synchronization */
+  {
+    vl_api_control_ping_t *mp;
+    M (CONTROL_PING, control_ping);
+    S;
+  }
+  /* Wait for a reply... */
+  W;
+
+  /* NOTREACHED */
+  return 0;
+}
+
+static int
 api_lisp_map_resolver_dump (vat_main_t * vam)
 {
   vl_api_lisp_map_resolver_dump_t *mp;
@@ -16991,12 +17387,16 @@ _(lisp_add_del_locator, "locator-set <locator_name> "                   \
                         "p <priority> w <weight> [del]")                \
 _(lisp_add_del_local_eid,"vni <vni> eid "                               \
                          "<ipv4|ipv6>/<prefix> | <L2 address> "         \
-                          "locator-set <locator_name> [del]")           \
+                         "locator-set <locator_name> [del]"             \
+                         "[key-id sha1|sha256 secret-key <secret-key>]")\
 _(lisp_gpe_add_del_fwd_entry, "rmt_eid <eid> [lcl_eid <eid>] vni <vni>" \
   "dp_table <table> loc-pair <lcl_loc> <rmt_loc> ... [del]")            \
 _(lisp_add_del_map_resolver, "<ip4|6-addr> [del]")                      \
+_(lisp_add_del_map_server, "<ip4|6-addr> [del]")                        \
 _(lisp_gpe_enable_disable, "enable|disable")                            \
 _(lisp_enable_disable, "enable|disable")                                \
+_(lisp_map_register_enable_disable, "enable|disable")                   \
+_(lisp_rloc_probe_enable_disable, "enable|disable")                     \
 _(lisp_gpe_add_del_iface, "up|down")                                    \
 _(lisp_add_del_remote_mapping, "add|del vni <vni> eid <dest-eid> "      \
                                "[seid <seid>] "                         \
@@ -17017,7 +17417,10 @@ _(lisp_eid_table_vni_dump, "")                                          \
 _(lisp_eid_table_map_dump, "l2|l3")                                     \
 _(lisp_gpe_tunnel_dump, "")                                             \
 _(lisp_map_resolver_dump, "")                                           \
+_(lisp_map_server_dump, "")                                             \
 _(lisp_adjacencies_get, "vni <vni>")                                    \
+_(show_lisp_rloc_probe_state, "")                                       \
+_(show_lisp_map_register_state, "")                                     \
 _(show_lisp_status, "")                                                 \
 _(lisp_get_map_request_itr_rlocs, "")                                   \
 _(show_lisp_pitr, "")                                                   \
diff --git a/vpp/vpp-api/api.c b/vpp/vpp-api/api.c
index d663bd87499..004dcb70053 100644
--- a/vpp/vpp-api/api.c
+++ b/vpp/vpp-api/api.c
@@ -254,8 +254,11 @@ _(LISP_ADD_DEL_LOCATOR, lisp_add_del_locator)                           \
 _(LISP_ADD_DEL_LOCAL_EID, lisp_add_del_local_eid)                       \
 _(LISP_GPE_ADD_DEL_FWD_ENTRY, lisp_gpe_add_del_fwd_entry)               \
 _(LISP_ADD_DEL_MAP_RESOLVER, lisp_add_del_map_resolver)                 \
+_(LISP_ADD_DEL_MAP_SERVER, lisp_add_del_map_server)                     \
 _(LISP_GPE_ENABLE_DISABLE, lisp_gpe_enable_disable)                     \
 _(LISP_ENABLE_DISABLE, lisp_enable_disable)                             \
+_(LISP_RLOC_PROBE_ENABLE_DISABLE, lisp_rloc_probe_enable_disable)       \
+_(LISP_MAP_REGISTER_ENABLE_DISABLE, lisp_map_register_enable_disable)   \
 _(LISP_GPE_ADD_DEL_IFACE, lisp_gpe_add_del_iface)                       \
 _(LISP_ADD_DEL_REMOTE_MAPPING, lisp_add_del_remote_mapping)             \
 _(LISP_ADD_DEL_ADJACENCY, lisp_add_del_adjacency)                       \
@@ -267,9 +270,12 @@ _(LISP_LOCATOR_DUMP, lisp_locator_dump)                                 \
 _(LISP_EID_TABLE_DUMP, lisp_eid_table_dump)                             \
 _(LISP_GPE_TUNNEL_DUMP, lisp_gpe_tunnel_dump)                           \
 _(LISP_MAP_RESOLVER_DUMP, lisp_map_resolver_dump)                       \
+_(LISP_MAP_SERVER_DUMP, lisp_map_server_dump)                           \
 _(LISP_EID_TABLE_MAP_DUMP, lisp_eid_table_map_dump)                     \
 _(LISP_EID_TABLE_VNI_DUMP, lisp_eid_table_vni_dump)                     \
 _(LISP_ADJACENCIES_GET, lisp_adjacencies_get)                           \
+_(SHOW_LISP_RLOC_PROBE_STATE, show_lisp_rloc_probe_state)               \
+_(SHOW_LISP_MAP_REGISTER_STATE, show_lisp_map_register_state)           \
 _(SHOW_LISP_STATUS, show_lisp_status)                                   \
 _(LISP_ADD_DEL_MAP_REQUEST_ITR_RLOCS,                                   \
   lisp_add_del_map_request_itr_rlocs)                                   \
@@ -4473,7 +4479,7 @@ vl_api_lisp_add_del_local_eid_t_handler (vl_api_lisp_add_del_local_eid_t * mp)
   uword *p = NULL;
   u32 locator_set_index = ~0, map_index = ~0;
   vnet_lisp_add_del_mapping_args_t _a, *a = &_a;
-  u8 *name = NULL;
+  u8 *name = NULL, *key = NULL;
   memset (a, 0, sizeof (a[0]));
   memset (eid, 0, sizeof (eid[0]));
 
@@ -4491,15 +4497,22 @@ vl_api_lisp_add_del_local_eid_t_handler (vl_api_lisp_add_del_local_eid_t * mp)
     }
   locator_set_index = p[0];
 
+  if (*mp->key)
+    key = format (0, "%s", mp->key);
+
   /* XXX treat batch configuration */
   a->is_add = mp->is_add;
   gid_address_copy (&a->eid, eid);
   a->locator_set_index = locator_set_index;
   a->local = 1;
+  a->key = key;
+  a->key_id = clib_net_to_host_u16 (mp->key_id);
+
   rv = vnet_lisp_add_del_local_mapping (a, &map_index);
 
 out:
   vec_free (name);
+  vec_free (key);
   gid_address_free (&a->eid);
 
   REPLY_MACRO (VL_API_LISP_ADD_DEL_LOCAL_EID_REPLY);
@@ -4609,6 +4622,22 @@ send_reply:
 }
 
 static void
+vl_api_lisp_add_del_map_server_t_handler (vl_api_lisp_add_del_map_server_t
+					  * mp)
+{
+  vl_api_lisp_add_del_map_server_reply_t *rmp;
+  int rv = 0;
+  ip_address_t addr;
+
+  memset (&addr, 0, sizeof (addr));
+
+  ip_address_set (&addr, mp->ip_address, mp->is_ipv6 ? IP6 : IP4);
+  rv = vnet_lisp_add_del_map_server (&addr, mp->is_add);
+
+  REPLY_MACRO (VL_API_LISP_ADD_DEL_MAP_SERVER_REPLY);
+}
+
+static void
 vl_api_lisp_add_del_map_resolver_t_handler (vl_api_lisp_add_del_map_resolver_t
 					    * mp)
 {
@@ -4641,6 +4670,28 @@ vl_api_lisp_gpe_enable_disable_t_handler (vl_api_lisp_gpe_enable_disable_t *
 }
 
 static void
+  vl_api_lisp_map_register_enable_disable_t_handler
+  (vl_api_lisp_map_register_enable_disable_t * mp)
+{
+  vl_api_lisp_map_register_enable_disable_reply_t *rmp;
+  int rv = 0;
+
+  vnet_lisp_map_register_enable_disable (mp->is_enabled);
+  REPLY_MACRO (VL_API_LISP_ENABLE_DISABLE_REPLY);
+}
+
+static void
+  vl_api_lisp_rloc_probe_enable_disable_t_handler
+  (vl_api_lisp_rloc_probe_enable_disable_t * mp)
+{
+  vl_api_lisp_rloc_probe_enable_disable_reply_t *rmp;
+  int rv = 0;
+
+  vnet_lisp_rloc_probe_enable_disable (mp->is_enabled);
+  REPLY_MACRO (VL_API_LISP_ENABLE_DISABLE_REPLY);
+}
+
+static void
 vl_api_lisp_enable_disable_t_handler (vl_api_lisp_enable_disable_t * mp)
 {
   vl_api_lisp_enable_disable_reply_t *rmp;
@@ -5080,6 +5131,8 @@ send_lisp_eid_table_details (mapping_t * mapit,
     }
   rmp->context = context;
   rmp->vni = clib_host_to_net_u32 (gid_address_vni (gid));
+  rmp->key_id = clib_host_to_net_u16 (mapit->key_id);
+  memcpy (rmp->key, mapit->key, vec_len (mapit->key));
   vl_msg_api_send_shmem (q, (u8 *) & rmp);
 }
 
@@ -5179,6 +5232,57 @@ vl_api_lisp_gpe_tunnel_dump_t_handler (vl_api_lisp_gpe_tunnel_dump_t * mp)
 }
 
 static void
+send_lisp_map_server_details (ip_address_t * ip,
+			      unix_shared_memory_queue_t * q, u32 context)
+{
+  vl_api_lisp_map_server_details_t *rmp = NULL;
+
+  rmp = vl_msg_api_alloc (sizeof (*rmp));
+  memset (rmp, 0, sizeof (*rmp));
+  rmp->_vl_msg_id = ntohs (VL_API_LISP_MAP_SERVER_DETAILS);
+
+  switch (ip_addr_version (ip))
+    {
+    case IP4:
+      rmp->is_ipv6 = 0;
+      clib_memcpy (rmp->ip_address, &ip_addr_v4 (ip),
+		   sizeof (ip_addr_v4 (ip)));
+      break;
+
+    case IP6:
+      rmp->is_ipv6 = 1;
+      clib_memcpy (rmp->ip_address, &ip_addr_v6 (ip),
+		   sizeof (ip_addr_v6 (ip)));
+      break;
+
+    default:
+      ASSERT (0);
+    }
+  rmp->context = context;
+
+  vl_msg_api_send_shmem (q, (u8 *) & rmp);
+}
+
+static void
+vl_api_lisp_map_server_dump_t_handler (vl_api_lisp_map_server_dump_t * mp)
+{
+  unix_shared_memory_queue_t *q = NULL;
+  lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
+  lisp_msmr_t *mr;
+
+  q = vl_api_client_index_to_input_queue (mp->client_index);
+  if (q == 0)
+    {
+      return;
+    }
+
+  vec_foreach (mr, lcm->map_servers)
+  {
+    send_lisp_map_server_details (&mr->address, q, mp->context);
+  }
+}
+
+static void
 send_lisp_map_resolver_details (ip_address_t * ip,
 				unix_shared_memory_queue_t * q, u32 context)
 {
@@ -5215,7 +5319,7 @@ vl_api_lisp_map_resolver_dump_t_handler (vl_api_lisp_map_resolver_dump_t * mp)
 {
   unix_shared_memory_queue_t *q = NULL;
   lisp_cp_main_t *lcm = vnet_lisp_cp_get_main ();
-  map_resolver_t *mr;
+  lisp_msmr_t *mr;
 
   q = vl_api_client_index_to_input_queue (mp->client_index);
   if (q == 0)
@@ -5333,6 +5437,36 @@ lisp_adjacency_copy (vl_api_lisp_adjacency_t * dst, lisp_adjacency_t * adjs)
 }
 
 static void
+  vl_api_show_lisp_rloc_probe_state_t_handler
+  (vl_api_show_lisp_rloc_probe_state_t * mp)
+{
+  vl_api_show_lisp_rloc_probe_state_reply_t *rmp = 0;
+  int rv = 0;
+
+  /* *INDENT-OFF* */
+  REPLY_MACRO2 (VL_API_SHOW_LISP_RLOC_PROBE_STATE_REPLY,
+  {
+    rmp->is_enabled = vnet_lisp_rloc_probe_state_get ();
+  });
+  /* *INDENT-ON* */
+}
+
+static void
+  vl_api_show_lisp_map_register_state_t_handler
+  (vl_api_show_lisp_map_register_state_t * mp)
+{
+  vl_api_show_lisp_map_register_state_reply_t *rmp = 0;
+  int rv = 0;
+
+  /* *INDENT-OFF* */
+  REPLY_MACRO2 (VL_API_SHOW_LISP_MAP_REGISTER_STATE_REPLY,
+  {
+    rmp->is_enabled = vnet_lisp_map_register_state_get ();
+  });
+  /* *INDENT-ON* */
+}
+
+static void
 vl_api_lisp_adjacencies_get_t_handler (vl_api_lisp_adjacencies_get_t * mp)
 {
   vl_api_lisp_adjacencies_get_reply_t *rmp = 0;
diff --git a/vpp/vpp-api/custom_dump.c b/vpp/vpp-api/custom_dump.c
index fdea94164b4..d0a8ecaea8f 100644
--- a/vpp/vpp-api/custom_dump.c
+++ b/vpp/vpp-api/custom_dump.c
@@ -35,6 +35,7 @@
 #include <vlib/unix/unix.h>
 #include <vlibapi/api.h>
 #include <vlibmemory/api.h>
+#include <vnet/lisp-cp/lisp_types.h>
 
 #include <stats/stats.h>
 #include <oam/oam.h>
@@ -2482,6 +2483,12 @@ static void *vl_api_lisp_add_del_local_eid_t_print
   s = format (s, "eid %U ", format_lisp_flat_eid, mp->eid_type, mp->eid,
 	      mp->prefix_len);
   s = format (s, "locator-set %s ", mp->locator_set_name);
+  if (*mp->key)
+    {
+      u32 key_id = mp->key_id;
+      s = format (s, "key-id %U", format_hmac_key_id, key_id);
+      s = format (s, "secret-key %s", mp->key);
+    }
   FINISH;
 }
 
@@ -2659,6 +2666,34 @@ static void *vl_api_lisp_eid_table_dump_t_print
   FINISH;
 }
 
+static void *vl_api_lisp_rloc_probe_enable_disable_t_print
+  (vl_api_lisp_rloc_probe_enable_disable_t * mp, void *handle)
+{
+  u8 *s;
+
+  s = format (0, "SCRIPT: lisp_rloc_probe_enable_disable ");
+  if (mp->is_enabled)
+    s = format (s, "enable");
+  else
+    s = format (s, "disable");
+
+  FINISH;
+}
+
+static void *vl_api_lisp_map_register_enable_disable_t_print
+  (vl_api_lisp_map_register_enable_disable_t * mp, void *handle)
+{
+  u8 *s;
+
+  s = format (0, "SCRIPT: lisp_map_register_enable_disable ");
+  if (mp->is_enabled)
+    s = format (s, "enable");
+  else
+    s = format (s, "disable");
+
+  FINISH;
+}
+
 static void *vl_api_lisp_adjacencies_get_t_print
   (vl_api_lisp_adjacencies_get_t * mp, void *handle)
 {
@@ -2894,6 +2929,9 @@ static void *vl_api_sw_interface_set_mtu_t_print
 #define foreach_custom_print_no_arg_function                            \
 _(lisp_eid_table_vni_dump)                                              \
 _(lisp_map_resolver_dump)                                               \
+_(lisp_map_server_dump)                                                 \
+_(show_lisp_rloc_probe_state)                                           \
+_(show_lisp_map_register_state)                                         \
 _(show_lisp_map_request_mode)                                           \
 _(lisp_gpe_tunnel_dump)
 
@@ -3045,9 +3083,14 @@ _(LISP_EID_TABLE_MAP_DUMP, lisp_eid_table_map_dump)                     \
 _(LISP_EID_TABLE_VNI_DUMP, lisp_eid_table_vni_dump)                     \
 _(LISP_GPE_TUNNEL_DUMP, lisp_gpe_tunnel_dump)                           \
 _(LISP_MAP_RESOLVER_DUMP, lisp_map_resolver_dump)                       \
+_(LISP_MAP_SERVER_DUMP, lisp_map_server_dump)                           \
 _(LISP_LOCATOR_SET_DUMP, lisp_locator_set_dump)                         \
 _(LISP_LOCATOR_DUMP, lisp_locator_dump)                                 \
 _(LISP_ADJACENCIES_GET, lisp_adjacencies_get)                           \
+_(SHOW_LISP_RLOC_PROBE_STATE, show_lisp_rloc_probe_state)               \
+_(SHOW_LISP_MAP_REGISTER_STATE, show_lisp_map_register_state)           \
+_(LISP_RLOC_PROBE_ENABLE_DISABLE, lisp_rloc_probe_enable_disable)       \
+_(LISP_MAP_REGISTER_ENABLE_DISABLE, lisp_map_register_enable_disable)   \
 _(IPSEC_GRE_ADD_DEL_TUNNEL, ipsec_gre_add_del_tunnel)                   \
 _(IPSEC_GRE_TUNNEL_DUMP, ipsec_gre_tunnel_dump)                         \
 _(DELETE_SUBIF, delete_subif)                                           \
diff --git a/vpp/vpp-api/vpe.api b/vpp/vpp-api/vpe.api
index 7ace8a24d83..66108acf70c 100644
--- a/vpp/vpp-api/vpe.api
+++ b/vpp/vpp-api/vpe.api
@@ -2369,6 +2369,11 @@ define lisp_add_del_locator_reply
     @param prefix_len - prefix len
     @param locator_set_name - name of locator_set to add/del eid-table
     @param vni - virtual network instance
+    @param key_id
+      HMAC_NO_KEY           0
+      HMAC_SHA_1_96         1
+      HMAC_SHA_256_128      2
+    @param key - secret key
 */
 define lisp_add_del_local_eid
 {
@@ -2380,6 +2385,8 @@ define lisp_add_del_local_eid
   u8 prefix_len;
   u8 locator_set_name[64];
   u32 vni;
+  u16 key_id;
+  u8 key[64];
 };
 
 /** \brief Reply for local_eid add/del
@@ -2439,6 +2446,32 @@ define lisp_gpe_add_del_fwd_entry_reply
   i32 retval;
 };
 
+/** \brief Add/delete map server
+    @param client_index - opaque cookie to identify the sender
+    @param context - sender context, to match reply w/ request
+    @param is_add - add address if non-zero; delete otherwise
+    @param is_ipv6 - if non-zero the address is ipv6, else ipv4
+    @param ip_address - map server IP address
+*/
+define lisp_add_del_map_server
+{
+  u32 client_index;
+  u32 context;
+  u8 is_add;
+  u8 is_ipv6;
+  u8 ip_address[16];
+};
+
+/** \brief Reply for lisp_add_del_map_server
+    @param context - returned sender context, to match reply w/ request
+    @param retval - return code
+*/
+define lisp_add_del_map_server_reply
+{
+  u32 context;
+  i32 retval;
+};
+
 /** \brief add or delete map-resolver
     @param client_index - opaque cookie to identify the sender
     @param context - sender context, to match reply w/ request
@@ -2558,6 +2591,93 @@ define lisp_pitr_set_locator_set_reply
   i32 retval;
 };
 
+/** \brief Get state of LISP RLOC probing
+    @param client_index - opaque cookie to identify the sender
+    @param context - sender context, to match reply w/ request
+*/
+define show_lisp_rloc_probe_state
+{
+  u32 client_index;
+  u32 context;
+};
+
+/** \brief Reply for show_lisp_rloc_probe_state
+    @param context - returned sender context, to match reply w/ request
+    @param retval - return code
+    @param is_enabled - state of RLOC probing
+*/
+define show_lisp_rloc_probe_state_reply
+{
+  u32 context;
+  i32 retval;
+  u8 is_enabled;
+};
+
+/** \brief enable/disable LISP RLOC probing
+    @param client_index - opaque cookie to identify the sender
+    @param context - sender context, to match reply w/ request
+    @param is_enable - enable if non-zero; disable otherwise
+*/
+define lisp_rloc_probe_enable_disable
+{
+  u32 client_index;
+  u32 context;
+  u8 is_enabled;
+};
+
+/** \brief Reply for lisp_rloc_probe_enable_disable
+    @param context - returned sender context, to match reply w/ request
+    @param retval - return code
+*/
+define lisp_rloc_probe_enable_disable_reply
+{
+  u32 context;
+  i32 retval;
+};
+
+/** \brief enable/disable LISP map-register
+    @param client_index - opaque cookie to identify the sender
+    @param context - sender context, to match reply w/ request
+    @param is_enable - enable if non-zero; disable otherwise
+*/
+define lisp_map_register_enable_disable
+{
+  u32 client_index;
+  u32 context;
+  u8 is_enabled;
+};
+
+/** \brief Reply for lisp_map_register_enable_disable
+    @param context - returned sender context, to match reply w/ request
+    @param retval - return code
+*/
+define lisp_map_register_enable_disable_reply
+{
+  u32 context;
+  i32 retval;
+};
+
+/** \brief Get state of LISP map-register
+    @param client_index - opaque cookie to identify the sender
+    @param context - sender context, to match reply w/ request
+*/
+define show_lisp_map_register_state
+{
+  u32 client_index;
+  u32 context;
+};
+
+/** \brief Reply for show_lisp_map_register_state
+    @param context - returned sender context, to match reply w/ request
+    @param retval - return code
+*/
+define show_lisp_map_register_state_reply
+{
+  u32 context;
+  i32 retval;
+  u8 is_enabled;
+};
+
 /** \brief set LISP map-request mode. Based on configuration VPP will send
       src/dest or just normal destination map requests.
     @param client_index - opaque cookie to identify the sender
@@ -2826,6 +2946,11 @@ define lisp_locator_set_dump
     @param vni - virtual network instance
     @param ttl - time to live
     @param authoritative - authoritative
+    @param key_id
+      HMAC_NO_KEY           0
+      HMAC_SHA_1_96         1
+      HMAC_SHA_256_128      2
+    @param key - secret key
 */
 
 define lisp_eid_table_details
@@ -2843,6 +2968,8 @@ define lisp_eid_table_details
   u8 seid_prefix_len;
   u32 ttl;
   u8 authoritative;
+  u16 key_id;
+  u8 key[64];
 };
 
 /** \brief Request for eid table summary status
@@ -2993,7 +3120,6 @@ define lisp_gpe_tunnel_dump
 };
 
 /** \brief LISP map resolver status
-    @param locator_set_name - name of the locator_set
     @param is_ipv6 - if non-zero the address is ipv6, else ipv4
     @param ip_address - array of address bytes
  */
@@ -3014,6 +3140,27 @@ define lisp_map_resolver_dump
   u32 context;
 };
 
+/** \brief LISP map server details
+    @param is_ipv6 - if non-zero the address is ipv6, else ipv4
+    @param ip_address - array of address bytes
+ */
+define lisp_map_server_details
+{
+  u32 context;
+  u8 is_ipv6;
+  u8 ip_address[16];
+};
+
+/** \brief Request for map server summary status
+    @param client_index - opaque cookie to identify the sender
+    @param context - sender context, to match reply w/ request
+ */
+define lisp_map_server_dump
+{
+  u32 client_index;
+  u32 context;
+};
+
 /** \brief Request for lisp-gpe protocol status
     @param client_index - opaque cookie to identify the sender
     @param context - sender context, to match reply w/ request
diff --git a/vppinfra/vppinfra/test_bihash_template.c b/vppinfra/vppinfra/test_bihash_template.c
index 5a5b63484aa..c505bd83d9a 100644
--- a/vppinfra/vppinfra/test_bihash_template.c
+++ b/vppinfra/vppinfra/test_bihash_template.c
@@ -23,7 +23,7 @@
 
 typedef struct
 {
-  u32 seed;
+  u64 seed;
   u32 nbuckets;
   u32 nitems;
   u32 search_iter;