Merge "diag: Add PD buffering mode configuration support" (a6c1b8e2) · Commits · e / devices / android_kernel_oneplus_sm8150

drivers/char/diag/diag_debugfs.c

+6 −3

Original line number	Diff line number	Diff line
		@@ -77,7 +77,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,
		@@ -92,11 +93,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],
		@@ -105,6 +107,7 @@ static ssize_t diag_dbgfs_read_status(struct file file, char __user ubuf,
		driver->feature[i].encode_hdlc ? 'H':'h',
		driver->feature[i].peripheral_buffering ? 'B':'b',
		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',

drivers/char/diag/diagchar.h

+4 −2

Original line number	Diff line number	Diff line
		@@ -517,6 +517,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;
		@@ -549,6 +550,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 */
		@@ -602,8 +604,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;

drivers/char/diag/diagchar_core.c

+35 −9

Original line number	Diff line number	Diff line
		@@ -1956,12 +1956,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;
		@@ -1972,24 +1993,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",
		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)

drivers/char/diag/diagfwd.c

+5 −0

Original line number	Diff line number	Diff line
		@@ -1690,6 +1690,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);
		@@ -1720,6 +1721,7 @@ 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;
		@@ -1727,6 +1729,9 @@ int diagfwd_init(void)
		driver->feature[i].log_on_demand = 0;
		driver->feature[i].sent_feature_mask = 0;
		driver->feature[i].diag_id_support = 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;

drivers/char/diag/diagfwd_cntl.c

+230 −88

Original line number	Diff line number	Diff line
		@@ -424,6 +424,8 @@ static void process_incoming_feature_mask(uint8_t *buf, uint32_t len,
		enable_socket_feature(peripheral);
		if (FEATURE_SUPPORTED(F_DIAG_DIAGID_SUPPORT))
		driver->feature[peripheral].diag_id_support = 1;
		if (FEATURE_SUPPORTED(F_DIAG_PD_BUFFERING))
		driver->feature[peripheral].pd_buffering = 1;
		}

		process_socket_feature(peripheral);
		@@ -947,14 +949,36 @@ 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;

		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;
		@@ -971,9 +995,9 @@ static void diag_create_diag_mode_ctrl_pkt(unsigned char *dest_buf,
		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)
		{
		@@ -1057,7 +1081,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);
		@@ -1163,14 +1187,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) {
		@@ -1185,12 +1213,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 socket, 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 {
		@@ -1216,27 +1249,56 @@ 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)
		{
		if (!diag_search_diagid_by_pd(pd, (void *)diag_id,
		(void *)peripheral)) {
		*diag_id = 0;
		if ((pd >= 0) && pd < NUM_PERIPHERALS)
		*peripheral = pd;
		else
		*peripheral = -EINVAL;
		}

		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:
		@@ -1255,7 +1317,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;
		}

		@@ -1270,35 +1332,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;
		@@ -1337,10 +1403,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",
		@@ -1355,32 +1423,57 @@ int diag_send_peripheral_drain_immediate(uint8_t peripheral)
		return -ENODEV;
		}

		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 */
		/*
		* 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));
		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",
		@@ -1388,9 +1481,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:
		@@ -1402,36 +1492,67 @@ int diag_send_buffering_tx_mode_pkt(uint8_t peripheral,
		return -EINVAL;
		}

		if (diag_id &&
		driver->feature[peripheral].pd_buffering) {

		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 */
		/*
		* 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));
		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",
		@@ -1446,9 +1567,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:
		@@ -1460,8 +1578,32 @@ int diag_send_buffering_wm_values(uint8_t peripheral,
		return -EINVAL;
		}

		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 */
		/*
		* 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;
		@@ -1474,7 +1616,7 @@ int diag_send_buffering_wm_values(uint8_t peripheral,
		pr_err("diag: Unable to send watermark values to peripheral %d, err: %d\n",
		peripheral, err);
		}

		}
		return err;
		}