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