From 5619c1db49a154126fc4aac569e245ac81f32ca4 Mon Sep 17 00:00:00 2001
From: Joonas Onatsu <joonas.onatsu@unikie.com>
Date: Tue, 12 Jul 2022 19:23:44 +0300
Subject: [PATCH] elfloader-tool,bcm2711: BCM2711 support

Cherry-pick 'smp.c' and 'smp_head.S' from
ARM Research IceCap repository for BCM2711 platform.
Commit ID: b2b92939bf94443e7bf13a15801561ef7caf565d
URL: https://gitlab.com/icecap-project/seL4_tools/-/commit/<SHA>

UART support files are not selected from the commit,
as kernel correctly generates header(s) for the
'bcm2835-aux-uart', which is the same as on RPi3, and
the common 'bcm-uart.c' driver can be used for RPi4 too.

Signed-off-by: Joonas Onatsu <joonasx@ssrc.tii.ae>
---
 elfloader-tool/src/plat/bcm2711/smp.c      | 50 ++++++++++++++++++++++
 elfloader-tool/src/plat/bcm2711/smp_head.S | 27 ++++++++++++
 2 files changed, 77 insertions(+)
 create mode 100644 elfloader-tool/src/plat/bcm2711/smp.c
 create mode 100644 elfloader-tool/src/plat/bcm2711/smp_head.S

diff --git a/elfloader-tool/src/plat/bcm2711/smp.c b/elfloader-tool/src/plat/bcm2711/smp.c
new file mode 100644
index 00000000..bc34bbc9
--- /dev/null
+++ b/elfloader-tool/src/plat/bcm2711/smp.c
@@ -0,0 +1,50 @@
+/*
+ * Copyright 2019, ARM Ltd
+ *
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ */
+
+#include <autoconf.h>
+#include <elfloader/gen_config.h>
+#include <types.h>
+
+#if CONFIG_MAX_NUM_NODES > 1
+#include <types.h>
+#include <elfloader.h>
+#include <armv/machine.h>
+#include <armv/smp.h>
+#include <printf.h>
+#include <abort.h>
+
+#define MAX_CORES 4
+
+extern void core_entry_head(unsigned long stack);
+
+unsigned long core_stack;
+
+uint64_t spin_table[4] = { 0xd8, 0xe0, 0xe8, 0xf0 };
+
+void init_cpus(void)
+{
+    int nodes = CONFIG_MAX_NUM_NODES;
+    if (nodes > MAX_CORES) {
+        printf("CONFIG_MAX_NUM_NODES %d is greater than max number cores %d, will abort\n",
+               CONFIG_MAX_NUM_NODES, MAX_CORES);
+        abort();
+    }
+    for (int i = 1; i < nodes; i++) {
+        /* all cores read the stack pointer from the same place */
+        core_stack = (unsigned long) &core_stacks[i][0];
+        *((volatile unsigned long *)(spin_table[i])) = (unsigned long)core_entry_head;
+        dsb();
+        asm volatile("sev");
+        while (!is_core_up(i));
+        printf("Core %d is up with logic ID %d\n", i, i);
+    }
+
+    /* set the logic id for the booting core */
+    MSR("tpidr_el1", 0);
+}
+
+#endif /* CONFIG_MAX_NUM_NODES > 1 */
diff --git a/elfloader-tool/src/plat/bcm2711/smp_head.S b/elfloader-tool/src/plat/bcm2711/smp_head.S
new file mode 100644
index 00000000..c9ee45eb
--- /dev/null
+++ b/elfloader-tool/src/plat/bcm2711/smp_head.S
@@ -0,0 +1,27 @@
+/*
+ * Copyright 2019, ARM Ltd
+ *
+ * SPDX-License-Identifier: BSD-2-Clause
+ *
+ */
+
+#include <autoconf.h>
+#include <elfloader/gen_config.h>
+#include <assembler.h>
+#include <armv/assembler.h>
+
+#if CONFIG_MAX_NUM_NODES > 1
+
+.text
+
+.extern core_entry
+.extern core_stack
+BEGIN_FUNC(core_entry_head)
+    ldr x0, =core_stack
+    ldr x3, [x0]
+    mov x0, x3
+    mov sp, x3
+    b   core_entry
+END_FUNC(core_entry_head)
+
+#endif