Merge "diag: Add PD buffering mode configuration support"

6 files changed, 353 insertions, 106 deletions

diff --git a/drivers/char/diag/diag_debugfs.c b/drivers/char/diag/diag_debugfs.c
index ca7dd88048ac..62ef9acc1df3 100644
--- a/drivers/char/diag/diag_debugfs.c
+++ b/drivers/char/diag/diag_debugfs.c
@@ -80,7 +80,8 @@ static ssize_t diag_dbgfs_read_status(struct file *file, char __user *ubuf,
 		"Time Sync Enabled: %d\n"
 		"MD session mode: %d\n"
 		"MD session mask: %d\n"
-		"Uses Time API: %d\n",
+		"Uses Time API: %d\n"
+		"Supports PD buffering: %d\n",
 		chk_config_get_id(),
 		chk_polling_response(),
 		driver->polling_reg_flag,
@@ -95,11 +96,12 @@ static ssize_t diag_dbgfs_read_status(struct file *file, char __user *ubuf,
 		driver->time_sync_enabled,
 		driver->md_session_mode,
 		driver->md_session_mask,
-		driver->uses_time_api);
+		driver->uses_time_api,
+		driver->supports_pd_buffering);
 
 	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%c|\n",
+			"p: %s Feature: %02x %02x |%c%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],
@@ -108,6 +110,7 @@ static ssize_t diag_dbgfs_read_status(struct file *file, char __user *ubuf,
 			driver->feature[i].separate_cmd_rsp ? 'C':'c',
 			driver->feature[i].encode_hdlc ? 'H':'h',
 			driver->feature[i].mask_centralization ? 'M':'m',
+			driver->feature[i].pd_buffering ? 'P':'p',
 			driver->feature[i].stm_support ? 'Q':'q',
 			driver->feature[i].sockets_enabled ? 'S':'s',
 			driver->feature[i].sent_feature_mask ? 'T':'t',
diff --git a/drivers/char/diag/diagchar.h b/drivers/char/diag/diagchar.h
index 4047a2c42bb7..73296b573436 100644
--- a/drivers/char/diag/diagchar.h
+++ b/drivers/char/diag/diagchar.h
@@ -478,6 +478,7 @@ struct diag_feature_t {
 	uint8_t encode_hdlc;
 	uint8_t untag_header;
 	uint8_t peripheral_buffering;
+	uint8_t pd_buffering;
 	uint8_t mask_centralization;
 	uint8_t stm_support;
 	uint8_t sockets_enabled;
@@ -509,6 +510,7 @@ struct diagchar_dev {
 	int supports_separate_cmdrsp;
 	int supports_apps_hdlc_encoding;
 	int supports_apps_header_untagging;
+	int supports_pd_buffering;
 	int peripheral_untag[NUM_PERIPHERALS];
 	int supports_sockets;
 	/* The state requested in the STM command */
@@ -561,8 +563,8 @@ struct diagchar_dev {
 	struct diagfwd_info *diagfwd_cmd[NUM_PERIPHERALS];
 	struct diagfwd_info *diagfwd_dci_cmd[NUM_PERIPHERALS];
 	struct diag_feature_t feature[NUM_PERIPHERALS];
-	struct diag_buffering_mode_t buffering_mode[NUM_PERIPHERALS];
-	uint8_t buffering_flag[NUM_PERIPHERALS];
+	struct diag_buffering_mode_t buffering_mode[NUM_MD_SESSIONS];
+	uint8_t buffering_flag[NUM_MD_SESSIONS];
 	struct mutex mode_lock;
 	unsigned char *user_space_data_buf;
 	uint8_t user_space_data_busy;
diff --git a/drivers/char/diag/diagchar_core.c b/drivers/char/diag/diagchar_core.c
index 60bfb2af49d0..5c1094b48e92 100644
--- a/drivers/char/diag/diagchar_core.c
+++ b/drivers/char/diag/diagchar_core.c
@@ -1908,12 +1908,33 @@ static int diag_ioctl_get_real_time(unsigned long ioarg)
 static int diag_ioctl_set_buffering_mode(unsigned long ioarg)
 {
 	struct diag_buffering_mode_t params;
+	int peripheral = 0;
+	uint8_t diag_id = 0;
 
 	if (copy_from_user(&params, (void __user *)ioarg, sizeof(params)))
 		return -EFAULT;
 
-	if (params.peripheral >= NUM_PERIPHERALS)
-		return -EINVAL;
+	diag_map_pd_to_diagid(params.peripheral, &diag_id, &peripheral);
+
+	if ((peripheral < 0) ||
+		peripheral >= NUM_PERIPHERALS) {
+		pr_err("diag: In %s, invalid peripheral %d\n", __func__,
+		       peripheral);
+		return -EIO;
+	}
+
+	if (params.peripheral > NUM_PERIPHERALS &&
+		!driver->feature[peripheral].pd_buffering) {
+		pr_err("diag: In %s, pd buffering not supported for peripheral:%d\n",
+			__func__, peripheral);
+		return -EIO;
+	}
+
+	if (!driver->feature[peripheral].peripheral_buffering) {
+		pr_err("diag: In %s, peripheral %d doesn't support buffering\n",
+		       __func__, peripheral);
+		return -EIO;
+	}
 
 	mutex_lock(&driver->mode_lock);
 	driver->buffering_flag[params.peripheral] = 1;
@@ -1924,24 +1945,29 @@ static int diag_ioctl_set_buffering_mode(unsigned long ioarg)
 
 static int diag_ioctl_peripheral_drain_immediate(unsigned long ioarg)
 {
-	uint8_t peripheral;
+	uint8_t pd, diag_id = 0;
+	int peripheral = 0;
 
-	if (copy_from_user(&peripheral, (void __user *)ioarg, sizeof(uint8_t)))
+	if (copy_from_user(&pd, (void __user *)ioarg, sizeof(uint8_t)))
 		return -EFAULT;
 
-	if (peripheral >= NUM_PERIPHERALS) {
+	diag_map_pd_to_diagid(pd, &diag_id, &peripheral);
+
+	if ((peripheral < 0) ||
+		peripheral >= NUM_PERIPHERALS) {
 		pr_err("diag: In %s, invalid peripheral %d\n", __func__,
 		       peripheral);
 		return -EINVAL;
 	}
 
-	if (!driver->feature[peripheral].peripheral_buffering) {
-		pr_err("diag: In %s, peripheral %d doesn't support buffering\n",
-		       __func__, peripheral);
+	if (pd > NUM_PERIPHERALS &&
+		!driver->feature[peripheral].pd_buffering) {
+		pr_err("diag: In %s, pd buffering not supported for peripheral:%d\n",
+			__func__, peripheral);
 		return -EIO;
 	}
 
-	return diag_send_peripheral_drain_immediate(peripheral);
+	return diag_send_peripheral_drain_immediate(pd, diag_id, peripheral);
 }
 
 static int diag_ioctl_dci_support(unsigned long ioarg)
diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c
index 8fb724305c03..019bf1946ac3 100644
--- a/drivers/char/diag/diagfwd.c
+++ b/drivers/char/diag/diagfwd.c
@@ -1601,6 +1601,7 @@ int diagfwd_init(void)
 	driver->supports_separate_cmdrsp = 1;
 	driver->supports_apps_hdlc_encoding = 1;
 	driver->supports_apps_header_untagging = 1;
+	driver->supports_pd_buffering = 1;
 	for (i = 0; i < NUM_PERIPHERALS; i++)
 		driver->peripheral_untag[i] = 0;
 	mutex_init(&driver->diag_hdlc_mutex);
@@ -1631,12 +1632,16 @@ int diagfwd_init(void)
 		driver->feature[i].stm_support = DISABLE_STM;
 		driver->feature[i].rcvd_feature_mask = 0;
 		driver->feature[i].peripheral_buffering = 0;
+		driver->feature[i].pd_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;
+	}
+
+	for (i = 0; i < NUM_MD_SESSIONS; i++) {
 		driver->buffering_mode[i].peripheral = i;
 		driver->buffering_mode[i].mode = DIAG_BUFFERING_MODE_STREAMING;
 		driver->buffering_mode[i].high_wm_val = DEFAULT_HIGH_WM_VAL;
diff --git a/drivers/char/diag/diagfwd_cntl.c b/drivers/char/diag/diagfwd_cntl.c
index 729fbf4fc145..4ae2158b5a6b 100644
--- a/drivers/char/diag/diagfwd_cntl.c
+++ b/drivers/char/diag/diagfwd_cntl.c
@@ -418,6 +418,8 @@ static void process_incoming_feature_mask(uint8_t *buf, uint32_t len,
 			driver->feature[peripheral].mask_centralization = 1;
 		if (FEATURE_SUPPORTED(F_DIAG_PERIPHERAL_BUFFERING))
 			driver->feature[peripheral].peripheral_buffering = 1;
+		if (FEATURE_SUPPORTED(F_DIAG_PD_BUFFERING))
+			driver->feature[peripheral].pd_buffering = 1;
 		if (FEATURE_SUPPORTED(F_DIAG_SOCKETS_ENABLED))
 			enable_socket_feature(peripheral);
 	}
@@ -792,32 +794,54 @@ static int diag_compute_real_time(int idx)
 }
 
 static void diag_create_diag_mode_ctrl_pkt(unsigned char *dest_buf,
-					   int real_time)
+					   uint8_t diag_id, int real_time)
 {
 	struct diag_ctrl_msg_diagmode diagmode;
+	struct diag_ctrl_msg_diagmode_v2 diagmode_v2;
 	int msg_size = sizeof(struct diag_ctrl_msg_diagmode);
+	int msg_size_2 = sizeof(struct diag_ctrl_msg_diagmode_v2);
 
 	if (!dest_buf)
 		return;
 
-	diagmode.ctrl_pkt_id = DIAG_CTRL_MSG_DIAGMODE;
-	diagmode.ctrl_pkt_data_len = DIAG_MODE_PKT_LEN;
-	diagmode.version = 1;
-	diagmode.sleep_vote = real_time ? 1 : 0;
-	/*
-	 * 0 - Disables real-time logging (to prevent
-	 *     frequent APPS wake-ups, etc.).
-	 * 1 - Enable real-time logging
-	 */
-	diagmode.real_time = real_time;
-	diagmode.use_nrt_values = 0;
-	diagmode.commit_threshold = 0;
-	diagmode.sleep_threshold = 0;
-	diagmode.sleep_time = 0;
-	diagmode.drain_timer_val = 0;
-	diagmode.event_stale_timer_val = 0;
-
-	memcpy(dest_buf, &diagmode, msg_size);
+	if (diag_id) {
+		diagmode_v2.ctrl_pkt_id = DIAG_CTRL_MSG_DIAGMODE;
+		diagmode_v2.ctrl_pkt_data_len = DIAG_MODE_PKT_LEN_V2;
+		diagmode_v2.version = 2;
+		diagmode_v2.sleep_vote = real_time ? 1 : 0;
+		/*
+		 * 0 - Disables real-time logging (to prevent
+		 *	   frequent APPS wake-ups, etc.).
+		 * 1 - Enable real-time logging
+		 */
+		diagmode_v2.real_time = real_time;
+		diagmode_v2.use_nrt_values = 0;
+		diagmode_v2.commit_threshold = 0;
+		diagmode_v2.sleep_threshold = 0;
+		diagmode_v2.sleep_time = 0;
+		diagmode_v2.drain_timer_val = 0;
+		diagmode_v2.event_stale_timer_val = 0;
+		diagmode_v2.diag_id = diag_id;
+		memcpy(dest_buf, &diagmode_v2, msg_size_2);
+	} else {
+		diagmode.ctrl_pkt_id = DIAG_CTRL_MSG_DIAGMODE;
+		diagmode.ctrl_pkt_data_len = DIAG_MODE_PKT_LEN;
+		diagmode.version = 1;
+		diagmode.sleep_vote = real_time ? 1 : 0;
+		/*
+		 * 0 - Disables real-time logging (to prevent
+		 *     frequent APPS wake-ups, etc.).
+		 * 1 - Enable real-time logging
+		 */
+		diagmode.real_time = real_time;
+		diagmode.use_nrt_values = 0;
+		diagmode.commit_threshold = 0;
+		diagmode.sleep_threshold = 0;
+		diagmode.sleep_time = 0;
+		diagmode.drain_timer_val = 0;
+		diagmode.event_stale_timer_val = 0;
+		memcpy(dest_buf, &diagmode, msg_size);
+	}
 }
 
 void diag_update_proc_vote(uint16_t proc, uint8_t vote, int index)
@@ -902,7 +926,7 @@ static void diag_send_diag_mode_update_remote(int token, int real_time)
 
 	memcpy(buf + write_len, &dci_header, dci_header_size);
 	write_len += dci_header_size;
-	diag_create_diag_mode_ctrl_pkt(buf + write_len, real_time);
+	diag_create_diag_mode_ctrl_pkt(buf + write_len, 0, real_time);
 	write_len += msg_size;
 	*(buf + write_len) = CONTROL_CHAR; /* End Terminator */
 	write_len += sizeof(uint8_t);
@@ -1008,14 +1032,18 @@ void diag_real_time_work_fn(struct work_struct *work)
 }
 #endif
 
-static int __diag_send_real_time_update(uint8_t peripheral, int real_time)
+static int __diag_send_real_time_update(uint8_t peripheral, int real_time,
+						uint8_t diag_id)
 {
-	char buf[sizeof(struct diag_ctrl_msg_diagmode)];
-	int msg_size = sizeof(struct diag_ctrl_msg_diagmode);
+	char buf[sizeof(struct diag_ctrl_msg_diagmode_v2)];
+	int msg_size = 0;
 	int err = 0;
 
-	if (peripheral >= NUM_PERIPHERALS)
+	if (peripheral >= NUM_PERIPHERALS) {
+		pr_err("diag: In %s, invalid peripheral %d\n", __func__,
+		       peripheral);
 		return -EINVAL;
+	}
 
 	if (!driver->diagfwd_cntl[peripheral] ||
 	    !driver->diagfwd_cntl[peripheral]->ch_open) {
@@ -1030,12 +1058,17 @@ static int __diag_send_real_time_update(uint8_t peripheral, int real_time)
 		return -EINVAL;
 	}
 
-	diag_create_diag_mode_ctrl_pkt(buf, real_time);
+	msg_size = (diag_id ? sizeof(struct diag_ctrl_msg_diagmode_v2) :
+		sizeof(struct diag_ctrl_msg_diagmode));
+
+	diag_create_diag_mode_ctrl_pkt(buf, diag_id, real_time);
 
 	mutex_lock(&driver->diag_cntl_mutex);
+
 	err = diagfwd_write(peripheral, TYPE_CNTL, buf, msg_size);
+
 	if (err && err != -ENODEV) {
-		pr_err("diag: In %s, unable to write to smd, peripheral: %d, type: %d, len: %d, err: %d\n",
+		pr_err("diag: In %s, unable to write, peripheral: %d, type: %d, len: %d, err: %d\n",
 		       __func__, peripheral, TYPE_CNTL,
 		       msg_size, err);
 	} else {
@@ -1061,27 +1094,79 @@ int diag_send_real_time_update(uint8_t peripheral, int real_time)
 		return -EINVAL;
 	}
 
-	return __diag_send_real_time_update(peripheral, real_time);
+	return __diag_send_real_time_update(peripheral, real_time, 0);
+}
+
+void diag_map_pd_to_diagid(uint8_t pd, uint8_t *diag_id, int *peripheral)
+{
+	switch (pd) {
+	case UPD_WLAN:
+		*diag_id = DIAG_ID_WLAN;
+		*peripheral = PERIPHERAL_MODEM;
+		break;
+	case UPD_AUDIO:
+		*diag_id = DIAG_ID_AUDIO;
+		*peripheral = PERIPHERAL_LPASS;
+		break;
+	case UPD_SENSORS:
+		*diag_id = DIAG_ID_SENSORS;
+		*peripheral = PERIPHERAL_LPASS;
+		break;
+	case PERIPHERAL_MODEM:
+		*diag_id = DIAG_ID_MPSS;
+		*peripheral = PERIPHERAL_MODEM;
+		break;
+	case PERIPHERAL_LPASS:
+		*diag_id = DIAG_ID_LPASS;
+		*peripheral = PERIPHERAL_LPASS;
+		break;
+	case PERIPHERAL_CDSP:
+		*diag_id = DIAG_ID_CDSP;
+		*peripheral = PERIPHERAL_CDSP;
+		break;
+	default:
+		pr_err("diag: In %s, invalid peripheral %d\n", __func__,
+			pd);
+		*peripheral = -EINVAL;
+		break;
+	}
+
+	if (*peripheral > 0)
+		if (!driver->feature[*peripheral].pd_buffering)
+			*diag_id = 0;
 }
 
 int diag_send_peripheral_buffering_mode(struct diag_buffering_mode_t *params)
 {
 	int err = 0;
 	int mode = MODE_REALTIME;
-	uint8_t peripheral = 0;
+	int peripheral = 0;
+	uint8_t diag_id = 0;
 
 	if (!params)
 		return -EIO;
 
-	peripheral = params->peripheral;
-	if (peripheral >= NUM_PERIPHERALS) {
+	diag_map_pd_to_diagid(params->peripheral,
+		&diag_id, &peripheral);
+
+	if ((peripheral < 0) ||
+		peripheral >= NUM_PERIPHERALS) {
 		pr_err("diag: In %s, invalid peripheral %d\n", __func__,
 		       peripheral);
 		return -EINVAL;
 	}
 
-	if (!driver->buffering_flag[peripheral])
+	if (!driver->buffering_flag[params->peripheral]) {
+		pr_err("diag: In %s, buffering flag not set for %d\n", __func__,
+		       params->peripheral);
 		return -EINVAL;
+	}
+
+	if (!driver->feature[peripheral].peripheral_buffering) {
+		pr_err("diag: In %s, peripheral %d doesn't support buffering\n",
+		       __func__, peripheral);
+		return -EIO;
+	}
 
 	switch (params->mode) {
 	case DIAG_BUFFERING_MODE_STREAMING:
@@ -1100,7 +1185,7 @@ int diag_send_peripheral_buffering_mode(struct diag_buffering_mode_t *params)
 	if (!driver->feature[peripheral].peripheral_buffering) {
 		pr_debug("diag: In %s, peripheral %d doesn't support buffering\n",
 			 __func__, peripheral);
-		driver->buffering_flag[peripheral] = 0;
+		driver->buffering_flag[params->peripheral] = 0;
 		return -EIO;
 	}
 
@@ -1115,35 +1200,39 @@ int diag_send_peripheral_buffering_mode(struct diag_buffering_mode_t *params)
 	     (params->low_wm_val != DIAG_MIN_WM_VAL))) {
 		pr_err("diag: In %s, invalid watermark values, high: %d, low: %d, peripheral: %d\n",
 		       __func__, params->high_wm_val, params->low_wm_val,
-		       peripheral);
+		       params->peripheral);
 		return -EINVAL;
 	}
 
 	mutex_lock(&driver->mode_lock);
-	err = diag_send_buffering_tx_mode_pkt(peripheral, params);
+	err = diag_send_buffering_tx_mode_pkt(peripheral, diag_id, params);
 	if (err) {
 		pr_err("diag: In %s, unable to send buffering mode packet to peripheral %d, err: %d\n",
 		       __func__, peripheral, err);
 		goto fail;
 	}
-	err = diag_send_buffering_wm_values(peripheral, params);
+	err = diag_send_buffering_wm_values(peripheral, diag_id, params);
 	if (err) {
 		pr_err("diag: In %s, unable to send buffering wm value packet to peripheral %d, err: %d\n",
 		       __func__, peripheral, err);
 		goto fail;
 	}
-	err = __diag_send_real_time_update(peripheral, mode);
+	err = __diag_send_real_time_update(peripheral, mode, diag_id);
 	if (err) {
 		pr_err("diag: In %s, unable to send mode update to peripheral %d, mode: %d, err: %d\n",
 		       __func__, peripheral, mode, err);
 		goto fail;
 	}
-	driver->buffering_mode[peripheral].peripheral = peripheral;
-	driver->buffering_mode[peripheral].mode = params->mode;
-	driver->buffering_mode[peripheral].low_wm_val = params->low_wm_val;
-	driver->buffering_mode[peripheral].high_wm_val = params->high_wm_val;
+	driver->buffering_mode[params->peripheral].peripheral =
+		params->peripheral;
+	driver->buffering_mode[params->peripheral].mode =
+		params->mode;
+	driver->buffering_mode[params->peripheral].low_wm_val =
+		params->low_wm_val;
+	driver->buffering_mode[params->peripheral].high_wm_val =
+		params->high_wm_val;
 	if (params->mode == DIAG_BUFFERING_MODE_STREAMING)
-		driver->buffering_flag[peripheral] = 0;
+		driver->buffering_flag[params->peripheral] = 0;
 fail:
 	mutex_unlock(&driver->mode_lock);
 	return err;
@@ -1182,10 +1271,12 @@ int diag_send_stm_state(uint8_t peripheral, uint8_t stm_control_data)
 	return err;
 }
 
-int diag_send_peripheral_drain_immediate(uint8_t peripheral)
+int diag_send_peripheral_drain_immediate(uint8_t pd,
+		uint8_t diag_id, int peripheral)
 {
 	int err = 0;
 	struct diag_ctrl_drain_immediate ctrl_pkt;
+	struct diag_ctrl_drain_immediate_v2 ctrl_pkt_v2;
 
 	if (!driver->feature[peripheral].peripheral_buffering) {
 		pr_debug("diag: In %s, peripheral  %d doesn't support buffering\n",
@@ -1200,32 +1291,57 @@ int diag_send_peripheral_drain_immediate(uint8_t peripheral)
 		return -ENODEV;
 	}
 
-	ctrl_pkt.pkt_id = DIAG_CTRL_MSG_PERIPHERAL_BUF_DRAIN_IMM;
-	/* The length of the ctrl pkt is size of version and stream id */
-	ctrl_pkt.len = sizeof(uint32_t) + sizeof(uint8_t);
-	ctrl_pkt.version = 1;
-	ctrl_pkt.stream_id = 1;
-
-	err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt, sizeof(ctrl_pkt));
-	if (err && err != -ENODEV) {
-		pr_err("diag: Unable to send drain immediate ctrl packet to peripheral %d, err: %d\n",
-		       peripheral, err);
+	if (diag_id && driver->feature[peripheral].pd_buffering) {
+		ctrl_pkt_v2.pkt_id = DIAG_CTRL_MSG_PERIPHERAL_BUF_DRAIN_IMM;
+		/*
+		 * The length of the ctrl pkt is size of version,
+		 * diag_id and stream id
+		 */
+		ctrl_pkt_v2.len = sizeof(uint32_t) + (2 * sizeof(uint8_t));
+		ctrl_pkt_v2.version = 2;
+		ctrl_pkt_v2.diag_id = diag_id;
+		ctrl_pkt_v2.stream_id = 1;
+		err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt_v2,
+				sizeof(ctrl_pkt_v2));
+		if (err && err != -ENODEV) {
+			pr_err("diag: Unable to send drain immediate ctrl packet to peripheral %d, err: %d\n",
+			peripheral, err);
+		}
+	} else {
+		ctrl_pkt.pkt_id = DIAG_CTRL_MSG_PERIPHERAL_BUF_DRAIN_IMM;
+		/*
+		 * The length of the ctrl pkt is
+		 * size of version and stream id
+		 */
+		ctrl_pkt.len = sizeof(uint32_t) + sizeof(uint8_t);
+		ctrl_pkt.version = 1;
+		ctrl_pkt.stream_id = 1;
+		err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt,
+				sizeof(ctrl_pkt));
+		if (err && err != -ENODEV) {
+			pr_err("diag: Unable to send drain immediate ctrl packet to peripheral %d, err: %d\n",
+			peripheral, err);
+		}
 	}
 
 	return err;
 }
 
 int diag_send_buffering_tx_mode_pkt(uint8_t peripheral,
-				    struct diag_buffering_mode_t *params)
+		    uint8_t diag_id, struct diag_buffering_mode_t *params)
 {
 	int err = 0;
 	struct diag_ctrl_peripheral_tx_mode ctrl_pkt;
+	struct diag_ctrl_peripheral_tx_mode_v2 ctrl_pkt_v2;
 
 	if (!params)
 		return -EIO;
 
-	if (peripheral >= NUM_PERIPHERALS)
+	if (peripheral >= NUM_PERIPHERALS) {
+		pr_err("diag: In %s, invalid peripheral %d\n", __func__,
+		       peripheral);
 		return -EINVAL;
+	}
 
 	if (!driver->feature[peripheral].peripheral_buffering) {
 		pr_debug("diag: In %s, peripheral  %d doesn't support buffering\n",
@@ -1233,9 +1349,6 @@ int diag_send_buffering_tx_mode_pkt(uint8_t peripheral,
 		return -EINVAL;
 	}
 
-	if (params->peripheral != peripheral)
-		return -EINVAL;
-
 	switch (params->mode) {
 	case DIAG_BUFFERING_MODE_STREAMING:
 	case DIAG_BUFFERING_MODE_THRESHOLD:
@@ -1247,36 +1360,67 @@ int diag_send_buffering_tx_mode_pkt(uint8_t peripheral,
 		return -EINVAL;
 	}
 
-	ctrl_pkt.pkt_id = DIAG_CTRL_MSG_CONFIG_PERIPHERAL_TX_MODE;
-	/* Control packet length is size of version, stream_id and tx_mode */
-	ctrl_pkt.len = sizeof(uint32_t) +  (2 * sizeof(uint8_t));
-	ctrl_pkt.version = 1;
-	ctrl_pkt.stream_id = 1;
-	ctrl_pkt.tx_mode = params->mode;
+	if (diag_id &&
+		driver->feature[peripheral].pd_buffering) {
 
-	err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt, sizeof(ctrl_pkt));
-	if (err && err != -ENODEV) {
-		pr_err("diag: Unable to send tx_mode ctrl packet to peripheral %d, err: %d\n",
-		       peripheral, err);
-		goto fail;
+		ctrl_pkt_v2.pkt_id = DIAG_CTRL_MSG_CONFIG_PERIPHERAL_TX_MODE;
+		/*
+		 * Control packet length is size of version, diag_id,
+		 * stream_id and tx_mode
+		 */
+		ctrl_pkt_v2.len = sizeof(uint32_t) +  (3 * sizeof(uint8_t));
+		ctrl_pkt_v2.version = 2;
+		ctrl_pkt_v2.diag_id = diag_id;
+		ctrl_pkt_v2.stream_id = 1;
+		ctrl_pkt_v2.tx_mode = params->mode;
+
+		err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt_v2,
+			sizeof(ctrl_pkt_v2));
+		if (err && err != -ENODEV) {
+			pr_err("diag: Unable to send tx_mode ctrl packet to peripheral %d, err: %d\n",
+				   peripheral, err);
+			goto fail;
+		}
+	} else {
+		ctrl_pkt.pkt_id = DIAG_CTRL_MSG_CONFIG_PERIPHERAL_TX_MODE;
+		/*
+		 * Control packet length is size of version,
+		 * stream_id and tx_mode
+		 */
+		ctrl_pkt.len = sizeof(uint32_t) +  (2 * sizeof(uint8_t));
+		ctrl_pkt.version = 1;
+		ctrl_pkt.stream_id = 1;
+		ctrl_pkt.tx_mode = params->mode;
+
+		err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt,
+			sizeof(ctrl_pkt));
+		if (err && err != -ENODEV) {
+			pr_err("diag: Unable to send tx_mode ctrl packet to peripheral %d, err: %d\n",
+			       peripheral, err);
+			goto fail;
+		}
 	}
-	driver->buffering_mode[peripheral].mode = params->mode;
+	driver->buffering_mode[params->peripheral].mode = params->mode;
 
 fail:
 	return err;
 }
 
 int diag_send_buffering_wm_values(uint8_t peripheral,
-				  struct diag_buffering_mode_t *params)
+		uint8_t diag_id, struct diag_buffering_mode_t *params)
 {
 	int err = 0;
 	struct diag_ctrl_set_wq_val ctrl_pkt;
+	struct diag_ctrl_set_wq_val_v2 ctrl_pkt_v2;
 
 	if (!params)
 		return -EIO;
 
-	if (peripheral >= NUM_PERIPHERALS)
+	if (peripheral >= NUM_PERIPHERALS) {
+		pr_err("diag: In %s, invalid peripheral %d\n", __func__,
+		       peripheral);
 		return -EINVAL;
+	}
 
 	if (!driver->feature[peripheral].peripheral_buffering) {
 		pr_debug("diag: In %s, peripheral  %d doesn't support buffering\n",
@@ -1291,9 +1435,6 @@ int diag_send_buffering_wm_values(uint8_t peripheral,
 		return -ENODEV;
 	}
 
-	if (params->peripheral != peripheral)
-		return -EINVAL;
-
 	switch (params->mode) {
 	case DIAG_BUFFERING_MODE_STREAMING:
 	case DIAG_BUFFERING_MODE_THRESHOLD:
@@ -1305,21 +1446,45 @@ int diag_send_buffering_wm_values(uint8_t peripheral,
 		return -EINVAL;
 	}
 
-	ctrl_pkt.pkt_id = DIAG_CTRL_MSG_CONFIG_PERIPHERAL_WMQ_VAL;
-	/* Control packet length is size of version, stream_id and wmq values */
-	ctrl_pkt.len = sizeof(uint32_t) + (3 * sizeof(uint8_t));
-	ctrl_pkt.version = 1;
-	ctrl_pkt.stream_id = 1;
-	ctrl_pkt.high_wm_val = params->high_wm_val;
-	ctrl_pkt.low_wm_val = params->low_wm_val;
-
-	err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt,
-			    sizeof(ctrl_pkt));
-	if (err && err != -ENODEV) {
-		pr_err("diag: Unable to send watermark values to peripheral %d, err: %d\n",
-		       peripheral, err);
+	if (diag_id &&
+		driver->feature[peripheral].pd_buffering) {
+		ctrl_pkt_v2.pkt_id = DIAG_CTRL_MSG_CONFIG_PERIPHERAL_WMQ_VAL;
+		/*
+		 * Control packet length is size of version, diag_id,
+		 * stream_id and wmq values
+		 */
+		ctrl_pkt_v2.len = sizeof(uint32_t) + (4 * sizeof(uint8_t));
+		ctrl_pkt_v2.version = 2;
+		ctrl_pkt_v2.diag_id = diag_id;
+		ctrl_pkt_v2.stream_id = 1;
+		ctrl_pkt_v2.high_wm_val = params->high_wm_val;
+		ctrl_pkt_v2.low_wm_val = params->low_wm_val;
+
+		err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt_v2,
+					sizeof(ctrl_pkt_v2));
+		if (err && err != -ENODEV) {
+			pr_err("diag: Unable to send watermark values to peripheral %d, err: %d\n",
+				   peripheral, err);
+		}
+	} else {
+		ctrl_pkt.pkt_id = DIAG_CTRL_MSG_CONFIG_PERIPHERAL_WMQ_VAL;
+		/*
+		 * Control packet length is size of version,
+		 * stream_id and wmq values
+		 */
+		ctrl_pkt.len = sizeof(uint32_t) + (3 * sizeof(uint8_t));
+		ctrl_pkt.version = 1;
+		ctrl_pkt.stream_id = 1;
+		ctrl_pkt.high_wm_val = params->high_wm_val;
+		ctrl_pkt.low_wm_val = params->low_wm_val;
+
+		err = diagfwd_write(peripheral, TYPE_CNTL, &ctrl_pkt,
+				    sizeof(ctrl_pkt));
+		if (err && err != -ENODEV) {
+			pr_err("diag: Unable to send watermark values to peripheral %d, err: %d\n",
+			       peripheral, err);
+		}
 	}
-
 	return err;
 }
 
diff --git a/drivers/char/diag/diagfwd_cntl.h b/drivers/char/diag/diagfwd_cntl.h
index e8608f47ff14..86442d838471 100644
--- a/drivers/char/diag/diagfwd_cntl.h
+++ b/drivers/char/diag/diagfwd_cntl.h
@@ -68,6 +68,7 @@
 #define F_DIAG_SOCKETS_ENABLED			13
 #define F_DIAG_DCI_EXTENDED_HEADER_SUPPORT	14
 #define F_DIAG_PKT_HEADER_UNTAG			16
+#define F_DIAG_PD_BUFFERING		17
 
 #define ENABLE_SEPARATE_CMDRSP	1
 #define DISABLE_SEPARATE_CMDRSP	0
@@ -85,7 +86,8 @@
 #define ENABLE_PKT_HEADER_UNTAGGING		1
 #define DISABLE_PKT_HEADER_UNTAGGING	0
 
-#define DIAG_MODE_PKT_LEN	36
+#define DIAG_MODE_PKT_LEN		36
+#define DIAG_MODE_PKT_LEN_V2	37
 
 struct diag_ctrl_pkt_header_t {
 	uint32_t pkt_id;
@@ -171,6 +173,21 @@ struct diag_ctrl_msg_diagmode {
 	uint32_t event_stale_timer_val;
 } __packed;
 
+struct diag_ctrl_msg_diagmode_v2 {
+	uint32_t ctrl_pkt_id;
+	uint32_t ctrl_pkt_data_len;
+	uint32_t version;
+	uint32_t sleep_vote;
+	uint32_t real_time;
+	uint32_t use_nrt_values;
+	uint32_t commit_threshold;
+	uint32_t sleep_threshold;
+	uint32_t sleep_time;
+	uint32_t drain_timer_val;
+	uint32_t event_stale_timer_val;
+	uint8_t diag_id;
+} __packed;
+
 struct diag_ctrl_msg_stm {
 	uint32_t ctrl_pkt_id;
 	uint32_t ctrl_pkt_data_len;
@@ -249,6 +266,15 @@ struct diag_ctrl_peripheral_tx_mode {
 	uint8_t tx_mode;
 } __packed;
 
+struct diag_ctrl_peripheral_tx_mode_v2 {
+	uint32_t pkt_id;
+	uint32_t len;
+	uint32_t version;
+	uint8_t diag_id;
+	uint8_t stream_id;
+	uint8_t tx_mode;
+} __packed;
+
 struct diag_ctrl_drain_immediate {
 	uint32_t pkt_id;
 	uint32_t len;
@@ -256,6 +282,14 @@ struct diag_ctrl_drain_immediate {
 	uint8_t stream_id;
 } __packed;
 
+struct diag_ctrl_drain_immediate_v2 {
+	uint32_t pkt_id;
+	uint32_t len;
+	uint32_t version;
+	uint8_t diag_id;
+	uint8_t stream_id;
+} __packed;
+
 struct diag_ctrl_set_wq_val {
 	uint32_t pkt_id;
 	uint32_t len;
@@ -265,6 +299,16 @@ struct diag_ctrl_set_wq_val {
 	uint8_t low_wm_val;
 } __packed;
 
+struct diag_ctrl_set_wq_val_v2 {
+	uint32_t pkt_id;
+	uint32_t len;
+	uint32_t version;
+	uint8_t diag_id;
+	uint8_t stream_id;
+	uint8_t high_wm_val;
+	uint8_t low_wm_val;
+} __packed;
+
 int diagfwd_cntl_init(void);
 void diagfwd_cntl_channel_init(void);
 void diagfwd_cntl_exit(void);
@@ -273,14 +317,16 @@ void diag_cntl_channel_close(struct diagfwd_info *p_info);
 void diag_cntl_process_read_data(struct diagfwd_info *p_info, void *buf,
 				 int len);
 int diag_send_real_time_update(uint8_t peripheral, int real_time);
+void diag_map_pd_to_diagid(uint8_t pd, uint8_t *diag_id, int *peripheral);
 int diag_send_peripheral_buffering_mode(struct diag_buffering_mode_t *params);
 void diag_update_proc_vote(uint16_t proc, uint8_t vote, int index);
 void diag_update_real_time_vote(uint16_t proc, uint8_t real_time, int index);
 void diag_real_time_work_fn(struct work_struct *work);
 int diag_send_stm_state(uint8_t peripheral, uint8_t stm_control_data);
-int diag_send_peripheral_drain_immediate(uint8_t peripheral);
+int diag_send_peripheral_drain_immediate(uint8_t pd,
+			uint8_t diag_id, int peripheral);
 int diag_send_buffering_tx_mode_pkt(uint8_t peripheral,
-				    struct diag_buffering_mode_t *params);
+		    uint8_t diag_id, struct diag_buffering_mode_t *params);
 int diag_send_buffering_wm_values(uint8_t peripheral,
-				  struct diag_buffering_mode_t *params);
+		    uint8_t diag_id, struct diag_buffering_mode_t *params);
 #endif