Index: c/src/libchip/serial/ns16550.c
===================================================================
--- c/src/libchip/serial/ns16550.c	(revision 84d7499a2b62893922661192ede2ac48857dda99)
+++ c/src/libchip/serial/ns16550.c	(revision ec5d45053b71f052d978e423f08ad4e28e1b800d)
@@ -31,4 +31,5 @@
 #include <rtems/ringbuf.h>
 #include <rtems/bspIo.h>
+#include <rtems/termiostypes.h>
 
 #include <libchip/serial.h>
@@ -165,18 +166,22 @@
 
 NS16550_STATIC int ns16550_open(
-  int      major,
-  int      minor,
-  void    * arg
+  int major,
+  int minor,
+  void *arg
 )
 {
-  /*
-   * Assert DTR
-   */
-
-  if(Console_Port_Tbl[minor].pDeviceFlow != &ns16550_flow_DTRCTS) {
-    ns16550_assert_DTR(minor);
-  }
-
-  return(RTEMS_SUCCESSFUL);
+  rtems_libio_open_close_args_t *oc = (rtems_libio_open_close_args_t *) arg;
+  struct rtems_termios_tty *tty = (struct rtems_termios_tty *) oc->iop->data1;
+  console_tbl *c = &Console_Port_Tbl [minor];
+
+  /* Assert DTR */
+  if (c->pDeviceFlow != &ns16550_flow_DTRCTS) {
+    ns16550_assert_DTR( minor);
+  }
+
+  /* Set initial baud */
+  rtems_termios_set_initial_baud( tty, (int) c->pDeviceParams);
+
+  return RTEMS_SUCCESSFUL;
 }
 
@@ -204,13 +209,20 @@
  * @brief Polled write for NS16550.
  */
-NS16550_STATIC void ns16550_write_polled( int minor, char c)
-{
-  uint32_t port = Console_Port_Tbl [minor].ulCtrlPort1;
-  getRegister_f get = Console_Port_Tbl [minor].getRegister;
-  setRegister_f set = Console_Port_Tbl [minor].setRegister;
-  uint32_t status;
+NS16550_STATIC void ns16550_write_polled( int minor, char out)
+{
+  console_tbl *c = &Console_Port_Tbl [minor];
+  uint32_t port = c->ulCtrlPort1;
+  getRegister_f get = c->getRegister;
+  setRegister_f set = c->setRegister;
+  uint32_t status = 0;
   rtems_interrupt_level level;
 
-  while (1) {
+  /* Save port interrupt mask */
+  uint32_t interrupt_mask = get( port, NS16550_INTERRUPT_ENABLE);
+
+  /* Disable port interrupts */
+  ns16550_enable_interrupts( minor, NS16550_DISABLE_ALL_INTR);
+
+  while (true) {
     /* Try to transmit the character in a critical section */
     rtems_interrupt_disable( level);
@@ -220,5 +232,5 @@
     if ((status & SP_LSR_THOLD) != 0) {
       /* Transmit character */
-      set( port, NS16550_TRANSMIT_BUFFER, c);
+      set( port, NS16550_TRANSMIT_BUFFER, out);
 
       /* Finished */
@@ -234,4 +246,7 @@
     } while ((status & SP_LSR_THOLD) == 0);
   }
+
+  /* Restore port interrupt mask */
+  set( port, NS16550_INTERRUPT_ENABLE, interrupt_mask);
 }
 
@@ -449,70 +464,89 @@
 
 #if defined(BSP_FEATURE_IRQ_EXTENSION) || defined(BSP_FEATURE_IRQ_LEGACY)
-/*
- *  ns16550_process
- *
- *  This routine is the console interrupt handler for A port.
- */
-
-NS16550_STATIC void ns16550_process(int minor)
+
+/**
+ * @brief Process interrupt.
+ */
+NS16550_STATIC void ns16550_process( int minor)
 {
   console_tbl *c = &Console_Port_Tbl [minor];
   console_data *d = &Console_Port_Data [minor];
-
-  uint32_t                pNS16550;
-  volatile uint8_t        ucLineStatus;
-  volatile uint8_t        ucInterruptId;
-  char                    cChar;
-  getRegister_f           getReg;
-  setRegister_f           setReg;
-
-  pNS16550 = c->ulCtrlPort1;
-  getReg   = c->getRegister;
-  setReg   = c->setRegister;
-
+  ns16550_context *ctx = d->pDeviceContext;
+  uint32_t port = c->ulCtrlPort1;
+  getRegister_f get = c->getRegister;
+  setRegister_f set = c->setRegister;
+  int i = 0;
+  char buf [SP_FIFO_SIZE];
+
+  /* Iterate until no more interrupts are pending */
   do {
-    /*
-     * Deal with any received characters
-     */
-    while (true) {
-      ucLineStatus = (*getReg)(pNS16550, NS16550_LINE_STATUS);
-      if (~ucLineStatus & SP_LSR_RDY) {
-        break;
-      }
-      cChar = (*getReg)(pNS16550, NS16550_RECEIVE_BUFFER);
-      rtems_termios_enqueue_raw_characters(
-        d->termios_data,
-        &cChar,
-        1
-      );
-    }
-
-    /*
-     *  TX all the characters we can
-     */
-
-    while (true) {
-      ucLineStatus = (*getReg)(pNS16550, NS16550_LINE_STATUS);
-      if (~ucLineStatus & SP_LSR_THOLD) {
-        /*
-         * We'll get another interrupt when
-         * the transmitter holding reg. becomes
-         * free again
-         */
-        break;
-      }
-
-      if (rtems_termios_dequeue_characters( d->termios_data, 1) == 0) {
-        if (c->pDeviceFlow != &ns16550_flow_RTSCTS) {
-          ns16550_negate_RTS(minor);
-        }
-        d->bActive = false;
-        ns16550_enable_interrupts(minor, NS16550_ENABLE_ALL_INTR_EXCEPT_TX);
+    /* Fetch received characters */
+    for (i = 0; i < SP_FIFO_SIZE; ++i) {
+      if ((get( port, NS16550_LINE_STATUS) & SP_LSR_RDY) != 0) {
+        buf [i] = (char) get(port, NS16550_RECEIVE_BUFFER);
+      } else {
         break;
       }
     }
 
-    ucInterruptId = (*getReg)(pNS16550, NS16550_INTERRUPT_ID);
-  } while((ucInterruptId&0xf)!=0x1);
+    /* Enqueue fetched characters */
+    rtems_termios_enqueue_raw_characters( d->termios_data, buf, i);
+
+    /* Check if we can dequeue transmitted characters */
+    if (ctx->transmitFifoChars > 0
+        && (get( port, NS16550_LINE_STATUS) & SP_LSR_THOLD) != 0) {
+      unsigned chars = ctx->transmitFifoChars;
+
+      /*
+       * We finished the transmission, so clear the number of characters in the
+       * transmit FIFO.
+       */
+      ctx->transmitFifoChars = 0;
+
+      /* Dequeue transmitted characters */
+      if (rtems_termios_dequeue_characters( d->termios_data, chars) == 0) {
+        /* Nothing to do */
+        d->bActive = false;
+        ns16550_enable_interrupts( minor, NS16550_ENABLE_ALL_INTR_EXCEPT_TX);
+      }
+    }
+  } while ((get( port, NS16550_INTERRUPT_ID) & SP_IID_0) == 0);
+}
+#endif
+
+/**
+ * @brief Transmits up to @a len characters from @a buf.
+ *
+ * This routine is invoked either from task context with disabled interrupts to
+ * start a new transmission process with exactly one character in case of an
+ * idle output state or from the interrupt handler to refill the transmitter.
+ *
+ * Returns always zero.
+ */
+NS16550_STATIC int ns16550_write_support_int(
+  int minor,
+  const char *buf,
+  int len
+)
+{
+  console_tbl *c = &Console_Port_Tbl [minor];
+  console_data *d = &Console_Port_Data [minor];
+  ns16550_context *ctx = d->pDeviceContext;
+  uint32_t port = c->ulCtrlPort1;
+  setRegister_f set = c->setRegister;
+  int i = 0;
+  int out = len > SP_FIFO_SIZE ? SP_FIFO_SIZE : len;
+
+  for (i = 0; i < out; ++i) {
+    set( port, NS16550_TRANSMIT_BUFFER, buf [i]);
+  }
+
+  if (len > 0) {
+    ctx->transmitFifoChars = out;
+    d->bActive = true;
+    ns16550_enable_interrupts( minor, NS16550_ENABLE_ALL_INTR);
+  }
+
+  return 0;
 }
 
@@ -522,6 +556,4 @@
  *  This routine initializes the port to have the specified interrupts masked.
  */
-#endif
-
 NS16550_STATIC void ns16550_enable_interrupts(
   int minor,
@@ -623,46 +655,4 @@
 
 /*
- *  ns16550_write_support_int
- *
- *  Console Termios output entry point.
- */
-
-NS16550_STATIC int ns16550_write_support_int(
-  int   minor,
-  const char *buf,
-  int   len
-)
-{
-  uint32_t       Irql;
-  uint32_t       pNS16550;
-  setRegister_f  setReg;
-
-  setReg   = Console_Port_Tbl[minor].setRegister;
-  pNS16550 = Console_Port_Tbl[minor].ulCtrlPort1;
-
-  /*
-   *  We are using interrupt driven output and termios only sends us
-   *  one character at a time.
-   */
-
-  if ( !len )
-    return 0;
-
-  if(Console_Port_Tbl[minor].pDeviceFlow != &ns16550_flow_RTSCTS) {
-    ns16550_assert_RTS(minor);
-  }
-
-  rtems_interrupt_disable(Irql);
-    if ( Console_Port_Data[minor].bActive == false) {
-      Console_Port_Data[minor].bActive = true;
-      ns16550_enable_interrupts(minor, NS16550_ENABLE_ALL_INTR);
-    }
-    (*setReg)(pNS16550, NS16550_TRANSMIT_BUFFER, *buf);
-  rtems_interrupt_enable(Irql);
-
-  return 1;
-}
-
-/*
  *  ns16550_write_support_polled
  *
Index: c/src/libchip/serial/ns16550_p.h
===================================================================
--- c/src/libchip/serial/ns16550_p.h	(revision 84d7499a2b62893922661192ede2ac48857dda99)
+++ c/src/libchip/serial/ns16550_p.h	(revision ec5d45053b71f052d978e423f08ad4e28e1b800d)
@@ -30,22 +30,9 @@
 #define NS16550_STATIC static
 
-/*
- * Define serial port read registers structure.
- */
-
-typedef volatile struct _SP_READ_REGISTERS {
-    unsigned char ReceiveBuffer;
-    unsigned char InterruptEnable;
-    unsigned char InterruptId;
-    unsigned char LineControl;
-    unsigned char ModemControl;
-    unsigned char LineStatus;
-    unsigned char ModemStatus;
-    unsigned char ScratchPad;
-} SP_READ_REGISTERS, *PSP_READ_REGISTERS;
-
 #define NS16550_RECEIVE_BUFFER   0
+#define NS16550_TRANSMIT_BUFFER  0
 #define NS16550_INTERRUPT_ENABLE 1
 #define NS16550_INTERRUPT_ID     2
+#define NS16550_FIFO_CONTROL     2
 #define NS16550_LINE_CONTROL     3
 #define NS16550_MODEM_CONTROL    4
@@ -55,22 +42,4 @@
 
 /*
- * Define serial port write registers structure.
- */
-
-typedef volatile struct _SP_WRITE_REGISTERS {
-    unsigned char TransmitBuffer;
-    unsigned char InterruptEnable;
-    unsigned char FifoControl;
-    unsigned char LineControl;
-    unsigned char ModemControl;
-    unsigned char Reserved1;
-    unsigned char ModemStatus;
-    unsigned char ScratchPad;
-} SP_WRITE_REGISTERS, *PSP_WRITE_REGISTERS;
-
-#define NS16550_TRANSMIT_BUFFER  0
-#define NS16550_FIFO_CONTROL     2
-
-/*
  * Define serial port interrupt enable register structure.
  */
@@ -86,13 +55,11 @@
 
 /*
- * Define serial port interrupt id register structure.
- */
-
-typedef struct _SP_INTERRUPT_ID {
-    unsigned char InterruptPending : 1;
-    unsigned char Identification : 3;
-    unsigned char Reserved1 : 2;
-    unsigned char FifoEnabled : 2;
-} SP_INTERRUPT_ID, *PSP_INTERRUPT_ID;
+ * Define serial port interrupt ID register structure.
+ */
+
+#define SP_IID_0 0x01
+#define SP_IID_1 0x02
+#define SP_IID_2 0x04
+#define SP_IID_3 0x08
 
 /*
@@ -105,4 +72,6 @@
 #define SP_FIFO_DMA   0x08
 #define SP_FIFO_RXLEVEL 0xc0
+
+#define SP_FIFO_SIZE 16
 
 /*
@@ -157,7 +126,7 @@
 #define SP_LSR_EFIFO  0x80
 
-typedef struct _ns16550_context
-{
-        uint8_t         ucModemCtrl;
+typedef struct {
+  uint8_t ucModemCtrl;
+  int transmitFifoChars;
 } ns16550_context;
 
