SPM: Create SPCI auxiliary function

[project/bcm63xx/atf.git] / services / std_svc / spm / spci.c

diff --git a/services/std_svc/spm/spci.c b/services/std_svc/spm/spci.c
index 1ee986af4285b8b7ebae6092e54b811d56c83c2f..2e12a6c6b2684b60ad0651d8888f899a992b1cc9 100644 (file)
--- a/services/std_svc/spm/spci.c
+++ b/services/std_svc/spm/spci.c
@@ -379,6 +379,41 @@ static uint64_t spci_service_request_blocking(void *handle,
        SMC_RET4(handle, SPCI_SUCCESS, rx1, rx2, rx3);
 }
 
+/*******************************************************************************
+ * 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.
  ******************************************************************************/
@@ -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);