rubenslte/android_kernel_samsung_msm8226-rubenslte
1
Fork 0
forked from rubenslte/android_kernel_samsung_msm8226
Code Pull Requests Activity
Files
33fde7c79bdbc6e661638b5df4f03a10914fb2b5
android_kernel_samsung_msm8…/drivers/char/diag/diag_dci.c
T
Manoj Prabhu B ad3a244b23 diag: Prevent resource leakage of task structure 
The task structure with reference count incremented while
dci client is registered should be updated with reference count
decremented in failure case of registration.

Change-Id: I093229d83dca2699e0343224756895eff0915e38
Signed-off-by: Manoj Prabhu B <bmanoj@codeaurora.org>
CVE-2020-11160
Signed-off-by: Kevin F. Haggerty <haggertk@lineageos.org>
2021-02-10 10:58:45 +01:00

2445 lines
69 KiB
C
Raw Blame History

/* Copyright (c) 2012-2020, The Linux Foundation. All rights reserved.
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 and
* only version 2 as published by the Free Software Foundation.
*
* This program is distributed in the hope that it will be useful,
* but WITHOUT ANY WARRANTY; without even the implied warranty of
* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
* GNU General Public License for more details.
*/
#include <linux/slab.h>
#include <linux/init.h>
#include <linux/uaccess.h>
#include <linux/diagchar.h>
#include <linux/sched.h>
#include <linux/err.h>
#include <linux/delay.h>
#include <linux/workqueue.h>
#include <linux/pm_runtime.h>
#include <linux/platform_device.h>
#include <linux/pm_wakeup.h>
#include <linux/spinlock.h>
#include <linux/ratelimit.h>
#include <linux/reboot.h>
#include <asm/current.h>
#include <mach/restart.h>
#ifdef CONFIG_DIAG_OVER_USB
#include <mach/usbdiag.h>
#endif
#include "diagchar_hdlc.h"
#include "diagmem.h"
#include "diagchar.h"
#include "diagfwd.h"
#include "diagfwd_cntl.h"
#include "diag_dci.h"
static struct timer_list dci_drain_timer;
static int dci_timer_in_progress;
static struct work_struct dci_data_drain_work;
unsigned int dci_max_reg = 100;
unsigned int dci_max_clients = 10;
unsigned char dci_cumulative_log_mask[DCI_LOG_MASK_SIZE];
unsigned char dci_cumulative_event_mask[DCI_EVENT_MASK_SIZE];
struct mutex dci_log_mask_mutex;
struct mutex dci_event_mask_mutex;
struct mutex dci_health_mutex;
spinlock_t ws_lock;
unsigned long ws_lock_flags;
/* 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_data_smd;
struct mutex dci_stat_mutex;
void diag_dci_smd_record_info(int read_bytes, uint8_t ch_type,
uint8_t peripheral)
{
static int curr_dci_data_smd;
static unsigned long iteration;
struct diag_dci_data_info *temp_data = dci_data_smd;
if (!temp_data)
return;
mutex_lock(&dci_stat_mutex);
if (curr_dci_data_smd == DIAG_DCI_DEBUG_CNT)
curr_dci_data_smd = 0;
temp_data += curr_dci_data_smd;
temp_data->iteration = iteration + 1;
temp_data->data_size = read_bytes;
temp_data->peripheral = peripheral;
temp_data->ch_type = ch_type;
diag_get_timestamp(temp_data->time_stamp);
curr_dci_data_smd++;
iteration++;
mutex_unlock(&dci_stat_mutex);
}
#else
void diag_dci_smd_record_info(int read_bytes, uint8_t ch_type,
uint8_t peripheral) { }
#endif
static void dci_drain_data(unsigned long data)
{
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(500));
}
}
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->data = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
if (!buffer->data)
return -ENOMEM;
buffer->capacity = IN_BUF_SIZE;
break;
case DCI_BUF_SECONDARY:
buffer->data = NULL;
buffer->capacity = IN_BUF_SIZE;
break;
case DCI_BUF_CMD:
buffer->data = kzalloc(PKT_SIZE, GFP_KERNEL);
if (!buffer->data)
return -ENOMEM;
buffer->capacity = PKT_SIZE;
break;
default:
pr_err("diag: In %s, unknown type %d", __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);
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, driver->itemsize_dci,
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()
{
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);
/*
* 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);
}
}
}
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;
list_for_each_safe(start, temp, &driver->dci_client_list) {
entry = list_entry(start, struct diag_dci_client_tbl, track);
for (i = 0; i < NUM_DCI_PROC; i++) {
proc_buf = &entry->buffers[i];
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);
mutex_lock(&proc_buf->buf_mutex);
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);
}
}
dci_timer_in_progress = 0;
}
/* Process the data read from apps userspace client */
void diag_process_apps_dci_read_data(int data_type, void *buf, int recd_bytes)
{
uint8_t cmd_code;
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;
}
cmd_code = *(uint8_t *)buf;
switch (cmd_code) {
case LOG_CMD_CODE:
extract_dci_log(buf, recd_bytes, APPS_DATA);
break;
case EVENT_CMD_CODE:
extract_dci_events(buf, recd_bytes, APPS_DATA);
break;
case DCI_PKT_RSP_CODE:
case DCI_DELAYED_RSP_CODE:
extract_dci_pkt_rsp(buf, recd_bytes, APPS_DATA, NULL);
break;
default:
pr_err("diag: In %s, unsupported command code: 0x%x, not log or event\n",
__func__, cmd_code);
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 smd dci channel */
int diag_process_smd_dci_read_data(struct diag_smd_info *smd_info, void *buf,
int recd_bytes)
{
int read_bytes, dci_pkt_len;
uint8_t recv_pkt_cmd_code;
/*
* Release wakeup source when there are no more clients to
* process DCI data
*/
if (driver->num_dci_client == 0) {
diag_dci_try_deactivate_wakeup_source();
return 0;
}
diag_dci_smd_record_info(recd_bytes, (uint8_t)smd_info->type,
(uint8_t)smd_info->peripheral);
/* Each SMD read can have multiple DCI packets */
read_bytes = 0;
while (read_bytes < recd_bytes) {
/* read actual length of dci pkt */
dci_pkt_len = *(uint16_t *)(buf+2);
/* 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",
__func__, recd_bytes, dci_pkt_len);
diag_dci_try_deactivate_wakeup_source();
return 0;
}
/* process one dci packet */
pr_debug("diag: dci: peripheral = %d bytes read = %d, single dci pkt len = %d\n",
smd_info->peripheral, read_bytes, dci_pkt_len);
/* print_hex_dump(KERN_DEBUG, "Single DCI packet :",
DUMP_PREFIX_ADDRESS, 16, 1, buf, 5 + dci_pkt_len, 1); */
recv_pkt_cmd_code = *(uint8_t *)(buf+4);
if (recv_pkt_cmd_code == LOG_CMD_CODE) {
/* Don't include the 4 bytes for command code */
extract_dci_log(buf + 4, recd_bytes - 4,
smd_info->peripheral);
} else if (recv_pkt_cmd_code == EVENT_CMD_CODE) {
/* Don't include the 4 bytes for command code */
extract_dci_events(buf + 4, recd_bytes - 4,
smd_info->peripheral);
} else
extract_dci_pkt_rsp(buf + 4, dci_pkt_len,
smd_info->peripheral, smd_info);
read_bytes += 5 + dci_pkt_len;
buf += 5 + dci_pkt_len; /* advance to next DCI pkt */
}
/* wake up all sleeping DCI clients which have some data */
diag_dci_wakeup_clients();
dci_check_drain_timer();
diag_dci_try_deactivate_wakeup_source();
return 0;
}
static inline 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;
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) {
pr_err("diag: valid pid doesn't exist for pid = %d\n",
entry->tgid);
continue;
}
task_s = get_pid_task(pid_struct, PIDTYPE_PID);
if (!task_s) {
pr_err("diag: valid task doesn't exist for pid = %d\n",
entry->tgid);
continue;
}
if (task_s == entry->client)
if (entry->client->tgid == client_id)
return entry;
}
return NULL;
}
static inline 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;
}
static inline 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)
{
if (!header)
return -ENOMEM;
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 */
return 1;
}
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 */
return 1;
}
}
return 0;
}
static struct dci_pkt_req_entry_t *diag_register_dci_transaction(int uid)
{
struct dci_pkt_req_entry_t *entry = NULL;
entry = kzalloc(sizeof(struct dci_pkt_req_entry_t), GFP_KERNEL);
if (!entry)
return NULL;
mutex_lock(&driver->dci_mutex);
driver->dci_tag++;
entry->pid = current->tgid;
entry->uid = uid;
entry->tag = driver->dci_tag;
list_add_tail(&entry->track, &driver->dci_req_list);
mutex_unlock(&driver->dci_mutex);
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;
mutex_lock(&driver->dci_mutex);
if (!buf || len <= 0 || !entry) {
pr_err("diag: In %s, invalid input buf: %p, len: %d, entry: %p\n",
__func__, buf, len, entry);
mutex_unlock(&driver->dci_mutex);
return -EIO;
}
/* It is an immediate response, delete it from the table */
if (*buf != 0x80) {
list_del(&entry->track);
kfree(entry);
mutex_unlock(&driver->dci_mutex);
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);
mutex_unlock(&driver->dci_mutex);
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);
mutex_unlock(&driver->dci_mutex);
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);
mutex_unlock(&driver->dci_mutex);
return 1;
}
mutex_unlock(&driver->dci_mutex);
return 0;
}
void extract_dci_pkt_rsp(unsigned char *buf, int len, int data_source,
struct diag_smd_info *smd_info)
{
int tag, curr_client_pid = 0;
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;
if (!buf) {
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;
}
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)
*/
rsp_len = len - (cmd_code_len + sizeof(int));
/*
* Check if the length embedded in the packet is correct.
* Include the start (1), version (1), length (2) and the end
* (1) bytes while checking. Total = 5 bytes
*/
if ((rsp_len == 0) || (rsp_len > (len - 5))) {
pr_err("diag: Invalid length in %s, len: %d, rsp_len: %d",
__func__, len, rsp_len);
return;
}
req_entry = diag_dci_get_request_entry(tag);
if (!req_entry) {
pr_err("diag: No matching PID for DCI data\n");
return;
}
curr_client_pid = req_entry->pid;
save_req_uid = req_entry->uid;
/* Remove the headers and send only the response to this function */
mutex_lock(&driver->dci_mutex);
delete_flag = diag_dci_remove_req_entry(temp, rsp_len, req_entry);
if (delete_flag < 0) {
mutex_unlock(&driver->dci_mutex);
return;
}
mutex_unlock(&driver->dci_mutex);
entry = __diag_dci_get_client_entry(curr_client_pid);
if (!entry) {
pr_err("diag: In %s, couldn't find entry\n", __func__);
return;
}
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_PKT_RSP_TYPE header (int)
* + field for length (int) + delete_flag (uint8_t)
*/
if ((rsp_buf->data_len + 9 + rsp_len) > rsp_buf->capacity) {
pr_alert("diag: create capacity for pkt rsp\n");
rsp_buf->capacity += 9 + rsp_len;
temp_buf = krealloc(rsp_buf->data, rsp_buf->capacity,
GFP_KERNEL);
if (!temp_buf) {
pr_err("diag: DCI realloc failed\n");
mutex_unlock(&rsp_buf->data_mutex);
return;
} else {
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, &pkt_rsp_header,
sizeof(struct diag_dci_pkt_rsp_header_t));
rsp_buf->data_len += sizeof(struct diag_dci_pkt_rsp_header_t);
memcpy(rsp_buf->data + rsp_buf->data_len, temp, rsp_len);
rsp_buf->data_len += rsp_len;
rsp_buf->data_source = data_source;
if (smd_info)
smd_info->in_busy_1 = 1;
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);
}
static void copy_dci_event(unsigned char *buf, int len,
struct diag_dci_client_tbl *client, int data_source)
{
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", __func__);
return;
}
total_len = sizeof(int) + 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);
*(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)
{
uint16_t event_id, event_id_packet, length, temp_len;
uint8_t payload_len, payload_len_field;
uint8_t timestamp[8], 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.
*/
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 - 1)) {
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",
__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",
__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",
__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 */
list_for_each_safe(start, temp, &driver->dci_client_list) {
entry = list_entry(start, struct diag_dci_client_tbl,
track);
if (__diag_dci_query_event_mask(entry, event_id)) {
/* copy to client buffer */
copy_dci_event(event_data, total_event_len,
entry, data_source);
}
}
}
}
static void copy_dci_log(unsigned char *buf, int len,
struct diag_dci_client_tbl *client, int data_source)
{
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", __func__);
return;
}
log_length = *(uint16_t *)(buf + 2);
if (log_length > USHRT_MAX - 4) {
pr_err("diag: Integer overflow in %s, log_len: %d",
__func__, log_length);
return;
}
total_len = sizeof(int) + log_length;
/* 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",
__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;
}
*(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)
{
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 */
list_for_each_safe(start, temp, &driver->dci_client_list) {
entry = list_entry(start, struct diag_dci_client_tbl, track);
if (__diag_dci_query_log_mask(entry, log_code)) {
pr_debug("\t log code %x needed by client %d",
log_code, entry->client->tgid);
/* copy to client buffer */
copy_dci_log(buf, len, entry, data_source);
}
}
}
void diag_update_smd_dci_work_fn(struct work_struct *work)
{
struct diag_smd_info *smd_info = container_of(work,
struct diag_smd_info,
diag_notify_update_smd_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.
*/
list_for_each_safe(start, temp, &driver->dci_client_list) {
entry = list_entry(start, struct diag_dci_client_tbl, track);
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_lock(&dci_log_mask_mutex);
/* Update the appropriate dirty bits in the cumulative mask */
log_mask_ptr = dci_cumulative_log_mask;
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 = diag_send_dci_log_mask();
/* Send updated event mask to userspace clients */
diag_update_userspace_clients(DCI_EVENT_MASKS_TYPE);
/* Send updated event mask to peripheral */
ret = diag_send_dci_event_mask();
smd_info->notify_context = 0;
}
void diag_dci_notify_client(int peripheral_mask, int data)
{
int stat;
struct siginfo info;
struct list_head *start, *temp;
struct diag_dci_client_tbl *entry = NULL;
memset(&info, 0, sizeof(struct siginfo));
info.si_code = SI_QUEUE;
info.si_int = (peripheral_mask | data);
/* Notify the DCI process that the peripheral DCI Channel is up */
list_for_each_safe(start, temp, &driver->dci_client_list) {
entry = list_entry(start, struct diag_dci_client_tbl, track);
if (entry->client_info.notification_list & peripheral_mask) {
info.si_signo = entry->client_info.signal_type;
stat = send_sig_info(entry->client_info.signal_type,
&info, entry->client);
if (stat)
pr_err("diag: Err sending dci signal to client, signal data: 0x%x, stat: %d\n",
info.si_int, stat);
}
}
}
static int diag_send_dci_pkt(struct diag_master_table entry,
unsigned char *buf, int len, int tag)
{
int i, status = DIAG_DCI_NO_ERROR;
unsigned int read_len = 0;
/* The first 4 bytes is the uid tag and the next four bytes is
the minmum packet length of a request packet */
if (len < DCI_PKT_REQ_MIN_LEN) {
pr_err("diag: dci: Invalid pkt len %d in %s\n", len, __func__);
return -EIO;
}
if (len > APPS_BUF_SIZE - 10) {
pr_err("diag: dci: Invalid payload length in %s\n", __func__);
return -EIO;
}
/* remove UID from user space pkt before sending to peripheral*/
buf = buf + sizeof(int);
read_len += sizeof(int);
len = len - sizeof(int);
mutex_lock(&driver->dci_mutex);
/* prepare DCI packet */
driver->apps_dci_buf[0] = CONTROL_CHAR; /* start */
driver->apps_dci_buf[1] = 1; /* version */
*(uint16_t *)(driver->apps_dci_buf + 2) = len + 4 + 1; /* length */
driver->apps_dci_buf[4] = DCI_PKT_RSP_CODE;
*(int *)(driver->apps_dci_buf + 5) = tag;
for (i = 0; i < len; i++)
driver->apps_dci_buf[i+9] = *(buf+i);
read_len += len;
driver->apps_dci_buf[9+len] = CONTROL_CHAR; /* end */
if ((read_len + 9) >= USER_SPACE_DATA) {
pr_err("diag: dci: Invalid length while forming dci pkt in %s",
__func__);
mutex_unlock(&driver->dci_mutex);
return -EIO;
}
/* This command is registered locally on the Apps */
if (entry.client_id == APPS_DATA) {
driver->dci_pkt_length = len + 10;
diag_update_pkt_buffer(driver->apps_dci_buf, DCI_PKT_TYPE);
diag_update_sleeping_process(entry.process_id, DCI_PKT_TYPE);
mutex_unlock(&driver->dci_mutex);
return DIAG_DCI_NO_ERROR;
}
for (i = 0; i < NUM_SMD_DCI_CHANNELS; i++)
if (entry.client_id == i) {
status = 1;
break;
}
if (status) {
status = diag_dci_write_proc(entry.client_id,
DIAG_DATA_TYPE,
driver->apps_dci_buf,
len + 10);
} else {
pr_err("diag: Cannot send packet to peripheral %d",
entry.client_id);
status = DIAG_DCI_SEND_DATA_FAIL;
}
mutex_unlock(&driver->dci_mutex);
return status;
}
static int diag_dci_process_apps_pkt(struct diag_pkt_header_t *pkt_header,
unsigned char *req_buf, int tag)
{
uint8_t cmd_code, subsys_id, i, goto_download = 0;
uint8_t header_len = sizeof(struct diag_dci_pkt_header_t);
uint16_t ss_cmd_code;
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 || tag < 0)
return -EIO;
cmd_code = pkt_header->cmd_code;
subsys_id = pkt_header->subsys_id;
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) {
if (driver->log_on_demand_support) {
*payload_ptr = DIAG_CMD_LOG_ON_DMND;
write_len = sizeof(uint8_t);
payload_ptr += sizeof(uint8_t);
*(uint16_t *)(payload_ptr) = *(uint16_t *)(req_buf + 1);
write_len += sizeof(uint16_t);
payload_ptr += sizeof(uint16_t);
*payload_ptr = 0x1; /* Unknown */
write_len += sizeof(uint8_t);
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) = PKT_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);
}
}
fill_buffer:
if (write_len > 0) {
/* Check if we are within the range of the buffer*/
if (write_len + header_len > PKT_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 req_uid, ret = DIAG_DCI_TABLE_ERR, i;
struct diag_pkt_header_t *header = NULL;
unsigned char *temp = buf;
unsigned char *req_buf = NULL;
uint8_t retry_count = 0, max_retries = 3, found = 0;
uint32_t read_len = 0;
struct diag_master_table entry;
struct dci_pkt_req_entry_t *req_entry = NULL;
if (!buf)
return -EIO;
if (len < DCI_PKT_REQ_MIN_LEN || len > USER_SPACE_DATA) {
pr_err("diag: dci: Invalid length %d len in %s", len, __func__);
return -EIO;
}
req_uid = *(int *)temp; /* UID of the request */
temp += sizeof(int);
req_buf = temp; /* Start of the Request */
header = (struct diag_pkt_header_t *)temp;
temp += sizeof(struct diag_pkt_header_t);
read_len = sizeof(int) + sizeof(struct diag_pkt_header_t);
if (read_len >= USER_SPACE_DATA) {
pr_err("diag: dci: Invalid length in %s\n", __func__);
return -EIO;
}
/* Check if the command is allowed on DCI */
if (diag_dci_filter_commands(header)) {
pr_debug("diag: command not supported %d %d %d",
header->cmd_code, header->subsys_id,
header->subsys_cmd_code);
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__);
return -EAGAIN;
}
/* Register this new DCI packet */
req_entry = diag_register_dci_transaction(req_uid);
if (!req_entry) {
pr_alert("diag: registering new DCI transaction failed\n");
return DIAG_DCI_NO_REG;
}
/* Check if it is a dedicated Apps command */
ret = diag_dci_process_apps_pkt(header, req_buf, req_entry->tag);
if (ret == DIAG_DCI_NO_ERROR || ret < 0)
return ret;
/* Check the registration table for command entries */
for (i = 0; i < diag_max_reg && !found; i++) {
entry = driver->table[i];
if (entry.process_id == NO_PROCESS)
continue;
if (entry.cmd_code == header->cmd_code &&
entry.subsys_id == header->subsys_id &&
entry.cmd_code_lo <= header->subsys_cmd_code &&
entry.cmd_code_hi >= header->subsys_cmd_code) {
ret = diag_send_dci_pkt(entry, buf, len,
req_entry->tag);
found = 1;
} else if (entry.cmd_code == 255 && header->cmd_code == 75) {
if (entry.subsys_id == header->subsys_id &&
entry.cmd_code_lo <= header->subsys_cmd_code &&
entry.cmd_code_hi >= header->subsys_cmd_code) {
ret = diag_send_dci_pkt(entry, buf, len,
req_entry->tag);
found = 1;
}
} else if (entry.cmd_code == 255 && entry.subsys_id == 255) {
if (entry.cmd_code_lo <= header->cmd_code &&
entry.cmd_code_hi >= header->cmd_code) {
/*
* If its a Mode reset command, make sure it is
* registered on the Apps Processor
*/
if (entry.cmd_code_lo == MODE_CMD &&
entry.cmd_code_hi == MODE_CMD &&
header->subsys_id == RESET_ID) {
if (entry.client_id != APPS_DATA)
continue;
}
ret = diag_send_dci_pkt(entry, buf, len,
req_entry->tag);
found = 1;
}
}
}
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;
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) {
pr_err("diag: Invalid buffer in %s\n", __func__);
return -ENOMEM;
}
/* 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;
}
/* find client table entry */
dci_entry = diag_dci_get_client_entry();
if (!dci_entry) {
pr_err("diag: In %s, invalid client\n", __func__);
return ret;
}
/* Extract each log code and put in client table */
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);
if (num_codes == 0 || (num_codes >= (USER_SPACE_DATA - 8)/2)) {
pr_err("diag: dci: Invalid number of log codes %d\n",
num_codes);
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__);
return -ENOMEM;
}
pr_debug("diag: head of dci log mask %p\n", head_log_mask_ptr);
count = 0; /* iterator for extracting log codes */
while (count < num_codes) {
if (read_len >= USER_SPACE_DATA) {
pr_err("diag: dci: Invalid length for log type in %s",
__func__);
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");
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 %p\n",
equip_id, log_mask_ptr);
break;
} else {
pr_debug("diag: did not find equip id = %x at %p\n",
equip_id, log_mask_ptr);
log_mask_ptr += 514;
offset += 514;
}
}
if (!found) {
pr_err("diag: dci equip id not found\n");
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);
temp += 2;
read_len += 2;
count++;
ret = DIAG_DCI_NO_ERROR;
}
/* send updated mask to userspace clients */
diag_update_userspace_clients(DCI_LOG_MASKS_TYPE);
/* send updated mask to peripherals */
ret = diag_send_dci_log_mask();
} 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;
}
/* find client table entry */
dci_entry = diag_dci_get_client_entry();
if (!dci_entry) {
pr_err("diag: In %s, invalid client\n", __func__);
return ret;
}
/* Extract each log code and put in client table */
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);
/* 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);
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__);
return -ENOMEM;
}
pr_debug("diag: head of dci event mask %p\n", event_mask_ptr);
count = 0; /* iterator for extracting log codes */
while (count < num_codes) {
if (read_len >= USER_SPACE_DATA) {
pr_err("diag: dci: Invalid length for event type in %s",
__func__);
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");
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);
temp += sizeof(int);
read_len += sizeof(int);
count++;
ret = DIAG_DCI_NO_ERROR;
}
/* send updated mask to userspace clients */
diag_update_userspace_clients(DCI_EVENT_MASKS_TYPE);
/* send updated mask to peripherals */
ret = diag_send_dci_event_mask();
} else {
pr_alert("diag: Incorrect DCI transaction\n");
}
return ret;
}
struct diag_dci_client_tbl *diag_dci_get_client_entry()
{
return __diag_dci_get_client_entry(current->tgid);
}
void update_dci_cumulative_event_mask(int offset, uint8_t byte_mask)
{
uint8_t *event_mask_ptr;
uint8_t *update_ptr = dci_cumulative_event_mask;
struct list_head *start, *temp;
struct diag_dci_client_tbl *entry = NULL;
bool is_set = false;
mutex_lock(&dci_event_mask_mutex);
update_ptr += offset;
list_for_each_safe(start, temp, &driver->dci_client_list) {
entry = list_entry(start, struct diag_dci_client_tbl, track);
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 == false)
*update_ptr &= ~byte_mask;
else
*update_ptr |= byte_mask;
mutex_unlock(&dci_event_mask_mutex);
}
void diag_dci_invalidate_cumulative_event_mask()
{
int i = 0;
struct list_head *start, *temp;
struct diag_dci_client_tbl *entry = NULL;
uint8_t *update_ptr, *event_mask_ptr;
update_ptr = dci_cumulative_event_mask;
mutex_lock(&dci_event_mask_mutex);
list_for_each_safe(start, temp, &driver->dci_client_list) {
entry = list_entry(start, struct diag_dci_client_tbl, track);
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);
}
int diag_send_dci_event_mask()
{
void *buf = driver->buf_event_mask_update;
int header_size = sizeof(struct diag_ctrl_event_mask);
int ret = DIAG_DCI_NO_ERROR, err = DIAG_DCI_NO_ERROR, i;
mutex_lock(&driver->diag_cntl_mutex);
/* send event mask update */
driver->event_mask->cmd_type = DIAG_CTRL_MSG_EVENT_MASK;
driver->event_mask->data_len = 7 + DCI_EVENT_MASK_SIZE;
driver->event_mask->stream_id = DCI_MASK_STREAM;
driver->event_mask->status = 3; /* status for valid mask */
driver->event_mask->event_config = 0; /* event config */
driver->event_mask->event_mask_size = DCI_EVENT_MASK_SIZE;
for (i = 0; i < DCI_EVENT_MASK_SIZE; i++) {
if (dci_cumulative_event_mask[i] != 0) {
driver->event_mask->event_config = 1;
break;
}
}
memcpy(buf, driver->event_mask, header_size);
memcpy(buf+header_size, dci_cumulative_event_mask, DCI_EVENT_MASK_SIZE);
for (i = 0; i < NUM_SMD_DCI_CHANNELS; i++) {
/*
* Don't send to peripheral if its regular channel
* is down. It may also mean that the peripheral doesn't
* support DCI.
*/
if (!driver->smd_dci[i].ch)
continue;
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(&driver->diag_cntl_mutex);
return ret;
}
void update_dci_cumulative_log_mask(int offset, unsigned int byte_index,
uint8_t byte_mask)
{
int i;
uint8_t *update_ptr = dci_cumulative_log_mask;
uint8_t *log_mask_ptr;
bool is_set = false;
struct list_head *start, *temp;
struct diag_dci_client_tbl *entry = NULL;
mutex_lock(&dci_log_mask_mutex);
*update_ptr = 0;
/* set the equipment IDs */
for (i = 0; i < 16; i++)
*(update_ptr + (i*514)) = i;
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);
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 == false)
*update_ptr &= ~byte_mask;
else
*update_ptr |= byte_mask;
mutex_unlock(&dci_log_mask_mutex);
}
void diag_dci_invalidate_cumulative_log_mask()
{
int i = 0;
struct list_head *start, *temp;
struct diag_dci_client_tbl *entry = NULL;
uint8_t *update_ptr, *log_mask_ptr;
update_ptr = dci_cumulative_log_mask;
mutex_lock(&dci_log_mask_mutex);
list_for_each_safe(start, temp, &driver->dci_client_list) {
entry = list_entry(start, struct diag_dci_client_tbl, track);
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);
}
int diag_send_dci_log_mask()
{
void *buf = driver->buf_log_mask_update;
int header_size = sizeof(struct diag_ctrl_log_mask);
uint8_t *log_mask_ptr = dci_cumulative_log_mask;
int i, j, ret = DIAG_DCI_NO_ERROR, err = DIAG_DCI_NO_ERROR;
int updated;
mutex_lock(&driver->diag_cntl_mutex);
for (i = 0; i < 16; i++) {
updated = 1;
driver->log_mask->cmd_type = DIAG_CTRL_MSG_LOG_MASK;
driver->log_mask->num_items = 512;
driver->log_mask->data_len = 11 + 512;
driver->log_mask->stream_id = DCI_MASK_STREAM;
driver->log_mask->status = 3; /* status for valid mask */
driver->log_mask->equip_id = *log_mask_ptr;
driver->log_mask->log_mask_size = 512;
memcpy(buf, driver->log_mask, header_size);
memcpy(buf+header_size, log_mask_ptr+2, 512);
/* if dirty byte is set and channel is valid */
for (j = 0; j < NUM_SMD_DCI_CHANNELS; j++) {
/*
* Don't send to peripheral if its regular channel
* is down. It may also mean that the peripheral
* doesn't support DCI.
*/
if (!driver->smd_dci[j].ch)
continue;
if (!(*(log_mask_ptr+1)))
continue;
err = diag_dci_write_proc(j, DIAG_CNTL_TYPE, buf,
header_size + DCI_MAX_ITEMS_PER_LOG_CODE);
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(&driver->diag_cntl_mutex);
return ret;
}
void create_dci_log_mask_tbl(unsigned char *tbl_buf)
{
uint8_t i; int count = 0;
if (!tbl_buf)
return;
/* create hard coded table for log mask with 16 categories */
for (i = 0; i < 16; i++) {
*(uint8_t *)tbl_buf = i;
pr_debug("diag: put value %x at %p\n", i, tbl_buf);
memset(tbl_buf+1, 0, 513); /* set dirty bit as 0 */
tbl_buf += 514;
count += 514;
}
}
void create_dci_event_mask_tbl(unsigned char *tbl_buf)
{
memset(tbl_buf, 0, 512);
}
static int diag_dci_probe(struct platform_device *pdev)
{
int err = 0;
int index;
if (pdev->id == SMD_APPS_MODEM) {
index = MODEM_DATA;
err = smd_named_open_on_edge("DIAG_2",
SMD_APPS_MODEM,
&driver->smd_dci[index].ch,
&driver->smd_dci[index],
diag_smd_notify);
driver->smd_dci[index].ch_save =
driver->smd_dci[index].ch;
if (err)
pr_err("diag: In %s, cannot open DCI Modem port, Id = %d, err: %d\n",
__func__, pdev->id, err);
}
return err;
}
static int diag_dci_cmd_probe(struct platform_device *pdev)
{
int err = 0;
int index;
if (pdev->id == SMD_APPS_MODEM) {
index = MODEM_DATA;
err = smd_named_open_on_edge("DIAG_2_CMD",
pdev->id,
&driver->smd_dci_cmd[index].ch,
&driver->smd_dci_cmd[index],
diag_smd_notify);
driver->smd_dci_cmd[index].ch_save =
driver->smd_dci_cmd[index].ch;
if (err)
pr_err("diag: In %s, cannot open DCI Modem CMD port, Id = %d, err: %d\n",
__func__, pdev->id, err);
}
return err;
}
static int diag_dci_runtime_suspend(struct device *dev)
{
dev_dbg(dev, "pm_runtime: suspending...\n");
return 0;
}
static int diag_dci_runtime_resume(struct device *dev)
{
dev_dbg(dev, "pm_runtime: resuming...\n");
return 0;
}
static const struct dev_pm_ops diag_dci_dev_pm_ops = {
.runtime_suspend = diag_dci_runtime_suspend,
.runtime_resume = diag_dci_runtime_resume,
};
struct platform_driver msm_diag_dci_driver = {
.probe = diag_dci_probe,
.driver = {
.name = "DIAG_2",
.owner = THIS_MODULE,
.pm = &diag_dci_dev_pm_ops,
},
};
struct platform_driver msm_diag_dci_cmd_driver = {
.probe = diag_dci_cmd_probe,
.driver = {
.name = "DIAG_2_CMD",
.owner = THIS_MODULE,
.pm = &diag_dci_dev_pm_ops,
},
};
int diag_dci_init(void)
{
int success = 0;
int i;
driver->dci_tag = 0;
driver->dci_client_id = 0;
driver->num_dci_client = 0;
mutex_init(&driver->dci_mutex);
mutex_init(&dci_log_mask_mutex);
mutex_init(&dci_event_mask_mutex);
mutex_init(&dci_health_mutex);
spin_lock_init(&ws_lock);
for (i = 0; i < NUM_SMD_DCI_CHANNELS; i++) {
success = diag_smd_constructor(&driver->smd_dci[i], i,
SMD_DCI_TYPE);
if (!success)
goto err;
}
if (driver->supports_separate_cmdrsp) {
for (i = 0; i < NUM_SMD_DCI_CMD_CHANNELS; i++) {
success = diag_smd_constructor(&driver->smd_dci_cmd[i],
i, SMD_DCI_CMD_TYPE);
if (!success)
goto err;
}
}
if (driver->apps_dci_buf == NULL) {
driver->apps_dci_buf = kzalloc(APPS_BUF_SIZE, GFP_KERNEL);
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");
INIT_WORK(&dci_data_drain_work, dci_data_drain_work_fn);
success = platform_driver_register(&msm_diag_dci_driver);
if (success) {
pr_err("diag: Could not register DCI driver\n");
goto err;
}
if (driver->supports_separate_cmdrsp) {
success = platform_driver_register(&msm_diag_dci_cmd_driver);
if (success) {
pr_err("diag: Could not register DCI cmd driver\n");
goto err;
}
}
setup_timer(&dci_drain_timer, dci_drain_data, 0);
return DIAG_DCI_NO_ERROR;
err:
pr_err("diag: Could not initialize diag DCI buffers");
kfree(driver->apps_dci_buf);
for (i = 0; i < NUM_SMD_DCI_CHANNELS; i++)
diag_smd_destructor(&driver->smd_dci[i]);
if (driver->supports_separate_cmdrsp)
for (i = 0; i < NUM_SMD_DCI_CMD_CHANNELS; i++)
diag_smd_destructor(&driver->smd_dci_cmd[i]);
if (driver->diag_dci_wq)
destroy_workqueue(driver->diag_dci_wq);
mutex_destroy(&driver->dci_mutex);
mutex_destroy(&dci_log_mask_mutex);
mutex_destroy(&dci_event_mask_mutex);
mutex_destroy(&dci_health_mutex);
return DIAG_DCI_NO_REG;
}
void diag_dci_exit(void)
{
int i;
for (i = 0; i < NUM_SMD_DCI_CHANNELS; i++)
diag_smd_destructor(&driver->smd_dci[i]);
platform_driver_unregister(&msm_diag_dci_driver);
if (driver->supports_separate_cmdrsp) {
for (i = 0; i < NUM_SMD_DCI_CMD_CHANNELS; i++)
diag_smd_destructor(&driver->smd_dci_cmd[i]);
platform_driver_unregister(&msm_diag_dci_cmd_driver);
}
kfree(driver->apps_dci_buf);
mutex_destroy(&driver->dci_mutex);
mutex_destroy(&dci_log_mask_mutex);
mutex_destroy(&dci_event_mask_mutex);
mutex_destroy(&dci_health_mutex);
destroy_workqueue(driver->diag_dci_wq);
}
int diag_dci_clear_log_mask()
{
int j, k, err = DIAG_DCI_NO_ERROR;
uint8_t *log_mask_ptr, *update_ptr;
struct list_head *start, *temp;
struct diag_dci_client_tbl *entry = NULL;
entry = diag_dci_get_client_entry();
if (!entry) {
pr_err("diag: In %s, invalid client entry\n", __func__);
return DIAG_DCI_TABLE_ERR;
}
mutex_lock(&dci_log_mask_mutex);
create_dci_log_mask_tbl(entry->dci_log_mask);
memset(dci_cumulative_log_mask, 0x0, DCI_LOG_MASK_SIZE);
list_for_each_safe(start, temp, &driver->dci_client_list) {
entry = list_entry(start, struct diag_dci_client_tbl, track);
update_ptr = dci_cumulative_log_mask;
log_mask_ptr = entry->dci_log_mask;
for (j = 0; j < 16; j++) {
*update_ptr = j;
*(update_ptr + 1) = 1;
update_ptr += 2;
log_mask_ptr += 2;
for (k = 0; k < 513; k++) {
*update_ptr |= *log_mask_ptr;
update_ptr++;
log_mask_ptr++;
}
}
}
mutex_unlock(&dci_log_mask_mutex);
/* send updated mask to userspace clients */
diag_update_userspace_clients(DCI_LOG_MASKS_TYPE);
/* Send updated mask to peripherals */
err = diag_send_dci_log_mask();
return err;
}
int diag_dci_clear_event_mask()
{
int j, err = DIAG_DCI_NO_ERROR;
uint8_t *event_mask_ptr, *update_ptr;
struct list_head *start, *temp;
struct diag_dci_client_tbl *entry = NULL;
entry = diag_dci_get_client_entry();
if (!entry) {
pr_err("diag: In %s, invalid client entry\n", __func__);
return DIAG_DCI_TABLE_ERR;
}
mutex_lock(&dci_event_mask_mutex);
memset(entry->dci_event_mask, 0x0, DCI_EVENT_MASK_SIZE);
memset(dci_cumulative_event_mask, 0x0, DCI_EVENT_MASK_SIZE);
update_ptr = dci_cumulative_event_mask;
list_for_each_safe(start, temp, &driver->dci_client_list) {
entry = list_entry(start, struct diag_dci_client_tbl, track);
event_mask_ptr = entry->dci_event_mask;
for (j = 0; j < DCI_EVENT_MASK_SIZE; j++)
*(update_ptr + j) |= *(event_mask_ptr + j);
}
mutex_unlock(&dci_event_mask_mutex);
/* send updated mask to userspace clients */
diag_update_userspace_clients(DCI_EVENT_MASKS_TYPE);
/* Send updated mask to peripherals */
err = diag_send_dci_event_mask();
return err;
}
int diag_dci_query_log_mask(uint16_t log_code)
{
return __diag_dci_query_log_mask(diag_dci_get_client_entry(),
log_code);
}
int diag_dci_query_event_mask(uint16_t event_id)
{
return __diag_dci_query_event_mask(diag_dci_get_client_entry(),
event_id);
}
uint8_t diag_dci_get_cumulative_real_time()
{
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) {
real_time = 1;
break;
}
}
return real_time;
}
int diag_dci_set_real_time(uint8_t real_time)
{
struct diag_dci_client_tbl *entry = NULL;
entry = diag_dci_get_client_entry();
if (!entry) {
pr_err("diag: In %s, invalid client entry\n", __func__);
return 0;
}
entry->real_time = real_time;
return 1;
}
void diag_dci_try_activate_wakeup_source()
{
spin_lock_irqsave(&ws_lock, ws_lock_flags);
pm_wakeup_event(driver->diag_dev, DCI_WAKEUP_TIMEOUT);
pm_stay_awake(driver->diag_dev);
spin_unlock_irqrestore(&ws_lock, ws_lock_flags);
}
void diag_dci_try_deactivate_wakeup_source()
{
spin_lock_irqsave(&ws_lock, ws_lock_flags);
pm_relax(driver->diag_dev);
spin_unlock_irqrestore(&ws_lock, ws_lock_flags);
}
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;
if (!reg_entry)
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;
new_entry = kzalloc(sizeof(struct diag_dci_client_tbl), GFP_KERNEL);
if (new_entry == NULL) {
pr_err("diag: unable to alloc memory\n");
return DIAG_DCI_NO_REG;
}
mutex_lock(&driver->dci_mutex);
if (!(driver->num_dci_client)) {
for (i = 0; i < NUM_SMD_DCI_CHANNELS; i++)
driver->smd_dci[i].in_busy_1 = 0;
if (driver->supports_separate_cmdrsp)
for (i = 0; i < NUM_SMD_DCI_CMD_CHANNELS; i++)
driver->smd_dci_cmd[i].in_busy_1 = 0;
}
new_entry->client = current;
new_entry->client_info.notification_list =
reg_entry->notification_list;
new_entry->client_info.signal_type =
reg_entry->signal_type;
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 = kzalloc(DCI_LOG_MASK_SIZE, GFP_KERNEL);
if (!new_entry->dci_log_mask) {
pr_err("diag: Unable to create log mask for client, %d",
driver->dci_client_id);
goto fail_alloc;
}
create_dci_log_mask_tbl(new_entry->dci_log_mask);
new_entry->dci_event_mask = kzalloc(DCI_EVENT_MASK_SIZE, GFP_KERNEL);
if (!new_entry->dci_event_mask) {
pr_err("diag: Unable to create event mask for client, %d",
driver->dci_client_id);
goto fail_alloc;
}
create_dci_event_mask_tbl(new_entry->dci_event_mask);
for (i = 0; i < NUM_DCI_PROC; 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;
}
list_add_tail(&new_entry->track, &driver->dci_client_list);
driver->dci_client_id++;
new_entry->client_info.client_id = driver->dci_client_id;
reg_entry->client_id = driver->dci_client_id;
driver->num_dci_client++;
if (driver->num_dci_client == 1)
diag_update_proc_vote(DIAG_PROC_DCI, VOTE_UP);
queue_work(driver->diag_real_time_wq, &driver->diag_real_time_work);
mutex_unlock(&driver->dci_mutex);
return driver->dci_client_id;
fail_alloc:
if (new_entry) {
for (i = 0; i < NUM_DCI_PROC; i++) {
proc_buf = &new_entry->buffers[i];
mutex_destroy(&proc_buf->health_mutex);
mutex_destroy(&proc_buf->buf_primary->data_mutex);
mutex_destroy(&proc_buf->buf_cmd->data_mutex);
if (proc_buf->buf_primary)
kfree(proc_buf->buf_primary->data);
kfree(proc_buf->buf_primary);
if (proc_buf->buf_cmd)
kfree(proc_buf->buf_cmd->data);
kfree(proc_buf->buf_cmd);
}
kfree(new_entry->dci_event_mask);
kfree(new_entry->dci_log_mask);
}
kfree(new_entry);
put_task_struct(current);
mutex_unlock(&driver->dci_mutex);
return DIAG_DCI_NO_REG;
}
int diag_dci_deinit_client()
{
int ret = DIAG_DCI_NO_ERROR, real_time = MODE_REALTIME, i, peripheral;
struct diag_dci_buf_peripheral_t *proc_buf = NULL;
struct diag_dci_client_tbl *entry = diag_dci_get_client_entry();
struct diag_dci_buffer_t *buf_entry, *temp;
struct list_head *start, *req_temp;
struct dci_pkt_req_entry_t *req_entry = NULL;
struct diag_smd_info *smd_info = NULL;
if (!entry)
return DIAG_DCI_NOT_SUPPORTED;
mutex_lock(&driver->dci_mutex);
/*
* Remove the entry from the list before freeing the buffers
* to ensure that we don't have any invalid access.
*/
list_del(&entry->track);
driver->num_dci_client--;
/*
* Clear the client's log and event masks, update the cumulative
* masks and send the masks to peripherals
*/
kfree(entry->dci_log_mask);
diag_update_userspace_clients(DCI_LOG_MASKS_TYPE);
diag_dci_invalidate_cumulative_log_mask();
ret = diag_send_dci_event_mask();
if (ret != DIAG_DCI_NO_ERROR) {
mutex_unlock(&driver->dci_mutex);
return ret;
}
kfree(entry->dci_event_mask);
diag_update_userspace_clients(DCI_EVENT_MASKS_TYPE);
diag_dci_invalidate_cumulative_event_mask();
ret = diag_send_dci_log_mask();
if (ret != DIAG_DCI_NO_ERROR) {
mutex_unlock(&driver->dci_mutex);
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->pid == current->tgid) {
list_del(&req_entry->track);
kfree(req_entry);
}
}
/* 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) {
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);
} else if (buf_entry->buf_type == DCI_BUF_CMD) {
peripheral = buf_entry->data_source;
if (peripheral == APPS_DATA)
continue;
mutex_lock(&buf_entry->data_mutex);
smd_info = driver->separate_cmdrsp[peripheral] ?
&driver->smd_dci_cmd[peripheral] :
&driver->smd_dci[peripheral];
smd_info->in_busy_1 = 0;
mutex_unlock(&buf_entry->data_mutex);
}
diag_dci_try_deactivate_wakeup_source();
}
mutex_unlock(&entry->write_buf_mutex);
for (i = 0; i < NUM_DCI_PROC; 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);
}
mutex_lock(&proc_buf->buf_primary->data_mutex);
kfree(proc_buf->buf_primary->data);
mutex_unlock(&proc_buf->buf_primary->data_mutex);
mutex_lock(&proc_buf->buf_cmd->data_mutex);
kfree(proc_buf->buf_cmd->data);
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);
kfree(proc_buf->buf_cmd);
mutex_unlock(&proc_buf->buf_mutex);
}
mutex_destroy(&entry->write_buf_mutex);
kfree(entry);
if (driver->num_dci_client == 0) {
diag_update_proc_vote(DIAG_PROC_DCI, VOTE_DOWN);
} else {
real_time = diag_dci_get_cumulative_real_time();
diag_update_real_time_vote(DIAG_PROC_DCI, real_time);
}
queue_work(driver->diag_real_time_wq, &driver->diag_real_time_work);
mutex_unlock(&driver->dci_mutex);
return DIAG_DCI_NO_ERROR;
}
int diag_dci_write_proc(int peripheral, int pkt_type, char *buf, int len)
{
struct diag_smd_info *smd_info = NULL;
int wr_size = 0, retry = 0, err = -EAGAIN, timer = 0, i;
if (!buf || (peripheral < 0 || peripheral > NUM_SMD_DCI_CHANNELS)
|| len < 0) {
pr_err("diag: In %s, invalid data 0x%p, peripheral: %d, len: %d\n",
__func__, buf, peripheral, len);
return -EINVAL;
}
if (pkt_type == DIAG_DATA_TYPE) {
for (i = 0; i < NUM_SMD_DCI_CMD_CHANNELS; i++)
if (peripheral == i)
smd_info = &driver->smd_dci_cmd[peripheral];
/*
* This peripheral doesn't support separate channel for
* command response.
*/
if (!smd_info)
smd_info = &driver->smd_dci[peripheral];
} else if (pkt_type == DIAG_CNTL_TYPE) {
smd_info = &driver->smd_cntl[peripheral];
} else {
pr_err("diag: Invalid DCI pkt type in %s", __func__);
return -EINVAL;
}
if (!smd_info || !smd_info->ch)
return -EINVAL;
while (retry < 3) {
mutex_lock(&smd_info->smd_ch_mutex);
wr_size = smd_write(smd_info->ch, buf, len);
if (wr_size == len) {
pr_debug("diag: successfully wrote pkt_type %d of len %d to %d in trial %d",
pkt_type, len, peripheral, (retry+1));
err = DIAG_DCI_NO_ERROR;
mutex_unlock(&smd_info->smd_ch_mutex);
break;
}
pr_debug("diag: cannot write pkt_type %d of len %d to %d in trial %d",
pkt_type, len, peripheral, (retry+1));
retry++;
mutex_unlock(&smd_info->smd_ch_mutex);
/*
* Sleep for sometime before retrying. The delay of 2000 was
* determined empirically as best value to use.
*/
for (timer = 0; timer < 5; timer++)
usleep(2000);
}
return err;
}
int diag_dci_copy_health_stats(struct diag_dci_health_stats *stats, int proc)
{
struct diag_dci_client_tbl *entry = NULL;
struct diag_dci_health_t *health = NULL;
int i;
if (!stats)
return -EINVAL;
if (proc < ALL_PROC || proc > APPS_DATA)
return -EINVAL;
entry = diag_dci_get_client_entry();
if (!entry)
return DIAG_DCI_NOT_SUPPORTED;
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 < NUM_DCI_PROC; 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;
}
View Git Blame Copy Permalink