SPM: Create SPCI auxiliary function

Fix variable shadowing warnings and prevent code duplication.

Change-Id: Idb29cc95d6b6943bc012d7bd430afa0e4a7cbf8c
Signed-off-by: Ambroise Vincent <ambroise.vincent@arm.com>

1 files changed, 39 insertions, 56 deletions

diff --git a/services/std_svc/spm/spci.c b/services/std_svc/spm/spci.c
index 1ee986af..2e12a6c6 100644
--- a/services/std_svc/spm/spci.c
+++ b/services/std_svc/spm/spci.c
@@ -380,6 +380,41 @@ static uint64_t spci_service_request_blocking(void *handle,
 }
 
 /*******************************************************************************
+ * This function handles the returned values from the Secure Partition.
+ ******************************************************************************/
+static void spci_handle_returned_values(const cpu_context_t *cpu_ctx,
+					uint64_t ret)
+{
+	if (ret == SPRT_PUT_RESPONSE_AARCH64) {
+		uint32_t token;
+		uint64_t x3, x4, x5, x6;
+
+		token = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X1);
+		x3 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X3);
+		x4 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X4);
+		x5 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X5);
+		x6 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X6);
+
+		uint16_t client_id = x6 & 0xFFFFU;
+		uint16_t service_handle = x6 >> 16;
+
+		int rc = spm_response_add(client_id, service_handle, token,
+					  x3, x4, x5);
+		if (rc != 0) {
+			/*
+			 * This is error fatal because we can't return to the SP
+			 * from this SMC. The SP has crashed.
+			 */
+			panic();
+		}
+	} else if ((ret != SPRT_YIELD_AARCH64) &&
+		   (ret != SPM_SECURE_PARTITION_PREEMPTED)) {
+		ERROR("SPM: %s: Unexpected x0 value 0x%llx\n", __func__, ret);
+		panic();
+	}
+}
+
+/*******************************************************************************
  * This function requests a Secure Service from a given handle and client ID.
  ******************************************************************************/
 static uint64_t spci_service_request_start(void *handle,
@@ -465,34 +500,8 @@ static uint64_t spci_service_request_start(void *handle,
 	/* Jump to the Secure Partition. */
 	uint64_t ret = spm_sp_synchronous_entry(sp_ctx, 1);
 
-	/* Verify returned values */
-	if (ret == SPRT_PUT_RESPONSE_AARCH64) {
-		uint32_t token;
-		uint64_t rx1, rx2, rx3, x6;
-
-		token = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X1);
-		rx1 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X3);
-		rx2 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X4);
-		rx3 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X5);
-		x6 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X6);
-
-		uint16_t client_id = x6 & 0xFFFFU;
-		uint16_t service_handle = x6 >> 16;
-
-		int rc = spm_response_add(client_id, service_handle, token,
-					  rx1, rx2, rx3);
-		if (rc != 0) {
-			/*
-			 * This is error fatal because we can't return to the SP
-			 * from this SMC. The SP has crashed.
-			 */
-			panic();
-		}
-	} else if ((ret != SPRT_YIELD_AARCH64) &&
-		   (ret != SPM_SECURE_PARTITION_PREEMPTED)) {
-		ERROR("SPM: %s: Unexpected x0 value 0x%llx\n", __func__, ret);
-		panic();
-	}
+	/* Handle returned values */
+	spci_handle_returned_values(cpu_ctx, ret);
 
 	/* Flag Secure Partition as idle. */
 	assert(sp_ctx->state == SP_STATE_BUSY);
@@ -572,34 +581,8 @@ static uint64_t spci_service_request_resume(void *handle, u_register_t x1,
 	/* Jump to the Secure Partition. */
 	uint64_t ret = spm_sp_synchronous_entry(sp_ctx, 1);
 
-	/* Verify returned values */
-	if (ret == SPRT_PUT_RESPONSE_AARCH64) {
-		uint32_t token;
-		uint64_t rx1, rx2, rx3, x6;
-
-		token = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X1);
-		rx1 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X3);
-		rx2 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X4);
-		rx3 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X5);
-		x6 = read_ctx_reg(get_gpregs_ctx(cpu_ctx), CTX_GPREG_X6);
-
-		uint16_t client_id = x6 & 0xFFFFU;
-		uint16_t service_handle = x6 >> 16;
-
-		int rc = spm_response_add(client_id, service_handle, token,
-					  rx1, rx2, rx3);
-		if (rc != 0) {
-			/*
-			 * This is error fatal because we can't return to the SP
-			 * from this SMC. The SP has crashed.
-			 */
-			panic();
-		}
-	} else if ((ret != SPRT_YIELD_AARCH64) &&
-		   (ret != SPM_SECURE_PARTITION_PREEMPTED)) {
-		ERROR("SPM: %s: Unexpected x0 value 0x%llx\n", __func__, ret);
-		panic();
-	}
+	/* Handle returned values */
+	spci_handle_returned_values(cpu_ctx, ret);
 
 	/* Flag Secure Partition as idle. */
 	assert(sp_ctx->state == SP_STATE_BUSY);