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;
}
/* clean up any DCI registrations for this client
* This will specially help in case of ungraceful exit of any DCI client
* This call will remove any pending registrations of such client
*/
diagchar_ioctl(NULL, DIAG_IOCTL_DCI_DEINIT, 0);
/* If the exiting process is the socket process */
if (driver->socket_process &&
(driver->socket_process->tgid == current->tgid)) {
driver->socket_process = NULL;
}
#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;
diagfwd_connect();
#ifdef CONFIG_DIAG_BRIDGE_CODE
diag_clear_hsic_tbl();
diagfwd_cancel_hsic();
diagfwd_connect_bridge(0);
#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;
}
/*
* FUNCTION mtc_send_hdlc_packet.
*/
static void mtc_send_hdlc_packet(byte * pBuf, int len)
{
int i;
struct mtc_data_buffer *mb;
word crc = CRC_16_L_SEED;
mb = kzalloc(sizeof(struct mtc_data_buffer), GFP_ATOMIC);
if (mb == NULL) {
printk(KERN_ERR "[MTC] %s: failed to alloc memory\n", __func__);
return;
}
//Generate crc data.
for (i = 0; i < len; i++) {
add_hdlc_esc_packet(mb, pBuf[i]);
crc = CRC_16_L_STEP(crc, (word) pBuf[i]);
}
crc ^= CRC_16_L_SEED;
add_hdlc_esc_packet(mb, ((unsigned char)crc));
add_hdlc_esc_packet(mb, ((unsigned char)((crc >> 8) & 0xFF)));
add_hdlc_packet(mb, CONTROL_CHAR);
if (diagchar_ioctl(DIAG_IOCTL_BULK_DATA, (unsigned long)mb)) {
printk(KERN_ERR "[MTC] %s: ioctl ignored\n", __func__);
}
kfree(mb);
mb = NULL;
}
/*
* FUNCTION add_hdlc_packet.
*/
static void add_hdlc_packet(struct mtc_data_buffer *mb, char data)
{
mb->data[mb->data_length++] = data;
//if (mb->data_length == BUFFER_MAX_SIZE) {
if (mb->data_length >= BUFFER_MAX_SIZE) {
mb->data_length = BUFFER_MAX_SIZE;
msleep(10);
if (diagchar_ioctl (DIAG_IOCTL_BULK_DATA, (unsigned long)mb)) {
printk(KERN_ERR "[MTC] %s: diagchar_ioctl error\n", __func__);
}
mb->data_length = 0;
}
}
/* Process the data read from the smd control channel */
int diag_process_smd_cntl_read_data(struct diag_smd_info *smd_info, void *buf,
int total_recd)
{
int data_len = 0, type = -1, count_bytes = 0, j, 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;
if (pkt_params == NULL) {
pr_alert("diag: In %s, Memory allocation failure\n",
__func__);
return 0;
}
if (!smd_info) {
pr_err("diag: In %s, No smd info. Not able to read.\n",
__func__);
kfree(pkt_params);
return 0;
}
while (count_bytes + HDR_SIZ <= total_recd) {
type = *(uint32_t *)(buf);
data_len = *(uint32_t *)(buf + 4);
if (type < DIAG_CTRL_MSG_REG ||
type > DIAG_CTRL_MSG_LAST) {
pr_alert("diag: In %s, Invalid Msg type %d proc %d",
__func__, type, smd_info->peripheral);
break;
}
if (data_len < 0 || data_len > total_recd) {
pr_alert("diag: In %s, Invalid data len %d, total_recd: %d, proc %d",
__func__, data_len, total_recd,
smd_info->peripheral);
break;
}
count_bytes = count_bytes+HDR_SIZ+data_len;
if (type == DIAG_CTRL_MSG_REG && total_recd >= count_bytes) {
msg = buf+HDR_SIZ;
range = buf+HDR_SIZ+
sizeof(struct diag_ctrl_msg);
pkt_params->count = msg->count_entries;
pkt_params->params = kzalloc(pkt_params->count *
sizeof(struct bindpkt_params), GFP_KERNEL);
if (ZERO_OR_NULL_PTR(pkt_params->params)) {
pr_alert("diag: In %s, Memory alloc fail\n",
__func__);
kfree(pkt_params);
return flag;
}
temp = pkt_params->params;
for (j = 0; j < pkt_params->count; j++) {
temp->cmd_code = msg->cmd_code;
temp->subsys_id = msg->subsysid;
temp->client_id = smd_info->peripheral;
temp->proc_id = NON_APPS_PROC;
temp->cmd_code_lo = range->cmd_code_lo;
temp->cmd_code_hi = range->cmd_code_hi;
range++;
temp++;
}
flag = 1;
/* peripheral undergoing SSR should not
* record new registration
*/
if (!(reg_dirty & smd_info->peripheral_mask))
diagchar_ioctl(NULL, DIAG_IOCTL_COMMAND_REG,
(unsigned long)pkt_params);
else
pr_err("diag: drop reg proc %d\n",
smd_info->peripheral);
kfree(pkt_params->params);
} else if (type == DIAG_CTRL_MSG_FEATURE &&
total_recd >= count_bytes) {
uint8_t feature_mask = 0;
int feature_mask_len = *(int *)(buf+8);
if (feature_mask_len > 0) {
int periph = smd_info->peripheral;
feature_mask = *(uint8_t *)(buf+12);
if (periph == MODEM_DATA)
driver->log_on_demand_support =
feature_mask &
F_DIAG_LOG_ON_DEMAND_RSP_ON_MASTER;
/*
* If apps supports separate cmd/rsp channels
* and the peripheral supports separate cmd/rsp
* channels
*/
if (driver->supports_separate_cmdrsp &&
(feature_mask & F_DIAG_REQ_RSP_CHANNEL))
driver->separate_cmdrsp[periph] =
ENABLE_SEPARATE_CMDRSP;
else
driver->separate_cmdrsp[periph] =
DISABLE_SEPARATE_CMDRSP;
/*
* Check if apps supports hdlc encoding and the
* peripheral supports apps hdlc encoding
*/
process_hdlc_encoding_feature(smd_info,
feature_mask);
if (feature_mask_len > 1) {
feature_mask = *(uint8_t *)(buf+13);
process_stm_feature(smd_info,
feature_mask);
}
}
flag = 1;
} else if (type != DIAG_CTRL_MSG_REG) {
flag = 1;
}
buf = buf + HDR_SIZ + data_len;
}
kfree(pkt_params);
return flag;
}
static void diag_smd_cntl_send_req(int proc_num)
{
int data_len = 0, type = -1, count_bytes = 0, j, r;
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 (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)
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);
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);
for (j = 0; j < pkt_params->count; j++) {
temp->cmd_code = msg->cmd_code;
temp->subsys_id = msg->subsysid;
if (proc_num == MODEM_PROC)
temp->client_id =
(uint32_t)(driver->ch);
else if (proc_num == QDSP_PROC)
temp->client_id =
(uint32_t)(driver->chqdsp);
else if (proc_num == WCNSS_PROC)
temp->client_id =
(uint32_t)(driver->ch_wcnss);
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;
diagchar_ioctl(NULL, DIAG_IOCTL_COMMAND_REG,
(unsigned long)pkt_params);
kfree(temp);
buf = buf + HDR_SIZ + data_len;
}
}
}
kfree(pkt_params);
}
/* Process the data read from the smd control channel */
int diag_process_smd_cntl_read_data(struct diag_smd_info *smd_info, void *buf,
int total_recd)
{
int data_len = 0, type = -1, count_bytes = 0, j, flag = 0;
int feature_mask_len;
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;
if (pkt_params == NULL) {
pr_alert("diag: In %s, Memory allocation failure\n",
__func__);
return 0;
}
if (!smd_info) {
pr_err("diag: In %s, No smd info. Not able to read.\n",
__func__);
kfree(pkt_params);
return 0;
}
while (count_bytes + HDR_SIZ <= total_recd) {
type = *(uint32_t *)(buf);
data_len = *(uint32_t *)(buf + 4);
if (type < DIAG_CTRL_MSG_REG ||
type > DIAG_CTRL_MSG_LAST) {
pr_alert("diag: In %s, Invalid Msg type %d proc %d",
__func__, type, smd_info->peripheral);
break;
}
if (data_len < 0 || data_len > total_recd) {
pr_alert("diag: In %s, Invalid data len %d, total_recd: %d, proc %d",
__func__, data_len, total_recd,
smd_info->peripheral);
break;
}
count_bytes = count_bytes+HDR_SIZ+data_len;
if (type == DIAG_CTRL_MSG_REG && total_recd >= count_bytes) {
msg = buf+HDR_SIZ;
range = buf+HDR_SIZ+
sizeof(struct diag_ctrl_msg);
pkt_params->count = msg->count_entries;
pkt_params->params = kzalloc(pkt_params->count *
sizeof(struct bindpkt_params), GFP_KERNEL);
if (ZERO_OR_NULL_PTR(pkt_params->params)) {
pr_alert("diag: In %s, Memory alloc fail\n",
__func__);
kfree(pkt_params);
return flag;
}
temp = pkt_params->params;
for (j = 0; j < pkt_params->count; j++) {
temp->cmd_code = msg->cmd_code;
temp->subsys_id = msg->subsysid;
temp->client_id = smd_info->peripheral;
temp->proc_id = NON_APPS_PROC;
temp->cmd_code_lo = range->cmd_code_lo;
temp->cmd_code_hi = range->cmd_code_hi;
range++;
temp++;
}
flag = 1;
/* peripheral undergoing SSR should not
* record new registration
*/
if (!(reg_dirty & smd_info->peripheral_mask))
diagchar_ioctl(NULL, DIAG_IOCTL_COMMAND_REG,
(unsigned long)pkt_params);
else
pr_err("diag: drop reg proc %d\n",
smd_info->peripheral);
kfree(pkt_params->params);
} else if ((type == DIAG_CTRL_MSG_FEATURE) &&
(smd_info->peripheral == MODEM_DATA)) {
feature_mask_len = *(int *)(buf + 8);
driver->log_on_demand_support = (*(uint8_t *)
(buf + 12)) & 0x04;
} else if (type != DIAG_CTRL_MSG_REG) {
flag = 1;
}
buf = buf + HDR_SIZ + data_len;
}
kfree(pkt_params);
return flag;
}
static void process_command_registration(uint8_t *buf, uint32_t len,
struct diag_smd_info *smd_info)
{
uint8_t *ptr = buf;
int i;
int header_len = sizeof(struct diag_ctrl_cmd_reg);
int read_len = 0;
struct bindpkt_params_per_process *pkt_params = NULL;
struct bindpkt_params *temp = NULL;
struct diag_ctrl_cmd_reg *reg = NULL;
struct cmd_code_range *range = NULL;
/*
* Perform Basic sanity. The len field is the size of the data payload.
* This doesn't include the header size.
*/
if (!buf || !smd_info || len == 0)
return;
/* Peripheral undergoing SSR should not record new registration */
if (reg_dirty & smd_info->peripheral_mask) {
pr_err("diag: dropping command registration from peripheral %d\n",
smd_info->peripheral);
return;
}
reg = (struct diag_ctrl_cmd_reg *)ptr;
ptr += header_len;
/* Don't account for pkt_id and length */
read_len += header_len - (2 * sizeof(uint32_t));
if (reg->count_entries == 0) {
pr_debug("diag: In %s, received reg tbl with no entries\n",
__func__);
return;
}
pkt_params = kzalloc(sizeof(struct bindpkt_params_per_process),
GFP_KERNEL);
if (!pkt_params) {
pr_err("diag: In %s, unable to allocate memory for new command table entry\n",
__func__);
return;
}
pkt_params->count = reg->count_entries;
pkt_params->params = kzalloc(pkt_params->count *
sizeof(struct bindpkt_params),
GFP_KERNEL);
if (!pkt_params->params) {
pr_err("diag: In %s, Memory alloc fail for cmd_code: %d, subsys: %d\n",
__func__, reg->cmd_code, reg->subsysid);
kfree(pkt_params);
return;
}
temp = pkt_params->params;
for (i = 0; i < reg->count_entries && read_len < len; i++, temp++) {
temp->cmd_code = reg->cmd_code;
temp->subsys_id = reg->subsysid;
temp->client_id = smd_info->peripheral;
temp->proc_id = NON_APPS_PROC;
range = (struct cmd_code_range *)ptr;
temp->cmd_code_lo = range->cmd_code_lo;
temp->cmd_code_hi = range->cmd_code_hi;
ptr += sizeof(struct cmd_code_range);
read_len += sizeof(struct cmd_code_range);
}
diagchar_ioctl(NULL, DIAG_IOCTL_COMMAND_REG, (unsigned long)pkt_params);
kfree(pkt_params->params);
kfree(pkt_params);
}
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);
}
}
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, *dump_buf = NULL;
smd_channel_t *smd_ch = NULL;
DIAG_INFO("%s: %s\n", __func__,
(proc_num == MODEM_PROC)?"MODEM_PROC":
(proc_num == QDSP_PROC)?"QDSP_PROC":"WCNSS_PROC");
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);
count_bytes = count_bytes+HDR_SIZ+data_len;
if (type == DIAG_CTRL_MSG_REG && r >= count_bytes) {
msg = buf+HDR_SIZ;
if (!msg->count_entries) {
DIAG_ERR("version: %d, cmd_code: %d,"
" subsysid: %d, count_entries: %d,"
" port:%d\n", msg->version,
msg->cmd_code, msg->subsysid,
msg->count_entries, msg->port);
dump_buf = kmalloc(r, GFP_KERNEL);
memcpy(dump_buf, buf, r);
continue;
}
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);
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;
}
}
}
if (dump_buf) {
print_hex_dump(KERN_DEBUG, "diag_debug_buf:",
16, 1, DUMP_PREFIX_ADDRESS, dump_buf, r, 1);
kfree(dump_buf);
}
kfree(pkt_params);
if (flag) {
/* Poll SMD CNTL channels to check for data */
queue_work(driver->diag_wq, &(driver->diag_read_smd_cntl_work));
queue_work(driver->diag_wq,
&(driver->diag_read_smd_qdsp_cntl_work));
queue_work(driver->diag_wq,
&(driver->diag_read_smd_wcnss_cntl_work));
}
}
