diff --git a/drivers/char/diag/diagfwd_glink.c b/drivers/char/diag/diagfwd_glink.c
index 03d496c2dd91..42182e3a939d 100644
--- a/drivers/char/diag/diagfwd_glink.c
+++ b/drivers/char/diag/diagfwd_glink.c
@@ -361,13 +361,41 @@ static void diag_glink_read_work_fn(struct work_struct *work)
 
 	diagfwd_channel_read(glink_info->fwd_ctxt);
 }
+struct diag_glink_read_work {
+	struct diag_glink_info *glink_info;
+	const void *ptr_read_done;
+	const void *ptr_rx_done;
+	size_t ptr_read_size;
+	struct work_struct work;
+};
+
+static void diag_glink_notify_rx_work_fn(struct work_struct *work)
+{
+	struct diag_glink_read_work *read_work = container_of(work,
+			struct diag_glink_read_work, work);
+	struct diag_glink_info *glink_info = read_work->glink_info;
+
+	if (!glink_info || !glink_info->hdl)
+		return;
+
+	diagfwd_channel_read_done(glink_info->fwd_ctxt,
+			(unsigned char *)(read_work->ptr_read_done),
+			read_work->ptr_read_size);
+
+	glink_rx_done(glink_info->hdl, read_work->ptr_rx_done, false);
+
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Rx done for packet %pK of len: %d periph: %d ch: %d\n",
+		read_work->ptr_rx_done, (int)read_work->ptr_read_size,
+		glink_info->peripheral, glink_info->type);
+}
 
 static void diag_glink_notify_rx(void *hdl, const void *priv,
 				const void *pkt_priv, const void *ptr,
 				size_t size)
 {
 	struct diag_glink_info *glink_info = (struct diag_glink_info *)priv;
-	int err = 0;
+	struct diag_glink_read_work *read_work;
 
 	if (!glink_info || !glink_info->hdl || !ptr || !pkt_priv || !hdl)
 		return;
@@ -379,12 +407,24 @@ static void diag_glink_notify_rx(void *hdl, const void *priv,
 		"diag: received a packet %pK of len:%d from periph:%d ch:%d\n",
 		ptr, (int)size, glink_info->peripheral, glink_info->type);
 
+	read_work = kmalloc(sizeof(*read_work), GFP_ATOMIC);
+	if (!read_work) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: Could not allocate read_work\n");
+		return;
+	}
+
 	memcpy((void *)pkt_priv, ptr, size);
-	err = diagfwd_channel_read_done(glink_info->fwd_ctxt,
-					(unsigned char *)pkt_priv, size);
-	glink_rx_done(glink_info->hdl, ptr, false);
+
+	read_work->glink_info = glink_info;
+	read_work->ptr_read_done = pkt_priv;
+	read_work->ptr_rx_done = ptr;
+	read_work->ptr_read_size = size;
+	INIT_WORK(&read_work->work, diag_glink_notify_rx_work_fn);
+	queue_work(glink_info->wq, &read_work->work);
+
 	DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
-		"diag: Rx done for packet %pK of len:%d periph:%d ch:%d\n",
+		"diag: Rx queued for packet %pK of len: %d periph: %d ch: %d\n",
 		ptr, (int)size, glink_info->peripheral, glink_info->type);
 }
 
@@ -473,6 +513,8 @@ static void diag_glink_connect_work_fn(struct work_struct *work)
 	atomic_set(&glink_info->opened, 1);
 	diagfwd_channel_open(glink_info->fwd_ctxt);
 	diagfwd_late_open(glink_info->fwd_ctxt);
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "glink channel open: p: %d t: %d\n",
+			glink_info->peripheral, glink_info->type);
 }
 
 static void diag_glink_remote_disconnect_work_fn(struct work_struct *work)
@@ -494,9 +536,9 @@ static void diag_glink_late_init_work_fn(struct work_struct *work)
 							late_init_work);
 	if (!glink_info || !glink_info->hdl)
 		return;
-	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "p: %d t: %d\n",
-			glink_info->peripheral, glink_info->type);
 	diagfwd_channel_open(glink_info->fwd_ctxt);
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "glink late init p: %d t: %d\n",
+			glink_info->peripheral, glink_info->type);
 }
 
 static void diag_glink_transport_notify_state(void *handle, const void *priv,
diff --git a/drivers/char/diag/diagfwd_peripheral.c b/drivers/char/diag/diagfwd_peripheral.c
index 9ac1ad62ffe0..6860de0d2288 100644
--- a/drivers/char/diag/diagfwd_peripheral.c
+++ b/drivers/char/diag/diagfwd_peripheral.c
@@ -1049,7 +1049,16 @@ void diagfwd_close_transport(uint8_t transport, uint8_t peripheral)
 		dest_info->buf_ptr[i] = fwd_info->buf_ptr[i];
 	if (!check_channel_state(dest_info->ctxt))
 		diagfwd_late_open(dest_info);
-	diagfwd_cntl_open(dest_info);
+
+	/*
+	 *	Open control channel to update masks after buffers are
+	 *	initialized for peripherals that have transport other than
+	 *	GLINK. GLINK supported peripheral mask update will
+	 *	happen after glink buffers are initialized.
+	 */
+
+	if (dest_info->transport != TRANSPORT_GLINK)
+		diagfwd_cntl_open(dest_info);
 	init_fn(peripheral);
 	mutex_unlock(&driver->diagfwd_channel_mutex[peripheral]);
 	diagfwd_queue_read(&peripheral_info[TYPE_DATA][peripheral]);
@@ -1240,7 +1249,18 @@ int diagfwd_channel_open(struct diagfwd_info *fwd_info)
 	mutex_lock(&driver->diagfwd_channel_mutex[fwd_info->peripheral]);
 	fwd_info->ch_open = 1;
 	diagfwd_buffers_init(fwd_info);
-	diagfwd_write_buffers_init(fwd_info);
+
+	/*
+	 *	Initialize buffers for glink supported
+	 *	peripherals only. Open control channel to update
+	 *	masks after buffers are initialized.
+	 */
+	if (fwd_info->transport == TRANSPORT_GLINK) {
+		diagfwd_write_buffers_init(fwd_info);
+		if (fwd_info->type == TYPE_CNTL)
+			diagfwd_cntl_open(fwd_info);
+	}
+
 	if (fwd_info && fwd_info->c_ops && fwd_info->c_ops->open)
 		fwd_info->c_ops->open(fwd_info);
 	for (i = 0; i < NUM_WRITE_BUFFERS; i++) {