diag: Process glink buffers in workqueue context

The patch initializes the glink buffers only to peripherals
supporting it and moves the handling of processing the data
received on glink buffers to process context.

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

2 files changed, 71 insertions, 9 deletions

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++) {