From 1eb9bc330bf387ff26a6df93d3b8c843174dc40b Mon Sep 17 00:00:00 2001
From: Harsimran Singh Tungal <harsimransingh.tungal@arm.com>
Date: Thu, 9 May 2024 13:20:57 +0000
Subject: [PATCH 10/10] platform: CS1000: Add multicore support for FVP

This changeset adds the support to enable the secondary cores for
the Corstone-1000 FVP

Upstream-Status: Backport [86383bb0ee5a99343c23c0b6fb9a1d161857a75c]
Signed-off-by: Harsimran Singh Tungal <harsimransingh.tungal@arm.com>
---
 .../target/arm/corstone1000/CMakeLists.txt    |  6 +++
 .../corstone1000/Device/Config/device_cfg.h   |  6 +++
 .../arm/corstone1000/tfm_hal_multi_core.c     | 38 ++++++++++++++++++-
 3 files changed, 48 insertions(+), 2 deletions(-)

diff --git a/platform/ext/target/arm/corstone1000/CMakeLists.txt b/platform/ext/target/arm/corstone1000/CMakeLists.txt
index 95e3f57b4f..e46123cc6f 100644
--- a/platform/ext/target/arm/corstone1000/CMakeLists.txt
+++ b/platform/ext/target/arm/corstone1000/CMakeLists.txt
@@ -381,6 +381,12 @@ target_sources(tfm_psa_rot_partition_ns_agent_mailbox
         tfm_hal_multi_core.c
 )
 
+if (PLATFORM_IS_FVP)
+target_compile_definitions(tfm_psa_rot_partition_ns_agent_mailbox
+    PUBLIC
+    	$<$<BOOL:${ENABLE_MULTICORE}>:CORSTONE1000_FVP_MULTICORE>
+)
+endif()
 #========================= tfm_spm ============================================#
 
 target_sources(tfm_spm
diff --git a/platform/ext/target/arm/corstone1000/Device/Config/device_cfg.h b/platform/ext/target/arm/corstone1000/Device/Config/device_cfg.h
index 222905d3dd..9d48f119ed 100644
--- a/platform/ext/target/arm/corstone1000/Device/Config/device_cfg.h
+++ b/platform/ext/target/arm/corstone1000/Device/Config/device_cfg.h
@@ -45,5 +45,11 @@
 /* CFI Controller */
 #define CFI_S
 
+/* Total number of host cores */
+#if CORSTONE1000_FVP_MULTICORE
+#define PLATFORM_HOST_MAX_CORE_COUNT 4
+#else
+#define PLATFORM_HOST_MAX_CORE_COUNT 1
+#endif
 
 #endif  /* __DEVICE_CFG_H__ */
diff --git a/platform/ext/target/arm/corstone1000/tfm_hal_multi_core.c b/platform/ext/target/arm/corstone1000/tfm_hal_multi_core.c
index f0e2bc333a..ce72e50c9b 100644
--- a/platform/ext/target/arm/corstone1000/tfm_hal_multi_core.c
+++ b/platform/ext/target/arm/corstone1000/tfm_hal_multi_core.c
@@ -11,9 +11,14 @@
 #include "tfm_hal_multi_core.h"
 #include "fwu_agent.h"
 
-#define HOST_SYS_RST_CTRL_OFFSET 0x0
+#define HOST_SYS_RST_CTRL_OFFSET     0x000
+#define HOST_CPU_PE0_CONFIG_OFFSET   0x010
+#define HOST_CPU_PE1_CONFIG_OFFSET   0x020
+#define HOST_CPU_PE2_CONFIG_OFFSET   0x030
+#define HOST_CPU_PE3_CONFIG_OFFSET   0x040
+#define HOST_CPU_BOOT_MASK_OFFSET    0x300
 #define HOST_CPU_CORE0_WAKEUP_OFFSET 0x308
-#define HOST_CPU_PE0_CONFIG_OFFSET 0x010
+
 #define AA64nAA32_MASK (1 << 3)
 
 #ifdef EXTERNAL_SYSTEM_SUPPORT
@@ -53,9 +58,29 @@ void tfm_hal_boot_ns_cpu(uintptr_t start_addr)
     volatile uint32_t *PE0_CONFIG =
                         (uint32_t *)(CORSTONE1000_HOST_BASE_SYSTEM_CONTROL_BASE
                                      + HOST_CPU_PE0_CONFIG_OFFSET);
+#if CORSTONE1000_FVP_MULTICORE
+    volatile uint32_t *PE1_CONFIG =
+                        (uint32_t *)(CORSTONE1000_HOST_BASE_SYSTEM_CONTROL_BASE
+                                     + HOST_CPU_PE1_CONFIG_OFFSET);
+    volatile uint32_t *PE2_CONFIG =
+                        (uint32_t *)(CORSTONE1000_HOST_BASE_SYSTEM_CONTROL_BASE
+                                     + HOST_CPU_PE2_CONFIG_OFFSET);
+    volatile uint32_t *PE3_CONFIG =
+                        (uint32_t *)(CORSTONE1000_HOST_BASE_SYSTEM_CONTROL_BASE
+                                     + HOST_CPU_PE3_CONFIG_OFFSET);
+    volatile uint32_t *CPU_BOOT_MASK =
+                        (uint32_t *)(CORSTONE1000_HOST_BASE_SYSTEM_CONTROL_BASE
+		                     + HOST_CPU_BOOT_MASK_OFFSET);
 
+    *CPU_BOOT_MASK = 0xf;
+#endif
     /* Select host CPU architecture as AArch64 */
     *PE0_CONFIG |= AA64nAA32_MASK; /* 0b1 – AArch64 */
+#if CORSTONE1000_FVP_MULTICORE
+    *PE1_CONFIG |= AA64nAA32_MASK; /* 0b1 – AArch64 */
+    *PE2_CONFIG |= AA64nAA32_MASK; /* 0b1 – AArch64 */
+    *PE3_CONFIG |= AA64nAA32_MASK; /* 0b1 – AArch64 */
+#endif
 
     /* wakeup CORE0 before bringing it out of reset */
     *reset_ctl_wakeup_reg = 0x1;
@@ -63,6 +88,15 @@ void tfm_hal_boot_ns_cpu(uintptr_t start_addr)
     /* Clear HOST_SYS_RST_CTRL register to bring host out of RESET */
     *reset_ctl_reg = 0;
 
+#if CORSTONE1000_FVP_MULTICORE
+    /* Wake up secondary cores.
+     * This should be done after bringing the primary core out of reset. */
+    for(int core_index=1; core_index < PLATFORM_HOST_MAX_CORE_COUNT; core_index++)
+    {
+	    *reset_ctl_wakeup_reg = (0x1 << core_index);
+    }
+#endif
+
     (void) start_addr;
 
 #ifdef EXTERNAL_SYSTEM_SUPPORT
-- 
2.25.1

