telnet: Properly quote IAC when sending SET_BAUDRATE negotiation

When the baudrate is to be set to say 130816 (= 0x1ff00) this has to be
send (raw) as:

	IAC SB COM_PORT_CONTROL SET_BAUDRATE_CS 0x00 0x01 0xff 0xff 0x00 IAC SE

This is explicitly mentioned in RFC855:

   Finally, if parameters in an option "subnegotiation" include a byte
   with a value of 255, it is necessary to double this byte in
   accordance the general TELNET rules.

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

1 files changed, 12 insertions, 4 deletions

diff --git a/telnet.c b/telnet.c
index c32a2d4..4a66b95 100644
--- a/telnet.c
+++ b/telnet.c
@@ -39,13 +39,21 @@ static ssize_t telnet_read(struct ios_ops *ios, void *buf, size_t count)
 
 static int telnet_set_speed(struct ios_ops *ios, unsigned long speed)
 {
+	unsigned char buf2[14] = {IAC, SB, TELNET_OPTION_COM_PORT_CONTROL, SET_BAUDRATE_CS};
+	size_t offset = 4;
+	int i;
+
+	for (i = 0; i < 4; ++i) {
+		buf2[offset] = (speed >> (24 - 8 * i)) & 0xff;
+		if (buf2[offset++] == IAC)
+			buf2[offset++] = IAC;
+	}
 
-	unsigned char buf2[] = {IAC, SB, TELNET_OPTION_COM_PORT_CONTROL, SET_BAUDRATE_CS, 0, 0, 0, 0, IAC, SE};
-	int *speedp = (int *)&buf2[4];
+	buf2[offset++] = IAC;
+	buf2[offset++] = SE;
 
-	*speedp = htonl(speed);
 	dbg_printf("-> IAC SB COM_PORT_CONTROL SET_BAUDRATE_CS 0x%lx IAC SE\n", speed);
-	write(ios->fd, buf2, 10);
+	write(ios->fd, buf2, offset);
 
 	return 0;
 }