Support asynchronous method for BL3-2 initialization
This patch adds support for BL3-2 initialization by asynchronous
method where BL3-1 transfers control to BL3-2 using world switch.
After BL3-2 initialization, it transfers control to BL3-3 via SPD
service handler. The SPD service handler initializes the CPU context
to BL3-3 entrypoint depending on the return function indentifier from
TSP initialization.
Fixes ARM-software/TF-issues#184
Change-Id: I7b135c2ceeb356d3bb5b6a287932e96ac67c7a34
diff --git a/services/spd/tspd/tspd_main.c b/services/spd/tspd/tspd_main.c
index b2c3615..22f302a 100644
--- a/services/spd/tspd/tspd_main.c
+++ b/services/spd/tspd/tspd_main.c
@@ -181,12 +181,15 @@
tsp_ep_info->pc,
&tspd_sp_context[linear_id]);
+#if TSP_INIT_ASYNC
+ bl31_set_next_image_type(SECURE);
+#else
/*
* All TSPD initialization done. Now register our init function with
* BL31 for deferred invocation
*/
bl31_register_bl32_init(&tspd_init);
-
+#endif
return 0;
}
@@ -202,10 +205,10 @@
int32_t tspd_init(void)
{
uint64_t mpidr = read_mpidr();
- uint32_t linear_id = platform_get_core_pos(mpidr), flags;
- uint64_t rc;
+ uint32_t linear_id = platform_get_core_pos(mpidr);
tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
entry_point_info_t *tsp_entry_point;
+ uint64_t rc;
/*
* Get information about the Secure Payload (BL32) image. Its
@@ -217,32 +220,11 @@
cm_init_context(mpidr, tsp_entry_point);
/*
- * Arrange for an entry into the test secure payload. We expect an array
- * of vectors in return
+ * Arrange for an entry into the test secure payload. It will be
+ * returned via TSP_ENTRY_DONE case
*/
rc = tspd_synchronous_sp_entry(tsp_ctx);
assert(rc != 0);
- if (rc) {
- set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_ON);
-
- /*
- * TSP has been successfully initialized. Register power
- * managemnt hooks with PSCI
- */
- psci_register_spd_pm_hook(&tspd_pm);
- }
-
- /*
- * Register an interrupt handler for S-EL1 interrupts when generated
- * during code executing in the non-secure state.
- */
- flags = 0;
- set_interrupt_rm_flag(flags, NON_SECURE);
- rc = register_interrupt_type_handler(INTR_TYPE_S_EL1,
- tspd_sel1_interrupt_handler,
- flags);
- if (rc)
- panic();
return rc;
}
@@ -269,6 +251,10 @@
unsigned long mpidr = read_mpidr();
uint32_t linear_id = platform_get_core_pos(mpidr), ns;
tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id];
+ uint64_t rc;
+#if TSP_INIT_ASYNC
+ entry_point_info_t *next_image_info;
+#endif
/* Determine which security state this SMC originated from */
ns = is_caller_non_secure(flags);
@@ -382,6 +368,45 @@
assert(tsp_vectors == NULL);
tsp_vectors = (tsp_vectors_t *) x1;
+ if (tsp_vectors) {
+ set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_ON);
+
+ /*
+ * TSP has been successfully initialized. Register power
+ * managemnt hooks with PSCI
+ */
+ psci_register_spd_pm_hook(&tspd_pm);
+
+ /*
+ * Register an interrupt handler for S-EL1 interrupts
+ * when generated during code executing in the
+ * non-secure state.
+ */
+ flags = 0;
+ set_interrupt_rm_flag(flags, NON_SECURE);
+ rc = register_interrupt_type_handler(INTR_TYPE_S_EL1,
+ tspd_sel1_interrupt_handler,
+ flags);
+ if (rc)
+ panic();
+ }
+
+
+#if TSP_INIT_ASYNC
+ /* Save the Secure EL1 system register context */
+ assert(cm_get_context(SECURE) == &tsp_ctx->cpu_ctx);
+ cm_el1_sysregs_context_save(SECURE);
+
+ /* Program EL3 registers to enable entry into the next EL */
+ next_image_info = bl31_plat_get_next_image_ep_info(NON_SECURE);
+ assert(next_image_info);
+ assert(NON_SECURE ==
+ GET_SECURITY_STATE(next_image_info->h.attr));
+
+ cm_init_context(read_mpidr_el1(), next_image_info);
+ cm_prepare_el3_exit(NON_SECURE);
+ SMC_RET0(cm_get_context(NON_SECURE));
+#else
/*
* SP reports completion. The SPD must have initiated
* the original request through a synchronous entry
@@ -389,6 +414,7 @@
* context.
*/
tspd_synchronous_sp_exit(tsp_ctx, x1);
+#endif
/*
* These function IDs is used only by the SP to indicate it has