static void hsic_read_work_fn(struct work_struct *work)
{
int err = 0;
unsigned char *buf = NULL;
struct diag_hsic_info *ch = container_of(work, struct diag_hsic_info,
read_work);
if (!ch || !ch->enabled || !ch->opened)
return;
do {
buf = diagmem_alloc(driver, DIAG_MDM_BUF_SIZE, ch->mempool);
if (!buf) {
err = -ENOMEM;
break;
}
err = diag_bridge_read(ch->id, buf, DIAG_MDM_BUF_SIZE);
if (err) {
diagmem_free(driver, buf, ch->mempool);
pr_err_ratelimited("diag: Unable to read from HSIC channel %d, err: %d\n",
ch->id, err);
break;
}
} while (buf);
/* Read from the HSIC channel continously if the channel is present */
if (!err)
queue_work(ch->hsic_wq, &ch->read_work);
}
static void diag_read_hsic_work_fn(struct work_struct *work)
{
unsigned char *buf_in_hsic = NULL;
int num_reads_submitted = 0;
int err = 0;
int write_ptrs_available;
if (!driver->hsic_ch) {
pr_err("DIAG in %s: driver->hsic_ch == 0\n", __func__);
return;
}
if (driver->logging_mode == MEMORY_DEVICE_MODE)
write_ptrs_available = driver->poolsize_hsic_write -
driver->num_hsic_buf_tbl_entries;
else
write_ptrs_available = driver->poolsize_hsic_write -
driver->count_hsic_write_pool;
do {
if (write_ptrs_available <= 0)
break;
write_ptrs_available--;
if (!driver->hsic_ch)
break;
buf_in_hsic = diagmem_alloc(driver, READ_HSIC_BUF_SIZE,
POOL_TYPE_HSIC);
if (buf_in_hsic) {
pr_debug("diag: read from HSIC\n");
num_reads_submitted++;
err = diag_bridge_read((char *)buf_in_hsic,
READ_HSIC_BUF_SIZE);
if (err) {
num_reads_submitted--;
diagmem_free(driver, buf_in_hsic,
POOL_TYPE_HSIC);
pr_err_ratelimited("diag: Error initiating HSIC read, err: %d\n",
err);
break;
}
}
} while (buf_in_hsic);
if ((driver->count_hsic_pool < driver->poolsize_hsic) &&
(num_reads_submitted == 0) && (err != -ENODEV) &&
(driver->hsic_ch != 0))
queue_work(diag_bridge[HSIC].wq,
&driver->diag_read_hsic_work);
}
int diag_usb_write(int id, unsigned char *buf, int len, int ctxt)
{
int err = 0;
struct diag_request *req = NULL;
struct diag_usb_info *usb_info = NULL;
if (id < 0 || id >= NUM_DIAG_USB_DEV) {
pr_err_ratelimited("diag: In %s, Incorrect id %d\n",
__func__, id);
return -EINVAL;
}
usb_info = &diag_usb[id];
req = diagmem_alloc(driver, sizeof(struct diag_request),
usb_info->mempool);
if (!req) {
/*
* This should never happen. It either means that we are
* trying to write more buffers than the max supported by
* this particualar diag USB channel at any given instance,
* or the previous write ptrs are stuck in the USB layer.
*/
pr_err_ratelimited("diag: In %s, cannot retrieve USB write ptrs for USB channel %s\n",
__func__, usb_info->name);
return -ENOMEM;
}
req->buf = buf;
req->length = len;
req->context = (void *)(uintptr_t)ctxt;
if (!usb_info->hdl || !usb_info->connected) {
pr_debug_ratelimited("diag: USB ch %s is not connected\n",
usb_info->name);
diagmem_free(driver, req, usb_info->mempool);
return -ENODEV;
}
err = usb_diag_write(usb_info->hdl, req);
if (err) {
pr_err_ratelimited("diag: In %s, error writing to usb channel %s, err: %d\n",
__func__, usb_info->name, err);
diagmem_free(driver, req, usb_info->mempool);
}
return err;
}
int diag_usb_write(int id, unsigned char *buf, int len, int ctxt)
{
int err = 0;
struct diag_request *req = NULL;
struct diag_usb_info *usb_info = NULL;
if (id < 0 || id >= NUM_DIAG_USB_DEV) {
pr_err_ratelimited("diag: In %s, Incorrect id %d\n",
__func__, id);
return -EINVAL;
}
usb_info = &diag_usb[id];
req = diagmem_alloc(driver, sizeof(struct diag_request),
usb_info->mempool);
if (!req) {
pr_err_ratelimited("diag: In %s, cannot retrieve USB write ptrs for USB channel %s\n",
__func__, usb_info->name);
return -ENOMEM;
}
req->buf = buf;
req->length = len;
req->context = (void *)(uintptr_t)ctxt;
if (!usb_info->hdl || !usb_info->connected) {
pr_debug_ratelimited("diag: USB ch %s is not connected\n",
usb_info->name);
diagmem_free(driver, req, usb_info->mempool);
return -ENODEV;
}
err = usb_diag_write(usb_info->hdl, req);
if (err) {
pr_err_ratelimited("diag: In %s, error writing to usb channel %s, err: %d\n",
__func__, usb_info->name, err);
diagmem_free(driver, req, usb_info->mempool);
}
return err;
}
static int diagchar_write(struct file *file, const char __user *buf,
size_t count, loff_t *ppos)
{
int err, ret = 0, pkt_type;
#ifdef DIAG_DEBUG
int length = 0, i;
#endif
struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 };
struct diag_hdlc_dest_type enc = { NULL, NULL, 0 };
void *buf_copy = NULL;
int payload_size;
unsigned char is_encoded = 0;
void *large_buf_hdlc;
#ifdef CONFIG_DIAG_OVER_USB
if (((driver->logging_mode == USB_MODE) && (!driver->usb_connected)) ||
(driver->logging_mode == NO_LOGGING_MODE)) {
/*Drop the diag payload */
return -EIO;
}
#endif /* DIAG over USB */
/* Get the packet type F3/log/event/Pkt response */
err = copy_from_user((&pkt_type), buf, 4);
/*First 4 bytes indicate the type of payload - ignore these */
payload_size = count - 4;
if(pkt_type == -3) { //-DIAG_DATA_TYPE_RESPONSE => no hdlc encode
is_encoded = 1;
} else {
is_encoded = 0;
}
if(is_encoded) {
large_buf_hdlc = diagmem_alloc(driver, HDLC_OUT_BUF_SIZE, POOL_TYPE_HDLC);
if (!large_buf_hdlc) {
driver->dropped_count++;
return -ENOMEM;
}
err = copy_from_user(large_buf_hdlc, buf + 4, payload_size);
if (err) {
diagmem_free(driver, large_buf_hdlc, POOL_TYPE_HDLC);
printk(KERN_INFO "diagchar : copy_from_user failed \n");
ret = -EFAULT;
return ret;
}
mutex_lock(&driver->diagchar_mutex);
driver->write_ptr_svc = (struct diag_request *)
(diagmem_alloc(driver, sizeof(struct diag_request),
POOL_TYPE_WRITE_STRUCT));
driver->write_ptr_svc->buf = large_buf_hdlc;
driver->used = payload_size;
driver->write_ptr_svc->length = driver->used;
err = usb_diag_write(driver->legacy_ch, driver->write_ptr_svc);
if (err) {
printk(KERN_INFO "\n size written length[%d] error[%d] \n", driver->used, err);
diagmem_free(driver, large_buf_hdlc, POOL_TYPE_HDLC);
ret = -EIO;
mutex_unlock(&driver->diagchar_mutex);
return -EIO;
}
#ifdef DIAG_DEBUG
printk(KERN_INFO "\n LARGE_size written is %d \n", driver->used);
#endif
driver->used = 0;
mutex_unlock(&driver->diagchar_mutex);
return 0;
}
if (pkt_type == MEMORY_DEVICE_LOG_TYPE) {
if (!mask_request_validate((unsigned char *)buf)) {
printk(KERN_ALERT "mask request Invalid ..cannot send to modem \n");
return -EFAULT;
}
buf = buf + 4;
#ifdef DIAG_DEBUG
printk(KERN_INFO "\n I got the masks: %d\n", payload_size);
for (i = 0; i < payload_size; i++)
printk(KERN_DEBUG "\t %x", *(((unsigned char *)buf)+i));
#endif
diag_process_hdlc((void *)buf, payload_size);
return 0;
}
buf_copy = diagmem_alloc(driver, payload_size, POOL_TYPE_COPY);
if (!buf_copy) {
driver->dropped_count++;
return -ENOMEM;
}
err = copy_from_user(buf_copy, buf + 4, payload_size);
if (err) {
printk(KERN_INFO "diagchar : copy_from_user failed\n");
ret = -EFAULT;
goto fail_free_copy;
}
#ifdef DIAG_DEBUG
printk(KERN_DEBUG "data is -->\n");
for (i = 0; i < payload_size; i++)
printk(KERN_DEBUG "\t %x \t", *(((unsigned char *)buf_copy)+i));
#endif
send.state = DIAG_STATE_START;
send.pkt = buf_copy;
send.last = (void *)(buf_copy + payload_size - 1);
send.terminate = 1;
#ifdef DIAG_DEBUG
printk(KERN_INFO "\n Already used bytes in buffer %d, and"
" incoming payload size is %d\n", driver->used, payload_size);
printk(KERN_DEBUG "hdlc encoded data is -->\n");
for (i = 0; i < payload_size + 8; i++) {
printk(KERN_DEBUG "\t %x \t", *(((unsigned char *)buf_hdlc)+i));
if (*(((unsigned char *)buf_hdlc)+i) != 0x7e)
length++;
}
#endif
mutex_lock(&driver->diagchar_mutex);
if (!buf_hdlc)
buf_hdlc = diagmem_alloc(driver, HDLC_OUT_BUF_SIZE,
POOL_TYPE_HDLC);
if (!buf_hdlc) {
ret = -ENOMEM;
goto fail_free_hdlc;
}
if (HDLC_OUT_BUF_SIZE - driver->used <= (2*payload_size) + 3) {
err = diag_device_write(buf_hdlc, APPS_DATA, NULL);
if (err) {
/*Free the buffer right away if write failed */
diagmem_free(driver, buf_hdlc, POOL_TYPE_HDLC);
diagmem_free(driver, (unsigned char *)driver->
write_ptr_svc, POOL_TYPE_WRITE_STRUCT);
ret = -EIO;
goto fail_free_hdlc;
}
buf_hdlc = NULL;
#ifdef DIAG_DEBUG
printk(KERN_INFO "\n size written is %d\n", driver->used);
#endif
driver->used = 0;
buf_hdlc = diagmem_alloc(driver, HDLC_OUT_BUF_SIZE,
POOL_TYPE_HDLC);
if (!buf_hdlc) {
ret = -ENOMEM;
goto fail_free_hdlc;
}
}
enc.dest = buf_hdlc + driver->used;
enc.dest_last = (void *)(buf_hdlc + driver->used + 2*payload_size + 3);
diag_hdlc_encode(&send, &enc);
/* This is to check if after HDLC encoding, we are still within the
limits of aggregation buffer. If not, we write out the current buffer
and start aggregation in a newly allocated buffer */
if ((unsigned int) enc.dest >=
(unsigned int)(buf_hdlc + HDLC_OUT_BUF_SIZE)) {
err = diag_device_write(buf_hdlc, APPS_DATA, NULL);
if (err) {
/*Free the buffer right away if write failed */
diagmem_free(driver, buf_hdlc, POOL_TYPE_HDLC);
diagmem_free(driver, (unsigned char *)driver->
write_ptr_svc, POOL_TYPE_WRITE_STRUCT);
ret = -EIO;
goto fail_free_hdlc;
}
buf_hdlc = NULL;
#ifdef DIAG_DEBUG
printk(KERN_INFO "\n size written is %d\n", driver->used);
#endif
driver->used = 0;
buf_hdlc = diagmem_alloc(driver, HDLC_OUT_BUF_SIZE,
POOL_TYPE_HDLC);
if (!buf_hdlc) {
ret = -ENOMEM;
goto fail_free_hdlc;
}
enc.dest = buf_hdlc + driver->used;
enc.dest_last = (void *)(buf_hdlc + driver->used +
(2*payload_size) + 3);
diag_hdlc_encode(&send, &enc);
}
driver->used = (uint32_t) enc.dest - (uint32_t) buf_hdlc;
if (pkt_type == DATA_TYPE_RESPONSE) {
err = diag_device_write(buf_hdlc, APPS_DATA, NULL);
if (err) {
/*Free the buffer right away if write failed */
diagmem_free(driver, buf_hdlc, POOL_TYPE_HDLC);
diagmem_free(driver, (unsigned char *)driver->
write_ptr_svc, POOL_TYPE_WRITE_STRUCT);
ret = -EIO;
goto fail_free_hdlc;
}
buf_hdlc = NULL;
#ifdef DIAG_DEBUG
printk(KERN_INFO "\n size written is %d\n", driver->used);
#endif
driver->used = 0;
}
mutex_unlock(&driver->diagchar_mutex);
diagmem_free(driver, buf_copy, POOL_TYPE_COPY);
if (!timer_in_progress) {
timer_in_progress = 1;
ret = mod_timer(&drain_timer, jiffies + msecs_to_jiffies(500));
}
return 0;
fail_free_hdlc:
buf_hdlc = NULL;
driver->used = 0;
diagmem_free(driver, buf_copy, POOL_TYPE_COPY);
mutex_unlock(&driver->diagchar_mutex);
return ret;
fail_free_copy:
diagmem_free(driver, buf_copy, POOL_TYPE_COPY);
return ret;
}
int diag_device_write(void *buf, int proc_num)
{
int i, err = 0;
if (driver->logging_mode == USB_MODE) {
if (proc_num == APPS_DATA) {
driver->usb_write_ptr_svc = (struct diag_request *)
(diagmem_alloc(driver, sizeof(struct diag_request),
POOL_TYPE_USB_STRUCT));
driver->usb_write_ptr_svc->length = driver->used;
driver->usb_write_ptr_svc->buf = buf;
err = diag_write(driver->usb_write_ptr_svc);
} else if (proc_num == MODEM_DATA) {
driver->usb_write_ptr->buf = buf;
#ifdef DIAG_DEBUG
printk(KERN_INFO "writing data to USB,"
" pkt length %d \n",
driver->usb_write_ptr->length);
print_hex_dump(KERN_DEBUG, "Written Packet Data"
" to USB: ", 16, 1, DUMP_PREFIX_
ADDRESS, buf, driver->
usb_write_ptr->length, 1);
#endif
err = diag_write(driver->usb_write_ptr);
} else if (proc_num == QDSP_DATA) {
driver->usb_write_ptr_qdsp->buf = buf;
err = diag_write(driver->usb_write_ptr_qdsp);
}
APPEND_DEBUG('k');
} else if (driver->logging_mode == MEMORY_DEVICE_MODE) {
if (proc_num == APPS_DATA) {
for (i = 0; i < driver->poolsize_usb_struct; i++)
if (driver->buf_tbl[i].length == 0) {
driver->buf_tbl[i].buf = buf;
driver->buf_tbl[i].length =
driver->used;
#ifdef DIAG_DEBUG
printk(KERN_INFO "\n ENQUEUE buf ptr"
" and length is %x , %d\n",
(unsigned int)(driver->buf_
tbl[i].buf), driver->buf_tbl[i].length);
#endif
break;
}
}
for (i = 0; i < driver->num_clients; i++)
if (driver->client_map[i] == driver->logging_process_id)
break;
if (i < driver->num_clients) {
driver->data_ready[i] |= MEMORY_DEVICE_LOG_TYPE;
wake_up_interruptible(&driver->wait_q);
} else
return -EINVAL;
} else if (driver->logging_mode == NO_LOGGING_MODE) {
if (proc_num == MODEM_DATA) {
driver->in_busy = 0;
queue_work(driver->diag_wq, &(driver->
diag_read_smd_work));
} else if (proc_num == QDSP_DATA) {
driver->in_busy_qdsp = 0;
queue_work(driver->diag_wq, &(driver->
diag_read_smd_qdsp_work));
}
err = -1;
}
return err;
}
static void diag_read_hsic_work_fn(struct work_struct *work)
{
unsigned char *buf_in_hsic = NULL;
int num_reads_submitted = 0;
int err = 0;
int write_ptrs_available;
struct diag_hsic_dev *hsic_struct = container_of(work,
struct diag_hsic_dev, diag_read_hsic_work);
int index = hsic_struct->id;
static DEFINE_RATELIMIT_STATE(rl, 10*HZ, 1);
if (!diag_hsic[index].hsic_ch) {
pr_err("DIAG in %s: diag_hsic[index].hsic_ch == 0\n", __func__);
return;
}
/*
* Determine the current number of available buffers for writing after
* reading from the HSIC has completed.
*/
if (driver->logging_mode == MEMORY_DEVICE_MODE)
write_ptrs_available = diag_hsic[index].poolsize_hsic_write -
diag_hsic[index].
num_hsic_buf_tbl_entries;
else
write_ptrs_available = diag_hsic[index].poolsize_hsic_write -
diag_hsic[index].count_hsic_write_pool;
/*
* Queue up a read on the HSIC for all available buffers in the
* pool, exhausting the pool.
*/
do {
/*
* If no more write buffers are available,
* stop queuing reads
*/
if (write_ptrs_available <= 0)
break;
write_ptrs_available--;
/*
* No sense queuing a read if the HSIC bridge was
* closed in another thread
*/
if (!diag_hsic[index].hsic_ch)
break;
buf_in_hsic = diagmem_alloc(driver, READ_HSIC_BUF_SIZE,
index+POOL_TYPE_HSIC);
if (buf_in_hsic) {
/*
* Initiate the read from the HSIC. The HSIC read is
* asynchronous. Once the read is complete the read
* callback function will be called.
*/
pr_debug("diag: read from HSIC\n");
num_reads_submitted++;
err = diag_bridge_read(hsic_data_bridge_map[index],
(char *)buf_in_hsic,
READ_HSIC_BUF_SIZE);
if (err) {
num_reads_submitted--;
/* Return the buffer to the pool */
diagmem_free(driver, buf_in_hsic,
index+POOL_TYPE_HSIC);
if (__ratelimit(&rl))
pr_err("diag: Error initiating HSIC read, err: %d\n",
err);
/*
* An error occurred, discontinue queuing
* reads
*/
break;
}
}
} while (buf_in_hsic);
/*
* If there are read buffers available and for some reason the
* read was not queued, and if no unrecoverable error occurred
* (-ENODEV is an unrecoverable error), then set up the next read
*/
if ((diag_hsic[index].count_hsic_pool <
diag_hsic[index].poolsize_hsic) &&
(num_reads_submitted == 0) && (err != -ENODEV) &&
(diag_hsic[index].hsic_ch != 0))
queue_work(diag_bridge[index].wq,
&diag_hsic[index].diag_read_hsic_work);
}
static void diag_read_hsic_dci_work_fn(struct work_struct *work)
{
unsigned char *buf_in_hsic = NULL;
int num_reads_submitted = 0;
int err = 0;
struct diag_hsic_dci_dev *hsic_struct = container_of(work,
struct diag_hsic_dci_dev,
diag_read_hsic_work);
int index = hsic_struct->id;
if (!diag_hsic_dci[index].hsic_ch) {
pr_err("diag: Invalid HSIC channel in %s\n", __func__);
return;
}
/*
* Queue up a read on the HSIC for all available buffers in the
* pool, exhausting the pool.
*/
do {
/*
* No sense queuing a read if the HSIC bridge was
* closed in another thread
*/
if (!diag_hsic_dci[index].hsic_ch)
break;
buf_in_hsic = diagmem_alloc(driver, READ_HSIC_BUF_SIZE_DCI,
POOL_TYPE_HSIC_DCI + index);
if (buf_in_hsic) {
/*
* Initiate the read from the HSIC. The HSIC read is
* asynchronous. Once the read is complete the read
* callback function will be called.
*/
num_reads_submitted++;
err = diag_bridge_read(hsic_dci_bridge_map[index],
(char *)buf_in_hsic,
READ_HSIC_BUF_SIZE_DCI);
if (err) {
num_reads_submitted--;
/* Return the buffer to the pool */
diagmem_free(driver, buf_in_hsic,
POOL_TYPE_HSIC_DCI + index);
pr_err_ratelimited("diag: Error initiating HSIC read, err: %d\n",
err);
/*
* An error occurred, discontinue queuing
* reads
*/
break;
}
}
} while (buf_in_hsic);
/*
* If there are read buffers available and for some reason the
* read was not queued, and if no unrecoverable error occurred
* (-ENODEV is an unrecoverable error), then set up the next read
*/
if ((diag_hsic_dci[index].count_hsic_pool <
diag_hsic_dci[index].poolsize_hsic) &&
(num_reads_submitted == 0) && (err != -ENODEV) &&
(diag_hsic_dci[index].hsic_ch != 0))
queue_work(diag_bridge_dci[index].wq,
&diag_hsic_dci[index].diag_read_hsic_work);
}
int diag_device_write(void *buf, int proc_num, struct diag_request *write_ptr)
{
int i, err = 0;
if (driver->logging_mode == MEMORY_DEVICE_MODE) {
if (proc_num == APPS_DATA) {
for (i = 0; i < driver->poolsize_write_struct; i++)
if (driver->buf_tbl[i].length == 0) {
driver->buf_tbl[i].buf = buf;
driver->buf_tbl[i].length =
driver->used;
#ifdef DIAG_DEBUG
pr_debug("diag: ENQUEUE buf ptr"
" and length is %x , %d\n",
(unsigned int)(driver->buf_tbl[i].buf), driver->buf_tbl[i].length);
#endif
break;
}
}
#ifdef CONFIG_DIAG_SDIO_PIPE
if (proc_num == SDIO_DATA) {
for (i = 0; i < driver->num_mdmclients; i++)
if (driver->mdmclient_map[i].pid ==
driver->logging_process_id)
break;
if (i < driver->num_mdmclients) {
driver->mdmdata_ready[i] |= USERMODE_DIAGFWD;
wake_up_interruptible(&driver->mdmwait_q);
return err;
} else
return -EINVAL;
}
#endif
for (i = 0; i < driver->num_clients; i++)
if (driver->client_map[i].pid ==
driver->logging_process_id)
break;
if (i < driver->num_clients) {
wake_lock_timeout(&driver->wake_lock, HZ / 2);
driver->data_ready[i] |= USERMODE_DIAGFWD;
wake_up_interruptible(&driver->wait_q);
} else
return -EINVAL;
} else if (driver->logging_mode == NO_LOGGING_MODE) {
if (proc_num == MODEM_DATA) {
driver->in_busy_1 = 0;
driver->in_busy_2 = 0;
queue_work(driver->diag_wq, &(driver->
diag_read_smd_work));
} else if (proc_num == QDSP_DATA) {
driver->in_busy_qdsp_1 = 0;
driver->in_busy_qdsp_2 = 0;
queue_work(driver->diag_wq, &(driver->
diag_read_smd_qdsp_work));
} else if (proc_num == WCNSS_DATA) {
driver->in_busy_wcnss = 0;
queue_work(driver->diag_wq, &(driver->
diag_read_smd_wcnss_work));
}
err = -1;
}
#ifdef CONFIG_DIAG_OVER_USB
else if (driver->logging_mode == USB_MODE) {
if (proc_num == APPS_DATA) {
driver->write_ptr_svc = (struct diag_request *)
(diagmem_alloc(driver, sizeof(struct diag_request),
POOL_TYPE_WRITE_STRUCT));
if (driver->write_ptr_svc) {
driver->write_ptr_svc->length = driver->used;
driver->write_ptr_svc->buf = buf;
err = usb_diag_write(driver->legacy_ch,
driver->write_ptr_svc);
} else
err = -1;
} else if (proc_num == MODEM_DATA) {
write_ptr->buf = buf;
#ifdef DIAG_DEBUG
printk(KERN_INFO "writing data to USB,"
"pkt length %d\n", write_ptr->length);
print_hex_dump(KERN_DEBUG, "Written Packet Data to"
" USB: ", 16, 1, DUMP_PREFIX_ADDRESS,
buf, write_ptr->length, 1);
#endif /* DIAG DEBUG */
err = usb_diag_write(driver->legacy_ch, write_ptr);
} else if (proc_num == QDSP_DATA) {
write_ptr->buf = buf;
err = usb_diag_write(driver->legacy_ch, write_ptr);
} else if (proc_num == WCNSS_DATA) {
write_ptr->buf = buf;
err = usb_diag_write(driver->legacy_ch, write_ptr);
}
#ifdef CONFIG_DIAG_SDIO_PIPE
else if (proc_num == SDIO_DATA) {
if (diag_support_mdm9k) {
write_ptr->buf = buf;
err = usb_diag_write(driver->mdm_ch, write_ptr);
} else
pr_err("diag: Incorrect data while USB write");
}
#endif
APPEND_DEBUG('d');
}
#endif /* DIAG OVER USB */
return err;
}
static int diagchar_write(struct file *file, const char __user *buf,
size_t count, loff_t *ppos)
{
int err, ret = 0, pkt_type;
#ifdef CONFIG_LGE_DM_APP
char *buf_cmp;
#endif
#ifdef DIAG_DEBUG
int length = 0, i;
#endif
struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 };
struct diag_hdlc_dest_type enc = { NULL, NULL, 0 };
void *buf_copy = NULL;
int payload_size;
#ifdef CONFIG_DIAG_OVER_USB
if (((driver->logging_mode == USB_MODE) && (!driver->usb_connected)) ||
(driver->logging_mode == NO_LOGGING_MODE)) {
/*Drop the diag payload */
return -EIO;
}
#endif /* DIAG over USB */
/* Get the packet type F3/log/event/Pkt response */
err = copy_from_user((&pkt_type), buf, 4);
/* First 4 bytes indicate the type of payload - ignore these */
payload_size = count - 4;
#ifdef CONFIG_LGE_DM_APP
if (driver->logging_mode == DM_APP_MODE) {
/* only diag cmd #250 for supporting testmode tool */
buf_cmp = (char *)buf + 4;
if (*(buf_cmp) != 0xFA)
return 0;
}
#endif
if (payload_size > USER_SPACE_DATA) {
pr_err("diag: Dropping packet, packet payload size crosses 8KB limit. Current payload size %d\n",
payload_size);
driver->dropped_count++;
return -EBADMSG;
}
if (pkt_type == DCI_DATA_TYPE) {
err = copy_from_user(driver->user_space_data, buf + 4,
payload_size);
if (err) {
pr_alert("diag: copy failed for DCI data\n");
return DIAG_DCI_SEND_DATA_FAIL;
}
err = diag_process_dci_client(driver->user_space_data,
payload_size);
return err;
}
if (pkt_type == USER_SPACE_LOG_TYPE) {
err = copy_from_user(driver->user_space_data, buf + 4,
payload_size);
/* Check masks for On-Device logging */
if (driver->mask_check) {
if (!mask_request_validate(driver->user_space_data)) {
pr_alert("diag: mask request Invalid\n");
return -EFAULT;
}
}
buf = buf + 4;
#ifdef DIAG_DEBUG
pr_debug("diag: user space data %d\n", payload_size);
for (i = 0; i < payload_size; i++)
pr_debug("\t %x", *((driver->user_space_data)+i));
#endif
#ifdef CONFIG_DIAG_SDIO_PIPE
/* send masks to 9k too */
if (driver->sdio_ch) {
wait_event_interruptible(driver->wait_q,
(sdio_write_avail(driver->sdio_ch) >=
payload_size));
if (driver->sdio_ch && (payload_size > 0)) {
sdio_write(driver->sdio_ch, (void *)
(driver->user_space_data), payload_size);
}
}
#endif
#ifdef CONFIG_DIAG_BRIDGE_CODE
/* send masks to 9k too */
if (driver->hsic_ch && (payload_size > 0)) {
/* wait sending mask updates if HSIC ch not ready */
if (driver->in_busy_hsic_write)
wait_event_interruptible(driver->wait_q,
(driver->in_busy_hsic_write != 1));
driver->in_busy_hsic_write = 1;
driver->in_busy_hsic_read_on_device = 0;
err = diag_bridge_write(driver->user_space_data,
payload_size);
if (err) {
pr_err("diag: err sending mask to MDM: %d\n",
err);
/*
* If the error is recoverable, then clear
* the write flag, so we will resubmit a
* write on the next frame. Otherwise, don't
* resubmit a write on the next frame.
*/
if ((-ESHUTDOWN) != err)
driver->in_busy_hsic_write = 0;
}
}
#endif
/* send masks to 8k now */
diag_process_hdlc((void *)(driver->user_space_data),
payload_size);
return 0;
}
if (payload_size > itemsize) {
pr_err("diag: Dropping packet, packet payload size crosses"
"4KB limit. Current payload size %d\n",
payload_size);
driver->dropped_count++;
return -EBADMSG;
}
buf_copy = diagmem_alloc(driver, payload_size, POOL_TYPE_COPY);
if (!buf_copy) {
driver->dropped_count++;
return -ENOMEM;
}
err = copy_from_user(buf_copy, buf + 4, payload_size);
if (err) {
printk(KERN_INFO "diagchar : copy_from_user failed\n");
ret = -EFAULT;
goto fail_free_copy;
}
#ifdef DIAG_DEBUG
printk(KERN_DEBUG "data is -->\n");
for (i = 0; i < payload_size; i++)
printk(KERN_DEBUG "\t %x \t", *(((unsigned char *)buf_copy)+i));
#endif
send.state = DIAG_STATE_START;
send.pkt = buf_copy;
send.last = (void *)(buf_copy + payload_size - 1);
send.terminate = 1;
#ifdef DIAG_DEBUG
pr_debug("diag: Already used bytes in buffer %d, and"
" incoming payload size is %d\n", driver->used, payload_size);
printk(KERN_DEBUG "hdlc encoded data is -->\n");
for (i = 0; i < payload_size + 8; i++) {
printk(KERN_DEBUG "\t %x \t", *(((unsigned char *)buf_hdlc)+i));
if (*(((unsigned char *)buf_hdlc)+i) != 0x7e)
length++;
}
#endif
mutex_lock(&driver->diagchar_mutex);
if (!buf_hdlc)
buf_hdlc = diagmem_alloc(driver, HDLC_OUT_BUF_SIZE,
POOL_TYPE_HDLC);
if (!buf_hdlc) {
ret = -ENOMEM;
goto fail_free_hdlc;
}
if (HDLC_OUT_BUF_SIZE - driver->used <= (2*payload_size) + 3) {
err = diag_device_write(buf_hdlc, APPS_DATA, NULL);
if (err) {
/*Free the buffer right away if write failed */
diagmem_free(driver, buf_hdlc, POOL_TYPE_HDLC);
diagmem_free(driver, (unsigned char *)driver->
write_ptr_svc, POOL_TYPE_WRITE_STRUCT);
ret = -EIO;
goto fail_free_hdlc;
}
buf_hdlc = NULL;
driver->used = 0;
buf_hdlc = diagmem_alloc(driver, HDLC_OUT_BUF_SIZE,
POOL_TYPE_HDLC);
if (!buf_hdlc) {
ret = -ENOMEM;
goto fail_free_hdlc;
}
}
enc.dest = buf_hdlc + driver->used;
enc.dest_last = (void *)(buf_hdlc + driver->used + 2*payload_size + 3);
diag_hdlc_encode(&send, &enc);
/* This is to check if after HDLC encoding, we are still within the
limits of aggregation buffer. If not, we write out the current buffer
and start aggregation in a newly allocated buffer */
if ((unsigned int) enc.dest >=
(unsigned int)(buf_hdlc + HDLC_OUT_BUF_SIZE)) {
err = diag_device_write(buf_hdlc, APPS_DATA, NULL);
if (err) {
/*Free the buffer right away if write failed */
diagmem_free(driver, buf_hdlc, POOL_TYPE_HDLC);
diagmem_free(driver, (unsigned char *)driver->
write_ptr_svc, POOL_TYPE_WRITE_STRUCT);
ret = -EIO;
goto fail_free_hdlc;
}
buf_hdlc = NULL;
driver->used = 0;
buf_hdlc = diagmem_alloc(driver, HDLC_OUT_BUF_SIZE,
POOL_TYPE_HDLC);
if (!buf_hdlc) {
ret = -ENOMEM;
goto fail_free_hdlc;
}
enc.dest = buf_hdlc + driver->used;
enc.dest_last = (void *)(buf_hdlc + driver->used +
(2*payload_size) + 3);
diag_hdlc_encode(&send, &enc);
}
driver->used = (uint32_t) enc.dest - (uint32_t) buf_hdlc;
if (pkt_type == DATA_TYPE_RESPONSE) {
err = diag_device_write(buf_hdlc, APPS_DATA, NULL);
if (err) {
/*Free the buffer right away if write failed */
diagmem_free(driver, buf_hdlc, POOL_TYPE_HDLC);
diagmem_free(driver, (unsigned char *)driver->
write_ptr_svc, POOL_TYPE_WRITE_STRUCT);
ret = -EIO;
goto fail_free_hdlc;
}
buf_hdlc = NULL;
driver->used = 0;
}
mutex_unlock(&driver->diagchar_mutex);
diagmem_free(driver, buf_copy, POOL_TYPE_COPY);
if (!timer_in_progress) {
timer_in_progress = 1;
ret = mod_timer(&drain_timer, jiffies + msecs_to_jiffies(500));
}
return 0;
fail_free_hdlc:
buf_hdlc = NULL;
driver->used = 0;
diagmem_free(driver, buf_copy, POOL_TYPE_COPY);
mutex_unlock(&driver->diagchar_mutex);
return ret;
fail_free_copy:
diagmem_free(driver, buf_copy, POOL_TYPE_COPY);
return ret;
}
static void diag_read_hsic_work_fn(struct work_struct *work)
{
unsigned char *buf_in_hsic = NULL;
int err = 0;
struct diag_hsic_dev *hsic_struct = container_of(work,
struct diag_hsic_dev, diag_read_hsic_work);
int index = hsic_struct->id;
static DEFINE_RATELIMIT_STATE(rl, 10*HZ, 1);
if (!diag_hsic[index].hsic_ch) {
pr_err("DIAG in %s: diag_hsic[index].hsic_ch == 0\n", __func__);
return;
}
/*
* Queue up a read on the HSIC for all available buffers in the
* pool, exhausting the pool.
*/
do {
/*
* No sense queuing a read if the HSIC bridge was
* closed in another thread
*/
if (!diag_hsic[index].hsic_ch)
break;
buf_in_hsic = diagmem_alloc(driver, DIAG_MDM_BUF_SIZE,
index+POOL_TYPE_MDM);
if (buf_in_hsic) {
/*
* Initiate the read from the HSIC. The HSIC read is
* asynchronous. Once the read is complete the read
* callback function will be called.
*/
pr_debug("diag: read from HSIC\n");
err = diag_bridge_read(hsic_data_bridge_map[index],
(char *)buf_in_hsic,
DIAG_MDM_BUF_SIZE);
if (err) {
/* Return the buffer to the pool */
diagmem_free(driver, buf_in_hsic,
index+POOL_TYPE_MDM);
if (__ratelimit(&rl))
pr_err("diag: Error initiating HSIC read, err: %d\n",
err);
/*
* An error occurred, discontinue queuing
* reads
*/
break;
}
}
} while (buf_in_hsic);
/*
* If no unrecoverable error occurred (-ENODEV is an
* unrecoverable error), then set up the next read
*/
if ((err != -ENODEV) && (diag_hsic[index].hsic_ch != 0))
queue_work(diag_bridge[index].wq,
&diag_hsic[index].diag_read_hsic_work);
}
int diag_device_write(void *buf, int proc_num, struct diag_request *write_ptr)
{
int i, err = 0;
if (driver->logging_mode == MEMORY_DEVICE_MODE) {
if (proc_num == APPS_DATA) {
for (i = 0; i < driver->poolsize_write_struct; i++)
if (driver->buf_tbl[i].length == 0) {
driver->buf_tbl[i].buf = buf;
driver->buf_tbl[i].length =
driver->used;
#ifdef DIAG_DEBUG
printk(KERN_INFO "\n ENQUEUE buf ptr"
" and length is %x , %d\n",
(unsigned int)(driver->buf_
tbl[i].buf), driver->buf_tbl[i].length);
#endif
break;
}
}
for (i = 0; i < driver->num_clients; i++)
if (driver->client_map[i].pid ==
driver->logging_process_id)
break;
if (i < driver->num_clients) {
driver->data_ready[i] |= MEMORY_DEVICE_LOG_TYPE;
wake_up_interruptible(&driver->wait_q);
} else
return -EINVAL;
} else if (driver->logging_mode == NO_LOGGING_MODE) {
if (proc_num == MODEM_DATA) {
driver->in_busy_1 = 0;
driver->in_busy_2 = 0;
queue_work(driver->diag_wq, &(driver->
diag_read_smd_work));
} else if (proc_num == QDSP_DATA) {
driver->in_busy_qdsp_1 = 0;
driver->in_busy_qdsp_2 = 0;
queue_work(driver->diag_wq, &(driver->
diag_read_smd_qdsp_work));
}
err = -1;
}
#ifdef CONFIG_DIAG_OVER_USB
else if (driver->logging_mode == USB_MODE) {
if (proc_num == APPS_DATA) {
driver->write_ptr_svc = (struct diag_request *)
(diagmem_alloc(driver, sizeof(struct diag_request),
POOL_TYPE_WRITE_STRUCT));
if (driver->write_ptr_svc) {
driver->write_ptr_svc->length = driver->used;
driver->write_ptr_svc->buf = buf;
#ifdef CONFIG_SH_USB_CUST
err = diag_write(driver->write_ptr_svc);
#else /* CONFIG_SH_USB_CUST */
err = usb_diag_write(driver->legacy_ch,
driver->write_ptr_svc);
#endif /* CONFIG_SH_USB_CUST */
} else
err = -1;
} else if (proc_num == MODEM_DATA) {
write_ptr->buf = buf;
#ifdef DIAG_DEBUG
printk(KERN_INFO "writing data to USB,"
"pkt length %d\n", write_ptr->length);
print_hex_dump(KERN_DEBUG, "Written Packet Data to"
" USB: ", 16, 1, DUMP_PREFIX_ADDRESS,
buf, write_ptr->length, 1);
#endif /* DIAG DEBUG */
#ifdef CONFIG_SH_USB_CUST
err = diag_write(write_ptr);
#else /* CONFIG_SH_USB_CUST */
err = usb_diag_write(driver->legacy_ch, write_ptr);
#endif /* CONFIG_SH_USB_CUST */
} else if (proc_num == QDSP_DATA) {
write_ptr->buf = buf;
#ifdef CONFIG_SH_USB_CUST
err = diag_write(write_ptr);
#else /* CONFIG_SH_USB_CUST */
err = usb_diag_write(driver->legacy_ch, write_ptr);
#endif /* CONFIG_SH_USB_CUST */
}
#ifdef CONFIG_DIAG_SDIO_PIPE
else if (proc_num == SDIO_DATA) {
/* COORDINATOR Qualcomm4040 MERGE start */
#ifdef CONFIG_SH_USB_CUST
write_ptr->buf = buf;
diag_write(write_ptr);
#else /* CONFIG_SH_USB_CUST */
if (machine_is_msm8x60_charm_surf() ||
machine_is_msm8x60_charm_ffa()) {
write_ptr->buf = buf;
err = usb_diag_write(driver->mdm_ch, write_ptr);
} else
pr_err("diag: Incorrect data while USB write");
#endif /* CONFIG_SH_USB_CUST */
/* COORDINATOR Qualcomm4040 MERGE end */
}
#endif
APPEND_DEBUG('d');
}
#endif /* DIAG OVER USB */
return err;
}
int diag_mux_write(int proc, unsigned char *buf, int len, int ctx)
{
#ifdef CONFIG_LGE_DM_APP
int i;
uint8_t found = 0;
unsigned long flags;
struct diag_usb_info *usb_info = NULL;
#endif
if (proc < 0 || proc >= NUM_MUX_PROC)
return -EINVAL;
#ifdef CONFIG_LGE_DM_APP
if (driver->logging_mode == DM_APP_MODE) {
/* only diag cmd #250 for supporting testmode tool */
if ((GET_BUF_PERIPHERAL(ctx) == APPS_DATA) && (*((char *)buf) == 0xFA)) {
usb_info = &diag_usb[lge_dm_tty->id];
lge_dm_tty->dm_usb_req = diagmem_alloc(driver, sizeof(struct diag_request),
usb_info->mempool);
if (lge_dm_tty->dm_usb_req) {
lge_dm_tty->dm_usb_req->buf = buf;
lge_dm_tty->dm_usb_req->length = len;
lge_dm_tty->dm_usb_req->context = (void *)(uintptr_t)ctx;
queue_work(lge_dm_tty->dm_wq,
&(lge_dm_tty->dm_usb_work));
flush_work(&(lge_dm_tty->dm_usb_work));
}
return 0;
}
for (i = 0; i < lge_dm_tty->num_tbl_entries && !found; i++) {
spin_lock_irqsave(&lge_dm_tty->tbl[i].lock, flags);
if (lge_dm_tty->tbl[i].len == 0) {
lge_dm_tty->tbl[i].buf = buf;
lge_dm_tty->tbl[i].len = len;
lge_dm_tty->tbl[i].ctx = ctx;
found = 1;
diag_ws_on_read(DIAG_WS_MD, len);
}
spin_unlock_irqrestore(&lge_dm_tty->tbl[i].lock, flags);
}
lge_dm_tty->set_logging = 1;
wake_up_interruptible(&lge_dm_tty->waitq);
return 0;
}
#endif
#ifdef CONFIG_LGE_DIAG_BYPASS
if(diag_bypass_response(buf, len, proc, ctx, logger) > 0) {
return 0;
}
#endif
if (logger && logger->log_ops && logger->log_ops->write)
return logger->log_ops->write(proc, buf, len, ctx);
return 0;
}
static void diag_read_hsic_work_fn(struct work_struct *work)
{
unsigned char *buf_in_hsic = NULL;
int num_reads_submitted = 0;
int err = 0;
int write_ptrs_available;
if (!driver->hsic_ch) {
pr_err("DIAG in %s: driver->hsic_ch == 0\n", __func__);
return;
}
/*
* Determine the current number of available buffers for writing after
* reading from the HSIC has completed.
*/
if (driver->logging_mode == MEMORY_DEVICE_MODE)
write_ptrs_available = driver->poolsize_hsic_write -
driver->num_hsic_buf_tbl_entries;
else
write_ptrs_available = driver->poolsize_hsic_write -
driver->count_hsic_write_pool;
/*
* Queue up a read on the HSIC for all available buffers in the
* pool, exhausting the pool.
*/
do {
/*
* If no more write buffers are available,
* stop queuing reads
*/
if (write_ptrs_available <= 0)
break;
write_ptrs_available--;
buf_in_hsic = diagmem_alloc(driver, READ_HSIC_BUF_SIZE,
POOL_TYPE_HSIC);
if (buf_in_hsic) {
/*
* Initiate the read from the hsic. The hsic read is
* asynchronous. Once the read is complete the read
* callback function will be called.
*/
pr_debug("diag: read from HSIC\n");
num_reads_submitted++;
err = diag_bridge_read((char *)buf_in_hsic,
READ_HSIC_BUF_SIZE);
if (err) {
num_reads_submitted--;
/* Return the buffer to the pool */
diagmem_free(driver, buf_in_hsic,
POOL_TYPE_HSIC);
pr_err_ratelimited("diag: Error initiating HSIC read, err: %d\n",
err);
/*
* An error occurred, discontinue queuing
* reads
*/
break;
}
}
} while (buf_in_hsic);
/*
* If there are no buffers available or for some reason there
* was no hsic data, and if no unrecoverable error occurred
* (-ENODEV is an unrecoverable error), then set up the next read
*/
if ((num_reads_submitted == 0) && (err != -ENODEV))
queue_work(driver->diag_bridge_wq,
&driver->diag_read_hsic_work);
}
int diag_device_write(void *buf, int proc_num, struct diag_request *write_ptr)
{
int i, err = 0;
if (driver->logging_mode == MEMORY_DEVICE_MODE) {
int data_type = 0;
if (proc_num == APPS_DATA) {
data_type = MEMORY_DEVICE_LOG_TYPE;
for (i = 0; i < driver->poolsize_write_struct; i++)
if (driver->buf_tbl[i].length == 0) {
driver->buf_tbl[i].buf = buf;
driver->buf_tbl[i].length = driver->used;
#ifdef DIAG_DEBUG
printk(KERN_INFO "\n ENQUEUE buf ptr"
" and length is %x , %d\n",
(unsigned int)(driver->buf_tbl[i].buf), driver->buf_tbl[i].length);
#endif
break;
}
#if defined(CONFIG_ARCH_MSM8X60_LTE)
} else if (proc_num == MODEM_DATA || proc_num == QDSP_DATA || proc_num == SDIO_DATA) {
#else
} else if (proc_num == MODEM_DATA || proc_num == QDSP_DATA) {
#endif
data_type = USERMODE_DIAGFWD;
#ifdef DIAG_DEBUG
pr_info("%s#%d: Modem Data buf=%p, len=%d\n", __func__, __LINE__, buf, write_ptr->length);
#endif
#if defined(CONFIG_ARCH_MSM8X60_LTE)
if (proc_num == SDIO_DATA) {
for (i = 0; i < driver->num_mdmclients; i++)
if (driver->mdmclient_map[i].pid ==
driver->logging_process_id)
break;
if (i < driver->num_mdmclients) {
driver->mdmdata_ready[i] |= data_type;
wake_up_interruptible(&driver->mdmwait_q);
return err;
} else
return -EINVAL;
}
#endif
}
#if defined(CONFIG_MACH_MECHA)
else if (proc_num == SDIO_DATA) {
for (i = 0; i < driver->num_mdmclients; i++)
if (driver->mdmclient_map[i].pid ==
driver->logging_process_id)
break;
if (i <= driver->num_mdmclients) {
driver->mdmdata_ready[i] |= MEMORY_DEVICE_LOG_TYPE;
wake_up_interruptible(&driver->mdmwait_q);
return err;
} else
return -EINVAL;
}
#endif
for (i = 0; i < driver->num_clients; i++)
if (driver->client_map[i].pid == driver->logging_process_id)
break;
if (i < driver->num_clients) {
driver->data_ready[i] |= data_type;
wake_up_interruptible(&driver->wait_q);
} else
return -EINVAL;
} else if (driver->logging_mode == NO_LOGGING_MODE) {
if (proc_num == MODEM_DATA) {
driver->in_busy_1 = 0;
driver->in_busy_2 = 0;
queue_work(driver->diag_wq, &(driver->
diag_read_smd_work));
} else if (proc_num == QDSP_DATA) {
driver->in_busy_qdsp_1 = 0;
driver->in_busy_qdsp_2 = 0;
queue_work(driver->diag_wq, &(driver->
diag_read_smd_qdsp_work));
}
err = -1;
}
#ifdef CONFIG_DIAG_OVER_USB
else if (driver->logging_mode == USB_MODE) {
if (proc_num == APPS_DATA) {
driver->write_ptr_svc = (struct diag_request *)
(diagmem_alloc(driver, sizeof(struct diag_request),
POOL_TYPE_WRITE_STRUCT));
if (driver->write_ptr_svc) {
driver->write_ptr_svc->length = driver->used;
driver->write_ptr_svc->buf = buf;
err = usb_diag_write(driver->legacy_ch,
driver->write_ptr_svc);
} else
err = -1;
} else if (proc_num == MODEM_DATA) {
write_ptr->buf = buf;
#ifdef DIAG_DEBUG
printk(KERN_INFO "writing data to USB,"
"pkt length %d\n", write_ptr->length);
print_hex_dump(KERN_DEBUG, "Written Packet Data to"
" USB: ", DUMP_PREFIX_ADDRESS, 16, 1,
buf, write_ptr->length, 1);
#endif /* DIAG DEBUG */
driver->diag_smd_count += write_ptr->length;
err = usb_diag_write(driver->legacy_ch, write_ptr);
} else if (proc_num == QDSP_DATA) {
write_ptr->buf = buf;
driver->diag_qdsp_count += write_ptr->length;
err = usb_diag_write(driver->legacy_ch, write_ptr);
}
#if defined(CONFIG_ARCH_MSM8X60_LTE)//|| defined(CONFIG_MACH_MECHA)
else if (proc_num == SDIO_DATA) {
write_ptr->buf = buf;
if (diag_ch_sdio) {
err = usb_diag_write(driver->legacy_ch, write_ptr);
} else {
err = usb_diag_write(driver->mdm_ch, write_ptr);
}
}
#endif
APPEND_DEBUG('d');
}
#endif /* DIAG OVER USB */
return err;
}
static int diagchar_write(struct file *file, const char __user *buf,
size_t count, loff_t *ppos)
{
int err, ret = 0, pkt_type;
#ifdef DIAG_DEBUG
int length = 0, i;
#endif
struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 };
struct diag_hdlc_dest_type enc = { NULL, NULL, 0 };
void *buf_copy = NULL;
int payload_size;
#ifdef CONFIG_DIAG_OVER_USB
if (((driver->logging_mode == USB_MODE) && (!driver->usb_connected)) ||
(driver->logging_mode == NO_LOGGING_MODE)) {
/*Drop the diag payload */
return -EIO;
}
#endif /* DIAG over USB */
/* Get the packet type F3/log/event/Pkt response */
err = copy_from_user((&pkt_type), buf, 4);
/* First 4 bytes indicate the type of payload - ignore these */
payload_size = count - 4;
if (pkt_type == USER_SPACE_LOG_TYPE) {
err = copy_from_user(driver->user_space_data, buf + 4,
payload_size);
/* Check masks for On-Device logging */
if (driver->mask_check) {
if (!mask_request_validate(driver->user_space_data)) {
pr_alert("diag: mask request Invalid\n");
return -EFAULT;
}
}
buf = buf + 4;
#ifdef DIAG_DEBUG
pr_debug("diag: user space data %d\n", payload_size);
for (i = 0; i < payload_size; i++)
pr_debug("\t %x", *((driver->user_space_data)+i));
#endif
#ifdef CONFIG_DIAG_SDIO_PIPE
/* send masks to 9k too */
if (driver->sdio_ch) {
wait_event_interruptible(driver->wait_q,
(sdio_write_avail(driver->sdio_ch) >=
payload_size));
if (driver->sdio_ch && (payload_size > 0)) {
sdio_write(driver->sdio_ch, (void *)
(driver->user_space_data), payload_size);
}
}
#endif
/* send masks to modem now */
diag_process_hdlc((void *)(driver->user_space_data),
payload_size);
return 0;
}
buf_copy = diagmem_alloc(driver, payload_size, POOL_TYPE_COPY);
if (!buf_copy) {
driver->dropped_count++;
return -ENOMEM;
}
err = copy_from_user(buf_copy, buf + 4, payload_size);
if (err) {
printk(KERN_INFO "diagchar : copy_from_user failed\n");
ret = -EFAULT;
goto fail_free_copy;
}
#ifdef DIAG_DEBUG
printk(KERN_DEBUG "data is -->\n");
for (i = 0; i < payload_size; i++)
printk(KERN_DEBUG "\t %x \t", *(((unsigned char *)buf_copy)+i));
#endif
send.state = DIAG_STATE_START;
send.pkt = buf_copy;
send.last = (void *)(buf_copy + payload_size - 1);
send.terminate = 1;
#ifdef DIAG_DEBUG
pr_debug("diag: Already used bytes in buffer %d, and"
" incoming payload size is %d\n", driver->used, payload_size);
printk(KERN_DEBUG "hdlc encoded data is -->\n");
for (i = 0; i < payload_size + 8; i++) {
printk(KERN_DEBUG "\t %x \t", *(((unsigned char *)buf_hdlc)+i));
if (*(((unsigned char *)buf_hdlc)+i) != 0x7e)
length++;
}
#endif
mutex_lock(&driver->diagchar_mutex);
if (!buf_hdlc)
buf_hdlc = diagmem_alloc(driver, HDLC_OUT_BUF_SIZE,
POOL_TYPE_HDLC);
if (!buf_hdlc) {
ret = -ENOMEM;
goto fail_free_hdlc;
}
if (HDLC_OUT_BUF_SIZE - driver->used <= (2*payload_size) + 3) {
err = diag_device_write(buf_hdlc, APPS_DATA, NULL);
if (err) {
/*Free the buffer right away if write failed */
diagmem_free(driver, buf_hdlc, POOL_TYPE_HDLC);
diagmem_free(driver, (unsigned char *)driver->
write_ptr_svc, POOL_TYPE_WRITE_STRUCT);
ret = -EIO;
goto fail_free_hdlc;
}
buf_hdlc = NULL;
driver->used = 0;
buf_hdlc = diagmem_alloc(driver, HDLC_OUT_BUF_SIZE,
POOL_TYPE_HDLC);
if (!buf_hdlc) {
ret = -ENOMEM;
goto fail_free_hdlc;
}
}
enc.dest = buf_hdlc + driver->used;
enc.dest_last = (void *)(buf_hdlc + driver->used + 2*payload_size + 3);
diag_hdlc_encode(&send, &enc);
/* This is to check if after HDLC encoding, we are still within the
limits of aggregation buffer. If not, we write out the current buffer
and start aggregation in a newly allocated buffer */
if ((unsigned int) enc.dest >=
(unsigned int)(buf_hdlc + HDLC_OUT_BUF_SIZE)) {
err = diag_device_write(buf_hdlc, APPS_DATA, NULL);
if (err) {
/*Free the buffer right away if write failed */
diagmem_free(driver, buf_hdlc, POOL_TYPE_HDLC);
diagmem_free(driver, (unsigned char *)driver->
write_ptr_svc, POOL_TYPE_WRITE_STRUCT);
ret = -EIO;
goto fail_free_hdlc;
}
buf_hdlc = NULL;
driver->used = 0;
buf_hdlc = diagmem_alloc(driver, HDLC_OUT_BUF_SIZE,
POOL_TYPE_HDLC);
if (!buf_hdlc) {
ret = -ENOMEM;
goto fail_free_hdlc;
}
enc.dest = buf_hdlc + driver->used;
enc.dest_last = (void *)(buf_hdlc + driver->used +
(2*payload_size) + 3);
diag_hdlc_encode(&send, &enc);
}
driver->used = (uint32_t) enc.dest - (uint32_t) buf_hdlc;
if (pkt_type == DATA_TYPE_RESPONSE) {
err = diag_device_write(buf_hdlc, APPS_DATA, NULL);
if (err) {
/*Free the buffer right away if write failed */
diagmem_free(driver, buf_hdlc, POOL_TYPE_HDLC);
diagmem_free(driver, (unsigned char *)driver->
write_ptr_svc, POOL_TYPE_WRITE_STRUCT);
ret = -EIO;
goto fail_free_hdlc;
}
buf_hdlc = NULL;
driver->used = 0;
}
mutex_unlock(&driver->diagchar_mutex);
diagmem_free(driver, buf_copy, POOL_TYPE_COPY);
if (!timer_in_progress) {
timer_in_progress = 1;
ret = mod_timer(&drain_timer, jiffies + msecs_to_jiffies(500));
}
return 0;
fail_free_hdlc:
buf_hdlc = NULL;
driver->used = 0;
diagmem_free(driver, buf_copy, POOL_TYPE_COPY);
mutex_unlock(&driver->diagchar_mutex);
return ret;
fail_free_copy:
diagmem_free(driver, buf_copy, POOL_TYPE_COPY);
return ret;
}
int diag_device_write(void *buf, int data_type, struct diag_request *write_ptr)
{
int i, err = 0, index;
index = 0;
if (driver->logging_mode == MEMORY_DEVICE_MODE) {
int hsic_updated = 0;
if (data_type == APPS_DATA) {
for (i = 0; i < driver->poolsize_write_struct; i++)
if (driver->buf_tbl[i].length == 0) {
driver->buf_tbl[i].buf = buf;
driver->buf_tbl[i].length =
driver->used;
#ifdef DIAG_DEBUG
pr_debug("diag: ENQUEUE buf ptr"
" and length is %x , %d\n",
(unsigned int)(driver->buf_
tbl[i].buf), driver->buf_tbl[i].length);
#endif
break;
}
}
#ifdef CONFIG_DIAGFWD_BRIDGE_CODE
else if (data_type == HSIC_DATA || data_type == HSIC_2_DATA) {
unsigned long flags;
int foundIndex = -1;
hsic_updated = 1;
index = data_type - HSIC_DATA;
spin_lock_irqsave(&diag_hsic[index].hsic_spinlock,
flags);
for (i = 0; i < diag_hsic[index].poolsize_hsic_write;
i++) {
if (diag_hsic[index].hsic_buf_tbl[i].length
== 0) {
diag_hsic[index].hsic_buf_tbl[i].buf
= buf;
diag_hsic[index].hsic_buf_tbl[i].length
= diag_bridge[index].write_len;
diag_hsic[index].
num_hsic_buf_tbl_entries++;
foundIndex = i;
break;
}
}
spin_unlock_irqrestore(&diag_hsic[index].hsic_spinlock,
flags);
if (foundIndex == -1)
err = -1;
else
pr_debug("diag: ENQUEUE HSIC buf ptr and length is %x , %d, ch %d\n",
(unsigned int)buf,
diag_bridge[index].write_len, index);
}
#endif
for (i = 0; i < driver->num_clients; i++)
if (driver->client_map[i].pid ==
driver->logging_process_id)
break;
if (i < driver->num_clients) {
diag_mem_dev_mode_ready_update(i, hsic_updated);
pr_debug("diag: wake up logging process\n");
wake_up_interruptible(&driver->wait_q);
} else
return -EINVAL;
} else if (driver->logging_mode == NO_LOGGING_MODE) {
if ((data_type >= 0) && (data_type < NUM_SMD_DATA_CHANNELS)) {
driver->smd_data[data_type].in_busy_1 = 0;
driver->smd_data[data_type].in_busy_2 = 0;
queue_work(driver->diag_wq,
&(driver->smd_data[data_type].
diag_read_smd_work));
}
#ifdef CONFIG_DIAG_SDIO_PIPE
else if (data_type == SDIO_DATA) {
driver->in_busy_sdio = 0;
queue_work(driver->diag_sdio_wq,
&(driver->diag_read_sdio_work));
}
#endif
#ifdef CONFIG_DIAGFWD_BRIDGE_CODE
else if (data_type == HSIC_DATA || data_type == HSIC_2_DATA) {
index = data_type - HSIC_DATA;
if (diag_hsic[index].hsic_ch)
queue_work(diag_bridge[index].wq,
&(diag_hsic[index].
diag_read_hsic_work));
}
#endif
err = -1;
}
#ifdef CONFIG_DIAG_OVER_USB
else if (driver->logging_mode == USB_MODE) {
if (data_type == APPS_DATA) {
driver->write_ptr_svc = (struct diag_request *)
(diagmem_alloc(driver, sizeof(struct diag_request),
POOL_TYPE_WRITE_STRUCT));
if (driver->write_ptr_svc) {
driver->write_ptr_svc->length = driver->used;
driver->write_ptr_svc->buf = buf;
err = usb_diag_write(driver->legacy_ch,
driver->write_ptr_svc);
} else
err = -1;
} else if ((data_type >= 0) &&
(data_type < NUM_SMD_DATA_CHANNELS)) {
write_ptr->buf = buf;
#ifdef DIAG_DEBUG
printk(KERN_INFO "writing data to USB,"
"pkt length %d\n", write_ptr->length);
print_hex_dump(KERN_DEBUG, "Written Packet Data to"
" USB: ", 16, 1, DUMP_PREFIX_ADDRESS,
buf, write_ptr->length, 1);
#endif /* DIAG DEBUG */
err = usb_diag_write(driver->legacy_ch, write_ptr);
}
#ifdef CONFIG_DIAG_SDIO_PIPE
else if (data_type == SDIO_DATA) {
if (machine_is_msm8x60_fusion() ||
machine_is_msm8x60_fusn_ffa()) {
write_ptr->buf = buf;
err = usb_diag_write(driver->mdm_ch, write_ptr);
} else
pr_err("diag: Incorrect sdio data "
"while USB write\n");
}
#endif
#ifdef CONFIG_DIAGFWD_BRIDGE_CODE
else if (data_type == HSIC_DATA || data_type == HSIC_2_DATA) {
index = data_type - HSIC_DATA;
if (diag_hsic[index].hsic_device_enabled) {
struct diag_request *write_ptr_mdm;
write_ptr_mdm = (struct diag_request *)
diagmem_alloc(driver,
sizeof(struct diag_request),
index +
POOL_TYPE_HSIC_WRITE);
if (write_ptr_mdm) {
write_ptr_mdm->buf = buf;
write_ptr_mdm->length =
diag_bridge[index].write_len;
write_ptr_mdm->context = (void *)index;
err = usb_diag_write(
diag_bridge[index].ch, write_ptr_mdm);
/* Return to the pool immediately */
if (err) {
diagmem_free(driver,
write_ptr_mdm,
index +
POOL_TYPE_HSIC_WRITE);
pr_err_ratelimited("diag: HSIC write failure, err: %d, ch %d\n",
err, index);
}
} else {
pr_err("diag: allocate write fail\n");
err = -1;
}
} else {
pr_err("diag: Incorrect HSIC data "
"while USB write\n");
err = -1;
}
} else if (data_type == SMUX_DATA) {
write_ptr->buf = buf;
write_ptr->context = (void *)SMUX;
pr_debug("diag: writing SMUX data\n");
err = usb_diag_write(diag_bridge[SMUX].ch,
write_ptr);
}
#endif
APPEND_DEBUG('d');
}
#endif /* DIAG OVER USB */
return err;
}
