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, num_items;
#if defined(CONFIG_DIAG_OVER_USB)
int payload_length;
#endif
/* Set log masks */
if (*buf == 0x73 && *(int *)(buf+4) == 3) {
buf += 8;
/* Read Equip ID and pass as first param below*/
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 + ((*(int *)(buf + 4)) + 7)/8;
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].ch,
*(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);
num_items = *(int *)(buf + 12);
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;
if (!chk_equip_id_and_mask(equip_id,
driver->apps_rsp_buf+20))
*(int *)(driver->apps_rsp_buf + 8) = 0x0;
else
*(int *)(driver->apps_rsp_buf + 8) = 0x1;
*(int *)(driver->apps_rsp_buf + 12) = equip_id;
*(int *)(driver->apps_rsp_buf + 16) = num_items;
encode_rsp_and_send(20+(num_items+7)/8-1);
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].ch,
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].ch,
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].ch,
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].ch,
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].ch,
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;
}
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, num_items;
#if defined(CONFIG_DIAG_OVER_USB)
int payload_length;
#endif
/* Set log masks */
if (*buf == 0x73 && *(int *)(buf+4) == 3) {
buf += 8;
/* Read Equip ID and pass as first param below*/
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 + ((*(int *)(buf + 4)) + 7)/8;
for (i = 0; i < payload_length; i++)
*(int *)(driver->apps_rsp_buf+12+i) = *(buf+i);
if (driver->ch_cntl)
diag_send_log_mask_update(driver->ch_cntl,
*(int *)buf);
if (driver->chlpass_cntl)
diag_send_log_mask_update(driver->chlpass_cntl,
*(int *)buf);
if (driver->ch_wcnss_cntl)
diag_send_log_mask_update(driver->ch_wcnss_cntl,
*(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->ch) && chk_apps_only()) {
equip_id = *(int *)(buf + 8);
num_items = *(int *)(buf + 12);
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;
if (!chk_equip_id_and_mask(equip_id,
driver->apps_rsp_buf+20))
*(int *)(driver->apps_rsp_buf + 8) = 0x0;
else
*(int *)(driver->apps_rsp_buf + 8) = 0x1;
*(int *)(driver->apps_rsp_buf + 12) = equip_id;
*(int *)(driver->apps_rsp_buf + 16) = num_items;
encode_rsp_and_send(20+(num_items+7)/8-1);
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 */
if (driver->ch_cntl)
diag_send_log_mask_update(driver->ch_cntl,
ALL_EQUIP_ID);
if (driver->chlpass_cntl)
diag_send_log_mask_update(driver->chlpass_cntl,
ALL_EQUIP_ID);
if (driver->ch_wcnss_cntl)
diag_send_log_mask_update(driver->ch_wcnss_cntl,
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->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);
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);
ssid_range = 4 * (ssid_last - ssid_first + 1);
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;
if (driver->ch_cntl)
diag_send_msg_mask_update(driver->ch_cntl,
ssid_first, ssid_last, MODEM_PROC);
if (driver->chlpass_cntl)
diag_send_msg_mask_update(driver->chlpass_cntl,
ssid_first, ssid_last, LPASS_PROC);
if (driver->ch_wcnss_cntl)
diag_send_msg_mask_update(driver->ch_wcnss_cntl,
ssid_first, ssid_last, WCNSS_PROC);
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 */
if (driver->ch_cntl)
diag_send_msg_mask_update(driver->ch_cntl,
ALL_SSID, ALL_SSID, MODEM_PROC);
if (driver->chlpass_cntl)
diag_send_msg_mask_update(driver->chlpass_cntl,
ALL_SSID, ALL_SSID, LPASS_PROC);
if (driver->ch_wcnss_cntl)
diag_send_msg_mask_update(driver->ch_wcnss_cntl,
ALL_SSID, ALL_SSID, WCNSS_PROC);
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, 1, (*(uint16_t *)buf)/8+1);
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);
if (driver->ch_cntl)
diag_send_event_mask_update(driver->ch_cntl,
diag_event_num_bytes);
if (driver->chlpass_cntl)
diag_send_event_mask_update(
driver->chlpass_cntl,
diag_event_num_bytes);
if (driver->ch_wcnss_cntl)
diag_send_event_mask_update(
driver->ch_wcnss_cntl, diag_event_num_bytes);
encode_rsp_and_send(6 + EVENT_LAST_ID/8);
return 0;
}
#endif
} else if (*buf == 0x60) {
diag_event_config = *(buf+1);
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;
if (driver->ch_cntl)
diag_send_event_mask_update(driver->ch_cntl,
diag_event_num_bytes);
if (driver->chlpass_cntl)
diag_send_event_mask_update(
driver->chlpass_cntl,
diag_event_num_bytes);
if (driver->ch_wcnss_cntl)
diag_send_event_mask_update(
driver->ch_wcnss_cntl, diag_event_num_bytes);
encode_rsp_and_send(2);
return 0;
}
#endif
}
return packet_type;
}