diff options
Diffstat (limited to 'drivers/char/diag')
| -rw-r--r-- | drivers/char/diag/Kconfig | 42 | ||||
| -rw-r--r-- | drivers/char/diag/Makefile | 4 | ||||
| -rw-r--r-- | drivers/char/diag/diagchar.h | 275 | ||||
| -rw-r--r-- | drivers/char/diag/diagchar_core.c | 1235 | ||||
| -rw-r--r-- | drivers/char/diag/diagchar_hdlc.c | 223 | ||||
| -rw-r--r-- | drivers/char/diag/diagchar_hdlc.h | 60 | ||||
| -rw-r--r-- | drivers/char/diag/diagfwd.c | 2090 | ||||
| -rw-r--r-- | drivers/char/diag/diagfwd.h | 43 | ||||
| -rw-r--r-- | drivers/char/diag/diagfwd_cntl.c | 319 | ||||
| -rw-r--r-- | drivers/char/diag/diagfwd_cntl.h | 89 | ||||
| -rw-r--r-- | drivers/char/diag/diagfwd_hsic.c | 651 | ||||
| -rw-r--r-- | drivers/char/diag/diagfwd_hsic.h | 30 | ||||
| -rw-r--r-- | drivers/char/diag/diagfwd_sdio.c | 296 | ||||
| -rw-r--r-- | drivers/char/diag/diagfwd_sdio.h | 27 | ||||
| -rw-r--r-- | drivers/char/diag/diagmem.c | 145 | ||||
| -rw-r--r-- | drivers/char/diag/diagmem.h | 22 |
16 files changed, 5551 insertions, 0 deletions
diff --git a/drivers/char/diag/Kconfig b/drivers/char/diag/Kconfig new file mode 100644 index 0000000..9d28b9f0 --- /dev/null +++ b/drivers/char/diag/Kconfig @@ -0,0 +1,42 @@ +menu "Diag Support" + +config DIAG_CHAR + tristate "char driver interface and diag forwarding to/from modem" + default n +# depends on USB_G_ANDROID || USB_FUNCTION_DIAG || USB_QCOM_MAEMO +# depends on ARCH_MSM + help + Char driver interface for diag user space and diag-forwarding to modem ARM and back. + This enables diagchar for maemo usb gadget or android usb gadget based on config selected. +endmenu + +menu "DIAG traffic over USB" + +config DIAG_OVER_USB + bool "Enable DIAG traffic to go over USB" +# depends on ARCH_MSM + depends on USB_QCOM_DIAG_BRIDGE + default y + help + This feature helps segregate code required for DIAG traffic to go over USB. +endmenu + +menu "SDIO support for DIAG" + +config DIAG_SDIO_PIPE + depends on MSM_SDIO_AL + default y + bool "Enable 9K DIAG traffic over SDIO" + help + SDIO Transport Layer for DIAG Router +endmenu + +menu "HSIC support for DIAG" + +config DIAG_HSIC_PIPE + depends on USB_QCOM_DIAG_BRIDGE + default y + bool "Enable 9K DIAG traffic over HSIC" + help + HSIC Transport Layer for DIAG Router +endmenu diff --git a/drivers/char/diag/Makefile b/drivers/char/diag/Makefile new file mode 100644 index 0000000..c62b7fd --- /dev/null +++ b/drivers/char/diag/Makefile @@ -0,0 +1,4 @@ +obj-$(CONFIG_DIAG_CHAR) := diagchar.o +obj-$(CONFIG_DIAG_SDIO_PIPE) += diagfwd_sdio.o +obj-$(CONFIG_DIAG_HSIC_PIPE) += diagfwd_hsic.o +diagchar-objs := diagchar_core.o diagchar_hdlc.o diagfwd.o diagmem.o diagfwd_cntl.o diff --git a/drivers/char/diag/diagchar.h b/drivers/char/diag/diagchar.h new file mode 100644 index 0000000..5d5e62e --- /dev/null +++ b/drivers/char/diag/diagchar.h @@ -0,0 +1,275 @@ +/* Copyright (c) 2008-2012, 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. + */ + +#ifndef DIAGCHAR_H +#define DIAGCHAR_H + +#include <linux/init.h> +#include <linux/module.h> +#include <linux/mempool.h> +#include <linux/mutex.h> +#include <linux/workqueue.h> +#include <mach/msm_smd.h> +#include <linux/atomic.h> +#include <asm/mach-types.h> +/* Size of the USB buffers used for read and write*/ +#define USB_MAX_OUT_BUF 4096 +#define APPS_BUF_SIZE 2000 +#define IN_BUF_SIZE 16384 +#define MAX_IN_BUF_SIZE 32768 +#define MAX_SYNC_OBJ_NAME_SIZE 32 +/* Size of the buffer used for deframing a packet + reveived from the PC tool*/ +#define HDLC_MAX 4096 +#define HDLC_OUT_BUF_SIZE 8192 +#define POOL_TYPE_COPY 1 +#define POOL_TYPE_HDLC 2 +#define POOL_TYPE_WRITE_STRUCT 4 +#define POOL_TYPE_ALL 7 +#define MODEM_DATA 1 +#define QDSP_DATA 2 +#define APPS_DATA 3 +#define SDIO_DATA 4 +#define WCNSS_DATA 5 +#define HSIC_DATA 6 +#define MODEM_PROC 0 +#define APPS_PROC 1 +#define QDSP_PROC 2 +#define WCNSS_PROC 3 +#define MSG_MASK_SIZE 9500 +#define LOG_MASK_SIZE 8000 +#define EVENT_MASK_SIZE 1000 +#define USER_SPACE_DATA 8000 +#define PKT_SIZE 4096 +#define MAX_EQUIP_ID 15 +#define DIAG_CTRL_MSG_LOG_MASK 9 +#define DIAG_CTRL_MSG_EVENT_MASK 10 +#define DIAG_CTRL_MSG_F3_MASK 11 +#define ZERO_CFG_SUBPACKET_MAX 15 // zero_pky.patch by jagadish + +/* Maximum number of pkt reg supported at initialization*/ +extern unsigned int diag_max_reg; +extern unsigned int diag_threshold_reg; + +#define APPEND_DEBUG(ch) \ +do { \ + diag_debug_buf[diag_debug_buf_idx] = ch; \ + (diag_debug_buf_idx < 1023) ? \ + (diag_debug_buf_idx++) : (diag_debug_buf_idx = 0); \ +} while (0) + +struct diag_master_table { + uint16_t cmd_code; + uint16_t subsys_id; + uint32_t client_id; + uint16_t cmd_code_lo; + uint16_t cmd_code_hi; + int process_id; +}; + +struct bindpkt_params_per_process { + /* Name of the synchronization object associated with this proc */ + char sync_obj_name[MAX_SYNC_OBJ_NAME_SIZE]; + uint32_t count; /* Number of entries in this bind */ + struct bindpkt_params *params; /* first bind params */ +}; + +struct bindpkt_params { + uint16_t cmd_code; + uint16_t subsys_id; + uint16_t cmd_code_lo; + uint16_t cmd_code_hi; + /* For Central Routing, used to store Processor number */ + uint16_t proc_id; + uint32_t event_id; + uint32_t log_code; + /* For Central Routing, used to store SMD channel pointer */ + uint32_t client_id; +}; + +struct diag_write_device { + void *buf; + int length; +}; + +struct diag_client_map { + char name[20]; + int pid; +}; + +/* This structure is defined in USB header file */ +#ifndef CONFIG_DIAG_OVER_USB +struct diag_request { + char *buf; + int length; + int actual; + int status; + void *context; +}; +#endif + +struct diagchar_dev { + + /* State for the char driver */ + unsigned int major; + unsigned int minor_start; + int num; + struct cdev *cdev; + char *name; + int dropped_count; + struct class *diagchar_class; + int ref_count; + struct mutex diagchar_mutex; + wait_queue_head_t wait_q; + struct diag_client_map *client_map; + int *data_ready; + int num_clients; + int polling_reg_flag; + struct diag_write_device *buf_tbl; + int use_device_tree; + + /* Memory pool parameters */ + unsigned int itemsize; + unsigned int poolsize; + unsigned int itemsize_hdlc; + unsigned int poolsize_hdlc; + unsigned int itemsize_write_struct; + unsigned int poolsize_write_struct; + unsigned int debug_flag; + /* State for the mempool for the char driver */ + mempool_t *diagpool; + mempool_t *diag_hdlc_pool; + mempool_t *diag_write_struct_pool; + struct mutex diagmem_mutex; + int count; + int count_hdlc_pool; + int count_write_struct_pool; + int used; + /* Buffers for masks */ + struct diag_ctrl_event_mask *event_mask; + struct diag_ctrl_log_mask *log_mask; + struct diag_ctrl_msg_mask *msg_mask; + /* State for diag forwarding */ + unsigned char *buf_in_1; + unsigned char *buf_in_2; + unsigned char *buf_in_cntl; + unsigned char *buf_in_qdsp_1; + unsigned char *buf_in_qdsp_2; + unsigned char *buf_in_qdsp_cntl; + unsigned char *buf_in_wcnss_1; + unsigned char *buf_in_wcnss_2; + unsigned char *buf_in_wcnss_cntl; + unsigned char *usb_buf_out; + unsigned char *apps_rsp_buf; + unsigned char *user_space_data; + /* buffer for updating mask to peripherals */ + unsigned char *buf_msg_mask_update; + unsigned char *buf_log_mask_update; + unsigned char *buf_event_mask_update; + smd_channel_t *ch; + smd_channel_t *ch_cntl; + smd_channel_t *chqdsp; + smd_channel_t *chqdsp_cntl; + smd_channel_t *ch_wcnss; + smd_channel_t *ch_wcnss_cntl; + int in_busy_1; + int in_busy_2; + int in_busy_qdsp_1; + int in_busy_qdsp_2; + int in_busy_wcnss_1; + int in_busy_wcnss_2; + int read_len_legacy; + unsigned char *hdlc_buf; + unsigned hdlc_count; + unsigned hdlc_escape; +#ifdef CONFIG_DIAG_OVER_USB + int usb_connected; + struct usb_diag_ch *legacy_ch; + struct work_struct diag_proc_hdlc_work; + struct work_struct diag_read_work; +#endif + struct workqueue_struct *diag_wq; + struct work_struct diag_drain_work; + struct work_struct diag_read_smd_work; + struct work_struct diag_read_smd_cntl_work; + struct work_struct diag_read_smd_qdsp_work; + struct work_struct diag_read_smd_qdsp_cntl_work; + struct work_struct diag_read_smd_wcnss_work; + struct work_struct diag_read_smd_wcnss_cntl_work; + struct workqueue_struct *diag_cntl_wq; + struct work_struct diag_modem_mask_update_work; + struct work_struct diag_qdsp_mask_update_work; + struct work_struct diag_wcnss_mask_update_work; + uint8_t *msg_masks; + uint8_t *log_masks; + int log_masks_length; + uint8_t *event_masks; + struct diag_master_table *table; + uint8_t *pkt_buf; + int pkt_length; + struct diag_request *write_ptr_1; + struct diag_request *write_ptr_2; + struct diag_request *usb_read_ptr; + struct diag_request *write_ptr_svc; + struct diag_request *write_ptr_qdsp_1; + struct diag_request *write_ptr_qdsp_2; + struct diag_request *write_ptr_wcnss_1; + struct diag_request *write_ptr_wcnss_2; + int logging_mode; + int sub_logging_mode; + int mask_check; + int logging_process_id; +#ifdef CONFIG_DIAG_SDIO_PIPE + unsigned char *buf_in_sdio; + unsigned char *usb_buf_mdm_out; + struct sdio_channel *sdio_ch; + int read_len_mdm; + int in_busy_sdio; + struct usb_diag_ch *mdm_ch; + struct work_struct diag_read_mdm_work; + struct workqueue_struct *diag_sdio_wq; + struct work_struct diag_read_sdio_work; + struct work_struct diag_close_sdio_work; + struct diag_request *usb_read_mdm_ptr; + struct diag_request *write_ptr_mdm; +#endif +#ifdef CONFIG_DIAG_HSIC_PIPE + unsigned char *buf_in_hsic; + unsigned char *usb_buf_mdm_out; + int hsic_initialized; + int hsic_ch; + int hsic_device_enabled; + int hsic_device_opened; + int hsic_suspend; + int read_len_mdm; + int in_busy_hsic_read_on_mdm; + int in_busy_hsic_write_on_mdm; + int in_busy_hsic_write; + int in_busy_hsic_read; + int usb_mdm_connected; + unsigned int zero_cfg_mode; // zero_pky.patch by jagadish + unsigned int zero_cfg_index; // zero_pky.patch by jagadish + unsigned int zero_cfg_packet_lens_index; // zero_pky.patch by jagadish + struct usb_diag_ch *mdm_ch; + struct workqueue_struct *diag_hsic_wq; + struct work_struct diag_read_mdm_work; + struct work_struct diag_read_hsic_work; + struct work_struct diag_zero_cfg_hsic_work; // zero_pky.patch by jagadish + struct work_struct diag_disconnect_work; + struct work_struct diag_usb_read_complete_work; + struct diag_request *usb_read_mdm_ptr; + struct diag_request *write_ptr_mdm; +#endif +}; + +extern struct diagchar_dev *driver; +#endif diff --git a/drivers/char/diag/diagchar_core.c b/drivers/char/diag/diagchar_core.c new file mode 100644 index 0000000..300b1d7 --- /dev/null +++ b/drivers/char/diag/diagchar_core.c @@ -0,0 +1,1235 @@ +/* Copyright (c) 2008-2012, 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. + */ + +#include <linux/slab.h> +#include <linux/init.h> +#include <linux/module.h> +#include <linux/cdev.h> +#include <linux/fs.h> +#include <linux/device.h> +#include <linux/uaccess.h> +#include <linux/diagchar.h> +#include <linux/sched.h> +#ifdef CONFIG_DIAG_OVER_USB +#include <mach/usbdiag.h> +#endif +#include <asm/current.h> +#include "diagchar_hdlc.h" +#include "diagmem.h" +#include "diagchar.h" +#include "diagfwd.h" +#include "diagfwd_cntl.h" +#ifdef CONFIG_DIAG_SDIO_PIPE +#include "diagfwd_sdio.h" +#endif +#ifdef CONFIG_DIAG_HSIC_PIPE +#include "diagfwd_hsic.h" +#endif +#include <linux/timer.h> + +MODULE_DESCRIPTION("Diag Char Driver"); +MODULE_LICENSE("GPL v2"); +MODULE_VERSION("1.0"); + +#define INIT 1 +#define EXIT -1 +struct diagchar_dev *driver; +struct diagchar_priv { + int pid; +}; +/* The following variables can be specified by module options */ + /* for copy buffer */ +static unsigned int itemsize = 4096; /*Size of item in the mempool */ +static unsigned int poolsize = 10; /*Number of items in the mempool */ +/* for hdlc buffer */ +static unsigned int itemsize_hdlc = 8192; /*Size of item in the mempool */ +static unsigned int poolsize_hdlc = 8; /*Number of items in the mempool */ +/* for write structure buffer */ +static unsigned int itemsize_write_struct = 20; /*Size of item in the mempool */ +static unsigned int poolsize_write_struct = 8; /* Num of items in the mempool */ +/* This is the max number of user-space clients supported at initialization*/ +static unsigned int max_clients = 15; +static unsigned int threshold_client_limit = 30; +/* This is the maximum number of pkt registrations supported at initialization*/ +unsigned int diag_max_reg = 600; +unsigned int diag_threshold_reg = 750; + +/* Timer variables */ +static struct timer_list drain_timer; +static int timer_in_progress; +void *buf_hdlc; +module_param(itemsize, uint, 0); +module_param(poolsize, uint, 0); +module_param(max_clients, uint, 0); + +/* delayed_rsp_id 0 represents no delay in the response. Any other number + means that the diag packet has a delayed response. */ +static uint16_t delayed_rsp_id = 1; +#define DIAGPKT_MAX_DELAYED_RSP 0xFFFF +/* This macro gets the next delayed respose id. Once it reaches + DIAGPKT_MAX_DELAYED_RSP, it stays at DIAGPKT_MAX_DELAYED_RSP */ + +#define DIAGPKT_NEXT_DELAYED_RSP_ID(x) \ +((x < DIAGPKT_MAX_DELAYED_RSP) ? x++ : DIAGPKT_MAX_DELAYED_RSP) + +#define COPY_USER_SPACE_OR_EXIT(buf, data, length) \ +do { \ + if ((count < ret+length) || (copy_to_user(buf, \ + (void *)&data, length))) { \ + ret = -EFAULT; \ + goto exit; \ + } \ + ret += length; \ +} while (0) + +static void drain_timer_func(unsigned long data) +{ + queue_work(driver->diag_wq , &(driver->diag_drain_work)); +} + +void diag_drain_work_fn(struct work_struct *work) +{ + int err = 0; + timer_in_progress = 0; + + mutex_lock(&driver->diagchar_mutex); + if (buf_hdlc) { + 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); + } + buf_hdlc = NULL; +#ifdef DIAG_DEBUG + pr_debug("diag: Number of bytes written " + "from timer is %d ", driver->used); +#endif + driver->used = 0; + } + mutex_unlock(&driver->diagchar_mutex); +} + +void diag_read_smd_work_fn(struct work_struct *work) +{ + __diag_smd_send_req(); +} + +void diag_read_smd_qdsp_work_fn(struct work_struct *work) +{ + __diag_smd_qdsp_send_req(); +} + +void diag_read_smd_wcnss_work_fn(struct work_struct *work) +{ + __diag_smd_wcnss_send_req(); +} + +void diag_add_client(int i, struct file *file) +{ + struct diagchar_priv *diagpriv_data; + + driver->client_map[i].pid = current->tgid; + diagpriv_data = kmalloc(sizeof(struct diagchar_priv), + GFP_KERNEL); + if (diagpriv_data) + diagpriv_data->pid = current->tgid; + file->private_data = diagpriv_data; + strlcpy(driver->client_map[i].name, current->comm, 20); + driver->client_map[i].name[19] = '\0'; +} + +static int diagchar_open(struct inode *inode, struct file *file) +{ + int i = 0; + void *temp; + + if (driver) { + mutex_lock(&driver->diagchar_mutex); + + for (i = 0; i < driver->num_clients; i++) + if (driver->client_map[i].pid == 0) + break; + + if (i < driver->num_clients) { + diag_add_client(i, file); + } else { + if (i < threshold_client_limit) { + driver->num_clients++; + temp = krealloc(driver->client_map + , (driver->num_clients) * sizeof(struct + diag_client_map), GFP_KERNEL); + if (!temp) + goto fail; + else + driver->client_map = temp; + temp = krealloc(driver->data_ready + , (driver->num_clients) * sizeof(int), + GFP_KERNEL); + if (!temp) + goto fail; + else + driver->data_ready = temp; + diag_add_client(i, file); + } else { + mutex_unlock(&driver->diagchar_mutex); + pr_alert("Max client limit for DIAG reached\n"); + pr_info("Cannot open handle %s" + " %d", current->comm, current->tgid); + for (i = 0; i < driver->num_clients; i++) + pr_debug("%d) %s PID=%d", i, driver-> + client_map[i].name, + driver->client_map[i].pid); + return -ENOMEM; + } + } + driver->data_ready[i] |= MSG_MASKS_TYPE; + driver->data_ready[i] |= EVENT_MASKS_TYPE; + driver->data_ready[i] |= LOG_MASKS_TYPE; + + if (driver->ref_count == 0) + diagmem_init(driver); + driver->ref_count++; + mutex_unlock(&driver->diagchar_mutex); + return 0; + } + return -ENOMEM; + +fail: + mutex_unlock(&driver->diagchar_mutex); + driver->num_clients--; + pr_alert("diag: Insufficient memory for new client"); + return -ENOMEM; +} + +static int diagchar_close(struct inode *inode, struct file *file) +{ + int i = 0; + struct diagchar_priv *diagpriv_data = file->private_data; + + if (!(file->private_data)) { + pr_alert("diag: Invalid file pointer"); + return -ENOMEM; + } +#ifdef CONFIG_DIAG_HSIC_PIPE + if (driver->logging_mode == MEMORY_DEVICE_MODE) + queue_work(driver->diag_hsic_wq, &driver->diag_disconnect_work); +#endif + +#ifdef CONFIG_DIAG_OVER_USB + /* If the SD logging process exits, change logging to USB mode */ + if (driver->logging_process_id == current->tgid) { + driver->logging_mode = USB_MODE; +#ifndef CONFIG_DIAG_HSIC_PIPE + /* HSIC PIPE use case, connect over usb is not required */ + diagfwd_connect(); +#endif + } +#endif /* DIAG over USB */ + /* Delete the pkt response table entry for the exiting process */ + for (i = 0; i < diag_max_reg; i++) + if (driver->table[i].process_id == current->tgid) + driver->table[i].process_id = 0; + + if (driver) { + mutex_lock(&driver->diagchar_mutex); + driver->ref_count--; + /* On Client exit, try to destroy all 3 pools */ + diagmem_exit(driver, POOL_TYPE_COPY); + diagmem_exit(driver, POOL_TYPE_HDLC); + diagmem_exit(driver, POOL_TYPE_WRITE_STRUCT); + for (i = 0; i < driver->num_clients; i++) { + if (NULL != diagpriv_data && diagpriv_data->pid == + driver->client_map[i].pid) { + driver->client_map[i].pid = 0; + kfree(diagpriv_data); + diagpriv_data = NULL; + break; + } + } + mutex_unlock(&driver->diagchar_mutex); + return 0; + } + return -ENOMEM; +} + +int diag_find_polling_reg(int i) +{ + uint16_t subsys_id, cmd_code_lo, cmd_code_hi; + + subsys_id = driver->table[i].subsys_id; + cmd_code_lo = driver->table[i].cmd_code_lo; + cmd_code_hi = driver->table[i].cmd_code_hi; + if (driver->table[i].cmd_code == 0x0C) + return 1; + else if (driver->table[i].cmd_code == 0xFF) { + if (subsys_id == 0x04 && cmd_code_hi == 0x0E && + cmd_code_lo == 0x0E) + return 1; + else if (subsys_id == 0x08 && cmd_code_hi == 0x02 && + cmd_code_lo == 0x02) + return 1; + else if (subsys_id == 0x32 && cmd_code_hi == 0x03 && + cmd_code_lo == 0x03) + return 1; + } + return 0; +} + +void diag_clear_reg(int proc_num) +{ + int i; + + mutex_lock(&driver->diagchar_mutex); + /* reset polling flag */ + driver->polling_reg_flag = 0; + for (i = 0; i < diag_max_reg; i++) { + if (driver->table[i].client_id == proc_num) + driver->table[i].process_id = 0; + } + /* re-scan the registration table */ + for (i = 0; i < diag_max_reg; i++) { + if (diag_find_polling_reg(i) == 1) { + driver->polling_reg_flag = 1; + break; + } + } + mutex_unlock(&driver->diagchar_mutex); +} + +void diag_add_reg(int j, struct bindpkt_params *params, + int *success, int *count_entries) +{ + *success = 1; + driver->table[j].cmd_code = params->cmd_code; + driver->table[j].subsys_id = params->subsys_id; + driver->table[j].cmd_code_lo = params->cmd_code_lo; + driver->table[j].cmd_code_hi = params->cmd_code_hi; + + /* check if incoming reg is polling & polling is yet not registered */ + if (driver->polling_reg_flag == 0) + if (diag_find_polling_reg(j) == 1) + driver->polling_reg_flag = 1; + if (params->proc_id == APPS_PROC) { + driver->table[j].process_id = current->tgid; + driver->table[j].client_id = APPS_PROC; + } else { + driver->table[j].process_id = NON_APPS_PROC; + driver->table[j].client_id = params->client_id; + } + (*count_entries)++; +} + +long diagchar_ioctl(struct file *filp, + unsigned int iocmd, unsigned long ioarg) +{ + int i, j, count_entries = 0, temp; + int success = -1; + void *temp_buf; + + if (iocmd == DIAG_IOCTL_COMMAND_REG) { + struct bindpkt_params_per_process *pkt_params = + (struct bindpkt_params_per_process *) ioarg; + mutex_lock(&driver->diagchar_mutex); + for (i = 0; i < diag_max_reg; i++) { + if (driver->table[i].process_id == 0) { + diag_add_reg(i, pkt_params->params, + &success, &count_entries); + if (pkt_params->count > count_entries) { + pkt_params->params++; + } else { + mutex_unlock(&driver->diagchar_mutex); + return success; + } + } + } + if (i < diag_threshold_reg) { + /* Increase table size by amount required */ + diag_max_reg += pkt_params->count - + count_entries; + /* Make sure size doesnt go beyond threshold */ + if (diag_max_reg > diag_threshold_reg) { + diag_max_reg = diag_threshold_reg; + pr_info("diag: best case memory allocation\n"); + } + temp_buf = krealloc(driver->table, + diag_max_reg*sizeof(struct + diag_master_table), GFP_KERNEL); + if (!temp_buf) { + diag_max_reg -= pkt_params->count - + count_entries; + pr_alert("diag: Insufficient memory for reg."); + mutex_unlock(&driver->diagchar_mutex); + return 0; + } else { + driver->table = temp_buf; + } + for (j = i; j < diag_max_reg; j++) { + diag_add_reg(j, pkt_params->params, + &success, &count_entries); + if (pkt_params->count > count_entries) { + pkt_params->params++; + } else { + mutex_unlock(&driver->diagchar_mutex); + return success; + } + } + mutex_unlock(&driver->diagchar_mutex); + } else { + mutex_unlock(&driver->diagchar_mutex); + pr_err("Max size reached, Pkt Registration failed for" + " Process %d", current->tgid); + } + success = 0; + } else if (iocmd == DIAG_IOCTL_GET_DELAYED_RSP_ID) { + struct diagpkt_delay_params *delay_params = + (struct diagpkt_delay_params *) ioarg; + + if ((delay_params->rsp_ptr) && + (delay_params->size == sizeof(delayed_rsp_id)) && + (delay_params->num_bytes_ptr)) { + *((uint16_t *)delay_params->rsp_ptr) = + DIAGPKT_NEXT_DELAYED_RSP_ID(delayed_rsp_id); + *(delay_params->num_bytes_ptr) = sizeof(delayed_rsp_id); + success = 0; + } + } else if (iocmd == DIAG_IOCTL_LSM_DEINIT) { + for (i = 0; i < driver->num_clients; i++) + if (driver->client_map[i].pid == current->tgid) + break; + if (i == -1) + return -EINVAL; + driver->data_ready[i] |= DEINIT_TYPE; + wake_up_interruptible(&driver->wait_q); + success = 1; + } else if (iocmd == DIAG_IOCTL_SWITCH_LOGGING) { + mutex_lock(&driver->diagchar_mutex); + temp = driver->logging_mode; + driver->logging_mode = (int)ioarg; + if (driver->logging_mode == MEMORY_DEVICE_MODE) + driver->mask_check = 1; + if (driver->logging_mode == UART_MODE) { + driver->mask_check = 0; + driver->logging_mode = MEMORY_DEVICE_MODE; + driver->sub_logging_mode = UART_MODE; + } else + driver->sub_logging_mode = NO_LOGGING_MODE; + driver->logging_process_id = current->tgid; + mutex_unlock(&driver->diagchar_mutex); + if (temp == MEMORY_DEVICE_MODE && driver->logging_mode + == NO_LOGGING_MODE) { + driver->in_busy_1 = 1; + driver->in_busy_2 = 1; + driver->in_busy_qdsp_1 = 1; + driver->in_busy_qdsp_2 = 1; + driver->in_busy_wcnss_1 = 1; + driver->in_busy_wcnss_2 = 1; +#ifdef CONFIG_DIAG_SDIO_PIPE + driver->in_busy_sdio = 1; +#endif +#ifdef CONFIG_DIAG_HSIC_PIPE + driver->in_busy_hsic_read = 1; + driver->in_busy_hsic_write = 1; +#endif + } else if (temp == NO_LOGGING_MODE && driver->logging_mode + == MEMORY_DEVICE_MODE) { + driver->in_busy_1 = 0; + driver->in_busy_2 = 0; + driver->in_busy_qdsp_1 = 0; + driver->in_busy_qdsp_2 = 0; + driver->in_busy_wcnss_1 = 0; + driver->in_busy_wcnss_2 = 0; + /* Poll SMD channels to check for data*/ + if (driver->ch) + queue_work(driver->diag_wq, + &(driver->diag_read_smd_work)); + if (driver->chqdsp) + queue_work(driver->diag_wq, + &(driver->diag_read_smd_qdsp_work)); + if (driver->ch_wcnss) + queue_work(driver->diag_wq, + &(driver->diag_read_smd_wcnss_work)); +#ifdef CONFIG_DIAG_SDIO_PIPE + driver->in_busy_sdio = 0; + /* Poll SDIO channel to check for data */ + if (driver->sdio_ch) + queue_work(driver->diag_sdio_wq, + &(driver->diag_read_sdio_work)); +#endif +#ifdef CONFIG_DIAG_HSIC_PIPE + driver->in_busy_hsic_read = 0; + if (driver->hsic_ch) + queue_work(driver->diag_hsic_wq, + &driver->diag_read_hsic_work); +#endif + } +#ifdef CONFIG_DIAG_OVER_USB + else if (temp == USB_MODE && driver->logging_mode + == NO_LOGGING_MODE) + diagfwd_disconnect(); + else if (temp == NO_LOGGING_MODE && driver->logging_mode + == USB_MODE) + diagfwd_connect(); + else if (temp == USB_MODE && driver->logging_mode + == MEMORY_DEVICE_MODE) { + diagfwd_disconnect(); +#ifdef CONFIG_DIAG_HSIC_PIPE + diagfwd_connect_hsic(WRITE_TO_SD); +#endif + driver->in_busy_1 = 0; + driver->in_busy_2 = 0; + driver->in_busy_qdsp_1 = 0; + driver->in_busy_qdsp_2 = 0; + driver->in_busy_wcnss_1 = 0; + driver->in_busy_wcnss_2 = 0; + /* Poll SMD channels to check for data*/ + if (driver->ch) + queue_work(driver->diag_wq, + &(driver->diag_read_smd_work)); + if (driver->chqdsp) + queue_work(driver->diag_wq, + &(driver->diag_read_smd_qdsp_work)); + if (driver->ch_wcnss) + queue_work(driver->diag_wq, + &(driver->diag_read_smd_wcnss_work)); +#ifdef CONFIG_DIAG_SDIO_PIPE + driver->in_busy_sdio = 0; + /* Poll SDIO channel to check for data */ + if (driver->sdio_ch) + queue_work(driver->diag_sdio_wq, + &(driver->diag_read_sdio_work)); +#endif +#ifdef CONFIG_DIAG_HSIC_PIPE + if (driver->hsic_ch) + queue_work(driver->diag_hsic_wq, + &driver->diag_read_hsic_work); +#endif + } else if (temp == MEMORY_DEVICE_MODE && + driver->logging_mode == USB_MODE) + diagfwd_connect(); +#endif /* DIAG over USB */ + success = 1; + } + + return success; +} + +static int diagchar_read(struct file *file, char __user *buf, size_t count, + loff_t *ppos) +{ + int index = -1, i = 0, ret = 0; + int num_data = 0, data_type; + for (i = 0; i < driver->num_clients; i++) + if (driver->client_map[i].pid == current->tgid) + index = i; + + if (index == -1) { + pr_err("diag: Client PID not found in table"); + return -EINVAL; + } + + wait_event_interruptible(driver->wait_q, + driver->data_ready[index]); + mutex_lock(&driver->diagchar_mutex); + + if ((driver->data_ready[index] & USER_SPACE_LOG_TYPE) && (driver-> + logging_mode == MEMORY_DEVICE_MODE)) { + /*Copy the type of data being passed*/ + data_type = driver->data_ready[index] & USER_SPACE_LOG_TYPE; + COPY_USER_SPACE_OR_EXIT(buf, data_type, 4); + /* place holder for number of data field */ + ret += 4; + + for (i = 0; i < driver->poolsize_write_struct; i++) { + if (driver->buf_tbl[i].length > 0) { +#ifdef DIAG_DEBUG + pr_debug("diag: WRITING the buf address " + "and length is %x , %d\n", (unsigned int) + (driver->buf_tbl[i].buf), + driver->buf_tbl[i].length); +#endif + num_data++; + /* Copy the length of data being passed */ + if (copy_to_user(buf+ret, (void *)&(driver-> + buf_tbl[i].length), 4)) { + num_data--; + goto drop; + } + ret += 4; + + /* Copy the actual data being passed */ + if (copy_to_user(buf+ret, (void *)driver-> + buf_tbl[i].buf, driver->buf_tbl[i].length)) { + ret -= 4; + num_data--; + goto drop; + } + ret += driver->buf_tbl[i].length; +drop: +#ifdef DIAG_DEBUG + pr_debug("diag: DEQUEUE buf address and" + " length is %x,%d\n", (unsigned int) + (driver->buf_tbl[i].buf), driver-> + buf_tbl[i].length); +#endif + diagmem_free(driver, (unsigned char *) + (driver->buf_tbl[i].buf), POOL_TYPE_HDLC); + driver->buf_tbl[i].length = 0; + driver->buf_tbl[i].buf = 0; + } + } + + /* copy modem data */ + if (driver->in_busy_1 == 1) { + num_data++; + /*Copy the length of data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, + (driver->write_ptr_1->length), 4); + /*Copy the actual data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, + *(driver->buf_in_1), + driver->write_ptr_1->length); + driver->in_busy_1 = 0; + } + if (driver->in_busy_2 == 1) { + num_data++; + /*Copy the length of data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, + (driver->write_ptr_2->length), 4); + /*Copy the actual data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, + *(driver->buf_in_2), + driver->write_ptr_2->length); + driver->in_busy_2 = 0; + } + /* copy lpass data */ + if (driver->in_busy_qdsp_1 == 1) { + num_data++; + /*Copy the length of data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, + (driver->write_ptr_qdsp_1->length), 4); + /*Copy the actual data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, *(driver-> + buf_in_qdsp_1), + driver->write_ptr_qdsp_1->length); + driver->in_busy_qdsp_1 = 0; + } + if (driver->in_busy_qdsp_2 == 1) { + num_data++; + /*Copy the length of data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, + (driver->write_ptr_qdsp_2->length), 4); + /*Copy the actual data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, *(driver-> + buf_in_qdsp_2), driver-> + write_ptr_qdsp_2->length); + driver->in_busy_qdsp_2 = 0; + } + /* copy wncss data */ + if (driver->in_busy_wcnss_1 == 1) { + num_data++; + /*Copy the length of data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, + (driver->write_ptr_wcnss_1->length), 4); + /*Copy the actual data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, *(driver-> + buf_in_wcnss_1), + driver->write_ptr_wcnss_1->length); + driver->in_busy_wcnss_1 = 0; + } + if (driver->in_busy_wcnss_2 == 1) { + num_data++; + /*Copy the length of data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, + (driver->write_ptr_wcnss_2->length), 4); + /*Copy the actual data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, *(driver-> + buf_in_wcnss_2), + driver->write_ptr_wcnss_2->length); + driver->in_busy_wcnss_2 = 0; + } +#ifdef CONFIG_DIAG_SDIO_PIPE + /* copy 9K data over SDIO */ + if (driver->in_busy_sdio == 1) { + num_data++; + /*Copy the length of data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, + (driver->write_ptr_mdm->length), 4); + /*Copy the actual data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, + *(driver->buf_in_sdio), + driver->write_ptr_mdm->length); + driver->in_busy_sdio = 0; + } +#endif +#ifdef CONFIG_DIAG_HSIC_PIPE + num_data++; + /*Copy the length of data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, + (driver->write_ptr_mdm->length), 4); + /*Copy the actual data being passed*/ + COPY_USER_SPACE_OR_EXIT(buf+ret, + *(driver->buf_in_hsic), + driver->write_ptr_mdm->length); +#endif + /* copy number of data fields */ + COPY_USER_SPACE_OR_EXIT(buf+4, num_data, 4); + ret -= 4; + driver->data_ready[index] ^= USER_SPACE_LOG_TYPE; + if (driver->ch) + queue_work(driver->diag_wq, + &(driver->diag_read_smd_work)); + if (driver->chqdsp) + queue_work(driver->diag_wq, + &(driver->diag_read_smd_qdsp_work)); + if (driver->ch_wcnss) + queue_work(driver->diag_wq, + &(driver->diag_read_smd_wcnss_work)); +#ifdef CONFIG_DIAG_SDIO_PIPE + if (driver->sdio_ch) + queue_work(driver->diag_sdio_wq, + &(driver->diag_read_sdio_work)); +#endif +#ifdef CONFIG_DIAG_HSIC_PIPE + /* driver->in_busy_hsic_read = 0; */ + if (driver->hsic_ch) + queue_work(driver->diag_hsic_wq, + &driver->diag_read_hsic_work); +#endif + APPEND_DEBUG('n'); + goto exit; + } else if (driver->data_ready[index] & USER_SPACE_LOG_TYPE) { + /* In case, the thread wakes up and the logging mode is + not memory device any more, the condition needs to be cleared */ + driver->data_ready[index] ^= USER_SPACE_LOG_TYPE; + } + + if (driver->data_ready[index] & DEINIT_TYPE) { + /*Copy the type of data being passed*/ + data_type = driver->data_ready[index] & DEINIT_TYPE; + COPY_USER_SPACE_OR_EXIT(buf, data_type, 4); + driver->data_ready[index] ^= DEINIT_TYPE; + goto exit; + } + + if (driver->data_ready[index] & MSG_MASKS_TYPE) { + /*Copy the type of data being passed*/ + data_type = driver->data_ready[index] & MSG_MASKS_TYPE; + COPY_USER_SPACE_OR_EXIT(buf, data_type, 4); + COPY_USER_SPACE_OR_EXIT(buf+4, *(driver->msg_masks), + MSG_MASK_SIZE); + driver->data_ready[index] ^= MSG_MASKS_TYPE; + goto exit; + } + + if (driver->data_ready[index] & EVENT_MASKS_TYPE) { + /*Copy the type of data being passed*/ + data_type = driver->data_ready[index] & EVENT_MASKS_TYPE; + COPY_USER_SPACE_OR_EXIT(buf, data_type, 4); + COPY_USER_SPACE_OR_EXIT(buf+4, *(driver->event_masks), + EVENT_MASK_SIZE); + driver->data_ready[index] ^= EVENT_MASKS_TYPE; + goto exit; + } + + if (driver->data_ready[index] & LOG_MASKS_TYPE) { + /*Copy the type of data being passed*/ + data_type = driver->data_ready[index] & LOG_MASKS_TYPE; + COPY_USER_SPACE_OR_EXIT(buf, data_type, 4); + COPY_USER_SPACE_OR_EXIT(buf+4, *(driver->log_masks), + LOG_MASK_SIZE); + driver->data_ready[index] ^= LOG_MASKS_TYPE; + goto exit; + } + + if (driver->data_ready[index] & PKT_TYPE) { + /*Copy the type of data being passed*/ + data_type = driver->data_ready[index] & PKT_TYPE; + COPY_USER_SPACE_OR_EXIT(buf, data_type, 4); + COPY_USER_SPACE_OR_EXIT(buf+4, *(driver->pkt_buf), + driver->pkt_length); + driver->data_ready[index] ^= PKT_TYPE; + goto exit; + } + +exit: + mutex_unlock(&driver->diagchar_mutex); + return ret; +} + +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; + + /* To removed "0x7E", when received only "0x7E" */ + if (0x7e == *(((unsigned char *)buf))) + return 0; + +#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_HSIC_PIPE + if (driver->hsic_ch) { + // QXDM Logging zero_pkt (with silent log on) second fix by JAGADISH KRISHNAMOORTHY + pr_info("%s: hsic line busy %d \n",__func__,driver->in_busy_hsic_write); + /* wait till write is succesfully written to CP */ + if (driver->in_busy_hsic_write) + wait_event_interruptible(driver->wait_q, + (driver->in_busy_hsic_write != 1)); + + if (!driver->in_busy_hsic_write) { + driver->in_busy_hsic_write = 1; + err = diag_bridge_write((driver->user_space_data), + payload_size); + if (err) { + pr_err("%s: data on hsic write err: %d\n", + __func__, 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 +#if 0 + /* send masks to modem now */ + diag_process_hdlc((void *)(driver->user_space_data), + payload_size); +#endif + return count; + } + + 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 count; + +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 mask_request_validate(unsigned char mask_buf[]) +{ + uint8_t packet_id; + uint8_t subsys_id; + uint16_t ss_cmd; + + packet_id = mask_buf[0]; + + if (packet_id == 0x4B) { + subsys_id = mask_buf[1]; + ss_cmd = *(uint16_t *)(mask_buf + 2); + /* Packets with SSID which are allowed */ + switch (subsys_id) { + case 0x04: /* DIAG_SUBSYS_WCDMA */ + if ((ss_cmd == 0) || (ss_cmd == 0xF)) + return 1; + break; + case 0x08: /* DIAG_SUBSYS_GSM */ + if ((ss_cmd == 0) || (ss_cmd == 0x1)) + return 1; + break; + case 0x09: /* DIAG_SUBSYS_UMTS */ + case 0x0F: /* DIAG_SUBSYS_CM */ + if (ss_cmd == 0) + return 1; + break; + case 0x0C: /* DIAG_SUBSYS_OS */ + if ((ss_cmd == 2) || (ss_cmd == 0x100)) + return 1; /* MPU and APU */ + break; + case 0x12: /* DIAG_SUBSYS_DIAG_SERV */ + if ((ss_cmd == 0) || (ss_cmd == 0x6) || (ss_cmd == 0x7)) + return 1; + break; + case 0x13: /* DIAG_SUBSYS_FS */ + if ((ss_cmd == 0) || (ss_cmd == 0x1)) + return 1; + break; + default: + return 0; + break; + } + } else { + switch (packet_id) { + case 0x00: /* Version Number */ + case 0x0C: /* CDMA status packet */ + case 0x1C: /* Diag Version */ + case 0x1D: /* Time Stamp */ + case 0x60: /* Event Report Control */ + case 0x63: /* Status snapshot */ + case 0x73: /* Logging Configuration */ + case 0x7C: /* Extended build ID */ + case 0x7D: /* Extended Message configuration */ + case 0x81: /* Event get mask */ + case 0x82: /* Set the event mask */ + return 1; + break; + default: + return 0; + break; + } + } + return 0; +} + +static const struct file_operations diagcharfops = { + .owner = THIS_MODULE, + .read = diagchar_read, + .write = diagchar_write, + .unlocked_ioctl = diagchar_ioctl, + .open = diagchar_open, + .release = diagchar_close +}; + +static int diagchar_setup_cdev(dev_t devno) +{ + + int err; + + cdev_init(driver->cdev, &diagcharfops); + + driver->cdev->owner = THIS_MODULE; + driver->cdev->ops = &diagcharfops; + + err = cdev_add(driver->cdev, devno, 1); + + if (err) { + printk(KERN_INFO "diagchar cdev registration failed !\n\n"); + return -1; + } + + driver->diagchar_class = class_create(THIS_MODULE, "diag"); + + if (IS_ERR(driver->diagchar_class)) { + printk(KERN_ERR "Error creating diagchar class.\n"); + return -1; + } + + device_create(driver->diagchar_class, NULL, devno, + (void *)driver, "diag"); + + return 0; + +} + +static int diagchar_cleanup(void) +{ + if (driver) { + if (driver->cdev) { + /* TODO - Check if device exists before deleting */ + device_destroy(driver->diagchar_class, + MKDEV(driver->major, + driver->minor_start)); + cdev_del(driver->cdev); + } + if (!IS_ERR(driver->diagchar_class)) + class_destroy(driver->diagchar_class); + kfree(driver); + } + return 0; +} + +#ifdef CONFIG_DIAG_SDIO_PIPE +void diag_sdio_fn(int type) +{ + if (machine_is_msm8x60_fusion() || machine_is_msm8x60_fusn_ffa()) { + if (type == INIT) + diagfwd_sdio_init(); + else if (type == EXIT) + diagfwd_sdio_exit(); + } +} +#else +inline void diag_sdio_fn(int type) {} +#endif + +#ifdef CONFIG_DIAG_HSIC_PIPE +void diag_hsic_fn(int type) +{ + if (type == INIT) + diagfwd_hsic_init(); + else if (type == EXIT) + diagfwd_hsic_exit(); +} +#else +inline void diag_hsic_fn(int type) {} +#endif + +static int __init diagchar_init(void) +{ + dev_t dev; + int error; + + pr_debug("diagfwd initializing ..\n"); + driver = kzalloc(sizeof(struct diagchar_dev) + 5, GFP_KERNEL); + + if (driver) { + driver->used = 0; + timer_in_progress = 0; + driver->debug_flag = 1; + setup_timer(&drain_timer, drain_timer_func, 1234); + driver->itemsize = itemsize; + driver->poolsize = poolsize; + driver->itemsize_hdlc = itemsize_hdlc; + driver->poolsize_hdlc = poolsize_hdlc; + driver->itemsize_write_struct = itemsize_write_struct; + driver->poolsize_write_struct = poolsize_write_struct; + driver->num_clients = max_clients; + driver->logging_mode = USB_MODE; + driver->mask_check = 0; + mutex_init(&driver->diagchar_mutex); + init_waitqueue_head(&driver->wait_q); + INIT_WORK(&(driver->diag_drain_work), diag_drain_work_fn); + INIT_WORK(&(driver->diag_read_smd_work), diag_read_smd_work_fn); + INIT_WORK(&(driver->diag_read_smd_cntl_work), + diag_read_smd_cntl_work_fn); + INIT_WORK(&(driver->diag_read_smd_qdsp_work), + diag_read_smd_qdsp_work_fn); + INIT_WORK(&(driver->diag_read_smd_qdsp_cntl_work), + diag_read_smd_qdsp_cntl_work_fn); + INIT_WORK(&(driver->diag_read_smd_wcnss_work), + diag_read_smd_wcnss_work_fn); + INIT_WORK(&(driver->diag_read_smd_wcnss_cntl_work), + diag_read_smd_wcnss_cntl_work_fn); + diagfwd_init(); + diagfwd_cntl_init(); + diag_sdio_fn(INIT); + diag_hsic_fn(INIT); + pr_debug("diagchar initializing ..\n"); + driver->num = 1; + driver->name = ((void *)driver) + sizeof(struct diagchar_dev); + strlcpy(driver->name, "diag", 4); + + /* Get major number from kernel and initialize */ + error = alloc_chrdev_region(&dev, driver->minor_start, + driver->num, driver->name); + if (!error) { + driver->major = MAJOR(dev); + driver->minor_start = MINOR(dev); + } else { + printk(KERN_INFO "Major number not allocated\n"); + goto fail; + } + driver->cdev = cdev_alloc(); + error = diagchar_setup_cdev(dev); + if (error) + goto fail; + } else { + printk(KERN_INFO "kzalloc failed\n"); + goto fail; + } + + pr_info("diagchar initialized now"); + return 0; + +fail: + diagchar_cleanup(); + diagfwd_exit(); + diagfwd_cntl_exit(); + diag_sdio_fn(EXIT); + diag_hsic_fn(EXIT); + return -1; +} + +static void __exit diagchar_exit(void) +{ + printk(KERN_INFO "diagchar exiting ..\n"); + /* On Driver exit, send special pool type to + ensure no memory leaks */ + diagmem_exit(driver, POOL_TYPE_ALL); + diagfwd_exit(); + diagfwd_cntl_exit(); + diag_sdio_fn(EXIT); + diag_hsic_fn(EXIT); + diagchar_cleanup(); + printk(KERN_INFO "done diagchar exit\n"); +} + +module_init(diagchar_init); +module_exit(diagchar_exit); diff --git a/drivers/char/diag/diagchar_hdlc.c b/drivers/char/diag/diagchar_hdlc.c new file mode 100644 index 0000000..ef57d52 --- /dev/null +++ b/drivers/char/diag/diagchar_hdlc.c @@ -0,0 +1,223 @@ +/* Copyright (c) 2008-2009, 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. + * + */ + +#include <linux/init.h> +#include <linux/module.h> +#include <linux/cdev.h> +#include <linux/fs.h> +#include <linux/device.h> +#include <linux/uaccess.h> +#include <linux/crc-ccitt.h> +#include "diagchar_hdlc.h" + + +MODULE_LICENSE("GPL v2"); + +#define CRC_16_L_SEED 0xFFFF + +#define CRC_16_L_STEP(xx_crc, xx_c) \ + crc_ccitt_byte(xx_crc, xx_c) + +void diag_hdlc_encode(struct diag_send_desc_type *src_desc, + struct diag_hdlc_dest_type *enc) +{ + uint8_t *dest; + uint8_t *dest_last; + const uint8_t *src; + const uint8_t *src_last; + uint16_t crc; + unsigned char src_byte = 0; + enum diag_send_state_enum_type state; + unsigned int used = 0; + + if (src_desc && enc) { + + /* Copy parts to local variables. */ + src = src_desc->pkt; + src_last = src_desc->last; + state = src_desc->state; + dest = enc->dest; + dest_last = enc->dest_last; + + if (state == DIAG_STATE_START) { + crc = CRC_16_L_SEED; + state++; + } else { + /* Get a local copy of the CRC */ + crc = enc->crc; + } + + /* dest or dest_last may be NULL to trigger a + state transition only */ + if (dest && dest_last) { + /* This condition needs to include the possibility + of 2 dest bytes for an escaped byte */ + while (src <= src_last && dest <= dest_last) { + + src_byte = *src++; + + if ((src_byte == CONTROL_CHAR) || + (src_byte == ESC_CHAR)) { + + /* If the escape character is not the + last byte */ + if (dest != dest_last) { + crc = CRC_16_L_STEP(crc, + src_byte); + + *dest++ = ESC_CHAR; + used++; + + *dest++ = src_byte + ^ ESC_MASK; + used++; + } else { + + src--; + break; + } + + } else { + crc = CRC_16_L_STEP(crc, src_byte); + *dest++ = src_byte; + used++; + } + } + + if (src > src_last) { + + if (state == DIAG_STATE_BUSY) { + if (src_desc->terminate) { + crc = ~crc; + state++; + } else { + /* Done with fragment */ + state = DIAG_STATE_COMPLETE; + } + } + + while (dest <= dest_last && + state >= DIAG_STATE_CRC1 && + state < DIAG_STATE_TERM) { + /* Encode a byte of the CRC next */ + src_byte = crc & 0xFF; + + if ((src_byte == CONTROL_CHAR) + || (src_byte == ESC_CHAR)) { + + if (dest != dest_last) { + + *dest++ = ESC_CHAR; + used++; + *dest++ = src_byte ^ + ESC_MASK; + used++; + + crc >>= 8; + } else { + + break; + } + } else { + + crc >>= 8; + *dest++ = src_byte; + used++; + } + + state++; + } + + if (state == DIAG_STATE_TERM) { + if (dest_last >= dest) { + *dest++ = CONTROL_CHAR; + used++; + state++; /* Complete */ + } + } + } + } + /* Copy local variables back into the encode structure. */ + + enc->dest = dest; + enc->dest_last = dest_last; + enc->crc = crc; + src_desc->pkt = src; + src_desc->last = src_last; + src_desc->state = state; + } + + return; +} + + +int diag_hdlc_decode(struct diag_hdlc_decode_type *hdlc) +{ + uint8_t *src_ptr = NULL, *dest_ptr = NULL; + unsigned int src_length = 0, dest_length = 0; + + unsigned int len = 0; + unsigned int i; + uint8_t src_byte; + + int pkt_bnd = 0; + + if (hdlc && hdlc->src_ptr && hdlc->dest_ptr && + (hdlc->src_size - hdlc->src_idx > 0) && + (hdlc->dest_size - hdlc->dest_idx > 0)) { + + src_ptr = hdlc->src_ptr; + src_ptr = &src_ptr[hdlc->src_idx]; + src_length = hdlc->src_size - hdlc->src_idx; + + dest_ptr = hdlc->dest_ptr; + dest_ptr = &dest_ptr[hdlc->dest_idx]; + dest_length = hdlc->dest_size - hdlc->dest_idx; + + for (i = 0; i < src_length; i++) { + + src_byte = src_ptr[i]; + + if (hdlc->escaping) { + dest_ptr[len++] = src_byte ^ ESC_MASK; + hdlc->escaping = 0; + } else if (src_byte == ESC_CHAR) { + if (i == (src_length - 1)) { + hdlc->escaping = 1; + i++; + break; + } else { + dest_ptr[len++] = src_ptr[++i] + ^ ESC_MASK; + } + } else if (src_byte == CONTROL_CHAR) { + dest_ptr[len++] = src_byte; + pkt_bnd = 1; + i++; + break; + } else { + dest_ptr[len++] = src_byte; + } + + if (len >= dest_length) { + i++; + break; + } + } + + hdlc->src_idx += i; + hdlc->dest_idx += len; + } + + return pkt_bnd; +} diff --git a/drivers/char/diag/diagchar_hdlc.h b/drivers/char/diag/diagchar_hdlc.h new file mode 100644 index 0000000..2df81de --- /dev/null +++ b/drivers/char/diag/diagchar_hdlc.h @@ -0,0 +1,60 @@ +/* Copyright (c) 2008-2009, 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. + */ + +#ifndef DIAGCHAR_HDLC +#define DIAGCHAR_HDLC + +enum diag_send_state_enum_type { + DIAG_STATE_START, + DIAG_STATE_BUSY, + DIAG_STATE_CRC1, + DIAG_STATE_CRC2, + DIAG_STATE_TERM, + DIAG_STATE_COMPLETE +}; + +struct diag_send_desc_type { + const void *pkt; + const void *last; /* Address of last byte to send. */ + enum diag_send_state_enum_type state; + unsigned char terminate; /* True if this fragment + terminates the packet */ +}; + +struct diag_hdlc_dest_type { + void *dest; + void *dest_last; + /* Below: internal use only */ + uint16_t crc; +}; + +struct diag_hdlc_decode_type { + uint8_t *src_ptr; + unsigned int src_idx; + unsigned int src_size; + uint8_t *dest_ptr; + unsigned int dest_idx; + unsigned int dest_size; + int escaping; + +}; + +void diag_hdlc_encode(struct diag_send_desc_type *src_desc, + struct diag_hdlc_dest_type *enc); + +int diag_hdlc_decode(struct diag_hdlc_decode_type *hdlc); + +#define ESC_CHAR 0x7D +#define CONTROL_CHAR 0x7E +#define ESC_MASK 0x20 + +#endif diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c new file mode 100644 index 0000000..d319487 --- /dev/null +++ b/drivers/char/diag/diagfwd.c @@ -0,0 +1,2090 @@ +/* Copyright (c) 2008-2012, 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. + */ +#include <linux/slab.h> +#include <linux/init.h> +#include <linux/module.h> +#include <linux/device.h> +#include <linux/err.h> +#include <linux/platform_device.h> +#include <linux/sched.h> +#include <linux/workqueue.h> +#include <linux/pm_runtime.h> +#include <linux/diagchar.h> +#include <linux/delay.h> +#include <linux/reboot.h> +#include <linux/of.h> +#include <linux/spinlock.h> +#ifdef CONFIG_DIAG_OVER_USB +#include <mach/usbdiag.h> +#endif +#include <mach/msm_smd.h> +#ifndef CONFIG_ARCH_EXYNOS +#include <mach/socinfo.h> +#endif +#include <mach/restart.h> +#include "diagmem.h" +#include "diagchar.h" +#include "diagfwd.h" +#include "diagfwd_cntl.h" +#include "diagchar_hdlc.h" +#ifdef CONFIG_DIAG_SDIO_PIPE +#include "diagfwd_sdio.h" +#endif +#ifdef CONFIG_DIAG_HSIC_PIPE +#include "diagfwd_hsic.h" +#endif +#define MODE_CMD 41 +#define RESET_ID 2 +#define ALL_EQUIP_ID 100 +#define ALL_SSID -1 +#define MAX_SSID_PER_RANGE 100 + +int diag_debug_buf_idx; +unsigned char diag_debug_buf[1024]; +static unsigned int buf_tbl_size = 8; /*Number of entries in table of buffers */ +struct diag_master_table entry; +smd_channel_t *ch_temp, *chqdsp_temp, *ch_wcnss_temp; +int diag_event_num_bytes; +int diag_event_config; +struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 }; +struct diag_hdlc_dest_type enc = { NULL, NULL, 0 }; +struct mask_info { + int equip_id; + int num_items; + int index; +}; +spinlock_t diag_cntl_lock; + +#define CREATE_MSG_MASK_TBL_ROW(XX) \ +do { \ + *(int *)(msg_mask_tbl_ptr) = MSG_SSID_ ## XX; \ + msg_mask_tbl_ptr += 4; \ + *(int *)(msg_mask_tbl_ptr) = MSG_SSID_ ## XX ## _LAST; \ + msg_mask_tbl_ptr += 4; \ + /* increment by MAX_SSID_PER_RANGE cells */ \ + msg_mask_tbl_ptr += MAX_SSID_PER_RANGE * sizeof(int); \ +} while (0) + +#define ENCODE_RSP_AND_SEND(buf_length) \ +do { \ + send.state = DIAG_STATE_START; \ + send.pkt = driver->apps_rsp_buf; \ + send.last = (void *)(driver->apps_rsp_buf + buf_length); \ + send.terminate = 1; \ + if (!driver->in_busy_1) { \ + enc.dest = driver->buf_in_1; \ + enc.dest_last = (void *)(driver->buf_in_1 + APPS_BUF_SIZE - 1);\ + diag_hdlc_encode(&send, &enc); \ + driver->write_ptr_1->buf = driver->buf_in_1; \ + driver->write_ptr_1->length = (int)(enc.dest - \ + (void *)(driver->buf_in_1)); \ + driver->in_busy_1 = 1; \ + diag_device_write(driver->buf_in_1, MODEM_DATA, \ + driver->write_ptr_1); \ + memset(driver->apps_rsp_buf, '\0', APPS_BUF_SIZE); \ + } \ +} while (0) + +#define CHK_OVERFLOW(bufStart, start, end, length) \ +((bufStart <= start) && (end - start >= length)) ? 1 : 0 + +/* Determine if this device uses a device tree */ +#ifdef CONFIG_OF +static int has_device_tree(void) +{ + struct device_node *node; + + node = of_find_node_by_path("/"); + if (node) { + of_node_put(node); + return 1; + } + return 0; +} +#else +static int has_device_tree(void) +{ + return 0; +} +#endif + +int chk_config_get_id(void) +{ + /* For all Fusion targets, Modem will always be present */ +#ifdef CONFIG_ARCH_EXYNOS + return 0; +#else + if (machine_is_msm8x60_fusion() || machine_is_msm8x60_fusn_ffa()) + return 0; + + if (driver->use_device_tree) { + if (machine_is_copper()) + return MSM8974_TOOLS_ID; + else + return 0; + } else { + switch (socinfo_get_msm_cpu()) { + case MSM_CPU_8X60: + return APQ8060_TOOLS_ID; + case MSM_CPU_8960: + return AO8960_TOOLS_ID; + case MSM_CPU_8064: + return APQ8064_TOOLS_ID; + case MSM_CPU_8930: + return MSM8930_TOOLS_ID; + case MSM_CPU_COPPER: + return MSM8974_TOOLS_ID; + case MSM_CPU_8625: + return MSM8625_TOOLS_ID; + default: + return 0; + } + } +#endif +} + +/* + * This will return TRUE for targets which support apps only mode and hence SSR. + * This applies to 8960 and newer targets. + */ +int chk_apps_only(void) +{ +#ifdef CONFIG_ARCH_EXYNOS + return 1; +#else + if (driver->use_device_tree) + return 1; + + switch (socinfo_get_msm_cpu()) { + case MSM_CPU_8960: + case MSM_CPU_8064: + case MSM_CPU_8930: + case MSM_CPU_8627: + case MSM_CPU_9615: + case MSM_CPU_COPPER: + return 1; + default: + return 0; + } +#endif +} + +/* + * This will return TRUE for targets which support apps as master. + * Thus, SW DLOAD and Mode Reset are supported on apps processor. + * This applies to 8960 and newer targets. + */ +int chk_apps_master(void) +{ +#ifdef CONFIG_ARCH_EXYNOS + return 1; +#else + if (driver->use_device_tree) + return 1; + else if (cpu_is_msm8960() || cpu_is_msm8930() || cpu_is_msm9615() || + cpu_is_apq8064() || cpu_is_msm8627()) + return 1; + else + return 0; +#endif +} + +inline int chk_polling_response(void) +{ + if (!(driver->polling_reg_flag) && chk_apps_master()) + /* + * If the apps processor is master and no other processor + * has registered to respond for polling + */ + return 1; + else if (!(driver->ch) && !(chk_apps_master())) + /* + * If the apps processor is not the master and the modem + * is not up + */ + return 1; + else + return 0; +} + +void __diag_smd_send_req(void) +{ + void *buf = NULL; + int *in_busy_ptr = NULL; + struct diag_request *write_ptr_modem = NULL; + + if (!driver->in_busy_1) { + buf = driver->buf_in_1; + write_ptr_modem = driver->write_ptr_1; + in_busy_ptr = &(driver->in_busy_1); + } else if (!driver->in_busy_2) { + buf = driver->buf_in_2; + write_ptr_modem = driver->write_ptr_2; + in_busy_ptr = &(driver->in_busy_2); + } + + if (driver->ch && buf) { + int r = smd_read_avail(driver->ch); + + if (r > IN_BUF_SIZE) { + if (r < MAX_IN_BUF_SIZE) { + pr_err("diag: SMD sending in " + "packets upto %d bytes", r); + buf = krealloc(buf, r, GFP_KERNEL); + } else { + pr_err("diag: SMD sending in " + "packets more than %d bytes", MAX_IN_BUF_SIZE); + return; + } + } + if (r > 0) { + if (!buf) + pr_info("Out of diagmem for Modem\n"); + else { + APPEND_DEBUG('i'); + smd_read(driver->ch, buf, r); + APPEND_DEBUG('j'); + write_ptr_modem->length = r; + *in_busy_ptr = 1; + diag_device_write(buf, MODEM_DATA, + write_ptr_modem); + } + } + } +} + +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 (driver->sub_logging_mode == UART_MODE && + proc_num != HSIC_DATA) + return 0; + 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; + } + } + 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] |= USER_SPACE_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)); + } else if (proc_num == WCNSS_DATA) { + driver->in_busy_wcnss_1 = 0; + driver->in_busy_wcnss_2 = 0; + queue_work(driver->diag_wq, &(driver-> + diag_read_smd_wcnss_work)); + } +#ifdef CONFIG_DIAG_SDIO_PIPE + else if (proc_num == SDIO_DATA) { + driver->in_busy_sdio = 0; + queue_work(driver->diag_sdio_wq, + &(driver->diag_read_sdio_work)); + } +#endif + 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 (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_DIAG_HSIC_PIPE + else if (proc_num == HSIC_DATA) { + if (driver->hsic_device_enabled) { + write_ptr->buf = buf; + err = usb_diag_write(driver->mdm_ch, write_ptr); + } else + pr_err("diag: Incorrect hsic data " + "while USB write\n"); + } +#endif + APPEND_DEBUG('d'); + } +#endif /* DIAG OVER USB */ + return err; +} + +void __diag_smd_wcnss_send_req(void) +{ + void *buf = NULL; + int *in_busy_wcnss_ptr = NULL; + struct diag_request *write_ptr_wcnss = NULL; + + if (!driver->in_busy_wcnss_1) { + buf = driver->buf_in_wcnss_1; + write_ptr_wcnss = driver->write_ptr_wcnss_1; + in_busy_wcnss_ptr = &(driver->in_busy_wcnss_1); + } else if (!driver->in_busy_wcnss_2) { + buf = driver->buf_in_wcnss_2; + write_ptr_wcnss = driver->write_ptr_wcnss_2; + in_busy_wcnss_ptr = &(driver->in_busy_wcnss_2); + } + + if (driver->ch_wcnss && buf) { + int r = smd_read_avail(driver->ch_wcnss); + if (r > IN_BUF_SIZE) { + if (r < MAX_IN_BUF_SIZE) { + pr_err("diag: wcnss packets > %d bytes", r); + buf = krealloc(buf, r, GFP_KERNEL); + } else { + pr_err("diag: wcnss pkt > %d", MAX_IN_BUF_SIZE); + return; + } + } + if (r > 0) { + if (!buf) { + pr_err("Out of diagmem for wcnss\n"); + } else { + APPEND_DEBUG('i'); + smd_read(driver->ch_wcnss, buf, r); + APPEND_DEBUG('j'); + write_ptr_wcnss->length = r; + *in_busy_wcnss_ptr = 1; + diag_device_write(buf, WCNSS_DATA, + write_ptr_wcnss); + } + } + } +} + +void __diag_smd_qdsp_send_req(void) +{ + void *buf = NULL; + int *in_busy_qdsp_ptr = NULL; + struct diag_request *write_ptr_qdsp = NULL; + + if (!driver->in_busy_qdsp_1) { + buf = driver->buf_in_qdsp_1; + write_ptr_qdsp = driver->write_ptr_qdsp_1; + in_busy_qdsp_ptr = &(driver->in_busy_qdsp_1); + } else if (!driver->in_busy_qdsp_2) { + buf = driver->buf_in_qdsp_2; + write_ptr_qdsp = driver->write_ptr_qdsp_2; + in_busy_qdsp_ptr = &(driver->in_busy_qdsp_2); + } + + if (driver->chqdsp && buf) { + int r = smd_read_avail(driver->chqdsp); + + if (r > IN_BUF_SIZE) { + if (r < MAX_IN_BUF_SIZE) { + pr_err("diag: SMD sending in " + "packets upto %d bytes", r); + buf = krealloc(buf, r, GFP_KERNEL); + } else { + pr_err("diag: SMD sending in " + "packets more than %d bytes", MAX_IN_BUF_SIZE); + return; + } + } + if (r > 0) { + if (!buf) + printk(KERN_INFO "Out of diagmem for QDSP\n"); + else { + APPEND_DEBUG('i'); + smd_read(driver->chqdsp, buf, r); + APPEND_DEBUG('j'); + write_ptr_qdsp->length = r; + *in_busy_qdsp_ptr = 1; + diag_device_write(buf, QDSP_DATA, + write_ptr_qdsp); + } + } + } +} + +static void diag_print_mask_table(void) +{ +/* Enable this to print mask table when updated */ +#ifdef MASK_DEBUG + int first; + int last; + uint8_t *ptr = driver->msg_masks; + int i = 0; + pr_info("diag: F3 message mask table\n"); + while (*(uint32_t *)(ptr + 4)) { + first = *(uint32_t *)ptr; + ptr += 4; + last = *(uint32_t *)ptr; + ptr += 4; + printk(KERN_INFO "SSID %d - %d\n", first, last); + for (i = 0 ; i <= last - first ; i++) + printk(KERN_INFO "MASK:%x\n", *((uint32_t *)ptr + i)); + ptr += MAX_SSID_PER_RANGE*4; + + } +#endif +} + +void diag_create_msg_mask_table(void) +{ + uint8_t *msg_mask_tbl_ptr = driver->msg_masks; + + CREATE_MSG_MASK_TBL_ROW(0); + CREATE_MSG_MASK_TBL_ROW(1); + CREATE_MSG_MASK_TBL_ROW(2); + CREATE_MSG_MASK_TBL_ROW(3); + CREATE_MSG_MASK_TBL_ROW(4); + CREATE_MSG_MASK_TBL_ROW(5); + CREATE_MSG_MASK_TBL_ROW(6); + CREATE_MSG_MASK_TBL_ROW(7); + CREATE_MSG_MASK_TBL_ROW(8); + CREATE_MSG_MASK_TBL_ROW(9); + CREATE_MSG_MASK_TBL_ROW(10); + CREATE_MSG_MASK_TBL_ROW(11); + CREATE_MSG_MASK_TBL_ROW(12); + CREATE_MSG_MASK_TBL_ROW(13); + CREATE_MSG_MASK_TBL_ROW(14); + CREATE_MSG_MASK_TBL_ROW(15); + CREATE_MSG_MASK_TBL_ROW(16); + CREATE_MSG_MASK_TBL_ROW(17); + CREATE_MSG_MASK_TBL_ROW(18); + CREATE_MSG_MASK_TBL_ROW(19); + CREATE_MSG_MASK_TBL_ROW(20); + CREATE_MSG_MASK_TBL_ROW(21); + CREATE_MSG_MASK_TBL_ROW(22); +} + +static void diag_set_msg_mask(int rt_mask) +{ + int first_ssid, last_ssid, i; + uint8_t *parse_ptr, *ptr = driver->msg_masks; + + mutex_lock(&driver->diagchar_mutex); + while (*(uint32_t *)(ptr + 4)) { + first_ssid = *(uint32_t *)ptr; + ptr += 4; + last_ssid = *(uint32_t *)ptr; + ptr += 4; + parse_ptr = ptr; + pr_debug("diag: updating range %d %d\n", first_ssid, last_ssid); + for (i = 0; i < last_ssid - first_ssid + 1; i++) { + *(int *)parse_ptr = rt_mask; + parse_ptr += 4; + } + ptr += MAX_SSID_PER_RANGE * 4; + } + mutex_unlock(&driver->diagchar_mutex); +} + +static void diag_update_msg_mask(int start, int end , uint8_t *buf) +{ + int found = 0; + int first; + int last; + uint8_t *ptr = driver->msg_masks; + uint8_t *ptr_buffer_start = &(*(driver->msg_masks)); + uint8_t *ptr_buffer_end = &(*(driver->msg_masks)) + MSG_MASK_SIZE; + + mutex_lock(&driver->diagchar_mutex); + + /* First SSID can be zero : So check that last is non-zero */ + while (*(uint32_t *)(ptr + 4)) { + first = *(uint32_t *)ptr; + ptr += 4; + last = *(uint32_t *)ptr; + ptr += 4; + if (start >= first && start <= last) { + ptr += (start - first)*4; + if (end <= last) + if (CHK_OVERFLOW(ptr_buffer_start, ptr, + ptr_buffer_end, + (((end - start)+1)*4))) { + pr_debug("diag: update ssid start %d," + " end %d\n", start, end); + memcpy(ptr, buf , ((end - start)+1)*4); + } else + printk(KERN_CRIT "Not enough" + " buffer space for" + " MSG_MASK\n"); + else + printk(KERN_INFO "Unable to copy" + " mask change\n"); + + found = 1; + break; + } else { + ptr += MAX_SSID_PER_RANGE*4; + } + } + /* Entry was not found - add new table */ + if (!found) { + if (CHK_OVERFLOW(ptr_buffer_start, ptr, ptr_buffer_end, + 8 + ((end - start) + 1)*4)) { + memcpy(ptr, &(start) , 4); + ptr += 4; + memcpy(ptr, &(end), 4); + ptr += 4; + pr_debug("diag: adding NEW ssid start %d, end %d\n", + start, end); + memcpy(ptr, buf , ((end - start) + 1)*4); + } else + printk(KERN_CRIT " Not enough buffer" + " space for MSG_MASK\n"); + } + mutex_unlock(&driver->diagchar_mutex); + diag_print_mask_table(); + +} + +void diag_toggle_event_mask(int toggle) +{ + uint8_t *ptr = driver->event_masks; + + mutex_lock(&driver->diagchar_mutex); + if (toggle) + memset(ptr, 0xFF, EVENT_MASK_SIZE); + else + memset(ptr, 0, EVENT_MASK_SIZE); + mutex_unlock(&driver->diagchar_mutex); +} + +static void diag_update_event_mask(uint8_t *buf, int toggle, int num_bytes) +{ + uint8_t *ptr = driver->event_masks; + uint8_t *temp = buf + 2; + + mutex_lock(&driver->diagchar_mutex); + if (!toggle) + memset(ptr, 0 , EVENT_MASK_SIZE); + else + if (CHK_OVERFLOW(ptr, ptr, + ptr+EVENT_MASK_SIZE, num_bytes)) + memcpy(ptr, temp , num_bytes); + else + printk(KERN_CRIT "Not enough buffer space " + "for EVENT_MASK\n"); + mutex_unlock(&driver->diagchar_mutex); +} + +static void diag_disable_log_mask(void) +{ + int i = 0; + struct mask_info *parse_ptr = (struct mask_info *)(driver->log_masks); + + pr_debug("diag: disable log masks\n"); + mutex_lock(&driver->diagchar_mutex); + for (i = 0; i < MAX_EQUIP_ID; i++) { + pr_debug("diag: equip id %d\n", parse_ptr->equip_id); + if (!(parse_ptr->equip_id)) /* Reached a null entry */ + break; + memset(driver->log_masks + parse_ptr->index, 0, + (parse_ptr->num_items + 7)/8); + parse_ptr++; + } + mutex_unlock(&driver->diagchar_mutex); +} + +static void diag_update_log_mask(int equip_id, uint8_t *buf, int num_items) +{ + uint8_t *temp = buf; + int i = 0; + unsigned char *ptr_data; + int offset = (sizeof(struct mask_info))*MAX_EQUIP_ID; + struct mask_info *ptr = (struct mask_info *)(driver->log_masks); + + pr_debug("diag: received equip id = %d\n", equip_id); + mutex_lock(&driver->diagchar_mutex); + /* Check if we already know index of this equipment ID */ + for (i = 0; i < MAX_EQUIP_ID; i++) { + if ((ptr->equip_id == equip_id) && (ptr->index != 0)) { + offset = ptr->index; + break; + } + if ((ptr->equip_id == 0) && (ptr->index == 0)) { + /* Reached a null entry */ + ptr->equip_id = equip_id; + ptr->num_items = num_items; + ptr->index = driver->log_masks_length; + offset = driver->log_masks_length; + driver->log_masks_length += ((num_items+7)/8); + break; + } + ptr++; + } + ptr_data = driver->log_masks + offset; + if (CHK_OVERFLOW(driver->log_masks, ptr_data, driver->log_masks + + LOG_MASK_SIZE, (num_items+7)/8)) + memcpy(ptr_data, temp , (num_items+7)/8); + else + pr_err("diag: Not enough buffer space for LOG_MASK\n"); + mutex_unlock(&driver->diagchar_mutex); +} + +static void diag_update_pkt_buffer(unsigned char *buf) +{ + unsigned char *ptr = driver->pkt_buf; + unsigned char *temp = buf; + + mutex_lock(&driver->diagchar_mutex); + if (CHK_OVERFLOW(ptr, ptr, ptr + PKT_SIZE, driver->pkt_length)) + memcpy(ptr, temp , driver->pkt_length); + else + printk(KERN_CRIT " Not enough buffer space for PKT_RESP\n"); + mutex_unlock(&driver->diagchar_mutex); +} + +void diag_update_userspace_clients(unsigned int type) +{ + int i; + + mutex_lock(&driver->diagchar_mutex); + for (i = 0; i < driver->num_clients; i++) + if (driver->client_map[i].pid != 0) + driver->data_ready[i] |= type; + wake_up_interruptible(&driver->wait_q); + mutex_unlock(&driver->diagchar_mutex); +} + +void diag_update_sleeping_process(int process_id) +{ + int i; + + mutex_lock(&driver->diagchar_mutex); + for (i = 0; i < driver->num_clients; i++) + if (driver->client_map[i].pid == process_id) { + driver->data_ready[i] |= PKT_TYPE; + break; + } + wake_up_interruptible(&driver->wait_q); + mutex_unlock(&driver->diagchar_mutex); +} + +void diag_send_data(struct diag_master_table entry, unsigned char *buf, + int len, int type) +{ + driver->pkt_length = len; + if (entry.process_id != NON_APPS_PROC && type != MODEM_DATA) { + diag_update_pkt_buffer(buf); + diag_update_sleeping_process(entry.process_id); + } else { + if (len > 0) { + if (entry.client_id == MODEM_PROC && driver->ch) { + if (chk_apps_master() && + (int)(*(char *)buf) == MODE_CMD) + if ((int)(*(char *)(buf+1)) == + RESET_ID) + return; + smd_write(driver->ch, buf, len); + } else if (entry.client_id == QDSP_PROC && + driver->chqdsp) { + smd_write(driver->chqdsp, buf, len); + } else if (entry.client_id == WCNSS_PROC && + driver->ch_wcnss) { + smd_write(driver->ch_wcnss, buf, len); + } else { + pr_alert("diag: incorrect channel"); + } + } + } +} + +void diag_modem_mask_update_fn(struct work_struct *work) +{ + diag_send_msg_mask_update(driver->ch_cntl, ALL_SSID, + ALL_SSID, MODEM_PROC); + diag_send_log_mask_update(driver->ch_cntl, ALL_EQUIP_ID); + diag_send_event_mask_update(driver->ch_cntl, diag_event_num_bytes); +} + +void diag_qdsp_mask_update_fn(struct work_struct *work) +{ + diag_send_msg_mask_update(driver->chqdsp_cntl, ALL_SSID, + ALL_SSID, QDSP_PROC); + diag_send_log_mask_update(driver->chqdsp_cntl, ALL_EQUIP_ID); + diag_send_event_mask_update(driver->chqdsp_cntl, diag_event_num_bytes); +} + +void diag_wcnss_mask_update_fn(struct work_struct *work) +{ + diag_send_msg_mask_update(driver->ch_wcnss_cntl, ALL_SSID, + ALL_SSID, WCNSS_PROC); + diag_send_log_mask_update(driver->ch_wcnss_cntl, ALL_EQUIP_ID); + diag_send_event_mask_update(driver->ch_wcnss_cntl, + diag_event_num_bytes); +} + +void diag_send_log_mask_update(smd_channel_t *ch, int equip_id) +{ + void *buf = driver->buf_log_mask_update; + int header_size = sizeof(struct diag_ctrl_log_mask); + struct mask_info *ptr = (struct mask_info *)driver->log_masks; + int i, size, wr_size = -ENOMEM, retry_count = 0; + unsigned long flags = 0; + + for (i = 0; i < MAX_EQUIP_ID; i++) { + size = (ptr->num_items+7)/8; + /* reached null entry */ + if ((ptr->equip_id == 0) && (ptr->index == 0)) + break; + driver->log_mask->cmd_type = DIAG_CTRL_MSG_LOG_MASK; + driver->log_mask->num_items = ptr->num_items; + driver->log_mask->data_len = 11 + size; + driver->log_mask->stream_id = 1; /* 2, if dual stream */ + driver->log_mask->status = 3; /* status for valid mask */ + driver->log_mask->equip_id = ptr->equip_id; + driver->log_mask->log_mask_size = size; + /* send only desired update, NOT ALL */ + if (equip_id == ALL_EQUIP_ID || equip_id == + driver->log_mask->equip_id) { + memcpy(buf, driver->log_mask, header_size); + memcpy(buf+header_size, driver->log_masks+ptr->index, + size); + if (ch) { + while (retry_count < 3) { + spin_lock_irqsave(&diag_cntl_lock, + flags); + wr_size = smd_write(ch, buf, + header_size + size); + spin_unlock_irqrestore(&diag_cntl_lock, + flags); + if (wr_size == -ENOMEM) { + retry_count++; + msleep(20); + } else + break; + } + if (wr_size != header_size + size) + pr_err("diag: log mask update failed" + " %d, tried %d", wr_size, header_size + size); + else + pr_debug("diag: updated log equip ID %d" + ",len %d\n", driver->log_mask->equip_id, + driver->log_mask->log_mask_size); + } else + pr_err("diag: ch not valid for log update\n"); + } + ptr++; + } +} + +void diag_send_event_mask_update(smd_channel_t *ch, int num_bytes) +{ + void *buf = driver->buf_event_mask_update; + int header_size = sizeof(struct diag_ctrl_event_mask); + int wr_size = -ENOMEM, retry_count = 0; + unsigned long flags = 0; + + if (num_bytes == 0) { + pr_debug("diag: event mask not set yet, so no update\n"); + return; + } + /* send event mask update */ + driver->event_mask->cmd_type = DIAG_CTRL_MSG_EVENT_MASK; + driver->event_mask->data_len = 7 + num_bytes; + driver->event_mask->stream_id = 1; /* 2, if dual stream */ + driver->event_mask->status = 3; /* status for valid mask */ + driver->event_mask->event_config = diag_event_config; /* event config */ + driver->event_mask->event_mask_size = num_bytes; + memcpy(buf, driver->event_mask, header_size); + memcpy(buf+header_size, driver->event_masks, num_bytes); + if (ch) { + while (retry_count < 3) { + spin_lock_irqsave(&diag_cntl_lock, flags); + wr_size = smd_write(ch, buf, header_size + num_bytes); + spin_unlock_irqrestore(&diag_cntl_lock, flags); + if (wr_size == -ENOMEM) { + retry_count++; + msleep(20); + } else + break; + } + if (wr_size != header_size + num_bytes) + pr_err("diag: error writing event mask %d, tried %d\n", + wr_size, header_size + num_bytes); + } else + pr_err("diag: ch not valid for event update\n"); +} + +void diag_send_msg_mask_update(smd_channel_t *ch, int updated_ssid_first, + int updated_ssid_last, int proc) +{ + void *buf = driver->buf_msg_mask_update; + int first, last, size = -ENOMEM, retry_count = 0; + int header_size = sizeof(struct diag_ctrl_msg_mask); + uint8_t *ptr = driver->msg_masks; + unsigned long flags = 0; + + while (*(uint32_t *)(ptr + 4)) { + first = *(uint32_t *)ptr; + ptr += 4; + last = *(uint32_t *)ptr; + ptr += 4; + if ((updated_ssid_first >= first && updated_ssid_last <= last) + || (updated_ssid_first == ALL_SSID)) { + /* send f3 mask update */ + driver->msg_mask->cmd_type = DIAG_CTRL_MSG_F3_MASK; + driver->msg_mask->msg_mask_size = last - first + 1; + driver->msg_mask->data_len = 11 + + 4 * (driver->msg_mask->msg_mask_size); + driver->msg_mask->stream_id = 1; /* 2, if dual stream */ + driver->msg_mask->status = 3; /* status valid mask */ + driver->msg_mask->msg_mode = 0; /* Legcay mode */ + driver->msg_mask->ssid_first = first; + driver->msg_mask->ssid_last = last; + memcpy(buf, driver->msg_mask, header_size); + memcpy(buf+header_size, ptr, + 4 * (driver->msg_mask->msg_mask_size)); + if (ch) { + while (retry_count < 3) { + spin_lock_irqsave(&diag_cntl_lock, + flags); + size = smd_write(ch, buf, header_size + + 4*(driver->msg_mask->msg_mask_size)); + spin_unlock_irqrestore(&diag_cntl_lock, + flags); + if (size == -ENOMEM) { + retry_count++; + msleep(20); + } else + break; + } + if (size != header_size + + 4*(driver->msg_mask->msg_mask_size)) + pr_err("diag: msg mask update fail %d," + " tried %d\n", size, + header_size + 4*(driver->msg_mask->msg_mask_size)); + else + pr_debug("diag: sending mask update for" + " ssid first %d, last %d on PROC %d\n", first, last, proc); + } else + pr_err("diag: ch invalid msg mask update\n"); + } + ptr += MAX_SSID_PER_RANGE*4; + } +} + +static int diag_process_apps_pkt(unsigned char *buf, int len) +{ + uint16_t subsys_cmd_code; + int subsys_id, ssid_first, ssid_last, ssid_range; + int packet_type = 1, i, cmd_code, rt_mask; + unsigned char *temp = buf; + int data_type; +#if defined(CONFIG_DIAG_OVER_USB) + int payload_length; + unsigned char *ptr; +#endif + + /* Set log masks */ + if (*buf == 0x73 && *(int *)(buf+4) == 3) { + buf += 8; + /* Read Equip ID and pass as first param below*/ + diag_update_log_mask(*(int *)buf, buf+8, *(int *)(buf+4)); + diag_update_userspace_clients(LOG_MASKS_TYPE); +#if defined(CONFIG_DIAG_OVER_USB) + if (chk_apps_only()) { + driver->apps_rsp_buf[0] = 0x73; + *(int *)(driver->apps_rsp_buf + 4) = 0x3; /* op. ID */ + *(int *)(driver->apps_rsp_buf + 8) = 0x0; /* success */ + payload_length = 8 + ((*(int *)(buf + 4)) + 7)/8; + for (i = 0; i < payload_length; i++) + *(int *)(driver->apps_rsp_buf+12+i) = *(buf+i); + if (driver->ch_cntl) + diag_send_log_mask_update(driver->ch_cntl, + *(int *)buf); + if (driver->chqdsp_cntl) + diag_send_log_mask_update(driver->chqdsp_cntl, + *(int *)buf); + if (driver->ch_wcnss_cntl) + diag_send_log_mask_update(driver->ch_wcnss_cntl, + *(int *)buf); + ENCODE_RSP_AND_SEND(12 + payload_length - 1); + return 0; + } else + buf = temp; +#endif + } /* Disable log masks */ + else if (*buf == 0x73 && *(int *)(buf+4) == 0) { + buf += 8; + /* Disable mask for each log code */ + diag_disable_log_mask(); + diag_update_userspace_clients(LOG_MASKS_TYPE); +#if defined(CONFIG_DIAG_OVER_USB) + if (chk_apps_only()) { + driver->apps_rsp_buf[0] = 0x73; + driver->apps_rsp_buf[1] = 0x0; + driver->apps_rsp_buf[2] = 0x0; + driver->apps_rsp_buf[3] = 0x0; + *(int *)(driver->apps_rsp_buf + 4) = 0x0; + if (driver->ch_cntl) + diag_send_log_mask_update(driver->ch_cntl, + *(int *)buf); + if (driver->chqdsp_cntl) + diag_send_log_mask_update(driver->chqdsp_cntl, + *(int *)buf); + if (driver->ch_wcnss_cntl) + diag_send_log_mask_update(driver->ch_wcnss_cntl, + *(int *)buf); + ENCODE_RSP_AND_SEND(7); + return 0; + } else + buf = temp; +#endif + } /* Set runtime message mask */ + else if ((*buf == 0x7d) && (*(buf+1) == 0x4)) { + ssid_first = *(uint16_t *)(buf + 2); + ssid_last = *(uint16_t *)(buf + 4); + ssid_range = 4 * (ssid_last - ssid_first + 1); + pr_debug("diag: received mask update for ssid_first = %d," + " ssid_last = %d", ssid_first, ssid_last); + diag_update_msg_mask(ssid_first, ssid_last , buf + 8); + diag_update_userspace_clients(MSG_MASKS_TYPE); +#if defined(CONFIG_DIAG_OVER_USB) + if (chk_apps_only()) { + for (i = 0; i < 8 + ssid_range; i++) + *(driver->apps_rsp_buf + i) = *(buf+i); + *(driver->apps_rsp_buf + 6) = 0x1; + if (driver->ch_cntl) + diag_send_msg_mask_update(driver->ch_cntl, + ssid_first, ssid_last, MODEM_PROC); + if (driver->chqdsp_cntl) + diag_send_msg_mask_update(driver->chqdsp_cntl, + ssid_first, ssid_last, QDSP_PROC); + if (driver->ch_wcnss_cntl) + diag_send_msg_mask_update(driver->ch_wcnss_cntl, + ssid_first, ssid_last, WCNSS_PROC); + ENCODE_RSP_AND_SEND(8 + ssid_range - 1); + return 0; + } else + buf = temp; +#endif + } /* Set ALL runtime message mask */ + else if ((*buf == 0x7d) && (*(buf+1) == 0x5)) { + rt_mask = *(int *)(buf + 4); + diag_set_msg_mask(rt_mask); + diag_update_userspace_clients(MSG_MASKS_TYPE); +#if defined(CONFIG_DIAG_OVER_USB) + if (chk_apps_only()) { + driver->apps_rsp_buf[0] = 0x7d; /* cmd_code */ + driver->apps_rsp_buf[1] = 0x5; /* set subcommand */ + driver->apps_rsp_buf[2] = 1; /* success */ + driver->apps_rsp_buf[3] = 0; /* rsvd */ + *(int *)(driver->apps_rsp_buf + 4) = rt_mask; + /* send msg mask update to peripheral */ + if (driver->ch_cntl) + diag_send_msg_mask_update(driver->ch_cntl, + ALL_SSID, ALL_SSID, MODEM_PROC); + if (driver->chqdsp_cntl) + diag_send_msg_mask_update(driver->chqdsp_cntl, + ALL_SSID, ALL_SSID, QDSP_PROC); + if (driver->ch_wcnss_cntl) + diag_send_msg_mask_update(driver->ch_wcnss_cntl, + ALL_SSID, ALL_SSID, WCNSS_PROC); + ENCODE_RSP_AND_SEND(7); + return 0; + } else + buf = temp; +#endif + } else if (*buf == 0x82) { /* event mask change */ + buf += 4; + diag_event_num_bytes = (*(uint16_t *)buf)/8+1; + diag_update_event_mask(buf, 1, (*(uint16_t *)buf)/8+1); + diag_update_userspace_clients(EVENT_MASKS_TYPE); +#if defined(CONFIG_DIAG_OVER_USB) + if (chk_apps_only()) { + driver->apps_rsp_buf[0] = 0x82; + driver->apps_rsp_buf[1] = 0x0; + *(uint16_t *)(driver->apps_rsp_buf + 2) = 0x0; + *(uint16_t *)(driver->apps_rsp_buf + 4) = + EVENT_LAST_ID + 1; + memcpy(driver->apps_rsp_buf+6, driver->event_masks, + EVENT_LAST_ID/8+1); + if (driver->ch_cntl) + diag_send_event_mask_update(driver->ch_cntl, + diag_event_num_bytes); + if (driver->chqdsp_cntl) + diag_send_event_mask_update(driver->chqdsp_cntl, + diag_event_num_bytes); + if (driver->ch_wcnss_cntl) + diag_send_event_mask_update( + driver->ch_wcnss_cntl, diag_event_num_bytes); + ENCODE_RSP_AND_SEND(6 + EVENT_LAST_ID/8); + return 0; + } else + buf = temp; +#endif + } else if (*buf == 0x60) { + diag_event_config = *(buf+1); + diag_toggle_event_mask(*(buf+1)); + diag_update_userspace_clients(EVENT_MASKS_TYPE); +#if defined(CONFIG_DIAG_OVER_USB) + if (chk_apps_only()) { + driver->apps_rsp_buf[0] = 0x60; + driver->apps_rsp_buf[1] = 0x0; + driver->apps_rsp_buf[2] = 0x0; + if (driver->ch_cntl) + diag_send_event_mask_update(driver->ch_cntl, + diag_event_num_bytes); + if (driver->chqdsp_cntl) + diag_send_event_mask_update(driver->chqdsp_cntl, + diag_event_num_bytes); + if (driver->ch_wcnss_cntl) + diag_send_event_mask_update( + driver->ch_wcnss_cntl, diag_event_num_bytes); + ENCODE_RSP_AND_SEND(2); + return 0; + } +#endif + } + /* Check for registered clients and forward packet to apropriate proc */ + cmd_code = (int)(*(char *)buf); + temp++; + subsys_id = (int)(*(char *)temp); + temp++; + subsys_cmd_code = *(uint16_t *)temp; + temp += 2; + data_type = APPS_DATA; + /* Dont send any command other than mode reset */ + if (chk_apps_master() && cmd_code == MODE_CMD) { + if (subsys_id != RESET_ID) + data_type = MODEM_DATA; + } + + pr_debug("diag: %d %d %d", cmd_code, subsys_id, subsys_cmd_code); + for (i = 0; i < diag_max_reg; i++) { + entry = driver->table[i]; + if (entry.process_id != NO_PROCESS) { + if (entry.cmd_code == cmd_code && entry.subsys_id == + subsys_id && entry.cmd_code_lo <= + subsys_cmd_code && + entry.cmd_code_hi >= subsys_cmd_code) { + diag_send_data(entry, buf, len, data_type); + packet_type = 0; + } else if (entry.cmd_code == 255 + && cmd_code == 75) { + if (entry.subsys_id == + subsys_id && + entry.cmd_code_lo <= + subsys_cmd_code && + entry.cmd_code_hi >= + subsys_cmd_code) { + diag_send_data(entry, buf, len, + data_type); + packet_type = 0; + } + } else if (entry.cmd_code == 255 && + entry.subsys_id == 255) { + if (entry.cmd_code_lo <= + cmd_code && + entry. + cmd_code_hi >= cmd_code) { + diag_send_data(entry, buf, len, + data_type); + packet_type = 0; + } + } + } + } +#if defined(CONFIG_DIAG_OVER_USB) + /* Check for Apps Only & get event mask request */ + if (!(driver->ch) && chk_apps_only() && *buf == 0x81) { + driver->apps_rsp_buf[0] = 0x81; + driver->apps_rsp_buf[1] = 0x0; + *(uint16_t *)(driver->apps_rsp_buf + 2) = 0x0; + *(uint16_t *)(driver->apps_rsp_buf + 4) = EVENT_LAST_ID + 1; + for (i = 0; i < EVENT_LAST_ID/8 + 1; i++) + *(unsigned char *)(driver->apps_rsp_buf + 6 + i) = 0x0; + ENCODE_RSP_AND_SEND(6 + EVENT_LAST_ID/8); + return 0; + } + /* Get log ID range & Check for Apps Only */ + else if (!(driver->ch) && chk_apps_only() + && (*buf == 0x73) && *(int *)(buf+4) == 1) { + driver->apps_rsp_buf[0] = 0x73; + *(int *)(driver->apps_rsp_buf + 4) = 0x1; /* operation ID */ + *(int *)(driver->apps_rsp_buf + 8) = 0x0; /* success code */ + *(int *)(driver->apps_rsp_buf + 12) = LOG_GET_ITEM_NUM(LOG_0); + *(int *)(driver->apps_rsp_buf + 16) = LOG_GET_ITEM_NUM(LOG_1); + *(int *)(driver->apps_rsp_buf + 20) = LOG_GET_ITEM_NUM(LOG_2); + *(int *)(driver->apps_rsp_buf + 24) = LOG_GET_ITEM_NUM(LOG_3); + *(int *)(driver->apps_rsp_buf + 28) = LOG_GET_ITEM_NUM(LOG_4); + *(int *)(driver->apps_rsp_buf + 32) = LOG_GET_ITEM_NUM(LOG_5); + *(int *)(driver->apps_rsp_buf + 36) = LOG_GET_ITEM_NUM(LOG_6); + *(int *)(driver->apps_rsp_buf + 40) = LOG_GET_ITEM_NUM(LOG_7); + *(int *)(driver->apps_rsp_buf + 44) = LOG_GET_ITEM_NUM(LOG_8); + *(int *)(driver->apps_rsp_buf + 48) = LOG_GET_ITEM_NUM(LOG_9); + *(int *)(driver->apps_rsp_buf + 52) = LOG_GET_ITEM_NUM(LOG_10); + *(int *)(driver->apps_rsp_buf + 56) = LOG_GET_ITEM_NUM(LOG_11); + *(int *)(driver->apps_rsp_buf + 60) = LOG_GET_ITEM_NUM(LOG_12); + *(int *)(driver->apps_rsp_buf + 64) = LOG_GET_ITEM_NUM(LOG_13); + *(int *)(driver->apps_rsp_buf + 68) = LOG_GET_ITEM_NUM(LOG_14); + *(int *)(driver->apps_rsp_buf + 72) = LOG_GET_ITEM_NUM(LOG_15); + ENCODE_RSP_AND_SEND(75); + return 0; + } + /* Respond to Get SSID Range request message */ + else if (!(driver->ch) && chk_apps_only() + && (*buf == 0x7d) && (*(buf+1) == 0x1)) { + driver->apps_rsp_buf[0] = 0x7d; + driver->apps_rsp_buf[1] = 0x1; + driver->apps_rsp_buf[2] = 0x1; + driver->apps_rsp_buf[3] = 0x0; + *(int *)(driver->apps_rsp_buf + 4) = MSG_MASK_TBL_CNT; + *(uint16_t *)(driver->apps_rsp_buf + 8) = MSG_SSID_0; + *(uint16_t *)(driver->apps_rsp_buf + 10) = MSG_SSID_0_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 12) = MSG_SSID_1; + *(uint16_t *)(driver->apps_rsp_buf + 14) = MSG_SSID_1_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 16) = MSG_SSID_2; + *(uint16_t *)(driver->apps_rsp_buf + 18) = MSG_SSID_2_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 20) = MSG_SSID_3; + *(uint16_t *)(driver->apps_rsp_buf + 22) = MSG_SSID_3_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 24) = MSG_SSID_4; + *(uint16_t *)(driver->apps_rsp_buf + 26) = MSG_SSID_4_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 28) = MSG_SSID_5; + *(uint16_t *)(driver->apps_rsp_buf + 30) = MSG_SSID_5_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 32) = MSG_SSID_6; + *(uint16_t *)(driver->apps_rsp_buf + 34) = MSG_SSID_6_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 36) = MSG_SSID_7; + *(uint16_t *)(driver->apps_rsp_buf + 38) = MSG_SSID_7_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 40) = MSG_SSID_8; + *(uint16_t *)(driver->apps_rsp_buf + 42) = MSG_SSID_8_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 44) = MSG_SSID_9; + *(uint16_t *)(driver->apps_rsp_buf + 46) = MSG_SSID_9_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 48) = MSG_SSID_10; + *(uint16_t *)(driver->apps_rsp_buf + 50) = MSG_SSID_10_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 52) = MSG_SSID_11; + *(uint16_t *)(driver->apps_rsp_buf + 54) = MSG_SSID_11_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 56) = MSG_SSID_12; + *(uint16_t *)(driver->apps_rsp_buf + 58) = MSG_SSID_12_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 60) = MSG_SSID_13; + *(uint16_t *)(driver->apps_rsp_buf + 62) = MSG_SSID_13_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 64) = MSG_SSID_14; + *(uint16_t *)(driver->apps_rsp_buf + 66) = MSG_SSID_14_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 68) = MSG_SSID_15; + *(uint16_t *)(driver->apps_rsp_buf + 70) = MSG_SSID_15_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 72) = MSG_SSID_16; + *(uint16_t *)(driver->apps_rsp_buf + 74) = MSG_SSID_16_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 76) = MSG_SSID_17; + *(uint16_t *)(driver->apps_rsp_buf + 78) = MSG_SSID_17_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 80) = MSG_SSID_18; + *(uint16_t *)(driver->apps_rsp_buf + 82) = MSG_SSID_18_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 84) = MSG_SSID_19; + *(uint16_t *)(driver->apps_rsp_buf + 86) = MSG_SSID_19_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 88) = MSG_SSID_20; + *(uint16_t *)(driver->apps_rsp_buf + 90) = MSG_SSID_20_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 92) = MSG_SSID_21; + *(uint16_t *)(driver->apps_rsp_buf + 94) = MSG_SSID_21_LAST; + *(uint16_t *)(driver->apps_rsp_buf + 96) = MSG_SSID_22; + *(uint16_t *)(driver->apps_rsp_buf + 98) = MSG_SSID_22_LAST; + ENCODE_RSP_AND_SEND(99); + return 0; + } + /* Check for Apps Only Respond to Get Subsys Build mask */ + else if (!(driver->ch) && chk_apps_only() + && (*buf == 0x7d) && (*(buf+1) == 0x2)) { + ssid_first = *(uint16_t *)(buf + 2); + ssid_last = *(uint16_t *)(buf + 4); + ssid_range = 4 * (ssid_last - ssid_first + 1); + /* frame response */ + driver->apps_rsp_buf[0] = 0x7d; + driver->apps_rsp_buf[1] = 0x2; + *(uint16_t *)(driver->apps_rsp_buf + 2) = ssid_first; + *(uint16_t *)(driver->apps_rsp_buf + 4) = ssid_last; + driver->apps_rsp_buf[6] = 0x1; + driver->apps_rsp_buf[7] = 0x0; + ptr = driver->apps_rsp_buf + 8; + /* bld time masks */ + switch (ssid_first) { + case MSG_SSID_0: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_0[i/4]; + break; + case MSG_SSID_1: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_1[i/4]; + break; + case MSG_SSID_2: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_2[i/4]; + break; + case MSG_SSID_3: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_3[i/4]; + break; + case MSG_SSID_4: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_4[i/4]; + break; + case MSG_SSID_5: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_5[i/4]; + break; + case MSG_SSID_6: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_6[i/4]; + break; + case MSG_SSID_7: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_7[i/4]; + break; + case MSG_SSID_8: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_8[i/4]; + break; + case MSG_SSID_9: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_9[i/4]; + break; + case MSG_SSID_10: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_10[i/4]; + break; + case MSG_SSID_11: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_11[i/4]; + break; + case MSG_SSID_12: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_12[i/4]; + break; + case MSG_SSID_13: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_13[i/4]; + break; + case MSG_SSID_14: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_14[i/4]; + break; + case MSG_SSID_15: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_15[i/4]; + break; + case MSG_SSID_16: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_16[i/4]; + break; + case MSG_SSID_17: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_17[i/4]; + break; + case MSG_SSID_18: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_18[i/4]; + break; + case MSG_SSID_19: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_19[i/4]; + break; + case MSG_SSID_20: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_20[i/4]; + break; + case MSG_SSID_21: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_21[i/4]; + break; + case MSG_SSID_22: + for (i = 0; i < ssid_range; i += 4) + *(int *)(ptr + i) = msg_bld_masks_22[i/4]; + break; + } + ENCODE_RSP_AND_SEND(8 + ssid_range - 1); + return 0; + } + /* Check for download command */ +#ifndef CONFIG_ARCH_EXYNOS + else if ((cpu_is_msm8x60() || chk_apps_master()) && (*buf == 0x3A)) { +#else + else if (chk_apps_master() && (*buf == 0x3A)) { +#endif + /* send response back */ + driver->apps_rsp_buf[0] = *buf; + ENCODE_RSP_AND_SEND(0); + msleep(5000); + /* call download API */ + msm_set_restart_mode(RESTART_DLOAD); + printk(KERN_CRIT "diag: download mode set, Rebooting SoC..\n"); + kernel_restart(NULL); + /* Not required, represents that command isnt sent to modem */ + return 0; + } + /* Check for polling for Apps only DIAG */ + else if ((*buf == 0x4b) && (*(buf+1) == 0x32) && + (*(buf+2) == 0x03)) { + /* If no one has registered for polling */ + if (chk_polling_response()) { + /* Respond to polling for Apps only DIAG */ + for (i = 0; i < 3; i++) + driver->apps_rsp_buf[i] = *(buf+i); + for (i = 0; i < 13; i++) + driver->apps_rsp_buf[i+3] = 0; + + ENCODE_RSP_AND_SEND(15); + return 0; + } + } + /* Check for ID for NO MODEM present */ + else if (chk_polling_response()) { + /* respond to 0x0 command */ + if (*buf == 0x00) { + for (i = 0; i < 55; i++) + driver->apps_rsp_buf[i] = 0; + + ENCODE_RSP_AND_SEND(54); + return 0; + } + /* respond to 0x7c command */ + else if (*buf == 0x7c) { + driver->apps_rsp_buf[0] = 0x7c; + for (i = 1; i < 8; i++) + driver->apps_rsp_buf[i] = 0; + /* Tools ID for APQ 8060 */ + *(int *)(driver->apps_rsp_buf + 8) = + chk_config_get_id(); + *(unsigned char *)(driver->apps_rsp_buf + 12) = '\0'; + *(unsigned char *)(driver->apps_rsp_buf + 13) = '\0'; + ENCODE_RSP_AND_SEND(13); + return 0; + } + } +#endif + return packet_type; +} + +#ifdef CONFIG_DIAG_OVER_USB +void diag_send_error_rsp(int index) +{ + int i; + + if (index > 490) { + pr_err("diag: error response too huge, aborting\n"); + return; + } + driver->apps_rsp_buf[0] = 0x13; /* error code 13 */ + for (i = 0; i < index; i++) + driver->apps_rsp_buf[i+1] = *(driver->hdlc_buf+i); + ENCODE_RSP_AND_SEND(index - 3); +} +#else +static inline void diag_send_error_rsp(int index) {} +#endif + +void diag_process_hdlc(void *data, unsigned len) +{ + struct diag_hdlc_decode_type hdlc; + int ret, type = 0; + pr_debug("diag: HDLC decode fn, len of data %d\n", len); + hdlc.dest_ptr = driver->hdlc_buf; + hdlc.dest_size = USB_MAX_OUT_BUF; + hdlc.src_ptr = data; + hdlc.src_size = len; + hdlc.src_idx = 0; + hdlc.dest_idx = 0; + hdlc.escaping = 0; + + ret = diag_hdlc_decode(&hdlc); + + if (ret) + type = diag_process_apps_pkt(driver->hdlc_buf, + hdlc.dest_idx - 3); + else if (driver->debug_flag) { + printk(KERN_ERR "Packet dropped due to bad HDLC coding/CRC" + " errors or partial packet received, packet" + " length = %d\n", len); + print_hex_dump(KERN_DEBUG, "Dropped Packet Data: ", 16, 1, + DUMP_PREFIX_ADDRESS, data, len, 1); + driver->debug_flag = 0; + } + /* send error responses from APPS for Central Routing */ + if (type == 1 && chk_apps_only()) { + diag_send_error_rsp(hdlc.dest_idx); + type = 0; + } + /* implies this packet is NOT meant for apps */ + if (!(driver->ch) && type == 1) { + if (chk_apps_only()) { + diag_send_error_rsp(hdlc.dest_idx); + } else { /* APQ 8060, Let Q6 respond */ + if (driver->chqdsp) + smd_write(driver->chqdsp, driver->hdlc_buf, + hdlc.dest_idx - 3); + } + type = 0; + } + +#ifdef DIAG_DEBUG + pr_debug("diag: hdlc.dest_idx = %d", hdlc.dest_idx); + for (i = 0; i < hdlc.dest_idx; i++) + printk(KERN_DEBUG "\t%x", *(((unsigned char *) + driver->hdlc_buf)+i)); +#endif /* DIAG DEBUG */ + /* ignore 2 bytes for CRC, one for 7E and send */ + if ((driver->ch) && (ret) && (type) && (hdlc.dest_idx > 3)) { + APPEND_DEBUG('g'); + smd_write(driver->ch, driver->hdlc_buf, hdlc.dest_idx - 3); + APPEND_DEBUG('h'); +#ifdef DIAG_DEBUG + printk(KERN_INFO "writing data to SMD, pkt length %d\n", len); + print_hex_dump(KERN_DEBUG, "Written Packet Data to SMD: ", 16, + 1, DUMP_PREFIX_ADDRESS, data, len, 1); +#endif /* DIAG DEBUG */ + } +} + +#ifdef CONFIG_DIAG_OVER_USB +/* 2+1 for modem ; 2 for LPASS ; 1 for WCNSS */ +#define N_LEGACY_WRITE (driver->poolsize + 6) +#define N_LEGACY_READ 1 + +int diagfwd_connect(void) +{ + int err; + + printk(KERN_DEBUG "diag: USB connected\n"); + err = usb_diag_alloc_req(driver->legacy_ch, N_LEGACY_WRITE, + N_LEGACY_READ); + if (err) + printk(KERN_ERR "diag: unable to alloc USB req on legacy ch"); + + driver->usb_connected = 1; + driver->in_busy_1 = 0; + driver->in_busy_2 = 0; + driver->in_busy_qdsp_1 = 0; + driver->in_busy_qdsp_2 = 0; + driver->in_busy_wcnss_1 = 0; + driver->in_busy_wcnss_2 = 0; + + /* Poll SMD channels to check for data*/ + queue_work(driver->diag_wq, &(driver->diag_read_smd_work)); + queue_work(driver->diag_wq, &(driver->diag_read_smd_qdsp_work)); + queue_work(driver->diag_wq, &(driver->diag_read_smd_wcnss_work)); + /* Poll SMD CNTL channels to check for data */ + diag_smd_cntl_notify(NULL, SMD_EVENT_DATA); + diag_smd_qdsp_cntl_notify(NULL, SMD_EVENT_DATA); + diag_smd_wcnss_cntl_notify(NULL, SMD_EVENT_DATA); + /* Poll USB channel to check for data*/ + queue_work(driver->diag_wq, &(driver->diag_read_work)); +#ifdef CONFIG_DIAG_SDIO_PIPE + if (machine_is_msm8x60_fusion() || machine_is_msm8x60_fusn_ffa()) { + if (driver->mdm_ch && !IS_ERR(driver->mdm_ch)) + diagfwd_connect_sdio(); + else + printk(KERN_INFO "diag: No USB MDM ch"); + } +#endif +#ifdef CONFIG_DIAG_HSIC_PIPE + if (driver->mdm_ch && !IS_ERR(driver->mdm_ch)) + diagfwd_connect_hsic(WRITE_TO_USB); + else + printk(KERN_INFO "diag: No USB MDM ch"); +#endif + return 0; +} + +int diagfwd_disconnect(void) +{ + printk(KERN_DEBUG "diag: USB disconnected\n"); + driver->usb_connected = 0; + driver->debug_flag = 1; + usb_diag_free_req(driver->legacy_ch); + if (driver->logging_mode == USB_MODE) { + driver->in_busy_1 = 1; + driver->in_busy_2 = 1; + driver->in_busy_qdsp_1 = 1; + driver->in_busy_qdsp_2 = 1; + driver->in_busy_wcnss_1 = 1; + driver->in_busy_wcnss_2 = 1; + } +#ifdef CONFIG_DIAG_SDIO_PIPE + if (machine_is_msm8x60_fusion() || machine_is_msm8x60_fusn_ffa()) + if (driver->mdm_ch && !IS_ERR(driver->mdm_ch)) + diagfwd_disconnect_sdio(); + else + printk(KERN_INFO "diag: No USB MDM ch"); +#endif +#ifdef CONFIG_DIAG_HSIC_PIPE + if (driver->mdm_ch && !IS_ERR(driver->mdm_ch)) + diagfwd_disconnect_hsic(); + else + printk(KERN_INFO "diag: No USB MDM ch"); +#endif + /* TBD - notify and flow control SMD */ + return 0; +} + +int diagfwd_write_complete(struct diag_request *diag_write_ptr) +{ + unsigned char *buf = diag_write_ptr->buf; + /*Determine if the write complete is for data from modem/apps/q6 */ + /* Need a context variable here instead */ + if (buf == (void *)driver->buf_in_1) { + driver->in_busy_1 = 0; + APPEND_DEBUG('o'); + queue_work(driver->diag_wq, &(driver->diag_read_smd_work)); + } else if (buf == (void *)driver->buf_in_2) { + driver->in_busy_2 = 0; + APPEND_DEBUG('O'); + queue_work(driver->diag_wq, &(driver->diag_read_smd_work)); + } else if (buf == (void *)driver->buf_in_qdsp_1) { + driver->in_busy_qdsp_1 = 0; + APPEND_DEBUG('p'); + queue_work(driver->diag_wq, &(driver->diag_read_smd_qdsp_work)); + } else if (buf == (void *)driver->buf_in_qdsp_2) { + driver->in_busy_qdsp_2 = 0; + APPEND_DEBUG('P'); + queue_work(driver->diag_wq, &(driver->diag_read_smd_qdsp_work)); + } else if (buf == driver->buf_in_wcnss_1) { + driver->in_busy_wcnss_1 = 0; + APPEND_DEBUG('r'); + queue_work(driver->diag_wq, + &(driver->diag_read_smd_wcnss_work)); + } else if (buf == driver->buf_in_wcnss_2) { + driver->in_busy_wcnss_2 = 0; + APPEND_DEBUG('R'); + queue_work(driver->diag_wq, + &(driver->diag_read_smd_wcnss_work)); + } +#ifdef CONFIG_DIAG_SDIO_PIPE + else if (buf == (void *)driver->buf_in_sdio) + if (machine_is_msm8x60_fusion() || + machine_is_msm8x60_fusn_ffa()) + diagfwd_write_complete_sdio(); + else + pr_err("diag: Incorrect buffer pointer while WRITE"); +#endif + else { + diagmem_free(driver, (unsigned char *)buf, POOL_TYPE_HDLC); + diagmem_free(driver, (unsigned char *)diag_write_ptr, + POOL_TYPE_WRITE_STRUCT); + APPEND_DEBUG('q'); + } + return 0; +} + +int diagfwd_read_complete(struct diag_request *diag_read_ptr) +{ + int status = diag_read_ptr->status; + unsigned char *buf = diag_read_ptr->buf; + + /* Determine if the read complete is for data on legacy/mdm ch */ + if (buf == (void *)driver->usb_buf_out) { + driver->read_len_legacy = diag_read_ptr->actual; + APPEND_DEBUG('s'); +#ifdef DIAG_DEBUG + printk(KERN_INFO "read data from USB, pkt length %d", + diag_read_ptr->actual); + print_hex_dump(KERN_DEBUG, "Read Packet Data from USB: ", 16, 1, + DUMP_PREFIX_ADDRESS, diag_read_ptr->buf, + diag_read_ptr->actual, 1); +#endif /* DIAG DEBUG */ + if (driver->logging_mode == USB_MODE) { + if (status != -ECONNRESET && status != -ESHUTDOWN) + queue_work(driver->diag_wq, + &(driver->diag_proc_hdlc_work)); + else + queue_work(driver->diag_wq, + &(driver->diag_read_work)); + } + } +#ifdef CONFIG_DIAG_SDIO_PIPE + else if (buf == (void *)driver->usb_buf_mdm_out) { + if (machine_is_msm8x60_fusion() || + machine_is_msm8x60_fusn_ffa()) { + driver->read_len_mdm = diag_read_ptr->actual; + diagfwd_read_complete_sdio(); + } else + pr_err("diag: Incorrect buffer pointer while READ"); + } +#endif + else + printk(KERN_ERR "diag: Unknown buffer ptr from USB"); + + return 0; +} + +void diag_read_work_fn(struct work_struct *work) +{ + APPEND_DEBUG('d'); + driver->usb_read_ptr->buf = driver->usb_buf_out; + driver->usb_read_ptr->length = USB_MAX_OUT_BUF; + usb_diag_read(driver->legacy_ch, driver->usb_read_ptr); + APPEND_DEBUG('e'); +} + +void diag_process_hdlc_fn(struct work_struct *work) +{ + APPEND_DEBUG('D'); + diag_process_hdlc(driver->usb_buf_out, driver->read_len_legacy); + diag_read_work_fn(work); + APPEND_DEBUG('E'); +} + +void diag_usb_legacy_notifier(void *priv, unsigned event, + struct diag_request *d_req) +{ + switch (event) { + case USB_DIAG_CONNECT: + diagfwd_connect(); + break; + case USB_DIAG_DISCONNECT: + diagfwd_disconnect(); + break; + case USB_DIAG_READ_DONE: + diagfwd_read_complete(d_req); + break; + case USB_DIAG_WRITE_DONE: + diagfwd_write_complete(d_req); + break; + default: + printk(KERN_ERR "Unknown event from USB diag\n"); + break; + } +} + +#endif /* DIAG OVER USB */ + +static void diag_smd_notify(void *ctxt, unsigned event) +{ + if (event == SMD_EVENT_CLOSE) { + pr_info("diag: clean modem registration\n"); + diag_clear_reg(MODEM_PROC); + driver->ch = 0; + return; + } else if (event == SMD_EVENT_OPEN) { + driver->ch = ch_temp; + } + queue_work(driver->diag_wq, &(driver->diag_read_smd_work)); +} + +#if defined(CONFIG_MSM_N_WAY_SMD) +static void diag_smd_qdsp_notify(void *ctxt, unsigned event) +{ + if (event == SMD_EVENT_CLOSE) { + pr_info("diag: clean lpass registration\n"); + diag_clear_reg(QDSP_PROC); + driver->chqdsp = 0; + return; + } else if (event == SMD_EVENT_OPEN) { + driver->chqdsp = chqdsp_temp; + } + queue_work(driver->diag_wq, &(driver->diag_read_smd_qdsp_work)); +} +#endif + +static void diag_smd_wcnss_notify(void *ctxt, unsigned event) +{ + if (event == SMD_EVENT_CLOSE) { + pr_info("diag: clean wcnss registration\n"); + diag_clear_reg(WCNSS_PROC); + driver->ch_wcnss = 0; + return; + } else if (event == SMD_EVENT_OPEN) { + driver->ch_wcnss = ch_wcnss_temp; + } + queue_work(driver->diag_wq, &(driver->diag_read_smd_wcnss_work)); +} + +static int diag_smd_probe(struct platform_device *pdev) +{ + int r = 0; + + if (pdev->id == SMD_APPS_MODEM) { + r = smd_open("DIAG", &driver->ch, driver, diag_smd_notify); + ch_temp = driver->ch; + } +#if defined(CONFIG_MSM_N_WAY_SMD) + if (pdev->id == SMD_APPS_QDSP) { + r = smd_named_open_on_edge("DIAG", SMD_APPS_QDSP + , &driver->chqdsp, driver, diag_smd_qdsp_notify); + chqdsp_temp = driver->chqdsp; + } +#endif + if (pdev->id == SMD_APPS_WCNSS) { + r = smd_named_open_on_edge("APPS_RIVA_DATA", SMD_APPS_WCNSS + , &driver->ch_wcnss, driver, diag_smd_wcnss_notify); + ch_wcnss_temp = driver->ch_wcnss; + } + pm_runtime_set_active(&pdev->dev); + pm_runtime_enable(&pdev->dev); + pr_debug("diag: open SMD port, Id = %d, r = %d\n", pdev->id, r); + + return 0; +} + +static int diagfwd_runtime_suspend(struct device *dev) +{ + dev_dbg(dev, "pm_runtime: suspending...\n"); + return 0; +} + +static int diagfwd_runtime_resume(struct device *dev) +{ + dev_dbg(dev, "pm_runtime: resuming...\n"); + return 0; +} + +static const struct dev_pm_ops diagfwd_dev_pm_ops = { + .runtime_suspend = diagfwd_runtime_suspend, + .runtime_resume = diagfwd_runtime_resume, +}; + +static struct platform_driver msm_smd_ch1_driver = { + + .probe = diag_smd_probe, + .driver = { + .name = "DIAG", + .owner = THIS_MODULE, + .pm = &diagfwd_dev_pm_ops, + }, +}; + +static struct platform_driver diag_smd_lite_driver = { + + .probe = diag_smd_probe, + .driver = { + .name = "APPS_RIVA_DATA", + .owner = THIS_MODULE, + .pm = &diagfwd_dev_pm_ops, + }, +}; + +void diagfwd_init(void) +{ + diag_debug_buf_idx = 0; + driver->read_len_legacy = 0; + driver->use_device_tree = has_device_tree(); + spin_lock_init(&diag_cntl_lock); + + if (driver->event_mask == NULL) { + driver->event_mask = kzalloc(sizeof( + struct diag_ctrl_event_mask), GFP_KERNEL); + if (driver->event_mask == NULL) + goto err; + } + if (driver->msg_mask == NULL) { + driver->msg_mask = kzalloc(sizeof( + struct diag_ctrl_msg_mask), GFP_KERNEL); + if (driver->msg_mask == NULL) + goto err; + } + if (driver->log_mask == NULL) { + driver->log_mask = kzalloc(sizeof( + struct diag_ctrl_log_mask), GFP_KERNEL); + if (driver->log_mask == NULL) + goto err; + } + if (driver->buf_in_1 == NULL) { + driver->buf_in_1 = kzalloc(IN_BUF_SIZE, GFP_KERNEL); + if (driver->buf_in_1 == NULL) + goto err; + } + if (driver->buf_in_2 == NULL) { + driver->buf_in_2 = kzalloc(IN_BUF_SIZE, GFP_KERNEL); + if (driver->buf_in_2 == NULL) + goto err; + } + if (driver->buf_in_qdsp_1 == NULL) { + driver->buf_in_qdsp_1 = kzalloc(IN_BUF_SIZE, GFP_KERNEL); + if (driver->buf_in_qdsp_1 == NULL) + goto err; + } + if (driver->buf_in_qdsp_2 == NULL) { + driver->buf_in_qdsp_2 = kzalloc(IN_BUF_SIZE, GFP_KERNEL); + if (driver->buf_in_qdsp_2 == NULL) + goto err; + } + if (driver->buf_in_wcnss_1 == NULL) { + driver->buf_in_wcnss_1 = kzalloc(IN_BUF_SIZE, GFP_KERNEL); + if (driver->buf_in_wcnss_1 == NULL) + goto err; + } + if (driver->buf_in_wcnss_2 == NULL) { + driver->buf_in_wcnss_2 = kzalloc(IN_BUF_SIZE, GFP_KERNEL); + if (driver->buf_in_wcnss_2 == NULL) + goto err; + } + + if (driver->buf_msg_mask_update == NULL) { + driver->buf_msg_mask_update = kzalloc(APPS_BUF_SIZE, + GFP_KERNEL); + if (driver->buf_msg_mask_update == NULL) + goto err; + } + if (driver->buf_log_mask_update == NULL) { + driver->buf_log_mask_update = kzalloc(APPS_BUF_SIZE, + GFP_KERNEL); + if (driver->buf_log_mask_update == NULL) + goto err; + } + if (driver->buf_event_mask_update == NULL) { + driver->buf_event_mask_update = kzalloc(APPS_BUF_SIZE, + GFP_KERNEL); + if (driver->buf_event_mask_update == NULL) + goto err; + } + if (driver->usb_buf_out == NULL && + (driver->usb_buf_out = kzalloc(USB_MAX_OUT_BUF, + GFP_KERNEL)) == NULL) + goto err; + if (driver->hdlc_buf == NULL + && (driver->hdlc_buf = kzalloc(HDLC_MAX, GFP_KERNEL)) == NULL) + goto err; + if (driver->user_space_data == NULL) + driver->user_space_data = kzalloc(USER_SPACE_DATA, GFP_KERNEL); + if (driver->user_space_data == NULL) + goto err; + if (driver->msg_masks == NULL + && (driver->msg_masks = kzalloc(MSG_MASK_SIZE, + GFP_KERNEL)) == NULL) + goto err; + diag_create_msg_mask_table(); + diag_event_num_bytes = 0; + if (driver->log_masks == NULL && + (driver->log_masks = kzalloc(LOG_MASK_SIZE, GFP_KERNEL)) == NULL) + goto err; + driver->log_masks_length = (sizeof(struct mask_info))*MAX_EQUIP_ID; + if (driver->event_masks == NULL && + (driver->event_masks = kzalloc(EVENT_MASK_SIZE, + GFP_KERNEL)) == NULL) + goto err; + if (driver->client_map == NULL && + (driver->client_map = kzalloc + ((driver->num_clients) * sizeof(struct diag_client_map), + GFP_KERNEL)) == NULL) + goto err; + if (driver->buf_tbl == NULL) + driver->buf_tbl = kzalloc(buf_tbl_size * + sizeof(struct diag_write_device), GFP_KERNEL); + if (driver->buf_tbl == NULL) + goto err; + if (driver->data_ready == NULL && + (driver->data_ready = kzalloc(driver->num_clients * sizeof(int) + , GFP_KERNEL)) == NULL) + goto err; + if (driver->table == NULL && + (driver->table = kzalloc(diag_max_reg* + sizeof(struct diag_master_table), + GFP_KERNEL)) == NULL) + goto err; + if (driver->write_ptr_1 == NULL) { + driver->write_ptr_1 = kzalloc( + sizeof(struct diag_request), GFP_KERNEL); + if (driver->write_ptr_1 == NULL) + goto err; + } + if (driver->write_ptr_2 == NULL) { + driver->write_ptr_2 = kzalloc( + sizeof(struct diag_request), GFP_KERNEL); + if (driver->write_ptr_2 == NULL) + goto err; + } + if (driver->write_ptr_qdsp_1 == NULL) { + driver->write_ptr_qdsp_1 = kzalloc( + sizeof(struct diag_request), GFP_KERNEL); + if (driver->write_ptr_qdsp_1 == NULL) + goto err; + } + if (driver->write_ptr_qdsp_2 == NULL) { + driver->write_ptr_qdsp_2 = kzalloc( + sizeof(struct diag_request), GFP_KERNEL); + if (driver->write_ptr_qdsp_2 == NULL) + goto err; + } + if (driver->write_ptr_wcnss_1 == NULL) { + driver->write_ptr_wcnss_1 = kzalloc( + sizeof(struct diag_request), GFP_KERNEL); + if (driver->write_ptr_wcnss_1 == NULL) + goto err; + } + if (driver->write_ptr_wcnss_2 == NULL) { + driver->write_ptr_wcnss_2 = kzalloc( + sizeof(struct diag_request), GFP_KERNEL); + if (driver->write_ptr_wcnss_2 == NULL) + goto err; + } + + if (driver->usb_read_ptr == NULL) { + driver->usb_read_ptr = kzalloc( + sizeof(struct diag_request), GFP_KERNEL); + if (driver->usb_read_ptr == NULL) + goto err; + } + if (driver->pkt_buf == NULL && + (driver->pkt_buf = kzalloc(PKT_SIZE, + GFP_KERNEL)) == NULL) + goto err; + if (driver->apps_rsp_buf == NULL) { + driver->apps_rsp_buf = kzalloc(APPS_BUF_SIZE, GFP_KERNEL); + if (driver->apps_rsp_buf == NULL) + goto err; + } + driver->diag_wq = create_singlethread_workqueue("diag_wq"); +#ifdef CONFIG_DIAG_OVER_USB + INIT_WORK(&(driver->diag_proc_hdlc_work), diag_process_hdlc_fn); + INIT_WORK(&(driver->diag_read_work), diag_read_work_fn); + INIT_WORK(&(driver->diag_modem_mask_update_work), + diag_modem_mask_update_fn); + INIT_WORK(&(driver->diag_qdsp_mask_update_work), + diag_qdsp_mask_update_fn); + INIT_WORK(&(driver->diag_wcnss_mask_update_work), + diag_wcnss_mask_update_fn); + driver->legacy_ch = usb_diag_open(DIAG_LEGACY, driver, + diag_usb_legacy_notifier); + if (IS_ERR(driver->legacy_ch)) { + printk(KERN_ERR "Unable to open USB diag legacy channel\n"); + goto err; + } +#endif + platform_driver_register(&msm_smd_ch1_driver); + platform_driver_register(&diag_smd_lite_driver); + + return; +err: + pr_err("diag: Could not initialize diag buffers"); + kfree(driver->event_mask); + kfree(driver->log_mask); + kfree(driver->msg_mask); + kfree(driver->buf_in_1); + kfree(driver->buf_in_2); + kfree(driver->buf_in_qdsp_1); + kfree(driver->buf_in_qdsp_2); + kfree(driver->buf_in_wcnss_1); + kfree(driver->buf_in_wcnss_2); + kfree(driver->buf_msg_mask_update); + kfree(driver->buf_log_mask_update); + kfree(driver->buf_event_mask_update); + kfree(driver->usb_buf_out); + kfree(driver->hdlc_buf); + kfree(driver->msg_masks); + kfree(driver->log_masks); + kfree(driver->event_masks); + kfree(driver->client_map); + kfree(driver->buf_tbl); + kfree(driver->data_ready); + kfree(driver->table); + kfree(driver->pkt_buf); + kfree(driver->write_ptr_1); + kfree(driver->write_ptr_2); + kfree(driver->write_ptr_qdsp_1); + kfree(driver->write_ptr_qdsp_2); + kfree(driver->write_ptr_wcnss_1); + kfree(driver->write_ptr_wcnss_2); + kfree(driver->usb_read_ptr); + kfree(driver->apps_rsp_buf); + kfree(driver->user_space_data); + if (driver->diag_wq) + destroy_workqueue(driver->diag_wq); +} + +void diagfwd_exit(void) +{ + smd_close(driver->ch); + smd_close(driver->chqdsp); + smd_close(driver->ch_wcnss); + driver->ch = 0; /* SMD can make this NULL */ + driver->chqdsp = 0; + driver->ch_wcnss = 0; +#ifdef CONFIG_DIAG_OVER_USB + if (driver->usb_connected) + usb_diag_free_req(driver->legacy_ch); + usb_diag_close(driver->legacy_ch); +#endif + platform_driver_unregister(&msm_smd_ch1_driver); + platform_driver_unregister(&diag_smd_lite_driver); + kfree(driver->event_mask); + kfree(driver->log_mask); + kfree(driver->msg_mask); + kfree(driver->buf_in_1); + kfree(driver->buf_in_2); + kfree(driver->buf_in_qdsp_1); + kfree(driver->buf_in_qdsp_2); + kfree(driver->buf_in_wcnss_1); + kfree(driver->buf_in_wcnss_2); + kfree(driver->buf_msg_mask_update); + kfree(driver->buf_log_mask_update); + kfree(driver->buf_event_mask_update); + kfree(driver->usb_buf_out); + kfree(driver->hdlc_buf); + kfree(driver->msg_masks); + kfree(driver->log_masks); + kfree(driver->event_masks); + kfree(driver->client_map); + kfree(driver->buf_tbl); + kfree(driver->data_ready); + kfree(driver->table); + kfree(driver->pkt_buf); + kfree(driver->write_ptr_1); + kfree(driver->write_ptr_2); + kfree(driver->write_ptr_qdsp_1); + kfree(driver->write_ptr_qdsp_2); + kfree(driver->write_ptr_wcnss_1); + kfree(driver->write_ptr_wcnss_2); + kfree(driver->usb_read_ptr); + kfree(driver->apps_rsp_buf); + kfree(driver->user_space_data); + destroy_workqueue(driver->diag_wq); +} diff --git a/drivers/char/diag/diagfwd.h b/drivers/char/diag/diagfwd.h new file mode 100644 index 0000000..5744459 --- /dev/null +++ b/drivers/char/diag/diagfwd.h @@ -0,0 +1,43 @@ +/* Copyright (c) 2008-2012, 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. + */ + +#ifndef DIAGFWD_H +#define DIAGFWD_H + +#define NO_PROCESS 0 +#define NON_APPS_PROC -1 + +void diagfwd_init(void); +void diagfwd_exit(void); +void diag_process_hdlc(void *data, unsigned len); +void __diag_smd_send_req(void); +void __diag_smd_qdsp_send_req(void); +void __diag_smd_wcnss_send_req(void); +void diag_usb_legacy_notifier(void *, unsigned, struct diag_request *); +long diagchar_ioctl(struct file *, unsigned int, unsigned long); +int diag_device_write(void *, int, struct diag_request *); +int mask_request_validate(unsigned char mask_buf[]); +void diag_clear_reg(int); +int chk_apps_only(void); +void diag_send_event_mask_update(smd_channel_t *, int num_bytes); +void diag_send_msg_mask_update(smd_channel_t *, int ssid_first, + int ssid_last, int proc); +void diag_send_log_mask_update(smd_channel_t *, int); +/* State for diag forwarding */ +#ifdef CONFIG_DIAG_OVER_USB +int diagfwd_connect(void); +int diagfwd_disconnect(void); +#endif +extern int diag_debug_buf_idx; +extern unsigned char diag_debug_buf[1024]; +extern int diag_event_num_bytes; +#endif diff --git a/drivers/char/diag/diagfwd_cntl.c b/drivers/char/diag/diagfwd_cntl.c new file mode 100644 index 0000000..171168f --- /dev/null +++ b/drivers/char/diag/diagfwd_cntl.c @@ -0,0 +1,319 @@ +/* Copyright (c) 2011-2012, 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. + */ + +#include <linux/slab.h> +#include <linux/diagchar.h> +#include <linux/platform_device.h> +#include "diagchar.h" +#include "diagfwd.h" +#include "diagfwd_cntl.h" + +#define HDR_SIZ 8 + +void diag_smd_cntl_notify(void *ctxt, unsigned event) +{ + int r1, r2; + + if (!(driver->ch_cntl)) + return; + + switch (event) { + case SMD_EVENT_DATA: + r1 = smd_read_avail(driver->ch_cntl); + r2 = smd_cur_packet_size(driver->ch_cntl); + if (r1 > 0 && r1 == r2) + queue_work(driver->diag_wq, + &(driver->diag_read_smd_cntl_work)); + else + pr_debug("diag: incomplete pkt on Modem CNTL ch\n"); + break; + case SMD_EVENT_OPEN: + queue_work(driver->diag_cntl_wq, + &(driver->diag_modem_mask_update_work)); + break; + } +} + +void diag_smd_qdsp_cntl_notify(void *ctxt, unsigned event) +{ + int r1, r2; + + if (!(driver->chqdsp_cntl)) + return; + + switch (event) { + case SMD_EVENT_DATA: + r1 = smd_read_avail(driver->chqdsp_cntl); + r2 = smd_cur_packet_size(driver->chqdsp_cntl); + if (r1 > 0 && r1 == r2) + queue_work(driver->diag_wq, + &(driver->diag_read_smd_qdsp_cntl_work)); + else + pr_debug("diag: incomplete pkt on LPASS CNTL ch\n"); + break; + case SMD_EVENT_OPEN: + queue_work(driver->diag_cntl_wq, + &(driver->diag_qdsp_mask_update_work)); + break; + } +} + +void diag_smd_wcnss_cntl_notify(void *ctxt, unsigned event) +{ + int r1, r2; + + if (!(driver->ch_wcnss_cntl)) + return; + + switch (event) { + case SMD_EVENT_DATA: + r1 = smd_read_avail(driver->ch_wcnss_cntl); + r2 = smd_cur_packet_size(driver->ch_wcnss_cntl); + if (r1 > 0 && r1 == r2) + queue_work(driver->diag_wq, + &(driver->diag_read_smd_wcnss_cntl_work)); + else + pr_debug("diag: incomplete pkt on WCNSS CNTL ch\n"); + break; + case SMD_EVENT_OPEN: + queue_work(driver->diag_cntl_wq, + &(driver->diag_wcnss_mask_update_work)); + break; + } +} + +static void diag_smd_cntl_send_req(int proc_num) +{ + int data_len = 0, type = -1, count_bytes = 0, j, r, flag = 0; + struct bindpkt_params_per_process *pkt_params = + kzalloc(sizeof(struct bindpkt_params_per_process), GFP_KERNEL); + struct diag_ctrl_msg *msg; + struct cmd_code_range *range; + struct bindpkt_params *temp; + void *buf = NULL; + smd_channel_t *smd_ch = NULL; + + if (pkt_params == NULL) { + pr_alert("diag: Memory allocation failure\n"); + return; + } + + if (proc_num == MODEM_PROC) { + buf = driver->buf_in_cntl; + smd_ch = driver->ch_cntl; + } else if (proc_num == QDSP_PROC) { + buf = driver->buf_in_qdsp_cntl; + smd_ch = driver->chqdsp_cntl; + } else if (proc_num == WCNSS_PROC) { + buf = driver->buf_in_wcnss_cntl; + smd_ch = driver->ch_wcnss_cntl; + } + + if (!smd_ch || !buf) { + kfree(pkt_params); + return; + } + + r = smd_read_avail(smd_ch); + if (r > IN_BUF_SIZE) { + if (r < MAX_IN_BUF_SIZE) { + pr_err("diag: SMD CNTL sending pkt upto %d bytes", r); + buf = krealloc(buf, r, GFP_KERNEL); + } else { + pr_err("diag: CNTL pkt > %d bytes", MAX_IN_BUF_SIZE); + kfree(pkt_params); + return; + } + } + if (buf && r > 0) { + smd_read(smd_ch, buf, r); + while (count_bytes + HDR_SIZ <= r) { + type = *(uint32_t *)(buf); + data_len = *(uint32_t *)(buf + 4); + if (type < DIAG_CTRL_MSG_REG || + type > DIAG_CTRL_MSG_F3_MASK_V2) { + pr_alert("diag: Invalid Msg type %d proc %d", + type, proc_num); + break; + } + if (data_len < 0 || data_len > r) { + pr_alert("diag: Invalid data len %d proc %d", + data_len, proc_num); + break; + } + count_bytes = count_bytes+HDR_SIZ+data_len; + if (type == DIAG_CTRL_MSG_REG && r >= count_bytes) { + msg = buf+HDR_SIZ; + range = buf+HDR_SIZ+ + sizeof(struct diag_ctrl_msg); + pkt_params->count = msg->count_entries; + temp = kzalloc(pkt_params->count * sizeof(struct + bindpkt_params), GFP_KERNEL); + if (temp == NULL) { + pr_alert("diag: Memory alloc fail\n"); + kfree(pkt_params); + return; + } + for (j = 0; j < pkt_params->count; j++) { + temp->cmd_code = msg->cmd_code; + temp->subsys_id = msg->subsysid; + temp->client_id = proc_num; + temp->proc_id = proc_num; + temp->cmd_code_lo = range->cmd_code_lo; + temp->cmd_code_hi = range->cmd_code_hi; + range++; + temp++; + } + temp -= pkt_params->count; + pkt_params->params = temp; + flag = 1; + diagchar_ioctl(NULL, DIAG_IOCTL_COMMAND_REG, + (unsigned long)pkt_params); + kfree(temp); + } + buf = buf + HDR_SIZ + data_len; + } + } + kfree(pkt_params); + if (flag) { + /* Poll SMD CNTL channels to check for data */ + if (proc_num == MODEM_PROC) + diag_smd_cntl_notify(NULL, SMD_EVENT_DATA); + else if (proc_num == QDSP_PROC) + diag_smd_qdsp_cntl_notify(NULL, SMD_EVENT_DATA); + else if (proc_num == WCNSS_PROC) + diag_smd_wcnss_cntl_notify(NULL, SMD_EVENT_DATA); + } +} + +void diag_read_smd_cntl_work_fn(struct work_struct *work) +{ + diag_smd_cntl_send_req(MODEM_PROC); +} + +void diag_read_smd_qdsp_cntl_work_fn(struct work_struct *work) +{ + diag_smd_cntl_send_req(QDSP_PROC); +} + +void diag_read_smd_wcnss_cntl_work_fn(struct work_struct *work) +{ + diag_smd_cntl_send_req(WCNSS_PROC); +} + +static int diag_smd_cntl_probe(struct platform_device *pdev) +{ + int r = 0; + + /* open control ports only on 8960 & newer targets */ + if (chk_apps_only()) { + if (pdev->id == SMD_APPS_MODEM) + r = smd_open("DIAG_CNTL", &driver->ch_cntl, driver, + diag_smd_cntl_notify); + if (pdev->id == SMD_APPS_QDSP) + r = smd_named_open_on_edge("DIAG_CNTL", SMD_APPS_QDSP + , &driver->chqdsp_cntl, driver, + diag_smd_qdsp_cntl_notify); + if (pdev->id == SMD_APPS_WCNSS) + r = smd_named_open_on_edge("APPS_RIVA_CTRL", + SMD_APPS_WCNSS, &driver->ch_wcnss_cntl, + driver, diag_smd_wcnss_cntl_notify); + pr_debug("diag: open CNTL port, ID = %d,r = %d\n", pdev->id, r); + } + return 0; +} + +static int diagfwd_cntl_runtime_suspend(struct device *dev) +{ + dev_dbg(dev, "pm_runtime: suspending...\n"); + return 0; +} + +static int diagfwd_cntl_runtime_resume(struct device *dev) +{ + dev_dbg(dev, "pm_runtime: resuming...\n"); + return 0; +} + +static const struct dev_pm_ops diagfwd_cntl_dev_pm_ops = { + .runtime_suspend = diagfwd_cntl_runtime_suspend, + .runtime_resume = diagfwd_cntl_runtime_resume, +}; + +static struct platform_driver msm_smd_ch1_cntl_driver = { + + .probe = diag_smd_cntl_probe, + .driver = { + .name = "DIAG_CNTL", + .owner = THIS_MODULE, + .pm = &diagfwd_cntl_dev_pm_ops, + }, +}; + +static struct platform_driver diag_smd_lite_cntl_driver = { + + .probe = diag_smd_cntl_probe, + .driver = { + .name = "APPS_RIVA_CTRL", + .owner = THIS_MODULE, + .pm = &diagfwd_cntl_dev_pm_ops, + }, +}; + +void diagfwd_cntl_init(void) +{ + driver->polling_reg_flag = 0; + driver->diag_cntl_wq = create_singlethread_workqueue("diag_cntl_wq"); + if (driver->buf_in_cntl == NULL) { + driver->buf_in_cntl = kzalloc(IN_BUF_SIZE, GFP_KERNEL); + if (driver->buf_in_cntl == NULL) + goto err; + } + if (driver->buf_in_qdsp_cntl == NULL) { + driver->buf_in_qdsp_cntl = kzalloc(IN_BUF_SIZE, GFP_KERNEL); + if (driver->buf_in_qdsp_cntl == NULL) + goto err; + } + if (driver->buf_in_wcnss_cntl == NULL) { + driver->buf_in_wcnss_cntl = kzalloc(IN_BUF_SIZE, GFP_KERNEL); + if (driver->buf_in_wcnss_cntl == NULL) + goto err; + } + platform_driver_register(&msm_smd_ch1_cntl_driver); + platform_driver_register(&diag_smd_lite_cntl_driver); + + return; +err: + pr_err("diag: Could not initialize diag buffers"); + kfree(driver->buf_in_cntl); + kfree(driver->buf_in_qdsp_cntl); + kfree(driver->buf_in_wcnss_cntl); + if (driver->diag_cntl_wq) + destroy_workqueue(driver->diag_cntl_wq); +} + +void diagfwd_cntl_exit(void) +{ + smd_close(driver->ch_cntl); + smd_close(driver->chqdsp_cntl); + smd_close(driver->ch_wcnss_cntl); + driver->ch_cntl = 0; + driver->chqdsp_cntl = 0; + driver->ch_wcnss_cntl = 0; + destroy_workqueue(driver->diag_cntl_wq); + platform_driver_unregister(&msm_smd_ch1_cntl_driver); + platform_driver_unregister(&diag_smd_lite_cntl_driver); + + kfree(driver->buf_in_cntl); + kfree(driver->buf_in_qdsp_cntl); + kfree(driver->buf_in_wcnss_cntl); +} diff --git a/drivers/char/diag/diagfwd_cntl.h b/drivers/char/diag/diagfwd_cntl.h new file mode 100644 index 0000000..ad1fec9 --- /dev/null +++ b/drivers/char/diag/diagfwd_cntl.h @@ -0,0 +1,89 @@ +/* Copyright (c) 2011-2012, 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. + */ + +#ifndef DIAGFWD_CNTL_H +#define DIAGFWD_CNTL_H + +/* Message registration commands */ +#define DIAG_CTRL_MSG_REG 1 +/* Message passing for DTR events */ +#define DIAG_CTRL_MSG_DTR 2 +/* Control Diag sleep vote, buffering etc */ +#define DIAG_CTRL_MSG_DIAGMODE 3 +/* Diag data based on "light" diag mask */ +#define DIAG_CTRL_MSG_DIAGDATA 4 +/* Send diag internal feature mask 'diag_int_feature_mask' */ +#define DIAG_CTRL_MSG_FEATURE 8 +/* Send Diag log mask for a particular equip id */ +#define DIAG_CTRL_MSG_EQUIP_LOG_MASK 9 +/* Send Diag event mask */ +#define DIAG_CTRL_MSG_EVENT_MASK_V2 10 +/* Send Diag F3 mask */ +#define DIAG_CTRL_MSG_F3_MASK_V2 11 + +struct cmd_code_range { + uint16_t cmd_code_lo; + uint16_t cmd_code_hi; + uint32_t data; +}; + +struct diag_ctrl_msg { + uint32_t version; + uint16_t cmd_code; + uint16_t subsysid; + uint16_t count_entries; + uint16_t port; +}; + +struct diag_ctrl_event_mask { + uint32_t cmd_type; + uint32_t data_len; + uint8_t stream_id; + uint8_t status; + uint8_t event_config; + uint32_t event_mask_size; + /* Copy event mask here */ +} __packed; + +struct diag_ctrl_log_mask { + uint32_t cmd_type; + uint32_t data_len; + uint8_t stream_id; + uint8_t status; + uint8_t equip_id; + uint32_t num_items; /* Last log code for this equip_id */ + uint32_t log_mask_size; /* Size of log mask stored in log_mask[] */ + /* Copy log mask here */ +} __packed; + +struct diag_ctrl_msg_mask { + uint32_t cmd_type; + uint32_t data_len; + uint8_t stream_id; + uint8_t status; + uint8_t msg_mode; + uint16_t ssid_first; /* Start of range of supported SSIDs */ + uint16_t ssid_last; /* Last SSID in range */ + uint32_t msg_mask_size; /* ssid_last - ssid_first + 1 */ + /* Copy msg mask here */ +} __packed; + +void diagfwd_cntl_init(void); +void diagfwd_cntl_exit(void); +void diag_read_smd_cntl_work_fn(struct work_struct *); +void diag_read_smd_qdsp_cntl_work_fn(struct work_struct *); +void diag_read_smd_wcnss_cntl_work_fn(struct work_struct *); +void diag_smd_cntl_notify(void *ctxt, unsigned event); +void diag_smd_qdsp_cntl_notify(void *ctxt, unsigned event); +void diag_smd_wcnss_cntl_notify(void *ctxt, unsigned event); + +#endif diff --git a/drivers/char/diag/diagfwd_hsic.c b/drivers/char/diag/diagfwd_hsic.c new file mode 100644 index 0000000..aa7e957 --- /dev/null +++ b/drivers/char/diag/diagfwd_hsic.c @@ -0,0 +1,651 @@ +/* Copyright (c) 2012, 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. + */ + +#include <linux/slab.h> +#include <linux/init.h> +#include <linux/uaccess.h> +#include <linux/diagchar.h> +#include <linux/sched.h> +#include <linux/err.h> +#include <linux/workqueue.h> +#include <linux/pm_runtime.h> +#include <linux/platform_device.h> +#include <asm/current.h> +#ifdef CONFIG_DIAG_OVER_USB +#include <mach/usbdiag.h> +#endif +#include "diagchar_hdlc.h" +#include "diagmem.h" +#include "diagchar.h" +#include "diagfwd.h" +#include "diagfwd_hsic.h" + +// zero_pky.patch by jagadish +/* ascii value of zero cfg packet */ +static char zero_cfg_buf[]= {29,28,59,126,0,120,240,126,124,147,73,126,28,149,42,126,12,20,58,126,99,229,161,126,75,15,0,0,187,96,126,75,9,0,0,98,182,126,75,8,0,0,190,236,126,75,8,1,0,102,245,126,75,4,0,0,29,73,126,75,4,15,0,213,202,126,125,93,5,0,0,0,0,0,0,116,65,126,115,0,0,0,0,0,0,0,218,129,126,96,0,18,106,126}; +/* zero cfg packet is divided into number of sub packets + and the size of each sub packet is given below*/ +static unsigned int zero_cfg_packet_lens[] = {4,4,4,4,4,4,7,7,7,7,7,7,12,11,5}; + + +static void diag_read_hsic_work_fn(struct work_struct *work) +{ + if (!driver->hsic_ch) { + pr_err("DIAG in %s: driver->hsic_ch == 0\n", __func__); + return; + } + + /* + * If there is no hsic data being read from the hsic and there + * is no hsic data being written to the usb mdm channel + */ + if (!driver->in_busy_hsic_read && (!driver->in_busy_hsic_write_on_mdm || + driver->logging_mode == MEMORY_DEVICE_MODE)) { + /* + * Initiate the read from the hsic. The hsic read is + * asynchronous. Once the read is complete the read + * callback function will be called. + */ + int err; + driver->in_busy_hsic_read = 1; + APPEND_DEBUG('i'); + err = diag_bridge_read((char *)driver->buf_in_hsic, + IN_BUF_SIZE); + if (err) { + pr_err("DIAG: Error initiating HSIC read, err: %d\n", + err); + /* + * If the error is recoverable, then clear + * the read flag, so we will resubmit a + * read on the next frame. Otherwise, don't + * resubmit a read on the next frame. + */ + if ((-ESHUTDOWN) != err) + driver->in_busy_hsic_read = 0; + } + } + + /* + * If for some reason there was no hsic data, set up + * the next read + */ + if (!driver->in_busy_hsic_read) + queue_work(driver->diag_hsic_wq, &driver->diag_read_hsic_work); +} + +static void diag_hsic_read_complete_callback(void *ctxt, char *buf, + int buf_size, int actual_size) +{ + /* The read of the data from the HSIC bridge is complete */ + driver->in_busy_hsic_read = 0; + + if (!driver->hsic_ch) { + pr_err("DIAG in %s: driver->hsic_ch == 0\n", __func__); + return; + } + + // zero_pky.patch by jagadish + /* if zero cfg packet mode is enabled, then send the sub packets */ + if (driver->zero_cfg_mode) { + driver->zero_cfg_index += zero_cfg_packet_lens + [driver->zero_cfg_packet_lens_index]; + driver->zero_cfg_packet_lens_index++; + if (driver->zero_cfg_packet_lens_index == ZERO_CFG_SUBPACKET_MAX) { + pr_info("%s sending zero_cfg packet over\n", __func__); + driver->zero_cfg_mode = 0; + queue_work(driver->diag_hsic_wq, &driver->diag_disconnect_work); + } + else { + pr_info("%s zero_cfg sub packet number:%d\n", __func__,driver->zero_cfg_packet_lens_index); + queue_work(driver->diag_hsic_wq, &driver->diag_zero_cfg_hsic_work); + } + return ; + } + + + APPEND_DEBUG('j'); + if (actual_size > 0) { + if (!buf) { + pr_err("Out of diagmem for HSIC\n"); + } else { + driver->write_ptr_mdm->length = actual_size; + /* + * Set flag to denote hsic data is currently + * being written to the usb mdm channel. + * driver->buf_in_hsic was given to + * diag_bridge_read(), so buf here should be + * driver->buf_in_hsic + */ + driver->in_busy_hsic_write_on_mdm = 1; + diag_device_write((void *)buf, HSIC_DATA, + driver->write_ptr_mdm); + } + } else { + pr_debug("%s: actual_size: %d\n", __func__, actual_size); + } + + /* + * If for some reason there was no hsic data to write to the + * mdm channel, set up another read + */ + if (!driver->in_busy_hsic_write_on_mdm && + driver->usb_mdm_connected && + !driver->hsic_suspend) + queue_work(driver->diag_hsic_wq, &driver->diag_read_hsic_work); +} + +// zero_pky.patch by jagadish +/* Work function used to send zero cfg packet and receive ack */ +static void diag_zero_cfg_hsic_work_fn(struct work_struct *work) +{ + int index = driver->zero_cfg_packet_lens_index; + if (index >= ZERO_CFG_SUBPACKET_MAX) + return; + + if (!driver->in_busy_hsic_write && !driver->in_busy_hsic_read) { + driver->in_busy_hsic_write = 1; + diag_bridge_write(&zero_cfg_buf[driver->zero_cfg_index], + zero_cfg_packet_lens[index]); + driver->in_busy_hsic_read = 1; + diag_bridge_read((char *)driver->buf_in_hsic, + IN_BUF_SIZE); + } +} + + +static void diag_hsic_write_complete_callback(void *ctxt, char *buf, + int buf_size, int actual_size) +{ + /* The write of the data to the HSIC bridge is complete */ + driver->in_busy_hsic_write = 0; + + if (!driver->hsic_ch) { + pr_err("DIAG in %s: driver->hsic_ch == 0\n", __func__); + return; + } + + if (actual_size < 0) + pr_err("DIAG in %s: actual_size: %d\n", __func__, actual_size); + + if (driver->usb_mdm_connected) + queue_work(driver->diag_hsic_wq, &driver->diag_read_mdm_work); +} + +static int diag_hsic_suspend(void *ctxt) +{ + if (driver->in_busy_hsic_write) + return -EBUSY; + + driver->hsic_suspend = 1; + + return 0; +} + +static void diag_hsic_resume(void *ctxt) +{ + driver->hsic_suspend = 0; + + if ((!driver->in_busy_hsic_write_on_mdm && driver->usb_mdm_connected) + || driver->logging_mode == MEMORY_DEVICE_MODE) + queue_work(driver->diag_hsic_wq, &driver->diag_read_hsic_work); +} + +static struct diag_bridge_ops hsic_diag_bridge_ops = { + .ctxt = NULL, + .read_complete_cb = diag_hsic_read_complete_callback, + .write_complete_cb = diag_hsic_write_complete_callback, + .suspend = diag_hsic_suspend, + .resume = diag_hsic_resume, +}; + +static int diag_hsic_close(void) +{ + // zero_pky.patch by jagadish + /* if zero cfg mode is enabled, dont close the bridge */ + if (driver->zero_cfg_mode) { + pr_info("%s sending zero_cfg packet start\n", __func__); + driver->in_busy_hsic_write = 0; + driver->in_busy_hsic_read = 0; + queue_work(driver->diag_hsic_wq, &driver->diag_zero_cfg_hsic_work); + return 0; + } + + + if (driver->hsic_device_enabled) { + driver->hsic_ch = 0; + if (driver->hsic_device_opened) { + driver->hsic_device_opened = 0; + // zero_pky.patch by jagadish + driver->zero_cfg_packet_lens_index = 0; + driver->zero_cfg_index =0; + diag_bridge_close(); + } + pr_debug("DIAG in %s: closed successfully\n", __func__); + } else { + pr_debug("DIAG in %s: already closed\n", __func__); + } + + return 0; +} + +/* diagfwd_connect_hsic is called when the USB mdm channel is connected */ +int diagfwd_connect_hsic(unsigned int mode) +{ + int err; + + pr_info("DIAG in %s\n", __func__); + + if (mode == WRITE_TO_USB) { + err = usb_diag_alloc_req(driver->mdm_ch, N_MDM_WRITE, + N_MDM_READ); + if (err) + pr_err("DIAG: unable to alloc req on mdm ch err:%d\n", + err); + driver->usb_mdm_connected = 1; + } else { + pr_info("silent log %s\n", __func__); + driver->usb_mdm_connected = 0; + } + driver->in_busy_hsic_write_on_mdm = 0; + driver->in_busy_hsic_read_on_mdm = 0; + driver->in_busy_hsic_write = 0; + driver->in_busy_hsic_read = 0; + + // zero_pky.patch by jagadish + /* zero cfg packet variables are set to 0 again here, + just in case to prevent any looping */ + driver->zero_cfg_mode = 0; + driver->zero_cfg_packet_lens_index = 0; + driver->zero_cfg_index =0; + + /* If the hsic (diag_bridge) platform device is not open */ + if (driver->hsic_device_enabled) { + if (!driver->hsic_device_opened) { + err = diag_bridge_open(&hsic_diag_bridge_ops); + if (err) { + pr_err("DIAG: HSIC channel open error: %d\n", + err); + } else { + pr_debug("DIAG: opened HSIC channel\n"); + driver->hsic_device_opened = 1; + } + } else { + pr_debug("DIAG: HSIC channel already open\n"); + } + + /* + * Turn on communication over usb mdm and hsic, if the hsic + * device driver is enabled and opened + */ + if (driver->hsic_device_opened) + driver->hsic_ch = 1; + + if (mode == WRITE_TO_USB) { + /* Poll USB mdm channel to check for data */ + queue_work(driver->diag_hsic_wq, + &driver->diag_read_mdm_work); + } + + /* Poll HSIC channel to check for data */ + queue_work(driver->diag_hsic_wq, &driver->diag_read_hsic_work); + } else { + /* The hsic device driver has not yet been enabled */ + pr_info("DIAG: HSIC channel not yet enabled\n"); + } + + return 0; +} + +/* + * diagfwd_disconnect_hsic is called when the USB mdm channel + * is disconnected + */ +int diagfwd_disconnect_hsic(void) +{ + pr_debug("DIAG in %s\n", __func__); + + if (driver->usb_mdm_connected) + usb_diag_free_req(driver->mdm_ch); + // zero_pky.patch by jagadish + driver->usb_mdm_connected = 0; + driver->in_busy_hsic_write_on_mdm = 1; + driver->in_busy_hsic_read_on_mdm = 1; + driver->in_busy_hsic_write = 1; + driver->in_busy_hsic_read = 1; + + /* Turn off communication over usb mdm and hsic */ + return diag_hsic_close(); +} + +/* + * diagfwd_write_complete_hsic is called after the asynchronous + * usb_diag_write() on mdm channel is complete + */ +static int diagfwd_write_complete_hsic(void) +{ + /* + * Clear flag to denote that the write of the hsic data on the + * usb mdm channel is complete + */ + driver->in_busy_hsic_write_on_mdm = 0; + + if (!driver->hsic_ch) { + pr_err("DIAG in %s: driver->hsic_ch == 0\n", __func__); + return 0; + } + + APPEND_DEBUG('q'); + + /* Read data from the hsic */ + queue_work(driver->diag_hsic_wq, &driver->diag_read_hsic_work); + + return 0; +} + +/* Called after the asychronous usb_diag_read() on mdm channel is complete */ +static int diagfwd_read_complete_hsic(struct diag_request *diag_read_ptr) +{ + /* The read of the usb driver on the mdm (not hsic) has completed */ + driver->in_busy_hsic_read_on_mdm = 0; + driver->read_len_mdm = diag_read_ptr->actual; + + if (!driver->hsic_ch) { + pr_err("DIAG in %s: driver->hsic_ch == 0\n", __func__); + return 0; + } + + /* + * The read of the usb driver on the mdm channel has completed. + * If there is no write on the hsic in progress, check if the + * read has data to pass on to the hsic. If so, pass the usb + * mdm data on to the hsic. + */ + if (!driver->in_busy_hsic_write && driver->usb_buf_mdm_out && + (driver->read_len_mdm > 0)) { + + /* + * Initiate the hsic write. The hsic write is + * asynchronous. When complete the write + * complete callback function will be called + */ + int err; + driver->in_busy_hsic_write = 1; + err = diag_bridge_write(driver->usb_buf_mdm_out, + driver->read_len_mdm); + if (err) { + pr_err("DIAG: mdm data on hsic write err: %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; + } + } + + /* + * If there is no write of the usb mdm data on the + * hsic channel + */ + if (!driver->in_busy_hsic_write) + queue_work(driver->diag_hsic_wq, &driver->diag_read_mdm_work); + + return 0; +} + +static void diagfwd_hsic_notifier(void *priv, unsigned event, + struct diag_request *d_req) +{ + switch (event) { + case USB_DIAG_CONNECT: + diagfwd_connect_hsic(WRITE_TO_USB); + break; + case USB_DIAG_QXDM_DISCONNECT: // zero_pky.patch by jagadish + /* send zero packet */ + driver->zero_cfg_mode = 1; + /* Intentional fall through */ + case USB_DIAG_DISCONNECT: + queue_work(driver->diag_hsic_wq, &driver->diag_disconnect_work); + break; + case USB_DIAG_READ_DONE: + queue_work(driver->diag_hsic_wq, + &driver->diag_usb_read_complete_work); + break; + case USB_DIAG_WRITE_DONE: + diagfwd_write_complete_hsic(); + break; + default: + pr_err("DIAG in %s: Unknown event from USB diag:%u\n", + __func__, event); + break; + } +} + +static void diag_usb_read_complete_fn(struct work_struct *w) +{ + diagfwd_read_complete_hsic(driver->usb_read_mdm_ptr); +} + +static void diag_disconnect_work_fn(struct work_struct *w) +{ + diagfwd_disconnect_hsic(); +} + +static void diag_read_mdm_work_fn(struct work_struct *work) +{ + if (!driver->hsic_ch) { + pr_err("DIAG in %s: driver->hsic_ch == 0\n", __func__); + return; + } + + /* + * If there is no data being read from the usb mdm channel + * and there is no mdm channel data currently being written + * to the hsic + */ + if (!driver->in_busy_hsic_read_on_mdm && !driver->in_busy_hsic_write) { + APPEND_DEBUG('x'); + + /* Setup the next read from usb mdm channel */ + driver->in_busy_hsic_read_on_mdm = 1; + driver->usb_read_mdm_ptr->buf = driver->usb_buf_mdm_out; + driver->usb_read_mdm_ptr->length = USB_MAX_OUT_BUF; + usb_diag_read(driver->mdm_ch, driver->usb_read_mdm_ptr); + APPEND_DEBUG('y'); + } + + /* + * If for some reason there was no mdm channel read initiated, + * queue up the reading of data from the mdm channel + */ + if (!driver->in_busy_hsic_read_on_mdm) + queue_work(driver->diag_hsic_wq, &driver->diag_read_mdm_work); +} + +int diag_hsic_enable(void) +{ + pr_debug("DIAG in %s\n", __func__); + + driver->read_len_mdm = 0; + if (driver->buf_in_hsic == NULL) + driver->buf_in_hsic = kzalloc(IN_BUF_SIZE, GFP_KERNEL); + if (driver->buf_in_hsic == NULL) + goto err; + if (driver->usb_buf_mdm_out == NULL) + driver->usb_buf_mdm_out = kzalloc(USB_MAX_OUT_BUF, GFP_KERNEL); + if (driver->usb_buf_mdm_out == NULL) + goto err; + if (driver->write_ptr_mdm == NULL) + driver->write_ptr_mdm = kzalloc( + sizeof(struct diag_request), GFP_KERNEL); + if (driver->write_ptr_mdm == NULL) + goto err; + if (driver->usb_read_mdm_ptr == NULL) + driver->usb_read_mdm_ptr = kzalloc( + sizeof(struct diag_request), GFP_KERNEL); + if (driver->usb_read_mdm_ptr == NULL) + goto err; +#ifdef CONFIG_DIAG_OVER_USB + INIT_WORK(&(driver->diag_read_mdm_work), diag_read_mdm_work_fn); +#endif + INIT_WORK(&(driver->diag_read_hsic_work), diag_read_hsic_work_fn); + INIT_WORK(&(driver->diag_zero_cfg_hsic_work), diag_zero_cfg_hsic_work_fn); + + + driver->hsic_device_enabled = 1; + + return 0; +err: + pr_err("DIAG could not initialize buf for HSIC\n"); + kfree(driver->buf_in_hsic); + kfree(driver->usb_buf_mdm_out); + kfree(driver->write_ptr_mdm); + kfree(driver->usb_read_mdm_ptr); + if (driver->diag_hsic_wq) + destroy_workqueue(driver->diag_hsic_wq); + + return -ENOMEM; +} + +static int diag_hsic_probe(struct platform_device *pdev) +{ + int err = 0; + + if (!driver->hsic_device_enabled) { + err = diag_hsic_enable(); + if (err) { + pr_err("DIAG could not enable HSIC, err: %d\n", err); + return err; + } + } + + /* + * The probe function was called after the usb was connected + * on the legacy channel. Communication over usb mdm and hsic + * needs to be turned on. + */ + if (driver->usb_mdm_connected) { + /* The hsic (diag_bridge) platform device driver is enabled */ + err = diag_bridge_open(&hsic_diag_bridge_ops); + if (err) { + pr_err("DIAG could not open HSIC, err: %d\n", err); + driver->hsic_device_opened = 0; + return err; + } + + pr_debug("DIAG opened HSIC channel\n"); + driver->hsic_device_opened = 1; + driver->hsic_ch = 1; + driver->in_busy_hsic_write_on_mdm = 0; + driver->in_busy_hsic_read_on_mdm = 0; + driver->in_busy_hsic_write = 0; + driver->in_busy_hsic_read = 0; + + /* Poll USB mdm channel to check for data */ + queue_work(driver->diag_hsic_wq, &driver->diag_read_mdm_work); + + /* Poll HSIC channel to check for data */ + queue_work(driver->diag_hsic_wq, &driver->diag_read_hsic_work); + } + + return err; +} + +static int diag_hsic_remove(struct platform_device *pdev) +{ + pr_debug("DIAG: %s called\n", __func__); + diag_hsic_close(); + return 0; +} + +static int diagfwd_hsic_runtime_suspend(struct device *dev) +{ + dev_dbg(dev, "pm_runtime: suspending...\n"); + return 0; +} + +static int diagfwd_hsic_runtime_resume(struct device *dev) +{ + dev_dbg(dev, "pm_runtime: resuming...\n"); + return 0; +} + +static const struct dev_pm_ops diagfwd_hsic_dev_pm_ops = { + .runtime_suspend = diagfwd_hsic_runtime_suspend, + .runtime_resume = diagfwd_hsic_runtime_resume, +}; + +static struct platform_driver msm_hsic_ch_driver = { + .probe = diag_hsic_probe, + .remove = diag_hsic_remove, + .driver = { + .name = "diag_bridge", + .owner = THIS_MODULE, + .pm = &diagfwd_hsic_dev_pm_ops, + }, +}; + + +void diagfwd_hsic_init(void) +{ + int ret; + + pr_debug("DIAG in %s\n", __func__); + + driver->diag_hsic_wq = create_singlethread_workqueue("diag_hsic_wq"); + INIT_WORK(&(driver->diag_disconnect_work), diag_disconnect_work_fn); + INIT_WORK(&(driver->diag_usb_read_complete_work), + diag_usb_read_complete_fn); + +#ifdef CONFIG_DIAG_OVER_USB + driver->mdm_ch = usb_diag_open(DIAG_MDM, driver, diagfwd_hsic_notifier); + if (IS_ERR(driver->mdm_ch)) { + pr_err("DIAG Unable to open USB diag MDM channel\n"); + goto err; + } +#endif + ret = platform_driver_register(&msm_hsic_ch_driver); + if (ret) + pr_err("DIAG could not register HSIC device, ret: %d\n", ret); + else + driver->hsic_initialized = 1; + + return; +err: + pr_err("DIAG could not initialize for HSIC execution\n"); +} + +void diagfwd_hsic_exit(void) +{ + pr_debug("DIAG in %s\n", __func__); + + if (driver->hsic_initialized) + diag_hsic_close(); + +#ifdef CONFIG_DIAG_OVER_USB + if (driver->usb_mdm_connected) + usb_diag_free_req(driver->mdm_ch); +#endif + platform_driver_unregister(&msm_hsic_ch_driver); +#ifdef CONFIG_DIAG_OVER_USB + usb_diag_close(driver->mdm_ch); +#endif + kfree(driver->buf_in_hsic); + kfree(driver->usb_buf_mdm_out); + kfree(driver->write_ptr_mdm); + kfree(driver->usb_read_mdm_ptr); + destroy_workqueue(driver->diag_hsic_wq); + + driver->hsic_device_enabled = 0; +} diff --git a/drivers/char/diag/diagfwd_hsic.h b/drivers/char/diag/diagfwd_hsic.h new file mode 100644 index 0000000..38e3cfb --- /dev/null +++ b/drivers/char/diag/diagfwd_hsic.h @@ -0,0 +1,30 @@ +/* Copyright (c) 2012, 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. + */ + +#ifndef DIAGFWD_HSIC_H +#define DIAGFWD_HSIC_H + +#include <mach/diag_bridge.h> +#define N_MDM_WRITE 1 /* Upgrade to 2 with ping pong buffer */ +#define N_MDM_READ 1 + +enum { + WRITE_TO_USB = 0, + WRITE_TO_SD +}; + +void diagfwd_hsic_init(void); +void diagfwd_hsic_exit(void); +int diagfwd_connect_hsic(unsigned int); +int diagfwd_disconnect_hsic(void); + +#endif diff --git a/drivers/char/diag/diagfwd_sdio.c b/drivers/char/diag/diagfwd_sdio.c new file mode 100644 index 0000000..a145c06 --- /dev/null +++ b/drivers/char/diag/diagfwd_sdio.c @@ -0,0 +1,296 @@ +/* Copyright (c) 2011, 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. + */ + +#include <linux/slab.h> +#include <linux/init.h> +#include <linux/uaccess.h> +#include <linux/diagchar.h> +#include <linux/sched.h> +#include <linux/err.h> +#include <linux/workqueue.h> +#include <linux/pm_runtime.h> +#include <linux/platform_device.h> +#include <asm/current.h> +#ifdef CONFIG_DIAG_OVER_USB +#include <mach/usbdiag.h> +#endif +#include "diagchar_hdlc.h" +#include "diagmem.h" +#include "diagchar.h" +#include "diagfwd.h" +#include "diagfwd_sdio.h" + +void __diag_sdio_send_req(void) +{ + int r = 0; + void *buf = driver->buf_in_sdio; + + if (driver->sdio_ch && (!driver->in_busy_sdio)) { + r = sdio_read_avail(driver->sdio_ch); + + if (r > IN_BUF_SIZE) { + if (r < MAX_IN_BUF_SIZE) { + pr_err("diag: SDIO sending" + " packets more than %d bytes\n", r); + buf = krealloc(buf, r, GFP_KERNEL); + } else { + pr_err("diag: SDIO sending" + " in packets more than %d bytes\n", MAX_IN_BUF_SIZE); + return; + } + } + if (r > 0) { + if (!buf) + printk(KERN_INFO "Out of diagmem for SDIO\n"); + else { + APPEND_DEBUG('i'); + sdio_read(driver->sdio_ch, buf, r); + if (((!driver->usb_connected) && (driver-> + logging_mode == USB_MODE)) || (driver-> + logging_mode == NO_LOGGING_MODE)) { + /* Drop the diag payload */ + driver->in_busy_sdio = 0; + return; + } + APPEND_DEBUG('j'); + driver->write_ptr_mdm->length = r; + driver->in_busy_sdio = 1; + diag_device_write(buf, SDIO_DATA, + driver->write_ptr_mdm); + } + } + } +} + +static void diag_read_sdio_work_fn(struct work_struct *work) +{ + __diag_sdio_send_req(); +} + +static void diag_sdio_notify(void *ctxt, unsigned event) +{ + if (event == SDIO_EVENT_DATA_READ_AVAIL) + queue_work(driver->diag_sdio_wq, + &(driver->diag_read_sdio_work)); + + if (event == SDIO_EVENT_DATA_WRITE_AVAIL) + wake_up_interruptible(&driver->wait_q); +} + +static int diag_sdio_close(void) +{ + queue_work(driver->diag_sdio_wq, &(driver->diag_close_sdio_work)); + return 0; +} + +static void diag_close_sdio_work_fn(struct work_struct *work) +{ + pr_debug("diag: sdio close called\n"); + if (sdio_close(driver->sdio_ch)) + pr_err("diag: could not close SDIO channel\n"); + else + driver->sdio_ch = NULL; /* channel successfully closed */ +} + +int diagfwd_connect_sdio(void) +{ + int err; + + err = usb_diag_alloc_req(driver->mdm_ch, N_MDM_WRITE, + N_MDM_READ); + if (err) + pr_err("diag: unable to alloc USB req on mdm ch\n"); + + driver->in_busy_sdio = 0; + if (!driver->sdio_ch) { + err = sdio_open("SDIO_DIAG", &driver->sdio_ch, driver, + diag_sdio_notify); + if (err) + pr_info("diag: could not open SDIO channel\n"); + else + pr_info("diag: opened SDIO channel\n"); + } else { + pr_info("diag: SDIO channel already open\n"); + } + + /* Poll USB channel to check for data*/ + queue_work(driver->diag_sdio_wq, &(driver->diag_read_mdm_work)); + /* Poll SDIO channel to check for data*/ + queue_work(driver->diag_sdio_wq, &(driver->diag_read_sdio_work)); + return 0; +} + +int diagfwd_disconnect_sdio(void) +{ + usb_diag_free_req(driver->mdm_ch); + if (driver->sdio_ch && (driver->logging_mode == USB_MODE)) { + driver->in_busy_sdio = 1; + diag_sdio_close(); + } + return 0; +} + +int diagfwd_write_complete_sdio(void) +{ + driver->in_busy_sdio = 0; + APPEND_DEBUG('q'); + queue_work(driver->diag_sdio_wq, &(driver->diag_read_sdio_work)); + return 0; +} + +int diagfwd_read_complete_sdio(void) +{ + queue_work(driver->diag_sdio_wq, &(driver->diag_read_mdm_work)); + return 0; +} + +void diag_read_mdm_work_fn(struct work_struct *work) +{ + if (driver->sdio_ch) { + wait_event_interruptible(driver->wait_q, ((sdio_write_avail + (driver->sdio_ch) >= driver->read_len_mdm) || + !(driver->sdio_ch))); + if (!(driver->sdio_ch)) { + pr_alert("diag: sdio channel not valid"); + return; + } + if (driver->sdio_ch && driver->usb_buf_mdm_out && + (driver->read_len_mdm > 0)) + sdio_write(driver->sdio_ch, driver->usb_buf_mdm_out, + driver->read_len_mdm); + APPEND_DEBUG('x'); + driver->usb_read_mdm_ptr->buf = driver->usb_buf_mdm_out; + driver->usb_read_mdm_ptr->length = USB_MAX_OUT_BUF; + usb_diag_read(driver->mdm_ch, driver->usb_read_mdm_ptr); + APPEND_DEBUG('y'); + } +} + +static int diag_sdio_probe(struct platform_device *pdev) +{ + int err; + + err = sdio_open("SDIO_DIAG", &driver->sdio_ch, driver, + diag_sdio_notify); + if (err) + printk(KERN_INFO "DIAG could not open SDIO channel"); + else { + printk(KERN_INFO "DIAG opened SDIO channel"); + queue_work(driver->diag_sdio_wq, &(driver->diag_read_mdm_work)); + } + + return err; +} + +static int diag_sdio_remove(struct platform_device *pdev) +{ + pr_debug("\n diag: sdio remove called"); + /* Disable SDIO channel to prevent further read/write */ + driver->sdio_ch = NULL; + return 0; +} + +static int diagfwd_sdio_runtime_suspend(struct device *dev) +{ + dev_dbg(dev, "pm_runtime: suspending...\n"); + return 0; +} + +static int diagfwd_sdio_runtime_resume(struct device *dev) +{ + dev_dbg(dev, "pm_runtime: resuming...\n"); + return 0; +} + +static const struct dev_pm_ops diagfwd_sdio_dev_pm_ops = { + .runtime_suspend = diagfwd_sdio_runtime_suspend, + .runtime_resume = diagfwd_sdio_runtime_resume, +}; + +static struct platform_driver msm_sdio_ch_driver = { + .probe = diag_sdio_probe, + .remove = diag_sdio_remove, + .driver = { + .name = "SDIO_DIAG", + .owner = THIS_MODULE, + .pm = &diagfwd_sdio_dev_pm_ops, + }, +}; + +void diagfwd_sdio_init(void) +{ + int ret; + + driver->read_len_mdm = 0; + if (driver->buf_in_sdio == NULL) + driver->buf_in_sdio = kzalloc(IN_BUF_SIZE, GFP_KERNEL); + if (driver->buf_in_sdio == NULL) + goto err; + if (driver->usb_buf_mdm_out == NULL) + driver->usb_buf_mdm_out = kzalloc(USB_MAX_OUT_BUF, GFP_KERNEL); + if (driver->usb_buf_mdm_out == NULL) + goto err; + if (driver->write_ptr_mdm == NULL) + driver->write_ptr_mdm = kzalloc( + sizeof(struct diag_request), GFP_KERNEL); + if (driver->write_ptr_mdm == NULL) + goto err; + if (driver->usb_read_mdm_ptr == NULL) + driver->usb_read_mdm_ptr = kzalloc( + sizeof(struct diag_request), GFP_KERNEL); + if (driver->usb_read_mdm_ptr == NULL) + goto err; + driver->diag_sdio_wq = create_singlethread_workqueue("diag_sdio_wq"); +#ifdef CONFIG_DIAG_OVER_USB + driver->mdm_ch = usb_diag_open(DIAG_MDM, driver, + diag_usb_legacy_notifier); + if (IS_ERR(driver->mdm_ch)) { + printk(KERN_ERR "Unable to open USB diag MDM channel\n"); + goto err; + } + INIT_WORK(&(driver->diag_read_mdm_work), diag_read_mdm_work_fn); +#endif + INIT_WORK(&(driver->diag_read_sdio_work), diag_read_sdio_work_fn); + INIT_WORK(&(driver->diag_close_sdio_work), diag_close_sdio_work_fn); + ret = platform_driver_register(&msm_sdio_ch_driver); + if (ret) + printk(KERN_INFO "DIAG could not register SDIO device"); + else + printk(KERN_INFO "DIAG registered SDIO device"); + + return; +err: + printk(KERN_INFO "\n Could not initialize diag buf for SDIO"); + kfree(driver->buf_in_sdio); + kfree(driver->usb_buf_mdm_out); + kfree(driver->write_ptr_mdm); + kfree(driver->usb_read_mdm_ptr); + if (driver->diag_sdio_wq) + destroy_workqueue(driver->diag_sdio_wq); +} + +void diagfwd_sdio_exit(void) +{ +#ifdef CONFIG_DIAG_OVER_USB + if (driver->usb_connected) + usb_diag_free_req(driver->mdm_ch); +#endif + platform_driver_unregister(&msm_sdio_ch_driver); +#ifdef CONFIG_DIAG_OVER_USB + usb_diag_close(driver->mdm_ch); +#endif + kfree(driver->buf_in_sdio); + kfree(driver->usb_buf_mdm_out); + kfree(driver->write_ptr_mdm); + kfree(driver->usb_read_mdm_ptr); + destroy_workqueue(driver->diag_sdio_wq); +} diff --git a/drivers/char/diag/diagfwd_sdio.h b/drivers/char/diag/diagfwd_sdio.h new file mode 100644 index 0000000..40982c3 --- /dev/null +++ b/drivers/char/diag/diagfwd_sdio.h @@ -0,0 +1,27 @@ +/* Copyright (c) 2011, 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. + */ + +#ifndef DIAGFWD_SDIO_H +#define DIAGFWD_SDIO_H + +#include <mach/sdio_al.h> +#define N_MDM_WRITE 1 /* Upgrade to 2 with ping pong buffer */ +#define N_MDM_READ 1 + +void diagfwd_sdio_init(void); +void diagfwd_sdio_exit(void); +int diagfwd_connect_sdio(void); +int diagfwd_disconnect_sdio(void); +int diagfwd_read_complete_sdio(void); +int diagfwd_write_complete_sdio(void); + +#endif diff --git a/drivers/char/diag/diagmem.c b/drivers/char/diag/diagmem.c new file mode 100644 index 0000000..317aff8 --- /dev/null +++ b/drivers/char/diag/diagmem.c @@ -0,0 +1,145 @@ +/* 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. + * + */ + +#include <linux/init.h> +#include <linux/module.h> +#include <linux/mempool.h> +#include <linux/mutex.h> +#include <linux/atomic.h> +#include "diagchar.h" + +void *diagmem_alloc(struct diagchar_dev *driver, int size, int pool_type) +{ + void *buf = NULL; + + if (pool_type == POOL_TYPE_COPY) { + if (driver->diagpool) { + mutex_lock(&driver->diagmem_mutex); + if (driver->count < driver->poolsize) { + atomic_add(1, (atomic_t *)&driver->count); + buf = mempool_alloc(driver->diagpool, + GFP_ATOMIC); + } + mutex_unlock(&driver->diagmem_mutex); + } + } else if (pool_type == POOL_TYPE_HDLC) { + if (driver->diag_hdlc_pool) { + if (driver->count_hdlc_pool < driver->poolsize_hdlc) { + atomic_add(1, + (atomic_t *)&driver->count_hdlc_pool); + buf = mempool_alloc(driver->diag_hdlc_pool, + GFP_ATOMIC); + } + } + } else if (pool_type == POOL_TYPE_WRITE_STRUCT) { + if (driver->diag_write_struct_pool) { + if (driver->count_write_struct_pool < + driver->poolsize_write_struct) { + atomic_add(1, + (atomic_t *)&driver->count_write_struct_pool); + buf = mempool_alloc( + driver->diag_write_struct_pool, GFP_ATOMIC); + } + } + } + return buf; +} + +void diagmem_exit(struct diagchar_dev *driver, int pool_type) +{ + if (driver->diagpool) { + if (driver->count == 0 && driver->ref_count == 0) { + mempool_destroy(driver->diagpool); + driver->diagpool = NULL; + } else if (driver->ref_count == 0 && pool_type == POOL_TYPE_ALL) + printk(KERN_ALERT "Unable to destroy COPY mempool"); + } + + if (driver->diag_hdlc_pool) { + if (driver->count_hdlc_pool == 0 && driver->ref_count == 0) { + mempool_destroy(driver->diag_hdlc_pool); + driver->diag_hdlc_pool = NULL; + } else if (driver->ref_count == 0 && pool_type == POOL_TYPE_ALL) + printk(KERN_ALERT "Unable to destroy HDLC mempool"); + } + + if (driver->diag_write_struct_pool) { + /* Free up struct pool ONLY if there are no outstanding + transactions(aggregation buffer) with USB */ + if (driver->count_write_struct_pool == 0 && + driver->count_hdlc_pool == 0 && driver->ref_count == 0) { + mempool_destroy(driver->diag_write_struct_pool); + driver->diag_write_struct_pool = NULL; + } else if (driver->ref_count == 0 && pool_type == POOL_TYPE_ALL) + printk(KERN_ALERT "Unable to destroy STRUCT mempool"); + } +} + +void diagmem_free(struct diagchar_dev *driver, void *buf, int pool_type) +{ + if (pool_type == POOL_TYPE_COPY) { + if (driver->diagpool != NULL && driver->count > 0) { + mempool_free(buf, driver->diagpool); + atomic_add(-1, (atomic_t *)&driver->count); + } else + pr_err("diag: Attempt to free up DIAG driver " + "mempool memory which is already free %d", driver->count); + } else if (pool_type == POOL_TYPE_HDLC) { + if (driver->diag_hdlc_pool != NULL && + driver->count_hdlc_pool > 0) { + mempool_free(buf, driver->diag_hdlc_pool); + atomic_add(-1, (atomic_t *)&driver->count_hdlc_pool); + } else + pr_err("diag: Attempt to free up DIAG driver " + "HDLC mempool which is already free %d ", driver->count_hdlc_pool); + } else if (pool_type == POOL_TYPE_WRITE_STRUCT) { + if (driver->diag_write_struct_pool != NULL && + driver->count_write_struct_pool > 0) { + mempool_free(buf, driver->diag_write_struct_pool); + atomic_add(-1, + (atomic_t *)&driver->count_write_struct_pool); + } else + pr_err("diag: Attempt to free up DIAG driver " + "USB structure mempool which is already free %d ", + driver->count_write_struct_pool); + } + + diagmem_exit(driver, pool_type); +} + +void diagmem_init(struct diagchar_dev *driver) +{ + mutex_init(&driver->diagmem_mutex); + + if (driver->count == 0) + driver->diagpool = mempool_create_kmalloc_pool( + driver->poolsize, driver->itemsize); + + if (driver->count_hdlc_pool == 0) + driver->diag_hdlc_pool = mempool_create_kmalloc_pool( + driver->poolsize_hdlc, driver->itemsize_hdlc); + + if (driver->count_write_struct_pool == 0) + driver->diag_write_struct_pool = mempool_create_kmalloc_pool( + driver->poolsize_write_struct, driver->itemsize_write_struct); + + if (!driver->diagpool) + printk(KERN_INFO "Cannot allocate diag mempool\n"); + + if (!driver->diag_hdlc_pool) + printk(KERN_INFO "Cannot allocate diag HDLC mempool\n"); + + if (!driver->diag_write_struct_pool) + printk(KERN_INFO "Cannot allocate diag USB struct mempool\n"); +} + diff --git a/drivers/char/diag/diagmem.h b/drivers/char/diag/diagmem.h new file mode 100644 index 0000000..43829ae --- /dev/null +++ b/drivers/char/diag/diagmem.h @@ -0,0 +1,22 @@ +/* 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. + */ + +#ifndef DIAGMEM_H +#define DIAGMEM_H +#include "diagchar.h" + +void *diagmem_alloc(struct diagchar_dev *driver, int size, int pool_type); +void diagmem_free(struct diagchar_dev *driver, void *buf, int pool_type); +void diagmem_init(struct diagchar_dev *driver); +void diagmem_exit(struct diagchar_dev *driver, int pool_type); + +#endif |