Diff between 7f20132c5494a6e8398a77d7429b5e5a4e0a2494 and 589a360db2b08fec01715fc1d0e3e83a54a7a468

Changed Files

File Additions Deletions Status
monitor/control.c +89 -0 modified
monitor/control.h +1 -0 modified
monitor/main.c +5 -1 modified

Full Patch

diff --git a/monitor/control.c b/monitor/control.c
index b3fc93d..508b068 100644
--- a/monitor/control.c
+++ b/monitor/control.c
@@ -35,6 +35,10 @@
 #include <sys/time.h>
 #include <sys/socket.h>
 #include <sys/un.h>
+#include <sys/types.h>
+#include <sys/stat.h>
+#include <termios.h>
+#include <fcntl.h>
 
 #include "lib/bluetooth.h"
 #include "lib/hci.h"
@@ -1157,6 +1161,91 @@ void control_server(const char *path)
 	server_fd = fd;
 }
 
+static void tty_callback(int fd, uint32_t events, void *user_data)
+{
+	struct control_data *data = user_data;
+	ssize_t len;
+
+	if (events & (EPOLLERR | EPOLLHUP)) {
+		mainloop_remove_fd(data->fd);
+		return;
+	}
+
+	len = read(data->fd, data->buf + data->offset,
+					sizeof(data->buf) - data->offset);
+	if (len < 0)
+		return;
+
+	data->offset += len;
+
+	if (data->offset > MGMT_HDR_SIZE) {
+		struct mgmt_hdr *hdr = (struct mgmt_hdr *) data->buf;
+		uint16_t pktlen = le16_to_cpu(hdr->len);
+
+		if (data->offset > pktlen + MGMT_HDR_SIZE) {
+			uint16_t opcode = le16_to_cpu(hdr->opcode);
+			uint16_t index = le16_to_cpu(hdr->index);
+
+			packet_monitor(NULL, NULL, index, opcode,
+					data->buf + MGMT_HDR_SIZE, pktlen);
+
+			data->offset -= pktlen + MGMT_HDR_SIZE;
+
+			if (data->offset > 0)
+				memmove(data->buf, data->buf +
+					 MGMT_HDR_SIZE + pktlen, data->offset);
+		}
+	}
+}
+
+void control_tty(const char *path)
+{
+	struct control_data *data;
+	struct termios ti;
+	int fd;
+
+	fd = open(path, O_RDWR | O_NOCTTY | O_NONBLOCK);
+	if (fd < 0) {
+		perror("Failed to open serial port");
+		return;
+	}
+
+	if (tcflush(fd, TCIOFLUSH) < 0) {
+		perror("Failed to flush serial port");
+		close(fd);
+		return;
+	}
+
+	memset(&ti, 0, sizeof(ti));
+	/* Switch TTY to raw mode */
+	cfmakeraw(&ti);
+
+	ti.c_cflag |= (CLOCAL | CREAD);
+	ti.c_cflag &= ~CRTSCTS;
+
+	cfsetspeed(&ti, B115200);
+
+	if (tcsetattr(fd, TCSANOW, &ti) < 0) {
+		perror("Failed to set serial port settings");
+		close(fd);
+		return;
+	}
+
+	printf("--- %s opened ---\n", path);
+
+	data = malloc(sizeof(*data));
+	if (!data) {
+		close(fd);
+		return;
+	}
+
+	memset(data, 0, sizeof(*data));
+	data->channel = HCI_CHANNEL_MONITOR;
+	data->fd = fd;
+
+	mainloop_add_fd(data->fd, EPOLLIN, tty_callback, data, free_data);
+}
+
 bool control_writer(const char *path)
 {
 	btsnoop_file = btsnoop_create(path, BTSNOOP_FORMAT_MONITOR);
diff --git a/monitor/control.h b/monitor/control.h
index 28f16db..f37d9cd 100644
--- a/monitor/control.h
+++ b/monitor/control.h
@@ -27,6 +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);
 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 5f6be79..fd3954b 100644
--- a/monitor/main.c
+++ b/monitor/main.c
@@ -71,6 +71,7 @@ static void usage(void)
 }
 
 static const struct option main_options[] = {
+	{ "tty",     required_argument, NULL, 'd' },
 	{ "read",    required_argument, NULL, 'r' },
 	{ "write",   required_argument, NULL, 'w' },
 	{ "analyze", required_argument, NULL, 'a' },
@@ -106,12 +107,15 @@ int main(int argc, char *argv[])
 	for (;;) {
 		int opt;
 
-		opt = getopt_long(argc, argv, "r:w:a:s:p:i:tTSE:vh",
+		opt = getopt_long(argc, argv, "d:r:w:a:s:p:i:tTSE:vh",
 						main_options, NULL);
 		if (opt < 0)
 			break;
 
 		switch (opt) {
+		case 'd':
+			control_tty(optarg);
+			break;
 		case 'r':
 			reader_path = optarg;
 			break;