[U-Boot] [PATCH v2 3/7] armv7R: K3: am654: Shut down R5 core after ATF startup on A53

[Lokesh Vutla <lokeshvutla@ti.com>]

From: Andreas Dannenberg <dannenberg@ti.com>

Rather than simply parking the R5 core in WFE after starting up ATF
on A53 instead use SYSFW API to properly shut down the R5 CPU cores
as well as associated timer resources that were pre-allocated. This
allows software further downstream to properly and gracefully bring
the R5 cores back online if desired.

Signed-off-by: Andreas Dannenberg <dannenberg@ti.com>
Signed-off-by: Lokesh Vutla <lokeshvutla@ti.com>
---
 arch/arm/mach-k3/am6_init.c               | 62 +++++++++++++++++++++++
 arch/arm/mach-k3/common.c                 |  6 ++-
 arch/arm/mach-k3/include/mach/sys_proto.h |  1 +
 3 files changed, 68 insertions(+), 1 deletion(-)

diff --git a/arch/arm/mach-k3/am6_init.c b/arch/arm/mach-k3/am6_init.c
index 60a580305d..922bd95580 100644
--- a/arch/arm/mach-k3/am6_init.c
+++ b/arch/arm/mach-k3/am6_init.c
@@ -12,6 +12,7 @@
 #include <asm/arch/hardware.h>
 #include "common.h"
 #include <dm.h>
+#include <linux/soc/ti/ti_sci_protocol.h>
 
 #ifdef CONFIG_SPL_BUILD
 static void mmr_unlock(u32 base, u32 partition)
@@ -185,3 +186,64 @@ void reset_cpu(ulong ignored)
 {
 }
 #endif
+
+#ifdef CONFIG_SYS_K3_SPL_ATF
+
+#define AM6_DEV_MCU_RTI0			134
+#define AM6_DEV_MCU_RTI1			135
+#define AM6_DEV_MCU_ARMSS0_CPU0			159
+#define AM6_DEV_MCU_ARMSS0_CPU1			245
+
+void release_resources_for_core_shutdown(void)
+{
+	struct udevice *dev;
+	struct ti_sci_handle *ti_sci;
+	struct ti_sci_dev_ops *dev_ops;
+	struct ti_sci_proc_ops *proc_ops;
+	int ret;
+	u32 i;
+
+	const u32 put_device_ids[] = {
+		AM6_DEV_MCU_RTI0,
+		AM6_DEV_MCU_RTI1,
+	};
+
+	/* Get handle to Device Management and Security Controller (SYSFW) */
+	ret = uclass_get_device_by_name(UCLASS_FIRMWARE, "dmsc", &dev);
+	if (ret)
+		panic("Failed to get handle to SYSFW (%d)\n", ret);
+
+	ti_sci = (struct ti_sci_handle *)(ti_sci_get_handle_from_sysfw(dev));
+	dev_ops = &ti_sci->ops.dev_ops;
+	proc_ops = &ti_sci->ops.proc_ops;
+
+	/* Iterate through list of devices to put (shutdown) */
+	for (i = 0; i < ARRAY_SIZE(put_device_ids); i++) {
+		u32 id = put_device_ids[i];
+
+		ret = dev_ops->put_device(ti_sci, id);
+		if (ret)
+			panic("Failed to put device %u (%d)\n", id, ret);
+	}
+
+	const u32 put_core_ids[] = {
+		AM6_DEV_MCU_ARMSS0_CPU1,
+		AM6_DEV_MCU_ARMSS0_CPU0,	/* Handle CPU0 after CPU1 */
+	};
+
+	/* Iterate through list of cores to put (shutdown) */
+	for (i = 0; i < ARRAY_SIZE(put_core_ids); i++) {
+		u32 id = put_core_ids[i];
+
+		/*
+		 * Queue up the core shutdown request. Note that this call
+		 * needs to be followed up by an actual invocation of an WFE
+		 * or WFI CPU instruction.
+		 */
+		ret = proc_ops->proc_shutdown_no_wait(ti_sci, id);
+		if (ret)
+			panic("Failed sending core %u shutdown message (%d)\n",
+			      id, ret);
+	}
+}
+#endif
diff --git a/arch/arm/mach-k3/common.c b/arch/arm/mach-k3/common.c
index 03f01d07ea..ee84d44de8 100644
--- a/arch/arm/mach-k3/common.c
+++ b/arch/arm/mach-k3/common.c
@@ -13,6 +13,7 @@
 #include <remoteproc.h>
 #include <linux/soc/ti/ti_sci_protocol.h>
 #include <fdt_support.h>
+#include <asm/arch/sys_proto.h>
 
 struct ti_sci_handle *get_ti_sci_handle(void)
 {
@@ -51,7 +52,10 @@ void __noreturn jump_to_image_no_args(struct spl_image_info *spl_image)
 	if (ret)
 		panic("%s: ATF failed to start on rproc (%d)\n", __func__, ret);
 
-	debug("ATF started. Waiting indefinitely...\n");
+	debug("Releasing resources...\n");
+	release_resources_for_core_shutdown();
+
+	debug("Finalizing core shutdown...\n");
 	while (1)
 		asm volatile("wfe");
 }
diff --git a/arch/arm/mach-k3/include/mach/sys_proto.h b/arch/arm/mach-k3/include/mach/sys_proto.h
index 018725b4d1..2fa53682ad 100644
--- a/arch/arm/mach-k3/include/mach/sys_proto.h
+++ b/arch/arm/mach-k3/include/mach/sys_proto.h
@@ -12,4 +12,5 @@ u32 wait_on_value(u32 read_bit_mask, u32 match_value, void *read_addr,
 		  u32 bound);
 struct ti_sci_handle *get_ti_sci_handle(void);
 int fdt_fixup_msmc_ram(void *blob, char *parent_path, char *node_name);
+void release_resources_for_core_shutdown(void);
 #endif
-- 
2.17.1