Merge branch 'fix-telnet-layer-violation' of https://github.com/ukleinek/microcom

5 files changed, 406 insertions, 398 deletions

diff --git a/can.c b/can.c
index 3948911..038eb05 100644
--- a/can.c
+++ b/can.c
@@ -135,7 +135,6 @@ struct ios_ops *can_init(char *interface_id)
 	ios->set_flow = can_set_flow;
 	ios->send_break = can_send_break;
 	ios->exit = can_exit;
-	ios->istelnet = false;
 
 	/*
 	 * the string is supposed to be formated this way:
diff --git a/microcom.h b/microcom.h
index 54aea56..d1c7035 100644
--- a/microcom.h
+++ b/microcom.h
@@ -51,7 +51,6 @@ struct ios_ops {
 	int (*set_handshake_line)(struct ios_ops *, int pin, int enable);
 	int (*send_break)(struct ios_ops *);
 	void (*exit)(struct ios_ops *);
-	bool istelnet;
 	int fd;
 };
 
diff --git a/mux.c b/mux.c
index a6284ca..44ba163 100644
--- a/mux.c
+++ b/mux.c
@@ -20,265 +20,10 @@
 #include "config.h"
 
 #include "microcom.h"
-#include <arpa/telnet.h>
-#include <arpa/inet.h>
-#include <stdarg.h>
 #include <stdbool.h>
 
 #define BUFSIZE 1024
 
-static ssize_t get(unsigned char *buf, unsigned char *out, size_t len)
-{
-	if (!len)
-		return -1;
-
-	if (buf[0] == IAC) {
-		if (len < 1)
-			return -1;
-		if (buf[1] == IAC) {
-			*out = IAC;
-			return 2;
-		}
-		return -1;
-	} else {
-		*out = buf[0];
-		return 1;
-	}
-}
-
-static size_t getl(unsigned char *buf, uint32_t *out, size_t len)
-{
-	*out = 0;
-	int i;
-	size_t offset = 0;
-
-	for (i = 0; i < 4; ++i) {
-		ssize_t getres;
-		unsigned char c;
-
-		getres = get(buf + offset, &c, len - offset);
-		if (getres < 0)
-			return getres;
-
-		*out <<= 8;
-		*out |= c;
-
-		offset += getres;
-	}
-
-	return offset;
-}
-
-/* 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 = 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:
-		{
-			uint32_t baudrate;
-			ssize_t getres = getl(buf + 2, &baudrate, len - 2);
-
-			if (getres < 0) {
-				fprintf(stderr, "Incomplete or broken SB (SET_BAUDRATE_SC)\n");
-				return getres;
-			}
-			dbg_printf("SET_BAUDRATE_SC %u ", baudrate);
-			i += getres;;
-		}
-		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:
-		{
-			unsigned char ctrl;
-			ssize_t getres = get(buf + 2, &ctrl, len - 2);
-
-			if (getres < 0) {
-				fprintf(stderr, "Incomplete or broken SB (SET_CONTROL_SC)\n");
-				return getres;
-			}
-
-			dbg_printf("SET_CONTROL_SC 0x%02x ", ctrl);
-			i += getres;
-		}
-		break;
-	case NOTIFY_LINESTATE_SC:
-		dbg_printf("NOTIFY_LINESTATE_SC ");
-		break;
-	case NOTIFY_MODEMSTATE_SC:
-		{
-			unsigned char ms;
-			ssize_t getres = get(buf + 2, &ms, len - 2);
-
-			if (getres < 0) {
-				fprintf(stderr, "Incomplete or broken SB (NOTIFY_MODEMSTATE_SC)\n");
-				return getres;
-			}
-
-			dbg_printf("NOTIFY_MODEMSTATE_SC 0x%02x ", ms);
-			i += getres;
-		}
-		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[1]);
-		break;
-	}
-
-	while (i < len) {
-		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;
-			}
-		}
-		dbg_printf("%d ", buf[i]);
-
-		++i;
-	}
-
-	fprintf(stderr, "Incomplete SB string\n");
-	return -EINVAL;
-}
-
-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;
-
-	for (i = 0; i < ARRAY_SIZE(telnet_options); ++i) {
-		if (id == telnet_options[i].id)
-			return &telnet_options[i];
-	}
-
-	return NULL;
-}
-
-
-/* This function is called with buf[-2:-1] being IAC SB */
-static int do_subneg(struct ios_ops *ios, unsigned char *buf, int len)
-{
-	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;
-			}
-
-			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++;
-			}
-		}
-
-		/* 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;
 char *answerback;
 
@@ -292,123 +37,9 @@ static void write_receive_buf(const unsigned char *buf, int len)
 		write(logfd, buf, len);
 }
 
-static int ios_printf(struct ios_ops *ios, const char *format, ...)
-{
-	char buf[20];
-	int size, written = 0;
-	ssize_t ret;
-	va_list args;
-
-	va_start(args, format);
-
-	size = vsnprintf(buf, sizeof(buf), format, args);
-
-	va_end(args);
-
-	if (size >= sizeof(buf)) {
-		/* truncated output */
-		errno = EIO;
-		return -1;
-	}
-
-	while (written < size) {
-		ret = ios->write(ios, buf + written, size - written);
-		if (ret < 0)
-			return ret;
-
-		written += ret;
-		assert(written <= size);
-	}
-
-	return written;
-}
-
-/* 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");
-			ios_printf(ios, "%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");
-			ios_printf(ios, "%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;
 
 	while (len) {
 		switch (*buf) {
@@ -423,19 +54,6 @@ static int handle_receive_buf(struct ios_ops *ios, unsigned char *buf, int len)
 			len -= 1;
 			sendbuf = buf;
 			break;
-		case IAC:
-			if (ios->istelnet) {
-				write_receive_buf(sendbuf, buf - sendbuf);
-				i = handle_command(ios, buf, len);
-				if (i < 0)
-					return i;
-
-				buf += i;
-				len -= i;
-				sendbuf = buf;
-				break;
-			}
-			/* fall through */
 		default:
 			buf += 1;
 			len -= 1;
@@ -519,19 +137,20 @@ int mux_loop(struct ios_ops *ios)
 			/* pf has characters for us */
 			len = ios->read(ios, buf, BUFSIZE);
 			if (len < 0) {
-				ret = -errno;
-				fprintf(stderr, "%s\n", strerror(-ret));
-				return ret;
-			}
-			if (len == 0) {
+				if (errno != EAGAIN && errno != EWOULDBLOCK) {
+					ret = -errno;
+					fprintf(stderr, "%s\n", strerror(-ret));
+					return ret;
+				}
+			} else if (len == 0) {
 				fprintf(stderr, "Got EOF from port\n");
 				return -EINVAL;
-			}
-
-			i = handle_receive_buf(ios, buf, len);
-			if (i < 0) {
-				fprintf(stderr, "%s\n", strerror(-i));
-				return i;
+			} else {
+				i = handle_receive_buf(ios, buf, len);
+				if (i < 0) {
+					fprintf(stderr, "%s\n", strerror(-i));
+					return i;
+				}
 			}
 		}
 
diff --git a/serial.c b/serial.c
index 9348dfd..87c4353 100644
--- a/serial.c
+++ b/serial.c
@@ -245,7 +245,6 @@ struct ios_ops * serial_init(char *device)
 	ops->set_handshake_line = serial_set_handshake_line;
 	ops->send_break = serial_send_break;
 	ops->exit = serial_exit;
-	ops->istelnet = false;
 
 	/* open the device */
 	fd = open(device, O_RDWR | O_NONBLOCK);
diff --git a/telnet.c b/telnet.c
index 4a66b95..347f8ac 100644
--- a/telnet.c
+++ b/telnet.c
@@ -24,9 +24,371 @@
 #include <netdb.h>
 #include <sys/types.h>
 #include <sys/socket.h>
+#include <stdarg.h>
+#include <string.h>
 
 #include "microcom.h"
 
+static int ios_printf(struct ios_ops *ios, const char *format, ...)
+{
+	char buf[20];
+	int size, written = 0;
+	ssize_t ret;
+	va_list args;
+
+	va_start(args, format);
+
+	size = vsnprintf(buf, sizeof(buf), format, args);
+
+	va_end(args);
+
+	if (size >= sizeof(buf)) {
+		/* truncated output */
+		errno = EIO;
+		return -1;
+	}
+
+	while (written < size) {
+		ret = ios->write(ios, buf + written, size - written);
+		if (ret < 0)
+			return ret;
+
+		written += ret;
+		assert(written <= size);
+	}
+
+	return written;
+}
+
+static ssize_t get(unsigned char *buf, unsigned char *out, size_t len)
+{
+	if (!len)
+		return -1;
+
+	if (buf[0] == IAC) {
+		if (len < 1)
+			return -1;
+		if (buf[1] == IAC) {
+			*out = IAC;
+			return 2;
+		}
+		return -1;
+	} else {
+		*out = buf[0];
+		return 1;
+	}
+}
+
+static size_t getl(unsigned char *buf, uint32_t *out, size_t len)
+{
+	*out = 0;
+	int i;
+	size_t offset = 0;
+
+	for (i = 0; i < 4; ++i) {
+		ssize_t getres;
+		unsigned char c;
+
+		getres = get(buf + offset, &c, len - offset);
+		if (getres < 0)
+			return getres;
+
+		*out <<= 8;
+		*out |= c;
+
+		offset += getres;
+	}
+
+	return offset;
+}
+
+/* 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 = 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:
+		{
+			uint32_t baudrate;
+			ssize_t getres = getl(buf + 2, &baudrate, len - 2);
+
+			if (getres < 0) {
+				fprintf(stderr, "Incomplete or broken SB (SET_BAUDRATE_SC)\n");
+				return getres;
+			}
+			dbg_printf("SET_BAUDRATE_SC %u ", baudrate);
+			i += getres;;
+		}
+		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:
+		{
+			unsigned char ctrl;
+			ssize_t getres = get(buf + 2, &ctrl, len - 2);
+
+			if (getres < 0) {
+				fprintf(stderr, "Incomplete or broken SB (SET_CONTROL_SC)\n");
+				return getres;
+			}
+
+			dbg_printf("SET_CONTROL_SC 0x%02x ", ctrl);
+			i += getres;
+		}
+		break;
+	case NOTIFY_LINESTATE_SC:
+		dbg_printf("NOTIFY_LINESTATE_SC ");
+		break;
+	case NOTIFY_MODEMSTATE_SC:
+		{
+			unsigned char ms;
+			ssize_t getres = get(buf + 2, &ms, len - 2);
+
+			if (getres < 0) {
+				fprintf(stderr, "Incomplete or broken SB (NOTIFY_MODEMSTATE_SC)\n");
+				return getres;
+			}
+
+			dbg_printf("NOTIFY_MODEMSTATE_SC 0x%02x ", ms);
+			i += getres;
+		}
+	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[1]);
+		break;
+	}
+
+	while (i < len) {
+		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;
+			}
+		}
+		dbg_printf("%d ", buf[i]);
+
+		++i;
+	}
+
+	fprintf(stderr, "Incomplete SB string\n");
+	return -EINVAL;
+}
+
+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;
+
+	for (i = 0; i < ARRAY_SIZE(telnet_options); ++i) {
+		if (id == telnet_options[i].id)
+			return &telnet_options[i];
+	}
+
+	return NULL;
+}
+
+
+/* This function is called with buf[-2:-1] being IAC SB */
+static int do_subneg(struct ios_ops *ios, unsigned char *buf, int len)
+{
+	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;
+			}
+
+			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++;
+			}
+		}
+
+		/* 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;
+	}
+}
+
+/* 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;
+
+	/* possible out-of-bounds access */
+	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 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");
+			ios_printf(ios, "%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");
+			ios_printf(ios, "%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 ssize_t telnet_write(struct ios_ops *ios, const void *buf, size_t count)
 {
 	return write(ios->fd, buf, count);
@@ -34,7 +396,38 @@ static ssize_t telnet_write(struct ios_ops *ios, const void *buf, size_t count)
 
 static ssize_t telnet_read(struct ios_ops *ios, void *buf, size_t count)
 {
-	return read(ios->fd, buf, count);
+	ssize_t ret = read(ios->fd, buf, count);
+	void *iac;
+	size_t handled = 0;
+
+	if (ret <= 0)
+		return ret;
+
+	while ((iac = memchr(buf + handled, IAC, ret - handled)) != NULL) {
+		handled = iac - buf;
+
+		/* XXX: possible out-of-bounds access */
+		if (((unsigned char *)iac)[1] == IAC) {
+			/* duplicated IAC = one payload IAC */
+			ret -= 1;
+			memmove(iac, iac + 1, ret - (iac - buf));
+			handled += 1;
+		} else {
+			int iaclen = handle_command(ios, iac, ret - handled);
+
+			if (iaclen < 0)
+				return iaclen;
+
+			memmove(iac, iac + iaclen, ret - (handled + iaclen));
+			ret -= iaclen;
+		}
+	}
+	if (ret) {
+		return ret;
+	} else {
+		errno = EAGAIN;
+		return -1;
+	}
 }
 
 static int telnet_set_speed(struct ios_ops *ios, unsigned long speed)
@@ -117,7 +510,6 @@ struct ios_ops *telnet_init(char *hostport)
 	ios->set_flow = telnet_set_flow;
 	ios->send_break = telnet_send_break;
 	ios->exit = telnet_exit;
-	ios->istelnet = true;
 
 	memset(&hints, '\0', sizeof(hints));
 	hints.ai_flags = AI_ADDRCONFIG;