+++ /dev/null
-/* 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("writing %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");
Code Review / kvmfornfv.git / blobdiff