[PATCH V5 5/5] firmware: scm: Modify only the DLOAD bit in TCSR register for download mode

[Poovendhan Selvaraj]

CrashDump collection is based on the DLOAD bit of TCSR register.
To retain other bits, we read the register and modify only the DLOAD bit as
the other bits have their own significance.

Co-developed-by: Anusha Rao <quic_anusha@xxxxxxxxxxx>
Signed-off-by: Anusha Rao <quic_anusha@xxxxxxxxxxx>
Co-developed-by: Kathiravan Thirumoorthy <quic_kathirav@xxxxxxxxxxx>
Signed-off-by: Kathiravan Thirumoorthy <quic_kathirav@xxxxxxxxxxx>
Signed-off-by: Poovendhan Selvaraj <quic_poovendh@xxxxxxxxxxx>
---
 Changes in V5:
	- checking the return value in qcom_scm_set_download_mode function as
	  suggested by Srinivas Kandagatla

 Changes in V4:
	- retain the orginal value of tcsr register when download mode
	  is not set

 drivers/firmware/qcom_scm.c | 21 ++++++++++++++++-----
 1 file changed, 16 insertions(+), 5 deletions(-)

diff --git a/drivers/firmware/qcom_scm.c b/drivers/firmware/qcom_scm.c
index 468d4d5ab550..d88c5f14bd54 100644
--- a/drivers/firmware/qcom_scm.c
+++ b/drivers/firmware/qcom_scm.c
@@ -407,7 +407,7 @@ int qcom_scm_set_remote_state(u32 state, u32 id)
 }
 EXPORT_SYMBOL(qcom_scm_set_remote_state);
 
-static int __qcom_scm_set_dload_mode(struct device *dev, bool enable)
+static int __qcom_scm_set_dload_mode(struct device *dev, u32 val, bool enable)
 {
 	struct qcom_scm_desc desc = {
 		.svc = QCOM_SCM_SVC_BOOT,
@@ -417,7 +417,8 @@ static int __qcom_scm_set_dload_mode(struct device *dev, bool enable)
 		.owner = ARM_SMCCC_OWNER_SIP,
 	};
 
-	desc.args[1] = enable ? QCOM_SCM_BOOT_SET_DLOAD_MODE : 0;
+	desc.args[1] = enable ? val | QCOM_SCM_BOOT_SET_DLOAD_MODE :
+				val & ~(QCOM_SCM_BOOT_SET_DLOAD_MODE);
 
 	return qcom_scm_call_atomic(__scm->dev, &desc, NULL);
 }
@@ -426,15 +427,25 @@ static void qcom_scm_set_download_mode(bool enable)
 {
 	bool avail;
 	int ret = 0;
+	u32 dload_addr_val;
 
 	avail = __qcom_scm_is_call_available(__scm->dev,
 					     QCOM_SCM_SVC_BOOT,
 					     QCOM_SCM_BOOT_SET_DLOAD_MODE);
+	ret = qcom_scm_io_readl(__scm->dload_mode_addr, &dload_addr_val);
+
+	if (ret) {
+		dev_err(__scm->dev,
+			"failed to read dload mode address value: %d\n", ret);
+		return;
+	}
+
 	if (avail) {
-		ret = __qcom_scm_set_dload_mode(__scm->dev, enable);
+		ret = __qcom_scm_set_dload_mode(__scm->dev, dload_addr_val, enable);
 	} else if (__scm->dload_mode_addr) {
-		ret = qcom_scm_io_writel(__scm->dload_mode_addr,
-				enable ? QCOM_SCM_BOOT_SET_DLOAD_MODE : 0);
+		ret = qcom_scm_io_writel(__scm->dload_mode_addr, enable ?
+				dload_addr_val | QCOM_SCM_BOOT_SET_DLOAD_MODE :
+				dload_addr_val & ~(QCOM_SCM_BOOT_SET_DLOAD_MODE));
 	} else {
 		dev_err(__scm->dev,
 			"No available mechanism for setting download mode\n");
-- 
2.17.1