keystone2: add possibility to turn off all dsps

By default all DSPs are turned off, for another case option
to turn off them is added in this commit.
Also add command to turn off itself.

Acked-by: Murali Karicheri <m-maricheri2@ti.com>
Signed-off-by: Hao Zhang <hzhang@ti.com>
Signed-off-by: Ivan Khoronzhuk <ivan.khoronzhuk@ti.com>

1 files changed, 59 insertions, 0 deletions

diff --git a/arch/arm/cpu/armv7/keystone/keystone.c b/arch/arm/cpu/armv7/keystone/keystone.c
index 48c869064e..1c8c038455 100644
--- a/arch/arm/cpu/armv7/keystone/keystone.c
+++ b/arch/arm/cpu/armv7/keystone/keystone.c
@@ -9,6 +9,9 @@
 
 #include <common.h>
 #include <asm/io.h>
+#include <asm/arch/mon.h>
+#include <asm/arch/psc_defs.h>
+#include <asm/arch/hardware.h>
 #include <asm/arch/hardware.h>
 
 /**
@@ -26,3 +29,59 @@ int cpu_to_bus(u32 *ptr, u32 length)
 
 	return 0;
 }
+
+static int turn_off_myself(void)
+{
+	printf("Turning off ourselves\r\n");
+	mon_power_off(0);
+
+	psc_disable_module(KS2_LPSC_TETRIS);
+	psc_disable_domain(KS2_TETRIS_PWR_DOMAIN);
+
+	asm volatile ("isb\n"
+		      "dsb\n"
+		      "wfi\n");
+
+	printf("What! Should not see that\n");
+	return 0;
+}
+
+static void turn_off_all_dsps(int num_dsps)
+{
+	int i;
+
+	for (i = 0; i < num_dsps; i++) {
+		if (psc_disable_module(i + KS2_LPSC_GEM_0))
+			printf("Cannot disable module for #%d DSP", i);
+
+		if (psc_disable_domain(i + 8))
+			printf("Cannot disable domain for #%d DSP", i);
+	}
+}
+
+int do_killme_cmd(cmd_tbl_t *cmdtp, int flag, int argc, char * const argv[])
+{
+	return turn_off_myself();
+}
+
+U_BOOT_CMD(
+	killme, 1,      0,      do_killme_cmd,
+	"turn off main ARM core",
+	"turn off main ARM core. Should not live after that :(\n"
+);
+
+int misc_init_r(void)
+{
+	char *env;
+	long ks2_debug = 0;
+
+	env = getenv("ks2_debug");
+
+	if (env)
+		ks2_debug = simple_strtol(env, NULL, 0);
+
+	if ((ks2_debug & DBG_LEAVE_DSPS_ON) == 0)
+		turn_off_all_dsps(KS2_NUM_DSPS);
+
+	return 0;
+}