From b3f03b20135fc5fcd5e6ec7e5ca49f1e59b5602e Mon Sep 17 00:00:00 2001
From: anans <anans@google.com>
Date: Mon, 21 Mar 2022 09:59:18 +0530
Subject: [PATCH] fix(ufs): disables controller if enabled

ufs controller needs to be disabled if already enabled, without
this we noticed a crash at linkstartup during reinit

Signed-off-by: anans <anans@google.com>
Change-Id: I523c5d57c1d34f6404a6368ee3f364fbffd2e542
---
 drivers/ufs/ufs.c     | 32 ++++++++++++++++++++++++++++++++
 include/drivers/ufs.h |  2 ++
 2 files changed, 34 insertions(+)

diff --git a/drivers/ufs/ufs.c b/drivers/ufs/ufs.c
index f4e54b3d3..720b73235 100644
--- a/drivers/ufs/ufs.c
+++ b/drivers/ufs/ufs.c
@@ -146,11 +146,43 @@ static int ufshc_hce_enable(uintptr_t base)
 	return 0;
 }
 
+static int ufshc_hce_disable(uintptr_t base)
+{
+	unsigned int data;
+	int timeout;
+
+	/* Disable Host Controller */
+	mmio_write_32(base + HCE, HCE_DISABLE);
+	timeout = HCE_DISABLE_TIMEOUT_US;
+	do {
+		data = mmio_read_32(base + HCE);
+		if ((data & HCE_ENABLE) == HCE_DISABLE) {
+			break;
+		}
+		udelay(1);
+	} while (--timeout > 0);
+
+	if (timeout <= 0) {
+		return -ETIMEDOUT;
+	}
+
+	return 0;
+}
+
+
 static int ufshc_reset(uintptr_t base)
 {
 	unsigned int data;
 	int retries, result;
 
+	/* disable controller if enabled */
+	if (mmio_read_32(base + HCE) & HCE_ENABLE) {
+		result = ufshc_hce_disable(base);
+		if (result != 0) {
+			return -EIO;
+		}
+	}
+
 	for (retries = 0; retries < HCE_ENABLE_OUTER_RETRIES; ++retries) {
 		result = ufshc_hce_enable(base);
 		if (result == 0) {
diff --git a/include/drivers/ufs.h b/include/drivers/ufs.h
index 8930474a9..4a5e46495 100644
--- a/include/drivers/ufs.h
+++ b/include/drivers/ufs.h
@@ -69,6 +69,7 @@
 /* Host Controller Enable */
 #define HCE				0x34
 #define HCE_ENABLE			1
+#define HCE_DISABLE			0
 
 /* Host UIC Error Code PHY Adapter Layer */
 #define UECPA				0x38
@@ -264,6 +265,7 @@
 #define HCE_ENABLE_OUTER_RETRIES	3
 #define HCE_ENABLE_INNER_RETRIES	50
 #define HCE_ENABLE_TIMEOUT_US		100
+#define HCE_DISABLE_TIMEOUT_US		1000
 
 #define FDEVICEINIT_TIMEOUT_MS	        1500