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>

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;
+		}
+
+		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