monitor: Add --tty-speed command line switch

1 files changed, 55 insertions, 2 deletions

diff --git a/monitor/control.c b/monitor/control.c
index 95df9802c..f9a0b53cb 100644
--- a/monitor/control.c
+++ b/monitor/control.c
@@ -1200,7 +1200,60 @@ static void tty_callback(int fd, uint32_t events, void *user_data)
 	}
 }
 
-void control_tty(const char *path)
+static unsigned int get_speed(unsigned int speed)
+{
+	switch (speed) {
+	case 57600:
+		return B57600;
+	case 115200:
+		return B115200;
+	case 230400:
+		return B230400;
+	case 460800:
+		return B460800;
+	case 500000:
+		return B500000;
+	case 576000:
+		return B576000;
+	case 921600:
+		return B921600;
+	case 1000000:
+		return B1000000;
+	case 1152000:
+		return B1152000;
+	case 1500000:
+		return B1500000;
+	case 2000000:
+		return B2000000;
+#ifdef B2500000
+	case 2500000:
+		return B2500000;
+#endif
+#ifdef B3000000
+	case 3000000:
+		return B3000000;
+#endif
+#ifdef B3500000
+	case 3500000:
+		return B3500000;
+#endif
+#ifdef B3710000
+	case 3710000:
+		return B3710000;
+#endif
+#ifdef B4000000
+	case 4000000:
+		return B4000000;
+#endif
+	default:
+		fprintf(stderr, "Unsupported speed. Using 115200.\n");
+		return B115200;
+	}
+
+	return 0;
+}
+
+void control_tty(const char *path, unsigned int speed)
 {
 	struct control_data *data;
 	struct termios ti;
@@ -1225,7 +1278,7 @@ void control_tty(const char *path)
 	ti.c_cflag |= (CLOCAL | CREAD);
 	ti.c_cflag &= ~CRTSCTS;
 
-	cfsetspeed(&ti, B115200);
+	cfsetspeed(&ti, get_speed(speed));
 
 	if (tcsetattr(fd, TCSANOW, &ti) < 0) {
 		perror("Failed to set serial port settings");