/*******************************************************************************
* This function invokes the PSCI library interface to initialize the
* non secure cpu context and copies the relevant cpu context register values
* to smc context. These registers will get programmed during `smc_exit`.
******************************************************************************/
static void sp_min_prepare_next_image_entry(void)
{
entry_point_info_t *next_image_info;
cpu_context_t *ctx = cm_get_context(NON_SECURE);
u_register_t ns_sctlr;
/* Program system registers to proceed to non-secure */
next_image_info = sp_min_plat_get_bl33_ep_info();
assert(next_image_info);
assert(NON_SECURE == GET_SECURITY_STATE(next_image_info->h.attr));
INFO("SP_MIN: Preparing exit to normal world\n");
psci_prepare_next_non_secure_ctx(next_image_info);
smc_set_next_ctx(NON_SECURE);
/* Copy r0, lr and spsr from cpu context to SMC context */
copy_cpu_ctx_to_smc_stx(get_regs_ctx(cm_get_context(NON_SECURE)),
smc_get_next_ctx());
/* Temporarily set the NS bit to access NS SCTLR */
write_scr(read_scr() | SCR_NS_BIT);
isb();
ns_sctlr = read_ctx_reg(get_regs_ctx(ctx), CTX_NS_SCTLR);
write_sctlr(ns_sctlr);
isb();
write_scr(read_scr() & ~SCR_NS_BIT);
isb();
}
/******************************************************************************
* This function is invoked during warm boot. Invoke the PSCI library
* warm boot entry point which takes care of Architectural and platform setup/
* restore. Copy the relevant cpu_context register values to smc context which
* will get programmed during `smc_exit`.
*****************************************************************************/
void sp_min_warm_boot(void)
{
smc_ctx_t *next_smc_ctx;
cpu_context_t *ctx = cm_get_context(NON_SECURE);
u_register_t ns_sctlr;
psci_warmboot_entrypoint();
smc_set_next_ctx(NON_SECURE);
next_smc_ctx = smc_get_next_ctx();
zeromem(next_smc_ctx, sizeof(smc_ctx_t));
copy_cpu_ctx_to_smc_stx(get_regs_ctx(cm_get_context(NON_SECURE)),
next_smc_ctx);
/* Temporarily set the NS bit to access NS SCTLR */
write_scr(read_scr() | SCR_NS_BIT);
isb();
ns_sctlr = read_ctx_reg(get_regs_ctx(ctx), CTX_NS_SCTLR);
write_sctlr(ns_sctlr);
isb();
write_scr(read_scr() & ~SCR_NS_BIT);
isb();
}
/*******************************************************************************
* This function programs EL3 registers and performs other setup to enable entry
* into the next image after BL31 at the next ERET.
******************************************************************************/
void bl31_prepare_next_image_entry()
{
el_change_info_t *next_image_info;
uint32_t scr, image_type;
/* Determine which image to execute next */
image_type = bl31_get_next_image_type();
/*
* Setup minimal architectural state of the next highest EL to
* allow execution in it immediately upon entering it.
*/
bl31_next_el_arch_setup(image_type);
/* Program EL3 registers to enable entry into the next EL */
next_image_info = bl31_get_next_image_info(image_type);
assert(next_image_info);
scr = read_scr();
if (image_type == NON_SECURE)
scr |= SCR_NS_BIT;
/*
* Tell the context mgmt. library to ensure that SP_EL3 points to
* the right context to exit from EL3 correctly.
*/
cm_set_el3_eret_context(next_image_info->security_state,
next_image_info->entrypoint,
next_image_info->spsr,
scr);
/* Finally set the next context */
cm_set_next_eret_context(next_image_info->security_state);
}
/*******************************************************************************
* The following function initializes the cpu_context 'ctx' for
* first use, and sets the initial entrypoint state as specified by the
* entry_point_info structure.
*
* The security state to initialize is determined by the SECURE attribute
* of the entry_point_info. The function returns a pointer to the initialized
* context and sets this as the next context to return to.
*
* The EE and ST attributes are used to configure the endianness and secure
* timer availability for the new execution context.
*
* To prepare the register state for entry call cm_prepare_el3_exit() and
* el3_exit(). For Secure-EL1 cm_prepare_el3_exit() is equivalent to
* cm_e1_sysreg_context_restore().
******************************************************************************/
static void cm_init_context_common(cpu_context_t *ctx, const entry_point_info_t *ep)
{
unsigned int security_state;
uint32_t scr, sctlr;
regs_t *reg_ctx;
assert(ctx);
security_state = GET_SECURITY_STATE(ep->h.attr);
/* Clear any residual register values from the context */
memset(ctx, 0, sizeof(*ctx));
reg_ctx = get_regs_ctx(ctx);
/*
* Base the context SCR on the current value, adjust for entry point
* specific requirements
*/
scr = read_scr();
scr &= ~(SCR_NS_BIT | SCR_HCE_BIT);
if (security_state != SECURE)
scr |= SCR_NS_BIT;
/*
* Set up SCTLR for the Non Secure context.
* EE bit is taken from the entrypoint attributes
* M, C and I bits must be zero (as required by PSCI specification)
*
* The target exception level is based on the spsr mode requested.
* If execution is requested to hyp mode, HVC is enabled
* via SCR.HCE.
*
* Always compute the SCTLR_EL1 value and save in the cpu_context
* - the HYP registers are set up by cm_preapre_ns_entry() as they
* are not part of the stored cpu_context
*
* TODO: In debug builds the spsr should be validated and checked
* against the CPU support, security state, endianness and pc
*/
if (security_state != SECURE) {
sctlr = EP_GET_EE(ep->h.attr) ? SCTLR_EE_BIT : 0;
sctlr |= SCTLR_RES1;
write_ctx_reg(reg_ctx, CTX_NS_SCTLR, sctlr);
}
if (GET_M32(ep->spsr) == MODE32_hyp)
scr |= SCR_HCE_BIT;
write_ctx_reg(reg_ctx, CTX_SCR, scr);
write_ctx_reg(reg_ctx, CTX_LR, ep->pc);
write_ctx_reg(reg_ctx, CTX_SPSR, ep->spsr);
/*
* Store the r0-r3 value from the entrypoint into the context
* Use memcpy as we are in control of the layout of the structures
*/
memcpy((void *)reg_ctx, (void *)&ep->args, sizeof(aapcs32_params_t));
}
void change_security_state(unsigned int target_security_state)
{
unsigned long scr = read_scr();
assert(sec_state_is_valid(target_security_state));
if (target_security_state == SECURE)
scr &= ~SCR_NS_BIT;
else
scr |= SCR_NS_BIT;
write_scr(scr);
}
void change_security_state(unsigned int target_security_state)
{
unsigned long scr = read_scr();
if (target_security_state == SECURE)
scr &= ~SCR_NS_BIT;
else if (target_security_state == NON_SECURE)
scr |= SCR_NS_BIT;
else
assert(0);
write_scr(scr);
}
/*******************************************************************************
* BL31 is responsible for setting up the runtime services for the primary cpu
* before passing control to the bootloader (UEFI) or Linux. This function calls
* runtime_svc_init() which initializes all registered runtime services. The run
* time services would setup enough context for the core to swtich to the next
* exception level. When this function returns, the core will switch to the
* programmed exception level via. an ERET.
******************************************************************************/
void bl31_main(void)
{
el_change_info *next_image_info;
uint32_t scr;
/* Perform remaining generic architectural setup from EL3 */
bl31_arch_setup();
/* Perform platform setup in BL1 */
bl31_platform_setup();
#if defined (__GNUC__)
printf("BL31 Built : %s, %s\n\r", __TIME__, __DATE__);
#endif
/* Initialise helper libraries */
bl31_lib_init();
/* Initialize the runtime services e.g. psci */
runtime_svc_init();
/* Clean caches before re-entering normal world */
dcsw_op_all(DCCSW);
/*
* Setup minimal architectural state of the next highest EL to
* allow execution in it immediately upon entering it.
*/
bl31_arch_next_el_setup();
/* Program EL3 registers to enable entry into the next EL */
next_image_info = bl31_get_next_image_info();
scr = read_scr();
if (next_image_info->security_state == NON_SECURE)
scr |= SCR_NS_BIT;
/*
* Tell the context mgmt. library to ensure that SP_EL3 points to
* the right context to exit from EL3 correctly.
*/
cm_set_el3_eret_context(next_image_info->security_state,
next_image_info->entrypoint,
next_image_info->spsr,
scr);
/* Finally set the next context */
cm_set_next_eret_context(next_image_info->security_state);
}
//*******************************************************************************
// Configure tbase and EL3 registers for intial entry
static void tbase_init_eret( uint64_t entrypoint, uint32_t rw ) {
uint32_t scr = read_scr();
uint32_t spsr = TBASE_INIT_SPSR;
assert(rw == TBASE_AARCH32);
// Set the right security state and register width for the SP
scr &= ~SCR_NS_BIT;
scr &= ~SCR_RW_BIT;
// Also IRQ and FIQ handled in secure state
scr &= ~(SCR_FIQ_BIT|SCR_IRQ_BIT);
// No execution from Non-secure memory
scr |= SCR_SIF_BIT;
cm_set_el3_eret_context(SECURE, entrypoint, spsr, scr);
entry_point_info_t *image_info = bl31_plat_get_next_image_ep_info(SECURE);
assert(image_info);
image_info->spsr = spsr;
}
/*******************************************************************************
* This function programs EL3 registers and performs other setup to enable entry
* into the next image after BL31 at the next ERET.
******************************************************************************/
void bl31_prepare_next_image_entry()
{
entry_point_info_t *next_image_info;
uint32_t scr, image_type;
cpu_context_t *ctx;
gp_regs_t *gp_regs;
/* Determine which image to execute next */
image_type = bl31_get_next_image_type();
/*
* Setup minimal architectural state of the next highest EL to
* allow execution in it immediately upon entering it.
*/
bl31_next_el_arch_setup(image_type);
/* Program EL3 registers to enable entry into the next EL */
next_image_info = bl31_plat_get_next_image_ep_info(image_type);
assert(next_image_info);
assert(image_type == GET_SECURITY_STATE(next_image_info->h.attr));
scr = read_scr();
scr &= ~SCR_NS_BIT;
if (image_type == NON_SECURE)
scr |= SCR_NS_BIT;
scr &= ~SCR_RW_BIT;
if ((next_image_info->spsr & (1 << MODE_RW_SHIFT)) ==
(MODE_RW_64 << MODE_RW_SHIFT))
{
scr |= SCR_RW_BIT;
scr |= SCR_HCE_BIT;
}
else
{
scr &= ~(SCR_HCE_BIT);
}
/*
* FIXME: Need a configurable flag when we have hypervisor installed
* This is for PSCI CPU_UP api to work correctly
* PSCI uses scr.hce to determine the target CPU of CPU_UP
* returns to NS world with HYP mode(HCE is set) or SVC mode(HCE is not set)
* (refer to psci_set_ns_entry_info() in psci_common.c)
* since we don't have hypervisor installed for now, we need to
* enter linux with SVC mode.
*/
// FIXME: For 64bit kernel, we need return to normal world with EL2
// Temporary comment out this to enable 64bit kernel smp.
// Need a method to configure this for 32bit/64bit kernel
//scr &= ~(SCR_HCE_BIT);
/*
* Tell the context mgmt. library to ensure that SP_EL3 points to
* the right context to exit from EL3 correctly.
*/
cm_set_el3_eret_context(image_type,
next_image_info->pc,
next_image_info->spsr,
scr);
/*
* Save the args generated in BL2 for the image in the right context
* used on its entry
*/
ctx = cm_get_context(read_mpidr(), image_type);
gp_regs = get_gpregs_ctx(ctx);
memcpy(gp_regs, (void *)&next_image_info->args, sizeof(aapcs64_params_t));
/* Finally set the next context */
cm_set_next_eret_context(image_type);
}
void bl31_prepare_k64_entry(void)
{
entry_point_info_t *next_image_info;
uint32_t scr, image_type;
cpu_context_t *ctx;
gp_regs_t *gp_regs;
/* Determine which image to execute next */
image_type = NON_SECURE; //bl31_get_next_image_type();
/*
* Setup minimal architectural state of the next highest EL to
* allow execution in it immediately upon entering it.
*/
bl31_next_el_arch_setup(image_type);
/* Program EL3 registers to enable entry into the next EL */
next_image_info = bl31_plat_get_next_kernel_ep_info(image_type);
assert(next_image_info);
assert(image_type == GET_SECURITY_STATE(next_image_info->h.attr));
/* check is set 64bit kernel*/
printf("next_image_info->spsr = 0x%llx\n", next_image_info->spsr);
scr = read_scr();
scr &= ~SCR_NS_BIT;
if (image_type == NON_SECURE)
scr |= SCR_NS_BIT;
scr &= ~SCR_RW_BIT;
if ((next_image_info->spsr & (1 << MODE_RW_SHIFT)) ==
(MODE_RW_64 << MODE_RW_SHIFT))
{
scr |= SCR_RW_BIT;
printf("spsr is 64 bit\n");
}
scr |= SCR_HCE_BIT;
/*
* Tell the context mgmt. library to ensure that SP_EL3 points to
* the right context to exit from EL3 correctly.
*/
cm_set_el3_eret_context(image_type,
next_image_info->pc,
next_image_info->spsr,
scr);
/*
* Save the args generated in BL2 for the image in the right context
* used on its entry
*/
ctx = cm_get_context(read_mpidr(), image_type);
gp_regs = get_gpregs_ctx(ctx);
memcpy(gp_regs, (void *)&next_image_info->args, sizeof(aapcs64_params_t));
printf("Finally set the next context\n");
/* Finally set the next context */
cm_set_next_eret_context(image_type);
}
/*******************************************************************************
* Prepare the CPU system registers for first entry into secure or normal world
*
* If execution is requested to hyp mode, HSCTLR is initialized
* If execution is requested to non-secure PL1, and the CPU supports
* HYP mode then HYP mode is disabled by configuring all necessary HYP mode
* registers.
******************************************************************************/
void cm_prepare_el3_exit(uint32_t security_state)
{
uint32_t sctlr, scr, hcptr;
cpu_context_t *ctx = cm_get_context(security_state);
assert(ctx);
if (security_state == NON_SECURE) {
scr = read_ctx_reg(get_regs_ctx(ctx), CTX_SCR);
if (scr & SCR_HCE_BIT) {
/* Use SCTLR value to initialize HSCTLR */
sctlr = read_ctx_reg(get_regs_ctx(ctx),
CTX_NS_SCTLR);
sctlr |= HSCTLR_RES1;
/* Temporarily set the NS bit to access HSCTLR */
write_scr(read_scr() | SCR_NS_BIT);
/*
* Make sure the write to SCR is complete so that
* we can access HSCTLR
*/
isb();
write_hsctlr(sctlr);
isb();
write_scr(read_scr() & ~SCR_NS_BIT);
isb();
} else if (read_id_pfr1() &
(ID_PFR1_VIRTEXT_MASK << ID_PFR1_VIRTEXT_SHIFT)) {
/*
* Set the NS bit to access NS copies of certain banked
* registers
*/
write_scr(read_scr() | SCR_NS_BIT);
isb();
/* PL2 present but unused, need to disable safely */
write_hcr(0);
/* HSCTLR : can be ignored when bypassing */
/* HCPTR : disable all traps TCPAC, TTA, TCP */
hcptr = read_hcptr();
hcptr &= ~(TCPAC_BIT | TTA_BIT | TCP11_BIT | TCP10_BIT);
write_hcptr(hcptr);
/* Enable EL1 access to timer */
write_cnthctl(PL1PCEN_BIT | PL1PCTEN_BIT);
/* Reset CNTVOFF_EL2 */
write64_cntvoff(0);
/* Set VPIDR, VMPIDR to match MIDR, MPIDR */
write_vpidr(read_midr());
write_vmpidr(read_mpidr());
/*
* Reset VTTBR.
* Needed because cache maintenance operations depend on
* the VMID even when non-secure EL1&0 stage 2 address
* translation are disabled.
*/
write64_vttbr(0);
/*
* Avoid unexpected debug traps in case where HDCR
* is not completely reset by the hardware - set
* HDCR.HPMN to PMCR.N and zero the remaining bits.
* The HDCR.HPMN and PMCR.N fields are the same size
* (5 bits) and HPMN is at offset zero within HDCR.
*/
write_hdcr((read_pmcr() & PMCR_N_BITS) >> PMCR_N_SHIFT);
/*
* Reset CNTHP_CTL to disable the EL2 physical timer and
* therefore prevent timer interrupts.
*/
write_cnthp_ctl(0);
isb();
write_scr(read_scr() & ~SCR_NS_BIT);
isb();
}
/*******************************************************************************
* Prepare the CPU system registers for first entry into secure or normal world
*
* If execution is requested to hyp mode, HSCTLR is initialized
* If execution is requested to non-secure PL1, and the CPU supports
* HYP mode then HYP mode is disabled by configuring all necessary HYP mode
* registers.
******************************************************************************/
void cm_prepare_el3_exit(uint32_t security_state)
{
uint32_t sctlr, scr, hcptr;
cpu_context_t *ctx = cm_get_context(security_state);
assert(ctx);
if (security_state == NON_SECURE) {
scr = read_ctx_reg(get_regs_ctx(ctx), CTX_SCR);
if (scr & SCR_HCE_BIT) {
/* Use SCTLR value to initialize HSCTLR */
sctlr = read_ctx_reg(get_regs_ctx(ctx),
CTX_NS_SCTLR);
sctlr |= HSCTLR_RES1;
/* Temporarily set the NS bit to access HSCTLR */
write_scr(read_scr() | SCR_NS_BIT);
/*
* Make sure the write to SCR is complete so that
* we can access HSCTLR
*/
isb();
write_hsctlr(sctlr);
isb();
write_scr(read_scr() & ~SCR_NS_BIT);
isb();
} else if (read_id_pfr1() &
(ID_PFR1_VIRTEXT_MASK << ID_PFR1_VIRTEXT_SHIFT)) {
/* Set the NS bit to access HCR, HCPTR, CNTHCTL, VPIDR, VMPIDR */
write_scr(read_scr() | SCR_NS_BIT);
isb();
/* PL2 present but unused, need to disable safely */
write_hcr(0);
/* HSCTLR : can be ignored when bypassing */
/* HCPTR : disable all traps TCPAC, TTA, TCP */
hcptr = read_hcptr();
hcptr &= ~(TCPAC_BIT | TTA_BIT | TCP11_BIT | TCP10_BIT);
write_hcptr(hcptr);
/* Enable EL1 access to timer */
write_cnthctl(PL1PCEN_BIT | PL1PCTEN_BIT);
/* Reset CNTVOFF_EL2 */
write64_cntvoff(0);
/* Set VPIDR, VMPIDR to match MIDR, MPIDR */
write_vpidr(read_midr());
write_vmpidr(read_mpidr());
/*
* Reset VTTBR.
* Needed because cache maintenance operations depend on
* the VMID even when non-secure EL1&0 stage 2 address
* translation are disabled.
*/
write64_vttbr(0);
isb();
write_scr(read_scr() & ~SCR_NS_BIT);
isb();
}
}
}