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/device.h>
  21 #include <linux/err.h>
  22 #include <linux/init.h>
  23 #include <linux/list.h>
  24 #include <linux/spinlock.h>
  25 #include <linux/virtio.h>
  26 #include <linux/virtio_console.h>
  27 #include <linux/workqueue.h>
  28 #include "hvc_console.h"
  29
  30 /*
  31  * This is a global struct for storing common data for all the devices
  32  * this driver handles.
  33  *
  34  * Mainly, it has a linked list for all the consoles in one place so
  35  * that callbacks from hvc for get_chars(), put_chars() work properly
  36  * across multiple devices and multiple ports per device.
  37  */
  38 struct ports_driver_data {
  39         /* Used for registering chardevs */
  40         struct class *class;
  41
  42         /* Number of devices this driver is handling */
  43         unsigned int index;
  44
  45         /*
  46          * This is used to keep track of the number of hvc consoles
  47          * spawned by this driver.  This number is given as the first
  48          * argument to hvc_alloc().  To correctly map an initial
  49          * console spawned via hvc_instantiate to the console being
  50          * hooked up via hvc_alloc, we need to pass the same vtermno.
  51          *
  52          * We also just assume the first console being initialised was
  53          * the first one that got used as the initial console.
  54          */
  55         unsigned int next_vtermno;
  56
  57         /* All the console devices handled by this driver */
  58         struct list_head consoles;
  59 };
  60 static struct ports_driver_data pdrvdata;
  61
  62 DEFINE_SPINLOCK(pdrvdata_lock);
  63
  64 /* This struct holds information that's relevant only for console ports */
  65 struct console {
  66         /* We'll place all consoles in a list in the pdrvdata struct */
  67         struct list_head list;
  68
  69         /* The hvc device associated with this console port */
  70         struct hvc_struct *hvc;
  71
  72         /*
  73          * This number identifies the number that we used to register
  74          * with hvc in hvc_instantiate() and hvc_alloc(); this is the
  75          * number passed on by the hvc callbacks to us to
  76          * differentiate between the other console ports handled by
  77          * this driver
  78          */
  79         u32 vtermno;
  80 };
  81
  82 struct port_buffer {
  83         char *buf;
  84
  85         /* size of the buffer in *buf above */
  86         size_t size;
  87
  88         /* used length of the buffer */
  89         size_t len;
  90         /* offset in the buf from which to consume data */
  91         size_t offset;
  92 };
  93
  94 /*
  95  * This is a per-device struct that stores data common to all the
  96  * ports for that device (vdev->priv).
  97  */
  98 struct ports_device {
  99         /*
 100          * Workqueue handlers where we process deferred work after
 101          * notification
 102          */
 103         struct work_struct control_work;
 104
 105         struct list_head ports;
 106
 107         /* To protect the list of ports */
 108         spinlock_t ports_lock;
 109
 110         /* To protect the vq operations for the control channel */
 111         spinlock_t cvq_lock;
 112
 113         /* The current config space is stored here */
 114         struct virtio_console_config config;
 115
 116         /* The virtio device we're associated with */
 117         struct virtio_device *vdev;
 118
 119         /*
 120          * A couple of virtqueues for the control channel: one for
 121          * guest->host transfers, one for host->guest transfers
 122          */
 123         struct virtqueue *c_ivq, *c_ovq;
 124
 125         /* Array of per-port IO virtqueues */
 126         struct virtqueue **in_vqs, **out_vqs;
 127
 128         /* Used for numbering devices for sysfs and debugfs */
 129         unsigned int drv_index;
 130
 131         /* Major number for this device.  Ports will be created as minors. */
 132         int chr_major;
 133 };
 134
 135 /* This struct holds the per-port data */
 136 struct port {
 137         /* Next port in the list, head is in the ports_device */
 138         struct list_head list;
 139
 140         /* Pointer to the parent virtio_console device */
 141         struct ports_device *portdev;
 142
 143         /* The current buffer from which data has to be fed to readers */
 144         struct port_buffer *inbuf;
 145
 146         /*
 147          * To protect the operations on the in_vq associated with this
 148          * port.  Has to be a spinlock because it can be called from
 149          * interrupt context (get_char()).
 150          */
 151         spinlock_t inbuf_lock;
 152
 153         /* The IO vqs for this port */
 154         struct virtqueue *in_vq, *out_vq;
 155
 156         /*
 157          * The entries in this struct will be valid if this port is
 158          * hooked up to an hvc console
 159          */
 160         struct console cons;
 161
 162         /* Each port associates with a separate char device */
 163         struct cdev cdev;
 164         struct device *dev;
 165
 166         /* The 'id' to identify the port with the Host */
 167         u32 id;
 168 };
 169
 170 /* This is the very early arch-specified put chars function. */
 171 static int (*early_put_chars)(u32, const char *, int);
 172
 173 static struct port *find_port_by_vtermno(u32 vtermno)
 174 {
 175         struct port *port;
 176         struct console *cons;
 177         unsigned long flags;
 178
 179         spin_lock_irqsave(&pdrvdata_lock, flags);
 180         list_for_each_entry(cons, &pdrvdata.consoles, list) {
 181                 if (cons->vtermno == vtermno) {
 182                         port = container_of(cons, struct port, cons);
 183                         goto out;
 184                 }
 185         }
 186         port = NULL;
 187 out:
 188         spin_unlock_irqrestore(&pdrvdata_lock, flags);
 189         return port;
 190 }
 191
 192 static struct port *find_port_by_id(struct ports_device *portdev, u32 id)
 193 {
 194         struct port *port;
 195         unsigned long flags;
 196
 197         spin_lock_irqsave(&portdev->ports_lock, flags);
 198         list_for_each_entry(port, &portdev->ports, list)
 199                 if (port->id == id)
 200                         goto out;
 201         port = NULL;
 202 out:
 203         spin_unlock_irqrestore(&portdev->ports_lock, flags);
 204
 205         return port;
 206 }
 207
 208 static struct port *find_port_by_vq(struct ports_device *portdev,
 209                                     struct virtqueue *vq)
 210 {
 211         struct port *port;
 212         unsigned long flags;
 213
 214         spin_lock_irqsave(&portdev->ports_lock, flags);
 215         list_for_each_entry(port, &portdev->ports, list)
 216                 if (port->in_vq == vq || port->out_vq == vq)
 217                         goto out;
 218         port = NULL;
 219 out:
 220         spin_unlock_irqrestore(&portdev->ports_lock, flags);
 221         return port;
 222 }
 223
 224 static bool is_console_port(struct port *port)
 225 {
 226         if (port->cons.hvc)
 227                 return true;
 228         return false;
 229 }
 230
 231 static inline bool use_multiport(struct ports_device *portdev)
 232 {
 233         /*
 234          * This condition can be true when put_chars is called from
 235          * early_init
 236          */
 237         if (!portdev->vdev)
 238                 return 0;
 239         return portdev->vdev->features[0] & (1 << VIRTIO_CONSOLE_F_MULTIPORT);
 240 }
 241
 242 static void free_buf(struct port_buffer *buf)
 243 {
 244         kfree(buf->buf);
 245         kfree(buf);
 246 }
 247
 248 static struct port_buffer *alloc_buf(size_t buf_size)
 249 {
 250         struct port_buffer *buf;
 251
 252         buf = kmalloc(sizeof(*buf), GFP_KERNEL);
 253         if (!buf)
 254                 goto fail;
 255         buf->buf = kzalloc(buf_size, GFP_KERNEL);
 256         if (!buf->buf)
 257                 goto free_buf;
 258         buf->len = 0;
 259         buf->offset = 0;
 260         buf->size = buf_size;
 261         return buf;
 262
 263 free_buf:
 264         kfree(buf);
 265 fail:
 266         return NULL;
 267 }
 268
 269 /* Callers should take appropriate locks */
 270 static void *get_inbuf(struct port *port)
 271 {
 272         struct port_buffer *buf;
 273         struct virtqueue *vq;
 274         unsigned int len;
 275
 276         vq = port->in_vq;
 277         buf = vq->vq_ops->get_buf(vq, &len);
 278         if (buf) {
 279                 buf->len = len;
 280                 buf->offset = 0;
 281         }
 282         return buf;
 283 }
 284
 285 /*
 286  * Create a scatter-gather list representing our input buffer and put
 287  * it in the queue.
 288  *
 289  * Callers should take appropriate locks.
 290  */
 291 static int add_inbuf(struct virtqueue *vq, struct port_buffer *buf)
 292 {
 293         struct scatterlist sg[1];
 294         int ret;
 295
 296         sg_init_one(sg, buf->buf, buf->size);
 297
 298         ret = vq->vq_ops->add_buf(vq, sg, 0, 1, buf);
 299         vq->vq_ops->kick(vq);
 300         return ret;
 301 }
 302
 303 static bool port_has_data(struct port *port)
 304 {
 305         unsigned long flags;
 306         bool ret;
 307
 308         ret = false;
 309         spin_lock_irqsave(&port->inbuf_lock, flags);
 310         if (port->inbuf)
 311                 ret = true;
 312         spin_unlock_irqrestore(&port->inbuf_lock, flags);
 313
 314         return ret;
 315 }
 316
 317 static ssize_t send_control_msg(struct port *port, unsigned int event,
 318                                 unsigned int value)
 319 {
 320         struct scatterlist sg[1];
 321         struct virtio_console_control cpkt;
 322         struct virtqueue *vq;
 323         int len;
 324
 325         if (!use_multiport(port->portdev))
 326                 return 0;
 327
 328         cpkt.id = port->id;
 329         cpkt.event = event;
 330         cpkt.value = value;
 331
 332         vq = port->portdev->c_ovq;
 333
 334         sg_init_one(sg, &cpkt, sizeof(cpkt));
 335         if (vq->vq_ops->add_buf(vq, sg, 1, 0, &cpkt) >= 0) {
 336                 vq->vq_ops->kick(vq);
 337                 while (!vq->vq_ops->get_buf(vq, &len))
 338                         cpu_relax();
 339         }
 340         return 0;
 341 }
 342
 343 static ssize_t send_buf(struct port *port, void *in_buf, size_t in_count)
 344 {
 345         struct scatterlist sg[1];
 346         struct virtqueue *out_vq;
 347         ssize_t ret;
 348         unsigned int len;
 349
 350         out_vq = port->out_vq;
 351
 352         sg_init_one(sg, in_buf, in_count);
 353         ret = out_vq->vq_ops->add_buf(out_vq, sg, 1, 0, in_buf);
 354
 355         /* Tell Host to go! */
 356         out_vq->vq_ops->kick(out_vq);
 357
 358         if (ret < 0) {
 359                 len = 0;
 360                 goto fail;
 361         }
 362
 363         /*
 364          * Wait till the host acknowledges it pushed out the data we
 365          * sent. Also ensure we return to userspace the number of
 366          * bytes that were successfully consumed by the host.
 367          */
 368         while (!out_vq->vq_ops->get_buf(out_vq, &len))
 369                 cpu_relax();
 370 fail:
 371         /* We're expected to return the amount of data we wrote */
 372         return len;
 373 }
 374
 375 /*
 376  * Give out the data that's requested from the buffer that we have
 377  * queued up.
 378  */
 379 static ssize_t fill_readbuf(struct port *port, char *out_buf, size_t out_count,
 380                             bool to_user)
 381 {
 382         struct port_buffer *buf;
 383         unsigned long flags;
 384
 385         if (!out_count || !port_has_data(port))
 386                 return 0;
 387
 388         buf = port->inbuf;
 389         out_count = min(out_count, buf->len - buf->offset);
 390
 391         if (to_user) {
 392                 ssize_t ret;
 393
 394                 ret = copy_to_user(out_buf, buf->buf + buf->offset, out_count);
 395                 if (ret)
 396                         return -EFAULT;
 397         } else {
 398                 memcpy(out_buf, buf->buf + buf->offset, out_count);
 399         }
 400
 401         buf->offset += out_count;
 402
 403         if (buf->offset == buf->len) {
 404                 /*
 405                  * We're done using all the data in this buffer.
 406                  * Re-queue so that the Host can send us more data.
 407                  */
 408                 spin_lock_irqsave(&port->inbuf_lock, flags);
 409                 port->inbuf = NULL;
 410
 411                 if (add_inbuf(port->in_vq, buf) < 0)
 412                         dev_warn(port->dev, "failed add_buf\n");
 413
 414                 spin_unlock_irqrestore(&port->inbuf_lock, flags);
 415         }
 416         /* Return the number of bytes actually copied */
 417         return out_count;
 418 }
 419
 420 /*
 421  * The put_chars() callback is pretty straightforward.
 422  *
 423  * We turn the characters into a scatter-gather list, add it to the
 424  * output queue and then kick the Host.  Then we sit here waiting for
 425  * it to finish: inefficient in theory, but in practice
 426  * implementations will do it immediately (lguest's Launcher does).
 427  */
 428 static int put_chars(u32 vtermno, const char *buf, int count)
 429 {
 430         struct port *port;
 431
 432         port = find_port_by_vtermno(vtermno);
 433         if (!port)
 434                 return 0;
 435
 436         if (unlikely(early_put_chars))
 437                 return early_put_chars(vtermno, buf, count);
 438
 439         return send_buf(port, (void *)buf, count);
 440 }
 441
 442 /*
 443  * get_chars() is the callback from the hvc_console infrastructure
 444  * when an interrupt is received.
 445  *
 446  * We call out to fill_readbuf that gets us the required data from the
 447  * buffers that are queued up.
 448  */
 449 static int get_chars(u32 vtermno, char *buf, int count)
 450 {
 451         struct port *port;
 452
 453         port = find_port_by_vtermno(vtermno);
 454         if (!port)
 455                 return 0;
 456
 457         /* If we don't have an input queue yet, we can't get input. */
 458         BUG_ON(!port->in_vq);
 459
 460         return fill_readbuf(port, buf, count, false);
 461 }
 462
 463 static void resize_console(struct port *port)
 464 {
 465         struct virtio_device *vdev;
 466         struct winsize ws;
 467
 468         vdev = port->portdev->vdev;
 469         if (virtio_has_feature(vdev, VIRTIO_CONSOLE_F_SIZE)) {
 470                 vdev->config->get(vdev,
 471                                   offsetof(struct virtio_console_config, cols),
 472                                   &ws.ws_col, sizeof(u16));
 473                 vdev->config->get(vdev,
 474                                   offsetof(struct virtio_console_config, rows),
 475                                   &ws.ws_row, sizeof(u16));
 476                 hvc_resize(port->cons.hvc, ws);
 477         }
 478 }
 479
 480 static void virtcons_apply_config(struct virtio_device *vdev)
 481 {
 482         resize_console(find_port_by_vtermno(0));
 483 }
 484
 485 /* We set the configuration at this point, since we now have a tty */
 486 static int notifier_add_vio(struct hvc_struct *hp, int data)
 487 {
 488         struct port *port;
 489
 490         port = find_port_by_vtermno(hp->vtermno);
 491         if (!port)
 492                 return -EINVAL;
 493
 494         hp->irq_requested = 1;
 495         resize_console(port);
 496
 497         return 0;
 498 }
 499
 500 static void notifier_del_vio(struct hvc_struct *hp, int data)
 501 {
 502         hp->irq_requested = 0;
 503 }
 504
 505 /* The operations for console ports. */
 506 static const struct hv_ops hv_ops = {
 507         .get_chars = get_chars,
 508         .put_chars = put_chars,
 509         .notifier_add = notifier_add_vio,
 510         .notifier_del = notifier_del_vio,
 511         .notifier_hangup = notifier_del_vio,
 512 };
 513
 514 /*
 515  * Console drivers are initialized very early so boot messages can go
 516  * out, so we do things slightly differently from the generic virtio
 517  * initialization of the net and block drivers.
 518  *
 519  * At this stage, the console is output-only.  It's too early to set
 520  * up a virtqueue, so we let the drivers do some boutique early-output
 521  * thing.
 522  */
 523 int __init virtio_cons_early_init(int (*put_chars)(u32, const char *, int))
 524 {
 525         early_put_chars = put_chars;
 526         return hvc_instantiate(0, 0, &hv_ops);
 527 }
 528
 529 int init_port_console(struct port *port)
 530 {
 531         int ret;
 532
 533         /*
 534          * The Host's telling us this port is a console port.  Hook it
 535          * up with an hvc console.
 536          *
 537          * To set up and manage our virtual console, we call
 538          * hvc_alloc().
 539          *
 540          * The first argument of hvc_alloc() is the virtual console
 541          * number.  The second argument is the parameter for the
 542          * notification mechanism (like irq number).  We currently
 543          * leave this as zero, virtqueues have implicit notifications.
 544          *
 545          * The third argument is a "struct hv_ops" containing the
 546          * put_chars() get_chars(), notifier_add() and notifier_del()
 547          * pointers.  The final argument is the output buffer size: we
 548          * can do any size, so we put PAGE_SIZE here.
 549          */
 550         port->cons.vtermno = pdrvdata.next_vtermno;
 551
 552         port->cons.hvc = hvc_alloc(port->cons.vtermno, 0, &hv_ops, PAGE_SIZE);
 553         if (IS_ERR(port->cons.hvc)) {
 554                 ret = PTR_ERR(port->cons.hvc);
 555                 port->cons.hvc = NULL;
 556                 return ret;
 557         }
 558         spin_lock_irq(&pdrvdata_lock);
 559         pdrvdata.next_vtermno++;
 560         list_add_tail(&port->cons.list, &pdrvdata.consoles);
 561         spin_unlock_irq(&pdrvdata_lock);
 562
 563         return 0;
 564 }
 565
 566 /* Any private messages that the Host and Guest want to share */
 567 static void handle_control_message(struct ports_device *portdev,
 568                                    struct port_buffer *buf)
 569 {
 570         struct virtio_console_control *cpkt;
 571         struct port *port;
 572
 573         cpkt = (struct virtio_console_control *)(buf->buf + buf->offset);
 574
 575         port = find_port_by_id(portdev, cpkt->id);
 576         if (!port) {
 577                 /* No valid header at start of buffer.  Drop it. */
 578                 dev_dbg(&portdev->vdev->dev,
 579                         "Invalid index %u in control packet\n", cpkt->id);
 580                 return;
 581         }
 582
 583         switch (cpkt->event) {
 584         case VIRTIO_CONSOLE_CONSOLE_PORT:
 585                 if (!cpkt->value)
 586                         break;
 587                 if (is_console_port(port))
 588                         break;
 589
 590                 init_port_console(port);
 591                 /*
 592                  * Could remove the port here in case init fails - but
 593                  * have to notify the host first.
 594                  */
 595                 break;
 596         case VIRTIO_CONSOLE_RESIZE:
 597                 if (!is_console_port(port))
 598                         break;
 599                 port->cons.hvc->irq_requested = 1;
 600                 resize_console(port);
 601                 break;
 602         }
 603 }
 604
 605 static void control_work_handler(struct work_struct *work)
 606 {
 607         struct ports_device *portdev;
 608         struct virtqueue *vq;
 609         struct port_buffer *buf;
 610         unsigned int len;
 611
 612         portdev = container_of(work, struct ports_device, control_work);
 613         vq = portdev->c_ivq;
 614
 615         spin_lock(&portdev->cvq_lock);
 616         while ((buf = vq->vq_ops->get_buf(vq, &len))) {
 617                 spin_unlock(&portdev->cvq_lock);
 618
 619                 buf->len = len;
 620                 buf->offset = 0;
 621
 622                 handle_control_message(portdev, buf);
 623
 624                 spin_lock(&portdev->cvq_lock);
 625                 if (add_inbuf(portdev->c_ivq, buf) < 0) {
 626                         dev_warn(&portdev->vdev->dev,
 627                                  "Error adding buffer to queue\n");
 628                         free_buf(buf);
 629                 }
 630         }
 631         spin_unlock(&portdev->cvq_lock);
 632 }
 633
 634 static void in_intr(struct virtqueue *vq)
 635 {
 636         struct port *port;
 637         unsigned long flags;
 638
 639         port = find_port_by_vq(vq->vdev->priv, vq);
 640         if (!port)
 641                 return;
 642
 643         spin_lock_irqsave(&port->inbuf_lock, flags);
 644         port->inbuf = get_inbuf(port);
 645
 646         spin_unlock_irqrestore(&port->inbuf_lock, flags);
 647
 648         if (is_console_port(port) && hvc_poll(port->cons.hvc))
 649                 hvc_kick();
 650 }
 651
 652 static void control_intr(struct virtqueue *vq)
 653 {
 654         struct ports_device *portdev;
 655
 656         portdev = vq->vdev->priv;
 657         schedule_work(&portdev->control_work);
 658 }
 659
 660 static void fill_queue(struct virtqueue *vq, spinlock_t *lock)
 661 {
 662         struct port_buffer *buf;
 663         int ret;
 664
 665         do {
 666                 buf = alloc_buf(PAGE_SIZE);
 667                 if (!buf)
 668                         break;
 669
 670                 spin_lock_irq(lock);
 671                 ret = add_inbuf(vq, buf);
 672                 if (ret < 0) {
 673                         spin_unlock_irq(lock);
 674                         free_buf(buf);
 675                         break;
 676                 }
 677                 spin_unlock_irq(lock);
 678         } while (ret > 0);
 679 }
 680
 681 static int add_port(struct ports_device *portdev, u32 id)
 682 {
 683         struct port *port;
 684         struct port_buffer *inbuf;
 685         dev_t devt;
 686         int err;
 687
 688         port = kmalloc(sizeof(*port), GFP_KERNEL);
 689         if (!port) {
 690                 err = -ENOMEM;
 691                 goto fail;
 692         }
 693
 694         port->portdev = portdev;
 695         port->id = id;
 696
 697         port->inbuf = NULL;
 698         port->cons.hvc = NULL;
 699
 700         port->in_vq = portdev->in_vqs[port->id];
 701         port->out_vq = portdev->out_vqs[port->id];
 702
 703         cdev_init(&port->cdev, NULL);
 704
 705         devt = MKDEV(portdev->chr_major, id);
 706         err = cdev_add(&port->cdev, devt, 1);
 707         if (err < 0) {
 708                 dev_err(&port->portdev->vdev->dev,
 709                         "Error %d adding cdev for port %u\n", err, id);
 710                 goto free_port;
 711         }
 712         port->dev = device_create(pdrvdata.class, &port->portdev->vdev->dev,
 713                                   devt, port, "vport%up%u",
 714                                   port->portdev->drv_index, id);
 715         if (IS_ERR(port->dev)) {
 716                 err = PTR_ERR(port->dev);
 717                 dev_err(&port->portdev->vdev->dev,
 718                         "Error %d creating device for port %u\n",
 719                         err, id);
 720                 goto free_cdev;
 721         }
 722
 723         spin_lock_init(&port->inbuf_lock);
 724
 725         inbuf = alloc_buf(PAGE_SIZE);
 726         if (!inbuf) {
 727                 err = -ENOMEM;
 728                 goto free_device;
 729         }
 730
 731         /* Register the input buffer the first time. */
 732         add_inbuf(port->in_vq, inbuf);
 733
 734         /*
 735          * If we're not using multiport support, this has to be a console port
 736          */
 737         if (!use_multiport(port->portdev)) {
 738                 err = init_port_console(port);
 739                 if (err)
 740                         goto free_inbuf;
 741         }
 742
 743         spin_lock_irq(&portdev->ports_lock);
 744         list_add_tail(&port->list, &port->portdev->ports);
 745         spin_unlock_irq(&portdev->ports_lock);
 746
 747         /*
 748          * Tell the Host we're set so that it can send us various
 749          * configuration parameters for this port (eg, port name,
 750          * caching, whether this is a console port, etc.)
 751          */
 752         send_control_msg(port, VIRTIO_CONSOLE_PORT_READY, 1);
 753
 754         return 0;
 755
 756 free_inbuf:
 757         free_buf(inbuf);
 758 free_device:
 759         device_destroy(pdrvdata.class, port->dev->devt);
 760 free_cdev:
 761         cdev_del(&port->cdev);
 762 free_port:
 763         kfree(port);
 764 fail:
 765         return err;
 766 }
 767
 768 static int init_vqs(struct ports_device *portdev)
 769 {
 770         vq_callback_t **io_callbacks;
 771         char **io_names;
 772         struct virtqueue **vqs;
 773         u32 i, j, nr_ports, nr_queues;
 774         int err;
 775
 776         nr_ports = portdev->config.max_nr_ports;
 777         nr_queues = use_multiport(portdev) ? (nr_ports + 1) * 2 : 2;
 778
 779         vqs = kmalloc(nr_queues * sizeof(struct virtqueue *), GFP_KERNEL);
 780         if (!vqs) {
 781                 err = -ENOMEM;
 782                 goto fail;
 783         }
 784         io_callbacks = kmalloc(nr_queues * sizeof(vq_callback_t *), GFP_KERNEL);
 785         if (!io_callbacks) {
 786                 err = -ENOMEM;
 787                 goto free_vqs;
 788         }
 789         io_names = kmalloc(nr_queues * sizeof(char *), GFP_KERNEL);
 790         if (!io_names) {
 791                 err = -ENOMEM;
 792                 goto free_callbacks;
 793         }
 794         portdev->in_vqs = kmalloc(nr_ports * sizeof(struct virtqueue *),
 795                                   GFP_KERNEL);
 796         if (!portdev->in_vqs) {
 797                 err = -ENOMEM;
 798                 goto free_names;
 799         }
 800         portdev->out_vqs = kmalloc(nr_ports * sizeof(struct virtqueue *),
 801                                    GFP_KERNEL);
 802         if (!portdev->out_vqs) {
 803                 err = -ENOMEM;
 804                 goto free_invqs;
 805         }
 806
 807         /*
 808          * For backward compat (newer host but older guest), the host
 809          * spawns a console port first and also inits the vqs for port
 810          * 0 before others.
 811          */
 812         j = 0;
 813         io_callbacks[j] = in_intr;
 814         io_callbacks[j + 1] = NULL;
 815         io_names[j] = "input";
 816         io_names[j + 1] = "output";
 817         j += 2;
 818
 819         if (use_multiport(portdev)) {
 820                 io_callbacks[j] = control_intr;
 821                 io_callbacks[j + 1] = NULL;
 822                 io_names[j] = "control-i";
 823                 io_names[j + 1] = "control-o";
 824
 825                 for (i = 1; i < nr_ports; i++) {
 826                         j += 2;
 827                         io_callbacks[j] = in_intr;
 828                         io_callbacks[j + 1] = NULL;
 829                         io_names[j] = "input";
 830                         io_names[j + 1] = "output";
 831                 }
 832         }
 833         /* Find the queues. */
 834         err = portdev->vdev->config->find_vqs(portdev->vdev, nr_queues, vqs,
 835                                               io_callbacks,
 836                                               (const char **)io_names);
 837         if (err)
 838                 goto free_outvqs;
 839
 840         j = 0;
 841         portdev->in_vqs[0] = vqs[0];
 842         portdev->out_vqs[0] = vqs[1];
 843         j += 2;
 844         if (use_multiport(portdev)) {
 845                 portdev->c_ivq = vqs[j];
 846                 portdev->c_ovq = vqs[j + 1];
 847
 848                 for (i = 1; i < nr_ports; i++) {
 849                         j += 2;
 850                         portdev->in_vqs[i] = vqs[j];
 851                         portdev->out_vqs[i] = vqs[j + 1];
 852                 }
 853         }
 854         kfree(io_callbacks);
 855         kfree(io_names);
 856         kfree(vqs);
 857
 858         return 0;
 859
 860 free_names:
 861         kfree(io_names);
 862 free_callbacks:
 863         kfree(io_callbacks);
 864 free_outvqs:
 865         kfree(portdev->out_vqs);
 866 free_invqs:
 867         kfree(portdev->in_vqs);
 868 free_vqs:
 869         kfree(vqs);
 870 fail:
 871         return err;
 872 }
 873
 874 static const struct file_operations portdev_fops = {
 875         .owner = THIS_MODULE,
 876 };
 877
 878 /*
 879  * Once we're further in boot, we get probed like any other virtio
 880  * device.
 881  *
 882  * If the host also supports multiple console ports, we check the
 883  * config space to see how many ports the host has spawned.  We
 884  * initialize each port found.
 885  */
 886 static int __devinit virtcons_probe(struct virtio_device *vdev)
 887 {
 888         struct ports_device *portdev;
 889         u32 i;
 890         int err;
 891         bool multiport;
 892
 893         portdev = kmalloc(sizeof(*portdev), GFP_KERNEL);
 894         if (!portdev) {
 895                 err = -ENOMEM;
 896                 goto fail;
 897         }
 898
 899         /* Attach this portdev to this virtio_device, and vice-versa. */
 900         portdev->vdev = vdev;
 901         vdev->priv = portdev;
 902
 903         spin_lock_irq(&pdrvdata_lock);
 904         portdev->drv_index = pdrvdata.index++;
 905         spin_unlock_irq(&pdrvdata_lock);
 906
 907         portdev->chr_major = register_chrdev(0, "virtio-portsdev",
 908                                              &portdev_fops);
 909         if (portdev->chr_major < 0) {
 910                 dev_err(&vdev->dev,
 911                         "Error %d registering chrdev for device %u\n",
 912                         portdev->chr_major, portdev->drv_index);
 913                 err = portdev->chr_major;
 914                 goto free;
 915         }
 916
 917         multiport = false;
 918         portdev->config.nr_ports = 1;
 919         portdev->config.max_nr_ports = 1;
 920         if (virtio_has_feature(vdev, VIRTIO_CONSOLE_F_MULTIPORT)) {
 921                 multiport = true;
 922                 vdev->features[0] |= 1 << VIRTIO_CONSOLE_F_MULTIPORT;
 923
 924                 vdev->config->get(vdev, offsetof(struct virtio_console_config,
 925                                                  nr_ports),
 926                                   &portdev->config.nr_ports,
 927                                   sizeof(portdev->config.nr_ports));
 928                 vdev->config->get(vdev, offsetof(struct virtio_console_config,
 929                                                  max_nr_ports),
 930                                   &portdev->config.max_nr_ports,
 931                                   sizeof(portdev->config.max_nr_ports));
 932                 if (portdev->config.nr_ports > portdev->config.max_nr_ports) {
 933                         dev_warn(&vdev->dev,
 934                                  "More ports (%u) specified than allowed (%u). Will init %u ports.",
 935                                  portdev->config.nr_ports,
 936                                  portdev->config.max_nr_ports,
 937                                  portdev->config.max_nr_ports);
 938
 939                         portdev->config.nr_ports = portdev->config.max_nr_ports;
 940                 }
 941         }
 942
 943         /* Let the Host know we support multiple ports.*/
 944         vdev->config->finalize_features(vdev);
 945
 946         err = init_vqs(portdev);
 947         if (err < 0) {
 948                 dev_err(&vdev->dev, "Error %d initializing vqs\n", err);
 949                 goto free_chrdev;
 950         }
 951
 952         spin_lock_init(&portdev->ports_lock);
 953         INIT_LIST_HEAD(&portdev->ports);
 954
 955         if (multiport) {
 956                 spin_lock_init(&portdev->cvq_lock);
 957                 INIT_WORK(&portdev->control_work, &control_work_handler);
 958
 959                 fill_queue(portdev->c_ivq, &portdev->cvq_lock);
 960         }
 961
 962         for (i = 0; i < portdev->config.nr_ports; i++)
 963                 add_port(portdev, i);
 964
 965         /* Start using the new console output. */
 966         early_put_chars = NULL;
 967         return 0;
 968
 969 free_chrdev:
 970         unregister_chrdev(portdev->chr_major, "virtio-portsdev");
 971 free:
 972         kfree(portdev);
 973 fail:
 974         return err;
 975 }
 976
 977 static struct virtio_device_id id_table[] = {
 978         { VIRTIO_ID_CONSOLE, VIRTIO_DEV_ANY_ID },
 979         { 0 },
 980 };
 981
 982 static unsigned int features[] = {
 983         VIRTIO_CONSOLE_F_SIZE,
 984         VIRTIO_CONSOLE_F_MULTIPORT,
 985 };
 986
 987 static struct virtio_driver virtio_console = {
 988         .feature_table = features,
 989         .feature_table_size = ARRAY_SIZE(features),
 990         .driver.name =  KBUILD_MODNAME,
 991         .driver.owner = THIS_MODULE,
 992         .id_table =     id_table,
 993         .probe =        virtcons_probe,
 994         .config_changed = virtcons_apply_config,
 995 };
 996
 997 static int __init init(void)
 998 {
 999         int err;
1000
1001         pdrvdata.class = class_create(THIS_MODULE, "virtio-ports");
1002         if (IS_ERR(pdrvdata.class)) {
1003                 err = PTR_ERR(pdrvdata.class);
1004                 pr_err("Error %d creating virtio-ports class\n", err);
1005                 return err;
1006         }
1007         INIT_LIST_HEAD(&pdrvdata.consoles);
1008
1009         return register_virtio_driver(&virtio_console);
1010 }
1011 module_init(init);
1012
1013 MODULE_DEVICE_TABLE(virtio, id_table);
1014 MODULE_DESCRIPTION("Virtio console driver");
1015 MODULE_LICENSE("GPL");