1 files changed, 109 insertions, 42 deletions

diff --git a/services/std_svc/rmmd/rmmd_main.c b/services/std_svc/rmmd/rmmd_main.c
index 746419e9c..6bd9fdf30 100644
--- a/services/std_svc/rmmd/rmmd_main.c
+++ b/services/std_svc/rmmd/rmmd_main.c
@@ -33,6 +33,11 @@
 #include "rmmd_private.h"
 
 /*******************************************************************************
+ * RMM boot failure flag
+ ******************************************************************************/
+static bool rmm_boot_failed;
+
+/*******************************************************************************
  * RMM context information.
  ******************************************************************************/
 rmmd_rmm_context_t rmm_context[PLATFORM_CORE_COUNT];
@@ -132,13 +137,10 @@ static void manage_extensions_realm(cpu_context_t *ctx)
  ******************************************************************************/
 static int32_t rmm_init(void)
 {
-
-	uint64_t rc;
-
+	long rc;
 	rmmd_rmm_context_t *ctx = &rmm_context[plat_my_core_pos()];
 
 	INFO("RMM init start.\n");
-	ctx->state = RMM_STATE_RESET;
 
 	/* Enable architecture extensions */
 	manage_extensions_realm(&ctx->cpu_ctx);
@@ -147,12 +149,13 @@ static int32_t rmm_init(void)
 	rmm_el2_context_init(&ctx->cpu_ctx.el2_sysregs_ctx);
 
 	rc = rmmd_rmm_sync_entry(ctx);
-	if (rc != 0ULL) {
-		ERROR("RMM initialisation failed 0x%" PRIx64 "\n", rc);
-		panic();
+	if (rc != E_RMM_BOOT_SUCCESS) {
+		ERROR("RMM init failed: %ld\n", rc);
+		/* Mark the boot as failed for all the CPUs */
+		rmm_boot_failed = true;
+		return 0;
 	}
 
-	ctx->state = RMM_STATE_IDLE;
 	INFO("RMM init end.\n");
 
 	return 1;
@@ -163,9 +166,13 @@ static int32_t rmm_init(void)
  ******************************************************************************/
 int rmmd_setup(void)
 {
+	size_t shared_buf_size __unused;
+	uintptr_t shared_buf_base;
 	uint32_t ep_attr;
 	unsigned int linear_id = plat_my_core_pos();
 	rmmd_rmm_context_t *rmm_ctx = &rmm_context[linear_id];
+	rmm_manifest_t *manifest;
+	int rc;
 
 	/* Make sure RME is supported. */
 	assert(get_armv9_2_feat_rme_support() != 0U);
@@ -192,6 +199,34 @@ int rmmd_setup(void)
 					MODE_SP_ELX,
 					DISABLE_ALL_EXCEPTIONS);
 
+	shared_buf_size =
+			plat_rmmd_get_el3_rmm_shared_mem(&shared_buf_base);
+
+	assert((shared_buf_size == SZ_4K) &&
+					((void *)shared_buf_base != NULL));
+
+	/* Load the boot manifest at the beginning of the shared area */
+	manifest = (rmm_manifest_t *)shared_buf_base;
+	rc = plat_rmmd_load_manifest(manifest);
+	if (rc != 0) {
+		ERROR("Error loading RMM Boot Manifest (%i)\n", rc);
+		return rc;
+	}
+	flush_dcache_range((uintptr_t)shared_buf_base, shared_buf_size);
+
+	/*
+	 * Prepare coldboot arguments for RMM:
+	 * arg0: This CPUID (primary processor).
+	 * arg1: Version for this Boot Interface.
+	 * arg2: PLATFORM_CORE_COUNT.
+	 * arg3: Base address for the EL3 <-> RMM shared area. The boot
+	 *       manifest will be stored at the beginning of this area.
+	 */
+	rmm_ep_info->args.arg0 = linear_id;
+	rmm_ep_info->args.arg1 = RMM_EL3_INTERFACE_VERSION;
+	rmm_ep_info->args.arg2 = PLATFORM_CORE_COUNT;
+	rmm_ep_info->args.arg3 = shared_buf_base;
+
 	/* Initialise RMM context with this entry point information */
 	cm_setup_context(&rmm_ctx->cpu_ctx, rmm_ep_info);
 
@@ -207,10 +242,12 @@ int rmmd_setup(void)
  * Forward SMC to the other security state
  ******************************************************************************/
 static uint64_t	rmmd_smc_forward(uint32_t src_sec_state,
-					uint32_t dst_sec_state, uint64_t x0,
-					uint64_t x1, uint64_t x2, uint64_t x3,
-					uint64_t x4, void *handle)
+				 uint32_t dst_sec_state, uint64_t x0,
+				 uint64_t x1, uint64_t x2, uint64_t x3,
+				 uint64_t x4, void *handle)
 {
+	cpu_context_t *ctx = cm_get_context(dst_sec_state);
+
 	/* Save incoming security state */
 	cm_el1_sysregs_context_save(src_sec_state);
 	cm_el2_sysregs_context_save(src_sec_state);
@@ -221,19 +258,21 @@ static uint64_t	rmmd_smc_forward(uint32_t src_sec_state,
 	cm_set_next_eret_context(dst_sec_state);
 
 	/*
-	 * As per SMCCCv1.1, we need to preserve x4 to x7 unless
+	 * As per SMCCCv1.2, we need to preserve x4 to x7 unless
 	 * being used as return args. Hence we differentiate the
 	 * onward and backward path. Support upto 8 args in the
 	 * onward path and 4 args in return path.
+	 * Register x4 will be preserved by RMM in case it is not
+	 * used in return path.
 	 */
 	if (src_sec_state == NON_SECURE) {
-		SMC_RET8(cm_get_context(dst_sec_state), x0, x1, x2, x3, x4,
-				SMC_GET_GP(handle, CTX_GPREG_X5),
-				SMC_GET_GP(handle, CTX_GPREG_X6),
-				SMC_GET_GP(handle, CTX_GPREG_X7));
-	} else {
-		SMC_RET4(cm_get_context(dst_sec_state), x0, x1, x2, x3);
+		SMC_RET8(ctx, x0, x1, x2, x3, x4,
+			 SMC_GET_GP(handle, CTX_GPREG_X5),
+			 SMC_GET_GP(handle, CTX_GPREG_X6),
+			 SMC_GET_GP(handle, CTX_GPREG_X7));
 	}
+
+	SMC_RET5(ctx, x0, x1, x2, x3, x4);
 }
 
 /*******************************************************************************
@@ -241,12 +280,17 @@ static uint64_t	rmmd_smc_forward(uint32_t src_sec_state,
  * either forwarded to the other security state or handled by the RMM dispatcher
  ******************************************************************************/
 uint64_t rmmd_rmi_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2,
-				uint64_t x3, uint64_t x4, void *cookie,
-				void *handle, uint64_t flags)
+			  uint64_t x3, uint64_t x4, void *cookie,
+			  void *handle, uint64_t flags)
 {
-	rmmd_rmm_context_t *ctx = &rmm_context[plat_my_core_pos()];
 	uint32_t src_sec_state;
 
+	/* If RMM failed to boot, treat any RMI SMC as unknown */
+	if (rmm_boot_failed) {
+		WARN("RMMD: Failed to boot up RMM. Ignoring RMI call\n");
+		SMC_RET1(handle, SMC_UNK);
+	}
+
 	/* Determine which security state this SMC originated from */
 	src_sec_state = caller_sec_state(flags);
 
@@ -271,15 +315,12 @@ uint64_t rmmd_rmi_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2,
 	}
 
 	switch (smc_fid) {
-	case RMMD_RMI_REQ_COMPLETE:
-		if (ctx->state == RMM_STATE_RESET) {
-			VERBOSE("RMMD: running rmmd_rmm_sync_exit\n");
-			rmmd_rmm_sync_exit(x1);
-		}
+	case RMM_RMI_REQ_COMPLETE: {
+		uint64_t x5 = SMC_GET_GP(handle, CTX_GPREG_X5);
 
 		return rmmd_smc_forward(REALM, NON_SECURE, x1,
-					x2, x3, x4, 0, handle);
-
+					x2, x3, x4, x5, handle);
+	}
 	default:
 		WARN("RMMD: Unsupported RMM call 0x%08x\n", smc_fid);
 		SMC_RET1(handle, SMC_UNK);
@@ -293,11 +334,26 @@ uint64_t rmmd_rmi_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2,
  ******************************************************************************/
 static void *rmmd_cpu_on_finish_handler(const void *arg)
 {
-	int32_t rc;
+	long rc;
 	uint32_t linear_id = plat_my_core_pos();
 	rmmd_rmm_context_t *ctx = &rmm_context[linear_id];
 
-	ctx->state = RMM_STATE_RESET;
+	if (rmm_boot_failed) {
+		/* RMM Boot failed on a previous CPU. Abort. */
+		ERROR("RMM Failed to initialize. Ignoring for CPU%d\n",
+								linear_id);
+		return NULL;
+	}
+
+	/*
+	 * Prepare warmboot arguments for RMM:
+	 * arg0: This CPUID.
+	 * arg1 to arg3: Not used.
+	 */
+	rmm_ep_info->args.arg0 = linear_id;
+	rmm_ep_info->args.arg1 = 0ULL;
+	rmm_ep_info->args.arg2 = 0ULL;
+	rmm_ep_info->args.arg3 = 0ULL;
 
 	/* Initialise RMM context with this entry point information */
 	cm_setup_context(&ctx->cpu_ctx, rmm_ep_info);
@@ -309,13 +365,13 @@ static void *rmmd_cpu_on_finish_handler(const void *arg)
 	rmm_el2_context_init(&ctx->cpu_ctx.el2_sysregs_ctx);
 
 	rc = rmmd_rmm_sync_entry(ctx);
-	if (rc != 0) {
-		ERROR("RMM initialisation failed (%d) on CPU%d\n", rc,
-		      linear_id);
-		panic();
+
+	if (rc != E_RMM_BOOT_SUCCESS) {
+		ERROR("RMM init failed on CPU%d: %ld\n", linear_id, rc);
+		/* Mark the boot as failed for any other booting CPU */
+		rmm_boot_failed = true;
 	}
 
-	ctx->state = RMM_STATE_IDLE;
 	return NULL;
 }
 
@@ -328,15 +384,15 @@ static int gpt_to_gts_error(int error, uint32_t smc_fid, uint64_t address)
 	int ret;
 
 	if (error == 0) {
-		return RMMD_OK;
+		return E_RMM_OK;
 	}
 
 	if (error == -EINVAL) {
-		ret = RMMD_ERR_BAD_ADDR;
+		ret = E_RMM_BAD_ADDR;
 	} else {
 		/* This is the only other error code we expect */
 		assert(error == -EPERM);
-		ret = RMMD_ERR_BAD_PAS;
+		ret = E_RMM_BAD_PAS;
 	}
 
 	ERROR("RMMD: PAS Transition failed. GPT ret = %d, PA: 0x%"PRIx64 ", FID = 0x%x\n",
@@ -354,6 +410,12 @@ uint64_t rmmd_rmm_el3_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2,
 	uint32_t src_sec_state;
 	int ret;
 
+	/* If RMM failed to boot, treat any RMM-EL3 interface SMC as unknown */
+	if (rmm_boot_failed) {
+		WARN("RMMD: Failed to boot up RMM. Ignoring RMM-EL3 call\n");
+		SMC_RET1(handle, SMC_UNK);
+	}
+
 	/* Determine which security state this SMC originated from */
 	src_sec_state = caller_sec_state(flags);
 
@@ -363,18 +425,23 @@ uint64_t rmmd_rmm_el3_handler(uint32_t smc_fid, uint64_t x1, uint64_t x2,
 	}
 
 	switch (smc_fid) {
-	case RMMD_GTSI_DELEGATE:
+	case RMM_GTSI_DELEGATE:
 		ret = gpt_delegate_pas(x1, PAGE_SIZE_4KB, SMC_FROM_REALM);
 		SMC_RET1(handle, gpt_to_gts_error(ret, smc_fid, x1));
-	case RMMD_GTSI_UNDELEGATE:
+	case RMM_GTSI_UNDELEGATE:
 		ret = gpt_undelegate_pas(x1, PAGE_SIZE_4KB, SMC_FROM_REALM);
 		SMC_RET1(handle, gpt_to_gts_error(ret, smc_fid, x1));
-	case RMMD_ATTEST_GET_PLAT_TOKEN:
+	case RMM_ATTEST_GET_PLAT_TOKEN:
 		ret = rmmd_attest_get_platform_token(x1, &x2, x3);
 		SMC_RET2(handle, ret, x2);
-	case RMMD_ATTEST_GET_REALM_KEY:
+	case RMM_ATTEST_GET_REALM_KEY:
 		ret = rmmd_attest_get_signing_key(x1, &x2, x3);
 		SMC_RET2(handle, ret, x2);
+
+	case RMM_BOOT_COMPLETE:
+		VERBOSE("RMMD: running rmmd_rmm_sync_exit\n");
+		rmmd_rmm_sync_exit(x1);
+
 	default:
 		WARN("RMMD: Unsupported RMM-EL3 call 0x%08x\n", smc_fid);
 		SMC_RET1(handle, SMC_UNK);