From e646c50019855f8a73b50114cfbff7162486bd55 Mon Sep 17 00:00:00 2001
From: Martin Lund <martin.lund@keep-it-simple.com>
Date: Mon, 13 Nov 2017 12:58:50 +0100
Subject: [PATCH] Add support for setting non-standard baudrates

Support for non-standard baudrate settings will be automatically enabled
if the termios2 interface is detected available. However, to play it
safe, the old and widely supported termios interface will still be used
when setting standard baudrates.
---
 configure.ac    |  4 ++++
 src/Makefile.am |  4 ++++
 src/setspeed2.c | 20 +++++++++++++++++++
 src/tty.c       | 53 +++++++++++++++++++++++++++++++++++++------------
 4 files changed, 68 insertions(+), 13 deletions(-)
 create mode 100644 src/setspeed2.c

diff --git a/configure.ac b/configure.ac
index 8a663ed..038d2f2 100644
--- a/configure.ac
+++ b/configure.ac
@@ -84,6 +84,10 @@ TIO_CHECK_BAUDRATES(
 
 AC_DEFINE_UNQUOTED([AUTOCONF_BAUDRATE_CASES],[$BAUDRATE_CASES],[Switch cases for detected baud rates])
 
+# Check for interface for setting arbitrary I/O speeds
+AC_CHECK_DECL([TCGETS2], [AC_DEFINE([HAVE_TERMIOS2],[1],[Define if termios2 exists.]) have_termios2=yes], , [[#include <asm/termios.h>]])
+AM_CONDITIONAL([ADD_SETSPEED2],[test "x$have_termios2" == "xyes"])
+
 AC_CONFIG_FILES([Makefile])
 AC_CONFIG_FILES([src/Makefile])
 AC_CONFIG_FILES([src/bash-completion/tio])
diff --git a/src/Makefile.am b/src/Makefile.am
index b06b6d1..483998f 100644
--- a/src/Makefile.am
+++ b/src/Makefile.am
@@ -13,6 +13,10 @@ tio_SOURCES = tty.c \
               include/tio/log.h \
               include/tio/error.h
 
+if ADD_SETSPEED2
+tio_SOURCES += setspeed2.c
+endif
+
 if ENABLE_BASH_COMPLETION
 bashcompletiondir=@BASH_COMPLETION_DIR@
 bashcompletion_DATA=bash-completion/tio
diff --git a/src/setspeed2.c b/src/setspeed2.c
new file mode 100644
index 0000000..22d0c14
--- /dev/null
+++ b/src/setspeed2.c
@@ -0,0 +1,20 @@
+#include <sys/ioctl.h>
+#include <asm/termbits.h>
+
+int setspeed2(int fd, int baudrate)
+{
+    struct termios2 tio;
+    int status;
+
+    status = ioctl(fd, TCGETS2, &tio);
+
+    // Set baudrate speed using termios2 interface
+    tio.c_cflag &= ~CBAUD;
+    tio.c_cflag |= BOTHER;
+    tio.c_ispeed = baudrate;
+    tio.c_ospeed = baudrate;
+
+    status = ioctl(fd, TCSETS2, &tio);
+
+    return status;
+}
diff --git a/src/tty.c b/src/tty.c
index 60d6c15..de4bcf8 100644
--- a/src/tty.c
+++ b/src/tty.c
@@ -42,13 +42,18 @@
 #include "tio/log.h"
 #include "tio/error.h"
 
+#ifdef HAVE_TERMIOS2
+extern int setspeed2(int fd, int baudrate);
+#endif
+
 static struct termios tio, tio_old, stdout_new, stdout_old, stdin_new, stdin_old;
 static unsigned long rx_total = 0, tx_total = 0;
 static bool connected = false;
 static bool tainted = false;
-static int fd;
 static bool print_mode = NORMAL;
+static bool standard_baudrate = true;
 static void (*print)(char c);
+static int fd;
 
 #define tio_printf(format, args...) \
 { \
@@ -276,24 +281,32 @@ void tty_configure(void)
         AUTOCONF_BAUDRATE_CASES
 
         default:
+#ifdef HAVE_TERMIOS2
+            standard_baudrate = false;
+            break;
+#else
             error_printf("Invalid baud rate");
             exit(EXIT_FAILURE);
+#endif
     }
 
-    // Set input speed
-    status = cfsetispeed(&tio, baudrate);
-    if (status == -1)
+    if (standard_baudrate)
     {
-        error_printf("Could not configure input speed (%s)", strerror(errno));
-        exit(EXIT_FAILURE);
-    }
+        // Set input speed
+        status = cfsetispeed(&tio, baudrate);
+        if (status == -1)
+        {
+            error_printf("Could not configure input speed (%s)", strerror(errno));
+            exit(EXIT_FAILURE);
+        }
 
-    // Set output speed
-    cfsetospeed(&tio, baudrate);
-    if (status == -1)
-    {
-        error_printf("Could not configure output speed (%s)", strerror(errno));
-        exit(EXIT_FAILURE);
+        // Set output speed
+        cfsetospeed(&tio, baudrate);
+        if (status == -1)
+        {
+            error_printf("Could not configure output speed (%s)", strerror(errno));
+            exit(EXIT_FAILURE);
+        }
     }
 
     /* Set databits */
@@ -551,6 +564,17 @@ int tty_connect(void)
         goto error_tcsetattr;
     }
 
+#ifdef HAVE_TERMIOS2
+    if (!standard_baudrate)
+    {
+        if (setspeed2(fd, option.baudrate) != 0)
+        {
+            error_printf_silent("Could not set baudrate speed (%s)", strerror(errno));
+            goto error_setspeed2;
+        }
+    }
+#endif
+
     maxfd = MAX(fd, STDIN_FILENO) + 1;  /* Maximum bit entry (fd) to test */
 
     /* Input loop */
@@ -636,6 +660,9 @@ int tty_connect(void)
 
     return TIO_SUCCESS;
 
+#ifdef HAVE_TERMIOS2
+error_setspeed2:
+#endif
 error_tcsetattr:
 error_tcgetattr:
 error_read: