drivers/char/msm_smd_pkt.c - kernel/msm-4.9 - Gitiles

 /* Copyright (c) 2008-2010, Code Aurora Forum. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
  * only version 2 as published by the Free Software Foundation.
  *
  * 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., 51 Franklin Street, Fifth Floor, Boston, MA
  * 02110-1301, USA.
  *
  */
 /*
  * SMD Packet Driver -- Provides userspace interface to SMD packet ports.
  */

 #include <linux/slab.h>
 #include <linux/cdev.h>
 #include <linux/module.h>
 #include <linux/fs.h>
 #include <linux/device.h>
 #include <linux/sched.h>
 #include <linux/mutex.h>
 #include <linux/delay.h>
 #include <linux/uaccess.h>
 #include <linux/workqueue.h>
 #include <linux/poll.h>

 #include <mach/msm_smd.h>

 #define NUM_SMD_PKT_PORTS 9
 #define DEVICE_NAME "smdpkt"
 #define MAX_BUF_SIZE 2048

 struct smd_pkt_dev {
 	struct cdev cdev;
 	struct device *devicep;

 	struct smd_channel *ch;
 	int open_count;
 	struct mutex ch_lock;
 	struct mutex rx_lock;
 	struct mutex tx_lock;
 	wait_queue_head_t ch_read_wait_queue;
 	wait_queue_head_t ch_opened_wait_queue;

 	int i;

 	unsigned char tx_buf[MAX_BUF_SIZE];
 	unsigned char rx_buf[MAX_BUF_SIZE];
 	int remote_open;

 } *smd_pkt_devp[NUM_SMD_PKT_PORTS];

 struct class *smd_pkt_classp;
 static dev_t smd_pkt_number;

 static int msm_smd_pkt_debug_enable;
 module_param_named(debug_enable, msm_smd_pkt_debug_enable,
 		   int, S_IRUGO | S_IWUSR | S_IWGRP);

 #ifdef DEBUG
 #define D_DUMP_BUFFER(prestr, cnt, buf) do {			\
 		int i;						\
 		if (msm_smd_pkt_debug_enable) {			\
 			pr_debug("%s", prestr);			\
 			for (i = 0; i < cnt; i++)		\
 				pr_debug("%.2x", buf[i]);	\
 			pr_debug("\n");				\
 		}						\
 	} while (0)
 #else
 #define D_DUMP_BUFFER(prestr, cnt, buf) do {} while (0)
 #endif

 #ifdef DEBUG
 #define DBG(x...) do {			\
 	if (msm_smd_pkt_debug_enable)	\
 		pr_debug(x);		\
 	} while (0)
 #else
 #define DBG(x...) do {} while (0)
 #endif

 static void check_and_wakeup_reader(struct smd_pkt_dev *smd_pkt_devp)
 {
 	int sz;

 	if (!smd_pkt_devp || !smd_pkt_devp->ch)
 		return;

 	sz = smd_cur_packet_size(smd_pkt_devp->ch);
 	if (sz == 0) {
 		DBG("no packet\n");
 		return;
 	}
 	if (sz > smd_read_avail(smd_pkt_devp->ch)) {
 		DBG("incomplete packet\n");
 		return;
 	}

 	DBG("waking up reader\n");
 	wake_up_interruptible(&smd_pkt_devp->ch_read_wait_queue);
 }

 static int smd_pkt_read(struct file *file, char __user *buf,
 			size_t count, loff_t *ppos)
 {
 	int r, bytes_read;
 	struct smd_pkt_dev *smd_pkt_devp;
 	struct smd_channel *chl;

 	DBG("read %d bytes\n", count);
 	if (count > MAX_BUF_SIZE)
 		return -EINVAL;

 	smd_pkt_devp = file->private_data;
 	if (!smd_pkt_devp || !smd_pkt_devp->ch)
 		return -EINVAL;

 	chl = smd_pkt_devp->ch;
 wait_for_packet:
 	r = wait_event_interruptible(smd_pkt_devp->ch_read_wait_queue,
 				     (smd_cur_packet_size(chl) > 0 &&
 				      smd_read_avail(chl) >=
 				      smd_cur_packet_size(chl)));

 	if (r < 0) {
 		if (r != -ERESTARTSYS)
 			pr_err("wait returned %d\n", r);
 		return r;
 	}

 	mutex_lock(&smd_pkt_devp->rx_lock);

 	bytes_read = smd_cur_packet_size(smd_pkt_devp->ch);
 	if (bytes_read == 0 ||
 	    bytes_read < smd_read_avail(smd_pkt_devp->ch)) {
 		mutex_unlock(&smd_pkt_devp->rx_lock);
 		DBG("Nothing to read\n");
 		goto wait_for_packet;
 	}

 	if (bytes_read > count) {
 		mutex_unlock(&smd_pkt_devp->rx_lock);
 		pr_info("packet size %d > buffer size %d", bytes_read, count);
 		return -EINVAL;
 	}

 	r = smd_read(smd_pkt_devp->ch, smd_pkt_devp->rx_buf, bytes_read);
 	if (r != bytes_read) {
 		mutex_unlock(&smd_pkt_devp->rx_lock);
 		pr_err("smd_read failed to read %d bytes: %d\n", bytes_read, r);
 		return -EIO;
 	}

 	D_DUMP_BUFFER("read: ", bytes_read, smd_pkt_devp->rx_buf);
 	r = copy_to_user(buf, smd_pkt_devp->rx_buf, bytes_read);
 	mutex_unlock(&smd_pkt_devp->rx_lock);
 	if (r) {
 		pr_err("copy_to_user failed %d\n", r);
 		return -EFAULT;
 	}

 	DBG("read complete %d bytes\n", bytes_read);
 	check_and_wakeup_reader(smd_pkt_devp);

 	return bytes_read;
 }

 static int smd_pkt_write(struct file *file, const char __user *buf,
 			 size_t count, loff_t *ppos)
 {
 	int r;
 	struct smd_pkt_dev *smd_pkt_devp;

 	if (count > MAX_BUF_SIZE)
 		return -EINVAL;

 	DBG("writting %d bytes\n", count);

 	smd_pkt_devp = file->private_data;
 	if (!smd_pkt_devp || !smd_pkt_devp->ch)
 		return -EINVAL;

 	mutex_lock(&smd_pkt_devp->tx_lock);
 	if (smd_write_avail(smd_pkt_devp->ch) < count) {
 		mutex_unlock(&smd_pkt_devp->tx_lock);
 		DBG("Not enough space to write\n");
 		return -ENOMEM;
 	}

 	D_DUMP_BUFFER("write: ", count, buf);
 	r = copy_from_user(smd_pkt_devp->tx_buf, buf, count);
 	if (r) {
 		mutex_unlock(&smd_pkt_devp->tx_lock);
 		pr_err("copy_from_user failed %d\n", r);
 		return -EFAULT;
 	}

 	r = smd_write(smd_pkt_devp->ch, smd_pkt_devp->tx_buf, count);
 	if (r != count) {
 		mutex_unlock(&smd_pkt_devp->tx_lock);
 		pr_err("smd_write failed to write %d bytes: %d.\n", count, r);
 		return -EIO;
 	}
 	mutex_unlock(&smd_pkt_devp->tx_lock);

 	DBG("wrote %d bytes\n", count);
 	return count;
 }

 static unsigned int smd_pkt_poll(struct file *file, poll_table *wait)
 {
 	struct smd_pkt_dev *smd_pkt_devp;
 	unsigned int mask = 0;

 	smd_pkt_devp = file->private_data;
 	if (!smd_pkt_devp)
 		return POLLERR;

 	DBG("poll waiting\n");
 	poll_wait(file, &smd_pkt_devp->ch_read_wait_queue, wait);
 	if (smd_read_avail(smd_pkt_devp->ch))
 		mask |= POLLIN | POLLRDNORM;

 	DBG("poll return\n");
 	return mask;
 }

 static void smd_pkt_ch_notify(void *priv, unsigned event)
 {
 	struct smd_pkt_dev *smd_pkt_devp = priv;

 	if (smd_pkt_devp->ch == 0)
 		return;

 	switch (event) {
 	case SMD_EVENT_DATA:
 		DBG("data\n");
 		check_and_wakeup_reader(smd_pkt_devp);
 		break;

 	case SMD_EVENT_OPEN:
 		DBG("remote open\n");
 		smd_pkt_devp->remote_open = 1;
 		wake_up_interruptible(&smd_pkt_devp->ch_opened_wait_queue);
 		break;

 	case SMD_EVENT_CLOSE:
 		smd_pkt_devp->remote_open = 0;
 		pr_info("remote closed\n");
 		break;

 	default:
 		pr_err("unknown event %d\n", event);
 		break;
 	}
 }

 static char *smd_pkt_dev_name[] = {
 	"smdcntl0",
 	"smdcntl1",
 	"smdcntl2",
 	"smdcntl3",
 	"smdcntl4",
 	"smdcntl5",
 	"smdcntl6",
 	"smdcntl7",
 	"smd22",
 };

 static char *smd_ch_name[] = {
 	"DATA5_CNTL",
 	"DATA6_CNTL",
 	"DATA7_CNTL",
 	"DATA8_CNTL",
 	"DATA9_CNTL",
 	"DATA12_CNTL",
 	"DATA13_CNTL",
 	"DATA14_CNTL",
 	"DATA22",
 };

 static int smd_pkt_open(struct inode *inode, struct file *file)
 {
 	int r = 0;
 	struct smd_pkt_dev *smd_pkt_devp;

 	smd_pkt_devp = container_of(inode->i_cdev, struct smd_pkt_dev, cdev);
 	if (!smd_pkt_devp)
 		return -EINVAL;

 	file->private_data = smd_pkt_devp;

 	mutex_lock(&smd_pkt_devp->ch_lock);
 	if (smd_pkt_devp->open_count == 0) {
 		r = smd_open(smd_ch_name[smd_pkt_devp->i],
 			     &smd_pkt_devp->ch, smd_pkt_devp,
 			     smd_pkt_ch_notify);
 		if (r < 0) {
 			pr_err("smd_open failed for %s, %d\n",
 			       smd_ch_name[smd_pkt_devp->i], r);
 			goto out;
 		}

 		r = wait_event_interruptible_timeout(
 				smd_pkt_devp->ch_opened_wait_queue,
 				smd_pkt_devp->remote_open,
 				msecs_to_jiffies(2 * HZ));
 		if (r == 0)
 			r = -ETIMEDOUT;

 		if (r < 0) {
 			pr_err("wait returned %d\n", r);
 			smd_close(smd_pkt_devp->ch);
 			smd_pkt_devp->ch = 0;
 		} else {
 			smd_pkt_devp->open_count++;
 			r = 0;
 		}
 	}
 out:
 	mutex_unlock(&smd_pkt_devp->ch_lock);
 	return r;
 }

 static int smd_pkt_release(struct inode *inode, struct file *file)
 {
 	int r = 0;
 	struct smd_pkt_dev *smd_pkt_devp = file->private_data;

 	if (!smd_pkt_devp)
 		return -EINVAL;

 	mutex_lock(&smd_pkt_devp->ch_lock);
 	if (--smd_pkt_devp->open_count == 0) {
 		r = smd_close(smd_pkt_devp->ch);
 		smd_pkt_devp->ch = 0;
 	}
 	mutex_unlock(&smd_pkt_devp->ch_lock);

 	return r;
 }

 static const struct file_operations smd_pkt_fops = {
 	.owner = THIS_MODULE,
 	.open = smd_pkt_open,
 	.release = smd_pkt_release,
 	.read = smd_pkt_read,
 	.write = smd_pkt_write,
 	.poll = smd_pkt_poll,
 };

 static int __init smd_pkt_init(void)
 {
 	int i;
 	int r;

 	r = alloc_chrdev_region(&smd_pkt_number, 0,
 				NUM_SMD_PKT_PORTS, DEVICE_NAME);
 	if (r) {
 		pr_err("alloc_chrdev_region() failed %d\n", r);
 		return r;
 	}

 	smd_pkt_classp = class_create(THIS_MODULE, DEVICE_NAME);
 	if (IS_ERR(smd_pkt_classp)) {
 		r = PTR_ERR(smd_pkt_classp);
 		pr_err("class_create() failed %d\n", r);
 		goto unreg_chardev;
 	}

 	for (i = 0; i < NUM_SMD_PKT_PORTS; ++i) {
 		smd_pkt_devp[i] = kzalloc(sizeof(struct smd_pkt_dev),
 					  GFP_KERNEL);
 		if (!smd_pkt_devp[i]) {
 			pr_err("kmalloc() failed\n");
 			goto clean_cdevs;
 		}

 		smd_pkt_devp[i]->i = i;

 		init_waitqueue_head(&smd_pkt_devp[i]->ch_read_wait_queue);
 		smd_pkt_devp[i]->remote_open = 0;
 		init_waitqueue_head(&smd_pkt_devp[i]->ch_opened_wait_queue);

 		mutex_init(&smd_pkt_devp[i]->ch_lock);
 		mutex_init(&smd_pkt_devp[i]->rx_lock);
 		mutex_init(&smd_pkt_devp[i]->tx_lock);

 		cdev_init(&smd_pkt_devp[i]->cdev, &smd_pkt_fops);
 		smd_pkt_devp[i]->cdev.owner = THIS_MODULE;

 		r = cdev_add(&smd_pkt_devp[i]->cdev,
 			     (smd_pkt_number + i), 1);
 		if (r) {
 			pr_err("cdev_add() failed %d\n", r);
 			kfree(smd_pkt_devp[i]);
 			goto clean_cdevs;
 		}

 		smd_pkt_devp[i]->devicep =
 			device_create(smd_pkt_classp, NULL,
 				      (smd_pkt_number + i), NULL,
 				      smd_pkt_dev_name[i]);
 		if (IS_ERR(smd_pkt_devp[i]->devicep)) {
 			r = PTR_ERR(smd_pkt_devp[i]->devicep);
 			pr_err("device_create() failed %d\n", r);
 			cdev_del(&smd_pkt_devp[i]->cdev);
 			kfree(smd_pkt_devp[i]);
 			goto clean_cdevs;
 		}

 	}

 	pr_info("SMD Packet Port Driver Initialized.\n");
 	return 0;

 clean_cdevs:
 	if (i > 0) {
 		while (--i >= 0) {
 			mutex_destroy(&smd_pkt_devp[i]->ch_lock);
 			mutex_destroy(&smd_pkt_devp[i]->rx_lock);
 			mutex_destroy(&smd_pkt_devp[i]->tx_lock);
 			cdev_del(&smd_pkt_devp[i]->cdev);
 			kfree(smd_pkt_devp[i]);
 			device_destroy(smd_pkt_classp,
 				       MKDEV(MAJOR(smd_pkt_number), i));
 		}
 	}

 	class_destroy(smd_pkt_classp);
 unreg_chardev:
 	unregister_chrdev_region(MAJOR(smd_pkt_number), NUM_SMD_PKT_PORTS);
 	return r;
 }
 module_init(smd_pkt_init);

 static void __exit smd_pkt_cleanup(void)
 {
 	int i;

 	for (i = 0; i < NUM_SMD_PKT_PORTS; ++i) {
 		mutex_destroy(&smd_pkt_devp[i]->ch_lock);
 		mutex_destroy(&smd_pkt_devp[i]->rx_lock);
 		mutex_destroy(&smd_pkt_devp[i]->tx_lock);
 		cdev_del(&smd_pkt_devp[i]->cdev);
 		kfree(smd_pkt_devp[i]);
 		device_destroy(smd_pkt_classp,
 			       MKDEV(MAJOR(smd_pkt_number), i));
 	}

 	class_destroy(smd_pkt_classp);
 	unregister_chrdev_region(MAJOR(smd_pkt_number), NUM_SMD_PKT_PORTS);
 }
 module_exit(smd_pkt_cleanup);

 MODULE_DESCRIPTION("MSM Shared Memory Packet Port");
 MODULE_LICENSE("GPL v2");
	/* Copyright (c) 2008-2010, Code Aurora Forum. All rights reserved.
	*
	* This program is free software; you can redistribute it and/or modify
	* it under the terms of the GNU General Public License version 2 and
	* only version 2 as published by the Free Software Foundation.
	*
	* 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., 51 Franklin Street, Fifth Floor, Boston, MA
	* 02110-1301, USA.
	*
	*/
	/*
	* SMD Packet Driver -- Provides userspace interface to SMD packet ports.
	*/

	#include <linux/slab.h>
	#include <linux/cdev.h>
	#include <linux/module.h>
	#include <linux/fs.h>
	#include <linux/device.h>
	#include <linux/sched.h>
	#include <linux/mutex.h>
	#include <linux/delay.h>
	#include <linux/uaccess.h>
	#include <linux/workqueue.h>
	#include <linux/poll.h>

	#include <mach/msm_smd.h>

	#define NUM_SMD_PKT_PORTS 9
	#define DEVICE_NAME "smdpkt"
	#define MAX_BUF_SIZE 2048

	struct smd_pkt_dev {
	struct cdev cdev;
	struct device *devicep;

	struct smd_channel *ch;
	int open_count;
	struct mutex ch_lock;
	struct mutex rx_lock;
	struct mutex tx_lock;
	wait_queue_head_t ch_read_wait_queue;
	wait_queue_head_t ch_opened_wait_queue;

	int i;

	unsigned char tx_buf[MAX_BUF_SIZE];
	unsigned char rx_buf[MAX_BUF_SIZE];
	int remote_open;

	} *smd_pkt_devp[NUM_SMD_PKT_PORTS];

	struct class *smd_pkt_classp;
	static dev_t smd_pkt_number;

	static int msm_smd_pkt_debug_enable;
	module_param_named(debug_enable, msm_smd_pkt_debug_enable,
	int, S_IRUGO \| S_IWUSR \| S_IWGRP);

	#ifdef DEBUG
	#define D_DUMP_BUFFER(prestr, cnt, buf) do { \
	int i; \
	if (msm_smd_pkt_debug_enable) { \
	pr_debug("%s", prestr); \
	for (i = 0; i < cnt; i++) \
	pr_debug("%.2x", buf[i]); \
	pr_debug("\n"); \
	} \
	} while (0)
	#else
	#define D_DUMP_BUFFER(prestr, cnt, buf) do {} while (0)
	#endif

	#ifdef DEBUG
	#define DBG(x...) do { \
	if (msm_smd_pkt_debug_enable) \
	pr_debug(x); \
	} while (0)
	#else
	#define DBG(x...) do {} while (0)
	#endif

	static void check_and_wakeup_reader(struct smd_pkt_dev *smd_pkt_devp)
	{
	int sz;

	if (!smd_pkt_devp \|\| !smd_pkt_devp->ch)
	return;

	sz = smd_cur_packet_size(smd_pkt_devp->ch);
	if (sz == 0) {
	DBG("no packet\n");
	return;
	}
	if (sz > smd_read_avail(smd_pkt_devp->ch)) {
	DBG("incomplete packet\n");
	return;
	}

	DBG("waking up reader\n");
	wake_up_interruptible(&smd_pkt_devp->ch_read_wait_queue);
	}

	static int smd_pkt_read(struct file file, char __user buf,
	size_t count, loff_t *ppos)
	{
	int r, bytes_read;
	struct smd_pkt_dev *smd_pkt_devp;
	struct smd_channel *chl;

	DBG("read %d bytes\n", count);
	if (count > MAX_BUF_SIZE)
	return -EINVAL;

	smd_pkt_devp = file->private_data;
	if (!smd_pkt_devp \|\| !smd_pkt_devp->ch)
	return -EINVAL;

	chl = smd_pkt_devp->ch;
	wait_for_packet:
	r = wait_event_interruptible(smd_pkt_devp->ch_read_wait_queue,
	(smd_cur_packet_size(chl) > 0 &&
	smd_read_avail(chl) >=
	smd_cur_packet_size(chl)));

	if (r < 0) {
	if (r != -ERESTARTSYS)
	pr_err("wait returned %d\n", r);
	return r;
	}

	mutex_lock(&smd_pkt_devp->rx_lock);

	bytes_read = smd_cur_packet_size(smd_pkt_devp->ch);
	if (bytes_read == 0 \|\|
	bytes_read < smd_read_avail(smd_pkt_devp->ch)) {
	mutex_unlock(&smd_pkt_devp->rx_lock);
	DBG("Nothing to read\n");
	goto wait_for_packet;
	}

	if (bytes_read > count) {
	mutex_unlock(&smd_pkt_devp->rx_lock);
	pr_info("packet size %d > buffer size %d", bytes_read, count);
	return -EINVAL;
	}

	r = smd_read(smd_pkt_devp->ch, smd_pkt_devp->rx_buf, bytes_read);
	if (r != bytes_read) {
	mutex_unlock(&smd_pkt_devp->rx_lock);
	pr_err("smd_read failed to read %d bytes: %d\n", bytes_read, r);
	return -EIO;
	}

	D_DUMP_BUFFER("read: ", bytes_read, smd_pkt_devp->rx_buf);
	r = copy_to_user(buf, smd_pkt_devp->rx_buf, bytes_read);
	mutex_unlock(&smd_pkt_devp->rx_lock);
	if (r) {
	pr_err("copy_to_user failed %d\n", r);
	return -EFAULT;
	}

	DBG("read complete %d bytes\n", bytes_read);
	check_and_wakeup_reader(smd_pkt_devp);

	return bytes_read;
	}

	static int smd_pkt_write(struct file file, const char __user buf,
	size_t count, loff_t *ppos)
	{
	int r;
	struct smd_pkt_dev *smd_pkt_devp;

	if (count > MAX_BUF_SIZE)
	return -EINVAL;

	DBG("writting %d bytes\n", count);

	smd_pkt_devp = file->private_data;
	if (!smd_pkt_devp \|\| !smd_pkt_devp->ch)
	return -EINVAL;

	mutex_lock(&smd_pkt_devp->tx_lock);
	if (smd_write_avail(smd_pkt_devp->ch) < count) {
	mutex_unlock(&smd_pkt_devp->tx_lock);
	DBG("Not enough space to write\n");
	return -ENOMEM;
	}

	D_DUMP_BUFFER("write: ", count, buf);
	r = copy_from_user(smd_pkt_devp->tx_buf, buf, count);
	if (r) {
	mutex_unlock(&smd_pkt_devp->tx_lock);
	pr_err("copy_from_user failed %d\n", r);
	return -EFAULT;
	}

	r = smd_write(smd_pkt_devp->ch, smd_pkt_devp->tx_buf, count);
	if (r != count) {
	mutex_unlock(&smd_pkt_devp->tx_lock);
	pr_err("smd_write failed to write %d bytes: %d.\n", count, r);
	return -EIO;
	}
	mutex_unlock(&smd_pkt_devp->tx_lock);

	DBG("wrote %d bytes\n", count);
	return count;
	}

	static unsigned int smd_pkt_poll(struct file file, poll_table wait)
	{
	struct smd_pkt_dev *smd_pkt_devp;
	unsigned int mask = 0;

	smd_pkt_devp = file->private_data;
	if (!smd_pkt_devp)
	return POLLERR;

	DBG("poll waiting\n");
	poll_wait(file, &smd_pkt_devp->ch_read_wait_queue, wait);
	if (smd_read_avail(smd_pkt_devp->ch))
	mask \|= POLLIN \| POLLRDNORM;

	DBG("poll return\n");
	return mask;
	}

	static void smd_pkt_ch_notify(void *priv, unsigned event)
	{
	struct smd_pkt_dev *smd_pkt_devp = priv;

	if (smd_pkt_devp->ch == 0)
	return;

	switch (event) {
	case SMD_EVENT_DATA:
	DBG("data\n");
	check_and_wakeup_reader(smd_pkt_devp);
	break;

	case SMD_EVENT_OPEN:
	DBG("remote open\n");
	smd_pkt_devp->remote_open = 1;
	wake_up_interruptible(&smd_pkt_devp->ch_opened_wait_queue);
	break;

	case SMD_EVENT_CLOSE:
	smd_pkt_devp->remote_open = 0;
	pr_info("remote closed\n");
	break;

	default:
	pr_err("unknown event %d\n", event);
	break;
	}
	}

	static char *smd_pkt_dev_name[] = {
	"smdcntl0",
	"smdcntl1",
	"smdcntl2",
	"smdcntl3",
	"smdcntl4",
	"smdcntl5",
	"smdcntl6",
	"smdcntl7",
	"smd22",
	};

	static char *smd_ch_name[] = {
	"DATA5_CNTL",
	"DATA6_CNTL",
	"DATA7_CNTL",
	"DATA8_CNTL",
	"DATA9_CNTL",
	"DATA12_CNTL",
	"DATA13_CNTL",
	"DATA14_CNTL",
	"DATA22",
	};

	static int smd_pkt_open(struct inode inode, struct file file)
	{
	int r = 0;
	struct smd_pkt_dev *smd_pkt_devp;

	smd_pkt_devp = container_of(inode->i_cdev, struct smd_pkt_dev, cdev);
	if (!smd_pkt_devp)
	return -EINVAL;

	file->private_data = smd_pkt_devp;

	mutex_lock(&smd_pkt_devp->ch_lock);
	if (smd_pkt_devp->open_count == 0) {
	r = smd_open(smd_ch_name[smd_pkt_devp->i],
	&smd_pkt_devp->ch, smd_pkt_devp,
	smd_pkt_ch_notify);
	if (r < 0) {
	pr_err("smd_open failed for %s, %d\n",
	smd_ch_name[smd_pkt_devp->i], r);
	goto out;
	}

	r = wait_event_interruptible_timeout(
	smd_pkt_devp->ch_opened_wait_queue,
	smd_pkt_devp->remote_open,
	msecs_to_jiffies(2 * HZ));
	if (r == 0)
	r = -ETIMEDOUT;

	if (r < 0) {
	pr_err("wait returned %d\n", r);
	smd_close(smd_pkt_devp->ch);
	smd_pkt_devp->ch = 0;
	} else {
	smd_pkt_devp->open_count++;
	r = 0;
	}
	}
	out:
	mutex_unlock(&smd_pkt_devp->ch_lock);
	return r;
	}

	static int smd_pkt_release(struct inode inode, struct file file)
	{
	int r = 0;
	struct smd_pkt_dev *smd_pkt_devp = file->private_data;

	if (!smd_pkt_devp)
	return -EINVAL;

	mutex_lock(&smd_pkt_devp->ch_lock);
	if (--smd_pkt_devp->open_count == 0) {
	r = smd_close(smd_pkt_devp->ch);
	smd_pkt_devp->ch = 0;
	}
	mutex_unlock(&smd_pkt_devp->ch_lock);

	return r;
	}

	static const struct file_operations smd_pkt_fops = {
	.owner = THIS_MODULE,
	.open = smd_pkt_open,
	.release = smd_pkt_release,
	.read = smd_pkt_read,
	.write = smd_pkt_write,
	.poll = smd_pkt_poll,
	};

	static int __init smd_pkt_init(void)
	{
	int i;
	int r;

	r = alloc_chrdev_region(&smd_pkt_number, 0,
	NUM_SMD_PKT_PORTS, DEVICE_NAME);
	if (r) {
	pr_err("alloc_chrdev_region() failed %d\n", r);
	return r;
	}

	smd_pkt_classp = class_create(THIS_MODULE, DEVICE_NAME);
	if (IS_ERR(smd_pkt_classp)) {
	r = PTR_ERR(smd_pkt_classp);
	pr_err("class_create() failed %d\n", r);
	goto unreg_chardev;
	}

	for (i = 0; i < NUM_SMD_PKT_PORTS; ++i) {
	smd_pkt_devp[i] = kzalloc(sizeof(struct smd_pkt_dev),
	GFP_KERNEL);
	if (!smd_pkt_devp[i]) {
	pr_err("kmalloc() failed\n");
	goto clean_cdevs;
	}

	smd_pkt_devp[i]->i = i;

	init_waitqueue_head(&smd_pkt_devp[i]->ch_read_wait_queue);
	smd_pkt_devp[i]->remote_open = 0;
	init_waitqueue_head(&smd_pkt_devp[i]->ch_opened_wait_queue);

	mutex_init(&smd_pkt_devp[i]->ch_lock);
	mutex_init(&smd_pkt_devp[i]->rx_lock);
	mutex_init(&smd_pkt_devp[i]->tx_lock);

	cdev_init(&smd_pkt_devp[i]->cdev, &smd_pkt_fops);
	smd_pkt_devp[i]->cdev.owner = THIS_MODULE;

	r = cdev_add(&smd_pkt_devp[i]->cdev,
	(smd_pkt_number + i), 1);
	if (r) {
	pr_err("cdev_add() failed %d\n", r);
	kfree(smd_pkt_devp[i]);
	goto clean_cdevs;
	}

	smd_pkt_devp[i]->devicep =
	device_create(smd_pkt_classp, NULL,
	(smd_pkt_number + i), NULL,
	smd_pkt_dev_name[i]);
	if (IS_ERR(smd_pkt_devp[i]->devicep)) {
	r = PTR_ERR(smd_pkt_devp[i]->devicep);
	pr_err("device_create() failed %d\n", r);
	cdev_del(&smd_pkt_devp[i]->cdev);
	kfree(smd_pkt_devp[i]);
	goto clean_cdevs;
	}

	}

	pr_info("SMD Packet Port Driver Initialized.\n");
	return 0;

	clean_cdevs:
	if (i > 0) {
	while (--i >= 0) {
	mutex_destroy(&smd_pkt_devp[i]->ch_lock);
	mutex_destroy(&smd_pkt_devp[i]->rx_lock);
	mutex_destroy(&smd_pkt_devp[i]->tx_lock);
	cdev_del(&smd_pkt_devp[i]->cdev);
	kfree(smd_pkt_devp[i]);
	device_destroy(smd_pkt_classp,
	MKDEV(MAJOR(smd_pkt_number), i));
	}
	}

	class_destroy(smd_pkt_classp);
	unreg_chardev:
	unregister_chrdev_region(MAJOR(smd_pkt_number), NUM_SMD_PKT_PORTS);
	return r;
	}
	module_init(smd_pkt_init);

	static void __exit smd_pkt_cleanup(void)
	{
	int i;

	for (i = 0; i < NUM_SMD_PKT_PORTS; ++i) {
	mutex_destroy(&smd_pkt_devp[i]->ch_lock);
	mutex_destroy(&smd_pkt_devp[i]->rx_lock);
	mutex_destroy(&smd_pkt_devp[i]->tx_lock);
	cdev_del(&smd_pkt_devp[i]->cdev);
	kfree(smd_pkt_devp[i]);
	device_destroy(smd_pkt_classp,
	MKDEV(MAJOR(smd_pkt_number), i));
	}

	class_destroy(smd_pkt_classp);
	unregister_chrdev_region(MAJOR(smd_pkt_number), NUM_SMD_PKT_PORTS);
	}
	module_exit(smd_pkt_cleanup);

	MODULE_DESCRIPTION("MSM Shared Memory Packet Port");
	MODULE_LICENSE("GPL v2");