static int diag_cmd_update_event_mask(unsigned char *src_buf, int src_len,
unsigned char *dest_buf, int dest_len)
{
int i;
int write_len = 0;
int mask_len = 0;
int header_len = sizeof(struct diag_event_mask_config_t);
struct diag_event_mask_config_t rsp;
struct diag_event_mask_config_t *req;
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_event_mask_config_t *)src_buf;
mask_len = EVENT_COUNT_TO_BYTES(req->num_bits);
if (mask_len <= 0 || mask_len > event_mask.mask_len) {
pr_err("diag: In %s, invalid event mask len: %d\n", __func__,
mask_len);
return -EIO;
}
mutex_lock(&event_mask.lock);
memcpy(event_mask.ptr, src_buf + header_len, mask_len);
event_mask.status = DIAG_CTRL_MASK_VALID;
mutex_unlock(&event_mask.lock);
diag_update_userspace_clients(EVENT_MASKS_TYPE);
/*
* Apps processor must send the response to this command. Frame the
* response.
*/
rsp.cmd_code = DIAG_CMD_SET_EVENT_MASK;
rsp.status = EVENT_STATUS_SUCCESS;
rsp.padding = 0;
rsp.num_bits = driver->last_event_id + 1;
memcpy(dest_buf, &rsp, header_len);
write_len += header_len;
memcpy(dest_buf + write_len, event_mask.ptr, mask_len);
write_len += mask_len;
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
diag_send_event_mask_update(&driver->smd_cntl[i]);
return write_len;
}
static int diag_cmd_set_all_msg_mask(unsigned char *src_buf, int src_len,
unsigned char *dest_buf, int dest_len)
{
int i;
int write_len = 0;
int header_len = sizeof(struct diag_msg_config_rsp_t);
struct diag_msg_config_rsp_t rsp;
struct diag_msg_config_rsp_t *req = NULL;
struct diag_msg_mask_t *mask = (struct diag_msg_mask_t *)msg_mask.ptr;
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_msg_config_rsp_t *)src_buf;
mutex_lock(&msg_mask.lock);
msg_mask.status = (req->rt_mask) ? DIAG_CTRL_MASK_ALL_ENABLED :
DIAG_CTRL_MASK_ALL_DISABLED;
for (i = 0; i < driver->msg_mask_tbl_count; i++, mask++) {
memset(mask->ptr, req->rt_mask,
mask->range * sizeof(uint32_t));
}
mutex_unlock(&msg_mask.lock);
diag_update_userspace_clients(MSG_MASKS_TYPE);
/*
* Apps processor must send the response to this command. Frame the
* response.
*/
rsp.cmd_code = DIAG_CMD_MSG_CONFIG;
rsp.sub_cmd = DIAG_CMD_OP_SET_ALL_MSG_MASK;
rsp.status = MSG_STATUS_SUCCESS;
rsp.padding = 0;
rsp.rt_mask = req->rt_mask;
memcpy(dest_buf, &rsp, header_len);
write_len += header_len;
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
diag_send_msg_mask_update(&driver->smd_cntl[i], ALL_SSID,
ALL_SSID);
}
return write_len;
}
static int diag_cmd_toggle_events(unsigned char *src_buf, int src_len,
unsigned char *dest_buf, int dest_len)
{
int i;
int write_len = 0;
uint8_t toggle = 0;
struct diag_event_report_t header;
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;
}
toggle = *(src_buf + 1);
mutex_lock(&event_mask.lock);
if (toggle) {
event_mask.status = DIAG_CTRL_MASK_ALL_ENABLED;
memset(event_mask.ptr, 0xFF, event_mask.mask_len);
} else {
event_mask.status = DIAG_CTRL_MASK_ALL_DISABLED;
memset(event_mask.ptr, 0, event_mask.mask_len);
}
mutex_unlock(&event_mask.lock);
diag_update_userspace_clients(EVENT_MASKS_TYPE);
/*
* Apps processor must send the response to this command. Frame the
* response.
*/
header.cmd_code = DIAG_CMD_EVENT_TOGGLE;
header.padding = 0;
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
diag_send_event_mask_update(&driver->smd_cntl[i]);
memcpy(dest_buf, &header, sizeof(header));
write_len += sizeof(header);
return write_len;
}
static int diag_cmd_disable_log_mask(unsigned char *src_buf, int src_len,
unsigned char *dest_buf, int dest_len)
{
struct diag_log_mask_t *mask = (struct diag_log_mask_t *)log_mask.ptr;
struct diag_log_config_rsp_t header;
int write_len = 0;
int i;
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;
}
mutex_lock(&log_mask.lock);
for (i = 0; i < MAX_EQUIP_ID; i++, mask++)
memset(mask->ptr, 0, mask->range);
log_mask.status = DIAG_CTRL_MASK_ALL_DISABLED;
mutex_unlock(&log_mask.lock);
diag_update_userspace_clients(LOG_MASKS_TYPE);
/*
* Apps processor must send the response to this command. Frame the
* response.
*/
header.cmd_code = DIAG_CMD_LOG_CONFIG;
header.padding[0] = 0;
header.padding[1] = 0;
header.padding[2] = 0;
header.sub_cmd = DIAG_CMD_OP_LOG_DISABLE;
header.status = LOG_STATUS_SUCCESS;
memcpy(dest_buf, &header, sizeof(struct diag_log_config_rsp_t));
write_len += sizeof(struct diag_log_config_rsp_t);
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++)
diag_send_log_mask_update(&driver->smd_cntl[i], ALL_EQUIP_ID);
return write_len;
}
static int diag_process_apps_pkt(unsigned char *buf, int len)
{
uint16_t start;
uint16_t end, subsys_cmd_code;
int i, cmd_code, subsys_id;
int packet_type = 1;
unsigned char *temp = buf;
/* event mask */
if ((*buf == 0x60) && (*(++buf) == 0x0)) {
diag_update_event_mask(buf, 0, 0);
diag_update_userspace_clients(EVENT_MASKS_TYPE);
}
/* check for set event mask */
else if (*buf == 0x82) {
buf += 4;
diag_update_event_mask(buf, 1, *(uint16_t *)buf);
diag_update_userspace_clients(
EVENT_MASKS_TYPE);
}
/* log mask */
else if (*buf == 0x73) {
buf += 4;
if (*(int *)buf == 3) {
buf += 4;
/* 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);
}
}
/* Check for set message mask */
else if ((*buf == 0x7d) && (*(++buf) == 0x4)) {
buf++;
start = *(uint16_t *)buf;
buf += 2;
end = *(uint16_t *)buf;
buf += 4;
diag_update_msg_mask((uint32_t)start, (uint32_t)end , buf);
diag_update_userspace_clients(MSG_MASKS_TYPE);
}
/* Set all run-time masks
if ((*buf == 0x7d) && (*(++buf) == 0x5)) {
TO DO
} */
#if defined(CONFIG_DIAG_NO_MODEM) && defined(CONFIG_DIAG_OVER_USB)
/* Respond to polling for Apps only DIAG */
else if ((*buf == 0x4b) && (*(buf+1) == 0x32) && (*(buf+2) == 0x03)) {
for (i = 0; i < 3; i++)
driver->apps_rsp_buf[i] = *(buf+i);
for (i = 0; i < 13; i++)
driver->apps_rsp_buf[i+3] = 0;
ENCODE_RSP_AND_SEND(15);
return 0;
}
/* respond to 0x0 command */
else if (*buf == 0x00) {
for (i = 0; i < 55; i++)
driver->apps_rsp_buf[i] = 0;
ENCODE_RSP_AND_SEND(54);
return 0;
}
/* respond to 0x7c command */
else if (*buf == 0x7c) {
driver->apps_rsp_buf[0] = 0x7c;
for (i = 1; i < 8; i++)
driver->apps_rsp_buf[i] = 0;
*(int *)(driver->apps_rsp_buf + 8) = 4062; /* ID for APQ 8060 */
*(unsigned char *)(driver->apps_rsp_buf + 12) = '\0';
*(unsigned char *)(driver->apps_rsp_buf + 13) = '\0';
ENCODE_RSP_AND_SEND(13);
return 0;
}
#endif /* DIAG over USB & NO MODEM present */
/* Check for registered clients and forward packet to user-space */
else{
cmd_code = (int)(*(char *)buf);
temp++;
subsys_id = (int)(*(char *)temp);
temp++;
subsys_cmd_code = *(uint16_t *)temp;
temp += 2;
for (i = 0; i < diag_max_registration; i++) {
if (driver->table[i].process_id != 0) {
if (driver->table[i].cmd_code ==
cmd_code && driver->table[i].subsys_id ==
subsys_id &&
driver->table[i].cmd_code_lo <=
subsys_cmd_code &&
driver->table[i].cmd_code_hi >=
subsys_cmd_code){
driver->pkt_length = len;
diag_update_pkt_buffer(buf);
diag_update_sleeping_process(
driver->table[i].process_id);
return 0;
} /* end of if */
else if (driver->table[i].cmd_code == 255
&& cmd_code == 75) {
if (driver->table[i].subsys_id ==
subsys_id &&
driver->table[i].cmd_code_lo <=
subsys_cmd_code &&
driver->table[i].cmd_code_hi >=
subsys_cmd_code){
driver->pkt_length = len;
diag_update_pkt_buffer(buf);
diag_update_sleeping_process(
driver->table[i].
process_id);
return 0;
}
} /* end of else-if */
else if (driver->table[i].cmd_code == 255 &&
driver->table[i].subsys_id == 255) {
if (driver->table[i].cmd_code_lo <=
cmd_code &&
driver->table[i].
cmd_code_hi >= cmd_code){
driver->pkt_length = len;
diag_update_pkt_buffer(buf);
diag_update_sleeping_process
(driver->table[i].
process_id);
return 0;
}
} /* end of else-if */
} /* if(driver->table[i].process_id != 0) */
} /* for (i = 0; i < diag_max_registration; i++) */
} /* else */
return packet_type;
}
static int diag_process_apps_pkt(unsigned char *buf, int len)
{
uint16_t start;
uint16_t end, subsys_cmd_code;
int i, cmd_code, subsys_id;
int packet_type = 1;
unsigned char *temp = buf;
/* event mask */
if ((*buf == 0x60) && (*(++buf) == 0x0)) {
diag_update_event_mask(buf, 0, 0);
diag_update_userspace_clients(EVENT_MASKS_TYPE);
}
/* check for set event mask */
else if (*buf == 0x82) {
buf += 4;
diag_update_event_mask(buf, 1, *(uint16_t *)buf);
diag_update_userspace_clients(
EVENT_MASKS_TYPE);
}
/* log mask */
else if (*buf == 0x73) {
buf += 4;
if (*(int *)buf == 3) {
buf += 8;
diag_update_log_mask(buf+4, *(int *)buf);
diag_update_userspace_clients(LOG_MASKS_TYPE);
}
}
/* Check for set message mask */
else if ((*buf == 0x7d) && (*(++buf) == 0x4)) {
buf++;
start = *(uint16_t *)buf;
buf += 2;
end = *(uint16_t *)buf;
buf += 4;
diag_update_msg_mask((uint32_t)start, (uint32_t)end , buf);
diag_update_userspace_clients(MSG_MASKS_TYPE);
}
/* Set all run-time masks
if ((*buf == 0x7d) && (*(++buf) == 0x5)) {
TO DO
} */
/* Check for registered clients and forward packet to user-space */
else{
cmd_code = (int)(*(char *)buf);
temp++;
subsys_id = (int)(*(char *)temp);
temp++;
subsys_cmd_code = *(uint16_t *)temp;
temp += 2;
for (i = 0; i < REG_TABLE_SIZE; i++) {
if (driver->table[i].process_id != 0) {
if (driver->table[i].cmd_code ==
cmd_code && driver->table[i].subsys_id ==
subsys_id &&
driver->table[i].cmd_code_lo <=
subsys_cmd_code &&
driver->table[i].cmd_code_hi >=
subsys_cmd_code){
driver->pkt_length = len;
diag_update_pkt_buffer(buf);
diag_update_sleeping_process(
driver->table[i].process_id);
return 0;
} /* end of if */
else if (driver->table[i].cmd_code == 255
&& cmd_code == 75) {
if (driver->table[i].subsys_id ==
subsys_id &&
driver->table[i].cmd_code_lo <=
subsys_cmd_code &&
driver->table[i].cmd_code_hi >=
subsys_cmd_code){
driver->pkt_length = len;
diag_update_pkt_buffer(buf);
diag_update_sleeping_process(
driver->table[i].
process_id);
return 0;
}
} /* end of else-if */
else if (driver->table[i].cmd_code == 255 &&
driver->table[i].subsys_id == 255) {
if (driver->table[i].cmd_code_lo <=
cmd_code &&
driver->table[i].
cmd_code_hi >= cmd_code){
driver->pkt_length = len;
diag_update_pkt_buffer(buf);
diag_update_sleeping_process
(driver->table[i].
process_id);
return 0;
}
} /* end of else-if */
} /* if(driver->table[i].process_id != 0) */
} /* for (i = 0; i < REG_TABLE_SIZE; i++) */
} /* else */
return packet_type;
}
static int diag_process_apps_pkt(unsigned char *buf, int len)
{
uint16_t subsys_cmd_code;
int subsys_id, ssid_first, ssid_last, ssid_range;
int packet_type = 1, i, cmd_code;
unsigned char *temp = buf;
int data_type;
#if defined(CONFIG_DIAG_OVER_USB)
int payload_length;
unsigned char *ptr;
#endif
/* Check for registered clients and forward packet to apropriate proc */
cmd_code = (int)(*(char *)buf);
temp++;
subsys_id = (int)(*(char *)temp);
temp++;
subsys_cmd_code = *(uint16_t *)temp;
temp += 2;
data_type = APPS_DATA;
/* Dont send any command other than mode reset */
if (cpu_is_msm8960() && cmd_code == MODE_CMD) {
if (subsys_id != RESET_ID)
data_type = MODEM_DATA;
}
pr_debug("diag: %d %d %d", cmd_code, subsys_id, subsys_cmd_code);
for (i = 0; i < diag_max_registration; 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) {
diag_send_data(entry, buf, len, data_type);
packet_type = 0;
} 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) {
diag_send_data(entry, buf, len,
data_type);
packet_type = 0;
}
} else if (entry.cmd_code == 255 &&
entry.subsys_id == 255) {
if (entry.cmd_code_lo <=
cmd_code &&
entry.
cmd_code_hi >= cmd_code) {
diag_send_data(entry, buf, len,
data_type);
packet_type = 0;
}
}
}
}
/* set event mask */
if (*buf == 0x82) {
buf += 4;
diag_update_event_mask(buf, 1, *(uint16_t *)buf);
diag_update_userspace_clients(EVENT_MASKS_TYPE);
}
/* event mask change */
else if ((*buf == 0x60) && (*(buf+1) == 0x0)) {
diag_update_event_mask(buf+1, 0, 0);
diag_update_userspace_clients(EVENT_MASKS_TYPE);
#if defined(CONFIG_DIAG_OVER_USB)
/* Check for Apps Only 8960 */
if (!(driver->ch) && (chk_config_get_id() == AO8960_TOOLS_ID)) {
/* echo response back for apps only DIAG */
driver->apps_rsp_buf[0] = 0x60;
driver->apps_rsp_buf[1] = 0x0;
driver->apps_rsp_buf[2] = 0x0;
ENCODE_RSP_AND_SEND(2);
return 0;
}
#endif
}
/* Set log masks */
else 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)
/* Check for Apps Only 8960 */
if (!(driver->ch) && (chk_config_get_id() == AO8960_TOOLS_ID)) {
/* echo response back for Apps only DIAG */
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+8+i);
ENCODE_RSP_AND_SEND(12 + payload_length - 1);
return 0;
}
#endif
}
/* Check for set 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);
diag_update_msg_mask(ssid_first, ssid_last , buf + 8);
diag_update_userspace_clients(MSG_MASKS_TYPE);
#if defined(CONFIG_DIAG_OVER_USB)
if (!(driver->ch) && (chk_config_get_id() == AO8960_TOOLS_ID)) {
/* echo response back for apps only DIAG */
for (i = 0; i < 8 + ssid_range; i++)
*(driver->apps_rsp_buf + i) = *(buf+i);
ENCODE_RSP_AND_SEND(8 + ssid_range - 1);
return 0;
}
#endif
}
#if defined(CONFIG_DIAG_OVER_USB)
/* Check for Apps Only 8960 & get event mask request */
else if (!(driver->ch) && (chk_config_get_id() == AO8960_TOOLS_ID)
&& *buf == 0x81) {
driver->apps_rsp_buf[0] = 0x81;
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;
for (i = 0; i < EVENT_LAST_ID/8 + 1; i++)
*(unsigned char *)(driver->apps_rsp_buf + 6 + i) = 0x0;
ENCODE_RSP_AND_SEND(6 + EVENT_LAST_ID/8);
return 0;
}
/* Get log ID range & Check for Apps Only 8960 */
else if (!(driver->ch) && (chk_config_get_id() == AO8960_TOOLS_ID)
&& (*buf == 0x73) && *(int *)(buf+4) == 1) {
driver->apps_rsp_buf[0] = 0x73;
*(int *)(driver->apps_rsp_buf + 4) = 0x1; /* operation ID */
*(int *)(driver->apps_rsp_buf + 8) = 0x0; /* success code */
*(int *)(driver->apps_rsp_buf + 12) = LOG_GET_ITEM_NUM(LOG_0);
*(int *)(driver->apps_rsp_buf + 16) = LOG_GET_ITEM_NUM(LOG_1);
*(int *)(driver->apps_rsp_buf + 20) = LOG_GET_ITEM_NUM(LOG_2);
*(int *)(driver->apps_rsp_buf + 24) = LOG_GET_ITEM_NUM(LOG_3);
*(int *)(driver->apps_rsp_buf + 28) = LOG_GET_ITEM_NUM(LOG_4);
*(int *)(driver->apps_rsp_buf + 32) = LOG_GET_ITEM_NUM(LOG_5);
*(int *)(driver->apps_rsp_buf + 36) = LOG_GET_ITEM_NUM(LOG_6);
*(int *)(driver->apps_rsp_buf + 40) = LOG_GET_ITEM_NUM(LOG_7);
*(int *)(driver->apps_rsp_buf + 44) = LOG_GET_ITEM_NUM(LOG_8);
*(int *)(driver->apps_rsp_buf + 48) = LOG_GET_ITEM_NUM(LOG_9);
*(int *)(driver->apps_rsp_buf + 52) = LOG_GET_ITEM_NUM(LOG_10);
*(int *)(driver->apps_rsp_buf + 56) = LOG_GET_ITEM_NUM(LOG_11);
*(int *)(driver->apps_rsp_buf + 60) = LOG_GET_ITEM_NUM(LOG_12);
*(int *)(driver->apps_rsp_buf + 64) = LOG_GET_ITEM_NUM(LOG_13);
*(int *)(driver->apps_rsp_buf + 68) = LOG_GET_ITEM_NUM(LOG_14);
*(int *)(driver->apps_rsp_buf + 72) = LOG_GET_ITEM_NUM(LOG_15);
ENCODE_RSP_AND_SEND(75);
return 0;
}
/* Respond to Get SSID Range request message */
else if (!(driver->ch) && (chk_config_get_id() == AO8960_TOOLS_ID)
&& (*buf == 0x7d) && (*(buf+1) == 0x1)) {
driver->apps_rsp_buf[0] = 0x7d;
driver->apps_rsp_buf[1] = 0x1;
driver->apps_rsp_buf[2] = 0x1;
driver->apps_rsp_buf[3] = 0x0;
*(int *)(driver->apps_rsp_buf + 4) = MSG_MASK_TBL_CNT;
*(uint16_t *)(driver->apps_rsp_buf + 8) = MSG_SSID_0;
*(uint16_t *)(driver->apps_rsp_buf + 10) = MSG_SSID_0_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 12) = MSG_SSID_1;
*(uint16_t *)(driver->apps_rsp_buf + 14) = MSG_SSID_1_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 16) = MSG_SSID_2;
*(uint16_t *)(driver->apps_rsp_buf + 18) = MSG_SSID_2_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 20) = MSG_SSID_3;
*(uint16_t *)(driver->apps_rsp_buf + 22) = MSG_SSID_3_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 24) = MSG_SSID_4;
*(uint16_t *)(driver->apps_rsp_buf + 26) = MSG_SSID_4_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 28) = MSG_SSID_5;
*(uint16_t *)(driver->apps_rsp_buf + 30) = MSG_SSID_5_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 32) = MSG_SSID_6;
*(uint16_t *)(driver->apps_rsp_buf + 34) = MSG_SSID_6_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 36) = MSG_SSID_7;
*(uint16_t *)(driver->apps_rsp_buf + 38) = MSG_SSID_7_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 40) = MSG_SSID_8;
*(uint16_t *)(driver->apps_rsp_buf + 42) = MSG_SSID_8_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 44) = MSG_SSID_9;
*(uint16_t *)(driver->apps_rsp_buf + 46) = MSG_SSID_9_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 48) = MSG_SSID_10;
*(uint16_t *)(driver->apps_rsp_buf + 50) = MSG_SSID_10_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 52) = MSG_SSID_11;
*(uint16_t *)(driver->apps_rsp_buf + 54) = MSG_SSID_11_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 56) = MSG_SSID_12;
*(uint16_t *)(driver->apps_rsp_buf + 58) = MSG_SSID_12_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 60) = MSG_SSID_13;
*(uint16_t *)(driver->apps_rsp_buf + 62) = MSG_SSID_13_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 64) = MSG_SSID_14;
*(uint16_t *)(driver->apps_rsp_buf + 66) = MSG_SSID_14_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 68) = MSG_SSID_15;
*(uint16_t *)(driver->apps_rsp_buf + 70) = MSG_SSID_15_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 72) = MSG_SSID_16;
*(uint16_t *)(driver->apps_rsp_buf + 74) = MSG_SSID_16_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 76) = MSG_SSID_17;
*(uint16_t *)(driver->apps_rsp_buf + 78) = MSG_SSID_17_LAST;
*(uint16_t *)(driver->apps_rsp_buf + 80) = MSG_SSID_18;
*(uint16_t *)(driver->apps_rsp_buf + 82) = MSG_SSID_18_LAST;
ENCODE_RSP_AND_SEND(83);
return 0;
}
/* Check for AO8960 Respond to Get Subsys Build mask */
else if (!(driver->ch) && (chk_config_get_id() == AO8960_TOOLS_ID)
&& (*buf == 0x7d) && (*(buf+1) == 0x2)) {
ssid_first = *(uint16_t *)(buf + 2);
ssid_last = *(uint16_t *)(buf + 4);
ssid_range = 4 * (ssid_last - ssid_first + 1);
/* frame response */
driver->apps_rsp_buf[0] = 0x7d;
driver->apps_rsp_buf[1] = 0x2;
*(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;
ptr = driver->apps_rsp_buf + 8;
/* bld time masks */
switch (ssid_first) {
case MSG_SSID_0:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_0[i/4];
break;
case MSG_SSID_1:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_1[i/4];
break;
case MSG_SSID_2:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_2[i/4];
break;
case MSG_SSID_3:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_3[i/4];
break;
case MSG_SSID_4:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_4[i/4];
break;
case MSG_SSID_5:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_5[i/4];
break;
case MSG_SSID_6:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_6[i/4];
break;
case MSG_SSID_7:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_7[i/4];
break;
case MSG_SSID_8:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_8[i/4];
break;
case MSG_SSID_9:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_9[i/4];
break;
case MSG_SSID_10:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_10[i/4];
break;
case MSG_SSID_11:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_11[i/4];
break;
case MSG_SSID_12:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_12[i/4];
break;
case MSG_SSID_13:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_13[i/4];
break;
case MSG_SSID_14:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_14[i/4];
break;
case MSG_SSID_15:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_15[i/4];
break;
case MSG_SSID_16:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_16[i/4];
break;
case MSG_SSID_17:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_17[i/4];
break;
case MSG_SSID_18:
for (i = 0; i < ssid_range; i += 4)
*(int *)(ptr + i) = msg_bld_masks_18[i/4];
break;
}
ENCODE_RSP_AND_SEND(8 + ssid_range - 1);
return 0;
}
/* Check for download command */
else if ((cpu_is_msm8x60() || cpu_is_msm8960()) && (*buf == 0x3A)) {
/* send response back */
driver->apps_rsp_buf[0] = *buf;
ENCODE_RSP_AND_SEND(0);
msleep(5000);
/* call download API */
msm_set_restart_mode(RESTART_DLOAD);
printk(KERN_CRIT "diag: download mode set, Rebooting SoC..\n");
kernel_restart(NULL);
/* Not required, represents that command isnt sent to modem */
return 0;
}
/* Check for ID for NO MODEM present */
else if (!(driver->ch)) {
/* Respond to polling for Apps only DIAG */
if ((*buf == 0x4b) && (*(buf+1) == 0x32) &&
(*(buf+2) == 0x03)) {
for (i = 0; i < 3; i++)
driver->apps_rsp_buf[i] = *(buf+i);
for (i = 0; i < 13; i++)
driver->apps_rsp_buf[i+3] = 0;
ENCODE_RSP_AND_SEND(15);
return 0;
}
/* respond to 0x0 command */
else if (*buf == 0x00) {
for (i = 0; i < 55; i++)
driver->apps_rsp_buf[i] = 0;
ENCODE_RSP_AND_SEND(54);
return 0;
}
/* respond to 0x7c command */
else if (*buf == 0x7c) {
driver->apps_rsp_buf[0] = 0x7c;
for (i = 1; i < 8; i++)
driver->apps_rsp_buf[i] = 0;
/* Tools ID for APQ 8060 */
*(int *)(driver->apps_rsp_buf + 8) =
chk_config_get_id();
*(unsigned char *)(driver->apps_rsp_buf + 12) = '\0';
*(unsigned char *)(driver->apps_rsp_buf + 13) = '\0';
ENCODE_RSP_AND_SEND(13);
return 0;
}
}
#endif
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, 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;
}
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;
}
static int diag_process_apps_pkt(unsigned char *buf, int len)
{
uint16_t start;
uint16_t end, subsys_cmd_code;
int i, cmd_code, subsys_id;
int packet_type = 1;
unsigned char *temp = buf;
if (*buf == 0xFA) {
g_testmode = 1;
}
if ((*buf == 0x60) && (*(++buf) == 0x0)) {
diag_update_event_mask(buf, 0, 0);
diag_update_userspace_clients(EVENT_MASKS_TYPE);
}
else if (*buf == 0x82) {
buf += 4;
diag_update_event_mask(buf, 1, *(uint16_t *)buf);
diag_update_userspace_clients(
EVENT_MASKS_TYPE);
}
else if (*buf == 0x73) {
buf += 4;
if (*(int *)buf == 3) {
buf += 8;
diag_update_log_mask(buf+4, *(int *)buf);
diag_update_userspace_clients(LOG_MASKS_TYPE);
}
}
else if ((*buf == 0x7d) && (*(++buf) == 0x4)) {
buf++;
start = *(uint16_t *)buf;
buf += 2;
end = *(uint16_t *)buf;
buf += 4;
diag_update_msg_mask((uint32_t)start, (uint32_t)end , buf);
diag_update_userspace_clients(MSG_MASKS_TYPE);
}
else{
cmd_code = (int)(*(char *)buf);
temp++;
subsys_id = (int)(*(char *)temp);
temp++;
subsys_cmd_code = *(uint16_t *)temp;
temp += 2;
for (i = 0; i < diag_max_registration; i++) {
if (driver->table[i].process_id != 0) {
if (driver->table[i].cmd_code ==
cmd_code && driver->table[i].subsys_id ==
subsys_id &&
driver->table[i].cmd_code_lo <=
subsys_cmd_code &&
driver->table[i].cmd_code_hi >=
subsys_cmd_code){
driver->pkt_length = len;
diag_update_pkt_buffer(buf);
diag_update_sleeping_process(
driver->table[i].process_id);
return 0;
}
else if (driver->table[i].cmd_code == 255
&& cmd_code == 75) {
if (driver->table[i].subsys_id ==
subsys_id &&
driver->table[i].cmd_code_lo <=
subsys_cmd_code &&
driver->table[i].cmd_code_hi >=
subsys_cmd_code){
driver->pkt_length = len;
diag_update_pkt_buffer(buf);
diag_update_sleeping_process(
driver->table[i].
process_id);
return 0;
}
}
else if (driver->table[i].cmd_code == 255 &&
driver->table[i].subsys_id == 255) {
if (driver->table[i].cmd_code_lo <=
cmd_code &&
driver->table[i].
cmd_code_hi >= cmd_code){
driver->pkt_length = len;
diag_update_pkt_buffer(buf);
diag_update_sleeping_process
(driver->table[i].
process_id);
return 0;
}
}
}
}
}
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_set_msg_mask(unsigned char *src_buf, int src_len,
unsigned char *dest_buf, int dest_len)
{
int i;
int write_len = 0;
int header_len = sizeof(struct diag_msg_build_mask_t);
int found = 0;
uint32_t mask_size = 0;
uint32_t offset = 0;
struct diag_msg_mask_t *mask = NULL;
struct diag_msg_build_mask_t *req = NULL;
struct diag_msg_build_mask_t rsp;
uint32_t *temp = 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_msg_build_mask_t *)src_buf;
mutex_lock(&msg_mask.lock);
mask = (struct diag_msg_mask_t *)msg_mask.ptr;
for (i = 0; i < driver->msg_mask_tbl_count; i++, mask++) {
if ((req->ssid_first < mask->ssid_first) ||
(req->ssid_first > mask->ssid_last_tools)) {
continue;
}
found = 1;
mask_size = req->ssid_last - req->ssid_first + 1;
if (mask_size > MAX_SSID_PER_RANGE) {
pr_warn("diag: In %s, truncating ssid range, %d-%d to max allowed: %d\n",
__func__, mask->ssid_first, mask->ssid_last,
MAX_SSID_PER_RANGE);
mask_size = MAX_SSID_PER_RANGE;
mask->range_tools = MAX_SSID_PER_RANGE;
mask->ssid_last_tools =
mask->ssid_first + mask->range_tools;
}
if (req->ssid_last > mask->ssid_last_tools) {
pr_debug("diag: Msg SSID range mismatch\n");
if (mask_size != MAX_SSID_PER_RANGE)
mask->ssid_last_tools = req->ssid_last;
temp = krealloc(mask->ptr,
mask_size * sizeof(uint32_t),
GFP_KERNEL);
if (!temp) {
pr_err_ratelimited("diag: In %s, unable to allocate memory for msg mask ptr, mask_size: %d\n",
__func__, mask_size);
return -ENOMEM;
}
mask->ptr = temp;
mask->range_tools = mask_size;
}
offset = req->ssid_first - mask->ssid_first;
if (offset + mask_size > mask->range_tools) {
pr_err("diag: In %s, Not in msg mask range, mask_size: %d, offset: %d\n",
__func__, mask_size, offset);
break;
}
mask_size = mask_size * sizeof(uint32_t);
memcpy(mask->ptr + offset, src_buf + header_len, mask_size);
msg_mask.status = DIAG_CTRL_MASK_VALID;
break;
}
mutex_unlock(&msg_mask.lock);
diag_update_userspace_clients(MSG_MASKS_TYPE);
/*
* Apps processor must send the response to this command. Frame the
* response.
*/
rsp.cmd_code = DIAG_CMD_MSG_CONFIG;
rsp.sub_cmd = DIAG_CMD_OP_SET_MSG_MASK;
rsp.ssid_first = req->ssid_first;
rsp.ssid_last = req->ssid_last;
rsp.status = found;
rsp.padding = 0;
memcpy(dest_buf, &rsp, header_len);
write_len += header_len;
if (!found)
goto end;
if (mask_size + write_len > dest_len)
mask_size = dest_len - write_len;
memcpy(dest_buf + write_len, src_buf + header_len, mask_size);
write_len += mask_size;
for (i = 0; i < NUM_SMD_CONTROL_CHANNELS; i++) {
diag_send_msg_mask_update(&driver->smd_cntl[i],
req->ssid_first, req->ssid_last);
}
end:
return write_len;
}
static int diag_process_apps_pkt(unsigned char *buf, int len)
{
uint16_t start;
uint16_t end, subsys_cmd_code;
int i, cmd_code, subsys_id;
int packet_type = 1;
unsigned char *temp = buf;
/* event mask */
if ((*buf == 0x60) && (*(++buf) == 0x0)) {
diag_update_event_mask(buf, 0, 0);
diag_update_userspace_clients(EVENT_MASKS_TYPE);
}
/* check for set event mask */
else if (*buf == 0x82) {
buf += 4;
diag_update_event_mask(buf, 1, *(uint16_t *)buf);
diag_update_userspace_clients(
EVENT_MASKS_TYPE);
}
/* log mask */
else if (*buf == 0x73) {
buf += 4;
if (*(int *)buf == 3) {
buf += 8;
diag_update_log_mask(buf+4, *(int *)buf);
diag_update_userspace_clients(LOG_MASKS_TYPE);
}
}
/* Check for set message mask */
else if ((*buf == 0x7d) && ((*(buf+1) == 0x4) || (*(buf+1) == 0x5))) {
/* printk("len = %d\n", len);
print_hex_dump(KERN_DEBUG, "Read Packet Data"
" from message mask: ", 16, 1, DUMP_PREFIX_ADDRESS, buf, len, 1);
*/
if (*(buf+1) == 0x4) {
buf += 2;
start = *(uint16_t *)buf;
buf += 2;
end = *(uint16_t *)buf;
buf += 4;
diag_update_msg_mask((uint32_t)start, (uint32_t)end , buf, 0);
diag_update_userspace_clients(MSG_MASKS_TYPE);
} else {
diag_update_msg_mask(0, 0x4e, buf, 1);
diag_update_userspace_clients(MSG_MASKS_TYPE);
}
}
/* Set all run-time masks
if ((*buf == 0x7d) && (*(++buf) == 0x5)) {
TO DO
} */
/* Check for registered clients and forward packet to user-space */
else{
cmd_code = (int)(*(char *)buf);
temp++;
subsys_id = (int)(*(char *)temp);
temp++;
subsys_cmd_code = *(uint16_t *)temp;
temp += 2;
for (i = 0; i < diag_max_registration; i++) {
if (driver->table[i].process_id != 0) {
if (driver->table[i].cmd_code ==
cmd_code && driver->table[i].subsys_id ==
subsys_id &&
driver->table[i].cmd_code_lo <=
subsys_cmd_code &&
driver->table[i].cmd_code_hi >=
subsys_cmd_code){
driver->pkt_length = len;
diag_update_pkt_buffer(buf);
diag_update_sleeping_process(
driver->table[i].process_id);
return 0;
} /* end of if */
else if (driver->table[i].cmd_code == 255
&& cmd_code == 75) {
if (driver->table[i].subsys_id ==
subsys_id &&
driver->table[i].cmd_code_lo <=
subsys_cmd_code &&
driver->table[i].cmd_code_hi >=
subsys_cmd_code){
driver->pkt_length = len;
diag_update_pkt_buffer(buf);
diag_update_sleeping_process(
driver->table[i].
process_id);
return 0;
}
} /* end of else-if */
else if (driver->table[i].cmd_code == 255 &&
driver->table[i].subsys_id == 255) {
if (driver->table[i].cmd_code_lo <=
cmd_code &&
driver->table[i].
cmd_code_hi >= cmd_code){
driver->pkt_length = len;
diag_update_pkt_buffer(buf);
diag_update_sleeping_process
(driver->table[i].
process_id);
return 0;
}
} /* end of else-if */
} /* if(driver->table[i].process_id != 0) */
} /* for (i = 0; i < diag_max_registration; i++) */
} /* else */
return packet_type;
}
