+/* Any private messages that the Host and Guest want to share */
+static void handle_control_message(struct ports_device *portdev,
+ struct port_buffer *buf)
+{
+ struct virtio_console_control *cpkt;
+ struct port *port;
+
+ cpkt = (struct virtio_console_control *)(buf->buf + buf->offset);
+
+ port = find_port_by_id(portdev, cpkt->id);
+ if (!port) {
+ /* No valid header at start of buffer. Drop it. */
+ dev_dbg(&portdev->vdev->dev,
+ "Invalid index %u in control packet\n", cpkt->id);
+ return;
+ }
+
+ switch (cpkt->event) {
+ case VIRTIO_CONSOLE_CONSOLE_PORT:
+ if (!cpkt->value)
+ break;
+ if (is_console_port(port))
+ break;
+
+ init_port_console(port);
+ /*
+ * Could remove the port here in case init fails - but
+ * have to notify the host first.
+ */
+ break;
+ case VIRTIO_CONSOLE_RESIZE:
+ if (!is_console_port(port))
+ break;
+ port->cons.hvc->irq_requested = 1;
+ resize_console(port);
+ break;
+ }
+}
+
+static void control_work_handler(struct work_struct *work)
+{
+ struct ports_device *portdev;
+ struct virtqueue *vq;
+ struct port_buffer *buf;
+ unsigned int len;
+
+ portdev = container_of(work, struct ports_device, control_work);
+ vq = portdev->c_ivq;
+
+ spin_lock(&portdev->cvq_lock);
+ while ((buf = vq->vq_ops->get_buf(vq, &len))) {
+ spin_unlock(&portdev->cvq_lock);
+
+ buf->len = len;
+ buf->offset = 0;
+
+ handle_control_message(portdev, buf);
+
+ spin_lock(&portdev->cvq_lock);
+ if (add_inbuf(portdev->c_ivq, buf) < 0) {
+ dev_warn(&portdev->vdev->dev,
+ "Error adding buffer to queue\n");
+ free_buf(buf);
+ }
+ }
+ spin_unlock(&portdev->cvq_lock);
+}
+
+static void in_intr(struct virtqueue *vq)
+{
+ struct port *port;
+ unsigned long flags;
+
+ port = find_port_by_vq(vq->vdev->priv, vq);
+ if (!port)
+ return;
+
+ spin_lock_irqsave(&port->inbuf_lock, flags);
+ port->inbuf = get_inbuf(port);
+
+ spin_unlock_irqrestore(&port->inbuf_lock, flags);
+
+ if (is_console_port(port) && hvc_poll(port->cons.hvc))
+ hvc_kick();
+}
+
+static void control_intr(struct virtqueue *vq)
+{
+ struct ports_device *portdev;
+
+ portdev = vq->vdev->priv;
+ schedule_work(&portdev->control_work);
+}
+
+static void fill_queue(struct virtqueue *vq, spinlock_t *lock)
+{
+ struct port_buffer *buf;
+ int ret;
+
+ 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;
+ }
+ spin_unlock_irq(lock);
+ } while (ret > 0);
+}
+
+static int add_port(struct ports_device *portdev, u32 id)