mux/telnet: Handle telnet specific stuff in telnet code

The functions are moved mostly unmodified to telnet.c. Parsing of
out-of-bound data now happens in telnet's read callback instead of
in the core.

Signed-off-by: Uwe Kleine-König <u.kleine-koenig@pengutronix.de>

2 files changed, 326 insertions, 313 deletions

diff --git a/mux.c b/mux.c
index 9dc684c..44ba163 100644
--- a/mux.c
+++ b/mux.c
@@ -20,195 +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
 
-/* 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:
-		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) {
-		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;
 
@@ -222,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) {
@@ -353,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;
diff --git a/telnet.c b/telnet.c
index c32a2d4..36e4e21 100644
--- a/telnet.c
+++ b/telnet.c
@@ -24,9 +24,303 @@
 #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;
+}
+
+
+/* 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:
+		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) {
+		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 +328,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)