Merge "diag: Add support for header untagging"

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

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);

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

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;
+	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 {
 
-	if (MD_PERIPHERAL_MASK(peripheral) & diag_mux->mux_mask)
-		logger = diag_mux->md_ptr;
-	else
-		logger = diag_mux->usb_ptr;
+		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;
 	}

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;
 
@@ -454,6 +470,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;
@@ -485,6 +502,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];
@@ -516,6 +534,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;
@@ -578,6 +597,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;

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);
@@ -1551,6 +1560,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;
 }
@@ -1572,8 +1583,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:
@@ -1593,7 +1624,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);
@@ -1914,6 +1945,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;
@@ -2153,6 +2205,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;
 }
@@ -2276,6 +2331,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;
 }
@@ -3287,7 +3345,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)
@@ -3416,6 +3474,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;
@@ -3433,6 +3493,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),

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;

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)

drivers/char/diag/diagfwd_cntl.c

@@ -200,6 +200,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)
 {
@@ -376,6 +390,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))

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 {

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,15 +216,123 @@ static int check_bufsize_for_encoding(struct diagfwd_buf_t *buf, uint32_t len)
 	return buf->len;
 }
 
-static void diagfwd_data_read_done(struct diagfwd_info *fwd_info,
-				   unsigned char *buf, int 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;
+	int write_len = 0, peripheral = 0;
 	unsigned char *write_buf = NULL;
-	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;
+	}
+
+	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;
@@ -240,6 +350,114 @@ static void diagfwd_data_read_done(struct diagfwd_info *fwd_info,
 		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)
+{
+	int err = 0;
+	int write_len = 0;
+	unsigned char *write_buf = NULL;
+	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;
+	}
+
+	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);
 	session_info = diag_md_session_get_peripheral(fwd_info->peripheral);
@@ -941,7 +1159,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);
@@ -1073,6 +1299,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;
@@ -1125,6 +1352,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) {
@@ -1134,7 +1409,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) {
@@ -1144,13 +1420,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 +

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;

include/linux/diagchar.h

@@ -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