[PATCH] rtems_termios_puts: Copy and write more than one char at once

[Kolja Waschk]

---
 cpukit/libcsupport/src/termios.c | 74 +++++++++++++++++++++++++++-------------
 1 file changed, 51 insertions(+), 23 deletions(-)

diff --git a/cpukit/libcsupport/src/termios.c b/cpukit/libcsupport/src/termios.c
index 2448ea1..d32ab74 100644
--- a/cpukit/libcsupport/src/termios.c
+++ b/cpukit/libcsupport/src/termios.c
@@ -879,7 +879,7 @@ rtems_termios_ioctl (void *arg)
       tty->rawInBufSemaphoreTimeout = RTEMS_NO_TIMEOUT;
       tty->rawInBufSemaphoreFirstTimeout = RTEMS_NO_TIMEOUT;
     } else {
-      tty->vtimeTicks = tty->termios.c_cc[VTIME] * 
+      tty->vtimeTicks = tty->termios.c_cc[VTIME] *
                     rtems_clock_get_ticks_per_second() / 10;
       if (tty->termios.c_cc[VTIME]) {
         tty->rawInBufSemaphoreOptions = RTEMS_WAIT;
@@ -984,26 +984,19 @@ rtems_termios_puts (
   unsigned int newHead;
   rtems_interrupt_lock_context lock_context;
   rtems_status_code sc;
+  size_t partlen;
 
   if (tty->handler.mode == TERMIOS_POLLED) {
     (*tty->handler.write)(tty, buf, len);
     return;
   }
-  newHead = tty->rawOutBuf.Head;
+
   while (len) {
-    /*
-     * Performance improvement could be made here.
-     * Copy multiple bytes to raw buffer:
-     * if (len > 1) && (space to buffer end, or tail > 1)
-     *  ncopy = MIN (len, space to buffer end or tail)
-     *  memcpy (raw buffer, buf, ncopy)
-     *  buf += ncopy
-     *  len -= ncopy
-     *
-     * To minimize latency, the memcpy should be done
-     * with interrupts enabled.
-     */
-    newHead = (newHead + 1) % tty->rawOutBuf.Size;
+    /* Check space for at least one char */
+    newHead = tty->rawOutBuf.Head + 1;
+    if (newHead >= tty->rawOutBuf.Size)
+      newHead -= tty->rawOutBuf.Size;
+
     rtems_termios_interrupt_lock_acquire (tty, &lock_context);
     while (newHead == tty->rawOutBuf.Tail) {
       tty->rawOutBufState = rob_wait;
@@ -1014,21 +1007,56 @@ rtems_termios_puts (
         rtems_fatal_error_occurred (sc);
       rtems_termios_interrupt_lock_acquire (tty, &lock_context);
     }
-    tty->rawOutBuf.theBuf[tty->rawOutBuf.Head] = *buf++;
+
+    /* Copy as much chars as fit until current tail or end of ring buffer */
+    partlen = len;
+    if (tty->rawOutBuf.Tail > tty->rawOutBuf.Head) {
+      /* Available space is contiguous from Head to Tail */
+      size_t available = tty->rawOutBuf.Tail - tty->rawOutBuf.Head - 1;
+      if (partlen > available)
+        partlen = available;
+    } else {
+      /* Available space wraps at buffer end. To keep code simple, utilize
+         only the part from Head to end during this iteration */
+      size_t available = tty->rawOutBuf.Size - tty->rawOutBuf.Head;
+      if (partlen > available)
+        partlen = available;
+    }
+
+    /* To minimize latency, the memcpy should be done
+     * with interrupts enabled (TBD) */
+    memcpy(&tty->rawOutBuf.theBuf[tty->rawOutBuf.Head], buf, partlen);
+    newHead = tty->rawOutBuf.Head + partlen;
+    if (newHead >= tty->rawOutBuf.Size)
+      newHead -= tty->rawOutBuf.Size;
     tty->rawOutBuf.Head = newHead;
+    buf += partlen;
+
     if (tty->rawOutBufState == rob_idle) {
-      /* check, whether XOFF has been received */
-      if (!(tty->flow_ctrl & FL_ORCVXOF)) {
-        (*tty->handler.write)(
-          tty, &tty->rawOutBuf.theBuf[tty->rawOutBuf.Tail],1);
+      if (tty->flow_ctrl & FL_MDXOF) {
+        /* Write only one byte at a time if using XON/XOFF flow ctrl */
+        /* check, whether XOFF has been received */
+        if (!(tty->flow_ctrl & FL_ORCVXOF)) {
+          (*tty->handler.write)(
+            tty, &tty->rawOutBuf.theBuf[tty->rawOutBuf.Tail], 1);
+        } else {
+          /* remember that output has been stopped due to flow ctrl*/
+          tty->flow_ctrl |= FL_OSTOP;
+        }
       } else {
-        /* remember that output has been stopped due to flow ctrl*/
-        tty->flow_ctrl |= FL_OSTOP;
+        size_t nwaiting;
+        if (tty->rawOutBuf.Head > tty->rawOutBuf.Tail)
+          nwaiting = tty->rawOutBuf.Head - tty->rawOutBuf.Tail;
+        else
+          nwaiting = tty->rawOutBuf.Size - tty->rawOutBuf.Tail;
+
+        (*tty->handler.write)(
+          tty, &tty->rawOutBuf.theBuf[tty->rawOutBuf.Tail], nwaiting);
       }
       tty->rawOutBufState = rob_busy;
     }
     rtems_termios_interrupt_lock_release (tty, &lock_context);
-    len--;
+    len -= partlen;
   }
 }
 
-- 
1.9.1