3 files changed, 555 insertions, 11 deletions

diff --git a/src/vnet/ip/punt.c b/src/vnet/ip/punt.c
index 48558401292..92f76b8d669 100644
--- a/src/vnet/ip/punt.c
+++ b/src/vnet/ip/punt.c
@@ -17,16 +17,27 @@
  * @file
  * @brief Local TCP/IP stack punt infrastructure.
  *
- * Provides a set of VPP nodes togather with the relevant APIs and CLI
+ * Provides a set of VPP nodes together with the relevant APIs and CLI
  * commands in order to adjust and dispatch packets from the VPP data plane
  * to the local TCP/IP stack
  */
+
+#include <vnet/ip/ip.h>
 #include <vlib/vlib.h>
 #include <vnet/pg/pg.h>
 #include <vnet/udp/udp.h>
 #include <vnet/ip/punt.h>
+#include <vppinfra/sparse_vec.h>
+#include <vlib/unix/unix.h>
+
+#include <stdio.h>
+#include <unistd.h>
+#include <sys/socket.h>
+#include <sys/un.h>
+#include <stdlib.h>
+#include <stdbool.h>
 
-#define foreach_punt_next \
+#define foreach_punt_next			\
   _ (PUNT, "error-punt")
 
 typedef enum
@@ -37,8 +48,28 @@ typedef enum
     PUNT_N_NEXT,
 } punt_next_t;
 
+enum punt_socket_rx_next_e
+{
+  PUNT_SOCKET_RX_NEXT_INTERFACE_OUTPUT,
+  PUNT_SOCKET_RX_NEXT_IP4_LOOKUP,
+  PUNT_SOCKET_RX_NEXT_IP6_LOOKUP,
+  PUNT_SOCKET_RX_N_NEXT
+};
+
 vlib_node_registration_t udp4_punt_node;
 vlib_node_registration_t udp6_punt_node;
+vlib_node_registration_t udp4_punt_socket_node;
+vlib_node_registration_t udp6_punt_socket_node;
+static vlib_node_registration_t punt_socket_rx_node;
+
+punt_main_t punt_main;
+
+char *
+vnet_punt_get_server_pathname (void)
+{
+  punt_main_t *pm = &punt_main;
+  return pm->sun_path;
+}
 
 /** @brief IPv4/IPv6 UDP punt node main loop.
 
@@ -194,10 +225,389 @@ VLIB_REGISTER_NODE (udp6_punt_node) = {
 #undef _
   },
 };
-/* *INDENT-ON* */
 
 VLIB_NODE_FUNCTION_MULTIARCH (udp6_punt_node, udp6_punt);;
 
+/* *INDENT-ON* */
+
+static struct sockaddr_un *
+punt_socket_get (bool is_ip4, u16 port)
+{
+  punt_main_t *pm = &punt_main;
+  punt_client_t *v = is_ip4 ? pm->clients_by_dst_port4 :
+    pm->clients_by_dst_port6;
+
+  u16 i = sparse_vec_index (v, port);
+  if (i == SPARSE_VEC_INVALID_INDEX)
+    return 0;
+
+  return &vec_elt (v, i).caddr;
+}
+
+static void
+punt_socket_register (bool is_ip4, u8 protocol, u16 port,
+		      char *client_pathname)
+{
+  punt_main_t *pm = &punt_main;
+  punt_client_t c, *n;
+  punt_client_t *v = is_ip4 ? pm->clients_by_dst_port4 :
+    pm->clients_by_dst_port6;
+
+  memset (&c, 0, sizeof (c));
+  memcpy (c.caddr.sun_path, client_pathname, sizeof (c.caddr.sun_path));
+  c.caddr.sun_family = AF_UNIX;
+  c.port = port;
+  n = sparse_vec_validate (v, port);
+  n[0] = c;
+}
+
+/* $$$$ Just leaves the mapping in place for now */
+static void
+punt_socket_unregister (bool is_ip4, u8 protocol, u16 port)
+{
+  return;
+}
+
+always_inline uword
+udp46_punt_socket_inline (vlib_main_t * vm,
+			  vlib_node_runtime_t * node,
+			  vlib_frame_t * frame, bool is_ip4)
+{
+  u32 *buffers = vlib_frame_args (frame);
+  uword n_packets = frame->n_vectors;
+  struct iovec *iovecs = 0;
+  punt_main_t *pm = &punt_main;
+  int i;
+
+  u32 node_index = is_ip4 ? udp4_punt_socket_node.index :
+    udp6_punt_socket_node.index;
+
+  for (i = 0; i < n_packets; i++)
+    {
+      struct iovec *iov;
+      vlib_buffer_t *b;
+      uword l;
+      punt_packetdesc_t packetdesc;
+
+      b = vlib_get_buffer (vm, buffers[i]);
+
+      /* Reverse UDP Punt advance */
+      udp_header_t *udp;
+      if (is_ip4)
+	{
+	  vlib_buffer_advance (b, -(sizeof (ip4_header_t) +
+				    sizeof (udp_header_t)));
+	  ip4_header_t *ip = vlib_buffer_get_current (b);
+	  udp = (udp_header_t *) (ip + 1);
+	}
+      else
+	{
+	  vlib_buffer_advance (b, -(sizeof (ip6_header_t) +
+				    sizeof (udp_header_t)));
+	  ip6_header_t *ip = vlib_buffer_get_current (b);
+	  udp = (udp_header_t *) (ip + 1);
+	}
+
+      u16 port = clib_net_to_host_u16 (udp->dst_port);
+
+      /*
+       * Find registerered client
+       * If no registered client, drop packet and count
+       */
+      struct sockaddr_un *caddr;
+      caddr = punt_socket_get (is_ip4, port);
+      if (!caddr)
+	{
+	  vlib_node_increment_counter (vm, node_index,
+				       PUNT_ERROR_SOCKET_TX_ERROR, 1);
+	  goto error;
+	}
+
+      /* Re-set iovecs if present. */
+      if (iovecs)
+	_vec_len (iovecs) = 0;
+
+      /* Add packet descriptor */
+      packetdesc.sw_if_index = vnet_buffer (b)->sw_if_index[VLIB_RX];
+      packetdesc.action = 0;
+      vec_add2 (iovecs, iov, 1);
+      iov->iov_base = &packetdesc;
+      iov->iov_len = sizeof (packetdesc);
+
+      /** VLIB buffer chain -> Unix iovec(s). */
+      vlib_buffer_advance (b, -(sizeof (ethernet_header_t)));
+      vec_add2 (iovecs, iov, 1);
+      iov->iov_base = b->data + b->current_data;
+      iov->iov_len = l = b->current_length;
+
+      if (PREDICT_FALSE (b->flags & VLIB_BUFFER_NEXT_PRESENT))
+	{
+	  do
+	    {
+	      b = vlib_get_buffer (vm, b->next_buffer);
+
+	      vec_add2 (iovecs, iov, 1);
+
+	      iov->iov_base = b->data + b->current_data;
+	      iov->iov_len = b->current_length;
+	      l += b->current_length;
+	    }
+	  while (b->flags & VLIB_BUFFER_NEXT_PRESENT);
+	}
+
+      struct msghdr msg = {
+	.msg_name = caddr,
+	.msg_namelen = sizeof (*caddr),
+	.msg_iov = iovecs,
+	.msg_iovlen = vec_len (iovecs),
+      };
+
+      if (sendmsg (pm->socket_fd, &msg, 0) < l)
+	vlib_node_increment_counter (vm, node_index,
+				     PUNT_ERROR_SOCKET_TX_ERROR, 1);
+    }
+
+error:
+  vlib_buffer_free_no_next (vm, buffers, n_packets);
+
+  return n_packets;
+}
+
+static uword
+udp4_punt_socket (vlib_main_t * vm,
+		  vlib_node_runtime_t * node, vlib_frame_t * from_frame)
+{
+  return udp46_punt_socket_inline (vm, node, from_frame, true /* is_ip4 */ );
+}
+
+static uword
+udp6_punt_socket (vlib_main_t * vm,
+		  vlib_node_runtime_t * node, vlib_frame_t * from_frame)
+{
+  return udp46_punt_socket_inline (vm, node, from_frame, false /* is_ip4 */ );
+}
+
+
+/* *INDENT-OFF* */
+VLIB_REGISTER_NODE (udp4_punt_socket_node) = {
+  .function = udp4_punt_socket,
+  .name = "ip4-udp-punt-socket",
+  .flags = VLIB_NODE_FLAG_IS_DROP,
+  /* Takes a vector of packets. */
+  .vector_size = sizeof (u32),
+  .n_errors = PUNT_N_ERROR,
+  .error_strings = punt_error_strings,
+};
+VLIB_REGISTER_NODE (udp6_punt_socket_node) = {
+  .function = udp6_punt_socket,
+  .name = "ip6-udp-punt-socket",
+  .flags = VLIB_NODE_FLAG_IS_DROP,
+  .vector_size = sizeof (u32),
+  .n_errors = PUNT_N_ERROR,
+  .error_strings = punt_error_strings,
+};
+/* *INDENT-ON* */
+
+typedef struct
+{
+  enum punt_action_e action;
+  u32 sw_if_index;
+} punt_trace_t;
+
+static u8 *
+format_punt_trace (u8 * s, va_list * va)
+{
+  CLIB_UNUSED (vlib_main_t * vm) = va_arg (*va, vlib_main_t *);
+  CLIB_UNUSED (vlib_node_t * node) = va_arg (*va, vlib_node_t *);
+  vnet_main_t *vnm = vnet_get_main ();
+  punt_trace_t *t = va_arg (*va, punt_trace_t *);
+  s = format (s, "%U Action: %d", format_vnet_sw_if_index_name,
+	      vnm, t->sw_if_index, t->action);
+  return s;
+}
+
+static uword
+punt_socket_rx_fd (vlib_main_t * vm, vlib_node_runtime_t * node, u32 fd)
+{
+  const uword buffer_size = VLIB_BUFFER_DATA_SIZE;
+  u32 n_trace = vlib_get_trace_count (vm, node);
+  u32 next = node->cached_next_index;
+  u32 n_left_to_next, next_index;
+  u32 *to_next;
+  u32 error = PUNT_ERROR_NONE;
+  vlib_get_next_frame (vm, node, next, to_next, n_left_to_next);
+
+  /* $$$$ Only dealing with one buffer at the time for now */
+
+  u32 bi;
+  vlib_buffer_t *b;
+  punt_packetdesc_t packetdesc;
+  ssize_t size;
+  struct iovec io[2];
+
+  if (vlib_buffer_alloc (vm, &bi, 1) != 1)
+    {
+      error = PUNT_ERROR_NOBUFFER;
+      goto error;
+    }
+
+  b = vlib_get_buffer (vm, bi);
+  io[0].iov_base = &packetdesc;
+  io[0].iov_len = sizeof (packetdesc);
+  io[1].iov_base = b->data;
+  io[1].iov_len = buffer_size;
+
+  size = readv (fd, io, 2);
+  /* We need at least the packet descriptor plus a header */
+  if (size <= (int) (sizeof (packetdesc) + sizeof (ip4_header_t)))
+    {
+      vlib_buffer_free (vm, &bi, 1);
+      error = PUNT_ERROR_READV;
+      goto error;
+    }
+
+  b->flags = VNET_BUFFER_F_LOCALLY_ORIGINATED;
+  b->current_length = size - sizeof (packetdesc);
+
+  VLIB_BUFFER_TRACE_TRAJECTORY_INIT (b);
+
+  switch (packetdesc.action)
+    {
+    case PUNT_L2:
+      vnet_buffer (b)->sw_if_index[VLIB_TX] = packetdesc.sw_if_index;
+      next_index = PUNT_SOCKET_RX_NEXT_INTERFACE_OUTPUT;
+      break;
+
+    case PUNT_IP4_ROUTED:
+      vnet_buffer (b)->sw_if_index[VLIB_RX] = packetdesc.sw_if_index;
+      vnet_buffer (b)->sw_if_index[VLIB_TX] = ~0;
+      next_index = PUNT_SOCKET_RX_NEXT_IP4_LOOKUP;
+      break;
+
+    case PUNT_IP6_ROUTED:
+      vnet_buffer (b)->sw_if_index[VLIB_RX] = packetdesc.sw_if_index;
+      vnet_buffer (b)->sw_if_index[VLIB_TX] = ~0;
+      next_index = PUNT_SOCKET_RX_NEXT_IP6_LOOKUP;
+      break;
+
+    default:
+      error = PUNT_ERROR_ACTION;
+      vlib_buffer_free (vm, &bi, 1);
+      goto error;
+    }
+
+  if (PREDICT_FALSE (n_trace > 0))
+    {
+      punt_trace_t *t;
+      vlib_trace_buffer (vm, node, next_index, b, 1 /* follow_chain */ );
+      vlib_set_trace_count (vm, node, --n_trace);
+      t = vlib_add_trace (vm, node, b, sizeof (*t));
+      t->sw_if_index = packetdesc.sw_if_index;
+      t->action = packetdesc.action;
+    }
+
+  to_next[0] = bi;
+  to_next++;
+  n_left_to_next--;
+
+  vlib_validate_buffer_enqueue_x1 (vm, node, next, to_next, n_left_to_next,
+				   bi, next_index);
+  vlib_put_next_frame (vm, node, next, n_left_to_next);
+  return 1;
+
+error:
+  vlib_node_increment_counter (vm, punt_socket_rx_node.index, error, 1);
+  return 0;
+}
+
+static uword
+punt_socket_rx (vlib_main_t * vm,
+		vlib_node_runtime_t * node, vlib_frame_t * frame)
+{
+  punt_main_t *pm = &punt_main;
+  u32 total_count = 0;
+  int i;
+
+  for (i = 0; i < vec_len (pm->ready_fds); i++)
+    {
+      total_count += punt_socket_rx_fd (vm, node, pm->ready_fds[i]);
+      vec_del1 (pm->ready_fds, i);
+    }
+  return total_count;
+}
+
+VLIB_REGISTER_NODE (punt_socket_rx_node, static) =
+{
+  .function = punt_socket_rx,.name = "punt-socket-rx",.type =
+    VLIB_NODE_TYPE_INPUT,.state = VLIB_NODE_STATE_INTERRUPT,.vector_size =
+    1,.n_errors = PUNT_N_ERROR,.error_strings =
+    punt_error_strings,.n_next_nodes = PUNT_SOCKET_RX_N_NEXT,.next_nodes =
+  {
+[PUNT_SOCKET_RX_NEXT_INTERFACE_OUTPUT] = "interface-output",
+      [PUNT_SOCKET_RX_NEXT_IP4_LOOKUP] = "ip4-lookup",
+      [PUNT_SOCKET_RX_NEXT_IP6_LOOKUP] = "ip6-lookup",},.format_trace =
+    format_punt_trace,};
+
+static clib_error_t *
+punt_socket_read_ready (unix_file_t * uf)
+{
+  vlib_main_t *vm = vlib_get_main ();
+  punt_main_t *pm = &punt_main;
+
+  /** Schedule the rx node */
+  vlib_node_set_interrupt_pending (vm, punt_socket_rx_node.index);
+  vec_add1 (pm->ready_fds, uf->file_descriptor);
+
+  return 0;
+}
+
+clib_error_t *
+vnet_punt_socket_add (vlib_main_t * vm, u32 header_version,
+		      bool is_ip4, u8 protocol, u16 port,
+		      char *client_pathname)
+{
+  punt_main_t *pm = &punt_main;
+
+  if (!pm->is_configured)
+    return clib_error_return (0, "socket is not configured");
+
+  if (header_version != PUNT_PACKETDESC_VERSION)
+    return clib_error_return (0, "Invalid packet descriptor version");
+
+  /* For now we only support UDP punt */
+  if (protocol != IP_PROTOCOL_UDP)
+    return clib_error_return (0,
+			      "only UDP protocol (%d) is supported, got %d",
+			      IP_PROTOCOL_UDP, protocol);
+
+  if (port == (u16) ~ 0)
+    return clib_error_return (0, "UDP port number required");
+
+  /* Register client */
+  punt_socket_register (is_ip4, protocol, port, client_pathname);
+
+  u32 node_index = is_ip4 ? udp4_punt_socket_node.index :
+    udp6_punt_socket_node.index;
+
+  udp_register_dst_port (vm, port, node_index, is_ip4);
+
+  return 0;
+}
+
+clib_error_t *
+vnet_punt_socket_del (vlib_main_t * vm, bool is_ip4, u8 l4_protocol, u16 port)
+{
+  punt_main_t *pm = &punt_main;
+
+  if (!pm->is_configured)
+    return clib_error_return (0, "socket is not configured");
+
+  punt_socket_unregister (is_ip4, l4_protocol, port);
+  udp_unregister_dst_port (vm, port, is_ip4);
+
+  return 0;
+}
+
 /**
  * @brief Request IP traffic punt to the local TCP/IP stack.
  *
@@ -217,7 +627,7 @@ VLIB_NODE_FUNCTION_MULTIARCH (udp6_punt_node, udp6_punt);;
  */
 clib_error_t *
 vnet_punt_add_del (vlib_main_t * vm, u8 ipv, u8 protocol, u16 port,
-		   int is_add)
+		   bool is_add)
 {
   /* For now we only support UDP punt */
   if (protocol != IP_PROTOCOL_UDP)
@@ -249,7 +659,6 @@ vnet_punt_add_del (vlib_main_t * vm, u8 ipv, u8 protocol, u16 port,
 
       return 0;
     }
-
   else
     return clib_error_return (0, "punt delete is not supported yet");
 }
@@ -259,13 +668,13 @@ udp_punt_cli (vlib_main_t * vm,
 	      unformat_input_t * input, vlib_cli_command_t * cmd)
 {
   u32 udp_port;
-  int is_add = 1;
+  bool is_add = true;
   clib_error_t *error;
 
   while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
     {
       if (unformat (input, "del"))
-	is_add = 0;
+	is_add = false;
       if (unformat (input, "all"))
 	{
 	  /* punt both IPv6 and IPv4 when used in CLI */
@@ -276,8 +685,8 @@ udp_punt_cli (vlib_main_t * vm,
       else if (unformat (input, "%d", &udp_port))
 	{
 	  /* punt both IPv6 and IPv4 when used in CLI */
-	  error = vnet_punt_add_del (vm, ~0, IP_PROTOCOL_UDP,
-				     udp_port, is_add);
+	  error =
+	    vnet_punt_add_del (vm, ~0, IP_PROTOCOL_UDP, udp_port, is_add);
 	  if (error)
 	    clib_error_report (error);
 	}
@@ -314,6 +723,85 @@ VLIB_CLI_COMMAND (punt_udp_command, static) = {
 };
 /* *INDENT-ON* */
 
+clib_error_t *
+punt_init (vlib_main_t * vm)
+{
+  punt_main_t *pm = &punt_main;
+
+  pm->clients_by_dst_port6 = sparse_vec_new
+    (sizeof (pm->clients_by_dst_port6[0]),
+     BITS (((udp_header_t *) 0)->dst_port));
+  pm->clients_by_dst_port4 = sparse_vec_new
+    (sizeof (pm->clients_by_dst_port4[0]),
+     BITS (((udp_header_t *) 0)->dst_port));
+
+  pm->is_configured = false;
+  pm->interface_output_node = vlib_get_node_by_name (vm,
+						     (u8 *)
+						     "interface-output");
+  return 0;
+}
+
+VLIB_INIT_FUNCTION (punt_init);
+
+static clib_error_t *
+punt_config (vlib_main_t * vm, unformat_input_t * input)
+{
+  punt_main_t *pm = &punt_main;
+  char *socket_path = 0;
+
+  while (unformat_check_input (input) != UNFORMAT_END_OF_INPUT)
+    {
+      if (unformat (input, "socket %s", &socket_path))
+	strncpy (pm->sun_path, socket_path, 108 - 1);
+      else
+	return clib_error_return (0, "unknown input `%U'",
+				  format_unformat_error, input);
+    }
+
+  if (socket_path == 0)
+    return 0;
+
+  /* UNIX domain socket */
+  struct sockaddr_un addr;
+  if ((pm->socket_fd = socket (AF_UNIX, SOCK_DGRAM | SOCK_NONBLOCK, 0)) == -1)
+    {
+      return clib_error_return (0, "socket error");
+    }
+
+  memset (&addr, 0, sizeof (addr));
+  addr.sun_family = AF_UNIX;
+  if (*socket_path == '\0')
+    {
+      *addr.sun_path = '\0';
+      strncpy (addr.sun_path + 1, socket_path + 1,
+	       sizeof (addr.sun_path) - 2);
+    }
+  else
+    {
+      strncpy (addr.sun_path, socket_path, sizeof (addr.sun_path) - 1);
+      unlink (socket_path);
+    }
+
+  if (bind (pm->socket_fd, (struct sockaddr *) &addr, sizeof (addr)) == -1)
+    {
+      return clib_error_return (0, "bind error");
+    }
+
+  /* Register socket */
+  unix_main_t *um = &unix_main;
+  unix_file_t template = { 0 };
+  template.read_function = punt_socket_read_ready;
+  template.file_descriptor = pm->socket_fd;
+  pm->unix_file_index = unix_file_add (um, &template);
+
+  pm->is_configured = true;
+
+  return 0;
+}
+
+VLIB_CONFIG_FUNCTION (punt_config, "punt");
+
 /*
  * fd.io coding-style-patch-verification: ON
  *
diff --git a/src/vnet/ip/punt.h b/src/vnet/ip/punt.h
index 09a9d4c55bf..0103249cca0 100644
--- a/src/vnet/ip/punt.h
+++ b/src/vnet/ip/punt.h
@@ -20,6 +20,7 @@
 #ifndef included_punt_h
 #define included_punt_h
 
+#include <sys/un.h>
 typedef enum
 {
 #define punt_error(n,s) PUNT_ERROR_##n,
@@ -30,9 +31,56 @@ typedef enum
 
 
 clib_error_t *vnet_punt_add_del (vlib_main_t * vm, u8 ipv,
-				 u8 protocol, u16 port, int is_add);
+				 u8 protocol, u16 port, bool is_add);
+clib_error_t *vnet_punt_socket_add (vlib_main_t * vm, u32 header_version,
+				    bool is_ip4, u8 protocol, u16 port,
+				    char *client_pathname);
+clib_error_t *vnet_punt_socket_del (vlib_main_t * vm, bool is_ip4,
+				    u8 l4_protocol, u16 port);
+char *vnet_punt_get_server_pathname (void);
 
-#endif /* included_punt_h */
+enum punt_action_e
+{
+  PUNT_L2 = 0,
+  PUNT_IP4_ROUTED,
+  PUNT_IP6_ROUTED,
+};
+
+/*
+ * Packet descriptor header. Version 1
+ * If this header changes, the version must also change to notify clients.
+ */
+#define PUNT_PACKETDESC_VERSION 1
+typedef struct __attribute__ ((packed))
+{
+  u32 sw_if_index;		/* RX or TX interface */
+  enum punt_action_e action;
+} punt_packetdesc_t;
+
+/*
+ * Client registration
+ */
+typedef struct
+{
+  u16 port;
+  struct sockaddr_un caddr;
+} punt_client_t;
+
+typedef struct
+{
+  int socket_fd;
+  char sun_path[sizeof (struct sockaddr_un)];
+  punt_client_t *clients_by_dst_port4;
+  punt_client_t *clients_by_dst_port6;
+  u32 unix_file_index;
+  bool is_configured;
+  vlib_node_t *interface_output_node;
+  u32 *ready_fds;
+  u32 *rx_buffers;
+} punt_main_t;
+extern punt_main_t punt_main;
+
+#endif
 
 /*
  * fd.io coding-style-patch-verification: ON
diff --git a/src/vnet/ip/punt_error.def b/src/vnet/ip/punt_error.def
index a76d7e7b817..13afa2c7f6e 100644
--- a/src/vnet/ip/punt_error.def
+++ b/src/vnet/ip/punt_error.def
@@ -17,3 +17,11 @@
 
 punt_error (NONE, "no error")
 punt_error (UDP_PORT, "udp port punt")
+punt_error (SOCKET_RX, "Socket RX")
+punt_error (SOCKET_TX, "Socket TX")
+punt_error (SOCKET_RX_ERROR, "Socket RX error")
+punt_error (SOCKET_TX_ERROR, "Socket TX error")
+punt_error (NOBUFFER, "buffer allocation failure")
+punt_error (READV, "socket read failure")
+punt_error (ACTION, "invalid packet descriptor")
+