diff --git a/drivers/char/diag/diag_memorydevice.c b/drivers/char/diag/diag_memorydevice.c
index a27f12883c8d..986aeed169f5 100644
--- a/drivers/char/diag/diag_memorydevice.c
+++ b/drivers/char/diag/diag_memorydevice.c
@@ -155,15 +155,20 @@ int diag_md_write(int id, unsigned char *buf, int len, int ctx)
 		return -EIO;
 
 	ch = &diag_md[id];
+	if (!ch)
+		return -EINVAL;
 
 	spin_lock_irqsave(&ch->lock, flags);
 	for (i = 0; i < ch->num_tbl_entries && !found; i++) {
 		if (ch->tbl[i].buf != buf)
 			continue;
 		found = 1;
-		pr_err_ratelimited("diag: trying to write the same buffer buf: %pK, ctxt: %d len: %d at i: %d back to the table, proc: %d, mode: %d\n",
-				   buf, ctx, ch->tbl[i].len,
-				   i, id, driver->logging_mode);
+		pr_err_ratelimited("diag: trying to write the same buffer buf: %pK, len: %d, back to the table for p: %d, t: %d, buf_num: %d, proc: %d, i: %d\n",
+				   buf, ch->tbl[i].len, GET_BUF_PERIPHERAL(ctx),
+				   GET_BUF_TYPE(ctx), GET_BUF_NUM(ctx), id, i);
+		ch->tbl[i].buf = NULL;
+		ch->tbl[i].len = 0;
+		ch->tbl[i].ctx = 0;
 	}
 	spin_unlock_irqrestore(&ch->lock, flags);
 
@@ -227,7 +232,7 @@ int diag_md_copy_to_user(char __user *buf, int *pret, size_t buf_size,
 		ch = &diag_md[i];
 		for (j = 0; j < ch->num_tbl_entries && !err; j++) {
 			entry = &ch->tbl[j];
-			if (entry->len <= 0)
+			if (entry->len <= 0 || entry->buf == NULL)
 				continue;
 
 			peripheral = diag_md_get_peripheral(entry->ctx);
diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c
index 7dc2eabf1bb9..ef08f939c36e 100644
--- a/drivers/char/diag/diagfwd.c
+++ b/drivers/char/diag/diagfwd.c
@@ -1572,6 +1572,9 @@ static int diagfwd_mux_write_done(unsigned char *buf, int len, int buf_ctxt,
 	switch (type) {
 	case TYPE_DATA:
 		if (peripheral >= 0 && peripheral < NUM_PERIPHERALS) {
+			DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"Marking buffer as free after write done p: %d, t: %d, buf_num: %d\n",
+				peripheral, type, num);
 			diagfwd_write_done(peripheral, type, num);
 			diag_ws_on_copy(DIAG_WS_MUX);
 		} else if (peripheral == APPS_DATA) {
diff --git a/drivers/char/diag/diagfwd_peripheral.c b/drivers/char/diag/diagfwd_peripheral.c
index 0f94bab3bf84..b7dff47623de 100644
--- a/drivers/char/diag/diagfwd_peripheral.c
+++ b/drivers/char/diag/diagfwd_peripheral.c
@@ -31,6 +31,7 @@
 #include "diag_mux.h"
 #include "diag_ipc_logging.h"
 #include "diagfwd_glink.h"
+#include "diag_memorydevice.h"
 
 struct data_header {
 	uint8_t control_char;
@@ -188,8 +189,10 @@ static int diag_add_hdlc_encoding(unsigned char *dest_buf, int *dest_len,
 
 static int check_bufsize_for_encoding(struct diagfwd_buf_t *buf, uint32_t len)
 {
+	int i, ctx = 0;
 	uint32_t max_size = 0;
 	unsigned char *temp_buf = NULL;
+	struct diag_md_info *ch = NULL;
 
 	if (!buf || len == 0)
 		return -EINVAL;
@@ -203,11 +206,31 @@ static int check_bufsize_for_encoding(struct diagfwd_buf_t *buf, uint32_t len)
 		}
 
 		if (buf->len < max_size) {
+			if (driver->logging_mode == DIAG_MEMORY_DEVICE_MODE) {
+				ch = &diag_md[DIAG_LOCAL_PROC];
+				for (i = 0; ch != NULL &&
+						i < ch->num_tbl_entries; i++) {
+					if (ch->tbl[i].buf == buf->data) {
+						ctx = ch->tbl[i].ctx;
+						ch->tbl[i].buf = NULL;
+						ch->tbl[i].len = 0;
+						ch->tbl[i].ctx = 0;
+						DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+						"Flushed mdlog table entries before reallocating data buffer, p:%d, t:%d\n",
+						GET_BUF_PERIPHERAL(ctx),
+						GET_BUF_TYPE(ctx));
+						break;
+					}
+				}
+			}
 			temp_buf = krealloc(buf->data, max_size +
 						APF_DIAG_PADDING,
 					    GFP_KERNEL);
 			if (!temp_buf)
 				return -ENOMEM;
+			DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"Reallocated data buffer: %pK with size: %d\n",
+			temp_buf, max_size);
 			buf->data = temp_buf;
 			buf->len = max_size;
 		}
@@ -360,6 +383,10 @@ end:
 	mutex_unlock(&fwd_info->data_mutex);
 	mutex_unlock(&driver->hdlc_disable_mutex);
 	if (buf) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"Marking buffer as free p: %d, t: %d, buf_num: %d\n",
+			fwd_info->peripheral, fwd_info->type,
+			GET_BUF_NUM(buf->ctxt));
 		diagfwd_write_done(fwd_info->peripheral, fwd_info->type,
 				   GET_BUF_NUM(buf->ctxt));
 	}
@@ -572,6 +599,10 @@ static void diagfwd_data_read_untag_done(struct diagfwd_info *fwd_info,
 end:
 	diag_ws_release();
 	if (temp_ptr_cpd) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"Marking buffer as free p: %d, t: %d, buf_num: %d\n",
+			fwd_info->peripheral, fwd_info->type,
+			GET_BUF_NUM(temp_ptr_cpd->ctxt));
 		diagfwd_write_done(fwd_info->peripheral, fwd_info->type,
 				   GET_BUF_NUM(temp_ptr_cpd->ctxt));
 	}
@@ -692,6 +723,10 @@ end:
 	mutex_unlock(&fwd_info->data_mutex);
 	mutex_unlock(&driver->hdlc_disable_mutex);
 	if (temp_buf) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"Marking buffer as free p: %d, t: %d, buf_num: %d\n",
+			fwd_info->peripheral, fwd_info->type,
+			GET_BUF_NUM(temp_buf->ctxt));
 		diagfwd_write_done(fwd_info->peripheral, fwd_info->type,
 				   GET_BUF_NUM(temp_buf->ctxt));
 	}
@@ -772,6 +807,16 @@ static void diagfwd_reset_buffers(struct diagfwd_info *fwd_info,
 		else if (fwd_info->buf_2 && fwd_info->buf_2->data_raw == buf)
 			atomic_set(&fwd_info->buf_2->in_busy, 0);
 	}
+	if (fwd_info->buf_1 && !atomic_read(&(fwd_info->buf_1->in_busy))) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"Buffer 1 for core PD is marked free, p: %d, t: %d\n",
+			fwd_info->peripheral, fwd_info->type);
+	}
+	if (fwd_info->buf_2 && !atomic_read(&(fwd_info->buf_2->in_busy))) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"Buffer 2 for core PD is marked free, p: %d, t: %d\n",
+			fwd_info->peripheral, fwd_info->type);
+	}
 }
 
 int diagfwd_peripheral_init(void)
@@ -1158,10 +1203,18 @@ static void __diag_fwd_open(struct diagfwd_info *fwd_info)
 
 	if ((driver->logging_mode != DIAG_USB_MODE) ||
 		driver->usb_connected) {
-		if (fwd_info->buf_1)
+		if (fwd_info->buf_1) {
 			atomic_set(&fwd_info->buf_1->in_busy, 0);
-		if (fwd_info->buf_2)
+			DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"Buffer 1 for core PD is marked free, p: %d, t: %d\n",
+				fwd_info->peripheral, fwd_info->type);
+		}
+		if (fwd_info->buf_2) {
 			atomic_set(&fwd_info->buf_2->in_busy, 0);
+			DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"Buffer 2 for core PD is marked free, p: %d, t: %d\n",
+				fwd_info->peripheral, fwd_info->type);
+		}
 	}
 
 	if (fwd_info->p_ops && fwd_info->p_ops->open)
@@ -1285,10 +1338,18 @@ int diagfwd_channel_close(struct diagfwd_info *fwd_info)
 	if (fwd_info && fwd_info->c_ops && fwd_info->c_ops->close)
 		fwd_info->c_ops->close(fwd_info);
 
-	if (fwd_info->buf_1 && fwd_info->buf_1->data)
+	if (fwd_info->buf_1 && fwd_info->buf_1->data) {
 		atomic_set(&fwd_info->buf_1->in_busy, 0);
-	if (fwd_info->buf_2 && fwd_info->buf_2->data)
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"Buffer 1 for core PD is marked free, p: %d, t: %d\n",
+			fwd_info->peripheral, fwd_info->type);
+	}
+	if (fwd_info->buf_2 && fwd_info->buf_2->data) {
 		atomic_set(&fwd_info->buf_2->in_busy, 0);
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"Buffer 2 for core PD is marked free, p: %d, t: %d\n",
+				fwd_info->peripheral, fwd_info->type);
+	}
 
 	for (i = 0; i < NUM_WRITE_BUFFERS; i++) {
 		if (fwd_info->buf_ptr[i])
@@ -1314,6 +1375,9 @@ int diagfwd_channel_read_done(struct diagfwd_info *fwd_info,
 	 * in_busy flags. No need to queue read in this case.
 	 */
 	if (len == 0) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"Read Length is 0, resetting the diag buffers p: %d, t: %d\n",
+			fwd_info->peripheral, fwd_info->type);
 		diagfwd_reset_buffers(fwd_info, buf);
 		diag_ws_release();
 		return 0;
@@ -1326,7 +1390,7 @@ int diagfwd_channel_read_done(struct diagfwd_info *fwd_info,
 	return 0;
 }
 
-void diagfwd_write_done(uint8_t peripheral, uint8_t type, int ctxt)
+void diagfwd_write_done(uint8_t peripheral, uint8_t type, int buf_num)
 {
 	struct diagfwd_info *fwd_info = NULL;
 
@@ -1334,8 +1398,10 @@ void diagfwd_write_done(uint8_t peripheral, uint8_t type, int ctxt)
 		return;
 
 	fwd_info = &peripheral_info[type][peripheral];
+	if (!fwd_info)
+		return;
 
-	if (ctxt == 1 && fwd_info->buf_1) {
+	if (buf_num == 1 && fwd_info->buf_1) {
 		/* Buffer 1 for core PD is freed */
 		fwd_info->cpd_len_1 = 0;
 
@@ -1349,7 +1415,12 @@ void diagfwd_write_done(uint8_t peripheral, uint8_t type, int ctxt)
 		} else {
 			atomic_set(&fwd_info->buf_1->in_busy, 0);
 		}
-	} else if (ctxt == 2 && fwd_info->buf_2) {
+		if (!atomic_read(&(fwd_info->buf_1->in_busy))) {
+			DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"Buffer 1 for core PD is marked free, p: %d, t: %d, buf_num: %d\n",
+				fwd_info->peripheral, fwd_info->type, buf_num);
+		}
+	} else if (buf_num == 2 && fwd_info->buf_2) {
 		/* Buffer 2 for core PD is freed */
 		fwd_info->cpd_len_2 = 0;
 
@@ -1363,8 +1434,12 @@ void diagfwd_write_done(uint8_t peripheral, uint8_t type, int ctxt)
 		} else {
 			atomic_set(&fwd_info->buf_2->in_busy, 0);
 		}
-
-	} else if (ctxt == 3 && fwd_info->buf_upd_1_a) {
+		if (!atomic_read(&(fwd_info->buf_2->in_busy))) {
+			DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"Buffer 2 for core PD is marked free, p: %d, t: %d, buf_num: %d\n",
+				fwd_info->peripheral, fwd_info->type, buf_num);
+		}
+	} else if (buf_num == 3 && fwd_info->buf_upd_1_a && fwd_info->buf_1) {
 		/* Buffer 1 for user pd 1  is freed */
 		atomic_set(&fwd_info->buf_upd_1_a->in_busy, 0);
 
@@ -1382,9 +1457,14 @@ void diagfwd_write_done(uint8_t peripheral, uint8_t type, int ctxt)
 			if (!fwd_info->cpd_len_1)
 				atomic_set(&fwd_info->buf_1->in_busy, 0);
 		}
+		if (!atomic_read(&(fwd_info->buf_1->in_busy))) {
+			DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"Buffer 1 for core PD is marked free, p: %d, t: %d, buf_num: %d\n",
+				fwd_info->peripheral, fwd_info->type, buf_num);
+		}
 		fwd_info->upd_len_1_a = 0;
 
-	} else if (ctxt == 4 && fwd_info->buf_upd_1_b) {
+	} else if (buf_num == 4 && fwd_info->buf_upd_1_b && fwd_info->buf_2) {
 		/* Buffer 2 for user pd 1  is freed */
 		atomic_set(&fwd_info->buf_upd_1_b->in_busy, 0);
 		if (peripheral == PERIPHERAL_LPASS) {
@@ -1401,34 +1481,46 @@ void diagfwd_write_done(uint8_t peripheral, uint8_t type, int ctxt)
 			if (!fwd_info->cpd_len_2)
 				atomic_set(&fwd_info->buf_2->in_busy, 0);
 		}
+		if (!atomic_read(&(fwd_info->buf_2->in_busy))) {
+			DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"Buffer 2 for core PD is marked free, p: %d, t: %d, buf_num: %d\n",
+				fwd_info->peripheral, fwd_info->type, buf_num);
+		}
 		fwd_info->upd_len_1_b = 0;
 
-	} else if (ctxt == 5 && fwd_info->buf_upd_2_a) {
+	} else if (buf_num == 5 && fwd_info->buf_upd_2_a && fwd_info->buf_1) {
 		/* Buffer 1 for user pd 2  is freed */
 		atomic_set(&fwd_info->buf_upd_2_a->in_busy, 0);
 		/* if not data in cpd and other user pd
 		 * free the core pd buffer for LPASS
 		 */
 		if (!fwd_info->cpd_len_1 &&
-			!fwd_info->upd_len_1_a)
+			!fwd_info->upd_len_1_a) {
 			atomic_set(&fwd_info->buf_1->in_busy, 0);
+			DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"Buffer 1 for core PD is marked free, p: %d, t: %d, buf_num: %d\n",
+				fwd_info->peripheral, fwd_info->type, buf_num);
+		}
 
 		fwd_info->upd_len_2_a = 0;
 
-	} else if (ctxt == 6 && fwd_info->buf_upd_2_b) {
+	} else if (buf_num == 6 && fwd_info->buf_upd_2_b && fwd_info->buf_2) {
 		/* Buffer 2 for user pd 2  is freed */
 		atomic_set(&fwd_info->buf_upd_2_b->in_busy, 0);
 		/* if not data in cpd and other user pd
 		 * free the core pd buffer for LPASS
 		 */
 		if (!fwd_info->cpd_len_2 &&
-			!fwd_info->upd_len_1_b)
+			!fwd_info->upd_len_1_b) {
 			atomic_set(&fwd_info->buf_2->in_busy, 0);
-
+			DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"Buffer 2 for core PD is marked free, p: %d, t: %d, buf_num: %d\n",
+				fwd_info->peripheral, fwd_info->type, buf_num);
+		}
 		fwd_info->upd_len_2_b = 0;
 
 	} else
-		pr_err("diag: In %s, invalid ctxt %d\n", __func__, ctxt);
+		pr_err("diag: In %s, invalid buf_num: %d\n", __func__, buf_num);
 
 	diagfwd_queue_read(fwd_info);
 }
diff --git a/drivers/char/diag/diagfwd_peripheral.h b/drivers/char/diag/diagfwd_peripheral.h
index eda70dcfdcd9..00621c178906 100644
--- a/drivers/char/diag/diagfwd_peripheral.h
+++ b/drivers/char/diag/diagfwd_peripheral.h
@@ -117,7 +117,7 @@ int diagfwd_cntl_register(uint8_t transport, uint8_t peripheral, void *ctxt,
 void diagfwd_deregister(uint8_t peripheral, uint8_t type, void *ctxt);
 
 int diagfwd_write(uint8_t peripheral, uint8_t type, void *buf, int len);
-void diagfwd_write_done(uint8_t peripheral, uint8_t type, int ctxt);
+void diagfwd_write_done(uint8_t peripheral, uint8_t type, int buf_num);
 void diagfwd_buffers_init(struct diagfwd_info *fwd_info);
 
 /*