lib: sbi: Add system_resume callback for restoring the system

The last core who performs the system suspend is responsible for
restoring the system after waking up. Add the system_resume callback for
restoring the system from suspend.

Suggested-by: Anup Patel <[email protected]>
Signed-off-by: Nick Hu <[email protected]>
Reviewed-by: Anup Patel <[email protected]>
Link: https://lore.kernel.org/r/[email protected]
Signed-off-by: Anup Patel <[email protected]>

3 files changed, 28 insertions, 1 deletions

diff --git a/include/sbi/sbi_system.h b/include/sbi/sbi_system.h
index 0fdcc98c..f48a019a 100644
--- a/include/sbi/sbi_system.h
+++ b/include/sbi/sbi_system.h
@@ -69,11 +69,18 @@ struct sbi_system_suspend_device {
 	 *     return from system_suspend() may ignore this parameter.
 	 */
 	int (*system_suspend)(u32 sleep_type, unsigned long mmode_resume_addr);
+
+	/**
+	 * Resume the system from system suspend
+	 */
+	void (*system_resume)(void);
 };
 
 const struct sbi_system_suspend_device *sbi_system_suspend_get_device(void);
 void sbi_system_suspend_set_device(struct sbi_system_suspend_device *dev);
 void sbi_system_suspend_test_enable(void);
+void sbi_system_resume(void);
+bool sbi_system_is_suspended(void);
 bool sbi_system_suspend_supported(u32 sleep_type);
 int sbi_system_suspend(u32 sleep_type, ulong resume_addr, ulong opaque);
 
diff --git a/lib/sbi/sbi_hsm.c b/lib/sbi/sbi_hsm.c
index 0d97b43e..0a355f9c 100644
--- a/lib/sbi/sbi_hsm.c
+++ b/lib/sbi/sbi_hsm.c
@@ -455,7 +455,10 @@ void sbi_hsm_hart_resume_start(struct sbi_scratch *scratch)
 					 SBI_HSM_STATE_RESUME_PENDING))
 		sbi_hart_hang();
 
-	hsm_device_hart_resume();
+	if (sbi_system_is_suspended())
+		sbi_system_resume();
+	else
+		hsm_device_hart_resume();
 }
 
 void __noreturn sbi_hsm_hart_resume_finish(struct sbi_scratch *scratch,
diff --git a/lib/sbi/sbi_system.c b/lib/sbi/sbi_system.c
index cd0f4ba4..5500b05d 100644
--- a/lib/sbi/sbi_system.c
+++ b/lib/sbi/sbi_system.c
@@ -87,6 +87,7 @@ void __noreturn sbi_system_reset(u32 reset_type, u32 reset_reason)
 }
 
 static const struct sbi_system_suspend_device *suspend_dev = NULL;
+static bool system_suspended;
 
 const struct sbi_system_suspend_device *sbi_system_suspend_get_device(void)
 {
@@ -137,6 +138,19 @@ bool sbi_system_suspend_supported(u32 sleep_type)
 	       suspend_dev->system_suspend_check(sleep_type) == 0;
 }
 
+bool sbi_system_is_suspended(void)
+{
+	return system_suspended;
+}
+
+void sbi_system_resume(void)
+{
+	if (suspend_dev && suspend_dev->system_resume)
+		suspend_dev->system_resume();
+
+	system_suspended = false;
+}
+
 int sbi_system_suspend(u32 sleep_type, ulong resume_addr, ulong opaque)
 {
 	struct sbi_domain *dom = sbi_domain_thishart_ptr();
@@ -189,11 +203,14 @@ int sbi_system_suspend(u32 sleep_type, ulong resume_addr, ulong opaque)
 	__sbi_hsm_suspend_non_ret_save(scratch);
 
 	/* Suspend */
+	system_suspended = true;
 	ret = suspend_dev->system_suspend(sleep_type, scratch->warmboot_addr);
 	if (ret != SBI_OK) {
 		if (!sbi_hsm_hart_change_state(scratch, SBI_HSM_STATE_SUSPENDED,
 					       SBI_HSM_STATE_STARTED))
 			sbi_hart_hang();
+
+		system_suspended = false;
 		return ret;
 	}