Fix TSPD implementation of STD SMC ABORT

ABORT SMC used to return to the previously executing world, which
happened to be S-EL1 as it calls a TSP handler using synchronous entry
into the TSP.

Now properly save and restore the non-secure context (including system
registers) and return to non-secure world as it should.

fixes ARM-Software/tf-issues#453

Change-Id: Ie40c79ca2636ab8b6b2ab3106e8f49e0f9117f5f
Signed-off-by: Douglas Raillard <douglas.raillard@arm.com>

diff --git a/services/spd/tspd/tspd_main.c b/services/spd/tspd/tspd_main.c
index 2850e7032418abc1c10b997504e27ff80232d5d1..ff515cca2f36c71f86cf027b0dbce113685d29e5 100644 (file)
--- a/services/spd/tspd/tspd_main.c
+++ b/services/spd/tspd/tspd_main.c
@@ -612,15 +612,26 @@ uint64_t tspd_smc_handler(uint32_t smc_fid,
                        break;
                }
 
+               assert(handle == cm_get_context(NON_SECURE));
+               cm_el1_sysregs_context_save(NON_SECURE);
+
                /* Abort the preempted SMC request */
-               if (!tspd_abort_preempted_smc(tsp_ctx))
+               if (!tspd_abort_preempted_smc(tsp_ctx)) {
                        /*
                         * If there was no preempted SMC to abort, return
                         * SMC_UNK.
+                        *
+                        * Restoring the NON_SECURE context is not necessary as
+                        * the synchronous entry did not take place if the
+                        * return code of tspd_abort_preempted_smc is zero.
                         */
-                       SMC_RET1(handle, SMC_UNK);
+                       cm_set_next_eret_context(NON_SECURE);
+                       break;
+               }
 
-               break;
+               cm_el1_sysregs_context_restore(NON_SECURE);
+               cm_set_next_eret_context(NON_SECURE);
+               SMC_RET0(handle);
 
                /*
                 * Request from non secure world to resume the preempted