diff --git a/bl1/aarch32/bl1_context_mgmt.c b/bl1/aarch32/bl1_context_mgmt.c index 005d046cc..b5a6a3417 100644 --- 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 032631905..8be8830a3 100644 --- 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); diff --git a/include/arch/aarch32/arch.h b/include/arch/aarch32/arch.h index 0db414588..34036d785 100644 --- a/include/arch/aarch32/arch.h +++ b/include/arch/aarch32/arch.h @@ -294,6 +294,8 @@ #define SPSR_MODE_SHIFT U(0) #define SPSR_MODE_MASK U(0x7) +#define SPSR_SSBS_BIT BIT_32(23) + #define DISABLE_ALL_EXCEPTIONS \ (SPSR_FIQ_BIT | SPSR_IRQ_BIT | SPSR_ABT_BIT) @@ -384,11 +386,12 @@ #define GET_M32(mode) (((mode) >> MODE32_SHIFT) & MODE32_MASK) #define SPSR_MODE32(mode, isa, endian, aif) \ - (MODE_RW_32 << MODE_RW_SHIFT | \ + ((MODE_RW_32 << MODE_RW_SHIFT | \ ((mode) & MODE32_MASK) << MODE32_SHIFT | \ ((isa) & SPSR_T_MASK) << SPSR_T_SHIFT | \ ((endian) & SPSR_E_MASK) << SPSR_E_SHIFT | \ - ((aif) & SPSR_AIF_MASK) << SPSR_AIF_SHIFT) + ((aif) & SPSR_AIF_MASK) << SPSR_AIF_SHIFT) & \ + (~(SPSR_SSBS_BIT))) /* * TTBR definitions diff --git a/include/arch/aarch64/arch.h b/include/arch/aarch64/arch.h index e4147d7e9..fa857fb1b 100644 --- a/include/arch/aarch64/arch.h +++ b/include/arch/aarch64/arch.h @@ -419,6 +419,9 @@ #define SPSR_M_AARCH64 U(0x0) #define SPSR_M_AARCH32 U(0x1) +#define SPSR_SSBS_BIT_AARCH64 BIT_64(12) +#define SPSR_SSBS_BIT_AARCH32 BIT_64(23) + #define DISABLE_ALL_EXCEPTIONS \ (DAIF_FIQ_BIT | DAIF_IRQ_BIT | DAIF_ABT_BIT | DAIF_DBG_BIT) @@ -543,18 +546,20 @@ #define GET_SP(mode) (((mode) >> MODE_SP_SHIFT) & MODE_SP_MASK) #define GET_M32(mode) (((mode) >> MODE32_SHIFT) & MODE32_MASK) -#define SPSR_64(el, sp, daif) \ - ((MODE_RW_64 << MODE_RW_SHIFT) | \ - (((el) & MODE_EL_MASK) << MODE_EL_SHIFT) | \ - (((sp) & MODE_SP_MASK) << MODE_SP_SHIFT) | \ - (((daif) & SPSR_DAIF_MASK) << SPSR_DAIF_SHIFT)) +#define SPSR_64(el, sp, daif) \ + (((MODE_RW_64 << MODE_RW_SHIFT) | \ + (((el) & MODE_EL_MASK) << MODE_EL_SHIFT) | \ + (((sp) & MODE_SP_MASK) << MODE_SP_SHIFT) | \ + (((daif) & SPSR_DAIF_MASK) << SPSR_DAIF_SHIFT)) & \ + (~(SPSR_SSBS_BIT_AARCH64))) #define SPSR_MODE32(mode, isa, endian, aif) \ - ((MODE_RW_32 << MODE_RW_SHIFT) | \ + (((MODE_RW_32 << MODE_RW_SHIFT) | \ (((mode) & MODE32_MASK) << MODE32_SHIFT) | \ (((isa) & SPSR_T_MASK) << SPSR_T_SHIFT) | \ (((endian) & SPSR_E_MASK) << SPSR_E_SHIFT) | \ - (((aif) & SPSR_AIF_MASK) << SPSR_AIF_SHIFT)) + (((aif) & SPSR_AIF_MASK) << SPSR_AIF_SHIFT)) & \ + (~(SPSR_SSBS_BIT_AARCH32))) /* * TTBR Definitions