diag: Read the HSIC data in a work queue

Add a new work queue to process the HSIC diag
data instead of processing it in the interrupt
context.

Change-Id: I8c546cd608c662d1c3133194f70af4953d734b08
Signed-off-by: Gustavo Solaira <gustavos@codeaurora.org>

2 files changed, 98 insertions, 6 deletions

diff --git a/drivers/char/diag/diagfwd_hsic.c b/drivers/char/diag/diagfwd_hsic.c
index 5fed1f88382d..1f161777aca5 100644
--- a/drivers/char/diag/diagfwd_hsic.c
+++ b/drivers/char/diag/diagfwd_hsic.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012-2014, 2016 The Linux Foundation. All rights reserved.
+/* Copyright (c) 2012-2014,2016,2018 The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -50,10 +50,68 @@ struct diag_hsic_info diag_hsic[NUM_HSIC_DEV] = {
 	}
 };
 
+static int hsic_buf_tbl_push(struct diag_hsic_info *ch, void *buf, int len)
+{
+	unsigned long flags;
+	struct diag_hsic_buf_tbl_t *item;
+
+	if (!ch || !buf || len < 0)
+		return -EINVAL;
+
+	item = kzalloc(sizeof(struct diag_hsic_buf_tbl_t), GFP_ATOMIC);
+	if (!item)
+		return -ENOMEM;
+	kmemleak_not_leak(item);
+
+	spin_lock_irqsave(&ch->lock, flags);
+	item->buf = buf;
+	item->len = len;
+	list_add_tail(&item->link, &ch->buf_tbl);
+	spin_unlock_irqrestore(&ch->lock, flags);
+
+	return 0;
+}
+
+static struct diag_hsic_buf_tbl_t *hsic_buf_tbl_pop(struct diag_hsic_info *ch)
+{
+	unsigned long flags;
+	struct diag_hsic_buf_tbl_t *item = NULL;
+
+	if (!ch || list_empty(&ch->buf_tbl))
+		return NULL;
+
+	spin_lock_irqsave(&ch->lock, flags);
+	item = list_first_entry(&ch->buf_tbl, struct diag_hsic_buf_tbl_t, link);
+	list_del(&item->link);
+	spin_unlock_irqrestore(&ch->lock, flags);
+
+	return item;
+}
+
+static void hsic_buf_tbl_clear(struct diag_hsic_info *ch)
+{
+	unsigned long flags;
+	struct list_head *start, *temp;
+	struct diag_hsic_buf_tbl_t *item = NULL;
+
+	if (!ch)
+		return;
+
+	/* At this point, the channel should already by closed */
+	spin_lock_irqsave(&ch->lock, flags);
+	list_for_each_safe(start, temp, &ch->buf_tbl) {
+		item = list_entry(start, struct diag_hsic_buf_tbl_t,
+				  link);
+		list_del(&item->link);
+		kfree(item);
+
+	}
+	spin_unlock_irqrestore(&ch->lock, flags);
+}
+
 static void diag_hsic_read_complete(void *ctxt, char *buf, int len,
 				    int actual_size)
 {
-	int err = 0;
 	int index = (int)(uintptr_t)ctxt;
 	struct diag_hsic_info *ch = NULL;
 
@@ -71,9 +129,8 @@ static void diag_hsic_read_complete(void *ctxt, char *buf, int len,
 	 */
 	if (!ch->opened || actual_size <= 0)
 		goto fail;
-	err = diag_remote_dev_read_done(ch->dev_id, buf, actual_size);
-	if (err)
-		goto fail;
+	hsic_buf_tbl_push(ch, buf, actual_size);
+	queue_work(ch->hsic_wq, &ch->read_complete_work);
 	return;
 
 fail:
@@ -185,6 +242,7 @@ static int hsic_open(int id)
 	diagmem_init(driver, ch->mempool);
 	/* Notify the bridge that the channel is open */
 	diag_remote_dev_open(ch->dev_id);
+	INIT_LIST_HEAD(&ch->buf_tbl);
 	queue_work(ch->hsic_wq, &(ch->read_work));
 	return 0;
 }
@@ -222,6 +280,7 @@ static int hsic_close(int id)
 	diag_bridge_close(ch->id);
 	diagmem_exit(driver, ch->mempool);
 	diag_remote_dev_close(ch->dev_id);
+	hsic_buf_tbl_clear(ch);
 	return 0;
 }
 
@@ -263,6 +322,27 @@ static void hsic_read_work_fn(struct work_struct *work)
 		queue_work(ch->hsic_wq, &ch->read_work);
 }
 
+static void hsic_read_complete_work_fn(struct work_struct *work)
+{
+	struct diag_hsic_info *ch = container_of(work, struct diag_hsic_info,
+						 read_complete_work);
+	struct diag_hsic_buf_tbl_t *item;
+
+	item = hsic_buf_tbl_pop(ch);
+	if (item) {
+		if (diag_remote_dev_read_done(ch->dev_id, item->buf, item->len))
+			goto fail;
+	}
+
+	kfree(item);
+	return;
+
+fail:
+	diagmem_free(driver, item->buf, ch->mempool);
+	queue_work(ch->hsic_wq, &ch->read_work);
+	kfree(item);
+}
+
 static int diag_hsic_probe(struct platform_device *pdev)
 {
 	unsigned long flags;
@@ -407,6 +487,8 @@ int diag_hsic_init()
 		ch = &diag_hsic[i];
 		spin_lock_init(&ch->lock);
 		INIT_WORK(&(ch->read_work), hsic_read_work_fn);
+		INIT_WORK(&(ch->read_complete_work),
+			  hsic_read_complete_work_fn);
 		INIT_WORK(&(ch->open_work), hsic_open_work_fn);
 		INIT_WORK(&(ch->close_work), hsic_close_work_fn);
 		strlcpy(wq_name, "DIAG_HSIC_", DIAG_HSIC_STRING_SZ);
diff --git a/drivers/char/diag/diagfwd_hsic.h b/drivers/char/diag/diagfwd_hsic.h
index c4d87a223105..618bc4e1939d 100644
--- a/drivers/char/diag/diagfwd_hsic.h
+++ b/drivers/char/diag/diagfwd_hsic.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2012-2014, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2012-2014, 2018 The Linux Foundation. All rights reserved.
  *
  * This program is free software; you can redistribute it and/or modify
  * it under the terms of the GNU General Public License version 2 and
@@ -12,6 +12,8 @@
 
 #ifndef DIAGFWD_HSIC_H
 #define DIAGFWD_HSIC_H
+
+#include <linux/list.h>
 #ifdef CONFIG_DIAG_OVER_USB
 #include <linux/usb/usbdiag.h>
 #endif
@@ -23,6 +25,12 @@
 
 #define DIAG_HSIC_NAME_SZ	24
 
+struct diag_hsic_buf_tbl_t {
+	struct list_head link;
+	unsigned char *buf;
+	int len;
+};
+
 struct diag_hsic_info {
 	int id;
 	int dev_id;
@@ -32,10 +40,12 @@ struct diag_hsic_info {
 	uint8_t suspended;
 	char name[DIAG_HSIC_NAME_SZ];
 	struct work_struct read_work;
+	struct work_struct read_complete_work;
 	struct work_struct open_work;
 	struct work_struct close_work;
 	struct workqueue_struct *hsic_wq;
 	spinlock_t lock;
+	struct list_head buf_tbl;
 };
 
 extern struct diag_hsic_info diag_hsic[NUM_HSIC_DEV];