diff --git a/src/vnet/bfd/bfd.api b/src/vnet/bfd/bfd.api
index f307ed2..93bf0fb 100644
--- a/src/vnet/bfd/bfd.api
+++ b/src/vnet/bfd/bfd.api
@@ -13,29 +13,43 @@
  * limitations under the License.
  */
 
-/** \brief Configure BFD feature
+/** \brief Set BFD echo source
     @param client_index - opaque cookie to identify the sender
     @param context - sender context, to match reply w/ request
-    @param slow_timer - slow timer (seconds)
-    @param min_tx - desired min tx interval
-    @param min_rx - desired min rx interval
-    @param detect_mult - desired detection multiplier
+    @param sw_if_index - interface to use as echo source
 */
-define bfd_set_config
+define bfd_udp_set_echo_source
 {
   u32 client_index;
   u32 context;
-  u32 slow_timer;
-  u32 min_tx;
-  u32 min_rx;
-  u8 detect_mult;
+  u32 sw_if_index;
 };
 
-/** \brief Configure BFD feature response
+/** \brief Set BFD feature response
     @param context - sender context, to match reply w/ request
     @param retval - return code for the request
 */
-define bfd_set_config_reply
+define bfd_udp_set_echo_source_reply
+{
+  u32 context;
+  i32 retval;
+};
+
+/** \brief Delete BFD echo source
+    @param client_index - opaque cookie to identify the sender
+    @param context - sender context, to match reply w/ request
+*/
+define bfd_udp_del_echo_source
+{
+  u32 client_index;
+  u32 context;
+};
+
+/** \brief Delete BFD feature response
+    @param context - sender context, to match reply w/ request
+    @param retval - return code for the request
+*/
+define bfd_udp_del_echo_source_reply
 {
   u32 context;
   i32 retval;
diff --git a/src/vnet/bfd/bfd_api.c b/src/vnet/bfd/bfd_api.c
index af70f0e..6632eae 100644
--- a/src/vnet/bfd/bfd_api.c
+++ b/src/vnet/bfd/bfd_api.c
@@ -54,7 +54,9 @@
   _ (BFD_AUTH_DEL_KEY, bfd_auth_del_key)                   \
   _ (BFD_AUTH_KEYS_DUMP, bfd_auth_keys_dump)               \
   _ (BFD_UDP_AUTH_ACTIVATE, bfd_udp_auth_activate)         \
-  _ (BFD_UDP_AUTH_DEACTIVATE, bfd_udp_auth_deactivate)
+  _ (BFD_UDP_AUTH_DEACTIVATE, bfd_udp_auth_deactivate)     \
+  _ (BFD_UDP_SET_ECHO_SOURCE, bfd_udp_set_echo_source)     \
+  _ (BFD_UDP_DEL_ECHO_SOURCE, bfd_udp_del_echo_source)
 
 pub_sub_handler (bfd_events, BFD_EVENTS);
 
@@ -314,6 +316,33 @@
   REPLY_MACRO (VL_API_BFD_UDP_AUTH_DEACTIVATE_REPLY);
 }
 
+static void
+vl_api_bfd_udp_set_echo_source_t_handler (vl_api_bfd_udp_set_echo_source_t *
+					  mp)
+{
+  vl_api_bfd_udp_set_echo_source_reply_t *rmp;
+  int rv;
+
+  VALIDATE_SW_IF_INDEX (mp);
+
+  rv = bfd_udp_set_echo_source (clib_net_to_host_u32 (mp->sw_if_index));
+
+  BAD_SW_IF_INDEX_LABEL;
+  REPLY_MACRO (VL_API_BFD_UDP_SET_ECHO_SOURCE_REPLY);
+}
+
+static void
+vl_api_bfd_udp_del_echo_source_t_handler (vl_api_bfd_udp_del_echo_source_t *
+					  mp)
+{
+  vl_api_bfd_udp_del_echo_source_reply_t *rmp;
+  int rv;
+
+  rv = bfd_udp_del_echo_source ();
+
+  REPLY_MACRO (VL_API_BFD_UDP_DEL_ECHO_SOURCE_REPLY);
+}
+
 /*
  * bfd_api_hookup
  * Add vpe's API message handlers to the table.
diff --git a/src/vnet/bfd/bfd_api.h b/src/vnet/bfd/bfd_api.h
index f4486a7..63d4a62 100644
--- a/src/vnet/bfd/bfd_api.h
+++ b/src/vnet/bfd/bfd_api.h
@@ -24,6 +24,17 @@
 #include <vnet/ip/ip6_packet.h>
 #include <vnet/bfd/bfd_udp.h>
 
+#define foreach_bfd_transport(F) \
+  F (UDP4, "ip4-rewrite")        \
+  F (UDP6, "ip6-rewrite")
+
+typedef enum
+{
+#define F(t, n) BFD_TRANSPORT_##t,
+  foreach_bfd_transport (F)
+#undef F
+} bfd_transport_e;
+
 vnet_api_error_t
 bfd_udp_add_session (u32 sw_if_index, const ip46_address_t * local_addr,
 		     const ip46_address_t * peer_addr,
@@ -31,12 +42,11 @@
 		     u8 detect_mult, u8 is_authenticated, u32 conf_key_id,
 		     u8 bfd_key_id);
 
-vnet_api_error_t bfd_udp_mod_session (u32 sw_if_index,
-				      const ip46_address_t * local_addr,
-				      const ip46_address_t * peer_addr,
-				      u32 desired_min_tx_usec,
-				      u32 required_min_rx_usec,
-				      u8 detect_mult);
+vnet_api_error_t
+bfd_udp_mod_session (u32 sw_if_index, const ip46_address_t * local_addr,
+		     const ip46_address_t * peer_addr,
+		     u32 desired_min_tx_usec, u32 required_min_rx_usec,
+		     u8 detect_mult);
 
 vnet_api_error_t bfd_udp_del_session (u32 sw_if_index,
 				      const ip46_address_t * local_addr,
@@ -63,6 +73,10 @@
 					  const ip46_address_t * peer_addr,
 					  u8 is_delayed);
 
+vnet_api_error_t bfd_udp_set_echo_source (u32 loopback_sw_if_index);
+
+vnet_api_error_t bfd_udp_del_echo_source ();
+
 #endif /* __included_bfd_api_h__ */
 
 /*
diff --git a/src/vnet/bfd/bfd_debug.h b/src/vnet/bfd/bfd_debug.h
index a06e934..3017352 100644
--- a/src/vnet/bfd/bfd_debug.h
+++ b/src/vnet/bfd/bfd_debug.h
@@ -20,7 +20,7 @@
 #define __included_bfd_debug_h__
 
 /* controls debug prints */
-#define BFD_DEBUG (0)
+#define BFD_DEBUG (1)
 
 #if BFD_DEBUG
 #define BFD_DEBUG_FILE_DEF            \
diff --git a/src/vnet/bfd/bfd_main.c b/src/vnet/bfd/bfd_main.c
index c0fd18d..29c4045 100644
--- a/src/vnet/bfd/bfd_main.c
+++ b/src/vnet/bfd/bfd_main.c
@@ -17,17 +17,37 @@
  * @brief BFD nodes implementation
  */
 
+#if WITH_LIBSSL > 0
+#include <openssl/sha.h>
+#endif
+
+#if __SSE4_2__
+#include <x86intrin.h>
+#endif
+
 #include <vppinfra/random.h>
 #include <vppinfra/error.h>
 #include <vppinfra/hash.h>
+#include <vppinfra/xxhash.h>
 #include <vnet/ethernet/ethernet.h>
 #include <vnet/ethernet/packet.h>
 #include <vnet/bfd/bfd_debug.h>
 #include <vnet/bfd/bfd_protocol.h>
 #include <vnet/bfd/bfd_main.h>
-#if WITH_LIBSSL > 0
-#include <openssl/sha.h>
+
+static u64
+bfd_calc_echo_checksum (u32 discriminator, u64 expire_time, u32 secret)
+{
+  u64 checksum = 0;
+#if __SSE4_2__
+  checksum = _mm_crc32_u64 (0, discriminator);
+  checksum = _mm_crc32_u64 (checksum, expire_time);
+  checksum = _mm_crc32_u64 (checksum, secret);
+#else
+  checksum = clib_xxhash (discriminator ^ expire_time ^ secret);
 #endif
+  return checksum;
+}
 
 static u64
 bfd_usec_to_clocks (const bfd_main_t * bm, u64 us)
@@ -35,6 +55,12 @@
   return bm->cpu_cps * ((f64) us / USEC_PER_SECOND);
 }
 
+static u32
+bfd_clocks_to_usec (const bfd_main_t * bm, u64 clocks)
+{
+  return (clocks / bm->cpu_cps) * USEC_PER_SECOND;
+}
+
 static vlib_node_registration_t bfd_process_node;
 
 /* set to 0 here, real values filled at startup */
@@ -81,17 +107,19 @@
   bs->local_state = BFD_STATE_down;
   bs->local_diag = BFD_DIAG_CODE_no_diag;
   bs->remote_state = BFD_STATE_down;
-  bs->local_demand = 0;
   bs->remote_discr = 0;
-  bs->config_desired_min_tx_usec = BFD_DEFAULT_DESIRED_MIN_TX_US;
+  bs->config_desired_min_tx_usec = BFD_DEFAULT_DESIRED_MIN_TX_USEC;
   bs->config_desired_min_tx_clocks = bm->default_desired_min_tx_clocks;
   bs->effective_desired_min_tx_clocks = bm->default_desired_min_tx_clocks;
   bs->remote_min_rx_usec = 1;
   bs->remote_min_rx_clocks = bfd_usec_to_clocks (bm, bs->remote_min_rx_usec);
+  bs->remote_min_echo_rx_usec = 0;
+  bs->remote_min_echo_rx_clocks = 0;
   bs->remote_demand = 0;
   bs->auth.remote_seq_number = 0;
   bs->auth.remote_seq_number_known = 0;
   bs->auth.local_seq_number = random_u32 (&bm->random_seed);
+  bs->echo_secret = random_u32 (&bm->random_seed);
 }
 
 static void
@@ -119,68 +147,90 @@
     }
 }
 
+static const char *
+bfd_poll_state_string (bfd_poll_state_e state)
+{
+  switch (state)
+    {
+#define F(x)         \
+  case BFD_POLL_##x: \
+    return "BFD_POLL_" #x;
+      foreach_bfd_poll_state (F)
+#undef F
+    }
+  return "UNKNOWN";
+}
+
+static void
+bfd_set_poll_state (bfd_session_t * bs, bfd_poll_state_e state)
+{
+  if (bs->poll_state != state)
+    {
+      BFD_DBG ("Setting poll state=%s, bs_idx=%u",
+	       bfd_poll_state_string (state), bs->bs_idx);
+      bs->poll_state = state;
+    }
+}
+
 static void
 bfd_recalc_tx_interval (bfd_main_t * bm, bfd_session_t * bs)
 {
-  if (!bs->local_demand)
-    {
-      bs->transmit_interval_clocks =
-	clib_max (bs->effective_desired_min_tx_clocks,
-		  bs->remote_min_rx_clocks);
-    }
-  else
-    {
-      /* TODO */
-    }
-  BFD_DBG ("Recalculated transmit interval %lu clocks/%.2fs",
-	   bs->transmit_interval_clocks,
-	   bs->transmit_interval_clocks / bm->cpu_cps);
+  bs->transmit_interval_clocks =
+    clib_max (bs->effective_desired_min_tx_clocks, bs->remote_min_rx_clocks);
+  BFD_DBG ("Recalculated transmit interval " BFD_CLK_FMT,
+	   BFD_CLK_PRN (bs->transmit_interval_clocks));
+}
+
+static void
+bfd_recalc_echo_tx_interval (bfd_main_t * bm, bfd_session_t * bs)
+{
+  bs->echo_transmit_interval_clocks =
+    clib_max (bs->effective_desired_min_tx_clocks,
+	      bs->remote_min_echo_rx_clocks);
+  BFD_DBG ("Recalculated echo transmit interval " BFD_CLK_FMT,
+	   BFD_CLK_PRN (bs->echo_transmit_interval_clocks));
 }
 
 static void
 bfd_calc_next_tx (bfd_main_t * bm, bfd_session_t * bs, u64 now)
 {
-  if (!bs->local_demand)
+  if (bs->local_detect_mult > 1)
     {
-      if (bs->local_detect_mult > 1)
+      /* common case - 75-100% of transmit interval */
+      bs->tx_timeout_clocks = bs->last_tx_clocks +
+	(1 - .25 * (random_f64 (&bm->random_seed))) *
+	bs->transmit_interval_clocks;
+      if (bs->tx_timeout_clocks < now)
 	{
-	  /* common case - 75-100% of transmit interval */
-	  bs->tx_timeout_clocks = bs->last_tx_clocks +
-	    (1 - .25 * (random_f64 (&bm->random_seed))) *
-	    bs->transmit_interval_clocks;
-	  if (bs->tx_timeout_clocks < now)
-	    {
-	      /* huh, we've missed it already, transmit now */
-	      BFD_DBG ("Missed %lu transmit events (now is %lu, calc "
-		       "tx_timeout is %lu)",
-		       (now - bs->tx_timeout_clocks) /
-		       bs->transmit_interval_clocks,
-		       now, bs->tx_timeout_clocks);
-	      bs->tx_timeout_clocks = now;
-	    }
-	}
-      else
-	{
-	  /* special case - 75-90% of transmit interval */
-	  bs->tx_timeout_clocks =
-	    bs->last_tx_clocks +
-	    (.9 - .15 * (random_f64 (&bm->random_seed))) *
-	    bs->transmit_interval_clocks;
-	  if (bs->tx_timeout_clocks < now)
-	    {
-	      /* huh, we've missed it already, transmit now */
-	      BFD_DBG ("Missed %lu transmit events (now is %lu, calc "
-		       "tx_timeout is %lu)",
-		       (now - bs->tx_timeout_clocks) /
-		       bs->transmit_interval_clocks,
-		       now, bs->tx_timeout_clocks);
-	      bs->tx_timeout_clocks = now;
-	    }
+	  /*
+	   * the timeout is in the past, which means that either remote
+	   * demand mode was set or performance/clock issues ...
+	   */
+	  BFD_DBG ("Missed %lu transmit events (now is %lu, calc "
+		   "tx_timeout is %lu)",
+		   (now - bs->tx_timeout_clocks) /
+		   bs->transmit_interval_clocks, now, bs->tx_timeout_clocks);
+	  bs->tx_timeout_clocks = now;
 	}
     }
   else
     {
-      /* TODO */
+      /* special case - 75-90% of transmit interval */
+      bs->tx_timeout_clocks = bs->last_tx_clocks +
+	(.9 - .15 * (random_f64 (&bm->random_seed))) *
+	bs->transmit_interval_clocks;
+      if (bs->tx_timeout_clocks < now)
+	{
+	  /*
+	   * the timeout is in the past, which means that either remote
+	   * demand mode was set or performance/clock issues ...
+	   */
+	  BFD_DBG ("Missed %lu transmit events (now is %lu, calc "
+		   "tx_timeout is %lu)",
+		   (now - bs->tx_timeout_clocks) /
+		   bs->transmit_interval_clocks, now, bs->tx_timeout_clocks);
+	  bs->tx_timeout_clocks = now;
+	}
     }
   if (bs->tx_timeout_clocks)
     {
@@ -192,23 +242,32 @@
 }
 
 static void
+bfd_calc_next_echo_tx (bfd_main_t * bm, bfd_session_t * bs, u64 now)
+{
+  bs->echo_tx_timeout_clocks =
+    bs->echo_last_tx_clocks + bs->echo_transmit_interval_clocks;
+  if (bs->echo_tx_timeout_clocks < now)
+    {
+      /* huh, we've missed it already, transmit now */
+      BFD_DBG ("Missed %lu echo transmit events (now is %lu, calc tx_timeout "
+	       "is %lu)",
+	       (now - bs->echo_tx_timeout_clocks) /
+	       bs->echo_transmit_interval_clocks,
+	       now, bs->echo_tx_timeout_clocks);
+      bs->echo_tx_timeout_clocks = now;
+    }
+  BFD_DBG ("Next echo transmit in %lu clocks/%.02fs@%lu",
+	   bs->echo_tx_timeout_clocks - now,
+	   (bs->echo_tx_timeout_clocks - now) / bm->cpu_cps,
+	   bs->echo_tx_timeout_clocks);
+}
+
+static void
 bfd_recalc_detection_time (bfd_main_t * bm, bfd_session_t * bs)
 {
-  if (!bs->local_demand)
-    {
-      /* asynchronous mode */
-      bs->detection_time_clocks =
-	bs->remote_detect_mult *
-	clib_max (bs->effective_required_min_rx_clocks,
-		  bs->remote_desired_min_tx_clocks);
-    }
-  else
-    {
-      /* demand mode */
-      bs->detection_time_clocks =
-	bs->local_detect_mult * clib_max (bs->config_desired_min_tx_clocks,
-					  bs->remote_min_rx_clocks);
-    }
+  bs->detection_time_clocks =
+    bs->remote_detect_mult * clib_max (bs->effective_required_min_rx_clocks,
+				       bs->remote_desired_min_tx_clocks);
   BFD_DBG ("Recalculated detection time %lu clocks/%.2fs",
 	   bs->detection_time_clocks,
 	   bs->detection_time_clocks / bm->cpu_cps);
@@ -220,25 +279,37 @@
 {
   u64 next = 0;
   u64 rx_timeout = 0;
+  u64 tx_timeout = 0;
   if (BFD_STATE_up == bs->local_state)
     {
       rx_timeout = bs->last_rx_clocks + bs->detection_time_clocks;
     }
-  if (bs->tx_timeout_clocks && rx_timeout)
+  if (BFD_STATE_up != bs->local_state || !bs->remote_demand ||
+      BFD_POLL_NOT_NEEDED != bs->poll_state)
     {
-      next = clib_min (bs->tx_timeout_clocks, rx_timeout);
+      tx_timeout = bs->tx_timeout_clocks;
     }
-  else if (bs->tx_timeout_clocks)
+  if (tx_timeout && rx_timeout)
     {
-      next = bs->tx_timeout_clocks;
+      next = clib_min (tx_timeout, rx_timeout);
+    }
+  else if (tx_timeout)
+    {
+      next = tx_timeout;
     }
   else if (rx_timeout)
     {
       next = rx_timeout;
     }
-  BFD_DBG ("bs_idx=%u, tx_timeout=%lu, rx_timeout=%lu, next=%s", bs->bs_idx,
-	   bs->tx_timeout_clocks, rx_timeout,
-	   next == bs->tx_timeout_clocks ? "tx" : "rx");
+  if (bs->echo && next > bs->echo_tx_timeout_clocks)
+    {
+      next = bs->echo_tx_timeout_clocks;
+    }
+  BFD_DBG ("bs_idx=%u, tx_timeout=%lu, echo_tx_timeout=%lu, rx_timeout=%lu, "
+	   "next=%s",
+	   bs->bs_idx, tx_timeout, bs->echo_tx_timeout_clocks, rx_timeout,
+	   next == tx_timeout
+	   ? "tx" : (next == bs->echo_tx_timeout_clocks ? "echo tx" : "rx"));
   /* sometimes the wheel expires an event a bit sooner than requested, account
      for that here */
   if (next && (now + bm->wheel_inaccuracy > bs->wheel_time_clocks ||
@@ -271,6 +342,7 @@
 	   BFD_CLK_PRN (bs->effective_desired_min_tx_clocks));
   bfd_recalc_detection_time (bm, bs);
   bfd_recalc_tx_interval (bm, bs);
+  bfd_recalc_echo_tx_interval (bm, bs);
   bfd_calc_next_tx (bm, bs, now);
 }
 
@@ -287,25 +359,40 @@
 
 static void
 bfd_set_remote_required_min_rx (bfd_main_t * bm, bfd_session_t * bs,
-				u64 now,
-				u32 remote_required_min_rx_usec,
-				int handling_wakeup)
+				u64 now, u32 remote_required_min_rx_usec)
 {
-  bs->remote_min_rx_usec = remote_required_min_rx_usec;
-  bs->remote_min_rx_clocks =
-    bfd_usec_to_clocks (bm, remote_required_min_rx_usec);
-  BFD_DBG ("Set remote min rx to " BFD_CLK_FMT,
-	   BFD_CLK_PRN (bs->remote_min_rx_clocks));
-  bfd_recalc_detection_time (bm, bs);
-  bfd_recalc_tx_interval (bm, bs);
-  bfd_calc_next_tx (bm, bs, now);
-  bfd_set_timer (bm, bs, now, handling_wakeup);
+  if (bs->remote_min_rx_usec != remote_required_min_rx_usec)
+    {
+      bs->remote_min_rx_usec = remote_required_min_rx_usec;
+      bs->remote_min_rx_clocks =
+	bfd_usec_to_clocks (bm, remote_required_min_rx_usec);
+      BFD_DBG ("Set remote min rx to " BFD_CLK_FMT,
+	       BFD_CLK_PRN (bs->remote_min_rx_clocks));
+      bfd_recalc_detection_time (bm, bs);
+      bfd_recalc_tx_interval (bm, bs);
+    }
+}
+
+static void
+bfd_set_remote_required_min_echo_rx (bfd_main_t * bm, bfd_session_t * bs,
+				     u64 now,
+				     u32 remote_required_min_echo_rx_usec)
+{
+  if (bs->remote_min_echo_rx_usec != remote_required_min_echo_rx_usec)
+    {
+      bs->remote_min_echo_rx_usec = remote_required_min_echo_rx_usec;
+      bs->remote_min_echo_rx_clocks =
+	bfd_usec_to_clocks (bm, bs->remote_min_echo_rx_usec);
+      BFD_DBG ("Set remote min echo rx to " BFD_CLK_FMT,
+	       BFD_CLK_PRN (bs->remote_min_echo_rx_clocks));
+      bfd_recalc_echo_tx_interval (bm, bs);
+    }
 }
 
 void
 bfd_session_start (bfd_main_t * bm, bfd_session_t * bs)
 {
-  BFD_DBG ("%U", format_bfd_session, bs);
+  BFD_DBG ("\nStarting session: %U", format_bfd_session, bs);
   bfd_recalc_tx_interval (bm, bs);
   vlib_process_signal_event (bm->vlib_main, bm->bfd_process_node_index,
 			     BFD_EVENT_NEW_SESSION, bs->bs_idx);
@@ -418,11 +505,12 @@
 bfd_on_state_change (bfd_main_t * bm, bfd_session_t * bs, u64 now,
 		     int handling_wakeup)
 {
-  BFD_DBG ("State changed: %U", format_bfd_session, bs);
+  BFD_DBG ("\nState changed: %U", format_bfd_session, bs);
   bfd_event (bm, bs);
   switch (bs->local_state)
     {
     case BFD_STATE_admin_down:
+      bs->echo = 0;
       bfd_set_effective_desired_min_tx (bm, bs, now,
 					clib_max
 					(bs->config_desired_min_tx_clocks,
@@ -432,6 +520,7 @@
       bfd_set_timer (bm, bs, now, handling_wakeup);
       break;
     case BFD_STATE_down:
+      bs->echo = 0;
       bfd_set_effective_desired_min_tx (bm, bs, now,
 					clib_max
 					(bs->config_desired_min_tx_clocks,
@@ -441,6 +530,7 @@
       bfd_set_timer (bm, bs, now, handling_wakeup);
       break;
     case BFD_STATE_init:
+      bs->echo = 0;
       bfd_set_effective_desired_min_tx (bm, bs, now,
 					bs->config_desired_min_tx_clocks);
       bfd_set_timer (bm, bs, now, handling_wakeup);
@@ -448,7 +538,7 @@
     case BFD_STATE_up:
       bfd_set_effective_desired_min_tx (bm, bs, now,
 					bs->config_desired_min_tx_clocks);
-      if (POLL_NOT_NEEDED == bs->poll_state)
+      if (BFD_POLL_NOT_NEEDED == bs->poll_state)
 	{
 	  bfd_set_effective_required_min_rx (bm, bs, now,
 					     bs->config_required_min_rx_clocks);
@@ -462,13 +552,14 @@
 bfd_on_config_change (vlib_main_t * vm, vlib_node_runtime_t * rt,
 		      bfd_main_t * bm, bfd_session_t * bs, u64 now)
 {
-  if (bs->remote_demand)
+  /*
+   * if remote demand mode is set and we need to do a poll, set the next
+   * timeout so that the session wakes up immediately
+   */
+  if (bs->remote_demand && BFD_POLL_NEEDED == bs->poll_state &&
+      bs->poll_state_start_or_timeout_clocks < now)
     {
-      /* TODO - initiate poll sequence here */
-    }
-  else
-    {
-      /* asynchronous - poll is part of periodic - nothing to do here */
+      bs->tx_timeout_clocks = now;
     }
   bfd_recalc_detection_time (bm, bs);
   bfd_set_timer (bm, bs, now, 0);
@@ -482,27 +573,36 @@
     {
     case BFD_TRANSPORT_UDP4:
       BFD_DBG ("Transport bfd via udp4, bs_idx=%u", bs->bs_idx);
-      bfd_add_udp4_transport (vm, b, bs);
+      bfd_add_udp4_transport (vm, b, bs, 0 /* is_echo */ );
       break;
     case BFD_TRANSPORT_UDP6:
       BFD_DBG ("Transport bfd via udp6, bs_idx=%u", bs->bs_idx);
-      bfd_add_udp6_transport (vm, b, bs);
+      bfd_add_udp6_transport (vm, b, bs, 0 /* is_echo */ );
       break;
     }
 }
 
-static vlib_buffer_t *
-bfd_create_frame_to_next_node (vlib_main_t * vm, bfd_session_t * bs)
+static int
+bfd_echo_add_transport_layer (vlib_main_t * vm, vlib_buffer_t * b,
+			      bfd_session_t * bs)
 {
-  u32 bi;
-  if (vlib_buffer_alloc (vm, &bi, 1) != 1)
+  switch (bs->transport)
     {
-      clib_warning ("buffer allocation failure");
-      return NULL;
+    case BFD_TRANSPORT_UDP4:
+      BFD_DBG ("Transport bfd echo via udp4, bs_idx=%u", bs->bs_idx);
+      return bfd_add_udp4_transport (vm, b, bs, 1 /* is_echo */ );
+      break;
+    case BFD_TRANSPORT_UDP6:
+      BFD_DBG ("Transport bfd echo via udp6, bs_idx=%u", bs->bs_idx);
+      return bfd_add_udp6_transport (vm, b, bs, 1 /* is_echo */ );
+      break;
     }
+  return 0;
+}
 
-  vlib_buffer_t *b = vlib_get_buffer (vm, bi);
-  ASSERT (b->current_data == 0);
+static void
+bfd_create_frame_to_next_node (vlib_main_t * vm, bfd_session_t * bs, u32 bi)
+{
 
   vlib_frame_t *f =
     vlib_get_frame_to_node (vm, bfd_node_index_by_transport[bs->transport]);
@@ -510,9 +610,7 @@
   u32 *to_next = vlib_frame_vector_args (f);
   to_next[0] = bi;
   f->n_vectors = 1;
-
   vlib_put_frame_to_node (vm, bfd_node_index_by_transport[bs->transport], f);
-  return b;
 }
 
 #if WITH_LIBSSL > 0
@@ -583,45 +681,118 @@
     }
 }
 
+static int
+bfd_is_echo_possible (bfd_session_t * bs)
+{
+  if (BFD_STATE_up == bs->local_state && BFD_STATE_up == bs->remote_state &&
+      bs->remote_min_echo_rx_usec > 0)
+    {
+      switch (bs->transport)
+	{
+	case BFD_TRANSPORT_UDP4:
+	  return bfd_udp_is_echo_available (BFD_TRANSPORT_UDP4);
+	case BFD_TRANSPORT_UDP6:
+	  return bfd_udp_is_echo_available (BFD_TRANSPORT_UDP6);
+	}
+    }
+  return 0;
+}
+
 static void
-bfd_init_control_frame (vlib_buffer_t * b, bfd_session_t * bs)
+bfd_init_control_frame (bfd_main_t * bm, bfd_session_t * bs,
+			vlib_buffer_t * b)
 {
   bfd_pkt_t *pkt = vlib_buffer_get_current (b);
-
   u32 bfd_length = 0;
   bfd_length = sizeof (bfd_pkt_t);
   memset (pkt, 0, sizeof (*pkt));
   bfd_pkt_set_version (pkt, 1);
   bfd_pkt_set_diag_code (pkt, bs->local_diag);
   bfd_pkt_set_state (pkt, bs->local_state);
-  if (bs->local_demand && BFD_STATE_up == bs->local_state &&
-      BFD_STATE_up == bs->remote_state)
-    {
-      bfd_pkt_set_demand (pkt);
-    }
   pkt->head.detect_mult = bs->local_detect_mult;
   pkt->head.length = clib_host_to_net_u32 (bfd_length);
   pkt->my_disc = bs->local_discr;
   pkt->your_disc = bs->remote_discr;
   pkt->des_min_tx = clib_host_to_net_u32 (bs->config_desired_min_tx_usec);
-  pkt->req_min_rx = clib_host_to_net_u32 (bs->config_required_min_rx_usec);
+  if (bs->echo)
+    {
+      pkt->req_min_rx =
+	clib_host_to_net_u32 (bfd_clocks_to_usec
+			      (bm, bs->effective_required_min_rx_clocks));
+    }
+  else
+    {
+      pkt->req_min_rx =
+	clib_host_to_net_u32 (bs->config_required_min_rx_usec);
+    }
   pkt->req_min_echo_rx = clib_host_to_net_u32 (1);
   b->current_length = bfd_length;
 }
 
 static void
+bfd_send_echo (vlib_main_t * vm, vlib_node_runtime_t * rt,
+	       bfd_main_t * bm, bfd_session_t * bs, u64 now,
+	       int handling_wakeup)
+{
+  if (!bfd_is_echo_possible (bs))
+    {
+      BFD_DBG ("\nSwitching off echo function: %U", format_bfd_session, bs);
+      bs->echo = 0;
+      return;
+    }
+  /* sometimes the wheel expires an event a bit sooner than requested, account
+     for that here */
+  if (now + bm->wheel_inaccuracy >= bs->echo_tx_timeout_clocks)
+    {
+      BFD_DBG ("\nSending echo packet: %U", format_bfd_session, bs);
+      u32 bi;
+      if (vlib_buffer_alloc (vm, &bi, 1) != 1)
+	{
+	  clib_warning ("buffer allocation failure");
+	  return;
+	}
+      vlib_buffer_t *b = vlib_get_buffer (vm, bi);
+      ASSERT (b->current_data == 0);
+      bfd_echo_pkt_t *pkt = vlib_buffer_get_current (b);
+      memset (pkt, 0, sizeof (*pkt));
+      pkt->discriminator = bs->local_discr;
+      pkt->expire_time_clocks =
+	now + bs->echo_transmit_interval_clocks * bs->local_detect_mult;
+      pkt->checksum =
+	bfd_calc_echo_checksum (bs->local_discr, pkt->expire_time_clocks,
+				bs->echo_secret);
+      b->current_length = sizeof (*pkt);
+      if (!bfd_echo_add_transport_layer (vm, b, bs))
+	{
+	  BFD_ERR ("cannot send echo packet out, turning echo off");
+	  bs->echo = 0;
+	  vlib_buffer_free_one (vm, bi);
+	  return;
+	}
+      bs->echo_last_tx_clocks = now;
+      bfd_calc_next_echo_tx (bm, bs, now);
+      bfd_create_frame_to_next_node (vm, bs, bi);
+    }
+  else
+    {
+      BFD_DBG
+	("No need to send echo packet now, now is %lu, tx_timeout is %lu",
+	 now, bs->echo_tx_timeout_clocks);
+    }
+}
+
+static void
 bfd_send_periodic (vlib_main_t * vm, vlib_node_runtime_t * rt,
 		   bfd_main_t * bm, bfd_session_t * bs, u64 now,
 		   int handling_wakeup)
 {
-  if (!bs->remote_min_rx_usec)
+  if (!bs->remote_min_rx_usec && BFD_POLL_NOT_NEEDED == bs->poll_state)
     {
-      BFD_DBG
-	("bfd.RemoteMinRxInterval is zero, not sending periodic control "
-	 "frame");
+      BFD_DBG ("Remote min rx interval is zero, not sending periodic control "
+	       "frame");
       return;
     }
-  if (POLL_NOT_NEEDED == bs->poll_state && bs->remote_demand &&
+  if (BFD_POLL_NOT_NEEDED == bs->poll_state && bs->remote_demand &&
       BFD_STATE_up == bs->local_state && BFD_STATE_up == bs->remote_state)
     {
       /*
@@ -630,33 +801,52 @@
        * bfd.SessionState is Up, and bfd.RemoteSessionState is Up) and a Poll
        * Sequence is not being transmitted.
        */
-      BFD_DBG ("bfd.RemoteDemand is non-zero, not sending periodic control "
-	       "frame");
+      BFD_DBG ("Remote demand is set, not sending periodic control frame");
       return;
     }
   /* sometimes the wheel expires an event a bit sooner than requested, account
      for that here */
   if (now + bm->wheel_inaccuracy >= bs->tx_timeout_clocks)
     {
-      BFD_DBG ("Send periodic control frame for bs_idx=%lu: %U", bs->bs_idx,
-	       format_bfd_session, bs);
-      vlib_buffer_t *b = bfd_create_frame_to_next_node (vm, bs);
-      if (!b)
+      BFD_DBG ("\nSending periodic control frame: %U", format_bfd_session,
+	       bs);
+      u32 bi;
+      if (vlib_buffer_alloc (vm, &bi, 1) != 1)
 	{
+	  clib_warning ("buffer allocation failure");
 	  return;
 	}
-      bfd_init_control_frame (b, bs);
-      if (POLL_NOT_NEEDED != bs->poll_state)
+      vlib_buffer_t *b = vlib_get_buffer (vm, bi);
+      ASSERT (b->current_data == 0);
+      bfd_init_control_frame (bm, bs, b);
+      switch (bs->poll_state)
 	{
-	  /* here we are either beginning a new poll sequence or retrying .. */
+	case BFD_POLL_NEEDED:
+	  if (now < bs->poll_state_start_or_timeout_clocks)
+	    {
+	      BFD_DBG ("Cannot start a poll sequence yet, need to wait "
+		       "for " BFD_CLK_FMT,
+		       BFD_CLK_PRN (bs->poll_state_start_or_timeout_clocks -
+				    now));
+	      break;
+	    }
+	  bs->poll_state_start_or_timeout_clocks = now;
+	  bfd_set_poll_state (bs, BFD_POLL_IN_PROGRESS);
+	  /* fallthrough */
+	case BFD_POLL_IN_PROGRESS:
+	case BFD_POLL_IN_PROGRESS_AND_QUEUED:
 	  bfd_pkt_set_poll (vlib_buffer_get_current (b));
-	  bs->poll_state = POLL_IN_PROGRESS;
 	  BFD_DBG ("Setting poll bit in packet, bs_idx=%u", bs->bs_idx);
+	  break;
+	case BFD_POLL_NOT_NEEDED:
+	  /* fallthrough */
+	  break;
 	}
       bfd_add_auth_section (b, bs);
       bfd_add_transport_layer (vm, b, bs);
       bs->last_tx_clocks = now;
       bfd_calc_next_tx (bm, bs, now);
+      bfd_create_frame_to_next_node (vm, bs, bi);
     }
   else
     {
@@ -664,15 +854,14 @@
 	("No need to send control frame now, now is %lu, tx_timeout is %lu",
 	 now, bs->tx_timeout_clocks);
     }
-  bfd_set_timer (bm, bs, now, handling_wakeup);
 }
 
 void
 bfd_init_final_control_frame (vlib_main_t * vm, vlib_buffer_t * b,
-			      bfd_session_t * bs)
+			      bfd_main_t * bm, bfd_session_t * bs)
 {
   BFD_DBG ("Send final control frame for bs_idx=%lu", bs->bs_idx);
-  bfd_init_control_frame (b, bs);
+  bfd_init_control_frame (bm, bs, b);
   bfd_pkt_set_final (vlib_buffer_get_current (b));
   bfd_add_auth_section (b, bs);
   bfd_add_transport_layer (vm, b, bs);
@@ -681,7 +870,7 @@
    * RFC allows to include changes in final frame, so if there were any
    * pending, we already did that, thus we can clear any pending poll needs
    */
-  bs->poll_state = POLL_NOT_NEEDED;
+  bfd_set_poll_state (bs, BFD_POLL_NOT_NEEDED);
 }
 
 static void
@@ -703,7 +892,16 @@
        * since it is no longer required to maintain previous session state)
        * and then can transmit at its own rate.
        */
-      bfd_set_remote_required_min_rx (bm, bs, now, 1, handling_wakeup);
+      bfd_set_remote_required_min_rx (bm, bs, now, 1);
+    }
+  else if (bs->echo &&
+	   bs->echo_last_rx_clocks +
+	   bs->echo_transmit_interval_clocks * bs->local_detect_mult <=
+	   now + bm->wheel_inaccuracy)
+    {
+      BFD_DBG ("Echo rx timeout, session goes down");
+      bfd_set_diag (bs, BFD_DIAG_CODE_echo_failed);
+      bfd_set_state (bm, bs, BFD_STATE_down, handling_wakeup);
     }
 }
 
@@ -721,11 +919,31 @@
       bfd_send_periodic (vm, rt, bm, bs, now, 1);
       break;
     case BFD_STATE_init:
-      /* fallthrough */
-    case BFD_STATE_up:
       bfd_check_rx_timeout (bm, bs, now, 1);
       bfd_send_periodic (vm, rt, bm, bs, now, 1);
       break;
+    case BFD_STATE_up:
+      bfd_check_rx_timeout (bm, bs, now, 1);
+      if (BFD_POLL_NOT_NEEDED == bs->poll_state && !bs->echo &&
+	  bfd_is_echo_possible (bs))
+	{
+	  /* switch on echo function as main detection method now */
+	  BFD_DBG ("Switching on echo function, bs_idx=%u", bs->bs_idx);
+	  bs->echo = 1;
+	  bs->echo_last_rx_clocks = now;
+	  bs->echo_tx_timeout_clocks = now;
+	  bfd_set_effective_required_min_rx (bm, bs, now,
+					     clib_max
+					     (bm->min_required_min_rx_while_echo_clocks,
+					      bs->config_required_min_rx_clocks));
+	  bfd_set_poll_state (bs, BFD_POLL_NEEDED);
+	}
+      bfd_send_periodic (vm, rt, bm, bs, now, 1);
+      if (bs->echo)
+	{
+	  bfd_send_echo (vm, rt, bm, bs, now, 1);
+	}
+      break;
     }
 }
 
@@ -822,6 +1040,7 @@
 	  {
 	    bfd_session_t *bs = pool_elt_at_index (bm->sessions, bs_idx);
 	    bfd_on_timeout (vm, rt, bm, bs, now);
+	    bfd_set_timer (bm, bs, now, 1);
 	  }
       }
       if (expired)
@@ -894,7 +1113,9 @@
   bm->cpu_cps = vm->clib_time.clocks_per_second;
   BFD_DBG ("cps is %.2f", bm->cpu_cps);
   bm->default_desired_min_tx_clocks =
-    bfd_usec_to_clocks (bm, BFD_DEFAULT_DESIRED_MIN_TX_US);
+    bfd_usec_to_clocks (bm, BFD_DEFAULT_DESIRED_MIN_TX_USEC);
+  bm->min_required_min_rx_while_echo_clocks =
+    bfd_usec_to_clocks (bm, BFD_REQUIRED_MIN_RX_USEC_WHILE_ECHO);
   const u64 now = clib_cpu_time_now ();
   timing_wheel_init (&bm->wheel, now, bm->cpu_cps);
   bm->wheel_inaccuracy = 2 << bm->wheel.log2_clocks_per_bin;
@@ -912,14 +1133,28 @@
 VLIB_INIT_FUNCTION (bfd_main_init);
 
 bfd_session_t *
-bfd_get_session (bfd_main_t * bm, bfd_transport_t t)
+bfd_get_session (bfd_main_t * bm, bfd_transport_e t)
 {
   bfd_session_t *result;
   pool_get (bm->sessions, result);
   memset (result, 0, sizeof (*result));
   result->bs_idx = result - bm->sessions;
   result->transport = t;
-  result->local_discr = random_u32 (&bm->random_seed);
+  const unsigned limit = 1000;
+  unsigned counter = 0;
+  do
+    {
+      result->local_discr = random_u32 (&bm->random_seed);
+      if (counter > limit)
+	{
+	  clib_warning ("Couldn't allocate unused session discriminator even "
+			"after %u tries!", limit);
+	  pool_put (bm->sessions, result);
+	  return NULL;
+	}
+      ++counter;
+    }
+  while (hash_get (bm->session_by_disc, result->local_discr));
   bfd_set_defaults (bm, result);
   hash_set (bm->session_by_disc, result->local_discr, result->bs_idx);
   return result;
@@ -1372,29 +1607,45 @@
     bfd_usec_to_clocks (bm, clib_net_to_host_u32 (pkt->des_min_tx));
   bs->remote_detect_mult = pkt->head.detect_mult;
   bfd_set_remote_required_min_rx (bm, bs, now,
-				  clib_net_to_host_u32 (pkt->req_min_rx), 0);
-  /* FIXME
-     If the Required Min Echo RX Interval field is zero, the
-     transmission of Echo packets, if any, MUST cease.
-
-     If a Poll Sequence is being transmitted by the local system and
-     the Final (F) bit in the received packet is set, the Poll Sequence
-     MUST be terminated.
-   */
+				  clib_net_to_host_u32 (pkt->req_min_rx));
+  bfd_set_remote_required_min_echo_rx (bm, bs, now,
+				       clib_net_to_host_u32
+				       (pkt->req_min_echo_rx));
   /* FIXME 6.8.2 */
   /* FIXME 6.8.4 */
-  if (bs->poll_state == POLL_IN_PROGRESS && bfd_pkt_get_final (pkt))
+  if (bfd_pkt_get_final (pkt))
     {
-      bs->poll_state = POLL_NOT_NEEDED;
-      BFD_DBG ("Poll sequence terminated, bs_idx=%u", bs->bs_idx);
-      if (BFD_STATE_up == bs->local_state)
+      if (BFD_POLL_IN_PROGRESS == bs->poll_state)
 	{
-	  bfd_set_effective_required_min_rx (bm, bs, now,
-					     bs->config_required_min_rx_clocks);
-	  bfd_recalc_detection_time (bm, bs);
-	  bfd_set_timer (bm, bs, now, 0);
+	  BFD_DBG ("Poll sequence terminated, bs_idx=%u", bs->bs_idx);
+	  bfd_set_poll_state (bs, BFD_POLL_NOT_NEEDED);
+	  if (BFD_STATE_up == bs->local_state)
+	    {
+	      bfd_set_effective_required_min_rx (bm, bs, now,
+						 clib_max (bs->echo *
+							   bm->min_required_min_rx_while_echo_clocks,
+							   bs->config_required_min_rx_clocks));
+	    }
+	}
+      else if (BFD_POLL_IN_PROGRESS_AND_QUEUED == bs->poll_state)
+	{
+	  /*
+	   * next poll sequence must be delayed by at least the round trip
+	   * time, so calculate that here
+	   */
+	  BFD_DBG ("Next poll sequence can commence in " BFD_CLK_FMT,
+		   BFD_CLK_PRN (now -
+				bs->poll_state_start_or_timeout_clocks));
+	  bs->poll_state_start_or_timeout_clocks =
+	    now + (now - bs->poll_state_start_or_timeout_clocks);
+	  BFD_DBG
+	    ("Poll sequence terminated, but another is needed, bs_idx=%u",
+	     bs->bs_idx);
+	  bfd_set_poll_state (bs, BFD_POLL_NEEDED);
 	}
     }
+  bfd_calc_next_tx (bm, bs, now);
+  bfd_set_timer (bm, bs, now, 0);
   if (BFD_STATE_admin_down == bs->local_state)
     {
       BFD_DBG ("Session is admin-down, ignoring packet, bs_idx=%u",
@@ -1435,52 +1686,75 @@
     }
 }
 
-static const char *
-bfd_poll_state_string (bfd_poll_state_e state)
+int
+bfd_consume_echo_pkt (bfd_main_t * bm, vlib_buffer_t * b)
 {
-  switch (state)
+  bfd_echo_pkt_t *pkt = NULL;
+  if (b->current_length != sizeof (*pkt))
     {
-#define F(x)     \
-  case POLL_##x: \
-    return "POLL_" #x;
-      foreach_bfd_poll_state (F)
-#undef F
+      return 0;
     }
-  return "UNKNOWN";
+  pkt = vlib_buffer_get_current (b);
+  bfd_session_t *bs = bfd_find_session_by_disc (bm, pkt->discriminator);
+  if (!bs)
+    {
+      return 0;
+    }
+  BFD_DBG ("Scanning bfd echo packet, bs_idx=%d", bs->bs_idx);
+  u64 checksum =
+    bfd_calc_echo_checksum (bs->local_discr, pkt->expire_time_clocks,
+			    bs->echo_secret);
+  if (checksum != pkt->checksum)
+    {
+      BFD_DBG ("Invalid echo packet, checksum mismatch");
+      return 1;
+    }
+  u64 now = clib_cpu_time_now ();
+  if (pkt->expire_time_clocks < now)
+    {
+      BFD_DBG ("Stale packet received, expire time %lu < now %lu",
+	       pkt->expire_time_clocks, now);
+    }
+  else
+    {
+      bs->echo_last_rx_clocks = now;
+    }
+  return 1;
 }
 
 u8 *
 format_bfd_session (u8 * s, va_list * args)
 {
   const bfd_session_t *bs = va_arg (*args, bfd_session_t *);
-  s = format (s, "BFD(%u): bfd.SessionState=%s, "
-	      "bfd.RemoteSessionState=%s, "
-	      "bfd.LocalDiscr=%u, "
-	      "bfd.RemoteDiscr=%u, "
-	      "bfd.LocalDiag=%s, "
-	      "bfd.DesiredMinTxInterval=%u, "
-	      "bfd.RequiredMinRxInterval=%u, "
-	      "bfd.RequiredMinEchoRxInterval=%u, "
-	      "bfd.RemoteMinRxInterval=%u, "
-	      "bfd.DemandMode=%s, "
-	      "bfd.RemoteDemandMode=%s, "
-	      "bfd.DetectMult=%u, "
-	      "Auth: {local-seq-num=%u, "
-	      "remote-seq-num=%u, "
-	      "is-delayed=%s, "
-	      "curr-key=%U, "
-	      "next-key=%U},"
-	      "poll-state: %s",
+  uword indent = format_get_indent (s);
+  s = format (s, "bs_idx=%u local-state=%s remote-state=%s\n"
+	      "%Ulocal-discriminator=%u remote-discriminator=%u\n"
+	      "%Ulocal-diag=%s echo-active=%s\n"
+	      "%Udesired-min-tx=%u required-min-rx=%u\n"
+	      "%Urequired-min-echo-rx=%u detect-mult=%u\n"
+	      "%Uremote-min-rx=%u remote-min-echo-rx=%u\n"
+	      "%Uremote-demand=%s poll-state=%s\n"
+	      "%Uauth: local-seq-num=%u remote-seq-num=%u\n"
+	      "%U      is-delayed=%s\n"
+	      "%U      curr-key=%U\n"
+	      "%U      next-key=%U",
 	      bs->bs_idx, bfd_state_string (bs->local_state),
-	      bfd_state_string (bs->remote_state), bs->local_discr,
-	      bs->remote_discr, bfd_diag_code_string (bs->local_diag),
+	      bfd_state_string (bs->remote_state), format_white_space, indent,
+	      bs->local_discr, bs->remote_discr, format_white_space, indent,
+	      bfd_diag_code_string (bs->local_diag),
+	      (bs->echo ? "yes" : "no"), format_white_space, indent,
 	      bs->config_desired_min_tx_usec, bs->config_required_min_rx_usec,
-	      1, bs->remote_min_rx_usec, (bs->local_demand ? "yes" : "no"),
-	      (bs->remote_demand ? "yes" : "no"), bs->local_detect_mult,
-	      bs->auth.local_seq_number, bs->auth.remote_seq_number,
-	      (bs->auth.is_delayed ? "yes" : "no"), format_bfd_auth_key,
-	      bs->auth.curr_key, format_bfd_auth_key, bs->auth.next_key,
-	      bfd_poll_state_string (bs->poll_state));
+	      format_white_space, indent, 1, bs->local_detect_mult,
+	      format_white_space, indent, bs->remote_min_rx_usec,
+	      bs->remote_min_echo_rx_usec, format_white_space, indent,
+	      (bs->remote_demand ? "yes" : "no"),
+	      bfd_poll_state_string (bs->poll_state), format_white_space,
+	      indent, bs->auth.local_seq_number, bs->auth.remote_seq_number,
+	      format_white_space, indent,
+	      (bs->auth.is_delayed ? "yes" : "no"), format_white_space,
+	      indent, format_bfd_auth_key, bs->auth.curr_key,
+	      format_white_space, indent, format_bfd_auth_key,
+	      bs->auth.next_key);
   return s;
 }
 
@@ -1537,7 +1811,7 @@
       bs->auth.is_delayed = 0;
     }
   ++key->use_count;
-  BFD_DBG ("Session auth modified: %U", format_bfd_session, bs);
+  BFD_DBG ("\nSession auth modified: %U", format_bfd_session, bs);
   return 0;
 }
 
@@ -1571,7 +1845,7 @@
       --bs->auth.next_key->use_count;
       bs->auth.next_key = NULL;
     }
-  BFD_DBG ("Session auth modified: %U", format_bfd_session, bs);
+  BFD_DBG ("\nSession auth modified: %U", format_bfd_session, bs);
   return 0;
 #else
   clib_warning ("SSL missing, cannot deactivate BFD authentication");
@@ -1588,10 +1862,10 @@
       bs->config_desired_min_tx_usec != desired_min_tx_usec ||
       bs->config_required_min_rx_usec != required_min_rx_usec)
     {
-      BFD_DBG ("Changing session params: %U", format_bfd_session, bs);
+      BFD_DBG ("\nChanging session params: %U", format_bfd_session, bs);
       switch (bs->poll_state)
 	{
-	case POLL_NOT_NEEDED:
+	case BFD_POLL_NOT_NEEDED:
 	  if (BFD_STATE_up == bs->local_state ||
 	      BFD_STATE_init == bs->local_state)
 	    {
@@ -1599,21 +1873,26 @@
 	      if (bs->config_desired_min_tx_usec != desired_min_tx_usec ||
 		  bs->config_required_min_rx_usec != required_min_rx_usec)
 		{
-		  bs->poll_state = POLL_NEEDED;
-		  BFD_DBG ("Set poll state=%s, bs_idx=%u",
-			   bfd_poll_state_string (bs->poll_state),
-			   bs->bs_idx);
+		  bfd_set_poll_state (bs, BFD_POLL_NEEDED);
 		}
 	    }
 	  break;
-	case POLL_NEEDED:
-	  /* nothing to do */
+	case BFD_POLL_NEEDED:
+	case BFD_POLL_IN_PROGRESS_AND_QUEUED:
+	  /*
+	   * nothing to do - will be handled in the future poll which is
+	   * already scheduled for execution
+	   */
 	  break;
-	case POLL_IN_PROGRESS:
-	  /* can't change params now ... */
-	  BFD_ERR ("Poll in progress, cannot change params for session with "
-		   "bs_idx=%u", bs->bs_idx);
-	  return VNET_API_ERROR_BFD_EAGAIN;
+	case BFD_POLL_IN_PROGRESS:
+	  /* poll sequence is not needed for detect multiplier change */
+	  if (bs->config_desired_min_tx_usec != desired_min_tx_usec ||
+	      bs->config_required_min_rx_usec != required_min_rx_usec)
+	    {
+	      BFD_DBG ("Poll in progress, queueing extra poll, bs_idx=%u",
+		       bs->bs_idx);
+	      bfd_set_poll_state (bs, BFD_POLL_IN_PROGRESS_AND_QUEUED);
+	    }
 	}
 
       bs->local_detect_mult = detect_mult;
@@ -1623,7 +1902,7 @@
       bs->config_required_min_rx_usec = required_min_rx_usec;
       bs->config_required_min_rx_clocks =
 	bfd_usec_to_clocks (bm, required_min_rx_usec);
-      BFD_DBG ("Changed session params: %U", format_bfd_session, bs);
+      BFD_DBG ("\nChanged session params: %U", format_bfd_session, bs);
 
       vlib_process_signal_event (bm->vlib_main, bm->bfd_process_node_index,
 				 BFD_EVENT_CONFIG_CHANGED, bs->bs_idx);
diff --git a/src/vnet/bfd/bfd_main.h b/src/vnet/bfd/bfd_main.h
index 14a54d6..d8063f9 100644
--- a/src/vnet/bfd/bfd_main.h
+++ b/src/vnet/bfd/bfd_main.h
@@ -24,17 +24,6 @@
 #include <vnet/bfd/bfd_protocol.h>
 #include <vnet/bfd/bfd_udp.h>
 
-#define foreach_bfd_transport(F) \
-  F (UDP4, "ip4-rewrite")        \
-  F (UDP6, "ip6-rewrite")
-
-typedef enum
-{
-#define F(t, n) BFD_TRANSPORT_##t,
-  foreach_bfd_transport (F)
-#undef F
-} bfd_transport_t;
-
 #define foreach_bfd_mode(F) \
   F (asynchronous)          \
   F (demand)
@@ -64,14 +53,15 @@
   bfd_auth_type_e auth_type;
 } bfd_auth_key_t;
 
-#define foreach_bfd_poll_state(F)\
-  F(NOT_NEEDED)\
-F(NEEDED)\
-F(IN_PROGRESS)
+#define foreach_bfd_poll_state(F) \
+  F (NOT_NEEDED)                  \
+  F (NEEDED)                      \
+  F (IN_PROGRESS)                 \
+  F (IN_PROGRESS_AND_QUEUED)
 
 typedef enum
 {
-#define F(x) POLL_##x,
+#define F(x) BFD_POLL_##x,
   foreach_bfd_poll_state (F)
 #undef F
 } bfd_poll_state_e;
@@ -120,21 +110,27 @@
   /* remote min rx interval (clocks) */
   u64 remote_min_rx_clocks;
 
+  /* remote min echo rx interval (microseconds) */
+  u64 remote_min_echo_rx_usec;
+
+  /* remote min echo rx interval (clocks) */
+  u64 remote_min_echo_rx_clocks;
+
   /* remote desired min tx interval (clocks) */
   u64 remote_desired_min_tx_clocks;
 
   /* configured detect multiplier */
   u8 local_detect_mult;
 
-  /* 1 if in demand mode, 0 otherwise */
-  u8 local_demand;
-
   /* 1 if remote system sets demand mode, 0 otherwise */
   u8 remote_demand;
 
   /* remote detect multiplier */
   u8 remote_detect_mult;
 
+  /* 1 is echo function is active, 0 otherwise */
+  u8 echo;
+
   /* set to value of timer in timing wheel, 0 if never set */
   u64 wheel_time_clocks;
 
@@ -150,12 +146,33 @@
   /* timestamp of last packet received */
   u64 last_rx_clocks;
 
+  /* transmit interval for echo packets */
+  u64 echo_transmit_interval_clocks;
+
+  /* next time at which to transmit echo packet */
+  u64 echo_tx_timeout_clocks;
+
+  /* timestamp of last echo packet transmitted */
+  u64 echo_last_tx_clocks;
+
+  /* timestamp of last echo packet received */
+  u64 echo_last_rx_clocks;
+
+  /* secret used for calculating/checking checksum of echo packets */
+  u32 echo_secret;
+
   /* detection time */
   u64 detection_time_clocks;
 
   /* state info regarding poll sequence */
   bfd_poll_state_e poll_state;
 
+  /*
+   * helper for delayed poll sequence - marks either start of running poll
+   * sequence or timeout, after which we can start the next poll sequnce
+   */
+  u64 poll_state_start_or_timeout_clocks;
+
   /* authentication information */
   struct
   {
@@ -191,7 +208,7 @@
   } auth;
 
   /* transport type for this session */
-  bfd_transport_t transport;
+  bfd_transport_e transport;
 
   /* union of transport-specific data */
   union
@@ -227,6 +244,9 @@
   /* default desired min tx in clocks */
   u64 default_desired_min_tx_clocks;
 
+  /* minimum required min rx while echo function is active - clocks */
+  u64 min_required_min_rx_while_echo_clocks;
+
   /* for generating random numbers */
   u32 random_seed;
 
@@ -268,36 +288,54 @@
   BFD_EVENT_CONFIG_CHANGED,
 } bfd_process_event_e;
 
-u8 *bfd_input_format_trace (u8 * s, va_list * args);
+/* echo packet structure */
+/* *INDENT-OFF* */
+typedef CLIB_PACKED (struct {
+  /* local discriminator */
+  u32 discriminator;
+  /* expire time of this packet - clocks */
+  u64 expire_time_clocks;
+  /* checksum - based on discriminator, local secret and expire time */
+  u64 checksum;
+}) bfd_echo_pkt_t;
+/* *INDENT-ON* */
 
-bfd_session_t *bfd_get_session (bfd_main_t * bm, bfd_transport_t t);
+u8 *bfd_input_format_trace (u8 * s, va_list * args);
+bfd_session_t *bfd_get_session (bfd_main_t * bm, bfd_transport_e t);
 void bfd_put_session (bfd_main_t * bm, bfd_session_t * bs);
 bfd_session_t *bfd_find_session_by_idx (bfd_main_t * bm, uword bs_idx);
 bfd_session_t *bfd_find_session_by_disc (bfd_main_t * bm, u32 disc);
 void bfd_session_start (bfd_main_t * bm, bfd_session_t * bs);
 void bfd_consume_pkt (bfd_main_t * bm, const bfd_pkt_t * bfd, u32 bs_idx);
+int bfd_consume_echo_pkt (bfd_main_t * bm, vlib_buffer_t * b);
 int bfd_verify_pkt_common (const bfd_pkt_t * pkt);
 int bfd_verify_pkt_auth (const bfd_pkt_t * pkt, u16 pkt_size,
 			 bfd_session_t * bs);
 void bfd_event (bfd_main_t * bm, bfd_session_t * bs);
 void bfd_init_final_control_frame (vlib_main_t * vm, vlib_buffer_t * b,
-				   bfd_session_t * bs);
+				   bfd_main_t * bm, bfd_session_t * bs);
 u8 *format_bfd_session (u8 * s, va_list * args);
 void bfd_session_set_flags (bfd_session_t * bs, u8 admin_up_down);
 unsigned bfd_auth_type_supported (bfd_auth_type_e auth_type);
 vnet_api_error_t bfd_auth_activate (bfd_session_t * bs, u32 conf_key_id,
 				    u8 bfd_key_id, u8 is_delayed);
 vnet_api_error_t bfd_auth_deactivate (bfd_session_t * bs, u8 is_delayed);
-vnet_api_error_t
-bfd_session_set_params (bfd_main_t * bm, bfd_session_t * bs,
-			u32 desired_min_tx_usec,
-			u32 required_min_rx_usec, u8 detect_mult);
+vnet_api_error_t bfd_session_set_params (bfd_main_t * bm, bfd_session_t * bs,
+					 u32 desired_min_tx_usec,
+					 u32 required_min_rx_usec,
+					 u8 detect_mult);
 
 #define USEC_PER_MS 1000LL
 #define USEC_PER_SECOND (1000 * USEC_PER_MS)
 
 /* default, slow transmission interval for BFD packets, per spec at least 1s */
-#define BFD_DEFAULT_DESIRED_MIN_TX_US USEC_PER_SECOND
+#define BFD_DEFAULT_DESIRED_MIN_TX_USEC USEC_PER_SECOND
+
+/*
+ * minimum required min rx set locally when echo function is used, per spec
+ * should be set to at least 1s
+ */
+#define BFD_REQUIRED_MIN_RX_USEC_WHILE_ECHO USEC_PER_SECOND
 
 #endif /* __included_bfd_main_h__ */
 
diff --git a/src/vnet/bfd/bfd_udp.c b/src/vnet/bfd/bfd_udp.c
index 8519009..146faad 100644
--- a/src/vnet/bfd/bfd_udp.c
+++ b/src/vnet/bfd/bfd_udp.c
@@ -27,6 +27,9 @@
 #include <vnet/ip/ip6_packet.h>
 #include <vnet/adj/adj.h>
 #include <vnet/adj/adj_nbr.h>
+#include <vnet/dpo/receive_dpo.h>
+#include <vnet/fib/fib_entry.h>
+#include <vnet/fib/fib_table.h>
 #include <vnet/bfd/bfd_debug.h>
 #include <vnet/bfd/bfd_udp.h>
 #include <vnet/bfd/bfd_main.h>
@@ -38,6 +41,12 @@
   /* hashmap - bfd session index by bfd key - used for CLI/API lookup, where
    * discriminator is unknown */
   mhash_t bfd_session_idx_by_bfd_key;
+  /* convenience variable */
+  vnet_main_t *vnet_main;
+  /* flag indicating whether echo_source_sw_if_index holds a valid value */
+  int echo_source_is_set;
+  /* loopback interface used to get echo source ip */
+  u32 echo_source_sw_if_index;
 } bfd_udp_main_t;
 
 static vlib_node_registration_t bfd_udp4_input_node;
@@ -47,6 +56,80 @@
 
 bfd_udp_main_t bfd_udp_main;
 
+vnet_api_error_t
+bfd_udp_set_echo_source (u32 sw_if_index)
+{
+  vnet_sw_interface_t *sw_if =
+    vnet_get_sw_interface_safe (bfd_udp_main.vnet_main,
+				bfd_udp_main.echo_source_sw_if_index);
+  if (sw_if)
+    {
+      bfd_udp_main.echo_source_sw_if_index = sw_if_index;
+      bfd_udp_main.echo_source_is_set = 1;
+      return 0;
+    }
+  return VNET_API_ERROR_BFD_ENOENT;
+}
+
+vnet_api_error_t
+bfd_udp_del_echo_source (u32 sw_if_index)
+{
+  bfd_udp_main.echo_source_sw_if_index = ~0;
+  bfd_udp_main.echo_source_is_set = 0;
+  return 0;
+}
+
+int
+bfd_udp_is_echo_available (bfd_transport_e transport)
+{
+  if (!bfd_udp_main.echo_source_is_set)
+    {
+      return 0;
+    }
+  /*
+   * for the echo to work, we need a loopback interface with at least one
+   * address with netmask length at most 31 (ip4) or 127 (ip6) so that we can
+   * pick an unused address from that subnet
+   */
+  vnet_sw_interface_t *sw_if =
+    vnet_get_sw_interface_safe (bfd_udp_main.vnet_main,
+				bfd_udp_main.echo_source_sw_if_index);
+  if (sw_if && sw_if->flags & VNET_SW_INTERFACE_FLAG_ADMIN_UP)
+    {
+      if (BFD_TRANSPORT_UDP4 == transport)
+	{
+	  ip4_main_t *im = &ip4_main;
+	  ip_interface_address_t *ia = NULL;
+          /* *INDENT-OFF* */
+          foreach_ip_interface_address (&im->lookup_main, ia,
+                                        bfd_udp_main.echo_source_sw_if_index,
+                                        0 /* honor unnumbered */, ({
+                                          if (ia->address_length <= 31)
+                                            {
+                                              return 1;
+                                            }
+                                        }));
+          /* *INDENT-ON* */
+	}
+      else if (BFD_TRANSPORT_UDP6 == transport)
+	{
+	  ip6_main_t *im = &ip6_main;
+	  ip_interface_address_t *ia = NULL;
+          /* *INDENT-OFF* */
+          foreach_ip_interface_address (&im->lookup_main, ia,
+                                        bfd_udp_main.echo_source_sw_if_index,
+                                        0 /* honor unnumbered */, ({
+                                          if (ia->address_length <= 127)
+                                            {
+                                              return 1;
+                                            }
+                                        }));
+          /* *INDENT-ON* */
+	}
+    }
+  return 0;
+}
+
 static u16
 bfd_udp_bs_idx_to_sport (u32 bs_idx)
 {
@@ -61,9 +144,78 @@
   return 49152 + bs_idx % (65535 - 49152 + 1);
 }
 
-void
+static void
+lol ()
+{
+}
+
+int
+bfd_udp_get_echo_src_ip4 (ip4_address_t * addr)
+{
+  if (!bfd_udp_main.echo_source_is_set)
+    {
+      BFD_ERR ("cannot find ip4 address, echo source not set");
+      return 0;
+    }
+  ip_interface_address_t *ia = NULL;
+  ip4_main_t *im = &ip4_main;
+
+  /* *INDENT-OFF* */
+  foreach_ip_interface_address (
+      &im->lookup_main, ia, bfd_udp_main.echo_source_sw_if_index,
+      0 /* honor unnumbered */, ({
+        ip4_address_t *x =
+            ip_interface_address_get_address (&im->lookup_main, ia);
+        if (ia->address_length <= 31)
+          {
+            addr->as_u32 = clib_host_to_net_u32 (x->as_u32);
+	    /*
+	     * flip the last bit to get a different address, might be network,
+	     * we don't care ...
+	     */
+	    addr->as_u32 ^= 1;
+            addr->as_u32 = clib_net_to_host_u32 (addr->as_u32);
+            return 1;
+          }
+      }));
+  /* *INDENT-ON* */
+  BFD_ERR ("cannot find ip4 address, no usable address found");
+  return 0;
+}
+
+int
+bfd_udp_get_echo_src_ip6 (ip6_address_t * addr)
+{
+  if (!bfd_udp_main.echo_source_is_set)
+    {
+      BFD_ERR ("cannot find ip6 address, echo source not set");
+      return 0;
+    }
+  ip_interface_address_t *ia = NULL;
+  ip6_main_t *im = &ip6_main;
+
+  /* *INDENT-OFF* */
+  foreach_ip_interface_address (
+      &im->lookup_main, ia, bfd_udp_main.echo_source_sw_if_index,
+      0 /* honor unnumbered */, ({
+        ip6_address_t *x =
+            ip_interface_address_get_address (&im->lookup_main, ia);
+        if (ia->address_length <= 127)
+          {
+            *addr = *x;
+            addr->as_u8[15] ^= 1; /* flip the last bit of the address */
+            lol ();
+            return 1;
+          }
+      }));
+  /* *INDENT-ON* */
+  BFD_ERR ("cannot find ip6 address, no usable address found");
+  return 0;
+}
+
+int
 bfd_add_udp4_transport (vlib_main_t * vm, vlib_buffer_t * b,
-			const bfd_session_t * bs)
+			const bfd_session_t * bs, int is_echo)
 {
   const bfd_udp_session_t *bus = &bs->udp;
   const bfd_udp_key_t *key = &bus->key;
@@ -83,12 +235,24 @@
   headers->ip4.ip_version_and_header_length = 0x45;
   headers->ip4.ttl = 255;
   headers->ip4.protocol = IP_PROTOCOL_UDP;
-  headers->ip4.src_address.as_u32 = key->local_addr.ip4.as_u32;
-  headers->ip4.dst_address.as_u32 = key->peer_addr.ip4.as_u32;
-
   headers->udp.src_port =
     clib_host_to_net_u16 (bfd_udp_bs_idx_to_sport (bs->bs_idx));
-  headers->udp.dst_port = clib_host_to_net_u16 (UDP_DST_PORT_bfd4);
+  if (is_echo)
+    {
+      int rv;
+      if (!(rv = bfd_udp_get_echo_src_ip4 (&headers->ip4.src_address)))
+	{
+	  return rv;
+	}
+      headers->ip4.dst_address.as_u32 = key->local_addr.ip4.as_u32;
+      headers->udp.dst_port = clib_host_to_net_u16 (UDP_DST_PORT_bfd_echo4);
+    }
+  else
+    {
+      headers->ip4.src_address.as_u32 = key->local_addr.ip4.as_u32;
+      headers->ip4.dst_address.as_u32 = key->peer_addr.ip4.as_u32;
+      headers->udp.dst_port = clib_host_to_net_u16 (UDP_DST_PORT_bfd4);
+    }
 
   /* fix ip length, checksum and udp length */
   const u16 ip_length = vlib_buffer_length_in_chain (vm, b);
@@ -98,11 +262,12 @@
 
   const u16 udp_length = ip_length - (sizeof (headers->ip4));
   headers->udp.length = clib_host_to_net_u16 (udp_length);
+  return 1;
 }
 
-void
+int
 bfd_add_udp6_transport (vlib_main_t * vm, vlib_buffer_t * b,
-			const bfd_session_t * bs)
+			const bfd_session_t * bs, int is_echo)
 {
   const bfd_udp_session_t *bus = &bs->udp;
   const bfd_udp_key_t *key = &bus->key;
@@ -123,14 +288,28 @@
     clib_host_to_net_u32 (0x6 << 28);
   headers->ip6.hop_limit = 255;
   headers->ip6.protocol = IP_PROTOCOL_UDP;
-  clib_memcpy (&headers->ip6.src_address, &key->local_addr.ip6,
-	       sizeof (headers->ip6.src_address));
-  clib_memcpy (&headers->ip6.dst_address, &key->peer_addr.ip6,
-	       sizeof (headers->ip6.dst_address));
-
   headers->udp.src_port =
     clib_host_to_net_u16 (bfd_udp_bs_idx_to_sport (bs->bs_idx));
-  headers->udp.dst_port = clib_host_to_net_u16 (UDP_DST_PORT_bfd6);
+  if (is_echo)
+    {
+      int rv;
+      if (!(rv = bfd_udp_get_echo_src_ip6 (&headers->ip6.src_address)))
+	{
+	  return rv;
+	}
+      clib_memcpy (&headers->ip6.dst_address, &key->local_addr.ip6,
+		   sizeof (headers->ip6.dst_address));
+
+      headers->udp.dst_port = clib_host_to_net_u16 (UDP_DST_PORT_bfd_echo6);
+    }
+  else
+    {
+      clib_memcpy (&headers->ip6.src_address, &key->local_addr.ip6,
+		   sizeof (headers->ip6.src_address));
+      clib_memcpy (&headers->ip6.dst_address, &key->peer_addr.ip6,
+		   sizeof (headers->ip6.dst_address));
+      headers->udp.dst_port = clib_host_to_net_u16 (UDP_DST_PORT_bfd6);
+    }
 
   /* fix ip payload length and udp length */
   const u16 udp_length =
@@ -147,6 +326,7 @@
     {
       headers->udp.checksum = 0xffff;
     }
+  return 1;
 }
 
 static bfd_session_t *
@@ -182,12 +362,17 @@
 			      bfd_session_t ** bs_out)
 {
   /* get a pool entry and if we end up not needing it, give it back */
-  bfd_transport_t t = BFD_TRANSPORT_UDP4;
+  bfd_transport_e t = BFD_TRANSPORT_UDP4;
   if (!ip46_address_is_ip4 (local_addr))
     {
       t = BFD_TRANSPORT_UDP6;
     }
   bfd_session_t *bs = bfd_get_session (bum->bfd_main, t);
+  if (!bs)
+    {
+      bfd_put_session (bum->bfd_main, bs);
+      return VNET_API_ERROR_BFD_EAGAIN;
+    }
   bfd_udp_session_t *bus = &bs->udp;
   memset (bus, 0, sizeof (*bus));
   bfd_udp_key_t *key = &bus->key;
@@ -213,6 +398,21 @@
       BFD_DBG ("adj_nbr_add_or_lock(FIB_PROTOCOL_IP4, VNET_LINK_IP4, %U, %d) "
 	       "returns %d", format_ip46_address, &key->peer_addr,
 	       IP46_TYPE_ANY, key->sw_if_index, bus->adj_index);
+
+      fib_prefix_t fib_prefix;
+      memset (&fib_prefix, 0, sizeof (fib_prefix));
+      fib_prefix.fp_len = 0;
+      fib_prefix.fp_proto = FIB_PROTOCOL_IP4;
+      fib_prefix.fp_addr = key->local_addr;
+      u32 fib_index = fib_table_find (FIB_PROTOCOL_IP4, 0);	/* FIXME table id 0? */
+      dpo_id_t dpo = DPO_INVALID;
+      dpo_proto_t dproto;
+      dproto = fib_proto_to_dpo (fib_prefix.fp_proto);
+      receive_dpo_add_or_lock (dproto, ~0, NULL, &dpo);
+      fib_table_entry_special_dpo_update (fib_index, &fib_prefix,
+					  FIB_SOURCE_API,
+					  FIB_ENTRY_FLAG_LOCAL, &dpo);
+      dpo_reset (&dpo);
     }
   else
     {
@@ -234,7 +434,7 @@
 			    const ip46_address_t * peer_addr)
 {
   vnet_sw_interface_t *sw_if =
-    vnet_get_sw_interface (vnet_get_main (), sw_if_index);
+    vnet_get_sw_interface_safe (bfd_udp_main.vnet_main, sw_if_index);
   u8 local_ip_valid = 0;
   ip_interface_address_t *ia = NULL;
   if (!sw_if)
@@ -1001,7 +1201,8 @@
 	  const bfd_pkt_t *pkt = vlib_buffer_get_current (b0);
 	  if (bfd_pkt_get_poll (pkt))
 	    {
-	      bfd_init_final_control_frame (vm, b0, bs);
+	      bfd_init_final_control_frame (vm, b0, bfd_udp_main.bfd_main,
+					    bs);
 	      if (is_ipv6)
 		{
 		  vlib_node_increment_counter (vm, bfd_udp6_input_node.index,
@@ -1081,44 +1282,6 @@
 };
 /* *INDENT-ON* */
 
-/**
- * @brief swap the source and destination IP addresses in the packet
- */
-static int
-bfd_echo_address_swap (vlib_buffer_t * b, int is_ipv6)
-{
-  udp_header_t *dummy = NULL;
-  if (is_ipv6)
-    {
-      ip6_header_t *ip6 = NULL;
-      bfd_udp6_find_headers (b, &ip6, &dummy);
-      if (!ip6)
-	{
-	  return 0;
-	}
-      ip6_address_t tmp = ip6->dst_address;
-      ip6->dst_address = ip6->src_address;
-      ip6->src_address = tmp;
-      vlib_buffer_advance (b,
-			   (u8 *) ip6 - (u8 *) vlib_buffer_get_current (b));
-    }
-  else
-    {
-      ip4_header_t *ip4 = NULL;
-      bfd_udp4_find_headers (b, &ip4, &dummy);
-      if (!ip4)
-	{
-	  return 0;
-	}
-      ip4_address_t tmp = ip4->dst_address;
-      ip4->dst_address = ip4->src_address;
-      ip4->src_address = tmp;
-      vlib_buffer_advance (b,
-			   (u8 *) ip4 - (u8 *) vlib_buffer_get_current (b));
-    }
-  return 1;
-}
-
 /*
  * Process a frame of bfd echo packets
  * Expect 1 packet / frame
@@ -1153,7 +1316,12 @@
 	  clib_memcpy (t0->data, vlib_buffer_get_current (b0), len);
 	}
 
-      if (bfd_echo_address_swap (b0, is_ipv6))
+      if (bfd_consume_echo_pkt (bfd_udp_main.bfd_main, b0))
+	{
+	  b0->error = rt->errors[BFD_UDP_ERROR_NONE];
+	  next0 = BFD_UDP_INPUT_NEXT_NORMAL;
+	}
+      else
 	{
 	  /* loop back the packet */
 	  b0->error = rt->errors[BFD_UDP_ERROR_NONE];
@@ -1169,11 +1337,6 @@
 	    }
 	  next0 = BFD_UDP_INPUT_NEXT_REPLY;
 	}
-      else
-	{
-	  b0->error = rt->errors[BFD_UDP_ERROR_BAD];
-	  next0 = BFD_UDP_INPUT_NEXT_NORMAL;
-	}
 
       vlib_set_next_frame_buffer (vm, rt, next0, bi0);
 
@@ -1294,6 +1457,7 @@
   mhash_init (&bfd_udp_main.bfd_session_idx_by_bfd_key, sizeof (uword),
 	      sizeof (bfd_udp_key_t));
   bfd_udp_main.bfd_main = &bfd_main;
+  bfd_udp_main.vnet_main = vnet_get_main ();
   udp_register_dst_port (vm, UDP_DST_PORT_bfd4, bfd_udp4_input_node.index, 1);
   udp_register_dst_port (vm, UDP_DST_PORT_bfd6, bfd_udp6_input_node.index, 0);
   udp_register_dst_port (vm, UDP_DST_PORT_bfd_echo4,
diff --git a/src/vnet/bfd/bfd_udp.h b/src/vnet/bfd/bfd_udp.h
index 502e231..ce2ee3c 100644
--- a/src/vnet/bfd/bfd_udp.h
+++ b/src/vnet/bfd/bfd_udp.h
@@ -22,6 +22,7 @@
 #include <vppinfra/clib.h>
 #include <vnet/adj/adj_types.h>
 #include <vnet/ip/ip6_packet.h>
+#include <vnet/bfd/bfd_api.h>
 
 /* *INDENT-OFF* */
 typedef CLIB_PACKED (struct {
@@ -49,10 +50,17 @@
 
 struct bfd_session_s;
 
-void bfd_add_udp4_transport (vlib_main_t * vm, vlib_buffer_t * b,
-			     const struct bfd_session_s *bs);
-void bfd_add_udp6_transport (vlib_main_t * vm, vlib_buffer_t * b,
-			     const struct bfd_session_s *bs);
+int bfd_add_udp4_transport (vlib_main_t * vm, vlib_buffer_t * b,
+			    const struct bfd_session_s *bs, int is_echo);
+int bfd_add_udp6_transport (vlib_main_t * vm, vlib_buffer_t * b,
+			    const struct bfd_session_s *bs, int is_echo);
+
+/**
+ * @brief check if the bfd udp layer is echo-capable at this time
+ *
+ * @return 1 if available, 0 otherwise
+ */
+int bfd_udp_is_echo_available (bfd_transport_e transport);
 
 #endif /* __included_bfd_udp_h__ */
 
diff --git a/test/bfd.py b/test/bfd.py
index 8bd9f9a..b467cc7 100644
--- a/test/bfd.py
+++ b/test/bfd.py
@@ -152,6 +152,27 @@
 bind_layers(UDP, BFD, dport=BFD.udp_dport)
 
 
+class BFD_vpp_echo(Packet):
+    """ BFD echo packet as used by VPP (non-rfc, as rfc doesn't define one) """
+
+    udp_dport = 3785  #: BFD echo destination port per RFC 5881
+    name = "BFD_VPP_ECHO"
+
+    fields_desc = [
+        BitField("discriminator", 0, 32),
+        BitField("expire_time_clocks", 0, 64),
+        BitField("checksum", 0, 64)
+    ]
+
+    def mysummary(self):
+        return self.sprintf(
+            "BFD_VPP_ECHO(disc=%BFD_VPP_ECHO.discriminator%,"
+            "expire_time_clocks=%BFD_VPP_ECHO.expire_time_clocks%)")
+
+# glue the BFD echo packet class to scapy parser
+bind_layers(UDP, BFD_vpp_echo, dport=BFD_vpp_echo.udp_dport)
+
+
 class VppBFDAuthKey(VppObject):
     """ Represents BFD authentication key in VPP """
 
diff --git a/test/framework.py b/test/framework.py
index 90e0574..3bbd37d 100644
--- a/test/framework.py
+++ b/test/framework.py
@@ -574,7 +574,7 @@
 
     def assert_equal(self, real_value, expected_value, name_or_class=None):
         if name_or_class is None:
-            self.assertEqual(real_value, expected_value, msg)
+            self.assertEqual(real_value, expected_value)
             return
         try:
             msg = "Invalid %s: %d('%s') does not match expected value %d('%s')"
diff --git a/test/test_bfd.py b/test/test_bfd.py
index 68baf83..ce0cca5 100644
--- a/test/test_bfd.py
+++ b/test/test_bfd.py
@@ -6,14 +6,14 @@
 import hashlib
 import binascii
 import time
-from random import randint, shuffle
+from random import randint, shuffle, getrandbits
 from socket import AF_INET, AF_INET6
 from scapy.packet import Raw
 from scapy.layers.l2 import Ether
 from scapy.layers.inet import UDP, IP
 from scapy.layers.inet6 import IPv6
 from bfd import VppBFDAuthKey, BFD, BFDAuthType, VppBFDUDPSession, \
-    BFDDiagCode, BFDState
+    BFDDiagCode, BFDState, BFD_vpp_echo
 from framework import VppTestCase, VppTestRunner
 from vpp_pg_interface import CaptureTimeoutError
 from util import ppp
@@ -266,6 +266,7 @@
         self.my_discriminator = 0
         self.desired_min_tx = 100000
         self.required_min_rx = 100000
+        self.required_min_echo_rx = None
         self.detect_mult = detect_mult
         self.diag = BFDDiagCode.no_diagnostic
         self.your_discriminator = None
@@ -280,24 +281,27 @@
             self.our_seq_number += 1
 
     def update(self, my_discriminator=None, your_discriminator=None,
-               desired_min_tx=None, required_min_rx=None, detect_mult=None,
+               desired_min_tx=None, required_min_rx=None,
+               required_min_echo_rx=None, detect_mult=None,
                diag=None, state=None, auth_type=None):
         """ update BFD parameters associated with session """
-        if my_discriminator:
+        if my_discriminator is not None:
             self.my_discriminator = my_discriminator
-        if your_discriminator:
+        if your_discriminator is not None:
             self.your_discriminator = your_discriminator
-        if required_min_rx:
+        if required_min_rx is not None:
             self.required_min_rx = required_min_rx
-        if desired_min_tx:
+        if required_min_echo_rx is not None:
+            self.required_min_echo_rx = required_min_echo_rx
+        if desired_min_tx is not None:
             self.desired_min_tx = desired_min_tx
-        if detect_mult:
+        if detect_mult is not None:
             self.detect_mult = detect_mult
-        if diag:
+        if diag is not None:
             self.diag = diag
-        if state:
+        if state is not None:
             self.state = state
-        if auth_type:
+        if auth_type is not None:
             self.auth_type = auth_type
 
     def fill_packet_fields(self, packet):
@@ -316,6 +320,11 @@
                 "BFD: setting packet.required_min_rx_interval=%s",
                 self.required_min_rx)
             bfd.required_min_rx_interval = self.required_min_rx
+        if self.required_min_echo_rx:
+            self.test.logger.debug(
+                "BFD: setting packet.required_min_echo_rx=%s",
+                self.required_min_echo_rx)
+            bfd.required_min_echo_rx_interval = self.required_min_echo_rx
         if self.desired_min_tx:
             self.test.logger.debug(
                 "BFD: setting packet.desired_min_tx_interval=%s",
@@ -579,6 +588,10 @@
         super(BFD4TestCase, cls).setUpClass()
         try:
             cls.create_pg_interfaces([0])
+            cls.create_loopback_interfaces([0])
+            cls.loopback0 = cls.lo_interfaces[0]
+            cls.loopback0.config_ip4()
+            cls.loopback0.admin_up()
             cls.pg0.config_ip4()
             cls.pg0.configure_ipv4_neighbors()
             cls.pg0.admin_up()
@@ -646,32 +659,29 @@
         bfd_session_up(self)
         self.test_session.update(required_min_rx=0)
         self.test_session.send_packet()
-        cap = 2 * self.vpp_session.desired_min_tx *\
-            self.test_session.detect_mult
-        time_mark = time.time()
-        count = 0
-        # busy wait here, trying to collect a packet or event, vpp is not
-        # allowed to send packets and the session will timeout first - so the
-        # Up->Down event must arrive before any packets do
-        while time.time() < time_mark + cap / USEC_IN_SEC:
+        for dummy in range(self.test_session.detect_mult):
+            self.sleep(self.vpp_session.required_min_rx / USEC_IN_SEC,
+                       "sleep before transmitting bfd packet")
+            self.test_session.send_packet()
             try:
-                p = wait_for_bfd_packet(
-                    self, timeout=0,
-                    pcap_time_min=time_mark - self.vpp_clock_offset)
+                p = wait_for_bfd_packet(self, timeout=0)
                 self.logger.error(ppp("Received unexpected packet:", p))
-                count += 1
             except CaptureTimeoutError:
                 pass
-            events = self.vapi.collect_events()
-            if len(events) > 0:
-                verify_event(self, events[0], BFDState.down)
-                break
-        self.assert_equal(count, 0, "number of packets received")
+        self.assert_equal(
+            len(self.vapi.collect_events()), 0, "number of bfd events")
+        self.test_session.update(required_min_rx=100000)
+        for dummy in range(3):
+            self.test_session.send_packet()
+            wait_for_bfd_packet(
+                self, timeout=self.test_session.required_min_rx / USEC_IN_SEC)
+        self.assert_equal(
+            len(self.vapi.collect_events()), 0, "number of bfd events")
 
     def test_conn_down(self):
         """ verify session goes down after inactivity """
         bfd_session_up(self)
-        detection_time = self.vpp_session.detect_mult *\
+        detection_time = self.test_session.detect_mult *\
             self.vpp_session.required_min_rx / USEC_IN_SEC
         self.sleep(detection_time, "waiting for BFD session time-out")
         e = self.vapi.wait_for_event(1, "bfd_udp_session_details")
@@ -799,7 +809,7 @@
         before = time.time()
         e = self.vapi.wait_for_event(1, "bfd_udp_session_details")
         after = time.time()
-        detection_time = self.vpp_session.detect_mult *\
+        detection_time = self.test_session.detect_mult *\
             self.vpp_session.required_min_rx / USEC_IN_SEC
         self.assert_in_range(after - before,
                              0.9 * detection_time,
@@ -830,6 +840,71 @@
         self.assertNotIn("P", p.sprintf("%BFD.flags%"),
                          "Poll bit not set in BFD packet")
 
+    def test_queued_poll(self):
+        """ test poll sequence queueing """
+        bfd_session_up(self)
+        p = wait_for_bfd_packet(self)
+        self.vpp_session.modify_parameters(
+            required_min_rx=2 * self.vpp_session.required_min_rx)
+        p = wait_for_bfd_packet(self)
+        poll_sequence_start = time.time()
+        poll_sequence_length_min = 0.5
+        send_final_after = time.time() + poll_sequence_length_min
+        # poll bit needs to be set
+        self.assertIn("P", p.sprintf("%BFD.flags%"),
+                      "Poll bit not set in BFD packet")
+        self.assert_equal(p[BFD].required_min_rx_interval,
+                          self.vpp_session.required_min_rx,
+                          "BFD required min rx interval")
+        self.vpp_session.modify_parameters(
+            required_min_rx=2 * self.vpp_session.required_min_rx)
+        # 2nd poll sequence should be queued now
+        # don't send the reply back yet, wait for some time to emulate
+        # longer round-trip time
+        packet_count = 0
+        while time.time() < send_final_after:
+            self.test_session.send_packet()
+            p = wait_for_bfd_packet(self)
+            self.assert_equal(len(self.vapi.collect_events()), 0,
+                              "number of bfd events")
+            self.assert_equal(p[BFD].required_min_rx_interval,
+                              self.vpp_session.required_min_rx,
+                              "BFD required min rx interval")
+            packet_count += 1
+            # poll bit must be set
+            self.assertIn("P", p.sprintf("%BFD.flags%"),
+                          "Poll bit not set in BFD packet")
+        final = self.test_session.create_packet()
+        final[BFD].flags = "F"
+        self.test_session.send_packet(final)
+        # finish 1st with final
+        poll_sequence_length = time.time() - poll_sequence_start
+        # vpp must wait for some time before starting new poll sequence
+        poll_no_2_started = False
+        for dummy in range(2 * packet_count):
+            p = wait_for_bfd_packet(self)
+            self.assert_equal(len(self.vapi.collect_events()), 0,
+                              "number of bfd events")
+            if "P" in p.sprintf("%BFD.flags%"):
+                poll_no_2_started = True
+                if time.time() < poll_sequence_start + poll_sequence_length:
+                    raise Exception("VPP started 2nd poll sequence too soon")
+                final = self.test_session.create_packet()
+                final[BFD].flags = "F"
+                self.test_session.send_packet(final)
+                break
+            else:
+                self.test_session.send_packet()
+        self.assertTrue(poll_no_2_started, "2nd poll sequence not performed")
+        # finish 2nd with final
+        final = self.test_session.create_packet()
+        final[BFD].flags = "F"
+        self.test_session.send_packet(final)
+        p = wait_for_bfd_packet(self)
+        # poll bit must not be set
+        self.assertNotIn("P", p.sprintf("%BFD.flags%"),
+                         "Poll bit set in BFD packet")
+
     def test_no_periodic_if_remote_demand(self):
         """ no periodic frames outside poll sequence if remote demand set """
         bfd_session_up(self)
@@ -868,7 +943,7 @@
         echo_packet = (Ether(src=self.pg0.remote_mac,
                              dst=self.pg0.local_mac) /
                        IP(src=self.pg0.remote_ip4,
-                          dst=self.pg0.local_ip4) /
+                          dst=self.pg0.remote_ip4) /
                        UDP(dport=BFD.udp_dport_echo) /
                        Raw("this should be looped back"))
         for dummy in range(echo_packet_count):
@@ -887,18 +962,236 @@
             self.assert_equal(self.pg0.local_mac, ether.src, "Source MAC")
             ip = p[IP]
             self.assert_equal(self.pg0.remote_ip4, ip.dst, "Destination IP")
-            self.assert_equal(self.pg0.local_ip4, ip.src, "Destination IP")
+            self.assert_equal(self.pg0.remote_ip4, ip.src, "Destination IP")
             udp = p[UDP]
             self.assert_equal(udp.dport, BFD.udp_dport_echo,
                               "UDP destination port")
             self.assert_equal(udp.sport, udp_sport_rx, "UDP source port")
             udp_sport_rx += 1
-            self.assertTrue(p.haslayer(Raw) and p[Raw] == echo_packet[Raw],
-                            "Received packet is not the echo packet sent")
+            # need to compare the hex payload here, otherwise BFD_vpp_echo
+            # gets in way
+            self.assertEqual(str(p[UDP].payload),
+                             str(echo_packet[UDP].payload),
+                             "Received packet is not the echo packet sent")
         self.assert_equal(udp_sport_tx, udp_sport_rx, "UDP source port (== "
                           "ECHO packet identifier for test purposes)")
 
+    def test_echo(self):
+        """ echo function """
+        bfd_session_up(self)
+        self.test_session.update(required_min_echo_rx=50000)
+        self.test_session.send_packet()
+        detection_time = self.test_session.detect_mult *\
+            self.vpp_session.required_min_rx / USEC_IN_SEC
+        # echo shouldn't work without echo source set
+        for dummy in range(3):
+            sleep = 0.75 * detection_time
+            self.sleep(sleep, "delay before sending bfd packet")
+            self.test_session.send_packet()
+        p = wait_for_bfd_packet(
+            self, pcap_time_min=time.time() - self.vpp_clock_offset)
+        self.assert_equal(p[BFD].required_min_rx_interval,
+                          self.vpp_session.required_min_rx,
+                          "BFD required min rx interval")
+        self.vapi.bfd_udp_set_echo_source(self.loopback0.sw_if_index)
+        # should be turned on - loopback echo packets
+        for dummy in range(3):
+            loop_until = time.time() + 0.75 * detection_time
+            while time.time() < loop_until:
+                p = self.pg0.wait_for_packet(1)
+                self.logger.debug(ppp("Got packet:", p))
+                if p[UDP].dport == BFD.udp_dport_echo:
+                    self.assert_equal(
+                        p[IP].dst, self.pg0.local_ip4, "BFD ECHO dst IP")
+                    self.assertNotEqual(p[IP].src, self.loopback0.local_ip4,
+                                        "BFD ECHO src IP equal to loopback IP")
+                    self.logger.debug(ppp("Looping back packet:", p))
+                    self.pg0.add_stream(p)
+                    self.pg_start()
+                elif p.haslayer(BFD):
+                    self.assertGreaterEqual(p[BFD].required_min_rx_interval,
+                                            1000000)
+                    if "P" in p.sprintf("%BFD.flags%"):
+                        final = self.test_session.create_packet()
+                        final[BFD].flags = "F"
+                        self.test_session.send_packet(final)
+                else:
+                    raise Exception(ppp("Received unknown packet:", p))
+
+                self.assert_equal(len(self.vapi.collect_events()), 0,
+                                  "number of bfd events")
+            self.test_session.send_packet()
+
+    def test_echo_fail(self):
+        """ session goes down if echo function fails """
+        bfd_session_up(self)
+        self.test_session.update(required_min_echo_rx=50000)
+        self.test_session.send_packet()
+        detection_time = self.test_session.detect_mult *\
+            self.vpp_session.required_min_rx / USEC_IN_SEC
+        self.vapi.bfd_udp_set_echo_source(self.loopback0.sw_if_index)
+        # echo function should be used now, but we will drop the echo packets
+        verified_diag = False
+        for dummy in range(3):
+            loop_until = time.time() + 0.75 * detection_time
+            while time.time() < loop_until:
+                p = self.pg0.wait_for_packet(1)
+                self.logger.debug(ppp("Got packet:", p))
+                if p[UDP].dport == BFD.udp_dport_echo:
+                    # dropped
+                    pass
+                elif p.haslayer(BFD):
+                    if "P" in p.sprintf("%BFD.flags%"):
+                        self.assertGreaterEqual(
+                            p[BFD].required_min_rx_interval,
+                            1000000)
+                        final = self.test_session.create_packet()
+                        final[BFD].flags = "F"
+                        self.test_session.send_packet(final)
+                    if p[BFD].state == BFDState.down:
+                        self.assert_equal(p[BFD].diag,
+                                          BFDDiagCode.echo_function_failed,
+                                          BFDDiagCode)
+                        verified_diag = True
+                else:
+                    raise Exception(ppp("Received unknown packet:", p))
+            self.test_session.send_packet()
+        events = self.vapi.collect_events()
+        self.assert_equal(len(events), 1, "number of bfd events")
+        self.assert_equal(events[0].state, BFDState.down, BFDState)
+        self.assertTrue(verified_diag, "Incorrect diagnostics code received")
+
+    def test_echo_stop(self):
+        """ echo function stops if peer sets required min echo rx zero """
+        bfd_session_up(self)
+        self.test_session.update(required_min_echo_rx=50000)
+        self.test_session.send_packet()
+        self.vapi.bfd_udp_set_echo_source(self.loopback0.sw_if_index)
+        # wait for first echo packet
+        while True:
+            p = self.pg0.wait_for_packet(1)
+            self.logger.debug(ppp("Got packet:", p))
+            if p[UDP].dport == BFD.udp_dport_echo:
+                self.logger.debug(ppp("Looping back packet:", p))
+                self.pg0.add_stream(p)
+                self.pg_start()
+                break
+            elif p.haslayer(BFD):
+                # ignore BFD
+                pass
+            else:
+                raise Exception(ppp("Received unknown packet:", p))
+        self.test_session.update(required_min_echo_rx=0)
+        self.test_session.send_packet()
+        # echo packets shouldn't arrive anymore
+        for dummy in range(5):
+            wait_for_bfd_packet(
+                self, pcap_time_min=time.time() - self.vpp_clock_offset)
+            self.test_session.send_packet()
+            events = self.vapi.collect_events()
+            self.assert_equal(len(events), 0, "number of bfd events")
+
+    def test_stale_echo(self):
+        """ stale echo packets don't keep a session up """
+        bfd_session_up(self)
+        self.test_session.update(required_min_echo_rx=50000)
+        self.vapi.bfd_udp_set_echo_source(self.loopback0.sw_if_index)
+        self.test_session.send_packet()
+        # should be turned on - loopback echo packets
+        echo_packet = None
+        timeout_at = None
+        timeout_ok = False
+        for dummy in range(10 * self.vpp_session.detect_mult):
+            p = self.pg0.wait_for_packet(1)
+            if p[UDP].dport == BFD.udp_dport_echo:
+                if echo_packet is None:
+                    self.logger.debug(ppp("Got first echo packet:", p))
+                    echo_packet = p
+                    timeout_at = time.time() + self.vpp_session.detect_mult * \
+                        self.test_session.required_min_echo_rx / USEC_IN_SEC
+                else:
+                    self.logger.debug(ppp("Got followup echo packet:", p))
+                self.logger.debug(ppp("Looping back first echo packet:", p))
+                self.pg0.add_stream(echo_packet)
+                self.pg_start()
+            elif p.haslayer(BFD):
+                self.logger.debug(ppp("Got packet:", p))
+                if "P" in p.sprintf("%BFD.flags%"):
+                    final = self.test_session.create_packet()
+                    final[BFD].flags = "F"
+                    self.test_session.send_packet(final)
+                if p[BFD].state == BFDState.down:
+                    self.assertIsNotNone(
+                        timeout_at,
+                        "Session went down before first echo packet received")
+                    now = time.time()
+                    self.assertGreaterEqual(
+                        now, timeout_at,
+                        "Session timeout at %s, but is expected at %s" %
+                        (now, timeout_at))
+                    self.assert_equal(p[BFD].diag,
+                                      BFDDiagCode.echo_function_failed,
+                                      BFDDiagCode)
+                    events = self.vapi.collect_events()
+                    self.assert_equal(len(events), 1, "number of bfd events")
+                    self.assert_equal(events[0].state, BFDState.down, BFDState)
+                    timeout_ok = True
+                    break
+            else:
+                raise Exception(ppp("Received unknown packet:", p))
+            self.test_session.send_packet()
+        self.assertTrue(timeout_ok, "Expected timeout event didn't occur")
+
+    def test_invalid_echo_checksum(self):
+        """ echo packets with invalid checksum don't keep a session up """
+        bfd_session_up(self)
+        self.test_session.update(required_min_echo_rx=50000)
+        self.vapi.bfd_udp_set_echo_source(self.loopback0.sw_if_index)
+        self.test_session.send_packet()
+        # should be turned on - loopback echo packets
+        timeout_at = None
+        timeout_ok = False
+        for dummy in range(10 * self.vpp_session.detect_mult):
+            p = self.pg0.wait_for_packet(1)
+            if p[UDP].dport == BFD.udp_dport_echo:
+                self.logger.debug(ppp("Got echo packet:", p))
+                if timeout_at is None:
+                    timeout_at = time.time() + self.vpp_session.detect_mult * \
+                        self.test_session.required_min_echo_rx / USEC_IN_SEC
+                p[BFD_vpp_echo].checksum = getrandbits(64)
+                self.logger.debug(ppp("Looping back modified echo packet:", p))
+                self.pg0.add_stream(p)
+                self.pg_start()
+            elif p.haslayer(BFD):
+                self.logger.debug(ppp("Got packet:", p))
+                if "P" in p.sprintf("%BFD.flags%"):
+                    final = self.test_session.create_packet()
+                    final[BFD].flags = "F"
+                    self.test_session.send_packet(final)
+                if p[BFD].state == BFDState.down:
+                    self.assertIsNotNone(
+                        timeout_at,
+                        "Session went down before first echo packet received")
+                    now = time.time()
+                    self.assertGreaterEqual(
+                        now, timeout_at,
+                        "Session timeout at %s, but is expected at %s" %
+                        (now, timeout_at))
+                    self.assert_equal(p[BFD].diag,
+                                      BFDDiagCode.echo_function_failed,
+                                      BFDDiagCode)
+                    events = self.vapi.collect_events()
+                    self.assert_equal(len(events), 1, "number of bfd events")
+                    self.assert_equal(events[0].state, BFDState.down, BFDState)
+                    timeout_ok = True
+                    break
+            else:
+                raise Exception(ppp("Received unknown packet:", p))
+            self.test_session.send_packet()
+        self.assertTrue(timeout_ok, "Expected timeout event didn't occur")
+
     def test_admin_up_down(self):
+        """ put session admin-up and admin-down """
         bfd_session_up(self)
         self.vpp_session.admin_down()
         self.pg0.enable_capture()
@@ -931,6 +1224,42 @@
         e = self.vapi.wait_for_event(1, "bfd_udp_session_details")
         verify_event(self, e, expected_state=BFDState.up)
 
+    def test_config_change_remote_demand(self):
+        """ configuration change while peer in demand mode """
+        bfd_session_up(self)
+        demand = self.test_session.create_packet()
+        demand[BFD].flags = "D"
+        self.test_session.send_packet(demand)
+        self.vpp_session.modify_parameters(
+            required_min_rx=2 * self.vpp_session.required_min_rx)
+        p = wait_for_bfd_packet(self)
+        # poll bit must be set
+        self.assertIn("P", p.sprintf("%BFD.flags%"), "Poll bit not set")
+        # terminate poll sequence
+        final = self.test_session.create_packet()
+        final[BFD].flags = "D+F"
+        self.test_session.send_packet(final)
+        # vpp should be quiet now again
+        transmit_time = 0.9 \
+            * max(self.vpp_session.required_min_rx,
+                  self.test_session.desired_min_tx) \
+            / USEC_IN_SEC
+        count = 0
+        for dummy in range(self.test_session.detect_mult * 2):
+            time.sleep(transmit_time)
+            self.test_session.send_packet(demand)
+            try:
+                p = wait_for_bfd_packet(self, timeout=0)
+                self.logger.error(ppp("Received unexpected packet:", p))
+                count += 1
+            except CaptureTimeoutError:
+                pass
+        events = self.vapi.collect_events()
+        for e in events:
+            self.logger.error("Received unexpected event: %s", e)
+        self.assert_equal(count, 0, "number of packets received")
+        self.assert_equal(len(events), 0, "number of events received")
+
 
 class BFD6TestCase(VppTestCase):
     """Bidirectional Forwarding Detection (BFD) (IPv6) """
@@ -949,6 +1278,10 @@
             cls.pg0.configure_ipv6_neighbors()
             cls.pg0.admin_up()
             cls.pg0.resolve_ndp()
+            cls.create_loopback_interfaces([0])
+            cls.loopback0 = cls.lo_interfaces[0]
+            cls.loopback0.config_ip6()
+            cls.loopback0.admin_up()
 
         except Exception:
             super(BFD6TestCase, cls).tearDownClass()
@@ -1003,7 +1336,7 @@
         echo_packet = (Ether(src=self.pg0.remote_mac,
                              dst=self.pg0.local_mac) /
                        IPv6(src=self.pg0.remote_ip6,
-                            dst=self.pg0.local_ip6) /
+                            dst=self.pg0.remote_ip6) /
                        UDP(dport=BFD.udp_dport_echo) /
                        Raw("this should be looped back"))
         for dummy in range(echo_packet_count):
@@ -1022,16 +1355,67 @@
             self.assert_equal(self.pg0.local_mac, ether.src, "Source MAC")
             ip = p[IPv6]
             self.assert_equal(self.pg0.remote_ip6, ip.dst, "Destination IP")
-            self.assert_equal(self.pg0.local_ip6, ip.src, "Destination IP")
+            self.assert_equal(self.pg0.remote_ip6, ip.src, "Destination IP")
             udp = p[UDP]
             self.assert_equal(udp.dport, BFD.udp_dport_echo,
                               "UDP destination port")
             self.assert_equal(udp.sport, udp_sport_rx, "UDP source port")
             udp_sport_rx += 1
-            self.assertTrue(p.haslayer(Raw) and p[Raw] == echo_packet[Raw],
-                            "Received packet is not the echo packet sent")
+            # need to compare the hex payload here, otherwise BFD_vpp_echo
+            # gets in way
+            self.assertEqual(str(p[UDP].payload),
+                             str(echo_packet[UDP].payload),
+                             "Received packet is not the echo packet sent")
         self.assert_equal(udp_sport_tx, udp_sport_rx, "UDP source port (== "
                           "ECHO packet identifier for test purposes)")
+        self.assert_equal(udp_sport_tx, udp_sport_rx, "UDP source port (== "
+                          "ECHO packet identifier for test purposes)")
+
+    def test_echo(self):
+        """ echo function used """
+        bfd_session_up(self)
+        self.test_session.update(required_min_echo_rx=50000)
+        self.test_session.send_packet()
+        detection_time = self.test_session.detect_mult *\
+            self.vpp_session.required_min_rx / USEC_IN_SEC
+        # echo shouldn't work without echo source set
+        for dummy in range(3):
+            sleep = 0.75 * detection_time
+            self.sleep(sleep, "delay before sending bfd packet")
+            self.test_session.send_packet()
+        p = wait_for_bfd_packet(
+            self, pcap_time_min=time.time() - self.vpp_clock_offset)
+        self.assert_equal(p[BFD].required_min_rx_interval,
+                          self.vpp_session.required_min_rx,
+                          "BFD required min rx interval")
+        self.vapi.bfd_udp_set_echo_source(self.loopback0.sw_if_index)
+        # should be turned on - loopback echo packets
+        for dummy in range(3):
+            loop_until = time.time() + 0.75 * detection_time
+            while time.time() < loop_until:
+                p = self.pg0.wait_for_packet(1)
+                self.logger.debug(ppp("Got packet:", p))
+                if p[UDP].dport == BFD.udp_dport_echo:
+                    self.assert_equal(
+                        p[IPv6].dst, self.pg0.local_ip6, "BFD ECHO dst IP")
+                    self.assertNotEqual(p[IPv6].src, self.loopback0.local_ip6,
+                                        "BFD ECHO src IP equal to loopback IP")
+                    self.logger.debug(ppp("Looping back packet:", p))
+                    self.pg0.add_stream(p)
+                    self.pg_start()
+                elif p.haslayer(BFD):
+                    self.assertGreaterEqual(p[BFD].required_min_rx_interval,
+                                            1000000)
+                    if "P" in p.sprintf("%BFD.flags%"):
+                        final = self.test_session.create_packet()
+                        final[BFD].flags = "F"
+                        self.test_session.send_packet(final)
+                else:
+                    raise Exception(ppp("Received unknown packet:", p))
+
+                self.assert_equal(len(self.vapi.collect_events()), 0,
+                                  "number of bfd events")
+            self.test_session.send_packet()
 
 
 class BFDSHA1TestCase(VppTestCase):
@@ -1121,7 +1505,7 @@
         self.assert_equal(self.vpp_session.state, BFDState.up, BFDState)
 
     def test_send_bad_seq_number(self):
-        """ session is not kept alive by msgs with bad seq numbers"""
+        """ session is not kept alive by msgs with bad sequence numbers"""
         key = self.factory.create_random_key(
             self, BFDAuthType.meticulous_keyed_sha1)
         key.add_vpp_config()
@@ -1133,16 +1517,13 @@
             self, self.pg0, AF_INET, sha1_key=key,
             bfd_key_id=self.vpp_session.bfd_key_id)
         bfd_session_up(self)
-        detection_time = self.vpp_session.detect_mult *\
+        detection_time = self.test_session.detect_mult *\
             self.vpp_session.required_min_rx / USEC_IN_SEC
-        session_timeout = time.time() + detection_time
-        while time.time() < session_timeout:
-            self.assert_equal(len(self.vapi.collect_events()), 0,
-                              "number of bfd events")
-            wait_for_bfd_packet(self)
+        send_until = time.time() + 2 * detection_time
+        while time.time() < send_until:
             self.test_session.send_packet()
-        wait_for_bfd_packet(self)
-        self.test_session.send_packet()
+            self.sleep(0.7 * self.vpp_session.required_min_rx / USEC_IN_SEC,
+                       "time between bfd packets")
         e = self.vapi.collect_events()
         # session should be down now, because the sequence numbers weren't
         # updated
@@ -1250,7 +1631,7 @@
             bfd_key_id=self.vpp_session.bfd_key_id, our_seq_number=0)
         bfd_session_up(self)
         # don't send any packets for 2*detection_time
-        detection_time = self.vpp_session.detect_mult *\
+        detection_time = self.test_session.detect_mult *\
             self.vpp_session.required_min_rx / USEC_IN_SEC
         self.sleep(detection_time, "simulating peer restart")
         events = self.vapi.collect_events()
diff --git a/test/vpp_papi_provider.py b/test/vpp_papi_provider.py
index 59e58ad..dd9baff 100644
--- a/test/vpp_papi_provider.py
+++ b/test/vpp_papi_provider.py
@@ -1117,6 +1117,10 @@
     def bfd_auth_keys_dump(self):
         return self.api(self.papi.bfd_auth_keys_dump, {})
 
+    def bfd_udp_set_echo_source(self, sw_if_index):
+        return self.api(self.papi.bfd_udp_set_echo_source,
+                        {'sw_if_index': sw_if_index})
+
     def classify_add_del_table(
             self,
             is_add,