[PATCH 7/7] lib: utils/andes: arm the SMU sleep command last

[Ben Zong-You Xie]

Once the sleep command is written, the next WFI puts the core to sleep,
so everything that can fail has to run before it.

Also, use writel() so the command store cannot still be in flight at the
WFI.

Signed-off-by: Ben Zong-You Xie <ben717 at andestech.com>
---
 lib/utils/hsm/fdt_hsm_andes_atcsmu.c         |  4 +--
 lib/utils/suspend/fdt_suspend_andes_atcsmu.c | 31 +++++++++++++++-----
 platform/generic/andes/ae350.c               |  8 +++++
 platform/generic/include/andes/andes.h       |  1 +
 4 files changed, 35 insertions(+), 9 deletions(-)

diff --git a/lib/utils/hsm/fdt_hsm_andes_atcsmu.c b/lib/utils/hsm/fdt_hsm_andes_atcsmu.c
index d57db8f881a6..2885a983c37d 100644
--- a/lib/utils/hsm/fdt_hsm_andes_atcsmu.c
+++ b/lib/utils/hsm/fdt_hsm_andes_atcsmu.c
@@ -56,7 +56,7 @@ bool atcsmu_support_sleep_mode(u32 sleep_type, u32 hartid)
 
 void atcsmu_set_command(u32 pcs_ctl, u32 hartid)
 {
-	writel_relaxed(pcs_ctl, (char *)atcsmu_base + PCSm_CTL_OFFSET(hartid));
+	writel(pcs_ctl, (char *)atcsmu_base + PCSm_CTL_OFFSET(hartid));
 }
 
 int atcsmu_set_reset_vector(u64 wakeup_addr, u32 hartid)
@@ -141,12 +141,12 @@ static int ae350_hart_stop(void)
 		atcsmu_set_command(LIGHT_SLEEP_CMD, hartid);
 	} else if (sleep_type == SBI_SUSP_SLEEP_TYPE_SUSPEND) {
 		/* Power-gated: SMU wakes it via cold reset, interrupts not needed */
-		atcsmu_set_command(DEEP_SLEEP_CMD, hartid);
 		rc = atcsmu_set_reset_vector((ulong)ae350_enable_coherency_warmboot, hartid);
 		if (rc)
 			return SBI_EFAIL;
 
 		ae350_non_ret_save(sbi_scratch_thishart_ptr());
+		atcsmu_set_command(DEEP_SLEEP_CMD, hartid);
 	}
 
 	ae350_disable_coherency();
diff --git a/lib/utils/suspend/fdt_suspend_andes_atcsmu.c b/lib/utils/suspend/fdt_suspend_andes_atcsmu.c
index 8d15346afcfe..86cdf95563ec 100644
--- a/lib/utils/suspend/fdt_suspend_andes_atcsmu.c
+++ b/lib/utils/suspend/fdt_suspend_andes_atcsmu.c
@@ -57,9 +57,11 @@ static int ae350_system_suspend_check(u32 sleep_type)
 static int ae350_system_suspend(u32 sleep_type, unsigned long addr)
 {
 	u32 hartid = current_hartid();
+	unsigned long saved_mie;
 	int rc;
 
 	/* Prevent the core leaving the WFI mode unexpectedly */
+	saved_mie = csr_read(CSR_MIE);
 	csr_write(CSR_MIE, 0);
 
 	/* SMU wakes the primary hart on RTC alarm / UART2 */
@@ -68,7 +70,7 @@ static int ae350_system_suspend(u32 sleep_type, unsigned long addr)
 	if (sleep_type == SBI_SUSP_AE350_LIGHT_SLEEP) {
 		rc = wait_secondary_harts_sleep(hartid, false);
 		if (rc)
-			return rc;
+			goto err_restore_mie;
 
 		/* Clock-gated only: enable SEI to resume past the WFI */
 		csr_set(CSR_MIE, MIP_SEIP);
@@ -76,18 +78,24 @@ static int ae350_system_suspend(u32 sleep_type, unsigned long addr)
 	} else if (sleep_type == SBI_SUSP_SLEEP_TYPE_SUSPEND) {
 		rc = wait_secondary_harts_sleep(hartid, true);
 		if (rc)
-			return rc;
+			goto err_restore_mie;
 
-		atcsmu_set_command(DEEP_SLEEP_CMD, hartid);
 		rc = atcsmu_set_reset_vector((ulong)ae350_enable_coherency_warmboot, hartid);
 		if (rc)
-			return rc;
+			goto err_restore_mie;
 
 		ae350_non_ret_save(sbi_scratch_thishart_ptr());
-		fdt_cmo_llc_enable(false);
+
+		/* No LLC is fine; only fail on real errors */
+		rc = fdt_cmo_llc_enable(false);
+		if (rc && rc != SBI_ENODEV)
+			goto err_discard_save;
+
 		rc = fdt_cmo_llc_flush_all();
-		if (rc)
-			return rc;
+		if (rc && rc != SBI_ENODEV)
+			goto err_enable_llc;
+
+		atcsmu_set_command(DEEP_SLEEP_CMD, hartid);
 	}
 
 	ae350_disable_coherency();
@@ -97,6 +105,15 @@ static int ae350_system_suspend(u32 sleep_type, unsigned long addr)
 	ae350_enable_coherency();
 
 	return SBI_OK;
+
+err_enable_llc:
+	fdt_cmo_llc_enable(true);
+err_discard_save:
+	ae350_non_ret_discard(sbi_scratch_thishart_ptr());
+err_restore_mie:
+	csr_write(CSR_MIE, saved_mie);
+
+	return rc;
 }
 
 static void ae350_system_resume(void)
diff --git a/platform/generic/andes/ae350.c b/platform/generic/andes/ae350.c
index 4a053f434272..7aafe5455ab4 100644
--- a/platform/generic/andes/ae350.c
+++ b/platform/generic/andes/ae350.c
@@ -38,6 +38,14 @@ void ae350_non_ret_save(struct sbi_scratch *scratch)
 	andes_hdata->saved = true;
 }
 
+void ae350_non_ret_discard(struct sbi_scratch *scratch)
+{
+	struct andes_hart_data *andes_hdata = sbi_scratch_offset_ptr(scratch,
+								     andes_hart_data_offset);
+
+	andes_hdata->saved = false;
+}
+
 void ae350_non_ret_restore(struct sbi_scratch *scratch)
 {
 	struct andes_hart_data *andes_hdata = sbi_scratch_offset_ptr(scratch,
diff --git a/platform/generic/include/andes/andes.h b/platform/generic/include/andes/andes.h
index dae112ce741f..dfa9f3c11710 100644
--- a/platform/generic/include/andes/andes.h
+++ b/platform/generic/include/andes/andes.h
@@ -102,6 +102,7 @@ struct andes_hart_data {
 };
 
 void ae350_non_ret_save(struct sbi_scratch *scratch);
+void ae350_non_ret_discard(struct sbi_scratch *scratch);
 void ae350_non_ret_restore(struct sbi_scratch *scratch);
 void ae350_enable_coherency_warmboot(void);
 
-- 
2.34.1