android/android_kernel_cmhtcleo
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Files
b47e67440925cf41ff06c7ece8c9a2b3ce3999fc
android_kernel_cmhtcleo/drivers/usb/gadget/diag.c
Markinus e34adb00cb Add EVOs source as default
2010-08-27 11:19:57 +02:00

1295 lines
30 KiB
C
Raw Blame History

/*
* Diag Function Device - Route DIAG frames between SMD and USB
*
* Copyright (C) 2007 Google, Inc.
* Author: Brian Swetland <swetland@google.com>
*
* This software is licensed under the terms of the GNU General Public
* License version 2, as published by the Free Software Foundation, and
* may be copied, distributed, and modified under those terms.
*
* 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.
*
*/
#include <linux/init.h>
#include <linux/module.h>
#include <linux/kernel.h>
#include <linux/platform_device.h>
#include <linux/miscdevice.h>
#include <linux/fs.h>
#include <asm/uaccess.h>
#include <linux/sched.h>
#include <linux/mutex.h>
#include <mach/msm_smd.h>
#include <linux/usb/android_composite.h>
#define NO_HDLC 1
#define ROUTE_TO_USERSPACE 1
#if 1
#define TRACE(tag,data,len,decode) do {} while(0)
#else
static void TRACE(const char *tag, const void *_data, int len, int decode)
{
const unsigned char *data = _data;
int escape = 0;
printk(KERN_INFO "%s", tag);
if (decode) {
while (len-- > 0) {
unsigned x = *data++;
if (x == 0x7e) {
printk(" $$");
escape = 0;
continue;
}
if (x == 0x7d) {
escape = 1;
continue;
}
if (escape) {
escape = 0;
printk(" %02x", x ^ 0x20);
} else {
printk(" %02x", x);
}
}
} else {
while (len-- > 0) {
printk(" %02x", *data++);
}
printk(" $$");
}
printk("\n");
}
#endif
#define HDLC_MAX 4096
#define SMD_MAX 8192
#define TX_REQ_BUF_SZ 8192
#define RX_REQ_BUF_SZ 8192
/* number of tx/rx requests to allocate */
#define TX_REQ_NUM 4
#define RX_REQ_NUM 4
struct diag_context
{
struct usb_function function;
struct usb_composite_dev *cdev;
struct usb_ep *out;
struct usb_ep *in;
struct list_head tx_req_idle;
struct list_head rx_req_idle;
struct list_head rx_arm9_idle;
struct list_head rx_arm9_done;
spinlock_t req_lock;
#if ROUTE_TO_USERSPACE
struct mutex user_lock;
#define ID_TABLE_SZ 10 /* keep this small */
struct list_head rx_req_user;
wait_queue_head_t read_wq;
wait_queue_head_t write_wq;
char *user_read_buf;
uint32_t user_read_len;
char *user_readp;
bool opened;
/* list of registered command ids to be routed to userspace */
unsigned char id_table[ID_TABLE_SZ];
#endif
smd_channel_t *ch;
struct mutex smd_lock;
int in_busy;
int online;
int error;
int init_done;
/* assembly buffer for USB->A9 HDLC frames */
unsigned char hdlc_buf[HDLC_MAX];
unsigned hdlc_count;
unsigned hdlc_escape;
u64 tx_count; /* to smd */
u64 rx_count; /* from smd */
int function_enable;
#if defined(CONFIG_MSM_N_WAY_SMD)
smd_channel_t *chqdsp;
struct list_head tx_qdsp_idle;
#endif
/* for slate test */
int is2ARM11;
struct mutex diag2arm9_lock;
struct mutex diag2arm9_read_lock;
struct mutex diag2arm9_write_lock;
bool diag2arm9_opened;
unsigned char toARM9_buf[SMD_MAX];
unsigned read_arm9_count;
unsigned char *read_arm9_buf;
wait_queue_head_t read_arm9_wq;
struct usb_request *read_arm9_req;
};
static struct usb_interface_descriptor diag_interface_desc = {
.bLength = USB_DT_INTERFACE_SIZE,
.bDescriptorType = USB_DT_INTERFACE,
.bInterfaceNumber = 0,
.bNumEndpoints = 2,
.bInterfaceClass = 0xFF,
.bInterfaceSubClass = 0xFF,
.bInterfaceProtocol = 0xFF,
};
static struct usb_endpoint_descriptor diag_highspeed_in_desc = {
.bLength = USB_DT_ENDPOINT_SIZE,
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = USB_DIR_IN,
.bmAttributes = USB_ENDPOINT_XFER_BULK,
.wMaxPacketSize = __constant_cpu_to_le16(512),
};
static struct usb_endpoint_descriptor diag_highspeed_out_desc = {
.bLength = USB_DT_ENDPOINT_SIZE,
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = USB_DIR_OUT,
.bmAttributes = USB_ENDPOINT_XFER_BULK,
.wMaxPacketSize = __constant_cpu_to_le16(512),
};
static struct usb_endpoint_descriptor diag_fullspeed_in_desc = {
.bLength = USB_DT_ENDPOINT_SIZE,
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = USB_DIR_IN,
.bmAttributes = USB_ENDPOINT_XFER_BULK,
};
static struct usb_endpoint_descriptor diag_fullspeed_out_desc = {
.bLength = USB_DT_ENDPOINT_SIZE,
.bDescriptorType = USB_DT_ENDPOINT,
.bEndpointAddress = USB_DIR_OUT,
.bmAttributes = USB_ENDPOINT_XFER_BULK,
};
static struct usb_descriptor_header *fs_diag_descs[] = {
(struct usb_descriptor_header *) &diag_interface_desc,
(struct usb_descriptor_header *) &diag_fullspeed_in_desc,
(struct usb_descriptor_header *) &diag_fullspeed_out_desc,
NULL,
};
static struct usb_descriptor_header *hs_diag_descs[] = {
(struct usb_descriptor_header *) &diag_interface_desc,
(struct usb_descriptor_header *) &diag_highspeed_in_desc,
(struct usb_descriptor_header *) &diag_highspeed_out_desc,
NULL,
};
/* string descriptors: */
static struct usb_string diag_string_defs[] = {
[0].s = "HTC DIAG",
{ } /* end of list */
};
static struct usb_gadget_strings diag_string_table = {
.language = 0x0409, /* en-us */
.strings = diag_string_defs,
};
static struct usb_gadget_strings *diag_strings[] = {
&diag_string_table,
NULL,
};
static struct diag_context _context;
static inline struct diag_context *func_to_dev(struct usb_function *f)
{
return container_of(f, struct diag_context, function);
}
static void smd_try_to_send(struct diag_context *ctxt);
static void smd_diag_notify(void *priv, unsigned event);
static void diag_queue_out(struct diag_context *ctxt);
#if defined(CONFIG_MSM_N_WAY_SMD)
static void diag_qdsp_complete_in(struct usb_ep *ept,
struct usb_request *req);
#endif
static struct usb_request *diag_req_new(unsigned len)
{
struct usb_request *req;
if (len > SMD_MAX)
return NULL;
req = kmalloc(sizeof(struct usb_request), GFP_KERNEL);
if (!req)
return NULL;
req->buf = kmalloc(len, GFP_KERNEL);
if (!req->buf) {
kfree(req);
return NULL;
}
return req;
}
static void diag_req_free(struct usb_request *req)
{
if (!req)
return;
if (req->buf) {
kfree(req->buf);
req->buf = 0;
}
kfree(req);
req = 0;
}
/* add a request to the tail of a list */
static void req_put(struct diag_context *ctxt, struct list_head *head,
struct usb_request *req)
{
unsigned long flags;
spin_lock_irqsave(&ctxt->req_lock, flags);
list_add_tail(&req->list, head);
spin_unlock_irqrestore(&ctxt->req_lock, flags);
}
/* remove a request from the head of a list */
static struct usb_request *req_get(struct diag_context *ctxt,
struct list_head *head)
{
struct usb_request *req = 0;
unsigned long flags;
spin_lock_irqsave(&ctxt->req_lock, flags);
if (!list_empty(head)) {
req = list_first_entry(head, struct usb_request, list);
list_del(&req->list);
}
spin_unlock_irqrestore(&ctxt->req_lock, flags);
return req;
}
static void reqs_free(struct diag_context *ctxt, struct usb_ep *ep,
struct list_head *head)
{
struct usb_request *req;
while ((req = req_get(ctxt, head))) {
kfree(req->buf);
usb_ep_free_request(ep, req);
}
}
static void smd_diag_enable(char *src, int enable)
{
struct diag_context *ctxt = &_context;
printk(KERN_INFO "smd_try_open(%s): %d\n", src, enable);
if (!ctxt->init_done)
return;
mutex_lock(&ctxt->smd_lock);
if (enable) {
if (!ctxt->ch)
smd_open("SMD_DIAG", &ctxt->ch, ctxt, smd_diag_notify);
} else {
if (ctxt->ch) {
smd_close(ctxt->ch);
ctxt->ch = NULL;
}
}
mutex_unlock(&ctxt->smd_lock);
}
#if ROUTE_TO_USERSPACE
#define USB_DIAG_IOC_MAGIC 0xFF
#define USB_DIAG_FUNC_IOC_ENABLE_SET _IOW(USB_DIAG_IOC_MAGIC, 1, int)
#define USB_DIAG_FUNC_IOC_ENABLE_GET _IOR(USB_DIAG_IOC_MAGIC, 2, int)
#define USB_DIAG_FUNC_IOC_REGISTER_SET _IOW(USB_DIAG_IOC_MAGIC, 3, char *)
#define USB_DIAG_FUNC_IOC_AMR_SET _IOW(USB_DIAG_IOC_MAGIC, 4, int)
static long diag_ioctl(struct file *file, unsigned int cmd, unsigned long arg)
{
struct diag_context *ctxt = &_context;
void __user *argp = (void __user *)arg;
int tmp_value;
unsigned long flags;
unsigned char temp_id_table[ID_TABLE_SZ];
printk(KERN_INFO "diag:diag_ioctl() cmd=%d\n", cmd);
if (_IOC_TYPE(cmd) != USB_DIAG_IOC_MAGIC)
return -ENOTTY;
switch (cmd) {
case USB_DIAG_FUNC_IOC_ENABLE_SET:
if (copy_from_user(&tmp_value, argp, sizeof(int)))
return -EFAULT;
printk(KERN_INFO "diag: enable %d\n", tmp_value);
android_enable_function(&_context.function, tmp_value);
smd_diag_enable("diag_ioctl", tmp_value);
/* force diag_read to return error when disable diag */
if (tmp_value == 0)
ctxt->error = 1;
wake_up(&ctxt->read_wq);
break;
case USB_DIAG_FUNC_IOC_ENABLE_GET:
tmp_value = !_context.function.hidden;
if (copy_to_user(argp, &tmp_value, sizeof(tmp_value)))
return -EFAULT;
break;
case USB_DIAG_FUNC_IOC_REGISTER_SET:
if (copy_from_user(temp_id_table, (unsigned char *)argp, ID_TABLE_SZ))
return -EFAULT;
spin_lock_irqsave(&ctxt->req_lock, flags);
memcpy(ctxt->id_table, temp_id_table, ID_TABLE_SZ);
spin_unlock_irqrestore(&ctxt->req_lock, flags);
break;
case USB_DIAG_FUNC_IOC_AMR_SET:
if (copy_from_user(&ctxt->is2ARM11, argp, sizeof(int)))
return -EFAULT;
printk(KERN_INFO "diag: is2ARM11 %d\n", ctxt->is2ARM11);
break;
default:
return -ENOTTY;
}
return 0;
}
static ssize_t diag_read(struct file *fp, char __user *buf,
size_t count, loff_t *pos)
{
struct diag_context *ctxt = &_context;
struct usb_request *req = 0;
int ret = 0;
/* we will block until we're online */
if (!ctxt->online) {
ret = wait_event_interruptible(ctxt->read_wq, (ctxt->online || ctxt->error));
if (ret < 0 || ctxt->error)
return -EFAULT;
}
mutex_lock(&ctxt->user_lock);
if (ctxt->user_read_len && ctxt->user_readp) {
if (count > ctxt->user_read_len)
count = ctxt->user_read_len;
if (copy_to_user(buf, ctxt->user_readp, count))
ret = -EFAULT;
else {
ctxt->user_readp += count;
ctxt->user_read_len -= count;
ret = count;
}
goto end;
}
mutex_unlock(&ctxt->user_lock);
ret = wait_event_interruptible(ctxt->read_wq,
(req = req_get(ctxt, &ctxt->rx_req_user)) || !ctxt->online);
mutex_lock(&ctxt->user_lock);
if (ret < 0) {
pr_err("%s: wait_event_interruptible error %d\n",
__func__, ret);
goto end;
}
if (!ctxt->online) {
/* pr_err("%s: offline\n", __func__); */
ret = -EIO;
goto end;
}
if (req) {
if (req->actual == 0) {
pr_info("%s: no data\n", __func__);
goto end;
}
if (count > req->actual)
count = req->actual;
if (copy_to_user(buf, req->buf, count)) {
ret = -EFAULT;
goto end;
}
req->actual -= count;
if (req->actual) {
memcpy(ctxt->user_read_buf, req->buf + count, req->actual);
ctxt->user_read_len = req->actual;
ctxt->user_readp = ctxt->user_read_buf;
}
ret = count;
}
end:
if (req)
req_put(ctxt, &ctxt->rx_req_idle, req);
mutex_unlock(&ctxt->user_lock);
return ret;
}
static ssize_t diag_write(struct file *fp, const char __user *buf,
size_t count, loff_t *pos)
{
struct diag_context *ctxt = &_context;
struct usb_request *req = 0;
int ret = 0;
ret = wait_event_interruptible(ctxt->write_wq,
((req = req_get(ctxt, &ctxt->tx_req_idle)) || !ctxt->online));
mutex_lock(&ctxt->user_lock);
if (ret < 0) {
pr_err("%s: wait_event_interruptible error %d\n",
__func__, ret);
goto end;
}
if (!ctxt->online) {
pr_err("%s: offline\n", __func__);
ret = -EIO;
goto end;
}
if (count > TX_REQ_BUF_SZ)
count = TX_REQ_BUF_SZ;
if (req) {
if (copy_from_user(req->buf, buf, count)) {
ret = -EFAULT;
goto end;
}
req->length = count;
ret = usb_ep_queue(ctxt->in, req, GFP_ATOMIC);
if (ret < 0) {
pr_err("%s: usb_ep_queue error %d\n", __func__, ret);
goto end;
}
ret = req->length;
/* zero this so we don't put it back to idle queue */
req = 0;
}
end:
if (req)
req_put(ctxt, &ctxt->tx_req_idle, req);
mutex_unlock(&ctxt->user_lock);
return ret;
}
static int diag_open(struct inode *ip, struct file *fp)
{
struct diag_context *ctxt = &_context;
int rc = 0;
mutex_lock(&ctxt->user_lock);
if (ctxt->opened) {
pr_err("%s: already opened\n", __func__);
rc = -EBUSY;
goto done;
}
ctxt->user_read_len = 0;
ctxt->user_readp = 0;
if (!ctxt->user_read_buf) {
ctxt->user_read_buf = kmalloc(RX_REQ_BUF_SZ, GFP_KERNEL);
if (!ctxt->user_read_buf) {
rc = -ENOMEM;
goto done;
}
}
ctxt->opened = true;
/* clear the error latch */
ctxt->error = 0;
done:
mutex_unlock(&ctxt->user_lock);
return rc;
}
static int diag_release(struct inode *ip, struct file *fp)
{
struct diag_context *ctxt = &_context;
mutex_lock(&ctxt->user_lock);
ctxt->opened = false;
ctxt->user_read_len = 0;
ctxt->user_readp = 0;
if (ctxt->user_read_buf) {
kfree(ctxt->user_read_buf);
ctxt->user_read_buf = 0;
}
mutex_unlock(&ctxt->user_lock);
return 0;
}
static struct file_operations diag_fops = {
.owner = THIS_MODULE,
.read = diag_read,
.write = diag_write,
.open = diag_open,
.release = diag_release,
.unlocked_ioctl = diag_ioctl,
};
static struct miscdevice diag_device_fops = {
.minor = MISC_DYNAMIC_MINOR,
.name = "diag",
.fops = &diag_fops,
};
#endif
static int diag2arm9_open(struct inode *ip, struct file *fp)
{
struct diag_context *ctxt = &_context;
struct usb_request *req;
int rc = 0;
int n;
printk(KERN_INFO "%s\n", __func__);
mutex_lock(&ctxt->diag2arm9_lock);
if (ctxt->diag2arm9_opened) {
pr_err("%s: already opened\n", __func__);
rc = -EBUSY;
goto done;
}
/* clear pending data if any */
while ((req = req_get(ctxt, &ctxt->rx_arm9_done)))
diag_req_free(req);
for (n = 0; n < 4; n++) {
req = diag_req_new(SMD_MAX);
if (!req) {
while ((req = req_get(ctxt, &ctxt->rx_arm9_idle)))
diag_req_free(req);
rc = -EFAULT;
goto done;
}
req_put(ctxt, &ctxt->rx_arm9_idle, req);
}
ctxt->read_arm9_count = 0;
ctxt->read_arm9_buf = 0;
ctxt->read_arm9_req = 0;
ctxt->diag2arm9_opened = true;
smd_diag_enable("diag2arm9_open", 1);
done:
mutex_unlock(&ctxt->diag2arm9_lock);
return rc;
}
static int diag2arm9_release(struct inode *ip, struct file *fp)
{
struct diag_context *ctxt = &_context;
struct usb_request *req;
printk(KERN_INFO "%s\n", __func__);
mutex_lock(&ctxt->diag2arm9_lock);
ctxt->diag2arm9_opened = false;
wake_up(&ctxt->read_arm9_wq);
mutex_lock(&ctxt->diag2arm9_read_lock);
while ((req = req_get(ctxt, &ctxt->rx_arm9_idle)))
diag_req_free(req);
while ((req = req_get(ctxt, &ctxt->rx_arm9_done)))
diag_req_free(req);
if (ctxt->read_arm9_req)
diag_req_free(ctxt->read_arm9_req);
mutex_unlock(&ctxt->diag2arm9_read_lock);
/*************************************
* If smd is closed, it will lead to slate can't be tested.
* slate will open it for one time
* but close it for several times and never open
*************************************/
/*smd_diag_enable("diag2arm9_release", 0);*/
mutex_unlock(&ctxt->diag2arm9_lock);
return 0;
}
static ssize_t diag2arm9_write(struct file *fp, const char __user *buf,
size_t count, loff_t *pos)
{
struct diag_context *ctxt = &_context;
int r = count;
int writed = 0;
mutex_lock(&ctxt->diag2arm9_write_lock);
while (count > 0) {
writed = count > SMD_MAX ? SMD_MAX : count;
if (copy_from_user(ctxt->toARM9_buf, buf, writed)) {
r = -EFAULT;
break;
}
if (ctxt->ch == NULL) {
printk(KERN_ERR "%s: ctxt->ch == NULL", __func__);
r = -EFAULT;
break;
} else if (ctxt->toARM9_buf == NULL) {
printk(KERN_ERR "%s: ctxt->toARM9_buf == NULL", __func__);
r = -EFAULT;
break;
}
smd_write(ctxt->ch, ctxt->toARM9_buf, writed);
buf += writed;
count -= writed;
}
mutex_unlock(&ctxt->diag2arm9_write_lock);
return r;
}
static ssize_t diag2arm9_read(struct file *fp, char __user *buf,
size_t count, loff_t *pos)
{
struct diag_context *ctxt = &_context;
struct usb_request *req;
int r = 0, xfer;
int ret;
mutex_lock(&ctxt->diag2arm9_read_lock);
/* if we have data pending, give it to userspace */
if (ctxt->read_arm9_count > 0)
req = ctxt->read_arm9_req;
else {
retry:
/* get data from done queue */
req = 0;
ret = wait_event_interruptible(ctxt->read_arm9_wq,
((req = req_get(ctxt, &ctxt->rx_arm9_done)) ||
!ctxt->diag2arm9_opened));
if (!ctxt->diag2arm9_opened) {
if (req)
req_put(ctxt, &ctxt->rx_arm9_idle, req);
goto done;
}
if (ret < 0 || req == 0)
goto done;
if (req->actual == 0) {
req_put(ctxt, &ctxt->rx_arm9_idle, req);
goto retry;
}
ctxt->read_arm9_req = req;
ctxt->read_arm9_count = req->actual;
ctxt->read_arm9_buf = req->buf;
}
xfer = (ctxt->read_arm9_count < count) ? ctxt->read_arm9_count : count;
if (copy_to_user(buf, ctxt->read_arm9_buf, xfer)) {
printk(KERN_INFO "diag: copy_to_user fail\n");
r = -EFAULT;
goto done;
}
ctxt->read_arm9_buf += xfer;
ctxt->read_arm9_count -= xfer;
r += xfer;
/* if we've emptied the buffer, release the request */
if (ctxt->read_arm9_count == 0) {
req_put(ctxt, &ctxt->rx_arm9_idle, ctxt->read_arm9_req);
ctxt->read_arm9_req = 0;
}
done:
mutex_unlock(&ctxt->diag2arm9_read_lock);
return r;
}
static struct file_operations diag2arm9_fops = {
.owner = THIS_MODULE,
.open = diag2arm9_open,
.release = diag2arm9_release,
.write = diag2arm9_write,
.read = diag2arm9_read,
};
static struct miscdevice diag2arm9_device = {
.minor = MISC_DYNAMIC_MINOR,
.name = "diag_arm9",
.fops = &diag2arm9_fops,
};
static void diag_in_complete(struct usb_ep *ept, struct usb_request *req)
{
struct diag_context *ctxt = req->context;
#if ROUTE_TO_USERSPACE
char c;
#endif
ctxt->in_busy = 0;
req_put(ctxt, &ctxt->tx_req_idle, req);
#if ROUTE_TO_USERSPACE
c = *((char *)req->buf + req->actual - 1);
if (c == 0x7e)
wake_up(&ctxt->write_wq);
#endif
smd_try_to_send(ctxt);
}
#if !NO_HDLC
static void diag_process_hdlc(struct diag_context *ctxt, void *_data, unsigned len)
{
unsigned char *data = _data;
unsigned count = ctxt->hdlc_count;
unsigned escape = ctxt->hdlc_escape;
unsigned char *hdlc = ctxt->hdlc_buf;
while (len-- > 0) {
unsigned char x = *data++;
if (x == 0x7E) {
if (count > 2) {
/* we're just ignoring the crc here */
TRACE("PC>", hdlc, count - 2, 0);
if (ctxt->ch)
smd_write(ctxt->ch, hdlc, count - 2);
}
count = 0;
escape = 0;
} else if (x == 0x7D) {
escape = 1;
} else {
if (escape) {
x = x ^ 0x20;
escape = 0;
}
hdlc[count++] = x;
/* discard frame if we overflow */
if (count == HDLC_MAX)
count = 0;
}
}
ctxt->hdlc_count = count;
ctxt->hdlc_escape = escape;
}
#endif
#if ROUTE_TO_USERSPACE
static int if_route_to_userspace(struct diag_context *ctxt, unsigned int cmd_id)
{
unsigned long flags;
int i;
if (!ctxt->opened || cmd_id == 0)
return 0;
/* command ids 0xfb..0xff are not used by msm diag; we steal these ids
* for communication between userspace tool and host test tool.
*/
if (cmd_id >= 0xfb && cmd_id <= 0xff)
return 1;
spin_lock_irqsave(&ctxt->req_lock, flags);
for (i = 0; i < ARRAY_SIZE(ctxt->id_table); i++)
if (ctxt->id_table[i] == cmd_id) {
/* if the command id equals to any of registered ids,
* route to userspace to handle.
*/
spin_unlock_irqrestore(&ctxt->req_lock, flags);
return 1;
}
spin_unlock_irqrestore(&ctxt->req_lock, flags);
return 0;
}
#endif
static void diag_out_complete(struct usb_ep *ept, struct usb_request *req)
{
struct diag_context *ctxt = req->context;
if (req->status == 0) {
#if ROUTE_TO_USERSPACE
unsigned int cmd_id = *((unsigned char *)req->buf);
if (if_route_to_userspace(ctxt, cmd_id)) {
req_put(ctxt, &ctxt->rx_req_user, req);
wake_up(&ctxt->read_wq);
diag_queue_out(ctxt);
return;
}
#endif
#if NO_HDLC
TRACE("PC>", req->buf, req->actual, 0);
if (ctxt->ch)
smd_write(ctxt->ch, req->buf, req->actual);
#else
diag_process_hdlc(ctxt, req->buf, req->actual);
#endif
ctxt->tx_count += req->actual;
}
req_put(ctxt, &ctxt->rx_req_idle, req);
diag_queue_out(ctxt);
}
static void diag_queue_out(struct diag_context *ctxt)
{
struct usb_request *req;
int rc;
req = req_get(ctxt, &ctxt->rx_req_idle);
if (!req) {
pr_err("%s: rx req queue - out of buffer\n", __func__);
return;
}
req->complete = diag_out_complete;
req->context = ctxt;
req->length = RX_REQ_BUF_SZ;
rc = usb_ep_queue(ctxt->out, req, GFP_ATOMIC);
if (rc < 0) {
pr_err("%s: usb_ep_queue failed: %d\n", __func__, rc);
req_put(ctxt, &ctxt->rx_req_idle, req);
}
}
static void smd_try_to_send(struct diag_context *ctxt)
{
again:
if (ctxt->ch && (!ctxt->in_busy)) {
int r = smd_read_avail(ctxt->ch);
if (r > TX_REQ_BUF_SZ) {
printk(KERN_ERR "The SMD data is too large to send!!\n");
return;
}
if (r > 0 && ctxt->is2ARM11) {
/* to arm11 user space */
struct usb_request *req;
if (!ctxt->diag2arm9_opened)
return;
req = req_get(ctxt, &ctxt->rx_arm9_idle);
if (!req) {
printk(KERN_ERR "There is no enough request to ARM11!!\n");
return;
}
smd_read(ctxt->ch, req->buf, r);
req->actual = r;
req_put(ctxt, &ctxt->rx_arm9_done, req);
wake_up(&ctxt->read_arm9_wq);
return;
}
if (!ctxt->online)
return;
if (r > 0) {
struct usb_request *req;
req = req_get(ctxt, &ctxt->tx_req_idle);
if (!req) {
pr_err("%s: tx req queue is out of buffers\n",
__func__);
return;
}
smd_read(ctxt->ch, req->buf, r);
ctxt->rx_count += r;
if (!ctxt->online) {
// printk("$$$ discard %d\n", r);
req_put(ctxt, &ctxt->tx_req_idle, req);
goto again;
}
req->complete = diag_in_complete;
req->context = ctxt;
req->length = r;
TRACE("A9>", req->buf, r, 1);
ctxt->in_busy = 1;
r = usb_ep_queue(ctxt->in, req, GFP_ATOMIC);
if (r < 0) {
pr_err("%s: usb_ep_queue failed: %d\n",
__func__, r);
req_put(ctxt, &ctxt->tx_req_idle, req);
ctxt->in_busy = 0;
}
}
}
}
static void smd_diag_notify(void *priv, unsigned event)
{
struct diag_context *ctxt = priv;
smd_try_to_send(ctxt);
}
static int __init create_bulk_endpoints(struct diag_context *ctxt,
struct usb_endpoint_descriptor *in_desc,
struct usb_endpoint_descriptor *out_desc)
{
struct usb_composite_dev *cdev = ctxt->cdev;
struct usb_ep *ep;
struct usb_request *req;
int n;
ep = usb_ep_autoconfig(cdev->gadget, in_desc);
if (!ep) {
DBG(cdev, "usb_ep_autoconfig for ep_in failed\n");
return -ENODEV;
}
ep->driver_data = ctxt; /* claim the endpoint */
ctxt->in = ep;
ep = usb_ep_autoconfig(cdev->gadget, out_desc);
if (!ep) {
return -ENODEV;
}
ep->driver_data = ctxt; /* claim the endpoint */
ctxt->out = ep;
ctxt->tx_count = ctxt->rx_count = 0;
for (n = 0; n < RX_REQ_NUM; n++) {
req = usb_ep_alloc_request(ctxt->out, GFP_KERNEL);
if (!req) {
DBG(cdev, "%s: usb_ep_alloc_request out of memory\n",
__func__);
goto rx_fail;
}
req->buf = kmalloc(RX_REQ_BUF_SZ, GFP_KERNEL);
if (!req->buf) {
DBG(cdev, "%s: kmalloc out of memory\n", __func__);
goto rx_fail;
}
req->context = ctxt;
req->complete = diag_out_complete;
req_put(ctxt, &ctxt->rx_req_idle, req);
}
for (n = 0; n < TX_REQ_NUM; n++) {
req = usb_ep_alloc_request(ctxt->in, GFP_KERNEL);
if (!req) {
DBG(cdev, "%s: usb_ep_alloc_request out of memory\n",
__func__);
goto tx_fail;
}
req->buf = kmalloc(TX_REQ_BUF_SZ, GFP_KERNEL);
if (!req->buf) {
DBG(cdev, "%s: kmalloc out of memory\n", __func__);
goto tx_fail;
}
req->context = ctxt;
req->complete = diag_in_complete;
req_put(ctxt, &ctxt->tx_req_idle, req);
}
#if defined(CONFIG_MSM_N_WAY_SMD)
for (n = 0; n < TX_REQ_NUM; n++) {
req = usb_ep_alloc_request(ctxt->in, GFP_KERNEL);
if (!req) {
DBG(cdev, "%s: usb_ep_alloc_request out of memory\n",
__func__);
goto qdsp_tx_fail;
}
req->buf = kmalloc(TX_REQ_BUF_SZ, GFP_KERNEL);
if (!req->buf) {
DBG(cdev, "%s: kmalloc out of memory\n", __func__);
goto qdsp_tx_fail;
}
req->context = ctxt;
req->complete = diag_qdsp_complete_in;
req_put(ctxt, &ctxt->tx_qdsp_idle, req);
}
#endif
return 0;
#if defined(CONFIG_MSM_N_WAY_SMD)
qdsp_tx_fail:
reqs_free(ctxt, ctxt->in, &ctxt->tx_qdsp_idle);
#endif
tx_fail:
reqs_free(ctxt, ctxt->in, &ctxt->tx_req_idle);
rx_fail:
reqs_free(ctxt, ctxt->out, &ctxt->rx_req_idle);
return -ENOMEM;
}
static int
diag_function_bind(struct usb_configuration *c, struct usb_function *f)
{
struct usb_composite_dev *cdev = c->cdev;
struct diag_context *ctxt = func_to_dev(f);
int id;
int ret;
ctxt->cdev = cdev;
/* allocate interface ID(s) */
id = usb_interface_id(c, f);
if (id < 0)
return id;
diag_interface_desc.bInterfaceNumber = id;
/* allocate endpoints */
ret = create_bulk_endpoints(ctxt, &diag_fullspeed_in_desc,
&diag_fullspeed_out_desc);
if (ret)
return ret;
/* support high speed hardware */
if (gadget_is_dualspeed(c->cdev->gadget)) {
diag_highspeed_in_desc.bEndpointAddress =
diag_fullspeed_in_desc.bEndpointAddress;
diag_highspeed_out_desc.bEndpointAddress =
diag_fullspeed_out_desc.bEndpointAddress;
}
#if ROUTE_TO_USERSPACE
misc_register(&diag_device_fops);
#endif
misc_register(&diag2arm9_device);
return 0;
}
static void
diag_function_unbind(struct usb_configuration *c, struct usb_function *f)
{
struct diag_context *ctxt = func_to_dev(f);
reqs_free(ctxt, ctxt->out, &ctxt->rx_req_idle);
reqs_free(ctxt, ctxt->in, &ctxt->tx_req_idle);
#if ROUTE_TO_USERSPACE
misc_deregister(&diag_device_fops);
#endif
misc_deregister(&diag2arm9_device);
ctxt->tx_count = ctxt->rx_count = 0;
}
static int diag_function_set_alt(struct usb_function *f,
unsigned intf, unsigned alt)
{
struct diag_context *ctxt = func_to_dev(f);
struct usb_composite_dev *cdev = f->config->cdev;
#if ROUTE_TO_USERSPACE
struct usb_request *req;
#endif
int ret;
ret = usb_ep_enable(ctxt->in,
ep_choose(cdev->gadget,
&diag_highspeed_in_desc,
&diag_fullspeed_in_desc));
if (ret)
return ret;
ret = usb_ep_enable(ctxt->out,
ep_choose(cdev->gadget,
&diag_highspeed_out_desc,
&diag_fullspeed_out_desc));
if (ret) {
usb_ep_disable(ctxt->in);
return ret;
}
ctxt->online = !ctxt->function.hidden;
#if ROUTE_TO_USERSPACE
/* recycle unhandled rx reqs to user if any */
while ((req = req_get(ctxt, &ctxt->rx_req_user)))
req_put(ctxt, &ctxt->rx_req_idle, req);
#endif
if (ctxt->online) {
diag_queue_out(ctxt);
smd_try_to_send(ctxt);
}
#if ROUTE_TO_USERSPACE
wake_up(&ctxt->read_wq);
wake_up(&ctxt->write_wq);
#endif
return 0;
}
static void diag_function_disable(struct usb_function *f)
{
struct diag_context *ctxt = func_to_dev(f);
ctxt->online = 0;
#if ROUTE_TO_USERSPACE
wake_up(&ctxt->read_wq);
wake_up(&ctxt->write_wq);
#endif
usb_ep_disable(ctxt->in);
usb_ep_disable(ctxt->out);
}
#if defined(CONFIG_MSM_N_WAY_SMD)
static void diag_qdsp_send(struct diag_context *ctxt)
{
int ret, r;
struct usb_request *req;
if (ctxt->chqdsp && ctxt->online) {
r = smd_read_avail(ctxt->chqdsp);
if (r > SMD_MAX || r <= 0)
return;
req = req_get(ctxt, &ctxt->tx_qdsp_idle);
if (!req)
return;
smd_read(ctxt->chqdsp, req->buf, r);
req->length = r;
ret = usb_ep_queue(ctxt->in, req, GFP_ATOMIC);
if (ret < 0) {
printk(KERN_INFO "diag: failed to queue qdsp req %d\n",
ret);
req_put(ctxt, &ctxt->tx_qdsp_idle, req);
}
}
}
static void diag_qdsp_complete_in(struct usb_ep *ept,
struct usb_request *req)
{
struct diag_context *ctxt = req->context;
req_put(ctxt, &ctxt->tx_qdsp_idle, req);
diag_qdsp_send(ctxt);
#if ROUTE_TO_USERSPACE
wake_up(&ctxt->write_wq);
#endif
}
static void diag_qdsp_notify(void *priv, unsigned event)
{
struct diag_context *ctxt = priv;
diag_qdsp_send(ctxt);
}
static int msm_diag_probe(struct platform_device *pdev)
{
struct diag_context *ctxt = &_context;
printk(KERN_INFO "diag:msm_diag_probe(), pdev->id=0x%x\n", pdev->id);
if (pdev->id == 1)
smd_open("DSP_DIAG", &ctxt->chqdsp, ctxt, diag_qdsp_notify);
return 0;
}
static struct platform_driver msm_smd_qdsp_ch1_driver = {
.probe = msm_diag_probe,
.driver = {
.name = "DSP_DIAG",
.owner = THIS_MODULE,
},
};
#endif
static int diag_set_enabled(const char *val, struct kernel_param *kp)
{
int enabled = simple_strtol(val, NULL, 0);
if (_context.cdev)
android_enable_function(&_context.function, enabled);
_context.function_enable = !!enabled;
smd_diag_enable("diag_set_enabled", enabled);
return 0;
}
static int diag_get_tx_rx_count(char *buffer, struct kernel_param *kp)
{
struct diag_context *ctxt = &_context;
return sprintf(buffer, "tx: %llu bytes, rx: %llu bytes",
ctxt->tx_count, ctxt->rx_count);
}
module_param_call(tx_rx_count, NULL, diag_get_tx_rx_count, NULL, 0444);
static int diag_get_enabled(char *buffer, struct kernel_param *kp)
{
buffer[0] = '0' + !_context.function.hidden;
return 1;
}
module_param_call(enabled, diag_set_enabled, diag_get_enabled, NULL, 0664);
int diag_bind_config(struct usb_configuration *c)
{
struct diag_context *ctxt = &_context;
int ret;
printk(KERN_INFO "diag_bind_config\n");
ret = usb_string_id(c->cdev);
if (ret < 0)
return ret;
diag_string_defs[0].id = ret;
diag_interface_desc.iInterface = ret;
ctxt->cdev = c->cdev;
ctxt->function.name = "diag";
ctxt->function.strings = diag_strings;
ctxt->function.descriptors = fs_diag_descs;
ctxt->function.hs_descriptors = hs_diag_descs;
ctxt->function.bind = diag_function_bind;
ctxt->function.unbind = diag_function_unbind;
ctxt->function.set_alt = diag_function_set_alt;
ctxt->function.disable = diag_function_disable;
ctxt->function.hidden = !_context.function_enable;
if (!ctxt->function.hidden)
smd_diag_enable("diag_bind_config", 1);
return usb_add_function(c, &ctxt->function);
}
static struct android_usb_function diag_function = {
.name = "diag",
.bind_config = diag_bind_config,
};
static int __init init(void)
{
struct diag_context *ctxt = &_context;
printk(KERN_INFO "diag init\n");
spin_lock_init(&ctxt->req_lock);
INIT_LIST_HEAD(&ctxt->rx_req_idle);
INIT_LIST_HEAD(&ctxt->tx_req_idle);
INIT_LIST_HEAD(&ctxt->rx_arm9_idle);
INIT_LIST_HEAD(&ctxt->rx_arm9_done);
#if ROUTE_TO_USERSPACE
mutex_init(&ctxt->user_lock);
INIT_LIST_HEAD(&ctxt->rx_req_user);
init_waitqueue_head(&ctxt->read_wq);
init_waitqueue_head(&ctxt->write_wq);
#endif
init_waitqueue_head(&ctxt->read_arm9_wq);
mutex_init(&ctxt->diag2arm9_lock);
mutex_init(&ctxt->diag2arm9_read_lock);
mutex_init(&ctxt->diag2arm9_write_lock);
mutex_init(&ctxt->smd_lock);
ctxt->is2ARM11 = 0;
#if defined(CONFIG_MSM_N_WAY_SMD)
INIT_LIST_HEAD(&ctxt->tx_qdsp_idle);
platform_driver_register(&msm_smd_qdsp_ch1_driver);
#endif
ctxt->init_done = 1;
android_register_function(&diag_function);
return 0;
}
module_init(init);
Reference in New Issue View Git Blame Copy Permalink