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)); } }