diag: Proper Update of the PD packet buffer status and length

The patch updates the PD packet length prior to
being processed, adds a check for core PD buffer busy
status before being processed and marks the core PD buffers free
only when there is no data to be processed in user PD.

CRs-Fixed: 2068243
Change-Id: Ida215b4df75ddc96ebd5d436850bddf56f3a4fce
Signed-off-by: Manoj Prabhu B <bmanoj@codeaurora.org>

1 files changed, 44 insertions, 18 deletions

diff --git a/drivers/char/diag/diagfwd_peripheral.c b/drivers/char/diag/diagfwd_peripheral.c
index 78b8452b19b3..67d5e28c82c0 100644
--- a/drivers/char/diag/diagfwd_peripheral.c
+++ b/drivers/char/diag/diagfwd_peripheral.c
@@ -500,15 +500,29 @@ static void diagfwd_data_read_untag_done(struct diagfwd_info *fwd_info,
 			temp_buf_main += (buf_len + 4);
 			processed += buf_len;
 		}
+
+		if (flag_buf_1) {
+			fwd_info->cpd_len_1 = len_cpd;
+			if (fwd_info->type == TYPE_DATA)
+				fwd_info->upd_len_1_a = len_upd_1;
+			if (peripheral == PERIPHERAL_LPASS &&
+				fwd_info->type == TYPE_DATA)
+				fwd_info->upd_len_2_a = len_upd_2;
+		} else if (flag_buf_2) {
+			fwd_info->cpd_len_2 = len_cpd;
+			if (fwd_info->type == TYPE_DATA)
+				fwd_info->upd_len_1_b = len_upd_1;
+			if (peripheral == PERIPHERAL_LPASS &&
+				fwd_info->type == TYPE_DATA)
+				fwd_info->upd_len_2_b = len_upd_2;
+		}
+
 		if (peripheral == PERIPHERAL_LPASS &&
 			fwd_info->type == TYPE_DATA && len_upd_2) {
-			if (flag_buf_1) {
-				fwd_info->upd_len_2_a = len_upd_2;
+			if (flag_buf_1)
 				temp_ptr_upd = fwd_info->buf_upd_2_a;
-			} else {
-				fwd_info->upd_len_2_b = len_upd_2;
+			else
 				temp_ptr_upd = fwd_info->buf_upd_2_b;
-			}
 			temp_ptr_upd->ctxt &= 0x00FFFFFF;
 			temp_ptr_upd->ctxt |=
 				(SET_PD_CTXT(ctxt_upd_2));
@@ -522,15 +536,10 @@ static void diagfwd_data_read_untag_done(struct diagfwd_info *fwd_info,
 				fwd_info->upd_len_2_b = 0;
 		}
 		if (fwd_info->type == TYPE_DATA && len_upd_1) {
-			if (flag_buf_1) {
-				fwd_info->upd_len_1_a =
-					len_upd_1;
+			if (flag_buf_1)
 				temp_ptr_upd = fwd_info->buf_upd_1_a;
-			} else {
-				fwd_info->upd_len_1_b =
-					len_upd_1;
+			else
 				temp_ptr_upd = fwd_info->buf_upd_1_b;
-			}
 			temp_ptr_upd->ctxt &= 0x00FFFFFF;
 			temp_ptr_upd->ctxt |=
 				(SET_PD_CTXT(ctxt_upd_1));
@@ -544,10 +553,6 @@ static void diagfwd_data_read_untag_done(struct diagfwd_info *fwd_info,
 				fwd_info->upd_len_1_b = 0;
 		}
 		if (len_cpd) {
-			if (flag_buf_1)
-				fwd_info->cpd_len_1 = len_cpd;
-			else
-				fwd_info->cpd_len_2 = len_cpd;
 			temp_ptr_cpd->ctxt &= 0x00FFFFFF;
 			temp_ptr_cpd->ctxt |=
 				(SET_PD_CTXT(ctxt_cpd));
@@ -1345,12 +1350,33 @@ void diagfwd_write_done(uint8_t peripheral, uint8_t type, int ctxt)
 
 	if (ctxt == 1 && fwd_info->buf_1) {
 		/* Buffer 1 for core PD is freed */
-		atomic_set(&fwd_info->buf_1->in_busy, 0);
 		fwd_info->cpd_len_1 = 0;
+
+		if (peripheral == PERIPHERAL_LPASS) {
+			if (!fwd_info->upd_len_1_a &&
+				!fwd_info->upd_len_2_a)
+				atomic_set(&fwd_info->buf_1->in_busy, 0);
+		} else if (peripheral == PERIPHERAL_MODEM) {
+			if (!fwd_info->upd_len_1_a)
+				atomic_set(&fwd_info->buf_1->in_busy, 0);
+		} else {
+			atomic_set(&fwd_info->buf_1->in_busy, 0);
+		}
 	} else if (ctxt == 2 && fwd_info->buf_2) {
 		/* Buffer 2 for core PD is freed */
-		atomic_set(&fwd_info->buf_2->in_busy, 0);
 		fwd_info->cpd_len_2 = 0;
+
+		if (peripheral == PERIPHERAL_LPASS) {
+			if (!fwd_info->upd_len_1_b &&
+				!fwd_info->upd_len_2_b)
+				atomic_set(&fwd_info->buf_2->in_busy, 0);
+		} else if (peripheral == PERIPHERAL_MODEM) {
+			if (!fwd_info->upd_len_1_b)
+				atomic_set(&fwd_info->buf_2->in_busy, 0);
+		} else {
+			atomic_set(&fwd_info->buf_2->in_busy, 0);
+		}
+
 	} else if (ctxt == 3 && fwd_info->buf_upd_1_a) {
 		/* Buffer 1 for user pd 1  is freed */
 		atomic_set(&fwd_info->buf_upd_1_a->in_busy, 0);