diag: Add support for header untagging

This patch provides the support for segregate different PD data onto
separate buffers and then onto its own data stream based on received
diag private id header from the peripheral data packets.
This patch adds a new feature mask supporting the feature and
a new IOCTL querying the support of the feature.

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

13 files changed, 544 insertions, 46 deletions

diff --git a/drivers/char/diag/diag_debugfs.c b/drivers/char/diag/diag_debugfs.c
index b861d5f32d03..ca7dd88048ac 100644
--- a/drivers/char/diag/diag_debugfs.c
+++ b/drivers/char/diag/diag_debugfs.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2011-2016, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2011-2017, 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
@@ -72,6 +72,7 @@ static ssize_t diag_dbgfs_read_status(struct file *file, char __user *ubuf,
 		"Uses Device Tree: %d\n"
 		"Apps Supports Separate CMDRSP: %d\n"
 		"Apps Supports HDLC Encoding: %d\n"
+		"Apps Supports Header Untagging: %d\n"
 		"Apps Supports Sockets: %d\n"
 		"Logging Mode: %d\n"
 		"RSP Buffer is Busy: %d\n"
@@ -86,6 +87,7 @@ static ssize_t diag_dbgfs_read_status(struct file *file, char __user *ubuf,
 		driver->use_device_tree,
 		driver->supports_separate_cmdrsp,
 		driver->supports_apps_hdlc_encoding,
+		driver->supports_apps_header_untagging,
 		driver->supports_sockets,
 		driver->logging_mode,
 		driver->rsp_buf_busy,
@@ -97,18 +99,19 @@ static ssize_t diag_dbgfs_read_status(struct file *file, char __user *ubuf,
 
 	for (i = 0; i < NUM_PERIPHERALS; i++) {
 		ret += scnprintf(buf+ret, buf_size-ret,
-			"p: %s Feature: %02x %02x |%c%c%c%c%c%c%c%c|\n",
+			"p: %s Feature: %02x %02x |%c%c%c%c%c%c%c%c%c|\n",
 			PERIPHERAL_STRING(i),
 			driver->feature[i].feature_mask[0],
 			driver->feature[i].feature_mask[1],
 			driver->feature[i].rcvd_feature_mask ? 'F':'f',
+			driver->feature[i].peripheral_buffering ? 'B':'b',
 			driver->feature[i].separate_cmd_rsp ? 'C':'c',
 			driver->feature[i].encode_hdlc ? 'H':'h',
-			driver->feature[i].peripheral_buffering ? 'B':'b',
 			driver->feature[i].mask_centralization ? 'M':'m',
 			driver->feature[i].stm_support ? 'Q':'q',
 			driver->feature[i].sockets_enabled ? 'S':'s',
-			driver->feature[i].sent_feature_mask ? 'T':'t');
+			driver->feature[i].sent_feature_mask ? 'T':'t',
+			driver->feature[i].untag_header ? 'U':'u');
 	}
 
 #ifdef CONFIG_DIAG_OVER_USB
diff --git a/drivers/char/diag/diag_masks.c b/drivers/char/diag/diag_masks.c
index 44e71a704e6a..0c958d855f94 100644
--- a/drivers/char/diag/diag_masks.c
+++ b/drivers/char/diag/diag_masks.c
@@ -456,6 +456,8 @@ static void diag_send_feature_mask_update(uint8_t peripheral)
 		DIAG_SET_FEATURE_MASK(F_DIAG_REQ_RSP_SUPPORT);
 	if (driver->supports_apps_hdlc_encoding)
 		DIAG_SET_FEATURE_MASK(F_DIAG_APPS_HDLC_ENCODE);
+	if (driver->supports_apps_header_untagging)
+		DIAG_SET_FEATURE_MASK(F_DIAG_PKT_HEADER_UNTAG);
 	DIAG_SET_FEATURE_MASK(F_DIAG_MASK_CENTRALIZATION);
 	if (driver->supports_sockets)
 		DIAG_SET_FEATURE_MASK(F_DIAG_SOCKETS_ENABLED);
diff --git a/drivers/char/diag/diag_memorydevice.c b/drivers/char/diag/diag_memorydevice.c
index c552f263d7e5..dc3029cc459d 100644
--- a/drivers/char/diag/diag_memorydevice.c
+++ b/drivers/char/diag/diag_memorydevice.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2014-2016, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2014-2017, 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
@@ -29,6 +29,7 @@
 #include "diagmem.h"
 #include "diagfwd.h"
 #include "diagfwd_peripheral.h"
+#include "diag_ipc_logging.h"
 
 struct diag_md_info diag_md[NUM_DIAG_MD_DEV] = {
 	{
@@ -143,9 +144,24 @@ int diag_md_write(int id, unsigned char *buf, int len, int ctx)
 	if (!buf || len < 0)
 		return -EINVAL;
 
-	peripheral = GET_BUF_PERIPHERAL(ctx);
-	if (peripheral > NUM_PERIPHERALS)
-		return -EINVAL;
+	if (driver->pd_logging_mode) {
+		peripheral = GET_PD_CTXT(ctx);
+		switch (peripheral) {
+		case UPD_WLAN:
+			break;
+		case DIAG_ID_MPSS:
+		default:
+			peripheral = GET_BUF_PERIPHERAL(ctx);
+			if (peripheral > NUM_PERIPHERALS)
+				return -EINVAL;
+			break;
+		}
+	} else {
+		/* Account for Apps data as well */
+		peripheral = GET_BUF_PERIPHERAL(ctx);
+		if (peripheral > NUM_PERIPHERALS)
+			return -EINVAL;
+	}
 
 	session_info = diag_md_session_get_peripheral(peripheral);
 	if (!session_info)
@@ -219,18 +235,41 @@ int diag_md_copy_to_user(char __user *buf, int *pret, size_t buf_size,
 	uint8_t peripheral = 0;
 	struct diag_md_session_t *session_info = NULL;
 
+	mutex_lock(&driver->diagfwd_untag_mutex);
+
 	for (i = 0; i < NUM_DIAG_MD_DEV && !err; i++) {
 		ch = &diag_md[i];
 		for (j = 0; j < ch->num_tbl_entries && !err; j++) {
 			entry = &ch->tbl[j];
 			if (entry->len <= 0)
 				continue;
-			peripheral = GET_BUF_PERIPHERAL(entry->ctx);
-			/* Account for Apps data as well */
-			if (peripheral > NUM_PERIPHERALS)
-				goto drop_data;
+			if (driver->pd_logging_mode) {
+				peripheral = GET_PD_CTXT(entry->ctx);
+				switch (peripheral) {
+				case UPD_WLAN:
+					break;
+				case DIAG_ID_MPSS:
+				default:
+					peripheral =
+						GET_BUF_PERIPHERAL(entry->ctx);
+					if (peripheral > NUM_PERIPHERALS)
+						goto drop_data;
+					break;
+				}
+			} else {
+				/* Account for Apps data as well */
+				peripheral = GET_BUF_PERIPHERAL(entry->ctx);
+				if (peripheral > NUM_PERIPHERALS)
+					goto drop_data;
+			}
+
 			session_info =
 			diag_md_session_get_peripheral(peripheral);
+			if (!session_info) {
+				mutex_unlock(&driver->diagfwd_untag_mutex);
+				return -EIO;
+			}
+
 			if (session_info && info &&
 				(session_info->pid != info->pid))
 				continue;
@@ -303,6 +342,8 @@ drop_data:
 	if (drain_again)
 		chk_logging_wakeup();
 
+	mutex_unlock(&driver->diagfwd_untag_mutex);
+
 	return err;
 }
 
@@ -322,7 +363,8 @@ int diag_md_close_peripheral(int id, uint8_t peripheral)
 	spin_lock_irqsave(&ch->lock, flags);
 	for (i = 0; i < ch->num_tbl_entries && !found; i++) {
 		entry = &ch->tbl[i];
-		if (GET_BUF_PERIPHERAL(entry->ctx) != peripheral)
+		if ((GET_BUF_PERIPHERAL(entry->ctx) != peripheral) ||
+			(GET_PD_CTXT(entry->ctx) != peripheral))
 			continue;
 		found = 1;
 		if (ch->ops && ch->ops->write_done) {
diff --git a/drivers/char/diag/diag_mux.c b/drivers/char/diag/diag_mux.c
index 6586f5e0cf86..55c5de1ea9fc 100644
--- a/drivers/char/diag/diag_mux.c
+++ b/drivers/char/diag/diag_mux.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2014-2016, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2014-2017, 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
@@ -133,21 +133,43 @@ int diag_mux_queue_read(int proc)
 int diag_mux_write(int proc, unsigned char *buf, int len, int ctx)
 {
 	struct diag_logger_t *logger = NULL;
-	int peripheral;
+	int peripheral, upd;
 
 	if (proc < 0 || proc >= NUM_MUX_PROC)
 		return -EINVAL;
 	if (!diag_mux)
 		return -EIO;
 
-	peripheral = GET_BUF_PERIPHERAL(ctx);
-	if (peripheral > NUM_PERIPHERALS)
-		return -EINVAL;
-
-	if (MD_PERIPHERAL_MASK(peripheral) & diag_mux->mux_mask)
-		logger = diag_mux->md_ptr;
-	else
-		logger = diag_mux->usb_ptr;
+	upd = GET_PD_CTXT(ctx);
+	if (upd) {
+		switch (upd) {
+		case DIAG_ID_MPSS:
+			upd = PERIPHERAL_MODEM;
+			break;
+		case UPD_WLAN:
+			break;
+		default:
+			pr_err("diag: invalid pd ctxt= %d\n", upd);
+			return -EINVAL;
+		}
+		if (((MD_PERIPHERAL_MASK(upd)) &
+			(diag_mux->mux_mask)) &&
+			driver->md_session_map[upd])
+			logger = diag_mux->md_ptr;
+		else
+			logger = diag_mux->usb_ptr;
+	} else {
+
+		peripheral = GET_BUF_PERIPHERAL(ctx);
+		if (peripheral > NUM_PERIPHERALS)
+			return -EINVAL;
+
+		if (MD_PERIPHERAL_MASK(peripheral) &
+			diag_mux->mux_mask)
+			logger = diag_mux->md_ptr;
+		else
+			logger = diag_mux->usb_ptr;
+	}
 
 	if (logger && logger->log_ops && logger->log_ops->write)
 		return logger->log_ops->write(proc, buf, len, ctx);
@@ -159,9 +181,17 @@ int diag_mux_close_peripheral(int proc, uint8_t peripheral)
 	struct diag_logger_t *logger = NULL;
 	if (proc < 0 || proc >= NUM_MUX_PROC)
 		return -EINVAL;
+
 	/* Peripheral should account for Apps data as well */
-	if (peripheral > NUM_PERIPHERALS)
-		return -EINVAL;
+	if (peripheral > NUM_PERIPHERALS) {
+		if (driver->num_pd_session) {
+			if (peripheral > NUM_MD_SESSIONS)
+				return -EINVAL;
+		} else {
+			return -EINVAL;
+		}
+	}
+
 	if (!diag_mux)
 		return -EIO;
 
@@ -182,7 +212,8 @@ int diag_mux_switch_logging(int *req_mode, int *peripheral_mask)
 	if (!req_mode)
 		return -EINVAL;
 
-	if (*peripheral_mask <= 0 || *peripheral_mask > DIAG_CON_ALL) {
+	if (*peripheral_mask <= 0 ||
+		(*peripheral_mask > (DIAG_CON_ALL | DIAG_CON_UPD_ALL))) {
 		pr_err("diag: mask %d in %s\n", *peripheral_mask, __func__);
 		return -EINVAL;
 	}
diff --git a/drivers/char/diag/diagchar.h b/drivers/char/diag/diagchar.h
index 9d235b7abc58..4a0e70b9c935 100644
--- a/drivers/char/diag/diagchar.h
+++ b/drivers/char/diag/diagchar.h
@@ -64,14 +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_CDSP (0x0040)
+#define DIAG_CON_WDSP		(0x0020)	/* Bit mask for WDSP */
+#define DIAG_CON_CDSP		(0x0040)	/* Bit mask for CDSP */
+
+#define DIAG_CON_UPD_WLAN		(0x1000) /*Bit mask for WLAN PD*/
+#define DIAG_CON_UPD_AUDIO		(0x2000) /*Bit mask for AUDIO PD*/
+#define DIAG_CON_UPD_SENSORS	(0x4000) /*Bit mask for SENSORS PD*/
 
 #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_WDSP \
 				| DIAG_CON_CDSP)
+#define DIAG_CON_UPD_ALL	(DIAG_CON_UPD_WLAN)
 
 #define DIAG_STM_MODEM	0x01
 #define DIAG_STM_LPASS	0x02
@@ -165,7 +170,7 @@
 #define PKT_ALLOC	1
 #define PKT_RESET	2
 
-#define FEATURE_MASK_LEN	2
+#define FEATURE_MASK_LEN	4
 
 #define DIAG_MD_NONE			0
 #define DIAG_MD_PERIPHERAL		1
@@ -209,8 +214,18 @@
 #define NUM_PERIPHERALS		6
 #define APPS_DATA		(NUM_PERIPHERALS)
 
+#define UPD_WLAN		7
+#define UPD_AUDIO		8
+#define UPD_SENSORS		9
+#define NUM_UPD			3
+
+#define DIAG_ID_APPS		1
+#define DIAG_ID_MPSS		2
+#define DIAG_ID_WLAN		3
+
 /* Number of sessions possible in Memory Device Mode. +1 for Apps data */
-#define NUM_MD_SESSIONS		(NUM_PERIPHERALS + 1)
+#define NUM_MD_SESSIONS		(NUM_PERIPHERALS \
+					+ NUM_UPD + 1)
 
 #define MD_PERIPHERAL_MASK(x)	(1 << x)
 
@@ -407,6 +422,7 @@ struct diag_partial_pkt_t {
 struct diag_logging_mode_param_t {
 	uint32_t req_mode;
 	uint32_t peripheral_mask;
+	uint32_t pd_mask;
 	uint8_t mode_param;
 } __packed;
 
@@ -453,6 +469,7 @@ struct diag_feature_t {
 	uint8_t log_on_demand;
 	uint8_t separate_cmd_rsp;
 	uint8_t encode_hdlc;
+	uint8_t untag_header;
 	uint8_t peripheral_buffering;
 	uint8_t mask_centralization;
 	uint8_t stm_support;
@@ -484,6 +501,7 @@ struct diagchar_dev {
 	int use_device_tree;
 	int supports_separate_cmdrsp;
 	int supports_apps_hdlc_encoding;
+	int supports_apps_header_untagging;
 	int supports_sockets;
 	/* The state requested in the STM command */
 	int stm_state_requested[NUM_STM_PROCESSORS];
@@ -515,6 +533,7 @@ struct diagchar_dev {
 	struct mutex cmd_reg_mutex;
 	uint32_t cmd_reg_count;
 	struct mutex diagfwd_channel_mutex[NUM_PERIPHERALS];
+	struct mutex diagfwd_untag_mutex;
 	/* Sizes that reflect memory pool sizes */
 	unsigned int poolsize;
 	unsigned int poolsize_hdlc;
@@ -577,6 +596,10 @@ struct diagchar_dev {
 	int in_busy_dcipktdata;
 	int logging_mode;
 	int logging_mask;
+	int pd_logging_mode;
+	int num_pd_session;
+	int cpd_len_1;
+	int cpd_len_2;
 	int mask_check;
 	uint32_t md_session_mask;
 	uint8_t md_session_mode;
diff --git a/drivers/char/diag/diagchar_core.c b/drivers/char/diag/diagchar_core.c
index 335064352789..22fac88e8132 100644
--- a/drivers/char/diag/diagchar_core.c
+++ b/drivers/char/diag/diagchar_core.c
@@ -395,7 +395,8 @@ static uint32_t diag_translate_kernel_to_user_mask(uint32_t peripheral_mask)
 		ret |= DIAG_CON_WDSP;
 	if (peripheral_mask & MD_PERIPHERAL_MASK(PERIPHERAL_CDSP))
 		ret |= DIAG_CON_CDSP;
-
+	if (peripheral_mask & MD_PERIPHERAL_MASK(UPD_WLAN))
+		ret |= DIAG_CON_UPD_WLAN;
 	return ret;
 }
 int diag_mask_param(void)
@@ -453,6 +454,14 @@ static void diag_close_logging_process(const int pid)
 	params.mode_param = 0;
 	params.peripheral_mask =
 		diag_translate_kernel_to_user_mask(session_peripheral_mask);
+	if (driver->pd_logging_mode)
+		params.pd_mask =
+		diag_translate_kernel_to_user_mask(session_peripheral_mask);
+
+	if (session_peripheral_mask & MD_PERIPHERAL_MASK(UPD_WLAN)) {
+		driver->pd_logging_mode--;
+		driver->num_pd_session--;
+	}
 	mutex_lock(&driver->diagchar_mutex);
 	diag_switch_logging(&params);
 	mutex_unlock(&driver->diagchar_mutex);
@@ -1548,6 +1557,8 @@ static uint32_t diag_translate_mask(uint32_t peripheral_mask)
 		ret |= (1 << PERIPHERAL_WDSP);
 	if (peripheral_mask & DIAG_CON_CDSP)
 		ret |= (1 << PERIPHERAL_CDSP);
+	if (peripheral_mask & DIAG_CON_UPD_WLAN)
+		ret |= (1 << UPD_WLAN);
 
 	return ret;
 }
@@ -1569,8 +1580,28 @@ static int diag_switch_logging(struct diag_logging_mode_param_t *param)
 		return -EINVAL;
 	}
 
-	peripheral_mask = diag_translate_mask(param->peripheral_mask);
-	param->peripheral_mask = peripheral_mask;
+	switch (param->pd_mask) {
+	case DIAG_CON_UPD_WLAN:
+		if (driver->md_session_map[PERIPHERAL_MODEM] &&
+			(MD_PERIPHERAL_MASK(PERIPHERAL_MODEM) &
+			diag_mux->mux_mask)) {
+			DIAG_LOG(DIAG_DEBUG_USERSPACE,
+			"diag_fr: User PD is already logging onto active peripheral logging\n");
+			return -EINVAL;
+		}
+		peripheral_mask =
+			diag_translate_mask(param->pd_mask);
+		param->peripheral_mask = peripheral_mask;
+		driver->pd_logging_mode++;
+		driver->num_pd_session++;
+		break;
+
+	default:
+		peripheral_mask =
+			diag_translate_mask(param->peripheral_mask);
+		param->peripheral_mask = peripheral_mask;
+		break;
+	}
 
 	switch (param->req_mode) {
 	case CALLBACK_MODE:
@@ -1590,7 +1621,7 @@ static int diag_switch_logging(struct diag_logging_mode_param_t *param)
 
 	curr_mode = driver->logging_mode;
 	DIAG_LOG(DIAG_DEBUG_USERSPACE,
-		"request to switch logging from %d mask:%0x to %d mask:%0x\n",
+		"request to switch logging from %d mask:%0x to new_mode %d mask:%0x\n",
 		curr_mode, driver->md_session_mask, new_mode, peripheral_mask);
 
 	err = diag_md_session_check(curr_mode, new_mode, param, &do_switch);
@@ -1910,6 +1941,27 @@ static int diag_ioctl_hdlc_toggle(unsigned long ioarg)
 	return 0;
 }
 
+static int diag_ioctl_query_pd_logging(unsigned long ioarg)
+{
+	int ret = -EINVAL;
+
+	DIAG_LOG(DIAG_DEBUG_USERSPACE,
+	"diag: %s: Untagging support on APPS is %s\n", __func__,
+	((driver->supports_apps_header_untagging) ?
+	"present" : "absent"));
+
+	DIAG_LOG(DIAG_DEBUG_USERSPACE,
+	"diag: %s: Tagging support on MODEM is %s\n", __func__,
+	(driver->feature[PERIPHERAL_MODEM].untag_header ?
+	"present" : "absent"));
+
+	if (driver->supports_apps_header_untagging &&
+		driver->feature[PERIPHERAL_MODEM].untag_header)
+		ret = 0;
+
+	return ret;
+}
+
 static int diag_ioctl_register_callback(unsigned long ioarg)
 {
 	int err = 0;
@@ -2149,6 +2201,9 @@ long diagchar_compat_ioctl(struct file *filp,
 	case DIAG_IOCTL_HDLC_TOGGLE:
 		result = diag_ioctl_hdlc_toggle(ioarg);
 		break;
+	case DIAG_IOCTL_QUERY_PD_LOGGING:
+		result = diag_ioctl_query_pd_logging(ioarg);
+		break;
 	}
 	return result;
 }
@@ -2272,6 +2327,9 @@ long diagchar_ioctl(struct file *filp,
 	case DIAG_IOCTL_HDLC_TOGGLE:
 		result = diag_ioctl_hdlc_toggle(ioarg);
 		break;
+	case DIAG_IOCTL_QUERY_PD_LOGGING:
+		result = diag_ioctl_query_pd_logging(ioarg);
+		break;
 	}
 	return result;
 }
@@ -3275,7 +3333,7 @@ static void diag_debug_init(void)
 	 * to be logged to IPC
 	 */
 	diag_debug_mask = DIAG_DEBUG_PERIPHERALS | DIAG_DEBUG_DCI |
-				DIAG_DEBUG_BRIDGE;
+				DIAG_DEBUG_USERSPACE | DIAG_DEBUG_BRIDGE;
 }
 #else
 static void diag_debug_init(void)
@@ -3404,6 +3462,8 @@ static int __init diagchar_init(void)
 			poolsize_usb_apps + 1 + (NUM_PERIPHERALS * 6));
 	driver->num_clients = max_clients;
 	driver->logging_mode = DIAG_USB_MODE;
+	driver->pd_logging_mode = 0;
+	driver->num_pd_session = 0;
 	driver->mask_check = 0;
 	driver->in_busy_pktdata = 0;
 	driver->in_busy_dcipktdata = 0;
@@ -3421,6 +3481,7 @@ static int __init diagchar_init(void)
 	mutex_init(&apps_data_mutex);
 	for (i = 0; i < NUM_PERIPHERALS; i++)
 		mutex_init(&driver->diagfwd_channel_mutex[i]);
+	mutex_init(&driver->diagfwd_untag_mutex);
 	init_waitqueue_head(&driver->wait_q);
 	INIT_WORK(&(driver->diag_drain_work), diag_drain_work_fn);
 	INIT_WORK(&(driver->update_user_clients),
diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c
index 99a16dd47cd4..532d2b149317 100644
--- a/drivers/char/diag/diagfwd.c
+++ b/drivers/char/diag/diagfwd.c
@@ -1587,6 +1587,7 @@ int diagfwd_init(void)
 		driver->real_time_mode[i] = 1;
 	driver->supports_separate_cmdrsp = 1;
 	driver->supports_apps_hdlc_encoding = 1;
+	driver->supports_apps_header_untagging = 1;
 	mutex_init(&driver->diag_hdlc_mutex);
 	mutex_init(&driver->diag_cntl_mutex);
 	mutex_init(&driver->mode_lock);
@@ -1616,6 +1617,8 @@ int diagfwd_init(void)
 		driver->feature[i].rcvd_feature_mask = 0;
 		driver->feature[i].peripheral_buffering = 0;
 		driver->feature[i].encode_hdlc = 0;
+		driver->feature[i].untag_header =
+			DISABLE_PKT_HEADER_UNTAGGING;
 		driver->feature[i].mask_centralization = 0;
 		driver->feature[i].log_on_demand = 0;
 		driver->feature[i].sent_feature_mask = 0;
diff --git a/drivers/char/diag/diagfwd.h b/drivers/char/diag/diagfwd.h
index 4c6d86fc36ae..97ad3f60ba5e 100644
--- a/drivers/char/diag/diagfwd.h
+++ b/drivers/char/diag/diagfwd.h
@@ -19,9 +19,11 @@
  */
 #define SET_BUF_CTXT(p, d, n) \
 	(((p & 0xFF) << 16) | ((d & 0xFF) << 8) | (n & 0xFF))
+#define SET_PD_CTXT(u)		((u & 0xFF) << 24)
 #define GET_BUF_PERIPHERAL(p)	((p & 0xFF0000) >> 16)
 #define GET_BUF_TYPE(d)		((d & 0x00FF00) >> 8)
 #define GET_BUF_NUM(n)		((n & 0x0000FF))
+#define GET_PD_CTXT(u)		((u & 0xFF000000) >> 24)
 
 #define CHK_OVERFLOW(bufStart, start, end, length) \
 	((((bufStart) <= (start)) && ((end) - (start) >= (length))) ? 1 : 0)
diff --git a/drivers/char/diag/diagfwd_cntl.c b/drivers/char/diag/diagfwd_cntl.c
index 62c8d0028af9..66c794c94d92 100644
--- a/drivers/char/diag/diagfwd_cntl.c
+++ b/drivers/char/diag/diagfwd_cntl.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2011-2016, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2011-2017, 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
@@ -198,6 +198,20 @@ static void process_hdlc_encoding_feature(uint8_t peripheral)
 	}
 }
 
+static void process_upd_header_untagging_feature(uint8_t peripheral)
+{
+	if (peripheral >= NUM_PERIPHERALS)
+		return;
+
+	if (driver->supports_apps_header_untagging) {
+		driver->feature[peripheral].untag_header =
+					ENABLE_PKT_HEADER_UNTAGGING;
+	} else {
+		driver->feature[peripheral].untag_header =
+					DISABLE_PKT_HEADER_UNTAGGING;
+	}
+}
+
 static void process_command_deregistration(uint8_t *buf, uint32_t len,
 					   uint8_t peripheral)
 {
@@ -374,6 +388,8 @@ static void process_incoming_feature_mask(uint8_t *buf, uint32_t len,
 			driver->feature[peripheral].separate_cmd_rsp = 1;
 		if (FEATURE_SUPPORTED(F_DIAG_APPS_HDLC_ENCODE))
 			process_hdlc_encoding_feature(peripheral);
+		if (FEATURE_SUPPORTED(F_DIAG_PKT_HEADER_UNTAG))
+			process_upd_header_untagging_feature(peripheral);
 		if (FEATURE_SUPPORTED(F_DIAG_STM))
 			enable_stm_feature(peripheral);
 		if (FEATURE_SUPPORTED(F_DIAG_MASK_CENTRALIZATION))
diff --git a/drivers/char/diag/diagfwd_cntl.h b/drivers/char/diag/diagfwd_cntl.h
index 7eed8ef8779e..e8608f47ff14 100644
--- a/drivers/char/diag/diagfwd_cntl.h
+++ b/drivers/char/diag/diagfwd_cntl.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2011-2016, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2011-2017, 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
@@ -67,6 +67,7 @@
 #define F_DIAG_MASK_CENTRALIZATION		11
 #define F_DIAG_SOCKETS_ENABLED			13
 #define F_DIAG_DCI_EXTENDED_HEADER_SUPPORT	14
+#define F_DIAG_PKT_HEADER_UNTAG			16
 
 #define ENABLE_SEPARATE_CMDRSP	1
 #define DISABLE_SEPARATE_CMDRSP	0
@@ -81,6 +82,9 @@
 #define ENABLE_APPS_HDLC_ENCODING	1
 #define DISABLE_APPS_HDLC_ENCODING	0
 
+#define ENABLE_PKT_HEADER_UNTAGGING		1
+#define DISABLE_PKT_HEADER_UNTAGGING	0
+
 #define DIAG_MODE_PKT_LEN	36
 
 struct diag_ctrl_pkt_header_t {
diff --git a/drivers/char/diag/diagfwd_peripheral.c b/drivers/char/diag/diagfwd_peripheral.c
index 3f9f7a999fdf..5779173de4b9 100644
--- a/drivers/char/diag/diagfwd_peripheral.c
+++ b/drivers/char/diag/diagfwd_peripheral.c
@@ -46,6 +46,8 @@ static void diagfwd_cntl_open(struct diagfwd_info *fwd_info);
 static void diagfwd_cntl_close(struct diagfwd_info *fwd_info);
 static void diagfwd_dci_open(struct diagfwd_info *fwd_info);
 static void diagfwd_dci_close(struct diagfwd_info *fwd_info);
+static void diagfwd_data_read_untag_done(struct diagfwd_info *fwd_info,
+				   unsigned char *buf, int len);
 static void diagfwd_data_read_done(struct diagfwd_info *fwd_info,
 				   unsigned char *buf, int len);
 static void diagfwd_cntl_read_done(struct diagfwd_info *fwd_info,
@@ -59,7 +61,7 @@ struct diagfwd_info peripheral_info[NUM_TYPES][NUM_PERIPHERALS];
 static struct diag_channel_ops data_ch_ops = {
 	.open = NULL,
 	.close = NULL,
-	.read_done = diagfwd_data_read_done
+	.read_done = diagfwd_data_read_untag_done
 };
 
 static struct diag_channel_ops cntl_ch_ops = {
@@ -214,6 +216,221 @@ static int check_bufsize_for_encoding(struct diagfwd_buf_t *buf, uint32_t len)
 	return buf->len;
 }
 
+static void diagfwd_data_process_done(struct diagfwd_info *fwd_info,
+				   struct diagfwd_buf_t *buf, int len)
+{
+	int err = 0;
+	int write_len = 0, peripheral = 0;
+	unsigned char *write_buf = NULL;
+	struct diag_md_session_t *session_info = NULL;
+	uint8_t hdlc_disabled = 0;
+
+	if (!fwd_info || !buf || len <= 0) {
+		diag_ws_release();
+		return;
+	}
+
+	switch (fwd_info->type) {
+	case TYPE_DATA:
+	case TYPE_CMD:
+		break;
+	default:
+		pr_err_ratelimited("diag: In %s, invalid type %d for peripheral %d\n",
+			__func__, fwd_info->type,
+			fwd_info->peripheral);
+		diag_ws_release();
+		return;
+	}
+
+	mutex_lock(&driver->hdlc_disable_mutex);
+	mutex_lock(&fwd_info->data_mutex);
+	peripheral = GET_PD_CTXT(buf->ctxt);
+	if (peripheral == DIAG_ID_MPSS)
+		peripheral = PERIPHERAL_MODEM;
+
+	session_info =
+		diag_md_session_get_peripheral(peripheral);
+	if (session_info)
+		hdlc_disabled = session_info->hdlc_disabled;
+	else
+		hdlc_disabled = driver->hdlc_disabled;
+
+	if (hdlc_disabled) {
+		/* The data is raw and and on APPS side HDLC is disabled */
+		if (!buf) {
+			pr_err("diag: In %s, no match for non encode buffer %pK, peripheral %d, type: %d\n",
+			       __func__, buf, fwd_info->peripheral,
+			       fwd_info->type);
+			goto end;
+		}
+		if (len > PERIPHERAL_BUF_SZ) {
+			pr_err("diag: In %s, Incoming buffer too large %d, peripheral %d, type: %d\n",
+			       __func__, len, fwd_info->peripheral,
+			       fwd_info->type);
+			goto end;
+		}
+		write_len = len;
+		if (write_len <= 0)
+			goto end;
+		write_buf = buf->data_raw;
+	} else {
+		if (!buf) {
+			pr_err("diag: In %s, no match for non encode buffer %pK, peripheral %d, type: %d\n",
+				__func__, buf, fwd_info->peripheral,
+				fwd_info->type);
+			goto end;
+		}
+
+		write_len = check_bufsize_for_encoding(buf, len);
+		if (write_len <= 0) {
+			pr_err("diag: error in checking buf for encoding\n");
+			goto end;
+		}
+		write_buf = buf->data;
+		err = diag_add_hdlc_encoding(write_buf, &write_len,
+			buf->data_raw, len);
+		if (err) {
+			pr_err("diag: error in adding hdlc encoding\n");
+			goto end;
+		}
+	}
+
+	if (write_len > 0) {
+		err = diag_mux_write(DIAG_LOCAL_PROC, write_buf, write_len,
+				     buf->ctxt);
+		if (err) {
+			pr_err_ratelimited("diag: In %s, unable to write to mux error: %d\n",
+					   __func__, err);
+			goto end;
+		}
+	}
+	mutex_unlock(&fwd_info->data_mutex);
+	mutex_unlock(&driver->hdlc_disable_mutex);
+	diagfwd_queue_read(fwd_info);
+	return;
+
+end:
+	diag_ws_release();
+	mutex_unlock(&fwd_info->data_mutex);
+	mutex_unlock(&driver->hdlc_disable_mutex);
+	if (buf) {
+		diagfwd_write_done(fwd_info->peripheral, fwd_info->type,
+				   GET_BUF_NUM(buf->ctxt));
+	}
+	diagfwd_queue_read(fwd_info);
+}
+
+static void diagfwd_data_read_untag_done(struct diagfwd_info *fwd_info,
+				   unsigned char *buf, int len)
+{
+	int len_cpd = 0, len_upd_1 = 0;
+	int ctxt_cpd = 0, ctxt_upd_1 = 0;
+	int buf_len = 0, processed = 0;
+	unsigned char *temp_buf_main = NULL;
+	unsigned char *temp_buf_cpd = NULL;
+	unsigned char *temp_buf_upd_1 = NULL;
+	struct diagfwd_buf_t *temp_ptr_upd = NULL;
+	struct diagfwd_buf_t *temp_ptr_cpd = NULL;
+	int flag_buf_1 = 0, flag_buf_2 = 0;
+
+	if (!fwd_info || !buf || len <= 0) {
+		diag_ws_release();
+		return;
+	}
+
+	switch (fwd_info->type) {
+	case TYPE_DATA:
+	case TYPE_CMD:
+		break;
+	default:
+		pr_err_ratelimited("diag: In %s, invalid type %d for peripheral %d\n",
+				   __func__, fwd_info->type,
+				   fwd_info->peripheral);
+		diag_ws_release();
+		return;
+	}
+
+	if (driver->feature[fwd_info->peripheral].encode_hdlc &&
+		driver->feature[fwd_info->peripheral].untag_header) {
+		mutex_lock(&driver->diagfwd_untag_mutex);
+		temp_buf_cpd = buf;
+		temp_buf_main = buf;
+		if (fwd_info->buf_1 &&
+			fwd_info->buf_1->data_raw == buf) {
+			flag_buf_1 = 1;
+			if (fwd_info->type == TYPE_DATA)
+				temp_buf_upd_1 =
+				fwd_info->buf_upd_1_a->data_raw;
+		} else {
+			flag_buf_2 = 1;
+			if (fwd_info->type == TYPE_DATA)
+				temp_buf_upd_1 =
+				fwd_info->buf_upd_1_b->data_raw;
+		}
+		while (processed < len) {
+			buf_len =
+				*(uint16_t *) (temp_buf_main + 2);
+			switch ((*temp_buf_main)) {
+			case DIAG_ID_MPSS:
+				ctxt_cpd = DIAG_ID_MPSS;
+				len_cpd += buf_len;
+				if (temp_buf_cpd) {
+					memcpy(temp_buf_cpd,
+					(temp_buf_main + 4), buf_len);
+					temp_buf_cpd += buf_len;
+				}
+				break;
+			case DIAG_ID_WLAN:
+				ctxt_upd_1 = UPD_WLAN;
+				len_upd_1 += buf_len;
+				if (temp_buf_upd_1) {
+					memcpy(temp_buf_upd_1,
+					(temp_buf_main + 4), buf_len);
+					temp_buf_upd_1 += buf_len;
+				}
+				break;
+			}
+			len = len - 4;
+			temp_buf_main += (buf_len + 4);
+			processed += buf_len;
+		}
+		if (fwd_info->type == TYPE_DATA && len_upd_1) {
+			if (flag_buf_1)
+				temp_ptr_upd = fwd_info->buf_upd_1_a;
+			else
+				temp_ptr_upd = fwd_info->buf_upd_1_b;
+			temp_ptr_upd->ctxt &= 0x00FFFFFF;
+			temp_ptr_upd->ctxt |=
+				(SET_PD_CTXT(ctxt_upd_1));
+				atomic_set(&temp_ptr_upd->in_busy, 1);
+			diagfwd_data_process_done(fwd_info,
+				temp_ptr_upd, len_upd_1);
+		}
+		if (len_cpd) {
+			if (flag_buf_1) {
+				driver->cpd_len_1 = len_cpd;
+				temp_ptr_cpd = fwd_info->buf_1;
+			} else {
+				driver->cpd_len_2 = len_cpd;
+				temp_ptr_cpd = fwd_info->buf_2;
+			}
+			temp_ptr_cpd->ctxt &= 0x00FFFFFF;
+			temp_ptr_cpd->ctxt |=
+				(SET_PD_CTXT(ctxt_cpd));
+			diagfwd_data_process_done(fwd_info,
+				temp_ptr_cpd, len_cpd);
+		} else {
+			if (flag_buf_1)
+				driver->cpd_len_1 = 0;
+			if (flag_buf_2)
+				driver->cpd_len_2 = 0;
+		}
+		mutex_unlock(&driver->diagfwd_untag_mutex);
+	} else {
+		diagfwd_data_read_done(fwd_info, buf, len);
+	}
+}
+
 static void diagfwd_data_read_done(struct diagfwd_info *fwd_info,
 				   unsigned char *buf, int len)
 {
@@ -223,6 +440,7 @@ static void diagfwd_data_read_done(struct diagfwd_info *fwd_info,
 	struct diagfwd_buf_t *temp_buf = NULL;
 	struct diag_md_session_t *session_info = NULL;
 	uint8_t hdlc_disabled = 0;
+
 	if (!fwd_info || !buf || len <= 0) {
 		diag_ws_release();
 		return;
@@ -234,8 +452,8 @@ static void diagfwd_data_read_done(struct diagfwd_info *fwd_info,
 		break;
 	default:
 		pr_err_ratelimited("diag: In %s, invalid type %d for peripheral %d\n",
-				   __func__, fwd_info->type,
-				   fwd_info->peripheral);
+			__func__, fwd_info->type,
+			fwd_info->peripheral);
 		diag_ws_release();
 		return;
 	}
@@ -940,7 +1158,15 @@ void diagfwd_write_done(uint8_t peripheral, uint8_t type, int ctxt)
 		atomic_set(&fwd_info->buf_1->in_busy, 0);
 	else if (ctxt == 2 && fwd_info->buf_2)
 		atomic_set(&fwd_info->buf_2->in_busy, 0);
-	else
+	else if (ctxt == 3 && fwd_info->buf_upd_1_a) {
+		atomic_set(&fwd_info->buf_upd_1_a->in_busy, 0);
+		if (driver->cpd_len_1 == 0)
+			atomic_set(&fwd_info->buf_1->in_busy, 0);
+	} else if (ctxt == 4 && fwd_info->buf_upd_1_b) {
+		atomic_set(&fwd_info->buf_upd_1_b->in_busy, 0);
+		if (driver->cpd_len_2 == 0)
+			atomic_set(&fwd_info->buf_2->in_busy, 0);
+	} else
 		pr_err("diag: In %s, invalid ctxt %d\n", __func__, ctxt);
 
 	diagfwd_queue_read(fwd_info);
@@ -1072,6 +1298,7 @@ static void diagfwd_queue_read(struct diagfwd_info *fwd_info)
 
 void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
 {
+	unsigned char *temp_buf;
 
 	if (!fwd_info)
 		return;
@@ -1124,6 +1351,54 @@ void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
 							fwd_info->type, 2);
 		}
 
+		if (driver->feature[fwd_info->peripheral].untag_header)	{
+			if (!fwd_info->buf_upd_1_a) {
+				fwd_info->buf_upd_1_a =
+					kzalloc(sizeof(struct diagfwd_buf_t),
+						      GFP_KERNEL);
+				if (!fwd_info->buf_upd_1_a)
+					goto err;
+				kmemleak_not_leak(fwd_info->buf_upd_1_a);
+			}
+
+			if (!fwd_info->buf_upd_1_a->data) {
+				fwd_info->buf_upd_1_a->data =
+					kzalloc(PERIPHERAL_BUF_SZ +
+						APF_DIAG_PADDING,
+					    GFP_KERNEL);
+				if (!fwd_info->buf_upd_1_a->data)
+					goto err;
+				fwd_info->buf_upd_1_a->len = PERIPHERAL_BUF_SZ;
+				kmemleak_not_leak(fwd_info->buf_upd_1_a->data);
+				fwd_info->buf_upd_1_a->ctxt = SET_BUF_CTXT(
+					fwd_info->peripheral,
+					fwd_info->type, 3);
+			}
+			if (!fwd_info->buf_upd_1_b) {
+			fwd_info->buf_upd_1_b =
+				kzalloc(sizeof(struct diagfwd_buf_t),
+					      GFP_KERNEL);
+			if (!fwd_info->buf_upd_1_b)
+				goto err;
+			kmemleak_not_leak(fwd_info->buf_upd_1_b);
+			}
+
+			if (!fwd_info->buf_upd_1_b->data) {
+				fwd_info->buf_upd_1_b->data =
+					kzalloc(PERIPHERAL_BUF_SZ +
+						APF_DIAG_PADDING,
+						GFP_KERNEL);
+				if (!fwd_info->buf_upd_1_b->data)
+					goto err;
+				fwd_info->buf_upd_1_b->len =
+					PERIPHERAL_BUF_SZ;
+				kmemleak_not_leak(fwd_info->buf_upd_1_b->data);
+				fwd_info->buf_upd_1_b->ctxt = SET_BUF_CTXT(
+					fwd_info->peripheral,
+					fwd_info->type, 4);
+			}
+		}
+
 		if (driver->supports_apps_hdlc_encoding) {
 			/* In support of hdlc encoding */
 			if (!fwd_info->buf_1->data_raw) {
@@ -1133,7 +1408,8 @@ void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
 						GFP_KERNEL);
 				if (!fwd_info->buf_1->data_raw)
 					goto err;
-				fwd_info->buf_1->len_raw = PERIPHERAL_BUF_SZ;
+				fwd_info->buf_1->len_raw =
+					PERIPHERAL_BUF_SZ;
 				kmemleak_not_leak(fwd_info->buf_1->data_raw);
 			}
 			if (!fwd_info->buf_2->data_raw) {
@@ -1143,13 +1419,45 @@ void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
 						GFP_KERNEL);
 				if (!fwd_info->buf_2->data_raw)
 					goto err;
-				fwd_info->buf_2->len_raw = PERIPHERAL_BUF_SZ;
+				fwd_info->buf_2->len_raw =
+					PERIPHERAL_BUF_SZ;
 				kmemleak_not_leak(fwd_info->buf_2->data_raw);
 			}
+
+			if (driver->feature[fwd_info->peripheral].
+					untag_header) {
+				if (!fwd_info->buf_upd_1_a->data_raw) {
+					fwd_info->buf_upd_1_a->data_raw =
+						kzalloc(PERIPHERAL_BUF_SZ +
+							APF_DIAG_PADDING,
+							GFP_KERNEL);
+					if (!fwd_info->buf_upd_1_a->data_raw)
+						goto err;
+					fwd_info->buf_upd_1_a->len_raw =
+						PERIPHERAL_BUF_SZ;
+					temp_buf =
+						fwd_info->buf_upd_1_a->data_raw;
+					kmemleak_not_leak(temp_buf);
+				}
+				if (!fwd_info->buf_upd_1_b->data_raw) {
+					fwd_info->buf_upd_1_b->data_raw =
+						kzalloc(PERIPHERAL_BUF_SZ +
+							APF_DIAG_PADDING,
+							GFP_KERNEL);
+					if (!fwd_info->buf_upd_1_b->data_raw)
+						goto err;
+					fwd_info->buf_upd_1_b->len_raw =
+						PERIPHERAL_BUF_SZ;
+					temp_buf =
+						fwd_info->buf_upd_1_b->data_raw;
+					kmemleak_not_leak(temp_buf);
+				}
+			}
 		}
 	}
 
-	if (fwd_info->type == TYPE_CMD && driver->supports_apps_hdlc_encoding) {
+	if (fwd_info->type == TYPE_CMD &&
+		driver->supports_apps_hdlc_encoding) {
 		/* In support of hdlc encoding */
 		if (!fwd_info->buf_1->data_raw) {
 			fwd_info->buf_1->data_raw = kzalloc(PERIPHERAL_BUF_SZ +
diff --git a/drivers/char/diag/diagfwd_peripheral.h b/drivers/char/diag/diagfwd_peripheral.h
index 23aa526b2c09..f483da81cc96 100644
--- a/drivers/char/diag/diagfwd_peripheral.h
+++ b/drivers/char/diag/diagfwd_peripheral.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015-2016, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015-2017, 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
@@ -78,6 +78,8 @@ struct diagfwd_info {
 	void *ctxt;
 	struct diagfwd_buf_t *buf_1;
 	struct diagfwd_buf_t *buf_2;
+	struct diagfwd_buf_t *buf_upd_1_a;
+	struct diagfwd_buf_t *buf_upd_1_b;
 	struct diagfwd_buf_t *buf_ptr[NUM_WRITE_BUFFERS];
 	struct diag_peripheral_ops *p_ops;
 	struct diag_channel_ops *c_ops;
diff --git a/include/linux/diagchar.h b/include/linux/diagchar.h
index 0ae23ddbc528..7ca9b876f309 100644
--- a/include/linux/diagchar.h
+++ b/include/linux/diagchar.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2008-2016, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2008-2017, 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
@@ -66,6 +66,7 @@
 #define DIAG_IOCTL_PERIPHERAL_BUF_DRAIN		36
 #define DIAG_IOCTL_REGISTER_CALLBACK	37
 #define DIAG_IOCTL_HDLC_TOGGLE	38
+#define DIAG_IOCTL_QUERY_PD_LOGGING	39
 
 /* PC Tools IDs */
 #define APQ8060_TOOLS_ID	4062