static void tbase_setup_entry_common( cpu_context_t *s_context,
cpu_context_t *ns_context,
uint32_t call_offset) {
// Set up registers
gp_regs_t *s_gpregs = get_gpregs_ctx(s_context);
// NWd spsr
uint64_t ns_spsr = read_ctx_reg(get_el3state_ctx(ns_context), CTX_SPSR_EL3);
write_ctx_reg(s_gpregs, CTX_GPREG_X2, ns_spsr);
// Entry to tbase
el3_state_t *el3sysregs = get_el3state_ctx(s_context);
write_ctx_reg(el3sysregs, CTX_SPSR_EL3, tbaseEntrySpsr);
cm_set_elr_el3(SECURE,tbaseEntryBase+call_offset);
}
/*
* Handle SMC from a lower exception level to switch its execution state
* (either from AArch64 to AArch32, or vice versa).
*
* smc_fid:
* SMC function ID - either ARM_SIP_SVC_STATE_SWITCH_64 or
* ARM_SIP_SVC_STATE_SWITCH_32.
* pc_hi, pc_lo:
* PC upon re-entry to the calling exception level; width dependent on the
* calling exception level.
* cookie_hi, cookie_lo:
* Opaque pointer pairs received from the caller to pass it back, upon
* re-entry.
* handle:
* Handle to saved context.
*/
int arm_execution_state_switch(unsigned int smc_fid,
uint32_t pc_hi,
uint32_t pc_lo,
uint32_t cookie_hi,
uint32_t cookie_lo,
void *handle)
{
/* Execution state can be switched only if EL3 is AArch64 */
#ifdef AARCH64
int caller_64, from_el2, el, endianness, thumb = 0;
u_register_t spsr, pc, scr, sctlr;
entry_point_info_t ep;
cpu_context_t *ctx = (cpu_context_t *) handle;
el3_state_t *el3_ctx = get_el3state_ctx(ctx);
/* That the SMC originated from NS is already validated by the caller */
/*
* Disallow state switch if any of the secondaries have been brought up.
*/
if (psci_secondaries_brought_up())
goto exec_denied;
spsr = read_ctx_reg(el3_ctx, CTX_SPSR_EL3);
caller_64 = (GET_RW(spsr) == MODE_RW_64);
if (caller_64) {
/*
* If the call originated from AArch64, expect 32-bit pointers when
* switching to AArch32.
*/
if ((pc_hi != 0) || (cookie_hi != 0))
goto invalid_param;
pc = pc_lo;
/* Instruction state when entering AArch32 */
thumb = pc & 1;
} else {
/* Construct AArch64 PC */
pc = (((u_register_t) pc_hi) << 32) | pc_lo;
}
/* Make sure PC is 4-byte aligned, except for Thumb */
if ((pc & 0x3) && !thumb)
goto invalid_param;
/*
* EL3 controls register width of the immediate lower EL only. Expect
* this request from EL2/Hyp unless:
*
* - EL2 is not implemented;
* - EL2 is implemented, but was disabled. This can be inferred from
* SCR_EL3.HCE.
*/
from_el2 = caller_64 ? (GET_EL(spsr) == MODE_EL2) :
(GET_M32(spsr) == MODE32_hyp);
scr = read_ctx_reg(el3_ctx, CTX_SCR_EL3);
if (!from_el2) {
/* The call is from NS privilege level other than HYP */
/*
* Disallow switching state if there's a Hypervisor in place;
* this request must be taken up with the Hypervisor instead.
*/
if (scr & SCR_HCE_BIT)
goto exec_denied;
}
/*
* Return to the caller using the same endianness. Extract
* endianness bit from the respective system control register
* directly.
*/
sctlr = from_el2 ? read_sctlr_el2() : read_sctlr_el1();
endianness = !!(sctlr & SCTLR_EE_BIT);
/* Construct SPSR for the exception state we're about to switch to */
if (caller_64) {
int impl;
/*
* Switching from AArch64 to AArch32. Ensure this CPU implements
* the target EL in AArch32.
*/
impl = from_el2 ? EL_IMPLEMENTED(2) : EL_IMPLEMENTED(1);
if (impl != EL_IMPL_A64_A32)
goto exec_denied;
/* Return to the equivalent AArch32 privilege level */
el = from_el2 ? MODE32_hyp : MODE32_svc;
spsr = SPSR_MODE32(el, thumb ? SPSR_T_THUMB : SPSR_T_ARM,
endianness, DISABLE_ALL_EXCEPTIONS);
} else {
/*
* Switching from AArch32 to AArch64. Since it's not possible to
* implement an EL as AArch32-only (from which this call was
* raised), it's safe to assume AArch64 is also implemented.
*/
el = from_el2 ? MODE_EL2 : MODE_EL1;
spsr = SPSR_64(el, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS);
}
/*
* Use the context management library to re-initialize the existing
* context with the execution state flipped. Since the library takes
* entry_point_info_t pointer as the argument, construct a dummy one
* with PC, state width, endianness, security etc. appropriately set.
* Other entries in the entry point structure are irrelevant for
* purpose.
*/
zeromem(&ep, sizeof(ep));
ep.pc = pc;
ep.spsr = spsr;
SET_PARAM_HEAD(&ep, PARAM_EP, VERSION_1,
((endianness ? EP_EE_BIG : EP_EE_LITTLE) | NON_SECURE |
EP_ST_DISABLE));
/*
* Re-initialize the system register context, and exit EL3 as if for the
* first time. State switch is effectively a soft reset of the
* calling EL.
*/
cm_init_my_context(&ep);
cm_prepare_el3_exit(NON_SECURE);
/*
* State switch success. The caller of SMC wouldn't see the SMC
* returning. Instead, execution starts at the supplied entry point,
* with context pointers populated in registers 0 and 1.
*/
SMC_RET2(handle, cookie_hi, cookie_lo);
invalid_param:
SMC_RET1(handle, STATE_SW_E_PARAM);
exec_denied:
#endif
/* State switch denied */
SMC_RET1(handle, STATE_SW_E_DENIED);
}
static int32_t tbase_init_entry()
{
DBG_PRINTF("tbase_init\n\r");
// Save el1 registers in case non-secure world has already been set up.
cm_el1_sysregs_context_save(NON_SECURE);
uint64_t mpidr = read_mpidr();
uint32_t linear_id = platform_get_core_pos(mpidr);
tbase_context *tbase_ctx = &secure_context[linear_id];
// Note: mapping is 1:1, so physical and virtual addresses are here the same.
cpu_context_t *ns_entry_context = (cpu_context_t *) cm_get_context(mpidr, NON_SECURE);
// ************************************************************************************
// Configure parameter passing to tbase
// Calculate page start addresses for register areas.
registerFileStart[REGISTER_FILE_NWD] = page_align((uint64_t)&ns_entry_context, DOWN);
registerFileStart[REGISTER_FILE_MONITOR] = page_align((uint64_t)&msm_area, DOWN);
// Calculate page end addresses for register areas.
registerFileEnd[REGISTER_FILE_NWD] = (uint64_t)(&ns_entry_context[TBASE_CORE_COUNT]);
registerFileEnd[REGISTER_FILE_MONITOR] = ((uint64_t)&msm_area) +sizeof(msm_area);
int32_t totalPages = 0;
for (int area=0; area<REGISTER_FILE_COUNT; area++) {
int32_t pages = page_align(registerFileEnd[area] - registerFileStart[area], UP) / PAGE_SIZE;
assert( pages +totalPages <= TBASE_INTERFACE_PAGES );
tbase_init_register_file(area, totalPages, pages);
totalPages += pages;
}
// ************************************************************************************
// Create boot structure
tbaseBootCfg.magic = TBASE_BOOTCFG_MAGIC;
tbaseBootCfg.length = sizeof(bootCfg_t);
tbaseBootCfg.version = TBASE_MONITOR_INTERFACE_VERSION;
tbaseBootCfg.dRamBase = TBASE_NWD_DRAM_BASE;
tbaseBootCfg.dRamSize = TBASE_NWD_DRAM_SIZE;
tbaseBootCfg.secDRamBase = TBASE_SWD_DRAM_BASE;
tbaseBootCfg.secDRamSize = TBASE_SWD_DRAM_SIZE;
tbaseBootCfg.secIRamBase = TBASE_SWD_IMEM_BASE;
tbaseBootCfg.secIRamSize = TBASE_SWD_IMEM_SIZE;
tbaseBootCfg.conf_mair_el3 = read_mair_el3();
tbaseBootCfg.MSMPteCount = totalPages;
tbaseBootCfg.MSMBase = (uint64_t)registerFileL2;
tbaseBootCfg.gic_distributor_base = TBASE_GIC_DIST_BASE;
tbaseBootCfg.gic_cpuinterface_base = TBASE_GIC_CPU_BASE;
tbaseBootCfg.gic_version = TBASE_GIC_VERSION;
tbaseBootCfg.total_number_spi = TBASE_SPI_COUNT;
tbaseBootCfg.ssiq_number = TBASE_SSIQ_NRO;
tbaseBootCfg.flags = TBASE_MONITOR_FLAGS;
DBG_PRINTF("*** tbase boot cfg ***\n\r");
DBG_PRINTF("* magic : 0x%.X\n\r",tbaseBootCfg.magic);
DBG_PRINTF("* length : 0x%.X\n\r",tbaseBootCfg.length);
DBG_PRINTF("* version : 0x%.X\n\r",tbaseBootCfg.version);
DBG_PRINTF("* dRamBase : 0x%.X\n\r",tbaseBootCfg.dRamBase);
DBG_PRINTF("* dRamSize : 0x%.X\n\r",tbaseBootCfg.dRamSize);
DBG_PRINTF("* secDRamBase : 0x%.X\n\r",tbaseBootCfg.secDRamBase);
DBG_PRINTF("* secDRamSize : 0x%.X\n\r",tbaseBootCfg.secDRamSize);
DBG_PRINTF("* secIRamBase : 0x%.X\n\r",tbaseBootCfg.secIRamBase);
DBG_PRINTF("* secIRamSize : 0x%.X\n\r",tbaseBootCfg.secIRamSize);
DBG_PRINTF("* conf_mair_el3 : 0x%.X\n\r",tbaseBootCfg.conf_mair_el3);
DBG_PRINTF("* MSMPteCount : 0x%.X\n\r",tbaseBootCfg.MSMPteCount);
DBG_PRINTF("* MSMBase : 0x%.X\n\r",tbaseBootCfg.MSMBase);
DBG_PRINTF("* gic_distributor_base : 0x%.X\n\r",tbaseBootCfg.gic_distributor_base);
DBG_PRINTF("* gic_cpuinterface_base : 0x%.X\n\r",tbaseBootCfg.gic_cpuinterface_base);
DBG_PRINTF("* gic_version : 0x%.X\n\r",tbaseBootCfg.gic_version);
DBG_PRINTF("* total_number_spi : 0x%.X\n\r",tbaseBootCfg.total_number_spi);
DBG_PRINTF("* ssiq_number : 0x%.X\n\r",tbaseBootCfg.ssiq_number);
DBG_PRINTF("* flags : 0x%.X\n\r",tbaseBootCfg.flags);
// ************************************************************************************
// tbaseBootCfg and l2 entries may be accesses uncached, so must flush those.
flush_dcache_range((unsigned long)&tbaseBootCfg, sizeof(bootCfg_t));
flush_dcache_range((unsigned long)®isterFileL2, sizeof(registerFileL2));
// ************************************************************************************
// Set registers for tbase initialization entry
cpu_context_t *s_entry_context = &tbase_ctx->cpu_ctx;
gp_regs_t *s_entry_gpregs = get_gpregs_ctx(s_entry_context);
write_ctx_reg(s_entry_gpregs, CTX_GPREG_X1, 0);
write_ctx_reg(s_entry_gpregs, CTX_GPREG_X1, (int64_t)&tbaseBootCfg);
// SPSR for SMC handling (FIQ mode)
tbaseEntrySpsr = TBASE_ENTRY_SPSR;
DBG_PRINTF("tbase init SPSR 0x%x\n\r", read_ctx_reg(get_el3state_ctx(&tbase_ctx->cpu_ctx),
CTX_SPSR_EL3) );
DBG_PRINTF("tbase SMC SPSR %x\nr\r", tbaseEntrySpsr );
// ************************************************************************************
// Start tbase
tbase_synchronous_sp_entry(tbase_ctx);
tbase_ctx->state = TBASE_STATE_ON;
#if TBASE_PM_ENABLE
// Register power managemnt hooks with PSCI
psci_register_spd_pm_hook(&tbase_pm);
#endif
cm_el1_sysregs_context_restore(NON_SECURE);
cm_set_next_eret_context(NON_SECURE);
return 1;
}