From b755e9ff49e953cf9c35807240769e8822fa932b Mon Sep 17 00:00:00 2001
From: Joel Cunningham <joel.cunningham@me.com>
Date: Thu, 21 Jan 2016 15:28:51 -0600
Subject: [PATCH] Vectorize netconn_write for TCP

This commit adds support to the netconn write APIs to take an input of
vectors instead of a single data pointer

This allows vectors sent on a TCP connection via sendmsg to be treated
atomically.  The set of vectors is segmented into as much data as can
fit into the send buffer and then the TCP output function is called

Previously, each vector was passed to netconn_write_partly and tcp_write
segmented it into its own packet, which was then it was sent via tcp_output
(if not Nagleing)

This commit adds vector support to lwip_netconn_do_writemore() which
is the meat of the TCP write functionality from netconn/sockets layer.
A new netconn API netconn_write_vectors_partly() takes a set of vectors
as input and hooks up to do_writemore()
---
 src/api/api_lib.c               | 33 +++++++++++++++++++-
 src/api/api_msg.c               | 69 ++++++++++++++++++++++++++++-------------
 src/api/sockets.c               | 26 +++-------------
 src/include/lwip/api.h          |  7 ++++-
 src/include/lwip/priv/api_msg.h |  5 ++-
 5 files changed, 94 insertions(+), 46 deletions(-)

diff --git a/src/api/api_lib.c b/src/api/api_lib.c
index d209cb9..4e9546b 100644
--- a/src/api/api_lib.c
+++ b/src/api/api_lib.c
@@ -655,12 +655,42 @@ err_t
 netconn_write_partly(struct netconn *conn, const void *dataptr, size_t size,
                      u8_t apiflags, size_t *bytes_written)
 {
+  struct iovec vectors;
+  vectors.iov_base = (void*)dataptr;
+  vectors.iov_len = size;
+  return netconn_write_vectors_partly(conn, &vectors, 1, apiflags, bytes_written);
+}
+
+/**
+ * Send vectorized data atomically over a TCP netconn.
+ *
+ * @param conn the TCP netconn over which to send data
+ * @param vectors array of vectors containing data to send
+ * @param vectorcnt number of vectors in the array
+ * @param apiflags combination of following flags :
+ * - NETCONN_COPY: data will be copied into memory belonging to the stack
+ * - NETCONN_MORE: for TCP connection, PSH flag will be set on last segment sent
+ * - NETCONN_DONTBLOCK: only write the data if all data can be written at once
+ * @param bytes_written pointer to a location that receives the number of written bytes
+ * @return ERR_OK if data was sent, any other err_t on error
+ */
+err_t
+netconn_write_vectors_partly(struct netconn *conn, struct iovec *vectors, int vectorcnt,
+                     u8_t apiflags, size_t *bytes_written)
+{
   API_MSG_VAR_DECLARE(msg);
   err_t err;
   u8_t dontblock;
+  size_t size;
+  int i;
 
   LWIP_ERROR("netconn_write: invalid conn",  (conn != NULL), return ERR_ARG;);
   LWIP_ERROR("netconn_write: invalid conn->type",  (NETCONNTYPE_GROUP(conn->type)== NETCONN_TCP), return ERR_VAL;);
+  LWIP_ERROR("netconn_write: maximum vectors exceeded", (vectorcnt <= 0xFFF), return ERR_ARG;);
+  size = 0;
+  for (i = 0; i < vectorcnt; i++) {
+    size += vectors[i].iov_len;
+  }
   if (size == 0) {
     return ERR_OK;
   }
@@ -674,7 +704,8 @@ netconn_write_partly(struct netconn *conn, const void *dataptr, size_t size,
   API_MSG_VAR_ALLOC(msg);
   /* non-blocking write sends as much  */
   API_MSG_VAR_REF(msg).msg.conn = conn;
-  API_MSG_VAR_REF(msg).msg.msg.w.dataptr = dataptr;
+  API_MSG_VAR_REF(msg).msg.msg.w.vector = vectors;
+  API_MSG_VAR_REF(msg).msg.msg.w.vectorcnt = (u16_t)vectorcnt;
   API_MSG_VAR_REF(msg).msg.msg.w.apiflags = apiflags;
   API_MSG_VAR_REF(msg).msg.msg.w.len = size;
 #if LWIP_SO_SNDTIMEO
diff --git a/src/api/api_msg.c b/src/api/api_msg.c
index 6ed470a..b41c9bd 100644
--- a/src/api/api_msg.c
+++ b/src/api/api_msg.c
@@ -1422,6 +1422,7 @@ lwip_netconn_do_writemore(struct netconn *conn  WRITE_DELAYED_PARAM)
   size_t diff;
   u8_t dontblock;
   u8_t apiflags;
+  u8_t write_more;
 
   LWIP_ASSERT("conn != NULL", conn != NULL);
   LWIP_ASSERT("conn->state == NETCONN_WRITE", (conn->state == NETCONN_WRITE));
@@ -1429,6 +1430,7 @@ lwip_netconn_do_writemore(struct netconn *conn  WRITE_DELAYED_PARAM)
   LWIP_ASSERT("conn->pcb.tcp != NULL", conn->pcb.tcp != NULL);
   LWIP_ASSERT("conn->write_offset < conn->current_msg->msg.w.len",
     conn->write_offset < conn->current_msg->msg.w.len);
+  LWIP_ASSERT("conn->current_msg->msg.w.vectorcnt > 0", conn->current_msg->msg.w.vectorcnt > 0);
 
   dontblock = netconn_is_nonblocking(conn) ||
        (conn->current_msg->msg.w.apiflags & NETCONN_DONTBLOCK);
@@ -1451,33 +1453,56 @@ lwip_netconn_do_writemore(struct netconn *conn  WRITE_DELAYED_PARAM)
   } else
 #endif /* LWIP_SO_SNDTIMEO */
   {
-    dataptr = (const u8_t*)conn->current_msg->msg.w.dataptr + conn->write_offset;
-    diff = conn->current_msg->msg.w.len - conn->write_offset;
-    if (diff > 0xffffUL) { /* max_u16_t */
-      len = 0xffff;
-      apiflags |= TCP_WRITE_FLAG_MORE;
-    } else {
-      len = (u16_t)diff;
-    }
-    available = tcp_sndbuf(conn->pcb.tcp);
-    if (available < len) {
-      /* don't try to write more than sendbuf */
-      len = available;
-      if (dontblock) {
-        if (!len) {
-          err = ERR_WOULDBLOCK;
-          goto err_mem;
-        }
+    do {
+      dataptr = (const u8_t*)conn->current_msg->msg.w.vector->iov_base + conn->vector_offset;
+      diff = conn->current_msg->msg.w.vector->iov_len - conn->vector_offset;
+      if (diff > 0xffffUL) { /* max_u16_t */
+        len = 0xffff;
+        apiflags |= TCP_WRITE_FLAG_MORE;
       } else {
+        len = (u16_t)diff;
+      }
+      available = tcp_sndbuf(conn->pcb.tcp);
+      if (available < len) {
+        /* don't try to write more than sendbuf */
+        len = available;
+        if (dontblock) { 
+          if (!len) {
+            /* set error according to partial write or not */
+            err = (conn->write_offset == 0) ? ERR_WOULDBLOCK : ERR_OK;
+            goto err_mem;
+          }
+        } else {
+          apiflags |= TCP_WRITE_FLAG_MORE;
+        }
+      }
+      /* if we can finish this vector and have more, mark for another loop */
+      if ((len == (u16_t)diff) && (conn->current_msg->msg.w.vectorcnt > 1)) {
+        write_more = 1;
         apiflags |= TCP_WRITE_FLAG_MORE;
+      } else {
+        write_more = 0;
       }
-    }
-    LWIP_ASSERT("lwip_netconn_do_writemore: invalid length!", ((conn->write_offset + len) <= conn->current_msg->msg.w.len));
-    err = tcp_write(conn->pcb.tcp, dataptr, len, apiflags);
+      LWIP_ASSERT("lwip_netconn_do_writemore: invalid length!", ((conn->vector_offset + len) <= conn->current_msg->msg.w.vector->iov_len));
+      err = tcp_write(conn->pcb.tcp, dataptr, len, apiflags);
+      if (err == ERR_OK) {
+        conn->vector_offset += len;
+        conn->write_offset += len;
+        /* check if current vector is finished */
+        if (conn->vector_offset == conn->current_msg->msg.w.vector->iov_len) {
+          conn->current_msg->msg.w.vectorcnt--;
+          /* if we have additional vectors, move on to them */
+          if (conn->current_msg->msg.w.vectorcnt > 0) {
+            conn->current_msg->msg.w.vector++;
+            conn->vector_offset = 0;
+          }
+        }
+      }
+    } while (write_more && err == ERR_OK);
     /* if OK or memory error, check available space */
     if ((err == ERR_OK) || (err == ERR_MEM)) {
 err_mem:
-      if (dontblock && (len < conn->current_msg->msg.w.len)) {
+      if (dontblock && (conn->write_offset < conn->current_msg->msg.w.len)) {
         /* non-blocking write did not write everything: mark the pcb non-writable
            and let poll_tcp check writable space to mark the pcb writable again */
         API_EVENT(conn, NETCONN_EVT_SENDMINUS, len);
@@ -1492,7 +1517,6 @@ err_mem:
 
     if (err == ERR_OK) {
       err_t out_err;
-      conn->write_offset += len;
       if ((conn->write_offset == conn->current_msg->msg.w.len) || dontblock) {
         /* return sent length */
         conn->current_msg->msg.w.len = conn->write_offset;
@@ -1581,6 +1605,7 @@ lwip_netconn_do_write(struct api_msg_msg *msg)
         LWIP_ASSERT("msg->msg.w.len != 0", msg->msg.w.len != 0);
         msg->conn->current_msg = msg;
         msg->conn->write_offset = 0;
+        msg->conn->vector_offset = 0;
 #if LWIP_TCPIP_CORE_LOCKING
         if (lwip_netconn_do_writemore(msg->conn, 0) != ERR_OK) {
           LWIP_ASSERT("state!", msg->conn->state == NETCONN_WRITE);
diff --git a/src/api/sockets.c b/src/api/sockets.c
index db73b37..7110ea7 100644
--- a/src/api/sockets.c
+++ b/src/api/sockets.c
@@ -974,7 +974,9 @@ lwip_sendmsg(int s, const struct msghdr *msg, int flags)
   struct lwip_sock *sock;
   struct netbuf *chain_buf;
   u16_t remote_port;
+#if LWIP_NETIF_TX_SINGLE_PBUF
   int i;
+#endif
 #if LWIP_TCP
   u8_t write_flags;
   size_t written;
@@ -1001,28 +1003,10 @@ lwip_sendmsg(int s, const struct msghdr *msg, int flags)
     write_flags = NETCONN_COPY |
     ((flags & MSG_MORE)     ? NETCONN_MORE      : 0) |
     ((flags & MSG_DONTWAIT) ? NETCONN_DONTBLOCK : 0);
-
-    for (i = 0; i < msg->msg_iovlen; i++) {
-      written = 0;
-      err = netconn_write_partly(sock->conn, msg->msg_iov[i].iov_base, msg->msg_iov[i].iov_len, write_flags, &written);
-      if (err == ERR_OK) {
-        size += written;
-        /* check that the entire IO vector was accepected, if not return a partial write */
-        if (written != msg->msg_iov[i].iov_len)
-          break;
-      }
-      /* none of this IO vector was accepted, but previous was, return partial write and conceal ERR_WOULDBLOCK */
-      else if (err == ERR_WOULDBLOCK && size > 0) {
-        err = ERR_OK;
-        /* let ERR_WOULDBLOCK persist on the netconn since we are returning ERR_OK */
-        break;
-      } else {
-        size = -1;
-        break;
-      }
-    }
+    written = 0;
+    err = netconn_write_vectors_partly(sock->conn, msg->msg_iov, msg->msg_iovlen, write_flags, &written);
     sock_set_errno(sock, err_to_errno(err));
-    return size;
+    return (err == ERR_OK ? (int)written : -1);
 #else /* LWIP_TCP */
     sock_set_errno(sock, err_to_errno(ERR_ARG));
     return -1;
diff --git a/src/include/lwip/api.h b/src/include/lwip/api.h
index 8499edb..a713b05 100644
--- a/src/include/lwip/api.h
+++ b/src/include/lwip/api.h
@@ -222,9 +222,12 @@ struct netconn {
   u8_t flags;
 #if LWIP_TCP
   /** TCP: when data passed to netconn_write doesn't fit into the send buffer,
-      this temporarily stores how much is already sent. */
+      this temporarily stores how much of the total write has been sent. */
   size_t write_offset;
   /** TCP: when data passed to netconn_write doesn't fit into the send buffer,
+      this temporarily stores how much of the current vector has been sent */
+  size_t vector_offset;
+  /** TCP: when data passed to netconn_write doesn't fit into the send buffer,
       this temporarily stores the message.
       Also used during connect and close. */
   struct api_msg_msg *current_msg;
@@ -276,6 +279,8 @@ err_t   netconn_sendto(struct netconn *conn, struct netbuf *buf,
 err_t   netconn_send(struct netconn *conn, struct netbuf *buf);
 err_t   netconn_write_partly(struct netconn *conn, const void *dataptr, size_t size,
                              u8_t apiflags, size_t *bytes_written);
+err_t   netconn_write_vectors_partly(struct netconn *conn, struct iovec *vectors, int vectorcnt,
+                             u8_t apiflags, size_t *bytes_written);
 #define netconn_write(conn, dataptr, size, apiflags) \
           netconn_write_partly(conn, dataptr, size, apiflags, NULL)
 err_t   netconn_close(struct netconn *conn);
diff --git a/src/include/lwip/priv/api_msg.h b/src/include/lwip/priv/api_msg.h
index 909a925..3d386f0 100644
--- a/src/include/lwip/priv/api_msg.h
+++ b/src/include/lwip/priv/api_msg.h
@@ -102,7 +102,10 @@ struct api_msg_msg {
     } ad;
     /** used for lwip_netconn_do_write */
     struct {
-      const void *dataptr;
+      /** current vector to write */
+      struct iovec *vector;
+      /** number of unwritten vectors */
+      u16_t vectorcnt;
       size_t len;
       u8_t apiflags;
 #if LWIP_SO_SNDTIMEO
-- 
1.9.5.msysgit.0

