[PATCH 6/7] lib: utils/suspend: wait for the secondary Andes harts to sleep

[Ben Zong-You Xie]

A secondary hart may not have reached the target sleep state by the time
the primary checks, so the one-shot check could fail spuriously. Poll
each PCS status instead and give up after HART_SLEEP_TIMEOUT_MS.

Rework atcsmu_pcs_is_sleep() into the atcsmu_hart_is_sleep() predicate so
sbi_timer_waitms_until() can drive it.

Signed-off-by: Ben Zong-You Xie <ben717 at andestech.com>
---
 include/sbi_utils/hsm/fdt_hsm_andes_atcsmu.h |  7 ++++-
 lib/utils/hsm/fdt_hsm_andes_atcsmu.c         | 22 ++++++----------
 lib/utils/suspend/fdt_suspend_andes_atcsmu.c | 27 +++++++++++++++-----
 3 files changed, 35 insertions(+), 21 deletions(-)

diff --git a/include/sbi_utils/hsm/fdt_hsm_andes_atcsmu.h b/include/sbi_utils/hsm/fdt_hsm_andes_atcsmu.h
index 20e6cea09379..bb9a2bc2e259 100644
--- a/include/sbi_utils/hsm/fdt_hsm_andes_atcsmu.h
+++ b/include/sbi_utils/hsm/fdt_hsm_andes_atcsmu.h
@@ -9,6 +9,11 @@
 
 #include <sbi/sbi_types.h>
 
+struct atcsmu_sleep_arg {
+	u32 hartid;
+	bool deep_sleep;
+};
+
 /* clang-format off */
 
 #define SCRATCH_PAD_OFFSET		0x40
@@ -61,6 +66,6 @@ int atcsmu_set_reset_vector(u64 wakeup_addr, u32 hartid);
 u32 atcsmu_get_sleep_type(u32 hartid);
 void atcsmu_write_scratch(u32 value);
 u32 atcsmu_read_scratch(void);
-bool atcsmu_pcs_is_sleep(u32 hartid, bool deep_sleep);
+bool atcsmu_hart_is_sleep(void *opaque);
 
 #endif
diff --git a/lib/utils/hsm/fdt_hsm_andes_atcsmu.c b/lib/utils/hsm/fdt_hsm_andes_atcsmu.c
index dfbb425cd2ad..d57db8f881a6 100644
--- a/lib/utils/hsm/fdt_hsm_andes_atcsmu.c
+++ b/lib/utils/hsm/fdt_hsm_andes_atcsmu.c
@@ -94,23 +94,17 @@ u32 atcsmu_read_scratch(void)
 	return readl_relaxed((char *)atcsmu_base + SCRATCH_PAD_OFFSET);
 }
 
-bool atcsmu_pcs_is_sleep(u32 hartid, bool deep_sleep)
+bool atcsmu_hart_is_sleep(void *opaque)
 {
-	u32 pcs_status = readl_relaxed((char *)atcsmu_base + PCSm_STATUS_OFFSET(hartid));
-	u32 pd_status = deep_sleep ? PD_STATUS_DEEP_SLEEP : PD_STATUS_LIGHT_SLEEP;
+	struct atcsmu_sleep_arg *arg = opaque;
 
-	if (EXTRACT_FIELD(pcs_status, PD_TYPE_MASK) != PD_TYPE_SLEEP) {
-		sbi_printf("ATCSMU: hart%d (PCS%d): failed to sleep\n", hartid, hartid + 3);
-		return false;
-	}
-
-	if (EXTRACT_FIELD(pcs_status, PD_STATUS_MASK) != pd_status) {
-		sbi_printf("ATCSMU: hart%d (PCS%d): failed to enter %s sleep\n",
-			   hartid, hartid + 3, deep_sleep ? "deep" : "light");
-		return false;
-	}
+	u32 pcs_status = readl_relaxed((char *)atcsmu_base +
+				       PCSm_STATUS_OFFSET(arg->hartid));
+	u32 pd_status = arg->deep_sleep ? PD_STATUS_DEEP_SLEEP :
+					  PD_STATUS_LIGHT_SLEEP;
 
-	return true;
+	return EXTRACT_FIELD(pcs_status, PD_TYPE_MASK) == PD_TYPE_SLEEP &&
+	       EXTRACT_FIELD(pcs_status, PD_STATUS_MASK) == pd_status;
 }
 
 static int ae350_hart_start(u32 hartid, ulong saddr)
diff --git a/lib/utils/suspend/fdt_suspend_andes_atcsmu.c b/lib/utils/suspend/fdt_suspend_andes_atcsmu.c
index ced0eb41d5b7..8d15346afcfe 100644
--- a/lib/utils/suspend/fdt_suspend_andes_atcsmu.c
+++ b/lib/utils/suspend/fdt_suspend_andes_atcsmu.c
@@ -12,22 +12,37 @@
 #include <sbi/sbi_ecall_interface.h>
 #include <sbi/sbi_hart.h>
 #include <sbi/sbi_system.h>
+#include <sbi/sbi_timer.h>
 #include <sbi_utils/cache/fdt_cmo_helper.h>
 #include <sbi_utils/fdt/fdt_driver.h>
 #include <sbi_utils/fdt/fdt_helper.h>
 #include <sbi_utils/hsm/fdt_hsm_andes_atcsmu.h>
 
-static int check_secondary_harts_sleep(u32 hartid, bool deep_sleep)
+#define HART_SLEEP_TIMEOUT_MS 1000
+
+static int wait_secondary_harts_sleep(u32 hartid, bool deep_sleep)
 {
 	const struct sbi_domain *dom = &root;
 	unsigned long i;
 	u32 target;
+	struct atcsmu_sleep_arg arg;
+
+	arg.deep_sleep = deep_sleep;
 
-	/* Ensure the secondary harts entering the corresponding sleep state */
+	/* Wait for the secondary harts entering the corresponding sleep state */
 	sbi_hartmask_for_each_hartindex(i, dom->possible_harts) {
 		target = sbi_hartindex_to_hartid(i);
-		if (target != hartid && !atcsmu_pcs_is_sleep(target, deep_sleep))
-			return SBI_EFAIL;
+		if (target == hartid)
+			continue;
+
+		arg.hartid = target;
+		if (!sbi_timer_waitms_until(atcsmu_hart_is_sleep, &arg,
+					    HART_SLEEP_TIMEOUT_MS)) {
+			sbi_printf("ATCSMU: hart%u (PCS%u): timed out waiting for %s sleep\n",
+				   target, target + 3,
+				   deep_sleep ? "deep" : "light");
+			return SBI_ETIMEOUT;
+		}
 	}
 
 	return SBI_OK;
@@ -51,7 +66,7 @@ static int ae350_system_suspend(u32 sleep_type, unsigned long addr)
 	atcsmu_set_wakeup_events(PCS_WAKEUP_RTC_ALARM_MASK | PCS_WAKEUP_UART2_MASK, hartid);
 
 	if (sleep_type == SBI_SUSP_AE350_LIGHT_SLEEP) {
-		rc = check_secondary_harts_sleep(hartid, false);
+		rc = wait_secondary_harts_sleep(hartid, false);
 		if (rc)
 			return rc;
 
@@ -59,7 +74,7 @@ static int ae350_system_suspend(u32 sleep_type, unsigned long addr)
 		csr_set(CSR_MIE, MIP_SEIP);
 		atcsmu_set_command(LIGHT_SLEEP_CMD, hartid);
 	} else if (sleep_type == SBI_SUSP_SLEEP_TYPE_SUSPEND) {
-		rc = check_secondary_harts_sleep(hartid, true);
+		rc = wait_secondary_harts_sleep(hartid, true);
 		if (rc)
 			return rc;
 
-- 
2.34.1