diff options
Diffstat (limited to 'services')
| -rw-r--r-- | services/spd/tspd/tspd.mk | 54 | ||||
| -rw-r--r-- | services/spd/tspd/tspd_common.c | 157 | ||||
| -rw-r--r-- | services/spd/tspd/tspd_helpers.S | 101 | ||||
| -rw-r--r-- | services/spd/tspd/tspd_main.c | 593 | ||||
| -rw-r--r-- | services/spd/tspd/tspd_pm.c | 205 | ||||
| -rw-r--r-- | services/spd/tspd/tspd_private.h | 203 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_afflvl_off.c | 285 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_afflvl_on.c | 486 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_afflvl_suspend.c | 608 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_common.c | 550 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_entry.S | 171 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_main.c | 270 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_private.h | 156 | ||||
| -rw-r--r-- | services/std_svc/psci/psci_setup.c | 360 | ||||
| -rw-r--r-- | services/std_svc/std_svc_setup.c | 106 |
15 files changed, 4305 insertions, 0 deletions
diff --git a/services/spd/tspd/tspd.mk b/services/spd/tspd/tspd.mk new file mode 100644 index 0000000..a32f4c4 --- /dev/null +++ b/services/spd/tspd/tspd.mk @@ -0,0 +1,54 @@ +# +# Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions are met: +# +# Redistributions of source code must retain the above copyright notice, this +# list of conditions and the following disclaimer. +# +# Redistributions in binary form must reproduce the above copyright notice, +# this list of conditions and the following disclaimer in the documentation +# and/or other materials provided with the distribution. +# +# Neither the name of ARM nor the names of its contributors may be used +# to endorse or promote products derived from this software without specific +# prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +# AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +# IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +# ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +# LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +# CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +# SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +# INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +# CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +# ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# + +TSPD_DIR := services/spd/tspd +SPD_INCLUDES := -Iinclude/bl32/payloads + +SPD_SOURCES := services/spd/tspd/tspd_common.c \ + services/spd/tspd/tspd_helpers.S \ + services/spd/tspd/tspd_main.c \ + services/spd/tspd/tspd_pm.c + +# This dispatcher is paired with a Test Secure Payload source and we intend to +# build the Test Secure Payload along with this dispatcher. +# +# In cases where an associated Secure Payload lies outside this build +# system/source tree, the the dispatcher Makefile can either invoke an external +# build command or assume it pre-built + +BL32_ROOT := bl32/tsp + +# Include SP's Makefile. The assumption is that the TSP's build system is +# compatible with that of Trusted Firmware, and it'll add and populate necessary +# build targets and variables +include ${BL32_ROOT}/tsp.mk + +# Let the top-level Makefile know that we intend to build the SP from source +NEED_BL32 := yes diff --git a/services/spd/tspd/tspd_common.c b/services/spd/tspd/tspd_common.c new file mode 100644 index 0000000..6b3592e --- /dev/null +++ b/services/spd/tspd/tspd_common.c @@ -0,0 +1,157 @@ +/* + * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <arch_helpers.h> +#include <assert.h> +#include <bl_common.h> +#include <context_mgmt.h> +#include <string.h> +#include "tspd_private.h" + +/******************************************************************************* + * Given a secure payload entrypoint, register width, cpu id & pointer to a + * context data structure, this function will create a secure context ready for + * programming an entry into the secure payload. + ******************************************************************************/ +int32_t tspd_init_secure_context(uint64_t entrypoint, + uint32_t rw, + uint64_t mpidr, + tsp_context_t *tsp_ctx) +{ + uint32_t scr, sctlr; + el1_sys_regs_t *el1_state; + uint32_t spsr; + + /* Passing a NULL context is a critical programming error */ + assert(tsp_ctx); + + /* + * We support AArch64 TSP for now. + * TODO: Add support for AArch32 TSP + */ + assert(rw == TSP_AARCH64); + + /* + * This might look redundant if the context was statically + * allocated but this function cannot make that assumption. + */ + memset(tsp_ctx, 0, sizeof(*tsp_ctx)); + + /* + * Set the right security state, register width and enable access to + * the secure physical timer for the SP. + */ + scr = read_scr(); + scr &= ~SCR_NS_BIT; + scr &= ~SCR_RW_BIT; + scr |= SCR_ST_BIT; + if (rw == TSP_AARCH64) + scr |= SCR_RW_BIT; + + /* Get a pointer to the S-EL1 context memory */ + el1_state = get_sysregs_ctx(&tsp_ctx->cpu_ctx); + + /* + * Program the SCTLR_EL1 such that upon entry in S-EL1, caches and MMU are + * disabled and exception endianess is set to be the same as EL3 + */ + sctlr = read_sctlr_el3(); + sctlr &= SCTLR_EE_BIT; + sctlr |= SCTLR_EL1_RES1; + write_ctx_reg(el1_state, CTX_SCTLR_EL1, sctlr); + + /* Set this context as ready to be initialised i.e OFF */ + set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_OFF); + + /* + * This context has not been used yet. It will become valid + * when the TSP is interrupted and wants the TSPD to preserve + * the context. + */ + clr_std_smc_active_flag(tsp_ctx->state); + + /* Associate this context with the cpu specified */ + tsp_ctx->mpidr = mpidr; + + cm_set_context(mpidr, &tsp_ctx->cpu_ctx, SECURE); + spsr = SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS); + cm_set_el3_eret_context(SECURE, entrypoint, spsr, scr); + + return 0; +} + +/******************************************************************************* + * This function takes an SP context pointer and: + * 1. Applies the S-EL1 system register context from tsp_ctx->cpu_ctx. + * 2. Saves the current C runtime state (callee saved registers) on the stack + * frame and saves a reference to this state. + * 3. Calls el3_exit() so that the EL3 system and general purpose registers + * from the tsp_ctx->cpu_ctx are used to enter the secure payload image. + ******************************************************************************/ +uint64_t tspd_synchronous_sp_entry(tsp_context_t *tsp_ctx) +{ + uint64_t rc; + + assert(tsp_ctx->c_rt_ctx == 0); + + /* Apply the Secure EL1 system register context and switch to it */ + assert(cm_get_context(read_mpidr(), SECURE) == &tsp_ctx->cpu_ctx); + cm_el1_sysregs_context_restore(SECURE); + cm_set_next_eret_context(SECURE); + + rc = tspd_enter_sp(&tsp_ctx->c_rt_ctx); +#if DEBUG + tsp_ctx->c_rt_ctx = 0; +#endif + + return rc; +} + + +/******************************************************************************* + * This function takes an SP context pointer and: + * 1. Saves the S-EL1 system register context tp tsp_ctx->cpu_ctx. + * 2. Restores the current C runtime state (callee saved registers) from the + * stack frame using the reference to this state saved in tspd_enter_sp(). + * 3. It does not need to save any general purpose or EL3 system register state + * as the generic smc entry routine should have saved those. + ******************************************************************************/ +void tspd_synchronous_sp_exit(tsp_context_t *tsp_ctx, uint64_t ret) +{ + /* Save the Secure EL1 system register context */ + assert(cm_get_context(read_mpidr(), SECURE) == &tsp_ctx->cpu_ctx); + cm_el1_sysregs_context_save(SECURE); + + assert(tsp_ctx->c_rt_ctx != 0); + tspd_exit_sp(tsp_ctx->c_rt_ctx, ret); + + /* Should never reach here */ + assert(0); +} diff --git a/services/spd/tspd/tspd_helpers.S b/services/spd/tspd/tspd_helpers.S new file mode 100644 index 0000000..dd3b07b --- /dev/null +++ b/services/spd/tspd/tspd_helpers.S @@ -0,0 +1,101 @@ +/* + * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <asm_macros.S> +#include "tspd_private.h" + + .global tspd_enter_sp + /* --------------------------------------------- + * This function is called with SP_EL0 as stack. + * Here we stash our EL3 callee-saved registers + * on to the stack as a part of saving the C + * runtime and enter the secure payload. + * 'x0' contains a pointer to the memory where + * the address of the C runtime context is to be + * saved. + * --------------------------------------------- + */ +func tspd_enter_sp + /* Make space for the registers that we're going to save */ + mov x3, sp + str x3, [x0, #0] + sub sp, sp, #TSPD_C_RT_CTX_SIZE + + /* Save callee-saved registers on to the stack */ + stp x19, x20, [sp, #TSPD_C_RT_CTX_X19] + stp x21, x22, [sp, #TSPD_C_RT_CTX_X21] + stp x23, x24, [sp, #TSPD_C_RT_CTX_X23] + stp x25, x26, [sp, #TSPD_C_RT_CTX_X25] + stp x27, x28, [sp, #TSPD_C_RT_CTX_X27] + stp x29, x30, [sp, #TSPD_C_RT_CTX_X29] + + /* --------------------------------------------- + * Everything is setup now. el3_exit() will + * use the secure context to restore to the + * general purpose and EL3 system registers to + * ERET into the secure payload. + * --------------------------------------------- + */ + b el3_exit + + /* --------------------------------------------- + * This function is called 'x0' pointing to a C + * runtime context saved in tspd_enter_sp(). It + * restores the saved registers and jumps to + * that runtime with 'x0' as the new sp. This + * destroys the C runtime context that had been + * built on the stack below the saved context by + * the caller. Later the second parameter 'x1' + * is passed as return value to the caller + * --------------------------------------------- + */ + .global tspd_exit_sp +func tspd_exit_sp + /* Restore the previous stack */ + mov sp, x0 + + /* Restore callee-saved registers on to the stack */ + ldp x19, x20, [x0, #(TSPD_C_RT_CTX_X19 - TSPD_C_RT_CTX_SIZE)] + ldp x21, x22, [x0, #(TSPD_C_RT_CTX_X21 - TSPD_C_RT_CTX_SIZE)] + ldp x23, x24, [x0, #(TSPD_C_RT_CTX_X23 - TSPD_C_RT_CTX_SIZE)] + ldp x25, x26, [x0, #(TSPD_C_RT_CTX_X25 - TSPD_C_RT_CTX_SIZE)] + ldp x27, x28, [x0, #(TSPD_C_RT_CTX_X27 - TSPD_C_RT_CTX_SIZE)] + ldp x29, x30, [x0, #(TSPD_C_RT_CTX_X29 - TSPD_C_RT_CTX_SIZE)] + + /* --------------------------------------------- + * This should take us back to the instruction + * after the call to the last tspd_enter_sp(). + * Place the second parameter to x0 so that the + * caller will see it as a return value from the + * original entry call + * --------------------------------------------- + */ + mov x0, x1 + ret diff --git a/services/spd/tspd/tspd_main.c b/services/spd/tspd/tspd_main.c new file mode 100644 index 0000000..1a6913a --- /dev/null +++ b/services/spd/tspd/tspd_main.c @@ -0,0 +1,593 @@ +/* + * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + + +/******************************************************************************* + * This is the Secure Payload Dispatcher (SPD). The dispatcher is meant to be a + * plug-in component to the Secure Monitor, registered as a runtime service. The + * SPD is expected to be a functional extension of the Secure Payload (SP) that + * executes in Secure EL1. The Secure Monitor will delegate all SMCs targeting + * the Trusted OS/Applications range to the dispatcher. The SPD will either + * handle the request locally or delegate it to the Secure Payload. It is also + * responsible for initialising and maintaining communication with the SP. + ******************************************************************************/ +#include <arch_helpers.h> +#include <assert.h> +#include <bl_common.h> +#include <bl31.h> +#include <context_mgmt.h> +#include <debug.h> +#include <errno.h> +#include <platform.h> +#include <runtime_svc.h> +#include <stddef.h> +#include <tsp.h> +#include <uuid.h> +#include "tspd_private.h" + +/******************************************************************************* + * Address of the entrypoint vector table in the Secure Payload. It is + * initialised once on the primary core after a cold boot. + ******************************************************************************/ +tsp_vectors_t *tsp_vectors; + +/******************************************************************************* + * Array to keep track of per-cpu Secure Payload state + ******************************************************************************/ +tsp_context_t tspd_sp_context[TSPD_CORE_COUNT]; + + +/* TSP UID */ +DEFINE_SVC_UUID(tsp_uuid, + 0x5b3056a0, 0x3291, 0x427b, 0x98, 0x11, + 0x71, 0x68, 0xca, 0x50, 0xf3, 0xfa); + +int32_t tspd_init(void); + +/******************************************************************************* + * This function is the handler registered for S-EL1 interrupts by the TSPD. It + * validates the interrupt and upon success arranges entry into the TSP at + * 'tsp_fiq_entry()' for handling the interrupt. + ******************************************************************************/ +static uint64_t tspd_sel1_interrupt_handler(uint32_t id, + uint32_t flags, + void *handle, + void *cookie) +{ + uint32_t linear_id; + uint64_t mpidr; + tsp_context_t *tsp_ctx; + + /* Check the security state when the exception was generated */ + assert(get_interrupt_src_ss(flags) == NON_SECURE); + +#if IMF_READ_INTERRUPT_ID + /* Check the security status of the interrupt */ + assert(plat_ic_get_interrupt_type(id) == INTR_TYPE_S_EL1); +#endif + + /* Sanity check the pointer to this cpu's context */ + mpidr = read_mpidr(); + assert(handle == cm_get_context(mpidr, NON_SECURE)); + + /* Save the non-secure context before entering the TSP */ + cm_el1_sysregs_context_save(NON_SECURE); + + /* Get a reference to this cpu's TSP context */ + linear_id = platform_get_core_pos(mpidr); + tsp_ctx = &tspd_sp_context[linear_id]; + assert(&tsp_ctx->cpu_ctx == cm_get_context(mpidr, SECURE)); + + /* + * Determine if the TSP was previously preempted. Its last known + * context has to be preserved in this case. + * The TSP should return control to the TSPD after handling this + * FIQ. Preserve essential EL3 context to allow entry into the + * TSP at the FIQ entry point using the 'cpu_context' structure. + * There is no need to save the secure system register context + * since the TSP is supposed to preserve it during S-EL1 interrupt + * handling. + */ + if (get_std_smc_active_flag(tsp_ctx->state)) { + tsp_ctx->saved_spsr_el3 = SMC_GET_EL3(&tsp_ctx->cpu_ctx, + CTX_SPSR_EL3); + tsp_ctx->saved_elr_el3 = SMC_GET_EL3(&tsp_ctx->cpu_ctx, + CTX_ELR_EL3); + } + + SMC_SET_EL3(&tsp_ctx->cpu_ctx, + CTX_SPSR_EL3, + SPSR_64(MODE_EL1, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS)); + SMC_SET_EL3(&tsp_ctx->cpu_ctx, + CTX_ELR_EL3, + (uint64_t) &tsp_vectors->fiq_entry); + cm_el1_sysregs_context_restore(SECURE); + cm_set_next_eret_context(SECURE); + + /* + * Tell the TSP that it has to handle an FIQ synchronously. Also the + * instruction in normal world where the interrupt was generated is + * passed for debugging purposes. It is safe to retrieve this address + * from ELR_EL3 as the secure context will not take effect until + * el3_exit(). + */ + SMC_RET2(&tsp_ctx->cpu_ctx, TSP_HANDLE_FIQ_AND_RETURN, read_elr_el3()); +} + +/******************************************************************************* + * Secure Payload Dispatcher setup. The SPD finds out the SP entrypoint and type + * (aarch32/aarch64) if not already known and initialises the context for entry + * into the SP for its initialisation. + ******************************************************************************/ +int32_t tspd_setup(void) +{ + entry_point_info_t *image_info; + int32_t rc; + uint64_t mpidr = read_mpidr(); + uint32_t linear_id; + + linear_id = platform_get_core_pos(mpidr); + + /* + * Get information about the Secure Payload (BL32) image. Its + * absence is a critical failure. TODO: Add support to + * conditionally include the SPD service + */ + image_info = bl31_plat_get_next_image_ep_info(SECURE); + assert(image_info); + + /* + * If there's no valid entry point for SP, we return a non-zero value + * signalling failure initializing the service. We bail out without + * registering any handlers + */ + if (!image_info->pc) + return 1; + + /* + * We could inspect the SP image and determine it's execution + * state i.e whether AArch32 or AArch64. Assuming it's AArch64 + * for the time being. + */ + rc = tspd_init_secure_context(image_info->pc, + TSP_AARCH64, + mpidr, + &tspd_sp_context[linear_id]); + assert(rc == 0); + + /* + * All TSPD initialization done. Now register our init function with + * BL31 for deferred invocation + */ + bl31_register_bl32_init(&tspd_init); + + return rc; +} + +/******************************************************************************* + * This function passes control to the Secure Payload image (BL32) for the first + * time on the primary cpu after a cold boot. It assumes that a valid secure + * context has already been created by tspd_setup() which can be directly used. + * It also assumes that a valid non-secure context has been initialised by PSCI + * so it does not need to save and restore any non-secure state. This function + * performs a synchronous entry into the Secure payload. The SP passes control + * back to this routine through a SMC. + ******************************************************************************/ +int32_t tspd_init(void) +{ + uint64_t mpidr = read_mpidr(); + uint32_t linear_id = platform_get_core_pos(mpidr), flags; + uint64_t rc; + tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id]; + + /* + * Arrange for an entry into the test secure payload. We expect an array + * of vectors in return + */ + rc = tspd_synchronous_sp_entry(tsp_ctx); + assert(rc != 0); + if (rc) { + set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_ON); + + /* + * TSP has been successfully initialized. Register power + * managemnt hooks with PSCI + */ + psci_register_spd_pm_hook(&tspd_pm); + } + + /* + * Register an interrupt handler for S-EL1 interrupts when generated + * during code executing in the non-secure state. + */ + flags = 0; + set_interrupt_rm_flag(flags, NON_SECURE); + rc = register_interrupt_type_handler(INTR_TYPE_S_EL1, + tspd_sel1_interrupt_handler, + flags); + if (rc) + panic(); + + return rc; +} + + +/******************************************************************************* + * This function is responsible for handling all SMCs in the Trusted OS/App + * range from the non-secure state as defined in the SMC Calling Convention + * Document. It is also responsible for communicating with the Secure payload + * to delegate work and return results back to the non-secure state. Lastly it + * will also return any information that the secure payload needs to do the + * work assigned to it. + ******************************************************************************/ +uint64_t tspd_smc_handler(uint32_t smc_fid, + uint64_t x1, + uint64_t x2, + uint64_t x3, + uint64_t x4, + void *cookie, + void *handle, + uint64_t flags) +{ + cpu_context_t *ns_cpu_context; + unsigned long mpidr = read_mpidr(); + uint32_t linear_id = platform_get_core_pos(mpidr), ns; + tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id]; + + /* Determine which security state this SMC originated from */ + ns = is_caller_non_secure(flags); + + switch (smc_fid) { + + /* + * This function ID is used by TSP to indicate that it was + * preempted by a normal world IRQ. + * + */ + case TSP_PREEMPTED: + if (ns) + SMC_RET1(handle, SMC_UNK); + + assert(handle == cm_get_context(mpidr, SECURE)); + cm_el1_sysregs_context_save(SECURE); + /* Get a reference to the non-secure context */ + ns_cpu_context = cm_get_context(mpidr, NON_SECURE); + assert(ns_cpu_context); + + /* + * Restore non-secure state. There is no need to save the + * secure system register context since the TSP was supposed + * to preserve it during S-EL1 interrupt handling. + */ + cm_el1_sysregs_context_restore(NON_SECURE); + cm_set_next_eret_context(NON_SECURE); + + SMC_RET1(ns_cpu_context, SMC_PREEMPTED); + + /* + * This function ID is used only by the TSP to indicate that it has + * finished handling a S-EL1 FIQ interrupt. Execution should resume + * in the normal world. + */ + case TSP_HANDLED_S_EL1_FIQ: + if (ns) + SMC_RET1(handle, SMC_UNK); + + assert(handle == cm_get_context(mpidr, SECURE)); + + /* + * Restore the relevant EL3 state which saved to service + * this SMC. + */ + if (get_std_smc_active_flag(tsp_ctx->state)) { + SMC_SET_EL3(&tsp_ctx->cpu_ctx, + CTX_SPSR_EL3, + tsp_ctx->saved_spsr_el3); + SMC_SET_EL3(&tsp_ctx->cpu_ctx, + CTX_ELR_EL3, + tsp_ctx->saved_elr_el3); + } + + /* Get a reference to the non-secure context */ + ns_cpu_context = cm_get_context(mpidr, NON_SECURE); + assert(ns_cpu_context); + + /* + * Restore non-secure state. There is no need to save the + * secure system register context since the TSP was supposed + * to preserve it during S-EL1 interrupt handling. + */ + cm_el1_sysregs_context_restore(NON_SECURE); + cm_set_next_eret_context(NON_SECURE); + + SMC_RET0((uint64_t) ns_cpu_context); + + + /* + * This function ID is used only by the TSP to indicate that it was + * interrupted due to a EL3 FIQ interrupt. Execution should resume + * in the normal world. + */ + case TSP_EL3_FIQ: + if (ns) + SMC_RET1(handle, SMC_UNK); + + assert(handle == cm_get_context(mpidr, SECURE)); + + /* Assert that standard SMC execution has been preempted */ + assert(get_std_smc_active_flag(tsp_ctx->state)); + + /* Save the secure system register state */ + cm_el1_sysregs_context_save(SECURE); + + /* Get a reference to the non-secure context */ + ns_cpu_context = cm_get_context(mpidr, NON_SECURE); + assert(ns_cpu_context); + + /* Restore non-secure state */ + cm_el1_sysregs_context_restore(NON_SECURE); + cm_set_next_eret_context(NON_SECURE); + + SMC_RET1(ns_cpu_context, TSP_EL3_FIQ); + + + /* + * This function ID is used only by the SP to indicate it has + * finished initialising itself after a cold boot + */ + case TSP_ENTRY_DONE: + if (ns) + SMC_RET1(handle, SMC_UNK); + + /* + * Stash the SP entry points information. This is done + * only once on the primary cpu + */ + assert(tsp_vectors == NULL); + tsp_vectors = (tsp_vectors_t *) x1; + + /* + * SP reports completion. The SPD must have initiated + * the original request through a synchronous entry + * into the SP. Jump back to the original C runtime + * context. + */ + tspd_synchronous_sp_exit(tsp_ctx, x1); + + /* + * These function IDs is used only by the SP to indicate it has + * finished: + * 1. turning itself on in response to an earlier psci + * cpu_on request + * 2. resuming itself after an earlier psci cpu_suspend + * request. + */ + case TSP_ON_DONE: + case TSP_RESUME_DONE: + + /* + * These function IDs is used only by the SP to indicate it has + * finished: + * 1. suspending itself after an earlier psci cpu_suspend + * request. + * 2. turning itself off in response to an earlier psci + * cpu_off request. + */ + case TSP_OFF_DONE: + case TSP_SUSPEND_DONE: + if (ns) + SMC_RET1(handle, SMC_UNK); + + /* + * SP reports completion. The SPD must have initiated the + * original request through a synchronous entry into the SP. + * Jump back to the original C runtime context, and pass x1 as + * return value to the caller + */ + tspd_synchronous_sp_exit(tsp_ctx, x1); + + /* + * Request from non-secure client to perform an + * arithmetic operation or response from secure + * payload to an earlier request. + */ + case TSP_FAST_FID(TSP_ADD): + case TSP_FAST_FID(TSP_SUB): + case TSP_FAST_FID(TSP_MUL): + case TSP_FAST_FID(TSP_DIV): + + case TSP_STD_FID(TSP_ADD): + case TSP_STD_FID(TSP_SUB): + case TSP_STD_FID(TSP_MUL): + case TSP_STD_FID(TSP_DIV): + if (ns) { + /* + * This is a fresh request from the non-secure client. + * The parameters are in x1 and x2. Figure out which + * registers need to be preserved, save the non-secure + * state and send the request to the secure payload. + */ + assert(handle == cm_get_context(mpidr, NON_SECURE)); + + /* Check if we are already preempted */ + if (get_std_smc_active_flag(tsp_ctx->state)) + SMC_RET1(handle, SMC_UNK); + + cm_el1_sysregs_context_save(NON_SECURE); + + /* Save x1 and x2 for use by TSP_GET_ARGS call below */ + store_tsp_args(tsp_ctx, x1, x2); + + /* + * We are done stashing the non-secure context. Ask the + * secure payload to do the work now. + */ + + /* + * Verify if there is a valid context to use, copy the + * operation type and parameters to the secure context + * and jump to the fast smc entry point in the secure + * payload. Entry into S-EL1 will take place upon exit + * from this function. + */ + assert(&tsp_ctx->cpu_ctx == cm_get_context(mpidr, SECURE)); + + /* Set appropriate entry for SMC. + * We expect the TSP to manage the PSTATE.I and PSTATE.F + * flags as appropriate. + */ + if (GET_SMC_TYPE(smc_fid) == SMC_TYPE_FAST) { + cm_set_elr_el3(SECURE, (uint64_t) + &tsp_vectors->fast_smc_entry); + } else { + set_std_smc_active_flag(tsp_ctx->state); + cm_set_elr_el3(SECURE, (uint64_t) + &tsp_vectors->std_smc_entry); + } + + cm_el1_sysregs_context_restore(SECURE); + cm_set_next_eret_context(SECURE); + SMC_RET3(&tsp_ctx->cpu_ctx, smc_fid, x1, x2); + } else { + /* + * This is the result from the secure client of an + * earlier request. The results are in x1-x3. Copy it + * into the non-secure context, save the secure state + * and return to the non-secure state. + */ + assert(handle == cm_get_context(mpidr, SECURE)); + cm_el1_sysregs_context_save(SECURE); + + /* Get a reference to the non-secure context */ + ns_cpu_context = cm_get_context(mpidr, NON_SECURE); + assert(ns_cpu_context); + + /* Restore non-secure state */ + cm_el1_sysregs_context_restore(NON_SECURE); + cm_set_next_eret_context(NON_SECURE); + if (GET_SMC_TYPE(smc_fid) == SMC_TYPE_STD) + clr_std_smc_active_flag(tsp_ctx->state); + SMC_RET3(ns_cpu_context, x1, x2, x3); + } + + break; + + /* + * Request from non secure world to resume the preempted + * Standard SMC call. + */ + case TSP_FID_RESUME: + /* RESUME should be invoked only by normal world */ + if (!ns) { + assert(0); + break; + } + + /* + * This is a resume request from the non-secure client. + * save the non-secure state and send the request to + * the secure payload. + */ + assert(handle == cm_get_context(mpidr, NON_SECURE)); + + /* Check if we are already preempted before resume */ + if (!get_std_smc_active_flag(tsp_ctx->state)) + SMC_RET1(handle, SMC_UNK); + + cm_el1_sysregs_context_save(NON_SECURE); + + /* + * We are done stashing the non-secure context. Ask the + * secure payload to do the work now. + */ + + /* We just need to return to the preempted point in + * TSP and the execution will resume as normal. + */ + cm_el1_sysregs_context_restore(SECURE); + cm_set_next_eret_context(SECURE); + SMC_RET0(&tsp_ctx->cpu_ctx); + + /* + * This is a request from the secure payload for more arguments + * for an ongoing arithmetic operation requested by the + * non-secure world. Simply return the arguments from the non- + * secure client in the original call. + */ + case TSP_GET_ARGS: + if (ns) + SMC_RET1(handle, SMC_UNK); + + get_tsp_args(tsp_ctx, x1, x2); + SMC_RET2(handle, x1, x2); + + case TOS_CALL_COUNT: + /* + * Return the number of service function IDs implemented to + * provide service to non-secure + */ + SMC_RET1(handle, TSP_NUM_FID); + + case TOS_UID: + /* Return TSP UID to the caller */ + SMC_UUID_RET(handle, tsp_uuid); + + case TOS_CALL_VERSION: + /* Return the version of current implementation */ + SMC_RET2(handle, TSP_VERSION_MAJOR, TSP_VERSION_MINOR); + + default: + break; + } + + SMC_RET1(handle, SMC_UNK); +} + +/* Define a SPD runtime service descriptor for fast SMC calls */ +DECLARE_RT_SVC( + tspd_fast, + + OEN_TOS_START, + OEN_TOS_END, + SMC_TYPE_FAST, + tspd_setup, + tspd_smc_handler +); + +/* Define a SPD runtime service descriptor for standard SMC calls */ +DECLARE_RT_SVC( + tspd_std, + + OEN_TOS_START, + OEN_TOS_END, + SMC_TYPE_STD, + NULL, + tspd_smc_handler +); diff --git a/services/spd/tspd/tspd_pm.c b/services/spd/tspd/tspd_pm.c new file mode 100644 index 0000000..ec4989d --- /dev/null +++ b/services/spd/tspd/tspd_pm.c @@ -0,0 +1,205 @@ +/* + * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <arch_helpers.h> +#include <assert.h> +#include <bl_common.h> +#include <context_mgmt.h> +#include <debug.h> +#include <platform.h> +#include <tsp.h> +#include "tspd_private.h" + +/******************************************************************************* + * The target cpu is being turned on. Allow the TSPD/TSP to perform any actions + * needed. Nothing at the moment. + ******************************************************************************/ +static void tspd_cpu_on_handler(uint64_t target_cpu) +{ +} + +/******************************************************************************* + * This cpu is being turned off. Allow the TSPD/TSP to perform any actions + * needed + ******************************************************************************/ +static int32_t tspd_cpu_off_handler(uint64_t cookie) +{ + int32_t rc = 0; + uint64_t mpidr = read_mpidr(); + uint32_t linear_id = platform_get_core_pos(mpidr); + tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id]; + + assert(tsp_vectors); + assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON); + + /* Program the entry point and enter the TSP */ + cm_set_elr_el3(SECURE, (uint64_t) &tsp_vectors->cpu_off_entry); + rc = tspd_synchronous_sp_entry(tsp_ctx); + + /* + * Read the response from the TSP. A non-zero return means that + * something went wrong while communicating with the TSP. + */ + if (rc != 0) + panic(); + + /* + * Reset TSP's context for a fresh start when this cpu is turned on + * subsequently. + */ + set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_OFF); + + return 0; +} + +/******************************************************************************* + * This cpu is being suspended. S-EL1 state must have been saved in the + * resident cpu (mpidr format) if it is a UP/UP migratable TSP. + ******************************************************************************/ +static void tspd_cpu_suspend_handler(uint64_t power_state) +{ + int32_t rc = 0; + uint64_t mpidr = read_mpidr(); + uint32_t linear_id = platform_get_core_pos(mpidr); + tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id]; + + assert(tsp_vectors); + assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_ON); + + /* Program the entry point, power_state parameter and enter the TSP */ + write_ctx_reg(get_gpregs_ctx(&tsp_ctx->cpu_ctx), + CTX_GPREG_X0, + power_state); + cm_set_elr_el3(SECURE, (uint64_t) &tsp_vectors->cpu_suspend_entry); + rc = tspd_synchronous_sp_entry(tsp_ctx); + + /* + * Read the response from the TSP. A non-zero return means that + * something went wrong while communicating with the TSP. + */ + if (rc != 0) + panic(); + + /* Update its context to reflect the state the TSP is in */ + set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_SUSPEND); +} + +/******************************************************************************* + * This cpu has been turned on. Enter the TSP to initialise S-EL1 and other bits + * before passing control back to the Secure Monitor. Entry in S-El1 is done + * after initialising minimal architectural state that guarantees safe + * execution. + ******************************************************************************/ +static void tspd_cpu_on_finish_handler(uint64_t cookie) +{ + int32_t rc = 0; + uint64_t mpidr = read_mpidr(); + uint32_t linear_id = platform_get_core_pos(mpidr); + tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id]; + + assert(tsp_vectors); + assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_OFF); + + /* Initialise this cpu's secure context */ + tspd_init_secure_context((uint64_t) &tsp_vectors->cpu_on_entry, + TSP_AARCH64, + mpidr, + tsp_ctx); + + /* Enter the TSP */ + rc = tspd_synchronous_sp_entry(tsp_ctx); + + /* + * Read the response from the TSP. A non-zero return means that + * something went wrong while communicating with the SP. + */ + if (rc != 0) + panic(); + + /* Update its context to reflect the state the SP is in */ + set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_ON); +} + +/******************************************************************************* + * This cpu has resumed from suspend. The SPD saved the TSP context when it + * completed the preceding suspend call. Use that context to program an entry + * into the TSP to allow it to do any remaining book keeping + ******************************************************************************/ +static void tspd_cpu_suspend_finish_handler(uint64_t suspend_level) +{ + int32_t rc = 0; + uint64_t mpidr = read_mpidr(); + uint32_t linear_id = platform_get_core_pos(mpidr); + tsp_context_t *tsp_ctx = &tspd_sp_context[linear_id]; + + assert(tsp_vectors); + assert(get_tsp_pstate(tsp_ctx->state) == TSP_PSTATE_SUSPEND); + + /* Program the entry point, suspend_level and enter the SP */ + write_ctx_reg(get_gpregs_ctx(&tsp_ctx->cpu_ctx), + CTX_GPREG_X0, + suspend_level); + cm_set_elr_el3(SECURE, (uint64_t) &tsp_vectors->cpu_resume_entry); + rc = tspd_synchronous_sp_entry(tsp_ctx); + + /* + * Read the response from the TSP. A non-zero return means that + * something went wrong while communicating with the TSP. + */ + if (rc != 0) + panic(); + + /* Update its context to reflect the state the SP is in */ + set_tsp_pstate(tsp_ctx->state, TSP_PSTATE_ON); +} + +/******************************************************************************* + * Return the type of TSP the TSPD is dealing with. Report the current resident + * cpu (mpidr format) if it is a UP/UP migratable TSP. + ******************************************************************************/ +static int32_t tspd_cpu_migrate_info(uint64_t *resident_cpu) +{ + return TSP_MIGRATE_INFO; +} + +/******************************************************************************* + * Structure populated by the TSP Dispatcher to be given a chance to perform any + * TSP bookkeeping before PSCI executes a power mgmt. operation. + ******************************************************************************/ +const spd_pm_ops_t tspd_pm = { + tspd_cpu_on_handler, + tspd_cpu_off_handler, + tspd_cpu_suspend_handler, + tspd_cpu_on_finish_handler, + tspd_cpu_suspend_finish_handler, + NULL, + tspd_cpu_migrate_info +}; + diff --git a/services/spd/tspd/tspd_private.h b/services/spd/tspd/tspd_private.h new file mode 100644 index 0000000..5d7bf4b --- /dev/null +++ b/services/spd/tspd/tspd_private.h @@ -0,0 +1,203 @@ +/* + * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef __TSPD_PRIVATE_H__ +#define __TSPD_PRIVATE_H__ + +#include <arch.h> +#include <context.h> +#include <interrupt_mgmt.h> +#include <platform_def.h> +#include <psci.h> + +/******************************************************************************* + * Secure Payload PM state information e.g. SP is suspended, uninitialised etc + * and macros to access the state information in the per-cpu 'state' flags + ******************************************************************************/ +#define TSP_PSTATE_OFF 0 +#define TSP_PSTATE_ON 1 +#define TSP_PSTATE_SUSPEND 2 +#define TSP_PSTATE_SHIFT 0 +#define TSP_PSTATE_MASK 0x3 +#define get_tsp_pstate(state) ((state >> TSP_PSTATE_SHIFT) & TSP_PSTATE_MASK) +#define clr_tsp_pstate(state) (state &= ~(TSP_PSTATE_MASK \ + << TSP_PSTATE_SHIFT)) +#define set_tsp_pstate(st, pst) do { \ + clr_tsp_pstate(st); \ + st |= (pst & TSP_PSTATE_MASK) << \ + TSP_PSTATE_SHIFT; \ + } while (0); + + +/* + * This flag is used by the TSPD to determine if the TSP is servicing a standard + * SMC request prior to programming the next entry into the TSP e.g. if TSP + * execution is preempted by a non-secure interrupt and handed control to the + * normal world. If another request which is distinct from what the TSP was + * previously doing arrives, then this flag will be help the TSPD to either + * reject the new request or service it while ensuring that the previous context + * is not corrupted. + */ +#define STD_SMC_ACTIVE_FLAG_SHIFT 2 +#define STD_SMC_ACTIVE_FLAG_MASK 1 +#define get_std_smc_active_flag(state) ((state >> STD_SMC_ACTIVE_FLAG_SHIFT) \ + & STD_SMC_ACTIVE_FLAG_MASK) +#define set_std_smc_active_flag(state) (state |= \ + 1 << STD_SMC_ACTIVE_FLAG_SHIFT) +#define clr_std_smc_active_flag(state) (state &= \ + ~(STD_SMC_ACTIVE_FLAG_MASK \ + << STD_SMC_ACTIVE_FLAG_SHIFT)) + +/******************************************************************************* + * Secure Payload execution state information i.e. aarch32 or aarch64 + ******************************************************************************/ +#define TSP_AARCH32 MODE_RW_32 +#define TSP_AARCH64 MODE_RW_64 + +/******************************************************************************* + * The SPD should know the type of Secure Payload. + ******************************************************************************/ +#define TSP_TYPE_UP PSCI_TOS_NOT_UP_MIG_CAP +#define TSP_TYPE_UPM PSCI_TOS_UP_MIG_CAP +#define TSP_TYPE_MP PSCI_TOS_NOT_PRESENT_MP + +/******************************************************************************* + * Secure Payload migrate type information as known to the SPD. We assume that + * the SPD is dealing with an MP Secure Payload. + ******************************************************************************/ +#define TSP_MIGRATE_INFO TSP_TYPE_MP + +/******************************************************************************* + * Number of cpus that the present on this platform. TODO: Rely on a topology + * tree to determine this in the future to avoid assumptions about mpidr + * allocation + ******************************************************************************/ +#define TSPD_CORE_COUNT PLATFORM_CORE_COUNT + +/******************************************************************************* + * Constants that allow assembler code to preserve callee-saved registers of the + * C runtime context while performing a security state switch. + ******************************************************************************/ +#define TSPD_C_RT_CTX_X19 0x0 +#define TSPD_C_RT_CTX_X20 0x8 +#define TSPD_C_RT_CTX_X21 0x10 +#define TSPD_C_RT_CTX_X22 0x18 +#define TSPD_C_RT_CTX_X23 0x20 +#define TSPD_C_RT_CTX_X24 0x28 +#define TSPD_C_RT_CTX_X25 0x30 +#define TSPD_C_RT_CTX_X26 0x38 +#define TSPD_C_RT_CTX_X27 0x40 +#define TSPD_C_RT_CTX_X28 0x48 +#define TSPD_C_RT_CTX_X29 0x50 +#define TSPD_C_RT_CTX_X30 0x58 +#define TSPD_C_RT_CTX_SIZE 0x60 +#define TSPD_C_RT_CTX_ENTRIES (TSPD_C_RT_CTX_SIZE >> DWORD_SHIFT) + +#ifndef __ASSEMBLY__ + +#include <cassert.h> +#include <stdint.h> + +/* + * The number of arguments to save during a SMC call for TSP. + * Currently only x1 and x2 are used by TSP. + */ +#define TSP_NUM_ARGS 0x2 + +/* AArch64 callee saved general purpose register context structure. */ +DEFINE_REG_STRUCT(c_rt_regs, TSPD_C_RT_CTX_ENTRIES); + +/* + * Compile time assertion to ensure that both the compiler and linker + * have the same double word aligned view of the size of the C runtime + * register context. + */ +CASSERT(TSPD_C_RT_CTX_SIZE == sizeof(c_rt_regs_t), \ + assert_spd_c_rt_regs_size_mismatch); + +/******************************************************************************* + * Structure which helps the SPD to maintain the per-cpu state of the SP. + * 'saved_spsr_el3' - temporary copy to allow FIQ handling when the TSP has been + * preempted. + * 'saved_elr_el3' - temporary copy to allow FIQ handling when the TSP has been + * preempted. + * 'state' - collection of flags to track SP state e.g. on/off + * 'mpidr' - mpidr to associate a context with a cpu + * 'c_rt_ctx' - stack address to restore C runtime context from after + * returning from a synchronous entry into the SP. + * 'cpu_ctx' - space to maintain SP architectural state + * 'saved_tsp_args' - space to store arguments for TSP arithmetic operations + * which will queried using the TSP_GET_ARGS SMC by TSP. + ******************************************************************************/ +typedef struct tsp_context { + uint64_t saved_elr_el3; + uint32_t saved_spsr_el3; + uint32_t state; + uint64_t mpidr; + uint64_t c_rt_ctx; + cpu_context_t cpu_ctx; + uint64_t saved_tsp_args[TSP_NUM_ARGS]; +} tsp_context_t; + +/* Helper macros to store and retrieve tsp args from tsp_context */ +#define store_tsp_args(tsp_ctx, x1, x2) do {\ + tsp_ctx->saved_tsp_args[0] = x1;\ + tsp_ctx->saved_tsp_args[1] = x2;\ + } while (0) + +#define get_tsp_args(tsp_ctx, x1, x2) do {\ + x1 = tsp_ctx->saved_tsp_args[0];\ + x2 = tsp_ctx->saved_tsp_args[1];\ + } while (0) + +/* TSPD power management handlers */ +extern const spd_pm_ops_t tspd_pm; + +/******************************************************************************* + * Forward declarations + ******************************************************************************/ +struct tsp_vectors; + +/******************************************************************************* + * Function & Data prototypes + ******************************************************************************/ +uint64_t tspd_enter_sp(uint64_t *c_rt_ctx); +void __dead2 tspd_exit_sp(uint64_t c_rt_ctx, uint64_t ret); +uint64_t tspd_synchronous_sp_entry(tsp_context_t *tsp_ctx); +void __dead2 tspd_synchronous_sp_exit(tsp_context_t *tsp_ctx, uint64_t ret); +int32_t tspd_init_secure_context(uint64_t entrypoint, + uint32_t rw, + uint64_t mpidr, + tsp_context_t *tsp_ctx); +extern tsp_context_t tspd_sp_context[TSPD_CORE_COUNT]; +extern struct tsp_vectors *tsp_vectors; +#endif /*__ASSEMBLY__*/ + +#endif /* __TSPD_PRIVATE_H__ */ diff --git a/services/std_svc/psci/psci_afflvl_off.c b/services/std_svc/psci/psci_afflvl_off.c new file mode 100644 index 0000000..21a4d1a --- /dev/null +++ b/services/std_svc/psci/psci_afflvl_off.c @@ -0,0 +1,285 @@ +/* + * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <arch.h> +#include <arch_helpers.h> +#include <assert.h> +#include <string.h> +#include "psci_private.h" + +typedef int (*afflvl_off_handler_t)(unsigned long, aff_map_node_t *); + +/******************************************************************************* + * The next three functions implement a handler for each supported affinity + * level which is called when that affinity level is turned off. + ******************************************************************************/ +static int psci_afflvl0_off(unsigned long mpidr, aff_map_node_t *cpu_node) +{ + unsigned int index, plat_state; + int rc = PSCI_E_SUCCESS; + unsigned long sctlr; + + assert(cpu_node->level == MPIDR_AFFLVL0); + + /* State management: mark this cpu as turned off */ + psci_set_state(cpu_node, PSCI_STATE_OFF); + + /* + * Generic management: Get the index for clearing any lingering re-entry + * information and allow the secure world to switch itself off + */ + + /* + * Call the cpu off handler registered by the Secure Payload Dispatcher + * to let it do any bookeeping. Assume that the SPD always reports an + * E_DENIED error if SP refuse to power down + */ + if (psci_spd_pm && psci_spd_pm->svc_off) { + rc = psci_spd_pm->svc_off(0); + if (rc) + return rc; + } + + index = cpu_node->data; + memset(&psci_ns_entry_info[index], 0, sizeof(psci_ns_entry_info[index])); + + /* + * Arch. management. Perform the necessary steps to flush all + * cpu caches. + * + * TODO: This power down sequence varies across cpus so it needs to be + * abstracted out on the basis of the MIDR like in cpu_reset_handler(). + * Do the bare minimal for the time being. Fix this before porting to + * Cortex models. + */ + sctlr = read_sctlr_el3(); + sctlr &= ~SCTLR_C_BIT; + write_sctlr_el3(sctlr); + isb(); /* ensure MMU disable takes immediate effect */ + + /* + * CAUTION: This flush to the level of unification makes an assumption + * about the cache hierarchy at affinity level 0 (cpu) in the platform. + * Ideally the platform should tell psci which levels to flush to exit + * coherency. + */ + dcsw_op_louis(DCCISW); + + /* + * Plat. management: Perform platform specific actions to turn this + * cpu off e.g. exit cpu coherency, program the power controller etc. + */ + if (psci_plat_pm_ops->affinst_off) { + + /* Get the current physical state of this cpu */ + plat_state = psci_get_phys_state(cpu_node); + rc = psci_plat_pm_ops->affinst_off(mpidr, + cpu_node->level, + plat_state); + } + + return rc; +} + +static int psci_afflvl1_off(unsigned long mpidr, aff_map_node_t *cluster_node) +{ + int rc = PSCI_E_SUCCESS; + unsigned int plat_state; + + /* Sanity check the cluster level */ + assert(cluster_node->level == MPIDR_AFFLVL1); + + /* State management: Decrement the cluster reference count */ + psci_set_state(cluster_node, PSCI_STATE_OFF); + + /* + * Keep the physical state of this cluster handy to decide + * what action needs to be taken + */ + plat_state = psci_get_phys_state(cluster_node); + + /* + * Arch. Management. Flush all levels of caches to PoC if + * the cluster is to be shutdown + */ + if (plat_state == PSCI_STATE_OFF) + dcsw_op_all(DCCISW); + + /* + * Plat. Management. Allow the platform to do its cluster + * specific bookeeping e.g. turn off interconnect coherency, + * program the power controller etc. + */ + if (psci_plat_pm_ops->affinst_off) + rc = psci_plat_pm_ops->affinst_off(mpidr, + cluster_node->level, + plat_state); + + return rc; +} + +static int psci_afflvl2_off(unsigned long mpidr, aff_map_node_t *system_node) +{ + int rc = PSCI_E_SUCCESS; + unsigned int plat_state; + + /* Cannot go beyond this level */ + assert(system_node->level == MPIDR_AFFLVL2); + + /* State management: Decrement the system reference count */ + psci_set_state(system_node, PSCI_STATE_OFF); + + /* + * Keep the physical state of the system handy to decide what + * action needs to be taken + */ + plat_state = psci_get_phys_state(system_node); + + /* No arch. and generic bookeeping to do here currently */ + + /* + * Plat. Management : Allow the platform to do its bookeeping + * at this affinity level + */ + if (psci_plat_pm_ops->affinst_off) + rc = psci_plat_pm_ops->affinst_off(mpidr, + system_node->level, + plat_state); + return rc; +} + +static const afflvl_off_handler_t psci_afflvl_off_handlers[] = { + psci_afflvl0_off, + psci_afflvl1_off, + psci_afflvl2_off, +}; + +/******************************************************************************* + * This function takes an array of pointers to affinity instance nodes in the + * topology tree and calls the off handler for the corresponding affinity + * levels + ******************************************************************************/ +static int psci_call_off_handlers(mpidr_aff_map_nodes_t mpidr_nodes, + int start_afflvl, + int end_afflvl, + unsigned long mpidr) +{ + int rc = PSCI_E_INVALID_PARAMS, level; + aff_map_node_t *node; + + for (level = start_afflvl; level <= end_afflvl; level++) { + node = mpidr_nodes[level]; + if (node == NULL) + continue; + + /* + * TODO: In case of an error should there be a way + * of restoring what we might have torn down at + * lower affinity levels. + */ + rc = psci_afflvl_off_handlers[level](mpidr, node); + if (rc != PSCI_E_SUCCESS) + break; + } + + return rc; +} + +/******************************************************************************* + * Top level handler which is called when a cpu wants to power itself down. + * It's assumed that along with turning the cpu off, higher affinity levels will + * be turned off as far as possible. It traverses through all the affinity + * levels performing generic, architectural, platform setup and state management + * e.g. for a cluster that's to be powered off, it will call the platform + * specific code which will disable coherency at the interconnect level if the + * cpu is the last in the cluster. For a cpu it could mean programming the power + * the power controller etc. + * + * The state of all the relevant affinity levels is changed prior to calling the + * affinity level specific handlers as their actions would depend upon the state + * the affinity level is about to enter. + * + * The affinity level specific handlers are called in ascending order i.e. from + * the lowest to the highest affinity level implemented by the platform because + * to turn off affinity level X it is neccesary to turn off affinity level X - 1 + * first. + * + * CAUTION: This function is called with coherent stacks so that coherency can + * be turned off and caches can be flushed safely. + ******************************************************************************/ +int psci_afflvl_off(unsigned long mpidr, + int start_afflvl, + int end_afflvl) +{ + int rc = PSCI_E_SUCCESS; + mpidr_aff_map_nodes_t mpidr_nodes; + + mpidr &= MPIDR_AFFINITY_MASK;; + + /* + * Collect the pointers to the nodes in the topology tree for + * each affinity instance in the mpidr. If this function does + * not return successfully then either the mpidr or the affinity + * levels are incorrect. In either case, we cannot return back + * to the caller as it would not know what to do. + */ + rc = psci_get_aff_map_nodes(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); + assert (rc == PSCI_E_SUCCESS); + + /* + * This function acquires the lock corresponding to each affinity + * level so that by the time all locks are taken, the system topology + * is snapshot and state management can be done safely. + */ + psci_acquire_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); + + /* Perform generic, architecture and platform specific handling */ + rc = psci_call_off_handlers(mpidr_nodes, + start_afflvl, + end_afflvl, + mpidr); + + /* + * Release the locks corresponding to each affinity level in the + * reverse order to which they were acquired. + */ + psci_release_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); + + return rc; +} diff --git a/services/std_svc/psci/psci_afflvl_on.c b/services/std_svc/psci/psci_afflvl_on.c new file mode 100644 index 0000000..e3a1831 --- /dev/null +++ b/services/std_svc/psci/psci_afflvl_on.c @@ -0,0 +1,486 @@ +/* + * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <arch.h> +#include <arch_helpers.h> +#include <assert.h> +#include <bl_common.h> +#include <bl31.h> +#include <context_mgmt.h> +#include <platform.h> +#include <runtime_svc.h> +#include <stddef.h> +#include "psci_private.h" + +typedef int (*afflvl_on_handler_t)(unsigned long, + aff_map_node_t *, + unsigned long, + unsigned long); + +/******************************************************************************* + * This function checks whether a cpu which has been requested to be turned on + * is OFF to begin with. + ******************************************************************************/ +static int cpu_on_validate_state(aff_map_node_t *node) +{ + unsigned int psci_state; + + /* Get the raw psci state */ + psci_state = psci_get_state(node); + + if (psci_state == PSCI_STATE_ON || psci_state == PSCI_STATE_SUSPEND) + return PSCI_E_ALREADY_ON; + + if (psci_state == PSCI_STATE_ON_PENDING) + return PSCI_E_ON_PENDING; + + assert(psci_state == PSCI_STATE_OFF); + return PSCI_E_SUCCESS; +} + +/******************************************************************************* + * 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 int index, plat_state; + unsigned long psci_entrypoint; + 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. + */ + index = cpu_node->data; + rc = psci_set_ns_entry_info(index, ns_entrypoint, context_id); + 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; + + /* State management: Set this cpu's state as ON PENDING */ + psci_set_state(cpu_node, PSCI_STATE_ON_PENDING); + + /* + * Plat. management: Give the platform the current state + * of the target cpu to allow it to perform the necessary + * steps to power on. + */ + if (psci_plat_pm_ops->affinst_on) { + + /* Get the current physical state of this cpu */ + plat_state = psci_get_phys_state(cpu_node); + rc = psci_plat_pm_ops->affinst_on(target_cpu, + psci_entrypoint, + ns_entrypoint, + cpu_node->level, + plat_state); + } + + return rc; +} + +/******************************************************************************* + * Handler routine to turn a cluster on. It takes care or any generic, arch. + * or platform specific setup required. + * TODO: Split this code across separate handlers for each type of setup? + ******************************************************************************/ +static int psci_afflvl1_on(unsigned long target_cpu, + aff_map_node_t *cluster_node, + unsigned long ns_entrypoint, + unsigned long context_id) +{ + int rc = PSCI_E_SUCCESS; + unsigned int plat_state; + unsigned long psci_entrypoint; + + assert(cluster_node->level == MPIDR_AFFLVL1); + + /* + * There is no generic and arch. specific cluster + * management required + */ + + /* State management: Is not required while turning a cluster on */ + + /* + * Plat. management: Give the platform the current state + * of the target cpu to allow it to perform the necessary + * steps to power on. + */ + if (psci_plat_pm_ops->affinst_on) { + plat_state = psci_get_phys_state(cluster_node); + psci_entrypoint = (unsigned long) psci_aff_on_finish_entry; + rc = psci_plat_pm_ops->affinst_on(target_cpu, + psci_entrypoint, + ns_entrypoint, + cluster_node->level, + plat_state); + } + + return rc; +} + +/******************************************************************************* + * Handler routine to turn a cluster of clusters on. It takes care or any + * generic, arch. or platform specific setup required. + * TODO: Split this code across separate handlers for each type of setup? + ******************************************************************************/ +static int psci_afflvl2_on(unsigned long target_cpu, + aff_map_node_t *system_node, + unsigned long ns_entrypoint, + unsigned long context_id) +{ + int rc = PSCI_E_SUCCESS; + unsigned int plat_state; + unsigned long psci_entrypoint; + + /* Cannot go beyond affinity level 2 in this psci imp. */ + assert(system_node->level == MPIDR_AFFLVL2); + + /* + * There is no generic and arch. specific system management + * required + */ + + /* State management: Is not required while turning a system on */ + + /* + * Plat. management: Give the platform the current state + * of the target cpu to allow it to perform the necessary + * steps to power on. + */ + if (psci_plat_pm_ops->affinst_on) { + plat_state = psci_get_phys_state(system_node); + psci_entrypoint = (unsigned long) psci_aff_on_finish_entry; + rc = psci_plat_pm_ops->affinst_on(target_cpu, + psci_entrypoint, + ns_entrypoint, + system_node->level, + plat_state); + } + + return rc; +} + +/* Private data structure to make this handlers accessible through indexing */ +static const afflvl_on_handler_t psci_afflvl_on_handlers[] = { + psci_afflvl0_on, + psci_afflvl1_on, + psci_afflvl2_on, +}; + +/******************************************************************************* + * This function takes an array of pointers to affinity instance nodes in the + * topology tree and calls the on handler for the corresponding affinity + * levels + ******************************************************************************/ +static int psci_call_on_handlers(mpidr_aff_map_nodes_t target_cpu_nodes, + int start_afflvl, + int end_afflvl, + unsigned long target_cpu, + unsigned long entrypoint, + unsigned long context_id) +{ + int rc = PSCI_E_INVALID_PARAMS, level; + aff_map_node_t *node; + + for (level = end_afflvl; level >= start_afflvl; level--) { + node = target_cpu_nodes[level]; + if (node == NULL) + continue; + + /* + * TODO: In case of an error should there be a way + * of undoing what we might have setup at higher + * affinity levels. + */ + rc = psci_afflvl_on_handlers[level](target_cpu, + node, + entrypoint, + context_id); + if (rc != PSCI_E_SUCCESS) + break; + } + + return rc; +} + +/******************************************************************************* + * Generic handler which is called to physically power on a cpu identified by + * its mpidr. It traverses through all the affinity levels performing generic, + * architectural, platform setup and state management e.g. for a cpu that is + * to be powered on, it will ensure that enough information is stashed for it + * to resume execution in the non-secure security state. + * + * The state of all the relevant affinity levels is changed after calling the + * affinity level specific handlers as their actions would depend upon the state + * the affinity level is currently in. + * + * The affinity level specific handlers are called in descending order i.e. from + * the highest to the lowest affinity level implemented by the platform because + * to turn on affinity level X it is neccesary to turn on affinity level X + 1 + * first. + ******************************************************************************/ +int psci_afflvl_on(unsigned long target_cpu, + unsigned long entrypoint, + unsigned long context_id, + int start_afflvl, + int end_afflvl) +{ + int rc = PSCI_E_SUCCESS; + mpidr_aff_map_nodes_t target_cpu_nodes; + unsigned long mpidr = read_mpidr() & MPIDR_AFFINITY_MASK; + + /* + * Collect the pointers to the nodes in the topology tree for + * each affinity instance in the mpidr. If this function does + * not return successfully then either the mpidr or the affinity + * levels are incorrect. + */ + rc = psci_get_aff_map_nodes(target_cpu, + start_afflvl, + end_afflvl, + target_cpu_nodes); + if (rc != PSCI_E_SUCCESS) + return rc; + + + /* + * This function acquires the lock corresponding to each affinity + * level so that by the time all locks are taken, the system topology + * is snapshot and state management can be done safely. + */ + psci_acquire_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + target_cpu_nodes); + + /* Perform generic, architecture and platform specific handling. */ + rc = psci_call_on_handlers(target_cpu_nodes, + start_afflvl, + end_afflvl, + target_cpu, + entrypoint, + context_id); + + /* + * This loop releases the lock corresponding to each affinity level + * in the reverse order to which they were acquired. + */ + psci_release_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + target_cpu_nodes); + + return rc; +} + +/******************************************************************************* + * The following functions finish an earlier affinity power on request. They + * are called by the common finisher routine in psci_common.c. + ******************************************************************************/ +static unsigned int psci_afflvl0_on_finish(unsigned long mpidr, + aff_map_node_t *cpu_node) +{ + unsigned int index, plat_state, state, rc = PSCI_E_SUCCESS; + + assert(cpu_node->level == MPIDR_AFFLVL0); + + /* Ensure we have been explicitly woken up by another cpu */ + state = psci_get_state(cpu_node); + assert(state == PSCI_STATE_ON_PENDING); + + /* + * Plat. management: Perform the platform specific actions + * for this cpu e.g. enabling the gic or zeroing the mailbox + * register. The actual state of this cpu has already been + * changed. + */ + if (psci_plat_pm_ops->affinst_on_finish) { + + /* Get the physical state of this cpu */ + plat_state = get_phys_state(state); + rc = psci_plat_pm_ops->affinst_on_finish(mpidr, + cpu_node->level, + plat_state); + assert(rc == PSCI_E_SUCCESS); + } + + /* + * Arch. management: Turn on mmu & restore architectural state + */ + bl31_plat_enable_mmu(); + + /* + * All the platform specific actions for turning this cpu + * on have completed. Perform enough arch.initialization + * to run in the non-secure address space. + */ + bl31_arch_setup(); + + /* + * Use the more complex exception vectors to enable SPD + * initialisation. SP_EL3 should point to a 'cpu_context' + * structure. The calling cpu should have set the + * context already + */ + assert(cm_get_context(mpidr, NON_SECURE)); + cm_set_next_eret_context(NON_SECURE); + cm_init_pcpu_ptr_cache(); + write_vbar_el3((uint64_t) runtime_exceptions); + + /* + * Call the cpu on finish 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_finish) + psci_spd_pm->svc_on_finish(0); + + /* + * Generic management: Now we just need to retrieve the + * information that we had stashed away during the cpu_on + * call to set this cpu on its way. First get the index + * for restoring the re-entry info + */ + index = cpu_node->data; + psci_get_ns_entry_info(index); + + /* State management: mark this cpu as on */ + psci_set_state(cpu_node, PSCI_STATE_ON); + + /* Clean caches before re-entering normal world */ + dcsw_op_louis(DCCSW); + + return rc; +} + +static unsigned int psci_afflvl1_on_finish(unsigned long mpidr, + aff_map_node_t *cluster_node) +{ + unsigned int plat_state, rc = PSCI_E_SUCCESS; + + assert(cluster_node->level == MPIDR_AFFLVL1); + + /* + * Plat. management: Perform the platform specific actions + * as per the old state of the cluster e.g. enabling + * coherency at the interconnect depends upon the state with + * which this cluster was powered up. If anything goes wrong + * then assert as there is no way to recover from this + * situation. + */ + if (psci_plat_pm_ops->affinst_on_finish) { + + /* Get the physical state of this cluster */ + plat_state = psci_get_phys_state(cluster_node); + rc = psci_plat_pm_ops->affinst_on_finish(mpidr, + cluster_node->level, + plat_state); + assert(rc == PSCI_E_SUCCESS); + } + + /* State management: Increment the cluster reference count */ + psci_set_state(cluster_node, PSCI_STATE_ON); + + return rc; +} + + +static unsigned int psci_afflvl2_on_finish(unsigned long mpidr, + aff_map_node_t *system_node) +{ + unsigned int plat_state, rc = PSCI_E_SUCCESS; + + /* Cannot go beyond this affinity level */ + assert(system_node->level == MPIDR_AFFLVL2); + + /* + * Currently, there are no architectural actions to perform + * at the system level. + */ + + /* + * Plat. management: Perform the platform specific actions + * as per the old state of the cluster e.g. enabling + * coherency at the interconnect depends upon the state with + * which this cluster was powered up. If anything goes wrong + * then assert as there is no way to recover from this + * situation. + */ + if (psci_plat_pm_ops->affinst_on_finish) { + + /* Get the physical state of the system */ + plat_state = psci_get_phys_state(system_node); + rc = psci_plat_pm_ops->affinst_on_finish(mpidr, + system_node->level, + plat_state); + assert(rc == PSCI_E_SUCCESS); + } + + /* State management: Increment the system reference count */ + psci_set_state(system_node, PSCI_STATE_ON); + + return rc; +} + +const afflvl_power_on_finisher_t psci_afflvl_on_finishers[] = { + psci_afflvl0_on_finish, + psci_afflvl1_on_finish, + psci_afflvl2_on_finish, +}; + diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c new file mode 100644 index 0000000..377afdf --- /dev/null +++ b/services/std_svc/psci/psci_afflvl_suspend.c @@ -0,0 +1,608 @@ +/* + * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <assert.h> +#include <bl_common.h> +#include <arch.h> +#include <arch_helpers.h> +#include <context.h> +#include <context_mgmt.h> +#include <runtime_svc.h> +#include <stddef.h> +#include "psci_private.h" + +typedef int (*afflvl_suspend_handler_t)(unsigned long, + aff_map_node_t *, + unsigned long, + unsigned long, + unsigned int); + +/******************************************************************************* + * This function sets the power state of the current cpu while + * powering down during a cpu_suspend call + ******************************************************************************/ +void psci_set_suspend_power_state(aff_map_node_t *node, unsigned int power_state) +{ + /* + * Check that nobody else is calling this function on our behalf & + * this information is being set only in the cpu node + */ + assert(node->mpidr == (read_mpidr() & MPIDR_AFFINITY_MASK)); + assert(node->level == MPIDR_AFFLVL0); + + /* Save PSCI power state parameter for the core in suspend context */ + psci_suspend_context[node->data].power_state = power_state; + + /* + * Flush the suspend data to PoC since it will be accessed while + * returning back from suspend with the caches turned off + */ + flush_dcache_range( + (unsigned long)&psci_suspend_context[node->data], + sizeof(suspend_context_t)); +} + +/******************************************************************************* + * This function gets the affinity level till which a cpu is powered down + * during a cpu_suspend call. Returns PSCI_INVALID_DATA if the + * power state saved for the node is invalid + ******************************************************************************/ +int psci_get_suspend_afflvl(unsigned long mpidr) +{ + aff_map_node_t *node; + + node = psci_get_aff_map_node(mpidr & MPIDR_AFFINITY_MASK, + MPIDR_AFFLVL0); + assert(node); + + return psci_get_aff_map_node_suspend_afflvl(node); +} + + +/******************************************************************************* + * This function gets the affinity level till which the current cpu was powered + * down during a cpu_suspend call. Returns PSCI_INVALID_DATA if the + * power state saved for the node is invalid + ******************************************************************************/ +int psci_get_aff_map_node_suspend_afflvl(aff_map_node_t *node) +{ + unsigned int power_state; + + assert(node->level == MPIDR_AFFLVL0); + + power_state = psci_suspend_context[node->data].power_state; + return ((power_state == PSCI_INVALID_DATA) ? + power_state : psci_get_pstate_afflvl(power_state)); +} + +/******************************************************************************* + * This function gets the state id of a cpu stored in suspend context + * while powering down during a cpu_suspend call. Returns 0xFFFFFFFF + * if the power state saved for the node is invalid + ******************************************************************************/ +int psci_get_suspend_stateid(unsigned long mpidr) +{ + aff_map_node_t *node; + unsigned int power_state; + + node = psci_get_aff_map_node(mpidr & MPIDR_AFFINITY_MASK, + MPIDR_AFFLVL0); + assert(node); + assert(node->level == MPIDR_AFFLVL0); + + power_state = psci_suspend_context[node->data].power_state; + return ((power_state == PSCI_INVALID_DATA) ? + power_state : psci_get_pstate_id(power_state)); +} + +/******************************************************************************* + * 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(unsigned long mpidr, + aff_map_node_t *cpu_node, + unsigned long ns_entrypoint, + unsigned long context_id, + unsigned int power_state) +{ + unsigned int index, plat_state; + unsigned long psci_entrypoint, sctlr; + el3_state_t *saved_el3_state; + int rc = PSCI_E_SUCCESS; + + /* 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(cpu_node, 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); + + /* State management: mark this cpu as suspended */ + psci_set_state(cpu_node, PSCI_STATE_SUSPEND); + + /* + * Generic management: Store the re-entry information for the + * non-secure world + */ + index = cpu_node->data; + rc = psci_set_ns_entry_info(index, ns_entrypoint, context_id); + if (rc != PSCI_E_SUCCESS) + return rc; + + /* + * Arch. management: Save the EL3 state in the 'cpu_context' + * structure that has been allocated for this cpu, flush the + * L1 caches and exit intra-cluster coherency et al + */ + cm_el3_sysregs_context_save(NON_SECURE); + rc = PSCI_E_SUCCESS; + + /* + * The EL3 state to PoC since it will be accessed after a + * reset with the caches turned off + */ + saved_el3_state = get_el3state_ctx(cm_get_context(mpidr, NON_SECURE)); + flush_dcache_range((uint64_t) saved_el3_state, sizeof(*saved_el3_state)); + + /* Set the secure world (EL3) re-entry point after BL1 */ + psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry; + + /* + * Arch. management. Perform the necessary steps to flush all + * cpu caches. + * + * TODO: This power down sequence varies across cpus so it needs to be + * abstracted out on the basis of the MIDR like in cpu_reset_handler(). + * Do the bare minimal for the time being. Fix this before porting to + * Cortex models. + */ + sctlr = read_sctlr_el3(); + sctlr &= ~SCTLR_C_BIT; + write_sctlr_el3(sctlr); + isb(); /* ensure MMU disable takes immediate effect */ + + /* + * CAUTION: This flush to the level of unification makes an assumption + * about the cache hierarchy at affinity level 0 (cpu) in the platform. + * Ideally the platform should tell psci which levels to flush to exit + * coherency. + */ + dcsw_op_louis(DCCISW); + + /* + * 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. + */ + if (psci_plat_pm_ops->affinst_suspend) { + plat_state = psci_get_phys_state(cpu_node); + rc = psci_plat_pm_ops->affinst_suspend(mpidr, + psci_entrypoint, + ns_entrypoint, + cpu_node->level, + plat_state); + } + + return rc; +} + +static int psci_afflvl1_suspend(unsigned long mpidr, + aff_map_node_t *cluster_node, + unsigned long ns_entrypoint, + unsigned long context_id, + unsigned int power_state) +{ + int rc = PSCI_E_SUCCESS; + unsigned int plat_state; + unsigned long psci_entrypoint; + + /* Sanity check the cluster level */ + assert(cluster_node->level == MPIDR_AFFLVL1); + + /* State management: Decrement the cluster reference count */ + psci_set_state(cluster_node, PSCI_STATE_SUSPEND); + + /* + * Keep the physical state of this cluster handy to decide + * what action needs to be taken + */ + plat_state = psci_get_phys_state(cluster_node); + + /* + * Arch. management: Flush all levels of caches to PoC if the + * cluster is to be shutdown + */ + if (plat_state == PSCI_STATE_OFF) + dcsw_op_all(DCCISW); + + /* + * Plat. Management. Allow the platform to do its cluster + * specific bookeeping e.g. turn off interconnect coherency, + * program the power controller etc. + */ + if (psci_plat_pm_ops->affinst_suspend) { + + /* + * Sending the psci entrypoint is currently redundant + * beyond affinity level 0 but one never knows what a + * platform might do. Also it allows us to keep the + * platform handler prototype the same. + */ + psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry; + rc = psci_plat_pm_ops->affinst_suspend(mpidr, + psci_entrypoint, + ns_entrypoint, + cluster_node->level, + plat_state); + } + + return rc; +} + + +static int psci_afflvl2_suspend(unsigned long mpidr, + aff_map_node_t *system_node, + unsigned long ns_entrypoint, + unsigned long context_id, + unsigned int power_state) +{ + int rc = PSCI_E_SUCCESS; + unsigned int plat_state; + unsigned long psci_entrypoint; + + /* Cannot go beyond this */ + assert(system_node->level == MPIDR_AFFLVL2); + + /* State management: Decrement the system reference count */ + psci_set_state(system_node, PSCI_STATE_SUSPEND); + + /* + * Keep the physical state of the system handy to decide what + * action needs to be taken + */ + plat_state = psci_get_phys_state(system_node); + + /* + * Plat. Management : Allow the platform to do its bookeeping + * at this affinity level + */ + if (psci_plat_pm_ops->affinst_suspend) { + + /* + * Sending the psci entrypoint is currently redundant + * beyond affinity level 0 but one never knows what a + * platform might do. Also it allows us to keep the + * platform handler prototype the same. + */ + psci_entrypoint = (unsigned long) psci_aff_suspend_finish_entry; + rc = psci_plat_pm_ops->affinst_suspend(mpidr, + psci_entrypoint, + ns_entrypoint, + system_node->level, + plat_state); + } + + return rc; +} + +static const afflvl_suspend_handler_t psci_afflvl_suspend_handlers[] = { + psci_afflvl0_suspend, + psci_afflvl1_suspend, + psci_afflvl2_suspend, +}; + +/******************************************************************************* + * This function takes an array of pointers to affinity instance nodes in the + * topology tree and calls the suspend handler for the corresponding affinity + * levels + ******************************************************************************/ +static int psci_call_suspend_handlers(mpidr_aff_map_nodes_t mpidr_nodes, + int start_afflvl, + int end_afflvl, + unsigned long mpidr, + unsigned long entrypoint, + unsigned long context_id, + unsigned int power_state) +{ + int rc = PSCI_E_INVALID_PARAMS, level; + aff_map_node_t *node; + + for (level = start_afflvl; level <= end_afflvl; level++) { + node = mpidr_nodes[level]; + if (node == NULL) + continue; + + /* + * TODO: In case of an error should there be a way + * of restoring what we might have torn down at + * lower affinity levels. + */ + rc = psci_afflvl_suspend_handlers[level](mpidr, + node, + entrypoint, + context_id, + power_state); + if (rc != PSCI_E_SUCCESS) + break; + } + + return rc; +} + +/******************************************************************************* + * Top level handler which is called when a cpu wants to suspend its execution. + * It is assumed that along with turning the cpu off, higher affinity levels + * until the target affinity level will be turned off as well. It traverses + * through all the affinity levels performing generic, architectural, platform + * setup and state management e.g. for a cluster that's to be suspended, it will + * call the platform specific code which will disable coherency at the + * interconnect level if the cpu is the last in the cluster. For a cpu it could + * mean programming the power controller etc. + * + * The state of all the relevant affinity levels is changed prior to calling the + * affinity level specific handlers as their actions would depend upon the state + * the affinity level is about to enter. + * + * The affinity level specific handlers are called in ascending order i.e. from + * the lowest to the highest affinity level implemented by the platform because + * to turn off affinity level X it is neccesary to turn off affinity level X - 1 + * first. + * + * CAUTION: This function is called with coherent stacks so that coherency can + * be turned off and caches can be flushed safely. + ******************************************************************************/ +int psci_afflvl_suspend(unsigned long mpidr, + unsigned long entrypoint, + unsigned long context_id, + unsigned int power_state, + int start_afflvl, + int end_afflvl) +{ + int rc = PSCI_E_SUCCESS; + mpidr_aff_map_nodes_t mpidr_nodes; + + mpidr &= MPIDR_AFFINITY_MASK; + + /* + * Collect the pointers to the nodes in the topology tree for + * each affinity instance in the mpidr. If this function does + * not return successfully then either the mpidr or the affinity + * levels are incorrect. + */ + rc = psci_get_aff_map_nodes(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); + if (rc != PSCI_E_SUCCESS) + return rc; + + /* + * This function acquires the lock corresponding to each affinity + * level so that by the time all locks are taken, the system topology + * is snapshot and state management can be done safely. + */ + psci_acquire_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); + + /* Perform generic, architecture and platform specific handling */ + rc = psci_call_suspend_handlers(mpidr_nodes, + start_afflvl, + end_afflvl, + mpidr, + entrypoint, + context_id, + power_state); + + /* + * Release the locks corresponding to each affinity level in the + * reverse order to which they were acquired. + */ + psci_release_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); + + return rc; +} + +/******************************************************************************* + * The following functions finish an earlier affinity suspend request. They + * are called by the common finisher routine in psci_common.c. + ******************************************************************************/ +static unsigned int psci_afflvl0_suspend_finish(unsigned long mpidr, + aff_map_node_t *cpu_node) +{ + unsigned int index, plat_state, state, rc = PSCI_E_SUCCESS; + int32_t suspend_level; + + assert(cpu_node->level == MPIDR_AFFLVL0); + + /* Ensure we have been woken up from a suspended state */ + state = psci_get_state(cpu_node); + assert(state == PSCI_STATE_SUSPEND); + + /* + * Plat. management: Perform the platform specific actions + * before we change the state of the cpu e.g. enabling the + * gic or zeroing the mailbox register. If anything goes + * wrong then assert as there is no way to recover from this + * situation. + */ + if (psci_plat_pm_ops->affinst_suspend_finish) { + + /* Get the physical state of this cpu */ + plat_state = get_phys_state(state); + rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr, + cpu_node->level, + plat_state); + assert(rc == PSCI_E_SUCCESS); + } + + /* Get the index for restoring the re-entry information */ + index = cpu_node->data; + + /* + * Arch. management: Restore the stashed EL3 architectural + * context from the 'cpu_context' structure for this cpu. + */ + cm_el3_sysregs_context_restore(NON_SECURE); + rc = PSCI_E_SUCCESS; + + /* + * Use the more complex exception vectors to enable SPD + * initialisation. SP_EL3 should point to a 'cpu_context' + * structure. The non-secure context should have been + * set on this cpu prior to suspension. + */ + assert(cm_get_context(mpidr, NON_SECURE)); + cm_set_next_eret_context(NON_SECURE); + cm_init_pcpu_ptr_cache(); + write_vbar_el3((uint64_t) runtime_exceptions); + + /* + * Call the cpu suspend finish 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) { + suspend_level = psci_get_aff_map_node_suspend_afflvl(cpu_node); + assert (suspend_level != PSCI_INVALID_DATA); + psci_spd_pm->svc_suspend_finish(suspend_level); + } + + /* Invalidate the suspend context for the node */ + psci_set_suspend_power_state(cpu_node, PSCI_INVALID_DATA); + + /* + * Generic management: Now we just need to retrieve the + * information that we had stashed away during the suspend + * call to set this cpu on its way. + */ + psci_get_ns_entry_info(index); + + /* State management: mark this cpu as on */ + psci_set_state(cpu_node, PSCI_STATE_ON); + + /* Clean caches before re-entering normal world */ + dcsw_op_louis(DCCSW); + + return rc; +} + +static unsigned int psci_afflvl1_suspend_finish(unsigned long mpidr, + aff_map_node_t *cluster_node) +{ + unsigned int plat_state, rc = PSCI_E_SUCCESS; + + assert(cluster_node->level == MPIDR_AFFLVL1); + + /* + * Plat. management: Perform the platform specific actions + * as per the old state of the cluster e.g. enabling + * coherency at the interconnect depends upon the state with + * which this cluster was powered up. If anything goes wrong + * then assert as there is no way to recover from this + * situation. + */ + if (psci_plat_pm_ops->affinst_suspend_finish) { + + /* Get the physical state of this cpu */ + plat_state = psci_get_phys_state(cluster_node); + rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr, + cluster_node->level, + plat_state); + assert(rc == PSCI_E_SUCCESS); + } + + /* State management: Increment the cluster reference count */ + psci_set_state(cluster_node, PSCI_STATE_ON); + + return rc; +} + + +static unsigned int psci_afflvl2_suspend_finish(unsigned long mpidr, + aff_map_node_t *system_node) +{ + unsigned int plat_state, rc = PSCI_E_SUCCESS;; + + /* Cannot go beyond this affinity level */ + assert(system_node->level == MPIDR_AFFLVL2); + + /* + * Currently, there are no architectural actions to perform + * at the system level. + */ + + /* + * Plat. management: Perform the platform specific actions + * as per the old state of the cluster e.g. enabling + * coherency at the interconnect depends upon the state with + * which this cluster was powered up. If anything goes wrong + * then assert as there is no way to recover from this + * situation. + */ + if (psci_plat_pm_ops->affinst_suspend_finish) { + + /* Get the physical state of the system */ + plat_state = psci_get_phys_state(system_node); + rc = psci_plat_pm_ops->affinst_suspend_finish(mpidr, + system_node->level, + plat_state); + assert(rc == PSCI_E_SUCCESS); + } + + /* State management: Increment the system reference count */ + psci_set_state(system_node, PSCI_STATE_ON); + + return rc; +} + +const afflvl_power_on_finisher_t psci_afflvl_suspend_finishers[] = { + psci_afflvl0_suspend_finish, + psci_afflvl1_suspend_finish, + psci_afflvl2_suspend_finish, +}; + diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c new file mode 100644 index 0000000..b1ee10d --- /dev/null +++ b/services/std_svc/psci/psci_common.c @@ -0,0 +1,550 @@ +/* + * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <arch.h> +#include <arch_helpers.h> +#include <assert.h> +#include <bl_common.h> +#include <context.h> +#include <context_mgmt.h> +#include <debug.h> +#include <platform.h> +#include "psci_private.h" + +/* + * SPD power management operations, expected to be supplied by the registered + * SPD on successful SP initialization + */ +const spd_pm_ops_t *psci_spd_pm; + +/******************************************************************************* + * Arrays that contains information needs to resume a cpu's execution when woken + * out of suspend or off states. Each cpu is allocated a single entry in each + * array during startup. + ******************************************************************************/ +suspend_context_t psci_suspend_context[PSCI_NUM_AFFS]; +ns_entry_info_t psci_ns_entry_info[PSCI_NUM_AFFS]; + +/******************************************************************************* + * Grand array that holds the platform's topology information for state + * management of affinity instances. Each node (aff_map_node) in the array + * corresponds to an affinity instance e.g. cluster, cpu within an mpidr + ******************************************************************************/ +aff_map_node_t psci_aff_map[PSCI_NUM_AFFS] +__attribute__ ((section("tzfw_coherent_mem"))); + +/******************************************************************************* + * Pointer to functions exported by the platform to complete power mgmt. ops + ******************************************************************************/ +const plat_pm_ops_t *psci_plat_pm_ops; + +/******************************************************************************* + * Routine to return the maximum affinity level to traverse to after a cpu has + * been physically powered up. It is expected to be called immediately after + * reset from assembler code. It has to find its 'aff_map_node' instead of + * getting it as an argument. + * TODO: Calling psci_get_aff_map_node() with the MMU disabled is slow. Add + * support to allow faster access to the target affinity level. + ******************************************************************************/ +int get_power_on_target_afflvl(unsigned long mpidr) +{ + aff_map_node_t *node; + unsigned int state; + int afflvl; + + /* Retrieve our node from the topology tree */ + node = psci_get_aff_map_node(mpidr & MPIDR_AFFINITY_MASK, + MPIDR_AFFLVL0); + assert(node); + + /* + * Return the maximum supported affinity level if this cpu was off. + * Call the handler in the suspend code if this cpu had been suspended. + * Any other state is invalid. + */ + state = psci_get_state(node); + if (state == PSCI_STATE_ON_PENDING) + return get_max_afflvl(); + + if (state == PSCI_STATE_SUSPEND) { + afflvl = psci_get_aff_map_node_suspend_afflvl(node); + assert(afflvl != PSCI_INVALID_DATA); + return afflvl; + } + return PSCI_E_INVALID_PARAMS; +} + +/******************************************************************************* + * Simple routine to retrieve the maximum affinity level supported by the + * platform and check that it makes sense. + ******************************************************************************/ +int get_max_afflvl() +{ + int aff_lvl; + + aff_lvl = plat_get_max_afflvl(); + assert(aff_lvl <= MPIDR_MAX_AFFLVL && aff_lvl >= MPIDR_AFFLVL0); + + return aff_lvl; +} + +/******************************************************************************* + * Simple routine to set the id of an affinity instance at a given level in the + * mpidr. + ******************************************************************************/ +unsigned long mpidr_set_aff_inst(unsigned long mpidr, + unsigned char aff_inst, + int aff_lvl) +{ + unsigned long aff_shift; + + assert(aff_lvl <= MPIDR_AFFLVL3); + + /* + * Decide the number of bits to shift by depending upon + * the affinity level + */ + aff_shift = get_afflvl_shift(aff_lvl); + + /* Clear the existing affinity instance & set the new one*/ + mpidr &= ~(MPIDR_AFFLVL_MASK << aff_shift); + mpidr |= aff_inst << aff_shift; + + return mpidr; +} + +/******************************************************************************* + * This function sanity checks a range of affinity levels. + ******************************************************************************/ +int psci_check_afflvl_range(int start_afflvl, int end_afflvl) +{ + /* Sanity check the parameters passed */ + if (end_afflvl > MPIDR_MAX_AFFLVL) + return PSCI_E_INVALID_PARAMS; + + if (start_afflvl < MPIDR_AFFLVL0) + return PSCI_E_INVALID_PARAMS; + + if (end_afflvl < start_afflvl) + return PSCI_E_INVALID_PARAMS; + + return PSCI_E_SUCCESS; +} + +/******************************************************************************* + * This function is passed an array of pointers to affinity level nodes in the + * topology tree for an mpidr. It picks up locks for each affinity level bottom + * up in the range specified. + ******************************************************************************/ +void psci_acquire_afflvl_locks(unsigned long mpidr, + int start_afflvl, + int end_afflvl, + mpidr_aff_map_nodes_t mpidr_nodes) +{ + int level; + + for (level = start_afflvl; level <= end_afflvl; level++) { + if (mpidr_nodes[level] == NULL) + continue; + bakery_lock_get(mpidr, &mpidr_nodes[level]->lock); + } +} + +/******************************************************************************* + * This function is passed an array of pointers to affinity level nodes in the + * topology tree for an mpidr. It releases the lock for each affinity level top + * down in the range specified. + ******************************************************************************/ +void psci_release_afflvl_locks(unsigned long mpidr, + int start_afflvl, + int end_afflvl, + mpidr_aff_map_nodes_t mpidr_nodes) +{ + int level; + + for (level = end_afflvl; level >= start_afflvl; level--) { + if (mpidr_nodes[level] == NULL) + continue; + bakery_lock_release(mpidr, &mpidr_nodes[level]->lock); + } +} + +/******************************************************************************* + * Simple routine to determine whether an affinity instance at a given level + * in an mpidr exists or not. + ******************************************************************************/ +int psci_validate_mpidr(unsigned long mpidr, int level) +{ + aff_map_node_t *node; + + node = psci_get_aff_map_node(mpidr, level); + if (node && (node->state & PSCI_AFF_PRESENT)) + return PSCI_E_SUCCESS; + else + return PSCI_E_INVALID_PARAMS; +} + +/******************************************************************************* + * This function retrieves all the stashed information needed to correctly + * resume a cpu's execution in the non-secure state after it has been physically + * powered on i.e. turned ON or resumed from SUSPEND + ******************************************************************************/ +void psci_get_ns_entry_info(unsigned int index) +{ + unsigned long sctlr = 0, scr, el_status, id_aa64pfr0; + uint64_t mpidr = read_mpidr(); + cpu_context_t *ns_entry_context; + gp_regs_t *ns_entry_gpregs; + + scr = read_scr(); + + /* Find out which EL we are going to */ + id_aa64pfr0 = read_id_aa64pfr0_el1(); + el_status = (id_aa64pfr0 >> ID_AA64PFR0_EL2_SHIFT) & + ID_AA64PFR0_ELX_MASK; + + /* Restore endianess */ + if (psci_ns_entry_info[index].sctlr & SCTLR_EE_BIT) + sctlr |= SCTLR_EE_BIT; + else + sctlr &= ~SCTLR_EE_BIT; + + /* Turn off MMU and Caching */ + sctlr &= ~(SCTLR_M_BIT | SCTLR_C_BIT | SCTLR_M_BIT); + + /* Set the register width */ + if (psci_ns_entry_info[index].scr & SCR_RW_BIT) + scr |= SCR_RW_BIT; + else + scr &= ~SCR_RW_BIT; + + scr |= SCR_NS_BIT; + + if (el_status) + write_sctlr_el2(sctlr); + else + write_sctlr_el1(sctlr); + + /* Fulfill the cpu_on entry reqs. as per the psci spec */ + ns_entry_context = (cpu_context_t *) cm_get_context(mpidr, NON_SECURE); + assert(ns_entry_context); + + /* + * Setup general purpose registers to return the context id and + * prevent leakage of secure information into the normal world. + */ + ns_entry_gpregs = get_gpregs_ctx(ns_entry_context); + write_ctx_reg(ns_entry_gpregs, + CTX_GPREG_X0, + psci_ns_entry_info[index].context_id); + + /* + * Tell the context management library to setup EL3 system registers to + * be able to ERET into the ns state, and SP_EL3 points to the right + * context to exit from EL3 correctly. + */ + cm_set_el3_eret_context(NON_SECURE, + psci_ns_entry_info[index].eret_info.entrypoint, + psci_ns_entry_info[index].eret_info.spsr, + scr); + + cm_set_next_eret_context(NON_SECURE); +} + +/******************************************************************************* + * This function retrieves and stashes all the information needed to correctly + * resume a cpu's execution in the non-secure state after it has been physically + * powered on i.e. turned ON or resumed from SUSPEND. This is done prior to + * turning it on or before suspending it. + ******************************************************************************/ +int psci_set_ns_entry_info(unsigned int index, + unsigned long entrypoint, + unsigned long context_id) +{ + int rc = PSCI_E_SUCCESS; + unsigned int rw, mode, ee, spsr = 0; + unsigned long id_aa64pfr0 = read_id_aa64pfr0_el1(), scr = read_scr(); + unsigned long el_status; + unsigned long daif; + + /* Figure out what mode do we enter the non-secure world in */ + el_status = (id_aa64pfr0 >> ID_AA64PFR0_EL2_SHIFT) & + ID_AA64PFR0_ELX_MASK; + + /* + * Figure out whether the cpu enters the non-secure address space + * in aarch32 or aarch64 + */ + rw = scr & SCR_RW_BIT; + if (rw) { + + /* + * Check whether a Thumb entry point has been provided for an + * aarch64 EL + */ + if (entrypoint & 0x1) + return PSCI_E_INVALID_PARAMS; + + if (el_status && (scr & SCR_HCE_BIT)) { + mode = MODE_EL2; + ee = read_sctlr_el2() & SCTLR_EE_BIT; + } else { + mode = MODE_EL1; + ee = read_sctlr_el1() & SCTLR_EE_BIT; + } + + spsr = SPSR_64(mode, MODE_SP_ELX, DISABLE_ALL_EXCEPTIONS); + + psci_ns_entry_info[index].sctlr |= ee; + psci_ns_entry_info[index].scr |= SCR_RW_BIT; + } else { + + + if (el_status && (scr & SCR_HCE_BIT)) { + mode = MODE32_hyp; + ee = read_sctlr_el2() & SCTLR_EE_BIT; + } else { + mode = MODE32_svc; + ee = read_sctlr_el1() & SCTLR_EE_BIT; + } + + /* + * 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; + + spsr = SPSR_MODE32(mode, entrypoint & 0x1, ee, daif); + + /* Ensure that the CSPR.E and SCTLR.EE bits match */ + psci_ns_entry_info[index].sctlr |= ee; + psci_ns_entry_info[index].scr &= ~SCR_RW_BIT; + } + + psci_ns_entry_info[index].eret_info.entrypoint = entrypoint; + psci_ns_entry_info[index].eret_info.spsr = spsr; + psci_ns_entry_info[index].context_id = context_id; + + return rc; +} + +/******************************************************************************* + * This function takes a pointer to an affinity node in the topology tree and + * returns its state. State of a non-leaf node needs to be calculated. + ******************************************************************************/ +unsigned short psci_get_state(aff_map_node_t *node) +{ + assert(node->level >= MPIDR_AFFLVL0 && node->level <= MPIDR_MAX_AFFLVL); + + /* A cpu node just contains the state which can be directly returned */ + if (node->level == MPIDR_AFFLVL0) + return (node->state >> PSCI_STATE_SHIFT) & PSCI_STATE_MASK; + + /* + * For an affinity level higher than a cpu, the state has to be + * calculated. It depends upon the value of the reference count + * which is managed by each node at the next lower affinity level + * e.g. for a cluster, each cpu increments/decrements the reference + * count. If the reference count is 0 then the affinity level is + * OFF else ON. + */ + if (node->ref_count) + return PSCI_STATE_ON; + else + return PSCI_STATE_OFF; +} + +/******************************************************************************* + * This function takes a pointer to an affinity node in the topology tree and + * a target state. State of a non-leaf node needs to be converted to a reference + * count. State of a leaf node can be set directly. + ******************************************************************************/ +void psci_set_state(aff_map_node_t *node, unsigned short state) +{ + assert(node->level >= MPIDR_AFFLVL0 && node->level <= MPIDR_MAX_AFFLVL); + + /* + * For an affinity level higher than a cpu, the state is used + * to decide whether the reference count is incremented or + * decremented. Entry into the ON_PENDING state does not have + * effect. + */ + if (node->level > MPIDR_AFFLVL0) { + switch (state) { + case PSCI_STATE_ON: + node->ref_count++; + break; + case PSCI_STATE_OFF: + case PSCI_STATE_SUSPEND: + node->ref_count--; + break; + case PSCI_STATE_ON_PENDING: + /* + * An affinity level higher than a cpu will not undergo + * a state change when it is about to be turned on + */ + return; + default: + assert(0); + } + } else { + node->state &= ~(PSCI_STATE_MASK << PSCI_STATE_SHIFT); + node->state |= (state & PSCI_STATE_MASK) << PSCI_STATE_SHIFT; + } +} + +/******************************************************************************* + * An affinity level could be on, on_pending, suspended or off. These are the + * logical states it can be in. Physically either it is off or on. When it is in + * the state on_pending then it is about to be turned on. It is not possible to + * tell whether that's actually happenned or not. So we err on the side of + * caution & treat the affinity level as being turned off. + ******************************************************************************/ +unsigned short psci_get_phys_state(aff_map_node_t *node) +{ + unsigned int state; + + state = psci_get_state(node); + return get_phys_state(state); +} + +/******************************************************************************* + * This function takes an array of pointers to affinity instance nodes in the + * topology tree and calls the physical power on handler for the corresponding + * affinity levels + ******************************************************************************/ +static int psci_call_power_on_handlers(mpidr_aff_map_nodes_t mpidr_nodes, + int start_afflvl, + int end_afflvl, + afflvl_power_on_finisher_t *pon_handlers, + unsigned long mpidr) +{ + int rc = PSCI_E_INVALID_PARAMS, level; + aff_map_node_t *node; + + for (level = end_afflvl; level >= start_afflvl; level--) { + node = mpidr_nodes[level]; + if (node == NULL) + continue; + + /* + * If we run into any trouble while powering up an + * affinity instance, then there is no recovery path + * so simply return an error and let the caller take + * care of the situation. + */ + rc = pon_handlers[level](mpidr, node); + if (rc != PSCI_E_SUCCESS) + break; + } + + return rc; +} + +/******************************************************************************* + * Generic handler which is called when a cpu is physically powered on. It + * traverses through all the affinity levels performing generic, architectural, + * platform setup and state management e.g. for a cluster that's been powered + * on, it will call the platform specific code which will enable coherency at + * the interconnect level. For a cpu it could mean turning on the MMU etc. + * + * The state of all the relevant affinity levels is changed after calling the + * affinity level specific handlers as their actions would depend upon the state + * the affinity level is exiting from. + * + * The affinity level specific handlers are called in descending order i.e. from + * the highest to the lowest affinity level implemented by the platform because + * to turn on affinity level X it is neccesary to turn on affinity level X + 1 + * first. + * + * CAUTION: This function is called with coherent stacks so that coherency and + * the mmu can be turned on safely. + ******************************************************************************/ +void psci_afflvl_power_on_finish(unsigned long mpidr, + int start_afflvl, + int end_afflvl, + afflvl_power_on_finisher_t *pon_handlers) +{ + mpidr_aff_map_nodes_t mpidr_nodes; + int rc; + + mpidr &= MPIDR_AFFINITY_MASK; + + /* + * Collect the pointers to the nodes in the topology tree for + * each affinity instance in the mpidr. If this function does + * not return successfully then either the mpidr or the affinity + * levels are incorrect. Either case is an irrecoverable error. + */ + rc = psci_get_aff_map_nodes(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); + if (rc != PSCI_E_SUCCESS) + panic(); + + /* + * This function acquires the lock corresponding to each affinity + * level so that by the time all locks are taken, the system topology + * is snapshot and state management can be done safely. + */ + psci_acquire_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); + + /* Perform generic, architecture and platform specific handling */ + rc = psci_call_power_on_handlers(mpidr_nodes, + start_afflvl, + end_afflvl, + pon_handlers, + mpidr); + if (rc != PSCI_E_SUCCESS) + panic(); + + /* + * This loop releases the lock corresponding to each affinity level + * in the reverse order to which they were acquired. + */ + psci_release_afflvl_locks(mpidr, + start_afflvl, + end_afflvl, + mpidr_nodes); +} + +/******************************************************************************* + * This function initializes the set of hooks that PSCI invokes as part of power + * management operation. The power management hooks are expected to be provided + * by the SPD, after it finishes all its initialization + ******************************************************************************/ +void psci_register_spd_pm_hook(const spd_pm_ops_t *pm) +{ + psci_spd_pm = pm; +} diff --git a/services/std_svc/psci/psci_entry.S b/services/std_svc/psci/psci_entry.S new file mode 100644 index 0000000..3954ab1 --- /dev/null +++ b/services/std_svc/psci/psci_entry.S @@ -0,0 +1,171 @@ +/* + * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <arch.h> +#include <asm_macros.S> +#include <psci.h> + + .globl psci_aff_on_finish_entry + .globl psci_aff_suspend_finish_entry + .globl __psci_cpu_off + .globl __psci_cpu_suspend + .globl psci_power_down_wfi + + /* ----------------------------------------------------- + * This cpu has been physically powered up. Depending + * upon whether it was resumed from suspend or simply + * turned on, call the common power on finisher with + * the handlers (chosen depending upon original state). + * For ease, the finisher is called with coherent + * stacks. This allows the cluster/cpu finishers to + * enter coherency and enable the mmu without running + * into issues. We switch back to normal stacks once + * all this is done. + * ----------------------------------------------------- + */ +func psci_aff_on_finish_entry + adr x23, psci_afflvl_on_finishers + b psci_aff_common_finish_entry + +psci_aff_suspend_finish_entry: + adr x23, psci_afflvl_suspend_finishers + +psci_aff_common_finish_entry: + adr x22, psci_afflvl_power_on_finish + + /* --------------------------------------------- + * Exceptions should not occur at this point. + * Set VBAR in order to handle and report any + * that do occur + * --------------------------------------------- + */ + adr x0, early_exceptions + msr vbar_el3, x0 + isb + + /* --------------------------------------------- + * Enable Debug and SError interrupts + * --------------------------------------------- + */ + msr daifclr, #(DAIF_ABT_BIT | DAIF_DBG_BIT) + + /* --------------------------------------------- + * Use SP_EL0 for the C runtime stack. + * --------------------------------------------- + */ + msr spsel, #0 + + mrs x0, mpidr_el1 + bl platform_set_coherent_stack + + /* --------------------------------------------- + * Call the finishers starting from affinity + * level 0. + * --------------------------------------------- + */ + mrs x0, mpidr_el1 + bl get_power_on_target_afflvl + cmp x0, xzr + b.lt _panic + mov x3, x23 + mov x2, x0 + mov x1, #MPIDR_AFFLVL0 + mrs x0, mpidr_el1 + blr x22 + + /* -------------------------------------------- + * Give ourselves a stack allocated in Normal + * -IS-WBWA memory + * -------------------------------------------- + */ + mrs x0, mpidr_el1 + bl platform_set_stack + + b el3_exit +_panic: + b _panic + + /* ----------------------------------------------------- + * The following two stubs give the calling cpu a + * coherent stack to allow flushing of caches without + * suffering from stack coherency issues + * ----------------------------------------------------- + */ +func __psci_cpu_off + func_prologue + sub sp, sp, #0x10 + stp x19, x20, [sp, #0] + mov x19, sp + mrs x0, mpidr_el1 + bl platform_set_coherent_stack + bl psci_cpu_off + mov sp, x19 + ldp x19, x20, [sp,#0] + add sp, sp, #0x10 + func_epilogue + ret + +func __psci_cpu_suspend + func_prologue + sub sp, sp, #0x20 + stp x19, x20, [sp, #0] + stp x21, x22, [sp, #0x10] + mov x19, sp + mov x20, x0 + mov x21, x1 + mov x22, x2 + mrs x0, mpidr_el1 + bl platform_set_coherent_stack + mov x0, x20 + mov x1, x21 + mov x2, x22 + bl psci_cpu_suspend + mov sp, x19 + ldp x21, x22, [sp,#0x10] + ldp x19, x20, [sp,#0] + add sp, sp, #0x20 + func_epilogue + ret + + /* -------------------------------------------- + * This function is called to indicate to the + * power controller that it is safe to power + * down this cpu. It should not exit the wfi + * and will be released from reset upon power + * up. 'wfi_spill' is used to catch erroneous + * exits from wfi. + * -------------------------------------------- + */ +func psci_power_down_wfi + dsb sy // ensure write buffer empty + wfi +wfi_spill: + b wfi_spill + diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c new file mode 100644 index 0000000..c0866fb --- /dev/null +++ b/services/std_svc/psci/psci_main.c @@ -0,0 +1,270 @@ +/* + * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <arch.h> +#include <arch_helpers.h> +#include <assert.h> +#include <runtime_svc.h> +#include <debug.h> +#include "psci_private.h" + +/******************************************************************************* + * PSCI frontend api for servicing SMCs. Described in the PSCI spec. + ******************************************************************************/ +int psci_cpu_on(unsigned long target_cpu, + unsigned long entrypoint, + unsigned long context_id) + +{ + int rc; + unsigned int start_afflvl, end_afflvl; + + /* Determine if the cpu exists of not */ + rc = psci_validate_mpidr(target_cpu, MPIDR_AFFLVL0); + if (rc != PSCI_E_SUCCESS) { + goto exit; + } + + /* + * To turn this cpu on, specify which affinity + * levels need to be turned on + */ + start_afflvl = MPIDR_AFFLVL0; + end_afflvl = get_max_afflvl(); + rc = psci_afflvl_on(target_cpu, + entrypoint, + context_id, + start_afflvl, + end_afflvl); + +exit: + return rc; +} + +unsigned int psci_version(void) +{ + return PSCI_MAJOR_VER | PSCI_MINOR_VER; +} + +int psci_cpu_suspend(unsigned int power_state, + unsigned long entrypoint, + unsigned long context_id) +{ + int rc; + unsigned long mpidr; + unsigned int target_afflvl, pstate_type; + + /* Check SBZ bits in power state are zero */ + if (psci_validate_power_state(power_state)) + return PSCI_E_INVALID_PARAMS; + + /* Sanity check the requested state */ + target_afflvl = psci_get_pstate_afflvl(power_state); + if (target_afflvl > MPIDR_MAX_AFFLVL) + return PSCI_E_INVALID_PARAMS; + + /* Determine the 'state type' in the 'power_state' parameter */ + pstate_type = psci_get_pstate_type(power_state); + + /* + * Ensure that we have a platform specific handler for entering + * a standby state. + */ + if (pstate_type == PSTATE_TYPE_STANDBY) { + if (!psci_plat_pm_ops->affinst_standby) + return PSCI_E_INVALID_PARAMS; + + rc = psci_plat_pm_ops->affinst_standby(power_state); + assert(rc == PSCI_E_INVALID_PARAMS || rc == PSCI_E_SUCCESS); + return rc; + } + + /* + * Do what is needed to enter the power down state. Upon success, + * enter the final wfi which will power down this cpu else return + * an error. + */ + mpidr = read_mpidr(); + rc = psci_afflvl_suspend(mpidr, + entrypoint, + context_id, + power_state, + MPIDR_AFFLVL0, + target_afflvl); + if (rc == PSCI_E_SUCCESS) + psci_power_down_wfi(); + assert(rc == PSCI_E_INVALID_PARAMS); + return rc; +} + +int psci_cpu_off(void) +{ + int rc; + unsigned long mpidr; + int target_afflvl = get_max_afflvl(); + + mpidr = read_mpidr(); + + /* + * Traverse from the highest to the lowest affinity level. When the + * lowest affinity level is hit, all the locks are acquired. State + * management is done immediately followed by cpu, cluster ... + * ..target_afflvl specific actions as this function unwinds back. + */ + rc = psci_afflvl_off(mpidr, MPIDR_AFFLVL0, target_afflvl); + + /* + * Check if all actions needed to safely power down this cpu have + * successfully completed. Enter a wfi loop which will allow the + * power controller to physically power down this cpu. + */ + if (rc == PSCI_E_SUCCESS) + psci_power_down_wfi(); + + /* + * The only error cpu_off can return is E_DENIED. So check if that's + * indeed the case. + */ + assert (rc == PSCI_E_DENIED); + + return rc; +} + +int psci_affinity_info(unsigned long target_affinity, + unsigned int lowest_affinity_level) +{ + int rc = PSCI_E_INVALID_PARAMS; + unsigned int aff_state; + aff_map_node_t *node; + + if (lowest_affinity_level > get_max_afflvl()) + return rc; + + node = psci_get_aff_map_node(target_affinity, lowest_affinity_level); + if (node && (node->state & PSCI_AFF_PRESENT)) { + + /* + * TODO: For affinity levels higher than 0 i.e. cpu, the + * state will always be either ON or OFF. Need to investigate + * how critical is it to support ON_PENDING here. + */ + aff_state = psci_get_state(node); + + /* A suspended cpu is available & on for the OS */ + if (aff_state == PSCI_STATE_SUSPEND) { + aff_state = PSCI_STATE_ON; + } + + rc = aff_state; + } + + return rc; +} + +/* Unimplemented */ +int psci_migrate(unsigned int target_cpu) +{ + return PSCI_E_NOT_SUPPORTED; +} + +/* Unimplemented */ +unsigned int psci_migrate_info_type(void) +{ + return PSCI_TOS_NOT_PRESENT_MP; +} + +unsigned long psci_migrate_info_up_cpu(void) +{ + /* + * Return value of this currently unsupported call depends upon + * what psci_migrate_info_type() returns. + */ + return PSCI_E_SUCCESS; +} + +/******************************************************************************* + * PSCI top level handler for servicing SMCs. + ******************************************************************************/ +uint64_t psci_smc_handler(uint32_t smc_fid, + uint64_t x1, + uint64_t x2, + uint64_t x3, + uint64_t x4, + void *cookie, + void *handle, + uint64_t flags) +{ + uint64_t rc; + + switch (smc_fid) { + case PSCI_VERSION: + rc = psci_version(); + break; + + case PSCI_CPU_OFF: + rc = __psci_cpu_off(); + break; + + case PSCI_CPU_SUSPEND_AARCH64: + case PSCI_CPU_SUSPEND_AARCH32: + rc = __psci_cpu_suspend(x1, x2, x3); + break; + + case PSCI_CPU_ON_AARCH64: + case PSCI_CPU_ON_AARCH32: + rc = psci_cpu_on(x1, x2, x3); + break; + + case PSCI_AFFINITY_INFO_AARCH32: + case PSCI_AFFINITY_INFO_AARCH64: + rc = psci_affinity_info(x1, x2); + break; + + case PSCI_MIG_AARCH32: + case PSCI_MIG_AARCH64: + rc = psci_migrate(x1); + break; + + case PSCI_MIG_INFO_TYPE: + rc = psci_migrate_info_type(); + break; + + case PSCI_MIG_INFO_UP_CPU_AARCH32: + case PSCI_MIG_INFO_UP_CPU_AARCH64: + rc = psci_migrate_info_up_cpu(); + break; + + default: + rc = SMC_UNK; + WARN("Unimplemented PSCI Call: 0x%x \n", smc_fid); + } + + SMC_RET1(handle, rc); +} diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h new file mode 100644 index 0000000..747a2d4 --- /dev/null +++ b/services/std_svc/psci/psci_private.h @@ -0,0 +1,156 @@ +/* + * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#ifndef __PSCI_PRIVATE_H__ +#define __PSCI_PRIVATE_H__ + +#include <arch.h> +#include <bakery_lock.h> +#include <psci.h> + +/******************************************************************************* + * The following two data structures hold the generic information to bringup + * a suspended/hotplugged out cpu + ******************************************************************************/ +typedef struct eret_params { + unsigned long entrypoint; + unsigned long spsr; +} eret_params_t; + +typedef struct ns_entry_info { + eret_params_t eret_info; + unsigned long context_id; + unsigned int scr; + unsigned int sctlr; +} ns_entry_info_t; + +/******************************************************************************* + * The following two data structures hold the topology tree which in turn tracks + * the state of the all the affinity instances supported by the platform. + ******************************************************************************/ +typedef struct aff_map_node { + unsigned long mpidr; + unsigned short ref_count; + unsigned char state; + unsigned char level; + unsigned int data; + bakery_lock_t lock; +} aff_map_node_t; + +typedef struct aff_limits_node { + int min; + int max; +} aff_limits_node_t; + +/******************************************************************************* + * This data structure holds secure world context that needs to be preserved + * across cpu_suspend calls which enter the power down state. + ******************************************************************************/ +typedef struct suspend_context { + unsigned int power_state; +} __aligned(CACHE_WRITEBACK_GRANULE) suspend_context_t; + +typedef aff_map_node_t (*mpidr_aff_map_nodes_t[MPIDR_MAX_AFFLVL]); +typedef unsigned int (*afflvl_power_on_finisher_t)(unsigned long, + aff_map_node_t *); + +/******************************************************************************* + * Data prototypes + ******************************************************************************/ +extern suspend_context_t psci_suspend_context[PSCI_NUM_AFFS]; +extern ns_entry_info_t psci_ns_entry_info[PSCI_NUM_AFFS]; +extern const plat_pm_ops_t *psci_plat_pm_ops; +extern aff_map_node_t psci_aff_map[PSCI_NUM_AFFS]; + +/******************************************************************************* + * SPD's power management hooks registered with PSCI + ******************************************************************************/ +extern const spd_pm_ops_t *psci_spd_pm; + +/******************************************************************************* + * Function prototypes + ******************************************************************************/ +/* Private exported functions from psci_common.c */ +int get_max_afflvl(void); +unsigned short psci_get_state(aff_map_node_t *node); +unsigned short psci_get_phys_state(aff_map_node_t *node); +void psci_set_state(aff_map_node_t *node, unsigned short state); +void psci_get_ns_entry_info(unsigned int index); +unsigned long mpidr_set_aff_inst(unsigned long, unsigned char, int); +int psci_validate_mpidr(unsigned long, int); +int get_power_on_target_afflvl(unsigned long mpidr); +void psci_afflvl_power_on_finish(unsigned long, + int, + int, + afflvl_power_on_finisher_t *); +int psci_set_ns_entry_info(unsigned int index, + unsigned long entrypoint, + unsigned long context_id); +int psci_check_afflvl_range(int start_afflvl, int end_afflvl); +void psci_acquire_afflvl_locks(unsigned long mpidr, + int start_afflvl, + int end_afflvl, + mpidr_aff_map_nodes_t mpidr_nodes); +void psci_release_afflvl_locks(unsigned long mpidr, + int start_afflvl, + int end_afflvl, + mpidr_aff_map_nodes_t mpidr_nodes); + +/* Private exported functions from psci_setup.c */ +int psci_get_aff_map_nodes(unsigned long mpidr, + int start_afflvl, + int end_afflvl, + mpidr_aff_map_nodes_t mpidr_nodes); +aff_map_node_t *psci_get_aff_map_node(unsigned long, int); + +/* Private exported functions from psci_affinity_on.c */ +int psci_afflvl_on(unsigned long, + unsigned long, + unsigned long, + int, + int); + +/* Private exported functions from psci_affinity_off.c */ +int psci_afflvl_off(unsigned long, int, int); + +/* Private exported functions from psci_affinity_suspend.c */ +void psci_set_suspend_power_state(aff_map_node_t *node, + unsigned int power_state); +int psci_get_aff_map_node_suspend_afflvl(aff_map_node_t *node); +int psci_afflvl_suspend(unsigned long, + unsigned long, + unsigned long, + unsigned int, + int, + int); +unsigned int psci_afflvl_suspend_finish(unsigned long, int, int); + + +#endif /* __PSCI_PRIVATE_H__ */ diff --git a/services/std_svc/psci/psci_setup.c b/services/std_svc/psci/psci_setup.c new file mode 100644 index 0000000..a1587b7 --- /dev/null +++ b/services/std_svc/psci/psci_setup.c @@ -0,0 +1,360 @@ +/* + * Copyright (c) 2013-2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <arch.h> +#include <arch_helpers.h> +#include <assert.h> +#include <bl_common.h> +#include <context.h> +#include <context_mgmt.h> +#include <platform.h> +#include <stddef.h> +#include "psci_private.h" + +/******************************************************************************* + * Per cpu non-secure contexts used to program the architectural state prior + * return to the normal world. + * TODO: Use the memory allocator to set aside memory for the contexts instead + * of relying on platform defined constants. Using PSCI_NUM_AFFS will be an + * overkill. + ******************************************************************************/ +static cpu_context_t psci_ns_context[PLATFORM_CORE_COUNT]; + +/******************************************************************************* + * In a system, a certain number of affinity instances are present at an + * affinity level. The cumulative number of instances across all levels are + * stored in 'psci_aff_map'. The topology tree has been flattenned into this + * array. To retrieve nodes, information about the extents of each affinity + * level i.e. start index and end index needs to be present. 'psci_aff_limits' + * stores this information. + ******************************************************************************/ +static aff_limits_node_t psci_aff_limits[MPIDR_MAX_AFFLVL + 1]; + +/******************************************************************************* + * 'psci_ns_einfo_idx' keeps track of the next free index in the + * 'psci_ns_entry_info' & 'psci_suspend_context' arrays. + ******************************************************************************/ +static unsigned int psci_ns_einfo_idx; + +/******************************************************************************* + * Routines for retrieving the node corresponding to an affinity level instance + * in the mpidr. The first one uses binary search to find the node corresponding + * to the mpidr (key) at a particular affinity level. The second routine decides + * extents of the binary search at each affinity level. + ******************************************************************************/ +static int psci_aff_map_get_idx(unsigned long key, + int min_idx, + int max_idx) +{ + int mid; + + /* + * Terminating condition: If the max and min indices have crossed paths + * during the binary search then the key has not been found. + */ + if (max_idx < min_idx) + return PSCI_E_INVALID_PARAMS; + + /* + * Bisect the array around 'mid' and then recurse into the array chunk + * where the key is likely to be found. The mpidrs in each node in the + * 'psci_aff_map' for a given affinity level are stored in an ascending + * order which makes the binary search possible. + */ + mid = min_idx + ((max_idx - min_idx) >> 1); /* Divide by 2 */ + if (psci_aff_map[mid].mpidr > key) + return psci_aff_map_get_idx(key, min_idx, mid - 1); + else if (psci_aff_map[mid].mpidr < key) + return psci_aff_map_get_idx(key, mid + 1, max_idx); + else + return mid; +} + +aff_map_node_t *psci_get_aff_map_node(unsigned long mpidr, int aff_lvl) +{ + int rc; + + /* Right shift the mpidr to the required affinity level */ + mpidr = mpidr_mask_lower_afflvls(mpidr, aff_lvl); + + rc = psci_aff_map_get_idx(mpidr, + psci_aff_limits[aff_lvl].min, + psci_aff_limits[aff_lvl].max); + if (rc >= 0) + return &psci_aff_map[rc]; + else + return NULL; +} + +/******************************************************************************* + * This function populates an array with nodes corresponding to a given range of + * affinity levels in an mpidr. It returns successfully only when the affinity + * levels are correct, the mpidr is valid i.e. no affinity level is absent from + * the topology tree & the affinity instance at level 0 is not absent. + ******************************************************************************/ +int psci_get_aff_map_nodes(unsigned long mpidr, + int start_afflvl, + int end_afflvl, + mpidr_aff_map_nodes_t mpidr_nodes) +{ + int rc = PSCI_E_INVALID_PARAMS, level; + aff_map_node_t *node; + + rc = psci_check_afflvl_range(start_afflvl, end_afflvl); + if (rc != PSCI_E_SUCCESS) + return rc; + + for (level = start_afflvl; level <= end_afflvl; level++) { + + /* + * Grab the node for each affinity level. No affinity level + * can be missing as that would mean that the topology tree + * is corrupted. + */ + node = psci_get_aff_map_node(mpidr, level); + if (node == NULL) { + rc = PSCI_E_INVALID_PARAMS; + break; + } + + /* + * Skip absent affinity levels unless it's afffinity level 0. + * An absent cpu means that the mpidr is invalid. Save the + * pointer to the node for the present affinity level + */ + if (!(node->state & PSCI_AFF_PRESENT)) { + if (level == MPIDR_AFFLVL0) { + rc = PSCI_E_INVALID_PARAMS; + break; + } + + mpidr_nodes[level] = NULL; + } else + mpidr_nodes[level] = node; + } + + return rc; +} + +/******************************************************************************* + * Function which initializes the 'aff_map_node' corresponding to an affinity + * level instance. Each node has a unique mpidr, level and bakery lock. The data + * field is opaque and holds affinity level specific data e.g. for affinity + * level 0 it contains the index into arrays that hold the secure/non-secure + * state for a cpu that's been turned on/off + ******************************************************************************/ +static void psci_init_aff_map_node(unsigned long mpidr, + int level, + unsigned int idx) +{ + unsigned char state; + uint32_t linear_id; + psci_aff_map[idx].mpidr = mpidr; + psci_aff_map[idx].level = level; + bakery_lock_init(&psci_aff_map[idx].lock); + + /* + * If an affinity instance is present then mark it as OFF to begin with. + */ + state = plat_get_aff_state(level, mpidr); + psci_aff_map[idx].state = state; + + if (level == MPIDR_AFFLVL0) { + + /* + * Mark the cpu as OFF. Higher affinity level reference counts + * have already been memset to 0 + */ + if (state & PSCI_AFF_PRESENT) + psci_set_state(&psci_aff_map[idx], PSCI_STATE_OFF); + + /* Ensure that we have not overflowed the psci_ns_einfo array */ + assert(psci_ns_einfo_idx < PSCI_NUM_AFFS); + + psci_aff_map[idx].data = psci_ns_einfo_idx; + /* Invalidate the suspend context for the node */ + psci_suspend_context[psci_ns_einfo_idx].power_state = PSCI_INVALID_DATA; + psci_ns_einfo_idx++; + + /* + * Associate a non-secure context with this affinity + * instance through the context management library. + */ + linear_id = platform_get_core_pos(mpidr); + assert(linear_id < PLATFORM_CORE_COUNT); + + cm_set_context(mpidr, + (void *) &psci_ns_context[linear_id], + NON_SECURE); + + } + + return; +} + +/******************************************************************************* + * Core routine used by the Breadth-First-Search algorithm to populate the + * affinity tree. Each level in the tree corresponds to an affinity level. This + * routine's aim is to traverse to the target affinity level and populate nodes + * in the 'psci_aff_map' for all the siblings at that level. It uses the current + * affinity level to keep track of how many levels from the root of the tree + * have been traversed. If the current affinity level != target affinity level, + * then the platform is asked to return the number of children that each + * affinity instance has at the current affinity level. Traversal is then done + * for each child at the next lower level i.e. current affinity level - 1. + * + * CAUTION: This routine assumes that affinity instance ids are allocated in a + * monotonically increasing manner at each affinity level in a mpidr starting + * from 0. If the platform breaks this assumption then this code will have to + * be reworked accordingly. + ******************************************************************************/ +static unsigned int psci_init_aff_map(unsigned long mpidr, + unsigned int affmap_idx, + int cur_afflvl, + int tgt_afflvl) +{ + unsigned int ctr, aff_count; + + assert(cur_afflvl >= tgt_afflvl); + + /* + * Find the number of siblings at the current affinity level & + * assert if there are none 'cause then we have been invoked with + * an invalid mpidr. + */ + aff_count = plat_get_aff_count(cur_afflvl, mpidr); + assert(aff_count); + + if (tgt_afflvl < cur_afflvl) { + for (ctr = 0; ctr < aff_count; ctr++) { + mpidr = mpidr_set_aff_inst(mpidr, ctr, cur_afflvl); + affmap_idx = psci_init_aff_map(mpidr, + affmap_idx, + cur_afflvl - 1, + tgt_afflvl); + } + } else { + for (ctr = 0; ctr < aff_count; ctr++, affmap_idx++) { + mpidr = mpidr_set_aff_inst(mpidr, ctr, cur_afflvl); + psci_init_aff_map_node(mpidr, cur_afflvl, affmap_idx); + } + + /* affmap_idx is 1 greater than the max index of cur_afflvl */ + psci_aff_limits[cur_afflvl].max = affmap_idx - 1; + } + + return affmap_idx; +} + +/******************************************************************************* + * This function initializes the topology tree by querying the platform. To do + * so, it's helper routines implement a Breadth-First-Search. At each affinity + * level the platform conveys the number of affinity instances that exist i.e. + * the affinity count. The algorithm populates the psci_aff_map recursively + * using this information. On a platform that implements two clusters of 4 cpus + * each, the populated aff_map_array would look like this: + * + * <- cpus cluster0 -><- cpus cluster1 -> + * --------------------------------------------------- + * | 0 | 1 | 0 | 1 | 2 | 3 | 0 | 1 | 2 | 3 | + * --------------------------------------------------- + * ^ ^ + * cluster __| cpu __| + * limit limit + * + * The first 2 entries are of the cluster nodes. The next 4 entries are of cpus + * within cluster 0. The last 4 entries are of cpus within cluster 1. + * The 'psci_aff_limits' array contains the max & min index of each affinity + * level within the 'psci_aff_map' array. This allows restricting search of a + * node at an affinity level between the indices in the limits array. + ******************************************************************************/ +int32_t psci_setup(void) +{ + unsigned long mpidr = read_mpidr(); + int afflvl, affmap_idx, max_afflvl; + aff_map_node_t *node; + + psci_ns_einfo_idx = 0; + psci_plat_pm_ops = NULL; + + /* Find out the maximum affinity level that the platform implements */ + max_afflvl = get_max_afflvl(); + assert(max_afflvl <= MPIDR_MAX_AFFLVL); + + /* + * This call traverses the topology tree with help from the platform and + * populates the affinity map using a breadth-first-search recursively. + * We assume that the platform allocates affinity instance ids from 0 + * onwards at each affinity level in the mpidr. FIRST_MPIDR = 0.0.0.0 + */ + affmap_idx = 0; + for (afflvl = max_afflvl; afflvl >= MPIDR_AFFLVL0; afflvl--) { + affmap_idx = psci_init_aff_map(FIRST_MPIDR, + affmap_idx, + max_afflvl, + afflvl); + } + + /* + * Set the bounds for the affinity counts of each level in the map. Also + * flush out the entire array so that it's visible to subsequent power + * management operations. The 'psci_aff_map' array is allocated in + * coherent memory so does not need flushing. The 'psci_aff_limits' + * array is allocated in normal memory. It will be accessed when the mmu + * is off e.g. after reset. Hence it needs to be flushed. + */ + for (afflvl = MPIDR_AFFLVL0; afflvl < max_afflvl; afflvl++) { + psci_aff_limits[afflvl].min = + psci_aff_limits[afflvl + 1].max + 1; + } + + flush_dcache_range((unsigned long) psci_aff_limits, + sizeof(psci_aff_limits)); + + /* + * Mark the affinity instances in our mpidr as ON. No need to lock as + * this is the primary cpu. + */ + mpidr &= MPIDR_AFFINITY_MASK; + for (afflvl = MPIDR_AFFLVL0; afflvl <= max_afflvl; afflvl++) { + + node = psci_get_aff_map_node(mpidr, afflvl); + assert(node); + + /* Mark each present node as ON. */ + if (node->state & PSCI_AFF_PRESENT) + psci_set_state(node, PSCI_STATE_ON); + } + + platform_setup_pm(&psci_plat_pm_ops); + assert(psci_plat_pm_ops); + + return 0; +} diff --git a/services/std_svc/std_svc_setup.c b/services/std_svc/std_svc_setup.c new file mode 100644 index 0000000..6cb0319 --- /dev/null +++ b/services/std_svc/std_svc_setup.c @@ -0,0 +1,106 @@ +/* + * Copyright (c) 2014, ARM Limited and Contributors. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions are met: + * + * Redistributions of source code must retain the above copyright notice, this + * list of conditions and the following disclaimer. + * + * Redistributions in binary form must reproduce the above copyright notice, + * this list of conditions and the following disclaimer in the documentation + * and/or other materials provided with the distribution. + * + * Neither the name of ARM nor the names of its contributors may be used + * to endorse or promote products derived from this software without specific + * prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" + * AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE + * IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE + * ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE + * LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR + * CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF + * SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS + * INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN + * CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) + * ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + */ + +#include <debug.h> +#include <psci.h> +#include <runtime_svc.h> +#include <std_svc.h> +#include <stdint.h> +#include <uuid.h> + +/* Standard Service UUID */ +DEFINE_SVC_UUID(arm_svc_uid, + 0x108d905b, 0xf863, 0x47e8, 0xae, 0x2d, + 0xc0, 0xfb, 0x56, 0x41, 0xf6, 0xe2); + +/* Setup Standard Services */ +static int32_t std_svc_setup(void) +{ + /* + * PSCI is the only specification implemented as a Standard Service. + * Invoke PSCI setup from here + */ + return psci_setup(); +} + +/* + * Top-level Standard Service SMC handler. This handler will in turn dispatch + * calls to PSCI SMC handler + */ +uint64_t std_svc_smc_handler(uint32_t smc_fid, + uint64_t x1, + uint64_t x2, + uint64_t x3, + uint64_t x4, + void *cookie, + void *handle, + uint64_t flags) +{ + /* + * Dispatch PSCI calls to PSCI SMC handler and return its return + * value + */ + if (is_psci_fid(smc_fid)) { + return psci_smc_handler(smc_fid, x1, x2, x3, x4, cookie, + handle, flags); + } + + switch (smc_fid) { + case ARM_STD_SVC_CALL_COUNT: + /* + * Return the number of Standard Service Calls. PSCI is the only + * standard service implemented; so return number of PSCI calls + */ + SMC_RET1(handle, PSCI_NUM_CALLS); + + case ARM_STD_SVC_UID: + /* Return UID to the caller */ + SMC_UUID_RET(handle, arm_svc_uid); + + case ARM_STD_SVC_VERSION: + /* Return the version of current implementation */ + SMC_RET2(handle, STD_SVC_VERSION_MAJOR, STD_SVC_VERSION_MINOR); + + default: + WARN("Unimplemented Standard Service Call: 0x%x \n", smc_fid); + SMC_RET1(handle, SMC_UNK); + } +} + +/* Register Standard Service Calls as runtime service */ +DECLARE_RT_SVC( + std_svc, + + OEN_STD_START, + OEN_STD_END, + SMC_TYPE_FAST, + std_svc_setup, + std_svc_smc_handler +); |