Revert "diag: Update Slave processor with mask info"
This reverts commit 4ad8986555f65949b8102c06518427ca1230d065.
Signed-off-by: Shalabh Jain <shalabhj@codeaurora.org>
diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c
index 8d914dd..6853654 100644
--- a/drivers/char/diag/diagfwd.c
+++ b/drivers/char/diag/diagfwd.c
@@ -43,15 +43,8 @@
static unsigned int buf_tbl_size = 8; /*Number of entries in table of buffers */
struct diag_master_table entry;
smd_channel_t *ch_temp, *chqdsp_temp, *ch_wcnss_temp;
-int diag_event_num_bytes;
-int diag_event_config;
struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 };
struct diag_hdlc_dest_type enc = { NULL, NULL, 0 };
-struct mask_info {
- int equip_id;
- int num_items;
- int index;
-};
#define ENCODE_RSP_AND_SEND(buf_length) \
do { \
@@ -98,7 +91,7 @@
}
/*
- * This will return TRUE for targets which support apps only mode and hence SSR.
+ * This will return TRUE for targets which support apps only mode.
* This applies to 8960 and newer targets.
*/
int chk_apps_only(void)
@@ -388,6 +381,7 @@
mutex_lock(&driver->diagchar_mutex);
/* First SSID can be zero : So check that last is non-zero */
+
while (*(uint32_t *)(ptr + 4)) {
first = *(uint32_t *)ptr;
ptr += 4;
@@ -407,6 +401,7 @@
else
printk(KERN_INFO "Unable to copy"
" mask change\n");
+
found = 1;
break;
} else {
@@ -428,9 +423,10 @@
}
mutex_unlock(&driver->diagchar_mutex);
diag_print_mask_table();
+
}
-static void diag_update_event_mask(uint8_t *buf, int toggle)
+static void diag_update_event_mask(uint8_t *buf, int toggle, int num_bits)
{
uint8_t *ptr = driver->event_masks;
uint8_t *temp = buf + 2;
@@ -441,8 +437,8 @@
else
if (CHK_OVERFLOW(ptr, ptr,
ptr+EVENT_MASK_SIZE,
- diag_event_num_bytes))
- memcpy(ptr, temp , diag_event_num_bytes);
+ num_bits/8 + 1))
+ memcpy(ptr, temp , num_bits/8 + 1);
else
printk(KERN_CRIT "Not enough buffer space "
"for EVENT_MASK\n");
@@ -452,6 +448,10 @@
static void diag_update_log_mask(int equip_id, uint8_t *buf, int num_items)
{
uint8_t *temp = buf;
+ struct mask_info {
+ int equip_id;
+ int index;
+ };
int i = 0;
unsigned char *ptr_data;
int offset = 8*MAX_EQUIP_ID;
@@ -465,9 +465,8 @@
break;
}
if ((ptr->equip_id == 0) && (ptr->index == 0)) {
- /* Reached a null entry */
+ /*Reached a null entry */
ptr->equip_id = equip_id;
- ptr->num_items = num_items;
ptr->index = driver->log_masks_length;
offset = driver->log_masks_length;
driver->log_masks_length += ((num_items+7)/8);
@@ -553,88 +552,6 @@
}
}
-void diag_send_log_mask_update(smd_channel_t *ch)
-{
- unsigned char buf[1000];
- int header_size = sizeof(struct diag_ctrl_log_mask);
- struct mask_info *ptr = (struct mask_info *)driver->log_masks;
- int i, size = (driver->log_mask->num_items+7)/8;
-
- for (i = 0; i < MAX_EQUIP_ID; i++) {
- /* reached null entry */
- if ((ptr->equip_id == 0) && (ptr->index == 0))
- break;
- driver->log_mask->cmd_type = DIAG_CTRL_MSG_LOG_MASK;
- driver->log_mask->num_items = ptr->num_items;
- driver->log_mask->data_len = 11 + size;
- driver->log_mask->stream_id = 1; /* 2, if dual stream */
- driver->log_mask->status = 3; /* status for valid mask */
- driver->log_mask->equip_id = ptr->equip_id;
- driver->log_mask->log_mask_size = size;
- memcpy(buf, driver->log_mask, header_size);
- memcpy(buf+header_size, driver->log_masks+ptr->index, size);
- if (ch)
- smd_write(ch, buf, header_size + size);
- else
- pr_info("diag: invalid SMD channel\n");
- ptr++;
- }
-}
-
-void diag_send_event_mask_update(smd_channel_t *ch)
-{
- unsigned char buf[EVENT_MASK_SIZE];
- int header_size = sizeof(struct diag_ctrl_event_mask);
-
- /* send event mask update */
- driver->event_mask->cmd_type = DIAG_CTRL_MSG_EVENT_MASK;
- driver->event_mask->data_len = 7 + diag_event_num_bytes;
- driver->event_mask->stream_id = 1; /* 2, if dual stream */
- driver->event_mask->status = 3; /* status for valid mask */
- driver->event_mask->event_config = diag_event_config; /* event config */
- driver->event_mask->event_mask_size = diag_event_num_bytes;
- memcpy(buf, driver->event_mask, header_size);
- memcpy(buf+header_size, driver->event_masks, diag_event_num_bytes);
- if (ch)
- smd_write(ch, buf, header_size + diag_event_num_bytes);
- else
- pr_info("diag: invalid SMD channel\n");
-}
-
-void diag_send_msg_mask_update(smd_channel_t *ch)
-{
- unsigned char buf[1000];
- int first, last;
- int header_size = sizeof(struct diag_ctrl_msg_mask);
- uint8_t *ptr = driver->msg_masks;
-
- while (*(uint32_t *)(ptr + 4)) {
- first = *(uint32_t *)ptr;
- ptr += 4;
- last = *(uint32_t *)ptr;
- ptr += 4;
- /* send event mask update */
- driver->msg_mask->cmd_type = DIAG_CTRL_MSG_F3_MASK;
- driver->msg_mask->msg_mask_size = last - first + 1;
- driver->msg_mask->data_len = 11 +
- 4 * (driver->msg_mask->msg_mask_size);
- driver->msg_mask->stream_id = 1; /* 2, if dual stream */
- driver->msg_mask->status = 3; /* status for valid mask */
- driver->msg_mask->msg_mode = 0; /* Legcay mode */
- driver->msg_mask->ssid_first = first;
- driver->msg_mask->ssid_last = last;
- memcpy(buf, driver->msg_mask, header_size);
- memcpy(buf+header_size, ptr,
- 4 * (driver->msg_mask->msg_mask_size));
- if (ch)
- smd_write(ch, buf,
- header_size + 4*(driver->msg_mask->msg_mask_size));
- else
- pr_info("diag: invalid SMD channel\n");
- ptr += ((last - first) + 1)*4;
- }
-}
-
static int diag_process_apps_pkt(unsigned char *buf, int len)
{
uint16_t subsys_cmd_code;
@@ -647,100 +564,6 @@
unsigned char *ptr;
#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+8+i);
- ENCODE_RSP_AND_SEND(12 + payload_length - 1);
- if (driver->ch_cntl) {
- diag_send_log_mask_update(driver->ch_cntl);
- diag_send_log_mask_update(driver->chqdsp_cntl);
- diag_send_log_mask_update(
- driver->ch_wcnss_cntl);
- }
- return 0;
- } else
- buf = temp;
-#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 (chk_apps_only()) {
- for (i = 0; i < 8 + ssid_range; i++)
- *(driver->apps_rsp_buf + i) = *(buf+i);
- ENCODE_RSP_AND_SEND(8 + ssid_range - 1);
- if (driver->ch_cntl) {
- diag_send_msg_mask_update(driver->ch_cntl);
- diag_send_msg_mask_update(driver->chqdsp_cntl);
- diag_send_msg_mask_update(
- driver->ch_wcnss_cntl);
- }
- return 0;
- } else
- buf = temp;
-#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);
- 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;
- 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);
- if (driver->ch_cntl) {
- diag_send_event_mask_update(driver->ch_cntl);
- diag_send_event_mask_update(
- driver->chqdsp_cntl);
- diag_send_event_mask_update(
- driver->ch_wcnss_cntl);
- }
- return 0;
- } else
- buf = temp;
-#endif
- } else if (*buf == 0x60) {
- diag_event_config = *(buf+1);
-#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;
- ENCODE_RSP_AND_SEND(2);
- if (driver->ch_cntl) {
- diag_send_event_mask_update(driver->ch_cntl);
- diag_send_event_mask_update(
- driver->chqdsp_cntl);
- diag_send_event_mask_update(
- driver->ch_wcnss_cntl);
- }
- return 0;
- }
-#endif
- }
/* Check for registered clients and forward packet to apropriate proc */
cmd_code = (int)(*(char *)buf);
temp++;
@@ -791,9 +614,70 @@
}
}
}
+ /* 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 */
+ if (!(driver->ch) && chk_apps_only()) {
+ /* 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 */
+ if (!(driver->ch) && chk_apps_only()) {
+ /* 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_apps_only()) {
+ /* 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 & get event mask request */
- if (!(driver->ch) && chk_apps_only() && *buf == 0x81) {
+ else if (!(driver->ch) && chk_apps_only() && *buf == 0x81) {
driver->apps_rsp_buf[0] = 0x81;
driver->apps_rsp_buf[1] = 0x0;
*(uint16_t *)(driver->apps_rsp_buf + 2) = 0x0;
@@ -1406,25 +1290,6 @@
{
diag_debug_buf_idx = 0;
driver->read_len_legacy = 0;
-
- if (driver->event_mask == NULL) {
- driver->event_mask = kzalloc(sizeof(
- struct diag_ctrl_event_mask), GFP_KERNEL);
- if (driver->event_mask == NULL)
- goto err;
- }
- if (driver->msg_mask == NULL) {
- driver->msg_mask = kzalloc(sizeof(
- struct diag_ctrl_msg_mask), GFP_KERNEL);
- if (driver->msg_mask == NULL)
- goto err;
- }
- if (driver->log_mask == NULL) {
- driver->log_mask = kzalloc(sizeof(
- struct diag_ctrl_log_mask), GFP_KERNEL);
- if (driver->log_mask == NULL)
- goto err;
- }
if (driver->buf_in_1 == NULL) {
driver->buf_in_1 = kzalloc(IN_BUF_SIZE, GFP_KERNEL);
if (driver->buf_in_1 == NULL)
@@ -1554,9 +1419,6 @@
return;
err:
pr_err("diag: Could not initialize diag buffers");
- kfree(driver->event_mask);
- kfree(driver->log_mask);
- kfree(driver->msg_mask);
kfree(driver->buf_in_1);
kfree(driver->buf_in_2);
kfree(driver->buf_in_qdsp_1);
@@ -1599,9 +1461,6 @@
#endif
platform_driver_unregister(&msm_smd_ch1_driver);
platform_driver_unregister(&diag_smd_lite_driver);
- kfree(driver->event_mask);
- kfree(driver->log_mask);
- kfree(driver->msg_mask);
kfree(driver->buf_in_1);
kfree(driver->buf_in_2);
kfree(driver->buf_in_qdsp_1);