[Xen-devel,10/14,v4] xen/arm: vpl011: Modify xenconsole to support multiple consoles

diff --git a/tools/console/daemon/io.c b/tools/console/daemon/io.c
index c5dd08d..db73e10 100644
--- a/tools/console/daemon/io.c
+++ b/tools/console/daemon/io.c
@@ -90,12 +90,15 @@  struct buffer {
 };
 
 struct console {
+	char *xsname;
+	char *ttyname;
 	int master_fd;
 	int master_pollfd_idx;
 	int slave_fd;
 	int log_fd;
 	struct buffer buffer;
-	char *conspath;
+	char *xspath;
+	char *log_suffix;
 	int ring_ref;
 	xenevtchn_port_or_error_t local_port;
 	xenevtchn_port_or_error_t remote_port;
@@ -103,6 +106,23 @@  struct console {
 	struct domain *d;
 };
 
+struct console_data {
+	char *xsname;
+	char *ttyname;
+	char *log_suffix;
+};
+
+static struct console_data console_data[] = {
+
+	{
+		.xsname = "/console",
+		.ttyname = "tty",
+		.log_suffix = "",
+	},
+};
+
+#define MAX_CONSOLE (sizeof(console_data)/sizeof(struct console_data))
+
 struct domain {
 	int domid;
 	bool is_dead;
@@ -112,11 +132,90 @@  struct domain {
 	int xce_pollfd_idx;
 	int event_count;
 	long long next_period;
-	struct console console;
+	struct console console[MAX_CONSOLE];
 };
 
 static struct domain *dom_head;
 
+typedef void (*VOID_ITER_FUNC_ARG1)(struct console *);
+typedef bool (*BOOL_ITER_FUNC_ARG1)(struct console *);
+typedef int (*INT_ITER_FUNC_ARG1)(struct console *);
+typedef void (*VOID_ITER_FUNC_ARG2)(struct console *,  unsigned int);
+typedef int (*INT_ITER_FUNC_ARG3)(struct console *,
+			 struct domain *dom, void **);
+
+static inline bool console_enabled(struct console *con)
+{
+	return con->local_port != -1;
+}
+
+static inline void console_iter_void_arg1(struct domain *d,
+										  VOID_ITER_FUNC_ARG1 iter_func)
+{
+	int i = 0;
+	struct console *con = &(d->console[0]);
+
+	for (i = 0; i < MAX_CONSOLE; i++, con++)
+	{
+		iter_func(con);
+	}
+}
+
+static inline void console_iter_void_arg2(struct domain *d,
+										  VOID_ITER_FUNC_ARG2 iter_func,
+										  unsigned int iter_data)
+{
+	int i = 0;
+	struct console *con = &(d->console[0]);
+
+	for (i = 0; i < MAX_CONSOLE; i++, con++)
+	{
+		iter_func(con, iter_data);
+	}
+}
+
+static inline bool console_iter_bool_arg1(struct domain *d,
+										  BOOL_ITER_FUNC_ARG1 iter_func)
+{
+	int i = 0;
+	struct console *con = &(d->console[0]);
+
+	for (i = 0; i < MAX_CONSOLE; i++, con++)
+	{
+		if (iter_func(con))
+			return true;
+	}
+	return false;
+}
+
+static inline int console_iter_int_arg1(struct domain *d,
+										INT_ITER_FUNC_ARG1 iter_func)
+{
+	int i = 0;
+	struct console *con = &(d->console[0]);
+
+	for (i = 0; i < MAX_CONSOLE; i++, con++)
+	{
+		if (iter_func(con))
+			return 1;
+	}
+	return 0;
+}
+
+static inline int console_iter_int_arg3(struct domain *d,
+										INT_ITER_FUNC_ARG3 iter_func,
+										void *iter_data)
+{
+	int i = 0;
+	struct console *con = &(d->console[0]);
+
+	for (i = 0; i < MAX_CONSOLE; i++, con++)
+	{
+		if (iter_func(con, d, iter_data))
+			return 1;
+	}
+	return 0;
+}
 static int write_all(int fd, const char* buf, size_t len)
 {
 	while (len) {
@@ -163,12 +262,27 @@  static int write_with_timestamp(int fd, const char *data, size_t sz,
 	return 0;
 }
 
-static void buffer_append(struct console *con)
+static inline bool buffer_available(struct console *con)
+{
+	if (discard_overflowed_data ||
+		!con->buffer.max_capacity ||
+		con->buffer.size < con->buffer.max_capacity)
+		return true;
+	else
+		return false;
+}
+
+static void buffer_append(struct console *con, unsigned int data)
 {
 	struct buffer *buffer = &con->buffer;
+	struct xencons_interface *intf = con->interface;
+	xenevtchn_port_or_error_t rxport = (xenevtchn_port_or_error_t)data;
 	struct domain *dom = con->d;
 	XENCONS_RING_IDX cons, prod, size;
-	struct xencons_interface *intf = con->interface;
+
+	/* If incoming data is not for the current console then ignore. */
+	if (con->local_port != rxport)
+		return;
 
 	cons = intf->out_cons;
 	prod = intf->out_prod;
@@ -321,7 +435,7 @@  static int create_console_log(struct console *con)
 		return -1;
 	}
 
-	snprintf(logfile, PATH_MAX-1, "%s/guest-%s.log", log_dir, data);
+	snprintf(logfile, PATH_MAX-1, "%s/guest-%s%s.log", log_dir, data, con->log_suffix);
 	free(data);
 	logfile[PATH_MAX-1] = '\0';
 
@@ -427,6 +541,9 @@  static int console_create_tty(struct console *con)
 	struct termios term;
 	struct domain *dom = con->d;
 
+	if (!console_enabled(con))
+		return 1;
+
 	assert(con->slave_fd == -1);
 	assert(con->master_fd == -1);
 
@@ -462,7 +579,7 @@  static int console_create_tty(struct console *con)
 		goto out;
 	}
 
-	success = asprintf(&path, "%s/limit", con->conspath) !=
+	success = asprintf(&path, "%s/limit", con->xspath) !=
 		-1;
 	if (!success)
 		goto out;
@@ -473,7 +590,7 @@  static int console_create_tty(struct console *con)
 	}
 	free(path);
 
-	success = (asprintf(&path, "%s/tty", con->conspath) != -1);
+	success = (asprintf(&path, "%s/%s", con->xspath, con->ttyname) != -1);
 	if (!success)
 		goto out;
 	success = xs_write(xs, XBT_NULL, path, slave, strlen(slave));
@@ -543,14 +660,14 @@  static int console_create_ring(struct console *con)
 	char *type, path[PATH_MAX];
 	struct domain *dom = con->d;
 
-	err = xs_gather(xs, con->conspath,
+	err = xs_gather(xs, con->xspath,
 			"ring-ref", "%u", &ring_ref,
 			"port", "%i", &remote_port,
 			NULL);
 	if (err)
 		goto out;
 
-	snprintf(path, sizeof(path), "%s/type", con->conspath);
+	snprintf(path, sizeof(path), "%s/type", con->xspath);
 	type = xs_read(xs, XBT_NULL, path, NULL);
 	if (type && strcmp(type, "xenconsoled") != 0) {
 		free(type);
@@ -594,15 +711,16 @@  static int console_create_ring(struct console *con)
 
 	con->local_port = -1;
 	con->remote_port = -1;
-	if (dom->xce_handle != NULL)
-		xenevtchn_close(dom->xce_handle);
 
-	/* Opening evtchn independently for each console is a bit
-	 * wasteful, but that's how the code is structured... */
-	dom->xce_handle = xenevtchn_open(NULL, 0);
-	if (dom->xce_handle == NULL) {
-		err = errno;
-		goto out;
+	if (dom->xce_handle == NULL)
+	{
+		/* Opening evtchn independently for each console is a bit
+		 * wasteful, but that's how the code is structured... */
+		dom->xce_handle = xenevtchn_open(NULL, 0);
+		if (dom->xce_handle == NULL) {
+			err = errno;
+			goto out;
+		}
 	}
  
 	rc = xenevtchn_bind_interdomain(dom->xce_handle,
@@ -639,29 +757,65 @@  static bool watch_domain(struct domain *dom, bool watch)
 {
 	char domid_str[3 + MAX_STRLEN(dom->domid)];
 	bool success;
-	struct console *con = &dom->console;
+	struct console *con = &dom->console[0];
 
 	snprintf(domid_str, sizeof(domid_str), "dom%u", dom->domid);
 	if (watch) {
-		success = xs_watch(xs, con->conspath, domid_str);
+		success = xs_watch(xs, con->xspath, domid_str);
 		if (success)
-			console_create_ring(con);
+			console_iter_int_arg1(dom, console_create_ring);
 		else
-			xs_unwatch(xs, con->conspath, domid_str);
+			xs_unwatch(xs, con->xspath, domid_str);
 	} else {
-		success = xs_unwatch(xs, con->conspath, domid_str);
+		success = xs_unwatch(xs, con->xspath, domid_str);
 	}
 
 	return success;
 }
 
+static int console_init(struct console *con, struct domain *dom, void **data)
+{
+	char *s;
+	int err = -1;
+	struct console_data **con_data = (struct console_data **)data;
+
+	con->master_fd = -1;
+	con->master_pollfd_idx = -1;
+	con->slave_fd = -1;
+	con->log_fd = -1;
+	con->ring_ref = -1;
+	con->local_port = -1;
+	con->remote_port = -1;
+	con->d = dom;
+	con->ttyname = (*con_data)->ttyname;
+	con->log_suffix = (*con_data)->log_suffix;
+	con->xsname = (*con_data)->xsname;
+	con->xspath = xs_get_domain_path(xs, dom->domid);
+	s = realloc(con->xspath, strlen(con->xspath) +
+				strlen(con->xsname) + 1);
+	if (s)
+	{
+		con->xspath = s;
+		strcat(con->xspath, con->xsname);
+		err = 0;
+	}
+
+	(*con_data)++;
+
+	return err;
+}
+
+static void console_free(struct console *con)
+{
+	if (con->xspath)
+		free(con->xspath);
+}
 
 static struct domain *create_domain(int domid)
 {
 	struct domain *dom;
-	char *s;
 	struct timespec ts;
-	struct console *con;
+	struct console_data *con_data = &console_data[0];
 
 	if (clock_gettime(CLOCK_MONOTONIC, &ts) < 0) {
 		dolog(LOG_ERR, "Cannot get time of day %s:%s:L%d",
@@ -678,28 +832,13 @@  static struct domain *create_domain(int domid)
 
 	dom->domid = domid;
 
-	con = &dom->console;
-	con->conspath = xs_get_domain_path(xs, dom->domid);
-	s = realloc(con->conspath, strlen(con->conspath) +
-		    strlen("/console") + 1);
-	if (s == NULL)
+	if (console_iter_int_arg3(dom, console_init, (void **)&con_data))
 		goto out;
-	con->conspath = s;
-	strcat(con->conspath, "/console");
 
-	con->master_fd = -1;
-	con->master_pollfd_idx = -1;
-	con->slave_fd = -1;
-	con->log_fd = -1;
-	con->d = dom;
 	dom->xce_pollfd_idx = -1;
 
 	dom->next_period = ((long long)ts.tv_sec * 1000) + (ts.tv_nsec / 1000000) + RATE_LIMIT_PERIOD;
 
-	con->ring_ref = -1;
-	con->local_port = -1;
-	con->remote_port = -1;
-
 	if (!watch_domain(dom, true))
 		goto out;
 
@@ -710,7 +849,7 @@  static struct domain *create_domain(int domid)
 
 	return dom;
  out:
-	free(con->conspath);
+	console_iter_void_arg1(dom, console_free);
 	free(dom);
 	return NULL;
 }
@@ -740,33 +879,40 @@  static void remove_domain(struct domain *dom)
 	}
 }
 
-static void cleanup_domain(struct domain *d)
+static void console_cleanup(struct console *con)
 {
-	struct console *con = &d->console;
-
-	console_close_tty(con);
-
 	if (con->log_fd != -1) {
 		close(con->log_fd);
 		con->log_fd = -1;
 	}
 
-	free(con->buffer.data);
-	con->buffer.data = NULL;
+	if (con->buffer.data)
+	{
+		free(con->buffer.data);
+		con->buffer.data = NULL;
+	}
 
-	free(con->conspath);
-	con->conspath = NULL;
+	if (con->xspath)
+	{
+		free(con->xspath);
+		con->xspath = NULL;
+	}
+}
+
+static void cleanup_domain(struct domain *d)
+{
+	console_iter_void_arg1(d, console_close_tty);
+
+	console_iter_void_arg1(d, console_cleanup);
 
 	remove_domain(d);
 }
 
 static void shutdown_domain(struct domain *d)
 {
-	struct console *con = &d->console;
-
 	d->is_dead = true;
 	watch_domain(d, false);
-	console_unmap_interface(con);
+	console_iter_void_arg1(d, console_unmap_interface);
 	if (d->xce_handle != NULL)
 		xenevtchn_close(d->xce_handle);
 	d->xce_handle = NULL;
@@ -885,10 +1031,15 @@  static void handle_tty_write(struct console *con)
 	}
 }
 
+static void console_event_unmask(struct console *con)
+{
+	if (con->local_port != -1)
+		(void)xenevtchn_unmask(con->d->xce_handle, con->local_port);
+}
+
 static void handle_ring_read(struct domain *dom)
 {
 	xenevtchn_port_or_error_t port;
-	struct console *con = &dom->console;
 
 	if (dom->is_dead)
 		return;
@@ -898,10 +1049,10 @@  static void handle_ring_read(struct domain *dom)
 
 	dom->event_count++;
 
-	buffer_append(con);
+	console_iter_void_arg2(dom, buffer_append, port);
 
 	if (dom->event_count < RATE_LIMIT_ALLOWANCE)
-		(void)xenevtchn_unmask(dom->xce_handle, port);
+		console_iter_void_arg1(dom, console_event_unmask);
 }
 
 static void handle_xs(void)
@@ -922,7 +1073,7 @@  static void handle_xs(void)
 		/* We may get watches firing for domains that have recently
 		   been removed, so dom may be NULL here. */
 		if (dom && dom->is_dead == false)
-			console_create_ring(&dom->console);
+			console_iter_int_arg1(dom, console_create_ring);
 	}
 
 	free(vec);
@@ -963,16 +1114,22 @@  static void handle_hv_logs(xenevtchn_handle *xce_handle, bool force)
 		(void)xenevtchn_unmask(xce_handle, port);
 }
 
+static void console_open_log(struct console *con)
+{
+	if (console_enabled(con))
+	{
+		if (con->log_fd != -1)
+			close(con->log_fd);
+		con->log_fd = create_console_log(con);
+	}
+}
+
 static void handle_log_reload(void)
 {
 	if (log_guest) {
 		struct domain *d;
 		for (d = dom_head; d; d = d->next) {
-			struct console *con = &d->console;
-
-			if (con->log_fd != -1)
-				close(con->log_fd);
-			con->log_fd = create_console_log(con);
+			console_iter_void_arg1(d, console_open_log);
 		}
 	}
 
@@ -1024,6 +1181,40 @@  static void reset_fds(void)
 		memset(fds, 0, sizeof(struct pollfd) * current_array_size);
 }
 
+static void add_console_fd(struct console *con)
+{
+	if (con->master_fd != -1) {
+		short events = 0;
+		if (!con->d->is_dead && ring_free_bytes(con))
+			events |= POLLIN;
+
+		if (!buffer_empty(&con->buffer))
+			events |= POLLOUT;
+
+		if (events)
+			con->master_pollfd_idx =
+				set_fds(con->master_fd, events|POLLPRI);
+	}
+}
+
+static void process_console(struct console *con)
+{
+	if (con->master_fd != -1 && con->master_pollfd_idx != -1) {
+		if (fds[con->master_pollfd_idx].revents &
+			~(POLLIN|POLLOUT|POLLPRI))
+			console_handle_broken_tty(con, domain_is_valid(con->d->domid));
+		else {
+			if (fds[con->master_pollfd_idx].revents &
+				POLLIN)
+				handle_tty_read(con);
+			if (fds[con->master_pollfd_idx].revents &
+				POLLOUT)
+				handle_tty_write(con);
+		}
+	}
+	con->master_pollfd_idx = -1;
+}
+
 void handle_io(void)
 {
 	int ret;
@@ -1081,7 +1272,6 @@  void handle_io(void)
 		/* Re-calculate any event counter allowances & unblock
 		   domains with new allowance */
 		for (d = dom_head; d; d = d->next) {
-			struct console *con = &d->console;
 
 			/* CS 16257:955ee4fa1345 introduces a 5ms fuzz
 			 * for select(), it is not clear poll() has
@@ -1092,14 +1282,13 @@  void handle_io(void)
 			if ((now+5) > d->next_period) {
 				d->next_period = now + RATE_LIMIT_PERIOD;
 				if (d->event_count >= RATE_LIMIT_ALLOWANCE) {
-					(void)xenevtchn_unmask(d->xce_handle, con->local_port);
+					console_iter_void_arg1(d, console_event_unmask);
 				}
 				d->event_count = 0;
 			}
 		}
 
 		for (d = dom_head; d; d = d->next) {
-			struct console *con = &d->console;
 
 			if (d->event_count >= RATE_LIMIT_ALLOWANCE) {
 				/* Determine if we're going to be the next time slice to expire */
@@ -1107,28 +1296,15 @@  void handle_io(void)
 				    d->next_period < next_timeout)
 					next_timeout = d->next_period;
 			} else if (d->xce_handle != NULL) {
-				if (discard_overflowed_data ||
-				    !con->buffer.max_capacity ||
-				    con->buffer.size < con->buffer.max_capacity) {
-					int evtchn_fd = xenevtchn_fd(d->xce_handle);
-					d->xce_pollfd_idx = set_fds(evtchn_fd,
-								    POLLIN|POLLPRI);
+					if (console_iter_bool_arg1(d, buffer_available))
+					{
+						int evtchn_fd = xenevtchn_fd(d->xce_handle);
+						d->xce_pollfd_idx = set_fds(evtchn_fd,
+													POLLIN|POLLPRI);
+					}
 				}
-			}
-
-			if (con->master_fd != -1) {
-				short events = 0;
-				if (!d->is_dead && ring_free_bytes(con))
-					events |= POLLIN;
 
-				if (!buffer_empty(&con->buffer))
-					events |= POLLOUT;
-
-				if (events)
-					con->master_pollfd_idx =
-						set_fds(con->master_fd,
-							events|POLLPRI);
-			}
+			console_iter_void_arg1(d, add_console_fd);
 		}
 
 		/* If any domain has been rate limited, we need to work
@@ -1185,7 +1361,6 @@  void handle_io(void)
 		}
 
 		for (d = dom_head; d; d = n) {
-			struct console *con = &d->console;
 
 			n = d->next;
 			if (d->event_count < RATE_LIMIT_ALLOWANCE) {
@@ -1198,22 +1373,9 @@  void handle_io(void)
 				    handle_ring_read(d);
 			}
 
-			if (con->master_fd != -1 && con->master_pollfd_idx != -1) {
-				if (fds[con->master_pollfd_idx].revents &
-				    ~(POLLIN|POLLOUT|POLLPRI))
-					console_handle_broken_tty(con,
-						   domain_is_valid(d->domid));
-				else {
-					if (fds[con->master_pollfd_idx].revents &
-					    POLLIN)
-						handle_tty_read(con);
-					if (fds[con->master_pollfd_idx].revents &
-					    POLLOUT)
-						handle_tty_write(con);
-				}
-			}
+			console_iter_void_arg1(d, process_console);
 
-			d->xce_pollfd_idx = con->master_pollfd_idx = -1;
+			d->xce_pollfd_idx = -1;
 
 			if (d->last_seen != enum_pass)
 				shutdown_domain(d);