drivers/char/virtio_console.c

   1 /*
   2  * Copyright (C) 2006, 2007, 2009 Rusty Russell, IBM Corporation
   3  * Copyright (C) 2009, 2010 Red Hat, Inc.
   4  *
   5  * This program is free software; you can redistribute it and/or modify
   6  * it under the terms of the GNU General Public License as published by
   7  * the Free Software Foundation; either version 2 of the License, or
   8  * (at your option) any later version.
   9  *
  10  * This program is distributed in the hope that it will be useful,
  11  * but WITHOUT ANY WARRANTY; without even the implied warranty of
  12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  13  * GNU General Public License for more details.
  14  *
  15  * You should have received a copy of the GNU General Public License
  16  * along with this program; if not, write to the Free Software
  17  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
  18  */
  19 #include <linux/cdev.h>
  20 #include <linux/debugfs.h>
  21 #include <linux/device.h>
  22 #include <linux/err.h>
  23 #include <linux/fs.h>
  24 #include <linux/init.h>
  25 #include <linux/list.h>
  26 #include <linux/poll.h>
  27 #include <linux/sched.h>
  28 #include <linux/slab.h>
  29 #include <linux/spinlock.h>
  30 #include <linux/virtio.h>
  31 #include <linux/virtio_console.h>
  32 #include <linux/wait.h>
  33 #include <linux/workqueue.h>
  34 #include "hvc_console.h"
  35
  36 /* Moved here from .h file in order to disable MULTIPORT. */
  37 #define VIRTIO_CONSOLE_F_MULTIPORT 1    /* Does host provide multiple ports? */
  38
  39 struct virtio_console_multiport_conf {
  40         struct virtio_console_config config;
  41         /* max. number of ports this device can hold */
  42         __u32 max_nr_ports;
  43         /* number of ports added so far */
  44         __u32 nr_ports;
  45 } __attribute__((packed));
  46
  47 /*
  48  * A message that's passed between the Host and the Guest for a
  49  * particular port.
  50  */
  51 struct virtio_console_control {
  52         __u32 id;               /* Port number */
  53         __u16 event;            /* The kind of control event (see below) */
  54         __u16 value;            /* Extra information for the key */
  55 };
  56
  57 /* Some events for control messages */
  58 #define VIRTIO_CONSOLE_PORT_READY       0
  59 #define VIRTIO_CONSOLE_CONSOLE_PORT     1
  60 #define VIRTIO_CONSOLE_RESIZE           2
  61 #define VIRTIO_CONSOLE_PORT_OPEN        3
  62 #define VIRTIO_CONSOLE_PORT_NAME        4
  63 #define VIRTIO_CONSOLE_PORT_REMOVE      5
  64
  65 /*
  66  * This is a global struct for storing common data for all the devices
  67  * this driver handles.
  68  *
  69  * Mainly, it has a linked list for all the consoles in one place so
  70  * that callbacks from hvc for get_chars(), put_chars() work properly
  71  * across multiple devices and multiple ports per device.
  72  */
  73 struct ports_driver_data {
  74         /* Used for registering chardevs */
  75         struct class *class;
  76
  77         /* Used for exporting per-port information to debugfs */
  78         struct dentry *debugfs_dir;
  79
  80         /* Number of devices this driver is handling */
  81         unsigned int index;
  82
  83         /*
  84          * This is used to keep track of the number of hvc consoles
  85          * spawned by this driver.  This number is given as the first
  86          * argument to hvc_alloc().  To correctly map an initial
  87          * console spawned via hvc_instantiate to the console being
  88          * hooked up via hvc_alloc, we need to pass the same vtermno.
  89          *
  90          * We also just assume the first console being initialised was
  91          * the first one that got used as the initial console.
  92          */
  93         unsigned int next_vtermno;
  94
  95         /* All the console devices handled by this driver */
  96         struct list_head consoles;
  97 };
  98 static struct ports_driver_data pdrvdata;
  99
 100 DEFINE_SPINLOCK(pdrvdata_lock);
 101
 102 /* This struct holds information that's relevant only for console ports */
 103 struct console {
 104         /* We'll place all consoles in a list in the pdrvdata struct */
 105         struct list_head list;
 106
 107         /* The hvc device associated with this console port */
 108         struct hvc_struct *hvc;
 109
 110         /*
 111          * This number identifies the number that we used to register
 112          * with hvc in hvc_instantiate() and hvc_alloc(); this is the
 113          * number passed on by the hvc callbacks to us to
 114          * differentiate between the other console ports handled by
 115          * this driver
 116          */
 117         u32 vtermno;
 118 };
 119
 120 struct port_buffer {
 121         char *buf;
 122
 123         /* size of the buffer in *buf above */
 124         size_t size;
 125
 126         /* used length of the buffer */
 127         size_t len;
 128         /* offset in the buf from which to consume data */
 129         size_t offset;
 130 };
 131
 132 /*
 133  * This is a per-device struct that stores data common to all the
 134  * ports for that device (vdev->priv).
 135  */
 136 struct ports_device {
 137         /*
 138          * Workqueue handlers where we process deferred work after
 139          * notification
 140          */
 141         struct work_struct control_work;
 142         struct work_struct config_work;
 143
 144         struct list_head ports;
 145
 146         /* To protect the list of ports */
 147         spinlock_t ports_lock;
 148
 149         /* To protect the vq operations for the control channel */
 150         spinlock_t cvq_lock;
 151
 152         /* The current config space is stored here */
 153         struct virtio_console_multiport_conf config;
 154
 155         /* The virtio device we're associated with */
 156         struct virtio_device *vdev;
 157
 158         /*
 159          * A couple of virtqueues for the control channel: one for
 160          * guest->host transfers, one for host->guest transfers
 161          */
 162         struct virtqueue *c_ivq, *c_ovq;
 163
 164         /* Array of per-port IO virtqueues */
 165         struct virtqueue **in_vqs, **out_vqs;
 166
 167         /* Used for numbering devices for sysfs and debugfs */
 168         unsigned int drv_index;
 169
 170         /* Major number for this device.  Ports will be created as minors. */
 171         int chr_major;
 172 };
 173
 174 /* This struct holds the per-port data */
 175 struct port {
 176         /* Next port in the list, head is in the ports_device */
 177         struct list_head list;
 178
 179         /* Pointer to the parent virtio_console device */
 180         struct ports_device *portdev;
 181
 182         /* The current buffer from which data has to be fed to readers */
 183         struct port_buffer *inbuf;
 184
 185         /*
 186          * To protect the operations on the in_vq associated with this
 187          * port.  Has to be a spinlock because it can be called from
 188          * interrupt context (get_char()).
 189          */
 190         spinlock_t inbuf_lock;
 191
 192         /* The IO vqs for this port */
 193         struct virtqueue *in_vq, *out_vq;
 194
 195         /* File in the debugfs directory that exposes this port's information */
 196         struct dentry *debugfs_file;
 197
 198         /*
 199          * The entries in this struct will be valid if this port is
 200          * hooked up to an hvc console
 201          */
 202         struct console cons;
 203
 204         /* Each port associates with a separate char device */
 205         struct cdev cdev;
 206         struct device *dev;
 207
 208         /* A waitqueue for poll() or blocking read operations */
 209         wait_queue_head_t waitqueue;
 210
 211         /* The 'name' of the port that we expose via sysfs properties */
 212         char *name;
 213
 214         /* The 'id' to identify the port with the Host */
 215         u32 id;
 216
 217         /* Is the host device open */
 218         bool host_connected;
 219
 220         /* We should allow only one process to open a port */
 221         bool guest_connected;
 222 };
 223
 224 /* This is the very early arch-specified put chars function. */
 225 static int (*early_put_chars)(u32, const char *, int);
 226
 227 static struct port *find_port_by_vtermno(u32 vtermno)
 228 {
 229         struct port *port;
 230         struct console *cons;
 231         unsigned long flags;
 232
 233         spin_lock_irqsave(&pdrvdata_lock, flags);
 234         list_for_each_entry(cons, &pdrvdata.consoles, list) {
 235                 if (cons->vtermno == vtermno) {
 236                         port = container_of(cons, struct port, cons);
 237                         goto out;
 238                 }
 239         }
 240         port = NULL;
 241 out:
 242         spin_unlock_irqrestore(&pdrvdata_lock, flags);
 243         return port;
 244 }
 245
 246 static struct port *find_port_by_id(struct ports_device *portdev, u32 id)
 247 {
 248         struct port *port;
 249         unsigned long flags;
 250
 251         spin_lock_irqsave(&portdev->ports_lock, flags);
 252         list_for_each_entry(port, &portdev->ports, list)
 253                 if (port->id == id)
 254                         goto out;
 255         port = NULL;
 256 out:
 257         spin_unlock_irqrestore(&portdev->ports_lock, flags);
 258
 259         return port;
 260 }
 261
 262 static struct port *find_port_by_vq(struct ports_device *portdev,
 263                                     struct virtqueue *vq)
 264 {
 265         struct port *port;
 266         unsigned long flags;
 267
 268         spin_lock_irqsave(&portdev->ports_lock, flags);
 269         list_for_each_entry(port, &portdev->ports, list)
 270                 if (port->in_vq == vq || port->out_vq == vq)
 271                         goto out;
 272         port = NULL;
 273 out:
 274         spin_unlock_irqrestore(&portdev->ports_lock, flags);
 275         return port;
 276 }
 277
 278 static bool is_console_port(struct port *port)
 279 {
 280         if (port->cons.hvc)
 281                 return true;
 282         return false;
 283 }
 284
 285 static inline bool use_multiport(struct ports_device *portdev)
 286 {
 287         /*
 288          * This condition can be true when put_chars is called from
 289          * early_init
 290          */
 291         if (!portdev->vdev)
 292                 return 0;
 293         return portdev->vdev->features[0] & (1 << VIRTIO_CONSOLE_F_MULTIPORT);
 294 }
 295
 296 static void free_buf(struct port_buffer *buf)
 297 {
 298         kfree(buf->buf);
 299         kfree(buf);
 300 }
 301
 302 static struct port_buffer *alloc_buf(size_t buf_size)
 303 {
 304         struct port_buffer *buf;
 305
 306         buf = kmalloc(sizeof(*buf), GFP_KERNEL);
 307         if (!buf)
 308                 goto fail;
 309         buf->buf = kzalloc(buf_size, GFP_KERNEL);
 310         if (!buf->buf)
 311                 goto free_buf;
 312         buf->len = 0;
 313         buf->offset = 0;
 314         buf->size = buf_size;
 315         return buf;
 316
 317 free_buf:
 318         kfree(buf);
 319 fail:
 320         return NULL;
 321 }
 322
 323 /* Callers should take appropriate locks */
 324 static void *get_inbuf(struct port *port)
 325 {
 326         struct port_buffer *buf;
 327         struct virtqueue *vq;
 328         unsigned int len;
 329
 330         vq = port->in_vq;
 331         buf = vq->vq_ops->get_buf(vq, &len);
 332         if (buf) {
 333                 buf->len = len;
 334                 buf->offset = 0;
 335         }
 336         return buf;
 337 }
 338
 339 /*
 340  * Create a scatter-gather list representing our input buffer and put
 341  * it in the queue.
 342  *
 343  * Callers should take appropriate locks.
 344  */
 345 static int add_inbuf(struct virtqueue *vq, struct port_buffer *buf)
 346 {
 347         struct scatterlist sg[1];
 348         int ret;
 349
 350         sg_init_one(sg, buf->buf, buf->size);
 351
 352         ret = vq->vq_ops->add_buf(vq, sg, 0, 1, buf);
 353         vq->vq_ops->kick(vq);
 354         return ret;
 355 }
 356
 357 /* Discard any unread data this port has. Callers lockers. */
 358 static void discard_port_data(struct port *port)
 359 {
 360         struct port_buffer *buf;
 361         struct virtqueue *vq;
 362         unsigned int len;
 363         int ret;
 364
 365         vq = port->in_vq;
 366         if (port->inbuf)
 367                 buf = port->inbuf;
 368         else
 369                 buf = vq->vq_ops->get_buf(vq, &len);
 370
 371         ret = 0;
 372         while (buf) {
 373                 if (add_inbuf(vq, buf) < 0) {
 374                         ret++;
 375                         free_buf(buf);
 376                 }
 377                 buf = vq->vq_ops->get_buf(vq, &len);
 378         }
 379         port->inbuf = NULL;
 380         if (ret)
 381                 dev_warn(port->dev, "Errors adding %d buffers back to vq\n",
 382                          ret);
 383 }
 384
 385 static bool port_has_data(struct port *port)
 386 {
 387         unsigned long flags;
 388         bool ret;
 389
 390         spin_lock_irqsave(&port->inbuf_lock, flags);
 391         if (port->inbuf) {
 392                 ret = true;
 393                 goto out;
 394         }
 395         port->inbuf = get_inbuf(port);
 396         if (port->inbuf) {
 397                 ret = true;
 398                 goto out;
 399         }
 400         ret = false;
 401 out:
 402         spin_unlock_irqrestore(&port->inbuf_lock, flags);
 403         return ret;
 404 }
 405
 406 static ssize_t send_control_msg(struct port *port, unsigned int event,
 407                                 unsigned int value)
 408 {
 409         struct scatterlist sg[1];
 410         struct virtio_console_control cpkt;
 411         struct virtqueue *vq;
 412         unsigned int len;
 413
 414         if (!use_multiport(port->portdev))
 415                 return 0;
 416
 417         cpkt.id = port->id;
 418         cpkt.event = event;
 419         cpkt.value = value;
 420
 421         vq = port->portdev->c_ovq;
 422
 423         sg_init_one(sg, &cpkt, sizeof(cpkt));
 424         if (vq->vq_ops->add_buf(vq, sg, 1, 0, &cpkt) >= 0) {
 425                 vq->vq_ops->kick(vq);
 426                 while (!vq->vq_ops->get_buf(vq, &len))
 427                         cpu_relax();
 428         }
 429         return 0;
 430 }
 431
 432 static ssize_t send_buf(struct port *port, void *in_buf, size_t in_count)
 433 {
 434         struct scatterlist sg[1];
 435         struct virtqueue *out_vq;
 436         ssize_t ret;
 437         unsigned int len;
 438
 439         out_vq = port->out_vq;
 440
 441         sg_init_one(sg, in_buf, in_count);
 442         ret = out_vq->vq_ops->add_buf(out_vq, sg, 1, 0, in_buf);
 443
 444         /* Tell Host to go! */
 445         out_vq->vq_ops->kick(out_vq);
 446
 447         if (ret < 0) {
 448                 in_count = 0;
 449                 goto fail;
 450         }
 451
 452         /* Wait till the host acknowledges it pushed out the data we sent. */
 453         while (!out_vq->vq_ops->get_buf(out_vq, &len))
 454                 cpu_relax();
 455 fail:
 456         /* We're expected to return the amount of data we wrote */
 457         return in_count;
 458 }
 459
 460 /*
 461  * Give out the data that's requested from the buffer that we have
 462  * queued up.
 463  */
 464 static ssize_t fill_readbuf(struct port *port, char *out_buf, size_t out_count,
 465                             bool to_user)
 466 {
 467         struct port_buffer *buf;
 468         unsigned long flags;
 469
 470         if (!out_count || !port_has_data(port))
 471                 return 0;
 472
 473         buf = port->inbuf;
 474         out_count = min(out_count, buf->len - buf->offset);
 475
 476         if (to_user) {
 477                 ssize_t ret;
 478
 479                 ret = copy_to_user(out_buf, buf->buf + buf->offset, out_count);
 480                 if (ret)
 481                         return -EFAULT;
 482         } else {
 483                 memcpy(out_buf, buf->buf + buf->offset, out_count);
 484         }
 485
 486         buf->offset += out_count;
 487
 488         if (buf->offset == buf->len) {
 489                 /*
 490                  * We're done using all the data in this buffer.
 491                  * Re-queue so that the Host can send us more data.
 492                  */
 493                 spin_lock_irqsave(&port->inbuf_lock, flags);
 494                 port->inbuf = NULL;
 495
 496                 if (add_inbuf(port->in_vq, buf) < 0)
 497                         dev_warn(port->dev, "failed add_buf\n");
 498
 499                 spin_unlock_irqrestore(&port->inbuf_lock, flags);
 500         }
 501         /* Return the number of bytes actually copied */
 502         return out_count;
 503 }
 504
 505 /* The condition that must be true for polling to end */
 506 static bool wait_is_over(struct port *port)
 507 {
 508         return port_has_data(port) || !port->host_connected;
 509 }
 510
 511 static ssize_t port_fops_read(struct file *filp, char __user *ubuf,
 512                               size_t count, loff_t *offp)
 513 {
 514         struct port *port;
 515         ssize_t ret;
 516
 517         port = filp->private_data;
 518
 519         if (!port_has_data(port)) {
 520                 /*
 521                  * If nothing's connected on the host just return 0 in
 522                  * case of list_empty; this tells the userspace app
 523                  * that there's no connection
 524                  */
 525                 if (!port->host_connected)
 526                         return 0;
 527                 if (filp->f_flags & O_NONBLOCK)
 528                         return -EAGAIN;
 529
 530                 ret = wait_event_interruptible(port->waitqueue,
 531                                                wait_is_over(port));
 532                 if (ret < 0)
 533                         return ret;
 534         }
 535         /*
 536          * We could've received a disconnection message while we were
 537          * waiting for more data.
 538          *
 539          * This check is not clubbed in the if() statement above as we
 540          * might receive some data as well as the host could get
 541          * disconnected after we got woken up from our wait.  So we
 542          * really want to give off whatever data we have and only then
 543          * check for host_connected.
 544          */
 545         if (!port_has_data(port) && !port->host_connected)
 546                 return 0;
 547
 548         return fill_readbuf(port, ubuf, count, true);
 549 }
 550
 551 static ssize_t port_fops_write(struct file *filp, const char __user *ubuf,
 552                                size_t count, loff_t *offp)
 553 {
 554         struct port *port;
 555         char *buf;
 556         ssize_t ret;
 557
 558         port = filp->private_data;
 559
 560         count = min((size_t)(32 * 1024), count);
 561
 562         buf = kmalloc(count, GFP_KERNEL);
 563         if (!buf)
 564                 return -ENOMEM;
 565
 566         ret = copy_from_user(buf, ubuf, count);
 567         if (ret) {
 568                 ret = -EFAULT;
 569                 goto free_buf;
 570         }
 571
 572         ret = send_buf(port, buf, count);
 573 free_buf:
 574         kfree(buf);
 575         return ret;
 576 }
 577
 578 static unsigned int port_fops_poll(struct file *filp, poll_table *wait)
 579 {
 580         struct port *port;
 581         unsigned int ret;
 582
 583         port = filp->private_data;
 584         poll_wait(filp, &port->waitqueue, wait);
 585
 586         ret = 0;
 587         if (port->inbuf)
 588                 ret |= POLLIN | POLLRDNORM;
 589         if (port->host_connected)
 590                 ret |= POLLOUT;
 591         if (!port->host_connected)
 592                 ret |= POLLHUP;
 593
 594         return ret;
 595 }
 596
 597 static int port_fops_release(struct inode *inode, struct file *filp)
 598 {
 599         struct port *port;
 600
 601         port = filp->private_data;
 602
 603         /* Notify host of port being closed */
 604         send_control_msg(port, VIRTIO_CONSOLE_PORT_OPEN, 0);
 605
 606         spin_lock_irq(&port->inbuf_lock);
 607         port->guest_connected = false;
 608
 609         discard_port_data(port);
 610
 611         spin_unlock_irq(&port->inbuf_lock);
 612
 613         return 0;
 614 }
 615
 616 static int port_fops_open(struct inode *inode, struct file *filp)
 617 {
 618         struct cdev *cdev = inode->i_cdev;
 619         struct port *port;
 620
 621         port = container_of(cdev, struct port, cdev);
 622         filp->private_data = port;
 623
 624         /*
 625          * Don't allow opening of console port devices -- that's done
 626          * via /dev/hvc
 627          */
 628         if (is_console_port(port))
 629                 return -ENXIO;
 630
 631         /* Allow only one process to open a particular port at a time */
 632         spin_lock_irq(&port->inbuf_lock);
 633         if (port->guest_connected) {
 634                 spin_unlock_irq(&port->inbuf_lock);
 635                 return -EMFILE;
 636         }
 637
 638         port->guest_connected = true;
 639         spin_unlock_irq(&port->inbuf_lock);
 640
 641         /* Notify host of port being opened */
 642         send_control_msg(filp->private_data, VIRTIO_CONSOLE_PORT_OPEN, 1);
 643
 644         return 0;
 645 }
 646
 647 /*
 648  * The file operations that we support: programs in the guest can open
 649  * a console device, read from it, write to it, poll for data and
 650  * close it.  The devices are at
 651  *   /dev/vport<device number>p<port number>
 652  */
 653 static const struct file_operations port_fops = {
 654         .owner = THIS_MODULE,
 655         .open  = port_fops_open,
 656         .read  = port_fops_read,
 657         .write = port_fops_write,
 658         .poll  = port_fops_poll,
 659         .release = port_fops_release,
 660 };
 661
 662 /*
 663  * The put_chars() callback is pretty straightforward.
 664  *
 665  * We turn the characters into a scatter-gather list, add it to the
 666  * output queue and then kick the Host.  Then we sit here waiting for
 667  * it to finish: inefficient in theory, but in practice
 668  * implementations will do it immediately (lguest's Launcher does).
 669  */
 670 static int put_chars(u32 vtermno, const char *buf, int count)
 671 {
 672         struct port *port;
 673
 674         if (unlikely(early_put_chars))
 675                 return early_put_chars(vtermno, buf, count);
 676
 677         port = find_port_by_vtermno(vtermno);
 678         if (!port)
 679                 return 0;
 680
 681         return send_buf(port, (void *)buf, count);
 682 }
 683
 684 /*
 685  * get_chars() is the callback from the hvc_console infrastructure
 686  * when an interrupt is received.
 687  *
 688  * We call out to fill_readbuf that gets us the required data from the
 689  * buffers that are queued up.
 690  */
 691 static int get_chars(u32 vtermno, char *buf, int count)
 692 {
 693         struct port *port;
 694
 695         port = find_port_by_vtermno(vtermno);
 696         if (!port)
 697                 return 0;
 698
 699         /* If we don't have an input queue yet, we can't get input. */
 700         BUG_ON(!port->in_vq);
 701
 702         return fill_readbuf(port, buf, count, false);
 703 }
 704
 705 static void resize_console(struct port *port)
 706 {
 707         struct virtio_device *vdev;
 708         struct winsize ws;
 709
 710         /* The port could have been hot-unplugged */
 711         if (!port)
 712                 return;
 713
 714         vdev = port->portdev->vdev;
 715         if (virtio_has_feature(vdev, VIRTIO_CONSOLE_F_SIZE)) {
 716                 vdev->config->get(vdev,
 717                                   offsetof(struct virtio_console_config, cols),
 718                                   &ws.ws_col, sizeof(u16));
 719                 vdev->config->get(vdev,
 720                                   offsetof(struct virtio_console_config, rows),
 721                                   &ws.ws_row, sizeof(u16));
 722                 hvc_resize(port->cons.hvc, ws);
 723         }
 724 }
 725
 726 /* We set the configuration at this point, since we now have a tty */
 727 static int notifier_add_vio(struct hvc_struct *hp, int data)
 728 {
 729         struct port *port;
 730
 731         port = find_port_by_vtermno(hp->vtermno);
 732         if (!port)
 733                 return -EINVAL;
 734
 735         hp->irq_requested = 1;
 736         resize_console(port);
 737
 738         return 0;
 739 }
 740
 741 static void notifier_del_vio(struct hvc_struct *hp, int data)
 742 {
 743         hp->irq_requested = 0;
 744 }
 745
 746 /* The operations for console ports. */
 747 static const struct hv_ops hv_ops = {
 748         .get_chars = get_chars,
 749         .put_chars = put_chars,
 750         .notifier_add = notifier_add_vio,
 751         .notifier_del = notifier_del_vio,
 752         .notifier_hangup = notifier_del_vio,
 753 };
 754
 755 /*
 756  * Console drivers are initialized very early so boot messages can go
 757  * out, so we do things slightly differently from the generic virtio
 758  * initialization of the net and block drivers.
 759  *
 760  * At this stage, the console is output-only.  It's too early to set
 761  * up a virtqueue, so we let the drivers do some boutique early-output
 762  * thing.
 763  */
 764 int __init virtio_cons_early_init(int (*put_chars)(u32, const char *, int))
 765 {
 766         early_put_chars = put_chars;
 767         return hvc_instantiate(0, 0, &hv_ops);
 768 }
 769
 770 int init_port_console(struct port *port)
 771 {
 772         int ret;
 773
 774         /*
 775          * The Host's telling us this port is a console port.  Hook it
 776          * up with an hvc console.
 777          *
 778          * To set up and manage our virtual console, we call
 779          * hvc_alloc().
 780          *
 781          * The first argument of hvc_alloc() is the virtual console
 782          * number.  The second argument is the parameter for the
 783          * notification mechanism (like irq number).  We currently
 784          * leave this as zero, virtqueues have implicit notifications.
 785          *
 786          * The third argument is a "struct hv_ops" containing the
 787          * put_chars() get_chars(), notifier_add() and notifier_del()
 788          * pointers.  The final argument is the output buffer size: we
 789          * can do any size, so we put PAGE_SIZE here.
 790          */
 791         port->cons.vtermno = pdrvdata.next_vtermno;
 792
 793         port->cons.hvc = hvc_alloc(port->cons.vtermno, 0, &hv_ops, PAGE_SIZE);
 794         if (IS_ERR(port->cons.hvc)) {
 795                 ret = PTR_ERR(port->cons.hvc);
 796                 dev_err(port->dev,
 797                         "error %d allocating hvc for port\n", ret);
 798                 port->cons.hvc = NULL;
 799                 return ret;
 800         }
 801         spin_lock_irq(&pdrvdata_lock);
 802         pdrvdata.next_vtermno++;
 803         list_add_tail(&port->cons.list, &pdrvdata.consoles);
 804         spin_unlock_irq(&pdrvdata_lock);
 805         port->guest_connected = true;
 806
 807         /* Notify host of port being opened */
 808         send_control_msg(port, VIRTIO_CONSOLE_PORT_OPEN, 1);
 809
 810         return 0;
 811 }
 812
 813 static ssize_t show_port_name(struct device *dev,
 814                               struct device_attribute *attr, char *buffer)
 815 {
 816         struct port *port;
 817
 818         port = dev_get_drvdata(dev);
 819
 820         return sprintf(buffer, "%s\n", port->name);
 821 }
 822
 823 static DEVICE_ATTR(name, S_IRUGO, show_port_name, NULL);
 824
 825 static struct attribute *port_sysfs_entries[] = {
 826         &dev_attr_name.attr,
 827         NULL
 828 };
 829
 830 static struct attribute_group port_attribute_group = {
 831         .name = NULL,           /* put in device directory */
 832         .attrs = port_sysfs_entries,
 833 };
 834
 835 static int debugfs_open(struct inode *inode, struct file *filp)
 836 {
 837         filp->private_data = inode->i_private;
 838         return 0;
 839 }
 840
 841 static ssize_t debugfs_read(struct file *filp, char __user *ubuf,
 842                             size_t count, loff_t *offp)
 843 {
 844         struct port *port;
 845         char *buf;
 846         ssize_t ret, out_offset, out_count;
 847
 848         out_count = 1024;
 849         buf = kmalloc(out_count, GFP_KERNEL);
 850         if (!buf)
 851                 return -ENOMEM;
 852
 853         port = filp->private_data;
 854         out_offset = 0;
 855         out_offset += snprintf(buf + out_offset, out_count,
 856                                "name: %s\n", port->name ? port->name : "");
 857         out_offset += snprintf(buf + out_offset, out_count - out_offset,
 858                                "guest_connected: %d\n", port->guest_connected);
 859         out_offset += snprintf(buf + out_offset, out_count - out_offset,
 860                                "host_connected: %d\n", port->host_connected);
 861         out_offset += snprintf(buf + out_offset, out_count - out_offset,
 862                                "is_console: %s\n",
 863                                is_console_port(port) ? "yes" : "no");
 864         out_offset += snprintf(buf + out_offset, out_count - out_offset,
 865                                "console_vtermno: %u\n", port->cons.vtermno);
 866
 867         ret = simple_read_from_buffer(ubuf, count, offp, buf, out_offset);
 868         kfree(buf);
 869         return ret;
 870 }
 871
 872 static const struct file_operations port_debugfs_ops = {
 873         .owner = THIS_MODULE,
 874         .open  = debugfs_open,
 875         .read  = debugfs_read,
 876 };
 877
 878 /* Remove all port-specific data. */
 879 static int remove_port(struct port *port)
 880 {
 881         struct port_buffer *buf;
 882
 883         spin_lock_irq(&port->portdev->ports_lock);
 884         list_del(&port->list);
 885         spin_unlock_irq(&port->portdev->ports_lock);
 886
 887         if (is_console_port(port)) {
 888                 spin_lock_irq(&pdrvdata_lock);
 889                 list_del(&port->cons.list);
 890                 spin_unlock_irq(&pdrvdata_lock);
 891                 hvc_remove(port->cons.hvc);
 892         }
 893         if (port->guest_connected)
 894                 send_control_msg(port, VIRTIO_CONSOLE_PORT_OPEN, 0);
 895
 896         sysfs_remove_group(&port->dev->kobj, &port_attribute_group);
 897         device_destroy(pdrvdata.class, port->dev->devt);
 898         cdev_del(&port->cdev);
 899
 900         /* Remove unused data this port might have received. */
 901         discard_port_data(port);
 902
 903         /* Remove buffers we queued up for the Host to send us data in. */
 904         while ((buf = port->in_vq->vq_ops->detach_unused_buf(port->in_vq)))
 905                 free_buf(buf);
 906
 907         kfree(port->name);
 908
 909         debugfs_remove(port->debugfs_file);
 910
 911         kfree(port);
 912         return 0;
 913 }
 914
 915 /* Any private messages that the Host and Guest want to share */
 916 static void handle_control_message(struct ports_device *portdev,
 917                                    struct port_buffer *buf)
 918 {
 919         struct virtio_console_control *cpkt;
 920         struct port *port;
 921         size_t name_size;
 922         int err;
 923
 924         cpkt = (struct virtio_console_control *)(buf->buf + buf->offset);
 925
 926         port = find_port_by_id(portdev, cpkt->id);
 927         if (!port) {
 928                 /* No valid header at start of buffer.  Drop it. */
 929                 dev_dbg(&portdev->vdev->dev,
 930                         "Invalid index %u in control packet\n", cpkt->id);
 931                 return;
 932         }
 933
 934         switch (cpkt->event) {
 935         case VIRTIO_CONSOLE_CONSOLE_PORT:
 936                 if (!cpkt->value)
 937                         break;
 938                 if (is_console_port(port))
 939                         break;
 940
 941                 init_port_console(port);
 942                 /*
 943                  * Could remove the port here in case init fails - but
 944                  * have to notify the host first.
 945                  */
 946                 break;
 947         case VIRTIO_CONSOLE_RESIZE:
 948                 if (!is_console_port(port))
 949                         break;
 950                 port->cons.hvc->irq_requested = 1;
 951                 resize_console(port);
 952                 break;
 953         case VIRTIO_CONSOLE_PORT_OPEN:
 954                 port->host_connected = cpkt->value;
 955                 wake_up_interruptible(&port->waitqueue);
 956                 break;
 957         case VIRTIO_CONSOLE_PORT_NAME:
 958                 /*
 959                  * Skip the size of the header and the cpkt to get the size
 960                  * of the name that was sent
 961                  */
 962                 name_size = buf->len - buf->offset - sizeof(*cpkt) + 1;
 963
 964                 port->name = kmalloc(name_size, GFP_KERNEL);
 965                 if (!port->name) {
 966                         dev_err(port->dev,
 967                                 "Not enough space to store port name\n");
 968                         break;
 969                 }
 970                 strncpy(port->name, buf->buf + buf->offset + sizeof(*cpkt),
 971                         name_size - 1);
 972                 port->name[name_size - 1] = 0;
 973
 974                 /*
 975                  * Since we only have one sysfs attribute, 'name',
 976                  * create it only if we have a name for the port.
 977                  */
 978                 err = sysfs_create_group(&port->dev->kobj,
 979                                          &port_attribute_group);
 980                 if (err) {
 981                         dev_err(port->dev,
 982                                 "Error %d creating sysfs device attributes\n",
 983                                 err);
 984                 } else {
 985                         /*
 986                          * Generate a udev event so that appropriate
 987                          * symlinks can be created based on udev
 988                          * rules.
 989                          */
 990                         kobject_uevent(&port->dev->kobj, KOBJ_CHANGE);
 991                 }
 992                 break;
 993         case VIRTIO_CONSOLE_PORT_REMOVE:
 994                 /*
 995                  * Hot unplug the port.  We don't decrement nr_ports
 996                  * since we don't want to deal with extra complexities
 997                  * of using the lowest-available port id: We can just
 998                  * pick up the nr_ports number as the id and not have
 999                  * userspace send it to us.  This helps us in two
1000                  * ways:
1001                  *
1002                  * - We don't need to have a 'port_id' field in the
1003                  *   config space when a port is hot-added.  This is a
1004                  *   good thing as we might queue up multiple hotplug
1005                  *   requests issued in our workqueue.
1006                  *
1007                  * - Another way to deal with this would have been to
1008                  *   use a bitmap of the active ports and select the
1009                  *   lowest non-active port from that map.  That
1010                  *   bloats the already tight config space and we
1011                  *   would end up artificially limiting the
1012                  *   max. number of ports to sizeof(bitmap).  Right
1013                  *   now we can support 2^32 ports (as the port id is
1014                  *   stored in a u32 type).
1015                  *
1016                  */
1017                 remove_port(port);
1018                 break;
1019         }
1020 }
1021
1022 static void control_work_handler(struct work_struct *work)
1023 {
1024         struct ports_device *portdev;
1025         struct virtqueue *vq;
1026         struct port_buffer *buf;
1027         unsigned int len;
1028
1029         portdev = container_of(work, struct ports_device, control_work);
1030         vq = portdev->c_ivq;
1031
1032         spin_lock(&portdev->cvq_lock);
1033         while ((buf = vq->vq_ops->get_buf(vq, &len))) {
1034                 spin_unlock(&portdev->cvq_lock);
1035
1036                 buf->len = len;
1037                 buf->offset = 0;
1038
1039                 handle_control_message(portdev, buf);
1040
1041                 spin_lock(&portdev->cvq_lock);
1042                 if (add_inbuf(portdev->c_ivq, buf) < 0) {
1043                         dev_warn(&portdev->vdev->dev,
1044                                  "Error adding buffer to queue\n");
1045                         free_buf(buf);
1046                 }
1047         }
1048         spin_unlock(&portdev->cvq_lock);
1049 }
1050
1051 static void in_intr(struct virtqueue *vq)
1052 {
1053         struct port *port;
1054         unsigned long flags;
1055
1056         port = find_port_by_vq(vq->vdev->priv, vq);
1057         if (!port)
1058                 return;
1059
1060         spin_lock_irqsave(&port->inbuf_lock, flags);
1061         if (!port->inbuf)
1062                 port->inbuf = get_inbuf(port);
1063
1064         /*
1065          * Don't queue up data when port is closed.  This condition
1066          * can be reached when a console port is not yet connected (no
1067          * tty is spawned) and the host sends out data to console
1068          * ports.  For generic serial ports, the host won't
1069          * (shouldn't) send data till the guest is connected.
1070          */
1071         if (!port->guest_connected)
1072                 discard_port_data(port);
1073
1074         spin_unlock_irqrestore(&port->inbuf_lock, flags);
1075
1076         wake_up_interruptible(&port->waitqueue);
1077
1078         if (is_console_port(port) && hvc_poll(port->cons.hvc))
1079                 hvc_kick();
1080 }
1081
1082 static void control_intr(struct virtqueue *vq)
1083 {
1084         struct ports_device *portdev;
1085
1086         portdev = vq->vdev->priv;
1087         schedule_work(&portdev->control_work);
1088 }
1089
1090 static void config_intr(struct virtio_device *vdev)
1091 {
1092         struct ports_device *portdev;
1093
1094         portdev = vdev->priv;
1095         if (use_multiport(portdev)) {
1096                 /* Handle port hot-add */
1097                 schedule_work(&portdev->config_work);
1098         }
1099         /*
1100          * We'll use this way of resizing only for legacy support.
1101          * For newer userspace (VIRTIO_CONSOLE_F_MULTPORT+), use
1102          * control messages to indicate console size changes so that
1103          * it can be done per-port
1104          */
1105         resize_console(find_port_by_id(portdev, 0));
1106 }
1107
1108 static unsigned int fill_queue(struct virtqueue *vq, spinlock_t *lock)
1109 {
1110         struct port_buffer *buf;
1111         unsigned int nr_added_bufs;
1112         int ret;
1113
1114         nr_added_bufs = 0;
1115         do {
1116                 buf = alloc_buf(PAGE_SIZE);
1117                 if (!buf)
1118                         break;
1119
1120                 spin_lock_irq(lock);
1121                 ret = add_inbuf(vq, buf);
1122                 if (ret < 0) {
1123                         spin_unlock_irq(lock);
1124                         free_buf(buf);
1125                         break;
1126                 }
1127                 nr_added_bufs++;
1128                 spin_unlock_irq(lock);
1129         } while (ret > 0);
1130
1131         return nr_added_bufs;
1132 }
1133
1134 static int add_port(struct ports_device *portdev, u32 id)
1135 {
1136         char debugfs_name[16];
1137         struct port *port;
1138         struct port_buffer *buf;
1139         dev_t devt;
1140         unsigned int nr_added_bufs;
1141         int err;
1142
1143         port = kmalloc(sizeof(*port), GFP_KERNEL);
1144         if (!port) {
1145                 err = -ENOMEM;
1146                 goto fail;
1147         }
1148
1149         port->portdev = portdev;
1150         port->id = id;
1151
1152         port->name = NULL;
1153         port->inbuf = NULL;
1154         port->cons.hvc = NULL;
1155
1156         port->host_connected = port->guest_connected = false;
1157
1158         port->in_vq = portdev->in_vqs[port->id];
1159         port->out_vq = portdev->out_vqs[port->id];
1160
1161         cdev_init(&port->cdev, &port_fops);
1162
1163         devt = MKDEV(portdev->chr_major, id);
1164         err = cdev_add(&port->cdev, devt, 1);
1165         if (err < 0) {
1166                 dev_err(&port->portdev->vdev->dev,
1167                         "Error %d adding cdev for port %u\n", err, id);
1168                 goto free_port;
1169         }
1170         port->dev = device_create(pdrvdata.class, &port->portdev->vdev->dev,
1171                                   devt, port, "vport%up%u",
1172                                   port->portdev->drv_index, id);
1173         if (IS_ERR(port->dev)) {
1174                 err = PTR_ERR(port->dev);
1175                 dev_err(&port->portdev->vdev->dev,
1176                         "Error %d creating device for port %u\n",
1177                         err, id);
1178                 goto free_cdev;
1179         }
1180
1181         spin_lock_init(&port->inbuf_lock);
1182         init_waitqueue_head(&port->waitqueue);
1183
1184         /* Fill the in_vq with buffers so the host can send us data. */
1185         nr_added_bufs = fill_queue(port->in_vq, &port->inbuf_lock);
1186         if (!nr_added_bufs) {
1187                 dev_err(port->dev, "Error allocating inbufs\n");
1188                 err = -ENOMEM;
1189                 goto free_device;
1190         }
1191
1192         /*
1193          * If we're not using multiport support, this has to be a console port
1194          */
1195         if (!use_multiport(port->portdev)) {
1196                 err = init_port_console(port);
1197                 if (err)
1198                         goto free_inbufs;
1199         }
1200
1201         spin_lock_irq(&portdev->ports_lock);
1202         list_add_tail(&port->list, &port->portdev->ports);
1203         spin_unlock_irq(&portdev->ports_lock);
1204
1205         /*
1206          * Tell the Host we're set so that it can send us various
1207          * configuration parameters for this port (eg, port name,
1208          * caching, whether this is a console port, etc.)
1209          */
1210         send_control_msg(port, VIRTIO_CONSOLE_PORT_READY, 1);
1211
1212         if (pdrvdata.debugfs_dir) {
1213                 /*
1214                  * Finally, create the debugfs file that we can use to
1215                  * inspect a port's state at any time
1216                  */
1217                 sprintf(debugfs_name, "vport%up%u",
1218                         port->portdev->drv_index, id);
1219                 port->debugfs_file = debugfs_create_file(debugfs_name, 0444,
1220                                                          pdrvdata.debugfs_dir,
1221                                                          port,
1222                                                          &port_debugfs_ops);
1223         }
1224         return 0;
1225
1226 free_inbufs:
1227         while ((buf = port->in_vq->vq_ops->detach_unused_buf(port->in_vq)))
1228                 free_buf(buf);
1229 free_device:
1230         device_destroy(pdrvdata.class, port->dev->devt);
1231 free_cdev:
1232         cdev_del(&port->cdev);
1233 free_port:
1234         kfree(port);
1235 fail:
1236         return err;
1237 }
1238
1239 /*
1240  * The workhandler for config-space updates.
1241  *
1242  * This is called when ports are hot-added.
1243  */
1244 static void config_work_handler(struct work_struct *work)
1245 {
1246         struct virtio_console_multiport_conf virtconconf;
1247         struct ports_device *portdev;
1248         struct virtio_device *vdev;
1249         int err;
1250
1251         portdev = container_of(work, struct ports_device, config_work);
1252
1253         vdev = portdev->vdev;
1254         vdev->config->get(vdev,
1255                           offsetof(struct virtio_console_multiport_conf,
1256                                    nr_ports),
1257                           &virtconconf.nr_ports,
1258                           sizeof(virtconconf.nr_ports));
1259
1260         if (portdev->config.nr_ports == virtconconf.nr_ports) {
1261                 /*
1262                  * Port 0 got hot-added.  Since we already did all the
1263                  * other initialisation for it, just tell the Host
1264                  * that the port is ready if we find the port.  In
1265                  * case the port was hot-removed earlier, we call
1266                  * add_port to add the port.
1267                  */
1268                 struct port *port;
1269
1270                 port = find_port_by_id(portdev, 0);
1271                 if (!port)
1272                         add_port(portdev, 0);
1273                 else
1274                         send_control_msg(port, VIRTIO_CONSOLE_PORT_READY, 1);
1275                 return;
1276         }
1277         if (virtconconf.nr_ports > portdev->config.max_nr_ports) {
1278                 dev_warn(&vdev->dev,
1279                          "More ports specified (%u) than allowed (%u)",
1280                          portdev->config.nr_ports + 1,
1281                          portdev->config.max_nr_ports);
1282                 return;
1283         }
1284         if (virtconconf.nr_ports < portdev->config.nr_ports)
1285                 return;
1286
1287         /* Hot-add ports */
1288         while (virtconconf.nr_ports - portdev->config.nr_ports) {
1289                 err = add_port(portdev, portdev->config.nr_ports);
1290                 if (err)
1291                         break;
1292                 portdev->config.nr_ports++;
1293         }
1294 }
1295
1296 static int init_vqs(struct ports_device *portdev)
1297 {
1298         vq_callback_t **io_callbacks;
1299         char **io_names;
1300         struct virtqueue **vqs;
1301         u32 i, j, nr_ports, nr_queues;
1302         int err;
1303
1304         nr_ports = portdev->config.max_nr_ports;
1305         nr_queues = use_multiport(portdev) ? (nr_ports + 1) * 2 : 2;
1306
1307         vqs = kmalloc(nr_queues * sizeof(struct virtqueue *), GFP_KERNEL);
1308         if (!vqs) {
1309                 err = -ENOMEM;
1310                 goto fail;
1311         }
1312         io_callbacks = kmalloc(nr_queues * sizeof(vq_callback_t *), GFP_KERNEL);
1313         if (!io_callbacks) {
1314                 err = -ENOMEM;
1315                 goto free_vqs;
1316         }
1317         io_names = kmalloc(nr_queues * sizeof(char *), GFP_KERNEL);
1318         if (!io_names) {
1319                 err = -ENOMEM;
1320                 goto free_callbacks;
1321         }
1322         portdev->in_vqs = kmalloc(nr_ports * sizeof(struct virtqueue *),
1323                                   GFP_KERNEL);
1324         if (!portdev->in_vqs) {
1325                 err = -ENOMEM;
1326                 goto free_names;
1327         }
1328         portdev->out_vqs = kmalloc(nr_ports * sizeof(struct virtqueue *),
1329                                    GFP_KERNEL);
1330         if (!portdev->out_vqs) {
1331                 err = -ENOMEM;
1332                 goto free_invqs;
1333         }
1334
1335         /*
1336          * For backward compat (newer host but older guest), the host
1337          * spawns a console port first and also inits the vqs for port
1338          * 0 before others.
1339          */
1340         j = 0;
1341         io_callbacks[j] = in_intr;
1342         io_callbacks[j + 1] = NULL;
1343         io_names[j] = "input";
1344         io_names[j + 1] = "output";
1345         j += 2;
1346
1347         if (use_multiport(portdev)) {
1348                 io_callbacks[j] = control_intr;
1349                 io_callbacks[j + 1] = NULL;
1350                 io_names[j] = "control-i";
1351                 io_names[j + 1] = "control-o";
1352
1353                 for (i = 1; i < nr_ports; i++) {
1354                         j += 2;
1355                         io_callbacks[j] = in_intr;
1356                         io_callbacks[j + 1] = NULL;
1357                         io_names[j] = "input";
1358                         io_names[j + 1] = "output";
1359                 }
1360         }
1361         /* Find the queues. */
1362         err = portdev->vdev->config->find_vqs(portdev->vdev, nr_queues, vqs,
1363                                               io_callbacks,
1364                                               (const char **)io_names);
1365         if (err)
1366                 goto free_outvqs;
1367
1368         j = 0;
1369         portdev->in_vqs[0] = vqs[0];
1370         portdev->out_vqs[0] = vqs[1];
1371         j += 2;
1372         if (use_multiport(portdev)) {
1373                 portdev->c_ivq = vqs[j];
1374                 portdev->c_ovq = vqs[j + 1];
1375
1376                 for (i = 1; i < nr_ports; i++) {
1377                         j += 2;
1378                         portdev->in_vqs[i] = vqs[j];
1379                         portdev->out_vqs[i] = vqs[j + 1];
1380                 }
1381         }
1382         kfree(io_callbacks);
1383         kfree(io_names);
1384         kfree(vqs);
1385
1386         return 0;
1387
1388 free_names:
1389         kfree(io_names);
1390 free_callbacks:
1391         kfree(io_callbacks);
1392 free_outvqs:
1393         kfree(portdev->out_vqs);
1394 free_invqs:
1395         kfree(portdev->in_vqs);
1396 free_vqs:
1397         kfree(vqs);
1398 fail:
1399         return err;
1400 }
1401
1402 static const struct file_operations portdev_fops = {
1403         .owner = THIS_MODULE,
1404 };
1405
1406 /*
1407  * Once we're further in boot, we get probed like any other virtio
1408  * device.
1409  *
1410  * If the host also supports multiple console ports, we check the
1411  * config space to see how many ports the host has spawned.  We
1412  * initialize each port found.
1413  */
1414 static int __devinit virtcons_probe(struct virtio_device *vdev)
1415 {
1416         struct ports_device *portdev;
1417         u32 i;
1418         int err;
1419         bool multiport;
1420
1421         portdev = kmalloc(sizeof(*portdev), GFP_KERNEL);
1422         if (!portdev) {
1423                 err = -ENOMEM;
1424                 goto fail;
1425         }
1426
1427         /* Attach this portdev to this virtio_device, and vice-versa. */
1428         portdev->vdev = vdev;
1429         vdev->priv = portdev;
1430
1431         spin_lock_irq(&pdrvdata_lock);
1432         portdev->drv_index = pdrvdata.index++;
1433         spin_unlock_irq(&pdrvdata_lock);
1434
1435         portdev->chr_major = register_chrdev(0, "virtio-portsdev",
1436                                              &portdev_fops);
1437         if (portdev->chr_major < 0) {
1438                 dev_err(&vdev->dev,
1439                         "Error %d registering chrdev for device %u\n",
1440                         portdev->chr_major, portdev->drv_index);
1441                 err = portdev->chr_major;
1442                 goto free;
1443         }
1444
1445         multiport = false;
1446         portdev->config.nr_ports = 1;
1447         portdev->config.max_nr_ports = 1;
1448 #if 0 /* Multiport is not quite ready yet --RR */
1449         if (virtio_has_feature(vdev, VIRTIO_CONSOLE_F_MULTIPORT)) {
1450                 multiport = true;
1451                 vdev->features[0] |= 1 << VIRTIO_CONSOLE_F_MULTIPORT;
1452
1453                 vdev->config->get(vdev,
1454                                   offsetof(struct virtio_console_multiport_conf,
1455                                            nr_ports),
1456                                   &portdev->config.nr_ports,
1457                                   sizeof(portdev->config.nr_ports));
1458                 vdev->config->get(vdev,
1459                                   offsetof(struct virtio_console_multiport_conf,
1460                                            max_nr_ports),
1461                                   &portdev->config.max_nr_ports,
1462                                   sizeof(portdev->config.max_nr_ports));
1463                 if (portdev->config.nr_ports > portdev->config.max_nr_ports) {
1464                         dev_warn(&vdev->dev,
1465                                  "More ports (%u) specified than allowed (%u). Will init %u ports.",
1466                                  portdev->config.nr_ports,
1467                                  portdev->config.max_nr_ports,
1468                                  portdev->config.max_nr_ports);
1469
1470                         portdev->config.nr_ports = portdev->config.max_nr_ports;
1471                 }
1472         }
1473
1474         /* Let the Host know we support multiple ports.*/
1475         vdev->config->finalize_features(vdev);
1476 #endif
1477
1478         err = init_vqs(portdev);
1479         if (err < 0) {
1480                 dev_err(&vdev->dev, "Error %d initializing vqs\n", err);
1481                 goto free_chrdev;
1482         }
1483
1484         spin_lock_init(&portdev->ports_lock);
1485         INIT_LIST_HEAD(&portdev->ports);
1486
1487         if (multiport) {
1488                 unsigned int nr_added_bufs;
1489
1490                 spin_lock_init(&portdev->cvq_lock);
1491                 INIT_WORK(&portdev->control_work, &control_work_handler);
1492                 INIT_WORK(&portdev->config_work, &config_work_handler);
1493
1494                 nr_added_bufs = fill_queue(portdev->c_ivq, &portdev->cvq_lock);
1495                 if (!nr_added_bufs) {
1496                         dev_err(&vdev->dev,
1497                                 "Error allocating buffers for control queue\n");
1498                         err = -ENOMEM;
1499                         goto free_vqs;
1500                 }
1501         }
1502
1503         for (i = 0; i < portdev->config.nr_ports; i++)
1504                 add_port(portdev, i);
1505
1506         /* Start using the new console output. */
1507         early_put_chars = NULL;
1508         return 0;
1509
1510 free_vqs:
1511         vdev->config->del_vqs(vdev);
1512         kfree(portdev->in_vqs);
1513         kfree(portdev->out_vqs);
1514 free_chrdev:
1515         unregister_chrdev(portdev->chr_major, "virtio-portsdev");
1516 free:
1517         kfree(portdev);
1518 fail:
1519         return err;
1520 }
1521
1522 static void virtcons_remove(struct virtio_device *vdev)
1523 {
1524         struct ports_device *portdev;
1525         struct port *port, *port2;
1526         struct port_buffer *buf;
1527         unsigned int len;
1528
1529         portdev = vdev->priv;
1530
1531         cancel_work_sync(&portdev->control_work);
1532         cancel_work_sync(&portdev->config_work);
1533
1534         list_for_each_entry_safe(port, port2, &portdev->ports, list)
1535                 remove_port(port);
1536
1537         unregister_chrdev(portdev->chr_major, "virtio-portsdev");
1538
1539         while ((buf = portdev->c_ivq->vq_ops->get_buf(portdev->c_ivq, &len)))
1540                 free_buf(buf);
1541
1542         while ((buf = portdev->c_ivq->vq_ops->detach_unused_buf(portdev->c_ivq)))
1543                 free_buf(buf);
1544
1545         vdev->config->del_vqs(vdev);
1546         kfree(portdev->in_vqs);
1547         kfree(portdev->out_vqs);
1548
1549         kfree(portdev);
1550 }
1551
1552 static struct virtio_device_id id_table[] = {
1553         { VIRTIO_ID_CONSOLE, VIRTIO_DEV_ANY_ID },
1554         { 0 },
1555 };
1556
1557 static unsigned int features[] = {
1558         VIRTIO_CONSOLE_F_SIZE,
1559 };
1560
1561 static struct virtio_driver virtio_console = {
1562         .feature_table = features,
1563         .feature_table_size = ARRAY_SIZE(features),
1564         .driver.name =  KBUILD_MODNAME,
1565         .driver.owner = THIS_MODULE,
1566         .id_table =     id_table,
1567         .probe =        virtcons_probe,
1568         .remove =       virtcons_remove,
1569         .config_changed = config_intr,
1570 };
1571
1572 static int __init init(void)
1573 {
1574         int err;
1575
1576         pdrvdata.class = class_create(THIS_MODULE, "virtio-ports");
1577         if (IS_ERR(pdrvdata.class)) {
1578                 err = PTR_ERR(pdrvdata.class);
1579                 pr_err("Error %d creating virtio-ports class\n", err);
1580                 return err;
1581         }
1582
1583         pdrvdata.debugfs_dir = debugfs_create_dir("virtio-ports", NULL);
1584         if (!pdrvdata.debugfs_dir) {
1585                 pr_warning("Error %ld creating debugfs dir for virtio-ports\n",
1586                            PTR_ERR(pdrvdata.debugfs_dir));
1587         }
1588         INIT_LIST_HEAD(&pdrvdata.consoles);
1589
1590         return register_virtio_driver(&virtio_console);
1591 }
1592
1593 static void __exit fini(void)
1594 {
1595         unregister_virtio_driver(&virtio_console);
1596
1597         class_destroy(pdrvdata.class);
1598         if (pdrvdata.debugfs_dir)
1599                 debugfs_remove_recursive(pdrvdata.debugfs_dir);
1600 }
1601 module_init(init);
1602 module_exit(fini);
1603
1604 MODULE_DEVICE_TABLE(virtio, id_table);
1605 MODULE_DESCRIPTION("Virtio console driver");
1606 MODULE_LICENSE("GPL");