diag: Avoid partial packet reads
DIAG driver talks to peripheral processors over SMD channels.
Sometimes, SMD channel can have a partial packet and if diag
reads it, it can mistake them as garbled packets. With this
change, if the packet is incomplete, diag will not read it
and wait until the packet is complete.
This approach will work, as long as, peripheral is sending packets
less than 1000 bytes. Currently even the peripheral is limited to
sending packets upto 1K bytes, so this limitation is fine.
Also fix potential memory allocation failure and unnecessary
reading from SMD channels.
Change-Id: I86e6aa202d1e1159e7b3f099cb1228530489a370
Signed-off-by: Shalabh Jain <shalabhj@codeaurora.org>
diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c
index 5838bba..079e04b 100644
--- a/drivers/char/diag/diagfwd.c
+++ b/drivers/char/diag/diagfwd.c
@@ -1038,9 +1038,9 @@
queue_work(driver->diag_wq, &(driver->diag_read_smd_qdsp_work));
queue_work(driver->diag_wq, &(driver->diag_read_smd_wcnss_work));
/* Poll SMD CNTL channels to check for data */
- queue_work(driver->diag_wq, &(driver->diag_read_smd_cntl_work));
- queue_work(driver->diag_wq, &(driver->diag_read_smd_qdsp_cntl_work));
- queue_work(driver->diag_wq, &(driver->diag_read_smd_wcnss_cntl_work));
+ diag_smd_cntl_notify(NULL, SMD_EVENT_DATA);
+ diag_smd_qdsp_cntl_notify(NULL, SMD_EVENT_DATA);
+ diag_smd_wcnss_cntl_notify(NULL, SMD_EVENT_DATA);
/* Poll USB channel to check for data*/
queue_work(driver->diag_wq, &(driver->diag_read_work));
#ifdef CONFIG_DIAG_SDIO_PIPE
diff --git a/drivers/char/diag/diagfwd_cntl.c b/drivers/char/diag/diagfwd_cntl.c
index f4db8e4..2c3dc54 100644
--- a/drivers/char/diag/diagfwd_cntl.c
+++ b/drivers/char/diag/diagfwd_cntl.c
@@ -19,6 +19,66 @@
#define HDR_SIZ 8
+void diag_smd_cntl_notify(void *ctxt, unsigned event)
+{
+ int r1, r2;
+
+ if (!(driver->ch_cntl))
+ return;
+
+ switch (event) {
+ case SMD_EVENT_DATA:
+ r1 = smd_read_avail(driver->ch_cntl);
+ r2 = smd_cur_packet_size(driver->ch_cntl);
+ if (r1 > 0 && r1 == r2)
+ queue_work(driver->diag_wq,
+ &(driver->diag_read_smd_cntl_work));
+ else
+ pr_debug("diag: incomplete pkt on Modem CNTL ch\n");
+ break;
+ }
+}
+
+void diag_smd_qdsp_cntl_notify(void *ctxt, unsigned event)
+{
+ int r1, r2;
+
+ if (!(driver->chqdsp_cntl))
+ return;
+
+ switch (event) {
+ case SMD_EVENT_DATA:
+ r1 = smd_read_avail(driver->chqdsp_cntl);
+ r2 = smd_cur_packet_size(driver->chqdsp_cntl);
+ if (r1 > 0 && r1 == r2)
+ queue_work(driver->diag_wq,
+ &(driver->diag_read_smd_qdsp_cntl_work));
+ else
+ pr_debug("diag: incomplete pkt on LPASS CNTL ch\n");
+ break;
+ }
+}
+
+void diag_smd_wcnss_cntl_notify(void *ctxt, unsigned event)
+{
+ int r1, r2;
+
+ if (!(driver->ch_wcnss_cntl))
+ return;
+
+ switch (event) {
+ case SMD_EVENT_DATA:
+ r1 = smd_read_avail(driver->ch_wcnss_cntl);
+ r2 = smd_cur_packet_size(driver->ch_wcnss_cntl);
+ if (r1 > 0 && r1 == r2)
+ queue_work(driver->diag_wq,
+ &(driver->diag_read_smd_wcnss_cntl_work));
+ else
+ pr_debug("diag: incomplete pkt on WCNSS CNTL ch\n");
+ break;
+ }
+}
+
static void diag_smd_cntl_send_req(int proc_num)
{
int data_len = 0, type = -1, count_bytes = 0, j, r, flag = 0;
@@ -30,6 +90,11 @@
void *buf = NULL;
smd_channel_t *smd_ch = NULL;
+ if (pkt_params == NULL) {
+ pr_alert("diag: Memory allocation failure\n");
+ return;
+ }
+
if (proc_num == MODEM_PROC) {
buf = driver->buf_in_cntl;
smd_ch = driver->ch_cntl;
@@ -64,11 +129,13 @@
data_len = *(uint32_t *)(buf + 4);
if (type < DIAG_CTRL_MSG_REG ||
type > DIAG_CTRL_MSG_F3_MASK_V2) {
- pr_alert("diag: Invalid Msg type %d", type);
+ pr_alert("diag: Invalid Msg type %d proc %d",
+ type, proc_num);
break;
}
if (data_len < 0 || data_len > r) {
- pr_alert("diag: Invalid data len %d", data_len);
+ pr_alert("diag: Invalid data len %d proc %d",
+ data_len, proc_num);
break;
}
count_bytes = count_bytes+HDR_SIZ+data_len;
@@ -79,6 +146,10 @@
pkt_params->count = msg->count_entries;
temp = kzalloc(pkt_params->count * sizeof(struct
bindpkt_params), GFP_KERNEL);
+ if (temp == NULL) {
+ pr_alert("diag: Memory alloc fail\n");
+ return;
+ }
for (j = 0; j < pkt_params->count; j++) {
temp->cmd_code = msg->cmd_code;
temp->subsys_id = msg->subsysid;
@@ -102,11 +173,12 @@
kfree(pkt_params);
if (flag) {
/* Poll SMD CNTL channels to check for data */
- queue_work(driver->diag_wq, &(driver->diag_read_smd_cntl_work));
- queue_work(driver->diag_wq,
- &(driver->diag_read_smd_qdsp_cntl_work));
- queue_work(driver->diag_wq,
- &(driver->diag_read_smd_wcnss_cntl_work));
+ if (proc_num == MODEM_PROC)
+ diag_smd_cntl_notify(NULL, SMD_EVENT_DATA);
+ else if (proc_num == QDSP_PROC)
+ diag_smd_qdsp_cntl_notify(NULL, SMD_EVENT_DATA);
+ else if (proc_num == WCNSS_PROC)
+ diag_smd_wcnss_cntl_notify(NULL, SMD_EVENT_DATA);
}
}
@@ -125,21 +197,6 @@
diag_smd_cntl_send_req(WCNSS_PROC);
}
-static void diag_smd_cntl_notify(void *ctxt, unsigned event)
-{
- queue_work(driver->diag_wq, &(driver->diag_read_smd_cntl_work));
-}
-
-static void diag_smd_qdsp_cntl_notify(void *ctxt, unsigned event)
-{
- queue_work(driver->diag_wq, &(driver->diag_read_smd_qdsp_cntl_work));
-}
-
-static void diag_smd_wcnss_cntl_notify(void *ctxt, unsigned event)
-{
- queue_work(driver->diag_wq, &(driver->diag_read_smd_wcnss_cntl_work));
-}
-
static int diag_smd_cntl_probe(struct platform_device *pdev)
{
int r = 0;
diff --git a/drivers/char/diag/diagfwd_cntl.h b/drivers/char/diag/diagfwd_cntl.h
index e486d25..a76d36d 100644
--- a/drivers/char/diag/diagfwd_cntl.h
+++ b/drivers/char/diag/diagfwd_cntl.h
@@ -53,5 +53,8 @@
void diag_read_smd_cntl_work_fn(struct work_struct *);
void diag_read_smd_qdsp_cntl_work_fn(struct work_struct *);
void diag_read_smd_wcnss_cntl_work_fn(struct work_struct *);
+void diag_smd_cntl_notify(void *ctxt, unsigned event);
+void diag_smd_qdsp_cntl_notify(void *ctxt, unsigned event);
+void diag_smd_wcnss_cntl_notify(void *ctxt, unsigned event);
#endif