lib: utils/andes: arm the SMU sleep command last

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 <[email protected]>
Reviewed-by: Anup Patel <[email protected]>
Link: https://lore.kernel.org/r/[email protected]
Signed-off-by: Anup Patel <[email protected]>

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 d57db8f8..2885a983 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 8d15346a..86cdf955 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 4a053f43..7aafe545 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 dae112ce..dfa9f3c1 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);