diag: Fix feature mask mismatch for peripherals

This patch adds a way to publish APPS support
of untagging header only to selected peripherals.

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

4 files changed, 20 insertions, 5 deletions

diff --git a/drivers/char/diag/diag_masks.c b/drivers/char/diag/diag_masks.c
index 0c958d855f94..437077c4d44d 100644
--- a/drivers/char/diag/diag_masks.c
+++ b/drivers/char/diag/diag_masks.c
@@ -456,8 +456,13 @@ 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);
+	if (driver->supports_apps_header_untagging) {
+		if (peripheral == PERIPHERAL_MODEM) {
+			DIAG_SET_FEATURE_MASK(F_DIAG_PKT_HEADER_UNTAG);
+			driver->peripheral_untag[peripheral] =
+				ENABLE_PKT_HEADER_UNTAGGING;
+		}
+	}
 	DIAG_SET_FEATURE_MASK(F_DIAG_MASK_CENTRALIZATION);
 	if (driver->supports_sockets)
 		DIAG_SET_FEATURE_MASK(F_DIAG_SOCKETS_ENABLED);
diff --git a/drivers/char/diag/diagchar.h b/drivers/char/diag/diagchar.h
index 511b019e33ec..b68a47219132 100644
--- a/drivers/char/diag/diagchar.h
+++ b/drivers/char/diag/diagchar.h
@@ -503,6 +503,7 @@ struct diagchar_dev {
 	int supports_separate_cmdrsp;
 	int supports_apps_hdlc_encoding;
 	int supports_apps_header_untagging;
+	int peripheral_untag[NUM_PERIPHERALS];
 	int supports_sockets;
 	/* The state requested in the STM command */
 	int stm_state_requested[NUM_STM_PROCESSORS];
diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c
index 532d2b149317..30960c99a38d 100644
--- a/drivers/char/diag/diagfwd.c
+++ b/drivers/char/diag/diagfwd.c
@@ -1588,6 +1588,8 @@ int diagfwd_init(void)
 	driver->supports_separate_cmdrsp = 1;
 	driver->supports_apps_hdlc_encoding = 1;
 	driver->supports_apps_header_untagging = 1;
+	for (i = 0; i < NUM_PERIPHERALS; i++)
+		driver->peripheral_untag[i] = 0;
 	mutex_init(&driver->diag_hdlc_mutex);
 	mutex_init(&driver->diag_cntl_mutex);
 	mutex_init(&driver->mode_lock);
diff --git a/drivers/char/diag/diagfwd_peripheral.c b/drivers/char/diag/diagfwd_peripheral.c
index 55d36abe4679..55288010a41f 100644
--- a/drivers/char/diag/diagfwd_peripheral.c
+++ b/drivers/char/diag/diagfwd_peripheral.c
@@ -351,7 +351,8 @@ static void diagfwd_data_read_untag_done(struct diagfwd_info *fwd_info,
 	}
 
 	if (driver->feature[fwd_info->peripheral].encode_hdlc &&
-		driver->feature[fwd_info->peripheral].untag_header) {
+		driver->feature[fwd_info->peripheral].untag_header &&
+		driver->peripheral_untag[fwd_info->peripheral]) {
 		mutex_lock(&driver->diagfwd_untag_mutex);
 		temp_buf_cpd = buf;
 		temp_buf_main = buf;
@@ -1300,6 +1301,7 @@ static void diagfwd_queue_read(struct diagfwd_info *fwd_info)
 void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
 {
 	unsigned char *temp_buf;
+	uint8_t peripheral;
 
 	if (!fwd_info)
 		return;
@@ -1311,6 +1313,9 @@ void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
 	}
 
 	mutex_lock(&fwd_info->buf_mutex);
+
+	peripheral = fwd_info->peripheral;
+
 	if (!fwd_info->buf_1) {
 		fwd_info->buf_1 = kzalloc(sizeof(struct diagfwd_buf_t),
 					  GFP_KERNEL);
@@ -1352,7 +1357,8 @@ void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
 							fwd_info->type, 2);
 		}
 
-		if (driver->feature[fwd_info->peripheral].untag_header)	{
+		if (driver->feature[peripheral].untag_header &&
+			driver->peripheral_untag[peripheral]) {
 			if (!fwd_info->buf_upd_1_a) {
 				fwd_info->buf_upd_1_a =
 					kzalloc(sizeof(struct diagfwd_buf_t),
@@ -1426,7 +1432,8 @@ void diagfwd_buffers_init(struct diagfwd_info *fwd_info)
 			}
 
 			if (driver->feature[fwd_info->peripheral].
-					untag_header) {
+					untag_header &&
+				driver->peripheral_untag[peripheral]) {
 				if (!fwd_info->buf_upd_1_a->data_raw) {
 					fwd_info->buf_upd_1_a->data_raw =
 						kzalloc(PERIPHERAL_BUF_SZ +