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>
diff --git a/services/std_svc/spm/spci.c b/services/std_svc/spm/spci.c
index 1ee986a..2e12a6c 100644
--- a/services/std_svc/spm/spci.c
+++ b/services/std_svc/spm/spci.c
@@ -380,6 +380,41 @@
}
/*******************************************************************************
+ * 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 @@
/* 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 @@
/* 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);