Merge branch 'for-next/qemu'

3 files changed, 148 insertions, 0 deletions

diff --git a/arch/arm/cpu/Makefile b/arch/arm/cpu/Makefile
index 9b737f80ef..97e4eb52e3 100644
--- a/arch/arm/cpu/Makefile
+++ b/arch/arm/cpu/Makefile
@@ -10,6 +10,9 @@ AFLAGS_pbl-hyp.o :=-Wa,-march=armv7-a -Wa,-mcpu=all
 
 obj-y += start.o entry.o
 
+pbl-$(CONFIG_BOARD_ARM_GENERIC_DT) += board-dt-2nd.o
+pbl-$(CONFIG_BOARD_ARM_GENERIC_DT_AARCH64) += board-dt-2nd-aarch64.o
+
 obj-pbl-y += setupc$(S64).o cache$(S64).o
 
 obj-$(CONFIG_BOOTM_OPTEE) += start-kernel-optee.o
diff --git a/arch/arm/cpu/board-dt-2nd-aarch64.S b/arch/arm/cpu/board-dt-2nd-aarch64.S
new file mode 100644
index 0000000000..0540a1690d
--- /dev/null
+++ b/arch/arm/cpu/board-dt-2nd-aarch64.S
@@ -0,0 +1,11 @@
+/* SPDX-License-Identifier: GPL-2.0-or-later */
+#include <linux/linkage.h>
+#include <asm/barebox-arm64.h>
+
+ENTRY_PROC(start_dt_2nd)
+	adr x1, stack
+	mov sp, x1
+	b dt_2nd_aarch64
+.word 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0, 0x0
+stack:
+ENTRY_PROC_END(start_dt_2nd)
diff --git a/arch/arm/cpu/board-dt-2nd.c b/arch/arm/cpu/board-dt-2nd.c
new file mode 100644
index 0000000000..4e7d575e8a
--- /dev/null
+++ b/arch/arm/cpu/board-dt-2nd.c
@@ -0,0 +1,134 @@
+// SPDX-License-Identifier: GPL-2.0
+
+#include <common.h>
+#include <linux/sizes.h>
+#include <asm/barebox-arm-head.h>
+#include <asm/barebox-arm.h>
+#include <io.h>
+#include <debug_ll.h>
+#include <asm/cache.h>
+#include <asm/sections.h>
+#include <linux/libfdt.h>
+
+static void of_find_mem(void *fdt, unsigned long *membase, unsigned long *memsize)
+{
+	const __be32 *nap, *nsp, *reg;
+	uint32_t na, ns;
+	uint64_t memsize64, membase64;
+	int node, size, i;
+
+	/* Make sure FDT blob is sane */
+	if (fdt_check_header(fdt) != 0) {
+		pr_err("Invalid device tree blob\n");
+		goto err;
+	}
+
+	/* Find the #address-cells and #size-cells properties */
+	node = fdt_path_offset(fdt, "/");
+	if (node < 0) {
+		pr_err("Cannot find root node\n");
+		goto err;
+	}
+
+	nap = fdt_getprop(fdt, node, "#address-cells", &size);
+	if (!nap || (size != 4)) {
+		pr_err("Cannot find #address-cells property");
+		goto err;
+	}
+	na = fdt32_to_cpu(*nap);
+
+	nsp = fdt_getprop(fdt, node, "#size-cells", &size);
+	if (!nsp || (size != 4)) {
+		pr_err("Cannot find #size-cells property");
+		goto err;
+	}
+	ns = fdt32_to_cpu(*nap);
+
+	/* Find the memory range */
+	node = fdt_node_offset_by_prop_value(fdt, -1, "device_type",
+					     "memory", sizeof("memory"));
+	if (node < 0) {
+		pr_err("Cannot find memory node\n");
+		goto err;
+	}
+
+	reg = fdt_getprop(fdt, node, "reg", &size);
+	if (size < (na + ns) * sizeof(u32)) {
+		pr_err("cannot get memory range\n");
+		goto err;
+	}
+
+	membase64 = 0;
+	for (i = 0; i < na; i++)
+		membase64 = (membase64 << 32) | fdt32_to_cpu(*reg++);
+
+	/* get the memsize and truncate it to under 4G on 32 bit machines */
+	memsize64 = 0;
+	for (i = 0; i < ns; i++)
+		memsize64 = (memsize64 << 32) | fdt32_to_cpu(*reg++);
+
+	*membase = membase64;
+	*memsize = memsize64;
+
+	return;
+err:
+	pr_err("No memory, cannot continue\n");
+	while (1);
+}
+
+#ifdef CONFIG_CPU_V8
+
+static noinline void dt_2nd_continue_aarch64(void *fdt)
+{
+	unsigned long membase, memsize;
+
+	if (!fdt)
+		hang();
+
+	of_find_mem(fdt, &membase, &memsize);
+
+	barebox_arm_entry(membase, memsize, fdt);
+}
+
+/* called from assembly */
+void dt_2nd_aarch64(void *fdt);
+
+void dt_2nd_aarch64(void *fdt)
+{
+	unsigned long image_start = (unsigned long)_text + global_variable_offset();
+
+	arm_setup_stack(image_start);
+
+	relocate_to_current_adr();
+	setup_c();
+
+	dt_2nd_continue_aarch64(fdt);
+}
+
+#else
+
+static noinline void dt_2nd_continue(void *fdt)
+{
+	unsigned long membase, memsize;
+
+	if (!fdt)
+		hang();
+
+	of_find_mem(fdt, &membase, &memsize);
+
+	barebox_arm_entry(membase, memsize, fdt);
+}
+
+ENTRY_FUNCTION(start_dt_2nd, r0, r1, r2)
+{
+	unsigned long image_start = (unsigned long)_text + global_variable_offset();
+
+	arm_setup_stack(image_start);
+
+	relocate_to_current_adr();
+	setup_c();
+	barrier();
+
+	dt_2nd_continue((void *)r2);
+}
+#endif