telnet: Quote IAC on sending

To send a single IAC in the payload, you have to send two IACs, the
first quoting the second. Otherwise the sequence will be interpreted as
a command by the other side.

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

commands.c

@@ -9,6 +9,7 @@
 #include "config.h"
 
 #include <stdlib.h>
+#include <arpa/telnet.h>
 #include "microcom.h"
 
 static int cmd_speed(int argc, char *argv[])
@@ -147,6 +148,15 @@ static int cmd_break(int argc, char *argv[])
 	return MICROCOM_CMD_START;
 }
 
+static int cmd_iac(int argc, char *argv[])
+{
+	char str[17];
+
+	sprintf(str, "01234567%c9abcdef", IAC);
+	ios->write(ios, str, 16);
+	return MICROCOM_CMD_START;
+}
+
 static int cmd_quit(int argc, char *argv[])
 {
 	microcom_exit(0);
@@ -211,6 +221,10 @@ static struct cmd cmds[] = {
 		.fn = cmd_flow,
 		.info = "set flow control",
 		.help = "flow hard|soft|none",
+	}, {
+		.name = "iac",
+		.fn = cmd_iac,
+		.info = "send IAC"
 	}, {
 		.name = "dtr",
 		.fn = cmd_set_handshake_line,

telnet.c

@@ -29,7 +29,32 @@
 
 static ssize_t telnet_write(struct ios_ops *ios, const void *buf, size_t count)
 {
-	return write(ios->fd, buf, count);
+	size_t handled = 0;
+	ssize_t ret;
+	void *iac;
+
+	/*
+	 * To send an IAC character in the data stream, two IACs must be sent.
+	 * So find the first IAC in the data to be send (if any), send the data
+	 * that can be send unquoted, then send the double IAC.
+	 */
+	while ((iac = memchr(buf + handled, IAC, count - handled)) != NULL) {
+		if (iac - (buf + handled)) {
+			ret = write(ios->fd, buf + handled, iac - (buf + handled));
+			if (ret < 0)
+				return ret;
+			handled += ret;
+		} else {
+			dprintf(ios->fd, "%c%c", IAC, IAC);
+			handled += 1;
+		}
+	}
+
+	/* Send the remaining data that needs no quoting. */
+	ret = write(ios->fd, buf + handled, count - handled);
+	if (ret < 0)
+		return ret;
+	return ret + handled;
 }
 
 static ssize_t telnet_read(struct ios_ops *ios, void *buf, size_t count)