diag: Update Slave processor with mask info
From 8960 onwards, application processor handles master diag. Other
slave processors can come up on different time. This change will
update each slave processor with latest mask info when it comes up.
Change-Id: Icc5f607043c8dc00dbfd504952beccaff5641f41
Signed-off-by: Shalabh Jain <shalabhj@codeaurora.org>
diff --git a/drivers/char/diag/diagchar.h b/drivers/char/diag/diagchar.h
index f493f79..ac8b50f 100644
--- a/drivers/char/diag/diagchar.h
+++ b/drivers/char/diag/diagchar.h
@@ -50,6 +50,9 @@
#define USER_SPACE_DATA 8000
#define PKT_SIZE 4096
#define MAX_EQUIP_ID 12
+#define DIAG_CTRL_MSG_LOG_MASK 9
+#define DIAG_CTRL_MSG_EVENT_MASK 10
+#define DIAG_CTRL_MSG_F3_MASK 11
/* Maximum number of pkt reg supported at initialization*/
extern unsigned int diag_max_registration;
@@ -147,7 +150,10 @@
int count_hdlc_pool;
int count_write_struct_pool;
int used;
-
+ /* Buffers for masks */
+ struct diag_ctrl_event_mask *event_mask;
+ struct diag_ctrl_log_mask *log_mask;
+ struct diag_ctrl_msg_mask *msg_mask;
/* State for diag forwarding */
unsigned char *buf_in_1;
unsigned char *buf_in_2;
diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c
index 6853654..8d914dd 100644
--- a/drivers/char/diag/diagfwd.c
+++ b/drivers/char/diag/diagfwd.c
@@ -43,8 +43,15 @@
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 { \
@@ -91,7 +98,7 @@
}
/*
- * This will return TRUE for targets which support apps only mode.
+ * This will return TRUE for targets which support apps only mode and hence SSR.
* This applies to 8960 and newer targets.
*/
int chk_apps_only(void)
@@ -381,7 +388,6 @@
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;
@@ -401,7 +407,6 @@
else
printk(KERN_INFO "Unable to copy"
" mask change\n");
-
found = 1;
break;
} else {
@@ -423,10 +428,9 @@
}
mutex_unlock(&driver->diagchar_mutex);
diag_print_mask_table();
-
}
-static void diag_update_event_mask(uint8_t *buf, int toggle, int num_bits)
+static void diag_update_event_mask(uint8_t *buf, int toggle)
{
uint8_t *ptr = driver->event_masks;
uint8_t *temp = buf + 2;
@@ -437,8 +441,8 @@
else
if (CHK_OVERFLOW(ptr, ptr,
ptr+EVENT_MASK_SIZE,
- num_bits/8 + 1))
- memcpy(ptr, temp , num_bits/8 + 1);
+ diag_event_num_bytes))
+ memcpy(ptr, temp , diag_event_num_bytes);
else
printk(KERN_CRIT "Not enough buffer space "
"for EVENT_MASK\n");
@@ -448,10 +452,6 @@
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,8 +465,9 @@
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);
@@ -552,6 +553,88 @@
}
}
+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;
@@ -564,6 +647,100 @@
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++;
@@ -614,70 +791,9 @@
}
}
}
- /* 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 */
- else if (!(driver->ch) && chk_apps_only() && *buf == 0x81) {
+ 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;
@@ -1290,6 +1406,25 @@
{
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)
@@ -1419,6 +1554,9 @@
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);
@@ -1461,6 +1599,9 @@
#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);
diff --git a/drivers/char/diag/diagfwd.h b/drivers/char/diag/diagfwd.h
index 6dacab7..d33343e 100644
--- a/drivers/char/diag/diagfwd.h
+++ b/drivers/char/diag/diagfwd.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2008-2011, Code Aurora Forum. All rights reserved.
+/* Copyright (c) 2008-2012, Code Aurora Forum. All rights reserved.
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 and
@@ -28,6 +28,9 @@
int mask_request_validate(unsigned char mask_buf[]);
void diag_clear_reg(int);
int chk_apps_only(void);
+void diag_send_event_mask_update(smd_channel_t *);
+void diag_send_msg_mask_update(smd_channel_t *);
+void diag_send_log_mask_update(smd_channel_t *);
/* State for diag forwarding */
#ifdef CONFIG_DIAG_OVER_USB
int diagfwd_connect(void);
@@ -35,5 +38,5 @@
#endif
extern int diag_debug_buf_idx;
extern unsigned char diag_debug_buf[1024];
-
+extern int diag_event_num_bytes;
#endif
diff --git a/drivers/char/diag/diagfwd_cntl.c b/drivers/char/diag/diagfwd_cntl.c
index 9730d4f..2c58772 100644
--- a/drivers/char/diag/diagfwd_cntl.c
+++ b/drivers/char/diag/diagfwd_cntl.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2011, Code Aurora Forum. All rights reserved.
+/* Copyright (c) 2011-2012, Code Aurora Forum. All rights reserved.
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 and
@@ -118,16 +118,31 @@
static void diag_smd_cntl_notify(void *ctxt, unsigned event)
{
+ if (event == SMD_EVENT_OPEN) {
+ diag_send_event_mask_update(driver->ch_cntl);
+ diag_send_msg_mask_update(driver->ch_cntl);
+ diag_send_log_mask_update(driver->ch_cntl);
+ }
queue_work(driver->diag_wq, &(driver->diag_read_smd_cntl_work));
}
static void diag_smd_qdsp_cntl_notify(void *ctxt, unsigned event)
{
+ if (event == SMD_EVENT_OPEN) {
+ diag_send_event_mask_update(driver->chqdsp_cntl);
+ diag_send_msg_mask_update(driver->chqdsp_cntl);
+ diag_send_log_mask_update(driver->chqdsp_cntl);
+ }
queue_work(driver->diag_wq, &(driver->diag_read_smd_qdsp_cntl_work));
}
static void diag_smd_wcnss_cntl_notify(void *ctxt, unsigned event)
{
+ if (event == SMD_EVENT_OPEN) {
+ diag_send_event_mask_update(driver->ch_wcnss_cntl);
+ diag_send_msg_mask_update(driver->ch_wcnss_cntl);
+ diag_send_log_mask_update(driver->ch_wcnss_cntl);
+ }
queue_work(driver->diag_wq, &(driver->diag_read_smd_wcnss_cntl_work));
}
diff --git a/drivers/char/diag/diagfwd_cntl.h b/drivers/char/diag/diagfwd_cntl.h
index 542138d..b567c5f 100644
--- a/drivers/char/diag/diagfwd_cntl.h
+++ b/drivers/char/diag/diagfwd_cntl.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2011, Code Aurora Forum. All rights reserved.
+/* Copyright (c) 2011-2012, Code Aurora Forum. All rights reserved.
*
* This program is free software; you can redistribute it and/or modify
* it under the terms of the GNU General Public License version 2 and
@@ -29,6 +29,39 @@
uint16_t port;
};
+struct diag_ctrl_event_mask {
+ uint32_t cmd_type;
+ uint32_t data_len;
+ uint8_t stream_id;
+ uint8_t status;
+ uint8_t event_config;
+ uint32_t event_mask_size;
+ /* Copy event mask here */
+} __packed;
+
+struct diag_ctrl_log_mask {
+ uint32_t cmd_type;
+ uint32_t data_len;
+ uint8_t stream_id;
+ uint8_t status;
+ uint8_t equip_id;
+ uint32_t num_items; /* Last log code for this equip_id */
+ uint32_t log_mask_size; /* Size of log mask stored in log_mask[] */
+ /* Copy log mask here */
+} __packed;
+
+struct diag_ctrl_msg_mask {
+ uint32_t cmd_type;
+ uint32_t data_len;
+ uint8_t stream_id;
+ uint8_t status;
+ uint8_t msg_mode;
+ uint16_t ssid_first; /* Start of range of supported SSIDs */
+ uint16_t ssid_last; /* Last SSID in range */
+ uint32_t msg_mask_size; /* ssid_last - ssid_first + 1 */
+ /* Copy msg mask here */
+} __packed;
+
void diagfwd_cntl_init(void);
void diagfwd_cntl_exit(void);
void diag_read_smd_cntl_work_fn(struct work_struct *);