Refactor SPSR initialisation code

Change-Id: Ic3b30de13e314efca30fc71370227d3e76f1148b
Signed-off-by: John Tsichritzis <john.tsichritzis@arm.com>

diff --git a/bl1/aarch32/bl1_context_mgmt.c b/bl1/aarch32/bl1_context_mgmt.c
index 005d046cc13a5f115bb509a5efc971f28ded51a4..b5a6a341787a123ed05facb847671ba0510f3917 100644 (file)
--- a/bl1/aarch32/bl1_context_mgmt.c
+++ b/bl1/aarch32/bl1_context_mgmt.c
@@ -102,7 +102,7 @@ static void flush_smc_and_cpu_ctx(void)
  ******************************************************************************/
 void bl1_prepare_next_image(unsigned int image_id)
 {
-       unsigned int security_state;
+       unsigned int security_state, mode = MODE32_svc;
        image_desc_t *image_desc;
        entry_point_info_t *next_bl_ep;
 
@@ -117,20 +117,13 @@ void bl1_prepare_next_image(unsigned int image_id)
        security_state = GET_SECURITY_STATE(next_bl_ep->h.attr);
 
        /* Prepare the SPSR for the next BL image. */
-       if (security_state == SECURE) {
-               next_bl_ep->spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM,
-                       SPSR_E_LITTLE, DISABLE_ALL_EXCEPTIONS);
-       } else {
-               /* Use HYP mode if supported else use SVC. */
-               if (GET_VIRT_EXT(read_id_pfr1())) {
-                       next_bl_ep->spsr = SPSR_MODE32(MODE32_hyp, SPSR_T_ARM,
-                               SPSR_E_LITTLE, DISABLE_ALL_EXCEPTIONS);
-               } else {
-                       next_bl_ep->spsr = SPSR_MODE32(MODE32_svc, SPSR_T_ARM,
-                               SPSR_E_LITTLE, DISABLE_ALL_EXCEPTIONS);
-               }
+       if ((security_state != SECURE) && (GET_VIRT_EXT(read_id_pfr1()))) {
+               mode = MODE32_hyp;
        }
 
+       next_bl_ep->spsr = SPSR_MODE32(mode, SPSR_T_ARM,
+                               SPSR_E_LITTLE, DISABLE_ALL_EXCEPTIONS);
+
        /* Allow platform to make change */
        bl1_plat_set_ep_info(image_id, next_bl_ep);
 

diff --git a/bl1/aarch64/bl1_context_mgmt.c b/bl1/aarch64/bl1_context_mgmt.c
index 0326319057b2fdbcb762e3a450d55a20936c5b7a..8be8830a3877f67a5a3a79c50a3d5506107f5b07 100644 (file)
--- a/bl1/aarch64/bl1_context_mgmt.c
+++ b/bl1/aarch64/bl1_context_mgmt.c
@@ -42,7 +42,7 @@ void cm_set_context(void *context, uint32_t security_state)
  ******************************************************************************/
 void bl1_prepare_next_image(unsigned int image_id)
 {
-       unsigned int security_state;
+       unsigned int security_state, mode = MODE_EL1;
        image_desc_t *image_desc;
        entry_point_info_t *next_bl_ep;
 
@@ -73,20 +73,13 @@ void bl1_prepare_next_image(unsigned int image_id)
                cm_set_context(&bl1_cpu_context[security_state], security_state);
 
        /* Prepare the SPSR for the next BL image. */
-       if (security_state == SECURE) {
-               next_bl_ep->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX,
-                                  DISABLE_ALL_EXCEPTIONS);
-       } else {
-               /* Use EL2 if supported; else use EL1. */
-               if (el_implemented(2) != EL_IMPL_NONE) {
-                       next_bl_ep->spsr = SPSR_64(MODE_EL2, MODE_SP_ELX,
-                               DISABLE_ALL_EXCEPTIONS);
-               } else {
-                       next_bl_ep->spsr = SPSR_64(MODE_EL1, MODE_SP_ELX,
-                          DISABLE_ALL_EXCEPTIONS);
-               }
+       if ((security_state != SECURE) && (el_implemented(2) != EL_IMPL_NONE)) {
+               mode = MODE_EL2;
        }
 
+       next_bl_ep->spsr = SPSR_64(mode, MODE_SP_ELX,
+               DISABLE_ALL_EXCEPTIONS);
+
        /* Allow platform to make change */
        bl1_plat_set_ep_info(image_id, next_bl_ep);