/******************************************************************************* * Handler routine to turn a cpu on. It takes care of any generic, architectural * or platform specific setup required. * TODO: Split this code across separate handlers for each type of setup? ******************************************************************************/ static int psci_afflvl0_on(unsigned long target_cpu, aff_map_node_t *cpu_node, unsigned long ns_entrypoint, unsigned long context_id) { unsigned long psci_entrypoint; uint32_t ns_scr_el3 = read_scr_el3(); uint32_t ns_sctlr_el1 = read_sctlr_el1(); int rc; /* Sanity check to safeguard against data corruption */ assert(cpu_node->level == MPIDR_AFFLVL0); /* * Generic management: Ensure that the cpu is off to be * turned on */ rc = cpu_on_validate_state(cpu_node); if (rc != PSCI_E_SUCCESS) return rc; /* * Call the cpu on handler registered by the Secure Payload Dispatcher * to let it do any bookeeping. If the handler encounters an error, it's * expected to assert within */ if (psci_spd_pm && psci_spd_pm->svc_on) psci_spd_pm->svc_on(target_cpu); /* * Arch. management: Derive the re-entry information for * the non-secure world from the non-secure state from * where this call originated. */ rc = psci_save_ns_entry(target_cpu, ns_entrypoint, context_id, ns_scr_el3, ns_sctlr_el1); if (rc != PSCI_E_SUCCESS) return rc; /* Set the secure world (EL3) re-entry point after BL1 */ psci_entrypoint = (unsigned long) psci_aff_on_finish_entry; if (!psci_plat_pm_ops->affinst_on) return PSCI_E_SUCCESS; /* * Plat. management: Give the platform the current state * of the target cpu to allow it to perform the necessary * steps to power on. */ return psci_plat_pm_ops->affinst_on(target_cpu, psci_entrypoint, ns_entrypoint, cpu_node->level, psci_get_phys_state(cpu_node)); }
/******************************************************************************* * This function determines the full entrypoint information for the requested * PSCI entrypoint on power on/resume and returns it. ******************************************************************************/ static int psci_get_ns_ep_info(entry_point_info_t *ep, uintptr_t entrypoint, u_register_t context_id) { unsigned long ep_attr, sctlr; unsigned int daif, ee, mode; unsigned long ns_scr_el3 = read_scr_el3(); unsigned long ns_sctlr_el1 = read_sctlr_el1(); sctlr = ns_scr_el3 & SCR_HCE_BIT ? read_sctlr_el2() : ns_sctlr_el1; ee = 0; ep_attr = NON_SECURE | EP_ST_DISABLE; if (sctlr & SCTLR_EE_BIT) { ep_attr |= EP_EE_BIG; ee = 1; } SET_PARAM_HEAD(ep, PARAM_EP, VERSION_1, ep_attr); ep->pc = entrypoint; memset(&ep->args, 0, sizeof(ep->args)); ep->args.arg0 = context_id; /* * Figure out whether the cpu enters the non-secure address space * in aarch32 or aarch64 */ if (ns_scr_el3 & SCR_RW_BIT) { /* * Check whether a Thumb entry point has been provided for an * aarch64 EL */ if (entrypoint & 0x1) return PSCI_E_INVALID_ADDRESS; mode = ns_scr_el3 & SCR_HCE_BIT ? MODE_EL2 : MODE_EL1; ep->spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS); } else { mode = ns_scr_el3 & SCR_HCE_BIT ? MODE32_hyp : MODE32_svc; /* * TODO: Choose async. exception bits if HYP mode is not * implemented according to the values of SCR.{AW, FW} bits */ daif = DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT; ep->spsr = SPSR_MODE32(mode, entrypoint & 0x1, ee, daif); } return PSCI_E_SUCCESS; }
bool cpu_mmu_enabled(void) { uint32_t sctlr; #ifdef ARM32 sctlr = read_sctlr(); #else sctlr = read_sctlr_el1(); #endif return sctlr & SCTLR_M ? true : false; }
/* * 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); }
/******************************************************************************* * The next three functions implement a handler for each supported affinity * level which is called when that affinity level is about to be suspended. ******************************************************************************/ static int psci_afflvl0_suspend(aff_map_node_t *cpu_node, unsigned long ns_entrypoint, unsigned long context_id, unsigned int power_state) { unsigned long psci_entrypoint; uint32_t ns_scr_el3 = read_scr_el3(); uint32_t ns_sctlr_el1 = read_sctlr_el1(); int rc; /* Sanity check to safeguard against data corruption */ assert(cpu_node->level == MPIDR_AFFLVL0); /* Save PSCI power state parameter for the core in suspend context */ psci_set_suspend_power_state(power_state); /* * Generic management: Store the re-entry information for the non-secure * world and allow the secure world to suspend itself */ /* * Call the cpu suspend handler registered by the Secure Payload * Dispatcher to let it do any bookeeping. If the handler encounters an * error, it's expected to assert within */ if (psci_spd_pm && psci_spd_pm->svc_suspend) psci_spd_pm->svc_suspend(power_state); /* * Generic management: Store the re-entry information for the * non-secure world */ rc = psci_save_ns_entry(read_mpidr_el1(), ns_entrypoint, context_id, ns_scr_el3, ns_sctlr_el1); if (rc != PSCI_E_SUCCESS) return rc; /* Set the secure world (EL3) re-entry point after BL1 */ psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry; if (!psci_plat_pm_ops->affinst_suspend) return PSCI_E_SUCCESS; /* * Plat. management: Allow the platform to perform the * necessary actions to turn off this cpu e.g. set the * platform defined mailbox with the psci entrypoint, * program the power controller etc. */ rc = psci_plat_pm_ops->affinst_suspend(read_mpidr_el1(), psci_entrypoint, ns_entrypoint, cpu_node->level, psci_get_phys_state(cpu_node)); /* * Arch. management. Perform the necessary steps to flush all * cpu caches. */ psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL0); return rc; }