test: add set up stage for to hftest services

Add the HFTEST_SERVICE_SET_UP macro which will run the function
during the initialisation stage of a service partition. This is the
time when the SP's primary vCPU is in execution context initialisation
just before the SP uses FFA_MSG_WAIT to terminate the boot sequence,
as specified in FF-A's boot protocol.

A service must specify the set up functions it wants to run in the
hftest-service-setup node in it's partition manifest.

Signed-off-by: Daniel Boulby <daniel.boulby@arm.com>
Change-Id: I33464dd3454137eb3a1bf7678cafc75f16c8c5de
diff --git a/test/hftest/service_common.c b/test/hftest/service_common.c
index 3e121e8..bb62aa3 100644
--- a/test/hftest/service_common.c
+++ b/test/hftest/service_common.c
@@ -131,6 +131,43 @@
 	ctx->is_ffa_manifest_parsed = true;
 }
 
+static void run_service_set_up(struct hftest_context *ctx, struct fdt *fdt)
+{
+	struct fdt_node node;
+	struct hftest_test *hftest_info;
+
+	ASSERT_TRUE(fdt_find_node(fdt, "/", &node));
+
+	if (!fdt_find_child(&node, &(STRING_INIT("hftest-service-setup")))) {
+		return;
+	}
+
+	EXPECT_TRUE(fdt_is_compatible(&node, "arm,hftest"));
+
+	for (hftest_info = hftest_begin; hftest_info < hftest_end;
+	     ++hftest_info) {
+		struct memiter data;
+		if (hftest_info->kind != HFTEST_KIND_SERVICE_SET_UP) {
+			continue;
+		}
+		if (fdt_read_property(&node, hftest_info->name, &data)) {
+			HFTEST_LOG("Running service_setup: %s\n",
+				   hftest_info->name);
+			hftest_info->fn();
+			if (ctx->failures) {
+				HFTEST_LOG_FAILURE();
+				HFTEST_LOG(HFTEST_LOG_INDENT
+					   "%s service_setup failed\n",
+					   hftest_info->name);
+				abort();
+			}
+		} else {
+			HFTEST_LOG("Skipping service_setup: %s\n",
+				   hftest_info->name);
+		}
+	}
+}
+
 noreturn void hftest_service_main(const void *fdt_ptr)
 {
 	struct memiter args;
@@ -143,6 +180,39 @@
 	ffa_notifications_bitmap_t bitmap;
 	struct ffa_partition_msg *message = (struct ffa_partition_msg *)mb.recv;
 
+	/* Clean the context. */
+	ctx = hftest_get_context();
+	hftest_context_init(ctx, mb.send, mb.recv);
+
+	if (!fdt_struct_from_ptr(fdt_ptr, &fdt)) {
+		HFTEST_LOG(HFTEST_LOG_INDENT "Unable to access the FDT");
+		abort();
+	}
+
+	/*
+	 * The memory size argument is to be used only by VMs. It is part of
+	 * the dt provided by the Hypervisor. SPs expect to receive their
+	 * FF-A manifest which doesn't have a memory size field.
+	 */
+	if (ffa_is_vm_id(own_id) &&
+	    !fdt_get_memory_size(&fdt, &ctx->memory_size)) {
+		HFTEST_LOG_FAILURE();
+		HFTEST_LOG(HFTEST_LOG_INDENT
+			   "No entry in the FDT on memory size details");
+		abort();
+	} else if (!ffa_is_vm_id(own_id)) {
+		/*
+		 * It is secure partition. We are currently using the partition
+		 * manifest for the SP.
+		 */
+		hftest_parse_ffa_manifest(ctx, &fdt);
+
+		/* TODO: Determine memory size referring to the SP Pkg. */
+		ctx->memory_size = 1048576;
+	}
+
+	run_service_set_up(ctx, &fdt);
+
 	/* Receive the name of the service to run. */
 	ret = ffa_msg_wait();
 	EXPECT_EQ(ret.func, FFA_RUN_32);
@@ -173,37 +243,6 @@
 		abort();
 	}
 
-	if (!fdt_struct_from_ptr(fdt_ptr, &fdt)) {
-		HFTEST_LOG(HFTEST_LOG_INDENT "Unable to access the FDT");
-		abort();
-	}
-
-	/* Clean the context. */
-	ctx = hftest_get_context();
-	hftest_context_init(ctx, mb.send, mb.recv);
-
-	/*
-	 * The memory size argument is to be used only by VMs. It is part of
-	 * the dt provided by the Hypervisor. SPs expect to receive their
-	 * FF-A manifest which doesn't have a memory size field.
-	 */
-	if (ffa_is_vm_id(own_id) &&
-	    !fdt_get_memory_size(&fdt, &ctx->memory_size)) {
-		HFTEST_LOG_FAILURE();
-		HFTEST_LOG(HFTEST_LOG_INDENT
-			   "No entry in the FDT on memory size details");
-		abort();
-	} else if (!ffa_is_vm_id(own_id)) {
-		/*
-		 * It is secure partition. We are currently using the partition
-		 * manifest for the SP.
-		 */
-		hftest_parse_ffa_manifest(ctx, &fdt);
-
-		/* TODO: Determine memory size referring to the SP Pkg. */
-		ctx->memory_size = 1048576;
-	}
-
 	/* Pause so the next time cycles are given the service will be run. */
 	ffa_yield();
 
diff --git a/test/inc/test/hftest.h b/test/inc/test/hftest.h
index f584c7c..661576e 100644
--- a/test/inc/test/hftest.h
+++ b/test/inc/test/hftest.h
@@ -52,6 +52,13 @@
 	TEST_PRECONDITION(suite, test, precon_skip_##suite_##test)
 
 /*
+ * Define set up functions to be run during a services initialisation phase.
+ * A service must partition must specify the set up functions it wishes to run
+ * in the partition manifest.
+ */
+#define SERVICE_SET_UP(service) HFTEST_SERVICE_SET_UP(service)
+
+/*
  * Define a test service.
  */
 #define TEST_SERVICE(service) HFTEST_TEST_SERVICE(service)
diff --git a/test/inc/test/hftest_impl.h b/test/inc/test/hftest_impl.h
index 68ca462..e994015 100644
--- a/test/inc/test/hftest_impl.h
+++ b/test/inc/test/hftest_impl.h
@@ -41,6 +41,8 @@
 	HFTEST_STR(.hftest.suite.suite_name .1tear_down)
 #define HFTEST_TEST_SECTION(suite_name, test_name) \
 	HFTEST_STR(.hftest.suite.suite_name .2test.test_name)
+#define HFTEST_SERVICE_SET_UP_SECTION(service_name) \
+	HFTEST_STR(.hftest.service_set_up.service_name)
 #define HFTEST_SERVICE_SECTION(service_name) \
 	HFTEST_STR(.hftest.service.service_name)
 
@@ -49,12 +51,16 @@
 #define HFTEST_TEAR_DOWN_STRUCT(suite_name) hftest_tear_down_##suite_name
 #define HFTEST_TEST_STRUCT(suite_name, test_name) \
 	hftest_test_##suite_name##_##test_name
+#define HFTEST_SERVICE_SET_UP_STRUCT(service_name) \
+	hftest_service_set_up_##service_name
 #define HFTEST_SERVICE_STRUCT(service_name) hftest_service_##service_name
 
 #define HFTEST_SET_UP_FN(suite_name) hftest_set_up_fn_##suite_name
 #define HFTEST_TEAR_DOWN_FN(suite_name) hftest_tear_down_fn_##suite_name
 #define HFTEST_TEST_FN(suite_name, test_name) \
 	hftest_test_fn_##suite_name##_##test_name
+#define HFTEST_SERVICE_SET_UP_FN(service_name) \
+	hftest_service_set_up_fn_##service_name
 #define HFTEST_SERVICE_FN(service_name) hftest_service_fn_##service_name
 
 #define HFTEST_SET_UP_CONSTRUCTOR(suite_name) hftest_set_up_ctor_##suite_name
@@ -115,6 +121,17 @@
 	}                                                                    \
 	static void HFTEST_TEST_FN(suite_name, test_name)(void)
 
+#define HFTEST_SERVICE_SET_UP(service_name)                                   \
+	static void HFTEST_SERVICE_SET_UP_FN(service_name)(void);             \
+	const struct hftest_test __attribute__((used))                        \
+	__attribute__((section(HFTEST_SERVICE_SET_UP_SECTION(service_name)))) \
+	HFTEST_SERVICE_SET_UP_STRUCT(service_name) = {                        \
+		.name = #service_name,                                        \
+		.kind = HFTEST_KIND_SERVICE_SET_UP,                           \
+		.fn = HFTEST_SERVICE_SET_UP_FN(service_name),                 \
+	};                                                                    \
+	static void HFTEST_SERVICE_SET_UP_FN(service_name)(void)
+
 #define HFTEST_TEST_SERVICE(service_name)                              \
 	static void HFTEST_SERVICE_FN(service_name)(void);             \
 	const struct hftest_test __attribute__((used))                 \
@@ -154,7 +171,8 @@
 	HFTEST_KIND_SET_UP = 0,
 	HFTEST_KIND_TEST = 1,
 	HFTEST_KIND_TEAR_DOWN = 2,
-	HFTEST_KIND_SERVICE = 3,
+	HFTEST_KIND_SERVICE_SET_UP = 3,
+	HFTEST_KIND_SERVICE = 4,
 };
 
 /**