cactus: update to SPCI Beta1 and add sample direct messaging test

The scenario is: TFTF (acting as an "hypervisor" at NS-EL2) sends a
direct message request to an SP targetting its SP ID. SPMD/SPMC relays
the message down to SP. SP extracts the message payload, amends it and
sends a direct message response to TFTF that SPMC/SPMD relay back to
TFTF. This sample SP can be instantiated into multiple SPs. The early
main entry flow discriminates the first VM from other secondary VMs by
getting its running id through SPCI_ID_GET. The SP runs an infinite
message loop and waits for a new message by calling SPCI_MSG_WAIT.
Notice SPCI_MSG_WAIT is also the hint to SPMD that the SP initialized
properly. About logging, only PVM for now has direct access to UART.
2nd VMs must use the debug log hypervisor call.

Signed-off-by: Olivier Deprez <olivier.deprez@arm.com>
Change-Id: I001261c3f54e0bc217e43fca6812d6cda1ac4430

4 files changed, 237 insertions, 87 deletions

diff --git a/spm/README.txt b/spm/README.txt
index bff23ef..0a25d6b 100644
--- a/spm/README.txt
+++ b/spm/README.txt
@@ -1,8 +1,7 @@
-This is a prototype loosely based on the SPCI Alpha and SPRT pre-alpha
-specifications. Any interface / platform API introduced for this is subject to
+This is a prototype loosely based on the SPCI Beta1 specification.
+Any interface / platform API introduced for this is subject to
 change as it evolves.
 
-Cactus is meant to be the main test Secure Partition. It is the one meant to
-have most of the tests that a Secure Partition has to do. Ivy is meant to be
-more minimalistic. In the future, Cactus may be modified to be a S-EL1 partition
-while Ivy will remain as a S-EL0 partition.
+Cactus is meant to be the main test Secure Partition run at S-EL1.
+It is the one meant to have most of the tests that a Secure Partition
+has to do. Ivy is meant to be more minimalistic and run at S-EL0.
diff --git a/spm/cactus/aarch64/cactus_entrypoint.S b/spm/cactus/aarch64/cactus_entrypoint.S
index 214b4cf..7d6b6b2 100644
--- a/spm/cactus/aarch64/cactus_entrypoint.S
+++ b/spm/cactus/aarch64/cactus_entrypoint.S
@@ -21,6 +21,12 @@ func cactus_entrypoint
 	adr	x0, stacks_end
 	mov	sp, x0
 
+	/* Enable I-Cache */
+	mrs	x0, sctlr_el1
+	orr	x0, x0, #SCTLR_I_BIT
+	msr	sctlr_el1, x0
+	isb
+
 	/* And jump to the C entrypoint. */
 	b	cactus_main
 
diff --git a/spm/cactus/cactus.mk b/spm/cactus/cactus.mk
index d4be8c6..c588267 100644
--- a/spm/cactus/cactus.mk
+++ b/spm/cactus/cactus.mk
@@ -1,14 +1,16 @@
 #
-# Copyright (c) 2018-2019, Arm Limited. All rights reserved.
+# Copyright (c) 2018-2020, Arm Limited. All rights reserved.
 #
 # SPDX-License-Identifier: BSD-3-Clause
 #
 
 include lib/sprt/sprt_client.mk
+include lib/xlat_tables_v2/xlat_tables.mk
 
 CACTUS_DTB	:= $(BUILD_PLAT)/cactus.dtb
 
 CACTUS_INCLUDES :=					\
+	-Itftf/framework/include			\
 	-Iinclude					\
 	-Iinclude/common				\
 	-Iinclude/common/${ARCH}			\
@@ -44,9 +46,13 @@ CACTUS_SOURCES	+=					\
 CACTUS_SOURCES	+= 	drivers/arm/pl011/${ARCH}/pl011_console.S	\
 			lib/${ARCH}/cache_helpers.S			\
 			lib/${ARCH}/misc_helpers.S			\
+			lib/smc/${ARCH}/asm_smc.S			\
+			lib/smc/${ARCH}/smc.c				\
+			lib/smc/${ARCH}/hvc.c				\
 			lib/locks/${ARCH}/spinlock.S			\
 			lib/utils/mp_printf.c				\
-			${SPRT_LIB_SOURCES}
+			${SPRT_LIB_SOURCES}				\
+			${XLAT_TABLES_LIB_SRCS}
 
 CACTUS_LINKERFILE	:=	spm/cactus/cactus.ld.S
 
diff --git a/spm/cactus/cactus_main.c b/spm/cactus/cactus_main.c
index 4580f23..c2c8ec2 100644
--- a/spm/cactus/cactus_main.c
+++ b/spm/cactus/cactus_main.c
@@ -1,5 +1,5 @@
 /*
- * Copyright (c) 2018, Arm Limited. All rights reserved.
+ * Copyright (c) 2018-2020, Arm Limited. All rights reserved.
  *
  * SPDX-License-Identifier: BSD-3-Clause
  */
@@ -10,21 +10,176 @@
 #include <drivers/arm/pl011.h>
 #include <drivers/console.h>
 #include <errno.h>
+#include <lib/aarch64/arch_helpers.h>
+#include <lib/xlat_tables/xlat_tables_v2.h>
+#include <lib/xlat_tables/xlat_mmu_helpers.h>
 #include <plat_arm.h>
+#include <plat/common/platform.h>
 #include <platform_def.h>
 #include <sp_helpers.h>
-#include <sprt_client.h>
-#include <sprt_svc.h>
+#include <spci_svc.h>
 #include <std_svc.h>
 
 #include "cactus.h"
 #include "cactus_def.h"
 #include "cactus_tests.h"
 
+#include "tftf_lib.h"
+
+#define SPM_VM_ID_FIRST			(1)
+
+#define SPM_VM_GET_COUNT		(0xFF01)
+#define SPM_VCPU_GET_COUNT		(0xFF02)
+#define SPM_DEBUG_LOG			(0xBD000000)
+
+/* Hypervisor ID at physical SPCI instance */
+#define HYP_ID		(0)
+
+/* By convention, SP IDs (as opposed to VM IDs) have bit 15 set */
+#define SP_ID(x)	(x | (1 << 15))
+
+typedef unsigned short spci_vm_id_t;
+typedef unsigned short spci_vm_count_t;
+typedef unsigned short spci_vcpu_count_t;
+
 /* Host machine information injected by the build system in the ELF file. */
 extern const char build_message[];
 extern const char version_string[];
 
+static spci_vcpu_count_t spm_vcpu_get_count(spci_vm_id_t vm_id)
+{
+	hvc_args args = {
+		.fid = SPM_VCPU_GET_COUNT,
+		.arg1 = vm_id
+	};
+
+	hvc_ret_values ret = tftf_hvc(&args);
+
+	return ret.ret0;
+}
+
+static spci_vm_count_t spm_vm_get_count(void)
+{
+	hvc_args args = {
+		.fid = SPM_VM_GET_COUNT
+	};
+
+	hvc_ret_values ret = tftf_hvc(&args);
+
+	return ret.ret0;
+}
+
+static void spm_debug_log(char c)
+{
+	hvc_args args = {
+		.fid = SPM_DEBUG_LOG,
+		.arg1 = c
+	};
+
+	(void)tftf_hvc(&args);
+}
+
+static smc_ret_values spci_id_get(void)
+{
+	smc_args args = {
+		.fid = SPCI_ID_GET
+	};
+
+	return tftf_smc(&args);
+}
+
+static smc_ret_values spci_msg_wait(void)
+{
+	smc_args args = {
+		.fid = SPCI_MSG_WAIT
+	};
+
+	return tftf_smc(&args);
+}
+
+/* Send response through registers using direct messaging */
+static smc_ret_values spci_msg_send_direct_resp(spci_vm_id_t sender_vm_id,
+					 spci_vm_id_t target_vm_id,
+					 uint32_t message)
+{
+	smc_args args = {
+		.fid = SPCI_MSG_SEND_DIRECT_RESP_SMC32,
+		.arg1 = ((uint32_t)sender_vm_id << 16) | target_vm_id,
+		.arg3 = message
+	};
+
+	return tftf_smc(&args);
+}
+
+static smc_ret_values spci_error(int32_t error_code)
+{
+	smc_args args = {
+		.fid = SPCI_ERROR,
+		.arg1 = 0,
+		.arg2 = error_code
+	};
+
+	return tftf_smc(&args);
+}
+
+/*
+ *
+ * Message loop function
+ * Notice we cannot use regular print functions because this serves to both
+ * "primary" and "secondary" VMs. Secondary VM cannot access UART directly
+ * but rather through Hafnium print hypercall.
+ *
+ */
+static void __dead2 message_loop(spci_vm_id_t vm_id)
+{
+	smc_ret_values spci_ret;
+	uint32_t sp_response;
+
+	/*
+	 * This initial wait call is necessary to inform SPMD that
+	 * SP initialization has completed. It blocks until receiving
+	 * a direct message request.
+	 */
+	spci_ret = spci_msg_wait();
+
+	for (;;) {
+
+		if (spci_ret.ret0 != SPCI_MSG_SEND_DIRECT_REQ_SMC32) {
+			spci_ret = spci_error(-1);
+			continue;
+		}
+
+		if (spci_ret.ret1 != SP_ID(vm_id)) {
+			spci_ret = spci_error(-2);
+			continue;
+		}
+
+		if (spci_ret.ret2 != HYP_ID) {
+			spci_ret = spci_error(-3);
+			continue;
+		}
+
+		/*
+		 * For the sake of testing, add the vm id to the
+		 * received message.
+		 */
+		sp_response = spci_ret.ret3 | vm_id;
+
+		/*
+		 * Send a response through direct messaging then block
+		 * until receiving a new message request.
+		 */
+		spci_ret = spci_msg_send_direct_resp(SP_ID(vm_id),
+						     HYP_ID, sp_response);
+	}
+}
+
+static const mmap_region_t cactus_mmap[] __attribute__((used)) = {
+	/* DEVICE0 area includes UART2 necessary to console */
+	MAP_REGION_FLAT(DEVICE0_BASE, DEVICE0_SIZE, MT_DEVICE | MT_RW),
+	{0}
+};
+
 static void cactus_print_memory_layout(void)
 {
 	NOTICE("Secure Partition memory layout:\n");
@@ -54,97 +209,81 @@ static void cactus_print_memory_layout(void)
 		(void *)(CACTUS_TEST_MEM_BASE + CACTUS_TEST_MEM_SIZE));
 }
 
-static void cactus_message_handler(struct sprt_queue_entry_message *message)
+static void cactus_plat_configure_mmu(void)
 {
-	u_register_t ret0 = 0U, ret1 = 0U, ret2 = 0U, ret3 = 0U;
-
-	if (message->type == SPRT_MSG_TYPE_SERVICE_REQUEST) {
-		switch (message->args[1]) {
-
-		case CACTUS_PRINT_MAGIC:
-			INFO("Cactus: Magic: 0x%x\n", CACTUS_MAGIC_NUMBER);
-			ret0 = SPRT_SUCCESS;
-			break;
-
-		case CACTUS_GET_MAGIC:
-			ret1 = CACTUS_MAGIC_NUMBER;
-			ret0 = SPRT_SUCCESS;
-			break;
-
-		case CACTUS_SLEEP_MS:
-			sp_sleep(message->args[2]);
-			ret0 = SPRT_SUCCESS;
-			break;
-
-		default:
-			NOTICE("Cactus: Unhandled Service ID 0x%x\n",
-			       (unsigned int)message->args[1]);
-			ret0 = SPRT_NOT_SUPPORTED;
-			break;
-		}
-	} else {
-		NOTICE("Cactus: Unhandled Service type 0x%x\n",
-		       (unsigned int)message->type);
-		ret0 = SPRT_NOT_SUPPORTED;
-	}
+	mmap_add_region(CACTUS_TEXT_START,
+			CACTUS_TEXT_START,
+			CACTUS_TEXT_END - CACTUS_TEXT_START,
+			MT_CODE);
+	mmap_add_region(CACTUS_RODATA_START,
+			CACTUS_RODATA_START,
+			CACTUS_RODATA_END - CACTUS_RODATA_START,
+			MT_RO_DATA);
+	mmap_add_region(CACTUS_DATA_START,
+			CACTUS_DATA_START,
+			CACTUS_DATA_END - CACTUS_DATA_START,
+			MT_RW_DATA);
+	mmap_add_region(CACTUS_BSS_START,
+			CACTUS_BSS_START,
+			CACTUS_BSS_END - CACTUS_BSS_START,
+			MT_RW_DATA);
 
-	sprt_message_end(message, ret0, ret1, ret2, ret3);
+	mmap_add(cactus_mmap);
+	init_xlat_tables();
 }
 
 void __dead2 cactus_main(void)
 {
+	assert(IS_IN_EL1() != 0);
+
+	/* Clear BSS */
+	memset((void *)CACTUS_BSS_START,
+	       0, CACTUS_BSS_END - CACTUS_BSS_START);
+
+	/* Configure and enable Stage-1 MMU, enable D-Cache */
+	cactus_plat_configure_mmu();
+	enable_mmu_el1(0);
+
+	/* Get current SPCI id */
+	smc_ret_values spci_id_ret = spci_id_get();
+	if (spci_id_ret.ret0 != SPCI_SUCCESS_SMC32) {
+		ERROR("SPCI_ID_GET failed.\n");
+		panic();
+	}
+
+	spci_vm_id_t spci_id = spci_id_ret.ret2 & 0xffff;
+	if (spci_id > SPM_VM_ID_FIRST) {
+		/* Indicate secondary VM start through debug log hypercall */
+		spm_debug_log('2');
+		spm_debug_log('N');
+		spm_debug_log('D');
+		spm_debug_log('\n');
+
+		/* Run straight to the message loop */
+		message_loop(spci_id);
+	}
+
+	/* Next initialization steps only performed by primary VM */
+
 	console_init(PL011_UART2_BASE,
 		     PL011_UART2_CLK_IN_HZ,
 		     PL011_BAUDRATE);
 
-	NOTICE("Booting test Secure Partition Cactus\n");
-	NOTICE("%s\n", build_message);
-	NOTICE("%s\n", version_string);
-	NOTICE("Running at S-EL0\n");
+	NOTICE("Booting Cactus Secure Partition\n%s\n%s\n",
+	       build_message, version_string);
 
 	cactus_print_memory_layout();
 
-	/*
-	 * Run some initial tests.
-	 *
-	 * These are executed when the system is still booting, just after SPM
-	 * has handed over to Cactus.
-	 */
-	misc_tests();
-	system_setup_tests();
-	mem_attr_changes_tests();
+	NOTICE("SPCI id: %u\n", spci_id); /* Expect VM id 1 */
 
-	/*
-	 * Handle secure service requests.
-	 */
-	sprt_initialize_queues((void *)CACTUS_SPM_BUF_BASE);
+	/* Get number of VMs */
+	NOTICE("VM count: %u\n", spm_vm_get_count());
 
-	while (1) {
-		struct sprt_queue_entry_message message;
+	/* Get virtual CPU count for current VM */
+	NOTICE("vCPU count: %u\n", spm_vcpu_get_count(spci_id));
 
-		/*
-		 * Try to fetch a message from the blocking requests queue. If
-		 * it is empty, try to fetch from the non-blocking requests
-		 * queue. Repeat until both of them are empty.
-		 */
-		while (1) {
-			int err = sprt_get_next_message(&message,
-					SPRT_QUEUE_NUM_BLOCKING);
-			if (err == -ENOENT) {
-				err = sprt_get_next_message(&message,
-						SPRT_QUEUE_NUM_NON_BLOCKING);
-				if (err == -ENOENT) {
-					break;
-				} else {
-					assert(err == 0);
-					cactus_message_handler(&message);
-				}
-			} else {
-				assert(err == 0);
-				cactus_message_handler(&message);
-			}
-		}
+	/* End up to message loop */
+	message_loop(spci_id);
 
-		sprt_wait_for_messages();
-	}
+	/* Not reached */
 }