diag: Add mdlog session to apps pkt response path

Apps pkt cmd response defaults to APPS_DATA peripheral.
Error response for mdlogging does not go to the right md
session. Pass md session along response path to ensure
correct destination.

Change-Id: Iece75fdb580a68350085bf48df374e60c2b4adb6
Signed-off-by: Chris Lew <clew@codeaurora.org>

3 files changed, 62 insertions, 40 deletions

diff --git a/drivers/char/diag/diagchar_core.c b/drivers/char/diag/diagchar_core.c
index facdb0f40e44..335064352789 100644
--- a/drivers/char/diag/diagchar_core.c
+++ b/drivers/char/diag/diagchar_core.c
@@ -2606,7 +2606,8 @@ static int diag_user_process_raw_data(const char __user *buf, int len)
 		info = diag_md_session_get_pid(current->tgid);
 		ret = diag_process_apps_pkt(user_space_data, len, info);
 		if (ret == 1)
-			diag_send_error_rsp((void *)(user_space_data), len);
+			diag_send_error_rsp((void *)(user_space_data), len,
+						info);
 	}
 fail:
 	diagmem_free(driver, user_space_data, mempool);
diff --git a/drivers/char/diag/diagfwd.c b/drivers/char/diag/diagfwd.c
index a7069bc0edf3..99a16dd47cd4 100644
--- a/drivers/char/diag/diagfwd.c
+++ b/drivers/char/diag/diagfwd.c
@@ -1,4 +1,4 @@
-/* Copyright (c) 2008-2016, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2008-2017, 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
@@ -239,10 +239,11 @@ void chk_logging_wakeup(void)
 	}
 }
 
-static void pack_rsp_and_send(unsigned char *buf, int len)
+static void pack_rsp_and_send(unsigned char *buf, int len,
+				struct diag_md_session_t *info)
 {
 	int err;
-	int retry_count = 0;
+	int retry_count = 0, i, rsp_ctxt;
 	uint32_t write_len = 0;
 	unsigned long flags;
 	unsigned char *rsp_ptr = driver->encoded_rsp_buf;
@@ -257,6 +258,15 @@ static void pack_rsp_and_send(unsigned char *buf, int len)
 		return;
 	}
 
+	if (info && info->peripheral_mask) {
+		for (i = 0; i <= NUM_PERIPHERALS; i++) {
+			if (info->peripheral_mask & (1 << i))
+				break;
+		}
+		rsp_ctxt = SET_BUF_CTXT(i, TYPE_CMD, 1);
+	} else
+		rsp_ctxt = driver->rsp_buf_ctxt;
+
 	/*
 	 * Keep trying till we get the buffer back. It should probably
 	 * take one or two iterations. When this loops till UINT_MAX, it
@@ -298,8 +308,7 @@ static void pack_rsp_and_send(unsigned char *buf, int len)
 	*(uint8_t *)(rsp_ptr + write_len) = CONTROL_CHAR;
 	write_len += sizeof(uint8_t);
 
-	err = diag_mux_write(DIAG_LOCAL_PROC, rsp_ptr, write_len,
-			     driver->rsp_buf_ctxt);
+	err = diag_mux_write(DIAG_LOCAL_PROC, rsp_ptr, write_len, rsp_ctxt);
 	if (err) {
 		pr_err("diag: In %s, unable to write to mux, err: %d\n",
 		       __func__, err);
@@ -309,12 +318,13 @@ static void pack_rsp_and_send(unsigned char *buf, int len)
 	}
 }
 
-static void encode_rsp_and_send(unsigned char *buf, int len)
+static void encode_rsp_and_send(unsigned char *buf, int len,
+				struct diag_md_session_t *info)
 {
 	struct diag_send_desc_type send = { NULL, NULL, DIAG_STATE_START, 0 };
 	struct diag_hdlc_dest_type enc = { NULL, NULL, 0 };
 	unsigned char *rsp_ptr = driver->encoded_rsp_buf;
-	int err, retry_count = 0;
+	int err, i, rsp_ctxt, retry_count = 0;
 	unsigned long flags;
 
 	if (!rsp_ptr || !buf)
@@ -326,6 +336,15 @@ static void encode_rsp_and_send(unsigned char *buf, int len)
 		return;
 	}
 
+	if (info && info->peripheral_mask) {
+		for (i = 0; i <= NUM_PERIPHERALS; i++) {
+			if (info->peripheral_mask & (1 << i))
+				break;
+		}
+		rsp_ctxt = SET_BUF_CTXT(i, TYPE_CMD, 1);
+	} else
+		rsp_ctxt = driver->rsp_buf_ctxt;
+
 	/*
 	 * Keep trying till we get the buffer back. It should probably
 	 * take one or two iterations. When this loops till UINT_MAX, it
@@ -369,7 +388,7 @@ static void encode_rsp_and_send(unsigned char *buf, int len)
 	diag_hdlc_encode(&send, &enc);
 	driver->encoded_rsp_len = (int)(enc.dest - (void *)rsp_ptr);
 	err = diag_mux_write(DIAG_LOCAL_PROC, rsp_ptr, driver->encoded_rsp_len,
-			     driver->rsp_buf_ctxt);
+			     rsp_ctxt);
 	if (err) {
 		pr_err("diag: In %s, Unable to write to device, err: %d\n",
 			__func__, err);
@@ -380,21 +399,22 @@ static void encode_rsp_and_send(unsigned char *buf, int len)
 	memset(buf, '\0', DIAG_MAX_RSP_SIZE);
 }
 
-void diag_send_rsp(unsigned char *buf, int len)
+void diag_send_rsp(unsigned char *buf, int len, struct diag_md_session_t *info)
 {
 	struct diag_md_session_t *session_info = NULL;
 	uint8_t hdlc_disabled;
 
-	session_info = diag_md_session_get_peripheral(APPS_DATA);
+	session_info = (info) ? info :
+				diag_md_session_get_peripheral(APPS_DATA);
 	if (session_info)
 		hdlc_disabled = session_info->hdlc_disabled;
 	else
 		hdlc_disabled = driver->hdlc_disabled;
 
 	if (hdlc_disabled)
-		pack_rsp_and_send(buf, len);
+		pack_rsp_and_send(buf, len, session_info);
 	else
-		encode_rsp_and_send(buf, len);
+		encode_rsp_and_send(buf, len, session_info);
 }
 
 void diag_update_pkt_buffer(unsigned char *buf, uint32_t len, int type)
@@ -865,7 +885,8 @@ static int diag_cmd_disable_hdlc(unsigned char *src_buf, int src_len,
 	return write_len;
 }
 
-void diag_send_error_rsp(unsigned char *buf, int len)
+void diag_send_error_rsp(unsigned char *buf, int len,
+			struct diag_md_session_t *info)
 {
 	/* -1 to accomodate the first byte 0x13 */
 	if (len > (DIAG_MAX_RSP_SIZE - 1)) {
@@ -875,7 +896,7 @@ void diag_send_error_rsp(unsigned char *buf, int len)
 
 	*(uint8_t *)driver->apps_rsp_buf = DIAG_CMD_ERROR;
 	memcpy((driver->apps_rsp_buf + sizeof(uint8_t)), buf, len);
-	diag_send_rsp(driver->apps_rsp_buf, len + 1);
+	diag_send_rsp(driver->apps_rsp_buf, len + 1, info);
 }
 
 int diag_process_apps_pkt(unsigned char *buf, int len,
@@ -895,7 +916,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
 	/* Check if the command is a supported mask command */
 	mask_ret = diag_process_apps_masks(buf, len, info);
 	if (mask_ret > 0) {
-		diag_send_rsp(driver->apps_rsp_buf, mask_ret);
+		diag_send_rsp(driver->apps_rsp_buf, mask_ret, info);
 		return 0;
 	}
 
@@ -917,7 +938,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
 						   driver->apps_rsp_buf,
 						   DIAG_MAX_RSP_SIZE);
 		if (write_len > 0)
-			diag_send_rsp(driver->apps_rsp_buf, write_len);
+			diag_send_rsp(driver->apps_rsp_buf, write_len, info);
 		return 0;
 	}
 
@@ -933,7 +954,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
 		} else {
 			if (MD_PERIPHERAL_MASK(reg_item->proc) &
 				driver->logging_mask)
-				diag_send_error_rsp(buf, len);
+				diag_send_error_rsp(buf, len, info);
 			else
 				write_len = diag_send_data(reg_item, buf, len);
 		}
@@ -949,13 +970,13 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
 		for (i = 0; i < 4; i++)
 			*(driver->apps_rsp_buf+i) = *(buf+i);
 		*(uint32_t *)(driver->apps_rsp_buf+4) = DIAG_MAX_REQ_SIZE;
-		diag_send_rsp(driver->apps_rsp_buf, 8);
+		diag_send_rsp(driver->apps_rsp_buf, 8, info);
 		return 0;
 	} else if ((*buf == 0x4b) && (*(buf+1) == 0x12) &&
 		(*(uint16_t *)(buf+2) == DIAG_DIAG_STM)) {
 		len = diag_process_stm_cmd(buf, driver->apps_rsp_buf);
 		if (len > 0) {
-			diag_send_rsp(driver->apps_rsp_buf, len);
+			diag_send_rsp(driver->apps_rsp_buf, len, info);
 			return 0;
 		}
 		return len;
@@ -968,7 +989,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
 							driver->apps_rsp_buf,
 							DIAG_MAX_RSP_SIZE);
 		if (write_len > 0)
-			diag_send_rsp(driver->apps_rsp_buf, write_len);
+			diag_send_rsp(driver->apps_rsp_buf, write_len, info);
 		return 0;
 	}
 	/* Check for time sync switch command */
@@ -979,14 +1000,14 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
 							driver->apps_rsp_buf,
 							DIAG_MAX_RSP_SIZE);
 		if (write_len > 0)
-			diag_send_rsp(driver->apps_rsp_buf, write_len);
+			diag_send_rsp(driver->apps_rsp_buf, write_len, info);
 		return 0;
 	}
 	/* Check for download command */
 	else if ((chk_apps_master()) && (*buf == 0x3A)) {
 		/* send response back */
 		driver->apps_rsp_buf[0] = *buf;
-		diag_send_rsp(driver->apps_rsp_buf, 1);
+		diag_send_rsp(driver->apps_rsp_buf, 1, info);
 		msleep(5000);
 		/* call download API */
 		msm_set_restart_mode(RESTART_DLOAD);
@@ -1006,7 +1027,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
 			for (i = 0; i < 13; i++)
 				driver->apps_rsp_buf[i+3] = 0;
 
-			diag_send_rsp(driver->apps_rsp_buf, 16);
+			diag_send_rsp(driver->apps_rsp_buf, 16, info);
 			return 0;
 		}
 	}
@@ -1015,7 +1036,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
 		(*(buf+2) == 0x04) && (*(buf+3) == 0x0)) {
 		memcpy(driver->apps_rsp_buf, buf, 4);
 		driver->apps_rsp_buf[4] = wrap_enabled;
-		diag_send_rsp(driver->apps_rsp_buf, 5);
+		diag_send_rsp(driver->apps_rsp_buf, 5, info);
 		return 0;
 	}
 	/* Wrap the Delayed Rsp ID */
@@ -1024,7 +1045,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
 		wrap_enabled = true;
 		memcpy(driver->apps_rsp_buf, buf, 4);
 		driver->apps_rsp_buf[4] = wrap_count;
-		diag_send_rsp(driver->apps_rsp_buf, 6);
+		diag_send_rsp(driver->apps_rsp_buf, 6, info);
 		return 0;
 	}
 	/* Mobile ID Rsp */
@@ -1035,7 +1056,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
 						   driver->apps_rsp_buf,
 						   DIAG_MAX_RSP_SIZE);
 		if (write_len > 0) {
-			diag_send_rsp(driver->apps_rsp_buf, write_len);
+			diag_send_rsp(driver->apps_rsp_buf, write_len, info);
 			return 0;
 		}
 	}
@@ -1055,7 +1076,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
 			for (i = 0; i < 55; i++)
 				driver->apps_rsp_buf[i] = 0;
 
-			diag_send_rsp(driver->apps_rsp_buf, 55);
+			diag_send_rsp(driver->apps_rsp_buf, 55, info);
 			return 0;
 		}
 		/* respond to 0x7c command */
@@ -1068,14 +1089,14 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
 							 chk_config_get_id();
 			*(unsigned char *)(driver->apps_rsp_buf + 12) = '\0';
 			*(unsigned char *)(driver->apps_rsp_buf + 13) = '\0';
-			diag_send_rsp(driver->apps_rsp_buf, 14);
+			diag_send_rsp(driver->apps_rsp_buf, 14, info);
 			return 0;
 		}
 	}
 	write_len = diag_cmd_chk_stats(buf, len, driver->apps_rsp_buf,
 				       DIAG_MAX_RSP_SIZE);
 	if (write_len > 0) {
-		diag_send_rsp(driver->apps_rsp_buf, write_len);
+		diag_send_rsp(driver->apps_rsp_buf, write_len, info);
 		return 0;
 	}
 	write_len = diag_cmd_disable_hdlc(buf, len, driver->apps_rsp_buf,
@@ -1087,7 +1108,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
 		 * before disabling HDLC encoding on Apps processor.
 		 */
 		mutex_lock(&driver->hdlc_disable_mutex);
-		diag_send_rsp(driver->apps_rsp_buf, write_len);
+		diag_send_rsp(driver->apps_rsp_buf, write_len, info);
 		/*
 		 * Set the value of hdlc_disabled after sending the response to
 		 * the tools. This is required since the tools is expecting a
@@ -1107,7 +1128,7 @@ int diag_process_apps_pkt(unsigned char *buf, int len,
 
 	/* We have now come to the end of the function. */
 	if (chk_apps_only())
-		diag_send_error_rsp(buf, len);
+		diag_send_error_rsp(buf, len, info);
 
 	return 0;
 }
@@ -1190,7 +1211,7 @@ fail:
 	 * recovery algorithm. Send an error response if the
 	 * packet is not in expected format.
 	 */
-	diag_send_error_rsp(driver->hdlc_buf, driver->hdlc_buf_len);
+	diag_send_error_rsp(driver->hdlc_buf, driver->hdlc_buf_len, info);
 	driver->hdlc_buf_len = 0;
 end:
 	mutex_unlock(&driver->diag_hdlc_mutex);
@@ -1446,7 +1467,7 @@ start:
 
 		if (actual_pkt->start != CONTROL_CHAR) {
 			diag_hdlc_start_recovery(buf, len, info);
-			diag_send_error_rsp(buf, len);
+			diag_send_error_rsp(buf, len, info);
 			goto end;
 		}
 
@@ -1528,15 +1549,14 @@ static int diagfwd_mux_write_done(unsigned char *buf, int len, int buf_ctxt,
 	case TYPE_CMD:
 		if (peripheral >= 0 && peripheral < NUM_PERIPHERALS) {
 			diagfwd_write_done(peripheral, type, num);
-		} else if (peripheral == APPS_DATA) {
+		}
+		if (peripheral == APPS_DATA ||
+				ctxt == DIAG_MEMORY_DEVICE_MODE) {
 			spin_lock_irqsave(&driver->rsp_buf_busy_lock, flags);
 			driver->rsp_buf_busy = 0;
 			driver->encoded_rsp_len = 0;
 			spin_unlock_irqrestore(&driver->rsp_buf_busy_lock,
 					       flags);
-		} else {
-			pr_err_ratelimited("diag: Invalid peripheral %d in %s, type: %d\n",
-					   peripheral, __func__, type);
 		}
 		break;
 	default:
diff --git a/drivers/char/diag/diagfwd.h b/drivers/char/diag/diagfwd.h
index 0023e0638aa2..4c6d86fc36ae 100644
--- a/drivers/char/diag/diagfwd.h
+++ b/drivers/char/diag/diagfwd.h
@@ -1,4 +1,4 @@
-/* Copyright (c) 2008-2015, The Linux Foundation. All rights reserved.
+/* Copyright (c) 2008-2015, 2017 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
@@ -45,7 +45,8 @@ void diag_update_userspace_clients(unsigned int type);
 void diag_update_sleeping_process(int process_id, int data_type);
 int diag_process_apps_pkt(unsigned char *buf, int len,
 			  struct diag_md_session_t *info);
-void diag_send_error_rsp(unsigned char *buf, int len);
+void diag_send_error_rsp(unsigned char *buf, int len,
+			 struct diag_md_session_t *info);
 void diag_update_pkt_buffer(unsigned char *buf, uint32_t len, int type);
 int diag_process_stm_cmd(unsigned char *buf, unsigned char *dest_buf);
 void diag_md_hdlc_reset_timer_func(unsigned long pid);