[PATCH 9/9] cmd: test_enter_suspend: Add test_enter_suspend command

[Thomas Richard]

From: Gregory CLEMENT <gregory.clement at bootlin.com>

Add test_enter_suspend command using PSCI.
This command is only to test suspend sequence from U-Boot.
The resume will not work.

Signed-off-by: Gregory CLEMENT <gregory.clement at bootlin.com>
Signed-off-by: Thomas Richard <thomas.richard at bootlin.com>

---

 cmd/Kconfig             |  7 +++++++
 cmd/boot.c              |  8 ++++++++
 drivers/firmware/psci.c | 23 +++++++++++++++++++++++
 include/command.h       |  2 ++
 4 files changed, 40 insertions(+)

diff --git a/cmd/Kconfig b/cmd/Kconfig
index 43ca10f69c..54e7d4b0e6 100644
--- a/cmd/Kconfig
+++ b/cmd/Kconfig
@@ -1450,6 +1450,13 @@ config CMD_POWEROFF
 	help
 	  Poweroff/Shutdown the system
 
+config CMD_TEST_ENTER_SUSPEND
+	bool "test enter suspend"
+	depends on ARM
+	select ARM_SMCCC
+	help
+	  Command to test the suspend sequence (only for test)
+
 config CMD_READ
 	bool "read - Read binary data from a partition"
 	help
diff --git a/cmd/boot.c b/cmd/boot.c
index 14839c1ced..609c07a5a5 100644
--- a/cmd/boot.c
+++ b/cmd/boot.c
@@ -70,3 +70,11 @@ U_BOOT_CMD(
 	""
 );
 #endif
+
+#if IS_ENABLED(CONFIG_CMD_TEST_ENTER_SUSPEND)
+U_BOOT_CMD(
+	test_enter_suspend, 1, 0, do_test_enter_suspend,
+	"Test the enter SUSPEND sequence of the device",
+	""
+);
+#endif
diff --git a/drivers/firmware/psci.c b/drivers/firmware/psci.c
index c6b9efab41..591ad1869d 100644
--- a/drivers/firmware/psci.c
+++ b/drivers/firmware/psci.c
@@ -23,6 +23,9 @@
 #include <linux/libfdt.h>
 #include <linux/printk.h>
 #include <linux/psci.h>
+#if IS_ENABLED(CONFIG_CMD_TEST_ENTER_SUSPEND)
+#include <cpu_func.h>
+#endif
 
 #define DRIVER_NAME "psci"
 
@@ -303,6 +306,26 @@ int do_poweroff(struct cmd_tbl *cmdtp, int flag, int argc, char *const argv[])
 }
 #endif
 
+#if IS_ENABLED(CONFIG_CMD_TEST_ENTER_SUSPEND)
+int do_test_enter_suspend(struct cmd_tbl *cmdtp, int flag, int argc, char *const argv[])
+{
+	do_psci_probe();
+
+	puts("suspend ...\n");
+	puts("disable dcache\n");
+
+	dcache_disable();
+
+	/* ARM_PSCI_1_0_FN64_SYSTEM_SUSPEND */
+	invoke_psci_fn(PSCI_0_2_FN64(14), (unsigned long)do_test_enter_suspend, 0, 0);
+
+	dcache_enable();
+
+	log_err("Suspend failed or not supported on this platform\n");
+	return CMD_RET_FAILURE;
+}
+#endif
+
 static const struct udevice_id psci_of_match[] = {
 	{ .compatible = "arm,psci" },
 	{ .compatible = "arm,psci-0.2" },
diff --git a/include/command.h b/include/command.h
index c4e3170967..0126da6b37 100644
--- a/include/command.h
+++ b/include/command.h
@@ -177,6 +177,8 @@ int do_reset(struct cmd_tbl *cmdtp, int flag, int argc,
 	     char *const argv[]);
 int do_poweroff(struct cmd_tbl *cmdtp, int flag, int argc,
 		char *const argv[]);
+int do_test_enter_suspend(struct cmd_tbl *cmdtp, int flag, int argc,
+			  char *const argv[]);
 
 unsigned long do_go_exec(ulong (*entry)(int, char * const []), int argc,
 			 char *const argv[]);
-- 
2.39.2