[PATCH 15/17] arm: mach-bcm283x: Add ARMV8_MULTIENTRY support

[Patrick Rudolph]

When ACPI is enabled over FDT the APs cannot be brought out of reset
by the OS using the "FDT spin-table" mechanism, as no FDT is provided
to the OS. The APs must be released out of reset in u-boot and then
brought up in an ACPI compliant fashion.

When ARMV8_MULTIENTRY is specified the APs are released from reset
and will enter U-Boot after it has been relocated as well.

By default ARMV8_MULTIENTRY is not selected, keeping existing behaviour.

TEST: All APs enter U-Boot when run on qemu-system-aarch64

Signed-off-by: Patrick Rudolph <patrick.rudolph at 9elements.com>
Cc: Matthias Brugger <mbrugger at suse.com>
Cc: Peter Robinson <pbrobinson at gmail.com>
Cc: Tom Rini <trini at konsulko.com>
---
 arch/arm/mach-bcm283x/Kconfig |  2 ++
 arch/arm/mach-bcm283x/init.c  | 47 +++++++++++++++++++++++++++++++++++
 2 files changed, 49 insertions(+)

diff --git a/arch/arm/mach-bcm283x/Kconfig b/arch/arm/mach-bcm283x/Kconfig
index b3287ce8bc..de783aa8f5 100644
--- a/arch/arm/mach-bcm283x/Kconfig
+++ b/arch/arm/mach-bcm283x/Kconfig
@@ -12,6 +12,7 @@ config BCM2836
 config BCM2837
 	bool "Broadcom BCM2837 SoC support"
 	depends on ARCH_BCM283X
+	select ARCH_EARLY_INIT_R
 
 config BCM2837_32B
 	bool "Broadcom BCM2837 SoC 32-bit support"
@@ -29,6 +30,7 @@ config BCM2837_64B
 config BCM2711
 	bool "Broadcom BCM2711 SoC support"
 	depends on ARCH_BCM283X
+	select ARCH_EARLY_INIT_R
 
 config BCM2711_32B
 	bool "Broadcom BCM2711 SoC 32-bit support"
diff --git a/arch/arm/mach-bcm283x/init.c b/arch/arm/mach-bcm283x/init.c
index 80a10f2212..f6ccc76f39 100644
--- a/arch/arm/mach-bcm283x/init.c
+++ b/arch/arm/mach-bcm283x/init.c
@@ -7,6 +7,7 @@
  */
 
 #include <acpi/acpi_table.h>
+#include <asm-generic/sections.h>
 #include <cpu_func.h>
 #include <init.h>
 #include <dm/device.h>
@@ -201,6 +202,52 @@ int mach_cpu_init(void)
 					       "brcm,bcm2712-pm");
 	if (offset > soc)
 		rpi_wdog_base = fdt_get_base_address(gd->fdt_blob, offset);
+	return 0;
+}
+
+int arch_early_init_r(void)
+{
+	int cpus, offset;
+	const char *prop;
+	u64 release_addr64;
+	uintptr_t *start_address;
+
+	if (CONFIG_IS_ENABLED(ARMV8_MULTIENTRY)) {
+		/*
+		 * Release CPUs from reset after u-boot has been relocated.
+		 */
+		cpus = fdt_path_offset(gd->fdt_blob, "/cpus");
+		if (cpus < 0)
+			return -ENODEV;
+
+		for (offset = fdt_first_subnode(gd->fdt_blob, cpus);
+		     offset >= 0;
+		     offset = fdt_next_subnode(gd->fdt_blob, offset)) {
+			prop = fdt_getprop(gd->fdt_blob, offset, "device_type", NULL);
+			if (!prop || strcmp(prop, "cpu"))
+				continue;
+
+			prop = fdt_getprop(gd->fdt_blob, offset, "enable-method", NULL);
+			if (!prop || strcmp(prop, "spin-table"))
+				continue;
+
+			release_addr64 = fdtdec_get_uint64(gd->fdt_blob, offset,
+							   "cpu-release-addr", ~0ULL);
+			if (release_addr64 == ~0ULL)
+				continue;
+
+			/* Point to U-Boot start */
+			start_address = (uintptr_t *)(uintptr_t)release_addr64;
+			*start_address = (uintptr_t)_start;
+
+			if (!CONFIG_IS_ENABLED(SYS_DCACHE_OFF))
+				flush_dcache_range(release_addr64,
+						   release_addr64 + sizeof(uintptr_t));
+			/* Send an event to wake up the secondary CPU. */
+			asm("dsb	ishst\n"
+			    "sev");
+		}
+	}
 
 	return 0;
 }
-- 
2.45.2