Merge branch 'ukl/rfc2217-fixes' of https://github.com/pengutronix/microcom

5 files changed, 285 insertions, 481 deletions

diff --git a/.travis.yml b/.travis.yml
index 0723a3a..87116f4 100644
--- a/.travis.yml
+++ b/.travis.yml
@@ -1,6 +1,10 @@
 language: c
 compiler: gcc
 dist: trusty
+addons:
+  apt:
+    packages:
+      - autoconf-archive
 script:
   - autoreconf -fi
   - ./configure
diff --git a/m4/ax_pthread.m4 b/m4/ax_pthread.m4
deleted file mode 100644
index d383ad5..0000000
--- a/m4/ax_pthread.m4
+++ /dev/null
@@ -1,332 +0,0 @@
-# ===========================================================================
-#        http://www.gnu.org/software/autoconf-archive/ax_pthread.html
-# ===========================================================================
-#
-# SYNOPSIS
-#
-#   AX_PTHREAD([ACTION-IF-FOUND[, ACTION-IF-NOT-FOUND]])
-#
-# DESCRIPTION
-#
-#   This macro figures out how to build C programs using POSIX threads. It
-#   sets the PTHREAD_LIBS output variable to the threads library and linker
-#   flags, and the PTHREAD_CFLAGS output variable to any special C compiler
-#   flags that are needed. (The user can also force certain compiler
-#   flags/libs to be tested by setting these environment variables.)
-#
-#   Also sets PTHREAD_CC to any special C compiler that is needed for
-#   multi-threaded programs (defaults to the value of CC otherwise). (This
-#   is necessary on AIX to use the special cc_r compiler alias.)
-#
-#   NOTE: You are assumed to not only compile your program with these flags,
-#   but also link it with them as well. e.g. you should link with
-#   $PTHREAD_CC $CFLAGS $PTHREAD_CFLAGS $LDFLAGS ... $PTHREAD_LIBS $LIBS
-#
-#   If you are only building threads programs, you may wish to use these
-#   variables in your default LIBS, CFLAGS, and CC:
-#
-#     LIBS="$PTHREAD_LIBS $LIBS"
-#     CFLAGS="$CFLAGS $PTHREAD_CFLAGS"
-#     CC="$PTHREAD_CC"
-#
-#   In addition, if the PTHREAD_CREATE_JOINABLE thread-attribute constant
-#   has a nonstandard name, defines PTHREAD_CREATE_JOINABLE to that name
-#   (e.g. PTHREAD_CREATE_UNDETACHED on AIX).
-#
-#   Also HAVE_PTHREAD_PRIO_INHERIT is defined if pthread is found and the
-#   PTHREAD_PRIO_INHERIT symbol is defined when compiling with
-#   PTHREAD_CFLAGS.
-#
-#   ACTION-IF-FOUND is a list of shell commands to run if a threads library
-#   is found, and ACTION-IF-NOT-FOUND is a list of commands to run it if it
-#   is not found. If ACTION-IF-FOUND is not specified, the default action
-#   will define HAVE_PTHREAD.
-#
-#   Please let the authors know if this macro fails on any platform, or if
-#   you have any other suggestions or comments. This macro was based on work
-#   by SGJ on autoconf scripts for FFTW (http://www.fftw.org/) (with help
-#   from M. Frigo), as well as ac_pthread and hb_pthread macros posted by
-#   Alejandro Forero Cuervo to the autoconf macro repository. We are also
-#   grateful for the helpful feedback of numerous users.
-#
-#   Updated for Autoconf 2.68 by Daniel Richard G.
-#
-# LICENSE
-#
-#   Copyright (c) 2008 Steven G. Johnson <stevenj@alum.mit.edu>
-#   Copyright (c) 2011 Daniel Richard G. <skunk@iSKUNK.ORG>
-#
-#   This program is free software: you can redistribute it and/or modify it
-#   under the terms of the GNU General Public License as published by the
-#   Free Software Foundation, either version 3 of the License, or (at your
-#   option) any later version.
-#
-#   This program is distributed in the hope that it will be useful, but
-#   WITHOUT ANY WARRANTY; without even the implied warranty of
-#   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General
-#   Public License for more details.
-#
-#   You should have received a copy of the GNU General Public License along
-#   with this program. If not, see <http://www.gnu.org/licenses/>.
-#
-#   As a special exception, the respective Autoconf Macro's copyright owner
-#   gives unlimited permission to copy, distribute and modify the configure
-#   scripts that are the output of Autoconf when processing the Macro. You
-#   need not follow the terms of the GNU General Public License when using
-#   or distributing such scripts, even though portions of the text of the
-#   Macro appear in them. The GNU General Public License (GPL) does govern
-#   all other use of the material that constitutes the Autoconf Macro.
-#
-#   This special exception to the GPL applies to versions of the Autoconf
-#   Macro released by the Autoconf Archive. When you make and distribute a
-#   modified version of the Autoconf Macro, you may extend this special
-#   exception to the GPL to apply to your modified version as well.
-
-#serial 21
-
-AU_ALIAS([ACX_PTHREAD], [AX_PTHREAD])
-AC_DEFUN([AX_PTHREAD], [
-AC_REQUIRE([AC_CANONICAL_HOST])
-AC_LANG_PUSH([C])
-ax_pthread_ok=no
-
-# We used to check for pthread.h first, but this fails if pthread.h
-# requires special compiler flags (e.g. on True64 or Sequent).
-# It gets checked for in the link test anyway.
-
-# First of all, check if the user has set any of the PTHREAD_LIBS,
-# etcetera environment variables, and if threads linking works using
-# them:
-if test x"$PTHREAD_LIBS$PTHREAD_CFLAGS" != x; then
-        save_CFLAGS="$CFLAGS"
-        CFLAGS="$CFLAGS $PTHREAD_CFLAGS"
-        save_LIBS="$LIBS"
-        LIBS="$PTHREAD_LIBS $LIBS"
-        AC_MSG_CHECKING([for pthread_join in LIBS=$PTHREAD_LIBS with CFLAGS=$PTHREAD_CFLAGS])
-        AC_TRY_LINK_FUNC([pthread_join], [ax_pthread_ok=yes])
-        AC_MSG_RESULT([$ax_pthread_ok])
-        if test x"$ax_pthread_ok" = xno; then
-                PTHREAD_LIBS=""
-                PTHREAD_CFLAGS=""
-        fi
-        LIBS="$save_LIBS"
-        CFLAGS="$save_CFLAGS"
-fi
-
-# We must check for the threads library under a number of different
-# names; the ordering is very important because some systems
-# (e.g. DEC) have both -lpthread and -lpthreads, where one of the
-# libraries is broken (non-POSIX).
-
-# Create a list of thread flags to try.  Items starting with a "-" are
-# C compiler flags, and other items are library names, except for "none"
-# which indicates that we try without any flags at all, and "pthread-config"
-# which is a program returning the flags for the Pth emulation library.
-
-ax_pthread_flags="pthreads none -Kthread -kthread lthread -pthread -pthreads -mthreads pthread --thread-safe -mt pthread-config"
-
-# The ordering *is* (sometimes) important.  Some notes on the
-# individual items follow:
-
-# pthreads: AIX (must check this before -lpthread)
-# none: in case threads are in libc; should be tried before -Kthread and
-#       other compiler flags to prevent continual compiler warnings
-# -Kthread: Sequent (threads in libc, but -Kthread needed for pthread.h)
-# -kthread: FreeBSD kernel threads (preferred to -pthread since SMP-able)
-# lthread: LinuxThreads port on FreeBSD (also preferred to -pthread)
-# -pthread: Linux/gcc (kernel threads), BSD/gcc (userland threads)
-# -pthreads: Solaris/gcc
-# -mthreads: Mingw32/gcc, Lynx/gcc
-# -mt: Sun Workshop C (may only link SunOS threads [-lthread], but it
-#      doesn't hurt to check since this sometimes defines pthreads too;
-#      also defines -D_REENTRANT)
-#      ... -mt is also the pthreads flag for HP/aCC
-# pthread: Linux, etcetera
-# --thread-safe: KAI C++
-# pthread-config: use pthread-config program (for GNU Pth library)
-
-case ${host_os} in
-        solaris*)
-
-        # On Solaris (at least, for some versions), libc contains stubbed
-        # (non-functional) versions of the pthreads routines, so link-based
-        # tests will erroneously succeed.  (We need to link with -pthreads/-mt/
-        # -lpthread.)  (The stubs are missing pthread_cleanup_push, or rather
-        # a function called by this macro, so we could check for that, but
-        # who knows whether they'll stub that too in a future libc.)  So,
-        # we'll just look for -pthreads and -lpthread first:
-
-        ax_pthread_flags="-pthreads pthread -mt -pthread $ax_pthread_flags"
-        ;;
-
-        darwin*)
-        ax_pthread_flags="-pthread $ax_pthread_flags"
-        ;;
-esac
-
-# Clang doesn't consider unrecognized options an error unless we specify
-# -Werror. We throw in some extra Clang-specific options to ensure that
-# this doesn't happen for GCC, which also accepts -Werror.
-
-AC_MSG_CHECKING([if compiler needs -Werror to reject unknown flags])
-save_CFLAGS="$CFLAGS"
-ax_pthread_extra_flags="-Werror"
-CFLAGS="$CFLAGS $ax_pthread_extra_flags -Wunknown-warning-option -Wsizeof-array-argument"
-AC_COMPILE_IFELSE([AC_LANG_PROGRAM([int foo(void);],[foo()])],
-                  [AC_MSG_RESULT([yes])],
-                  [ax_pthread_extra_flags=
-                   AC_MSG_RESULT([no])])
-CFLAGS="$save_CFLAGS"
-
-if test x"$ax_pthread_ok" = xno; then
-for flag in $ax_pthread_flags; do
-
-        case $flag in
-                none)
-                AC_MSG_CHECKING([whether pthreads work without any flags])
-                ;;
-
-                -*)
-                AC_MSG_CHECKING([whether pthreads work with $flag])
-                PTHREAD_CFLAGS="$flag"
-                ;;
-
-                pthread-config)
-                AC_CHECK_PROG([ax_pthread_config], [pthread-config], [yes], [no])
-                if test x"$ax_pthread_config" = xno; then continue; fi
-                PTHREAD_CFLAGS="`pthread-config --cflags`"
-                PTHREAD_LIBS="`pthread-config --ldflags` `pthread-config --libs`"
-                ;;
-
-                *)
-                AC_MSG_CHECKING([for the pthreads library -l$flag])
-                PTHREAD_LIBS="-l$flag"
-                ;;
-        esac
-
-        save_LIBS="$LIBS"
-        save_CFLAGS="$CFLAGS"
-        LIBS="$PTHREAD_LIBS $LIBS"
-        CFLAGS="$CFLAGS $PTHREAD_CFLAGS $ax_pthread_extra_flags"
-
-        # Check for various functions.  We must include pthread.h,
-        # since some functions may be macros.  (On the Sequent, we
-        # need a special flag -Kthread to make this header compile.)
-        # We check for pthread_join because it is in -lpthread on IRIX
-        # while pthread_create is in libc.  We check for pthread_attr_init
-        # due to DEC craziness with -lpthreads.  We check for
-        # pthread_cleanup_push because it is one of the few pthread
-        # functions on Solaris that doesn't have a non-functional libc stub.
-        # We try pthread_create on general principles.
-        AC_LINK_IFELSE([AC_LANG_PROGRAM([#include <pthread.h>
-                        static void routine(void *a) { a = 0; }
-                        static void *start_routine(void *a) { return a; }],
-                       [pthread_t th; pthread_attr_t attr;
-                        pthread_create(&th, 0, start_routine, 0);
-                        pthread_join(th, 0);
-                        pthread_attr_init(&attr);
-                        pthread_cleanup_push(routine, 0);
-                        pthread_cleanup_pop(0) /* ; */])],
-                [ax_pthread_ok=yes],
-                [])
-
-        LIBS="$save_LIBS"
-        CFLAGS="$save_CFLAGS"
-
-        AC_MSG_RESULT([$ax_pthread_ok])
-        if test "x$ax_pthread_ok" = xyes; then
-                break;
-        fi
-
-        PTHREAD_LIBS=""
-        PTHREAD_CFLAGS=""
-done
-fi
-
-# Various other checks:
-if test "x$ax_pthread_ok" = xyes; then
-        save_LIBS="$LIBS"
-        LIBS="$PTHREAD_LIBS $LIBS"
-        save_CFLAGS="$CFLAGS"
-        CFLAGS="$CFLAGS $PTHREAD_CFLAGS"
-
-        # Detect AIX lossage: JOINABLE attribute is called UNDETACHED.
-        AC_MSG_CHECKING([for joinable pthread attribute])
-        attr_name=unknown
-        for attr in PTHREAD_CREATE_JOINABLE PTHREAD_CREATE_UNDETACHED; do
-            AC_LINK_IFELSE([AC_LANG_PROGRAM([#include <pthread.h>],
-                           [int attr = $attr; return attr /* ; */])],
-                [attr_name=$attr; break],
-                [])
-        done
-        AC_MSG_RESULT([$attr_name])
-        if test "$attr_name" != PTHREAD_CREATE_JOINABLE; then
-            AC_DEFINE_UNQUOTED([PTHREAD_CREATE_JOINABLE], [$attr_name],
-                               [Define to necessary symbol if this constant
-                                uses a non-standard name on your system.])
-        fi
-
-        AC_MSG_CHECKING([if more special flags are required for pthreads])
-        flag=no
-        case ${host_os} in
-            aix* | freebsd* | darwin*) flag="-D_THREAD_SAFE";;
-            osf* | hpux*) flag="-D_REENTRANT";;
-            solaris*)
-            if test "$GCC" = "yes"; then
-                flag="-D_REENTRANT"
-            else
-                # TODO: What about Clang on Solaris?
-                flag="-mt -D_REENTRANT"
-            fi
-            ;;
-        esac
-        AC_MSG_RESULT([$flag])
-        if test "x$flag" != xno; then
-            PTHREAD_CFLAGS="$flag $PTHREAD_CFLAGS"
-        fi
-
-        AC_CACHE_CHECK([for PTHREAD_PRIO_INHERIT],
-            [ax_cv_PTHREAD_PRIO_INHERIT], [
-                AC_LINK_IFELSE([AC_LANG_PROGRAM([[#include <pthread.h>]],
-                                                [[int i = PTHREAD_PRIO_INHERIT;]])],
-                    [ax_cv_PTHREAD_PRIO_INHERIT=yes],
-                    [ax_cv_PTHREAD_PRIO_INHERIT=no])
-            ])
-        AS_IF([test "x$ax_cv_PTHREAD_PRIO_INHERIT" = "xyes"],
-            [AC_DEFINE([HAVE_PTHREAD_PRIO_INHERIT], [1], [Have PTHREAD_PRIO_INHERIT.])])
-
-        LIBS="$save_LIBS"
-        CFLAGS="$save_CFLAGS"
-
-        # More AIX lossage: compile with *_r variant
-        if test "x$GCC" != xyes; then
-            case $host_os in
-                aix*)
-                AS_CASE(["x/$CC"],
-                  [x*/c89|x*/c89_128|x*/c99|x*/c99_128|x*/cc|x*/cc128|x*/xlc|x*/xlc_v6|x*/xlc128|x*/xlc128_v6],
-                  [#handle absolute path differently from PATH based program lookup
-                   AS_CASE(["x$CC"],
-                     [x/*],
-                     [AS_IF([AS_EXECUTABLE_P([${CC}_r])],[PTHREAD_CC="${CC}_r"])],
-                     [AC_CHECK_PROGS([PTHREAD_CC],[${CC}_r],[$CC])])])
-                ;;
-            esac
-        fi
-fi
-
-test -n "$PTHREAD_CC" || PTHREAD_CC="$CC"
-
-AC_SUBST([PTHREAD_LIBS])
-AC_SUBST([PTHREAD_CFLAGS])
-AC_SUBST([PTHREAD_CC])
-
-# Finally, execute ACTION-IF-FOUND/ACTION-IF-NOT-FOUND:
-if test x"$ax_pthread_ok" = xyes; then
-        ifelse([$1],,[AC_DEFINE([HAVE_PTHREAD],[1],[Define if you have POSIX threads libraries and header files.])],[$1])
-        :
-else
-        ax_pthread_ok=no
-        $2
-fi
-AC_LANG_POP
-])dnl AX_PTHREAD
diff --git a/microcom.h b/microcom.h
index c0d9f2c..e8ed544 100644
--- a/microcom.h
+++ b/microcom.h
@@ -118,10 +118,19 @@ extern int current_flow;
 int do_commandline(void);
 int do_script(char *script);
 
-#define dprintf(fmt,args...)  ({ if (debug) printf (fmt ,##args); })
+#define dbg_printf(fmt,args...)  ({ if (debug) printf(fmt ,##args); })
+
+/*
+ * Some telnet options according to
+ * https://www.iana.org/assignments/telnet-options/telnet-options.xhtmls
+ */
+
+#define TELNET_OPTION_BINARY_TRANSMISSION		0
+#define TELNET_OPTION_ECHO				1
+#define TELNET_OPTION_SUPPRESS_GO_AHEAD			3
+#define TELNET_OPTION_COM_PORT_CONTROL			44
 
 /* RFC2217 */
-#define COM_PORT_OPTION		 44
 #define SET_BAUDRATE_CS		  1
 #define SET_DATASIZE_CS		  2
 #define SET_PARITY_CS		  3
diff --git a/mux.c b/mux.c
index c393a43..1cfb914 100644
--- a/mux.c
+++ b/mux.c
@@ -20,165 +20,190 @@
 #include "microcom.h"
 #include <arpa/telnet.h>
 #include <arpa/inet.h>
+#include <stdbool.h>
 
 #define BUFSIZE 1024
 
-static int do_com_port_option(unsigned char *buf, int len)
+/* This is called with buf[-2:0] being IAC SB COM_PORT_OPTION */
+static int do_com_port_option(struct ios_ops *ios, unsigned char *buf, int len)
 {
-	int i = 0;
+	int i = 2;
+
+	switch (buf[1]) {
+	case SET_BAUDRATE_CS:
+		dbg_printf("SET_BAUDRATE_CS ");
+		break;
+	case SET_DATASIZE_CS:
+		dbg_printf("SET_DATASIZE_CS ");
+		break;
+	case SET_PARITY_CS:
+		dbg_printf("SET_PARITY_CS ");
+		break;
+	case SET_STOPSIZE_CS:
+		dbg_printf("SET_STOPSIZE_CS ");
+		break;
+	case SET_CONTROL_CS:
+		dbg_printf("SET_CONTROL_CS ");
+		break;
+	case NOTIFY_LINESTATE_CS:
+		dbg_printf("NOTIFY_LINESTATE_CS ");
+		break;
+	case NOTIFY_MODEMSTATE_CS:
+		dbg_printf("NOTIFY_MODEMSTATE_CS ");
+		break;
+	case FLOWCONTROL_SUSPEND_CS:
+		dbg_printf("FLOWCONTROL_SUSPEND_CS ");
+		break;
+	case FLOWCONTROL_RESUME_CS:
+		dbg_printf("FLOWCONTROL_RESUME_CS ");
+		break;
+	case SET_LINESTATE_MASK_CS:
+		dbg_printf("SET_LINESTATE_MASK_CS ");
+		break;
+	case SET_MODEMSTATE_MASK_CS:
+		dbg_printf("SET_MODEMSTATE_MASK_CS ");
+		break;
+	case PURGE_DATA_CS:
+		dbg_printf("PURGE_DATA_CS ");
+		break;
+	case SET_BAUDRATE_SC:
+		dbg_printf("SET_BAUDRATE_SC %d ",
+			buf[2] << 24 | buf[3] << 16 | buf[4] << 8 | buf[5]);
+		i += 4;
+		break;
+	case SET_DATASIZE_SC:
+		dbg_printf("SET_DATASIZE_SC ");
+		break;
+	case SET_PARITY_SC:
+		dbg_printf("SET_PARITY_SC ");
+		break;
+	case SET_STOPSIZE_SC:
+		dbg_printf("SET_STOPSIZE_SC ");
+		break;
+	case SET_CONTROL_SC:
+		i++;
+		dbg_printf("SET_CONTROL_SC 0x%02x ", buf[i]);
+		break;
+	case NOTIFY_LINESTATE_SC:
+		dbg_printf("NOTIFY_LINESTATE_SC ");
+		break;
+	case NOTIFY_MODEMSTATE_SC:
+		i++;
+		dbg_printf("NOTIFY_MODEMSTATE_SC 0x%02x ", buf[i]);
+		break;
+	case FLOWCONTROL_SUSPEND_SC:
+		dbg_printf("FLOWCONTROL_SUSPEND_SC ");
+		break;
+	case FLOWCONTROL_RESUME_SC:
+		dbg_printf("FLOWCONTROL_RESUME_SC ");
+		break;
+	case SET_LINESTATE_MASK_SC:
+		dbg_printf("SET_LINESTATE_MASK_SC ");
+		break;
+	case SET_MODEMSTATE_MASK_SC:
+		dbg_printf("SET_MODEMSTATE_MASK_SC ");
+		break;
+	case PURGE_DATA_SC:
+		dbg_printf("PURGE_DATA_SC ");
+		break;
+	default:
+		dbg_printf("??? %d ", buf[i]);
+		break;
+	}
 
 	while (i < len) {
-		switch (buf[i]) {
-		case IAC:
-			dprintf("IAC ");
-			return i + 1;
-		case SET_BAUDRATE_CS:
-			dprintf("SET_BAUDRATE_CS ");
-			break;
-		case SET_DATASIZE_CS:
-			dprintf("SET_DATASIZE_CS ");
-			break;
-		case SET_PARITY_CS:
-			dprintf("SET_PARITY_CS ");
-			break;
-		case SET_STOPSIZE_CS:
-			dprintf("SET_STOPSIZE_CS ");
-			break;
-		case SET_CONTROL_CS:
-			dprintf("SET_CONTROL_CS ");
-			break;
-		case NOTIFY_LINESTATE_CS:
-			dprintf("NOTIFY_LINESTATE_CS ");
-			break;
-		case NOTIFY_MODEMSTATE_CS:
-			dprintf("NOTIFY_MODEMSTATE_CS ");
-			break;
-		case FLOWCONTROL_SUSPEND_CS:
-			dprintf("FLOWCONTROL_SUSPEND_CS ");
-			break;
-		case FLOWCONTROL_RESUME_CS:
-			dprintf("FLOWCONTROL_RESUME_CS ");
-			break;
-		case SET_LINESTATE_MASK_CS:
-			dprintf("SET_LINESTATE_MASK_CS ");
-			break;
-		case SET_MODEMSTATE_MASK_CS:
-			dprintf("SET_MODEMSTATE_MASK_CS ");
-			break;
-		case PURGE_DATA_CS:
-			dprintf("PURGE_DATA_CS ");
-			break;
-		case SET_BAUDRATE_SC:
-			dprintf("SET_BAUDRATE_SC %d ", ntohl(*(int *)&buf[i + 1]));
-			i += 4;
-			break;
-		case SET_DATASIZE_SC:
-			dprintf("SET_DATASIZE_SC ");
-			break;
-		case SET_PARITY_SC:
-			dprintf("SET_PARITY_SC ");
-			break;
-		case SET_STOPSIZE_SC:
-			dprintf("SET_STOPSIZE_SC ");
-			break;
-		case SET_CONTROL_SC:
-			i++;
-			dprintf("SET_CONTROL_SC 0x%02x ", buf[i]);
-			break;
-		case NOTIFY_LINESTATE_SC:
-			dprintf("NOTIFY_LINESTATE_SC ");
-			break;
-		case NOTIFY_MODEMSTATE_SC:
-			i++;
-			dprintf("NOTIFY_MODEMSTATE_SC 0x%02x ", buf[i]);
-			break;
-		case FLOWCONTROL_SUSPEND_SC:
-			dprintf("FLOWCONTROL_SUSPEND_SC ");
-			break;
-		case FLOWCONTROL_RESUME_SC:
-			dprintf("FLOWCONTROL_RESUME_SC ");
-			break;
-		case SET_LINESTATE_MASK_SC:
-			dprintf("SET_LINESTATE_MASK_SC ");
-			break;
-		case SET_MODEMSTATE_MASK_SC:
-			dprintf("SET_MODEMSTATE_MASK_SC ");
-			break;
-		case PURGE_DATA_SC:
-			dprintf("PURGE_DATA_SC ");
-			break;
-		default:
-			dprintf("%d ", buf[i]);
-			break;
+		if (buf[i] == IAC) {
+			if (i + 1 < len && buf[i+1] == IAC) {
+				/* quoted IAC -> unquote */
+				++i;
+			} else if (i + 1 < len && buf[i+1] == SE) {
+				dbg_printf("IAC SE\n");
+				return i + 2;
+			}
 		}
-		i++;
+		dbg_printf("%d ", buf[i]);
+
+		++i;
 	}
 
-	return len;
+	fprintf(stderr, "Incomplete SB string\n");
+	return -EINVAL;
 }
 
-static int do_subneg(unsigned char *buf, int len)
+struct telnet_option {
+	unsigned char id;
+	const char *name;
+	int (*subneg_handler)(struct ios_ops *ios, unsigned char *buf, int len);
+	bool sent_will;
+};
+
+#define TELNET_OPTION(x)	.id = TELNET_OPTION_ ## x, .name = #x
+
+static const struct telnet_option telnet_options[] = {
+	{
+		TELNET_OPTION(COM_PORT_CONTROL),
+		.subneg_handler = do_com_port_option,
+		.sent_will = true,
+	}, {
+		TELNET_OPTION(BINARY_TRANSMISSION),
+	}, {
+		TELNET_OPTION(ECHO),
+	}, {
+		TELNET_OPTION(SUPPRESS_GO_AHEAD),
+	}
+};
+
+static const struct telnet_option *get_telnet_option(unsigned char id)
 {
-	int i = 0;
+	int i;
 
-	while (i < len) {
-		switch (buf[i]) {
-		case COM_PORT_OPTION:
-			dprintf("COM_PORT_OPTION ");
-			return do_com_port_option(&buf[i + 1], len - i) + 1;
-		case IAC:
-			dprintf("IAC ");
-			return len - i;
-		default:
-			dprintf("%d ", buf[i]);
-			break;
-		}
-		i++;
+	for (i = 0; i < ARRAY_SIZE(telnet_options); ++i) {
+		if (id == telnet_options[i].id)
+			return &telnet_options[i];
 	}
 
-	return len;
+	return NULL;
 }
 
-static int handle_command(unsigned char *buf, int len)
+
+/* This function is called with buf[-2:-1] being IAC SB */
+static int do_subneg(struct ios_ops *ios, unsigned char *buf, int len)
 {
-	int i = 0;
+	const struct telnet_option *option = get_telnet_option(buf[0]);
+
+	if (option)
+		dbg_printf("%s ", option->name);
+	if (option->subneg_handler) {
+		return option->subneg_handler(ios, buf, len);
+	} else {
+		/* skip over subneg string */
+		int i;
+		for (i = 0; i < len - 1; ++i) {
+			if (buf[i] != IAC) {
+				dbg_printf("%d ", buf[i]);
+				continue;
+			}
 
-	while (i < len) {
-		switch (buf[i]) {
-		case SB:
-			dprintf("SB ");
-			i += do_subneg(&buf[i+1], len - i);
-			break;
-		case IAC:
-			dprintf("IAC ");
-			break;
-		case COM_PORT_OPTION:
-			dprintf("COM_PORT_OPTION ");
-			break;
-		case SE:
-			dprintf("SE ");
-			break;
-		case WILL:
-			dprintf("WILL ");
-			break;
-		case WONT:
-			dprintf("WONT ");
-			break;
-		case DO:
-			dprintf("DO ");
-			break;
-		case DONT:
-			dprintf("DONT ");
-			break;
-		default:
-			dprintf("%d ", buf[i]);
-			break;
+			if (buf[i + 1] == SE) {
+				dbg_printf("IAC SE\n");
+				return i + 1;
+			}
+
+			/* skip over IAC IAC */
+			if (buf[i + 1] == IAC) {
+				dbg_printf("%d \n", IAC);
+				i++;
+			}
 		}
-		i++;
-	}
 
-	dprintf("\n");
-	return len;
+		/* the subneg string isn't finished yet */
+		if (i == len - 1)
+			dbg_printf("%d", buf[i]);
+		dbg_printf("\\\n");
+		fprintf(stderr, "Incomplete SB string\n");
+
+		return -EINVAL;
+	}
 }
 
 static int logfd = -1;
@@ -194,7 +219,89 @@ static void write_receive_buf(const unsigned char *buf, int len)
 		write(logfd, buf, len);
 }
 
-static void handle_receive_buf(struct ios_ops *ios, unsigned char *buf, int len)
+/* This function is called with buf[0] being IAC. */
+static int handle_command(struct ios_ops *ios, unsigned char *buf, int len)
+{
+	int ret;
+	const struct telnet_option *option;
+
+	switch (buf[1]) {
+	case SB:
+		dbg_printf("SB ");
+		ret = do_subneg(ios, &buf[2], len - 2);
+		if (ret < 0)
+			return ret;
+		return ret + 2;
+
+	case IAC:
+		/* escaped IAC -> unescape */
+		write_receive_buf(&buf[1], 1);
+		return 2;
+
+	case WILL:
+		option = get_telnet_option(buf[2]);
+		if (option)
+			dbg_printf("WILL %s", option->name);
+		else
+			dbg_printf("WILL #%d", buf[2]);
+
+		if (option && option->subneg_handler) {
+			/* ok, we already requested that, so take this as
+			 * confirmation to actually do COM_PORT stuff.
+			 * Everything is fine. Don't reconfirm to prevent an
+			 * request/confirm storm.
+			 */
+			dbg_printf("\n");
+		} else {
+			/* unknown/unimplemented option -> DONT */
+			dbg_printf(" -> DONT\n");
+			dprintf(ios->fd, "%c%c%c", IAC, DONT, buf[2]);
+		}
+		return 3;
+
+	case WONT:
+		option = get_telnet_option(buf[2]);
+		if (option)
+			dbg_printf("WONT %s\n", option->name);
+		else
+			dbg_printf("WONT #%d\n", buf[2]);
+		return 3;
+
+	case DO:
+		option = get_telnet_option(buf[2]);
+		if (option)
+			dbg_printf("DO %s", option->name);
+		else
+			dbg_printf("DO #%d", buf[2]);
+
+		if (option && option->sent_will) {
+			/*
+			 * This is a confirmation of an WILL sent by us before.
+			 * There is nothing to do now.
+			 */
+			dbg_printf("\n");
+		} else {
+			/* Oh, cannot handle that one, so send a WONT */
+			dbg_printf(" -> WONT\n");
+			dprintf(ios->fd, "%c%c%c", IAC, WONT, buf[2]);
+		}
+		return 3;
+
+	case DONT:
+		option = get_telnet_option(buf[2]);
+		if (option)
+			dbg_printf("DONT %s\n", option->name);
+		else
+			dbg_printf("DONT #%d\n", buf[2]);
+		return 3;
+
+	default:
+		dbg_printf("??? %d\n", buf[1]);
+		return 1;
+	}
+}
+
+static int handle_receive_buf(struct ios_ops *ios, unsigned char *buf, int len)
 {
 	unsigned char *sendbuf = buf;
 	int i;
@@ -204,7 +311,10 @@ static void handle_receive_buf(struct ios_ops *ios, unsigned char *buf, int len)
 		case IAC:
 			/* BUG: this is telnet specific */
 			write_receive_buf(sendbuf, buf - sendbuf);
-			i = handle_command(buf, len);
+			i = handle_command(ios, buf, len);
+			if (i < 0)
+				return i;
+
 			buf += i;
 			len -= i;
 			sendbuf = buf;
@@ -213,6 +323,8 @@ static void handle_receive_buf(struct ios_ops *ios, unsigned char *buf, int len)
 			write_receive_buf(sendbuf, buf - sendbuf);
 			if (answerback)
 				write(ios->fd, answerback, strlen(answerback));
+			else
+				write_receive_buf(buf, 1);
 
 			buf += 1;
 			len -= 1;
@@ -226,6 +338,7 @@ static void handle_receive_buf(struct ios_ops *ios, unsigned char *buf, int len)
 	}
 
 	write_receive_buf(sendbuf, buf - sendbuf);
+	return 0;
 }
 
 /* handle escape characters, writing to output */
@@ -304,7 +417,9 @@ int mux_loop(struct ios_ops *ios)
 				return 0;
 			}
 
-			handle_receive_buf(ios, buf, len);
+			i = handle_receive_buf(ios, buf, len);
+			if (i < 0)
+				return i;
 		}
 
 		if (!listenonly && FD_ISSET(STDIN_FILENO, &ready)) {
diff --git a/telnet.c b/telnet.c
index 89b6057..fe30da7 100644
--- a/telnet.c
+++ b/telnet.c
@@ -29,11 +29,11 @@
 static int telnet_set_speed(struct ios_ops *ios, unsigned long speed)
 {
 
-//	unsigned char buf1[] = {IAC, WILL , COM_PORT_OPTION};
-	unsigned char buf2[] = {IAC, SB, COM_PORT_OPTION, SET_BAUDRATE_CS, 0, 0, 0, 0, IAC, SE};
+	unsigned char buf2[] = {IAC, SB, TELNET_OPTION_COM_PORT_CONTROL, SET_BAUDRATE_CS, 0, 0, 0, 0, IAC, SE};
 	int *speedp = (int *)&buf2[4];
 
 	*speedp = htonl(speed);
+	dbg_printf("-> IAC SB COM_PORT_CONTROL SET_BAUDRATE_CS 0x%lx IAC SE\n", speed);
 	write(ios->fd, buf2, 10);
 
 	return 0;
@@ -41,7 +41,7 @@ static int telnet_set_speed(struct ios_ops *ios, unsigned long speed)
 
 static int telnet_set_flow(struct ios_ops *ios, int flow)
 {
-	unsigned char buf2[] = {IAC, SB, COM_PORT_OPTION, SET_CONTROL_CS, 0, IAC, SE};
+	unsigned char buf2[] = {IAC, SB, TELNET_OPTION_COM_PORT_CONTROL, SET_CONTROL_CS, 0, IAC, SE};
 
 	switch (flow) {
 	case FLOW_NONE:
@@ -58,6 +58,7 @@ static int telnet_set_flow(struct ios_ops *ios, int flow)
 		break;
 	}
 
+	dbg_printf("-> IAC SB COM_PORT_CONTROL SET_CONTROL_CS %d IAC SE\n", buf2[4]);
 	write(ios->fd, buf2, sizeof(buf2));
 
 	return 0;
@@ -149,10 +150,17 @@ struct ios_ops *telnet_init(char *hostport)
 				  connected_host, sizeof(connected_host),
 				  connected_port, sizeof(connected_port),
 				  NI_NUMERICHOST | NI_NUMERICSERV);
-		if (ret)
+		if (ret) {
 			fprintf(stderr, "getnameinfo: %s\n", gai_strerror(ret));
-		else
-			printf("connected to %s (port %s)\n", connected_host, connected_port);
+			goto out;
+		}
+		printf("connected to %s (port %s)\n", connected_host, connected_port);
+
+		/* send intent to do and accept COM_PORT stuff */
+		dbg_printf("-> WILL COM_PORT_CONTROL\n");
+		dprintf(sock, "%c%c%c", IAC, WILL, TELNET_OPTION_COM_PORT_CONTROL);
+		dbg_printf("-> DO COM_PORT_CONTROL\n");
+		dprintf(sock, "%c%c%c", IAC, DO, TELNET_OPTION_COM_PORT_CONTROL);
 		goto out;
 	}