drivers/char/vservices_serial.c - kernel/msm-4.19 - Gitiles

 /*
  * drivers/char/vservice_serial.c
  *
  * Copyright (c) 2012-2018 General Dynamics
  * Copyright (c) 2014 Open Kernel Labs, Inc.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 as
  * published by the Free Software Foundation.
  *
  * serial vservice client driver
  *
  */

 #include <linux/kernel.h>
 #include <linux/module.h>
 #include <linux/bitmap.h>
 #include <linux/tty.h>
 #include <linux/tty_driver.h>
 #include <linux/tty_flip.h>
 #include <linux/console.h>
 #include <linux/version.h>
 #include <linux/sched.h>
 #include <linux/wait.h>
 #include <linux/mutex.h>

 #include <vservices/transport.h>
 #include <vservices/types.h>
 #include <vservices/buffer.h>
 #include <vservices/protocol/serial/common.h>
 #include <vservices/protocol/serial/types.h>
 #include <vservices/protocol/serial/server.h>
 #include <vservices/service.h>
 #include <vservices/wait.h>

 #include "vs_serial_common.h"

 struct vtty_in_packet {
 	struct vs_pbuf	pbuf;
 	size_t		offset;
 };

 static int max_ttys = CONFIG_VSERVICES_VTTY_COUNT;
 static unsigned long *alloced_ttys;
 module_param(max_ttys, int, S_IRUGO);

 static struct tty_driver *vtty_driver;

 static DEFINE_MUTEX(tty_bitmap_lock);

 static struct vtty_port *dev_to_port(struct device *dev)
 {
 	struct vs_service_device *service = to_vs_service_device(dev);

 #if defined(CONFIG_VSERVICES_SERIAL_SERVER) || \
     defined(CONFIG_VSERIVCES_SERIAL_SERVER_MODULE)
 	if (service->is_server) {
 		struct vs_server_serial_state *server = dev_get_drvdata(dev);
 		return container_of(server, struct vtty_port, u.vs_server);
 	}
 #endif
 #if defined(CONFIG_VSERVICES_SERIAL_CLIENT) || \
     defined(CONFIG_VSERIVCES_SERIAL_CLIENT_MODULE)
 	if (!service->is_server) {
 		struct vs_client_serial_state *client = dev_get_drvdata(dev);
 		return container_of(client, struct vtty_port, u.vs_client);
 	}
 #endif
 	/* should never get here */
 	WARN_ON(1);
 	return NULL;
 }

 static struct vtty_port *port_from_tty(struct tty_struct *tty)
 {
 	return dev_to_port(tty->dev->parent);
 }

 static int vtty_install(struct tty_driver *driver, struct tty_struct *tty)
 {
 	struct vtty_port *port;

 	if (tty->index < 0 || !test_bit(tty->index, alloced_ttys))
 		return -ENXIO;

 	port = port_from_tty(tty);

 	if (!port)
 		return -ENXIO;

 	tty->driver_data = port;
 #if LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0)
 	if (tty->port)
 		tty->port->low_latency = 0;
 #else
 	tty->low_latency = 0;
 #endif

 #if LINUX_VERSION_CODE >= KERNEL_VERSION(3,7,0)
 	tty_port_install(&port->port, driver, tty);
 #elif LINUX_VERSION_CODE >= KERNEL_VERSION(3,4,0)
 	tty->port = &port->port;
 	tty_standard_install(driver, tty);
 #else
 	tty->port = &port->port;
 	if (tty_init_termios(tty) != 0)
 		return -ENOMEM;

 	tty_driver_kref_get(driver);
 	tty->count++;
 	driver->ttys[tty->index] = tty;
 #endif

 	return 0;
 }

 static int vtty_open(struct tty_struct *tty, struct file *file)
 {
 	struct vtty_port *port = tty->driver_data;
 	return tty_port_open(&port->port, tty, file);
 }

 static void vtty_close(struct tty_struct *tty, struct file *file)
 {
 	struct vtty_port *port = tty->driver_data;
 	if (port)
 		tty_port_close(&port->port, tty, file);
 }

 static void vtty_shutdown(struct tty_port *port)
 {
 	struct vtty_port *vtty_port =
 			container_of(port, struct vtty_port, port);

 	if (vtty_port->doing_release)
 		kfree(port);
 }

 static int vtty_write_room(struct tty_struct *tty)
 {
 	struct vtty_port *port = tty->driver_data;

 	return vs_service_send_mbufs_available(port->service) *
 			port->max_transfer_size;
 }

 static struct vs_mbuf *vserial_alloc_send_buffer(struct vtty_port *port,
 		const unsigned char *buf, size_t size, struct vs_pbuf *pbuf,
 		gfp_t gfp_flags)
 {
 	struct vs_mbuf *mbuf;
 	ssize_t ret;

 	mbuf = port->ops.alloc_msg_buf(port, pbuf, gfp_flags);
 	if (IS_ERR(mbuf)) {
 		ret = PTR_ERR(mbuf);
 		goto fail;
 	}

 	ret = vs_pbuf_resize(pbuf, size);
 	if (ret < (ssize_t)size)
 		goto fail_free_buf;

 	ret = vs_pbuf_copyin(pbuf, 0, buf, size);
 	if (ret < (ssize_t)size)
 		goto fail_free_buf;

 	return mbuf;

 fail_free_buf:
 	port->ops.free_msg_buf(port, mbuf, pbuf);
 fail:
 	return ERR_PTR(ret);
 }

 static int vtty_write(struct tty_struct *tty, const unsigned char *buf,
 		int count)
 {
 	struct vtty_port *port;
 	size_t sent_bytes = 0, size;
 	struct vs_mbuf *mbuf;
 	struct vs_pbuf pbuf;
 	int err;

 	if (WARN_ON(!tty || !buf))
 		return -EINVAL;

 	port = tty->driver_data;
 	if (!port->ops.is_running(port)) {
 		dev_dbg(&port->service->dev, "tty is not running!");
 		return 0;
 	}

 	/*
 	 * We need to break our message up into chunks of
 	 * port->max_transfer_size.
 	 */
 	dev_dbg(&port->service->dev, "Writing %d bytes\n", count);
 	while (sent_bytes < count) {
 		size = min_t(size_t, count - sent_bytes,
 				port->max_transfer_size);

 		/*
 		 * Passing &port->u.vs_client here works for both the client
 		 * and the server since vs_client and vs_server are in the
 		 * same union, and therefore have the same address.
 		 */
 		mbuf = vs_service_waiting_alloc(&port->u.vs_client,
 				vserial_alloc_send_buffer(port,
 				buf + sent_bytes, size, &pbuf, GFP_KERNEL));
 		if (IS_ERR(mbuf)) {
 			dev_err(&port->service->dev,
 					"Failed to alloc mbuf of %zu bytes: %ld - resetting service\n",
 					size, PTR_ERR(mbuf));
 			vs_service_reset(port->service, port->service);
 			return -EIO;
 		}

 		vs_service_state_lock(port->service);
 		err = port->ops.send_msg_buf(port, mbuf, &pbuf);
 		vs_service_state_unlock(port->service);
 		if (err) {
 			port->ops.free_msg_buf(port, mbuf, &pbuf);
 			dev_err(&port->service->dev,
 					"send failed: %d - resetting service\n",
 					err);
 			vs_service_reset(port->service, port->service);
 			return -EIO;
 		}

 		dev_dbg(&port->service->dev, "Sent %zu bytes (%zu/%d)\n",
 				size, sent_bytes + size, count);
 		sent_bytes += size;
 	}

 	dev_dbg(&port->service->dev, "Write complete - sent %zu/%d bytes\n",
 			sent_bytes, count);
 	return sent_bytes;
 }

 static int vtty_put_char(struct tty_struct *tty, unsigned char ch)
 {
 	return vtty_write(tty, &ch, 1);
 }

 static size_t vs_serial_send_pbuf_to_tty(struct vtty_port *port,
 		struct vs_pbuf *pbuf, size_t offset)
 {
 	struct tty_struct *tty = tty_port_tty_get(&port->port);
 	size_t space, size;

 	lockdep_assert_held(&port->in_lock);

 	size = vs_pbuf_size(pbuf) - offset;
 #if LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0)
 	space = tty_buffer_request_room(tty->port, size);
 #else
 	space = tty_buffer_request_room(tty, size);
 #endif
 	if (space) {
 #if LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0)
 		tty_insert_flip_string(tty->port, pbuf->data + offset, space);
 		tty_flip_buffer_push(tty->port);
 #else
 		tty_insert_flip_string(tty, pbuf->data + offset, space);
 		tty_flip_buffer_push(tty);
 #endif
 	}

 	tty_kref_put(tty);

 	/* Return the number of bytes written */
 	return space;
 }

 static void vtty_throttle(struct tty_struct *tty)
 {
 	struct vtty_port *port = tty->driver_data;

 	dev_dbg(&port->service->dev, "throttle\n");

 	spin_lock_bh(&port->in_lock);
 	port->tty_canrecv = false;
 	spin_unlock_bh(&port->in_lock);
 }

 static void vtty_unthrottle(struct tty_struct *tty)
 {
 	struct vtty_port *port = tty->driver_data;
 	struct vtty_in_packet *packet;
 	struct vs_mbuf *mbuf;
 	size_t sent;

 	dev_dbg(&port->service->dev, "unthrottle\n");

 	spin_lock_bh(&port->in_lock);

 	while (!list_empty(&port->pending_in_packets)) {
 		mbuf = list_first_entry(&port->pending_in_packets,
 				struct vs_mbuf, queue);
 		packet = mbuf->priv;

 		sent = vs_serial_send_pbuf_to_tty(port, &packet->pbuf,
 				packet->offset);
 		packet->offset += sent;
 		if (packet->offset < vs_pbuf_size(&packet->pbuf)) {
 			/*
 			 * Only wrote part of the buffer. This means that we
 			 * still have pending data that cannot be written to
 			 * the tty at this time. The tty layer will rethrottle
 			 * and this function will be called again when the tty
 			 * layer is next able to handle data and we can write
 			 * the remainder of the buffer.
 			 */
 			dev_dbg(&port->service->dev,
 					"unthrottle: Only wrote %zu (%zu/%zu) bytes\n",
 					sent, packet->offset,
 					vs_pbuf_size(&packet->pbuf));
 			break;
 		}

 		dev_dbg(&port->service->dev,
 				"unthrottle: wrote %zu (%zu/%zu) bytes\n",
 				sent, packet->offset,
 				vs_pbuf_size(&packet->pbuf));

 		/* Wrote the whole buffer - free it */
 		list_del(&mbuf->queue);
 		port->ops.free_msg_buf(port, mbuf, &packet->pbuf);
 		kfree(packet);
 	}

 	port->tty_canrecv = true;
 	spin_unlock_bh(&port->in_lock);
 }

 static struct tty_port_operations vtty_port_ops = {
 	.shutdown	= vtty_shutdown,
 };

 static struct tty_operations vtty_ops = {
 	.install	= vtty_install,
 	.open		= vtty_open,
 	.close		= vtty_close,
 	.write		= vtty_write,
 	.write_room	= vtty_write_room,
 	.put_char	= vtty_put_char,
 	.throttle	= vtty_throttle,
 	.unthrottle	= vtty_unthrottle
 };

 static int vs_serial_queue_incoming_packet(struct vtty_port *port,
 		struct vs_mbuf *mbuf, struct vs_pbuf *pbuf, size_t offset)
 {
 	struct vtty_in_packet *packet;

 	lockdep_assert_held(&port->in_lock);

 	packet = kzalloc(sizeof(*packet), GFP_ATOMIC);
 	if (!packet) {
 		/*
 		 * Uh oh, we are seriously out of memory. The incoming data
 		 * will be lost.
 		 */
 		return -ENOMEM;
 	}

 	dev_dbg(&port->service->dev, "Queuing packet %zu bytes, offset %zu\n",
 			vs_pbuf_size(pbuf), offset);
 	mbuf->priv = packet;
 	memcpy(&packet->pbuf, pbuf, sizeof(*pbuf));
 	packet->offset = offset;

 	list_add_tail(&mbuf->queue, &port->pending_in_packets);
 	return 0;
 }

 int vs_serial_handle_message(struct vtty_port *port, struct vs_mbuf *mbuf,
 		struct vs_pbuf *pbuf)
 {
 	struct tty_struct *tty = tty_port_tty_get(&port->port);
 	bool queue_packet = false;
 	size_t sent = 0;
 	int err;

 	if (!tty) {
 		dev_dbg(&port->service->dev,
 				"tty not open. Dropping %zu chars\n",
 				pbuf->size);
 		port->ops.free_msg_buf(port, mbuf, pbuf);
 		return 0;
 	}

 	dev_dbg(&port->service->dev, "Incoming message - len = %zu\n",
 			pbuf->size);

 	spin_lock(&port->in_lock);
 	if (!port->tty_canrecv || !list_empty(&port->pending_in_packets)) {
 		/*
 		 * We cannot send to the tty right now, either because we are
 		 * being throttled or because we still have pending data
 		 * to write out to the tty. Queue the buffer up so we can
 		 * write it later.
 		 */
 		dev_dbg(&port->service->dev,
 				"Cannot send (canrecv = %d, queued = %d) - queuing message\n",
 				port->tty_canrecv,
 				!list_empty(&port->pending_in_packets));
 		queue_packet = true;

 	} else {
 		sent = vs_serial_send_pbuf_to_tty(port, pbuf, 0);
 		if (sent < vs_pbuf_size(pbuf)) {
 			/*
 			 * Only wrote part of the buffer to the tty. Queue
 			 * the buffer to write the rest.
 			 */
 			dev_dbg(&port->service->dev,
 					"Sent %zu/%zu bytes to tty - queueing rest\n",
 					sent, vs_pbuf_size(pbuf));
 			queue_packet = true;
 		}
 	}

 	if (queue_packet) {
 		/*
 		 * Queue the incoming data up. If we are not already throttled,
 		 * the tty layer will do so now since it has no room in its
 		 * buffers.
 		 */
 		err = vs_serial_queue_incoming_packet(port, mbuf, pbuf, sent);
 		if (err) {
 			dev_err(&port->service->dev,
 					"Failed to queue packet - dropping chars\n");
 			port->ops.free_msg_buf(port, mbuf, pbuf);
 		}

 	} else {
 		port->ops.free_msg_buf(port, mbuf, pbuf);
 	}

 	spin_unlock(&port->in_lock);
 	tty_kref_put(tty);

 	return 0;
 }
 EXPORT_SYMBOL_GPL(vs_serial_handle_message);

 #ifdef CONFIG_OKL4_VTTY_CONSOLE
 static int vconsole_setup(struct console *co, char *options)
 {
 	if (co->index < 0 || co->index >= max_ttys)
 		co->index = 0;

 	pr_info("OKL4 virtual console init\n");

 	return 0;
 }

 static void vconsole_write(struct console *co, const char *p, unsigned count)
 {
 }

 static struct tty_driver *vconsole_device(struct console *co, int *index)
 {
 	*index = co->index;

 	return vtty_driver;
 }
 #endif /* CONFIG_OKL4_VTTY_CONSOLE */

 static void vs_serial_free_buffers(struct vtty_port *port)
 {
 	struct vtty_in_packet *packet;
 	struct vs_mbuf *mbuf;

 	/* Free the list of incoming buffers */
 	spin_lock_bh(&port->in_lock);
 	while (!list_empty(&port->pending_in_packets)) {
 		mbuf = list_first_entry(&port->pending_in_packets,
 				struct vs_mbuf, queue);
 		packet = mbuf->priv;

 		list_del(&mbuf->queue);
 		port->ops.free_msg_buf(port, mbuf, &packet->pbuf);
 		kfree(packet);
 	}
 	spin_unlock_bh(&port->in_lock);
 }

 /** vservices callbacks **/
 struct vtty_port *vs_serial_alloc_port(struct vs_service_device *service,
 		struct vtty_port_ops *port_ops)
 {
 	struct vtty_port *port;
 	int port_num;

 	mutex_lock(&tty_bitmap_lock);
 	port_num = find_first_zero_bit(alloced_ttys, max_ttys);

 	if (port_num >= max_ttys) {
 		mutex_unlock(&tty_bitmap_lock);
 		return NULL;
 	}

 	port = kzalloc(sizeof(struct vtty_port), GFP_KERNEL);
 	if (!port) {
 		mutex_unlock(&tty_bitmap_lock);
 		return NULL;
 	}

 	port->service = service;
 	port->ops = *port_ops;
 	port->tty_canrecv = true;
 	port->port_num = port_num;
 	INIT_LIST_HEAD(&port->pending_in_packets);
 	spin_lock_init(&port->in_lock);
 #ifdef CONFIG_OKL4_VTTY_CONSOLE
 	/* Set up and register the port's console device */
 	strlcpy(port->console.name, "vconvs", sizeof(port->console.name));
 	port->console.write = vconsole_write;
 	port->console.flags = CON_PRINTBUFFER;
 	port->console.device = vconsole_device;
 	port->console.setup = vconsole_setup;
 	port->console.index = port_num;

 	register_console(&port->console);
 #endif
 	port->vtty_driver = vtty_driver;

 	tty_port_init(&port->port);
 	port->port.ops = &vtty_port_ops;

 	tty_register_device(vtty_driver, port_num, &service->dev);
 	bitmap_set(alloced_ttys, port_num, 1);
 	mutex_unlock(&tty_bitmap_lock);

 	return port;
 }
 EXPORT_SYMBOL(vs_serial_alloc_port);

 void vs_serial_release(struct vtty_port *port)
 {
 	dev_dbg(&port->service->dev, "Release\n");

 #ifdef CONFIG_OKL4_VTTY_CONSOLE
 	unregister_console(&port->console);
 #endif

 	mutex_lock(&tty_bitmap_lock);
 	bitmap_clear(alloced_ttys, port->port_num, 1);
 	mutex_unlock(&tty_bitmap_lock);

 	if (port->port.tty) {
 		tty_vhangup(port->port.tty);
 		tty_kref_put(port->port.tty);
 	}

 	vs_serial_free_buffers(port);
 	port->doing_release = true;
 	tty_unregister_device(vtty_driver, port->port_num);
 }
 EXPORT_SYMBOL_GPL(vs_serial_release);

 void vs_serial_reset(struct vtty_port *port)
 {
 	/* Free list of in and out mbufs. */
 	vs_serial_free_buffers(port);
 }
 EXPORT_SYMBOL_GPL(vs_serial_reset);

 static int __init vs_serial_init(void)
 {
 	int err;

 	if (max_ttys == 0)
 		return -EINVAL;

 	alloced_ttys = kzalloc(sizeof(unsigned long) * BITS_TO_LONGS(max_ttys),
 			GFP_KERNEL);
 	if (!alloced_ttys) {
 		err = -ENOMEM;
 		goto fail_alloc_ttys;
 	}

 	/* Set up the tty driver. */
 	vtty_driver = alloc_tty_driver(max_ttys);
 	if (!vtty_driver) {
 		err = -ENOMEM;
 		goto fail_alloc_tty_driver;
 	}

 	vtty_driver->owner = THIS_MODULE;
 	vtty_driver->driver_name = "okl4-vservices-serial";
 	vtty_driver->name = "ttyVS";
 	vtty_driver->type = TTY_DRIVER_TYPE_SERIAL;
 	vtty_driver->subtype = SERIAL_TYPE_NORMAL;
 	vtty_driver->flags = TTY_DRIVER_REAL_RAW | TTY_DRIVER_DYNAMIC_DEV;
 	vtty_driver->init_termios = tty_std_termios;
 	vtty_driver->num = max_ttys;

 	/* These flags don't really matter; just use sensible defaults. */
 	vtty_driver->init_termios.c_cflag =
 			B9600 | CS8 | CREAD | HUPCL | CLOCAL;
 	vtty_driver->init_termios.c_ispeed = 9600;
 	vtty_driver->init_termios.c_ospeed = 9600;

 	tty_set_operations(vtty_driver, &vtty_ops);

 	err = tty_register_driver(vtty_driver);
 	if (err)
 		goto fail_tty_driver_register;

 	return 0;

 fail_tty_driver_register:
 	put_tty_driver(vtty_driver);
 fail_alloc_tty_driver:
 	kfree(alloced_ttys);
 fail_alloc_ttys:
 	return err;
 }

 static void __exit vs_serial_exit(void)
 {
 	tty_unregister_driver(vtty_driver);
 	put_tty_driver(vtty_driver);
 }

 module_init(vs_serial_init);
 module_exit(vs_serial_exit);

 MODULE_DESCRIPTION("OKL4 Virtual Services Serial Core Driver");
 MODULE_AUTHOR("Open Kernel Labs, Inc");
	/*
	* drivers/char/vservice_serial.c
	*
	* Copyright (c) 2012-2018 General Dynamics
	* Copyright (c) 2014 Open Kernel Labs, Inc.
	*
	* This program is free software; you can redistribute it and/or modify
	* it under the terms of the GNU General Public License version 2 as
	* published by the Free Software Foundation.
	*
	* serial vservice client driver
	*
	*/

	#include <linux/kernel.h>
	#include <linux/module.h>
	#include <linux/bitmap.h>
	#include <linux/tty.h>
	#include <linux/tty_driver.h>
	#include <linux/tty_flip.h>
	#include <linux/console.h>
	#include <linux/version.h>
	#include <linux/sched.h>
	#include <linux/wait.h>
	#include <linux/mutex.h>

	#include <vservices/transport.h>
	#include <vservices/types.h>
	#include <vservices/buffer.h>
	#include <vservices/protocol/serial/common.h>
	#include <vservices/protocol/serial/types.h>
	#include <vservices/protocol/serial/server.h>
	#include <vservices/service.h>
	#include <vservices/wait.h>

	#include "vs_serial_common.h"

	struct vtty_in_packet {
	struct vs_pbuf pbuf;
	size_t offset;
	};

	static int max_ttys = CONFIG_VSERVICES_VTTY_COUNT;
	static unsigned long *alloced_ttys;
	module_param(max_ttys, int, S_IRUGO);

	static struct tty_driver *vtty_driver;

	static DEFINE_MUTEX(tty_bitmap_lock);

	static struct vtty_port dev_to_port(struct device dev)
	{
	struct vs_service_device *service = to_vs_service_device(dev);

	#if defined(CONFIG_VSERVICES_SERIAL_SERVER) \|\| \
	defined(CONFIG_VSERIVCES_SERIAL_SERVER_MODULE)
	if (service->is_server) {
	struct vs_server_serial_state *server = dev_get_drvdata(dev);
	return container_of(server, struct vtty_port, u.vs_server);
	}
	#endif
	#if defined(CONFIG_VSERVICES_SERIAL_CLIENT) \|\| \
	defined(CONFIG_VSERIVCES_SERIAL_CLIENT_MODULE)
	if (!service->is_server) {
	struct vs_client_serial_state *client = dev_get_drvdata(dev);
	return container_of(client, struct vtty_port, u.vs_client);
	}
	#endif
	/* should never get here */
	WARN_ON(1);
	return NULL;
	}

	static struct vtty_port port_from_tty(struct tty_struct tty)
	{
	return dev_to_port(tty->dev->parent);
	}

	static int vtty_install(struct tty_driver driver, struct tty_struct tty)
	{
	struct vtty_port *port;

	if (tty->index < 0 \|\| !test_bit(tty->index, alloced_ttys))
	return -ENXIO;

	port = port_from_tty(tty);

	if (!port)
	return -ENXIO;

	tty->driver_data = port;
	#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0)
	if (tty->port)
	tty->port->low_latency = 0;
	#else
	tty->low_latency = 0;
	#endif

	#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,7,0)
	tty_port_install(&port->port, driver, tty);
	#elif LINUX_VERSION_CODE >= KERNEL_VERSION(3,4,0)
	tty->port = &port->port;
	tty_standard_install(driver, tty);
	#else
	tty->port = &port->port;
	if (tty_init_termios(tty) != 0)
	return -ENOMEM;

	tty_driver_kref_get(driver);
	tty->count++;
	driver->ttys[tty->index] = tty;
	#endif

	return 0;
	}

	static int vtty_open(struct tty_struct tty, struct file file)
	{
	struct vtty_port *port = tty->driver_data;
	return tty_port_open(&port->port, tty, file);
	}

	static void vtty_close(struct tty_struct tty, struct file file)
	{
	struct vtty_port *port = tty->driver_data;
	if (port)
	tty_port_close(&port->port, tty, file);
	}

	static void vtty_shutdown(struct tty_port *port)
	{
	struct vtty_port *vtty_port =
	container_of(port, struct vtty_port, port);

	if (vtty_port->doing_release)
	kfree(port);
	}

	static int vtty_write_room(struct tty_struct *tty)
	{
	struct vtty_port *port = tty->driver_data;

	return vs_service_send_mbufs_available(port->service) *
	port->max_transfer_size;
	}

	static struct vs_mbuf vserial_alloc_send_buffer(struct vtty_port port,
	const unsigned char buf, size_t size, struct vs_pbuf pbuf,
	gfp_t gfp_flags)
	{
	struct vs_mbuf *mbuf;
	ssize_t ret;

	mbuf = port->ops.alloc_msg_buf(port, pbuf, gfp_flags);
	if (IS_ERR(mbuf)) {
	ret = PTR_ERR(mbuf);
	goto fail;
	}

	ret = vs_pbuf_resize(pbuf, size);
	if (ret < (ssize_t)size)
	goto fail_free_buf;

	ret = vs_pbuf_copyin(pbuf, 0, buf, size);
	if (ret < (ssize_t)size)
	goto fail_free_buf;

	return mbuf;

	fail_free_buf:
	port->ops.free_msg_buf(port, mbuf, pbuf);
	fail:
	return ERR_PTR(ret);
	}

	static int vtty_write(struct tty_struct tty, const unsigned char buf,
	int count)
	{
	struct vtty_port *port;
	size_t sent_bytes = 0, size;
	struct vs_mbuf *mbuf;
	struct vs_pbuf pbuf;
	int err;

	if (WARN_ON(!tty \|\| !buf))
	return -EINVAL;

	port = tty->driver_data;
	if (!port->ops.is_running(port)) {
	dev_dbg(&port->service->dev, "tty is not running!");
	return 0;
	}

	/*
	* We need to break our message up into chunks of
	* port->max_transfer_size.
	*/
	dev_dbg(&port->service->dev, "Writing %d bytes\n", count);
	while (sent_bytes < count) {
	size = min_t(size_t, count - sent_bytes,
	port->max_transfer_size);

	/*
	* Passing &port->u.vs_client here works for both the client
	* and the server since vs_client and vs_server are in the
	* same union, and therefore have the same address.
	*/
	mbuf = vs_service_waiting_alloc(&port->u.vs_client,
	vserial_alloc_send_buffer(port,
	buf + sent_bytes, size, &pbuf, GFP_KERNEL));
	if (IS_ERR(mbuf)) {
	dev_err(&port->service->dev,
	"Failed to alloc mbuf of %zu bytes: %ld - resetting service\n",
	size, PTR_ERR(mbuf));
	vs_service_reset(port->service, port->service);
	return -EIO;
	}

	vs_service_state_lock(port->service);
	err = port->ops.send_msg_buf(port, mbuf, &pbuf);
	vs_service_state_unlock(port->service);
	if (err) {
	port->ops.free_msg_buf(port, mbuf, &pbuf);
	dev_err(&port->service->dev,
	"send failed: %d - resetting service\n",
	err);
	vs_service_reset(port->service, port->service);
	return -EIO;
	}

	dev_dbg(&port->service->dev, "Sent %zu bytes (%zu/%d)\n",
	size, sent_bytes + size, count);
	sent_bytes += size;
	}

	dev_dbg(&port->service->dev, "Write complete - sent %zu/%d bytes\n",
	sent_bytes, count);
	return sent_bytes;
	}

	static int vtty_put_char(struct tty_struct *tty, unsigned char ch)
	{
	return vtty_write(tty, &ch, 1);
	}

	static size_t vs_serial_send_pbuf_to_tty(struct vtty_port *port,
	struct vs_pbuf *pbuf, size_t offset)
	{
	struct tty_struct *tty = tty_port_tty_get(&port->port);
	size_t space, size;

	lockdep_assert_held(&port->in_lock);

	size = vs_pbuf_size(pbuf) - offset;
	#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0)
	space = tty_buffer_request_room(tty->port, size);
	#else
	space = tty_buffer_request_room(tty, size);
	#endif
	if (space) {
	#if LINUX_VERSION_CODE >= KERNEL_VERSION(3,9,0)
	tty_insert_flip_string(tty->port, pbuf->data + offset, space);
	tty_flip_buffer_push(tty->port);
	#else
	tty_insert_flip_string(tty, pbuf->data + offset, space);
	tty_flip_buffer_push(tty);
	#endif
	}

	tty_kref_put(tty);

	/* Return the number of bytes written */
	return space;
	}

	static void vtty_throttle(struct tty_struct *tty)
	{
	struct vtty_port *port = tty->driver_data;

	dev_dbg(&port->service->dev, "throttle\n");

	spin_lock_bh(&port->in_lock);
	port->tty_canrecv = false;
	spin_unlock_bh(&port->in_lock);
	}

	static void vtty_unthrottle(struct tty_struct *tty)
	{
	struct vtty_port *port = tty->driver_data;
	struct vtty_in_packet *packet;
	struct vs_mbuf *mbuf;
	size_t sent;

	dev_dbg(&port->service->dev, "unthrottle\n");

	spin_lock_bh(&port->in_lock);

	while (!list_empty(&port->pending_in_packets)) {
	mbuf = list_first_entry(&port->pending_in_packets,
	struct vs_mbuf, queue);
	packet = mbuf->priv;

	sent = vs_serial_send_pbuf_to_tty(port, &packet->pbuf,
	packet->offset);
	packet->offset += sent;
	if (packet->offset < vs_pbuf_size(&packet->pbuf)) {
	/*
	* Only wrote part of the buffer. This means that we
	* still have pending data that cannot be written to
	* the tty at this time. The tty layer will rethrottle
	* and this function will be called again when the tty
	* layer is next able to handle data and we can write
	* the remainder of the buffer.
	*/
	dev_dbg(&port->service->dev,
	"unthrottle: Only wrote %zu (%zu/%zu) bytes\n",
	sent, packet->offset,
	vs_pbuf_size(&packet->pbuf));
	break;
	}

	dev_dbg(&port->service->dev,
	"unthrottle: wrote %zu (%zu/%zu) bytes\n",
	sent, packet->offset,
	vs_pbuf_size(&packet->pbuf));

	/* Wrote the whole buffer - free it */
	list_del(&mbuf->queue);
	port->ops.free_msg_buf(port, mbuf, &packet->pbuf);
	kfree(packet);
	}

	port->tty_canrecv = true;
	spin_unlock_bh(&port->in_lock);
	}

	static struct tty_port_operations vtty_port_ops = {
	.shutdown = vtty_shutdown,
	};

	static struct tty_operations vtty_ops = {
	.install = vtty_install,
	.open = vtty_open,
	.close = vtty_close,
	.write = vtty_write,
	.write_room = vtty_write_room,
	.put_char = vtty_put_char,
	.throttle = vtty_throttle,
	.unthrottle = vtty_unthrottle
	};

	static int vs_serial_queue_incoming_packet(struct vtty_port *port,
	struct vs_mbuf mbuf, struct vs_pbuf pbuf, size_t offset)
	{
	struct vtty_in_packet *packet;

	lockdep_assert_held(&port->in_lock);

	packet = kzalloc(sizeof(*packet), GFP_ATOMIC);
	if (!packet) {
	/*
	* Uh oh, we are seriously out of memory. The incoming data
	* will be lost.
	*/
	return -ENOMEM;
	}

	dev_dbg(&port->service->dev, "Queuing packet %zu bytes, offset %zu\n",
	vs_pbuf_size(pbuf), offset);
	mbuf->priv = packet;
	memcpy(&packet->pbuf, pbuf, sizeof(*pbuf));
	packet->offset = offset;

	list_add_tail(&mbuf->queue, &port->pending_in_packets);
	return 0;
	}

	int vs_serial_handle_message(struct vtty_port port, struct vs_mbuf mbuf,
	struct vs_pbuf *pbuf)
	{
	struct tty_struct *tty = tty_port_tty_get(&port->port);
	bool queue_packet = false;
	size_t sent = 0;
	int err;

	if (!tty) {
	dev_dbg(&port->service->dev,
	"tty not open. Dropping %zu chars\n",
	pbuf->size);
	port->ops.free_msg_buf(port, mbuf, pbuf);
	return 0;
	}

	dev_dbg(&port->service->dev, "Incoming message - len = %zu\n",
	pbuf->size);

	spin_lock(&port->in_lock);
	if (!port->tty_canrecv \|\| !list_empty(&port->pending_in_packets)) {
	/*
	* We cannot send to the tty right now, either because we are
	* being throttled or because we still have pending data
	* to write out to the tty. Queue the buffer up so we can
	* write it later.
	*/
	dev_dbg(&port->service->dev,
	"Cannot send (canrecv = %d, queued = %d) - queuing message\n",
	port->tty_canrecv,
	!list_empty(&port->pending_in_packets));
	queue_packet = true;

	} else {
	sent = vs_serial_send_pbuf_to_tty(port, pbuf, 0);
	if (sent < vs_pbuf_size(pbuf)) {
	/*
	* Only wrote part of the buffer to the tty. Queue
	* the buffer to write the rest.
	*/
	dev_dbg(&port->service->dev,
	"Sent %zu/%zu bytes to tty - queueing rest\n",
	sent, vs_pbuf_size(pbuf));
	queue_packet = true;
	}
	}

	if (queue_packet) {
	/*
	* Queue the incoming data up. If we are not already throttled,
	* the tty layer will do so now since it has no room in its
	* buffers.
	*/
	err = vs_serial_queue_incoming_packet(port, mbuf, pbuf, sent);
	if (err) {
	dev_err(&port->service->dev,
	"Failed to queue packet - dropping chars\n");
	port->ops.free_msg_buf(port, mbuf, pbuf);
	}

	} else {
	port->ops.free_msg_buf(port, mbuf, pbuf);
	}

	spin_unlock(&port->in_lock);
	tty_kref_put(tty);

	return 0;
	}
	EXPORT_SYMBOL_GPL(vs_serial_handle_message);

	#ifdef CONFIG_OKL4_VTTY_CONSOLE
	static int vconsole_setup(struct console co, char options)
	{
	if (co->index < 0 \|\| co->index >= max_ttys)
	co->index = 0;

	pr_info("OKL4 virtual console init\n");

	return 0;
	}

	static void vconsole_write(struct console co, const char p, unsigned count)
	{
	}

	static struct tty_driver vconsole_device(struct console co, int *index)
	{
	*index = co->index;

	return vtty_driver;
	}
	#endif /* CONFIG_OKL4_VTTY_CONSOLE */

	static void vs_serial_free_buffers(struct vtty_port *port)
	{
	struct vtty_in_packet *packet;
	struct vs_mbuf *mbuf;

	/* Free the list of incoming buffers */
	spin_lock_bh(&port->in_lock);
	while (!list_empty(&port->pending_in_packets)) {
	mbuf = list_first_entry(&port->pending_in_packets,
	struct vs_mbuf, queue);
	packet = mbuf->priv;

	list_del(&mbuf->queue);
	port->ops.free_msg_buf(port, mbuf, &packet->pbuf);
	kfree(packet);
	}
	spin_unlock_bh(&port->in_lock);
	}

	/ vservices callbacks /
	struct vtty_port vs_serial_alloc_port(struct vs_service_device service,
	struct vtty_port_ops *port_ops)
	{
	struct vtty_port *port;
	int port_num;

	mutex_lock(&tty_bitmap_lock);
	port_num = find_first_zero_bit(alloced_ttys, max_ttys);

	if (port_num >= max_ttys) {
	mutex_unlock(&tty_bitmap_lock);
	return NULL;
	}

	port = kzalloc(sizeof(struct vtty_port), GFP_KERNEL);
	if (!port) {
	mutex_unlock(&tty_bitmap_lock);
	return NULL;
	}

	port->service = service;
	port->ops = *port_ops;
	port->tty_canrecv = true;
	port->port_num = port_num;
	INIT_LIST_HEAD(&port->pending_in_packets);
	spin_lock_init(&port->in_lock);
	#ifdef CONFIG_OKL4_VTTY_CONSOLE
	/* Set up and register the port's console device */
	strlcpy(port->console.name, "vconvs", sizeof(port->console.name));
	port->console.write = vconsole_write;
	port->console.flags = CON_PRINTBUFFER;
	port->console.device = vconsole_device;
	port->console.setup = vconsole_setup;
	port->console.index = port_num;

	register_console(&port->console);
	#endif
	port->vtty_driver = vtty_driver;

	tty_port_init(&port->port);
	port->port.ops = &vtty_port_ops;

	tty_register_device(vtty_driver, port_num, &service->dev);
	bitmap_set(alloced_ttys, port_num, 1);
	mutex_unlock(&tty_bitmap_lock);

	return port;
	}
	EXPORT_SYMBOL(vs_serial_alloc_port);

	void vs_serial_release(struct vtty_port *port)
	{
	dev_dbg(&port->service->dev, "Release\n");

	#ifdef CONFIG_OKL4_VTTY_CONSOLE
	unregister_console(&port->console);
	#endif

	mutex_lock(&tty_bitmap_lock);
	bitmap_clear(alloced_ttys, port->port_num, 1);
	mutex_unlock(&tty_bitmap_lock);

	if (port->port.tty) {
	tty_vhangup(port->port.tty);
	tty_kref_put(port->port.tty);
	}

	vs_serial_free_buffers(port);
	port->doing_release = true;
	tty_unregister_device(vtty_driver, port->port_num);
	}
	EXPORT_SYMBOL_GPL(vs_serial_release);

	void vs_serial_reset(struct vtty_port *port)
	{
	/* Free list of in and out mbufs. */
	vs_serial_free_buffers(port);
	}
	EXPORT_SYMBOL_GPL(vs_serial_reset);

	static int __init vs_serial_init(void)
	{
	int err;

	if (max_ttys == 0)
	return -EINVAL;

	alloced_ttys = kzalloc(sizeof(unsigned long) * BITS_TO_LONGS(max_ttys),
	GFP_KERNEL);
	if (!alloced_ttys) {
	err = -ENOMEM;
	goto fail_alloc_ttys;
	}

	/* Set up the tty driver. */
	vtty_driver = alloc_tty_driver(max_ttys);
	if (!vtty_driver) {
	err = -ENOMEM;
	goto fail_alloc_tty_driver;
	}

	vtty_driver->owner = THIS_MODULE;
	vtty_driver->driver_name = "okl4-vservices-serial";
	vtty_driver->name = "ttyVS";
	vtty_driver->type = TTY_DRIVER_TYPE_SERIAL;
	vtty_driver->subtype = SERIAL_TYPE_NORMAL;
	vtty_driver->flags = TTY_DRIVER_REAL_RAW \| TTY_DRIVER_DYNAMIC_DEV;
	vtty_driver->init_termios = tty_std_termios;
	vtty_driver->num = max_ttys;

	/* These flags don't really matter; just use sensible defaults. */
	vtty_driver->init_termios.c_cflag =
	B9600 \| CS8 \| CREAD \| HUPCL \| CLOCAL;
	vtty_driver->init_termios.c_ispeed = 9600;
	vtty_driver->init_termios.c_ospeed = 9600;

	tty_set_operations(vtty_driver, &vtty_ops);

	err = tty_register_driver(vtty_driver);
	if (err)
	goto fail_tty_driver_register;

	return 0;

	fail_tty_driver_register:
	put_tty_driver(vtty_driver);
	fail_alloc_tty_driver:
	kfree(alloced_ttys);
	fail_alloc_ttys:
	return err;
	}

	static void __exit vs_serial_exit(void)
	{
	tty_unregister_driver(vtty_driver);
	put_tty_driver(vtty_driver);
	}

	module_init(vs_serial_init);
	module_exit(vs_serial_exit);

	MODULE_DESCRIPTION("OKL4 Virtual Services Serial Core Driver");
	MODULE_AUTHOR("Open Kernel Labs, Inc");