Implement LISP control plane messages

* Map-register
* Map-notify
* RLOC probing

Change-Id: I7f6295376b21cd67805446dfd1c1033acead2d4b
Signed-off-by: Filip Tehlar <ftehlar@cisco.com>

8 files changed, 1395 insertions, 64 deletions

diff --git a/vnet/test/lisp-cp/test_cp_serdes.c b/vnet/test/lisp-cp/test_cp_serdes.c
index 7bfe6e35..2d1cf062 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 3b843a01..40ce6b0c 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 7a56d4f9..14b54606 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 d52f37b6..278f60e1 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 de9a4f5b..57a04547 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 5f9ca718..d794eff6 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 04b8462e..d206e6bc 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 dd7a53ee..7df0e653 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_ */
 
 /*