virtio_console: add support for set termio

drivers/char/virtio_console.c

@@ -190,6 +190,8 @@ struct port {
 	/* Protect the operations on the out_vq. */
 	spinlock_t outvq_lock;
 
+	spinlock_t write_lock;
+
 	/* The IO vqs for this port */
 	struct virtqueue *in_vq, *out_vq;
 
@@ -235,11 +237,18 @@ struct port {
 
 	/* We should allow only one process to open a port */
 	bool guest_connected;
+
+	bool tiocm_updated;
+	int tiocm;
+	bool is_outvq_filled;
+//	struct mutex port_mutex;
 };
 
 /* This is the very early arch-specified put chars function. */
 static int (*early_put_chars)(u32, const char *, int);
 
+static int wait_port_writable(struct port *port, bool nonblock);
+
 static struct port *find_port_by_vtermno(u32 vtermno)
 {
 	struct port *port;
@@ -563,6 +572,7 @@ static ssize_t __send_control_msg(struct ports_device *portdev, u32 port_id,
 	portdev->cpkt.event = cpu_to_virtio16(portdev->vdev, event);
 	portdev->cpkt.value = cpu_to_virtio16(portdev->vdev, value);
 
+	//printk("send ctl %d %d %x\n", portdev->cpkt.id, portdev->cpkt.event, portdev->cpkt.value);
 	sg_init_one(sg, &portdev->cpkt, sizeof(struct virtio_console_control));
 
 	if (virtqueue_add_outbuf(vq, sg, 1, &portdev->cpkt, GFP_ATOMIC) == 0) {
@@ -811,6 +821,7 @@ static ssize_t port_fops_write(struct file *filp, const char __user *ubuf,
 	bool nonblock;
 	struct scatterlist sg[1];
 
+		printk("vc write\n");
 	/* Userspace could be out to fool us */
 	if (!count)
 		return 0;
@@ -1120,6 +1131,7 @@ static int put_chars(u32 vtermno, const char *buf, int count)
 	struct scatterlist sg[1];
 	void *data;
 	int ret;
+	unsigned int len;
 
 	if (unlikely(early_put_chars))
 		return early_put_chars(vtermno, buf, count);
@@ -1128,13 +1140,54 @@ static int put_chars(u32 vtermno, const char *buf, int count)
 	if (!port)
 		return -EPIPE;
 
+//	if (will_write_block(port)) {
+//		printk("b\n");
+//		return 0;
+//	}
+
+	if (!spin_trylock(&port->write_lock)) {
+		return 0;
+	}
+	if (port->is_outvq_filled && !virtqueue_get_buf(port->out_vq, &len)) {
+		spin_unlock(&port->write_lock);
+		return 0;
+	}
+#if 0
+	/* Has the backend released the buffer we filled last time? */
+	if (port->is_outvq_filled && !virtqueue_get_buf(port->out_vq, &len) ) {
+//		printk("b\n");
+		spin_unlock(&port->write_lock);
+		return 0;
+	}
+#endif
+	port->is_outvq_filled = false;
+
+//	printk("p\n");
+
 	data = kmemdup(buf, count, GFP_ATOMIC);
-	if (!data)
+	if (!data) {
+		spin_unlock(&port->write_lock);
 		return -ENOMEM;
+	}
 
 	sg_init_one(sg, data, count);
-	ret = __send_to_port(port, sg, 1, count, data, false);
-	kfree(data);
+	ret = __send_to_port(port, sg, 1, count, data, true);
+	port->is_outvq_filled = true;
+	spin_unlock(&port->write_lock);
+	/*
+	if (virtio_has_feature(port, VIRTIO_CONSOLE_F_UART_BE)) {
+		mutex_lock(&port->port_mutex);
+		port->write_done = false;
+		ret = __send_to_port(port, sg, 1, count, data, false);
+		wait_event_freezable(port->waitqueue, port->);
+		mutex_unlock(&port->port_mutex);
+	} else {
+		ret = __send_to_port(port, sg, 1, count, data, false);
+	} */
+
+//	printk("o\n");
+
+	//kfree(data);
 	return ret;
 }
 
@@ -1199,13 +1252,96 @@ static void notifier_del_vio(struct hvc_struct *hp, int data)
 	hp->irq_requested = 0;
 }
 
+static void virtio_console_set_termios(struct hvc_struct *hp, const struct ktermios *old)
+{
+	struct tty_struct *tty = hp->port.tty;
+	struct port *port;
+	uint16_t value;
+
+	port = find_port_by_vtermno(hp->vtermno);
+	if (!port)
+		return;
+
+	if(!virtio_has_feature(port->portdev->vdev, VIRTIO_CONSOLE_F_UART_BE))
+		return;
+
+//	mutex_lock(&port->port_mutex);
+
+	if (tty->termios.c_ospeed != old->c_ospeed) {
+		value = (uint16_t)(tty->termios.c_cflag & CBAUD);
+		send_control_msg(port, VIRTIO_CONSOLE_SET_TERMIO_OBAUD, value);
+	}
+
+	if (tty->termios.c_ispeed != old->c_ispeed) {
+		value = (uint16_t)((tty->termios.c_cflag >> IBSHIFT) & CBAUD);
+		if (value == B0)
+			value = (uint16_t)(tty->termios.c_cflag & CBAUD);
+		send_control_msg(port, VIRTIO_CONSOLE_SET_TERMIO_IBAUD, value);
+	}
+
+	if ((tty->termios.c_cflag ^ old->c_cflag) & CRTSCTS) {
+		value = (tty->termios.c_cflag & CRTSCTS) ? 1 : 0;
+		send_control_msg(port, VIRTIO_CONSOLE_SET_TERMIO_CRTSCTS, value);
+	}
+//	mutex_unlock(&port->port_mutex);
+}
+
+static int virtio_console_tiocmget(struct hvc_struct *hp)
+{
+	struct port *port;
+	int ret;
+
+	port = find_port_by_vtermno(hp->vtermno);
+	if (!port)
+		return -ENXIO;
+
+	if(!virtio_has_feature(port->portdev->vdev, VIRTIO_CONSOLE_F_UART_BE))
+		return -EPERM;
+
+//	mutex_lock(&port->port_mutex);
+
+	port->tiocm_updated = false;
+	send_control_msg(port, VIRTIO_CONSOLE_TIOCMGET, 0);
+
+	ret = wait_event_freezable(port->waitqueue, port->tiocm_updated);
+
+//	mutex_unlock(&port->port_mutex);
+
+	if (ret < 0)
+		return ret;
+
+	return port->tiocm;
+}
+
+static int virtio_console_tiocmset(struct hvc_struct *hp, unsigned int set, unsigned int clear)
+{
+	struct port *port;
+	unsigned int value;
+
+	port = find_port_by_vtermno(hp->vtermno);
+	if (!port)
+		return -ENXIO;
+
+	if(!virtio_has_feature(port->portdev->vdev, VIRTIO_CONSOLE_F_UART_BE))
+		return -EPERM;
+
+	value = (set & (~clear));
+
+	send_control_msg(port, VIRTIO_CONSOLE_TIOCMSET, value);
+
+	return 0;
+}
+
 /* The operations for console ports. */
 static const struct hv_ops hv_ops = {
 	.get_chars = get_chars,
 	.put_chars = put_chars,
 	.notifier_add = notifier_add_vio,
 	.notifier_del = notifier_del_vio,
 	.notifier_hangup = notifier_del_vio,
+	.hvc_set_termios = virtio_console_set_termios,
+	.tiocmget = virtio_console_tiocmget,
+	.tiocmset = virtio_console_tiocmset,
 };
 
 /*
@@ -1267,6 +1403,8 @@ static int init_port_console(struct port *port)
 	if (early_put_chars)
 		early_put_chars = NULL;
 
+//	mutex_init(&port->port_mutex);
+
 	/* Notify host of port being opened */
 	send_control_msg(port, VIRTIO_CONSOLE_PORT_OPEN, 1);
 
@@ -1417,7 +1555,9 @@ static int add_port(struct ports_device *portdev, u32 id)
 
 	spin_lock_init(&port->inbuf_lock);
 	spin_lock_init(&port->outvq_lock);
-	init_waitqueue_head(&port->waitqueue);
+	spin_lock_init(&port->write_lock);
+
+	init_waitqueue_head(&port->waitqueue);	
 
 	/* We can safely ignore ENOSPC because it means
 	 * the queue already has buffers. Buffers are removed
@@ -1580,6 +1720,25 @@ static void handle_control_message(struct virtio_device *vdev,
 	}
 
 	switch (virtio16_to_cpu(vdev, cpkt->event)) {
+	case VIRTIO_CONSOLE_TIOCMGET:
+		if (!port || !is_console_port(port))
+			break;
+
+		port->tiocm = (int)cpkt->value;
+		port->tiocm_updated = true;
+
+		wake_up_interruptible(&port->waitqueue);
+
+		break;
+	case VIRTIO_CONSOLE_TIOCMGET_FAIL:
+		if (!port || !is_console_port(port))
+			break;
+
+		port->tiocm = -EPERM;
+		port->tiocm_updated = true;
+		wake_up_interruptible(&port->waitqueue);
+
+		break;
 	case VIRTIO_CONSOLE_PORT_ADD:
 		if (port) {
 			dev_dbg(&portdev->vdev->dev,
@@ -2134,6 +2293,7 @@ MODULE_DEVICE_TABLE(virtio, id_table);
 static const unsigned int features[] = {
 	VIRTIO_CONSOLE_F_SIZE,
 	VIRTIO_CONSOLE_F_MULTIPORT,
+	VIRTIO_CONSOLE_F_UART_BE,
 };
 
 static const struct virtio_device_id rproc_serial_id_table[] = {

drivers/tty/hvc/hvc_console.c

@@ -885,6 +885,15 @@ static void hvc_poll_put_char(struct tty_driver *driver, int line, char ch)
 }
 #endif
 
+static void hvc_set_termios(struct tty_struct *tty,
+			     const struct ktermios *old_termios)
+{
+	struct hvc_struct *hp = tty->driver_data;
+
+	if (hp && hp->ops->hvc_set_termios)
+		hp->ops->hvc_set_termios(hp, old_termios);
+}
+
 static const struct tty_operations hvc_ops = {
 	.install = hvc_install,
 	.open = hvc_open,
@@ -895,6 +904,7 @@ static const struct tty_operations hvc_ops = {
 	.unthrottle = hvc_unthrottle,
 	.write_room = hvc_write_room,
 	.chars_in_buffer = hvc_chars_in_buffer,
+	.set_termios = hvc_set_termios,
 	.tiocmget = hvc_tiocmget,
 	.tiocmset = hvc_tiocmset,
 #ifdef CONFIG_CONSOLE_POLL

drivers/tty/hvc/hvc_console.h

@@ -61,6 +61,8 @@ struct hv_ops {
 	void (*notifier_del)(struct hvc_struct *hp, int irq);
 	void (*notifier_hangup)(struct hvc_struct *hp, int irq);
 
+	/* Handler for tty set_termios implementation */
+	void (*hvc_set_termios)(struct hvc_struct *hp, const struct ktermios *old);
 	/* tiocmget/set implementation */
 	int (*tiocmget)(struct hvc_struct *hp);
 	int (*tiocmset)(struct hvc_struct *hp, unsigned int set, unsigned int clear);

include/uapi/linux/virtio_console.h

@@ -40,6 +40,7 @@
 #define VIRTIO_CONSOLE_F_SIZE	0	/* Does host provide console size? */
 #define VIRTIO_CONSOLE_F_MULTIPORT 1	/* Does host provide multiple ports? */
 #define VIRTIO_CONSOLE_F_EMERG_WRITE 2 /* Does host support emergency write? */
+#define VIRTIO_CONSOLE_F_UART_BE 23 /* Emulate some UART features on this console */
 
 #define VIRTIO_CONSOLE_BAD_ID		(~(__u32)0)
 
@@ -73,6 +74,11 @@ struct virtio_console_control {
 #define VIRTIO_CONSOLE_RESIZE		5
 #define VIRTIO_CONSOLE_PORT_OPEN	6
 #define VIRTIO_CONSOLE_PORT_NAME	7
-
+#define	VIRTIO_CONSOLE_SET_TERMIO_OBAUD	1000
+#define	VIRTIO_CONSOLE_SET_TERMIO_IBAUD	1001
+#define	VIRTIO_CONSOLE_SET_TERMIO_CRTSCTS	1002
+#define	VIRTIO_CONSOLE_TIOCMGET		1003
+#define	VIRTIO_CONSOLE_TIOCMGET_FAIL	1004
+#define	VIRTIO_CONSOLE_TIOCMSET		1005
 
 #endif /* _UAPI_LINUX_VIRTIO_CONSOLE_H */