Donate to e Foundation | Murena handsets with /e/OS | Own a part of Murena! Learn more

Commit 702defa9 authored by Manoj Prabhu B's avatar Manoj Prabhu B Committed by Gerrit - the friendly Code Review server
Browse files

diag: Add PD buffering mode configuration support 



This patch extends support to configure buffering
modes for PD buffers by defining new control packets.

CRs-Fixed: 2019180
Change-Id: I5a72ff3ec7e5ab80a7461c3c3130e3b699e75522
Signed-off-by: default avatarManoj Prabhu B <bmanoj@codeaurora.org>
parent 81ab0bca

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;

}