Merge branch 'iac-in-subneg' of https://github.com/ukleinek/microcom

2 files changed, 90 insertions, 12 deletions

diff --git a/mux.c b/mux.c
index b7535b8..a6284ca 100644
--- a/mux.c
+++ b/mux.c
@@ -27,6 +27,48 @@
 
 #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)
 {
@@ -70,9 +112,17 @@ static int do_com_port_option(struct ios_ops *ios, unsigned char *buf, int len)
 		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;
+		{
+			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 ");
@@ -84,15 +134,35 @@ static int do_com_port_option(struct ios_ops *ios, unsigned char *buf, int len)
 		dbg_printf("SET_STOPSIZE_SC ");
 		break;
 	case SET_CONTROL_SC:
-		i++;
-		dbg_printf("SET_CONTROL_SC 0x%02x ", buf[i]);
+		{
+			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:
-		i++;
-		dbg_printf("NOTIFY_MODEMSTATE_SC 0x%02x ", buf[i]);
+		{
+			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 ");
@@ -110,7 +180,7 @@ static int do_com_port_option(struct ios_ops *ios, unsigned char *buf, int len)
 		dbg_printf("PURGE_DATA_SC ");
 		break;
 	default:
-		dbg_printf("??? %d ", buf[i]);
+		dbg_printf("??? %d ", buf[1]);
 		break;
 	}
 
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;
 }