M7350/kernel/drivers/char/diag/diag_dci.c
2024-09-09 08:52:07 +00:00

1350 lines
37 KiB
C

/* Copyright (c) 2012-2013, 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 <asm/current.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"
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;
#define DCI_CHK_CAPACITY(entry, new_data_len) \
((entry->data_len + new_data_len > entry->total_capacity) ? 1 : 0) \
/* 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, i;
uint8_t recv_pkt_cmd_code;
/* 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);
/* process one dci packet */
pr_debug("diag: bytes read = %d, single dci pkt len = %d\n",
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)
extract_dci_log(buf+4);
else if (recv_pkt_cmd_code == EVENT_CMD_CODE)
extract_dci_events(buf+4);
else
extract_dci_pkt_rsp(smd_info, buf); /* pkt response */
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 */
for (i = 0; i < MAX_DCI_CLIENTS; i++) {
if (driver->dci_client_tbl[i].client &&
driver->dci_client_tbl[i].data_len) {
smd_info->in_busy_1 = 1;
diag_update_sleeping_process(
driver->dci_client_tbl[i].client->tgid,
DCI_DATA_TYPE);
}
}
return 0;
}
void extract_dci_pkt_rsp(struct diag_smd_info *smd_info, unsigned char *buf)
{
int i = 0, index = -1, cmd_code_len = 1;
int curr_client_pid = 0, write_len;
struct diag_dci_client_tbl *entry;
void *temp_buf = NULL;
uint8_t recv_pkt_cmd_code;
recv_pkt_cmd_code = *(uint8_t *)(buf+4);
if (recv_pkt_cmd_code != DCI_PKT_RSP_CODE)
cmd_code_len = 4; /* delayed response */
write_len = (int)(*(uint16_t *)(buf+2)) - cmd_code_len;
if (write_len <= 0) {
pr_err("diag: Invalid length in %s, write_len: %d",
__func__, write_len);
return;
}
pr_debug("diag: len = %d\n", write_len);
/* look up DCI client with tag */
for (i = 0; i < dci_max_reg; i++) {
if (driver->req_tracking_tbl[i].tag ==
*(int *)(buf+(4+cmd_code_len))) {
*(int *)(buf+4+cmd_code_len) =
driver->req_tracking_tbl[i].uid;
curr_client_pid =
driver->req_tracking_tbl[i].pid;
index = i;
break;
}
}
if (index == -1) {
pr_alert("diag: No matching PID for DCI data\n");
return;
}
/* Using PID of client process, find client buffer */
i = diag_dci_find_client_index(curr_client_pid);
if (i != DCI_CLIENT_INDEX_INVALID) {
/* copy pkt rsp in client buf */
entry = &(driver->dci_client_tbl[i]);
if (DCI_CHK_CAPACITY(entry, 8+write_len)) {
pr_alert("diag: create capacity for pkt rsp\n");
entry->total_capacity += 8+write_len;
temp_buf = krealloc(entry->dci_data,
entry->total_capacity, GFP_KERNEL);
if (!temp_buf) {
pr_err("diag: DCI realloc failed\n");
return;
} else {
entry->dci_data = temp_buf;
}
}
*(int *)(entry->dci_data+entry->data_len) =
DCI_PKT_RSP_TYPE;
entry->data_len += 4;
*(int *)(entry->dci_data+entry->data_len)
= write_len;
entry->data_len += 4;
memcpy(entry->dci_data+entry->data_len,
buf+4+cmd_code_len, write_len);
entry->data_len += write_len;
/* delete immediate response entry */
if (smd_info->buf_in_1[8+cmd_code_len] != 0x80)
driver->req_tracking_tbl[index].pid = 0;
}
}
void extract_dci_events(unsigned char *buf)
{
uint16_t event_id, event_id_packet, length, temp_len;
uint8_t *event_mask_ptr, byte_mask, payload_len, payload_len_field;
uint8_t timestamp[8], bit_index, timestamp_len;
uint8_t event_data[MAX_EVENT_SIZE];
unsigned int byte_index, total_event_len, i;
struct diag_dci_client_tbl *entry;
length = *(uint16_t *)(buf + 1); /* total length of event series */
if (length == 0) {
pr_err("diag: Incoming dci event length is invalid\n");
return;
}
temp_len = 0;
buf = buf + 3; /* start of event series */
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) {
timestamp_len = 2;
memset(timestamp, 0, 8);
} else {
timestamp_len = 8;
memcpy(timestamp, buf + temp_len + 2, timestamp_len);
}
/* 13th and 14th bit represent the payload length */
if (((event_id_packet & 0x6000) >> 13) == 3) {
payload_len_field = 1;
payload_len = *(uint8_t *)
(buf + temp_len + 2 + timestamp_len);
if (payload_len < (MAX_EVENT_SIZE - 13)) {
/* copy the payload length and the payload */
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\n",
(MAX_EVENT_SIZE - 13), payload_len);
return;
}
} else {
payload_len_field = 0;
payload_len = (event_id_packet & 0x6000) >> 13;
/* copy the payload */
memcpy(event_data + 12, buf + temp_len + 2 +
timestamp_len, payload_len);
}
/* 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;
byte_index = event_id / 8;
bit_index = event_id % 8;
byte_mask = 0x1 << bit_index;
/* parse through event mask tbl of each client and check mask */
for (i = 0; i < MAX_DCI_CLIENTS; i++) {
if (driver->dci_client_tbl[i].client) {
entry = &(driver->dci_client_tbl[i]);
event_mask_ptr = entry->dci_event_mask +
byte_index;
mutex_lock(&dci_health_mutex);
if (*event_mask_ptr & byte_mask) {
/* copy to client buffer */
if (DCI_CHK_CAPACITY(entry,
4 + total_event_len)) {
pr_err("diag: DCI event drop\n");
driver->dci_client_tbl[i].
dropped_events++;
mutex_unlock(
&dci_health_mutex);
break;
}
driver->dci_client_tbl[i].
received_events++;
*(int *)(entry->dci_data+
entry->data_len) = DCI_EVENT_TYPE;
/* 4 bytes for DCI_EVENT_TYPE */
memcpy(entry->dci_data +
entry->data_len + 4, event_data
, total_event_len);
entry->data_len += 4 + total_event_len;
}
mutex_unlock(&dci_health_mutex);
}
}
temp_len += 2 + timestamp_len + payload_len_field + payload_len;
}
}
void extract_dci_log(unsigned char *buf)
{
uint16_t log_code, item_num;
uint8_t equip_id, *log_mask_ptr, byte_mask;
unsigned int i, byte_index, byte_offset = 0;
struct diag_dci_client_tbl *entry;
log_code = *(uint16_t *)(buf + 6);
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);
byte_offset = (equip_id * 514) + byte_index;
if (byte_offset >= DCI_LOG_MASK_SIZE) {
pr_err("diag: Invalid byte_offset %d in dci log\n",
byte_offset);
return;
}
/* parse through log mask table of each client and check mask */
for (i = 0; i < MAX_DCI_CLIENTS; i++) {
if (driver->dci_client_tbl[i].client) {
entry = &(driver->dci_client_tbl[i]);
log_mask_ptr = entry->dci_log_mask;
if (!log_mask_ptr)
return;
log_mask_ptr = log_mask_ptr + byte_offset;
mutex_lock(&dci_health_mutex);
if (*log_mask_ptr & byte_mask) {
pr_debug("\t log code %x needed by client %d",
log_code, entry->client->tgid);
/* copy to client buffer */
if (DCI_CHK_CAPACITY(entry,
4 + *(uint16_t *)(buf + 2))) {
pr_err("diag: DCI log drop\n");
driver->dci_client_tbl[i].
dropped_logs++;
mutex_unlock(
&dci_health_mutex);
return;
}
driver->dci_client_tbl[i].received_logs++;
*(int *)(entry->dci_data+entry->data_len) =
DCI_LOG_TYPE;
memcpy(entry->dci_data + entry->data_len + 4,
buf + 4, *(uint16_t *)(buf + 2));
entry->data_len += 4 + *(uint16_t *)(buf + 2);
}
mutex_unlock(&dci_health_mutex);
}
}
}
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;
int index = smd_info->peripheral;
/* Update the peripheral(s) with the dci log and event masks */
/* If the cntl channel is not up, we can't update logs and events */
if (!driver->smd_cntl[index].ch)
return;
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.
*/
for (i = 0; i < MAX_DCI_CLIENTS; i++) {
if (driver->dci_client_tbl[i].client) {
client_log_mask_ptr =
driver->dci_client_tbl[i].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);
ret = diag_send_dci_log_mask(driver->smd_cntl[index].ch);
ret = diag_send_dci_event_mask(driver->smd_cntl[index].ch);
smd_info->notify_context = 0;
}
void diag_dci_notify_client(int peripheral_mask, int data)
{
int i, stat;
struct siginfo info;
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 */
for (i = 0; i < MAX_DCI_CLIENTS; i++) {
if (driver->dci_client_tbl[i].list & peripheral_mask) {
info.si_signo = driver->dci_client_tbl[i].signal_type;
stat = send_sig_info(
driver->dci_client_tbl[i].signal_type,
&info, driver->dci_client_tbl[i].client);
if (stat)
pr_err("diag: Err sending dci signal to client, signal data: 0x%x, stat: %d\n",
info.si_int, stat);
}
} /* end of loop for all DCI clients */
}
int diag_send_dci_pkt(struct diag_master_table entry, unsigned char *buf,
int len, int index)
{
int i, status = 0;
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) =
driver->req_tracking_tbl[index].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__);
return -EIO;
}
for (i = 0; i < NUM_SMD_DCI_CHANNELS; i++) {
struct diag_smd_info *smd_info = driver->separate_cmdrsp[i] ?
&driver->smd_dci_cmd[i] :
&driver->smd_dci[i];
if (entry.client_id == smd_info->peripheral) {
if (smd_info->ch) {
smd_write(smd_info->ch,
driver->apps_dci_buf, len + 10);
status = DIAG_DCI_NO_ERROR;
}
break;
}
}
if (status != DIAG_DCI_NO_ERROR) {
pr_alert("diag: check DCI channel\n");
status = DIAG_DCI_SEND_DATA_FAIL;
}
mutex_unlock(&driver->dci_mutex);
return status;
}
int diag_register_dci_transaction(int uid)
{
int i, new_dci_client = 1, ret = -1;
for (i = 0; i < dci_max_reg; i++) {
if (driver->req_tracking_tbl[i].pid == current->tgid) {
new_dci_client = 0;
break;
}
}
mutex_lock(&driver->dci_mutex);
/* Make an entry in kernel DCI table */
driver->dci_tag++;
for (i = 0; i < dci_max_reg; i++) {
if (driver->req_tracking_tbl[i].pid == 0) {
driver->req_tracking_tbl[i].pid = current->tgid;
driver->req_tracking_tbl[i].uid = uid;
driver->req_tracking_tbl[i].tag = driver->dci_tag;
ret = i;
break;
}
}
mutex_unlock(&driver->dci_mutex);
return ret;
}
int diag_process_dci_transaction(unsigned char *buf, int len)
{
unsigned char *temp = buf;
uint16_t subsys_cmd_code, log_code, item_num;
int subsys_id, cmd_code, ret = -1, index = -1, found = 0;
struct diag_master_table entry;
int count, set_mask, num_codes, bit_index, event_id, offset = 0, i;
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;
if (!driver->smd_dci[MODEM_DATA].ch) {
pr_err("diag: DCI smd channel for peripheral %d not valid for dci updates\n",
driver->smd_dci[MODEM_DATA].peripheral);
return DIAG_DCI_SEND_DATA_FAIL;
}
if (!temp) {
pr_err("diag: Invalid buffer in %s\n", __func__);
return -ENOMEM;
}
/* This is Pkt request/response transaction */
if (*(int *)temp > 0) {
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;
}
/* enter this UID into kernel table and return index */
index = diag_register_dci_transaction(*(int *)temp);
if (index < 0) {
pr_alert("diag: registering new DCI transaction failed\n");
return DIAG_DCI_NO_REG;
}
temp += sizeof(int);
/*
* Check for registered peripheral and fwd pkt to
* appropriate proc
*/
cmd_code = (int)(*(char *)temp);
temp++;
subsys_id = (int)(*(char *)temp);
temp++;
subsys_cmd_code = *(uint16_t *)temp;
temp += sizeof(uint16_t);
read_len += sizeof(int) + 2 + sizeof(uint16_t);
if (read_len >= USER_SPACE_DATA) {
pr_err("diag: dci: Invalid length in %s\n", __func__);
return -EIO;
}
pr_debug("diag: %d %d %d", cmd_code, subsys_id,
subsys_cmd_code);
for (i = 0; i < diag_max_reg; i++) {
entry = driver->table[i];
if (entry.process_id != NO_PROCESS) {
if (entry.cmd_code == cmd_code &&
entry.subsys_id == subsys_id &&
entry.cmd_code_lo <= subsys_cmd_code &&
entry.cmd_code_hi >= subsys_cmd_code) {
ret = diag_send_dci_pkt(entry, buf,
len, index);
} else if (entry.cmd_code == 255
&& cmd_code == 75) {
if (entry.subsys_id == subsys_id &&
entry.cmd_code_lo <=
subsys_cmd_code &&
entry.cmd_code_hi >=
subsys_cmd_code) {
ret = diag_send_dci_pkt(entry,
buf, len, index);
}
} else if (entry.cmd_code == 255 &&
entry.subsys_id == 255) {
if (entry.cmd_code_lo <= cmd_code &&
entry.cmd_code_hi >=
cmd_code) {
ret = diag_send_dci_pkt(entry,
buf, len, index);
}
}
}
}
} 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 id and table */
i = diag_dci_find_client_index(current->tgid);
if (i == DCI_CLIENT_INDEX_INVALID) {
pr_err("diag: dci client not registered/found\n");
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 = driver->dci_client_tbl[i].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 peripherals */
ret = diag_send_dci_log_mask(driver->smd_cntl[MODEM_DATA].ch);
} 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 id and table */
i = diag_dci_find_client_index(current->tgid);
if (i == DCI_CLIENT_INDEX_INVALID) {
pr_err("diag: dci client not registered/found\n");
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 = driver->dci_client_tbl[i].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 peripherals */
ret = diag_send_dci_event_mask(driver->smd_cntl[MODEM_DATA].ch);
} else {
pr_alert("diag: Incorrect DCI transaction\n");
}
return ret;
}
int diag_dci_find_client_index(int client_id)
{
int i, ret = DCI_CLIENT_INDEX_INVALID;
for (i = 0; i < MAX_DCI_CLIENTS; i++) {
if (driver->dci_client_tbl[i].client != NULL) {
if (driver->dci_client_tbl[i].client->tgid ==
client_id) {
ret = i;
break;
}
}
}
return ret;
}
void update_dci_cumulative_event_mask(int offset, uint8_t byte_mask)
{
int i;
uint8_t *event_mask_ptr;
uint8_t *update_ptr = dci_cumulative_event_mask;
bool is_set = false;
mutex_lock(&dci_event_mask_mutex);
update_ptr += offset;
for (i = 0; i < MAX_DCI_CLIENTS; i++) {
event_mask_ptr =
driver->dci_client_tbl[i].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 clear_client_dci_cumulative_event_mask(int client_index)
{
int i, j;
uint8_t *update_ptr = dci_cumulative_event_mask;
uint8_t *event_mask_ptr, *client_event_mask_ptr, byte_mask = 0;
bool is_set = false;
event_mask_ptr =
(driver->dci_client_tbl[client_index].dci_event_mask);
mutex_lock(&dci_event_mask_mutex);
for (i = 0; i < DCI_EVENT_MASK_SIZE; i++) {
is_set = false;
/* Already cleared event masks need not to be considered */
if (*event_mask_ptr != 0) {
byte_mask = *event_mask_ptr;
} else {
update_ptr++;
event_mask_ptr++;
continue;
}
for (j = 0; j < MAX_DCI_CLIENTS; j++) {
/* continue searching for valid client */
if (driver->dci_client_tbl[j].client == NULL ||
client_index == j)
continue;
client_event_mask_ptr =
(driver->dci_client_tbl[j].dci_event_mask);
client_event_mask_ptr += i;
if (*client_event_mask_ptr & byte_mask) {
/*
* Break if another client has same
* event mask set
*/
if ((*client_event_mask_ptr &
byte_mask) == byte_mask) {
is_set = true;
break;
} else {
byte_mask =
(~(*client_event_mask_ptr) &
byte_mask);
is_set = false;
}
}
}
/*
* Clear only if this client has event mask set else
* don't update cumulative event mask ptr
*/
if (is_set == false)
*update_ptr &= ~byte_mask;
update_ptr++;
event_mask_ptr++;
}
event_mask_ptr =
(driver->dci_client_tbl[client_index].dci_event_mask);
memset(event_mask_ptr, 0, DCI_EVENT_MASK_SIZE);
mutex_unlock(&dci_event_mask_mutex);
}
int diag_send_dci_event_mask(smd_channel_t *ch)
{
void *buf = driver->buf_event_mask_update;
int header_size = sizeof(struct diag_ctrl_event_mask);
int wr_size = -ENOMEM, retry_count = 0, timer;
int ret = DIAG_DCI_NO_ERROR;
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 = 1; /* event config */
driver->event_mask->event_mask_size = DCI_EVENT_MASK_SIZE;
memcpy(buf, driver->event_mask, header_size);
memcpy(buf+header_size, dci_cumulative_event_mask, DCI_EVENT_MASK_SIZE);
if (ch) {
while (retry_count < 3) {
wr_size = smd_write(ch, buf,
header_size + DCI_EVENT_MASK_SIZE);
if (wr_size == -ENOMEM) {
retry_count++;
for (timer = 0; timer < 5; timer++)
udelay(2000);
} else {
break;
}
}
if (wr_size != header_size + DCI_EVENT_MASK_SIZE) {
pr_err("diag: error writing dci event mask %d, tried %d\n",
wr_size, header_size + DCI_EVENT_MASK_SIZE);
ret = DIAG_DCI_SEND_DATA_FAIL;
}
} else {
pr_err("diag: ch not valid for dci event mask update\n");
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;
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;
for (i = 0; i < MAX_DCI_CLIENTS; i++) {
log_mask_ptr =
(driver->dci_client_tbl[i].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 clear_client_dci_cumulative_log_mask(int client_index)
{
int i, j, k;
uint8_t *update_ptr = dci_cumulative_log_mask;
uint8_t *log_mask_ptr, *client_log_mask_ptr, byte_mask = 0;
bool is_set = false;
log_mask_ptr = driver->dci_client_tbl[client_index].dci_log_mask;
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 cumulative log mask ptr*/
update_ptr += 2;
log_mask_ptr += 2;
for (i = 0; i < 16; i++) {
for (j = 0; j < 512; j++) {
is_set = false;
/*
* Already cleared log masks need
* not to be considered
*/
if (*log_mask_ptr != 0) {
byte_mask = *log_mask_ptr;
} else {
update_ptr++;
log_mask_ptr++;
continue;
}
for (k = 0; k < MAX_DCI_CLIENTS; k++) {
/* continue searching for valid client */
if (driver->dci_client_tbl[k].client == NULL ||
client_index == k)
continue;
client_log_mask_ptr =
(driver->dci_client_tbl[k].dci_log_mask);
client_log_mask_ptr += (i*514) + 2 + j;
if (*client_log_mask_ptr & byte_mask) {
/*
* Break if another client has same
* log mask set
*/
if ((*client_log_mask_ptr &
byte_mask) == byte_mask) {
is_set = true;
break;
} else {
byte_mask =
(~(*client_log_mask_ptr) &
byte_mask);
is_set = false;
}
}
}
/*
* Clear only if this client has log mask set else
* don't update cumulative log mask ptr
*/
if (is_set == false) {
/*
* Update the dirty bit for the equipment
* whose mask is changing
*/
dci_cumulative_log_mask[1+(i*514)] = 1;
*update_ptr &= ~byte_mask;
}
update_ptr++;
log_mask_ptr++;
}
update_ptr += 2;
log_mask_ptr += 2;
}
log_mask_ptr = driver->dci_client_tbl[client_index].dci_log_mask;
memset(log_mask_ptr, 0, DCI_LOG_MASK_SIZE);
mutex_unlock(&dci_log_mask_mutex);
}
int diag_send_dci_log_mask(smd_channel_t *ch)
{
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, wr_size = -ENOMEM, retry_count = 0, timer;
int ret = DIAG_DCI_NO_ERROR;
if (!ch) {
pr_err("diag: ch not valid for dci log mask update\n");
return DIAG_DCI_SEND_DATA_FAIL;
}
mutex_lock(&driver->diag_cntl_mutex);
for (i = 0; i < 16; i++) {
retry_count = 0;
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 */
if (ch && *(log_mask_ptr+1)) {
while (retry_count < 3) {
wr_size = smd_write(ch, buf, header_size + 512);
if (wr_size == -ENOMEM) {
retry_count++;
for (timer = 0; timer < 5; timer++)
udelay(2000);
} else
break;
}
if (wr_size != header_size + 512) {
pr_err("diag: dci log mask update failed %d, tried %d for equip_id %d\n",
wr_size, header_size + 512,
driver->log_mask->equip_id);
ret = DIAG_DCI_SEND_DATA_FAIL;
} else {
*(log_mask_ptr+1) = 0; /* clear dirty byte */
pr_debug("diag: updated dci log equip ID %d\n",
*log_mask_ptr);
}
}
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;
/* 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_open("DIAG_2",
&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 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 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);
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->req_tracking_tbl == NULL) {
driver->req_tracking_tbl = kzalloc(dci_max_reg *
sizeof(struct dci_pkt_req_tracking_tbl), GFP_KERNEL);
if (driver->req_tracking_tbl == NULL)
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;
}
if (driver->dci_client_tbl == NULL) {
driver->dci_client_tbl = kzalloc(MAX_DCI_CLIENTS *
sizeof(struct diag_dci_client_tbl), GFP_KERNEL);
if (driver->dci_client_tbl == NULL)
goto err;
}
driver->diag_dci_wq = create_singlethread_workqueue("diag_dci_wq");
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;
}
}
return DIAG_DCI_NO_ERROR;
err:
pr_err("diag: Could not initialize diag DCI buffers");
kfree(driver->req_tracking_tbl);
kfree(driver->dci_client_tbl);
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);
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->dci_client_tbl) {
for (i = 0; i < MAX_DCI_CLIENTS; i++)
kfree(driver->dci_client_tbl[i].dci_data);
}
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->req_tracking_tbl);
kfree(driver->dci_client_tbl);
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 i, j, k, err = DIAG_DCI_NO_ERROR;
uint8_t *log_mask_ptr, *update_ptr;
i = diag_dci_find_client_index(current->tgid);
if (i == DCI_CLIENT_INDEX_INVALID)
return DIAG_DCI_TABLE_ERR;
mutex_lock(&dci_log_mask_mutex);
create_dci_log_mask_tbl(
driver->dci_client_tbl[i].dci_log_mask);
memset(dci_cumulative_log_mask,
0x0, DCI_LOG_MASK_SIZE);
for (i = 0; i < MAX_DCI_CLIENTS; i++) {
update_ptr = dci_cumulative_log_mask;
if (driver->dci_client_tbl[i].client) {
log_mask_ptr =
driver->dci_client_tbl[i].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);
err = diag_send_dci_log_mask(driver->smd_cntl[MODEM_DATA].ch);
return err;
}
int diag_dci_clear_event_mask()
{
int i, j, err = DIAG_DCI_NO_ERROR;
uint8_t *event_mask_ptr, *update_ptr;
i = diag_dci_find_client_index(current->tgid);
if (i == DCI_CLIENT_INDEX_INVALID)
return DIAG_DCI_TABLE_ERR;
mutex_lock(&dci_event_mask_mutex);
memset(driver->dci_client_tbl[i].dci_event_mask,
0x0, DCI_EVENT_MASK_SIZE);
memset(dci_cumulative_event_mask,
0x0, DCI_EVENT_MASK_SIZE);
update_ptr = dci_cumulative_event_mask;
for (i = 0; i < MAX_DCI_CLIENTS; i++) {
event_mask_ptr =
driver->dci_client_tbl[i].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);
err = diag_send_dci_event_mask(driver->smd_cntl[MODEM_DATA].ch);
return err;
}
int diag_dci_query_log_mask(uint16_t log_code)
{
uint16_t item_num;
uint8_t equip_id, *log_mask_ptr, byte_mask;
int i, byte_index, offset;
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;
i = diag_dci_find_client_index(current->tgid);
if (i != DCI_CLIENT_INDEX_INVALID) {
log_mask_ptr = driver->dci_client_tbl[i].dci_log_mask;
log_mask_ptr = log_mask_ptr + offset + byte_index;
return ((*log_mask_ptr & byte_mask) == byte_mask) ?
1 : 0;
}
return 0;
}
int diag_dci_query_event_mask(uint16_t event_id)
{
uint8_t *event_mask_ptr, byte_mask;
int i, byte_index, bit_index;
byte_index = event_id/8;
bit_index = event_id % 8;
byte_mask = 0x1 << bit_index;
i = diag_dci_find_client_index(current->tgid);
if (i != DCI_CLIENT_INDEX_INVALID) {
event_mask_ptr =
driver->dci_client_tbl[i].dci_event_mask;
event_mask_ptr = event_mask_ptr + byte_index;
if ((*event_mask_ptr & byte_mask) == byte_mask)
return 1;
else
return 0;
}
return 0;
}