BFD: add ARP-awareness, fix bugs

Make BFD ARP-aware when sending out packets.
Fix a few one-liner bugs discovered while integrating with cisco
nexus. Enhance CLI view to better observe session state.

Change-Id: I266c29492f351207b84328ab665d9d697969da9c
Signed-off-by: Klement Sekera <ksekera@cisco.com>

5 files changed, 282 insertions, 77 deletions

diff --git a/src/vnet/bfd/bfd_cli.c b/src/vnet/bfd/bfd_cli.c
index f15acb4ba36..44e671c534e 100644
--- a/src/vnet/bfd/bfd_cli.c
+++ b/src/vnet/bfd/bfd_cli.c
@@ -28,6 +28,7 @@
 static u8 *
 format_bfd_session_cli (u8 * s, va_list * args)
 {
+  vlib_main_t *vm = va_arg (*args, vlib_main_t *);
   bfd_main_t *bm = va_arg (*args, bfd_main_t *);
   bfd_session_t *bs = va_arg (*args, bfd_session_t *);
   switch (bs->transport)
@@ -51,7 +52,7 @@ format_bfd_session_cli (u8 * s, va_list * args)
 	      bfd_diag_code_string (bs->remote_diag));
   s = format (s, "%10s %-32s %20u %20u\n", "", "Detect multiplier",
 	      bs->local_detect_mult, bs->remote_detect_mult);
-  s = format (s, "%10s %-32s %20u %20u\n", "",
+  s = format (s, "%10s %-32s %20u %20llu\n", "",
 	      "Required Min Rx Interval (usec)",
 	      bs->config_required_min_rx_usec, bs->remote_min_rx_usec);
   s = format (s, "%10s %-32s %20u %20u\n", "",
@@ -61,18 +62,54 @@ format_bfd_session_cli (u8 * s, va_list * args)
   s =
     format (s, "%10s %-32s %20u\n", "", "Transmit interval",
 	    bfd_clocks_to_usec (bm, bs->transmit_interval_clocks));
+  u64 now = clib_cpu_time_now ();
+  u8 *tmp = NULL;
+  if (bs->last_tx_clocks > 0)
+    {
+      tmp = format (tmp, "%.2fs ago", (now - bs->last_tx_clocks) *
+		    vm->clib_time.seconds_per_clock);
+      s = format (s, "%10s %-32s %20v\n", "", "Last control frame tx", tmp);
+      vec_reset_length (tmp);
+    }
+  if (bs->last_rx_clocks)
+    {
+      tmp = format (tmp, "%.2fs ago", (now - bs->last_rx_clocks) *
+		    vm->clib_time.seconds_per_clock);
+      s = format (s, "%10s %-32s %20v\n", "", "Last control frame rx", tmp);
+      vec_reset_length (tmp);
+    }
   s =
-    format (s, "%10s %-32s %20s %20s\n", "", "Demand mode", "no",
-	    bs->remote_demand ? "yes" : "no");
-  s =
-    format (s, "%10s %-32s %20s\n", "", "Poll state",
-	    bfd_poll_state_string (bs->poll_state));
+    format (s, "%10s %-32s %20u %20llu\n", "", "Min Echo Rx Interval (usec)",
+	    1, bs->remote_min_echo_rx_usec);
+  if (bs->echo)
+    {
+      s = format (s, "%10s %-32s %20u\n", "", "Echo transmit interval",
+		  bfd_clocks_to_usec (bm, bs->echo_transmit_interval_clocks));
+      tmp = format (tmp, "%.2fs ago", (now - bs->echo_last_tx_clocks) *
+		    vm->clib_time.seconds_per_clock);
+      s = format (s, "%10s %-32s %20v\n", "", "Last echo frame tx", tmp);
+      vec_reset_length (tmp);
+      tmp = format (tmp, "%.6fs",
+		    (bs->echo_last_rx_clocks - bs->echo_last_tx_clocks) *
+		    vm->clib_time.seconds_per_clock);
+      s =
+	format (s, "%10s %-32s %20v\n", "", "Last echo frame roundtrip time",
+		tmp);
+    }
+  vec_free (tmp);
+  tmp = NULL;
+  s = format (s, "%10s %-32s %20s %20s\n", "", "Demand mode", "no",
+	      bs->remote_demand ? "yes" : "no");
+  s = format (s, "%10s %-32s %20s\n", "", "Poll state",
+	      bfd_poll_state_string (bs->poll_state));
   if (bs->auth.curr_key)
     {
       s = format (s, "%10s %-32s %20u\n", "", "Authentication config key ID",
 		  bs->auth.curr_key->conf_key_id);
       s = format (s, "%10s %-32s %20u\n", "", "Authentication BFD key ID",
 		  bs->auth.curr_bfd_key_id);
+      s = format (s, "%10s %-32s %20u %20u\n", "", "Sequence number",
+		  bs->auth.local_seq_number, bs->auth.remote_seq_number);
     }
   return s;
 }
@@ -96,6 +133,7 @@ show_bfd (vlib_main_t * vm, unformat_input_t * input,
       });
       /* *INDENT-ON* */
       vlib_cli_output (vm, "%v\n", s);
+      vec_free (s);
       vlib_cli_output (vm, "Number of configured BFD keys: %lu\n",
 		       (u64) pool_elts (bm->auth_keys));
     }
@@ -104,8 +142,9 @@ show_bfd (vlib_main_t * vm, unformat_input_t * input,
       u8 *s = format (NULL, "%=10s %=32s %=20s %=20s\n", "Index", "Property",
 		      "Local value", "Remote value");
       /* *INDENT-OFF* */
-      pool_foreach (bs, bm->sessions,
-                    { s = format (s, "%U", format_bfd_session_cli, bm, bs); });
+      pool_foreach (bs, bm->sessions, {
+        s = format (s, "%U", format_bfd_session_cli, vm, bm, bs);
+      });
       /* *INDENT-ON* */
       vlib_cli_output (vm, "%v", s);
       vec_free (s);
@@ -349,7 +388,7 @@ static const unsigned optional = 0;
 #define CHECK_MANDATORY(t, n, s, r, ...)                                  \
   if (mandatory == r && !have_##n)                                        \
     {                                                                     \
-      ret = clib_error_return (0, "Required parameter `%s' missing.", n); \
+      ret = clib_error_return (0, "Required parameter `%s' missing.", s); \
       goto out;                                                           \
     }
 
diff --git a/src/vnet/bfd/bfd_main.c b/src/vnet/bfd/bfd_main.c
index ea6db1f9d27..2b70a20c9fd 100644
--- a/src/vnet/bfd/bfd_main.c
+++ b/src/vnet/bfd/bfd_main.c
@@ -63,13 +63,6 @@ bfd_clocks_to_usec (const bfd_main_t * bm, u64 clocks)
 
 static vlib_node_registration_t bfd_process_node;
 
-/* set to 0 here, real values filled at startup */
-static u32 bfd_node_index_by_transport[] = {
-#define F(t, n) [BFD_TRANSPORT_##t] = 0,
-  foreach_bfd_transport (F)
-#undef F
-};
-
 u8 *
 format_bfd_auth_key (u8 * s, va_list * args)
 {
@@ -560,51 +553,70 @@ bfd_on_config_change (vlib_main_t * vm, vlib_node_runtime_t * rt,
 }
 
 static void
-bfd_add_transport_layer (vlib_main_t * vm, vlib_buffer_t * b,
-			 bfd_session_t * bs)
+bfd_add_transport_layer (vlib_main_t * vm, u32 bi, bfd_session_t * bs)
+{
+  switch (bs->transport)
+    {
+    case BFD_TRANSPORT_UDP4:
+      BFD_DBG ("Transport bfd via udp4, bs_idx=%u", bs->bs_idx);
+      bfd_add_udp4_transport (vm, bi, 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, bi, bs, 0 /* is_echo */ );
+      break;
+    }
+}
+
+static int
+bfd_transport_control_frame (vlib_main_t * vm, u32 bi, bfd_session_t * bs)
 {
   switch (bs->transport)
     {
     case BFD_TRANSPORT_UDP4:
       BFD_DBG ("Transport bfd via udp4, bs_idx=%u", bs->bs_idx);
-      bfd_add_udp4_transport (vm, b, bs, 0 /* is_echo */ );
+      return bfd_transport_udp4 (vm, bi, bs);
       break;
     case BFD_TRANSPORT_UDP6:
       BFD_DBG ("Transport bfd via udp6, bs_idx=%u", bs->bs_idx);
-      bfd_add_udp6_transport (vm, b, bs, 0 /* is_echo */ );
+      return bfd_transport_udp6 (vm, bi, bs);
       break;
     }
+  return 0;
 }
 
 static int
-bfd_echo_add_transport_layer (vlib_main_t * vm, vlib_buffer_t * b,
-			      bfd_session_t * bs)
+bfd_echo_add_transport_layer (vlib_main_t * vm, u32 bi, bfd_session_t * bs)
 {
   switch (bs->transport)
     {
     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 */ );
+      return bfd_add_udp4_transport (vm, bi, 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 */ );
+      return bfd_add_udp6_transport (vm, bi, bs, 1 /* is_echo */ );
       break;
     }
   return 0;
 }
 
-static void
-bfd_create_frame_to_next_node (vlib_main_t * vm, bfd_session_t * bs, u32 bi)
+static int
+bfd_transport_echo (vlib_main_t * vm, u32 bi, bfd_session_t * bs)
 {
-
-  vlib_frame_t *f =
-    vlib_get_frame_to_node (vm, bfd_node_index_by_transport[bs->transport]);
-
-  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);
+  switch (bs->transport)
+    {
+    case BFD_TRANSPORT_UDP4:
+      BFD_DBG ("Transport bfd echo via udp4, bs_idx=%u", bs->bs_idx);
+      return bfd_transport_udp4 (vm, bi, bs);
+      break;
+    case BFD_TRANSPORT_UDP6:
+      BFD_DBG ("Transport bfd echo via udp6, bs_idx=%u", bs->bs_idx);
+      return bfd_transport_udp6 (vm, bi, bs);
+      break;
+    }
+  return 0;
 }
 
 #if WITH_LIBSSL > 0
@@ -704,7 +716,7 @@ bfd_init_control_frame (bfd_main_t * bm, bfd_session_t * bs,
   bfd_pkt_set_diag_code (pkt, bs->local_diag);
   bfd_pkt_set_state (pkt, bs->local_state);
   pkt->head.detect_mult = bs->local_detect_mult;
-  pkt->head.length = clib_host_to_net_u32 (bfd_length);
+  pkt->head.length = 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);
@@ -725,8 +737,7 @@ bfd_init_control_frame (bfd_main_t * bm, bfd_session_t * bs,
 
 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)
+	       bfd_main_t * bm, bfd_session_t * bs, u64 now)
 {
   if (!bfd_is_echo_possible (bs))
     {
@@ -734,7 +745,8 @@ bfd_send_echo (vlib_main_t * vm, vlib_node_runtime_t * rt,
       bs->echo = 0;
       return;
     }
-  /* sometimes the wheel expires an event a bit sooner than requested, account
+  /* 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)
     {
@@ -747,6 +759,8 @@ bfd_send_echo (vlib_main_t * vm, vlib_node_runtime_t * rt,
 	}
       vlib_buffer_t *b = vlib_get_buffer (vm, bi);
       ASSERT (b->current_data == 0);
+      memset (vnet_buffer (b), 0, sizeof (*vnet_buffer (b)));
+      VLIB_BUFFER_TRACE_TRAJECTORY_INIT (b);
       bfd_echo_pkt_t *pkt = vlib_buffer_get_current (b);
       memset (pkt, 0, sizeof (*pkt));
       pkt->discriminator = bs->local_discr;
@@ -756,7 +770,14 @@ bfd_send_echo (vlib_main_t * vm, vlib_node_runtime_t * rt,
 	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))
+      if (!bfd_echo_add_transport_layer (vm, bi, bs))
+	{
+	  BFD_ERR ("cannot send echo packet out, turning echo off");
+	  bs->echo = 0;
+	  vlib_buffer_free_one (vm, bi);
+	  return;
+	}
+      if (!bfd_transport_echo (vm, bi, bs))
 	{
 	  BFD_ERR ("cannot send echo packet out, turning echo off");
 	  bs->echo = 0;
@@ -765,7 +786,6 @@ bfd_send_echo (vlib_main_t * vm, vlib_node_runtime_t * rt,
 	}
       bs->echo_last_tx_clocks = now;
       bfd_calc_next_echo_tx (bm, bs, now);
-      bfd_create_frame_to_next_node (vm, bs, bi);
     }
   else
     {
@@ -777,8 +797,7 @@ bfd_send_echo (vlib_main_t * vm, vlib_node_runtime_t * rt,
 
 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)
+		   bfd_main_t * bm, bfd_session_t * bs, u64 now)
 {
   if (!bs->remote_min_rx_usec && BFD_POLL_NOT_NEEDED == bs->poll_state)
     {
@@ -798,8 +817,10 @@ bfd_send_periodic (vlib_main_t * vm, vlib_node_runtime_t * rt,
       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 */
+  /*
+   * 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 ("\nSending periodic control frame: %U", format_bfd_session,
@@ -812,6 +833,8 @@ bfd_send_periodic (vlib_main_t * vm, vlib_node_runtime_t * rt,
 	}
       vlib_buffer_t *b = vlib_get_buffer (vm, bi);
       ASSERT (b->current_data == 0);
+      memset (vnet_buffer (b), 0, sizeof (*vnet_buffer (b)));
+      VLIB_BUFFER_TRACE_TRAJECTORY_INIT (b);
       bfd_init_control_frame (bm, bs, b);
       switch (bs->poll_state)
 	{
@@ -837,10 +860,13 @@ bfd_send_periodic (vlib_main_t * vm, vlib_node_runtime_t * rt,
 	  break;
 	}
       bfd_add_auth_section (b, bs);
-      bfd_add_transport_layer (vm, b, bs);
+      bfd_add_transport_layer (vm, bi, bs);
+      if (!bfd_transport_control_frame (vm, bi, bs))
+	{
+	  vlib_buffer_free_one (vm, bi);
+	}
       bs->last_tx_clocks = now;
       bfd_calc_next_tx (bm, bs, now);
-      bfd_create_frame_to_next_node (vm, bs, bi);
     }
   else
     {
@@ -852,13 +878,15 @@ bfd_send_periodic (vlib_main_t * vm, vlib_node_runtime_t * rt,
 
 void
 bfd_init_final_control_frame (vlib_main_t * vm, vlib_buffer_t * b,
-			      bfd_main_t * bm, bfd_session_t * bs)
+			      bfd_main_t * bm, bfd_session_t * bs,
+			      int is_local)
 {
   BFD_DBG ("Send final control frame for bs_idx=%lu", bs->bs_idx);
   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);
+  u32 bi = vlib_get_buffer_index (vm, b);
+  bfd_add_transport_layer (vm, bi, bs);
   bs->last_tx_clocks = clib_cpu_time_now ();
   /*
    * RFC allows to include changes in final frame, so if there were any
@@ -871,8 +899,10 @@ static void
 bfd_check_rx_timeout (bfd_main_t * bm, bfd_session_t * bs, u64 now,
 		      int handling_wakeup)
 {
-  /* sometimes the wheel expires an event a bit sooner than requested, account
-     for that here */
+  /*
+   * sometimes the wheel expires an event a bit sooner than requested, account
+   * for that here
+   */
   if (bs->last_rx_clocks + bs->detection_time_clocks <=
       now + bm->wheel_inaccuracy)
     {
@@ -907,14 +937,14 @@ bfd_on_timeout (vlib_main_t * vm, vlib_node_runtime_t * rt, bfd_main_t * bm,
   switch (bs->local_state)
     {
     case BFD_STATE_admin_down:
-      bfd_send_periodic (vm, rt, bm, bs, now, 1);
+      bfd_send_periodic (vm, rt, bm, bs, now);
       break;
     case BFD_STATE_down:
-      bfd_send_periodic (vm, rt, bm, bs, now, 1);
+      bfd_send_periodic (vm, rt, bm, bs, now);
       break;
     case BFD_STATE_init:
       bfd_check_rx_timeout (bm, bs, now, 1);
-      bfd_send_periodic (vm, rt, bm, bs, now, 1);
+      bfd_send_periodic (vm, rt, bm, bs, now);
       break;
     case BFD_STATE_up:
       bfd_check_rx_timeout (bm, bs, now, 1);
@@ -932,10 +962,10 @@ bfd_on_timeout (vlib_main_t * vm, vlib_node_runtime_t * rt, bfd_main_t * bm,
 					      bs->config_required_min_rx_clocks));
 	  bfd_set_poll_state (bs, BFD_POLL_NEEDED);
 	}
-      bfd_send_periodic (vm, rt, bm, bs, now, 1);
+      bfd_send_periodic (vm, rt, bm, bs, now);
       if (bs->echo)
 	{
-	  bfd_send_echo (vm, rt, bm, bs, now, 1);
+	  bfd_send_echo (vm, rt, bm, bs, now);
 	}
       break;
     }
@@ -996,7 +1026,8 @@ bfd_process (vlib_main_t * vm, vlib_node_runtime_t * rt, vlib_frame_t * f)
 	    {
 	      bfd_session_t *bs =
 		pool_elt_at_index (bm->sessions, *event_data);
-	      bfd_send_periodic (vm, rt, bm, bs, now, 1);
+	      bfd_send_periodic (vm, rt, bm, bs, now);
+	      bfd_set_timer (bm, bs, now, 1);
 	    }
 	  else
 	    {
@@ -1113,14 +1144,6 @@ bfd_main_init (vlib_main_t * vm)
   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;
-
-  vlib_node_t *node = NULL;
-#define F(t, n)                                                 \
-  node = vlib_get_node_by_name (vm, (u8 *)n);                   \
-  bfd_node_index_by_transport[BFD_TRANSPORT_##t] = node->index; \
-  BFD_DBG ("node '%s' has index %u", n, node->index);
-  foreach_bfd_transport (F);
-#undef F
   return 0;
 }
 
@@ -1654,10 +1677,12 @@ bfd_consume_pkt (bfd_main_t * bm, const bfd_pkt_t * pkt, u32 bs_idx)
     {
       if (BFD_STATE_down == bs->remote_state)
 	{
+	  bfd_set_diag (bs, BFD_DIAG_CODE_no_diag);
 	  bfd_set_state (bm, bs, BFD_STATE_init, 0);
 	}
       else if (BFD_STATE_init == bs->remote_state)
 	{
+	  bfd_set_diag (bs, BFD_DIAG_CODE_no_diag);
 	  bfd_set_state (bm, bs, BFD_STATE_up, 0);
 	}
     }
@@ -1666,6 +1691,7 @@ bfd_consume_pkt (bfd_main_t * bm, const bfd_pkt_t * pkt, u32 bs_idx)
       if (BFD_STATE_up == bs->remote_state ||
 	  BFD_STATE_init == bs->remote_state)
 	{
+	  bfd_set_diag (bs, BFD_DIAG_CODE_no_diag);
 	  bfd_set_state (bm, bs, BFD_STATE_up, 0);
 	}
     }
diff --git a/src/vnet/bfd/bfd_main.h b/src/vnet/bfd/bfd_main.h
index 4d460f41f46..d722a55274c 100644
--- a/src/vnet/bfd/bfd_main.h
+++ b/src/vnet/bfd/bfd_main.h
@@ -316,7 +316,8 @@ 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_main_t * bm, bfd_session_t * bs);
+				   bfd_main_t * bm, bfd_session_t * bs,
+				   int is_local);
 u8 *format_bfd_session (u8 * s, va_list * args);
 u8 *format_bfd_auth_key (u8 * s, va_list * args);
 void bfd_session_set_flags (bfd_session_t * bs, u8 admin_up_down);
diff --git a/src/vnet/bfd/bfd_udp.c b/src/vnet/bfd/bfd_udp.c
index b3eabc9cda9..ebee590b795 100644
--- a/src/vnet/bfd/bfd_udp.c
+++ b/src/vnet/bfd/bfd_udp.c
@@ -51,6 +51,14 @@ typedef struct
   int echo_source_is_set;
   /* loopback interface used to get echo source ip */
   u32 echo_source_sw_if_index;
+  /* node index of "ip4-arp" node */
+  u32 ip4_arp_idx;
+  /* node index of "ip6-discover-neighbor" node */
+  u32 ip6_ndp_idx;
+  /* node index of "ip4-rewrite" node */
+  u32 ip4_rewrite_idx;
+  /* node index of "ip6-rewrite" node */
+  u32 ip6_rewrite_idx;
 } bfd_udp_main_t;
 
 static vlib_node_registration_t bfd_udp4_input_node;
@@ -231,15 +239,18 @@ bfd_udp_get_echo_source (int *is_set, u32 * sw_if_index, int *have_usable_ip4,
 }
 
 int
-bfd_add_udp4_transport (vlib_main_t * vm, vlib_buffer_t * b,
+bfd_add_udp4_transport (vlib_main_t * vm, u32 bi,
 			const bfd_session_t * bs, int is_echo)
 {
   const bfd_udp_session_t *bus = &bs->udp;
   const bfd_udp_key_t *key = &bus->key;
+  vlib_buffer_t *b = vlib_get_buffer (vm, bi);
 
   b->flags |= VNET_BUFFER_LOCALLY_ORIGINATED;
   vnet_buffer (b)->ip.adj_index[VLIB_RX] = bus->adj_index;
   vnet_buffer (b)->ip.adj_index[VLIB_TX] = bus->adj_index;
+  vnet_buffer (b)->sw_if_index[VLIB_RX] = 0;
+  vnet_buffer (b)->sw_if_index[VLIB_TX] = ~0;
   typedef struct
   {
     ip4_header_t ip4;
@@ -283,15 +294,18 @@ bfd_add_udp4_transport (vlib_main_t * vm, vlib_buffer_t * b,
 }
 
 int
-bfd_add_udp6_transport (vlib_main_t * vm, vlib_buffer_t * b,
+bfd_add_udp6_transport (vlib_main_t * vm, u32 bi,
 			const bfd_session_t * bs, int is_echo)
 {
   const bfd_udp_session_t *bus = &bs->udp;
   const bfd_udp_key_t *key = &bus->key;
+  vlib_buffer_t *b = vlib_get_buffer (vm, bi);
 
   b->flags |= VNET_BUFFER_LOCALLY_ORIGINATED;
   vnet_buffer (b)->ip.adj_index[VLIB_RX] = bus->adj_index;
   vnet_buffer (b)->ip.adj_index[VLIB_TX] = bus->adj_index;
+  vnet_buffer (b)->sw_if_index[VLIB_RX] = 0;
+  vnet_buffer (b)->sw_if_index[VLIB_TX] = 0;
   typedef struct
   {
     ip6_header_t ip6;
@@ -346,6 +360,76 @@ bfd_add_udp6_transport (vlib_main_t * vm, vlib_buffer_t * b,
   return 1;
 }
 
+static void
+bfd_create_frame_to_next_node (vlib_main_t * vm, u32 bi, u32 next_node)
+{
+  vlib_frame_t *f = vlib_get_frame_to_node (vm, next_node);
+  u32 *to_next = vlib_frame_vector_args (f);
+  to_next[0] = bi;
+  f->n_vectors = 1;
+  vlib_put_frame_to_node (vm, next_node, f);
+}
+
+int
+bfd_udp_calc_next_node (const struct bfd_session_s *bs, u32 * next_node)
+{
+  const bfd_udp_session_t *bus = &bs->udp;
+  ip_adjacency_t *adj = adj_get (bus->adj_index);
+  switch (adj->lookup_next_index)
+    {
+    case IP_LOOKUP_NEXT_ARP:
+      switch (bs->transport)
+	{
+	case BFD_TRANSPORT_UDP4:
+	  *next_node = bfd_udp_main.ip4_arp_idx;
+	  return 1;
+	case BFD_TRANSPORT_UDP6:
+	  *next_node = bfd_udp_main.ip6_ndp_idx;
+	  return 1;
+	}
+      break;
+    case IP_LOOKUP_NEXT_REWRITE:
+      switch (bs->transport)
+	{
+	case BFD_TRANSPORT_UDP4:
+	  *next_node = bfd_udp_main.ip4_rewrite_idx;
+	  return 1;
+	case BFD_TRANSPORT_UDP6:
+	  *next_node = bfd_udp_main.ip6_rewrite_idx;
+	  return 1;
+	}
+      break;
+    default:
+      /* drop */
+      break;
+    }
+  return 0;
+}
+
+int
+bfd_transport_udp4 (vlib_main_t * vm, u32 bi, const struct bfd_session_s *bs)
+{
+  u32 next_node;
+  int rv = bfd_udp_calc_next_node (bs, &next_node);
+  if (rv)
+    {
+      bfd_create_frame_to_next_node (vm, bi, next_node);
+    }
+  return rv;
+}
+
+int
+bfd_transport_udp6 (vlib_main_t * vm, u32 bi, const struct bfd_session_s *bs)
+{
+  u32 next_node;
+  int rv = bfd_udp_calc_next_node (bs, &next_node);
+  if (rv)
+    {
+      bfd_create_frame_to_next_node (vm, bi, next_node);
+    }
+  return 1;
+}
+
 static bfd_session_t *
 bfd_lookup_session (bfd_udp_main_t * bum, const bfd_udp_key_t * key)
 {
@@ -703,7 +787,8 @@ bfd_udp_auth_deactivate (u32 sw_if_index,
 typedef enum
 {
   BFD_UDP_INPUT_NEXT_NORMAL,
-  BFD_UDP_INPUT_NEXT_REPLY,
+  BFD_UDP_INPUT_NEXT_REPLY_ARP,
+  BFD_UDP_INPUT_NEXT_REPLY_REWRITE,
   BFD_UDP_INPUT_N_NEXT,
 } bfd_udp_input_next_t;
 
@@ -1112,8 +1197,11 @@ bfd_udp_input (vlib_main_t * vm, vlib_node_runtime_t * rt,
 	  const bfd_pkt_t *pkt = vlib_buffer_get_current (b0);
 	  if (bfd_pkt_get_poll (pkt))
 	    {
+	      b0->current_data = 0;
+	      b0->current_length = 0;
+	      memset (vnet_buffer (b0), 0, sizeof (*vnet_buffer (b0)));
 	      bfd_init_final_control_frame (vm, b0, bfd_udp_main.bfd_main,
-					    bs);
+					    bs, 0);
 	      if (is_ipv6)
 		{
 		  vlib_node_increment_counter (vm, bfd_udp6_input_node.index,
@@ -1124,7 +1212,20 @@ bfd_udp_input (vlib_main_t * vm, vlib_node_runtime_t * rt,
 		  vlib_node_increment_counter (vm, bfd_udp4_input_node.index,
 					       b0->error, 1);
 		}
-	      next0 = BFD_UDP_INPUT_NEXT_REPLY;
+	      const bfd_udp_session_t *bus = &bs->udp;
+	      ip_adjacency_t *adj = adj_get (bus->adj_index);
+	      switch (adj->lookup_next_index)
+		{
+		case IP_LOOKUP_NEXT_ARP:
+		  next0 = BFD_UDP_INPUT_NEXT_REPLY_ARP;
+		  break;
+		case IP_LOOKUP_NEXT_REWRITE:
+		  next0 = BFD_UDP_INPUT_NEXT_REPLY_REWRITE;
+		  break;
+		default:
+		  /* drop */
+		  break;
+		}
 	    }
 	}
       vlib_set_next_frame_buffer (vm, rt, next0, bi0);
@@ -1161,7 +1262,8 @@ VLIB_REGISTER_NODE (bfd_udp4_input_node, static) = {
   .next_nodes =
       {
               [BFD_UDP_INPUT_NEXT_NORMAL] = "error-drop",
-              [BFD_UDP_INPUT_NEXT_REPLY] = "ip4-lookup",
+              [BFD_UDP_INPUT_NEXT_REPLY_ARP] = "ip4-arp",
+              [BFD_UDP_INPUT_NEXT_REPLY_REWRITE] = "ip4-lookup",
       },
 };
 /* *INDENT-ON* */
@@ -1188,7 +1290,8 @@ VLIB_REGISTER_NODE (bfd_udp6_input_node, static) = {
   .next_nodes =
       {
               [BFD_UDP_INPUT_NEXT_NORMAL] = "error-drop",
-              [BFD_UDP_INPUT_NEXT_REPLY] = "ip6-lookup",
+              [BFD_UDP_INPUT_NEXT_REPLY_ARP] = "ip6-discover-neighbor",
+              [BFD_UDP_INPUT_NEXT_REPLY_REWRITE] = "ip6-lookup",
       },
 };
 /* *INDENT-ON* */
@@ -1246,7 +1349,7 @@ bfd_udp_echo_input (vlib_main_t * vm, vlib_node_runtime_t * rt,
 	      vlib_node_increment_counter (vm, bfd_udp_echo4_input_node.index,
 					   b0->error, 1);
 	    }
-	  next0 = BFD_UDP_INPUT_NEXT_REPLY;
+	  next0 = BFD_UDP_INPUT_NEXT_REPLY_REWRITE;
 	}
 
       vlib_set_next_frame_buffer (vm, rt, next0, bi0);
@@ -1300,7 +1403,8 @@ VLIB_REGISTER_NODE (bfd_udp_echo4_input_node, static) = {
   .next_nodes =
       {
               [BFD_UDP_INPUT_NEXT_NORMAL] = "error-drop",
-              [BFD_UDP_INPUT_NEXT_REPLY] = "ip4-lookup",
+              [BFD_UDP_INPUT_NEXT_REPLY_ARP] = "ip4-arp",
+              [BFD_UDP_INPUT_NEXT_REPLY_REWRITE] = "ip4-lookup",
       },
 };
 /* *INDENT-ON* */
@@ -1328,7 +1432,8 @@ VLIB_REGISTER_NODE (bfd_udp_echo6_input_node, static) = {
   .next_nodes =
       {
               [BFD_UDP_INPUT_NEXT_NORMAL] = "error-drop",
-              [BFD_UDP_INPUT_NEXT_REPLY] = "ip6-lookup",
+              [BFD_UDP_INPUT_NEXT_REPLY_ARP] = "ip6-discover-neighbor",
+              [BFD_UDP_INPUT_NEXT_REPLY_REWRITE] = "ip6-lookup",
       },
 };
 
@@ -1375,6 +1480,19 @@ bfd_udp_init (vlib_main_t * vm)
 			 bfd_udp_echo4_input_node.index, 1);
   udp_register_dst_port (vm, UDP_DST_PORT_bfd_echo6,
 			 bfd_udp_echo6_input_node.index, 0);
+  vlib_node_t *node = vlib_get_node_by_name (vm, (u8 *) "ip4-arp");
+  ASSERT (node);
+  bfd_udp_main.ip4_arp_idx = node->index;
+  node = vlib_get_node_by_name (vm, (u8 *) "ip6-discover-neighbor");
+  ASSERT (node);
+  bfd_udp_main.ip6_ndp_idx = node->index;
+  node = vlib_get_node_by_name (vm, (u8 *) "ip4-rewrite");
+  ASSERT (node);
+  bfd_udp_main.ip4_rewrite_idx = node->index;
+  node = vlib_get_node_by_name (vm, (u8 *) "ip6-rewrite");
+  ASSERT (node);
+  bfd_udp_main.ip6_rewrite_idx = node->index;
+
   return 0;
 }
 
diff --git a/src/vnet/bfd/bfd_udp.h b/src/vnet/bfd/bfd_udp.h
index 5080ec98f75..a4adbadf861 100644
--- a/src/vnet/bfd/bfd_udp.h
+++ b/src/vnet/bfd/bfd_udp.h
@@ -57,11 +57,12 @@ struct bfd_session_s;
 /**
  * @brief add the necessary transport layer by prepending it to existing data
  *
+ *
  * @param is_echo 1 if this is echo packet, 0 if control frame
  *
  * @return 1 on success, 0 on failure
  */
-int bfd_add_udp4_transport (vlib_main_t * vm, vlib_buffer_t * b,
+int bfd_add_udp4_transport (vlib_main_t * vm, u32 bi,
 			    const struct bfd_session_s *bs, int is_echo);
 
 /**
@@ -71,10 +72,30 @@ int bfd_add_udp4_transport (vlib_main_t * vm, vlib_buffer_t * b,
  *
  * @return 1 on success, 0 on failure
  */
-int bfd_add_udp6_transport (vlib_main_t * vm, vlib_buffer_t * b,
+int bfd_add_udp6_transport (vlib_main_t * vm, u32 bi,
 			    const struct bfd_session_s *bs, int is_echo);
 
 /**
+ * @brief transport packet over udpv4
+ *
+ * @param is_echo 1 if this is echo packet, 0 if control frame
+ *
+ * @return 1 on success, 0 on failure
+ */
+int bfd_transport_udp4 (vlib_main_t * vm, u32 bi,
+			const struct bfd_session_s *bs);
+
+/**
+ * @brief transport packet over udpv6
+ *
+ * @param is_echo 1 if this is echo packet, 0 if control frame
+ *
+ * @return 1 on success, 0 on failure
+ */
+int bfd_transport_udp6 (vlib_main_t * vm, u32 bi,
+			const struct bfd_session_s *bs);
+
+/**
  * @brief check if the bfd udp layer is echo-capable at this time
  *
  * @return 1 if available, 0 otherwise