// SPDX-License-Identifier: GPL-2.0-only /* Copyright (c) 2012-2021, The Linux Foundation. All rights reserved. * Copyright (c) 2022, Qualcomm Innovation Center, Inc. All rights reserved. */ #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #ifdef CONFIG_DIAG_OVER_USB #include #endif #include "diagchar_hdlc.h" #include "diagmem.h" #include "diagchar.h" #include "diagfwd.h" #include "diagfwd_cntl.h" #include "diag_dci.h" #include "diag_masks.h" #include "diagfwd_bridge.h" #include "diagfwd_peripheral.h" #include "diag_ipc_logging.h" static struct timer_list dci_drain_timer; static int dci_timer_in_progress; static struct work_struct dci_data_drain_work; struct diag_dci_partial_pkt_t partial_pkt; unsigned int dci_max_reg = 100; unsigned int dci_max_clients = 10; struct mutex dci_log_mask_mutex; struct mutex dci_event_mask_mutex; /* * DCI_HANDSHAKE_RETRY_TIME: Time to wait (in microseconds) before checking the * connection status again. * * DCI_HANDSHAKE_WAIT_TIME: Timeout (in milliseconds) to check for dci * connection status */ #define DCI_HANDSHAKE_RETRY_TIME 500000 #define DCI_HANDSHAKE_WAIT_TIME 200 spinlock_t ws_lock; unsigned long ws_lock_flags; struct dci_ops_tbl_t dci_ops_tbl[NUM_DCI_PROC] = { { .ctx = 0, .send_log_mask = diag_send_dci_log_mask, .send_event_mask = diag_send_dci_event_mask, .peripheral_status = 0, .mempool = 0, }, #ifdef CONFIG_DIAGFWD_BRIDGE_CODE { .ctx = DIAGFWD_MDM_DCI, .send_log_mask = diag_send_dci_log_mask_remote, .send_event_mask = diag_send_dci_event_mask_remote, .peripheral_status = 0, .mempool = POOL_TYPE_MDM_DCI_WRITE, }, { .ctx = DIAGFWD_MDM_DCI_2, .send_log_mask = diag_send_dci_log_mask_remote, .send_event_mask = diag_send_dci_event_mask_remote, .peripheral_status = 0, .mempool = POOL_TYPE_MDM2_DCI_WRITE, } #endif }; struct dci_channel_status_t dci_channel_status[NUM_DCI_PROC] = { { .id = 0, .open = 0, .retry_count = 0 }, #ifdef CONFIG_DIAGFWD_BRIDGE_CODE { .id = DIAGFWD_MDM_DCI, .open = 0, .retry_count = 0 } #endif }; /* Number of milliseconds anticipated to process the DCI data */ #define DCI_WAKEUP_TIMEOUT 1 #define DCI_CAN_ADD_BUF_TO_LIST(buf) \ (buf && buf->data && !buf->in_busy && buf->data_len > 0) \ #ifdef CONFIG_DEBUG_FS struct diag_dci_data_info *dci_traffic; struct mutex dci_stat_mutex; void diag_dci_record_traffic(int read_bytes, uint8_t ch_type, uint8_t peripheral, uint8_t proc) { static int curr_dci_data; static unsigned long iteration; struct diag_dci_data_info *temp_data = dci_traffic; if (!temp_data) return; mutex_lock(&dci_stat_mutex); if (curr_dci_data == DIAG_DCI_DEBUG_CNT) curr_dci_data = 0; temp_data += curr_dci_data; temp_data->iteration = iteration + 1; temp_data->data_size = read_bytes; temp_data->peripheral = peripheral; temp_data->ch_type = ch_type; temp_data->proc = proc; diag_get_timestamp(temp_data->time_stamp); curr_dci_data++; iteration++; mutex_unlock(&dci_stat_mutex); } #else void diag_dci_record_traffic(int read_bytes, uint8_t ch_type, uint8_t peripheral, uint8_t proc) { } #endif static int check_peripheral_dci_support(int peripheral_id, int dci_proc_id) { int dci_peripheral_list = 0; if (dci_proc_id < 0 || dci_proc_id >= NUM_DCI_PROC) { pr_err("diag:In %s,not a supported DCI proc id\n", __func__); return 0; } if (peripheral_id < 0 || peripheral_id >= NUM_PERIPHERALS) { pr_err("diag:In %s,not a valid peripheral id\n", __func__); return 0; } dci_peripheral_list = dci_ops_tbl[dci_proc_id].peripheral_status; if (dci_peripheral_list <= 0 || dci_peripheral_list > DIAG_CON_ALL) { pr_err("diag:In %s,not a valid dci peripheral mask\n", __func__); return 0; } /* Remove APSS bit mask information */ dci_peripheral_list = dci_peripheral_list >> 1; if ((1 << peripheral_id) & (dci_peripheral_list)) return 1; else return 0; } static void create_dci_log_mask_tbl(unsigned char *mask, uint8_t dirty) { unsigned char *temp = mask; uint8_t i; if (!mask) return; /* create hard coded table for log mask with 16 categories */ for (i = 0; i < DCI_MAX_LOG_CODES; i++) { *temp = i; temp++; *temp = dirty ? 1 : 0; temp++; memset(temp, 0, DCI_MAX_ITEMS_PER_LOG_CODE); temp += DCI_MAX_ITEMS_PER_LOG_CODE; } } static void create_dci_event_mask_tbl(unsigned char *tbl_buf) { if (tbl_buf) memset(tbl_buf, 0, DCI_EVENT_MASK_SIZE); } void dci_drain_data(struct timer_list *tlist) { queue_work(driver->diag_dci_wq, &dci_data_drain_work); } static void dci_check_drain_timer(void) { if (!dci_timer_in_progress) { dci_timer_in_progress = 1; mod_timer(&dci_drain_timer, jiffies + msecs_to_jiffies(200)); } } #ifdef CONFIG_DIAGFWD_BRIDGE_CODE static void dci_handshake_work_fn(struct work_struct *work) { int err = 0; int max_retries = 5; struct dci_channel_status_t *status = container_of(work, struct dci_channel_status_t, handshake_work); if (status->open) { pr_debug("diag: In %s, remote dci channel is open, index: %d\n", __func__, status->id); return; } if (status->retry_count == max_retries) { status->retry_count = 0; pr_info("diag: dci channel connection handshake timed out, id: %d\n", status->id); err = diagfwd_bridge_close(TOKEN_TO_BRIDGE(status->id)); if (err) { pr_err("diag: In %s, unable to close dci channel id: %d, err: %d\n", __func__, status->id, err); } return; } status->retry_count++; /* * Sleep for sometime to check for the connection status again. The * value should be optimum to include a roundabout time for a small * packet to the remote processor. */ usleep_range(DCI_HANDSHAKE_RETRY_TIME, DCI_HANDSHAKE_RETRY_TIME + 100); mod_timer(&status->wait_time, jiffies + msecs_to_jiffies(DCI_HANDSHAKE_WAIT_TIME)); } static void dci_chk_handshake(struct timer_list *tlist) { int index; struct dci_channel_status_t *status = container_of(tlist, struct dci_channel_status_t, wait_time); if (!status) return; index = status->id; if (index < 0 || index >= NUM_DCI_PROC) return; } #endif static int diag_dci_init_buffer(struct diag_dci_buffer_t *buffer, int type) { if (!buffer || buffer->data) return -EINVAL; switch (type) { case DCI_BUF_PRIMARY: buffer->capacity = IN_BUF_SIZE; buffer->data = vzalloc(buffer->capacity); if (!buffer->data) return -ENOMEM; break; case DCI_BUF_SECONDARY: buffer->data = NULL; buffer->capacity = IN_BUF_SIZE; break; case DCI_BUF_CMD: buffer->capacity = DIAG_MAX_REQ_SIZE + DCI_BUF_SIZE; buffer->data = vzalloc(buffer->capacity); if (!buffer->data) return -ENOMEM; break; default: pr_err("diag: In %s, unknown type %d\n", __func__, type); return -EINVAL; } buffer->data_len = 0; buffer->in_busy = 0; buffer->buf_type = type; mutex_init(&buffer->data_mutex); return 0; } static inline int diag_dci_check_buffer(struct diag_dci_buffer_t *buf, int len) { if (!buf) return -EINVAL; /* Return 1 if the buffer is not busy and can hold new data */ if ((buf->data_len + len < buf->capacity) && !buf->in_busy) return 1; return 0; } static void dci_add_buffer_to_list(struct diag_dci_client_tbl *client, struct diag_dci_buffer_t *buf) { if (!buf || !client || !buf->data) return; if (buf->in_list || buf->data_len == 0) return; mutex_lock(&client->write_buf_mutex); list_add_tail(&buf->buf_track, &client->list_write_buf); /* * In the case of DCI, there can be multiple packets in one read. To * calculate the wakeup source reference count, we must account for each * packet in a single read. */ diag_ws_on_read(DIAG_WS_DCI, buf->data_len); mutex_lock(&buf->data_mutex); buf->in_busy = 1; buf->in_list = 1; mutex_unlock(&buf->data_mutex); mutex_unlock(&client->write_buf_mutex); } static int diag_dci_get_buffer(struct diag_dci_client_tbl *client, int data_source, int len) { struct diag_dci_buffer_t *buf_primary = NULL; struct diag_dci_buffer_t *buf_temp = NULL; struct diag_dci_buffer_t *curr = NULL; if (!client) return -EINVAL; if (len < 0 || len > IN_BUF_SIZE) return -EINVAL; curr = client->buffers[data_source].buf_curr; buf_primary = client->buffers[data_source].buf_primary; if (curr && diag_dci_check_buffer(curr, len) == 1) return 0; dci_add_buffer_to_list(client, curr); client->buffers[data_source].buf_curr = NULL; if (diag_dci_check_buffer(buf_primary, len) == 1) { client->buffers[data_source].buf_curr = buf_primary; return 0; } buf_temp = kzalloc(sizeof(struct diag_dci_buffer_t), GFP_KERNEL); if (!buf_temp) return -EIO; if (!diag_dci_init_buffer(buf_temp, DCI_BUF_SECONDARY)) { buf_temp->data = diagmem_alloc(driver, IN_BUF_SIZE, POOL_TYPE_DCI); if (!buf_temp->data) { kfree(buf_temp); buf_temp = NULL; return -ENOMEM; } client->buffers[data_source].buf_curr = buf_temp; return 0; } kfree(buf_temp); buf_temp = NULL; return -EIO; } void diag_dci_wakeup_clients(void) { struct list_head *start, *temp; struct diag_dci_client_tbl *entry = NULL; mutex_lock(&driver->dci_mutex); list_for_each_safe(start, temp, &driver->dci_client_list) { entry = list_entry(start, struct diag_dci_client_tbl, track); /* * Don't wake up the client when there is no pending buffer to * write or when it is writing to user space */ if (!list_empty(&entry->list_write_buf) && !entry->in_service) { mutex_lock(&entry->write_buf_mutex); entry->in_service = 1; mutex_unlock(&entry->write_buf_mutex); diag_update_sleeping_process(entry->client->tgid, DCI_DATA_TYPE); } } mutex_unlock(&driver->dci_mutex); } void dci_data_drain_work_fn(struct work_struct *work) { int i; struct list_head *start, *temp; struct diag_dci_client_tbl *entry = NULL; struct diag_dci_buf_peripheral_t *proc_buf = NULL; struct diag_dci_buffer_t *buf_temp = NULL; mutex_lock(&driver->dci_mutex); list_for_each_safe(start, temp, &driver->dci_client_list) { entry = list_entry(start, struct diag_dci_client_tbl, track); for (i = 0; i < entry->num_buffers; i++) { proc_buf = &entry->buffers[i]; mutex_lock(&proc_buf->buf_mutex); buf_temp = proc_buf->buf_primary; if (DCI_CAN_ADD_BUF_TO_LIST(buf_temp)) dci_add_buffer_to_list(entry, buf_temp); buf_temp = proc_buf->buf_cmd; if (DCI_CAN_ADD_BUF_TO_LIST(buf_temp)) dci_add_buffer_to_list(entry, buf_temp); buf_temp = proc_buf->buf_curr; if (DCI_CAN_ADD_BUF_TO_LIST(buf_temp)) { dci_add_buffer_to_list(entry, buf_temp); proc_buf->buf_curr = NULL; } mutex_unlock(&proc_buf->buf_mutex); } if (!list_empty(&entry->list_write_buf) && !entry->in_service) { mutex_lock(&entry->write_buf_mutex); entry->in_service = 1; mutex_unlock(&entry->write_buf_mutex); diag_update_sleeping_process(entry->client->tgid, DCI_DATA_TYPE); } } mutex_unlock(&driver->dci_mutex); dci_timer_in_progress = 0; } static int diag_process_single_dci_pkt(unsigned char *buf, int len, int data_source, int token) { uint8_t cmd_code = 0; if (!buf || len < 0) { pr_err("diag: Invalid input in %s, buf: %pK, len: %d\n", __func__, buf, len); return -EIO; } cmd_code = *(uint8_t *)buf; switch (cmd_code) { case LOG_CMD_CODE: extract_dci_log(buf, len, data_source, token, NULL); break; case EVENT_CMD_CODE: extract_dci_events(buf, len, data_source, token, NULL); break; case EXT_HDR_CMD_CODE: extract_dci_ext_pkt(buf, len, data_source, token); break; case DCI_PKT_RSP_CODE: case DCI_DELAYED_RSP_CODE: extract_dci_pkt_rsp(buf, len, data_source, token); break; case DCI_CONTROL_PKT_CODE: extract_dci_ctrl_pkt(buf, len, token); break; default: pr_err("diag: Unable to process single DCI packet, cmd_code: %d, data_source: %d\n", cmd_code, data_source); return -EINVAL; } return 0; } /* Process the data read from apps userspace client */ void diag_process_apps_dci_read_data(int data_type, void *buf, int recd_bytes) { int err = 0; if (!buf) { pr_err_ratelimited("diag: In %s, Null buf pointer\n", __func__); return; } if (data_type != DATA_TYPE_DCI_LOG && data_type != DATA_TYPE_DCI_EVENT && data_type != DCI_PKT_TYPE) { pr_err("diag: In %s, unsupported data_type: 0x%x\n", __func__, (unsigned int)data_type); return; } err = diag_process_single_dci_pkt(buf, recd_bytes, APPS_DATA, DCI_LOCAL_PROC); if (err) return; /* wake up all sleeping DCI clients which have some data */ diag_dci_wakeup_clients(); dci_check_drain_timer(); } void diag_process_remote_dci_read_data(int index, void *buf, int recd_bytes) { int read_bytes = 0, err = 0; uint16_t dci_pkt_len; struct diag_dci_header_t *header = NULL; int header_len = sizeof(struct diag_dci_header_t); int token = BRIDGE_TO_TOKEN(index); if (!buf) return; diag_dci_record_traffic(recd_bytes, 0, 0, token); if (!partial_pkt.processing) goto start; if (partial_pkt.remaining > recd_bytes) { if ((partial_pkt.read_len + recd_bytes) > (MAX_DCI_PACKET_SZ)) { pr_err("diag: Invalid length %d, %d received in %s\n", partial_pkt.read_len, recd_bytes, __func__); goto end; } memcpy(partial_pkt.data + partial_pkt.read_len, buf, recd_bytes); read_bytes += recd_bytes; buf += read_bytes; partial_pkt.read_len += recd_bytes; partial_pkt.remaining -= recd_bytes; } else { if ((partial_pkt.read_len + partial_pkt.remaining) > (MAX_DCI_PACKET_SZ)) { pr_err("diag: Invalid length during partial read %d, %d received in %s\n", partial_pkt.read_len, partial_pkt.remaining, __func__); 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) { /* * Retrieve from the DCI control packet after the header = start * (1 byte) + version (1 byte) + length (2 bytes) */ diag_process_single_dci_pkt(partial_pkt.data + 4, partial_pkt.read_len - header_len, DCI_REMOTE_DATA, token); partial_pkt.read_len = 0; partial_pkt.total_len = 0; partial_pkt.processing = 0; goto start; } goto end; start: while (read_bytes < recd_bytes) { header = (struct diag_dci_header_t *)buf; dci_pkt_len = header->length; if (header->cmd_code != DCI_CONTROL_PKT_CODE && driver->num_dci_client == 0) { read_bytes += header_len + dci_pkt_len; buf += header_len + dci_pkt_len; continue; } if (dci_pkt_len + header_len > MAX_DCI_PACKET_SZ) { pr_err("diag: Invalid length in the dci packet field %d\n", dci_pkt_len); break; } if ((dci_pkt_len + header_len) > (recd_bytes - read_bytes)) { partial_pkt.read_len = recd_bytes - read_bytes; partial_pkt.total_len = dci_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); break; } /* * Retrieve from the DCI control packet after the header = start * (1 byte) + version (1 byte) + length (2 bytes) */ err = diag_process_single_dci_pkt(buf + 4, dci_pkt_len, DCI_REMOTE_DATA, token); if (err) break; read_bytes += header_len + dci_pkt_len; buf += header_len + dci_pkt_len; /* advance to next DCI pkt */ } end: if (err) return; /* wake up all sleeping DCI clients which have some data */ diag_dci_wakeup_clients(); dci_check_drain_timer(); } /* Process the data read from the peripheral dci channels */ void diag_dci_process_peripheral_data(struct diagfwd_info *p_info, void *buf, int recd_bytes) { int read_bytes = 0, err = 0; uint16_t dci_pkt_len; struct diag_dci_pkt_header_t *header = NULL; uint8_t recv_pkt_cmd_code; if (!buf || !p_info) return; /* * Release wakeup source when there are no more clients to * process DCI data */ if (driver->num_dci_client == 0) { diag_ws_reset(DIAG_WS_DCI); return; } diag_dci_record_traffic(recd_bytes, p_info->type, p_info->peripheral, DCI_LOCAL_PROC); while (read_bytes < recd_bytes) { header = (struct diag_dci_pkt_header_t *)buf; recv_pkt_cmd_code = header->pkt_code; dci_pkt_len = header->len; /* * Check if the length of the current packet is lesser than the * remaining bytes in the received buffer. This includes space * for the Start byte (1), Version byte (1), length bytes (2) * and End byte (1) */ if ((dci_pkt_len + 5) > (recd_bytes - read_bytes)) { pr_err("diag: Invalid length in %s, len: %d, dci_pkt_len: %d\n", __func__, recd_bytes, dci_pkt_len); diag_ws_release(); return; } /* * Retrieve from the DCI control packet after the header = start * (1 byte) + version (1 byte) + length (2 bytes) */ err = diag_process_single_dci_pkt(buf + 4, dci_pkt_len, (int)p_info->peripheral, DCI_LOCAL_PROC); if (err) { diag_ws_release(); break; } read_bytes += 5 + dci_pkt_len; buf += 5 + dci_pkt_len; /* advance to next DCI pkt */ } if (err) return; /* wake up all sleeping DCI clients which have some data */ diag_dci_wakeup_clients(); dci_check_drain_timer(); } int diag_dci_query_log_mask(struct diag_dci_client_tbl *entry, uint16_t log_code) { uint16_t item_num; uint8_t equip_id, *log_mask_ptr, byte_mask; int byte_index, offset; if (!entry) { pr_err("diag: In %s, invalid client entry\n", __func__); return 0; } equip_id = LOG_GET_EQUIP_ID(log_code); item_num = LOG_GET_ITEM_NUM(log_code); byte_index = item_num/8 + 2; byte_mask = 0x01 << (item_num % 8); offset = equip_id * 514; if (offset + byte_index >= DCI_LOG_MASK_SIZE) { pr_err("diag: In %s, invalid offset: %d, log_code: %d, byte_index: %d\n", __func__, offset, log_code, byte_index); return 0; } log_mask_ptr = entry->dci_log_mask; log_mask_ptr = log_mask_ptr + offset + byte_index; return ((*log_mask_ptr & byte_mask) == byte_mask) ? 1 : 0; } int diag_dci_query_event_mask(struct diag_dci_client_tbl *entry, uint16_t event_id) { uint8_t *event_mask_ptr, byte_mask; int byte_index, bit_index; if (!entry) { pr_err("diag: In %s, invalid client entry\n", __func__); return 0; } byte_index = event_id/8; bit_index = event_id % 8; byte_mask = 0x1 << bit_index; if (byte_index >= DCI_EVENT_MASK_SIZE) { pr_err("diag: In %s, invalid, event_id: %d, byte_index: %d\n", __func__, event_id, byte_index); return 0; } event_mask_ptr = entry->dci_event_mask; event_mask_ptr = event_mask_ptr + byte_index; return ((*event_mask_ptr & byte_mask) == byte_mask) ? 1 : 0; } static int diag_dci_filter_commands(struct diag_pkt_header_t *header, int header_len) { if (!header) return -ENOMEM; if (header_len <= 0) return -EIO; if (header_len) { switch (header->cmd_code) { case 0x7d: /* Msg Mask Configuration */ case 0x73: /* Log Mask Configuration */ case 0x81: /* Event Mask Configuration */ case 0x82: /* Event Mask Change */ case 0x60: /* Event Mask Toggle */ DIAG_LOG(DIAG_DEBUG_DCI, "diag: command not supported: %d\n", header->cmd_code); return 1; } } if (header_len >= (3*sizeof(uint8_t))) { if (header->cmd_code == 0x4b && header->subsys_id == 0x12) { switch (header->subsys_cmd_code) { case 0x60: /* Extended Event Mask Config */ case 0x61: /* Extended Msg Mask Config */ case 0x62: /* Extended Log Mask Config */ case 0x20C: /* Set current Preset ID */ case 0x20D: /* Get current Preset ID */ case 0x218: /* HDLC Disabled Command */ DIAG_LOG(DIAG_DEBUG_DCI, "diag: command not supported %d %d %d\n", header->cmd_code, header->subsys_id, header->subsys_cmd_code); return 1; } } } return 0; } static struct dci_pkt_req_entry_t *diag_register_dci_transaction(int uid, int client_id) { struct dci_pkt_req_entry_t *entry = NULL; entry = kzalloc(sizeof(struct dci_pkt_req_entry_t), GFP_KERNEL); if (!entry) return NULL; driver->dci_tag++; entry->client_id = client_id; entry->uid = uid; entry->tag = driver->dci_tag; pr_debug("diag: Registering DCI cmd req, client_id: %d, uid: %d, tag:%d\n", entry->client_id, entry->uid, entry->tag); list_add_tail(&entry->track, &driver->dci_req_list); return entry; } static struct dci_pkt_req_entry_t *diag_dci_get_request_entry(int tag) { struct list_head *start, *temp; struct dci_pkt_req_entry_t *entry = NULL; list_for_each_safe(start, temp, &driver->dci_req_list) { entry = list_entry(start, struct dci_pkt_req_entry_t, track); if (entry->tag == tag) return entry; } return NULL; } static int diag_dci_remove_req_entry(unsigned char *buf, int len, struct dci_pkt_req_entry_t *entry) { uint16_t rsp_count = 0, delayed_rsp_id = 0; if (!buf || len <= 0 || !entry) { pr_err("diag: In %s, invalid input buf: %pK, len: %d, entry: %pK\n", __func__, buf, len, entry); return -EIO; } /* It is an immediate response, delete it from the table */ if (*buf != 0x80) { list_del(&entry->track); kfree(entry); entry = NULL; return 1; } /* It is a delayed response. Check if the length is valid */ if (len < MIN_DELAYED_RSP_LEN) { pr_err("diag: Invalid delayed rsp packet length %d\n", len); return -EINVAL; } /* * If the delayed response id field (uint16_t at byte 8) is 0 then * there is only one response and we can remove the request entry. */ delayed_rsp_id = *(uint16_t *)(buf + 8); if (delayed_rsp_id == 0) { list_del(&entry->track); kfree(entry); entry = NULL; return 1; } /* * Check the response count field (uint16 at byte 10). The request * entry can be deleted it it is the last response in the sequence. * It is the last response in the sequence if the response count * is 1 or if the signed bit gets dropped. */ rsp_count = *(uint16_t *)(buf + 10); if (rsp_count > 0 && rsp_count < 0x1000) { list_del(&entry->track); kfree(entry); entry = NULL; return 1; } return 0; } static void dci_process_ctrl_status(unsigned char *buf, int len, int token) { struct diag_ctrl_dci_status *header = NULL; unsigned char *temp = buf; uint32_t read_len = 0; uint8_t i; int peripheral_mask, status; if (!buf || (len < sizeof(struct diag_ctrl_dci_status))) { pr_err("diag: In %s, invalid buf %pK or length: %d\n", __func__, buf, len); return; } if (!VALID_DCI_TOKEN(token)) { pr_err("diag: In %s, invalid DCI token %d\n", __func__, token); return; } header = (struct diag_ctrl_dci_status *)temp; temp += sizeof(struct diag_ctrl_dci_status); read_len += sizeof(struct diag_ctrl_dci_status); for (i = 0; i < header->count; i++) { if (read_len > (len - 2)) { pr_err("diag: In %s, Invalid length len: %d\n", __func__, len); return; } switch (*(uint8_t *)temp) { case PERIPHERAL_MODEM: peripheral_mask = DIAG_CON_MPSS; break; case PERIPHERAL_LPASS: peripheral_mask = DIAG_CON_LPASS; break; case PERIPHERAL_WCNSS: peripheral_mask = DIAG_CON_WCNSS; break; case PERIPHERAL_SENSORS: peripheral_mask = DIAG_CON_SENSORS; break; default: pr_err("diag: In %s, unknown peripheral, peripheral: %d\n", __func__, *(uint8_t *)temp); return; } temp += sizeof(uint8_t); read_len += sizeof(uint8_t); status = (*(uint8_t *)temp) ? DIAG_STATUS_OPEN : DIAG_STATUS_CLOSED; temp += sizeof(uint8_t); read_len += sizeof(uint8_t); diag_dci_notify_client(peripheral_mask, status, token); } } static void dci_process_ctrl_handshake_pkt(unsigned char *buf, int len, int token) { struct diag_ctrl_dci_handshake_pkt *header = NULL; unsigned char *temp = buf; int err = 0; if (!buf || (len < sizeof(struct diag_ctrl_dci_handshake_pkt))) return; if (!VALID_DCI_TOKEN(token)) return; header = (struct diag_ctrl_dci_handshake_pkt *)temp; if (header->magic == DCI_MAGIC) { dci_channel_status[token].open = 1; err = dci_ops_tbl[token].send_log_mask(token); if (err && err != DIAG_DCI_NO_ERROR) { pr_err("diag: In %s, unable to send log mask to token: %d, err: %d\n", __func__, token, err); } err = dci_ops_tbl[token].send_event_mask(token); if (err && err != DIAG_DCI_NO_ERROR) { pr_err("diag: In %s, unable to send event mask to token: %d, err: %d\n", __func__, token, err); } } } void extract_dci_ctrl_pkt(unsigned char *buf, int len, int token) { unsigned char *temp = buf; uint32_t ctrl_pkt_id; diag_ws_on_read(DIAG_WS_DCI, len); if (!buf) { pr_err("diag: Invalid buffer in %s\n", __func__); goto err; } if (len < (sizeof(uint8_t) + sizeof(uint32_t))) { pr_err("diag: In %s, invalid length %d\n", __func__, len); goto err; } /* Skip the Control packet command code */ temp += sizeof(uint8_t); len -= sizeof(uint8_t); ctrl_pkt_id = *(uint32_t *)temp; switch (ctrl_pkt_id) { case DIAG_CTRL_MSG_DCI_CONNECTION_STATUS: dci_process_ctrl_status(temp, len, token); break; case DIAG_CTRL_MSG_DCI_HANDSHAKE_PKT: dci_process_ctrl_handshake_pkt(temp, len, token); break; default: pr_debug("diag: In %s, unknown control pkt %d\n", __func__, ctrl_pkt_id); break; } err: /* * DCI control packets are not consumed by the clients. Mimic client * consumption by setting and clearing the wakeup source copy_count * explicitly. */ diag_ws_on_copy_fail(DIAG_WS_DCI); } void extract_dci_pkt_rsp(unsigned char *buf, int len, int data_source, int token) { int tag; struct diag_dci_client_tbl *entry = NULL; void *temp_buf = NULL; uint8_t dci_cmd_code, cmd_code_len, delete_flag = 0; uint32_t rsp_len = 0; struct diag_dci_buffer_t *rsp_buf = NULL; struct dci_pkt_req_entry_t *req_entry = NULL; unsigned char *temp = buf; int save_req_uid = 0; struct diag_dci_pkt_rsp_header_t pkt_rsp_header; int header_len = sizeof(struct diag_dci_pkt_rsp_header_t); if (!buf || len <= 0) { pr_err("diag: Invalid pointer in %s\n", __func__); return; } dci_cmd_code = *(uint8_t *)(temp); if (dci_cmd_code == DCI_PKT_RSP_CODE) { cmd_code_len = sizeof(uint8_t); } else if (dci_cmd_code == DCI_DELAYED_RSP_CODE) { cmd_code_len = sizeof(uint32_t); } else { pr_err("diag: In %s, invalid command code %d\n", __func__, dci_cmd_code); return; } if (len < (cmd_code_len + sizeof(int))) return; temp += cmd_code_len; tag = *(int *)temp; temp += sizeof(int); /* * The size of the response is (total length) - (length of the command * code, the tag (int) */ if (len >= cmd_code_len + sizeof(int)) { rsp_len = len - (cmd_code_len + sizeof(int)); if ((rsp_len == 0) || (rsp_len > (len - 5))) { pr_err("diag: Invalid length in %s, len: %d, rsp_len: %d\n", __func__, len, rsp_len); return; } } else { pr_err("diag:%s: Invalid length(%d) for calculating rsp_len\n", __func__, len); return; } mutex_lock(&driver->dci_mutex); req_entry = diag_dci_get_request_entry(tag); if (!req_entry) { pr_err_ratelimited("diag: No matching client for DCI data\n"); mutex_unlock(&driver->dci_mutex); return; } entry = diag_dci_get_client_entry(req_entry->client_id); if (!entry) { pr_err("diag: In %s, couldn't find client entry, id:%d\n", __func__, req_entry->client_id); mutex_unlock(&driver->dci_mutex); return; } save_req_uid = req_entry->uid; /* Remove the headers and send only the response to this function */ delete_flag = diag_dci_remove_req_entry(temp, rsp_len, req_entry); if (delete_flag < 0) { mutex_unlock(&driver->dci_mutex); return; } if (token != entry->client_info.token) { mutex_unlock(&driver->dci_mutex); return; } mutex_lock(&entry->buffers[data_source].buf_mutex); rsp_buf = entry->buffers[data_source].buf_cmd; mutex_lock(&rsp_buf->data_mutex); /* * Check if we can fit the data in the rsp buffer. The total length of * the rsp is the rsp length (write_len) + dci response packet header * length (sizeof(struct diag_dci_pkt_rsp_header_t)) */ if ((rsp_buf->data_len + header_len + rsp_len) > rsp_buf->capacity) { pr_alert("diag: create capacity for pkt rsp\n"); temp_buf = vzalloc(rsp_buf->capacity + header_len + rsp_len); if (!temp_buf) { pr_err("diag: DCI realloc failed\n"); mutex_unlock(&rsp_buf->data_mutex); mutex_unlock(&entry->buffers[data_source].buf_mutex); mutex_unlock(&driver->dci_mutex); return; } rsp_buf->capacity += header_len + rsp_len; if (rsp_buf->capacity > rsp_buf->data_len) memcpy(temp_buf, rsp_buf->data, rsp_buf->data_len); vfree(rsp_buf->data); rsp_buf->data = temp_buf; } /* Fill in packet response header information */ pkt_rsp_header.type = DCI_PKT_RSP_TYPE; /* Packet Length = Response Length + Length of uid field (int) */ pkt_rsp_header.length = rsp_len + sizeof(int); pkt_rsp_header.delete_flag = delete_flag; pkt_rsp_header.uid = save_req_uid; memcpy(rsp_buf->data + rsp_buf->data_len, &pkt_rsp_header, header_len); rsp_buf->data_len += header_len; memcpy(rsp_buf->data + rsp_buf->data_len, temp, rsp_len); rsp_buf->data_len += rsp_len; rsp_buf->data_source = data_source; mutex_unlock(&rsp_buf->data_mutex); /* * Add directly to the list for writing responses to the * userspace as these shouldn't be buffered and shouldn't wait * for log and event buffers to be full */ dci_add_buffer_to_list(entry, rsp_buf); mutex_unlock(&entry->buffers[data_source].buf_mutex); mutex_unlock(&driver->dci_mutex); } static void copy_ext_hdr(struct diag_dci_buffer_t *data_buffer, void *ext_hdr) { if (!data_buffer) { pr_err("diag: In %s, data buffer is NULL\n", __func__); return; } *(int *)(data_buffer->data + data_buffer->data_len) = DCI_EXT_HDR_TYPE; data_buffer->data_len += sizeof(int); memcpy(data_buffer->data + data_buffer->data_len, ext_hdr, EXT_HDR_LEN); data_buffer->data_len += EXT_HDR_LEN; } static void copy_dci_event(unsigned char *buf, int len, struct diag_dci_client_tbl *client, int data_source, void *ext_hdr) { struct diag_dci_buffer_t *data_buffer = NULL; struct diag_dci_buf_peripheral_t *proc_buf = NULL; int err = 0, total_len = 0; if (!buf || !client) { pr_err("diag: Invalid pointers in %s\n", __func__); return; } total_len = sizeof(int) + len; if (ext_hdr) total_len += sizeof(int) + EXT_HDR_LEN; proc_buf = &client->buffers[data_source]; mutex_lock(&proc_buf->buf_mutex); mutex_lock(&proc_buf->health_mutex); err = diag_dci_get_buffer(client, data_source, total_len); if (err) { if (err == -ENOMEM) proc_buf->health.dropped_events++; else pr_err("diag: In %s, invalid packet\n", __func__); mutex_unlock(&proc_buf->health_mutex); mutex_unlock(&proc_buf->buf_mutex); return; } data_buffer = proc_buf->buf_curr; proc_buf->health.received_events++; mutex_unlock(&proc_buf->health_mutex); mutex_unlock(&proc_buf->buf_mutex); mutex_lock(&data_buffer->data_mutex); if (ext_hdr) copy_ext_hdr(data_buffer, ext_hdr); *(int *)(data_buffer->data + data_buffer->data_len) = DCI_EVENT_TYPE; data_buffer->data_len += sizeof(int); memcpy(data_buffer->data + data_buffer->data_len, buf, len); data_buffer->data_len += len; data_buffer->data_source = data_source; mutex_unlock(&data_buffer->data_mutex); } void extract_dci_events(unsigned char *buf, int len, int data_source, int token, void *ext_hdr) { uint16_t event_id, event_id_packet, length, temp_len; uint8_t payload_len, payload_len_field; uint8_t timestamp[8] = {0}, timestamp_len; unsigned char event_data[MAX_EVENT_SIZE]; unsigned int total_event_len; struct list_head *start, *temp; struct diag_dci_client_tbl *entry = NULL; if (!buf) { pr_err("diag: In %s buffer is NULL\n", __func__); return; } /* * 1 byte for event code and 2 bytes for the length field. * The length field indicates the total length removing the cmd_code * and the length field. The event parsing in that case should happen * till the end. */ if (len < 3) { pr_err("diag: In %s invalid len: %d\n", __func__, len); return; } length = *(uint16_t *)(buf + 1); /* total length of event series */ if ((length == 0) || (len != (length + 3))) { pr_err("diag: Incoming dci event length: %d is invalid\n", length); return; } /* * Move directly to the start of the event series. * The event parsing should happen from start of event * series till the end. */ temp_len = 3; while (temp_len < length) { event_id_packet = *(uint16_t *)(buf + temp_len); event_id = event_id_packet & 0x0FFF; /* extract 12 bits */ if (event_id_packet & 0x8000) { /* The packet has the two smallest byte of the * timestamp */ timestamp_len = 2; } else { /* The packet has the full timestamp. The first event * will always have full timestamp. Save it in the * timestamp buffer and use it for subsequent events if * necessary. */ timestamp_len = 8; if ((temp_len + timestamp_len + 2) <= len) memcpy(timestamp, buf + temp_len + 2, timestamp_len); else { pr_err("diag: Invalid length in %s, len: %d, temp_len: %d\n", __func__, len, temp_len); return; } } /* 13th and 14th bit represent the payload length */ if (((event_id_packet & 0x6000) >> 13) == 3) { payload_len_field = 1; if ((temp_len + timestamp_len + 3) <= len) { payload_len = *(uint8_t *) (buf + temp_len + 2 + timestamp_len); } else { pr_err("diag: Invalid length in %s, len: %d, temp_len: %d\n", __func__, len, temp_len); return; } if ((payload_len < (MAX_EVENT_SIZE - 13)) && ((temp_len + timestamp_len + payload_len + 3) <= len)) { /* * Copy the payload length and the payload * after skipping temp_len bytes for already * parsed packet, timestamp_len for timestamp * buffer, 2 bytes for event_id_packet. */ memcpy(event_data + 12, buf + temp_len + 2 + timestamp_len, 1); memcpy(event_data + 13, buf + temp_len + 2 + timestamp_len + 1, payload_len); } else { pr_err("diag: event > %d, payload_len = %d, temp_len = %d\n", (MAX_EVENT_SIZE - 13), payload_len, temp_len); return; } } else { payload_len_field = 0; payload_len = (event_id_packet & 0x6000) >> 13; /* * Copy the payload after skipping temp_len bytes * for already parsed packet, timestamp_len for * timestamp buffer, 2 bytes for event_id_packet. */ if ((payload_len < (MAX_EVENT_SIZE - 12)) && ((temp_len + timestamp_len + payload_len + 2) <= len)) memcpy(event_data + 12, buf + temp_len + 2 + timestamp_len, payload_len); else { pr_err("diag: event > %d, payload_len = %d, temp_len = %d\n", (MAX_EVENT_SIZE - 12), payload_len, temp_len); return; } } /* Before copying the data to userspace, check if we are still * within the buffer limit. This is an error case, don't count * it towards the health statistics. * * Here, the offset of 2 bytes(uint16_t) is for the * event_id_packet length */ temp_len += sizeof(uint16_t) + timestamp_len + payload_len_field + payload_len; if (temp_len > len) { pr_err("diag: Invalid length in %s, len: %d, read: %d\n", __func__, len, temp_len); return; } /* 2 bytes for the event id & timestamp len is hard coded to 8, * as individual events have full timestamp. */ *(uint16_t *)(event_data) = 10 + payload_len_field + payload_len; *(uint16_t *)(event_data + 2) = event_id_packet & 0x7FFF; memcpy(event_data + 4, timestamp, 8); /* 2 bytes for the event length field which is added to * the event data. */ total_event_len = 2 + 10 + payload_len_field + payload_len; /* parse through event mask tbl of each client and check mask */ mutex_lock(&driver->dci_mutex); list_for_each_safe(start, temp, &driver->dci_client_list) { entry = list_entry(start, struct diag_dci_client_tbl, track); if (entry->client_info.token != token) continue; if (diag_dci_query_event_mask(entry, event_id)) { /* copy to client buffer */ copy_dci_event(event_data, total_event_len, entry, data_source, ext_hdr); } } mutex_unlock(&driver->dci_mutex); } } static void copy_dci_log(unsigned char *buf, int len, struct diag_dci_client_tbl *client, int data_source, void *ext_hdr) { uint16_t log_length = 0; struct diag_dci_buffer_t *data_buffer = NULL; struct diag_dci_buf_peripheral_t *proc_buf = NULL; int err = 0, total_len = 0; if (!buf || !client) { pr_err("diag: Invalid pointers in %s\n", __func__); return; } log_length = *(uint16_t *)(buf + 2); if (log_length > USHRT_MAX - 4) { pr_err("diag: Integer overflow in %s, log_len: %d\n", __func__, log_length); return; } total_len = sizeof(int) + log_length; if (ext_hdr) total_len += sizeof(int) + EXT_HDR_LEN; /* Check if we are within the len. The check should include the * first 4 bytes for the Log code(2) and the length bytes (2) */ if ((log_length + sizeof(uint16_t) + 2) > len) { pr_err("diag: Invalid length in %s, log_len: %d, len: %d\n", __func__, log_length, len); return; } proc_buf = &client->buffers[data_source]; mutex_lock(&proc_buf->buf_mutex); mutex_lock(&proc_buf->health_mutex); err = diag_dci_get_buffer(client, data_source, total_len); if (err) { if (err == -ENOMEM) proc_buf->health.dropped_logs++; else pr_err("diag: In %s, invalid packet\n", __func__); mutex_unlock(&proc_buf->health_mutex); mutex_unlock(&proc_buf->buf_mutex); return; } data_buffer = proc_buf->buf_curr; proc_buf->health.received_logs++; mutex_unlock(&proc_buf->health_mutex); mutex_unlock(&proc_buf->buf_mutex); mutex_lock(&data_buffer->data_mutex); if (!data_buffer->data) { mutex_unlock(&data_buffer->data_mutex); return; } if (ext_hdr) copy_ext_hdr(data_buffer, ext_hdr); *(int *)(data_buffer->data + data_buffer->data_len) = DCI_LOG_TYPE; data_buffer->data_len += sizeof(int); memcpy(data_buffer->data + data_buffer->data_len, buf + sizeof(int), log_length); data_buffer->data_len += log_length; data_buffer->data_source = data_source; mutex_unlock(&data_buffer->data_mutex); } void extract_dci_log(unsigned char *buf, int len, int data_source, int token, void *ext_hdr) { uint16_t log_code, read_bytes = 0; struct list_head *start, *temp; struct diag_dci_client_tbl *entry = NULL; if (!buf) { pr_err("diag: In %s buffer is NULL\n", __func__); return; } /* * The first eight bytes for the incoming log packet contains * Command code (2), the length of the packet (2), the length * of the log (2) and log code (2) */ if (len < 8) { pr_err("diag: In %s invalid len: %d\n", __func__, len); return; } log_code = *(uint16_t *)(buf + 6); read_bytes += sizeof(uint16_t) + 6; /* parse through log mask table of each client and check mask */ mutex_lock(&driver->dci_mutex); list_for_each_safe(start, temp, &driver->dci_client_list) { entry = list_entry(start, struct diag_dci_client_tbl, track); if (entry->client_info.token != token) continue; if (diag_dci_query_log_mask(entry, log_code)) { pr_debug("\t log code %x needed by client %d\n", log_code, entry->client->tgid); /* copy to client buffer */ copy_dci_log(buf, len, entry, data_source, ext_hdr); } } mutex_unlock(&driver->dci_mutex); } void extract_dci_ext_pkt(unsigned char *buf, int len, int data_source, int token) { uint8_t version, pkt_cmd_code = 0; unsigned char *pkt = NULL; if (!buf) { pr_err("diag: In %s buffer is NULL\n", __func__); return; } if (len < (EXT_HDR_LEN + sizeof(uint8_t))) { pr_err("diag: In %s invalid len: %d\n", __func__, len); return; } version = *(uint8_t *)buf + 1; if (version < EXT_HDR_VERSION) { pr_err("diag: %s, Extended header with invalid version: %d\n", __func__, version); return; } pkt = buf + EXT_HDR_LEN; pkt_cmd_code = *(uint8_t *)pkt; len -= EXT_HDR_LEN; switch (pkt_cmd_code) { case LOG_CMD_CODE: extract_dci_log(pkt, len, data_source, token, buf); break; case EVENT_CMD_CODE: extract_dci_events(pkt, len, data_source, token, buf); break; default: pr_err("diag: %s unsupported cmd_code: %d, data_source: %d\n", __func__, pkt_cmd_code, data_source); return; } } void diag_dci_channel_open_work(struct work_struct *work) { int i, j; char dirty_bits[16]; uint8_t *client_log_mask_ptr; uint8_t *log_mask_ptr; int ret; struct list_head *start, *temp; struct diag_dci_client_tbl *entry = NULL; /* Update apps and peripheral(s) with the dci log and event masks */ memset(dirty_bits, 0, 16 * sizeof(uint8_t)); /* * From each log entry used by each client, determine * which log entries in the cumulative logs that need * to be updated on the peripheral. */ mutex_lock(&driver->dci_mutex); list_for_each_safe(start, temp, &driver->dci_client_list) { entry = list_entry(start, struct diag_dci_client_tbl, track); if (entry->client_info.token != DCI_LOCAL_PROC) continue; client_log_mask_ptr = entry->dci_log_mask; for (j = 0; j < 16; j++) { if (*(client_log_mask_ptr+1)) dirty_bits[j] = 1; client_log_mask_ptr += 514; } } mutex_unlock(&driver->dci_mutex); mutex_lock(&dci_log_mask_mutex); /* Update the appropriate dirty bits in the cumulative mask */ log_mask_ptr = dci_ops_tbl[DCI_LOCAL_PROC].log_mask_composite; for (i = 0; i < 16; i++) { if (dirty_bits[i]) *(log_mask_ptr+1) = dirty_bits[i]; log_mask_ptr += 514; } mutex_unlock(&dci_log_mask_mutex); /* Send updated mask to userspace clients */ diag_update_userspace_clients(DCI_LOG_MASKS_TYPE); /* Send updated log mask to peripherals */ ret = dci_ops_tbl[DCI_LOCAL_PROC].send_log_mask(DCI_LOCAL_PROC); /* Send updated event mask to userspace clients */ diag_update_userspace_clients(DCI_EVENT_MASKS_TYPE); /* Send updated event mask to peripheral */ ret = dci_ops_tbl[DCI_LOCAL_PROC].send_event_mask(DCI_LOCAL_PROC); } void diag_dci_notify_client(int peripheral_mask, int data, int proc) { int stat = 0; struct siginfo info; struct list_head *start, *temp; struct diag_dci_client_tbl *entry = NULL; struct pid *pid_struct = NULL; struct task_struct *dci_task = NULL; memset(&info, 0, sizeof(struct siginfo)); info.si_code = SI_QUEUE; info.si_int = (peripheral_mask | data); if (data == DIAG_STATUS_OPEN) dci_ops_tbl[proc].peripheral_status |= peripheral_mask; else dci_ops_tbl[proc].peripheral_status &= ~peripheral_mask; /* Notify the DCI process that the peripheral DCI Channel is up */ mutex_lock(&driver->dci_mutex); list_for_each_safe(start, temp, &driver->dci_client_list) { entry = list_entry(start, struct diag_dci_client_tbl, track); if (entry->client_info.token != proc) continue; if (entry->client_info.notification_list & peripheral_mask) { info.si_signo = entry->client_info.signal_type; pid_struct = find_get_pid(entry->tgid); if (pid_struct) { dci_task = get_pid_task(pid_struct, PIDTYPE_PID); if (!dci_task) { DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "diag: dci client with pid = %d Exited..\n", entry->tgid); put_pid(pid_struct); mutex_unlock(&driver->dci_mutex); return; } if (entry->client && entry->tgid == dci_task->tgid) { DIAG_LOG(DIAG_DEBUG_DCI, "entry tgid = %d, dci client tgid = %d\n", entry->tgid, dci_task->tgid); stat = send_sig_info( entry->client_info.signal_type, &info, dci_task); if (stat) pr_err("diag: Err sending dci signal to client, signal data: 0x%x, stat: %d\n", info.si_int, stat); } else { pr_err("diag: client data is corrupted, signal data: 0x%x, stat: %d\n", info.si_int, stat); } put_task_struct(dci_task); put_pid(pid_struct); } } } mutex_unlock(&driver->dci_mutex); } static int diag_send_dci_pkt(struct diag_cmd_reg_t *entry, unsigned char *buf, int len, int tag) { int i, status = DIAG_DCI_NO_ERROR; uint32_t write_len = 0; struct diag_dci_pkt_header_t header; if (!entry) return -EIO; if (len < 1 || len > DIAG_MAX_REQ_SIZE) { pr_err("diag: dci: In %s, invalid length %d, max_length: %d\n", __func__, len, (int)(DCI_REQ_BUF_SIZE - sizeof(header))); return -EIO; } if ((len + sizeof(header) + sizeof(uint8_t)) > DCI_BUF_SIZE) { pr_err("diag: dci: In %s, invalid length %d for apps_dci_buf, max_length: %d\n", __func__, len, DIAG_MAX_REQ_SIZE); return -EIO; } /* prepare DCI packet */ header.start = CONTROL_CHAR; header.version = 1; header.len = len + sizeof(int) + sizeof(uint8_t); header.pkt_code = DCI_PKT_RSP_CODE; header.tag = tag; memcpy(driver->apps_dci_buf, &header, sizeof(header)); write_len += sizeof(header); memcpy(driver->apps_dci_buf + write_len, buf, len); write_len += len; *(uint8_t *)(driver->apps_dci_buf + write_len) = CONTROL_CHAR; write_len += sizeof(uint8_t); /* This command is registered locally on the Apps */ if (entry->proc == APPS_DATA) { diag_update_pkt_buffer(driver->apps_dci_buf, write_len, DCI_PKT_TYPE); diag_update_sleeping_process(entry->pid, DCI_PKT_TYPE); return DIAG_DCI_NO_ERROR; } for (i = 0; i < NUM_PERIPHERALS; i++) if (entry->proc == i) { status = 1; break; } if (status) { status = diag_dci_write_proc(entry->proc, DIAG_DATA_TYPE, driver->apps_dci_buf, write_len); } else { pr_err("diag: Cannot send packet to peripheral %d\n", entry->proc); status = DIAG_DCI_SEND_DATA_FAIL; } return status; } #ifdef CONFIG_DIAGFWD_BRIDGE_CODE unsigned char *dci_get_buffer_from_bridge(int token) { uint8_t retries = 0, max_retries = 50; unsigned char *buf = NULL; unsigned long flags; do { spin_lock_irqsave(&driver->dci_mempool_lock, flags); buf = diagmem_alloc(driver, DIAG_MDM_BUF_SIZE, dci_ops_tbl[token].mempool); spin_unlock_irqrestore(&driver->dci_mempool_lock, flags); if (!buf) { usleep_range(5000, 5100); retries++; } else break; } while (retries < max_retries); return buf; } int diag_dci_write_bridge(int token, unsigned char *buf, int len) { return diagfwd_bridge_write(TOKEN_TO_BRIDGE(token), buf, len); } int diag_dci_write_done_bridge(int index, unsigned char *buf, int len) { unsigned long flags; int token = BRIDGE_TO_TOKEN(index); if (!VALID_DCI_TOKEN(token)) { pr_err("diag: Invalid DCI token %d in %s\n", token, __func__); return -EINVAL; } spin_lock_irqsave(&driver->dci_mempool_lock, flags); diagmem_free(driver, buf, dci_ops_tbl[token].mempool); spin_unlock_irqrestore(&driver->dci_mempool_lock, flags); return 0; } #endif #ifdef CONFIG_DIAGFWD_BRIDGE_CODE static int diag_send_dci_pkt_remote(unsigned char *data, int len, int tag, int token) { unsigned char *buf = NULL; struct diag_dci_header_t dci_header; int dci_header_size = sizeof(struct diag_dci_header_t); int ret = DIAG_DCI_NO_ERROR; uint32_t write_len = 0; unsigned long flags; if (!data) return -EIO; buf = dci_get_buffer_from_bridge(token); if (!buf) { pr_err("diag: In %s, unable to get dci buffers to write data\n", __func__); return -EAGAIN; } dci_header.start = CONTROL_CHAR; dci_header.version = 1; /* * The Length of the DCI packet = length of the command + tag (int) + * the command code size (uint8_t) */ dci_header.length = len + sizeof(int) + sizeof(uint8_t); dci_header.cmd_code = DCI_PKT_RSP_CODE; memcpy(buf + write_len, &dci_header, dci_header_size); write_len += dci_header_size; *(int *)(buf + write_len) = tag; write_len += sizeof(int); if ((write_len + len) < DIAG_MDM_BUF_SIZE) { memcpy(buf + write_len, data, len); } else { pr_err("diag: skip writing invalid length packet, token: %d, pkt_len: %d\n", token, (write_len + len)); spin_lock_irqsave(&driver->dci_mempool_lock, flags); diagmem_free(driver, buf, dci_ops_tbl[token].mempool); spin_unlock_irqrestore(&driver->dci_mempool_lock, flags); return -EAGAIN; } write_len += len; *(buf + write_len) = CONTROL_CHAR; /* End Terminator */ write_len += sizeof(uint8_t); ret = diag_dci_write_bridge(token, buf, write_len); if (ret) { pr_err("diag: error writing dci pkt to remote proc, token: %d, err: %d\n", token, ret); spin_lock_irqsave(&driver->dci_mempool_lock, flags); diagmem_free(driver, buf, dci_ops_tbl[token].mempool); spin_unlock_irqrestore(&driver->dci_mempool_lock, flags); } else { ret = DIAG_DCI_NO_ERROR; } return ret; } #else static int diag_send_dci_pkt_remote(unsigned char *data, int len, int tag, int token) { return DIAG_DCI_NO_ERROR; } #endif #ifdef CONFIG_DIAGFWD_BRIDGE_CODE int diag_dci_send_handshake_pkt(int index) { int err = 0; int token = BRIDGE_TO_TOKEN(index); int write_len = 0; struct diag_ctrl_dci_handshake_pkt ctrl_pkt; unsigned char *buf = NULL; struct diag_dci_header_t dci_header; unsigned long flags; if (!VALID_DCI_TOKEN(token)) { pr_err("diag: In %s, invalid DCI token %d\n", __func__, token); return -EINVAL; } buf = dci_get_buffer_from_bridge(token); if (!buf) { pr_err("diag: In %s, unable to get dci buffers to write data\n", __func__); return -EAGAIN; } dci_header.start = CONTROL_CHAR; dci_header.version = 1; /* Include the cmd code (uint8_t) in the length */ dci_header.length = sizeof(ctrl_pkt) + sizeof(uint8_t); dci_header.cmd_code = DCI_CONTROL_PKT_CODE; memcpy(buf, &dci_header, sizeof(dci_header)); write_len += sizeof(dci_header); ctrl_pkt.ctrl_pkt_id = DIAG_CTRL_MSG_DCI_HANDSHAKE_PKT; /* * The control packet data length accounts for the version (uint32_t) * of the packet and the magic number (uint32_t). */ ctrl_pkt.ctrl_pkt_data_len = 2 * sizeof(uint32_t); ctrl_pkt.version = 1; ctrl_pkt.magic = DCI_MAGIC; memcpy(buf + write_len, &ctrl_pkt, sizeof(ctrl_pkt)); write_len += sizeof(ctrl_pkt); *(uint8_t *)(buf + write_len) = CONTROL_CHAR; write_len += sizeof(uint8_t); err = diag_dci_write_bridge(token, buf, write_len); if (err) { pr_err("diag: error writing ack packet to remote proc, token: %d, err: %d\n", token, err); spin_lock_irqsave(&driver->dci_mempool_lock, flags); diagmem_free(driver, buf, dci_ops_tbl[token].mempool); spin_unlock_irqrestore(&driver->dci_mempool_lock, flags); return err; } mod_timer(&(dci_channel_status[token].wait_time), jiffies + msecs_to_jiffies(DCI_HANDSHAKE_WAIT_TIME)); return 0; } #else int diag_dci_send_handshake_pkt(int index) { return 0; } #endif static int diag_dci_process_apps_pkt(struct diag_pkt_header_t *pkt_header, unsigned char *req_buf, int req_len, int tag, int pkt_header_len) { uint8_t cmd_code = 0, subsys_id = 0, i, goto_download = 0; uint8_t header_len = sizeof(struct diag_dci_pkt_header_t); uint16_t ss_cmd_code = 0; uint32_t write_len = 0; unsigned char *dest_buf = driver->apps_dci_buf; unsigned char *payload_ptr = driver->apps_dci_buf + header_len; struct diag_dci_pkt_header_t dci_header; if (!pkt_header || !req_buf || req_len <= 0 || tag < 0 || pkt_header_len <= 0) return -EIO; if (pkt_header_len >= (sizeof(uint8_t))) cmd_code = pkt_header->cmd_code; if (pkt_header_len >= (2 * sizeof(uint8_t))) subsys_id = pkt_header->subsys_id; if (pkt_header_len >= (3 * sizeof(uint8_t))) ss_cmd_code = pkt_header->subsys_cmd_code; if (cmd_code == DIAG_CMD_DOWNLOAD) { *payload_ptr = DIAG_CMD_DOWNLOAD; write_len = sizeof(uint8_t); goto_download = 1; goto fill_buffer; } else if (cmd_code == DIAG_CMD_VERSION) { if (chk_polling_response()) { for (i = 0; i < 55; i++, write_len++, payload_ptr++) *(payload_ptr) = 0; goto fill_buffer; } } else if (cmd_code == DIAG_CMD_EXT_BUILD) { if (chk_polling_response()) { *payload_ptr = DIAG_CMD_EXT_BUILD; write_len = sizeof(uint8_t); payload_ptr += sizeof(uint8_t); for (i = 0; i < 8; i++, write_len++, payload_ptr++) *(payload_ptr) = 0; *(int *)(payload_ptr) = chk_config_get_id(); write_len += sizeof(int); goto fill_buffer; } } else if (cmd_code == DIAG_CMD_LOG_ON_DMND) { write_len = diag_cmd_log_on_demand(req_buf, req_len, payload_ptr, APPS_BUF_SIZE - header_len); goto fill_buffer; } else if (cmd_code != DIAG_CMD_DIAG_SUBSYS) { return DIAG_DCI_TABLE_ERR; } if (subsys_id == DIAG_SS_DIAG) { if (ss_cmd_code == DIAG_DIAG_MAX_PKT_SZ) { memcpy(payload_ptr, pkt_header, sizeof(struct diag_pkt_header_t)); write_len = sizeof(struct diag_pkt_header_t); *(uint32_t *)(payload_ptr + write_len) = DIAG_MAX_REQ_SIZE; write_len += sizeof(uint32_t); } else if (ss_cmd_code == DIAG_DIAG_STM) { write_len = diag_process_stm_cmd(req_buf, payload_ptr); } } else if (subsys_id == DIAG_SS_PARAMS) { if (ss_cmd_code == DIAG_DIAG_POLL) { if (chk_polling_response()) { memcpy(payload_ptr, pkt_header, sizeof(struct diag_pkt_header_t)); write_len = sizeof(struct diag_pkt_header_t); payload_ptr += write_len; for (i = 0; i < 12; i++, write_len++) { *(payload_ptr) = 0; payload_ptr++; } } } else if (ss_cmd_code == DIAG_DEL_RSP_WRAP) { memcpy(payload_ptr, pkt_header, sizeof(struct diag_pkt_header_t)); write_len = sizeof(struct diag_pkt_header_t); *(int *)(payload_ptr + write_len) = wrap_enabled; write_len += sizeof(int); } else if (ss_cmd_code == DIAG_DEL_RSP_WRAP_CNT) { wrap_enabled = true; memcpy(payload_ptr, pkt_header, sizeof(struct diag_pkt_header_t)); write_len = sizeof(struct diag_pkt_header_t); *(uint16_t *)(payload_ptr + write_len) = wrap_count; write_len += sizeof(uint16_t); } else if (ss_cmd_code == DIAG_EXT_MOBILE_ID) { write_len = diag_cmd_get_mobile_id(req_buf, req_len, payload_ptr, APPS_BUF_SIZE - header_len); } } fill_buffer: if (write_len > 0) { /* Check if we are within the range of the buffer*/ if (write_len + header_len > DIAG_MAX_REQ_SIZE) { pr_err("diag: In %s, invalid length %d\n", __func__, write_len + header_len); return -ENOMEM; } dci_header.start = CONTROL_CHAR; dci_header.version = 1; /* * Length of the rsp pkt = actual data len + pkt rsp code * (uint8_t) + tag (int) */ dci_header.len = write_len + sizeof(uint8_t) + sizeof(int); dci_header.pkt_code = DCI_PKT_RSP_CODE; dci_header.tag = tag; driver->in_busy_dcipktdata = 1; memcpy(dest_buf, &dci_header, header_len); diag_process_apps_dci_read_data(DCI_PKT_TYPE, dest_buf + 4, dci_header.len); driver->in_busy_dcipktdata = 0; if (goto_download) { /* * Sleep for sometime so that the response reaches the * client. The value 5000 empirically as an optimum * time for the response to reach the client. */ usleep_range(5000, 5100); /* call download API */ msm_set_restart_mode(RESTART_DLOAD); pr_alert("diag: download mode set, Rebooting SoC..\n"); kernel_restart(NULL); } return DIAG_DCI_NO_ERROR; } return DIAG_DCI_TABLE_ERR; } static int diag_process_dci_pkt_rsp(unsigned char *buf, int len) { int ret = DIAG_DCI_TABLE_ERR; int common_cmd = 0, header_len = 0; int req_tag = 0; struct diag_pkt_header_t *header = NULL; unsigned char *temp = buf; unsigned char *req_buf = NULL; uint8_t retry_count = 0, max_retries = 3; uint32_t read_len = 0, req_len = len; struct dci_pkt_req_entry_t *req_entry = NULL, *test_entry = NULL; struct diag_dci_client_tbl *dci_entry = NULL; struct dci_pkt_req_t req_hdr; struct diag_cmd_reg_t *reg_item; struct diag_cmd_reg_entry_t reg_entry; struct diag_cmd_reg_entry_t *temp_entry; if (!buf) return -EIO; if (len < sizeof(struct dci_pkt_req_t) || len > DCI_REQ_BUF_SIZE) { pr_err("diag: dci: Invalid length %d len in %s\n", len, __func__); return -EIO; } req_hdr = *(struct dci_pkt_req_t *)temp; temp += sizeof(struct dci_pkt_req_t); read_len += sizeof(struct dci_pkt_req_t); req_len -= sizeof(struct dci_pkt_req_t); req_buf = temp; /* Start of the Request */ mutex_lock(&driver->dci_mutex); dci_entry = diag_dci_get_client_entry(req_hdr.client_id); if (!dci_entry) { pr_err("diag: Invalid client %d in %s\n", req_hdr.client_id, __func__); mutex_unlock(&driver->dci_mutex); return DIAG_DCI_NO_REG; } header = (void *)temp; header_len = len - sizeof(struct dci_pkt_req_t); if (header_len <= 0) { mutex_unlock(&driver->dci_mutex); return -EIO; } if (header_len >= sizeof(uint8_t)) { header->cmd_code = (uint16_t)(*(uint8_t *)temp); read_len += sizeof(uint8_t); } if (header_len >= (2 * sizeof(uint8_t))) { temp += sizeof(uint8_t); header->subsys_id = (uint16_t)(*(uint8_t *)temp); read_len += sizeof(uint8_t); } if (header_len == (3 * sizeof(uint8_t))) { temp += sizeof(uint8_t); header->subsys_cmd_code = (uint16_t)(*(uint8_t *)temp); read_len += sizeof(uint8_t); } else if (header_len >= (2 * sizeof(uint8_t)) + sizeof(uint16_t)) { temp += sizeof(uint8_t); header->subsys_cmd_code = (uint16_t)(*(uint16_t *)temp); read_len += sizeof(uint16_t); } if (read_len > DCI_REQ_BUF_SIZE) { pr_err("diag: dci: In %s, invalid read_len: %d\n", __func__, read_len); mutex_unlock(&driver->dci_mutex); return -EIO; } /* Check if the command is allowed on DCI */ if (diag_dci_filter_commands(header, header_len)) { mutex_unlock(&driver->dci_mutex); return DIAG_DCI_SEND_DATA_FAIL; } common_cmd = diag_check_common_cmd(header); if (common_cmd < 0) { pr_debug("diag: error in checking common command, %d\n", common_cmd); mutex_unlock(&driver->dci_mutex); return DIAG_DCI_SEND_DATA_FAIL; } /* * Previous packet is yet to be consumed by the client. Wait * till the buffer is free. */ while (retry_count < max_retries) { retry_count++; if (driver->in_busy_dcipktdata) usleep_range(10000, 10100); else break; } /* The buffer is still busy */ if (driver->in_busy_dcipktdata) { pr_err("diag: In %s, apps dci buffer is still busy. Dropping packet\n", __func__); mutex_unlock(&driver->dci_mutex); return -EAGAIN; } /* Register this new DCI packet */ req_entry = diag_register_dci_transaction(req_hdr.uid, req_hdr.client_id); if (!req_entry) { pr_alert("diag: registering new DCI transaction failed\n"); mutex_unlock(&driver->dci_mutex); return DIAG_DCI_NO_REG; } req_tag = req_entry->tag; mutex_unlock(&driver->dci_mutex); /* * If the client has registered for remote data, route the packet to the * remote processor */ if (dci_entry->client_info.token > 0) { ret = diag_send_dci_pkt_remote(req_buf, req_len, req_tag, dci_entry->client_info.token); return ret; } /* Check if it is a dedicated Apps command */ ret = diag_dci_process_apps_pkt(header, req_buf, req_len, req_tag, header_len); if ((ret == DIAG_DCI_NO_ERROR && !common_cmd) || ret < 0) return ret; reg_entry.cmd_code = 0; reg_entry.subsys_id = 0; reg_entry.cmd_code_hi = 0; reg_entry.cmd_code_lo = 0; if (header_len >= (sizeof(uint8_t))) reg_entry.cmd_code = header->cmd_code; if (header_len >= (2 * sizeof(uint8_t))) reg_entry.subsys_id = header->subsys_id; if (header_len >= (3 * sizeof(uint8_t))) { reg_entry.cmd_code_hi = header->subsys_cmd_code; reg_entry.cmd_code_lo = header->subsys_cmd_code; } 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->dci_mutex); test_entry = diag_dci_get_request_entry(req_tag); if (test_entry) ret = diag_send_dci_pkt(reg_item, req_buf, req_len, test_entry->tag); else ret = -EIO; mutex_unlock(&driver->dci_mutex); } else { DIAG_LOG(DIAG_DEBUG_DCI, "Command not found: %02x %02x %02x\n", reg_entry.cmd_code, reg_entry.subsys_id, reg_entry.cmd_code_hi); } mutex_unlock(&driver->cmd_reg_mutex); return ret; } int diag_process_dci_transaction(unsigned char *buf, int len) { unsigned char *temp = buf; uint16_t log_code, item_num; int ret = -1, found = 0, client_id = 0, client_token = 0; int count, set_mask, num_codes, bit_index, event_id, offset = 0; unsigned int byte_index, read_len = 0; uint8_t equip_id, *log_mask_ptr, *head_log_mask_ptr, byte_mask; uint8_t *event_mask_ptr; struct diag_dci_client_tbl *dci_entry = NULL; if (!temp || len < sizeof(int)) { pr_err("diag: Invalid input in %s\n", __func__); return -EINVAL; } /* This is Pkt request/response transaction */ if (*(int *)temp > 0) { return diag_process_dci_pkt_rsp(buf, len); } else if (*(int *)temp == DCI_LOG_TYPE) { /* Minimum length of a log mask config is 12 + 2 bytes for * atleast one log code to be set or reset. */ if (len < DCI_LOG_CON_MIN_LEN || len > USER_SPACE_DATA) { pr_err("diag: dci: Invalid length in %s\n", __func__); return -EIO; } /* Extract each log code and put in client table */ temp += sizeof(int); read_len += sizeof(int); client_id = *(int *)temp; temp += sizeof(int); read_len += sizeof(int); set_mask = *(int *)temp; temp += sizeof(int); read_len += sizeof(int); num_codes = *(int *)temp; temp += sizeof(int); read_len += sizeof(int); /* Find client table entry */ mutex_lock(&driver->dci_mutex); dci_entry = diag_dci_get_client_entry(client_id); if (!dci_entry) { pr_err("diag: In %s, invalid client\n", __func__); mutex_unlock(&driver->dci_mutex); return ret; } client_token = dci_entry->client_info.token; if (num_codes == 0 || (num_codes >= (USER_SPACE_DATA - 8)/2)) { pr_err("diag: dci: Invalid number of log codes %d\n", num_codes); mutex_unlock(&driver->dci_mutex); return -EIO; } head_log_mask_ptr = dci_entry->dci_log_mask; if (!head_log_mask_ptr) { pr_err("diag: dci: Invalid Log mask pointer in %s\n", __func__); mutex_unlock(&driver->dci_mutex); return -ENOMEM; } pr_debug("diag: head of dci log mask %pK\n", head_log_mask_ptr); count = 0; /* iterator for extracting log codes */ while (count < num_codes) { if (read_len + sizeof(uint16_t) > len) { pr_err("diag: dci: Invalid length for log type in %s\n", __func__); mutex_unlock(&driver->dci_mutex); return -EIO; } log_code = *(uint16_t *)temp; equip_id = LOG_GET_EQUIP_ID(log_code); item_num = LOG_GET_ITEM_NUM(log_code); byte_index = item_num/8 + 2; if (byte_index >= (DCI_MAX_ITEMS_PER_LOG_CODE+2)) { pr_err("diag: dci: Log type, invalid byte index\n"); mutex_unlock(&driver->dci_mutex); return ret; } byte_mask = 0x01 << (item_num % 8); /* * Parse through log mask table and find * relevant range */ log_mask_ptr = head_log_mask_ptr; found = 0; offset = 0; while (log_mask_ptr && (offset < DCI_LOG_MASK_SIZE)) { if (*log_mask_ptr == equip_id) { found = 1; pr_debug("diag: find equip id = %x at %pK\n", equip_id, log_mask_ptr); break; } pr_debug("diag: did not find equip id = %x at %d\n", equip_id, *log_mask_ptr); log_mask_ptr += 514; offset += 514; } if (!found) { pr_err("diag: dci equip id not found\n"); mutex_unlock(&driver->dci_mutex); return ret; } *(log_mask_ptr+1) = 1; /* set the dirty byte */ log_mask_ptr = log_mask_ptr + byte_index; if (set_mask) *log_mask_ptr |= byte_mask; else *log_mask_ptr &= ~byte_mask; /* add to cumulative mask */ update_dci_cumulative_log_mask( offset, byte_index, byte_mask, client_token); temp += 2; read_len += 2; count++; ret = DIAG_DCI_NO_ERROR; } /* send updated mask to userspace clients */ if (client_token == DCI_LOCAL_PROC) diag_update_userspace_clients(DCI_LOG_MASKS_TYPE); /* send updated mask to peripherals */ ret = dci_ops_tbl[client_token].send_log_mask(client_token); mutex_unlock(&driver->dci_mutex); } else if (*(int *)temp == DCI_EVENT_TYPE) { /* Minimum length of a event mask config is 12 + 4 bytes for * atleast one event id to be set or reset. */ if (len < DCI_EVENT_CON_MIN_LEN || len > USER_SPACE_DATA) { pr_err("diag: dci: Invalid length in %s\n", __func__); return -EIO; } /* Extract each event id and put in client table */ temp += sizeof(int); read_len += sizeof(int); client_id = *(int *)temp; temp += sizeof(int); read_len += sizeof(int); set_mask = *(int *)temp; temp += sizeof(int); read_len += sizeof(int); num_codes = *(int *)temp; temp += sizeof(int); read_len += sizeof(int); /* find client table entry */ mutex_lock(&driver->dci_mutex); dci_entry = diag_dci_get_client_entry(client_id); if (!dci_entry) { pr_err("diag: In %s, invalid client\n", __func__); mutex_unlock(&driver->dci_mutex); return ret; } client_token = dci_entry->client_info.token; /* Check for positive number of event ids. Also, the number of * event ids should fit in the buffer along with set_mask and * num_codes which are 4 bytes each. */ if (num_codes == 0 || (num_codes >= (USER_SPACE_DATA - 8)/2)) { pr_err("diag: dci: Invalid number of event ids %d\n", num_codes); mutex_unlock(&driver->dci_mutex); return -EIO; } event_mask_ptr = dci_entry->dci_event_mask; if (!event_mask_ptr) { pr_err("diag: dci: Invalid event mask pointer in %s\n", __func__); mutex_unlock(&driver->dci_mutex); return -ENOMEM; } pr_debug("diag: head of dci event mask %pK\n", event_mask_ptr); count = 0; /* iterator for extracting log codes */ while (count < num_codes) { if (read_len + sizeof(int) > len) { pr_err("diag: dci: Invalid length for event type in %s\n", __func__); mutex_unlock(&driver->dci_mutex); return -EIO; } event_id = *(int *)temp; byte_index = event_id/8; if (byte_index >= DCI_EVENT_MASK_SIZE) { pr_err("diag: dci: Event type, invalid byte index\n"); mutex_unlock(&driver->dci_mutex); return ret; } bit_index = event_id % 8; byte_mask = 0x1 << bit_index; /* * Parse through event mask table and set * relevant byte & bit combination */ if (set_mask) *(event_mask_ptr + byte_index) |= byte_mask; else *(event_mask_ptr + byte_index) &= ~byte_mask; /* add to cumulative mask */ update_dci_cumulative_event_mask(byte_index, byte_mask, client_token); temp += sizeof(int); read_len += sizeof(int); count++; ret = DIAG_DCI_NO_ERROR; } /* send updated mask to userspace clients */ if (dci_entry->client_info.token == DCI_LOCAL_PROC) diag_update_userspace_clients(DCI_EVENT_MASKS_TYPE); /* send updated mask to peripherals */ ret = dci_ops_tbl[client_token].send_event_mask(client_token); mutex_unlock(&driver->dci_mutex); } else { pr_alert("diag: Incorrect DCI transaction\n"); } return ret; } struct diag_dci_client_tbl *diag_dci_get_client_entry(int client_id) { struct list_head *start, *temp; struct diag_dci_client_tbl *entry = NULL; list_for_each_safe(start, temp, &driver->dci_client_list) { entry = list_entry(start, struct diag_dci_client_tbl, track); if (entry->client_info.client_id == client_id) return entry; } return NULL; } struct diag_dci_client_tbl *dci_lookup_client_entry_pid(int tgid) { struct list_head *start, *temp; struct diag_dci_client_tbl *entry = NULL; struct pid *pid_struct = NULL; struct task_struct *task_s = NULL; list_for_each_safe(start, temp, &driver->dci_client_list) { entry = list_entry(start, struct diag_dci_client_tbl, track); pid_struct = find_get_pid(entry->tgid); if (!pid_struct) { DIAG_LOG(DIAG_DEBUG_DCI, "diag: Exited pid (%d) doesn't match dci client of pid (%d)\n", tgid, entry->tgid); continue; } task_s = get_pid_task(pid_struct, PIDTYPE_PID); if (!task_s) { DIAG_LOG(DIAG_DEBUG_DCI, "diag: valid task doesn't exist for pid = %d\n", entry->tgid); put_pid(pid_struct); continue; } if (task_s == entry->client) { if (entry->client->tgid == tgid) { put_task_struct(task_s); put_pid(pid_struct); return entry; } } put_task_struct(task_s); put_pid(pid_struct); } return NULL; } void update_dci_cumulative_event_mask(int offset, uint8_t byte_mask, int token) { uint8_t *event_mask_ptr, *update_ptr = NULL; struct list_head *start, *temp; struct diag_dci_client_tbl *entry = NULL; bool is_set = false; mutex_lock(&dci_event_mask_mutex); update_ptr = dci_ops_tbl[token].event_mask_composite; if (!update_ptr) { mutex_unlock(&dci_event_mask_mutex); return; } update_ptr += offset; list_for_each_safe(start, temp, &driver->dci_client_list) { entry = list_entry(start, struct diag_dci_client_tbl, track); if (entry->client_info.token != token) continue; event_mask_ptr = entry->dci_event_mask; event_mask_ptr += offset; if ((*event_mask_ptr & byte_mask) == byte_mask) { is_set = true; /* break even if one client has the event mask set */ break; } } if (!is_set) *update_ptr &= ~byte_mask; else *update_ptr |= byte_mask; mutex_unlock(&dci_event_mask_mutex); } void diag_dci_invalidate_cumulative_event_mask(int token) { int i = 0; struct list_head *start, *temp; struct diag_dci_client_tbl *entry = NULL; uint8_t *event_mask_ptr, *update_ptr = NULL; mutex_lock(&dci_event_mask_mutex); update_ptr = dci_ops_tbl[token].event_mask_composite; if (!update_ptr) { mutex_unlock(&dci_event_mask_mutex); return; } create_dci_event_mask_tbl(update_ptr); list_for_each_safe(start, temp, &driver->dci_client_list) { entry = list_entry(start, struct diag_dci_client_tbl, track); if (entry->client_info.token != token) continue; event_mask_ptr = entry->dci_event_mask; for (i = 0; i < DCI_EVENT_MASK_SIZE; i++) *(update_ptr+i) |= *(event_mask_ptr+i); } mutex_unlock(&dci_event_mask_mutex); } #ifdef CONFIG_DIAGFWD_BRIDGE_CODE int diag_send_dci_event_mask_remote(int token) { unsigned char *buf = NULL; struct diag_dci_header_t dci_header; struct diag_ctrl_event_mask event_mask; int dci_header_size = sizeof(struct diag_dci_header_t); int event_header_size = sizeof(struct diag_ctrl_event_mask); int i, ret = DIAG_DCI_NO_ERROR, err = DIAG_DCI_NO_ERROR; unsigned char *event_mask_ptr = NULL; uint32_t write_len = 0; unsigned long flags; mutex_lock(&dci_event_mask_mutex); event_mask_ptr = dci_ops_tbl[token].event_mask_composite; if (!event_mask_ptr) { mutex_unlock(&dci_event_mask_mutex); return -EINVAL; } buf = dci_get_buffer_from_bridge(token); if (!buf) { pr_err("diag: In %s, unable to get dci buffers to write data\n", __func__); mutex_unlock(&dci_event_mask_mutex); return -EAGAIN; } /* Frame the DCI header */ dci_header.start = CONTROL_CHAR; dci_header.version = 1; dci_header.length = event_header_size + DCI_EVENT_MASK_SIZE + 1; dci_header.cmd_code = DCI_CONTROL_PKT_CODE; event_mask.cmd_type = DIAG_CTRL_MSG_EVENT_MASK; event_mask.data_len = EVENT_MASK_CTRL_HEADER_LEN + DCI_EVENT_MASK_SIZE; event_mask.stream_id = DCI_MASK_STREAM; event_mask.status = DIAG_CTRL_MASK_VALID; event_mask.event_config = 0; /* event config */ event_mask.event_mask_size = DCI_EVENT_MASK_SIZE; for (i = 0; i < DCI_EVENT_MASK_SIZE; i++) { if (event_mask_ptr[i] != 0) { event_mask.event_config = 1; break; } } memcpy(buf + write_len, &dci_header, dci_header_size); write_len += dci_header_size; memcpy(buf + write_len, &event_mask, event_header_size); write_len += event_header_size; memcpy(buf + write_len, event_mask_ptr, DCI_EVENT_MASK_SIZE); write_len += DCI_EVENT_MASK_SIZE; *(buf + write_len) = CONTROL_CHAR; /* End Terminator */ write_len += sizeof(uint8_t); err = diag_dci_write_bridge(token, buf, write_len); if (err) { pr_err("diag: error writing event mask to remote proc, token: %d, err: %d\n", token, err); spin_lock_irqsave(&driver->dci_mempool_lock, flags); diagmem_free(driver, buf, dci_ops_tbl[token].mempool); spin_unlock_irqrestore(&driver->dci_mempool_lock, flags); ret = err; } else { ret = DIAG_DCI_NO_ERROR; } mutex_unlock(&dci_event_mask_mutex); return ret; } #endif int diag_send_dci_event_mask(int token) { void *buf = event_mask.update_buf; struct diag_ctrl_event_mask header; int header_size = sizeof(struct diag_ctrl_event_mask); int ret = DIAG_DCI_NO_ERROR, err = DIAG_DCI_NO_ERROR, i; unsigned char *event_mask_ptr = NULL; mutex_lock(&dci_event_mask_mutex); event_mask_ptr = dci_ops_tbl[DCI_LOCAL_PROC].event_mask_composite; if (!event_mask_ptr) { mutex_unlock(&dci_event_mask_mutex); return -EINVAL; } mutex_lock(&event_mask.lock); /* send event mask update */ header.cmd_type = DIAG_CTRL_MSG_EVENT_MASK; header.data_len = EVENT_MASK_CTRL_HEADER_LEN + DCI_EVENT_MASK_SIZE; header.stream_id = DCI_MASK_STREAM; header.status = DIAG_CTRL_MASK_VALID; header.event_config = 0; /* event config */ header.event_mask_size = DCI_EVENT_MASK_SIZE; for (i = 0; i < DCI_EVENT_MASK_SIZE; i++) { if (event_mask_ptr[i] != 0) { header.event_config = 1; break; } } memcpy(buf, &header, header_size); memcpy(buf+header_size, event_mask_ptr, DCI_EVENT_MASK_SIZE); for (i = 0; i < NUM_PERIPHERALS; i++) { /* * Don't send to peripheral if its regular channel * is down. It may also mean that the peripheral doesn't * support DCI. */ if (check_peripheral_dci_support(i, DCI_LOCAL_PROC)) { err = diag_dci_write_proc(i, DIAG_CNTL_TYPE, buf, header_size + DCI_EVENT_MASK_SIZE); if (err != DIAG_DCI_NO_ERROR) ret = DIAG_DCI_SEND_DATA_FAIL; } } mutex_unlock(&event_mask.lock); mutex_unlock(&dci_event_mask_mutex); return ret; } void update_dci_cumulative_log_mask(int offset, unsigned int byte_index, uint8_t byte_mask, int token) { uint8_t *log_mask_ptr, *update_ptr = NULL; bool is_set = false; struct list_head *start, *temp; struct diag_dci_client_tbl *entry = NULL; mutex_lock(&dci_log_mask_mutex); update_ptr = dci_ops_tbl[token].log_mask_composite; if (!update_ptr) { mutex_unlock(&dci_log_mask_mutex); return; } update_ptr += offset; /* update the dirty bit */ *(update_ptr+1) = 1; update_ptr = update_ptr + byte_index; list_for_each_safe(start, temp, &driver->dci_client_list) { entry = list_entry(start, struct diag_dci_client_tbl, track); if (entry->client_info.token != token) continue; log_mask_ptr = entry->dci_log_mask; log_mask_ptr = log_mask_ptr + offset + byte_index; if ((*log_mask_ptr & byte_mask) == byte_mask) { is_set = true; /* break even if one client has the log mask set */ break; } } if (!is_set) *update_ptr &= ~byte_mask; else *update_ptr |= byte_mask; mutex_unlock(&dci_log_mask_mutex); } void diag_dci_invalidate_cumulative_log_mask(int token) { int i = 0; struct list_head *start, *temp; struct diag_dci_client_tbl *entry = NULL; uint8_t *log_mask_ptr, *update_ptr = NULL; /* Clear the composite mask and redo all the masks */ mutex_lock(&dci_log_mask_mutex); update_ptr = dci_ops_tbl[token].log_mask_composite; if (!update_ptr) { mutex_unlock(&dci_log_mask_mutex); return; } create_dci_log_mask_tbl(update_ptr, DCI_LOG_MASK_DIRTY); list_for_each_safe(start, temp, &driver->dci_client_list) { entry = list_entry(start, struct diag_dci_client_tbl, track); if (entry->client_info.token != token) continue; log_mask_ptr = entry->dci_log_mask; for (i = 0; i < DCI_LOG_MASK_SIZE; i++) *(update_ptr+i) |= *(log_mask_ptr+i); } mutex_unlock(&dci_log_mask_mutex); } static int dci_fill_log_mask(unsigned char *dest_ptr, unsigned char *src_ptr) { struct diag_ctrl_log_mask header; int header_len = sizeof(struct diag_ctrl_log_mask); header.cmd_type = DIAG_CTRL_MSG_LOG_MASK; header.num_items = LOG_SIZE_TO_ITEMS(DCI_MAX_ITEMS_PER_LOG_CODE); header.data_len = 11 + DCI_MAX_ITEMS_PER_LOG_CODE; header.stream_id = DCI_MASK_STREAM; header.status = 3; header.equip_id = *src_ptr; header.log_mask_size = DCI_MAX_ITEMS_PER_LOG_CODE; memcpy(dest_ptr, &header, header_len); memcpy(dest_ptr + header_len, src_ptr + 2, DCI_MAX_ITEMS_PER_LOG_CODE); return header_len + DCI_MAX_ITEMS_PER_LOG_CODE; } #ifdef CONFIG_DIAGFWD_BRIDGE_CODE int diag_send_dci_log_mask_remote(int token) { unsigned char *buf = NULL; struct diag_dci_header_t dci_header; int dci_header_size = sizeof(struct diag_dci_header_t); int log_header_size = sizeof(struct diag_ctrl_log_mask); uint8_t *log_mask_ptr = NULL; int i, ret = DIAG_DCI_NO_ERROR, err = DIAG_DCI_NO_ERROR; int updated; uint32_t write_len = 0; unsigned long flags; mutex_lock(&dci_log_mask_mutex); log_mask_ptr = dci_ops_tbl[token].log_mask_composite; if (!log_mask_ptr) { mutex_unlock(&dci_log_mask_mutex); return -EINVAL; } /* DCI header is common to all equipment IDs */ dci_header.start = CONTROL_CHAR; dci_header.version = 1; dci_header.length = log_header_size + DCI_MAX_ITEMS_PER_LOG_CODE + 1; dci_header.cmd_code = DCI_CONTROL_PKT_CODE; for (i = 0; i < DCI_MAX_LOG_CODES; i++) { updated = 1; write_len = 0; if (!*(log_mask_ptr + 1)) { log_mask_ptr += 514; continue; } buf = dci_get_buffer_from_bridge(token); if (!buf) { pr_err("diag: In %s, unable to get dci buffers to write data\n", __func__); mutex_unlock(&dci_log_mask_mutex); return -EAGAIN; } memcpy(buf + write_len, &dci_header, dci_header_size); write_len += dci_header_size; write_len += dci_fill_log_mask(buf + write_len, log_mask_ptr); *(buf + write_len) = CONTROL_CHAR; /* End Terminator */ write_len += sizeof(uint8_t); err = diag_dci_write_bridge(token, buf, write_len); if (err) { pr_err("diag: error writing log mask to remote processor, equip_id: %d, token: %d, err: %d\n", i, token, err); spin_lock_irqsave(&driver->dci_mempool_lock, flags); diagmem_free(driver, buf, dci_ops_tbl[token].mempool); spin_unlock_irqrestore(&driver->dci_mempool_lock, flags); updated = 0; } if (updated) *(log_mask_ptr + 1) = 0; /* clear dirty byte */ log_mask_ptr += 514; } mutex_unlock(&dci_log_mask_mutex); return ret; } #endif int diag_send_dci_log_mask(int token) { void *buf = log_mask.update_buf; int write_len = 0; uint8_t *log_mask_ptr = NULL; int i, j, ret = DIAG_DCI_NO_ERROR, err = DIAG_DCI_NO_ERROR; int updated; mutex_lock(&dci_log_mask_mutex); log_mask_ptr = dci_ops_tbl[DCI_LOCAL_PROC].log_mask_composite; if (!log_mask_ptr) { mutex_unlock(&dci_log_mask_mutex); return -EINVAL; } mutex_lock(&log_mask.lock); for (i = 0; i < 16; i++) { updated = 1; /* Dirty bit is set don't update the mask for this equip id */ if (!(*(log_mask_ptr + 1))) { log_mask_ptr += 514; continue; } write_len = dci_fill_log_mask(buf, log_mask_ptr); for (j = 0; j < NUM_PERIPHERALS && write_len; j++) { if (check_peripheral_dci_support(j, DCI_LOCAL_PROC)) { err = diag_dci_write_proc(j, DIAG_CNTL_TYPE, buf, write_len); if (err != DIAG_DCI_NO_ERROR) { updated = 0; ret = DIAG_DCI_SEND_DATA_FAIL; } } } if (updated) *(log_mask_ptr+1) = 0; /* clear dirty byte */ log_mask_ptr += 514; } mutex_unlock(&log_mask.lock); mutex_unlock(&dci_log_mask_mutex); return ret; } static int diag_dci_init_local(void) { struct dci_ops_tbl_t *temp = &dci_ops_tbl[DCI_LOCAL_PROC]; create_dci_log_mask_tbl(temp->log_mask_composite, DCI_LOG_MASK_CLEAN); create_dci_event_mask_tbl(temp->event_mask_composite); temp->peripheral_status |= DIAG_CON_APSS; return 0; } #ifdef CONFIG_DIAGFWD_BRIDGE_CODE static void diag_dci_init_handshake_remote(void) { int i; struct dci_channel_status_t *temp = NULL; for (i = DCI_REMOTE_BASE; i < NUM_DCI_PROC; i++) { temp = &dci_channel_status[i]; temp->id = i; INIT_WORK(&temp->handshake_work, dci_handshake_work_fn); timer_setup(&temp->wait_time, dci_chk_handshake, 0); } } int diag_dci_init_remote(void) { int i; struct dci_ops_tbl_t *temp = NULL; diagmem_init(driver, POOL_TYPE_MDM_DCI_WRITE); for (i = DCI_REMOTE_BASE; i < DCI_REMOTE_LAST; i++) { temp = &dci_ops_tbl[i]; create_dci_log_mask_tbl(temp->log_mask_composite, DCI_LOG_MASK_CLEAN); create_dci_event_mask_tbl(temp->event_mask_composite); } partial_pkt.data = vzalloc(MAX_DCI_PACKET_SZ); if (!partial_pkt.data) return -ENOMEM; partial_pkt.total_len = 0; partial_pkt.read_len = 0; partial_pkt.remaining = 0; partial_pkt.processing = 0; diag_dci_init_handshake_remote(); return 0; } #else int diag_dci_init_remote(void) { return 0; } #endif static int diag_dci_init_ops_tbl(void) { int err = 0; err = diag_dci_init_local(); if (err) goto err; return 0; err: return -ENOMEM; } int diag_dci_init(void) { int ret = 0, i; driver->dci_tag = 0; for (i = 0; i < MAX_DCI_CLIENTS; i++) driver->dci_client_id[i] = 0; driver->num_dci_client = 0; mutex_init(&driver->dci_mutex); mutex_init(&dci_log_mask_mutex); mutex_init(&dci_event_mask_mutex); spin_lock_init(&ws_lock); spin_lock_init(&driver->dci_mempool_lock); ret = diag_dci_init_ops_tbl(); if (ret) goto err; if (driver->apps_dci_buf == NULL) { driver->apps_dci_buf = vzalloc(DCI_BUF_SIZE); if (driver->apps_dci_buf == NULL) goto err; } INIT_LIST_HEAD(&driver->dci_client_list); INIT_LIST_HEAD(&driver->dci_req_list); driver->diag_dci_wq = create_singlethread_workqueue("diag_dci_wq"); if (!driver->diag_dci_wq) goto err; INIT_WORK(&dci_data_drain_work, dci_data_drain_work_fn); timer_setup(&dci_drain_timer, dci_drain_data, 0); return DIAG_DCI_NO_ERROR; err: pr_err("diag: Could not initialize diag DCI buffers\n"); vfree(driver->apps_dci_buf); driver->apps_dci_buf = NULL; if (driver->diag_dci_wq) destroy_workqueue(driver->diag_dci_wq); vfree(partial_pkt.data); partial_pkt.data = NULL; mutex_destroy(&driver->dci_mutex); mutex_destroy(&dci_log_mask_mutex); mutex_destroy(&dci_event_mask_mutex); return DIAG_DCI_NO_REG; } void diag_dci_channel_init(void) { uint8_t peripheral; for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) { diagfwd_open(peripheral, TYPE_DCI); diagfwd_open(peripheral, TYPE_DCI_CMD); } } void diag_dci_exit(void) { vfree(partial_pkt.data); partial_pkt.data = NULL; vfree(driver->apps_dci_buf); driver->apps_dci_buf = NULL; mutex_destroy(&driver->dci_mutex); mutex_destroy(&dci_log_mask_mutex); mutex_destroy(&dci_event_mask_mutex); destroy_workqueue(driver->diag_dci_wq); } int diag_dci_clear_log_mask(int client_id) { int err = DIAG_DCI_NO_ERROR, token = DCI_LOCAL_PROC; uint8_t *update_ptr; struct diag_dci_client_tbl *entry = NULL; entry = diag_dci_get_client_entry(client_id); if (!entry) { pr_err("diag: In %s, invalid client entry\n", __func__); return DIAG_DCI_TABLE_ERR; } token = entry->client_info.token; update_ptr = dci_ops_tbl[token].log_mask_composite; create_dci_log_mask_tbl(entry->dci_log_mask, DCI_LOG_MASK_CLEAN); diag_dci_invalidate_cumulative_log_mask(token); /* * Send updated mask to userspace clients only if the client * is registered on the local processor */ if (token == DCI_LOCAL_PROC) diag_update_userspace_clients(DCI_LOG_MASKS_TYPE); /* Send updated mask to peripherals */ err = dci_ops_tbl[token].send_log_mask(token); return err; } int diag_dci_clear_event_mask(int client_id) { int err = DIAG_DCI_NO_ERROR, token = DCI_LOCAL_PROC; uint8_t *update_ptr; struct diag_dci_client_tbl *entry = NULL; entry = diag_dci_get_client_entry(client_id); if (!entry) { pr_err("diag: In %s, invalid client entry\n", __func__); return DIAG_DCI_TABLE_ERR; } token = entry->client_info.token; update_ptr = dci_ops_tbl[token].event_mask_composite; create_dci_event_mask_tbl(entry->dci_event_mask); diag_dci_invalidate_cumulative_event_mask(token); /* * Send updated mask to userspace clients only if the client is * registerted on the local processor */ if (token == DCI_LOCAL_PROC) diag_update_userspace_clients(DCI_EVENT_MASKS_TYPE); /* Send updated mask to peripherals */ err = dci_ops_tbl[token].send_event_mask(token); return err; } uint8_t diag_dci_get_cumulative_real_time(int token) { uint8_t real_time = MODE_NONREALTIME; struct list_head *start, *temp; struct diag_dci_client_tbl *entry = NULL; list_for_each_safe(start, temp, &driver->dci_client_list) { entry = list_entry(start, struct diag_dci_client_tbl, track); if (entry->real_time == MODE_REALTIME && entry->client_info.token == token) { real_time = 1; break; } } return real_time; } int diag_dci_set_real_time(struct diag_dci_client_tbl *entry, uint8_t real_time) { if (!entry) { pr_err("diag: In %s, invalid client entry\n", __func__); return 0; } entry->real_time = real_time; return 1; } int diag_dci_register_client(struct diag_dci_reg_tbl_t *reg_entry) { int i, err = 0; struct diag_dci_client_tbl *new_entry = NULL; struct diag_dci_buf_peripheral_t *proc_buf = NULL; struct pid *pid_struct = NULL; struct task_struct *task_s = NULL; if (!reg_entry) return DIAG_DCI_NO_REG; if (!VALID_DCI_TOKEN(reg_entry->token)) { pr_alert("diag: Invalid DCI client token, %d\n", reg_entry->token); return DIAG_DCI_NO_REG; } if (driver->dci_state == DIAG_DCI_NO_REG) return DIAG_DCI_NO_REG; if (driver->num_dci_client >= MAX_DCI_CLIENTS) return DIAG_DCI_NO_REG; pid_struct = find_get_pid(current->tgid); if (!pid_struct) return DIAG_DCI_NO_REG; task_s = get_pid_task(pid_struct, PIDTYPE_PID); if (!task_s) { put_pid(pid_struct); return DIAG_DCI_NO_REG; } new_entry = kzalloc(sizeof(struct diag_dci_client_tbl), GFP_KERNEL); if (!new_entry) { put_pid(pid_struct); put_task_struct(task_s); return DIAG_DCI_NO_REG; } get_task_struct(task_s); mutex_lock(&driver->dci_mutex); new_entry->client = task_s; new_entry->tgid = current->tgid; new_entry->client_info.notification_list = reg_entry->notification_list; new_entry->client_info.signal_type = reg_entry->signal_type; new_entry->client_info.token = reg_entry->token; switch (reg_entry->token) { case DCI_LOCAL_PROC: new_entry->num_buffers = NUM_DCI_PERIPHERALS; break; case DCI_MDM_PROC: case DCI_MDM_2_PROC: new_entry->num_buffers = 1; break; } new_entry->buffers = NULL; new_entry->real_time = MODE_REALTIME; new_entry->in_service = 0; INIT_LIST_HEAD(&new_entry->list_write_buf); mutex_init(&new_entry->write_buf_mutex); new_entry->dci_log_mask = vzalloc(DCI_LOG_MASK_SIZE); if (!new_entry->dci_log_mask) { pr_err("diag: Unable to create log mask for DCI client, tgid: %d\n", current->tgid); goto fail_alloc; } create_dci_log_mask_tbl(new_entry->dci_log_mask, DCI_LOG_MASK_CLEAN); new_entry->dci_event_mask = vzalloc(DCI_EVENT_MASK_SIZE); if (!new_entry->dci_event_mask) goto fail_alloc; create_dci_event_mask_tbl(new_entry->dci_event_mask); new_entry->buffers = kcalloc(new_entry->num_buffers, sizeof(struct diag_dci_buf_peripheral_t), GFP_KERNEL); if (!new_entry->buffers) { pr_err("diag: Unable to allocate buffers for peripherals in %s\n", __func__); goto fail_alloc; } for (i = 0; i < new_entry->num_buffers; i++) { proc_buf = &new_entry->buffers[i]; if (!proc_buf) goto fail_alloc; mutex_init(&proc_buf->health_mutex); mutex_init(&proc_buf->buf_mutex); proc_buf->health.dropped_events = 0; proc_buf->health.dropped_logs = 0; proc_buf->health.received_events = 0; proc_buf->health.received_logs = 0; proc_buf->buf_primary = kzalloc( sizeof(struct diag_dci_buffer_t), GFP_KERNEL); if (!proc_buf->buf_primary) goto fail_alloc; proc_buf->buf_cmd = kzalloc(sizeof(struct diag_dci_buffer_t), GFP_KERNEL); if (!proc_buf->buf_cmd) goto fail_alloc; err = diag_dci_init_buffer(proc_buf->buf_primary, DCI_BUF_PRIMARY); if (err) goto fail_alloc; err = diag_dci_init_buffer(proc_buf->buf_cmd, DCI_BUF_CMD); if (err) goto fail_alloc; proc_buf->buf_curr = proc_buf->buf_primary; } for (i = 0; i < MAX_DCI_CLIENTS; i++) { if (driver->dci_client_id[i] == 0) break; } if (i == MAX_DCI_CLIENTS) goto fail_alloc; driver->dci_client_id[i] = 1; new_entry->client_info.client_id = i+1; reg_entry->client_id = i+1; driver->num_dci_client++; list_add_tail(&new_entry->track, &driver->dci_client_list); if (driver->num_dci_client == 1) diag_update_proc_vote(DIAG_PROC_DCI, VOTE_UP, reg_entry->token); queue_work(driver->diag_real_time_wq, &driver->diag_real_time_work); mutex_unlock(&driver->dci_mutex); put_pid(pid_struct); put_task_struct(task_s); return reg_entry->client_id; fail_alloc: if (new_entry) { for (i = 0; ((i < new_entry->num_buffers) && new_entry->buffers); i++) { proc_buf = &new_entry->buffers[i]; if (proc_buf) { mutex_destroy(&proc_buf->health_mutex); if (proc_buf->buf_primary) { vfree(proc_buf->buf_primary->data); proc_buf->buf_primary->data = NULL; mutex_destroy( &proc_buf->buf_primary->data_mutex); } kfree(proc_buf->buf_primary); proc_buf->buf_primary = NULL; if (proc_buf->buf_cmd) { vfree(proc_buf->buf_cmd->data); proc_buf->buf_cmd->data = NULL; mutex_destroy( &proc_buf->buf_cmd->data_mutex); } kfree(proc_buf->buf_cmd); proc_buf->buf_cmd = NULL; } } vfree(new_entry->dci_event_mask); new_entry->dci_event_mask = NULL; vfree(new_entry->dci_log_mask); new_entry->dci_log_mask = NULL; kfree(new_entry->buffers); new_entry->buffers = NULL; kfree(new_entry); new_entry = NULL; } mutex_unlock(&driver->dci_mutex); put_task_struct(task_s); put_task_struct(task_s); put_pid(pid_struct); return DIAG_DCI_NO_REG; } int diag_dci_deinit_client(struct diag_dci_client_tbl *entry) { int ret = DIAG_DCI_NO_ERROR, real_time = MODE_REALTIME, i, peripheral; struct diag_dci_buf_peripheral_t *proc_buf = NULL; struct diag_dci_buffer_t *buf_entry, *temp; struct list_head *start, *req_temp; struct dci_pkt_req_entry_t *req_entry = NULL; int token = DCI_LOCAL_PROC; if (!entry) return DIAG_DCI_NOT_SUPPORTED; token = entry->client_info.token; /* * Remove the entry from the list before freeing the buffers * to ensure that we don't have any invalid access. */ if (!list_empty(&entry->track)) list_del(&entry->track); if (entry->client_info.client_id > MAX_DCI_CLIENTS) return DIAG_DCI_NO_REG; driver->num_dci_client--; driver->dci_client_id[entry->client_info.client_id - 1] = 0; put_task_struct(entry->client); entry->client = NULL; /* * Clear the client's log and event masks, update the cumulative * masks and send the masks to peripherals */ vfree(entry->dci_log_mask); entry->dci_log_mask = NULL; diag_dci_invalidate_cumulative_log_mask(token); if (token == DCI_LOCAL_PROC) diag_update_userspace_clients(DCI_LOG_MASKS_TYPE); ret = dci_ops_tbl[token].send_log_mask(token); if (ret != DIAG_DCI_NO_ERROR) return ret; vfree(entry->dci_event_mask); entry->dci_event_mask = NULL; diag_dci_invalidate_cumulative_event_mask(token); if (token == DCI_LOCAL_PROC) diag_update_userspace_clients(DCI_EVENT_MASKS_TYPE); ret = dci_ops_tbl[token].send_event_mask(token); if (ret != DIAG_DCI_NO_ERROR) return ret; list_for_each_safe(start, req_temp, &driver->dci_req_list) { req_entry = list_entry(start, struct dci_pkt_req_entry_t, track); if (req_entry->client_id == entry->client_info.client_id) { if (!list_empty(&req_entry->track)) list_del(&req_entry->track); kfree(req_entry); req_entry = NULL; } } /* Clean up any buffer that is pending write */ mutex_lock(&entry->write_buf_mutex); list_for_each_entry_safe(buf_entry, temp, &entry->list_write_buf, buf_track) { if (!list_empty(&buf_entry->buf_track)) list_del(&buf_entry->buf_track); if (buf_entry->buf_type == DCI_BUF_SECONDARY) { mutex_lock(&buf_entry->data_mutex); diagmem_free(driver, buf_entry->data, POOL_TYPE_DCI); buf_entry->data = NULL; mutex_unlock(&buf_entry->data_mutex); kfree(buf_entry); buf_entry = NULL; } else if (buf_entry->buf_type == DCI_BUF_CMD) { peripheral = buf_entry->data_source; if (peripheral == APPS_DATA) continue; } /* * These are buffers that can't be written to the client which * means that the copy cannot be completed. Make sure that we * remove those references in DCI wakeup source. */ diag_ws_on_copy_fail(DIAG_WS_DCI); } mutex_unlock(&entry->write_buf_mutex); for (i = 0; i < entry->num_buffers; i++) { proc_buf = &entry->buffers[i]; buf_entry = proc_buf->buf_curr; mutex_lock(&proc_buf->buf_mutex); /* Clean up secondary buffer from mempool that is active */ if (buf_entry && buf_entry->buf_type == DCI_BUF_SECONDARY) { mutex_lock(&buf_entry->data_mutex); diagmem_free(driver, buf_entry->data, POOL_TYPE_DCI); buf_entry->data = NULL; mutex_unlock(&buf_entry->data_mutex); mutex_destroy(&buf_entry->data_mutex); kfree(buf_entry); buf_entry = NULL; } mutex_lock(&proc_buf->buf_primary->data_mutex); vfree(proc_buf->buf_primary->data); proc_buf->buf_primary->data = NULL; mutex_unlock(&proc_buf->buf_primary->data_mutex); mutex_lock(&proc_buf->buf_cmd->data_mutex); vfree(proc_buf->buf_cmd->data); proc_buf->buf_cmd->data = NULL; mutex_unlock(&proc_buf->buf_cmd->data_mutex); mutex_destroy(&proc_buf->health_mutex); mutex_destroy(&proc_buf->buf_primary->data_mutex); mutex_destroy(&proc_buf->buf_cmd->data_mutex); kfree(proc_buf->buf_primary); proc_buf->buf_primary = NULL; kfree(proc_buf->buf_cmd); proc_buf->buf_cmd = NULL; mutex_unlock(&proc_buf->buf_mutex); } mutex_destroy(&entry->write_buf_mutex); kfree(entry->buffers); entry->buffers = NULL; kfree(entry); entry = NULL; if (driver->num_dci_client == 0) { diag_update_proc_vote(DIAG_PROC_DCI, VOTE_DOWN, token); } else { real_time = diag_dci_get_cumulative_real_time(token); diag_update_real_time_vote(DIAG_PROC_DCI, real_time, token); } queue_work(driver->diag_real_time_wq, &driver->diag_real_time_work); return DIAG_DCI_NO_ERROR; } int diag_dci_write_proc(uint8_t peripheral, int pkt_type, char *buf, int len) { uint8_t dest_channel = TYPE_DATA; int err = 0; if (!buf || peripheral >= NUM_PERIPHERALS || len < 0 || !(driver->feature[PERIPHERAL_MODEM].rcvd_feature_mask)) { DIAG_LOG(DIAG_DEBUG_DCI, "buf: 0x%pK, p: %d, len: %d, f_mask: %d\n", buf, peripheral, len, driver->feature[PERIPHERAL_MODEM].rcvd_feature_mask); return -EINVAL; } if (pkt_type == DIAG_DATA_TYPE) { dest_channel = TYPE_DCI_CMD; } else if (pkt_type == DIAG_CNTL_TYPE) { dest_channel = TYPE_CNTL; } else { pr_err("diag: Invalid DCI pkt type in %s\n", __func__); return -EINVAL; } err = diagfwd_write(peripheral, dest_channel, buf, len); if (err && err != -ENODEV) { pr_err("diag: In %s, unable to write to peripheral: %d, type: %d, len: %d, err: %d\n", __func__, peripheral, dest_channel, len, err); } else { err = DIAG_DCI_NO_ERROR; } return err; } int diag_dci_copy_health_stats(struct diag_dci_health_stats_proc *stats_proc) { struct diag_dci_client_tbl *entry = NULL; struct diag_dci_health_t *health = NULL; struct diag_dci_health_stats *stats = NULL; int i, proc; if (!stats_proc) return -EINVAL; stats = &stats_proc->health; proc = stats_proc->proc; if (proc < ALL_PROC || proc > APPS_DATA) return -EINVAL; entry = diag_dci_get_client_entry(stats_proc->client_id); if (!entry) return DIAG_DCI_NOT_SUPPORTED; /* * If the client has registered for remote processor, the * proc field doesn't have any effect as they have only one buffer. */ if (entry->client_info.token) proc = 0; stats->stats.dropped_logs = 0; stats->stats.dropped_events = 0; stats->stats.received_logs = 0; stats->stats.received_events = 0; if (proc != ALL_PROC) { health = &entry->buffers[proc].health; stats->stats.dropped_logs = health->dropped_logs; stats->stats.dropped_events = health->dropped_events; stats->stats.received_logs = health->received_logs; stats->stats.received_events = health->received_events; if (stats->reset_status) { mutex_lock(&entry->buffers[proc].health_mutex); health->dropped_logs = 0; health->dropped_events = 0; health->received_logs = 0; health->received_events = 0; mutex_unlock(&entry->buffers[proc].health_mutex); } return DIAG_DCI_NO_ERROR; } for (i = 0; i < entry->num_buffers; i++) { health = &entry->buffers[i].health; stats->stats.dropped_logs += health->dropped_logs; stats->stats.dropped_events += health->dropped_events; stats->stats.received_logs += health->received_logs; stats->stats.received_events += health->received_events; if (stats->reset_status) { mutex_lock(&entry->buffers[i].health_mutex); health->dropped_logs = 0; health->dropped_events = 0; health->received_logs = 0; health->received_events = 0; mutex_unlock(&entry->buffers[i].health_mutex); } } return DIAG_DCI_NO_ERROR; } int diag_dci_get_support_list(struct diag_dci_peripherals_t *support_list) { if (!support_list) return -ENOMEM; if (!VALID_DCI_TOKEN(support_list->proc)) return -EIO; support_list->list = dci_ops_tbl[support_list->proc].peripheral_status; return DIAG_DCI_NO_ERROR; }