1 files changed, 377 insertions, 323 deletions

diff --git a/drivers/char/virtio_console.c b/drivers/char/virtio_console.c
index 196428c2287a..8c99bf1b5e9f 100644
--- a/drivers/char/virtio_console.c
+++ b/drivers/char/virtio_console.c
@@ -33,35 +33,6 @@
 #include <linux/workqueue.h>
 #include "hvc_console.h"
 
-/* Moved here from .h file in order to disable MULTIPORT. */
-#define VIRTIO_CONSOLE_F_MULTIPORT 1    /* Does host provide multiple ports? */
-
-struct virtio_console_multiport_conf {
-        struct virtio_console_config config;
-        /* max. number of ports this device can hold */
-        __u32 max_nr_ports;
-        /* number of ports added so far */
-        __u32 nr_ports;
-} __attribute__((packed));
-
-/*
- * A message that's passed between the Host and the Guest for a
- * particular port.
- */
-struct virtio_console_control {
-        __u32 id;               /* Port number */
-        __u16 event;            /* The kind of control event (see below) */
-        __u16 value;            /* Extra information for the key */
-};
-
-/* Some events for control messages */
-#define VIRTIO_CONSOLE_PORT_READY       0
-#define VIRTIO_CONSOLE_CONSOLE_PORT     1
-#define VIRTIO_CONSOLE_RESIZE           2
-#define VIRTIO_CONSOLE_PORT_OPEN        3
-#define VIRTIO_CONSOLE_PORT_NAME        4
-#define VIRTIO_CONSOLE_PORT_REMOVE      5
-
 /*
  * This is a global struct for storing common data for all the devices
  * this driver handles.
@@ -107,6 +78,9 @@ struct console {
         /* The hvc device associated with this console port */
         struct hvc_struct *hvc;
 
+        /* The size of the console */
+        struct winsize ws;
+
         /*
          * This number identifies the number that we used to register
          * with hvc in hvc_instantiate() and hvc_alloc(); this is the
@@ -139,7 +113,6 @@ struct ports_device {
          * notification
          */
         struct work_struct control_work;
-        struct work_struct config_work;
 
         struct list_head ports;
 
@@ -150,7 +123,7 @@ struct ports_device {
         spinlock_t cvq_lock;
 
         /* The current config space is stored here */
-        struct virtio_console_multiport_conf config;
+        struct virtio_console_config config;
 
         /* The virtio device we're associated with */
         struct virtio_device *vdev;
@@ -189,6 +162,9 @@ struct port {
          */
         spinlock_t inbuf_lock;
 
+        /* Protect the operations on the out_vq. */
+        spinlock_t outvq_lock;
+
         /* The IO vqs for this port */
         struct virtqueue *in_vq, *out_vq;
 
@@ -214,6 +190,8 @@ struct port {
         /* The 'id' to identify the port with the Host */
         u32 id;
 
+        bool outvq_full;
+
         /* Is the host device open */
         bool host_connected;
 
@@ -328,7 +306,7 @@ static void *get_inbuf(struct port *port)
         unsigned int len;
 
         vq = port->in_vq;
-        buf = vq->vq_ops->get_buf(vq, &len);
+        buf = virtqueue_get_buf(vq, &len);
         if (buf) {
                 buf->len = len;
                 buf->offset = 0;
@@ -349,8 +327,8 @@ static int add_inbuf(struct virtqueue *vq, struct port_buffer *buf)
 
         sg_init_one(sg, buf->buf, buf->size);
 
-        ret = vq->vq_ops->add_buf(vq, sg, 0, 1, buf);
-        vq->vq_ops->kick(vq);
+        ret = virtqueue_add_buf(vq, sg, 0, 1, buf);
+        virtqueue_kick(vq);
         return ret;
 }
 
@@ -366,7 +344,7 @@ static void discard_port_data(struct port *port)
         if (port->inbuf)
                 buf = port->inbuf;
         else
-                buf = vq->vq_ops->get_buf(vq, &len);
+                buf = virtqueue_get_buf(vq, &len);
 
         ret = 0;
         while (buf) {
@@ -374,7 +352,7 @@ static void discard_port_data(struct port *port)
                         ret++;
                         free_buf(buf);
                 }
-                buf = vq->vq_ops->get_buf(vq, &len);
+                buf = virtqueue_get_buf(vq, &len);
         }
         port->inbuf = NULL;
         if (ret)
@@ -403,57 +381,96 @@ out:
         return ret;
 }
 
-static ssize_t send_control_msg(struct port *port, unsigned int event,
-                                unsigned int value)
+static ssize_t __send_control_msg(struct ports_device *portdev, u32 port_id,
+                                  unsigned int event, unsigned int value)
 {
         struct scatterlist sg[1];
         struct virtio_console_control cpkt;
         struct virtqueue *vq;
         unsigned int len;
 
-        if (!use_multiport(port->portdev))
+        if (!use_multiport(portdev))
                 return 0;
 
-        cpkt.id = port->id;
+        cpkt.id = port_id;
         cpkt.event = event;
         cpkt.value = value;
 
-        vq = port->portdev->c_ovq;
+        vq = portdev->c_ovq;
 
         sg_init_one(sg, &cpkt, sizeof(cpkt));
-        if (vq->vq_ops->add_buf(vq, sg, 1, 0, &cpkt) >= 0) {
-                vq->vq_ops->kick(vq);
-                while (!vq->vq_ops->get_buf(vq, &len))
+        if (virtqueue_add_buf(vq, sg, 1, 0, &cpkt) >= 0) {
+                virtqueue_kick(vq);
+                while (!virtqueue_get_buf(vq, &len))
                         cpu_relax();
         }
         return 0;
 }
 
-static ssize_t send_buf(struct port *port, void *in_buf, size_t in_count)
+static ssize_t send_control_msg(struct port *port, unsigned int event,
+                                unsigned int value)
+{
+        return __send_control_msg(port->portdev, port->id, event, value);
+}
+
+/* Callers must take the port->outvq_lock */
+static void reclaim_consumed_buffers(struct port *port)
+{
+        void *buf;
+        unsigned int len;
+
+        while ((buf = virtqueue_get_buf(port->out_vq, &len))) {
+                kfree(buf);
+                port->outvq_full = false;
+        }
+}
+
+static ssize_t send_buf(struct port *port, void *in_buf, size_t in_count,
+                        bool nonblock)
 {
         struct scatterlist sg[1];
         struct virtqueue *out_vq;
         ssize_t ret;
+        unsigned long flags;
         unsigned int len;
 
         out_vq = port->out_vq;
 
+        spin_lock_irqsave(&port->outvq_lock, flags);
+
+        reclaim_consumed_buffers(port);
+
         sg_init_one(sg, in_buf, in_count);
-        ret = out_vq->vq_ops->add_buf(out_vq, sg, 1, 0, in_buf);
+        ret = virtqueue_add_buf(out_vq, sg, 1, 0, in_buf);
 
         /* Tell Host to go! */
-        out_vq->vq_ops->kick(out_vq);
+        virtqueue_kick(out_vq);
 
         if (ret < 0) {
                 in_count = 0;
-                goto fail;
+                goto done;
         }
 
-        /* Wait till the host acknowledges it pushed out the data we sent. */
-        while (!out_vq->vq_ops->get_buf(out_vq, &len))
+        if (ret == 0)
+                port->outvq_full = true;
+
+        if (nonblock)
+                goto done;
+
+        /*
+         * Wait till the host acknowledges it pushed out the data we
+         * sent.  This is done for ports in blocking mode or for data
+         * from the hvc_console; the tty operations are performed with
+         * spinlocks held so we can't sleep here.
+         */
+        while (!virtqueue_get_buf(out_vq, &len))
                 cpu_relax();
-fail:
-        /* We're expected to return the amount of data we wrote */
+done:
+        spin_unlock_irqrestore(&port->outvq_lock, flags);
+        /*
+         * We're expected to return the amount of data we wrote -- all
+         * of it
+         */
         return in_count;
 }
 
@@ -503,9 +520,28 @@ static ssize_t fill_readbuf(struct port *port, char *out_buf, size_t out_count,
 }
 
 /* The condition that must be true for polling to end */
-static bool wait_is_over(struct port *port)
+static bool will_read_block(struct port *port)
+{
+        return !port_has_data(port) && port->host_connected;
+}
+
+static bool will_write_block(struct port *port)
 {
-        return port_has_data(port) || !port->host_connected;
+        bool ret;
+
+        if (!port->host_connected)
+                return true;
+
+        spin_lock_irq(&port->outvq_lock);
+        /*
+         * Check if the Host has consumed any buffers since we last
+         * sent data (this is only applicable for nonblocking ports).
+         */
+        reclaim_consumed_buffers(port);
+        ret = port->outvq_full;
+        spin_unlock_irq(&port->outvq_lock);
+
+        return ret;
 }
 
 static ssize_t port_fops_read(struct file *filp, char __user *ubuf,
@@ -528,7 +564,7 @@ static ssize_t port_fops_read(struct file *filp, char __user *ubuf,
                         return -EAGAIN;
 
                 ret = wait_event_interruptible(port->waitqueue,
-                                               wait_is_over(port));
+                                               !will_read_block(port));
                 if (ret < 0)
                         return ret;
         }
@@ -554,9 +590,22 @@ static ssize_t port_fops_write(struct file *filp, const char __user *ubuf,
         struct port *port;
         char *buf;
         ssize_t ret;
+        bool nonblock;
 
         port = filp->private_data;
 
+        nonblock = filp->f_flags & O_NONBLOCK;
+
+        if (will_write_block(port)) {
+                if (nonblock)
+                        return -EAGAIN;
+
+                ret = wait_event_interruptible(port->waitqueue,
+                                               !will_write_block(port));
+                if (ret < 0)
+                        return ret;
+        }
+
         count = min((size_t)(32 * 1024), count);
 
         buf = kmalloc(count, GFP_KERNEL);
@@ -569,9 +618,14 @@ static ssize_t port_fops_write(struct file *filp, const char __user *ubuf,
                 goto free_buf;
         }
 
-        ret = send_buf(port, buf, count);
+        ret = send_buf(port, buf, count, nonblock);
+
+        if (nonblock && ret > 0)
+                goto out;
+
 free_buf:
         kfree(buf);
+out:
         return ret;
 }
 
@@ -586,7 +640,7 @@ static unsigned int port_fops_poll(struct file *filp, poll_table *wait)
         ret = 0;
         if (port->inbuf)
                 ret |= POLLIN | POLLRDNORM;
-        if (port->host_connected)
+        if (!will_write_block(port))
                 ret |= POLLOUT;
         if (!port->host_connected)
                 ret |= POLLHUP;
@@ -610,6 +664,10 @@ static int port_fops_release(struct inode *inode, struct file *filp)
 
         spin_unlock_irq(&port->inbuf_lock);
 
+        spin_lock_irq(&port->outvq_lock);
+        reclaim_consumed_buffers(port);
+        spin_unlock_irq(&port->outvq_lock);
+
         return 0;
 }
 
@@ -638,6 +696,15 @@ static int port_fops_open(struct inode *inode, struct file *filp)
         port->guest_connected = true;
         spin_unlock_irq(&port->inbuf_lock);
 
+        spin_lock_irq(&port->outvq_lock);
+        /*
+         * There might be a chance that we missed reclaiming a few
+         * buffers in the window of the port getting previously closed
+         * and opening now.
+         */
+        reclaim_consumed_buffers(port);
+        spin_unlock_irq(&port->outvq_lock);
+
         /* Notify host of port being opened */
         send_control_msg(filp->private_data, VIRTIO_CONSOLE_PORT_OPEN, 1);
 
@@ -676,9 +743,9 @@ static int put_chars(u32 vtermno, const char *buf, int count)
 
         port = find_port_by_vtermno(vtermno);
         if (!port)
-                return 0;
+                return -EPIPE;
 
-        return send_buf(port, (void *)buf, count);
+        return send_buf(port, (void *)buf, count, false);
 }
 
 /*
@@ -692,9 +759,13 @@ static int get_chars(u32 vtermno, char *buf, int count)
 {
         struct port *port;
 
+        /* If we've not set up the port yet, we have no input to give. */
+        if (unlikely(early_put_chars))
+                return 0;
+
         port = find_port_by_vtermno(vtermno);
         if (!port)
-                return 0;
+                return -EPIPE;
 
         /* If we don't have an input queue yet, we can't get input. */
         BUG_ON(!port->in_vq);
@@ -705,22 +776,14 @@ static int get_chars(u32 vtermno, char *buf, int count)
 static void resize_console(struct port *port)
 {
         struct virtio_device *vdev;
-        struct winsize ws;
 
         /* The port could have been hot-unplugged */
-        if (!port)
+        if (!port || !is_console_port(port))
                 return;
 
         vdev = port->portdev->vdev;
-        if (virtio_has_feature(vdev, VIRTIO_CONSOLE_F_SIZE)) {
-                vdev->config->get(vdev,
-                                  offsetof(struct virtio_console_config, cols),
-                                  &ws.ws_col, sizeof(u16));
-                vdev->config->get(vdev,
-                                  offsetof(struct virtio_console_config, rows),
-                                  &ws.ws_row, sizeof(u16));
-                hvc_resize(port->cons.hvc, ws);
-        }
+        if (virtio_has_feature(vdev, VIRTIO_CONSOLE_F_SIZE))
+                hvc_resize(port->cons.hvc, port->cons.ws);
 }
 
 /* We set the configuration at this point, since we now have a tty */
@@ -804,6 +867,13 @@ int init_port_console(struct port *port)
         spin_unlock_irq(&pdrvdata_lock);
         port->guest_connected = true;
 
+        /*
+         * Start using the new console output if this is the first
+         * console to come up.
+         */
+        if (early_put_chars)
+                early_put_chars = NULL;
+
         /* Notify host of port being opened */
         send_control_msg(port, VIRTIO_CONSOLE_PORT_OPEN, 1);
 
@@ -859,6 +929,8 @@ static ssize_t debugfs_read(struct file *filp, char __user *ubuf,
         out_offset += snprintf(buf + out_offset, out_count - out_offset,
                                "host_connected: %d\n", port->host_connected);
         out_offset += snprintf(buf + out_offset, out_count - out_offset,
+                               "outvq_full: %d\n", port->outvq_full);
+        out_offset += snprintf(buf + out_offset, out_count - out_offset,
                                "is_console: %s\n",
                                is_console_port(port) ? "yes" : "no");
         out_offset += snprintf(buf + out_offset, out_count - out_offset,
@@ -875,6 +947,153 @@ static const struct file_operations port_debugfs_ops = {
         .read  = debugfs_read,
 };
 
+static void set_console_size(struct port *port, u16 rows, u16 cols)
+{
+        if (!port || !is_console_port(port))
+                return;
+
+        port->cons.ws.ws_row = rows;
+        port->cons.ws.ws_col = cols;
+}
+
+static unsigned int fill_queue(struct virtqueue *vq, spinlock_t *lock)
+{
+        struct port_buffer *buf;
+        unsigned int nr_added_bufs;
+        int ret;
+
+        nr_added_bufs = 0;
+        do {
+                buf = alloc_buf(PAGE_SIZE);
+                if (!buf)
+                        break;
+
+                spin_lock_irq(lock);
+                ret = add_inbuf(vq, buf);
+                if (ret < 0) {
+                        spin_unlock_irq(lock);
+                        free_buf(buf);
+                        break;
+                }
+                nr_added_bufs++;
+                spin_unlock_irq(lock);
+        } while (ret > 0);
+
+        return nr_added_bufs;
+}
+
+static int add_port(struct ports_device *portdev, u32 id)
+{
+        char debugfs_name[16];
+        struct port *port;
+        struct port_buffer *buf;
+        dev_t devt;
+        unsigned int nr_added_bufs;
+        int err;
+
+        port = kmalloc(sizeof(*port), GFP_KERNEL);
+        if (!port) {
+                err = -ENOMEM;
+                goto fail;
+        }
+
+        port->portdev = portdev;
+        port->id = id;
+
+        port->name = NULL;
+        port->inbuf = NULL;
+        port->cons.hvc = NULL;
+
+        port->cons.ws.ws_row = port->cons.ws.ws_col = 0;
+
+        port->host_connected = port->guest_connected = false;
+
+        port->outvq_full = false;
+
+        port->in_vq = portdev->in_vqs[port->id];
+        port->out_vq = portdev->out_vqs[port->id];
+
+        cdev_init(&port->cdev, &port_fops);
+
+        devt = MKDEV(portdev->chr_major, id);
+        err = cdev_add(&port->cdev, devt, 1);
+        if (err < 0) {
+                dev_err(&port->portdev->vdev->dev,
+                        "Error %d adding cdev for port %u\n", err, id);
+                goto free_port;
+        }
+        port->dev = device_create(pdrvdata.class, &port->portdev->vdev->dev,
+                                  devt, port, "vport%up%u",
+                                  port->portdev->drv_index, id);
+        if (IS_ERR(port->dev)) {
+                err = PTR_ERR(port->dev);
+                dev_err(&port->portdev->vdev->dev,
+                        "Error %d creating device for port %u\n",
+                        err, id);
+                goto free_cdev;
+        }
+
+        spin_lock_init(&port->inbuf_lock);
+        spin_lock_init(&port->outvq_lock);
+        init_waitqueue_head(&port->waitqueue);
+
+        /* Fill the in_vq with buffers so the host can send us data. */
+        nr_added_bufs = fill_queue(port->in_vq, &port->inbuf_lock);
+        if (!nr_added_bufs) {
+                dev_err(port->dev, "Error allocating inbufs\n");
+                err = -ENOMEM;
+                goto free_device;
+        }
+
+        /*
+         * If we're not using multiport support, this has to be a console port
+         */
+        if (!use_multiport(port->portdev)) {
+                err = init_port_console(port);
+                if (err)
+                        goto free_inbufs;
+        }
+
+        spin_lock_irq(&portdev->ports_lock);
+        list_add_tail(&port->list, &port->portdev->ports);
+        spin_unlock_irq(&portdev->ports_lock);
+
+        /*
+         * Tell the Host we're set so that it can send us various
+         * configuration parameters for this port (eg, port name,
+         * caching, whether this is a console port, etc.)
+         */
+        send_control_msg(port, VIRTIO_CONSOLE_PORT_READY, 1);
+
+        if (pdrvdata.debugfs_dir) {
+                /*
+                 * Finally, create the debugfs file that we can use to
+                 * inspect a port's state at any time
+                 */
+                sprintf(debugfs_name, "vport%up%u",
+                        port->portdev->drv_index, id);
+                port->debugfs_file = debugfs_create_file(debugfs_name, 0444,
+                                                         pdrvdata.debugfs_dir,
+                                                         port,
+                                                         &port_debugfs_ops);
+        }
+        return 0;
+
+free_inbufs:
+        while ((buf = virtqueue_detach_unused_buf(port->in_vq)))
+                free_buf(buf);
+free_device:
+        device_destroy(pdrvdata.class, port->dev->devt);
+free_cdev:
+        cdev_del(&port->cdev);
+free_port:
+        kfree(port);
+fail:
+        /* The host might want to notify management sw about port add failure */
+        __send_control_msg(portdev, id, VIRTIO_CONSOLE_PORT_READY, 0);
+        return err;
+}
+
 /* Remove all port-specific data. */
 static int remove_port(struct port *port)
 {
@@ -888,7 +1107,18 @@ static int remove_port(struct port *port)
                 spin_lock_irq(&pdrvdata_lock);
                 list_del(&port->cons.list);
                 spin_unlock_irq(&pdrvdata_lock);
+#if 0
+                /*
+                 * hvc_remove() not called as removing one hvc port
+                 * results in other hvc ports getting frozen.
+                 *
+                 * Once this is resolved in hvc, this functionality
+                 * will be enabled.  Till that is done, the -EPIPE
+                 * return from get_chars() above will help
+                 * hvc_console.c to clean up on ports we remove here.
+                 */
                 hvc_remove(port->cons.hvc);
+#endif
         }
         if (port->guest_connected)
                 send_control_msg(port, VIRTIO_CONSOLE_PORT_OPEN, 0);
@@ -900,8 +1130,10 @@ static int remove_port(struct port *port)
         /* Remove unused data this port might have received. */
         discard_port_data(port);
 
+        reclaim_consumed_buffers(port);
+
         /* Remove buffers we queued up for the Host to send us data in. */
-        while ((buf = port->in_vq->vq_ops->detach_unused_buf(port->in_vq)))
+        while ((buf = virtqueue_detach_unused_buf(port->in_vq)))
                 free_buf(buf);
 
         kfree(port->name);
@@ -924,7 +1156,7 @@ static void handle_control_message(struct ports_device *portdev,
         cpkt = (struct virtio_console_control *)(buf->buf + buf->offset);
 
         port = find_port_by_id(portdev, cpkt->id);
-        if (!port) {
+        if (!port && cpkt->event != VIRTIO_CONSOLE_PORT_ADD) {
                 /* No valid header at start of buffer.  Drop it. */
                 dev_dbg(&portdev->vdev->dev,
                         "Invalid index %u in control packet\n", cpkt->id);
@@ -932,6 +1164,24 @@ static void handle_control_message(struct ports_device *portdev,
         }
 
         switch (cpkt->event) {
+        case VIRTIO_CONSOLE_PORT_ADD:
+                if (port) {
+                        dev_dbg(&portdev->vdev->dev,
+                                "Port %u already added\n", port->id);
+                        send_control_msg(port, VIRTIO_CONSOLE_PORT_READY, 1);
+                        break;
+                }
+                if (cpkt->id >= portdev->config.max_nr_ports) {
+                        dev_warn(&portdev->vdev->dev,
+                                "Request for adding port with out-of-bound id %u, max. supported id: %u\n",
+                                cpkt->id, portdev->config.max_nr_ports - 1);
+                        break;
+                }
+                add_port(portdev, cpkt->id);
+                break;
+        case VIRTIO_CONSOLE_PORT_REMOVE:
+                remove_port(port);
+                break;
         case VIRTIO_CONSOLE_CONSOLE_PORT:
                 if (!cpkt->value)
                         break;
@@ -944,15 +1194,34 @@ static void handle_control_message(struct ports_device *portdev,
                  * have to notify the host first.
                  */
                 break;
-        case VIRTIO_CONSOLE_RESIZE:
+        case VIRTIO_CONSOLE_RESIZE: {
+                struct {
+                        __u16 rows;
+                        __u16 cols;
+                } size;
+
                 if (!is_console_port(port))
                         break;
+
+                memcpy(&size, buf->buf + buf->offset + sizeof(*cpkt),
+                       sizeof(size));
+                set_console_size(port, size.rows, size.cols);
+
                 port->cons.hvc->irq_requested = 1;
                 resize_console(port);
                 break;
+        }
         case VIRTIO_CONSOLE_PORT_OPEN:
                 port->host_connected = cpkt->value;
                 wake_up_interruptible(&port->waitqueue);
+                /*
+                 * If the host port got closed and the host had any
+                 * unconsumed buffers, we'll be able to reclaim them
+                 * now.
+                 */
+                spin_lock_irq(&port->outvq_lock);
+                reclaim_consumed_buffers(port);
+                spin_unlock_irq(&port->outvq_lock);
                 break;
         case VIRTIO_CONSOLE_PORT_NAME:
                 /*
@@ -990,32 +1259,6 @@ static void handle_control_message(struct ports_device *portdev,
                         kobject_uevent(&port->dev->kobj, KOBJ_CHANGE);
                 }
                 break;
-        case VIRTIO_CONSOLE_PORT_REMOVE:
-                /*
-                 * Hot unplug the port.  We don't decrement nr_ports
-                 * since we don't want to deal with extra complexities
-                 * of using the lowest-available port id: We can just
-                 * pick up the nr_ports number as the id and not have
-                 * userspace send it to us.  This helps us in two
-                 * ways:
-                 *
-                 * - We don't need to have a 'port_id' field in the
-                 *   config space when a port is hot-added.  This is a
-                 *   good thing as we might queue up multiple hotplug
-                 *   requests issued in our workqueue.
-                 *
-                 * - Another way to deal with this would have been to
-                 *   use a bitmap of the active ports and select the
-                 *   lowest non-active port from that map.  That
-                 *   bloats the already tight config space and we
-                 *   would end up artificially limiting the
-                 *   max. number of ports to sizeof(bitmap).  Right
-                 *   now we can support 2^32 ports (as the port id is
-                 *   stored in a u32 type).
-                 *
-                 */
-                remove_port(port);
-                break;
         }
 }
 
@@ -1030,7 +1273,7 @@ static void control_work_handler(struct work_struct *work)
         vq = portdev->c_ivq;
 
         spin_lock(&portdev->cvq_lock);
-        while ((buf = vq->vq_ops->get_buf(vq, &len))) {
+        while ((buf = virtqueue_get_buf(vq, &len))) {
                 spin_unlock(&portdev->cvq_lock);
 
                 buf->len = len;
@@ -1092,204 +1335,29 @@ static void config_intr(struct virtio_device *vdev)
         struct ports_device *portdev;
 
         portdev = vdev->priv;
-        if (use_multiport(portdev)) {
-                /* Handle port hot-add */
-                schedule_work(&portdev->config_work);
-        }
-        /*
-         * We'll use this way of resizing only for legacy support.
-         * For newer userspace (VIRTIO_CONSOLE_F_MULTPORT+), use
-         * control messages to indicate console size changes so that
-         * it can be done per-port
-         */
-        resize_console(find_port_by_id(portdev, 0));
-}
-
-static unsigned int fill_queue(struct virtqueue *vq, spinlock_t *lock)
-{
-        struct port_buffer *buf;
-        unsigned int nr_added_bufs;
-        int ret;
-
-        nr_added_bufs = 0;
-        do {
-                buf = alloc_buf(PAGE_SIZE);
-                if (!buf)
-                        break;
-
-                spin_lock_irq(lock);
-                ret = add_inbuf(vq, buf);
-                if (ret < 0) {
-                        spin_unlock_irq(lock);
-                        free_buf(buf);
-                        break;
-                }
-                nr_added_bufs++;
-                spin_unlock_irq(lock);
-        } while (ret > 0);
-
-        return nr_added_bufs;
-}
-
-static int add_port(struct ports_device *portdev, u32 id)
-{
-        char debugfs_name[16];
-        struct port *port;
-        struct port_buffer *buf;
-        dev_t devt;
-        unsigned int nr_added_bufs;
-        int err;
-
-        port = kmalloc(sizeof(*port), GFP_KERNEL);
-        if (!port) {
-                err = -ENOMEM;
-                goto fail;
-        }
-
-        port->portdev = portdev;
-        port->id = id;
-
-        port->name = NULL;
-        port->inbuf = NULL;
-        port->cons.hvc = NULL;
-
-        port->host_connected = port->guest_connected = false;
-
-        port->in_vq = portdev->in_vqs[port->id];
-        port->out_vq = portdev->out_vqs[port->id];
-
-        cdev_init(&port->cdev, &port_fops);
-
-        devt = MKDEV(portdev->chr_major, id);
-        err = cdev_add(&port->cdev, devt, 1);
-        if (err < 0) {
-                dev_err(&port->portdev->vdev->dev,
-                        "Error %d adding cdev for port %u\n", err, id);
-                goto free_port;
-        }
-        port->dev = device_create(pdrvdata.class, &port->portdev->vdev->dev,
-                                  devt, port, "vport%up%u",
-                                  port->portdev->drv_index, id);
-        if (IS_ERR(port->dev)) {
-                err = PTR_ERR(port->dev);
-                dev_err(&port->portdev->vdev->dev,
-                        "Error %d creating device for port %u\n",
-                        err, id);
-                goto free_cdev;
-        }
-
-        spin_lock_init(&port->inbuf_lock);
-        init_waitqueue_head(&port->waitqueue);
-
-        /* Fill the in_vq with buffers so the host can send us data. */
-        nr_added_bufs = fill_queue(port->in_vq, &port->inbuf_lock);
-        if (!nr_added_bufs) {
-                dev_err(port->dev, "Error allocating inbufs\n");
-                err = -ENOMEM;
-                goto free_device;
-        }
-
-        /*
-         * If we're not using multiport support, this has to be a console port
-         */
-        if (!use_multiport(port->portdev)) {
-                err = init_port_console(port);
-                if (err)
-                        goto free_inbufs;
-        }
-
-        spin_lock_irq(&portdev->ports_lock);
-        list_add_tail(&port->list, &port->portdev->ports);
-        spin_unlock_irq(&portdev->ports_lock);
-
-        /*
-         * Tell the Host we're set so that it can send us various
-         * configuration parameters for this port (eg, port name,
-         * caching, whether this is a console port, etc.)
-         */
-        send_control_msg(port, VIRTIO_CONSOLE_PORT_READY, 1);
-
-        if (pdrvdata.debugfs_dir) {
-                /*
-                 * Finally, create the debugfs file that we can use to
-                 * inspect a port's state at any time
-                 */
-                sprintf(debugfs_name, "vport%up%u",
-                        port->portdev->drv_index, id);
-                port->debugfs_file = debugfs_create_file(debugfs_name, 0444,
-                                                         pdrvdata.debugfs_dir,
-                                                         port,
-                                                         &port_debugfs_ops);
-        }
-        return 0;
-
-free_inbufs:
-        while ((buf = port->in_vq->vq_ops->detach_unused_buf(port->in_vq)))
-                free_buf(buf);
-free_device:
-        device_destroy(pdrvdata.class, port->dev->devt);
-free_cdev:
-        cdev_del(&port->cdev);
-free_port:
-        kfree(port);
-fail:
-        return err;
-}
 
-/*
- * The workhandler for config-space updates.
- *
- * This is called when ports are hot-added.
- */
-static void config_work_handler(struct work_struct *work)
-{
-        struct virtio_console_multiport_conf virtconconf;
-        struct ports_device *portdev;
-        struct virtio_device *vdev;
-        int err;
+        if (!use_multiport(portdev)) {
+                struct port *port;
+                u16 rows, cols;
 
-        portdev = container_of(work, struct ports_device, config_work);
+                vdev->config->get(vdev,
+                                  offsetof(struct virtio_console_config, cols),
+                                  &cols, sizeof(u16));
+                vdev->config->get(vdev,
+                                  offsetof(struct virtio_console_config, rows),
+                                  &rows, sizeof(u16));
 
-        vdev = portdev->vdev;
-        vdev->config->get(vdev,
-                          offsetof(struct virtio_console_multiport_conf,
-                                   nr_ports),
-                          &virtconconf.nr_ports,
-                          sizeof(virtconconf.nr_ports));
+                port = find_port_by_id(portdev, 0);
+                set_console_size(port, rows, cols);
 
-        if (portdev->config.nr_ports == virtconconf.nr_ports) {
                 /*
-                 * Port 0 got hot-added.  Since we already did all the
-                 * other initialisation for it, just tell the Host
-                 * that the port is ready if we find the port.  In
-                 * case the port was hot-removed earlier, we call
-                 * add_port to add the port.
+                 * We'll use this way of resizing only for legacy
+                 * support.  For newer userspace
+                 * (VIRTIO_CONSOLE_F_MULTPORT+), use control messages
+                 * to indicate console size changes so that it can be
+                 * done per-port.
                  */
-                struct port *port;
-
-                port = find_port_by_id(portdev, 0);
-                if (!port)
-                        add_port(portdev, 0);
-                else
-                        send_control_msg(port, VIRTIO_CONSOLE_PORT_READY, 1);
-                return;
-        }
-        if (virtconconf.nr_ports > portdev->config.max_nr_ports) {
-                dev_warn(&vdev->dev,
-                         "More ports specified (%u) than allowed (%u)",
-                         portdev->config.nr_ports + 1,
-                         portdev->config.max_nr_ports);
-                return;
-        }
-        if (virtconconf.nr_ports < portdev->config.nr_ports)
-                return;
-
-        /* Hot-add ports */
-        while (virtconconf.nr_ports - portdev->config.nr_ports) {
-                err = add_port(portdev, portdev->config.nr_ports);
-                if (err)
-                        break;
-                portdev->config.nr_ports++;
+                resize_console(port);
         }
 }
 
@@ -1414,7 +1482,6 @@ static const struct file_operations portdev_fops = {
 static int __devinit virtcons_probe(struct virtio_device *vdev)
 {
         struct ports_device *portdev;
-        u32 i;
         int err;
         bool multiport;
 
@@ -1443,37 +1510,19 @@ static int __devinit virtcons_probe(struct virtio_device *vdev)
         }
 
         multiport = false;
-        portdev->config.nr_ports = 1;
         portdev->config.max_nr_ports = 1;
-#if 0 /* Multiport is not quite ready yet --RR */
         if (virtio_has_feature(vdev, VIRTIO_CONSOLE_F_MULTIPORT)) {
                 multiport = true;
                 vdev->features[0] |= 1 << VIRTIO_CONSOLE_F_MULTIPORT;
 
-                vdev->config->get(vdev,
-                                  offsetof(struct virtio_console_multiport_conf,
-                                           nr_ports),
-                                  &portdev->config.nr_ports,
-                                  sizeof(portdev->config.nr_ports));
-                vdev->config->get(vdev,
-                                  offsetof(struct virtio_console_multiport_conf,
-                                           max_nr_ports),
+                vdev->config->get(vdev, offsetof(struct virtio_console_config,
+                                                 max_nr_ports),
                                   &portdev->config.max_nr_ports,
                                   sizeof(portdev->config.max_nr_ports));
-                if (portdev->config.nr_ports > portdev->config.max_nr_ports) {
-                        dev_warn(&vdev->dev,
-                                 "More ports (%u) specified than allowed (%u). Will init %u ports.",
-                                 portdev->config.nr_ports,
-                                 portdev->config.max_nr_ports,
-                                 portdev->config.max_nr_ports);
-
-                        portdev->config.nr_ports = portdev->config.max_nr_ports;
-                }
         }
 
         /* Let the Host know we support multiple ports.*/
         vdev->config->finalize_features(vdev);
-#endif
 
         err = init_vqs(portdev);
         if (err < 0) {
@@ -1489,7 +1538,6 @@ static int __devinit virtcons_probe(struct virtio_device *vdev)
 
                 spin_lock_init(&portdev->cvq_lock);
                 INIT_WORK(&portdev->control_work, &control_work_handler);
-                INIT_WORK(&portdev->config_work, &config_work_handler);
 
                 nr_added_bufs = fill_queue(portdev->c_ivq, &portdev->cvq_lock);
                 if (!nr_added_bufs) {
@@ -1498,16 +1546,22 @@ static int __devinit virtcons_probe(struct virtio_device *vdev)
                         err = -ENOMEM;
                         goto free_vqs;
                 }
+        } else {
+                /*
+                 * For backward compatibility: Create a console port
+                 * if we're running on older host.
+                 */
+                add_port(portdev, 0);
         }
 
-        for (i = 0; i < portdev->config.nr_ports; i++)
-                add_port(portdev, i);
-
-        /* Start using the new console output. */
-        early_put_chars = NULL;
+        __send_control_msg(portdev, VIRTIO_CONSOLE_BAD_ID,
+                           VIRTIO_CONSOLE_DEVICE_READY, 1);
         return 0;
 
 free_vqs:
+        /* The host might want to notify mgmt sw about device add failure */
+        __send_control_msg(portdev, VIRTIO_CONSOLE_BAD_ID,
+                           VIRTIO_CONSOLE_DEVICE_READY, 0);
         vdev->config->del_vqs(vdev);
         kfree(portdev->in_vqs);
         kfree(portdev->out_vqs);
@@ -1529,17 +1583,16 @@ static void virtcons_remove(struct virtio_device *vdev)
         portdev = vdev->priv;
 
         cancel_work_sync(&portdev->control_work);
-        cancel_work_sync(&portdev->config_work);
 
         list_for_each_entry_safe(port, port2, &portdev->ports, list)
                 remove_port(port);
 
         unregister_chrdev(portdev->chr_major, "virtio-portsdev");
 
-        while ((buf = portdev->c_ivq->vq_ops->get_buf(portdev->c_ivq, &len)))
+        while ((buf = virtqueue_get_buf(portdev->c_ivq, &len)))
                 free_buf(buf);
 
-        while ((buf = portdev->c_ivq->vq_ops->detach_unused_buf(portdev->c_ivq)))
+        while ((buf = virtqueue_detach_unused_buf(portdev->c_ivq)))
                 free_buf(buf);
 
         vdev->config->del_vqs(vdev);
@@ -1556,6 +1609,7 @@ static struct virtio_device_id id_table[] = {
 
 static unsigned int features[] = {
         VIRTIO_CONSOLE_F_SIZE,
+        VIRTIO_CONSOLE_F_MULTIPORT,
 };
 
 static struct virtio_driver virtio_console = {