virtio_console.c 40.0 KB
Newer Older
R
Rusty Russell 已提交
1 2
/*
 * Copyright (C) 2006, 2007, 2009 Rusty Russell, IBM Corporation
3
 * Copyright (C) 2009, 2010 Red Hat, Inc.
R
Rusty Russell 已提交
4 5 6 7 8 9 10 11 12 13 14 15 16 17 18
 *
 * This program is free software; you can redistribute it and/or modify
 * it under the terms of the GNU General Public License as published by
 * the Free Software Foundation; either version 2 of the License, or
 * (at your option) any later version.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 *
 * You should have received a copy of the GNU General Public License
 * along with this program; if not, write to the Free Software
 * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 */
19
#include <linux/cdev.h>
20
#include <linux/debugfs.h>
21
#include <linux/device.h>
R
Rusty Russell 已提交
22
#include <linux/err.h>
23
#include <linux/fs.h>
R
Rusty Russell 已提交
24
#include <linux/init.h>
25
#include <linux/list.h>
26 27
#include <linux/poll.h>
#include <linux/sched.h>
28
#include <linux/slab.h>
29
#include <linux/spinlock.h>
R
Rusty Russell 已提交
30 31
#include <linux/virtio.h>
#include <linux/virtio_console.h>
32
#include <linux/wait.h>
33
#include <linux/workqueue.h>
R
Rusty Russell 已提交
34 35
#include "hvc_console.h"

36 37 38 39 40 41 42 43 44
/*
 * This is a global struct for storing common data for all the devices
 * this driver handles.
 *
 * Mainly, it has a linked list for all the consoles in one place so
 * that callbacks from hvc for get_chars(), put_chars() work properly
 * across multiple devices and multiple ports per device.
 */
struct ports_driver_data {
45 46 47
	/* Used for registering chardevs */
	struct class *class;

48 49 50
	/* Used for exporting per-port information to debugfs */
	struct dentry *debugfs_dir;

51 52 53
	/* Number of devices this driver is handling */
	unsigned int index;

54 55 56 57 58 59 60 61 62 63 64 65
	/*
	 * This is used to keep track of the number of hvc consoles
	 * spawned by this driver.  This number is given as the first
	 * argument to hvc_alloc().  To correctly map an initial
	 * console spawned via hvc_instantiate to the console being
	 * hooked up via hvc_alloc, we need to pass the same vtermno.
	 *
	 * We also just assume the first console being initialised was
	 * the first one that got used as the initial console.
	 */
	unsigned int next_vtermno;

66 67 68 69 70 71 72
	/* All the console devices handled by this driver */
	struct list_head consoles;
};
static struct ports_driver_data pdrvdata;

DEFINE_SPINLOCK(pdrvdata_lock);

73 74 75 76 77 78 79 80
/* This struct holds information that's relevant only for console ports */
struct console {
	/* We'll place all consoles in a list in the pdrvdata struct */
	struct list_head list;

	/* The hvc device associated with this console port */
	struct hvc_struct *hvc;

81 82 83
	/* The size of the console */
	struct winsize ws;

84 85 86 87 88 89 90 91 92 93
	/*
	 * This number identifies the number that we used to register
	 * with hvc in hvc_instantiate() and hvc_alloc(); this is the
	 * number passed on by the hvc callbacks to us to
	 * differentiate between the other console ports handled by
	 * this driver
	 */
	u32 vtermno;
};

94 95 96 97 98 99 100 101 102 103 104 105
struct port_buffer {
	char *buf;

	/* size of the buffer in *buf above */
	size_t size;

	/* used length of the buffer */
	size_t len;
	/* offset in the buf from which to consume data */
	size_t offset;
};

106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125
/*
 * This is a per-device struct that stores data common to all the
 * ports for that device (vdev->priv).
 */
struct ports_device {
	/*
	 * Workqueue handlers where we process deferred work after
	 * notification
	 */
	struct work_struct control_work;

	struct list_head ports;

	/* To protect the list of ports */
	spinlock_t ports_lock;

	/* To protect the vq operations for the control channel */
	spinlock_t cvq_lock;

	/* The current config space is stored here */
126
	struct virtio_console_config config;
127 128 129 130 131 132 133 134 135 136 137 138

	/* The virtio device we're associated with */
	struct virtio_device *vdev;

	/*
	 * A couple of virtqueues for the control channel: one for
	 * guest->host transfers, one for host->guest transfers
	 */
	struct virtqueue *c_ivq, *c_ovq;

	/* Array of per-port IO virtqueues */
	struct virtqueue **in_vqs, **out_vqs;
139 140 141 142 143 144

	/* Used for numbering devices for sysfs and debugfs */
	unsigned int drv_index;

	/* Major number for this device.  Ports will be created as minors. */
	int chr_major;
145 146
};

147
/* This struct holds the per-port data */
148
struct port {
149 150 151
	/* Next port in the list, head is in the ports_device */
	struct list_head list;

152 153
	/* Pointer to the parent virtio_console device */
	struct ports_device *portdev;
154 155 156

	/* The current buffer from which data has to be fed to readers */
	struct port_buffer *inbuf;
157

158 159 160 161 162 163 164
	/*
	 * To protect the operations on the in_vq associated with this
	 * port.  Has to be a spinlock because it can be called from
	 * interrupt context (get_char()).
	 */
	spinlock_t inbuf_lock;

165 166 167
	/* Protect the operations on the out_vq. */
	spinlock_t outvq_lock;

168 169 170
	/* The IO vqs for this port */
	struct virtqueue *in_vq, *out_vq;

171 172 173
	/* File in the debugfs directory that exposes this port's information */
	struct dentry *debugfs_file;

174 175 176 177 178
	/*
	 * The entries in this struct will be valid if this port is
	 * hooked up to an hvc console
	 */
	struct console cons;
179

180 181 182 183
	/* Each port associates with a separate char device */
	struct cdev cdev;
	struct device *dev;

184 185 186
	/* A waitqueue for poll() or blocking read operations */
	wait_queue_head_t waitqueue;

187 188 189
	/* The 'name' of the port that we expose via sysfs properties */
	char *name;

190 191
	/* The 'id' to identify the port with the Host */
	u32 id;
192

193 194
	bool outvq_full;

195 196
	/* Is the host device open */
	bool host_connected;
197 198 199

	/* We should allow only one process to open a port */
	bool guest_connected;
200
};
R
Rusty Russell 已提交
201

202 203 204
/* This is the very early arch-specified put chars function. */
static int (*early_put_chars)(u32, const char *, int);

205 206 207
static struct port *find_port_by_vtermno(u32 vtermno)
{
	struct port *port;
208
	struct console *cons;
209 210 211
	unsigned long flags;

	spin_lock_irqsave(&pdrvdata_lock, flags);
212 213 214
	list_for_each_entry(cons, &pdrvdata.consoles, list) {
		if (cons->vtermno == vtermno) {
			port = container_of(cons, struct port, cons);
215
			goto out;
216
		}
217 218 219 220 221 222 223
	}
	port = NULL;
out:
	spin_unlock_irqrestore(&pdrvdata_lock, flags);
	return port;
}

224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239
static struct port *find_port_by_id(struct ports_device *portdev, u32 id)
{
	struct port *port;
	unsigned long flags;

	spin_lock_irqsave(&portdev->ports_lock, flags);
	list_for_each_entry(port, &portdev->ports, list)
		if (port->id == id)
			goto out;
	port = NULL;
out:
	spin_unlock_irqrestore(&portdev->ports_lock, flags);

	return port;
}

240 241 242 243 244 245
static struct port *find_port_by_vq(struct ports_device *portdev,
				    struct virtqueue *vq)
{
	struct port *port;
	unsigned long flags;

246 247
	spin_lock_irqsave(&portdev->ports_lock, flags);
	list_for_each_entry(port, &portdev->ports, list)
248 249 250 251
		if (port->in_vq == vq || port->out_vq == vq)
			goto out;
	port = NULL;
out:
252
	spin_unlock_irqrestore(&portdev->ports_lock, flags);
253 254 255
	return port;
}

256 257 258 259 260 261 262 263 264 265 266 267 268 269 270 271 272 273
static bool is_console_port(struct port *port)
{
	if (port->cons.hvc)
		return true;
	return false;
}

static inline bool use_multiport(struct ports_device *portdev)
{
	/*
	 * This condition can be true when put_chars is called from
	 * early_init
	 */
	if (!portdev->vdev)
		return 0;
	return portdev->vdev->features[0] & (1 << VIRTIO_CONSOLE_F_MULTIPORT);
}

274 275 276 277 278 279 280 281 282 283 284 285 286 287 288 289 290 291 292 293 294 295 296 297 298 299 300
static void free_buf(struct port_buffer *buf)
{
	kfree(buf->buf);
	kfree(buf);
}

static struct port_buffer *alloc_buf(size_t buf_size)
{
	struct port_buffer *buf;

	buf = kmalloc(sizeof(*buf), GFP_KERNEL);
	if (!buf)
		goto fail;
	buf->buf = kzalloc(buf_size, GFP_KERNEL);
	if (!buf->buf)
		goto free_buf;
	buf->len = 0;
	buf->offset = 0;
	buf->size = buf_size;
	return buf;

free_buf:
	kfree(buf);
fail:
	return NULL;
}

301 302 303 304 305 306 307 308
/* Callers should take appropriate locks */
static void *get_inbuf(struct port *port)
{
	struct port_buffer *buf;
	struct virtqueue *vq;
	unsigned int len;

	vq = port->in_vq;
309
	buf = virtqueue_get_buf(vq, &len);
310 311 312 313 314 315 316
	if (buf) {
		buf->len = len;
		buf->offset = 0;
	}
	return buf;
}

317 318 319 320 321 322
/*
 * Create a scatter-gather list representing our input buffer and put
 * it in the queue.
 *
 * Callers should take appropriate locks.
 */
323
static int add_inbuf(struct virtqueue *vq, struct port_buffer *buf)
324 325
{
	struct scatterlist sg[1];
326
	int ret;
327

328 329
	sg_init_one(sg, buf->buf, buf->size);

330 331
	ret = virtqueue_add_buf(vq, sg, 0, 1, buf);
	virtqueue_kick(vq);
332 333 334
	return ret;
}

335 336 337 338 339 340
/* Discard any unread data this port has. Callers lockers. */
static void discard_port_data(struct port *port)
{
	struct port_buffer *buf;
	struct virtqueue *vq;
	unsigned int len;
341
	int ret;
342 343 344 345 346

	vq = port->in_vq;
	if (port->inbuf)
		buf = port->inbuf;
	else
347
		buf = virtqueue_get_buf(vq, &len);
348

349 350 351 352 353 354
	ret = 0;
	while (buf) {
		if (add_inbuf(vq, buf) < 0) {
			ret++;
			free_buf(buf);
		}
355
		buf = virtqueue_get_buf(vq, &len);
356 357
	}
	port->inbuf = NULL;
358 359 360
	if (ret)
		dev_warn(port->dev, "Errors adding %d buffers back to vq\n",
			 ret);
361 362
}

363 364 365 366 367 368
static bool port_has_data(struct port *port)
{
	unsigned long flags;
	bool ret;

	spin_lock_irqsave(&port->inbuf_lock, flags);
369
	if (port->inbuf) {
370
		ret = true;
371 372 373 374 375 376 377 378 379
		goto out;
	}
	port->inbuf = get_inbuf(port);
	if (port->inbuf) {
		ret = true;
		goto out;
	}
	ret = false;
out:
380 381 382 383
	spin_unlock_irqrestore(&port->inbuf_lock, flags);
	return ret;
}

384 385
static ssize_t __send_control_msg(struct ports_device *portdev, u32 port_id,
				  unsigned int event, unsigned int value)
386 387 388 389
{
	struct scatterlist sg[1];
	struct virtio_console_control cpkt;
	struct virtqueue *vq;
390
	unsigned int len;
391

392
	if (!use_multiport(portdev))
393 394
		return 0;

395
	cpkt.id = port_id;
396 397 398
	cpkt.event = event;
	cpkt.value = value;

399
	vq = portdev->c_ovq;
400 401

	sg_init_one(sg, &cpkt, sizeof(cpkt));
402 403 404
	if (virtqueue_add_buf(vq, sg, 1, 0, &cpkt) >= 0) {
		virtqueue_kick(vq);
		while (!virtqueue_get_buf(vq, &len))
405 406 407 408 409
			cpu_relax();
	}
	return 0;
}

410 411 412
static ssize_t send_control_msg(struct port *port, unsigned int event,
				unsigned int value)
{
413 414 415 416
	/* Did the port get unplugged before userspace closed it? */
	if (port->portdev)
		return __send_control_msg(port->portdev, port->id, event, value);
	return 0;
417 418
}

419 420 421 422 423 424 425 426 427 428 429 430 431 432
/* Callers must take the port->outvq_lock */
static void reclaim_consumed_buffers(struct port *port)
{
	void *buf;
	unsigned int len;

	while ((buf = virtqueue_get_buf(port->out_vq, &len))) {
		kfree(buf);
		port->outvq_full = false;
	}
}

static ssize_t send_buf(struct port *port, void *in_buf, size_t in_count,
			bool nonblock)
433 434 435 436
{
	struct scatterlist sg[1];
	struct virtqueue *out_vq;
	ssize_t ret;
437
	unsigned long flags;
438 439 440 441
	unsigned int len;

	out_vq = port->out_vq;

442 443 444 445
	spin_lock_irqsave(&port->outvq_lock, flags);

	reclaim_consumed_buffers(port);

446
	sg_init_one(sg, in_buf, in_count);
447
	ret = virtqueue_add_buf(out_vq, sg, 1, 0, in_buf);
448 449

	/* Tell Host to go! */
450
	virtqueue_kick(out_vq);
451 452

	if (ret < 0) {
453
		in_count = 0;
454
		goto done;
455 456
	}

457 458 459 460 461 462 463 464
	if (ret == 0)
		port->outvq_full = true;

	if (nonblock)
		goto done;

	/*
	 * Wait till the host acknowledges it pushed out the data we
465 466 467 468 469 470
	 * sent.  This is done for data from the hvc_console; the tty
	 * operations are performed with spinlocks held so we can't
	 * sleep here.  An alternative would be to copy the data to a
	 * buffer and relax the spinning requirement.  The downside is
	 * we need to kmalloc a GFP_ATOMIC buffer each time the
	 * console driver writes something out.
471
	 */
472
	while (!virtqueue_get_buf(out_vq, &len))
473
		cpu_relax();
474 475 476 477 478 479
done:
	spin_unlock_irqrestore(&port->outvq_lock, flags);
	/*
	 * We're expected to return the amount of data we wrote -- all
	 * of it
	 */
480
	return in_count;
481 482
}

483 484 485 486
/*
 * Give out the data that's requested from the buffer that we have
 * queued up.
 */
487 488
static ssize_t fill_readbuf(struct port *port, char *out_buf, size_t out_count,
			    bool to_user)
489 490 491 492 493 494 495 496
{
	struct port_buffer *buf;
	unsigned long flags;

	if (!out_count || !port_has_data(port))
		return 0;

	buf = port->inbuf;
497
	out_count = min(out_count, buf->len - buf->offset);
498

499 500 501 502 503 504 505 506 507
	if (to_user) {
		ssize_t ret;

		ret = copy_to_user(out_buf, buf->buf + buf->offset, out_count);
		if (ret)
			return -EFAULT;
	} else {
		memcpy(out_buf, buf->buf + buf->offset, out_count);
	}
508 509 510 511 512 513 514 515 516 517 518 519

	buf->offset += out_count;

	if (buf->offset == buf->len) {
		/*
		 * We're done using all the data in this buffer.
		 * Re-queue so that the Host can send us more data.
		 */
		spin_lock_irqsave(&port->inbuf_lock, flags);
		port->inbuf = NULL;

		if (add_inbuf(port->in_vq, buf) < 0)
520
			dev_warn(port->dev, "failed add_buf\n");
521 522 523

		spin_unlock_irqrestore(&port->inbuf_lock, flags);
	}
524
	/* Return the number of bytes actually copied */
525
	return out_count;
526 527
}

528
/* The condition that must be true for polling to end */
529
static bool will_read_block(struct port *port)
530
{
531 532 533 534
	if (!port->guest_connected) {
		/* Port got hot-unplugged. Let's exit. */
		return false;
	}
535
	return !port_has_data(port) && port->host_connected;
536 537
}

538 539 540 541
static bool will_write_block(struct port *port)
{
	bool ret;

542 543 544 545
	if (!port->guest_connected) {
		/* Port got hot-unplugged. Let's exit. */
		return false;
	}
546 547 548 549 550 551 552 553 554 555 556 557 558 559 560
	if (!port->host_connected)
		return true;

	spin_lock_irq(&port->outvq_lock);
	/*
	 * Check if the Host has consumed any buffers since we last
	 * sent data (this is only applicable for nonblocking ports).
	 */
	reclaim_consumed_buffers(port);
	ret = port->outvq_full;
	spin_unlock_irq(&port->outvq_lock);

	return ret;
}

561 562 563 564 565 566 567 568 569 570 571 572 573 574 575 576 577 578 579 580
static ssize_t port_fops_read(struct file *filp, char __user *ubuf,
			      size_t count, loff_t *offp)
{
	struct port *port;
	ssize_t ret;

	port = filp->private_data;

	if (!port_has_data(port)) {
		/*
		 * If nothing's connected on the host just return 0 in
		 * case of list_empty; this tells the userspace app
		 * that there's no connection
		 */
		if (!port->host_connected)
			return 0;
		if (filp->f_flags & O_NONBLOCK)
			return -EAGAIN;

		ret = wait_event_interruptible(port->waitqueue,
581
					       !will_read_block(port));
582 583 584
		if (ret < 0)
			return ret;
	}
585 586 587
	/* Port got hot-unplugged. */
	if (!port->guest_connected)
		return -ENODEV;
588 589 590 591 592 593 594 595 596 597 598 599 600 601 602 603 604 605 606 607 608 609
	/*
	 * We could've received a disconnection message while we were
	 * waiting for more data.
	 *
	 * This check is not clubbed in the if() statement above as we
	 * might receive some data as well as the host could get
	 * disconnected after we got woken up from our wait.  So we
	 * really want to give off whatever data we have and only then
	 * check for host_connected.
	 */
	if (!port_has_data(port) && !port->host_connected)
		return 0;

	return fill_readbuf(port, ubuf, count, true);
}

static ssize_t port_fops_write(struct file *filp, const char __user *ubuf,
			       size_t count, loff_t *offp)
{
	struct port *port;
	char *buf;
	ssize_t ret;
610
	bool nonblock;
611

612 613 614 615
	/* Userspace could be out to fool us */
	if (!count)
		return 0;

616 617
	port = filp->private_data;

618 619 620 621 622 623 624 625 626 627 628 629
	nonblock = filp->f_flags & O_NONBLOCK;

	if (will_write_block(port)) {
		if (nonblock)
			return -EAGAIN;

		ret = wait_event_interruptible(port->waitqueue,
					       !will_write_block(port));
		if (ret < 0)
			return ret;
	}

630 631 632 633 634 635 636 637 638 639 640 641
	count = min((size_t)(32 * 1024), count);

	buf = kmalloc(count, GFP_KERNEL);
	if (!buf)
		return -ENOMEM;

	ret = copy_from_user(buf, ubuf, count);
	if (ret) {
		ret = -EFAULT;
		goto free_buf;
	}

642 643 644 645 646 647 648 649
	/*
	 * We now ask send_buf() to not spin for generic ports -- we
	 * can re-use the same code path that non-blocking file
	 * descriptors take for blocking file descriptors since the
	 * wait is already done and we're certain the write will go
	 * through to the host.
	 */
	nonblock = true;
650 651 652 653 654
	ret = send_buf(port, buf, count, nonblock);

	if (nonblock && ret > 0)
		goto out;

655 656
free_buf:
	kfree(buf);
657
out:
658 659 660 661 662 663 664 665 666 667 668
	return ret;
}

static unsigned int port_fops_poll(struct file *filp, poll_table *wait)
{
	struct port *port;
	unsigned int ret;

	port = filp->private_data;
	poll_wait(filp, &port->waitqueue, wait);

669 670 671 672
	if (!port->guest_connected) {
		/* Port got unplugged */
		return POLLHUP;
	}
673
	ret = 0;
674
	if (!will_read_block(port))
675
		ret |= POLLIN | POLLRDNORM;
676
	if (!will_write_block(port))
677 678 679 680 681 682 683 684 685 686 687 688 689 690 691 692
		ret |= POLLOUT;
	if (!port->host_connected)
		ret |= POLLHUP;

	return ret;
}

static int port_fops_release(struct inode *inode, struct file *filp)
{
	struct port *port;

	port = filp->private_data;

	/* Notify host of port being closed */
	send_control_msg(port, VIRTIO_CONSOLE_PORT_OPEN, 0);

693
	spin_lock_irq(&port->inbuf_lock);
694 695
	port->guest_connected = false;

696 697 698 699
	discard_port_data(port);

	spin_unlock_irq(&port->inbuf_lock);

700 701 702 703
	spin_lock_irq(&port->outvq_lock);
	reclaim_consumed_buffers(port);
	spin_unlock_irq(&port->outvq_lock);

704 705 706 707 708 709 710 711 712 713 714 715 716 717 718 719 720 721
	return 0;
}

static int port_fops_open(struct inode *inode, struct file *filp)
{
	struct cdev *cdev = inode->i_cdev;
	struct port *port;

	port = container_of(cdev, struct port, cdev);
	filp->private_data = port;

	/*
	 * Don't allow opening of console port devices -- that's done
	 * via /dev/hvc
	 */
	if (is_console_port(port))
		return -ENXIO;

722 723 724 725 726 727 728 729 730 731
	/* Allow only one process to open a particular port at a time */
	spin_lock_irq(&port->inbuf_lock);
	if (port->guest_connected) {
		spin_unlock_irq(&port->inbuf_lock);
		return -EMFILE;
	}

	port->guest_connected = true;
	spin_unlock_irq(&port->inbuf_lock);

732 733 734 735 736 737 738 739 740
	spin_lock_irq(&port->outvq_lock);
	/*
	 * There might be a chance that we missed reclaiming a few
	 * buffers in the window of the port getting previously closed
	 * and opening now.
	 */
	reclaim_consumed_buffers(port);
	spin_unlock_irq(&port->outvq_lock);

741 742 743 744 745 746 747 748 749 750 751 752 753 754 755 756 757 758 759 760 761
	/* Notify host of port being opened */
	send_control_msg(filp->private_data, VIRTIO_CONSOLE_PORT_OPEN, 1);

	return 0;
}

/*
 * The file operations that we support: programs in the guest can open
 * a console device, read from it, write to it, poll for data and
 * close it.  The devices are at
 *   /dev/vport<device number>p<port number>
 */
static const struct file_operations port_fops = {
	.owner = THIS_MODULE,
	.open  = port_fops_open,
	.read  = port_fops_read,
	.write = port_fops_write,
	.poll  = port_fops_poll,
	.release = port_fops_release,
};

R
Rusty Russell 已提交
762 763
/*
 * The put_chars() callback is pretty straightforward.
R
Rusty Russell 已提交
764
 *
R
Rusty Russell 已提交
765 766 767 768 769
 * We turn the characters into a scatter-gather list, add it to the
 * output queue and then kick the Host.  Then we sit here waiting for
 * it to finish: inefficient in theory, but in practice
 * implementations will do it immediately (lguest's Launcher does).
 */
R
Rusty Russell 已提交
770 771
static int put_chars(u32 vtermno, const char *buf, int count)
{
772
	struct port *port;
773

774 775 776
	if (unlikely(early_put_chars))
		return early_put_chars(vtermno, buf, count);

777 778
	port = find_port_by_vtermno(vtermno);
	if (!port)
779
		return -EPIPE;
R
Rusty Russell 已提交
780

781
	return send_buf(port, (void *)buf, count, false);
R
Rusty Russell 已提交
782 783
}

R
Rusty Russell 已提交
784 785 786
/*
 * get_chars() is the callback from the hvc_console infrastructure
 * when an interrupt is received.
R
Rusty Russell 已提交
787
 *
788 789
 * We call out to fill_readbuf that gets us the required data from the
 * buffers that are queued up.
R
Rusty Russell 已提交
790
 */
R
Rusty Russell 已提交
791 792
static int get_chars(u32 vtermno, char *buf, int count)
{
793 794
	struct port *port;

795 796 797 798
	/* If we've not set up the port yet, we have no input to give. */
	if (unlikely(early_put_chars))
		return 0;

799 800
	port = find_port_by_vtermno(vtermno);
	if (!port)
801
		return -EPIPE;
802

R
Rusty Russell 已提交
803
	/* If we don't have an input queue yet, we can't get input. */
804
	BUG_ON(!port->in_vq);
R
Rusty Russell 已提交
805

806
	return fill_readbuf(port, buf, count, false);
R
Rusty Russell 已提交
807 808
}

809
static void resize_console(struct port *port)
810
{
811
	struct virtio_device *vdev;
812

813
	/* The port could have been hot-unplugged */
814
	if (!port || !is_console_port(port))
815 816
		return;

817
	vdev = port->portdev->vdev;
818 819
	if (virtio_has_feature(vdev, VIRTIO_CONSOLE_F_SIZE))
		hvc_resize(port->cons.hvc, port->cons.ws);
820 821
}

822
/* We set the configuration at this point, since we now have a tty */
823 824
static int notifier_add_vio(struct hvc_struct *hp, int data)
{
825 826 827 828 829 830
	struct port *port;

	port = find_port_by_vtermno(hp->vtermno);
	if (!port)
		return -EINVAL;

831
	hp->irq_requested = 1;
832
	resize_console(port);
833

834 835 836 837 838 839 840 841
	return 0;
}

static void notifier_del_vio(struct hvc_struct *hp, int data)
{
	hp->irq_requested = 0;
}

842
/* The operations for console ports. */
843
static const struct hv_ops hv_ops = {
844 845 846 847 848 849 850 851 852 853 854 855 856 857 858 859 860 861 862 863 864 865
	.get_chars = get_chars,
	.put_chars = put_chars,
	.notifier_add = notifier_add_vio,
	.notifier_del = notifier_del_vio,
	.notifier_hangup = notifier_del_vio,
};

/*
 * Console drivers are initialized very early so boot messages can go
 * out, so we do things slightly differently from the generic virtio
 * initialization of the net and block drivers.
 *
 * At this stage, the console is output-only.  It's too early to set
 * up a virtqueue, so we let the drivers do some boutique early-output
 * thing.
 */
int __init virtio_cons_early_init(int (*put_chars)(u32, const char *, int))
{
	early_put_chars = put_chars;
	return hvc_instantiate(0, 0, &hv_ops);
}

866
int init_port_console(struct port *port)
867 868 869 870 871 872 873 874 875 876 877 878 879 880 881 882 883 884 885 886 887 888 889 890 891
{
	int ret;

	/*
	 * The Host's telling us this port is a console port.  Hook it
	 * up with an hvc console.
	 *
	 * To set up and manage our virtual console, we call
	 * hvc_alloc().
	 *
	 * The first argument of hvc_alloc() is the virtual console
	 * number.  The second argument is the parameter for the
	 * notification mechanism (like irq number).  We currently
	 * leave this as zero, virtqueues have implicit notifications.
	 *
	 * The third argument is a "struct hv_ops" containing the
	 * put_chars() get_chars(), notifier_add() and notifier_del()
	 * pointers.  The final argument is the output buffer size: we
	 * can do any size, so we put PAGE_SIZE here.
	 */
	port->cons.vtermno = pdrvdata.next_vtermno;

	port->cons.hvc = hvc_alloc(port->cons.vtermno, 0, &hv_ops, PAGE_SIZE);
	if (IS_ERR(port->cons.hvc)) {
		ret = PTR_ERR(port->cons.hvc);
892 893
		dev_err(port->dev,
			"error %d allocating hvc for port\n", ret);
894 895 896 897 898 899 900
		port->cons.hvc = NULL;
		return ret;
	}
	spin_lock_irq(&pdrvdata_lock);
	pdrvdata.next_vtermno++;
	list_add_tail(&port->cons.list, &pdrvdata.consoles);
	spin_unlock_irq(&pdrvdata_lock);
901
	port->guest_connected = true;
902

903 904 905 906 907 908 909
	/*
	 * Start using the new console output if this is the first
	 * console to come up.
	 */
	if (early_put_chars)
		early_put_chars = NULL;

910 911 912
	/* Notify host of port being opened */
	send_control_msg(port, VIRTIO_CONSOLE_PORT_OPEN, 1);

913 914 915
	return 0;
}

916 917 918 919 920 921 922 923 924 925 926 927 928 929 930 931 932 933 934 935 936 937
static ssize_t show_port_name(struct device *dev,
			      struct device_attribute *attr, char *buffer)
{
	struct port *port;

	port = dev_get_drvdata(dev);

	return sprintf(buffer, "%s\n", port->name);
}

static DEVICE_ATTR(name, S_IRUGO, show_port_name, NULL);

static struct attribute *port_sysfs_entries[] = {
	&dev_attr_name.attr,
	NULL
};

static struct attribute_group port_attribute_group = {
	.name = NULL,		/* put in device directory */
	.attrs = port_sysfs_entries,
};

938 939 940 941 942 943 944 945 946 947 948 949 950 951 952 953 954 955 956 957 958 959 960 961 962 963
static int debugfs_open(struct inode *inode, struct file *filp)
{
	filp->private_data = inode->i_private;
	return 0;
}

static ssize_t debugfs_read(struct file *filp, char __user *ubuf,
			    size_t count, loff_t *offp)
{
	struct port *port;
	char *buf;
	ssize_t ret, out_offset, out_count;

	out_count = 1024;
	buf = kmalloc(out_count, GFP_KERNEL);
	if (!buf)
		return -ENOMEM;

	port = filp->private_data;
	out_offset = 0;
	out_offset += snprintf(buf + out_offset, out_count,
			       "name: %s\n", port->name ? port->name : "");
	out_offset += snprintf(buf + out_offset, out_count - out_offset,
			       "guest_connected: %d\n", port->guest_connected);
	out_offset += snprintf(buf + out_offset, out_count - out_offset,
			       "host_connected: %d\n", port->host_connected);
964 965
	out_offset += snprintf(buf + out_offset, out_count - out_offset,
			       "outvq_full: %d\n", port->outvq_full);
966 967 968 969 970 971 972 973 974 975 976 977 978 979 980 981 982
	out_offset += snprintf(buf + out_offset, out_count - out_offset,
			       "is_console: %s\n",
			       is_console_port(port) ? "yes" : "no");
	out_offset += snprintf(buf + out_offset, out_count - out_offset,
			       "console_vtermno: %u\n", port->cons.vtermno);

	ret = simple_read_from_buffer(ubuf, count, offp, buf, out_offset);
	kfree(buf);
	return ret;
}

static const struct file_operations port_debugfs_ops = {
	.owner = THIS_MODULE,
	.open  = debugfs_open,
	.read  = debugfs_read,
};

983 984 985 986 987 988 989 990 991
static void set_console_size(struct port *port, u16 rows, u16 cols)
{
	if (!port || !is_console_port(port))
		return;

	port->cons.ws.ws_row = rows;
	port->cons.ws.ws_col = cols;
}

992 993 994 995 996 997 998 999 1000 1001 1002 1003 1004 1005 1006 1007 1008 1009 1010 1011 1012 1013 1014 1015 1016 1017 1018 1019 1020 1021 1022 1023 1024 1025 1026 1027 1028 1029 1030 1031 1032 1033 1034 1035 1036 1037 1038 1039
static unsigned int fill_queue(struct virtqueue *vq, spinlock_t *lock)
{
	struct port_buffer *buf;
	unsigned int nr_added_bufs;
	int ret;

	nr_added_bufs = 0;
	do {
		buf = alloc_buf(PAGE_SIZE);
		if (!buf)
			break;

		spin_lock_irq(lock);
		ret = add_inbuf(vq, buf);
		if (ret < 0) {
			spin_unlock_irq(lock);
			free_buf(buf);
			break;
		}
		nr_added_bufs++;
		spin_unlock_irq(lock);
	} while (ret > 0);

	return nr_added_bufs;
}

static int add_port(struct ports_device *portdev, u32 id)
{
	char debugfs_name[16];
	struct port *port;
	struct port_buffer *buf;
	dev_t devt;
	unsigned int nr_added_bufs;
	int err;

	port = kmalloc(sizeof(*port), GFP_KERNEL);
	if (!port) {
		err = -ENOMEM;
		goto fail;
	}

	port->portdev = portdev;
	port->id = id;

	port->name = NULL;
	port->inbuf = NULL;
	port->cons.hvc = NULL;

1040 1041
	port->cons.ws.ws_row = port->cons.ws.ws_col = 0;

1042 1043
	port->host_connected = port->guest_connected = false;

1044 1045
	port->outvq_full = false;

1046 1047 1048 1049 1050 1051 1052 1053 1054 1055 1056 1057 1058 1059 1060 1061 1062 1063 1064 1065 1066 1067 1068 1069
	port->in_vq = portdev->in_vqs[port->id];
	port->out_vq = portdev->out_vqs[port->id];

	cdev_init(&port->cdev, &port_fops);

	devt = MKDEV(portdev->chr_major, id);
	err = cdev_add(&port->cdev, devt, 1);
	if (err < 0) {
		dev_err(&port->portdev->vdev->dev,
			"Error %d adding cdev for port %u\n", err, id);
		goto free_port;
	}
	port->dev = device_create(pdrvdata.class, &port->portdev->vdev->dev,
				  devt, port, "vport%up%u",
				  port->portdev->drv_index, id);
	if (IS_ERR(port->dev)) {
		err = PTR_ERR(port->dev);
		dev_err(&port->portdev->vdev->dev,
			"Error %d creating device for port %u\n",
			err, id);
		goto free_cdev;
	}

	spin_lock_init(&port->inbuf_lock);
1070
	spin_lock_init(&port->outvq_lock);
1071 1072 1073 1074 1075 1076 1077 1078 1079 1080 1081 1082 1083 1084 1085 1086 1087 1088 1089 1090 1091 1092 1093 1094 1095 1096 1097 1098 1099 1100 1101 1102 1103 1104 1105 1106 1107 1108 1109 1110 1111 1112 1113 1114 1115 1116 1117 1118 1119 1120 1121 1122 1123 1124 1125
	init_waitqueue_head(&port->waitqueue);

	/* Fill the in_vq with buffers so the host can send us data. */
	nr_added_bufs = fill_queue(port->in_vq, &port->inbuf_lock);
	if (!nr_added_bufs) {
		dev_err(port->dev, "Error allocating inbufs\n");
		err = -ENOMEM;
		goto free_device;
	}

	/*
	 * If we're not using multiport support, this has to be a console port
	 */
	if (!use_multiport(port->portdev)) {
		err = init_port_console(port);
		if (err)
			goto free_inbufs;
	}

	spin_lock_irq(&portdev->ports_lock);
	list_add_tail(&port->list, &port->portdev->ports);
	spin_unlock_irq(&portdev->ports_lock);

	/*
	 * Tell the Host we're set so that it can send us various
	 * configuration parameters for this port (eg, port name,
	 * caching, whether this is a console port, etc.)
	 */
	send_control_msg(port, VIRTIO_CONSOLE_PORT_READY, 1);

	if (pdrvdata.debugfs_dir) {
		/*
		 * Finally, create the debugfs file that we can use to
		 * inspect a port's state at any time
		 */
		sprintf(debugfs_name, "vport%up%u",
			port->portdev->drv_index, id);
		port->debugfs_file = debugfs_create_file(debugfs_name, 0444,
							 pdrvdata.debugfs_dir,
							 port,
							 &port_debugfs_ops);
	}
	return 0;

free_inbufs:
	while ((buf = virtqueue_detach_unused_buf(port->in_vq)))
		free_buf(buf);
free_device:
	device_destroy(pdrvdata.class, port->dev->devt);
free_cdev:
	cdev_del(&port->cdev);
free_port:
	kfree(port);
fail:
	/* The host might want to notify management sw about port add failure */
1126
	__send_control_msg(portdev, id, VIRTIO_CONSOLE_PORT_READY, 0);
1127 1128 1129
	return err;
}

1130 1131 1132
/* Remove all port-specific data. */
static int remove_port(struct port *port)
{
1133 1134
	struct port_buffer *buf;

1135 1136 1137 1138 1139 1140 1141
	if (port->guest_connected) {
		port->guest_connected = false;
		port->host_connected = false;
		wake_up_interruptible(&port->waitqueue);
		send_control_msg(port, VIRTIO_CONSOLE_PORT_OPEN, 0);
	}

1142 1143 1144 1145 1146 1147 1148 1149
	spin_lock_irq(&port->portdev->ports_lock);
	list_del(&port->list);
	spin_unlock_irq(&port->portdev->ports_lock);

	if (is_console_port(port)) {
		spin_lock_irq(&pdrvdata_lock);
		list_del(&port->cons.list);
		spin_unlock_irq(&pdrvdata_lock);
1150 1151 1152 1153 1154 1155 1156 1157 1158 1159
#if 0
		/*
		 * hvc_remove() not called as removing one hvc port
		 * results in other hvc ports getting frozen.
		 *
		 * Once this is resolved in hvc, this functionality
		 * will be enabled.  Till that is done, the -EPIPE
		 * return from get_chars() above will help
		 * hvc_console.c to clean up on ports we remove here.
		 */
1160
		hvc_remove(port->cons.hvc);
1161
#endif
1162 1163 1164 1165 1166
	}
	sysfs_remove_group(&port->dev->kobj, &port_attribute_group);
	device_destroy(pdrvdata.class, port->dev->devt);
	cdev_del(&port->cdev);

1167
	/* Remove unused data this port might have received. */
1168
	discard_port_data(port);
1169

1170 1171
	reclaim_consumed_buffers(port);

1172
	/* Remove buffers we queued up for the Host to send us data in. */
1173
	while ((buf = virtqueue_detach_unused_buf(port->in_vq)))
1174 1175
		free_buf(buf);

1176 1177
	kfree(port->name);

1178 1179
	debugfs_remove(port->debugfs_file);

1180 1181 1182 1183
	kfree(port);
	return 0;
}

1184 1185 1186 1187 1188 1189
/* Any private messages that the Host and Guest want to share */
static void handle_control_message(struct ports_device *portdev,
				   struct port_buffer *buf)
{
	struct virtio_console_control *cpkt;
	struct port *port;
1190 1191
	size_t name_size;
	int err;
1192 1193 1194 1195

	cpkt = (struct virtio_console_control *)(buf->buf + buf->offset);

	port = find_port_by_id(portdev, cpkt->id);
1196
	if (!port && cpkt->event != VIRTIO_CONSOLE_PORT_ADD) {
1197 1198 1199 1200 1201 1202 1203
		/* No valid header at start of buffer.  Drop it. */
		dev_dbg(&portdev->vdev->dev,
			"Invalid index %u in control packet\n", cpkt->id);
		return;
	}

	switch (cpkt->event) {
1204 1205
	case VIRTIO_CONSOLE_PORT_ADD:
		if (port) {
1206 1207
			dev_dbg(&portdev->vdev->dev,
				"Port %u already added\n", port->id);
1208 1209 1210 1211 1212 1213 1214 1215 1216 1217 1218 1219 1220 1221
			send_control_msg(port, VIRTIO_CONSOLE_PORT_READY, 1);
			break;
		}
		if (cpkt->id >= portdev->config.max_nr_ports) {
			dev_warn(&portdev->vdev->dev,
				"Request for adding port with out-of-bound id %u, max. supported id: %u\n",
				cpkt->id, portdev->config.max_nr_ports - 1);
			break;
		}
		add_port(portdev, cpkt->id);
		break;
	case VIRTIO_CONSOLE_PORT_REMOVE:
		remove_port(port);
		break;
1222 1223 1224 1225 1226 1227 1228 1229 1230 1231 1232 1233
	case VIRTIO_CONSOLE_CONSOLE_PORT:
		if (!cpkt->value)
			break;
		if (is_console_port(port))
			break;

		init_port_console(port);
		/*
		 * Could remove the port here in case init fails - but
		 * have to notify the host first.
		 */
		break;
1234 1235 1236 1237 1238 1239
	case VIRTIO_CONSOLE_RESIZE: {
		struct {
			__u16 rows;
			__u16 cols;
		} size;

1240 1241
		if (!is_console_port(port))
			break;
1242 1243 1244 1245 1246

		memcpy(&size, buf->buf + buf->offset + sizeof(*cpkt),
		       sizeof(size));
		set_console_size(port, size.rows, size.cols);

1247 1248 1249
		port->cons.hvc->irq_requested = 1;
		resize_console(port);
		break;
1250
	}
1251 1252 1253
	case VIRTIO_CONSOLE_PORT_OPEN:
		port->host_connected = cpkt->value;
		wake_up_interruptible(&port->waitqueue);
1254 1255 1256 1257 1258 1259 1260 1261
		/*
		 * If the host port got closed and the host had any
		 * unconsumed buffers, we'll be able to reclaim them
		 * now.
		 */
		spin_lock_irq(&port->outvq_lock);
		reclaim_consumed_buffers(port);
		spin_unlock_irq(&port->outvq_lock);
1262
		break;
1263 1264 1265 1266 1267 1268 1269 1270 1271 1272 1273 1274 1275 1276 1277 1278 1279 1280 1281 1282 1283 1284 1285
	case VIRTIO_CONSOLE_PORT_NAME:
		/*
		 * Skip the size of the header and the cpkt to get the size
		 * of the name that was sent
		 */
		name_size = buf->len - buf->offset - sizeof(*cpkt) + 1;

		port->name = kmalloc(name_size, GFP_KERNEL);
		if (!port->name) {
			dev_err(port->dev,
				"Not enough space to store port name\n");
			break;
		}
		strncpy(port->name, buf->buf + buf->offset + sizeof(*cpkt),
			name_size - 1);
		port->name[name_size - 1] = 0;

		/*
		 * Since we only have one sysfs attribute, 'name',
		 * create it only if we have a name for the port.
		 */
		err = sysfs_create_group(&port->dev->kobj,
					 &port_attribute_group);
1286
		if (err) {
1287 1288 1289
			dev_err(port->dev,
				"Error %d creating sysfs device attributes\n",
				err);
1290 1291 1292 1293 1294 1295 1296 1297
		} else {
			/*
			 * Generate a udev event so that appropriate
			 * symlinks can be created based on udev
			 * rules.
			 */
			kobject_uevent(&port->dev->kobj, KOBJ_CHANGE);
		}
1298
		break;
1299 1300 1301 1302 1303 1304 1305 1306 1307 1308 1309 1310 1311 1312
	}
}

static void control_work_handler(struct work_struct *work)
{
	struct ports_device *portdev;
	struct virtqueue *vq;
	struct port_buffer *buf;
	unsigned int len;

	portdev = container_of(work, struct ports_device, control_work);
	vq = portdev->c_ivq;

	spin_lock(&portdev->cvq_lock);
1313
	while ((buf = virtqueue_get_buf(vq, &len))) {
1314 1315 1316 1317 1318 1319 1320 1321 1322 1323 1324 1325 1326 1327 1328 1329 1330 1331 1332 1333 1334 1335 1336 1337 1338 1339 1340
		spin_unlock(&portdev->cvq_lock);

		buf->len = len;
		buf->offset = 0;

		handle_control_message(portdev, buf);

		spin_lock(&portdev->cvq_lock);
		if (add_inbuf(portdev->c_ivq, buf) < 0) {
			dev_warn(&portdev->vdev->dev,
				 "Error adding buffer to queue\n");
			free_buf(buf);
		}
	}
	spin_unlock(&portdev->cvq_lock);
}

static void in_intr(struct virtqueue *vq)
{
	struct port *port;
	unsigned long flags;

	port = find_port_by_vq(vq->vdev->priv, vq);
	if (!port)
		return;

	spin_lock_irqsave(&port->inbuf_lock, flags);
1341 1342
	if (!port->inbuf)
		port->inbuf = get_inbuf(port);
1343

1344 1345 1346 1347 1348 1349 1350 1351 1352 1353
	/*
	 * Don't queue up data when port is closed.  This condition
	 * can be reached when a console port is not yet connected (no
	 * tty is spawned) and the host sends out data to console
	 * ports.  For generic serial ports, the host won't
	 * (shouldn't) send data till the guest is connected.
	 */
	if (!port->guest_connected)
		discard_port_data(port);

1354 1355
	spin_unlock_irqrestore(&port->inbuf_lock, flags);

1356 1357
	wake_up_interruptible(&port->waitqueue);

1358 1359 1360 1361 1362 1363 1364 1365 1366 1367 1368 1369
	if (is_console_port(port) && hvc_poll(port->cons.hvc))
		hvc_kick();
}

static void control_intr(struct virtqueue *vq)
{
	struct ports_device *portdev;

	portdev = vq->vdev->priv;
	schedule_work(&portdev->control_work);
}

1370 1371 1372 1373 1374
static void config_intr(struct virtio_device *vdev)
{
	struct ports_device *portdev;

	portdev = vdev->priv;
1375

1376
	if (!use_multiport(portdev)) {
1377 1378 1379 1380 1381 1382 1383 1384 1385 1386 1387 1388 1389
		struct port *port;
		u16 rows, cols;

		vdev->config->get(vdev,
				  offsetof(struct virtio_console_config, cols),
				  &cols, sizeof(u16));
		vdev->config->get(vdev,
				  offsetof(struct virtio_console_config, rows),
				  &rows, sizeof(u16));

		port = find_port_by_id(portdev, 0);
		set_console_size(port, rows, cols);

1390 1391 1392 1393 1394 1395 1396
		/*
		 * We'll use this way of resizing only for legacy
		 * support.  For newer userspace
		 * (VIRTIO_CONSOLE_F_MULTPORT+), use control messages
		 * to indicate console size changes so that it can be
		 * done per-port.
		 */
1397
		resize_console(port);
1398
	}
1399 1400
}

1401 1402 1403 1404 1405
static int init_vqs(struct ports_device *portdev)
{
	vq_callback_t **io_callbacks;
	char **io_names;
	struct virtqueue **vqs;
1406
	u32 i, j, nr_ports, nr_queues;
1407 1408
	int err;

1409 1410
	nr_ports = portdev->config.max_nr_ports;
	nr_queues = use_multiport(portdev) ? (nr_ports + 1) * 2 : 2;
1411 1412 1413 1414 1415 1416 1417 1418 1419 1420 1421 1422 1423 1424 1425 1426 1427 1428 1429 1430 1431 1432 1433 1434 1435 1436 1437 1438 1439

	vqs = kmalloc(nr_queues * sizeof(struct virtqueue *), GFP_KERNEL);
	if (!vqs) {
		err = -ENOMEM;
		goto fail;
	}
	io_callbacks = kmalloc(nr_queues * sizeof(vq_callback_t *), GFP_KERNEL);
	if (!io_callbacks) {
		err = -ENOMEM;
		goto free_vqs;
	}
	io_names = kmalloc(nr_queues * sizeof(char *), GFP_KERNEL);
	if (!io_names) {
		err = -ENOMEM;
		goto free_callbacks;
	}
	portdev->in_vqs = kmalloc(nr_ports * sizeof(struct virtqueue *),
				  GFP_KERNEL);
	if (!portdev->in_vqs) {
		err = -ENOMEM;
		goto free_names;
	}
	portdev->out_vqs = kmalloc(nr_ports * sizeof(struct virtqueue *),
				   GFP_KERNEL);
	if (!portdev->out_vqs) {
		err = -ENOMEM;
		goto free_invqs;
	}

1440 1441 1442 1443 1444 1445 1446 1447 1448 1449 1450 1451 1452 1453 1454 1455 1456 1457 1458 1459 1460 1461 1462 1463 1464 1465
	/*
	 * For backward compat (newer host but older guest), the host
	 * spawns a console port first and also inits the vqs for port
	 * 0 before others.
	 */
	j = 0;
	io_callbacks[j] = in_intr;
	io_callbacks[j + 1] = NULL;
	io_names[j] = "input";
	io_names[j + 1] = "output";
	j += 2;

	if (use_multiport(portdev)) {
		io_callbacks[j] = control_intr;
		io_callbacks[j + 1] = NULL;
		io_names[j] = "control-i";
		io_names[j + 1] = "control-o";

		for (i = 1; i < nr_ports; i++) {
			j += 2;
			io_callbacks[j] = in_intr;
			io_callbacks[j + 1] = NULL;
			io_names[j] = "input";
			io_names[j + 1] = "output";
		}
	}
1466 1467 1468 1469 1470 1471 1472
	/* Find the queues. */
	err = portdev->vdev->config->find_vqs(portdev->vdev, nr_queues, vqs,
					      io_callbacks,
					      (const char **)io_names);
	if (err)
		goto free_outvqs;

1473
	j = 0;
1474 1475
	portdev->in_vqs[0] = vqs[0];
	portdev->out_vqs[0] = vqs[1];
1476 1477 1478 1479 1480 1481 1482 1483 1484 1485 1486
	j += 2;
	if (use_multiport(portdev)) {
		portdev->c_ivq = vqs[j];
		portdev->c_ovq = vqs[j + 1];

		for (i = 1; i < nr_ports; i++) {
			j += 2;
			portdev->in_vqs[i] = vqs[j];
			portdev->out_vqs[i] = vqs[j + 1];
		}
	}
1487 1488 1489 1490 1491 1492 1493 1494 1495 1496 1497 1498 1499 1500 1501 1502 1503 1504 1505 1506
	kfree(io_callbacks);
	kfree(io_names);
	kfree(vqs);

	return 0;

free_names:
	kfree(io_names);
free_callbacks:
	kfree(io_callbacks);
free_outvqs:
	kfree(portdev->out_vqs);
free_invqs:
	kfree(portdev->in_vqs);
free_vqs:
	kfree(vqs);
fail:
	return err;
}

1507 1508 1509 1510
static const struct file_operations portdev_fops = {
	.owner = THIS_MODULE,
};

1511 1512 1513
/*
 * Once we're further in boot, we get probed like any other virtio
 * device.
1514 1515 1516 1517
 *
 * If the host also supports multiple console ports, we check the
 * config space to see how many ports the host has spawned.  We
 * initialize each port found.
1518 1519 1520 1521 1522
 */
static int __devinit virtcons_probe(struct virtio_device *vdev)
{
	struct ports_device *portdev;
	int err;
1523
	bool multiport;
1524 1525 1526 1527 1528 1529 1530 1531 1532 1533 1534

	portdev = kmalloc(sizeof(*portdev), GFP_KERNEL);
	if (!portdev) {
		err = -ENOMEM;
		goto fail;
	}

	/* Attach this portdev to this virtio_device, and vice-versa. */
	portdev->vdev = vdev;
	vdev->priv = portdev;

1535 1536 1537 1538 1539 1540 1541 1542 1543 1544 1545 1546 1547 1548
	spin_lock_irq(&pdrvdata_lock);
	portdev->drv_index = pdrvdata.index++;
	spin_unlock_irq(&pdrvdata_lock);

	portdev->chr_major = register_chrdev(0, "virtio-portsdev",
					     &portdev_fops);
	if (portdev->chr_major < 0) {
		dev_err(&vdev->dev,
			"Error %d registering chrdev for device %u\n",
			portdev->chr_major, portdev->drv_index);
		err = portdev->chr_major;
		goto free;
	}

1549 1550 1551 1552 1553 1554
	multiport = false;
	portdev->config.max_nr_ports = 1;
	if (virtio_has_feature(vdev, VIRTIO_CONSOLE_F_MULTIPORT)) {
		multiport = true;
		vdev->features[0] |= 1 << VIRTIO_CONSOLE_F_MULTIPORT;

1555 1556
		vdev->config->get(vdev, offsetof(struct virtio_console_config,
						 max_nr_ports),
1557 1558 1559 1560 1561 1562 1563
				  &portdev->config.max_nr_ports,
				  sizeof(portdev->config.max_nr_ports));
	}

	/* Let the Host know we support multiple ports.*/
	vdev->config->finalize_features(vdev);

1564 1565 1566
	err = init_vqs(portdev);
	if (err < 0) {
		dev_err(&vdev->dev, "Error %d initializing vqs\n", err);
1567
		goto free_chrdev;
1568
	}
1569

1570 1571 1572 1573
	spin_lock_init(&portdev->ports_lock);
	INIT_LIST_HEAD(&portdev->ports);

	if (multiport) {
1574 1575
		unsigned int nr_added_bufs;

1576 1577 1578
		spin_lock_init(&portdev->cvq_lock);
		INIT_WORK(&portdev->control_work, &control_work_handler);

1579 1580
		nr_added_bufs = fill_queue(portdev->c_ivq, &portdev->cvq_lock);
		if (!nr_added_bufs) {
1581 1582 1583 1584 1585
			dev_err(&vdev->dev,
				"Error allocating buffers for control queue\n");
			err = -ENOMEM;
			goto free_vqs;
		}
1586 1587 1588 1589 1590 1591
	} else {
		/*
		 * For backward compatibility: Create a console port
		 * if we're running on older host.
		 */
		add_port(portdev, 0);
1592 1593
	}

1594 1595
	__send_control_msg(portdev, VIRTIO_CONSOLE_BAD_ID,
			   VIRTIO_CONSOLE_DEVICE_READY, 1);
R
Rusty Russell 已提交
1596 1597
	return 0;

1598
free_vqs:
1599 1600 1601
	/* The host might want to notify mgmt sw about device add failure */
	__send_control_msg(portdev, VIRTIO_CONSOLE_BAD_ID,
			   VIRTIO_CONSOLE_DEVICE_READY, 0);
1602 1603 1604
	vdev->config->del_vqs(vdev);
	kfree(portdev->in_vqs);
	kfree(portdev->out_vqs);
1605 1606
free_chrdev:
	unregister_chrdev(portdev->chr_major, "virtio-portsdev");
R
Rusty Russell 已提交
1607
free:
1608
	kfree(portdev);
R
Rusty Russell 已提交
1609 1610 1611 1612
fail:
	return err;
}

1613 1614 1615 1616 1617 1618 1619
static void virtcons_remove(struct virtio_device *vdev)
{
	struct ports_device *portdev;
	struct port *port, *port2;

	portdev = vdev->priv;

1620 1621 1622
	/* Disable interrupts for vqs */
	vdev->config->reset(vdev);
	/* Finish up work that's lined up */
1623 1624 1625 1626 1627 1628 1629
	cancel_work_sync(&portdev->control_work);

	list_for_each_entry_safe(port, port2, &portdev->ports, list)
		remove_port(port);

	unregister_chrdev(portdev->chr_major, "virtio-portsdev");

1630 1631 1632
	if (use_multiport(portdev)) {
		struct port_buffer *buf;
		unsigned int len;
1633

1634 1635 1636 1637 1638 1639
		while ((buf = virtqueue_get_buf(portdev->c_ivq, &len)))
			free_buf(buf);

		while ((buf = virtqueue_detach_unused_buf(portdev->c_ivq)))
			free_buf(buf);
	}
1640 1641 1642 1643 1644 1645 1646 1647

	vdev->config->del_vqs(vdev);
	kfree(portdev->in_vqs);
	kfree(portdev->out_vqs);

	kfree(portdev);
}

R
Rusty Russell 已提交
1648 1649 1650 1651 1652
static struct virtio_device_id id_table[] = {
	{ VIRTIO_ID_CONSOLE, VIRTIO_DEV_ANY_ID },
	{ 0 },
};

1653 1654
static unsigned int features[] = {
	VIRTIO_CONSOLE_F_SIZE,
1655
	VIRTIO_CONSOLE_F_MULTIPORT,
1656 1657
};

R
Rusty Russell 已提交
1658
static struct virtio_driver virtio_console = {
1659 1660
	.feature_table = features,
	.feature_table_size = ARRAY_SIZE(features),
R
Rusty Russell 已提交
1661 1662 1663 1664
	.driver.name =	KBUILD_MODNAME,
	.driver.owner =	THIS_MODULE,
	.id_table =	id_table,
	.probe =	virtcons_probe,
1665
	.remove =	virtcons_remove,
1666
	.config_changed = config_intr,
R
Rusty Russell 已提交
1667 1668 1669 1670
};

static int __init init(void)
{
1671 1672 1673 1674 1675 1676 1677 1678
	int err;

	pdrvdata.class = class_create(THIS_MODULE, "virtio-ports");
	if (IS_ERR(pdrvdata.class)) {
		err = PTR_ERR(pdrvdata.class);
		pr_err("Error %d creating virtio-ports class\n", err);
		return err;
	}
1679 1680 1681 1682 1683 1684

	pdrvdata.debugfs_dir = debugfs_create_dir("virtio-ports", NULL);
	if (!pdrvdata.debugfs_dir) {
		pr_warning("Error %ld creating debugfs dir for virtio-ports\n",
			   PTR_ERR(pdrvdata.debugfs_dir));
	}
1685 1686
	INIT_LIST_HEAD(&pdrvdata.consoles);

R
Rusty Russell 已提交
1687 1688
	return register_virtio_driver(&virtio_console);
}
1689 1690 1691 1692 1693 1694 1695 1696 1697

static void __exit fini(void)
{
	unregister_virtio_driver(&virtio_console);

	class_destroy(pdrvdata.class);
	if (pdrvdata.debugfs_dir)
		debugfs_remove_recursive(pdrvdata.debugfs_dir);
}
R
Rusty Russell 已提交
1698
module_init(init);
1699
module_exit(fini);
R
Rusty Russell 已提交
1700 1701 1702 1703

MODULE_DEVICE_TABLE(virtio, id_table);
MODULE_DESCRIPTION("Virtio console driver");
MODULE_LICENSE("GPL");