From dac899f0ab81dec8127f2ca32f676735bf4691c7 Mon Sep 17 00:00:00 2001
From: Johan Hedberg <johan.hedberg@intel.com>
Date: Fri, 22 Apr 2016 15:56:09 +0300
Subject: [PATCH] monitor: Add --tty-speed command line switch

---
 monitor/control.c | 57 +++++++++++++++++++++++++++++++++++++++++++++--
 monitor/control.h |  2 +-
 monitor/main.c    | 12 +++++++++-
 3 files changed, 67 insertions(+), 4 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");
diff --git a/monitor/control.h b/monitor/control.h
index f37d9cd3b..601a45f01 100644
--- a/monitor/control.h
+++ b/monitor/control.h
@@ -27,7 +27,7 @@
 bool control_writer(const char *path);
 void control_reader(const char *path);
 void control_server(const char *path);
-void control_tty(const char *path);
+void control_tty(const char *path, unsigned int speed);
 int control_tracing(void);
 
 void control_message(uint16_t opcode, const void *data, uint16_t size);
diff --git a/monitor/main.c b/monitor/main.c
index e1241578c..38e4fc9a1 100644
--- a/monitor/main.c
+++ b/monitor/main.c
@@ -64,6 +64,7 @@ static void usage(void)
 		"\t-p, --priority <level> Show only priority or lower\n"
 		"\t-i, --index <num>      Show only specified controller\n"
 		"\t-d, --tty <tty>        Read data from TTY\n"
+		"\t-B, --tty-speed <rate> Set TTY speed (default 115200)\n"
 		"\t-t, --time             Show time instead of time offset\n"
 		"\t-T, --date             Show time and date information\n"
 		"\t-S, --sco              Dump SCO traffic\n"
@@ -73,6 +74,7 @@ static void usage(void)
 
 static const struct option main_options[] = {
 	{ "tty",     required_argument, NULL, 'd' },
+	{ "tty-speed", required_argument, NULL, 'B' },
 	{ "read",    required_argument, NULL, 'r' },
 	{ "write",   required_argument, NULL, 'w' },
 	{ "analyze", required_argument, NULL, 'a' },
@@ -96,6 +98,8 @@ int main(int argc, char *argv[])
 	const char *writer_path = NULL;
 	const char *analyze_path = NULL;
 	const char *ellisys_server = NULL;
+	const char *tty = NULL;
+	unsigned int tty_speed = 115200;
 	unsigned short ellisys_port = 0;
 	const char *str;
 	int exit_status;
@@ -115,7 +119,10 @@ int main(int argc, char *argv[])
 
 		switch (opt) {
 		case 'd':
-			control_tty(optarg);
+			tty= optarg;
+			break;
+		case 'B':
+			tty_speed = atoi(optarg);
 			break;
 		case 'r':
 			reader_path = optarg;
@@ -217,6 +224,9 @@ int main(int argc, char *argv[])
 	if (ellisys_server)
 		ellisys_enable(ellisys_server, ellisys_port);
 
+	if (tty)
+		control_tty(tty, tty_speed);
+
 	if (control_tracing() < 0)
 		return EXIT_FAILURE;
 
-- 
2.47.3