diff --git a/plat/socionext/synquacer/sq_bl31_setup.c b/plat/socionext/synquacer/sq_bl31_setup.c index 3ebf788..481913b 100644 --- a/plat/socionext/synquacer/sq_bl31_setup.c +++ b/plat/socionext/synquacer/sq_bl31_setup.c @@ -13,6 +13,45 @@ #include static console_pl011_t console; +static entry_point_info_t bl32_image_ep_info; +static entry_point_info_t bl33_image_ep_info; + +entry_point_info_t *bl31_plat_get_next_image_ep_info(uint32_t type) +{ + assert(sec_state_is_valid(type)); + return type == NON_SECURE ? &bl33_image_ep_info : &bl32_image_ep_info; +} + +/******************************************************************************* + * Gets SPSR for BL32 entry + ******************************************************************************/ +uint32_t sq_get_spsr_for_bl32_entry(void) +{ + /* + * The Secure Payload Dispatcher service is responsible for + * setting the SPSR prior to entry into the BL32 image. + */ + return 0; +} + +/******************************************************************************* + * Gets SPSR for BL33 entry + ******************************************************************************/ +uint32_t sq_get_spsr_for_bl33_entry(void) +{ + unsigned long el_status; + unsigned int mode; + uint32_t spsr; + + /* Figure out what mode we enter the non-secure world in */ + el_status = read_id_aa64pfr0_el1() >> ID_AA64PFR0_EL2_SHIFT; + el_status &= ID_AA64PFR0_ELX_MASK; + + mode = (el_status) ? MODE_EL2 : MODE_EL1; + + spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS); + return spsr; +} void bl31_early_platform_setup(bl31_params_t *from_bl2, void *plat_params_from_bl2) @@ -28,6 +67,30 @@ /* There are no parameters from BL2 if BL31 is a reset vector */ assert(from_bl2 == NULL); assert(plat_params_from_bl2 == NULL); + +#ifdef BL32_BASE + /* Populate entry point information for BL32 */ + SET_PARAM_HEAD(&bl32_image_ep_info, + PARAM_EP, + VERSION_1, + 0); + SET_SECURITY_STATE(bl32_image_ep_info.h.attr, SECURE); + bl32_image_ep_info.pc = BL32_BASE; + bl32_image_ep_info.spsr = sq_get_spsr_for_bl32_entry(); +#endif /* BL32_BASE */ + + /* Populate entry point information for BL33 */ + SET_PARAM_HEAD(&bl33_image_ep_info, + PARAM_EP, + VERSION_1, + 0); + /* + * Tell BL31 where the non-trusted software image + * is located and the entry state information + */ + bl33_image_ep_info.pc = PRELOADED_BL33_BASE; + bl33_image_ep_info.spsr = sq_get_spsr_for_bl33_entry(); + SET_SECURITY_STATE(bl33_image_ep_info.h.attr, NON_SECURE); } void bl31_platform_setup(void)