1 files changed, 51 insertions, 0 deletions
diff --git a/arch/arm/mach-k3/am642_init.c b/arch/arm/mach-k3/am642_init.c
index d09fc59146..2b58a9f94c 100644
--- a/arch/arm/mach-k3/am642_init.c
+++ b/arch/arm/mach-k3/am642_init.c
@@ -230,3 +230,54 @@ u32 spl_boot_device(void)
 		return __get_backup_bootmedia(devstat);
 }
 #endif
+
+#if defined(CONFIG_SYS_K3_SPL_ATF)
+
+#define AM64X_DEV_RTI8			127
+#define AM64X_DEV_RTI9			128
+#define AM64X_DEV_R5FSS0_CORE0		121
+#define AM64X_DEV_R5FSS0_CORE1		122
+
+void release_resources_for_core_shutdown(void)
+{
+	struct ti_sci_handle *ti_sci = get_ti_sci_handle();
+	struct ti_sci_dev_ops *dev_ops = &ti_sci->ops.dev_ops;
+	struct ti_sci_proc_ops *proc_ops = &ti_sci->ops.proc_ops;
+	int ret;
+	u32 i;
+
+	const u32 put_device_ids[] = {
+		AM64X_DEV_RTI9,
+		AM64X_DEV_RTI8,
+	};
+
+	/* 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[] = {
+		AM64X_DEV_R5FSS0_CORE1,
+		AM64X_DEV_R5FSS0_CORE0, /* 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