qemu_virt: enable smp boot

Enables SMP boot of the plat-vexpress qemu_virt flavor. This includes
PSCI support and coherent memory shared with bios.

Acked-by: Jerome Forissier <jerome.forissier@linaro.org>
Tested-by: Jens Wiklander <jens.wiklander@linaro.org>
Signed-off-by: Jens Wiklander <jens.wiklander@linaro.org>

core/arch/arm/plat-vexpress/conf.mk

@@ -59,8 +59,10 @@ ifeq ($(CFG_CORE_SANITIZE_KADDRESS),y)
 # This is unfortunately currently not possible to do in make so we have to
 # calculate it offline, there's some asserts in
 # core/arch/arm/kernel/generic_boot.c to check that we got it right
-CFG_ASAN_SHADOW_OFFSET = 0xc5c71c0
+CFG_ASAN_SHADOW_OFFSET = 0xc5c7fc0
 endif
+$(call force,CFG_BOOT_SECONDARY_REQUEST,y)
+$(call force,CFG_PSCI_ARM32,y)
 $(call force,CFG_DT,y)
 # SE API is only supported by QEMU Virt platform
 CFG_SE_API ?= y

core/arch/arm/plat-vexpress/main.c

@@ -26,29 +26,27 @@
  * POSSIBILITY OF SUCH DAMAGE.
  */
 
-#include <platform_config.h>
-
-#include <stdint.h>
-#include <string.h>
-
+#include <arm.h>
+#include <console.h>
 #include <drivers/gic.h>
 #include <drivers/pl011.h>
 #include <drivers/tzc400.h>
-
-#include <arm.h>
+#include <initcall.h>
+#include <keep.h>
 #include <kernel/generic_boot.h>
-#include <kernel/pm_stubs.h>
-#include <trace.h>
 #include <kernel/misc.h>
 #include <kernel/panic.h>
+#include <kernel/pm_stubs.h>
 #include <kernel/tee_time.h>
-#include <tee/entry_fast.h>
-#include <tee/entry_std.h>
 #include <mm/core_memprot.h>
 #include <mm/core_mmu.h>
-#include <console.h>
-#include <keep.h>
-#include <initcall.h>
+#include <platform_config.h>
+#include <sm/psci.h>
+#include <stdint.h>
+#include <string.h>
+#include <tee/entry_fast.h>
+#include <tee/entry_std.h>
+#include <trace.h>
 
 static void main_fiq(void);
 
@@ -79,6 +77,9 @@ static struct pl011_data console_data;
 #if defined(PLATFORM_FLAVOR_fvp)
 register_phys_mem(MEM_AREA_RAM_SEC, TZCDRAM_BASE, TZCDRAM_SIZE);
 #endif
+#if defined(PLATFORM_FLAVOR_qemu_virt)
+register_phys_mem(MEM_AREA_IO_SEC, SECRAM_BASE, SECRAM_COHERENT_SIZE);
+#endif
 register_phys_mem(MEM_AREA_IO_SEC, CONSOLE_UART_BASE, PL011_REG_SIZE);
 register_nsec_ddr(DRAM0_BASE, DRAM0_SIZE);
 
@@ -187,3 +188,36 @@ static TEE_Result init_tzc400(void)
 
 service_init(init_tzc400);
 #endif /*CFG_TZC400*/
+
+#if defined(PLATFORM_FLAVOR_qemu_virt)
+int psci_cpu_on(uint32_t core_id, uint32_t entry, uint32_t context_id __unused)
+{
+	size_t pos = get_core_pos_mpidr(core_id);
+	uint32_t *sec_entry_addrs = phys_to_virt(SECRAM_BASE, MEM_AREA_IO_SEC);
+	static bool core_is_released[CFG_TEE_CORE_NB_CORE];
+
+	if (!sec_entry_addrs)
+		panic();
+
+	if (!pos || pos >= CFG_TEE_CORE_NB_CORE)
+		return PSCI_RET_INVALID_PARAMETERS;
+
+	DMSG("core pos: %zu: ns_entry %#" PRIx32, pos, entry);
+
+	if (core_is_released[pos]) {
+		EMSG("core %zu already released", pos);
+		return PSCI_RET_DENIED;
+	}
+	core_is_released[pos] = true;
+
+	/* set NS entry addresses of core */
+	ns_entry_addrs[pos] = entry;
+	dsb_ishst();
+
+	sec_entry_addrs[pos] = CFG_TEE_RAM_START;
+	dsb_ishst();
+	sev();
+
+	return PSCI_RET_SUCCESS;
+}
+#endif /*PLATFORM_FLAVOR_qemu_virt*/

core/arch/arm/plat-vexpress/platform_config.h

@@ -193,20 +193,22 @@
 
 #define SECRAM_BASE		0x0e000000
 #define SECRAM_SIZE		0x01000000
+#define SECRAM_COHERENT_SIZE	4096
 
 #ifdef CFG_WITH_PAGER
 
 /* Emulated SRAM */
-#define TZSRAM_BASE		SECRAM_BASE
+#define TZSRAM_BASE		(SECRAM_BASE + SECRAM_COHERENT_SIZE)
 #define TZSRAM_SIZE		CFG_CORE_TZSRAM_EMUL_SIZE
 
 #define TZDRAM_BASE		(TZSRAM_BASE + TZSRAM_SIZE)
-#define TZDRAM_SIZE		(SECRAM_SIZE - TZSRAM_SIZE)
+#define TZDRAM_SIZE		(SECRAM_SIZE - TZSRAM_SIZE - \
+				 SECRAM_COHERENT_SIZE)
 
 #else /* CFG_WITH_PAGER */
 
-#define TZDRAM_BASE		SECRAM_BASE
-#define TZDRAM_SIZE		SECRAM_SIZE
+#define TZDRAM_BASE		(SECRAM_BASE + SECRAM_COHERENT_SIZE)
+#define TZDRAM_SIZE		(SECRAM_SIZE - SECRAM_COHERENT_SIZE)
 
 #endif /* CFG_WITH_PAGER */