// SPDX-License-Identifier: GPL-2.0-only /* Copyright (c) 2008-2020, The Linux Foundation. All rights reserved. */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #ifdef CONFIG_DIAG_OVER_USB #include #endif #include #include #include "diagmem.h" #include "diagchar.h" #include "diagfwd.h" #include "diagfwd_peripheral.h" #include "diagfwd_cntl.h" #include "diagchar_hdlc.h" #include "diag_dci.h" #include "diag_masks.h" #include "diag_usb.h" #include "diag_mux.h" #include "diag_ipc_logging.h" #define STM_CMD_VERSION_OFFSET 4 #define STM_CMD_MASK_OFFSET 5 #define STM_CMD_DATA_OFFSET 6 #define STM_CMD_NUM_BYTES 7 #define STM_RSP_SUPPORTED_INDEX 7 #define STM_RSP_STATUS_INDEX 8 #define STM_RSP_NUM_BYTES 9 #define RETRY_MAX_COUNT 1000 #define SET_APPS_FEATURE(driver, n) ((driver->apps_feature) |= (1 << (n))) struct diag_md_hdlc_reset_work { int pid; struct work_struct work; }; static int timestamp_switch; module_param(timestamp_switch, int, 0644); int wrap_enabled; uint16_t wrap_count; static struct diag_hdlc_decode_type *hdlc_decode; #define DIAG_NUM_COMMON_CMD 1 static uint8_t common_cmds[DIAG_NUM_COMMON_CMD] = { DIAG_CMD_LOG_ON_DMND }; static uint8_t hdlc_timer_in_progress; /* 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) { switch (socinfo_get_msm_cpu()) { case MSM_CPU_8960: case MSM_CPU_8960AB: return AO8960_TOOLS_ID; case MSM_CPU_8064: return APQ8064_TOOLS_ID; case MSM_CPU_8974: return MSM8974_TOOLS_ID; case MSM_CPU_8084: return APQ8084_TOOLS_ID; case MSM_CPU_8916: return MSM8916_TOOLS_ID; case MSM_CPU_8996: return MSM_8996_TOOLS_ID; default: if (driver->use_device_tree) { if (machine_is_msm8974()) return MSM8974_TOOLS_ID; else return 0; } else { return 0; } } } /* * 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) { if (driver->use_device_tree) return 1; switch (socinfo_get_msm_cpu()) { case MSM_CPU_8960: case MSM_CPU_8960AB: case MSM_CPU_8064: case MSM_CPU_8974: return 1; default: return 0; } } /* * 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) { if (driver->use_device_tree) return 1; else return 0; } 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->diagfwd_cntl[PERIPHERAL_MODEM] && driver->diagfwd_cntl[PERIPHERAL_MODEM]->ch_open) && (driver->feature[PERIPHERAL_MODEM].rcvd_feature_mask)) /* * If the apps processor is not the master and the modem * is not up or we did not receive the feature masks from Modem */ return 1; else return 0; } /* * This function should be called if you feel that the logging process may * need to be woken up. For instance, if the logging mode is MEMORY_DEVICE MODE * and while trying to read data from data channel there are no buffers * available to read the data into, then this function should be called to * determine if the logging process needs to be woken up. */ void chk_logging_wakeup(void) { int i; int j; int pid = 0; int proc; for (proc = 0; proc < NUM_DIAG_MD_DEV; proc++) { for (j = 0; j < NUM_MD_SESSIONS; j++) { if (!driver->md_session_map[proc][j]) continue; pid = driver->md_session_map[proc][j]->pid; /* Find the index of the logging process */ for (i = 0; i < driver->num_clients; i++) { if (driver->client_map[i].pid != pid) continue; if (driver->data_ready[i] & USER_SPACE_DATA_TYPE) continue; /* * At very high logging rates a race condition * can occur where the buffers containing the * data from a channel are all in use, but the * data_ready flag is cleared. In this case, * the buffers never have their data * read/logged. Detect and remedy this * situation. */ driver->data_ready[i] |= USER_SPACE_DATA_TYPE; atomic_inc(&driver->data_ready_notif[i]); pr_debug("diag: Force wakeup of logging process\n"); wake_up_interruptible(&driver->wait_q); break; } /* * Diag Memory Device is in normal. Check only for the * first index as all the indices point to the same * session structure. */ if ((driver->md_session_mask[proc] == DIAG_CON_ALL) && (j == 0)) break; } } } static void pack_rsp_and_send(unsigned char *buf, int len, int pid) { int err; int retry_count = 0, i, rsp_ctxt; uint32_t write_len = 0; unsigned long flags; unsigned char *rsp_ptr = driver->encoded_rsp_buf; struct diag_pkt_frame_t header; struct diag_md_session_t *session_info = NULL, *info = NULL; if (!rsp_ptr || !buf) return; if (len > DIAG_MAX_RSP_SIZE || len < 0) { pr_err("diag: In %s, invalid len %d, permissible len %d\n", __func__, len, DIAG_MAX_RSP_SIZE); return; } mutex_lock(&driver->md_session_lock); session_info = diag_md_session_get_pid(pid); info = (session_info) ? session_info : diag_md_session_get_peripheral(DIAG_LOCAL_PROC, APPS_DATA); /* * Explicitly check for the Peripheral Modem here * is necessary till a way to identify a peripheral * if its supporting qshrink4 feature. */ if (info && info->peripheral_mask[DIAG_LOCAL_PROC]) { for (i = 0; i < NUM_MD_SESSIONS; i++) { if (info->peripheral_mask[DIAG_LOCAL_PROC] & (1 << i)) break; } rsp_ctxt = SET_BUF_CTXT(i, TYPE_CMD, TYPE_CMD); } else rsp_ctxt = driver->rsp_buf_ctxt; mutex_unlock(&driver->md_session_lock); /* * Keep trying till we get the buffer back. It should probably * take one or two iterations. When this loops till RETRY_MAX_COUNT, it * means we did not get a write complete for the previous * response. */ while (retry_count < RETRY_MAX_COUNT) { if (!driver->rsp_buf_busy) break; /* * Wait for sometime and try again. The value 10000 was chosen * empirically as an optimum value for USB to complete a write */ usleep_range(10000, 10100); retry_count++; /* * There can be a race conditon that clears the data ready flag * for responses. Make sure we don't miss previous wakeups for * draining responses when we are in Memory Device Mode. */ if (driver->logging_mode[DIAG_LOCAL_PROC] == DIAG_MEMORY_DEVICE_MODE || driver->logging_mode[DIAG_LOCAL_PROC] == DIAG_MULTI_MODE) { mutex_lock(&driver->md_session_lock); chk_logging_wakeup(); mutex_unlock(&driver->md_session_lock); } } if (driver->rsp_buf_busy) { pr_err("diag: unable to get hold of response buffer\n"); return; } driver->rsp_buf_busy = 1; header.start = CONTROL_CHAR; header.version = 1; header.length = len; memcpy(rsp_ptr, &header, sizeof(header)); write_len += sizeof(header); memcpy(rsp_ptr + write_len, buf, len); write_len += len; *(uint8_t *)(rsp_ptr + write_len) = CONTROL_CHAR; write_len += sizeof(uint8_t); err = diag_mux_write(DIAG_LOCAL_PROC, rsp_ptr, write_len, rsp_ctxt); if (err) { pr_err("diag: In %s, unable to write to mux, err: %d\n", __func__, err); spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags); driver->rsp_buf_busy = 0; spin_unlock_irqrestore(&driver->rsp_buf_busy_lock, flags); } } static void encode_rsp_and_send(unsigned char *buf, int len, int pid) { struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 }; struct diag_hdlc_dest_type enc = { NULL, NULL, 0 }; unsigned char *rsp_ptr = driver->encoded_rsp_buf; int err, i, rsp_ctxt, retry_count = 0; unsigned long flags; struct diag_md_session_t *session_info = NULL, *info = NULL; if (!rsp_ptr || !buf) return; if (len > DIAG_MAX_RSP_SIZE || len < 0) { pr_err("diag: In %s, invalid len %d, permissible len %d\n", __func__, len, DIAG_MAX_RSP_SIZE); return; } mutex_lock(&driver->md_session_lock); session_info = diag_md_session_get_pid(pid); info = (session_info) ? session_info : diag_md_session_get_peripheral(DIAG_LOCAL_PROC, APPS_DATA); /* * Explicitly check for the Peripheral Modem here * is necessary till a way to identify a peripheral * if its supporting qshrink4 feature. */ if (info && info->peripheral_mask[DIAG_LOCAL_PROC]) { for (i = 0; i < NUM_MD_SESSIONS; i++) { if (info->peripheral_mask[DIAG_LOCAL_PROC] & (1 << i)) break; } rsp_ctxt = SET_BUF_CTXT(i, TYPE_CMD, TYPE_CMD); } else rsp_ctxt = driver->rsp_buf_ctxt; mutex_unlock(&driver->md_session_lock); /* * Keep trying till we get the buffer back. It should probably * take one or two iterations. When this loops till RETRY_MAX_COUNT, it * means we did not get a write complete for the previous * response. */ while (retry_count < RETRY_MAX_COUNT) { if (!driver->rsp_buf_busy) break; /* * Wait for sometime and try again. The value 10000 was chosen * empirically as an optimum value for USB to complete a write */ usleep_range(10000, 10100); retry_count++; /* * There can be a race conditon that clears the data ready flag * for responses. Make sure we don't miss previous wakeups for * draining responses when we are in Memory Device Mode. */ if (driver->logging_mode[DIAG_LOCAL_PROC] == DIAG_MEMORY_DEVICE_MODE || driver->logging_mode[DIAG_LOCAL_PROC] == DIAG_MULTI_MODE) { mutex_lock(&driver->md_session_lock); chk_logging_wakeup(); mutex_unlock(&driver->md_session_lock); } } if (driver->rsp_buf_busy) { pr_err("diag: unable to get hold of response buffer\n"); return; } spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags); driver->rsp_buf_busy = 1; spin_unlock_irqrestore(&driver->rsp_buf_busy_lock, flags); send.state = DIAG_STATE_START; send.pkt = buf; send.last = (void *)(buf + len - 1); send.terminate = 1; enc.dest = rsp_ptr; enc.dest_last = (void *)(rsp_ptr + DIAG_MAX_HDLC_BUF_SIZE - 1); diag_hdlc_encode(&send, &enc); driver->encoded_rsp_len = (int)(enc.dest - (void *)rsp_ptr); err = diag_mux_write(DIAG_LOCAL_PROC, rsp_ptr, driver->encoded_rsp_len, rsp_ctxt); if (err) { pr_err("diag: In %s, Unable to write to device, err: %d\n", __func__, err); spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags); driver->rsp_buf_busy = 0; spin_unlock_irqrestore(&driver->rsp_buf_busy_lock, flags); } memset(buf, '\0', DIAG_MAX_RSP_SIZE); } static void diag_send_rsp(unsigned char *buf, int len, int pid) { struct diag_md_session_t *session_info = NULL, *info = NULL; uint8_t hdlc_disabled; mutex_lock(&driver->md_session_lock); info = diag_md_session_get_pid(pid); session_info = (info) ? info : diag_md_session_get_peripheral(DIAG_LOCAL_PROC, APPS_DATA); if (session_info) hdlc_disabled = session_info->hdlc_disabled; else hdlc_disabled = driver->hdlc_disabled; mutex_unlock(&driver->md_session_lock); if (hdlc_disabled) pack_rsp_and_send(buf, len, pid); else encode_rsp_and_send(buf, len, pid); } void diag_update_pkt_buffer(unsigned char *buf, uint32_t len, int type) { unsigned char *ptr = NULL; unsigned char *temp = buf; int *in_busy = NULL; uint32_t *length = NULL; uint32_t max_len = 0; if (!buf || len == 0) { pr_err("diag: In %s, Invalid ptr %pK and length %d\n", __func__, buf, len); return; } switch (type) { case PKT_TYPE: ptr = driver->apps_req_buf; length = &driver->apps_req_buf_len; max_len = DIAG_MAX_REQ_SIZE; in_busy = &driver->in_busy_pktdata; break; case DCI_PKT_TYPE: ptr = driver->dci_pkt_buf; length = &driver->dci_pkt_length; max_len = DCI_BUF_SIZE; in_busy = &driver->in_busy_dcipktdata; break; default: pr_err("diag: Invalid type %d in %s\n", type, __func__); return; } mutex_lock(&driver->diagchar_mutex); if (CHK_OVERFLOW(ptr, ptr, ptr + max_len, len)) { memcpy(ptr, temp, len); *length = len; *in_busy = 1; } else { pr_alert("diag: In %s, no space for response packet, len: %d, type: %d\n", __func__, len, type); } 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)) { driver->data_ready[i] |= type; atomic_inc(&driver->data_ready_notif[i]); } wake_up_interruptible(&driver->wait_q); mutex_unlock(&driver->diagchar_mutex); } void diag_update_md_clients_proc(unsigned int proc, unsigned int type) { int i, j; for (i = 0; i < NUM_MD_SESSIONS; i++) { if (driver->md_session_map[proc][i]) { for (j = 0; j < driver->num_clients; j++) { if (driver->client_map[j].pid != 0 && driver->client_map[j].pid == driver->md_session_map[proc][i]->pid) { if (!(driver->data_ready[j] & type)) { driver->data_ready[j] |= type; atomic_inc( &driver->data_ready_notif[j]); } break; } } } } } void diag_update_md_clients(unsigned int type) { int proc; mutex_lock(&driver->diagchar_mutex); mutex_lock(&driver->md_session_lock); for (proc = 0; proc < NUM_DIAG_MD_DEV; proc++) diag_update_md_clients_proc(proc, type); mutex_unlock(&driver->md_session_lock); wake_up_interruptible(&driver->wait_q); mutex_unlock(&driver->diagchar_mutex); } void diag_update_sleeping_process(int process_id, int data_type) { int i; mutex_lock(&driver->diagchar_mutex); for (i = 0; i < driver->num_clients; i++) if (driver->client_map[i].pid == process_id) { if (!(driver->data_ready[i] & data_type)) { driver->data_ready[i] |= data_type; atomic_inc(&driver->data_ready_notif[i]); } break; } wake_up_interruptible(&driver->wait_q); mutex_unlock(&driver->diagchar_mutex); } static int diag_send_data(struct diag_cmd_reg_t *entry, unsigned char *buf, int len) { if (!entry) return -EIO; if (entry->proc == APPS_DATA) { diag_update_pkt_buffer(buf, len, PKT_TYPE); diag_update_sleeping_process(entry->pid, PKT_TYPE); return 0; } return diagfwd_write(entry->proc, TYPE_CMD, buf, len); } void diag_process_stm_mask(uint8_t cmd, uint8_t data_mask, int data_type) { int status = 0; if (data_type >= PERIPHERAL_MODEM && data_type < NUM_PERIPHERALS) { if (driver->feature[data_type].stm_support) { status = diag_send_stm_state(data_type, cmd); if (status == 0) driver->stm_state[data_type] = cmd; } driver->stm_state_requested[data_type] = cmd; } else if (data_type == APPS_DATA) { driver->stm_state[data_type] = cmd; driver->stm_state_requested[data_type] = cmd; } } static uint8_t diag_get_stm_peripheral_mask(void) { uint8_t rsp_supported = 0; if (driver->feature[PERIPHERAL_MODEM].stm_support) rsp_supported |= DIAG_STM_MODEM; if (driver->feature[PERIPHERAL_LPASS].stm_support) rsp_supported |= DIAG_STM_LPASS; if (driver->feature[PERIPHERAL_WCNSS].stm_support) rsp_supported |= DIAG_STM_WCNSS; if (driver->feature[PERIPHERAL_SENSORS].stm_support) rsp_supported |= DIAG_STM_SENSORS; if (driver->feature[PERIPHERAL_CDSP].stm_support) rsp_supported |= DIAG_STM_CDSP; if (driver->feature[PERIPHERAL_NPU].stm_support) rsp_supported |= DIAG_STM_NPU; rsp_supported |= DIAG_STM_APPS; return rsp_supported; } static uint8_t diag_get_stm_state_for_peripherals(void) { uint8_t rsp_status = 0; if (driver->stm_state[PERIPHERAL_MODEM]) rsp_status |= DIAG_STM_MODEM; if (driver->stm_state[PERIPHERAL_LPASS]) rsp_status |= DIAG_STM_LPASS; if (driver->stm_state[PERIPHERAL_WCNSS]) rsp_status |= DIAG_STM_WCNSS; if (driver->stm_state[PERIPHERAL_SENSORS]) rsp_status |= DIAG_STM_SENSORS; if (driver->stm_state[PERIPHERAL_CDSP]) rsp_status |= DIAG_STM_CDSP; if (driver->stm_state[PERIPHERAL_NPU]) rsp_status |= DIAG_STM_NPU; if (driver->stm_state[APPS_DATA]) rsp_status |= DIAG_STM_APPS; return rsp_status; } static void diag_process_stm_mask_for_peripherals(uint8_t cmd, uint8_t mask) { if (mask & DIAG_STM_MODEM) diag_process_stm_mask(cmd, DIAG_STM_MODEM, PERIPHERAL_MODEM); if (mask & DIAG_STM_LPASS) diag_process_stm_mask(cmd, DIAG_STM_LPASS, PERIPHERAL_LPASS); if (mask & DIAG_STM_WCNSS) diag_process_stm_mask(cmd, DIAG_STM_WCNSS, PERIPHERAL_WCNSS); if (mask & DIAG_STM_SENSORS) diag_process_stm_mask(cmd, DIAG_STM_SENSORS, PERIPHERAL_SENSORS); if (mask & DIAG_STM_CDSP) diag_process_stm_mask(cmd, DIAG_STM_CDSP, PERIPHERAL_CDSP); if (mask & DIAG_STM_NPU) diag_process_stm_mask(cmd, DIAG_STM_NPU, PERIPHERAL_NPU); if (mask & DIAG_STM_APPS) diag_process_stm_mask(cmd, DIAG_STM_APPS, APPS_DATA); } int diag_process_stm_cmd(unsigned char *buf, unsigned char *dest_buf) { uint8_t version, mask, cmd; uint8_t rsp_supported = 0; uint8_t rsp_status = 0; int i; if (!buf || !dest_buf) { pr_err("diag: Invalid pointers buf: %pK, dest_buf %pK in %s\n", buf, dest_buf, __func__); return -EIO; } version = *(buf + STM_CMD_VERSION_OFFSET); mask = *(buf + STM_CMD_MASK_OFFSET); cmd = *(buf + STM_CMD_DATA_OFFSET); /* * Check if command is valid. If the command is asking for * status, then the processor mask field is to be ignored. */ if ((version != 2) || (cmd > STM_AUTO_QUERY) || ((cmd != STATUS_STM && cmd != STM_AUTO_QUERY) && ((mask == 0) || (0 != (mask >> (NUM_PERIPHERALS + 1)))))) { /* Command is invalid. Send bad param message response */ dest_buf[0] = BAD_PARAM_RESPONSE_MESSAGE; for (i = 0; i < STM_CMD_NUM_BYTES; i++) dest_buf[i+1] = *(buf + i); return STM_CMD_NUM_BYTES+1; } else if (cmd != STATUS_STM && cmd != STM_AUTO_QUERY) diag_process_stm_mask_for_peripherals(cmd, mask); for (i = 0; i < STM_CMD_NUM_BYTES; i++) dest_buf[i] = *(buf + i); /* Set mask denoting which peripherals support STM */ rsp_supported = diag_get_stm_peripheral_mask(); /* Set mask denoting STM state/status for each peripheral/APSS */ rsp_status = diag_get_stm_state_for_peripherals(); dest_buf[STM_RSP_SUPPORTED_INDEX] = rsp_supported; dest_buf[STM_RSP_STATUS_INDEX] = rsp_status; return STM_RSP_NUM_BYTES; } int diag_process_time_sync_query_cmd(unsigned char *src_buf, int src_len, unsigned char *dest_buf, int dest_len) { int write_len = 0; struct diag_cmd_time_sync_query_req_t *req = NULL; struct diag_cmd_time_sync_query_rsp_t rsp; if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0 || src_len < sizeof(struct diag_cmd_time_sync_query_req_t)) { pr_err("diag: Invalid input in %s, src_buf: %pK, src_len: %d, dest_buf: %pK, dest_len: %d\n", __func__, src_buf, src_len, dest_buf, dest_len); return -EINVAL; } req = (struct diag_cmd_time_sync_query_req_t *)src_buf; rsp.header.cmd_code = req->header.cmd_code; rsp.header.subsys_id = req->header.subsys_id; rsp.header.subsys_cmd_code = req->header.subsys_cmd_code; rsp.version = req->version; rsp.time_api = driver->uses_time_api; memcpy(dest_buf, &rsp, sizeof(rsp)); write_len = sizeof(rsp); return write_len; } int diag_process_diag_id_query_cmd(unsigned char *src_buf, int src_len, unsigned char *dest_buf, int dest_len) { int write_len = 0; struct diag_cmd_diag_id_query_req_t *req = NULL; struct diag_cmd_diag_id_query_rsp_t rsp; struct list_head *start; struct list_head *temp; struct diag_id_tbl_t *item = NULL; int rsp_len = 0; int num_entries = 0; uint8_t process_name_len = 0; if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0 || src_len < sizeof(struct diag_cmd_diag_id_query_req_t)) { pr_err("diag: Invalid input in %s, src_buf:%pK, src_len:%d, dest_buf:%pK, dest_len:%d\n", __func__, src_buf, src_len, dest_buf, dest_len); return -EINVAL; } req = (struct diag_cmd_diag_id_query_req_t *) src_buf; rsp.header.cmd_code = req->header.cmd_code; rsp.header.subsys_id = req->header.subsys_id; rsp.header.subsys_cmd_code = req->header.subsys_cmd_code; rsp.version = req->version; rsp.entry.process_name = NULL; rsp.entry.len = 0; rsp.entry.diag_id = 0; write_len = sizeof(rsp.header) + sizeof(rsp.version) + sizeof(rsp.num_entries); rsp_len = write_len; mutex_lock(&driver->diag_id_mutex); list_for_each_safe(start, temp, &driver->diag_id_list) { item = list_entry(start, struct diag_id_tbl_t, link); memcpy(dest_buf + write_len, &item->diag_id, sizeof(item->diag_id)); write_len = write_len + sizeof(item->diag_id); process_name_len = strlen(item->process_name) + 1; memcpy(dest_buf + write_len, &process_name_len, sizeof(process_name_len)); write_len = write_len + sizeof(process_name_len); memcpy(dest_buf + write_len, item->process_name, strlen(item->process_name) + 1); write_len = write_len + strlen(item->process_name) + 1; num_entries++; } mutex_unlock(&driver->diag_id_mutex); rsp.num_entries = num_entries; memcpy(dest_buf, &rsp, rsp_len); return write_len; } int diag_process_time_sync_switch_cmd(unsigned char *src_buf, int src_len, unsigned char *dest_buf, int dest_len) { uint8_t peripheral, status = 0; struct diag_cmd_time_sync_switch_req_t *req = NULL; struct diag_cmd_time_sync_switch_rsp_t rsp; struct diag_ctrl_msg_time_sync time_sync_msg; int msg_size = sizeof(struct diag_ctrl_msg_time_sync); int err = 0, write_len = 0; if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0 || src_len < sizeof(struct diag_cmd_time_sync_switch_req_t)) { pr_err("diag: Invalid input in %s, src_buf: %pK, src_len: %d, dest_buf: %pK, dest_len: %d\n", __func__, src_buf, src_len, dest_buf, dest_len); return -EINVAL; } req = (struct diag_cmd_time_sync_switch_req_t *)src_buf; rsp.header.cmd_code = req->header.cmd_code; rsp.header.subsys_id = req->header.subsys_id; rsp.header.subsys_cmd_code = req->header.subsys_cmd_code; rsp.version = req->version; rsp.time_api = req->time_api; if ((req->version > 1) || (req->time_api > 1) || (req->persist_time > 0)) { dest_buf[0] = BAD_PARAM_RESPONSE_MESSAGE; rsp.time_api_status = 0; rsp.persist_time_status = PERSIST_TIME_NOT_SUPPORTED; memcpy(dest_buf + 1, &rsp, sizeof(rsp)); write_len = sizeof(rsp) + 1; timestamp_switch = 0; return write_len; } time_sync_msg.ctrl_pkt_id = DIAG_CTRL_MSG_TIME_SYNC_PKT; time_sync_msg.ctrl_pkt_data_len = 5; time_sync_msg.version = 1; time_sync_msg.time_api = req->time_api; for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) { err = diagfwd_write(peripheral, TYPE_CNTL, &time_sync_msg, msg_size); if (err && err != -ENODEV) { pr_err("diag: In %s, unable to write to peripheral: %d, type: %d, len: %d, err: %d\n", __func__, peripheral, TYPE_CNTL, msg_size, err); status |= (1 << peripheral); } } driver->time_sync_enabled = 1; driver->uses_time_api = req->time_api; switch (req->time_api) { case 0: timestamp_switch = 0; break; case 1: timestamp_switch = 1; break; default: timestamp_switch = 0; break; } rsp.time_api_status = status; rsp.persist_time_status = PERSIST_TIME_NOT_SUPPORTED; memcpy(dest_buf, &rsp, sizeof(rsp)); write_len = sizeof(rsp); return write_len; } int diag_cmd_log_on_demand(unsigned char *src_buf, int src_len, unsigned char *dest_buf, int dest_len) { int write_len = 0; struct diag_log_on_demand_rsp_t header; if (!driver->diagfwd_cntl[PERIPHERAL_MODEM] || !driver->diagfwd_cntl[PERIPHERAL_MODEM]->ch_open || !driver->log_on_demand_support) return 0; if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) { pr_err("diag: Invalid input in %s, src_buf: %pK, src_len: %d, dest_buf: %pK, dest_len: %d\n", __func__, src_buf, src_len, dest_buf, dest_len); return -EINVAL; } header.cmd_code = DIAG_CMD_LOG_ON_DMND; header.log_code = *(uint16_t *)(src_buf + 1); header.status = 1; memcpy(dest_buf, &header, sizeof(struct diag_log_on_demand_rsp_t)); write_len += sizeof(struct diag_log_on_demand_rsp_t); return write_len; } int diag_cmd_get_mobile_id(unsigned char *src_buf, int src_len, unsigned char *dest_buf, int dest_len) { int write_len = 0; struct diag_pkt_header_t *header = NULL; struct diag_cmd_ext_mobile_rsp_t rsp; if (!src_buf || src_len != sizeof(*header) || !dest_buf || dest_len < sizeof(rsp)) return -EIO; header = (struct diag_pkt_header_t *)src_buf; rsp.header.cmd_code = header->cmd_code; rsp.header.subsys_id = header->subsys_id; rsp.header.subsys_cmd_code = header->subsys_cmd_code; rsp.version = 2; rsp.padding[0] = 0; rsp.padding[1] = 0; rsp.padding[2] = 0; rsp.family = 0; rsp.chip_id = (uint32_t)socinfo_get_id(); memcpy(dest_buf, &rsp, sizeof(rsp)); write_len += sizeof(rsp); return write_len; } int diag_check_common_cmd(struct diag_pkt_header_t *header) { int i; if (!header) return -EIO; for (i = 0; i < DIAG_NUM_COMMON_CMD; i++) { if (header->cmd_code == common_cmds[i]) return 1; } return 0; } static int diag_cmd_chk_stats(unsigned char *src_buf, int src_len, unsigned char *dest_buf, int dest_len) { int payload = 0; int write_len = 0; struct diag_pkt_header_t *header = NULL; struct diag_cmd_stats_rsp_t rsp; if (!src_buf || src_len < sizeof(struct diag_pkt_header_t) || !dest_buf || dest_len < sizeof(rsp)) return -EINVAL; header = (struct diag_pkt_header_t *)src_buf; if (header->cmd_code != DIAG_CMD_DIAG_SUBSYS || header->subsys_id != DIAG_SS_DIAG) return -EINVAL; switch (header->subsys_cmd_code) { case DIAG_CMD_OP_GET_MSG_ALLOC: payload = driver->msg_stats.alloc_count; break; case DIAG_CMD_OP_GET_MSG_DROP: payload = driver->msg_stats.drop_count; break; case DIAG_CMD_OP_RESET_MSG_STATS: diag_record_stats(DATA_TYPE_F3, PKT_RESET); break; case DIAG_CMD_OP_GET_LOG_ALLOC: payload = driver->log_stats.alloc_count; break; case DIAG_CMD_OP_GET_LOG_DROP: payload = driver->log_stats.drop_count; break; case DIAG_CMD_OP_RESET_LOG_STATS: diag_record_stats(DATA_TYPE_LOG, PKT_RESET); break; case DIAG_CMD_OP_GET_EVENT_ALLOC: payload = driver->event_stats.alloc_count; break; case DIAG_CMD_OP_GET_EVENT_DROP: payload = driver->event_stats.drop_count; break; case DIAG_CMD_OP_RESET_EVENT_STATS: diag_record_stats(DATA_TYPE_EVENT, PKT_RESET); break; default: return -EINVAL; } memcpy(&rsp.header, header, sizeof(struct diag_pkt_header_t)); rsp.payload = payload; write_len = sizeof(rsp); memcpy(dest_buf, &rsp, sizeof(rsp)); return write_len; } static int diag_cmd_disable_hdlc(unsigned char *src_buf, int src_len, unsigned char *dest_buf, int dest_len) { struct diag_pkt_header_t *header = NULL; struct diag_cmd_hdlc_disable_rsp_t rsp; int write_len = 0; if (!src_buf || src_len < sizeof(*header) || !dest_buf || dest_len < sizeof(rsp)) { return -EIO; } header = (struct diag_pkt_header_t *)src_buf; if (header->cmd_code != DIAG_CMD_DIAG_SUBSYS || header->subsys_id != DIAG_SS_DIAG || header->subsys_cmd_code != DIAG_CMD_OP_HDLC_DISABLE) { return -EINVAL; } memcpy(&rsp.header, header, sizeof(struct diag_pkt_header_t)); rsp.framing_version = 1; rsp.result = 0; write_len = sizeof(rsp); memcpy(dest_buf, &rsp, sizeof(rsp)); return write_len; } int diag_cmd_feature_query(unsigned char *src_buf, int src_len, unsigned char *dest_buf, int dest_len) { int write_len = 0; struct diag_pkt_header_t *header = NULL; struct diag_cmd_feature_query_rsp_t rsp; if (!src_buf || !dest_buf || src_len <= sizeof(struct diag_pkt_header_t) || dest_len <= 0 || dest_len > DIAG_MAX_RSP_SIZE) { pr_err("diag: Feature query, invalid input src_buf: %pK, src_len: %d, dest_buf: %pK, dest_len: %d\n", src_buf, src_len, dest_buf, dest_len); return -EINVAL; } header = (struct diag_pkt_header_t *)src_buf; rsp.header.cmd_code = header->cmd_code; rsp.header.subsys_id = header->subsys_id; rsp.header.subsys_cmd_code = header->subsys_cmd_code; rsp.version = 1; rsp.feature_len = sizeof(driver->apps_feature); if (dest_len < (sizeof(rsp) + sizeof(driver->apps_feature))) return -EINVAL; memcpy(dest_buf, &rsp, sizeof(rsp)); memcpy(dest_buf + sizeof(rsp), &(driver->apps_feature), sizeof(driver->apps_feature)); write_len = sizeof(rsp) + sizeof(driver->apps_feature); return write_len; } static void diag_init_apps_feature(void) { driver->apps_feature = 0; SET_APPS_FEATURE(driver, F_DIAG_EVENT_REPORT); SET_APPS_FEATURE(driver, F_DIAG_HW_ACCELERATION); SET_APPS_FEATURE(driver, F_DIAG_MULTI_SIM_MASK); } void diag_send_error_rsp(unsigned char *buf, int len, int pid) { /* -1 to accommodate the first byte 0x13 */ if (len > (DIAG_MAX_RSP_SIZE - 1)) { pr_err("diag: cannot send err rsp, huge length: %d\n", len); return; } *(uint8_t *)driver->apps_rsp_buf = DIAG_CMD_ERROR; memcpy((driver->apps_rsp_buf + sizeof(uint8_t)), buf, len); diag_send_rsp(driver->apps_rsp_buf, len + 1, pid); } static int diag_process_ss_diag_cmd(unsigned char *buf, int len, int pid) { int ret = 0, write_len = 0, i; if (len < (2 * sizeof(uint8_t) + sizeof(uint16_t))) return -EINVAL; switch (*(uint16_t *)(buf + DIAG_SS_CMD_OFFSET)) { case DIAG_DIAG_MAX_PKT_SZ: /* * Check for the command/respond msg * for the maximum packet length */ for (i = 0; i < 4; i++) *(driver->apps_rsp_buf+i) = *(buf+i); *(uint32_t *)(driver->apps_rsp_buf+4) = DIAG_MAX_REQ_SIZE; diag_send_rsp(driver->apps_rsp_buf, 8, pid); ret = 0; break; case DIAG_DIAG_STM: /* Check for STM command */ len = diag_process_stm_cmd(buf, driver->apps_rsp_buf); if (len > 0) { diag_send_rsp(driver->apps_rsp_buf, len, pid); ret = 0; } else { ret = len; } break; case DIAG_GET_TIME_API: /* Check for time sync query command */ write_len = diag_process_time_sync_query_cmd(buf, len, driver->apps_rsp_buf, DIAG_MAX_RSP_SIZE); if (write_len > 0) diag_send_rsp(driver->apps_rsp_buf, write_len, pid); ret = 0; break; case DIAG_SET_TIME_API: /* Check for time sync switch command */ write_len = diag_process_time_sync_switch_cmd(buf, len, driver->apps_rsp_buf, DIAG_MAX_RSP_SIZE); if (write_len > 0) diag_send_rsp(driver->apps_rsp_buf, write_len, pid); ret = 0; break; case DIAG_GET_DIAG_ID: /* Check for diag id command */ write_len = diag_process_diag_id_query_cmd(buf, len, driver->apps_rsp_buf, DIAG_MAX_RSP_SIZE); if (write_len > 0) diag_send_rsp(driver->apps_rsp_buf, write_len, pid); ret = 0; break; case DIAG_FEATURE_QUERY: /* Check for Diag Feature Query command */ write_len = diag_cmd_feature_query(buf, len, driver->apps_rsp_buf, DIAG_MAX_RSP_SIZE); if (write_len > 0) diag_send_rsp(driver->apps_rsp_buf, write_len, pid); ret = 0; break; default: ret = -EINVAL; break; } return ret; } static int diag_process_ss_diag_params(unsigned char *buf, int len, int pid) { int ret = 0, write_len = 0, i; if (len < (2 * sizeof(uint8_t) + sizeof(uint16_t))) return -EINVAL; switch (*(uint16_t *)(buf + DIAG_SS_CMD_OFFSET)) { case DIAG_DIAG_POLL: /* Check for polling for Apps only DIAG */ 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; diag_send_rsp(driver->apps_rsp_buf, 16, pid); } break; case DIAG_DEL_RSP_WRAP: /* Return the Delayed Response Wrap Status */ memcpy(driver->apps_rsp_buf, buf, 4); driver->apps_rsp_buf[4] = wrap_enabled; diag_send_rsp(driver->apps_rsp_buf, 5, pid); break; case DIAG_DEL_RSP_WRAP_CNT: /* Wrap the Delayed Rsp ID */ wrap_enabled = true; memcpy(driver->apps_rsp_buf, buf, 4); driver->apps_rsp_buf[4] = wrap_count; diag_send_rsp(driver->apps_rsp_buf, 6, pid); break; case DIAG_EXT_MOBILE_ID: /* Mobile ID Rsp */ write_len = diag_cmd_get_mobile_id(buf, len, driver->apps_rsp_buf, DIAG_MAX_RSP_SIZE); if (write_len > 0) diag_send_rsp(driver->apps_rsp_buf, write_len, pid); break; default: ret = -EINVAL; break; } return ret; } int diag_process_apps_pkt(unsigned char *buf, int len, int pid) { int i, p_mask = 0; int mask_ret, peripheral = -EINVAL; int ret = 0, write_len = 0; unsigned char *temp = NULL; struct diag_cmd_reg_entry_t entry; struct diag_cmd_reg_entry_t *temp_entry = NULL; struct diag_cmd_reg_t *reg_item = NULL; uint32_t pd_mask = 0; struct diagfwd_info *fwd_info = NULL; struct diag_md_session_t *info = NULL; if (!buf || len <= 0) return -EIO; /* Check if the command is a supported mask command */ mask_ret = diag_process_apps_masks(buf, len, pid); if (mask_ret > 0) { diag_send_rsp(driver->apps_rsp_buf, mask_ret, pid); return 0; } temp = buf; if (len >= sizeof(uint8_t)) { entry.cmd_code = (uint16_t)(*(uint8_t *)temp); DIAG_LOG(DIAG_DEBUG_CMD_INFO, "diag: received cmd_code %02x\n", entry.cmd_code); } if (len >= (2 * sizeof(uint8_t))) { temp += sizeof(uint8_t); entry.subsys_id = (uint16_t)(*(uint8_t *)temp); DIAG_LOG(DIAG_DEBUG_CMD_INFO, "diag: received subsys_id %02x\n", entry.subsys_id); } if (len == (3 * sizeof(uint8_t))) { temp += sizeof(uint8_t); entry.cmd_code_hi = (uint16_t)(*(uint8_t *)temp); entry.cmd_code_lo = (uint16_t)(*(uint8_t *)temp); DIAG_LOG(DIAG_DEBUG_CMD_INFO, "diag: received cmd_code_hi %02x\n", entry.cmd_code_hi); } else if (len >= (2 * sizeof(uint8_t)) + sizeof(uint16_t)) { temp += sizeof(uint8_t); entry.cmd_code_hi = (uint16_t)(*(uint16_t *)temp); entry.cmd_code_lo = (uint16_t)(*(uint16_t *)temp); DIAG_LOG(DIAG_DEBUG_CMD_INFO, "diag: received cmd_code_hi %02x\n", entry.cmd_code_hi); } if ((len >= sizeof(uint8_t)) && *buf == DIAG_CMD_LOG_ON_DMND && driver->log_on_demand_support && driver->feature[PERIPHERAL_MODEM].rcvd_feature_mask) { write_len = diag_cmd_log_on_demand(buf, len, driver->apps_rsp_buf, DIAG_MAX_RSP_SIZE); if (write_len > 0) diag_send_rsp(driver->apps_rsp_buf, write_len, pid); return 0; } mutex_lock(&driver->cmd_reg_mutex); temp_entry = diag_cmd_search(&entry, ALL_PROC); if (temp_entry) { reg_item = container_of(temp_entry, struct diag_cmd_reg_t, entry); mutex_lock(&driver->md_session_lock); info = diag_md_session_get_pid(pid); if (info) { p_mask = info->peripheral_mask[DIAG_LOCAL_PROC]; mutex_unlock(&driver->md_session_lock); MD_PERIPHERAL_PD_MASK(TYPE_CMD, reg_item->proc, pd_mask); if ((MD_PERIPHERAL_MASK(reg_item->proc) & p_mask) || (pd_mask & p_mask)) write_len = diag_send_data(reg_item, buf, len); } else { mutex_unlock(&driver->md_session_lock); if (MD_PERIPHERAL_MASK(reg_item->proc) & driver->logging_mask[DIAG_LOCAL_PROC]) { mutex_unlock(&driver->cmd_reg_mutex); diag_send_error_rsp(buf, len, pid); return write_len; } write_len = diag_send_data(reg_item, buf, len); } mutex_unlock(&driver->cmd_reg_mutex); return write_len; } mutex_unlock(&driver->cmd_reg_mutex); #if defined(CONFIG_DIAG_OVER_USB) if ((len >= 2 * sizeof(uint8_t)) && (*buf == DIAG_CMD_DIAG_SUBSYS) && (*(buf+1) == DIAG_SS_DIAG)) { ret = diag_process_ss_diag_cmd(buf, len, pid); if (ret == 0) return ret; } else if ((len >= sizeof(uint8_t)) && (chk_apps_master()) && (*buf == 0x3A)) { /* Check for download command */ /* send response back */ driver->apps_rsp_buf[0] = *buf; diag_send_rsp(driver->apps_rsp_buf, 1, pid); msleep(5000); /* call download API */ msm_set_restart_mode(RESTART_DLOAD); pr_crit("diag: download mode set, Rebooting SoC..\n"); kernel_restart(NULL); /* Not required, represents that command isn't sent to modem */ return 0; } else if ((len >= 2 * sizeof(uint8_t)) && (*buf == DIAG_CMD_DIAG_SUBSYS) && (*(buf+1) == DIAG_SS_PARAMS)) { ret = diag_process_ss_diag_params(buf, len, pid); if (ret == 0) return ret; } /* * If the apps processor is master and no other * processor has registered for polling command. * If modem is not up and we have not received feature * mask update from modem, in that case APPS should * respond for 0X7C command */ else if (chk_apps_master() && !(driver->polling_reg_flag) && !(driver->diagfwd_cntl[PERIPHERAL_MODEM]->ch_open) && !(driver->feature[PERIPHERAL_MODEM].rcvd_feature_mask)) { /* respond to 0x0 command */ if ((len >= sizeof(uint8_t)) && *buf == 0x00) { for (i = 0; i < 55; i++) driver->apps_rsp_buf[i] = 0; diag_send_rsp(driver->apps_rsp_buf, 55, pid); return 0; } /* respond to 0x7c command */ else if ((len >= sizeof(uint8_t)) && *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'; diag_send_rsp(driver->apps_rsp_buf, 14, pid); return 0; } } write_len = diag_cmd_chk_stats(buf, len, driver->apps_rsp_buf, DIAG_MAX_RSP_SIZE); if (write_len > 0) { diag_send_rsp(driver->apps_rsp_buf, write_len, pid); return 0; } write_len = diag_cmd_disable_hdlc(buf, len, driver->apps_rsp_buf, DIAG_MAX_RSP_SIZE); if (write_len > 0) { /* * This mutex lock is necessary since we need to drain all the * pending buffers from peripherals which may be HDLC encoded * before disabling HDLC encoding on Apps processor. */ mutex_lock(&driver->hdlc_disable_mutex); diag_send_rsp(driver->apps_rsp_buf, write_len, pid); /* * Set the value of hdlc_disabled after sending the response to * the tools. This is required since the tools is expecting a * HDLC encoded response for this request. */ pr_debug("diag: In %s, disabling HDLC encoding\n", __func__); mutex_lock(&driver->md_session_lock); info = diag_md_session_get_pid(pid); if (info) info->hdlc_disabled = 1; else driver->hdlc_disabled = 1; peripheral = diag_md_session_match_pid_peripheral(DIAG_LOCAL_PROC, pid, 0); for (i = 0; i < NUM_MD_SESSIONS; i++) { if (peripheral > 0 && info) { if (peripheral & (1 << i)) driver->p_hdlc_disabled[i] = info->hdlc_disabled; else if (!diag_md_session_get_peripheral( DIAG_LOCAL_PROC, i)) driver->p_hdlc_disabled[i] = driver->hdlc_disabled; } else { if (!diag_md_session_get_peripheral( DIAG_LOCAL_PROC, i)) driver->p_hdlc_disabled[i] = driver->hdlc_disabled; } } mutex_unlock(&driver->md_session_lock); mutex_unlock(&driver->hdlc_disable_mutex); diag_update_md_clients(HDLC_SUPPORT_TYPE); return 0; } #endif /* We have now come to the end of the function. */ if (chk_apps_only()) diag_send_error_rsp(buf, len, pid); return 0; } void diag_process_hdlc_pkt(void *data, unsigned int len, int pid) { int err = 0; int ret = 0; if (len > DIAG_MAX_HDLC_BUF_SIZE) { pr_err("diag: In %s, invalid length: %d\n", __func__, len); return; } mutex_lock(&driver->diag_hdlc_mutex); pr_debug("diag: In %s, received packet of length: %d, req_buf_len: %d\n", __func__, len, driver->hdlc_buf_len); if (driver->hdlc_buf_len >= DIAG_MAX_REQ_SIZE) { pr_err("diag: In %s, request length is more than supported len. Dropping packet.\n", __func__); goto fail; } hdlc_decode->dest_ptr = driver->hdlc_buf + driver->hdlc_buf_len; hdlc_decode->dest_size = DIAG_MAX_HDLC_BUF_SIZE - driver->hdlc_buf_len; hdlc_decode->src_ptr = data; hdlc_decode->src_size = len; hdlc_decode->src_idx = 0; hdlc_decode->dest_idx = 0; ret = diag_hdlc_decode(hdlc_decode); /* * driver->hdlc_buf is of size DIAG_MAX_HDLC_BUF_SIZE. But the decoded * packet should be within DIAG_MAX_REQ_SIZE. */ if (driver->hdlc_buf_len + hdlc_decode->dest_idx <= DIAG_MAX_REQ_SIZE) { driver->hdlc_buf_len += hdlc_decode->dest_idx; } else { pr_err_ratelimited("diag: In %s, Dropping packet. pkt_size: %d, max: %d\n", __func__, driver->hdlc_buf_len + hdlc_decode->dest_idx, DIAG_MAX_REQ_SIZE); goto fail; } if (ret == HDLC_COMPLETE) { err = crc_check(driver->hdlc_buf, driver->hdlc_buf_len); if (err) { /* CRC check failed. */ pr_err_ratelimited("diag: In %s, bad CRC. Dropping packet\n", __func__); goto fail; } driver->hdlc_buf_len -= HDLC_FOOTER_LEN; if (driver->hdlc_buf_len < 1) { pr_err_ratelimited("diag: In %s, message is too short, len: %d, dest len: %d\n", __func__, driver->hdlc_buf_len, hdlc_decode->dest_idx); goto fail; } err = diag_process_apps_pkt(driver->hdlc_buf, driver->hdlc_buf_len, pid); if (err < 0) goto fail; } else { goto end; } driver->hdlc_buf_len = 0; mutex_unlock(&driver->diag_hdlc_mutex); return; fail: /* * Tools needs to get a response in order to start its * recovery algorithm. Send an error response if the * packet is not in expected format. */ diag_send_error_rsp(driver->hdlc_buf, driver->hdlc_buf_len, pid); driver->hdlc_buf_len = 0; end: mutex_unlock(&driver->diag_hdlc_mutex); } static int diagfwd_mux_open(int id, int mode) { uint8_t i; unsigned long flags; switch (mode) { case DIAG_USB_MODE: driver->usb_connected = 1; break; case DIAG_MEMORY_DEVICE_MODE: break; default: return -EINVAL; } if (driver->rsp_buf_busy) { /* * When a client switches from callback mode to USB mode * explicitly, there can be a situation when the last response * is not drained to the user space application. Reset the * in_busy flag in this case. */ spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags); driver->rsp_buf_busy = 0; spin_unlock_irqrestore(&driver->rsp_buf_busy_lock, flags); } for (i = 0; i < NUM_PERIPHERALS; i++) { diagfwd_open(i, TYPE_DATA); diagfwd_open(i, TYPE_CMD); } queue_work(driver->diag_real_time_wq, &driver->diag_real_time_work); return 0; } static int diagfwd_mux_close(int id, int mode) { uint8_t i; switch (mode) { case DIAG_USB_MODE: driver->usb_connected = 0; break; case DIAG_MEMORY_DEVICE_MODE: break; default: return -EINVAL; } if ((driver->logging_mode[DIAG_LOCAL_PROC] == DIAG_MULTI_MODE && driver->md_session_mode[DIAG_LOCAL_PROC] == DIAG_MD_NONE) || (driver->md_session_mode[DIAG_LOCAL_PROC] == DIAG_MD_PERIPHERAL)) { /* * This case indicates that the USB is removed * but there is a client running in background * with Memory Device mode. */ } else { /* * With sysfs parameter to clear masks set, * peripheral masks are cleared on ODL exit and * USB disconnection and buffers are not marked busy. * This enables read and drop of stale packets. * * With sysfs parameter to clear masks cleared, * masks are not cleared and buffers are to be marked * busy to ensure traffic generated by peripheral * are not read */ if (!(diag_mask_param())) { for (i = 0; i < NUM_PERIPHERALS; i++) { diagfwd_close(i, TYPE_DATA); diagfwd_close(i, TYPE_CMD); } } /* Re enable HDLC encoding */ pr_debug("diag: In %s, re-enabling HDLC encoding\n", __func__); mutex_lock(&driver->hdlc_disable_mutex); if (driver->md_session_mode[DIAG_LOCAL_PROC] == DIAG_MD_NONE) { driver->hdlc_disabled = 0; /* * HDLC encoding is re-enabled when * there is logical/physical disconnection of diag * to USB. */ for (i = 0; i < NUM_MD_SESSIONS; i++) driver->p_hdlc_disabled[i] = driver->hdlc_disabled; } mutex_unlock(&driver->hdlc_disable_mutex); queue_work(driver->diag_wq, &(driver->update_user_clients)); } queue_work(driver->diag_real_time_wq, &driver->diag_real_time_work); return 0; } static uint8_t hdlc_reset; static void hdlc_reset_timer_start(int pid) { struct diag_md_session_t *info = NULL; mutex_lock(&driver->md_session_lock); info = diag_md_session_get_pid(pid); if (!hdlc_timer_in_progress) { hdlc_timer_in_progress = 1; if (info) mod_timer(&info->hdlc_reset_timer, jiffies + msecs_to_jiffies(200)); else mod_timer(&driver->hdlc_reset_timer, jiffies + msecs_to_jiffies(200)); } mutex_unlock(&driver->md_session_lock); } /* * diag_timer_work_fn * Queued in workqueue to protect md_session_info structure * * Update hdlc_disabled for each peripheral * which are not in any md_session_info. * */ static void diag_timer_work_fn(struct work_struct *work) { int i = 0; struct diag_md_session_t *session_info = NULL; mutex_lock(&driver->hdlc_disable_mutex); driver->hdlc_disabled = 0; mutex_lock(&driver->md_session_lock); for (i = 0; i < NUM_MD_SESSIONS; i++) { session_info = diag_md_session_get_peripheral(DIAG_LOCAL_PROC, i); if (!session_info) driver->p_hdlc_disabled[i] = driver->hdlc_disabled; } mutex_unlock(&driver->md_session_lock); mutex_unlock(&driver->hdlc_disable_mutex); } /* * diag_md_timer_work_fn * Queued in workqueue to protect md_session_info structure * * Update hdlc_disabled for each peripheral * which are in any md_session_info * */ static void diag_md_timer_work_fn(struct work_struct *work) { int peripheral = -EINVAL, i = 0; struct diag_md_session_t *session_info = NULL; struct diag_md_hdlc_reset_work *hdlc_work = container_of(work, struct diag_md_hdlc_reset_work, work); if (!hdlc_work) return; mutex_lock(&driver->hdlc_disable_mutex); mutex_lock(&driver->md_session_lock); session_info = diag_md_session_get_pid(hdlc_work->pid); if (session_info) session_info->hdlc_disabled = 0; peripheral = diag_md_session_match_pid_peripheral(DIAG_LOCAL_PROC, hdlc_work->pid, 0); if (peripheral > 0 && session_info) { for (i = 0; i < NUM_MD_SESSIONS; i++) { if (peripheral & (1 << i)) driver->p_hdlc_disabled[i] = session_info->hdlc_disabled; } } kfree(hdlc_work); mutex_unlock(&driver->md_session_lock); mutex_unlock(&driver->hdlc_disable_mutex); } static void hdlc_reset_timer_func(struct timer_list *tlist) { pr_debug("diag: In %s, re-enabling HDLC encoding\n", __func__); if (hdlc_reset) { queue_work(driver->diag_wq, &(driver->diag_hdlc_reset_work)); queue_work(driver->diag_wq, &(driver->update_user_clients)); } hdlc_timer_in_progress = 0; } void diag_md_hdlc_reset_timer_func(struct timer_list *tlist) { struct diag_md_hdlc_reset_work *hdlc_reset_work = NULL; struct diag_md_session_t *session_info = container_of(tlist, struct diag_md_session_t, hdlc_reset_timer); pr_debug("diag: In %s, re-enabling HDLC encoding\n", __func__); hdlc_reset_work = kmalloc(sizeof(*hdlc_reset_work), GFP_ATOMIC); if (!hdlc_reset_work) { DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "diag: Could not allocate hdlc_reset_work\n"); hdlc_timer_in_progress = 0; return; } if (hdlc_reset) { INIT_WORK(&hdlc_reset_work->work, diag_md_timer_work_fn); hdlc_reset_work->pid = session_info->pid; queue_work(driver->diag_wq, &(hdlc_reset_work->work)); queue_work(driver->diag_wq, &(driver->update_md_clients)); } hdlc_timer_in_progress = 0; } static void diag_hdlc_start_recovery(unsigned char *buf, int len, int pid) { int i, peripheral = -EINVAL; static uint32_t bad_byte_counter; unsigned char *start_ptr = NULL; struct diag_pkt_frame_t *actual_pkt = NULL; struct diag_md_session_t *info = NULL; hdlc_reset = 1; hdlc_reset_timer_start(pid); actual_pkt = (struct diag_pkt_frame_t *)buf; for (i = 0; i < len; i++) { if (actual_pkt->start == CONTROL_CHAR && actual_pkt->version == 1 && actual_pkt->length < len && (*(uint8_t *)(buf + sizeof(struct diag_pkt_frame_t) + actual_pkt->length) == CONTROL_CHAR)) { start_ptr = &buf[i]; break; } bad_byte_counter++; if (bad_byte_counter > (DIAG_MAX_REQ_SIZE + sizeof(struct diag_pkt_frame_t) + 1)) { bad_byte_counter = 0; pr_err("diag: In %s, re-enabling HDLC encoding\n", __func__); mutex_lock(&driver->hdlc_disable_mutex); mutex_lock(&driver->md_session_lock); info = diag_md_session_get_pid(pid); if (info) info->hdlc_disabled = 0; else driver->hdlc_disabled = 0; peripheral = diag_md_session_match_pid_peripheral( DIAG_LOCAL_PROC, pid, 0); for (i = 0; i < NUM_MD_SESSIONS; i++) { if (peripheral > 0 && info) { if (peripheral & (1 << i)) driver->p_hdlc_disabled[i] = info->hdlc_disabled; else if ( !diag_md_session_get_peripheral( DIAG_LOCAL_PROC, i)) driver->p_hdlc_disabled[i] = driver->hdlc_disabled; } else { if ( !diag_md_session_get_peripheral( DIAG_LOCAL_PROC, i)) driver->p_hdlc_disabled[i] = driver->hdlc_disabled; } } mutex_unlock(&driver->md_session_lock); mutex_unlock(&driver->hdlc_disable_mutex); diag_update_md_clients(HDLC_SUPPORT_TYPE); return; } } if (start_ptr) { /* Discard any partial packet reads */ mutex_lock(&driver->hdlc_recovery_mutex); driver->incoming_pkt.processing = 0; mutex_unlock(&driver->hdlc_recovery_mutex); diag_process_non_hdlc_pkt(start_ptr, len - i, pid); } } void diag_process_non_hdlc_pkt(unsigned char *buf, int len, int pid) { int err = 0; uint16_t pkt_len = 0; uint32_t read_bytes = 0; const uint32_t header_len = sizeof(struct diag_pkt_frame_t); struct diag_pkt_frame_t *actual_pkt = NULL; unsigned char *data_ptr = NULL; struct diag_partial_pkt_t *partial_pkt = NULL; mutex_lock(&driver->hdlc_recovery_mutex); if (!buf || len <= 0) { mutex_unlock(&driver->hdlc_recovery_mutex); return; } partial_pkt = &driver->incoming_pkt; if (!partial_pkt->processing) { mutex_unlock(&driver->hdlc_recovery_mutex); goto start; } if (partial_pkt->remaining > len) { if ((partial_pkt->read_len + len) > partial_pkt->capacity) { pr_err("diag: Invalid length %d, %d received in %s\n", partial_pkt->read_len, len, __func__); mutex_unlock(&driver->hdlc_recovery_mutex); goto end; } memcpy(partial_pkt->data + partial_pkt->read_len, buf, len); read_bytes += len; buf += read_bytes; partial_pkt->read_len += len; partial_pkt->remaining -= len; } else { if ((partial_pkt->read_len + partial_pkt->remaining) > partial_pkt->capacity) { pr_err("diag: Invalid length during partial read %d, %d received in %s\n", partial_pkt->read_len, partial_pkt->remaining, __func__); mutex_unlock(&driver->hdlc_recovery_mutex); goto end; } memcpy(partial_pkt->data + partial_pkt->read_len, buf, partial_pkt->remaining); read_bytes += partial_pkt->remaining; buf += read_bytes; partial_pkt->read_len += partial_pkt->remaining; partial_pkt->remaining = 0; } if (partial_pkt->remaining == 0) { actual_pkt = (struct diag_pkt_frame_t *)(partial_pkt->data); data_ptr = partial_pkt->data + header_len; if ((header_len + actual_pkt->length + 1) > partial_pkt->capacity) { mutex_unlock(&driver->hdlc_recovery_mutex); return; } if (*(uint8_t *)(data_ptr + actual_pkt->length) != CONTROL_CHAR) { mutex_unlock(&driver->hdlc_recovery_mutex); diag_hdlc_start_recovery(buf, len, pid); mutex_lock(&driver->hdlc_recovery_mutex); } err = diag_process_apps_pkt(data_ptr, actual_pkt->length, pid); if (err) { pr_err("diag: In %s, unable to process incoming data packet, err: %d\n", __func__, err); mutex_unlock(&driver->hdlc_recovery_mutex); goto end; } partial_pkt->read_len = 0; partial_pkt->total_len = 0; partial_pkt->processing = 0; mutex_unlock(&driver->hdlc_recovery_mutex); goto start; } mutex_unlock(&driver->hdlc_recovery_mutex); goto end; start: while (read_bytes < len) { actual_pkt = (struct diag_pkt_frame_t *)buf; pkt_len = actual_pkt->length; if (actual_pkt->start != CONTROL_CHAR) { diag_hdlc_start_recovery(buf, len, pid); diag_send_error_rsp(buf, len, pid); goto end; } mutex_lock(&driver->hdlc_recovery_mutex); if (pkt_len + header_len > partial_pkt->capacity) { pr_err("diag: In %s, incoming data is too large for the request buffer %d\n", __func__, pkt_len); mutex_unlock(&driver->hdlc_recovery_mutex); diag_hdlc_start_recovery(buf, len, pid); break; } if ((pkt_len + header_len) > (len - read_bytes)) { partial_pkt->read_len = len - read_bytes; partial_pkt->total_len = pkt_len + header_len; partial_pkt->remaining = partial_pkt->total_len - partial_pkt->read_len; partial_pkt->processing = 1; memcpy(partial_pkt->data, buf, partial_pkt->read_len); mutex_unlock(&driver->hdlc_recovery_mutex); break; } if ((header_len + actual_pkt->length + 1) > partial_pkt->capacity) { mutex_unlock(&driver->hdlc_recovery_mutex); break; } data_ptr = buf + header_len; if (*(uint8_t *)(data_ptr + actual_pkt->length) != CONTROL_CHAR) { mutex_unlock(&driver->hdlc_recovery_mutex); diag_hdlc_start_recovery(buf, len, pid); mutex_lock(&driver->hdlc_recovery_mutex); } else hdlc_reset = 0; err = diag_process_apps_pkt(data_ptr, actual_pkt->length, pid); if (err) { mutex_unlock(&driver->hdlc_recovery_mutex); break; } read_bytes += header_len + pkt_len + 1; buf += header_len + pkt_len + 1; /* advance to next pkt */ mutex_unlock(&driver->hdlc_recovery_mutex); } end: return; } static int diagfwd_mux_read_done(unsigned char *buf, int len, int ctxt) { if (!buf || len <= 0) return -EINVAL; if (!driver->hdlc_disabled) diag_process_hdlc_pkt(buf, len, 0); else diag_process_non_hdlc_pkt(buf, len, 0); diag_mux_queue_read(ctxt); return 0; } static int diagfwd_mux_write_done(unsigned char *buf, int len, int buf_ctxt, int ctxt) { unsigned long flags; int peripheral = -1, type = -1; int num = -1, hdlc_ctxt = -1; struct diag_apps_data_t *temp = NULL; if (!buf || len < 0) return -EINVAL; peripheral = GET_BUF_PERIPHERAL(buf_ctxt); type = GET_BUF_TYPE(buf_ctxt); num = GET_BUF_NUM(buf_ctxt); switch (type) { case TYPE_DATA: if (peripheral >= 0 && peripheral < NUM_PERIPHERALS) { DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "Marking buffer as free after write done p: %d, t: %d, buf_num: %d\n", peripheral, type, num); diagfwd_write_done(peripheral, type, num); diag_ws_on_copy(DIAG_WS_MUX); } else if (peripheral == APPS_DATA) { spin_lock_irqsave(&driver->diagmem_lock, flags); hdlc_ctxt = GET_HDLC_CTXT(buf_ctxt); if ((hdlc_ctxt == HDLC_CTXT) && hdlc_data.allocated) temp = &hdlc_data; else if ((hdlc_ctxt == NON_HDLC_CTXT) && non_hdlc_data.allocated) temp = &non_hdlc_data; else DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "No apps data buffer is allocated to be freed\n"); if (temp) { DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "Freeing Apps data buffer after write done hdlc_ctxt: %d, hdlc.allocated: %d, non_hdlc.allocated: %d\n", hdlc_ctxt, hdlc_data.allocated, non_hdlc_data.allocated); diagmem_free(driver, temp->buf, POOL_TYPE_HDLC); temp->buf = NULL; temp->len = 0; temp->allocated = 0; temp->flushed = 0; wake_up_interruptible(&driver->hdlc_wait_q); } spin_unlock_irqrestore(&driver->diagmem_lock, flags); } else { pr_err_ratelimited("diag: Invalid peripheral %d in %s, type: %d\n", peripheral, __func__, type); } break; case TYPE_CMD: if (peripheral >= 0 && peripheral < NUM_PERIPHERALS && num != TYPE_CMD) { DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "Marking buffer as free after write done p: %d, t: %d, buf_num: %d\n", peripheral, type, num); diagfwd_write_done(peripheral, type, num); } else if ((peripheral >= 0 && peripheral <= NUM_PERIPHERALS + NUM_UPD) && num == TYPE_CMD) { DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "Marking APPS response buffer free after write done for p: %d, t: %d, buf_num: %d\n", peripheral, type, num); spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags); driver->rsp_buf_busy = 0; driver->encoded_rsp_len = 0; spin_unlock_irqrestore(&driver->rsp_buf_busy_lock, flags); } break; default: pr_err_ratelimited("diag: Incorrect data type %d, buf_ctxt: %d in %s\n", type, buf_ctxt, __func__); break; } return 0; } static struct diag_mux_ops diagfwd_mux_ops = { .open = diagfwd_mux_open, .close = diagfwd_mux_close, .read_done = diagfwd_mux_read_done, .write_done = diagfwd_mux_write_done }; int diagfwd_init(void) { int ret; int i; wrap_enabled = 0; wrap_count = 0; driver->use_device_tree = has_device_tree(); for (i = 0; i < DIAG_NUM_PROC; i++) driver->real_time_mode[i] = 1; driver->supports_separate_cmdrsp = 1; driver->supports_apps_hdlc_encoding = 1; driver->supports_apps_header_untagging = 1; driver->supports_pd_buffering = 1; driver->supports_diagid_v2_feature_mask = 1; for (i = 0; i < NUM_PERIPHERALS; i++) driver->peripheral_untag[i] = 0; mutex_init(&driver->diag_hdlc_mutex); mutex_init(&driver->diag_cntl_mutex); mutex_init(&driver->mode_lock); driver->encoded_rsp_buf = kzalloc(DIAG_MAX_HDLC_BUF_SIZE + APF_DIAG_PADDING, GFP_KERNEL); if (!driver->encoded_rsp_buf) goto err; kmemleak_not_leak(driver->encoded_rsp_buf); hdlc_decode = kzalloc(sizeof(struct diag_hdlc_decode_type), GFP_KERNEL); if (!hdlc_decode) goto err; timer_setup(&driver->hdlc_reset_timer, hdlc_reset_timer_func, 0); kmemleak_not_leak(hdlc_decode); driver->encoded_rsp_len = 0; driver->rsp_buf_busy = 0; spin_lock_init(&driver->rsp_buf_busy_lock); driver->user_space_data_busy = 0; driver->hdlc_buf_len = 0; INIT_LIST_HEAD(&driver->cmd_reg_list); driver->cmd_reg_count = 0; mutex_init(&driver->cmd_reg_mutex); INIT_WORK(&(driver->diag_hdlc_reset_work), diag_timer_work_fn); for (i = 0; i < NUM_PERIPHERALS; i++) { driver->feature[i].separate_cmd_rsp = 0; driver->feature[i].stm_support = DISABLE_STM; driver->feature[i].rcvd_feature_mask = 0; driver->feature[i].peripheral_buffering = 0; driver->feature[i].pd_buffering = 0; driver->feature[i].encode_hdlc = 0; driver->feature[i].untag_header = DISABLE_PKT_HEADER_UNTAGGING; driver->feature[i].mask_centralization = 0; driver->feature[i].log_on_demand = 0; driver->feature[i].sent_feature_mask = 0; driver->feature[i].diag_id_support = 0; driver->feature[i].diagid_v2_feature_mask = 0; driver->feature[i].multi_sim_support = 0; } diag_init_apps_feature(); for (i = 0; i < NUM_MD_SESSIONS; i++) { driver->buffering_mode[i].peripheral = i; driver->buffering_mode[i].mode = DIAG_BUFFERING_MODE_STREAMING; driver->buffering_mode[i].high_wm_val = DEFAULT_HIGH_WM_VAL; driver->buffering_mode[i].low_wm_val = DEFAULT_LOW_WM_VAL; } for (i = 0; i < NUM_STM_PROCESSORS; i++) { driver->stm_state_requested[i] = DISABLE_STM; driver->stm_state[i] = DISABLE_STM; } if (driver->hdlc_buf == NULL) { driver->hdlc_buf = kzalloc(DIAG_MAX_HDLC_BUF_SIZE, GFP_KERNEL); if (!driver->hdlc_buf) goto err; kmemleak_not_leak(driver->hdlc_buf); } if (driver->user_space_data_buf == NULL) driver->user_space_data_buf = kzalloc(USER_SPACE_DATA, GFP_KERNEL); if (driver->user_space_data_buf == NULL) goto err; kmemleak_not_leak(driver->user_space_data_buf); if (!driver->client_map) { driver->client_map = kcalloc(driver->num_clients, sizeof(struct diag_client_map), GFP_KERNEL); if (!driver->client_map) goto err; } kmemleak_not_leak(driver->client_map); if (!driver->data_ready) { driver->data_ready = kcalloc(driver->num_clients, sizeof(int), GFP_KERNEL); if (!driver->data_ready) goto err; } kmemleak_not_leak(driver->data_ready); for (i = 0; i < THRESHOLD_CLIENT_LIMIT; i++) atomic_set(&driver->data_ready_notif[i], 0); if (driver->apps_req_buf == NULL) { driver->apps_req_buf = kzalloc(DIAG_MAX_REQ_SIZE, GFP_KERNEL); if (!driver->apps_req_buf) goto err; kmemleak_not_leak(driver->apps_req_buf); } if (driver->dci_pkt_buf == NULL) { driver->dci_pkt_buf = kzalloc(DCI_BUF_SIZE, GFP_KERNEL); if (!driver->dci_pkt_buf) goto err; kmemleak_not_leak(driver->dci_pkt_buf); } if (driver->apps_rsp_buf == NULL) { driver->apps_rsp_buf = kzalloc(DIAG_MAX_RSP_SIZE, GFP_KERNEL); if (driver->apps_rsp_buf == NULL) goto err; kmemleak_not_leak(driver->apps_rsp_buf); } driver->diag_wq = create_singlethread_workqueue("diag_wq"); if (!driver->diag_wq) goto err; ret = diag_mux_register(DIAG_LOCAL_PROC, DIAG_LOCAL_PROC, &diagfwd_mux_ops); if (ret) { pr_err("diag: Unable to register with USB, err: %d\n", ret); goto err; } return 0; err: pr_err("diag: In %s, couldn't initialize diag\n", __func__); diag_usb_exit(DIAG_USB_LOCAL); kfree(driver->encoded_rsp_buf); kfree(driver->hdlc_buf); kfree(driver->client_map); kfree(driver->data_ready); kfree(driver->apps_req_buf); kfree(driver->dci_pkt_buf); kfree(driver->apps_rsp_buf); kfree(hdlc_decode); kfree(driver->user_space_data_buf); if (driver->diag_wq) destroy_workqueue(driver->diag_wq); return -ENOMEM; } void diagfwd_exit(void) { kfree(driver->encoded_rsp_buf); kfree(driver->hdlc_buf); kfree(hdlc_decode); kfree(driver->client_map); kfree(driver->data_ready); kfree(driver->apps_req_buf); kfree(driver->dci_pkt_buf); kfree(driver->apps_rsp_buf); kfree(driver->user_space_data_buf); destroy_workqueue(driver->diag_wq); }