libchip: Add support for NS16550 with FDR

4 files changed, 48 insertions, 26 deletions

diff --git a/c/src/lib/libbsp/arm/lpc24xx/console/console-config.c b/c/src/lib/libbsp/arm/lpc24xx/console/console-config.c
index cf6f4a6e15..3691fe0dae 100644
--- a/c/src/lib/libbsp/arm/lpc24xx/console/console-config.c
+++ b/c/src/lib/libbsp/arm/lpc24xx/console/console-config.c
@@ -93,7 +93,7 @@ console_tbl Console_Configuration_Ports [] = {
   #ifdef LPC24XX_CONFIG_CONSOLE
     {
       .sDeviceName = "/dev/ttyS0",
-      .deviceType = SERIAL_NS16550,
+      .deviceType = SERIAL_NS16550_WITH_FDR,
       .pDeviceFns = &ns16550_fns,
       .deviceProbe = NULL,
       .pDeviceFlow = NULL,
@@ -114,7 +114,7 @@ console_tbl Console_Configuration_Ports [] = {
   #ifdef LPC24XX_CONFIG_UART_1
     {
       .sDeviceName = "/dev/ttyS1",
-      .deviceType = SERIAL_NS16550,
+      .deviceType = SERIAL_NS16550_WITH_FDR,
       .pDeviceFns = &ns16550_fns,
       .deviceProbe = lpc24xx_uart_probe_1,
       .pDeviceFlow = NULL,
@@ -135,7 +135,7 @@ console_tbl Console_Configuration_Ports [] = {
   #ifdef LPC24XX_CONFIG_UART_2
     {
       .sDeviceName = "/dev/ttyS2",
-      .deviceType = SERIAL_NS16550,
+      .deviceType = SERIAL_NS16550_WITH_FDR,
       .pDeviceFns = &ns16550_fns,
       .deviceProbe = lpc24xx_uart_probe_2,
       .pDeviceFlow = NULL,
@@ -156,7 +156,7 @@ console_tbl Console_Configuration_Ports [] = {
   #ifdef LPC24XX_CONFIG_UART_3
     {
       .sDeviceName = "/dev/ttyS3",
-      .deviceType = SERIAL_NS16550,
+      .deviceType = SERIAL_NS16550_WITH_FDR,
       .pDeviceFns = &ns16550_fns,
       .deviceProbe = lpc24xx_uart_probe_3,
       .pDeviceFlow = NULL,
diff --git a/c/src/libchip/serial/ns16550.c b/c/src/libchip/serial/ns16550.c
index a8fdf983c7..3277b7ef0d 100644
--- a/c/src/libchip/serial/ns16550.c
+++ b/c/src/libchip/serial/ns16550.c
@@ -95,6 +95,40 @@ console_fns ns16550_fns_polled = {
   false                                /* deviceOutputUsesInterrupts */
 };
 
+static uint32_t NS16550_GetBaudDivisor(const console_tbl *c, uint32_t baud)
+{
+  uint32_t clock = c->ulClock;
+  uint32_t baudDivisor = (clock != 0 ? clock : 115200) / (baud * 16);
+
+  if (c->deviceType == SERIAL_NS16550_WITH_FDR) {
+    uint32_t fractionalDivider = 0x10;
+    uint32_t err = baud;
+    uint32_t mulVal;
+    uint32_t divAddVal;
+
+    clock /= 16 * baudDivisor;
+    for (mulVal = 1; mulVal < 16; ++mulVal) {
+      for (divAddVal = 0; divAddVal < mulVal; ++divAddVal) {
+        uint32_t actual = (mulVal * clock) / (mulVal + divAddVal);
+        uint32_t newErr = actual > baud ? actual - baud : baud - actual;
+
+        if (newErr < err) {
+          err = newErr;
+          fractionalDivider = (mulVal << 4) | divAddVal;
+        }
+      }
+    }
+
+    (*c->setRegister)(
+      c->ulCtrlPort1,
+      NS16550_FRACTIONAL_DIVIDER,
+      fractionalDivider
+    );
+  }
+
+  return baudDivisor;
+}
+
 /*
  *  ns16550_init
  */
@@ -133,10 +167,7 @@ NS16550_STATIC void ns16550_init(int minor)
 
   /* Set the divisor latch and set the baud rate. */
 
-  ulBaudDivisor = NS16550_Baud(
-    (uint32_t) c->ulClock,
-    (uint32_t) ((uintptr_t)c->pDeviceParams)
-  );
+  ulBaudDivisor = NS16550_GetBaudDivisor(c, (uintptr_t) c->pDeviceParams);
   ucDataByte = SP_LINE_DLAB;
   (*setReg)(pNS16550, NS16550_LINE_CONTROL, ucDataByte);
 
@@ -413,23 +444,18 @@ NS16550_STATIC int ns16550_set_attributes(
   setRegister_f           setReg;
   getRegister_f           getReg;
   uint32_t                Irql;
+  const console_tbl      *c = Console_Port_Tbl [minor];
 
-  pNS16550 = Console_Port_Tbl[minor]->ulCtrlPort1;
-  setReg   = Console_Port_Tbl[minor]->setRegister;
-  getReg   = Console_Port_Tbl[minor]->getRegister;
+  pNS16550 = c->ulCtrlPort1;
+  setReg   = c->setRegister;
+  getReg   = c->getRegister;
 
   /*
    *  Calculate the baud rate divisor
    */
 
-  baud_requested = t->c_cflag & CBAUD;
-  if (!baud_requested)
-    baud_requested = B9600;              /* default to 9600 baud */
-
-  ulBaudDivisor = NS16550_Baud(
-    (uint32_t) Console_Port_Tbl[minor]->ulClock,
-    rtems_termios_baud_to_number(baud_requested)
-  );
+  baud_requested = rtems_termios_baud_to_number(t->c_cflag);
+  ulBaudDivisor = NS16550_GetBaudDivisor(c, baud_requested);
 
   ucLineControl = 0;
 
diff --git a/c/src/libchip/serial/ns16550_p.h b/c/src/libchip/serial/ns16550_p.h
index be99cf1475..1e261612f2 100644
--- a/c/src/libchip/serial/ns16550_p.h
+++ b/c/src/libchip/serial/ns16550_p.h
@@ -49,6 +49,7 @@ extern "C" {
 #define NS16550_LINE_STATUS      5
 #define NS16550_MODEM_STATUS     6
 #define NS16550_SCRATCH_PAD      7
+#define NS16550_FRACTIONAL_DIVIDER 10
 
 /*
  * Define serial port interrupt enable register structure.
@@ -106,13 +107,6 @@ extern "C" {
 #define EIGHT_BITS 0x3                  /* eight bits per character */
 
 /*
- * Line speed divisor definition.
- */
-
-#define NS16550_Baud(_clock, _baud_rate) \
-  ((((_clock) == 0) ? 115200 : (_clock))/(_baud_rate*16))
-
-/*
  * Define serial port modem control register structure.
  */
 
diff --git a/c/src/libchip/serial/serial.h b/c/src/libchip/serial/serial.h
index 7fe1da4f3e..6faa4d34a0 100644
--- a/c/src/libchip/serial/serial.h
+++ b/c/src/libchip/serial/serial.h
@@ -104,6 +104,8 @@ typedef struct _console_flow {
 typedef enum {
   SERIAL_MC68681,              /* Motorola MC68681 or Exar 88681 */
   SERIAL_NS16550,              /* National Semiconductor NS16550 */
+  SERIAL_NS16550_WITH_FDR,     /* National Semiconductor NS16550
+                                  with Fractional Divider Register (FDR) */
   SERIAL_Z85C30,               /* Zilog Z85C30 */
   SERIAL_CUSTOM                /* BSP specific driver */
 } console_devs;