static int diag_create_log_mask_table(void)
{
struct diag_log_mask_t *mask = NULL;
uint8_t i;
int err = 0;
mutex_lock(&log_mask.lock);
mask = (struct diag_log_mask_t *)(log_mask.ptr);
for (i = 0; i < MAX_EQUIP_ID; i++, mask++) {
mask->equip_id = i;
mask->num_items = LOG_GET_ITEM_NUM(log_code_last_tbl[i]);
mask->num_items_tools = mask->num_items;
if (LOG_ITEMS_TO_SIZE(mask->num_items) > MAX_ITEMS_PER_EQUIP_ID)
mask->range = LOG_ITEMS_TO_SIZE(mask->num_items);
else
mask->range = MAX_ITEMS_PER_EQUIP_ID;
mask->range_tools = mask->range;
mask->ptr = kzalloc(mask->range, GFP_KERNEL);
if (!mask->ptr) {
err = -ENOMEM;
break;
}
kmemleak_not_leak(mask->ptr);
}
mutex_unlock(&log_mask.lock);
return err;
}
void log_update_mask(unsigned char* ptr, int len)
{
int i;
int read_len = 0;
int copy_len = 0;
diag_log_mask_update_t *src = NULL;
diag_log_mask_t *dest = NULL;
if (!ptr || len <= (int)sizeof(diag_log_mask_update_t) || !log_inited)
return;
dest = (diag_log_mask_t *)(log_mask);
for (i = 0; i < MAX_EQUIP_ID && read_len < len; i++, dest++) {
src = (diag_log_mask_update_t *)(ptr + read_len);
read_len += sizeof(diag_log_mask_update_t);
dest->equip_id = src->equip_id;
dest->num_items = src->num_items;
copy_len = LOG_ITEMS_TO_SIZE(dest->num_items);
if (copy_len > MAX_ITEMS_PER_EQUIP_ID)
copy_len = MAX_ITEMS_PER_EQUIP_ID;
memcpy(dest->mask, ptr + read_len, copy_len);
read_len += LOG_ITEMS_TO_SIZE(dest->num_items);
}
}
static int copy_log_mask_equip(int equip_id, uint8_t *buf)
{
int i, ret = 0;
uint8_t *temp = buf;
struct diag_log_mask_t *log_item = NULL;
uint32_t mask_size = 0;
if (!buf)
return ret;
log_item = (struct diag_log_mask_t *)driver->log_masks;
for (i = 0; i < MAX_EQUIP_ID; i++, log_item++) {
if (log_item->equip_id != equip_id)
continue;
*(int *)temp = log_item->equip_id;
temp += sizeof(int);
*(int *)(temp) = log_item->num_items;
temp += sizeof(int);
mask_size = LOG_ITEMS_TO_SIZE(log_item->num_items);
if (mask_size > MAX_ITEMS_PER_EQUIP_ID) {
pr_err("diag: Invalid length: %d in %s, perimissible: %d",
mask_size, __func__, MAX_ITEMS_PER_EQUIP_ID);
break;
}
if (mask_size > 0) {
memcpy(temp, log_item->ptr, mask_size);
ret = (2 * sizeof(int)) + mask_size;
}
break;
}
return ret;
}
static void process_log_range_report(uint8_t *buf, uint32_t len,
struct diag_smd_info *smd_info)
{
int i;
int read_len = 0;
int peripheral = 0;
int header_len = sizeof(struct diag_ctrl_log_range_report);
uint8_t *ptr = buf;
uint8_t *temp = NULL;
uint32_t mask_size;
struct diag_ctrl_log_range_report *header = NULL;
struct diag_ctrl_log_range *log_range = NULL;
struct diag_log_mask_t *mask_ptr = NULL;
if (!buf || !smd_info || len < 0)
return;
peripheral = smd_info->peripheral;
header = (struct diag_ctrl_log_range_report *)ptr;
ptr += header_len;
/* Don't account for pkt_id and length */
read_len += header_len - (2 * sizeof(uint32_t));
mutex_lock(&log_mask.lock);
driver->num_equip_id[peripheral] = header->num_ranges;
for (i = 0; i < header->num_ranges && read_len < len; i++) {
log_range = (struct diag_ctrl_log_range *)ptr;
ptr += sizeof(struct diag_ctrl_log_range);
read_len += sizeof(struct diag_ctrl_log_range);
if (log_range->equip_id >= MAX_EQUIP_ID) {
pr_err("diag: receiving log equip id %d more than supported equip id: %d from peripheral: %d\n",
log_range->equip_id, MAX_EQUIP_ID, peripheral);
continue;
}
mask_ptr = (struct diag_log_mask_t *)log_mask.ptr;
mask_ptr = &mask_ptr[log_range->equip_id];
mask_size = LOG_ITEMS_TO_SIZE(log_range->num_items);
if (mask_size < mask_ptr->range)
goto proceed;
temp = krealloc(mask_ptr->ptr, mask_size, GFP_KERNEL);
if (!temp) {
pr_err("diag: In %s, Unable to reallocate log mask ptr to size: %d, equip_id: %d\n",
__func__, mask_size, log_range->equip_id);
continue;
}
mask_ptr->ptr = temp;
mask_ptr->range = mask_size;
proceed:
if (log_range->num_items > mask_ptr->num_items)
mask_ptr->num_items = log_range->num_items;
}
mutex_unlock(&log_mask.lock);
}
int diag_copy_to_user_log_mask(char __user *buf, size_t count)
{
int i;
int err = 0;
int len = 0;
int copy_len = 0;
int total_len = 0;
struct diag_log_mask_userspace_t header;
struct diag_log_mask_t *mask = NULL;
unsigned char *ptr = NULL;
if (!buf || count == 0)
return -EINVAL;
mutex_lock(&log_mask.lock);
mask = (struct diag_log_mask_t *)(log_mask.ptr);
for (i = 0; i < MAX_EQUIP_ID; i++, mask++) {
ptr = log_mask.update_buf;
len = 0;
header.equip_id = mask->equip_id;
header.num_items = mask->num_items_tools;
memcpy(ptr, &header, sizeof(header));
len += sizeof(header);
copy_len = LOG_ITEMS_TO_SIZE(header.num_items);
if ((len + copy_len) > log_mask.update_buf_len) {
pr_err("diag: In %s, no space to update log mask, equip_id: %d\n",
__func__, mask->equip_id);
continue;
}
memcpy(ptr + len, mask->ptr, copy_len);
len += copy_len;
/* + sizeof(int) to account for data_type already in buf */
if (total_len + sizeof(int) + len > count) {
pr_err("diag: In %s, unable to send log masks to user space, total_len: %d, count: %zu\n",
__func__, total_len, count);
err = -ENOMEM;
break;
}
err = copy_to_user(buf + total_len, (void *)ptr, len);
if (err) {
pr_err("diag: In %s Unable to send log masks to user space clients, err: %d\n",
__func__, err);
break;
}
total_len += len;
}
mutex_unlock(&log_mask.lock);
return err ? err : total_len;
}
static void diag_update_log_mask(int equip_id, uint8_t *buf, int num_items)
{
int i = 0;
struct diag_log_mask_t *log_item = NULL;
uint32_t mask_size = 0;
mutex_lock(&driver->log_mask_mutex);
driver->log_status = DIAG_CTRL_MASK_INVALID;
if (!buf || (equip_id < 0 || equip_id >= MAX_EQUIP_ID) ||
num_items < 1) {
pr_err("diag: Invalid params in %s, buf: %x equip_id: %d, num_items: %d\n",
__func__, (unsigned int)buf, equip_id, num_items);
mutex_unlock(&driver->log_mask_mutex);
return;
}
mask_size = LOG_ITEMS_TO_SIZE(num_items);
if (mask_size > MAX_ITEMS_PER_EQUIP_ID) {
pr_err("diag: In %s, Invalid mask_size %d\n", __func__,
mask_size);
mutex_unlock(&driver->log_mask_mutex);
return;
}
log_item = (struct diag_log_mask_t *)driver->log_masks;
for (i = 0; i < MAX_EQUIP_ID; i++, log_item++) {
if (log_item->equip_id != equip_id)
continue;
log_item->num_items = num_items;
if (mask_size > 0)
memcpy(log_item->ptr, buf, mask_size);
driver->log_status = DIAG_CTRL_MASK_VALID;
break;
}
mutex_unlock(&driver->log_mask_mutex);
}
int diag_process_apps_masks(unsigned char *buf, int len)
{
int packet_type = 1;
int i;
int ssid_first, ssid_last, ssid_range;
int rt_mask, rt_first_ssid, rt_last_ssid, rt_mask_size;
uint8_t *rt_mask_ptr;
int equip_id, copy_len;
#if defined(CONFIG_DIAG_OVER_USB)
int payload_length;
#endif
if (*buf == 0x73 && *(int *)(buf+4) == 3) {
buf += 8;
diag_update_log_mask(*(int *)buf, buf+8, *(int *)(buf+4));
diag_update_userspace_clients(LOG_MASKS_TYPE);
#if defined(CONFIG_DIAG_OVER_USB)
if (chk_apps_only()) {
driver->apps_rsp_buf[0] = 0x73;
*(int *)(driver->apps_rsp_buf + 4) = 0x3;
*(int *)(driver->apps_rsp_buf + 8) = 0x0;
payload_length = 8 +
LOG_ITEMS_TO_SIZE(*(int *)(buf + 4));
if (payload_length > APPS_BUF_SIZE - 12) {
pr_err("diag: log masks: buffer overflow\n");
return -EIO;
}
for (i = 0; i < payload_length; i++)
*(int *)(driver->apps_rsp_buf+12+i) = *(buf+i);
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
#ifdef CONFIG_DIAG_RB_DUMP
if ((i == MODEM_DATA || i == WCNSS_DATA) && (diag_rb_enable & 1)){
printk("diag(%d): Filter Modem and WCNSS mask\n", __LINE__);
continue;
}
#endif
if (driver->smd_cntl[i].ch)
diag_send_log_mask_update(
driver->smd_cntl[i].ch,
*(int *)buf);
#ifdef CONFIG_DIAG_RB_DUMP
#endif
}
encode_rsp_and_send(12 + payload_length - 1);
return 0;
}
#endif
}
else if (*buf == 0x73 && *(int *)(buf+4) == 4) {
#if defined(CONFIG_DIAG_OVER_USB)
if (!(driver->smd_data[MODEM_DATA].ch) &&
chk_apps_only()) {
equip_id = *(int *)(buf + 8);
driver->apps_rsp_buf[0] = 0x73;
driver->apps_rsp_buf[1] = 0x0;
driver->apps_rsp_buf[2] = 0x0;
driver->apps_rsp_buf[3] = 0x0;
*(int *)(driver->apps_rsp_buf + 4) = 0x4;
copy_len = copy_log_mask_equip(equip_id,
driver->apps_rsp_buf + 12);
*(int *)(driver->apps_rsp_buf + 8) =
(copy_len == 0) ? 1 : 0;
encode_rsp_and_send(12 + copy_len);
return 0;
}
#endif
}
else if (*buf == 0x73 && *(int *)(buf+4) == 0) {
diag_disable_log_mask();
diag_update_userspace_clients(LOG_MASKS_TYPE);
#if defined(CONFIG_DIAG_OVER_USB)
if (chk_apps_only()) {
driver->apps_rsp_buf[0] = 0x73;
driver->apps_rsp_buf[1] = 0x0;
driver->apps_rsp_buf[2] = 0x0;
driver->apps_rsp_buf[3] = 0x0;
*(int *)(driver->apps_rsp_buf + 4) = 0x0;
*(int *)(driver->apps_rsp_buf + 8) = 0x0;
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
#ifdef CONFIG_DIAG_RB_DUMP
if ((i == MODEM_DATA || i == WCNSS_DATA) && (diag_rb_enable & 1)){
printk("diag(%d): Filter Modem and WCNSS mask\n", __LINE__);
continue;
}
#endif
if (driver->smd_cntl[i].ch)
diag_send_log_mask_update(
driver->smd_cntl[i].ch,
ALL_EQUIP_ID);
#ifdef CONFIG_DIAG_RB_DUMP
#endif
}
encode_rsp_and_send(11);
return 0;
}
#endif
}
else if ((*buf == 0x7d) && (*(buf+1) == 0x3)) {
ssid_first = *(uint16_t *)(buf + 2);
ssid_last = *(uint16_t *)(buf + 4);
#if defined(CONFIG_DIAG_OVER_USB)
if (!(driver->smd_data[MODEM_DATA].ch) &&
chk_apps_only()) {
driver->apps_rsp_buf[0] = 0x7d;
driver->apps_rsp_buf[1] = 0x3;
*(uint16_t *)(driver->apps_rsp_buf+2) = ssid_first;
*(uint16_t *)(driver->apps_rsp_buf+4) = ssid_last;
driver->apps_rsp_buf[6] = 0x1;
driver->apps_rsp_buf[7] = 0x0;
rt_mask_ptr = driver->msg_masks;
while (*(uint32_t *)(rt_mask_ptr + 4)) {
rt_first_ssid = *(uint32_t *)rt_mask_ptr;
rt_mask_ptr += 8;
rt_last_ssid = *(uint32_t *)rt_mask_ptr;
rt_mask_ptr += 4;
if (ssid_first == rt_first_ssid && ssid_last ==
rt_last_ssid) {
rt_mask_size = 4 * (rt_last_ssid -
rt_first_ssid + 1);
if (rt_mask_size > APPS_BUF_SIZE - 8) {
pr_err("diag: rt masks: buffer overflow\n");
return -EIO;
}
memcpy(driver->apps_rsp_buf+8,
rt_mask_ptr, rt_mask_size);
encode_rsp_and_send(8+rt_mask_size-1);
return 0;
}
rt_mask_ptr += MAX_SSID_PER_RANGE*4;
}
}
#endif
}
else if ((*buf == 0x7d) && (*(buf+1) == 0x4)) {
ssid_first = *(uint16_t *)(buf + 2);
ssid_last = *(uint16_t *)(buf + 4);
if (ssid_last < ssid_first) {
pr_err("diag: Invalid msg mask ssid values, first: %d, last: %d\n",
ssid_first, ssid_last);
return -EIO;
}
ssid_range = 4 * (ssid_last - ssid_first + 1);
if (ssid_range > APPS_BUF_SIZE - 8) {
pr_err("diag: Not enough space for message mask, ssid_range: %d\n",
ssid_range);
return -EIO;
}
pr_debug("diag: received mask update for ssid_first = %d, ssid_last = %d",
ssid_first, ssid_last);
diag_update_msg_mask(ssid_first, ssid_last , buf + 8);
diag_update_userspace_clients(MSG_MASKS_TYPE);
#if defined(CONFIG_DIAG_OVER_USB)
if (chk_apps_only()) {
for (i = 0; i < 8 + ssid_range; i++)
*(driver->apps_rsp_buf + i) = *(buf+i);
*(driver->apps_rsp_buf + 6) = 0x1;
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
#ifdef CONFIG_DIAG_RB_DUMP
if ((i == MODEM_DATA || i == WCNSS_DATA) && (diag_rb_enable & 1)){
printk("diag(%d): Filter Modem and WCNSS mask\n", __LINE__);
continue;
}
#endif
if (driver->smd_cntl[i].ch)
diag_send_msg_mask_update(
driver->smd_cntl[i].ch,
ssid_first, ssid_last,
driver->smd_cntl[i].peripheral);
#ifdef CONFIG_DIAG_RB_DUMP
#endif
}
encode_rsp_and_send(8 + ssid_range - 1);
return 0;
}
#endif
}
else if ((*buf == 0x7d) && (*(buf+1) == 0x5)) {
rt_mask = *(int *)(buf + 4);
diag_set_msg_mask(rt_mask);
diag_update_userspace_clients(MSG_MASKS_TYPE);
#if defined(CONFIG_DIAG_OVER_USB)
if (chk_apps_only()) {
driver->apps_rsp_buf[0] = 0x7d;
driver->apps_rsp_buf[1] = 0x5;
driver->apps_rsp_buf[2] = 1;
driver->apps_rsp_buf[3] = 0;
*(int *)(driver->apps_rsp_buf + 4) = rt_mask;
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
#ifdef CONFIG_DIAG_RB_DUMP
if ((i == MODEM_DATA || i == WCNSS_DATA) && (diag_rb_enable & 1)) {
printk("diag(%d): Filter Modem and WCNSS mask\n", __LINE__);
continue;
}
#endif
if (driver->smd_cntl[i].ch)
diag_send_msg_mask_update(
driver->smd_cntl[i].ch,
ALL_SSID, ALL_SSID,
driver->smd_cntl[i].peripheral);
#ifdef CONFIG_DIAG_RB_DUMP
#endif
}
encode_rsp_and_send(7);
return 0;
}
#endif
} else if (*buf == 0x82) {
buf += 4;
diag_event_num_bytes = (*(uint16_t *)buf)/8+1;
diag_update_event_mask(buf, diag_event_num_bytes);
diag_update_userspace_clients(EVENT_MASKS_TYPE);
#if defined(CONFIG_DIAG_OVER_USB)
if (chk_apps_only()) {
driver->apps_rsp_buf[0] = 0x82;
driver->apps_rsp_buf[1] = 0x0;
*(uint16_t *)(driver->apps_rsp_buf + 2) = 0x0;
*(uint16_t *)(driver->apps_rsp_buf + 4) =
EVENT_LAST_ID + 1;
memcpy(driver->apps_rsp_buf+6, driver->event_masks,
EVENT_LAST_ID/8+1);
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
#ifdef CONFIG_DIAG_RB_DUMP
if ((i == MODEM_DATA || i == WCNSS_DATA) && (diag_rb_enable & 1)){
printk("diag(%d): Filter Modem and WCNSS mask\n", __LINE__);
continue;
}
#endif
if (driver->smd_cntl[i].ch)
diag_send_event_mask_update(
driver->smd_cntl[i].ch,
diag_event_num_bytes);
#ifdef CONFIG_DIAG_RB_DUMP
#endif
}
encode_rsp_and_send(6 + EVENT_LAST_ID/8);
return 0;
}
#endif
} else if (*buf == 0x60) {
diag_toggle_event_mask(*(buf+1));
diag_update_userspace_clients(EVENT_MASKS_TYPE);
#if defined(CONFIG_DIAG_OVER_USB)
if (chk_apps_only()) {
driver->apps_rsp_buf[0] = 0x60;
driver->apps_rsp_buf[1] = 0x0;
driver->apps_rsp_buf[2] = 0x0;
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
#ifdef CONFIG_DIAG_RB_DUMP
if ((i == MODEM_DATA || i == WCNSS_DATA) && (diag_rb_enable & 1)) {
printk("diag(%d): Filter Modem and WCNSS mask\n", __LINE__);
continue;
}
#endif
if (driver->smd_cntl[i].ch)
diag_send_event_mask_update(
driver->smd_cntl[i].ch,
diag_event_num_bytes);
#ifdef CONFIG_DIAG_RB_DUMP
#endif
}
encode_rsp_and_send(2);
return 0;
}
#endif
} else if (*buf == 0x78) {
if (!(driver->smd_cntl[MODEM_DATA].ch) ||
(driver->log_on_demand_support)) {
driver->apps_rsp_buf[0] = 0x78;
*(uint16_t *)(driver->apps_rsp_buf + 1) =
*(uint16_t *)(buf + 1);
driver->apps_rsp_buf[3] = 0x1;
encode_rsp_and_send(3);
}
}
return packet_type;
}
void diag_send_log_mask_update(smd_channel_t *ch, int equip_id)
{
void *buf = driver->buf_log_mask_update;
struct diag_log_mask_t *log_item = NULL;
struct diag_ctrl_log_mask ctrl_pkt;
uint32_t log_mask_size = 0;
int wr_size = -ENOMEM, retry_count = 0;
int i, header_size, send_once = 0;
header_size = sizeof(struct diag_ctrl_log_mask);
log_item = (struct diag_log_mask_t *)driver->log_masks;
mutex_lock(&driver->diag_cntl_mutex);
for (i = 0; i < MAX_EQUIP_ID; i++, log_item++) {
if (equip_id != i && equip_id != ALL_EQUIP_ID)
continue;
log_mask_size = LOG_ITEMS_TO_SIZE(log_item->num_items);
ctrl_pkt.cmd_type = DIAG_CTRL_MSG_LOG_MASK;
ctrl_pkt.data_len = 11 + log_mask_size;
ctrl_pkt.stream_id = 1;
ctrl_pkt.status = driver->log_status;
switch (driver->log_status) {
case DIAG_CTRL_MASK_ALL_DISABLED:
ctrl_pkt.equip_id = 0;
ctrl_pkt.num_items = 0;
ctrl_pkt.log_mask_size = 0;
send_once = 1;
break;
case DIAG_CTRL_MASK_ALL_ENABLED:
ctrl_pkt.equip_id = 0;
ctrl_pkt.num_items = 0;
ctrl_pkt.log_mask_size = 0;
send_once = 1;
break;
case DIAG_CTRL_MASK_VALID:
ctrl_pkt.equip_id = i;
ctrl_pkt.num_items = log_item->num_items;
ctrl_pkt.log_mask_size = log_mask_size;
send_once = 0;
break;
default:
pr_err("diag: In %s, invalid status %d", __func__,
driver->log_status);
mutex_unlock(&driver->diag_cntl_mutex);
return;
}
memcpy(buf, &ctrl_pkt, header_size);
if (log_mask_size > 0) {
memcpy(buf + header_size, log_item->ptr,
log_mask_size);
}
if (ch) {
while (retry_count < 3) {
wr_size = smd_write(ch, buf,
header_size + log_mask_size);
if (wr_size == -ENOMEM) {
retry_count++;
usleep_range(10000, 10100);
} else
break;
}
if (wr_size != header_size + log_mask_size)
pr_err("diag: log mask update failed %d, tried %d",
wr_size, header_size + log_mask_size);
else
pr_debug("diag: updated log equip ID %d,len %d\n",
i, log_mask_size);
} else
pr_err("diag: ch not valid for log update\n");
if (send_once)
break;
}
mutex_unlock(&driver->diag_cntl_mutex);
}
int diag_process_apps_masks(unsigned char *buf, int len)
{
int packet_type = 1;
int i;
int ssid_first, ssid_last, ssid_range;
int rt_mask, rt_first_ssid, rt_last_ssid, rt_mask_size;
uint8_t *rt_mask_ptr;
int equip_id, copy_len;
#if defined(CONFIG_DIAG_OVER_USB)
int payload_length;
#endif
/* Set log masks */
if (*buf == 0x73 && *(int *)(buf+4) == 3) {
buf += 8;
diag_update_log_mask(*(int *)buf, buf+8, *(int *)(buf+4));
diag_update_userspace_clients(LOG_MASKS_TYPE);
#if defined(CONFIG_DIAG_OVER_USB)
if (chk_apps_only()) {
driver->apps_rsp_buf[0] = 0x73;
*(int *)(driver->apps_rsp_buf + 4) = 0x3; /* op. ID */
*(int *)(driver->apps_rsp_buf + 8) = 0x0; /* success */
payload_length = 8 +
LOG_ITEMS_TO_SIZE(*(int *)(buf + 4));
if (payload_length > APPS_BUF_SIZE - 12) {
pr_err("diag: log masks: buffer overflow\n");
return -EIO;
}
for (i = 0; i < payload_length; i++)
*(int *)(driver->apps_rsp_buf+12+i) = *(buf+i);
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
if (driver->smd_cntl[i].ch)
diag_send_log_mask_update(
&driver->smd_cntl[i],
*(int *)buf);
}
encode_rsp_and_send(12 + payload_length - 1);
return 0;
}
#endif
} /* Get log masks */
else if (*buf == 0x73 && *(int *)(buf+4) == 4) {
#if defined(CONFIG_DIAG_OVER_USB)
if (!(driver->smd_data[MODEM_DATA].ch) &&
chk_apps_only()) {
equip_id = *(int *)(buf + 8);
driver->apps_rsp_buf[0] = 0x73;
driver->apps_rsp_buf[1] = 0x0;
driver->apps_rsp_buf[2] = 0x0;
driver->apps_rsp_buf[3] = 0x0;
*(int *)(driver->apps_rsp_buf + 4) = 0x4;
copy_len = copy_log_mask_equip(equip_id,
driver->apps_rsp_buf + 12);
*(int *)(driver->apps_rsp_buf + 8) =
(copy_len == 0) ? 1 : 0;
encode_rsp_and_send(12 + copy_len);
return 0;
}
#endif
} /* Disable log masks */
else if (*buf == 0x73 && *(int *)(buf+4) == 0) {
/* Disable mask for each log code */
diag_disable_log_mask();
diag_update_userspace_clients(LOG_MASKS_TYPE);
#if defined(CONFIG_DIAG_OVER_USB)
if (chk_apps_only()) {
driver->apps_rsp_buf[0] = 0x73;
driver->apps_rsp_buf[1] = 0x0;
driver->apps_rsp_buf[2] = 0x0;
driver->apps_rsp_buf[3] = 0x0;
*(int *)(driver->apps_rsp_buf + 4) = 0x0;
*(int *)(driver->apps_rsp_buf + 8) = 0x0; /* status */
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
if (driver->smd_cntl[i].ch)
diag_send_log_mask_update(
&driver->smd_cntl[i],
ALL_EQUIP_ID);
}
encode_rsp_and_send(11);
return 0;
}
#endif
} /* Get runtime message mask */
else if ((*buf == 0x7d) && (*(buf+1) == 0x3)) {
ssid_first = *(uint16_t *)(buf + 2);
ssid_last = *(uint16_t *)(buf + 4);
#if defined(CONFIG_DIAG_OVER_USB)
if (!(driver->smd_data[MODEM_DATA].ch) &&
chk_apps_only()) {
driver->apps_rsp_buf[0] = 0x7d;
driver->apps_rsp_buf[1] = 0x3;
*(uint16_t *)(driver->apps_rsp_buf+2) = ssid_first;
*(uint16_t *)(driver->apps_rsp_buf+4) = ssid_last;
driver->apps_rsp_buf[6] = 0x1; /* Success Status */
driver->apps_rsp_buf[7] = 0x0;
rt_mask_ptr = driver->msg_masks;
while (*(uint32_t *)(rt_mask_ptr + 4)) {
rt_first_ssid = *(uint32_t *)rt_mask_ptr;
rt_mask_ptr += 8; /* +8 to skip 'last' */
rt_last_ssid = *(uint32_t *)rt_mask_ptr;
rt_mask_ptr += 4;
if (ssid_first == rt_first_ssid && ssid_last ==
rt_last_ssid) {
rt_mask_size = 4 * (rt_last_ssid -
rt_first_ssid + 1);
if (rt_mask_size > APPS_BUF_SIZE - 8) {
pr_err("diag: rt masks: buffer overflow\n");
return -EIO;
}
memcpy(driver->apps_rsp_buf+8,
rt_mask_ptr, rt_mask_size);
encode_rsp_and_send(8+rt_mask_size-1);
return 0;
}
rt_mask_ptr += MAX_SSID_PER_RANGE*4;
}
}
#endif
} /* Set runtime message mask */
else if ((*buf == 0x7d) && (*(buf+1) == 0x4)) {
ssid_first = *(uint16_t *)(buf + 2);
ssid_last = *(uint16_t *)(buf + 4);
if (ssid_last < ssid_first) {
pr_err("diag: Invalid msg mask ssid values, first: %d, last: %d\n",
ssid_first, ssid_last);
return -EIO;
}
ssid_range = 4 * (ssid_last - ssid_first + 1);
if (ssid_range > APPS_BUF_SIZE - 8) {
pr_err("diag: Not enough space for message mask, ssid_range: %d\n",
ssid_range);
return -EIO;
}
pr_debug("diag: received mask update for ssid_first = %d, ssid_last = %d",
ssid_first, ssid_last);
diag_update_msg_mask(ssid_first, ssid_last , buf + 8);
diag_update_userspace_clients(MSG_MASKS_TYPE);
#if defined(CONFIG_DIAG_OVER_USB)
if (chk_apps_only()) {
for (i = 0; i < 8 + ssid_range; i++)
*(driver->apps_rsp_buf + i) = *(buf+i);
*(driver->apps_rsp_buf + 6) = 0x1;
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
if (driver->smd_cntl[i].ch)
diag_send_msg_mask_update(
&driver->smd_cntl[i],
ssid_first, ssid_last,
driver->smd_cntl[i].peripheral);
}
encode_rsp_and_send(8 + ssid_range - 1);
return 0;
}
#endif
} /* Set ALL runtime message mask */
else if ((*buf == 0x7d) && (*(buf+1) == 0x5)) {
rt_mask = *(int *)(buf + 4);
diag_set_msg_mask(rt_mask);
diag_update_userspace_clients(MSG_MASKS_TYPE);
#if defined(CONFIG_DIAG_OVER_USB)
if (chk_apps_only()) {
driver->apps_rsp_buf[0] = 0x7d; /* cmd_code */
driver->apps_rsp_buf[1] = 0x5; /* set subcommand */
driver->apps_rsp_buf[2] = 1; /* success */
driver->apps_rsp_buf[3] = 0; /* rsvd */
*(int *)(driver->apps_rsp_buf + 4) = rt_mask;
/* send msg mask update to peripheral */
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
if (driver->smd_cntl[i].ch)
diag_send_msg_mask_update(
&driver->smd_cntl[i],
ALL_SSID, ALL_SSID,
driver->smd_cntl[i].peripheral);
}
encode_rsp_and_send(7);
return 0;
}
#endif
} else if (*buf == 0x82) { /* event mask change */
buf += 4;
diag_event_num_bytes = (*(uint16_t *)buf)/8+1;
diag_update_event_mask(buf, diag_event_num_bytes);
diag_update_userspace_clients(EVENT_MASKS_TYPE);
#if defined(CONFIG_DIAG_OVER_USB)
if (chk_apps_only()) {
driver->apps_rsp_buf[0] = 0x82;
driver->apps_rsp_buf[1] = 0x0;
*(uint16_t *)(driver->apps_rsp_buf + 2) = 0x0;
*(uint16_t *)(driver->apps_rsp_buf + 4) =
EVENT_LAST_ID + 1;
memcpy(driver->apps_rsp_buf+6, driver->event_masks,
EVENT_LAST_ID/8+1);
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
if (driver->smd_cntl[i].ch)
diag_send_event_mask_update(
&driver->smd_cntl[i],
diag_event_num_bytes);
}
encode_rsp_and_send(6 + EVENT_LAST_ID/8);
return 0;
}
#endif
} else if (*buf == 0x60) {
diag_toggle_event_mask(*(buf+1));
diag_update_userspace_clients(EVENT_MASKS_TYPE);
#if defined(CONFIG_DIAG_OVER_USB)
if (chk_apps_only()) {
driver->apps_rsp_buf[0] = 0x60;
driver->apps_rsp_buf[1] = 0x0;
driver->apps_rsp_buf[2] = 0x0;
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
if (driver->smd_cntl[i].ch)
diag_send_event_mask_update(
&driver->smd_cntl[i],
diag_event_num_bytes);
}
encode_rsp_and_send(2);
return 0;
}
#endif
} else if (*buf == 0x78) {
if (!(driver->smd_cntl[MODEM_DATA].ch) ||
(driver->log_on_demand_support)) {
driver->apps_rsp_buf[0] = 0x78;
/* Copy log code received */
*(uint16_t *)(driver->apps_rsp_buf + 1) =
*(uint16_t *)(buf + 1);
driver->apps_rsp_buf[3] = 0x1;/* Unknown */
encode_rsp_and_send(3);
}
}
return packet_type;
}
static int diag_cmd_set_log_mask(unsigned char *src_buf, int src_len,
unsigned char *dest_buf, int dest_len)
{
int i;
int write_len = 0;
int status = LOG_STATUS_SUCCESS;
int read_len = 0;
int payload_len = 0;
int req_header_len = sizeof(struct diag_log_config_req_t);
int rsp_header_len = sizeof(struct diag_log_config_set_rsp_t);
uint32_t mask_size = 0;
struct diag_log_mask_t *mask = (struct diag_log_mask_t *)log_mask.ptr;
struct diag_log_config_req_t *req;
struct diag_log_config_set_rsp_t rsp;
unsigned char *temp_buf = NULL;
if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
__func__, src_buf, src_len, dest_buf, dest_len);
return -EINVAL;
}
req = (struct diag_log_config_req_t *)src_buf;
read_len += req_header_len;
if (req->equip_id >= MAX_EQUIP_ID) {
pr_err("diag: In %s, Invalid logging mask request, equip_id: %d\n",
__func__, req->equip_id);
status = LOG_STATUS_INVALID;
}
if (req->num_items == 0) {
pr_err("diag: In %s, Invalid number of items in log mask request, equip_id: %d\n",
__func__, req->equip_id);
status = LOG_STATUS_INVALID;
}
mutex_lock(&log_mask.lock);
for (i = 0; i < MAX_EQUIP_ID && !status; i++, mask++) {
if (mask->equip_id != req->equip_id)
continue;
/*
* If the size of the log mask cannot fit into our
* buffer, trim till we have space left in the buffer.
* num_items should then reflect the items that we have
* in our buffer.
*/
mask->num_items_tools = (req->num_items > MAX_ITEMS_ALLOWED) ?
MAX_ITEMS_ALLOWED : req->num_items;
mask_size = LOG_ITEMS_TO_SIZE(mask->num_items_tools);
memset(mask->ptr, 0, mask->range_tools);
if (mask_size > mask->range_tools) {
/* Change in the mask reported by tools */
temp_buf = krealloc(mask->ptr, mask_size, GFP_KERNEL);
if (!temp_buf) {
log_mask.status = DIAG_CTRL_MASK_INVALID;
break;
}
mask->ptr = temp_buf;
memset(mask->ptr, 0, mask_size);
mask->range_tools = mask_size;
}
req->num_items = mask->num_items_tools;
if (mask_size > 0)
memcpy(mask->ptr, src_buf + read_len, mask_size);
log_mask.status = DIAG_CTRL_MASK_VALID;
break;
}
mutex_unlock(&log_mask.lock);
diag_update_userspace_clients(LOG_MASKS_TYPE);
/*
* Apps processor must send the response to this command. Frame the
* response.
*/
payload_len = LOG_ITEMS_TO_SIZE(req->num_items);
if ((payload_len + rsp_header_len > dest_len) || (payload_len == 0)) {
pr_err("diag: In %s, invalid length, payload_len: %d, header_len: %d, dest_len: %d\n",
__func__, payload_len, rsp_header_len , dest_len);
status = LOG_STATUS_FAIL;
}
rsp.cmd_code = DIAG_CMD_LOG_CONFIG;
rsp.padding[0] = 0;
rsp.padding[1] = 0;
rsp.padding[2] = 0;
rsp.sub_cmd = DIAG_CMD_OP_SET_LOG_MASK;
rsp.status = status;
rsp.equip_id = req->equip_id;
rsp.num_items = req->num_items;
memcpy(dest_buf, &rsp, rsp_header_len);
write_len += rsp_header_len;
if (status != LOG_STATUS_SUCCESS)
goto end;
memcpy(dest_buf + write_len, src_buf + read_len, payload_len);
write_len += payload_len;
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
diag_send_log_mask_update(&driver->smd_cntl[i], req->equip_id);
end:
return write_len;
}
static int diag_cmd_get_log_mask(unsigned char *src_buf, int src_len,
unsigned char *dest_buf, int dest_len)
{
int i;
int status = LOG_STATUS_INVALID;
int write_len = 0;
int read_len = 0;
int req_header_len = sizeof(struct diag_log_config_req_t);
int rsp_header_len = sizeof(struct diag_log_config_rsp_t);
uint32_t mask_size = 0;
struct diag_log_mask_t *log_item = NULL;
struct diag_log_config_req_t *req;
struct diag_log_config_rsp_t rsp;
if (!src_buf || !dest_buf || src_len <= 0 || dest_len <= 0) {
pr_err("diag: Invalid input in %s, src_buf: %p, src_len: %d, dest_buf: %p, dest_len: %d",
__func__, src_buf, src_len, dest_buf, dest_len);
return -EINVAL;
}
if (!diag_apps_responds())
return 0;
req = (struct diag_log_config_req_t *)src_buf;
read_len += req_header_len;
rsp.cmd_code = DIAG_CMD_LOG_CONFIG;
rsp.padding[0] = 0;
rsp.padding[1] = 0;
rsp.padding[2] = 0;
rsp.sub_cmd = DIAG_CMD_OP_GET_LOG_MASK;
/*
* Don't copy the response header now. Copy at the end after
* calculating the status field value
*/
write_len += rsp_header_len;
log_item = (struct diag_log_mask_t *)log_mask.ptr;
for (i = 0; i < MAX_EQUIP_ID; i++, log_item++) {
if (log_item->equip_id != req->equip_id)
continue;
mask_size = LOG_ITEMS_TO_SIZE(log_item->num_items_tools);
/*
* Make sure we have space to fill the response in the buffer.
* Destination buffer should atleast be able to hold equip_id
* (uint32_t), num_items(uint32_t), mask (mask_size) and the
* response header.
*/
if ((mask_size + (2 * sizeof(uint32_t)) + rsp_header_len) >
dest_len) {
pr_err("diag: In %s, invalid length: %d, max rsp_len: %d\n",
__func__, mask_size, dest_len);
status = LOG_STATUS_FAIL;
break;
}
*(uint32_t *)(dest_buf + write_len) = log_item->equip_id;
write_len += sizeof(uint32_t);
*(uint32_t *)(dest_buf + write_len) = log_item->num_items_tools;
write_len += sizeof(uint32_t);
if (mask_size > 0) {
memcpy(dest_buf + write_len, log_item->ptr, mask_size);
write_len += mask_size;
}
status = LOG_STATUS_SUCCESS;
break;
}
rsp.status = status;
memcpy(dest_buf, &rsp, rsp_header_len);
return write_len;
}
static void diag_send_log_mask_update(struct diag_smd_info *smd_info,
int equip_id)
{
int i;
int err = 0;
int send_once = 0;
int header_len = sizeof(struct diag_ctrl_log_mask);
uint8_t *buf = log_mask.update_buf;
uint8_t *temp = NULL;
uint32_t mask_size = 0;
struct diag_ctrl_log_mask ctrl_pkt;
struct diag_log_mask_t *mask = (struct diag_log_mask_t *)log_mask.ptr;
if (!smd_info)
return;
if (!smd_info->ch) {
pr_debug("diag: In %s, SMD channel is closed, peripheral: %d\n",
__func__, smd_info->peripheral);
return;
}
switch (log_mask.status) {
case DIAG_CTRL_MASK_ALL_DISABLED:
ctrl_pkt.equip_id = 0;
ctrl_pkt.num_items = 0;
ctrl_pkt.log_mask_size = 0;
send_once = 1;
break;
case DIAG_CTRL_MASK_ALL_ENABLED:
ctrl_pkt.equip_id = 0;
ctrl_pkt.num_items = 0;
ctrl_pkt.log_mask_size = 0;
send_once = 1;
break;
case DIAG_CTRL_MASK_VALID:
send_once = 0;
break;
default:
pr_debug("diag: In %s, invalid log_mask status\n", __func__);
return;
}
mutex_lock(&log_mask.lock);
for (i = 0; i < MAX_EQUIP_ID; i++, mask++) {
if (equip_id != i && equip_id != ALL_EQUIP_ID)
continue;
ctrl_pkt.cmd_type = DIAG_CTRL_MSG_LOG_MASK;
ctrl_pkt.stream_id = 1;
ctrl_pkt.status = log_mask.status;
if (log_mask.status == DIAG_CTRL_MASK_VALID) {
mask_size = LOG_ITEMS_TO_SIZE(mask->num_items_tools);
ctrl_pkt.equip_id = i;
ctrl_pkt.num_items = mask->num_items_tools;
ctrl_pkt.log_mask_size = mask_size;
}
ctrl_pkt.data_len = LOG_MASK_CTRL_HEADER_LEN + mask_size;
if (header_len + mask_size > log_mask.update_buf_len) {
temp = krealloc(buf, header_len + mask_size,
GFP_KERNEL);
if (!temp) {
pr_err("diag: Unable to realloc log update buffer, new size: %d, equip_id: %d\n",
header_len + mask_size, equip_id);
break;
}
log_mask.update_buf = temp;
log_mask.update_buf_len = header_len + mask_size;
}
memcpy(buf, &ctrl_pkt, header_len);
if (mask_size > 0)
memcpy(buf + header_len, mask->ptr, mask_size);
err = diag_smd_write(smd_info, buf, header_len + mask_size);
if (err) {
pr_err("diag: Unable to send log masks to peripheral %d, equip_id: %d, err: %d\n",
smd_info->peripheral, i, err);
}
if (send_once || equip_id != ALL_EQUIP_ID)
break;
}
mutex_unlock(&log_mask.lock);
}
