Merge "diag: Add diag over glink support"

11 files changed, 1090 insertions, 16 deletions

diff --git a/drivers/char/diag/Makefile b/drivers/char/diag/Makefile
index 3de7aba12a2f..c5ec4f081c55 100644
--- a/drivers/char/diag/Makefile
+++ b/drivers/char/diag/Makefile
@@ -3,4 +3,4 @@ obj-$(CONFIG_DIAGFWD_BRIDGE_CODE) += diagfwd_bridge.o
 obj-$(CONFIG_USB_QCOM_DIAG_BRIDGE) += diagfwd_hsic.o
 obj-$(CONFIG_USB_QCOM_DIAG_BRIDGE) += diagfwd_smux.o
 obj-$(CONFIG_MSM_MHI) += diagfwd_mhi.o
-diagchar-objs := diagchar_core.o diagchar_hdlc.o diagfwd.o diagfwd_peripheral.o diagfwd_smd.o diagfwd_socket.o diag_mux.o diag_memorydevice.o diag_usb.o diagmem.o diagfwd_cntl.o diag_dci.o diag_masks.o diag_debugfs.o
+diagchar-objs := diagchar_core.o diagchar_hdlc.o diagfwd.o diagfwd_glink.o diagfwd_peripheral.o diagfwd_smd.o diagfwd_socket.o diag_mux.o diag_memorydevice.o diag_usb.o diagmem.o diagfwd_cntl.o diag_dci.o diag_masks.o diag_debugfs.o
diff --git a/drivers/char/diag/diag_debugfs.c b/drivers/char/diag/diag_debugfs.c
index f5e4eba1e96b..b861d5f32d03 100644
--- a/drivers/char/diag/diag_debugfs.c
+++ b/drivers/char/diag/diag_debugfs.c
@@ -34,6 +34,7 @@
 #include "diagfwd_peripheral.h"
 #include "diagfwd_smd.h"
 #include "diagfwd_socket.h"
+#include "diagfwd_glink.h"
 #include "diag_debugfs.h"
 #include "diag_ipc_logging.h"
 
@@ -44,6 +45,7 @@ static int diag_dbgfs_mempool_index;
 static int diag_dbgfs_usbinfo_index;
 static int diag_dbgfs_smdinfo_index;
 static int diag_dbgfs_socketinfo_index;
+static int diag_dbgfs_glinkinfo_index;
 static int diag_dbgfs_hsicinfo_index;
 static int diag_dbgfs_mhiinfo_index;
 static int diag_dbgfs_bridgeinfo_index;
@@ -684,6 +686,110 @@ static ssize_t diag_dbgfs_read_socketinfo(struct file *file, char __user *ubuf,
 	return ret;
 }
 
+static ssize_t diag_dbgfs_read_glinkinfo(struct file *file, char __user *ubuf,
+					  size_t count, loff_t *ppos)
+{
+	char *buf = NULL;
+	int ret = 0;
+	int i = 0;
+	int j = 0;
+	unsigned int buf_size;
+	unsigned int bytes_remaining = 0;
+	unsigned int bytes_written = 0;
+	unsigned int bytes_in_buffer = 0;
+	struct diag_glink_info *info = NULL;
+	struct diagfwd_info *fwd_ctxt = NULL;
+
+	if (diag_dbgfs_glinkinfo_index >= NUM_PERIPHERALS) {
+		/* Done. Reset to prepare for future requests */
+		diag_dbgfs_socketinfo_index = 0;
+		return 0;
+	}
+
+	buf = kzalloc(sizeof(char) * DEBUG_BUF_SIZE, GFP_KERNEL);
+	if (!buf)
+		return -ENOMEM;
+
+	buf_size = ksize(buf);
+	bytes_remaining = buf_size;
+	for (i = 0; i < NUM_TYPES; i++) {
+		for (j = 0; j < NUM_PERIPHERALS; j++) {
+			switch (i) {
+			case TYPE_DATA:
+				info = &glink_data[j];
+				break;
+			case TYPE_CNTL:
+				info = &glink_cntl[j];
+				break;
+			case TYPE_DCI:
+				info = &glink_dci[j];
+				break;
+			case TYPE_CMD:
+				info = &glink_cmd[j];
+				break;
+			case TYPE_DCI_CMD:
+				info = &glink_dci_cmd[j];
+				break;
+			default:
+				return -EINVAL;
+			}
+
+			fwd_ctxt = (struct diagfwd_info *)(info->fwd_ctxt);
+
+			bytes_written = scnprintf(buf+bytes_in_buffer,
+				bytes_remaining,
+				"name\t\t:\t%s\n"
+				"hdl\t\t:\t%pK\n"
+				"inited\t\t:\t%d\n"
+				"opened\t\t:\t%d\n"
+				"diag_state\t:\t%d\n"
+				"buf_1 busy\t:\t%d\n"
+				"buf_2 busy\t:\t%d\n"
+				"tx_intent_ready\t:\t%d\n"
+				"open pending\t:\t%d\n"
+				"close pending\t:\t%d\n"
+				"read pending\t:\t%d\n"
+				"bytes read\t:\t%lu\n"
+				"bytes written\t:\t%lu\n"
+				"fwd inited\t:\t%d\n"
+				"fwd opened\t:\t%d\n"
+				"fwd ch_open\t:\t%d\n\n",
+				info->name,
+				info->hdl,
+				info->inited,
+				atomic_read(&info->opened),
+				atomic_read(&info->diag_state),
+				(fwd_ctxt && fwd_ctxt->buf_1) ?
+				atomic_read(&fwd_ctxt->buf_1->in_busy) : -1,
+				(fwd_ctxt && fwd_ctxt->buf_2) ?
+				atomic_read(&fwd_ctxt->buf_2->in_busy) : -1,
+				atomic_read(&info->tx_intent_ready),
+				work_pending(&info->open_work),
+				work_pending(&info->close_work),
+				work_pending(&info->read_work),
+				(fwd_ctxt) ? fwd_ctxt->read_bytes : 0,
+				(fwd_ctxt) ? fwd_ctxt->write_bytes : 0,
+				(fwd_ctxt) ? fwd_ctxt->inited : -1,
+				(fwd_ctxt) ?
+				atomic_read(&fwd_ctxt->opened) : -1,
+				(fwd_ctxt) ? fwd_ctxt->ch_open : -1);
+			bytes_in_buffer += bytes_written;
+
+			/* Check if there is room to add another table entry */
+			bytes_remaining = buf_size - bytes_in_buffer;
+
+			if (bytes_remaining < bytes_written)
+				break;
+		}
+	}
+	diag_dbgfs_glinkinfo_index = i+1;
+	*ppos = 0;
+	ret = simple_read_from_buffer(ubuf, count, ppos, buf, bytes_in_buffer);
+
+	kfree(buf);
+	return ret;
+}
+
 static ssize_t diag_dbgfs_write_debug(struct file *fp, const char __user *buf,
 				      size_t count, loff_t *ppos)
 {
@@ -947,6 +1053,10 @@ const struct file_operations diag_dbgfs_socketinfo_ops = {
 	.read = diag_dbgfs_read_socketinfo,
 };
 
+const struct file_operations diag_dbgfs_glinkinfo_ops = {
+	.read = diag_dbgfs_read_glinkinfo,
+};
+
 const struct file_operations diag_dbgfs_table_ops = {
 	.read = diag_dbgfs_read_table,
 };
@@ -994,6 +1104,11 @@ int diag_debugfs_init(void)
 	if (!entry)
 		goto err;
 
+	entry = debugfs_create_file("glinkinfo", 0444, diag_dbgfs_dent, 0,
+				    &diag_dbgfs_glinkinfo_ops);
+	if (!entry)
+		goto err;
+
 	entry = debugfs_create_file("table", 0444, diag_dbgfs_dent, 0,
 				    &diag_dbgfs_table_ops);
 	if (!entry)
diff --git a/drivers/char/diag/diagchar.h b/drivers/char/diag/diagchar.h
index 5d7b1e7fe757..dccaa6a0d9c4 100644
--- a/drivers/char/diag/diagchar.h
+++ b/drivers/char/diag/diagchar.h
@@ -64,16 +64,19 @@
 #define DIAG_CON_LPASS		(0x0004)	/* Bit mask for LPASS */
 #define DIAG_CON_WCNSS		(0x0008)	/* Bit mask for WCNSS */
 #define DIAG_CON_SENSORS	(0x0010)	/* Bit mask for Sensors */
+#define DIAG_CON_WDSP (0x0020) /* Bit mask for WDSP */
+
 #define DIAG_CON_NONE		(0x0000)	/* Bit mask for No SS*/
 #define DIAG_CON_ALL		(DIAG_CON_APSS | DIAG_CON_MPSS \
 				| DIAG_CON_LPASS | DIAG_CON_WCNSS \
-				| DIAG_CON_SENSORS)
+				| DIAG_CON_SENSORS | DIAG_CON_WDSP)
 
 #define DIAG_STM_MODEM	0x01
 #define DIAG_STM_LPASS	0x02
 #define DIAG_STM_WCNSS	0x04
 #define DIAG_STM_APPS	0x08
 #define DIAG_STM_SENSORS 0x10
+#define DIAG_STM_WDSP 0x20
 
 #define INVALID_PID		-1
 #define DIAG_CMD_FOUND		1
@@ -198,7 +201,8 @@
 #define PERIPHERAL_LPASS	1
 #define PERIPHERAL_WCNSS	2
 #define PERIPHERAL_SENSORS	3
-#define NUM_PERIPHERALS		4
+#define PERIPHERAL_WDSP		4
+#define NUM_PERIPHERALS		5
 #define APPS_DATA		(NUM_PERIPHERALS)
 
 /* Number of sessions possible in Memory Device Mode. +1 for Apps data */
diff --git a/drivers/char/diag/diagchar_core.c b/drivers/char/diag/diagchar_core.c
index ecdbf9f9480e..a39e4929d999 100644
--- a/drivers/char/diag/diagchar_core.c
+++ b/drivers/char/diag/diagchar_core.c
@@ -383,6 +383,8 @@ static uint32_t diag_translate_kernel_to_user_mask(uint32_t peripheral_mask)
 		ret |= DIAG_CON_WCNSS;
 	if (peripheral_mask & MD_PERIPHERAL_MASK(PERIPHERAL_SENSORS))
 		ret |= DIAG_CON_SENSORS;
+	if (peripheral_mask & MD_PERIPHERAL_MASK(PERIPHERAL_WDSP))
+		ret |= DIAG_CON_WDSP;
 
 	return ret;
 }
@@ -1489,6 +1491,8 @@ static uint32_t diag_translate_mask(uint32_t peripheral_mask)
 		ret |= (1 << PERIPHERAL_WCNSS);
 	if (peripheral_mask & DIAG_CON_SENSORS)
 		ret |= (1 << PERIPHERAL_SENSORS);
+	if (peripheral_mask & DIAG_CON_WDSP)
+		ret |= (1 << PERIPHERAL_WDSP);
 
 	return ret;
 }
diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c
index aec4f965b13e..8205e5b05d85 100644
--- a/drivers/char/diag/diagfwd.c
+++ b/drivers/char/diag/diagfwd.c
@@ -561,6 +561,9 @@ int diag_process_stm_cmd(unsigned char *buf, unsigned char *dest_buf)
 		if (mask & DIAG_STM_SENSORS)
 			diag_process_stm_mask(cmd, DIAG_STM_SENSORS,
 						PERIPHERAL_SENSORS);
+		if (mask & DIAG_STM_WDSP)
+			diag_process_stm_mask(cmd, DIAG_STM_WDSP,
+						PERIPHERAL_WDSP);
 
 		if (mask & DIAG_STM_APPS)
 			diag_process_stm_mask(cmd, DIAG_STM_APPS, APPS_DATA);
@@ -582,6 +585,9 @@ int diag_process_stm_cmd(unsigned char *buf, unsigned char *dest_buf)
 	if (driver->feature[PERIPHERAL_SENSORS].stm_support)
 		rsp_supported |= DIAG_STM_SENSORS;
 
+	if (driver->feature[PERIPHERAL_WDSP].stm_support)
+		rsp_supported |= DIAG_STM_WDSP;
+
 	rsp_supported |= DIAG_STM_APPS;
 
 	/* Set mask denoting STM state/status for each peripheral/APSS */
@@ -597,6 +603,9 @@ int diag_process_stm_cmd(unsigned char *buf, unsigned char *dest_buf)
 	if (driver->stm_state[PERIPHERAL_SENSORS])
 		rsp_status |= DIAG_STM_SENSORS;
 
+	if (driver->stm_state[PERIPHERAL_WDSP])
+		rsp_status |= DIAG_STM_WDSP;
+
 	if (driver->stm_state[APPS_DATA])
 		rsp_status |= DIAG_STM_APPS;
 
diff --git a/drivers/char/diag/diagfwd_glink.c b/drivers/char/diag/diagfwd_glink.c
new file mode 100644
index 000000000000..0a6d8bb7b21f
--- /dev/null
+++ b/drivers/char/diag/diagfwd_glink.c
@@ -0,0 +1,702 @@
+/* Copyright (c) 2016, 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
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#include <linux/slab.h>
+#include <linux/device.h>
+#include <linux/err.h>
+#include <linux/sched.h>
+#include <linux/ratelimit.h>
+#include <linux/workqueue.h>
+#include <linux/pm_runtime.h>
+#include <linux/delay.h>
+#include <linux/atomic.h>
+#include <linux/diagchar.h>
+#include <linux/of.h>
+#include <linux/kmemleak.h>
+#include <soc/qcom/glink.h>
+#include "diagchar.h"
+#include "diagfwd.h"
+#include "diagfwd_peripheral.h"
+#include "diagfwd_glink.h"
+#include "diag_ipc_logging.h"
+
+struct diag_glink_info glink_data[NUM_PERIPHERALS] = {
+	{
+		.peripheral = PERIPHERAL_MODEM,
+		.type = TYPE_DATA,
+		.edge = "mpss",
+		.name = "DIAG_DATA",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_LPASS,
+		.type = TYPE_DATA,
+		.edge = "lpass",
+		.name = "DIAG_DATA",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_WCNSS,
+		.type = TYPE_DATA,
+		.edge = "wcnss",
+		.name = "DIAG_DATA",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_SENSORS,
+		.type = TYPE_DATA,
+		.edge = "dsps",
+		.name = "DIAG_DATA",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_WDSP,
+		.type = TYPE_DATA,
+		.edge = "wdsp",
+		.name = "DIAG_DATA",
+		.hdl = NULL
+	}
+};
+
+struct diag_glink_info glink_cntl[NUM_PERIPHERALS] = {
+	{
+		.peripheral = PERIPHERAL_MODEM,
+		.type = TYPE_CNTL,
+		.edge = "mpss",
+		.name = "DIAG_CTRL",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_LPASS,
+		.type = TYPE_CNTL,
+		.edge = "lpass",
+		.name = "DIAG_CTRL",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_WCNSS,
+		.type = TYPE_CNTL,
+		.edge = "wcnss",
+		.name = "DIAG_CTRL",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_SENSORS,
+		.type = TYPE_CNTL,
+		.edge = "dsps",
+		.name = "DIAG_CTRL",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_WDSP,
+		.type = TYPE_CNTL,
+		.edge = "wdsp",
+		.name = "DIAG_CTRL",
+		.hdl = NULL
+	}
+};
+
+struct diag_glink_info glink_dci[NUM_PERIPHERALS] = {
+	{
+		.peripheral = PERIPHERAL_MODEM,
+		.type = TYPE_DCI,
+		.edge = "mpss",
+		.name = "DIAG_DCI_DATA",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_LPASS,
+		.type = TYPE_DCI,
+		.edge = "lpass",
+		.name = "DIAG_DCI_DATA",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_WCNSS,
+		.type = TYPE_DCI,
+		.edge = "wcnss",
+		.name = "DIAG_DCI_DATA",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_SENSORS,
+		.type = TYPE_DCI,
+		.edge = "dsps",
+		.name = "DIAG_DCI_DATA",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_WDSP,
+		.type = TYPE_DCI,
+		.edge = "wdsp",
+		.name = "DIAG_DCI_DATA",
+		.hdl = NULL
+	}
+};
+
+struct diag_glink_info glink_cmd[NUM_PERIPHERALS] = {
+	{
+		.peripheral = PERIPHERAL_MODEM,
+		.type = TYPE_CMD,
+		.edge = "mpss",
+		.name = "DIAG_CMD",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_LPASS,
+		.type = TYPE_CMD,
+		.edge = "lpass",
+		.name = "DIAG_CMD",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_WCNSS,
+		.type = TYPE_CMD,
+		.edge = "wcnss",
+		.name = "DIAG_CMD",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_SENSORS,
+		.type = TYPE_CMD,
+		.edge = "dsps",
+		.name = "DIAG_CMD",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_WDSP,
+		.type = TYPE_CMD,
+		.edge = "wdsp",
+		.name = "DIAG_CMD",
+		.hdl = NULL
+	}
+};
+
+struct diag_glink_info glink_dci_cmd[NUM_PERIPHERALS] = {
+	{
+		.peripheral = PERIPHERAL_MODEM,
+		.type = TYPE_DCI_CMD,
+		.edge = "mpss",
+		.name = "DIAG_DCI_CMD",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_LPASS,
+		.type = TYPE_DCI_CMD,
+		.edge = "lpass",
+		.name = "DIAG_DCI_CMD",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_WCNSS,
+		.type = TYPE_DCI_CMD,
+		.edge = "wcnss",
+		.name = "DIAG_DCI_CMD",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_SENSORS,
+		.type = TYPE_DCI_CMD,
+		.edge = "dsps",
+		.name = "DIAG_DCI_CMD",
+		.hdl = NULL
+	},
+	{
+		.peripheral = PERIPHERAL_WDSP,
+		.type = TYPE_DCI_CMD,
+		.edge = "wdsp",
+		.name = "DIAG_DCI_CMD",
+		.hdl = NULL
+	}
+};
+
+static void diag_state_open_glink(void *ctxt);
+static void diag_state_close_glink(void *ctxt);
+static int diag_glink_write(void *ctxt, unsigned char *buf, int len);
+static int diag_glink_read(void *ctxt, unsigned char *buf, int buf_len);
+static void diag_glink_queue_read(void *ctxt);
+
+static struct diag_peripheral_ops glink_ops = {
+	.open = diag_state_open_glink,
+	.close = diag_state_close_glink,
+	.write = diag_glink_write,
+	.read = diag_glink_read,
+	.queue_read = diag_glink_queue_read
+};
+
+static void diag_state_open_glink(void *ctxt)
+{
+	struct diag_glink_info *glink_info = NULL;
+
+	if (!ctxt)
+		return;
+
+	glink_info = (struct diag_glink_info *)(ctxt);
+	atomic_set(&glink_info->diag_state, 1);
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			 "%s setting diag state to 1", glink_info->name);
+}
+
+static void diag_glink_queue_read(void *ctxt)
+{
+	struct diag_glink_info *glink_info = NULL;
+
+	if (!ctxt)
+		return;
+
+	glink_info = (struct diag_glink_info *)ctxt;
+	if (glink_info->hdl && glink_info->wq &&
+		atomic_read(&glink_info->opened))
+		queue_work(glink_info->wq, &(glink_info->read_work));
+}
+
+static void diag_state_close_glink(void *ctxt)
+{
+	struct diag_glink_info *glink_info = NULL;
+
+	if (!ctxt)
+		return;
+
+	glink_info = (struct diag_glink_info *)(ctxt);
+	atomic_set(&glink_info->diag_state, 0);
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			 "%s setting diag state to 0", glink_info->name);
+	wake_up_interruptible(&glink_info->read_wait_q);
+	flush_workqueue(glink_info->wq);
+}
+
+int diag_glink_check_state(void *ctxt)
+{
+	struct diag_glink_info *info = NULL;
+
+	if (!ctxt)
+		return 0;
+
+	info = (struct diag_glink_info *)ctxt;
+	return (int)(atomic_read(&info->diag_state));
+}
+
+static int diag_glink_read(void *ctxt, unsigned char *buf, int buf_len)
+{
+	struct diag_glink_info *glink_info =  NULL;
+	int ret_val = 0;
+
+	if (!ctxt || !buf || buf_len <= 0)
+		return -EIO;
+
+	glink_info = (struct diag_glink_info *)ctxt;
+	if (!glink_info || !atomic_read(&glink_info->opened) ||
+		!glink_info->hdl || !glink_info->inited) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag:Glink channel not opened");
+		return -EIO;
+	}
+
+	ret_val = glink_queue_rx_intent(glink_info->hdl, buf, buf_len);
+	if (ret_val == 0)
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: queued an rx intent ch:%s perip:%d buf:%pK of len:%d\n",
+		glink_info->name, glink_info->peripheral, buf, buf_len);
+
+	return ret_val;
+}
+
+static void diag_glink_read_work_fn(struct work_struct *work)
+{
+	struct diag_glink_info *glink_info = container_of(work,
+							struct diag_glink_info,
+							read_work);
+
+	if (!glink_info || !atomic_read(&glink_info->opened))
+		return;
+
+	if (!glink_info->inited) {
+		diag_ws_release();
+		return;
+	}
+
+	diagfwd_channel_read(glink_info->fwd_ctxt);
+}
+
+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;
+
+	if (!glink_info || !glink_info->hdl || !ptr || !pkt_priv || !hdl)
+		return;
+
+	if (size <= 0)
+		return;
+
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: received a packet %pK of len:%d from periph:%d ch:%d\n",
+		ptr, (int)size, glink_info->peripheral, glink_info->type);
+
+	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);
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Rx done for packet %pK of len:%d periph:%d ch:%d\n",
+		ptr, (int)size, glink_info->peripheral, glink_info->type);
+}
+
+static void diag_glink_notify_remote_rx_intent(void *hdl, const void *priv,
+						size_t size)
+{
+	struct diag_glink_info *glink_info = (struct diag_glink_info *)priv;
+
+	if (!glink_info)
+		return;
+
+	atomic_inc(&glink_info->tx_intent_ready);
+	wake_up_interruptible(&glink_info->wait_q);
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag:received remote rx intent for %d type %d\n",
+		glink_info->peripheral, glink_info->type);
+}
+
+static void diag_glink_notify_tx_done(void *hdl, const void *priv,
+					const void *pkt_priv,
+					const void *ptr)
+{
+	struct diag_glink_info *glink_info = NULL;
+	struct diagfwd_info *fwd_info = NULL;
+	int found = 0;
+
+	glink_info = (struct diag_glink_info *)priv;
+	if (!glink_info)
+		return;
+
+	fwd_info = glink_info->fwd_ctxt;
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Received glink tx done notify for ptr%pK pkt_priv %pK\n",
+		ptr, pkt_priv);
+	found = diagfwd_write_buffer_done(fwd_info, ptr);
+	if (!found)
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"Received Tx done on invalid buffer ptr %pK\n", ptr);
+}
+
+static int  diag_glink_write(void *ctxt, unsigned char *buf, int len)
+{
+	struct diag_glink_info *glink_info = NULL;
+	int err = 0;
+	uint32_t tx_flags = GLINK_TX_REQ_INTENT;
+
+	if (!ctxt || !buf)
+		return -EIO;
+
+	glink_info = (struct diag_glink_info *)ctxt;
+	if (!glink_info || len <= 0) {
+		pr_err_ratelimited("diag: In %s, invalid params, glink_info: %pK, buf: %pK, len: %d\n",
+				__func__, glink_info, buf, len);
+		return -EINVAL;
+	}
+
+	if (!glink_info->inited || !glink_info->hdl ||
+		!atomic_read(&glink_info->opened)) {
+		pr_err_ratelimited("diag: In %s, glink not inited, glink_info: %pK, buf: %pK, len: %d\n",
+				 __func__, glink_info, buf, len);
+		return -ENODEV;
+	}
+
+	err = wait_event_interruptible(glink_info->wait_q,
+				   atomic_read(&glink_info->tx_intent_ready));
+	if (err) {
+		diagfwd_write_buffer_done(glink_info->fwd_ctxt, buf);
+		return -ERESTARTSYS;
+	}
+
+	atomic_dec(&glink_info->tx_intent_ready);
+	err = glink_tx(glink_info->hdl, glink_info, buf, len, tx_flags);
+	if (!err) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s wrote to glink, len: %d\n",
+				 glink_info->name, len);
+	}
+
+	return err;
+
+}
+static void diag_glink_transport_notify_state(void *handle, const void *priv,
+					  unsigned event)
+{
+	struct diag_glink_info *glink_info = (struct diag_glink_info *)priv;
+
+	if (!glink_info)
+		return;
+
+	switch (event) {
+	case GLINK_CONNECTED:
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"%s received channel connect for periph:%d\n",
+			 glink_info->name, glink_info->peripheral);
+		atomic_set(&glink_info->opened, 1);
+		diagfwd_channel_open(glink_info->fwd_ctxt);
+		diagfwd_late_open(glink_info->fwd_ctxt);
+		break;
+	case GLINK_LOCAL_DISCONNECTED:
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"%s received channel disconnect for periph:%d\n",
+			glink_info->name, glink_info->peripheral);
+
+		break;
+	case GLINK_REMOTE_DISCONNECTED:
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"%s received channel remote disconnect for periph:%d\n",
+			 glink_info->name, glink_info->peripheral);
+		atomic_set(&glink_info->opened, 0);
+		break;
+	default:
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"%s received invalid notification\n",
+			glink_info->name);
+		break;
+	}
+
+}
+static void diag_glink_open_work_fn(struct work_struct *work)
+{
+	struct diag_glink_info *glink_info = container_of(work,
+							struct diag_glink_info,
+							open_work);
+	struct glink_open_config open_cfg;
+	void *handle = NULL;
+
+	if (!glink_info)
+		return;
+
+	memset(&open_cfg, 0, sizeof(struct glink_open_config));
+	open_cfg.priv = glink_info;
+	open_cfg.edge = glink_info->edge;
+	open_cfg.name = glink_info->name;
+	open_cfg.notify_rx = diag_glink_notify_rx;
+	open_cfg.notify_tx_done = diag_glink_notify_tx_done;
+	open_cfg.notify_state = diag_glink_transport_notify_state;
+	open_cfg.notify_remote_rx_intent = diag_glink_notify_remote_rx_intent;
+	handle = glink_open(&open_cfg);
+	if (IS_ERR_OR_NULL(handle)) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "error opening channel %s",
+			glink_info->name);
+	} else
+		glink_info->hdl = handle;
+}
+
+static void diag_glink_close_work_fn(struct work_struct *work)
+{
+	struct diag_glink_info *glink_info = container_of(work,
+							struct diag_glink_info,
+							close_work);
+	if (!glink_info->inited)
+		return;
+
+	glink_close(glink_info->hdl);
+	atomic_set(&glink_info->opened, 0);
+	diagfwd_channel_close(glink_info->fwd_ctxt);
+}
+
+static void diag_glink_notify_cb(struct glink_link_state_cb_info *cb_info,
+				void *priv)
+{
+	struct diag_glink_info *glink_info = NULL;
+
+	glink_info = (struct diag_glink_info *)priv;
+	if (!glink_info)
+		return;
+	if (!cb_info)
+		return;
+
+	switch (cb_info->link_state) {
+	case GLINK_LINK_STATE_UP:
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"%s channel opened for periph:%d\n",
+			glink_info->name, glink_info->peripheral);
+		queue_work(glink_info->wq, &glink_info->open_work);
+		break;
+	case GLINK_LINK_STATE_DOWN:
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"%s channel closed for periph:%d\n",
+			glink_info->name, glink_info->peripheral);
+		queue_work(glink_info->wq, &glink_info->close_work);
+		break;
+	default:
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"Invalid link state notification for ch:%s\n",
+			glink_info->name);
+		break;
+
+	}
+}
+
+static void glink_late_init(struct diag_glink_info *glink_info)
+{
+	struct diagfwd_info *fwd_info = NULL;
+
+	if (!glink_info)
+		return;
+
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s entering\n",
+		 glink_info->name);
+
+	diagfwd_register(TRANSPORT_GLINK, glink_info->peripheral,
+			glink_info->type, (void *)glink_info,
+			&glink_ops, &glink_info->fwd_ctxt);
+	fwd_info = glink_info->fwd_ctxt;
+	if (!fwd_info)
+		return;
+
+	glink_info->inited = 1;
+
+	if (atomic_read(&glink_info->opened))
+		diagfwd_channel_open(glink_info->fwd_ctxt);
+
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s exiting\n",
+		 glink_info->name);
+}
+
+int diag_glink_init_peripheral(uint8_t peripheral)
+{
+	if (peripheral >= NUM_PERIPHERALS) {
+		pr_err("diag: In %s, invalid peripheral %d\n",
+		       __func__, peripheral);
+		return -EINVAL;
+	}
+
+	glink_late_init(&glink_data[peripheral]);
+	glink_late_init(&glink_dci[peripheral]);
+	glink_late_init(&glink_cmd[peripheral]);
+	glink_late_init(&glink_dci_cmd[peripheral]);
+
+	return 0;
+}
+
+static void __diag_glink_init(struct diag_glink_info *glink_info)
+{
+	char wq_name[DIAG_GLINK_NAME_SZ + 12];
+	struct glink_link_info link_info;
+
+	if (!glink_info)
+		return;
+
+	init_waitqueue_head(&glink_info->wait_q);
+	init_waitqueue_head(&glink_info->read_wait_q);
+	mutex_init(&glink_info->lock);
+	strlcpy(wq_name, "DIAG_GLINK_", 12);
+	strlcat(wq_name, glink_info->name, sizeof(glink_info->name));
+	glink_info->wq = create_singlethread_workqueue(wq_name);
+	if (!glink_info->wq) {
+		pr_err("diag: In %s, unable to create workqueue for glink ch:%s\n",
+			   __func__, glink_info->name);
+		return;
+	}
+	INIT_WORK(&(glink_info->open_work), diag_glink_open_work_fn);
+	INIT_WORK(&(glink_info->close_work), diag_glink_close_work_fn);
+	INIT_WORK(&(glink_info->read_work), diag_glink_read_work_fn);
+	link_info.glink_link_state_notif_cb = diag_glink_notify_cb;
+	link_info.transport = NULL;
+	strlcpy((char *)link_info.edge, glink_info->edge,
+			sizeof(link_info.edge));
+	glink_info->hdl = glink_register_link_state_cb(&link_info,
+							(void *)glink_info);
+	if (IS_ERR_OR_NULL(glink_info->hdl)) {
+		pr_err("diag: In %s, unable to register for glink channel %s\n",
+			   __func__, glink_info->name);
+		destroy_workqueue(glink_info->wq);
+		return;
+	}
+	glink_info->fwd_ctxt = NULL;
+	atomic_set(&glink_info->tx_intent_ready, 0);
+	atomic_set(&glink_info->opened, 0);
+	atomic_set(&glink_info->diag_state, 0);
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"%s initialized fwd_ctxt: %pK hdl: %pK\n",
+		glink_info->name, glink_info->fwd_ctxt, glink_info->hdl);
+}
+
+void diag_glink_invalidate(void *ctxt, struct diagfwd_info *fwd_ctxt)
+{
+	struct diag_glink_info *info = NULL;
+
+	if (!ctxt || !fwd_ctxt)
+		return;
+
+	info = (struct diag_glink_info *)ctxt;
+	info->fwd_ctxt = fwd_ctxt;
+}
+
+int diag_glink_init(void)
+{
+	uint8_t peripheral;
+	struct diag_glink_info *glink_info = NULL;
+
+	for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) {
+		glink_info = &glink_cntl[peripheral];
+		__diag_glink_init(glink_info);
+		diagfwd_cntl_register(TRANSPORT_GLINK, glink_info->peripheral,
+					(void *)glink_info, &glink_ops,
+					&(glink_info->fwd_ctxt));
+		glink_info->inited = 1;
+		__diag_glink_init(&glink_data[peripheral]);
+		__diag_glink_init(&glink_cmd[peripheral]);
+		__diag_glink_init(&glink_dci[peripheral]);
+		__diag_glink_init(&glink_dci_cmd[peripheral]);
+	}
+	return 0;
+}
+
+static void __diag_glink_exit(struct diag_glink_info *glink_info)
+{
+	if (!glink_info)
+		return;
+
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s entering\n",
+			 glink_info->name);
+
+	diagfwd_deregister(glink_info->peripheral, glink_info->type,
+					   (void *)glink_info);
+	glink_info->fwd_ctxt = NULL;
+	glink_info->hdl = NULL;
+	if (glink_info->wq)
+		destroy_workqueue(glink_info->wq);
+
+	DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "%s exiting\n",
+			 glink_info->name);
+}
+
+void diag_glink_early_exit(void)
+{
+	int peripheral = 0;
+
+	for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) {
+		__diag_glink_exit(&glink_cntl[peripheral]);
+		glink_unregister_link_state_cb(&glink_cntl[peripheral].hdl);
+	}
+}
+
+void diag_glink_exit(void)
+{
+	int peripheral = 0;
+
+	for (peripheral = 0; peripheral < NUM_PERIPHERALS; peripheral++) {
+		__diag_glink_exit(&glink_data[peripheral]);
+		__diag_glink_exit(&glink_cmd[peripheral]);
+		__diag_glink_exit(&glink_dci[peripheral]);
+		__diag_glink_exit(&glink_dci_cmd[peripheral]);
+		glink_unregister_link_state_cb(&glink_data[peripheral].hdl);
+		glink_unregister_link_state_cb(&glink_cmd[peripheral].hdl);
+		glink_unregister_link_state_cb(&glink_dci[peripheral].hdl);
+		glink_unregister_link_state_cb(&glink_dci_cmd[peripheral].hdl);
+	}
+}
diff --git a/drivers/char/diag/diagfwd_glink.h b/drivers/char/diag/diagfwd_glink.h
new file mode 100644
index 000000000000..3f00a7ed60a8
--- /dev/null
+++ b/drivers/char/diag/diagfwd_glink.h
@@ -0,0 +1,53 @@
+/* Copyright (c) 2016, 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
+ * only version 2 as published by the Free Software Foundation.
+ *
+ * This program is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ */
+
+#ifndef DIAGFWD_GLINK_H
+#define DIAGFWD_GLINK_H
+
+#define DIAG_GLINK_NAME_SZ	24
+#define GLINK_DRAIN_BUF_SIZE	4096
+
+struct diag_glink_info {
+	uint8_t peripheral;
+	uint8_t type;
+	uint8_t inited;
+	atomic_t opened;
+	atomic_t diag_state;
+	uint32_t fifo_size;
+	atomic_t tx_intent_ready;
+	void *hdl;
+	char edge[DIAG_GLINK_NAME_SZ];
+	char name[DIAG_GLINK_NAME_SZ];
+	struct mutex lock;
+	wait_queue_head_t read_wait_q;
+	wait_queue_head_t wait_q;
+	struct workqueue_struct *wq;
+	struct work_struct open_work;
+	struct work_struct close_work;
+	struct work_struct read_work;
+	struct diagfwd_info *fwd_ctxt;
+};
+
+extern struct diag_glink_info glink_data[NUM_PERIPHERALS];
+extern struct diag_glink_info glink_cntl[NUM_PERIPHERALS];
+extern struct diag_glink_info glink_cmd[NUM_PERIPHERALS];
+extern struct diag_glink_info glink_dci_cmd[NUM_PERIPHERALS];
+extern struct diag_glink_info glink_dci[NUM_PERIPHERALS];
+
+int diag_glink_init_peripheral(uint8_t peripheral);
+void diag_glink_exit(void);
+int diag_glink_init(void);
+void diag_glink_early_exit(void);
+void diag_glink_invalidate(void *ctxt, struct diagfwd_info *fwd_ctxt);
+int diag_glink_check_state(void *ctxt);
+
+#endif
diff --git a/drivers/char/diag/diagfwd_peripheral.c b/drivers/char/diag/diagfwd_peripheral.c
index 755c8fab27b1..066890aebf39 100644
--- a/drivers/char/diag/diagfwd_peripheral.c
+++ b/drivers/char/diag/diagfwd_peripheral.c
@@ -30,6 +30,7 @@
 #include "diagfwd_socket.h"
 #include "diag_mux.h"
 #include "diag_ipc_logging.h"
+#include "diagfwd_glink.h"
 
 struct data_header {
 	uint8_t control_char;
@@ -51,7 +52,8 @@ static void diagfwd_cntl_read_done(struct diagfwd_info *fwd_info,
 				   unsigned char *buf, int len);
 static void diagfwd_dci_read_done(struct diagfwd_info *fwd_info,
 				  unsigned char *buf, int len);
-
+static void diagfwd_write_buffers_init(struct diagfwd_info *fwd_info);
+static void diagfwd_write_buffers_exit(struct diagfwd_info *fwd_info);
 struct diagfwd_info peripheral_info[NUM_TYPES][NUM_PERIPHERALS];
 
 static struct diag_channel_ops data_ch_ops = {
@@ -475,6 +477,7 @@ int diagfwd_peripheral_init(void)
 	diag_smd_init();
 	if (driver->supports_sockets)
 		diag_socket_init();
+	diag_glink_init();
 
 	return 0;
 }
@@ -621,6 +624,7 @@ void diagfwd_close_transport(uint8_t transport, uint8_t peripheral)
 	void (*invalidate_fn)(void *, struct diagfwd_info *) = NULL;
 	int (*check_channel_state)(void *) = NULL;
 	uint8_t transport_open = 0;
+	int i = 0;
 
 	if (peripheral >= NUM_PERIPHERALS)
 		return;
@@ -633,10 +637,17 @@ void diagfwd_close_transport(uint8_t transport, uint8_t peripheral)
 		check_channel_state = diag_socket_check_state;
 		break;
 	case TRANSPORT_SOCKET:
-		transport_open = TRANSPORT_SMD;
-		init_fn = diag_smd_init_peripheral;
-		invalidate_fn = diag_smd_invalidate;
-		check_channel_state = diag_smd_check_state;
+		if (peripheral == PERIPHERAL_WDSP) {
+			transport_open = TRANSPORT_GLINK;
+			init_fn = diag_glink_init_peripheral;
+			invalidate_fn = diag_glink_invalidate;
+			check_channel_state = diag_glink_check_state;
+		} else {
+			transport_open = TRANSPORT_SMD;
+			init_fn = diag_smd_init_peripheral;
+			invalidate_fn = diag_smd_invalidate;
+			check_channel_state = diag_smd_check_state;
+		}
 		break;
 	default:
 		return;
@@ -660,6 +671,8 @@ void diagfwd_close_transport(uint8_t transport, uint8_t peripheral)
 	dest_info->buf_2 = fwd_info->buf_2;
 	dest_info->transport = fwd_info->transport;
 	invalidate_fn(dest_info->ctxt, dest_info);
+	for (i = 0; i < NUM_WRITE_BUFFERS; i++)
+		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);
@@ -668,13 +681,30 @@ void diagfwd_close_transport(uint8_t transport, uint8_t peripheral)
 	diagfwd_queue_read(&peripheral_info[TYPE_CMD][peripheral]);
 }
 
+void *diagfwd_request_write_buf(struct diagfwd_info *fwd_info)
+{
+	void *buf = NULL;
+	int index;
+
+	for (index = 0 ; index < NUM_WRITE_BUFFERS; index++) {
+		if (!atomic_read(&(fwd_info->buf_ptr[index]->in_busy))) {
+			buf = fwd_info->buf_ptr[index]->data;
+			if (!buf)
+				return NULL;
+			atomic_set(&(fwd_info->buf_ptr[index]->in_busy), 1);
+			break;
+		}
+	}
+	return buf;
+}
+
 int diagfwd_write(uint8_t peripheral, uint8_t type, void *buf, int len)
 {
 	struct diagfwd_info *fwd_info = NULL;
 	int err = 0;
 	uint8_t retry_count = 0;
 	uint8_t max_retries = 3;
-
+	void *buf_ptr = NULL;
 	if (peripheral >= NUM_PERIPHERALS || type >= NUM_TYPES)
 		return -EINVAL;
 
@@ -696,9 +726,21 @@ int diagfwd_write(uint8_t peripheral, uint8_t type, void *buf, int len)
 	if (!(fwd_info->p_ops && fwd_info->p_ops->write && fwd_info->ctxt))
 		return -EIO;
 
+	if (fwd_info->transport == TRANSPORT_GLINK) {
+		buf_ptr = diagfwd_request_write_buf(fwd_info);
+		if (buf_ptr)
+			memcpy(buf_ptr, buf, len);
+		else {
+			DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+				 "diag: buffer not found for writing\n");
+			return -EIO;
+		}
+	} else
+		buf_ptr = buf;
+
 	while (retry_count < max_retries) {
 		err = 0;
-		err = fwd_info->p_ops->write(fwd_info->ctxt, buf, len);
+		err = fwd_info->p_ops->write(fwd_info->ctxt, buf_ptr, len);
 		if (err && err != -ENODEV) {
 			usleep_range(100000, 101000);
 			retry_count++;
@@ -715,6 +757,7 @@ int diagfwd_write(uint8_t peripheral, uint8_t type, void *buf, int len)
 
 static void __diag_fwd_open(struct diagfwd_info *fwd_info)
 {
+	int i;
 	if (!fwd_info)
 		return;
 
@@ -729,7 +772,10 @@ static void __diag_fwd_open(struct diagfwd_info *fwd_info)
 
 	if (fwd_info->p_ops && fwd_info->p_ops->open)
 		fwd_info->p_ops->open(fwd_info->ctxt);
-
+	for (i = 0; i < NUM_WRITE_BUFFERS; i++) {
+		if (fwd_info->buf_ptr[i])
+			atomic_set(&fwd_info->buf_ptr[i]->in_busy, 0);
+	}
 	diagfwd_queue_read(fwd_info);
 }
 
@@ -807,6 +853,7 @@ int diagfwd_channel_open(struct diagfwd_info *fwd_info)
 
 	fwd_info->ch_open = 1;
 	diagfwd_buffers_init(fwd_info);
+	diagfwd_write_buffers_init(fwd_info);
 	if (fwd_info && fwd_info->c_ops && fwd_info->c_ops->open)
 		fwd_info->c_ops->open(fwd_info);
 	diagfwd_queue_read(fwd_info);
@@ -885,6 +932,25 @@ void diagfwd_write_done(uint8_t peripheral, uint8_t type, int ctxt)
 	diagfwd_queue_read(fwd_info);
 }
 
+int diagfwd_write_buffer_done(struct diagfwd_info *fwd_info, const void *ptr)
+{
+
+	int found = 0;
+	int index = 0;
+
+	if (!fwd_info || !ptr)
+		return found;
+
+	for (index = 0; index < NUM_WRITE_BUFFERS; index++) {
+		if (fwd_info->buf_ptr[index]->data == ptr) {
+			atomic_set(&fwd_info->buf_ptr[index]->in_busy, 0);
+			found = 1;
+			break;
+		}
+	}
+	return found;
+}
+
 void diagfwd_channel_read(struct diagfwd_info *fwd_info)
 {
 	int err = 0;
@@ -1114,3 +1180,64 @@ static void diagfwd_buffers_exit(struct diagfwd_info *fwd_info)
 	spin_unlock_irqrestore(&fwd_info->buf_lock, flags);
 }
 
+void diagfwd_write_buffers_init(struct diagfwd_info *fwd_info)
+{
+	unsigned long flags;
+	int i;
+
+	if (!fwd_info)
+		return;
+
+	if (!fwd_info->inited) {
+		pr_err("diag: In %s, channel not inited, p: %d, t: %d\n",
+		       __func__, fwd_info->peripheral, fwd_info->type);
+		return;
+	}
+
+	spin_lock_irqsave(&fwd_info->buf_lock, flags);
+	for (i = 0; i < NUM_WRITE_BUFFERS; i++) {
+		if (!fwd_info->buf_ptr[i])
+			fwd_info->buf_ptr[i] =
+					kzalloc(sizeof(struct diagfwd_buf_t),
+						GFP_ATOMIC);
+		if (!fwd_info->buf_ptr[i])
+			goto err;
+		kmemleak_not_leak(fwd_info->buf_ptr[i]);
+		if (!fwd_info->buf_ptr[i]->data) {
+			fwd_info->buf_ptr[i]->data = kzalloc(PERIPHERAL_BUF_SZ,
+								GFP_ATOMIC);
+			if (!fwd_info->buf_ptr[i]->data)
+				goto err;
+			fwd_info->buf_ptr[i]->len = PERIPHERAL_BUF_SZ;
+			kmemleak_not_leak(fwd_info->buf_ptr[i]->data);
+		}
+	}
+	spin_unlock_irqrestore(&fwd_info->buf_lock, flags);
+	return;
+
+err:
+	spin_unlock_irqrestore(&fwd_info->buf_lock, flags);
+	pr_err("diag:unable to allocate write buffers\n");
+	diagfwd_write_buffers_exit(fwd_info);
+
+}
+
+static void diagfwd_write_buffers_exit(struct diagfwd_info *fwd_info)
+{
+	unsigned long flags;
+	int i;
+
+	if (!fwd_info)
+		return;
+
+	spin_lock_irqsave(&fwd_info->buf_lock, flags);
+	for (i = 0; i < NUM_WRITE_BUFFERS; i++) {
+		if (fwd_info->buf_ptr[i]) {
+			kfree(fwd_info->buf_ptr[i]->data);
+			fwd_info->buf_ptr[i]->data = NULL;
+			kfree(fwd_info->buf_ptr[i]);
+			fwd_info->buf_ptr[i] = NULL;
+		}
+	}
+	spin_unlock_irqrestore(&fwd_info->buf_lock, flags);
+}
diff --git a/drivers/char/diag/diagfwd_peripheral.h b/drivers/char/diag/diagfwd_peripheral.h
index dc50d70e80b4..b511bf495bc2 100644
--- a/drivers/char/diag/diagfwd_peripheral.h
+++ b/drivers/char/diag/diagfwd_peripheral.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2016, 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
@@ -20,19 +20,22 @@
 #define TRANSPORT_UNKNOWN		-1
 #define TRANSPORT_SMD			0
 #define TRANSPORT_SOCKET		1
-#define NUM_TRANSPORT			2
-
+#define TRANSPORT_GLINK			2
+#define NUM_TRANSPORT			3
+#define NUM_WRITE_BUFFERS		2
 #define PERIPHERAL_MASK(x)					\
 	((x == PERIPHERAL_MODEM) ? DIAG_CON_MPSS :		\
 	((x == PERIPHERAL_LPASS) ? DIAG_CON_LPASS :		\
 	((x == PERIPHERAL_WCNSS) ? DIAG_CON_WCNSS :		\
-	((x == PERIPHERAL_SENSORS) ? DIAG_CON_SENSORS : 0))))	\
+	((x == PERIPHERAL_SENSORS) ? DIAG_CON_SENSORS : \
+	((x == PERIPHERAL_WDSP) ? DIAG_CON_WDSP : 0)))))	\
 
 #define PERIPHERAL_STRING(x)					\
 	((x == PERIPHERAL_MODEM) ? "MODEM" :			\
 	((x == PERIPHERAL_LPASS) ? "LPASS" :			\
 	((x == PERIPHERAL_WCNSS) ? "WCNSS" :			\
-	((x == PERIPHERAL_SENSORS) ? "SENSORS" : "UNKNOWN"))))	\
+	((x == PERIPHERAL_SENSORS) ? "SENSORS" :		\
+	((x == PERIPHERAL_WDSP) ? "WDSP" : "UNKNOWN")))))	\
 
 struct diagfwd_buf_t {
 	unsigned char *data;
@@ -72,6 +75,7 @@ struct diagfwd_info {
 	void *ctxt;
 	struct diagfwd_buf_t *buf_1;
 	struct diagfwd_buf_t *buf_2;
+	struct diagfwd_buf_t *buf_ptr[NUM_WRITE_BUFFERS];
 	struct diag_peripheral_ops *p_ops;
 	struct diag_channel_ops *c_ops;
 };
@@ -108,5 +112,6 @@ int diagfwd_channel_close(struct diagfwd_info *fwd_info);
 void diagfwd_channel_read(struct diagfwd_info *fwd_info);
 int diagfwd_channel_read_done(struct diagfwd_info *fwd_info,
 			      unsigned char *buf, uint32_t len);
+int diagfwd_write_buffer_done(struct diagfwd_info *fwd_info, const void *ptr);
 
 #endif
diff --git a/drivers/char/diag/diagfwd_smd.c b/drivers/char/diag/diagfwd_smd.c
index 3ee21101e2f2..12069df1224d 100644
--- a/drivers/char/diag/diagfwd_smd.c
+++ b/drivers/char/diag/diagfwd_smd.c
@@ -49,6 +49,11 @@ struct diag_smd_info smd_data[NUM_PERIPHERALS] = {
 		.peripheral = PERIPHERAL_SENSORS,
 		.type = TYPE_DATA,
 		.name = "SENSORS_DATA"
+	},
+	{
+		.peripheral = PERIPHERAL_WDSP,
+		.type = TYPE_DATA,
+		.name = "DIAG_DATA"
 	}
 };
 
@@ -72,6 +77,11 @@ struct diag_smd_info smd_cntl[NUM_PERIPHERALS] = {
 		.peripheral = PERIPHERAL_SENSORS,
 		.type = TYPE_CNTL,
 		.name = "SENSORS_CNTL"
+	},
+	{
+		.peripheral = PERIPHERAL_WDSP,
+		.type = TYPE_CNTL,
+		.name = "DIAG_CTRL"
 	}
 };
 
@@ -95,6 +105,11 @@ struct diag_smd_info smd_dci[NUM_PERIPHERALS] = {
 		.peripheral = PERIPHERAL_SENSORS,
 		.type = TYPE_DCI,
 		.name = "SENSORS_DCI"
+	},
+	{
+		.peripheral = PERIPHERAL_WDSP,
+		.type = TYPE_DCI,
+		.name = "DIAG_DCI_DATA"
 	}
 };
 
@@ -118,6 +133,11 @@ struct diag_smd_info smd_cmd[NUM_PERIPHERALS] = {
 		.peripheral = PERIPHERAL_SENSORS,
 		.type = TYPE_CMD,
 		.name = "SENSORS_CMD"
+	},
+	{
+		.peripheral = PERIPHERAL_WDSP,
+		.type = TYPE_CMD,
+		.name = "DIAG_CMD"
 	}
 };
 
@@ -141,6 +161,11 @@ struct diag_smd_info smd_dci_cmd[NUM_PERIPHERALS] = {
 		.peripheral = PERIPHERAL_SENSORS,
 		.type = TYPE_DCI_CMD,
 		.name = "SENSORS_DCI_CMD"
+	},
+	{
+		.peripheral = PERIPHERAL_WDSP,
+		.type = TYPE_DCI_CMD,
+		.name = "DIAG_DCI_CMD"
 	}
 };
 
diff --git a/drivers/char/diag/diagfwd_socket.c b/drivers/char/diag/diagfwd_socket.c
index 18f76ea89ec5..fd927e931414 100644
--- a/drivers/char/diag/diagfwd_socket.c
+++ b/drivers/char/diag/diagfwd_socket.c
@@ -40,6 +40,7 @@
 #define LPASS_INST_BASE		64
 #define WCNSS_INST_BASE		128
 #define SENSORS_INST_BASE	192
+#define WDSP_INST_BASE	256
 
 #define INST_ID_CNTL		0
 #define INST_ID_CMD		1
@@ -69,6 +70,11 @@ struct diag_socket_info socket_data[NUM_PERIPHERALS] = {
 		.peripheral = PERIPHERAL_SENSORS,
 		.type = TYPE_DATA,
 		.name = "SENSORS_DATA"
+	},
+	{
+		.peripheral = PERIPHERAL_WDSP,
+		.type = TYPE_DATA,
+		.name = "DIAG_DATA"
 	}
 };
 
@@ -92,6 +98,11 @@ struct diag_socket_info socket_cntl[NUM_PERIPHERALS] = {
 		.peripheral = PERIPHERAL_SENSORS,
 		.type = TYPE_CNTL,
 		.name = "SENSORS_CNTL"
+	},
+	{
+		.peripheral = PERIPHERAL_WDSP,
+		.type = TYPE_CNTL,
+		.name = "DIAG_CTRL"
 	}
 };
 
@@ -115,6 +126,11 @@ struct diag_socket_info socket_dci[NUM_PERIPHERALS] = {
 		.peripheral = PERIPHERAL_SENSORS,
 		.type = TYPE_DCI,
 		.name = "SENSORS_DCI"
+	},
+	{
+		.peripheral = PERIPHERAL_WDSP,
+		.type = TYPE_DCI,
+		.name = "DIAG_DCI_DATA"
 	}
 };
 
@@ -138,7 +154,13 @@ struct diag_socket_info socket_cmd[NUM_PERIPHERALS] = {
 		.peripheral = PERIPHERAL_SENSORS,
 		.type = TYPE_CMD,
 		.name = "SENSORS_CMD"
+	},
+	{
+		.peripheral = PERIPHERAL_WDSP,
+		.type = TYPE_CMD,
+		.name = "DIAG_CMD"
 	}
+
 };
 
 struct diag_socket_info socket_dci_cmd[NUM_PERIPHERALS] = {
@@ -161,6 +183,11 @@ struct diag_socket_info socket_dci_cmd[NUM_PERIPHERALS] = {
 		.peripheral = PERIPHERAL_SENSORS,
 		.type = TYPE_DCI_CMD,
 		.name = "SENSORS_DCI_CMD"
+	},
+	{
+		.peripheral = PERIPHERAL_WDSP,
+		.type = TYPE_DCI_CMD,
+		.name = "DIAG_DCI_CMD"
 	}
 };
 
@@ -711,6 +738,9 @@ static void __diag_socket_init(struct diag_socket_info *info)
 	case PERIPHERAL_SENSORS:
 		ins_base = SENSORS_INST_BASE;
 		break;
+	case PERIPHERAL_WDSP:
+		ins_base = WDSP_INST_BASE;
+		break;
 	}
 
 	switch (info->type) {