Merge "diag: Enhance IPC logging for diag CNTL channel"

3 files changed, 209 insertions, 58 deletions

diff --git a/drivers/char/diag/diag_ipc_logging.h b/drivers/char/diag/diag_ipc_logging.h
index b9958a433c46..4b8dd1b12c1c 100644
--- a/drivers/char/diag/diag_ipc_logging.h
+++ b/drivers/char/diag/diag_ipc_logging.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2015, 2018, 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
@@ -24,6 +24,7 @@
 #define DIAG_DEBUG_MASKS	0x0010
 #define DIAG_DEBUG_POWER	0x0020
 #define DIAG_DEBUG_BRIDGE	0x0040
+#define DIAG_DEBUG_CONTROL	0x0080
 
 #define DIAG_DEBUG
 
diff --git a/drivers/char/diag/diagfwd_cntl.c b/drivers/char/diag/diagfwd_cntl.c
index 10038e629e6c..d8ec09f90ec4 100644
--- a/drivers/char/diag/diagfwd_cntl.c
+++ b/drivers/char/diag/diagfwd_cntl.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2011-2017, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2011-2018, 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
@@ -47,8 +47,11 @@ static void diag_mask_update_work_fn(struct work_struct *work)
 
 void diag_cntl_channel_open(struct diagfwd_info *p_info)
 {
-	if (!p_info)
+	if (!p_info) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid fwd_info structure\n");
 		return;
+	}
 	driver->mask_update |= PERIPHERAL_MASK(p_info->peripheral);
 	queue_work(driver->cntl_wq, &driver->mask_update_work);
 	diag_notify_md_client(p_info->peripheral, DIAG_STATUS_OPEN);
@@ -58,12 +61,18 @@ void diag_cntl_channel_close(struct diagfwd_info *p_info)
 {
 	uint8_t peripheral;
 
-	if (!p_info)
+	if (!p_info) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid fwd_info structure\n");
 		return;
+	}
 
 	peripheral = p_info->peripheral;
-	if (peripheral >= NUM_PERIPHERALS)
+	if (peripheral >= NUM_PERIPHERALS) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: Invalid peripheral (%d)\n", peripheral);
 		return;
+	}
 
 	driver->feature[peripheral].sent_feature_mask = 0;
 	driver->feature[peripheral].rcvd_feature_mask = 0;
@@ -88,8 +97,11 @@ static void diag_stm_update_work_fn(struct work_struct *work)
 	driver->stm_peripheral = 0;
 	mutex_unlock(&driver->cntl_lock);
 
-	if (peripheral_mask == 0)
+	if (peripheral_mask == 0) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: Empty Peripheral mask\n");
 		return;
+	}
 
 	for (i = 0; i < NUM_PERIPHERALS; i++) {
 		if (!driver->feature[i].stm_support)
@@ -112,11 +124,18 @@ void diag_notify_md_client(uint8_t peripheral, int data)
 	struct pid *pid_struct;
 	struct task_struct *result;
 
-	if (peripheral > NUM_PERIPHERALS)
+	if (peripheral > NUM_PERIPHERALS) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: Invalid peripheral (%d)\n", peripheral);
 		return;
+	}
 
-	if (driver->logging_mode != DIAG_MEMORY_DEVICE_MODE)
+	if (driver->logging_mode != DIAG_MEMORY_DEVICE_MODE) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: Invalid logging_mode (%d)\n",
+			driver->logging_mode);
 		return;
+	}
 
 	mutex_lock(&driver->md_session_lock);
 	memset(&info, 0, sizeof(struct siginfo));
@@ -172,8 +191,12 @@ static void process_pd_status(uint8_t *buf, uint32_t len,
 	uint32_t pd;
 	int status = DIAG_STATUS_CLOSED;
 
-	if (!buf || peripheral >= NUM_PERIPHERALS || len < sizeof(*pd_msg))
+	if (!buf || peripheral >= NUM_PERIPHERALS || len < sizeof(*pd_msg)) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid parameters:(!buf) = %d, peripheral = %d, len = %d, pd_msg_len = %d\n",
+		!buf, peripheral, len, (int)sizeof(*pd_msg));
 		return;
+	}
 
 	pd_msg = (struct diag_ctrl_msg_pd_status *)buf;
 	pd = pd_msg->pd_id;
@@ -183,8 +206,11 @@ static void process_pd_status(uint8_t *buf, uint32_t len,
 
 static void enable_stm_feature(uint8_t peripheral)
 {
-	if (peripheral >= NUM_PERIPHERALS)
+	if (peripheral >= NUM_PERIPHERALS) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid peripheral (%d)\n", peripheral);
 		return;
+	}
 
 	mutex_lock(&driver->cntl_lock);
 	driver->feature[peripheral].stm_support = ENABLE_STM;
@@ -196,8 +222,11 @@ static void enable_stm_feature(uint8_t peripheral)
 
 static void enable_socket_feature(uint8_t peripheral)
 {
-	if (peripheral >= NUM_PERIPHERALS)
+	if (peripheral >= NUM_PERIPHERALS) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid peripheral (%d)\n", peripheral);
 		return;
+	}
 
 	if (driver->supports_sockets)
 		driver->feature[peripheral].sockets_enabled = 1;
@@ -207,8 +236,11 @@ static void enable_socket_feature(uint8_t peripheral)
 
 static void process_hdlc_encoding_feature(uint8_t peripheral)
 {
-	if (peripheral >= NUM_PERIPHERALS)
+	if (peripheral >= NUM_PERIPHERALS) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid peripheral (%d)\n", peripheral);
 		return;
+	}
 
 	if (driver->supports_apps_hdlc_encoding) {
 		driver->feature[peripheral].encode_hdlc =
@@ -221,8 +253,11 @@ static void process_hdlc_encoding_feature(uint8_t peripheral)
 
 static void process_upd_header_untagging_feature(uint8_t peripheral)
 {
-	if (peripheral >= NUM_PERIPHERALS)
+	if (peripheral >= NUM_PERIPHERALS) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid peripheral (%d)\n", peripheral);
 		return;
+	}
 
 	if (driver->supports_apps_header_untagging) {
 		driver->feature[peripheral].untag_header =
@@ -248,8 +283,16 @@ static void process_command_deregistration(uint8_t *buf, uint32_t len,
 	 * Perform Basic sanity. The len field is the size of the data payload.
 	 * This doesn't include the header size.
 	 */
-	if (!buf || peripheral >= NUM_PERIPHERALS || len == 0)
+	if (!buf || peripheral >= NUM_PERIPHERALS || len == 0) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid parameters:(!buf) = %d, peripheral = %d, len = %d\n",
+		!buf, peripheral, len);
 		return;
+	}
+
+	DIAG_LOG(DIAG_DEBUG_CONTROL,
+		"diag:peripheral(%d) command deregistration packet processing started\n",
+		peripheral);
 
 	dereg = (struct diag_ctrl_cmd_dereg *)ptr;
 	ptr += header_len;
@@ -257,8 +300,8 @@ static void process_command_deregistration(uint8_t *buf, uint32_t len,
 	read_len += header_len - (2 * sizeof(uint32_t));
 
 	if (dereg->count_entries == 0) {
-		pr_debug("diag: In %s, received reg tbl with no entries\n",
-			 __func__);
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: received reg tbl with no entries\n");
 		return;
 	}
 
@@ -277,6 +320,9 @@ static void process_command_deregistration(uint8_t *buf, uint32_t len,
 		pr_err("diag: In %s, reading less than available, read_len: %d, len: %d count: %d\n",
 		       __func__, read_len, len, dereg->count_entries);
 	}
+	DIAG_LOG(DIAG_DEBUG_CONTROL,
+		"diag:peripheral(%d) command deregistration packet processing complete\n",
+		peripheral);
 }
 static void process_command_registration(uint8_t *buf, uint32_t len,
 					 uint8_t peripheral)
@@ -293,8 +339,15 @@ static void process_command_registration(uint8_t *buf, uint32_t len,
 	 * Perform Basic sanity. The len field is the size of the data payload.
 	 * This doesn't include the header size.
 	 */
-	if (!buf || peripheral >= NUM_PERIPHERALS || len == 0)
+	if (!buf || peripheral >= NUM_PERIPHERALS || len == 0) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid parameters:(!buf) = %d, peripheral = %d, len = %d\n",
+		!buf, peripheral, len);
 		return;
+	}
+	DIAG_LOG(DIAG_DEBUG_CONTROL,
+		"diag: peripheral(%d) command registration packet processing started\n",
+		peripheral);
 
 	reg = (struct diag_ctrl_cmd_reg *)ptr;
 	ptr += header_len;
@@ -302,7 +355,8 @@ static void process_command_registration(uint8_t *buf, uint32_t len,
 	read_len += header_len - (2 * sizeof(uint32_t));
 
 	if (reg->count_entries == 0) {
-		pr_debug("diag: In %s, received reg tbl with no entries\n",
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: In %s, received reg tbl with no entries\n",
 			 __func__);
 		return;
 	}
@@ -322,6 +376,9 @@ static void process_command_registration(uint8_t *buf, uint32_t len,
 		pr_err("diag: In %s, reading less than available, read_len: %d, len: %d count: %d\n",
 		       __func__, read_len, len, reg->count_entries);
 	}
+	DIAG_LOG(DIAG_DEBUG_CONTROL,
+		"diag: peripheral(%d) command registration packet processing complete\n",
+		peripheral);
 }
 
 static void diag_close_transport_work_fn(struct work_struct *work)
@@ -343,8 +400,11 @@ static void diag_close_transport_work_fn(struct work_struct *work)
 
 static void process_socket_feature(uint8_t peripheral)
 {
-	if (peripheral >= NUM_PERIPHERALS)
+	if (peripheral >= NUM_PERIPHERALS) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid peripheral (%d)\n", peripheral);
 		return;
+	}
 
 	mutex_lock(&driver->cntl_lock);
 	driver->close_transport |= PERIPHERAL_MASK(peripheral);
@@ -375,15 +435,20 @@ static void process_incoming_feature_mask(uint8_t *buf, uint32_t len,
 	uint32_t feature_mask = 0;
 	uint8_t *ptr = buf;
 
-	if (!buf || peripheral >= NUM_PERIPHERALS || len == 0)
+	if (!buf || peripheral >= NUM_PERIPHERALS || len == 0) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid parameters:(!buf) = %d, peripheral = %d, len = %d\n",
+		!buf, peripheral, len);
 		return;
+	}
 
 	header = (struct diag_ctrl_feature_mask *)ptr;
 	ptr += header_len;
 	feature_mask_len = header->feature_mask_len;
 
 	if (feature_mask_len == 0) {
-		pr_debug("diag: In %s, received invalid feature mask from peripheral %d\n",
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: In %s, received invalid feature mask from peripheral %d\n",
 			 __func__, peripheral);
 		return;
 	}
@@ -396,6 +461,8 @@ static void process_incoming_feature_mask(uint8_t *buf, uint32_t len,
 	diag_cmd_remove_reg_by_proc(peripheral);
 
 	driver->feature[peripheral].rcvd_feature_mask = 1;
+	DIAG_LOG(DIAG_DEBUG_CONTROL,
+	"diag: Received feature mask for peripheral %d\n", peripheral);
 
 	for (i = 0; i < feature_mask_len && read_len < len; i++) {
 		feature_mask = *(uint8_t *)ptr;
@@ -425,6 +492,10 @@ static void process_incoming_feature_mask(uint8_t *buf, uint32_t len,
 
 	process_socket_feature(peripheral);
 	process_log_on_demand_feature(peripheral);
+
+	DIAG_LOG(DIAG_DEBUG_CONTROL,
+		"diag: Peripheral(%d) feature mask is processed\n",
+		peripheral);
 }
 
 static void process_last_event_report(uint8_t *buf, uint32_t len,
@@ -436,14 +507,23 @@ static void process_last_event_report(uint8_t *buf, uint32_t len,
 	uint32_t pkt_len = sizeof(uint32_t) + sizeof(uint16_t);
 	uint16_t event_size = 0;
 
-	if (!buf || peripheral >= NUM_PERIPHERALS || len != pkt_len)
+	if (!buf || peripheral >= NUM_PERIPHERALS || len != pkt_len) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid parameters:(!buf) = %d, peripheral = %d, len = %d, pkt_len = %d\n",
+		!buf, peripheral, len, pkt_len);
 		return;
+	}
+
+	DIAG_LOG(DIAG_DEBUG_CONTROL,
+		"diag:started processing last event report for peripheral (%d)\n",
+		peripheral);
 
 	mutex_lock(&event_mask.lock);
 	header = (struct diag_ctrl_last_event_report *)ptr;
 	event_size = ((header->event_last_id / 8) + 1);
 	if (event_size >= driver->event_mask_size) {
-		pr_debug("diag: In %s, receiving event mask size more that Apps can handle\n",
+		DIAG_LOG(DIAG_DEBUG_CONTROL,
+			"diag: In %s, receiving event mask size more that Apps can handle\n",
 			 __func__);
 		temp = krealloc(driver->event_mask->ptr, event_size,
 				GFP_KERNEL);
@@ -461,6 +541,9 @@ static void process_last_event_report(uint8_t *buf, uint32_t len,
 		driver->last_event_id = header->event_last_id;
 err:
 	mutex_unlock(&event_mask.lock);
+	DIAG_LOG(DIAG_DEBUG_CONTROL,
+		"diag: last event report processed for peripheral (%d)\n",
+		peripheral);
 }
 
 static void process_log_range_report(uint8_t *buf, uint32_t len,
@@ -474,8 +557,15 @@ static void process_log_range_report(uint8_t *buf, uint32_t len,
 	struct diag_ctrl_log_range *log_range = NULL;
 	struct diag_log_mask_t *mask_ptr = NULL;
 
-	if (!buf || peripheral >= NUM_PERIPHERALS || len < 0)
+	if (!buf || peripheral >= NUM_PERIPHERALS || len < 0) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid parameters:(!buf) = %d, peripheral = %d, len = %d\n",
+		!buf, peripheral, len);
 		return;
+	}
+	DIAG_LOG(DIAG_DEBUG_CONTROL,
+		"diag:started processing log range report for peripheral(%d)\n",
+		peripheral);
 
 	header = (struct diag_ctrl_log_range_report *)ptr;
 	ptr += header_len;
@@ -501,6 +591,9 @@ static void process_log_range_report(uint8_t *buf, uint32_t len,
 		mask_ptr->range = LOG_ITEMS_TO_SIZE(log_range->num_items);
 		mutex_unlock(&(mask_ptr->lock));
 	}
+	DIAG_LOG(DIAG_DEBUG_CONTROL,
+		"diag: log range report processed for peripheral (%d)\n",
+		peripheral);
 }
 
 static int update_msg_mask_tbl_entry(struct diag_msg_mask_t *mask,
@@ -508,8 +601,12 @@ static int update_msg_mask_tbl_entry(struct diag_msg_mask_t *mask,
 {
 	uint32_t temp_range;
 
-	if (!mask || !range)
+	if (!mask || !range) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid %s\n",
+		(!mask ? "mask" : (!range ? "range" : " ")));
 		return -EIO;
+	}
 	if (range->ssid_last < range->ssid_first) {
 		pr_err("diag: In %s, invalid ssid range, first: %d, last: %d\n",
 		       __func__, range->ssid_first, range->ssid_last);
@@ -541,8 +638,16 @@ static void process_ssid_range_report(uint8_t *buf, uint32_t len,
 	uint8_t *temp = NULL;
 	uint32_t min_len = header_len - sizeof(struct diag_ctrl_pkt_header_t);
 
-	if (!buf || peripheral >= NUM_PERIPHERALS || len < min_len)
+	if (!buf || peripheral >= NUM_PERIPHERALS || len < min_len) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid parameters:(!buf) = %d, peripheral = %d, len = %d, min_len = %d\n",
+		!buf, peripheral, len, min_len);
 		return;
+	}
+
+	DIAG_LOG(DIAG_DEBUG_CONTROL,
+		"diag: started processing ssid range for peripheral (%d)\n",
+		peripheral);
 
 	header = (struct diag_ctrl_ssid_range_report *)ptr;
 	ptr += header_len;
@@ -594,6 +699,9 @@ static void process_ssid_range_report(uint8_t *buf, uint32_t len,
 		driver->msg_mask_tbl_count += 1;
 	}
 	mutex_unlock(&driver->msg_mask_lock);
+	DIAG_LOG(DIAG_DEBUG_CONTROL,
+		"diag: processed ssid range for peripheral(%d)\n",
+		peripheral);
 }
 
 static void diag_build_time_mask_update(uint8_t *buf,
@@ -610,8 +718,12 @@ static void diag_build_time_mask_update(uint8_t *buf,
 	uint32_t *dest_ptr = NULL;
 	struct diag_msg_mask_t *build_mask = NULL;
 
-	if (!range || !buf)
+	if (!range || !buf) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid %s\n",
+		(!range ? "range" : (!buf ? "buf" : " ")));
 		return;
+	}
 
 	if (range->ssid_last < range->ssid_first) {
 		pr_err("diag: In %s, invalid ssid range, first: %d, last: %d\n",
@@ -673,8 +785,16 @@ static void process_build_mask_report(uint8_t *buf, uint32_t len,
 	struct diag_ctrl_build_mask_report *header = NULL;
 	struct diag_ssid_range_t *range = NULL;
 
-	if (!buf || peripheral >= NUM_PERIPHERALS || len < header_len)
+	if (!buf || peripheral >= NUM_PERIPHERALS || len < header_len) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: Invalid parameters:(!buf) = %d, peripheral = %d, len = %d, header_len = %d\n",
+		!buf, peripheral, len, header_len);
 		return;
+	}
+
+	DIAG_LOG(DIAG_DEBUG_CONTROL,
+		"diag: started processing build mask for peripheral(%d)\n",
+		peripheral);
 
 	header = (struct diag_ctrl_build_mask_report *)ptr;
 	ptr += header_len;
@@ -690,6 +810,8 @@ static void process_build_mask_report(uint8_t *buf, uint32_t len,
 		ptr += num_items * sizeof(uint32_t);
 		read_len += num_items * sizeof(uint32_t);
 	}
+	DIAG_LOG(DIAG_DEBUG_CONTROL,
+		"diag: processing build mask complete (%d)\n", peripheral);
 }
 
 void diag_cntl_process_read_data(struct diagfwd_info *p_info, void *buf,
@@ -700,8 +822,10 @@ void diag_cntl_process_read_data(struct diagfwd_info *p_info, void *buf,
 	uint8_t *ptr = buf;
 	struct diag_ctrl_pkt_header_t *ctrl_pkt = NULL;
 
-	if (!buf || len <= 0 || !p_info)
+	if (!buf || len <= 0 || !p_info) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "diag: Invalid parameters\n");
 		return;
+	}
 
 	if (reg_dirty & PERIPHERAL_MASK(p_info->peripheral)) {
 		pr_err_ratelimited("diag: dropping command registration from peripheral %d\n",
@@ -711,6 +835,9 @@ void diag_cntl_process_read_data(struct diagfwd_info *p_info, void *buf,
 
 	while (read_len + header_len < len) {
 		ctrl_pkt = (struct diag_ctrl_pkt_header_t *)ptr;
+		DIAG_LOG(DIAG_DEBUG_CONTROL,
+			"diag:peripheral: %d: pkt_id: %d\n",
+			p_info->peripheral, ctrl_pkt->pkt_id);
 		switch (ctrl_pkt->pkt_id) {
 		case DIAG_CTRL_MSG_REG:
 			process_command_registration(ptr, ctrl_pkt->len,
@@ -745,13 +872,15 @@ void diag_cntl_process_read_data(struct diagfwd_info *p_info, void *buf,
 						p_info->peripheral);
 			break;
 		default:
-			pr_debug("diag: Control packet %d not supported\n",
-				 ctrl_pkt->pkt_id);
+			DIAG_LOG(DIAG_DEBUG_CONTROL,
+			"diag: Control packet %d not supported\n",
+			 ctrl_pkt->pkt_id);
 		}
 		ptr += header_len + ctrl_pkt->len;
 		read_len += header_len + ctrl_pkt->len;
 	}
-
+	DIAG_LOG(DIAG_DEBUG_CONTROL,
+	"diag: control packet processing complete\n");
 	return;
 }
 
@@ -969,15 +1098,16 @@ void diag_real_time_work_fn(struct work_struct *work)
 	for (i = 0; i < DIAG_NUM_PROC; i++) {
 		temp_real_time = diag_compute_real_time(i);
 		if (temp_real_time == driver->real_time_mode[i]) {
-			pr_debug("diag: did not update real time mode on proc %d, already in the req mode %d",
+			DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+				"diag: did not update real time mode on proc %d, already in the req mode %d\n",
 				i, temp_real_time);
 			continue;
 		}
 
 		if (i == DIAG_LOCAL_PROC) {
 			if (!send_update) {
-				pr_debug("diag: In %s, cannot send real time mode pkt since one of the periperhal is in buffering mode\n",
-					 __func__);
+				DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+				"diag: cannot send real time mode pkt since one of the periperhal is in buffering mode\n");
 				break;
 			}
 			for (j = 0; j < NUM_PERIPHERALS; j++)
@@ -1011,7 +1141,8 @@ void diag_real_time_work_fn(struct work_struct *work)
 			temp_real_time = MODE_NONREALTIME;
 		}
 		if (temp_real_time == driver->real_time_mode[i]) {
-			pr_debug("diag: did not update real time mode on proc %d, already in the req mode %d",
+			DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+				"diag: did not update real time mode on proc %d, already in the req mode %d\n",
 				i, temp_real_time);
 			continue;
 		}
@@ -1046,8 +1177,8 @@ static int __diag_send_real_time_update(uint8_t peripheral, int real_time,
 
 	if (!driver->diagfwd_cntl[peripheral] ||
 	    !driver->diagfwd_cntl[peripheral]->ch_open) {
-		pr_debug("diag: In %s, control channel is not open, p: %d\n",
-			 __func__, peripheral);
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"diag: control channel is not open, p: %d\n", peripheral);
 		return err;
 	}
 
@@ -1194,8 +1325,9 @@ 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);
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: peripheral %d doesn't support buffering\n",
+			 peripheral);
 		driver->buffering_flag[params->peripheral] = 0;
 		return -EIO;
 	}
@@ -1260,8 +1392,9 @@ int diag_send_stm_state(uint8_t peripheral, uint8_t stm_control_data)
 
 	if (!driver->diagfwd_cntl[peripheral] ||
 	    !driver->diagfwd_cntl[peripheral]->ch_open) {
-		pr_debug("diag: In %s, control channel is not open, p: %d\n",
-			 __func__, peripheral);
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: control channel is not open, p: %d\n",
+			 peripheral);
 		return -ENODEV;
 	}
 
@@ -1290,15 +1423,17 @@ int diag_send_peripheral_drain_immediate(uint8_t pd,
 	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",
-			 __func__, peripheral);
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: peripheral  %d doesn't support buffering\n",
+			 peripheral);
 		return -EINVAL;
 	}
 
 	if (!driver->diagfwd_cntl[peripheral] ||
 	    !driver->diagfwd_cntl[peripheral]->ch_open) {
-		pr_debug("diag: In %s, control channel is not open, p: %d\n",
-			 __func__, peripheral);
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: control channel is not open, p: %d\n",
+			 peripheral);
 		return -ENODEV;
 	}
 
@@ -1355,8 +1490,9 @@ int diag_send_buffering_tx_mode_pkt(uint8_t peripheral,
 	}
 
 	if (!driver->feature[peripheral].peripheral_buffering) {
-		pr_debug("diag: In %s, peripheral  %d doesn't support buffering\n",
-			 __func__, peripheral);
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: peripheral  %d doesn't support buffering\n",
+			 peripheral);
 		return -EINVAL;
 	}
 
@@ -1434,15 +1570,17 @@ int diag_send_buffering_wm_values(uint8_t peripheral,
 	}
 
 	if (!driver->feature[peripheral].peripheral_buffering) {
-		pr_debug("diag: In %s, peripheral  %d doesn't support buffering\n",
-			 __func__, peripheral);
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: peripheral  %d doesn't support buffering\n",
+			 peripheral);
 		return -EINVAL;
 	}
 
 	if (!driver->diagfwd_cntl[peripheral] ||
 	    !driver->diagfwd_cntl[peripheral]->ch_open) {
-		pr_debug("diag: In %s, control channel is not open, p: %d\n",
-			 __func__, peripheral);
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: control channel is not open, p: %d\n",
+			 peripheral);
 		return -ENODEV;
 	}
 
diff --git a/drivers/char/diag/diagfwd_peripheral.c b/drivers/char/diag/diagfwd_peripheral.c
index 6f240cacff58..bfdce051d405 100644
--- a/drivers/char/diag/diagfwd_peripheral.c
+++ b/drivers/char/diag/diagfwd_peripheral.c
@@ -728,6 +728,7 @@ static void diagfwd_cntl_read_done(struct diagfwd_info *fwd_info,
 				   unsigned char *buf, int len)
 {
 	if (!fwd_info) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "diag: Invalid fwd_info\n");
 		diag_ws_release();
 		return;
 	}
@@ -748,8 +749,12 @@ static void diagfwd_cntl_read_done(struct diagfwd_info *fwd_info,
 	 */
 	diag_ws_on_copy_fail(DIAG_WS_MUX);
 	/* Reset the buffer in_busy value after processing the data */
-	if (fwd_info->buf_1)
+	if (fwd_info->buf_1) {
 		atomic_set(&fwd_info->buf_1->in_busy, 0);
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+		"Buffer 1 for core PD is marked free, p: %d, t: %d\n",
+			fwd_info->peripheral, fwd_info->type);
+	}
 
 	diagfwd_queue_read(fwd_info);
 	diagfwd_queue_read(&peripheral_info[TYPE_DATA][fwd_info->peripheral]);
@@ -774,8 +779,12 @@ static void diagfwd_dci_read_done(struct diagfwd_info *fwd_info,
 
 	diag_dci_process_peripheral_data(fwd_info, (void *)buf, len);
 	/* Reset the buffer in_busy value after processing the data */
-	if (fwd_info->buf_1)
+	if (fwd_info->buf_1) {
 		atomic_set(&fwd_info->buf_1->in_busy, 0);
+		DIAG_LOG(DIAG_DEBUG_DCI,
+		"Buffer 1 for core PD is marked free, p: %d, t: %d\n",
+			fwd_info->peripheral, fwd_info->type);
+	}
 
 	diagfwd_queue_read(fwd_info);
 }
@@ -1552,13 +1561,15 @@ void diagfwd_channel_read(struct diagfwd_info *fwd_info)
 	struct diagfwd_buf_t *temp_buf = NULL;
 
 	if (!fwd_info) {
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS, "diag: Invalid fwd_info\n");
 		diag_ws_release();
 		return;
 	}
 
 	if (!fwd_info->inited || !atomic_read(&fwd_info->opened)) {
-		pr_debug("diag: In %s, p: %d, t: %d, inited: %d, opened: %d  ch_open: %d\n",
-			 __func__, fwd_info->peripheral, fwd_info->type,
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: p: %d, t: %d, inited: %d, opened: %d, ch_open: %d\n",
+			 fwd_info->peripheral, fwd_info->type,
 			 fwd_info->inited, atomic_read(&fwd_info->opened),
 			 fwd_info->ch_open);
 		diag_ws_release();
@@ -1594,8 +1605,9 @@ void diagfwd_channel_read(struct diagfwd_info *fwd_info)
 			atomic_set(&temp_buf->in_busy, 1);
 		}
 	} else {
-		pr_debug("diag: In %s, both buffers are empty for p: %d, t: %d\n",
-			 __func__, fwd_info->peripheral, fwd_info->type);
+		DIAG_LOG(DIAG_DEBUG_PERIPHERALS,
+			"diag: both buffers are busy for p: %d, t: %d\n",
+			 fwd_info->peripheral, fwd_info->type);
 	}
 
 	if (!read_buf) {