You are viewing a plain text version of this content. The canonical link for it is here.
Posted to commits@nuttx.apache.org by pr...@apache.org on 2020/09/14 12:23:56 UTC

[incubator-nuttx] branch master updated (34b34e2 -> 03c7951)

This is an automated email from the ASF dual-hosted git repository.

protobits pushed a change to branch master
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx.git.


    from 34b34e2  Fix: ensure archive files do not carry object files from prior builds
     new 3634bb6  sim/uart: support tty operation in arch/sim
     new 03c7951  libc/termios: modify termios setting follow linux and posix

The 2 revisions listed above as "new" are entirely new to this
repository and will be described in separate emails.  The revisions
listed as "add" were already present in the repository and have only
been added to this reference.


Summary of changes:
 arch/sim/Kconfig                   |  24 ++
 arch/sim/src/Makefile              |  11 +-
 arch/sim/src/sim/up_devconsole.c   | 319 ---------------------
 arch/sim/src/sim/up_idle.c         |   4 +-
 arch/sim/src/sim/up_initialize.c   |  10 +-
 arch/sim/src/sim/up_internal.h     |  16 +-
 arch/sim/src/sim/up_simuart.c      | 124 +++++---
 arch/sim/src/sim/up_uart.c         | 572 +++++++++++++++++++++++++++++++++++++
 include/termios.h                  | 242 ++++++++--------
 libs/libc/termios/Make.defs        |   2 +-
 libs/libc/termios/lib_cfgetspeed.c |  77 -----
 libs/libc/termios/lib_cfsetspeed.c |  96 -------
 libs/libc/termios/lib_cfspeed.c    | 196 +++++++++++++
 13 files changed, 1008 insertions(+), 685 deletions(-)
 delete mode 100644 arch/sim/src/sim/up_devconsole.c
 create mode 100644 arch/sim/src/sim/up_uart.c
 delete mode 100644 libs/libc/termios/lib_cfgetspeed.c
 delete mode 100644 libs/libc/termios/lib_cfsetspeed.c
 create mode 100644 libs/libc/termios/lib_cfspeed.c


[incubator-nuttx] 01/02: sim/uart: support tty operation in arch/sim

Posted by pr...@apache.org.
This is an automated email from the ASF dual-hosted git repository.

protobits pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx.git

commit 3634bb6ba5d51f869b38acaecc1c7088211bc4d3
Author: dongjiuzhu <do...@xiaomi.com>
AuthorDate: Mon Aug 31 20:12:32 2020 +0800

    sim/uart: support tty operation in arch/sim
    
    Change-Id: I6943c1e928ed821aa913bc619e5b8c581b5610c3
    Signed-off-by: dongjiuzhu <do...@xiaomi.com>
---
 arch/sim/Kconfig                 |  24 ++
 arch/sim/src/Makefile            |  11 +-
 arch/sim/src/sim/up_devconsole.c | 319 ----------------------
 arch/sim/src/sim/up_idle.c       |   4 +-
 arch/sim/src/sim/up_initialize.c |  10 +-
 arch/sim/src/sim/up_internal.h   |  16 +-
 arch/sim/src/sim/up_simuart.c    | 124 ++++++---
 arch/sim/src/sim/up_uart.c       | 572 +++++++++++++++++++++++++++++++++++++++
 8 files changed, 699 insertions(+), 381 deletions(-)

diff --git a/arch/sim/Kconfig b/arch/sim/Kconfig
index 28f8cf4..a165b45 100644
--- a/arch/sim/Kconfig
+++ b/arch/sim/Kconfig
@@ -485,4 +485,28 @@ config SIM_HCISOCKET
 		control of the device, but is abstracted from the
 		physical interface which is still handled by Linux.
 
+config SIM_UART_NUMBER
+	int "The number of tty ports on sim platform, range is 0~4"
+	default 0
+
+config SIM_UART0_NAME
+	string "the name of uart0 on sim"
+	default "/dev/ttySIM0"
+	depends on SIM_UART_NUMBER >= 1
+
+config SIM_UART1_NAME
+	string "the name of uart1 on sim"
+	default "/dev/ttySIM1"
+	depends on SIM_UART_NUMBER >= 2
+
+config SIM_UART2_NAME
+	string "the name of uart2 on sim"
+	default "/dev/ttySIM2"
+	depends on SIM_UART_NUMBER >= 3
+
+config SIM_UART3_NAME
+	string "the name of uart3 on sim"
+	default "/dev/ttySIM3"
+	depends on SIM_UART_NUMBER >= 4
+
 endif # ARCH_SIM
diff --git a/arch/sim/src/Makefile b/arch/sim/src/Makefile
index a6e58fc..76278e2 100644
--- a/arch/sim/src/Makefile
+++ b/arch/sim/src/Makefile
@@ -73,7 +73,7 @@ CSRCS  = up_initialize.c up_idle.c up_interruptcontext.c up_initialstate.c
 CSRCS += up_createstack.c up_usestack.c up_releasestack.c up_stackframe.c
 CSRCS += up_unblocktask.c up_blocktask.c up_releasepending.c
 CSRCS += up_reprioritizertr.c up_exit.c up_schedulesigaction.c
-CSRCS += up_allocateheap.c
+CSRCS += up_allocateheap.c up_uart.c
 
 VPATH = sim
 DEPPATH = $(patsubst %,--dep-path %,$(subst :, ,$(VPATH)))
@@ -83,7 +83,7 @@ ifeq ($(CONFIG_HOST_MACOS),y)
   HOSTCFLAGS += -Wno-deprecated-declarations
 endif
 
-HOSTSRCS = up_hostirq.c up_hostmemory.c up_hosttime.c
+HOSTSRCS = up_hostirq.c up_hostmemory.c up_hosttime.c up_simuart.c
 STDLIBS += -lpthread
 ifneq ($(CONFIG_HOST_MACOS),y)
   STDLIBS += -lrt
@@ -117,13 +117,6 @@ ifneq ($(CONFIG_SCHED_INSTRUMENTATION_BUFFER),y)
 endif
 endif
 
-ifeq ($(CONFIG_DEV_CONSOLE),y)
-ifneq ($(CONFIG_SYSLOG_RPMSG),y)
-  CSRCS += up_devconsole.c
-  HOSTSRCS += up_simuart.c
-endif
-endif
-
 ifeq ($(CONFIG_ONESHOT),y)
   CSRCS += up_oneshot.c
 endif
diff --git a/arch/sim/src/sim/up_devconsole.c b/arch/sim/src/sim/up_devconsole.c
deleted file mode 100644
index 74fff98..0000000
--- a/arch/sim/src/sim/up_devconsole.c
+++ /dev/null
@@ -1,319 +0,0 @@
-/****************************************************************************
- * arch/sim/src/sim/up_devconsole.c
- *
- *   Copyright (C) 2007-2009, 2013 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gn...@nuttx.org>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <nuttx/config.h>
-#include <nuttx/serial/serial.h>
-
-#include "up_internal.h"
-
-/****************************************************************************
- * Pre-processor Definitions
- ****************************************************************************/
-
-#define DEVCONSOLE_BUFSIZE 256
-
-/****************************************************************************
- * Private Function Prototypes
- ****************************************************************************/
-
-static int  devconsole_setup(FAR struct uart_dev_s *dev);
-static void devconsole_shutdown(FAR struct uart_dev_s *dev);
-static int  devconsole_attach(FAR struct uart_dev_s *dev);
-static void devconsole_detach(FAR struct uart_dev_s *dev);
-static int  devconsole_ioctl(FAR struct file *filep, int cmd,
-                             unsigned long arg);
-static int  devconsole_receive(FAR struct uart_dev_s *dev, uint32_t *status);
-static void devconsole_rxint(FAR struct uart_dev_s *dev, bool enable);
-static bool devconsole_rxavailable(FAR struct uart_dev_s *dev);
-static void devconsole_send(FAR struct uart_dev_s *dev, int ch);
-static void devconsole_txint(FAR struct uart_dev_s *dev, bool enable);
-static bool devconsole_txready(FAR struct uart_dev_s *dev);
-static bool devconsole_txempty(FAR struct uart_dev_s *dev);
-
-/****************************************************************************
- * Private Data
- ****************************************************************************/
-
-static const struct uart_ops_s g_devconsole_ops =
-{
-  .setup          = devconsole_setup,
-  .shutdown       = devconsole_shutdown,
-  .attach         = devconsole_attach,
-  .detach         = devconsole_detach,
-  .ioctl          = devconsole_ioctl,
-  .receive        = devconsole_receive,
-  .rxint          = devconsole_rxint,
-  .rxavailable    = devconsole_rxavailable,
-  .send           = devconsole_send,
-  .txint          = devconsole_txint,
-  .txready        = devconsole_txready,
-  .txempty        = devconsole_txempty,
-};
-
-static char g_devconsole_rxbuf[DEVCONSOLE_BUFSIZE];
-static char g_devconsole_txbuf[DEVCONSOLE_BUFSIZE];
-
-static struct uart_dev_s g_devconsole_dev =
-{
-  .isconsole      = true,
-  .ops            = &g_devconsole_ops,
-  .xmit =
-  {
-    .size         = DEVCONSOLE_BUFSIZE,
-    .buffer       = g_devconsole_txbuf,
-  },
-  .recv =
-  {
-    .size         = DEVCONSOLE_BUFSIZE,
-    .buffer       = g_devconsole_rxbuf,
-  },
-};
-
-/****************************************************************************
- * Private Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: devconsole_setup
- *
- * Description:
- *   Configure the UART baud, bits, parity, fifos, etc. This
- *   method is called the first time that the serial port is
- *   opened.
- *
- ****************************************************************************/
-
-static int devconsole_setup(FAR struct uart_dev_s *dev)
-{
-  return OK;
-}
-
-/****************************************************************************
- * Name: devconsole_shutdown
- *
- * Description:
- *   Disable the UART.  This method is called when the serial
- *   port is closed
- *
- ****************************************************************************/
-
-static void devconsole_shutdown(struct uart_dev_s *dev)
-{
-}
-
-/****************************************************************************
- * Name: devconsole_attach
- *
- * Description:
- *   Configure the UART to operation in interrupt driven mode.  This method
- *   is called when the serial port is opened.  Normally, this is just after
- *   the setup() method is called, however, the serial console may operate in
- *   a non-interrupt driven mode during the boot phase.
- *
- *   RX and TX interrupts are not enabled when by the attach method (unless
- *   the hardware supports multiple levels of interrupt enabling).  The RX
- *   and TX interrupts are not enabled until the txint() and rxint() methods
- *   are called.
- *
- ****************************************************************************/
-
-static int devconsole_attach(struct uart_dev_s *dev)
-{
-  return OK;
-}
-
-/****************************************************************************
- * Name: devconsole_detach
- *
- * Description:
- *   Detach UART interrupts.  This method is called when the serial port is
- *   closed normally just before the shutdown method is called.  The
- *   exception is the serial console which is never shutdown.
- *
- ****************************************************************************/
-
-static void devconsole_detach(FAR struct uart_dev_s *dev)
-{
-}
-
-/****************************************************************************
- * Name: devconsole_ioctl
- *
- * Description:
- *   All ioctl calls will be routed through this method
- *
- ****************************************************************************/
-
-static int devconsole_ioctl(struct file *filep, int cmd, unsigned long arg)
-{
-  return -ENOTTY;
-}
-
-/****************************************************************************
- * Name: devconsole_receive
- *
- * Description:
- *   Called (usually) from the interrupt level to receive one
- *   character from the UART.  Error bits associated with the
- *   receipt are provided in the return 'status'.
- *
- ****************************************************************************/
-
-static int devconsole_receive(struct uart_dev_s *dev, uint32_t *status)
-{
-  *status = 0;
-  return simuart_getc();
-}
-
-/****************************************************************************
- * Name: devconsole_rxint
- *
- * Description:
- *   Call to enable or disable RX interrupts
- *
- ****************************************************************************/
-
-static void devconsole_rxint(struct uart_dev_s *dev, bool enable)
-{
-}
-
-/****************************************************************************
- * Name: devconsole_rxavailable
- *
- * Description:
- *   Return true if the receive fifo is not empty
- *
- ****************************************************************************/
-
-static bool devconsole_rxavailable(struct uart_dev_s *dev)
-{
-  return simuart_checkc();
-}
-
-/****************************************************************************
- * Name: devconsole_send
- *
- * Description:
- *   This method will send one byte on the UART
- *
- ****************************************************************************/
-
-static void devconsole_send(struct uart_dev_s *dev, int ch)
-{
-  simuart_putc(ch);
-}
-
-/****************************************************************************
- * Name: devconsole_txint
- *
- * Description:
- *   Call to enable or disable TX interrupts
- *
- ****************************************************************************/
-
-static void devconsole_txint(struct uart_dev_s *dev, bool enable)
-{
-  if (enable)
-    {
-      uart_xmitchars(&g_devconsole_dev);
-    }
-}
-
-/****************************************************************************
- * Name: devconsole_txready
- *
- * Description:
- *   Return true if the tranmsit fifo is not full
- *
- ****************************************************************************/
-
-static bool devconsole_txready(struct uart_dev_s *dev)
-{
-  return true;
-}
-
-/****************************************************************************
- * Name: devconsole_txempty
- *
- * Description:
- *   Return true if the transmit fifo is empty
- *
- ****************************************************************************/
-
-static bool devconsole_txempty(struct uart_dev_s *dev)
-{
-  return true;
-}
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: up_devconsole
- ****************************************************************************/
-
-void up_devconsole(void)
-{
-  uart_register("/dev/console", &g_devconsole_dev);
-  uart_register("/dev/ttyS0", &g_devconsole_dev);
-}
-
-/****************************************************************************
- * Name: up_devconloop
- ****************************************************************************/
-
-void up_devconloop(void)
-{
-  if (simuart_checkc())
-    {
-      uart_recvchars(&g_devconsole_dev);
-    }
-}
-
-/****************************************************************************
- * Name: up_putc
- ****************************************************************************/
-
-int up_putc(int ch)
-{
-  /* Just map to the host simuart_putc routine */
-
-  return simuart_putc(ch);
-}
diff --git a/arch/sim/src/sim/up_idle.c b/arch/sim/src/sim/up_idle.c
index d319b9d..b7fda31 100644
--- a/arch/sim/src/sim/up_idle.c
+++ b/arch/sim/src/sim/up_idle.c
@@ -89,11 +89,9 @@ void up_idle(void)
     }
 #endif
 
-#ifdef USE_DEVCONSOLE
   /* Handle UART data availability */
 
-  up_devconloop();
-#endif
+  up_uartloop();
 
 #if defined(CONFIG_SIM_TOUCHSCREEN) || defined(CONFIG_SIM_AJOYSTICK)
   /* Drive the X11 event loop */
diff --git a/arch/sim/src/sim/up_initialize.c b/arch/sim/src/sim/up_initialize.c
index c226bff..ad253c1 100644
--- a/arch/sim/src/sim/up_initialize.c
+++ b/arch/sim/src/sim/up_initialize.c
@@ -228,15 +228,11 @@ void up_initialize(void)
   rpmsg_serialinit();
 #endif
 
-#if defined(USE_DEVCONSOLE)
-  /* Start the simulated UART device */
+  /* Register some tty-port to access tty-port on sim platform */
 
-  simuart_start();
+  up_uartinit();
 
-  /* Register a console (or not) */
-
-  up_devconsole();          /* Our private /dev/console */
-#elif defined(CONFIG_CONSOLE_SYSLOG)
+#if defined(CONFIG_CONSOLE_SYSLOG)
   syslog_console_init();
 #endif
 
diff --git a/arch/sim/src/sim/up_internal.h b/arch/sim/src/sim/up_internal.h
index df24178..7eb8a1b 100644
--- a/arch/sim/src/sim/up_internal.h
+++ b/arch/sim/src/sim/up_internal.h
@@ -259,17 +259,21 @@ void up_timer_update(void);
 void rpmsg_serialinit(void);
 #endif
 
-/* up_devconsole.c **********************************************************/
+/* up_uart.c ****************************************************************/
 
-void up_devconsole(void);
-void up_devconloop(void);
+void up_uartinit(void);
+void up_uartloop(void);
 
 /* up_simuart.c *************************************************************/
 
 void simuart_start(void);
-int  simuart_putc(int ch);
-int  simuart_getc(void);
-bool simuart_checkc(void);
+int  simuart_open(const char *pathname);
+void simuart_close(int fd);
+int  simuart_putc(int fd, int ch);
+int  simuart_getc(int fd);
+bool simuart_checkc(int fd);
+int  simuart_setcflag(int fd, unsigned int cflag);
+int  simuart_getcflag(int fd, unsigned int *cflag);
 
 /* up_deviceimage.c *********************************************************/
 
diff --git a/arch/sim/src/sim/up_simuart.c b/arch/sim/src/sim/up_simuart.c
index cdb7551..7d89e46 100644
--- a/arch/sim/src/sim/up_simuart.c
+++ b/arch/sim/src/sim/up_simuart.c
@@ -37,12 +37,14 @@
  * Included Files
  ****************************************************************************/
 
+#include <fcntl.h>
 #include <stdbool.h>
 #include <unistd.h>
 #include <string.h>
 #include <stdlib.h>
 #include <termios.h>
 #include <poll.h>
+#include <errno.h>
 
 /****************************************************************************
  * Private Data
@@ -58,18 +60,14 @@ static struct termios g_cooked;
  * Name: setrawmode
  ****************************************************************************/
 
-static void setrawmode(void)
+static void setrawmode(int fd)
 {
   struct termios raw;
 
-  /* Get the current stdin terminal mode */
-
-  tcgetattr(0, &g_cooked);
+  tcgetattr(fd, &raw);
 
   /* Switch to raw mode */
 
-  memcpy(&raw, &g_cooked, sizeof(struct termios));
-
   raw.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | IGNCR |
                    ICRNL | IXON);
   raw.c_oflag &= ~OPOST;
@@ -77,7 +75,7 @@ static void setrawmode(void)
   raw.c_cflag &= ~(CSIZE | PARENB);
   raw.c_cflag |= CS8;
 
-  tcsetattr(0, TCSANOW, &raw);
+  tcsetattr(fd, TCSANOW, &raw);
 }
 
 /****************************************************************************
@@ -92,85 +90,137 @@ static void restoremode(void)
 }
 
 /****************************************************************************
- * Name: simuart_putraw
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: simuart_start
+ ****************************************************************************/
+
+void simuart_start(void)
+{
+  /* Get the current stdin terminal mode */
+
+  tcgetattr(0, &g_cooked);
+
+  /* Put stdin into raw mode */
+
+  setrawmode(0);
+
+  /* Restore the original terminal mode before exit */
+
+  atexit(restoremode);
+}
+
+/****************************************************************************
+ * Name: simuart_open
  ****************************************************************************/
 
-int simuart_putraw(int ch)
+int simuart_open(const char *pathname)
 {
-  ssize_t nwritten;
-  unsigned char buf = ch;
+  int fd;
 
-  nwritten = write(1, &buf, 1);
-  if (nwritten != 1)
+  fd = open(pathname, O_RDWR | O_NONBLOCK);
+  if (fd >= 0)
     {
-      return -1;
+      /* keep raw mode */
+
+      setrawmode(fd);
+    }
+  else
+    {
+      fd = -errno;
     }
 
-  return ch;
+  return fd;
 }
 
 /****************************************************************************
- * Public Functions
+ * Name: simuart_close
  ****************************************************************************/
 
+void simuart_close(int fd)
+{
+  close(fd);
+}
+
 /****************************************************************************
- * Name: simuart_start
+ * Name: simuart_putc
  ****************************************************************************/
 
-void simuart_start(void)
+int simuart_putc(int fd, int ch)
 {
-  /* Put stdin into raw mode */
+  return write(fd, &ch, 1) == 1 ? ch : -1;
+}
 
-  setrawmode();
+/****************************************************************************
+ * Name: simuart_getc
+ ****************************************************************************/
 
-  /* Restore the original terminal mode before exit */
+int simuart_getc(int fd)
+{
+  int ret;
+  unsigned char ch;
 
-  atexit(restoremode);
+  ret = read(fd, &ch, 1);
+  return ret < 0 ? ret : ch;
 }
 
 /****************************************************************************
- * Name: simuart_putc
+ * Name: simuart_getcflag
  ****************************************************************************/
 
-int simuart_putc(int ch)
+int simuart_getcflag(int fd, tcflag_t *cflag)
 {
-  int ret = ch;
+  struct termios t;
+  int ret;
 
-  if (ch == '\n')
+  ret = tcgetattr(fd, &t);
+  if (ret < 0)
     {
-      ret = simuart_putraw('\r');
+      ret = -errno;
     }
-
-  if (ret >= 0)
+  else
     {
-      ret = simuart_putraw(ch);
+      *cflag = t.c_cflag;
     }
 
   return ret;
 }
 
 /****************************************************************************
- * Name: simuart_getc
+ * Name: simuart_setcflag
  ****************************************************************************/
 
-int simuart_getc(void)
+int simuart_setcflag(int fd, tcflag_t cflag)
 {
+  struct termios t;
   int ret;
-  unsigned char ch;
 
-  ret = read(0, &ch, 1);
-  return ret < 0 ? ret : ch;
+  ret = tcgetattr(fd, &t);
+  if (!ret)
+    {
+      t.c_cflag = cflag;
+      ret = tcsetattr(fd, TCSANOW, &t);
+    }
+
+  if (ret < 0)
+    {
+      ret = -errno;
+    }
+
+  return ret;
 }
 
 /****************************************************************************
  * Name: simuart_checkc
  ****************************************************************************/
 
-bool simuart_checkc(void)
+bool simuart_checkc(int fd)
 {
   struct pollfd pfd;
 
-  pfd.fd     = 0;
+  pfd.fd     = fd;
   pfd.events = POLLIN;
   return poll(&pfd, 1, 0) == 1;
 }
diff --git a/arch/sim/src/sim/up_uart.c b/arch/sim/src/sim/up_uart.c
new file mode 100644
index 0000000..5d08cb5
--- /dev/null
+++ b/arch/sim/src/sim/up_uart.c
@@ -0,0 +1,572 @@
+/****************************************************************************
+ * arch/sim/src/sim/up_uart.c
+ *
+ *   Copyright (C) 2007-2009, 2013 Gregory Nutt. All rights reserved.
+ *   Author: Gregory Nutt <gn...@nuttx.org>
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name NuttX nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <nuttx/config.h>
+#include <nuttx/serial/serial.h>
+#include <nuttx/fs/ioctl.h>
+#include <sys/types.h>
+#include <fcntl.h>
+#include <errno.h>
+
+#include "up_internal.h"
+
+#if defined(USE_DEVCONSOLE) || CONFIG_SIM_UART_NUMBER > 0
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define BUFSIZE 256
+
+/****************************************************************************
+ * Private Types
+ ****************************************************************************/
+
+struct tty_priv_s
+{
+  /* tty-port path name */
+
+  FAR const char *path;
+
+  /* The file descriptor. It is returned by open */
+
+  int             fd;
+};
+
+/****************************************************************************
+ * Private Function Prototypes
+ ****************************************************************************/
+
+static int  tty_setup(FAR struct uart_dev_s *dev);
+static void tty_shutdown(FAR struct uart_dev_s *dev);
+static int  tty_attach(FAR struct uart_dev_s *dev);
+static void tty_detach(FAR struct uart_dev_s *dev);
+static int  tty_ioctl(FAR struct file *filep, int cmd,
+                      unsigned long arg);
+static int  tty_receive(FAR struct uart_dev_s *dev, uint32_t *status);
+static void tty_rxint(FAR struct uart_dev_s *dev, bool enable);
+static bool tty_rxavailable(FAR struct uart_dev_s *dev);
+static void tty_send(FAR struct uart_dev_s *dev, int ch);
+static void tty_txint(FAR struct uart_dev_s *dev, bool enable);
+static bool tty_txready(FAR struct uart_dev_s *dev);
+static bool tty_txempty(FAR struct uart_dev_s *dev);
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+static const struct uart_ops_s g_tty_ops =
+{
+  .setup          = tty_setup,
+  .shutdown       = tty_shutdown,
+  .attach         = tty_attach,
+  .detach         = tty_detach,
+  .ioctl          = tty_ioctl,
+  .receive        = tty_receive,
+  .rxint          = tty_rxint,
+  .rxavailable    = tty_rxavailable,
+  .send           = tty_send,
+  .txint          = tty_txint,
+  .txready        = tty_txready,
+  .txempty        = tty_txempty,
+};
+#endif
+
+#ifdef USE_DEVCONSOLE
+static char g_console_rxbuf[BUFSIZE];
+static char g_console_txbuf[BUFSIZE];
+#endif
+
+#ifdef CONFIG_SIM_UART0_NAME
+static char g_tty0_rxbuf[BUFSIZE];
+static char g_tty0_txbuf[BUFSIZE];
+#endif
+
+#ifdef CONFIG_SIM_UART1_NAME
+static char g_tty1_rxbuf[BUFSIZE];
+static char g_tty1_txbuf[BUFSIZE];
+#endif
+
+#ifdef CONFIG_SIM_UART2_NAME
+static char g_tty2_rxbuf[BUFSIZE];
+static char g_tty2_txbuf[BUFSIZE];
+#endif
+
+#ifdef CONFIG_SIM_UART3_NAME
+static char g_tty3_rxbuf[BUFSIZE];
+static char g_tty3_txbuf[BUFSIZE];
+#endif
+
+#ifdef USE_DEVCONSOLE
+static struct uart_dev_s g_console_dev =
+{
+  .isconsole      = true,
+  .ops            = &g_tty_ops,
+  .xmit =
+  {
+    .size         = BUFSIZE,
+    .buffer       = g_console_txbuf,
+  },
+  .recv =
+  {
+    .size         = BUFSIZE,
+    .buffer       = g_console_rxbuf,
+  },
+};
+#endif
+
+#ifdef CONFIG_SIM_UART0_NAME
+static struct tty_priv_s g_tty0_priv =
+{
+  .path           = CONFIG_SIM_UART0_NAME,
+  .fd             = -1,
+};
+
+static struct uart_dev_s g_tty0_dev =
+{
+  .ops            = &g_tty_ops,
+  .priv           = &g_tty0_priv,
+  .xmit =
+  {
+    .size         = BUFSIZE,
+    .buffer       = g_tty0_txbuf,
+  },
+  .recv =
+  {
+    .size         = BUFSIZE,
+    .buffer       = g_tty0_rxbuf,
+  },
+};
+#endif
+
+#ifdef CONFIG_SIM_UART1_NAME
+static struct tty_priv_s g_tty1_priv =
+{
+  .path           = CONFIG_SIM_UART1_NAME,
+  .fd             = -1,
+};
+
+static struct uart_dev_s g_tty1_dev =
+{
+  .ops            = &g_tty_ops,
+  .priv           = &g_tty1_priv,
+  .xmit =
+  {
+    .size         = BUFSIZE,
+    .buffer       = g_tty1_txbuf,
+  },
+  .recv =
+  {
+    .size         = BUFSIZE,
+    .buffer       = g_tty1_rxbuf,
+  },
+};
+#endif
+
+#ifdef CONFIG_SIM_UART2_NAME
+static struct tty_priv_s g_tty2_priv =
+{
+  .path           = CONFIG_SIM_UART2_NAME,
+  .fd             = -1,
+};
+
+static struct uart_dev_s g_tty2_dev =
+{
+  .ops            = &g_tty_ops,
+  .priv           = &g_tty2_priv,
+  .xmit =
+  {
+    .size         = BUFSIZE,
+    .buffer       = g_tty2_txbuf,
+  },
+  .recv =
+  {
+    .size         = BUFSIZE,
+    .buffer       = g_tty2_rxbuf,
+  },
+};
+#endif
+
+#ifdef CONFIG_SIM_UART3_NAME
+static struct tty_priv_s g_tty3_priv =
+{
+  .path           = CONFIG_SIM_UART3_NAME,
+  .fd             = -1,
+};
+
+static struct uart_dev_s g_tty3_dev =
+{
+  .ops            = &g_tty_ops,
+  .priv           = &g_tty3_priv,
+  .xmit =
+  {
+    .size         = BUFSIZE,
+    .buffer       = g_tty3_txbuf,
+  },
+  .recv =
+  {
+    .size         = BUFSIZE,
+    .buffer       = g_tty3_rxbuf,
+  },
+};
+#endif
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+#if defined(USE_DEVCONSOLE) || CONFIG_SIM_UART_NUMBER > 0
+/****************************************************************************
+ * Name: tty_setup
+ *
+ * Description:
+ *   Configure the UART baud, bits, parity, fifos, etc. This
+ *   method is called the first time that the serial port is
+ *   opened.
+ *
+ ****************************************************************************/
+
+static int tty_setup(FAR struct uart_dev_s *dev)
+{
+  FAR struct tty_priv_s *priv = dev->priv;
+
+  if (!dev->isconsole && priv->fd < 0)
+    {
+      priv->fd = simuart_open(priv->path);
+      if (priv->fd < 0)
+        {
+          return priv->fd;
+        }
+    }
+
+  return OK;
+}
+
+/****************************************************************************
+ * Name: tty_shutdown
+ *
+ * Description:
+ *   Disable the UART.  This method is called when the serial
+ *   port is closed
+ *
+ ****************************************************************************/
+
+static void tty_shutdown(struct uart_dev_s *dev)
+{
+  FAR struct tty_priv_s *priv = dev->priv;
+
+  if (!dev->isconsole && priv->fd >= 0)
+    {
+      /* close file Description and reset fd */
+
+      simuart_close(priv->fd);
+      priv->fd = -1;
+    }
+}
+
+/****************************************************************************
+ * Name: tty_attach
+ *
+ * Description:
+ *   Configure the UART to operation in interrupt driven mode.  This method
+ *   is called when the serial port is opened.  Normally, this is just after
+ *   the setup() method is called, however, the serial console may operate in
+ *   a non-interrupt driven mode during the boot phase.
+ *
+ *   RX and TX interrupts are not enabled when by the attach method (unless
+ *   the hardware supports multiple levels of interrupt enabling).  The RX
+ *   and TX interrupts are not enabled until the txint() and rxint() methods
+ *   are called.
+ *
+ ****************************************************************************/
+
+static int tty_attach(struct uart_dev_s *dev)
+{
+  return OK;
+}
+
+/****************************************************************************
+ * Name: tty_detach
+ *
+ * Description:
+ *   Detach UART interrupts.  This method is called when the serial port is
+ *   closed normally just before the shutdown method is called.  The
+ *   exception is the serial console which is never shutdown.
+ *
+ ****************************************************************************/
+
+static void tty_detach(FAR struct uart_dev_s *dev)
+{
+}
+
+/****************************************************************************
+ * Name: tty_ioctl
+ *
+ * Description:
+ *   All ioctl calls will be routed through this method
+ *
+ ****************************************************************************/
+
+static int tty_ioctl(struct file *filep, int cmd, unsigned long arg)
+{
+#ifdef CONFIG_SERIAL_TERMIOS
+  FAR struct inode *inode = filep->f_inode;
+  FAR struct uart_dev_s *dev = inode->i_private;
+  FAR struct tty_priv_s *priv = dev->priv;
+  FAR struct termios *termiosp = (FAR struct termios *)(uintptr_t)arg;
+
+  if (!termiosp)
+    {
+      return -EINVAL;
+    }
+
+  switch (cmd)
+    {
+      case TCGETS:
+        return simuart_getcflag(dev->isconsole ? 0 : priv->fd,
+                                &termiosp->c_cflag);
+
+      case TCSETS:
+        return simuart_setcflag(dev->isconsole ? 0 : priv->fd,
+                               termiosp->c_cflag);
+    }
+#endif
+
+  return -ENOTTY;
+}
+
+/****************************************************************************
+ * Name: tty_receive
+ *
+ * Description:
+ *   Called (usually) from the interrupt level to receive one
+ *   character from the UART.  Error bits associated with the
+ *   receipt are provided in the return 'status'.
+ *
+ ****************************************************************************/
+
+static int tty_receive(struct uart_dev_s *dev, uint32_t *status)
+{
+  FAR struct tty_priv_s *priv = dev->priv;
+
+  *status = 0;
+  return simuart_getc(dev->isconsole ? 0 : priv->fd);
+}
+
+/****************************************************************************
+ * Name: tty_rxint
+ *
+ * Description:
+ *   Call to enable or disable RX interrupts
+ *
+ ****************************************************************************/
+
+static void tty_rxint(struct uart_dev_s *dev, bool enable)
+{
+}
+
+/****************************************************************************
+ * Name: tty_rxavailable
+ *
+ * Description:
+ *   Return true if the receive fifo is not empty
+ *
+ ****************************************************************************/
+
+static bool tty_rxavailable(struct uart_dev_s *dev)
+{
+  FAR struct tty_priv_s *priv = dev->priv;
+
+  return simuart_checkc(dev->isconsole ? 0 : priv->fd);
+}
+
+/****************************************************************************
+ * Name: tty_send
+ *
+ * Description:
+ *   This method will send one byte on the UART
+ *
+ ****************************************************************************/
+
+static void tty_send(struct uart_dev_s *dev, int ch)
+{
+  FAR struct tty_priv_s *priv = dev->priv;
+
+  /* For console device */
+
+  if (dev->isconsole && ch == '\n')
+    {
+      simuart_putc(1, '\r');
+    }
+
+  simuart_putc(dev->isconsole ? 1 : priv->fd, ch);
+}
+
+/****************************************************************************
+ * Name: tty_txint
+ *
+ * Description:
+ *   Call to enable or disable TX interrupts
+ *
+ ****************************************************************************/
+
+static void tty_txint(struct uart_dev_s *dev, bool enable)
+{
+  if (enable)
+    {
+      uart_xmitchars(dev);
+    }
+}
+
+/****************************************************************************
+ * Name: uart_txready
+ *
+ * Description:
+ *   Return true if the tranmsit fifo is not full
+ *
+ ****************************************************************************/
+
+static bool tty_txready(struct uart_dev_s *dev)
+{
+  return true;
+}
+
+/****************************************************************************
+ * Name: tty_txempty
+ *
+ * Description:
+ *   Return true if the transmit fifo is empty
+ *
+ ****************************************************************************/
+
+static bool tty_txempty(struct uart_dev_s *dev)
+{
+  return true;
+}
+#endif
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: up_uart
+ ****************************************************************************/
+
+void up_uartinit(void)
+{
+#ifdef USE_DEVCONSOLE
+  /* Start the simulated UART device */
+
+  simuart_start();
+
+  /* Register console device */
+
+  uart_register("/dev/console", &g_console_dev);
+#endif
+
+#ifdef CONFIG_SIM_UART0_NAME
+  uart_register(CONFIG_SIM_UART0_NAME, &g_tty0_dev);
+#endif
+
+#ifdef CONFIG_SIM_UART1_NAME
+  uart_register(CONFIG_SIM_UART1_NAME, &g_tty1_dev);
+#endif
+
+#ifdef CONFIG_SIM_UART2_NAME
+  uart_register(CONFIG_SIM_UART2_NAME, &g_tty2_dev);
+#endif
+
+#ifdef CONFIG_SIM_UART3_NAME
+  uart_register(CONFIG_SIM_UART3_NAME, &g_tty3_dev);
+#endif
+}
+
+/****************************************************************************
+ * Name: up_uartloop
+ ****************************************************************************/
+
+void up_uartloop(void)
+{
+#ifdef USE_DEVCONSOLE
+  if (simuart_checkc(0))
+    {
+      uart_recvchars(&g_console_dev);
+    }
+#endif
+
+#ifdef CONFIG_SIM_UART0_NAME
+  if (g_tty0_priv.fd > 0 && simuart_checkc(g_tty0_priv.fd))
+    {
+      uart_recvchars(&g_tty0_dev);
+    }
+#endif
+
+#ifdef CONFIG_SIM_UART1_NAME
+  if (g_tty1_priv.fd > 0 && simuart_checkc(g_tty1_priv.fd))
+    {
+      uart_recvchars(&g_tty1_dev);
+    }
+#endif
+
+#ifdef CONFIG_SIM_UART2_NAME
+  if (g_tty2_priv.fd > 0 && simuart_checkc(g_tty2_priv.fd))
+    {
+      uart_recvchars(&g_tty2_dev);
+    }
+#endif
+
+#ifdef CONFIG_SIM_UART3_NAME
+  if (g_tty3_priv.fd > 0 && simuart_checkc(g_tty3_priv.fd))
+    {
+      uart_recvchars(&g_tty3_dev);
+    }
+#endif
+}
+
+/****************************************************************************
+ * Name: up_putc
+ ****************************************************************************/
+
+#ifdef USE_DEVCONSOLE
+int up_putc(int ch)
+{
+  tty_send(&g_console_dev, ch);
+  return 0;
+}
+#endif


[incubator-nuttx] 02/02: libc/termios: modify termios setting follow linux and posix

Posted by pr...@apache.org.
This is an automated email from the ASF dual-hosted git repository.

protobits pushed a commit to branch master
in repository https://gitbox.apache.org/repos/asf/incubator-nuttx.git

commit 03c7951cbd1430f238f18582631e586269938997
Author: dongjiuzhu <do...@xiaomi.com>
AuthorDate: Mon Sep 7 22:28:45 2020 +0800

    libc/termios: modify termios setting follow linux and posix
    
    Change-Id: Id323b3169e74f4153fd8d132d20926b7fb8336a3
    Signed-off-by: dongjiuzhu <do...@xiaomi.com>
---
 include/termios.h                  | 242 +++++++++++++++++--------------------
 libs/libc/termios/Make.defs        |   2 +-
 libs/libc/termios/lib_cfgetspeed.c |  77 ------------
 libs/libc/termios/lib_cfsetspeed.c |  96 ---------------
 libs/libc/termios/lib_cfspeed.c    | 196 ++++++++++++++++++++++++++++++
 5 files changed, 309 insertions(+), 304 deletions(-)

diff --git a/include/termios.h b/include/termios.h
index 5032a6b..291b3f3 100644
--- a/include/termios.h
+++ b/include/termios.h
@@ -50,87 +50,81 @@
 
 /* Terminal input modes (c_iflag in the termios structure) */
 
-#define BRKINT    (1 << 0)  /* Bit 0:  Signal interrupt on break */
-#define ICRNL     (1 << 1)  /* Bit 1:  Map CR to NL on input */
-#define IGNBRK    (1 << 2)  /* Bit 2:  Ignore break condition */
-#define IGNCR     (1 << 3)  /* Bit 3:  Ignore CR */
-#define IGNPAR    (1 << 4)  /* Bit 4:  Ignore characters with parity errors */
-#define INLCR     (1 << 5)  /* Bit 5:  Map NL to CR on input */
-#define INPCK     (1 << 6)  /* Bit 6:  Enable input parity check */
-#define ISTRIP    (1 << 7)  /* Bit 7:  Strip character */
-#define IUCLC     (1 << 8)  /* Bit 8:  Map upper-case to lower-case on input
-                             *         (LEGACY) */
-#define IXANY     (1 << 9)  /* Bit 9:  Enable any character to restart output */
-#define IXOFF     (1 << 10) /* Bit 10: Enable start/stop input control */
-#define IXON      (1 << 11) /* Bit 11: Enable start/stop output control */
-#define PARMRK    (1 << 12) /* Bit 12: Mark parity errors */
+#define IGNBRK    (1 << 0)  /* Bit 0:  Ignore break condition */
+#define BRKINT    (1 << 1)  /* Bit 1:  Signal interrupt on break */
+#define IGNPAR    (1 << 2)  /* Bit 2:  Ignore characters with parity errors */
+#define PARMRK    (1 << 3)  /* Bit 3:  Mark parity errors */
+#define INPCK     (1 << 4)  /* Bit 4:  Enable input parity check */
+#define ISTRIP    (1 << 5)  /* Bit 5:  Strip character */
+#define INLCR     (1 << 6)  /* Bit 6:  Map NL to CR on input */
+#define IGNCR     (1 << 7)  /* Bit 7:  Ignore CR */
+#define ICRNL     (1 << 8)  /* Bit 8:  Map CR to NL on input */
+#define IUCLC     (1 << 9)  /* Bit 9:  Map upper-case to lower-case on input (LEGACY) */
+#define IXON      (1 << 10) /* Bit 10: Enable start/stop output control */
+#define IXANY     (1 << 11) /* Bit 11: Enable any character to restart output */
+#define IXOFF     (1 << 12) /* Bit 12: Enable start/stop input control */
 
 /* Terminal output modes (c_oflag in the termios structure) */
 
 #define OPOST     (1 << 0)  /* Bit 0:  Post-process output */
-#define OLCUC     (1 << 1)  /* Bit 1:  Map lower-case to upper-case on
-*                            *         output (LEGACY) */
+#define OLCUC     (1 << 1)  /* Bit 1:  Map lower-case to upper-case on output (LEGACY) */
 #define ONLCR     (1 << 2)  /* Bit 2:  Map NL to CR-NL on output */
 #define OCRNL     (1 << 3)  /* Bit 3:  Map CR to NL on output */
 #define ONOCR     (1 << 4)  /* Bit 4:  No CR output at column 0 */
 #define ONLRET    (1 << 5)  /* Bit 5:  NL performs CR function */
 #define OFILL     (1 << 6)  /* Bit 6:  Use fill characters for delay */
-#define NLDLY     (1 << 7)  /* Bit 7:  Select newline delays: */
-#  define NL0     (0 << 7)  /*   Newline character type 0 */
-#  define NL1     (1 << 7)  /*   Newline character type 1 */
-#define CRDLY     (3 << 8)  /* Bits 8-9:  Select carriage-return delays: */
-#  define CR0     (0 << 8)  /*   Carriage-return delay type 0 */
-#  define CR1     (1 << 8)  /*   Carriage-return delay type 1 */
-#  define CR2     (2 << 8)  /*   Carriage-return delay type 2 */
-#  define CR3     (3 << 8)  /*   Carriage-return delay type 3 */
-#define TABDLY    (3 << 10) /* Bit 10-11:  Select horizontal-tab delays: */
-#  define TAB0    (0 << 10) /*   Horizontal-tab delay type 0 */
-#  define TAB1    (1 << 10) /*   Horizontal-tab delay type 1 */
-#  define TAB2    (2 << 10) /*   Horizontal-tab delay type 2 */
-#  define TAB3    (3 << 10) /*   Expand tabs to spaces */
-#define BSDLY     (1 << 12) /* Bit 12:  Select backspace delays: */
-#  define BS0     (0 << 12) /*   Backspace-delay type 0 */
-#  define BS1     (1 << 12) /*   Backspace-delay type 1 */
-#define VTDLY     (1 << 13) /* Bit 13:  Select vertical-tab delays: */
-#  define VT0     (0 << 13) /*   Vertical-tab delay type 0 */
-#  define VT1     (1 << 13) /*   Vertical-tab delay type 1 */
-#define FFDLY     (1 << 14) /* Bit 14:  Select form-feed delays: */
-#  define FF0     (0 << 14) /*   Form-feed delay type 0 */
-#  define FF1     (1 << 14) /*   Form-feed delay type 1 */
+#define NLDLY     (1 << 8)  /* Bit 8:  Select newline delays: */
+#  define NL0     (0 << 8)  /*   Newline character type 0 */
+#  define NL1     (1 << 8)  /*   Newline character type 1 */
+#define CRDLY     (3 << 9)  /* Bits 9-10: Select carriage-return delays: */
+#  define CR0     (0 << 9)  /*   Carriage-return delay type 0 */
+#  define CR1     (1 << 9)  /*   Carriage-return delay type 1 */
+#  define CR2     (2 << 9)  /*   Carriage-return delay type 2 */
+#  define CR3     (3 << 9)  /*   Carriage-return delay type 3 */
+#define TABDLY    (3 << 11) /* Bits 11-12: Select horizontal-tab delays: */
+#  define TAB0    (0 << 11) /*   Horizontal-tab delay type 0 */
+#  define TAB1    (1 << 11) /*   Horizontal-tab delay type 1 */
+#  define TAB2    (2 << 11) /*   Horizontal-tab delay type 2 */
+#  define TAB3    (3 << 11) /*   Expand tabs to spaces */
+#define BSDLY     (1 << 13) /* Bit 13: Select backspace delays: */
+#  define BS0     (0 << 13) /*   Backspace-delay type 0 */
+#  define BS1     (1 << 13) /*   Backspace-delay type 1 */
+#define VTDLY     (1 << 14) /* Bit 14: Select vertical-tab delays: */
+#  define VT0     (0 << 14) /*   Vertical-tab delay type 0 */
+#  define VT1     (1 << 14) /*   Vertical-tab delay type 1 */
+#define FFDLY     (1 << 15) /* Bit 15: Select form-feed delays: */
+#  define FF0     (0 << 15) /*   Form-feed delay type 0 */
+#  define FF1     (1 << 15) /*   Form-feed delay type 1 */
 
 /* Control Modes (c_cflag in the termios structure) */
 
-#define CSIZE     (3 << 0)  /* Bits 0-1: Character size: */
-#  define CS5     (0 << 0)  /*   5 bits */
-#  define CS6     (1 << 0)  /*   6 bits */
-#  define CS7     (2 << 0)  /*   7 bits */
-#  define CS8     (3 << 0)  /*   8 bits */
-#define CSTOPB    (1 << 2)  /* Bit 2: Send two stop bits, else one */
-#define CREAD     (1 << 3)  /* Bit 3: Enable receiver */
-#define PARENB    (1 << 4)  /* Bit 4: Parity enable */
-#define PARODD    (1 << 5)  /* Bit 5: Odd parity, else even */
-#define HUPCL     (1 << 6)  /* Bit 6: Hang up on last close */
-#define CLOCAL    (1 << 7)  /* Bit 7: Ignore modem status lines */
-#define CCTS_OFLOW (1 << 8) /* Bit 8: CTS flow control of output */
-#define CRTS_IFLOW (1 << 9) /* Bit 9: RTS flow control of input */
-#define CRTSCTS   (CRTS_IFLOW | CCTS_OFLOW)
+#define CSIZE     (3 << 4)  /* Bits 4-5: Character size: */
+#  define CS5     (0 << 4)  /*   5 bits */
+#  define CS6     (1 << 4)  /*   6 bits */
+#  define CS7     (2 << 4)  /*   7 bits */
+#  define CS8     (3 << 4)  /*   8 bits */
+#define CSTOPB    (1 << 6)  /* Bit 6:  Send two stop bits, else one */
+#define CREAD     (1 << 7)  /* Bit 7:  Enable receiver */
+#define PARENB    (1 << 8)  /* Bit 8: Parity enable */
+#define PARODD    (1 << 9)  /* Bit 9: Odd parity, else even */
+#define HUPCL     (1 << 10) /* Bit 10: Hang up on last close */
+#define CLOCAL    (1 << 11) /* Bit 11: Ignore modem status lines */
+#define CCTS_OFLOW (1 << 29)/* Bit 29: CTS flow control of output */
+#define CRTS_IFLOW (1 << 31)/* Bit 31: RTS flow control of input */
+#define CRTSCTS   (CCTS_OFLOW | CRTS_IFLOW)
 
 /* Local Modes (c_lflag in the termios structure) */
 
-#define ECHO      (1 << 0)  /* Bit 0:  Enable echo */
-#define ECHOE     (1 << 1)  /* Bit 1:  Echo erase character as error-
-                             *         correcting backspace */
-#define ECHOK     (1 << 2)  /* Bit 2:  Echo KILL */
-#define ECHONL    (1 << 3)  /* Bit 3:  Echo NL */
-#define ICANON    (1 << 4)  /* Bit 4:  Canonical input (erase and kill
-                             *         processing) */
-#define IEXTEN    (1 << 5)  /* Bit 5:  Enable extended input character
-                             *         processing */
-#define ISIG      (1 << 6)  /* Bit 6:  Enable signals */
+#define ISIG      (1 << 0)  /* Bit 0:  Enable signals */
+#define ICANON    (1 << 1)  /* Bit 1:  Canonical input (erase and kill processing) */
+#define XCASE     (1 << 2)  /* Bit 2:  Canonical upper/lower presentation (LEGACY) */
+#define ECHO      (1 << 3)  /* Bit 3:  Enable echo */
+#define ECHOE     (1 << 4)  /* Bit 4:  Echo erase character as error correcting backspace */
+#define ECHOK     (1 << 5)  /* Bit 5:  Echo KILL */
+#define ECHONL    (1 << 6)  /* Bit 6:  Echo NL */
 #define NOFLSH    (1 << 7)  /* Bit 7:  Disable flush after interrupt or quit */
 #define TOSTOP    (1 << 8)  /* Bit 8:  Send SIGTTOU for background output */
-#define XCASE     (1 << 9)  /* Bit 9:  Canonical upper/lower presentation
-                             *         (LEGACY) */
+#define IEXTEN    (1 << 15) /* Bit 15: Enable extended input character processing */
 
 /* The following are subscript names for the termios c_cc array.
  *
@@ -155,63 +149,57 @@
  *   VTIME:  Timeout in deciseconds for non-canonical read
  */
 
-#define VEOF      0         /* Bit 0:  EOF character (canonical mode) */
-#define VMIN      VEOF      /* Bit 0:  MIN value (non-canonical mode) */
-#define VEOL      1         /* Bit 1:  EOL character (canonical mode) */
-#define VTIME     VEOL      /* Bit 1:  TIME value (non-canonical mode) */
+#define VINTR     0         /* Bit 0:  INTR character */
+#define VQUIT     1         /* Bit 1:  QUIT character */
 #define VERASE    2         /* Bit 2:  ERASE character (canonical mode) */
-#define VINTR     3         /* Bit 3:  INTR character */
-#define VKILL     4         /* Bit 4:  KILL character (canonical mode) */
-#define VQUIT     5         /* Bit 5:  QUIT character */
-#define VSTART    6         /* Bit 6:  START character */
-#define VSTOP     7         /* Bit 7:  STOP character */
-#define VSUSP     8         /* Bit 8:  SUSP character */
-#define NCCS      9         /* Bit 9:  Size of the array c_cc for control
-                             *         characters */
+#define VKILL     3         /* Bit 3:  KILL character (canonical mode) */
+#define VEOF      4         /* Bit 4:  EOF character (canonical mode) */
+#define VTIME     5         /* Bit 5:  TIME value (non-canonical mode) */
+#define VMIN      6         /* Bit 6:  MIN value (non-canonical mode) */
+#define VSTART    8         /* Bit 8:  START character */
+#define VSTOP     9         /* Bit 9:  STOP character */
+#define VSUSP     10        /* Bit 10: SUSP character */
+#define VEOL      11        /* Bit 11: EOL character (canonical mode) */
+#define NCCS      12        /* Bit 12: Size of the array c_cc for control characters */
 
 /* Baud Rate Selection.  These are instances of type speed_t.  Values of
  * 38400 and below are specified by POSIX; values above 38400 are sometimes
  * referred to as extended values and most values appear in most termios.h
  * implementations.
- *
- * NOTE that is NuttX that the encoding of the speed_t values is simply the
- * value of the baud itself.  So this opens a window for non-portable abuse
- * of the speed-related interfaces:  The defined values should be used where-
- * ever possible for reasons of portability.
  */
 
-#define B0        0         /* Hang up */
-#define B50       50        /* 50 baud */
-#define B75       75        /* 75 baud */
-#define B110      110       /* 110 baud */
-#define B134      134       /* 134.5 baud */
-#define B150      150       /* 150 baud */
-#define B200      200       /* 200 baud */
-#define B300      300       /* 300 baud */
-#define B600      600       /* 600 baud */
-#define B1200     1200      /* 1,200 baud */
-#define B1800     1800      /* 1,800 baud */
-#define B2400     2400      /* 2,400 baud */
-#define B4800     4800      /* 4,800 baud */
-#define B9600     9600      /* 9,600 baud */
-#define B19200    19200     /* 19,200 baud */
-#define B38400    38400     /* 38,400 baud */
-
-#define B57600    57600     /* 57,600 baud */
-#define B115200   115200    /* 115,200 baud */
-#define B128000   128000    /* 128,000 baud */
-#define B230400   230400    /* 230,400 baud */
-#define B256000   256000    /* 256,000 baud */
-#define B460800   460800    /* 460,800 baud */
-#define B500000   500000    /* 500,000 baud */
-#define B576000   576000    /* 576,000 baud */
-#define B921600   921600    /* 921,600 baud */
-#define B1000000  1000000   /* 1,000,000 baud */
-#define B1152000  1152000   /* 1,152,000 baud */
-#define B1500000  1500000   /* 1,500,000 baud */
-#define B2000000  2000000   /* 2,000,000 baud */
-#define B2500000  2500000   /* 2,500,000 baud */
-#define B3000000  3000000   /* 3,000,000 baud */
+#define B0        0000000   /* Hang up */
+#define B50       0000001   /* 50 baud */
+#define B75       0000002   /* 75 baud */
+#define B110      0000003   /* 110 baud */
+#define B134      0000004   /* 134.5 baud */
+#define B150      0000005   /* 150 baud */
+#define B200      0000006   /* 200 baud */
+#define B300      0000007   /* 300 baud */
+#define B600      0000010   /* 600 baud */
+#define B1200     0000011   /* 1,200 baud */
+#define B1800     0000012   /* 1,800 baud */
+#define B2400     0000013   /* 2,400 baud */
+#define B4800     0000014   /* 4,800 baud */
+#define B9600     0000015   /* 9,600 baud */
+#define B19200    0000016   /* 19,200 baud */
+#define B38400    0000017   /* 38,400 baud */
+
+#define B57600    0010001   /* 57,600 baud */
+#define B115200   0010002   /* 115,200 baud */
+#define B230400   0010003   /* 230,400 baud */
+#define B460800   0010004   /* 460,800 baud */
+#define B500000   0010005   /* 500,000 baud */
+#define B576000   0010006   /* 576,000 baud */
+#define B921600   0010007   /* 921,600 baud */
+#define B1000000  0010010   /* 1,000,000 baud */
+#define B1152000  0010011   /* 1,152,000 baud */
+#define B1500000  0010012   /* 1,500,000 baud */
+#define B2000000  0010013   /* 2,000,000 baud */
+#define B2500000  0010014   /* 2,500,000 baud */
+#define B3000000  0010015   /* 3,000,000 baud */
+#define B3500000  0010016   /* 3,500,000 baud */
+#define B4000000  0010017   /* 4,000,000 baud */
 
 /* Attribute Selection (used with tcsetattr()) */
 
@@ -223,18 +211,18 @@
 /* Line Control (used with tcflush()) */
 
 #define TCIFLUSH  0         /* Flush pending input */
-#define TCIOFLUSH 1         /* Flush both pending input and untransmitted
+#define TCOFLUSH  1         /* Flush untransmitted output */
+#define TCIOFLUSH 2         /* Flush both pending input and untransmitted
                              * output */
-#define TCOFLUSH  2         /* Flush untransmitted output */
 
 /* Constants for use with tcflow() */
 
-#define TCIOFF    0         /* Transmit a STOP character, intended to
+#define TCOOFF    0         /* Suspend output */
+#define TCOON     1         /* Restart output */
+#define TCIOFF    2         /* Transmit a STOP character, intended to
                              * suspend input data */
-#define TCION     1         /* Transmit a START character, intended to
+#define TCION     3         /* Transmit a START character, intended to
                              * restart input data */
-#define TCOOFF    2         /* Suspend output */
-#define TCOON     3         /* Restart output */
 
 /****************************************************************************
  * Public Type Definitions
@@ -242,12 +230,12 @@
 
 /* Baud rate selection */
 
-typedef uint32_t speed_t;   /* Used for terminal baud rates */
+typedef unsigned int  speed_t;   /* Used for terminal baud rates */
 
 /* Types used within the termios structure */
 
-typedef uint16_t tcflag_t;  /* Used for terminal modes */
-typedef int      cc_t;      /* Used for terminal special characters */
+typedef unsigned int  tcflag_t;  /* Used for terminal modes */
+typedef unsigned char cc_t;      /* Used for terminal special characters */
 
 /* The termios structure */
 
@@ -260,13 +248,6 @@ struct termios
   tcflag_t  c_cflag;        /* Control modes */
   tcflag_t  c_lflag;        /* Local modes */
   cc_t      c_cc[NCCS];     /* Control chars */
-
-  /* Implementation specific fields.  For portability reasons, these fields
-   * should not be accessed directly, but rather through only through the
-   * cf[set|get][o|i]speed() POSIX interfaces.
-   */
-
-  speed_t c_speed;          /* Input/output speed (non-POSIX)*/
 };
 
 /****************************************************************************
@@ -286,6 +267,7 @@ extern "C"
  * does not control input/output baud independently.  Both must be the same.
  * The POSIX standard interfaces, cfigetispeed() and cfigetospeed() are
  * supported by simply defining them to be cfgetspeed().
+ * The return value is baud value(9600).
  */
 
 speed_t cfgetspeed(FAR const struct termios *termiosp);
@@ -297,6 +279,7 @@ speed_t cfgetspeed(FAR const struct termios *termiosp);
  * not control input/output baud independently.  Both must be the same.
  * The POSIX standard interfaces, cfigetispeed() and cfigetospeed() are
  * supported by simply defining them to be cfsetspeed().
+ * Speed could be baud value(9600) or could be baud mask(B9600).
  */
 
 int cfsetspeed(FAR struct termios *termiosp, speed_t speed);
@@ -337,7 +320,6 @@ int tcsendbreak(int fd, int duration);
 
 int tcsetattr(int fd, int options, FAR const struct termios *termiosp);
 
-
 #undef EXTERN
 #ifdef __cplusplus
 }
diff --git a/libs/libc/termios/Make.defs b/libs/libc/termios/Make.defs
index 2d9f345..a6f8df9 100644
--- a/libs/libc/termios/Make.defs
+++ b/libs/libc/termios/Make.defs
@@ -40,7 +40,7 @@ ifeq ($(CONFIG_SERIAL_TERMIOS),y)
 
 # Add the termios C files to the build
 
-CSRCS += lib_cfgetspeed.c lib_cfsetspeed.c lib_cfmakeraw.c lib_isatty.c lib_tcflush.c
+CSRCS += lib_cfspeed.c lib_cfmakeraw.c lib_isatty.c lib_tcflush.c
 CSRCS += lib_tcflow.c lib_tcgetattr.c lib_tcsetattr.c
 
 # Add the termios directory to the build
diff --git a/libs/libc/termios/lib_cfgetspeed.c b/libs/libc/termios/lib_cfgetspeed.c
deleted file mode 100644
index ce53227..0000000
--- a/libs/libc/termios/lib_cfgetspeed.c
+++ /dev/null
@@ -1,77 +0,0 @@
-/****************************************************************************
- * libs/libc/termios/lib_cfgetspeed.c
- *
- *   Copyright (C) 2012 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gn...@nuttx.org>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <termios.h>
-#include <assert.h>
-
-/****************************************************************************
- * Public Functions
- ****************************************************************************/
-
-/****************************************************************************
- * Name: cfgetspeed
- *
- * Description:
- *   The cfgetspeed() function is a non-POSIX function will extract the baud
- *   from the termios structure to which the termiosp argument points.
- *
- *   This function will return exactly the value in the termios data
- *   structure, without interpretation.
- *
- *   NOTE 1: NuttX does not control input/output baud independently.  Both
- *   must be the same.  The POSIX standard interfaces, cfisetispeed() and
- *   cfisetospeed() are defined to be cfgetspeed() in termios.h.
- *   NOTE 2.  In Nuttx, the speed_t is defined to be uint32_t and the baud
- *   encodings of termios.h are the actual baud values themselves.  Therefore,
- *   any baud value may be returned here... not just those enumerated in
- *   termios.h
- *
- * Input Parameters:
- *   termiosp - The termiosp argument is a pointer to a termios structure.
- *
- * Returned Value:
- *   Encoded baud value from the termios structure.
- *
- ****************************************************************************/
-
-speed_t cfgetspeed(FAR const struct termios *termiosp)
-{
-  DEBUGASSERT(termiosp);
-  return termiosp->c_speed;
-}
diff --git a/libs/libc/termios/lib_cfsetspeed.c b/libs/libc/termios/lib_cfsetspeed.c
deleted file mode 100644
index fe7bdb1..0000000
--- a/libs/libc/termios/lib_cfsetspeed.c
+++ /dev/null
@@ -1,96 +0,0 @@
-/****************************************************************************
- * libs/libc/termios/lib_cfsetspeed.c
- *
- *   Copyright (C) 2012 Gregory Nutt. All rights reserved.
- *   Author: Gregory Nutt <gn...@nuttx.org>
- *
- * Redistribution and use in source and binary forms, with or without
- * modification, are permitted provided that the following conditions
- * are met:
- *
- * 1. Redistributions of source code must retain the above copyright
- *    notice, this list of conditions and the following disclaimer.
- * 2. Redistributions in binary form must reproduce the above copyright
- *    notice, this list of conditions and the following disclaimer in
- *    the documentation and/or other materials provided with the
- *    distribution.
- * 3. Neither the name NuttX nor the names of its contributors may be
- *    used to endorse or promote products derived from this software
- *    without specific prior written permission.
- *
- * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
- * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
- * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
- * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
- * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
- * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
- * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
- * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
- * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
- * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
- * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
- * POSSIBILITY OF SUCH DAMAGE.
- *
- ****************************************************************************/
-
-/****************************************************************************
- * Included Files
- ****************************************************************************/
-
-#include <sys/types.h>
-#include <termios.h>
-#include <assert.h>
-
-/****************************************************************************
- * Name: cfsetspeed
- *
- * Description:
- *   The cfsetspeed() function is a non-POSIX function that sets the baud
- *   stored in the structure pointed to by termiosp to speed.
- *
- *   There is no effect on the baud set in the hardware until a subsequent
- *   successful call to tcsetattr() on the same termios structure.
- *
- *   NOTE 1: NuttX does not control input/output baud independently.  Both
- *   must be the same.  The POSIX standard interfaces, cfisetispeed() and
- *   cfisetospeed() are defined to be cfsetspeed() in termios.h.
- *
- *   NOTE 3:  A consequence of NOTE 1 is that you should never attempt to
- *   set the input and output baud to different values.
- *
- *   Also, the following POSIX requirement cannot be supported: "If the input
- *   baud rate stored in the termios structure pointed to by termios_p is 0,
- *   the input baud rate given to the hardware will be the same as the output
- *   baud rate stored in the termios structure."
- *
- *   NOTE 2.  In Nuttx, the speed_t is defined to be uint32_t and the baud
- *   encodings of termios.h are the actual baud values themselves.  Therefore,
- *   any baud value can be provided as the speed argument here.  However, if
- *   you do so, your code will *NOT* be portable to other environments where
- *   speed_t is smaller and where the termios.h baud values are encoded! To
- *   avoid portability issues, use the baud definitions in termios.h!
- *
- *   Linux, for example, would require this (also non-portable) sequence:
- *
- *     cfsetispeed(termiosp, BOTHER);
- *     termiosp->c_ispeed = baud;
- *
- *     cfsetospeed(termiosp, BOTHER);
- *     termiosp->c_ospeed = baud;
- *
- * Input Parameters:
- *   termiosp - The termiosp argument is a pointer to a termios structure.
- *   speed - The new input speed
- *
- * Returned Value:
- *   Baud is not checked... OK is always returned (this is non-standard
- *   behavior).
- *
- ****************************************************************************/
-
-int cfsetspeed(FAR struct termios *termiosp, speed_t speed)
-{
-  DEBUGASSERT(termiosp);
-  termiosp->c_speed = speed;
-  return OK;
-}
diff --git a/libs/libc/termios/lib_cfspeed.c b/libs/libc/termios/lib_cfspeed.c
new file mode 100644
index 0000000..b0e08f8
--- /dev/null
+++ b/libs/libc/termios/lib_cfspeed.c
@@ -0,0 +1,196 @@
+/****************************************************************************
+ * libs/libc/termios/lib_cfspeed.c
+ *
+ * Licensed to the Apache Software Foundation (ASF) under one or more
+ * contributor license agreements.  See the NOTICE file distributed with
+ * this work for additional information regarding copyright ownership.  The
+ * ASF licenses this file to you under the Apache License, Version 2.0 (the
+ * "License"); you may not use this file except in compliance with the
+ * License.  You may obtain a copy of the License at
+ *
+ *   http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS, WITHOUT
+ * WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.  See the
+ * License for the specific language governing permissions and limitations
+ * under the License.
+ *
+ ****************************************************************************/
+
+/****************************************************************************
+ * Included Files
+ ****************************************************************************/
+
+#include <sys/types.h>
+#include <termios.h>
+#include <assert.h>
+#include <errno.h>
+
+/****************************************************************************
+ * Pre-processor Definitions
+ ****************************************************************************/
+
+#define CBAUD          0010017  /* Baud speed mask (not in POSIX) */
+#define CBAUDEX        0010000  /* Extra baud speed mask, included in CBAUD.
+                                 * (not in POSIX) */
+
+#define ARRAYSIZE(a)   (sizeof((a))/sizeof(a[0]))
+
+/****************************************************************************
+ * Private Data
+ ****************************************************************************/
+
+/* Routine which returns the baud rate of the tty
+ *
+ * Note that the baud_table needs to be kept in sync with the
+ * include/termios.h file.
+ */
+
+static const speed_t g_baud_table[] =
+{
+  0,       50,      75,      110,     134,
+  150,     200,     300,     600,     1200,
+  1800,    2400,    4800,    9600,    19200,
+  38400,   57600,   115200,  230400,  460800,
+  500000,  576000,  921600,  1000000, 1152000,
+  1500000, 2000000, 2500000, 3000000, 3500000,
+  4000000
+};
+
+/****************************************************************************
+ * Private Functions
+ ****************************************************************************/
+
+static int baud_mask(speed_t speed)
+{
+  speed_t idx = 0;
+
+  for (; idx < ARRAYSIZE(g_baud_table); idx++)
+    {
+      if (speed == g_baud_table[idx])
+        {
+          break;
+        }
+    }
+
+  /* we don't find the speed value, it could be mask */
+
+  if (idx == ARRAYSIZE(g_baud_table))
+    {
+      return (speed & ~CBAUD) ? -1 : speed;
+    }
+
+  /* If idx > B38400, we should will idx minus 15, and or CBAUDEX */
+
+  if (idx > B38400)
+    {
+      idx -= B38400;
+      idx |= CBAUDEX;
+    }
+
+  return idx;
+}
+
+/****************************************************************************
+ * Public Functions
+ ****************************************************************************/
+
+/****************************************************************************
+ * Name: cfsetspeed
+ *
+ * Description:
+ *   The cfsetspeed() function is a non-POSIX function that sets the baud
+ *   stored in the structure pointed to by termiosp to speed.
+ *
+ *   There is no effect on the baud set in the hardware until a subsequent
+ *   successful call to tcsetattr() on the same termios structure.
+ *
+ *   NOTE 1: NuttX does not control input/output baud independently.  Both
+ *   must be the same.  The POSIX standard interfaces, cfisetispeed() and
+ *   cfisetospeed() are defined to be cfsetspeed() in termios.h.
+ *
+ *   NOTE 3: A consequence of NOTE 1 is that you should never attempt to
+ *   set the input and output baud to different values.
+ *
+ *   Also, the following POSIX requirement cannot be supported: "If the input
+ *   baud rate stored in the termios structure pointed to by termios_p is 0,
+ *   the input baud rate given to the hardware will be the same as the output
+ *   baud rate stored in the termios structure."
+ *
+ *   NOTE 2. In Nuttx, the speed_t is defined to be unsigned int and the baud
+ *   encodings of termios.h are baud value mask. And their corresponding
+ *   values are in array g_baud_table. However, if you do so, your code will
+ *   *NOT* be portable to other environments where speed_t is smaller and
+ *   where the termios.h baud values are encoded! To avoid portability
+ *   issues, use the baud definitions in termios.h!
+ *
+ *   Linux, for example, would require this (also non-portable) sequence:
+ *
+ *     cfsetispeed(termiosp, BOTHER);
+ *     termiosp->c_ispeed = baud;
+ *
+ *     cfsetospeed(termiosp, BOTHER);
+ *     termiosp->c_ospeed = baud;
+ *
+ * Input Parameters:
+ *   termiosp - The termiosp argument is a pointer to a termios structure.
+ *   speed - The new input speed. It could be baud rate or could be mask.
+ *
+ * Returned Value:
+ *   Baud is returned. If speed don't match g_baud_table and mask in
+ *   termios.h, -1 is returned and set errno EINVAL.
+ *
+ ****************************************************************************/
+
+int cfsetspeed(FAR struct termios *termiosp, speed_t speed)
+{
+  int mask = baud_mask(speed);
+
+  DEBUGASSERT(termiosp);
+  if (mask == -1)
+    {
+      set_errno(EINVAL);
+      return mask;
+    }
+
+  termiosp->c_cflag &= ~CBAUD;
+  termiosp->c_cflag |= mask;
+  return 0;
+}
+
+/****************************************************************************
+ * Name: cfgetspeed
+ *
+ * Description:
+ *   The cfgetspeed() function is a non-POSIX function will extract the baud
+ *   from the termios structure to which the termiosp argument points.
+ *
+ *   This function will return exactly the value in the termios data
+ *   structure, without interpretation.
+ *
+ *   NOTE 1: NuttX does not control input/output baud independently.  Both
+ *   must be the same.  The POSIX standard interfaces, cfisetispeed() and
+ *   cfisetospeed() are defined to be cfgetspeed() in termios.h.
+ *   NOTE 2.  In Nuttx, the speed_t is defined to be uint32_t and the baud
+ *   encodings of termios.h are the actual baud values themselves. Therefore,
+ *   any baud value may be returned here... not just those enumerated in
+ *   termios.h
+ *
+ * Input Parameters:
+ *   termiosp - The termiosp argument is a pointer to a termios structure.
+ *
+ * Returned Value:
+ *   Encoded baud value from the termios structure.
+ *
+ ****************************************************************************/
+
+speed_t cfgetspeed(FAR const struct termios *termiosp)
+{
+  int idx;
+
+  DEBUGASSERT(termiosp);
+  idx = termiosp->c_cflag & CBAUD & ~CBAUDEX;
+  idx += (termiosp->c_cflag & CBAUDEX) ? 15 : 0;
+  return g_baud_table[idx];
+}