From d663fe7a3002ff028c190eb732278b878e78b7b7 Mon Sep 17 00:00:00 2001
From: Marc Bonnici <marc.bonnici@arm.com>
Date: Fri, 10 Dec 2021 09:21:56 +0000
Subject: [PATCH] feat(spmc): add support for handling FFA_ERROR ABI

This ABI is only valid during SP initialisation to indicate
failure. If this occurs during SP initialisation signal a failure,
otherwise respond with a not supported error code.

Signed-off-by: Marc Bonnici <marc.bonnici@arm.com>
Change-Id: I0182a1641c0f6850e82173af333be79b594f2318
---
 services/std_svc/spm/el3_spmc/spmc_main.c | 46 +++++++++++++++++++++++
 1 file changed, 46 insertions(+)

diff --git a/services/std_svc/spm/el3_spmc/spmc_main.c b/services/std_svc/spm/el3_spmc/spmc_main.c
index ccebcff2e..cfd5a80ba 100644
--- a/services/std_svc/spm/el3_spmc/spmc_main.c
+++ b/services/std_svc/spm/el3_spmc/spmc_main.c
@@ -255,6 +255,48 @@ static uint64_t msg_wait_handler(uint32_t smc_fid,
 			       handle, cookie, flags, FFA_NWD_ID);
 }
 
+static uint64_t ffa_error_handler(uint32_t smc_fid,
+				 bool secure_origin,
+				 uint64_t x1,
+				 uint64_t x2,
+				 uint64_t x3,
+				 uint64_t x4,
+				 void *cookie,
+				 void *handle,
+				 uint64_t flags)
+{
+	struct secure_partition_desc *sp;
+	unsigned int idx;
+
+	/* Check that the response did not originate from the Normal world. */
+	if (!secure_origin) {
+		return spmc_ffa_error_return(handle, FFA_ERROR_NOT_SUPPORTED);
+	}
+
+	/* Get the descriptor of the SP that invoked FFA_ERROR. */
+	sp = spmc_get_current_sp_ctx();
+	if (sp == NULL) {
+		return spmc_ffa_error_return(handle,
+					     FFA_ERROR_INVALID_PARAMETER);
+	}
+
+	/* Get the execution context of the SP that invoked FFA_ERROR. */
+	idx = get_ec_index(sp);
+
+	/*
+	 * We only expect FFA_ERROR to be received during SP initialisation
+	 * otherwise this is an invalid call.
+	 */
+	if (sp->ec[idx].rt_model == RT_MODEL_INIT) {
+		ERROR("SP 0x%x failed to initialize.\n", sp->sp_id);
+		spmc_sp_synchronous_exit(&sp->ec[idx], x2);
+		/* Should not get here. */
+		panic();
+	}
+
+	return spmc_ffa_error_return(handle, FFA_ERROR_NOT_SUPPORTED);
+}
+
 /*******************************************************************************
  * This function will parse the Secure Partition Manifest. From manifest, it
  * will fetch details for preparing Secure partition image context and secure
@@ -564,6 +606,10 @@ uint64_t spmc_smc_handler(uint32_t smc_fid,
 		return msg_wait_handler(smc_fid, secure_origin, x1, x2, x3, x4,
 					cookie, handle, flags);
 
+	case FFA_ERROR:
+		return ffa_error_handler(smc_fid, secure_origin, x1, x2, x3, x4,
+					cookie, handle, flags);
+
 	default:
 		WARN("Unsupported FF-A call 0x%08x.\n", smc_fid);
 		break;