4 files changed, 223 insertions, 82 deletions

diff --git a/src/vnet/bfd/bfd_main.c b/src/vnet/bfd/bfd_main.c
index 62be1842723..7e1a2ef24a7 100644
--- a/src/vnet/bfd/bfd_main.c
+++ b/src/vnet/bfd/bfd_main.c
@@ -34,16 +34,9 @@ bfd_us_to_clocks (bfd_main_t * bm, u64 us)
 
 static vlib_node_registration_t bfd_process_node;
 
-typedef enum
-{
-#define F(t, n) BFD_OUTPUT_##t,
-  foreach_bfd_transport (F)
-#undef F
-    BFD_OUTPUT_N_NEXT,
-} bfd_output_next_t;
-
-static u32 bfd_next_index_by_transport[] = {
-#define F(t, n) [BFD_TRANSPORT_##t] = BFD_OUTPUT_##t,
+/* 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
 };
@@ -378,7 +371,7 @@ bfd_input_format_trace (u8 * s, va_list * args)
 		      clib_net_to_host_u32 (pkt->des_min_tx));
 	  s = format (s, "    required min rx interval: %u\n",
 		      clib_net_to_host_u32 (pkt->req_min_rx));
-	  s = format (s, "    required min echo rx interval: %u\n",
+	  s = format (s, "    required min echo rx interval: %u",
 		      clib_net_to_host_u32 (pkt->req_min_echo_rx));
 	}
     }
@@ -426,10 +419,12 @@ bfd_add_transport_layer (vlib_main_t * vm, vlib_buffer_t * b,
   switch (bs->transport)
     {
     case BFD_TRANSPORT_UDP4:
-      /* fallthrough */
+      BFD_DBG ("Transport bfd via udp4, bs_idx=%u", bs->bs_idx);
+      bfd_add_udp4_transport (vm, b, &bs->udp);
+      break;
     case BFD_TRANSPORT_UDP6:
-      BFD_DBG ("Transport bfd via udp, bs_idx=%u", bs->bs_idx);
-      bfd_add_udp_transport (vm, b, &bs->udp);
+      BFD_DBG ("Transport bfd via udp6, bs_idx=%u", bs->bs_idx);
+      bfd_add_udp6_transport (vm, b, &bs->udp);
       break;
     }
 }
@@ -448,17 +443,14 @@ bfd_create_frame (vlib_main_t * vm, vlib_node_runtime_t * rt,
   vlib_buffer_t *b = vlib_get_buffer (vm, bi);
   ASSERT (b->current_data == 0);
 
-  u32 *to_next;
-  u32 n_left_to_next;
-
-  vlib_get_next_frame (vm, rt, bfd_next_index_by_transport[bs->transport],
-		       to_next, n_left_to_next);
+  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;
-  n_left_to_next -= 1;
+  f->n_vectors = 1;
 
-  vlib_put_next_frame (vm, rt, bfd_next_index_by_transport[bs->transport],
-		       n_left_to_next);
+  vlib_put_frame_to_node (vm, bfd_node_index_by_transport[bs->transport], f);
   return b;
 }
 
@@ -680,13 +672,8 @@ VLIB_REGISTER_NODE (bfd_process_node, static) = {
   .function = bfd_process,
   .type = VLIB_NODE_TYPE_PROCESS,
   .name = "bfd-process",
-  .n_next_nodes = BFD_OUTPUT_N_NEXT,
-  .next_nodes =
-      {
-#define F(t, n) [BFD_OUTPUT_##t] = n,
-          foreach_bfd_transport (F)
-#undef F
-      },
+  .n_next_nodes = 0,
+  .next_nodes = {},
 };
 /* *INDENT-ON* */
 
@@ -734,6 +721,13 @@ bfd_main_init (vlib_main_t * vm)
   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;
 }
 
diff --git a/src/vnet/bfd/bfd_main.h b/src/vnet/bfd/bfd_main.h
index cc82c839ce9..20da381ac23 100644
--- a/src/vnet/bfd/bfd_main.h
+++ b/src/vnet/bfd/bfd_main.h
@@ -25,7 +25,7 @@
 #include <vnet/bfd/bfd_udp.h>
 
 #define foreach_bfd_transport(F) \
-  F (UDP4, "ip4-rewrite")  \
+  F (UDP4, "ip4-rewrite") \
   F (UDP6, "ip6-rewrite")
 
 typedef enum
diff --git a/src/vnet/bfd/bfd_udp.c b/src/vnet/bfd/bfd_udp.c
index c1596bf6012..fe348404472 100644
--- a/src/vnet/bfd/bfd_udp.c
+++ b/src/vnet/bfd/bfd_udp.c
@@ -31,53 +31,80 @@ static vlib_node_registration_t bfd_udp6_input_node;
 
 bfd_udp_main_t bfd_udp_main;
 
-void bfd_udp_transport_to_buffer (vlib_main_t *vm, vlib_buffer_t *b,
-                                  bfd_udp_session_t *bus)
+void bfd_add_udp4_transport (vlib_main_t *vm, vlib_buffer_t *b,
+                             bfd_udp_session_t *bus)
 {
   udp_header_t *udp;
-  u16 udp_length, ip_length;
-  bfd_udp_key_t *key = &bus->key;
+  const bfd_udp_key_t *key = &bus->key;
 
   b->flags |= VNET_BUFFER_LOCALLY_ORIGINATED;
-  if (ip46_address_is_ip4 (&key->local_addr))
-    {
-      ip4_header_t *ip4;
-      const size_t data_size = sizeof (*ip4) + sizeof (*udp);
-      vlib_buffer_advance (b, -data_size);
-      ip4 = vlib_buffer_get_current (b);
-      udp = (udp_header_t *)(ip4 + 1);
-      memset (ip4, 0, data_size);
-      ip4->ip_version_and_header_length = 0x45;
-      ip4->ttl = 255;
-      ip4->protocol = IP_PROTOCOL_UDP;
-      ip4->src_address.as_u32 = key->local_addr.ip4.as_u32;
-      ip4->dst_address.as_u32 = key->peer_addr.ip4.as_u32;
-
-      udp->src_port = clib_host_to_net_u16 (50000); /* FIXME */
-      udp->dst_port = clib_host_to_net_u16 (UDP_DST_PORT_bfd4);
-
-      /* fix ip length, checksum and udp length */
-      ip_length = vlib_buffer_length_in_chain (vm, b);
-
-      ip4->length = clib_host_to_net_u16 (ip_length);
-      ip4->checksum = ip4_header_checksum (ip4);
-
-      udp_length = ip_length - (sizeof (*ip4));
-      udp->length = clib_host_to_net_u16 (udp_length);
-    }
-  else
-    {
-      BFD_ERR ("not implemented");
-      abort ();
-    }
+  vnet_buffer (b)->ip.adj_index[VLIB_RX] = bus->adj_index;
+  vnet_buffer (b)->ip.adj_index[VLIB_TX] = bus->adj_index;
+  ip4_header_t *ip4;
+  const size_t headers_size = sizeof (*ip4) + sizeof (*udp);
+  vlib_buffer_advance (b, -headers_size);
+  ip4 = vlib_buffer_get_current (b);
+  udp = (udp_header_t *)(ip4 + 1);
+  memset (ip4, 0, headers_size);
+  ip4->ip_version_and_header_length = 0x45;
+  ip4->ttl = 255;
+  ip4->protocol = IP_PROTOCOL_UDP;
+  ip4->src_address.as_u32 = key->local_addr.ip4.as_u32;
+  ip4->dst_address.as_u32 = key->peer_addr.ip4.as_u32;
+
+  udp->src_port = clib_host_to_net_u16 (50000); /* FIXME */
+  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);
+
+  ip4->length = clib_host_to_net_u16 (ip_length);
+  ip4->checksum = ip4_header_checksum (ip4);
+
+  const u16 udp_length = ip_length - (sizeof (*ip4));
+  udp->length = clib_host_to_net_u16 (udp_length);
 }
 
-void bfd_add_udp_transport (vlib_main_t *vm, vlib_buffer_t *b,
-                            bfd_udp_session_t *bus)
+void bfd_add_udp6_transport (vlib_main_t *vm, vlib_buffer_t *b,
+                             bfd_udp_session_t *bus)
 {
+  udp_header_t *udp;
+  const bfd_udp_key_t *key = &bus->key;
+
+  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;
-  bfd_udp_transport_to_buffer (vm, b, bus);
+  ip6_header_t *ip6;
+  const size_t headers_size = sizeof (*ip6) + sizeof (*udp);
+  vlib_buffer_advance (b, -headers_size);
+  ip6 = vlib_buffer_get_current (b);
+  udp = (udp_header_t *)(ip6 + 1);
+  memset (ip6, 0, headers_size);
+  ip6->ip_version_traffic_class_and_flow_label =
+      clib_host_to_net_u32 (0x6 << 28);
+  ip6->hop_limit = 255;
+  ip6->protocol = IP_PROTOCOL_UDP;
+  clib_memcpy (&ip6->src_address, &key->local_addr.ip6,
+               sizeof (ip6->src_address));
+  clib_memcpy (&ip6->dst_address, &key->peer_addr.ip6,
+               sizeof (ip6->dst_address));
+
+  udp->src_port = clib_host_to_net_u16 (50000); /* FIXME */
+  udp->dst_port = clib_host_to_net_u16 (UDP_DST_PORT_bfd6);
+
+  /* fix ip payload length and udp length */
+  const u16 udp_length = vlib_buffer_length_in_chain (vm, b) - (sizeof (*ip6));
+  udp->length = clib_host_to_net_u16 (udp_length);
+  ip6->payload_length = udp->length;
+
+  /* IPv6 UDP checksum is mandatory */
+  int bogus = 0;
+  udp->checksum = ip6_tcp_udp_icmp_compute_checksum (vm, b, ip6, &bogus);
+  ASSERT (bogus == 0);
+  if (udp->checksum == 0)
+    {
+      udp->checksum = 0xffff;
+    }
 }
 
 static bfd_session_t *bfd_lookup_session (bfd_udp_main_t *bum,
@@ -345,29 +372,29 @@ static bfd_udp_error_t bfd_udp4_verify_transport (const ip4_header_t *ip4,
   const bfd_udp_key_t *key = &bus->key;
   if (ip4->src_address.as_u32 != key->peer_addr.ip4.as_u32)
     {
-      BFD_ERR ("IP src addr mismatch, got %U, expected %U", format_ip4_address,
-               ip4->src_address.as_u32, format_ip4_address,
-               key->peer_addr.ip4.as_u32);
+      BFD_ERR ("IPv4 src addr mismatch, got %U, expected %U",
+               format_ip4_address, ip4->src_address.as_u8, format_ip4_address,
+               key->peer_addr.ip4.as_u8);
       return BFD_UDP_ERROR_BAD;
     }
   if (ip4->dst_address.as_u32 != key->local_addr.ip4.as_u32)
     {
-      BFD_ERR ("IP dst addr mismatch, got %U, expected %U", format_ip4_address,
-               ip4->dst_address.as_u32, format_ip4_address,
-               key->local_addr.ip4.as_u32);
+      BFD_ERR ("IPv4 dst addr mismatch, got %U, expected %U",
+               format_ip4_address, ip4->dst_address.as_u8, format_ip4_address,
+               key->local_addr.ip4.as_u8);
       return BFD_UDP_ERROR_BAD;
     }
   const u8 expected_ttl = 255;
   if (ip4->ttl != expected_ttl)
     {
-      BFD_ERR ("IP unexpected TTL value %d, expected %d", ip4->ttl,
+      BFD_ERR ("IPv4 unexpected TTL value %u, expected %u", ip4->ttl,
                expected_ttl);
       return BFD_UDP_ERROR_BAD;
     }
   if (clib_net_to_host_u16 (udp->src_port) < 49152 ||
       clib_net_to_host_u16 (udp->src_port) > 65535)
     {
-      BFD_ERR ("Invalid UDP src port %d, out of range <49152,65535>",
+      BFD_ERR ("Invalid UDP src port %u, out of range <49152,65535>",
                udp->src_port);
     }
   return BFD_UDP_ERROR_NONE;
@@ -460,10 +487,128 @@ static bfd_udp_error_t bfd_udp4_scan (vlib_main_t *vm, vlib_node_runtime_t *rt,
   return BFD_UDP_ERROR_NONE;
 }
 
-static bfd_udp_error_t bfd_udp6_scan (vlib_main_t *vm, vlib_buffer_t *b)
+static void bfd_udp6_find_headers (vlib_buffer_t *b, const ip6_header_t **ip6,
+                                   const udp_header_t **udp)
+{
+  /* sanity check first */
+  const i32 start = vnet_buffer (b)->ip.start_of_ip_header;
+  if (start < 0 && start < sizeof (b->pre_data))
+    {
+      BFD_ERR ("Start of ip header is before pre_data, ignoring");
+      *ip6 = NULL;
+      *udp = NULL;
+      return;
+    }
+  *ip6 = (ip6_header_t *)(b->data + start);
+  if ((u8 *)*ip6 > (u8 *)vlib_buffer_get_current (b))
+    {
+      BFD_ERR ("Start of ip header is beyond current data, ignoring");
+      *ip6 = NULL;
+      *udp = NULL;
+      return;
+    }
+  *udp = (udp_header_t *)((*ip6) + 1);
+}
+
+static bfd_udp_error_t bfd_udp6_verify_transport (const ip6_header_t *ip6,
+                                                  const udp_header_t *udp,
+                                                  const bfd_session_t *bs)
 {
-  /* TODO */
-  return BFD_UDP_ERROR_BAD;
+  const bfd_udp_session_t *bus = &bs->udp;
+  const bfd_udp_key_t *key = &bus->key;
+  if (ip6->src_address.as_u64[0] != key->peer_addr.ip6.as_u64[0] &&
+      ip6->src_address.as_u64[1] != key->peer_addr.ip6.as_u64[1])
+    {
+      BFD_ERR ("IP src addr mismatch, got %U, expected %U", format_ip6_address,
+               ip6, format_ip6_address, &key->peer_addr.ip6);
+      return BFD_UDP_ERROR_BAD;
+    }
+  if (ip6->dst_address.as_u64[0] != key->local_addr.ip6.as_u64[0] &&
+      ip6->dst_address.as_u64[1] != key->local_addr.ip6.as_u64[1])
+    {
+      BFD_ERR ("IP dst addr mismatch, got %U, expected %U", format_ip6_address,
+               ip6, format_ip6_address, &key->local_addr.ip6);
+      return BFD_UDP_ERROR_BAD;
+    }
+  const u8 expected_hop_limit = 255;
+  if (ip6->hop_limit != expected_hop_limit)
+    {
+      BFD_ERR ("IPv6 unexpected hop-limit value %u, expected %u",
+               ip6->hop_limit, expected_hop_limit);
+      return BFD_UDP_ERROR_BAD;
+    }
+  if (clib_net_to_host_u16 (udp->src_port) < 49152 ||
+      clib_net_to_host_u16 (udp->src_port) > 65535)
+    {
+      BFD_ERR ("Invalid UDP src port %u, out of range <49152,65535>",
+               udp->src_port);
+    }
+  return BFD_UDP_ERROR_NONE;
+}
+
+static bfd_udp_error_t bfd_udp6_scan (vlib_main_t *vm, vlib_node_runtime_t *rt,
+                                      vlib_buffer_t *b, bfd_session_t **bs_out)
+{
+  const bfd_pkt_t *pkt = vlib_buffer_get_current (b);
+  if (sizeof (*pkt) > b->current_length)
+    {
+      BFD_ERR (
+          "Payload size %d too small to hold bfd packet of minimum size %d",
+          b->current_length, sizeof (*pkt));
+      return BFD_UDP_ERROR_BAD;
+    }
+  const ip6_header_t *ip6;
+  const udp_header_t *udp;
+  bfd_udp6_find_headers (b, &ip6, &udp);
+  if (!ip6 || !udp)
+    {
+      BFD_ERR ("Couldn't find ip6 or udp header");
+      return BFD_UDP_ERROR_BAD;
+    }
+  if (!bfd_verify_pkt_common (pkt))
+    {
+      return BFD_UDP_ERROR_BAD;
+    }
+  bfd_session_t *bs = NULL;
+  if (pkt->your_disc)
+    {
+      BFD_DBG ("Looking up BFD session using discriminator %u",
+               pkt->your_disc);
+      bs = bfd_find_session_by_disc (bfd_udp_main.bfd_main, pkt->your_disc);
+    }
+  else
+    {
+      bfd_udp_key_t key;
+      memset (&key, 0, sizeof (key));
+      key.sw_if_index = vnet_buffer (b)->sw_if_index[VLIB_RX];
+      key.local_addr.ip6.as_u64[0] = ip6->dst_address.as_u64[0];
+      key.local_addr.ip6.as_u64[1] = ip6->dst_address.as_u64[1];
+      key.peer_addr.ip6.as_u64[0] = ip6->src_address.as_u64[0];
+      key.peer_addr.ip6.as_u64[1] = ip6->src_address.as_u64[1];
+      BFD_DBG ("Looking up BFD session using key (sw_if_index=%u, local=%U, "
+               "peer=%U)",
+               key.sw_if_index, format_ip6_address, &key.local_addr,
+               format_ip6_address, &key.peer_addr);
+      bs = bfd_lookup_session (&bfd_udp_main, &key);
+    }
+  if (!bs)
+    {
+      BFD_ERR ("BFD session lookup failed - no session matches BFD pkt");
+      return BFD_UDP_ERROR_BAD;
+    }
+  BFD_DBG ("BFD session found, bs_idx=%u", bs->bs_idx);
+  if (!bfd_verify_pkt_session (pkt, b->current_length, bs))
+    {
+      return BFD_UDP_ERROR_BAD;
+    }
+  bfd_udp_error_t err;
+  if (BFD_UDP_ERROR_NONE != (err = bfd_udp6_verify_transport (ip6, udp, bs)))
+    {
+      return err;
+    }
+  bfd_rpc_update_session (bs->bs_idx, pkt);
+  *bs_out = bs;
+  return BFD_UDP_ERROR_NONE;
 }
 
 /*
@@ -504,7 +649,7 @@ static uword bfd_udp_input (vlib_main_t *vm, vlib_node_runtime_t *rt,
       /* scan this bfd pkt. error0 is the counter index to bmp */
       if (is_ipv6)
         {
-          error0 = bfd_udp6_scan (vm, b0);
+          error0 = bfd_udp6_scan (vm, rt, b0, &bs);
         }
       else
         {
diff --git a/src/vnet/bfd/bfd_udp.h b/src/vnet/bfd/bfd_udp.h
index 51f5327be01..2cd89ca28ce 100644
--- a/src/vnet/bfd/bfd_udp.h
+++ b/src/vnet/bfd/bfd_udp.h
@@ -42,8 +42,10 @@ typedef struct
   adj_index_t adj_index;
 } bfd_udp_session_t;
 
-void bfd_add_udp_transport (vlib_main_t * vm, vlib_buffer_t * b,
-			    bfd_udp_session_t * bs);
+void bfd_add_udp4_transport (vlib_main_t * vm, vlib_buffer_t * b,
+			     bfd_udp_session_t * bs);
+void bfd_add_udp6_transport (vlib_main_t * vm, vlib_buffer_t * b,
+			     bfd_udp_session_t * bs);
 
 #endif /* __included_bfd_udp_h__ */