Move PSCI to runtime services directory
This patch creates a 'services' directory and moves the PSCI under
it. Other runtime services e.g. the Secure Payload Dispatcher service
will be placed under the same directory in the future.
Also fixes issue ARM-software/tf-issues#12
Change-Id: I187f83dcb660b728f82155d91882e961d2255068
diff --git a/services/psci/psci_afflvl_off.c b/services/psci/psci_afflvl_off.c
new file mode 100644
index 0000000..72557aa
--- /dev/null
+++ b/services/psci/psci_afflvl_off.c
@@ -0,0 +1,275 @@
+/*
+ * 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <debug.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <psci.h>
+#include <psci_private.h>
+
+typedef int (*afflvl_off_handler)(unsigned long, aff_map_node *);
+
+/*******************************************************************************
+ * 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 *cpu_node)
+{
+ unsigned int index, plat_state;
+ int rc = PSCI_E_SUCCESS;
+ unsigned long sctlr = read_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
+ */
+ 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 &= ~SCTLR_C_BIT;
+ write_sctlr(sctlr);
+
+ /*
+ * 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 *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 *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 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 mpidr_nodes,
+ int start_afflvl,
+ int end_afflvl,
+ unsigned long mpidr)
+{
+ int rc = PSCI_E_INVALID_PARAMS, level;
+ aff_map_node *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 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/psci/psci_afflvl_on.c b/services/psci/psci_afflvl_on.c
new file mode 100644
index 0000000..d22904c
--- /dev/null
+++ b/services/psci/psci_afflvl_on.c
@@ -0,0 +1,459 @@
+/*
+ * 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <debug.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <psci.h>
+#include <psci_private.h>
+#include <context_mgmt.h>
+
+typedef int (*afflvl_on_handler)(unsigned long,
+ aff_map_node *,
+ 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 *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 *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;
+
+ /*
+ * 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 *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 *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 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 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 *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 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 *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
+ */
+ 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();
+
+ /*
+ * 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 *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 *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 psci_afflvl_on_finishers[] = {
+ psci_afflvl0_on_finish,
+ psci_afflvl1_on_finish,
+ psci_afflvl2_on_finish,
+};
+
diff --git a/services/psci/psci_afflvl_suspend.c b/services/psci/psci_afflvl_suspend.c
new file mode 100644
index 0000000..4391580
--- /dev/null
+++ b/services/psci/psci_afflvl_suspend.c
@@ -0,0 +1,522 @@
+/*
+ * 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <debug.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <psci.h>
+#include <psci_private.h>
+#include <context_mgmt.h>
+
+typedef int (*afflvl_suspend_handler)(unsigned long,
+ aff_map_node *,
+ unsigned long,
+ unsigned long,
+ unsigned int);
+
+/*******************************************************************************
+ * This function sets the affinity level till which the current cpu is being
+ * powered down to during a cpu_suspend call
+ ******************************************************************************/
+void psci_set_suspend_afflvl(aff_map_node *node, int afflvl)
+{
+ /*
+ * 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);
+
+ /*
+ * Store the affinity level we are powering down to in our context.
+ * The cache flush in the suspend code will ensure that this info
+ * is available immediately upon resuming.
+ */
+ psci_suspend_context[node->data].suspend_level = afflvl;
+}
+
+/*******************************************************************************
+ * This function gets the affinity level till which the current cpu was powered
+ * down during a cpu_suspend call.
+ ******************************************************************************/
+int psci_get_suspend_afflvl(aff_map_node *node)
+{
+ /* Return the target affinity level */
+ return psci_suspend_context[node->data].suspend_level;
+}
+
+/*******************************************************************************
+ * 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 *cpu_node,
+ unsigned long ns_entrypoint,
+ unsigned long context_id,
+ unsigned int power_state)
+{
+ unsigned int index, plat_state;
+ unsigned long psci_entrypoint, sctlr = read_sctlr();
+ el3_state *saved_el3_state;
+ int rc = PSCI_E_SUCCESS;
+
+ /* Sanity check to safeguard against data corruption */
+ assert(cpu_node->level == MPIDR_AFFLVL0);
+
+ /* 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 &= ~SCTLR_C_BIT;
+ write_sctlr(sctlr);
+
+ /*
+ * 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 *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 *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 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 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 *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 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);
+
+
+ /* Save the affinity level till which this cpu can be powered down */
+ psci_set_suspend_afflvl(mpidr_nodes[MPIDR_AFFLVL0], end_afflvl);
+
+ /* 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 *cpu_node)
+{
+ unsigned int index, plat_state, state, rc = PSCI_E_SUCCESS;
+
+ 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;
+
+ /*
+ * 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 *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 *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 psci_afflvl_suspend_finishers[] = {
+ psci_afflvl0_suspend_finish,
+ psci_afflvl1_suspend_finish,
+ psci_afflvl2_suspend_finish,
+};
+
diff --git a/services/psci/psci_common.c b/services/psci/psci_common.c
new file mode 100644
index 0000000..214db78
--- /dev/null
+++ b/services/psci/psci_common.c
@@ -0,0 +1,552 @@
+/*
+ * 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <psci.h>
+#include <psci_private.h>
+#include <context_mgmt.h>
+#include <runtime_svc.h>
+#include "debug.h"
+
+/*******************************************************************************
+ * Arrays that contains information needs to resume a cpu's execution when woken
+ * out of suspend or off states. 'psci_ns_einfo_idx' keeps track of the next
+ * free index in the 'psci_ns_entry_info' & 'psci_suspend_context' arrays. Each
+ * cpu is allocated a single entry in each array during startup.
+ ******************************************************************************/
+suspend_context psci_suspend_context[PSCI_NUM_AFFS];
+ns_entry_info psci_ns_entry_info[PSCI_NUM_AFFS];
+unsigned int psci_ns_einfo_idx;
+
+/*******************************************************************************
+ * 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 psci_aff_map[PSCI_NUM_AFFS]
+__attribute__ ((section("tzfw_coherent_mem")));
+
+/*******************************************************************************
+ * 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.
+ ******************************************************************************/
+aff_limits_node psci_aff_limits[MPIDR_MAX_AFFLVL + 1];
+
+/*******************************************************************************
+ * Pointer to functions exported by the platform to complete power mgmt. ops
+ ******************************************************************************/
+plat_pm_ops *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 *node;
+ unsigned int state;
+
+ /* 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)
+ return psci_get_suspend_afflvl(node);
+
+ 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 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 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 *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 *ns_entry_context;
+ gp_regs *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 *) 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;
+
+ /* 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 = DAIF_DBG_BIT | DAIF_ABT_BIT;
+ spsr |= DAIF_IRQ_BIT | DAIF_FIQ_BIT;
+ spsr <<= PSR_DAIF_SHIFT;
+ spsr |= make_spsr(mode, MODE_SP_ELX, !rw);
+
+ psci_ns_entry_info[index].sctlr |= ee;
+ psci_ns_entry_info[index].scr |= SCR_RW_BIT;
+ } else {
+
+ /* Check whether aarch32 has to be entered in Thumb mode */
+ if (entrypoint & 0x1)
+ spsr = SPSR32_T_BIT;
+
+ if (el_status && (scr & SCR_HCE_BIT)) {
+ mode = AARCH32_MODE_HYP;
+ ee = read_sctlr_el2() & SCTLR_EE_BIT;
+ } else {
+ mode = AARCH32_MODE_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
+ */
+ spsr |= DAIF_ABT_BIT | DAIF_IRQ_BIT | DAIF_FIQ_BIT;
+ spsr <<= PSR_DAIF_SHIFT;
+ if (ee)
+ spsr |= SPSR32_EE_BIT;
+ spsr |= mode;
+
+ /* 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 *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 *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 *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 mpidr_nodes,
+ int start_afflvl,
+ int end_afflvl,
+ afflvl_power_on_finisher *pon_handlers,
+ unsigned long mpidr)
+{
+ int rc = PSCI_E_INVALID_PARAMS, level;
+ aff_map_node *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 *pon_handlers)
+{
+ mpidr_aff_map_nodes 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);
+}
diff --git a/services/psci/psci_entry.S b/services/psci/psci_entry.S
new file mode 100644
index 0000000..15e074c
--- /dev/null
+++ b/services/psci/psci_entry.S
@@ -0,0 +1,182 @@
+/*
+ * 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 <platform.h>
+#include <psci.h>
+#include <psci_private.h>
+#include <runtime_svc.h>
+#include <asm_macros.S>
+#include <cm_macros.S>
+
+ .globl psci_aff_on_finish_entry
+ .globl psci_aff_suspend_finish_entry
+ .globl __psci_cpu_off
+ .globl __psci_cpu_suspend
+
+ .section .text, "ax"; .align 3
+
+ /* -----------------------------------------------------
+ * 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.
+ * -----------------------------------------------------
+ */
+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
+
+ /* ---------------------------------------------
+ * Use SP_EL0 for the C runtime stack.
+ * ---------------------------------------------
+ */
+ msr spsel, #0
+ isb
+
+ bl read_mpidr
+ mov x19, x0
+ bl platform_set_coherent_stack
+
+ /* ---------------------------------------------
+ * Call the finishers starting from affinity
+ * level 0.
+ * ---------------------------------------------
+ */
+ mov x0, x19
+ bl get_power_on_target_afflvl
+ cmp x0, xzr
+ b.lt _panic
+ mov x3, x23
+ mov x2, x0
+ mov x0, x19
+ mov x1, #MPIDR_AFFLVL0
+ blr x22
+
+ /* --------------------------------------------
+ * Give ourselves a stack allocated in Normal
+ * -IS-WBWA memory
+ * --------------------------------------------
+ */
+ mov x0, x19
+ bl platform_set_stack
+
+ /* ---------------------------------------------
+ * Now that the context management has been set
+ * up, enable full runtime exception handling.
+ * SP_EL3 is pointing to a 'cpu_context'
+ * structure which has an exception stack
+ * allocated. Since we're just about to leave
+ * this EL with ERET, we don't need an ISB here
+ * ---------------------------------------------
+ */
+ adr x0, runtime_exceptions
+ msr vbar_el3, x0
+
+ zero_callee_saved_regs
+ 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
+ * -----------------------------------------------------
+ */
+__psci_cpu_off:
+ func_prologue
+ sub sp, sp, #0x10
+ stp x19, x20, [sp, #0]
+ mov x19, sp
+ bl read_mpidr
+ bl platform_set_coherent_stack
+ bl psci_cpu_off
+ mov x1, #PSCI_E_SUCCESS
+ cmp x0, x1
+ b.eq final_wfi
+ mov sp, x19
+ ldp x19, x20, [sp,#0]
+ add sp, sp, #0x10
+ func_epilogue
+ ret
+
+__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
+ bl read_mpidr
+ bl platform_set_coherent_stack
+ mov x0, x20
+ mov x1, x21
+ mov x2, x22
+ bl psci_cpu_suspend
+ mov x1, #PSCI_E_SUCCESS
+ cmp x0, x1
+ b.eq final_wfi
+ mov sp, x19
+ ldp x21, x22, [sp,#0x10]
+ ldp x19, x20, [sp,#0]
+ add sp, sp, #0x20
+ func_epilogue
+ ret
+
+final_wfi:
+ dsb sy
+ wfi
+wfi_spill:
+ b wfi_spill
+
diff --git a/services/psci/psci_main.c b/services/psci/psci_main.c
new file mode 100644
index 0000000..67f189d
--- /dev/null
+++ b/services/psci/psci_main.c
@@ -0,0 +1,271 @@
+/*
+ * 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <psci_private.h>
+#include <runtime_svc.h>
+#include <debug.h>
+#include <context_mgmt.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;
+
+ /* TODO: Standby states are not supported at the moment */
+ pstate_type = psci_get_pstate_type(power_state);
+ if (pstate_type == 0) {
+ rc = PSCI_E_INVALID_PARAMS;
+ goto exit;
+ }
+
+ /* Sanity check the requested state */
+ target_afflvl = psci_get_pstate_afflvl(power_state);
+ if (target_afflvl > MPIDR_MAX_AFFLVL) {
+ rc = PSCI_E_INVALID_PARAMS;
+ goto exit;
+ }
+
+ mpidr = read_mpidr();
+ rc = psci_afflvl_suspend(mpidr,
+ entrypoint,
+ context_id,
+ power_state,
+ MPIDR_AFFLVL0,
+ target_afflvl);
+
+exit:
+ if (rc != PSCI_E_SUCCESS)
+ 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);
+
+ /*
+ * The only error cpu_off can return is E_DENIED. So check if that's
+ * indeed the case.
+ */
+ assert (rc == PSCI_E_SUCCESS || 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 *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;
+}
+
+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;
+}
+
+/* Unimplemented */
+void psci_system_off(void)
+{
+ assert(0);
+}
+
+/* Unimplemented */
+void psci_system_reset(void)
+{
+ assert(0);
+}
+
+/*******************************************************************************
+ * 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;
+
+ case PSCI_SYSTEM_OFF:
+ psci_system_off();
+ assert(0);
+
+ case PSCI_SYSTEM_RESET:
+ psci_system_reset();
+ assert(0);
+
+ default:
+ rc = SMC_UNK;
+ WARN("Unimplemented psci call -> 0x%x \n", smc_fid);
+ }
+
+ SMC_RET1(handle, rc);
+}
diff --git a/services/psci/psci_private.h b/services/psci/psci_private.h
new file mode 100644
index 0000000..3d7ae74
--- /dev/null
+++ b/services/psci/psci_private.h
@@ -0,0 +1,166 @@
+/*
+ * 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>
+
+#ifndef __ASSEMBLY__
+/*******************************************************************************
+ * The following two data structures hold the generic information to bringup
+ * a suspended/hotplugged out cpu
+ ******************************************************************************/
+typedef struct {
+ unsigned long entrypoint;
+ unsigned long spsr;
+} eret_params;
+
+typedef struct {
+ eret_params eret_info;
+ unsigned long context_id;
+ unsigned int scr;
+ unsigned int sctlr;
+} ns_entry_info;
+
+/*******************************************************************************
+ * 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 {
+ unsigned long mpidr;
+ unsigned short ref_count;
+ unsigned char state;
+ unsigned char level;
+ unsigned int data;
+ bakery_lock lock;
+} aff_map_node;
+
+typedef struct {
+ int min;
+ int max;
+} aff_limits_node;
+
+/*******************************************************************************
+ * This data structure holds secure world context that needs to be preserved
+ * across cpu_suspend calls which enter the power down state.
+ ******************************************************************************/
+typedef struct {
+ /* Align the suspend level to allow per-cpu lockless access */
+ int suspend_level
+ __attribute__((__aligned__(CACHE_WRITEBACK_GRANULE)));
+} suspend_context;
+
+typedef aff_map_node (*mpidr_aff_map_nodes[MPIDR_MAX_AFFLVL]);
+typedef unsigned int (*afflvl_power_on_finisher)(unsigned long,
+ aff_map_node *);
+
+/*******************************************************************************
+ * Data prototypes
+ ******************************************************************************/
+extern suspend_context psci_suspend_context[PSCI_NUM_AFFS];
+extern ns_entry_info psci_ns_entry_info[PSCI_NUM_AFFS];
+extern unsigned int psci_ns_einfo_idx;
+extern aff_limits_node psci_aff_limits[MPIDR_MAX_AFFLVL + 1];
+extern plat_pm_ops *psci_plat_pm_ops;
+extern aff_map_node psci_aff_map[PSCI_NUM_AFFS];
+extern afflvl_power_on_finisher psci_afflvl_off_finish_handlers[];
+extern afflvl_power_on_finisher psci_afflvl_sus_finish_handlers[];
+
+/*******************************************************************************
+ * Function prototypes
+ ******************************************************************************/
+/* Private exported functions from psci_common.c */
+extern int get_max_afflvl(void);
+extern unsigned short psci_get_state(aff_map_node *node);
+extern unsigned short psci_get_phys_state(aff_map_node *node);
+extern void psci_set_state(aff_map_node *node, unsigned short state);
+extern void psci_get_ns_entry_info(unsigned int index);
+extern unsigned long mpidr_set_aff_inst(unsigned long, unsigned char, int);
+extern int psci_validate_mpidr(unsigned long, int);
+extern int get_power_on_target_afflvl(unsigned long mpidr);
+extern void psci_afflvl_power_on_finish(unsigned long,
+ int,
+ int,
+ afflvl_power_on_finisher *);
+extern int psci_set_ns_entry_info(unsigned int index,
+ unsigned long entrypoint,
+ unsigned long context_id);
+extern int psci_check_afflvl_range(int start_afflvl, int end_afflvl);
+extern void psci_acquire_afflvl_locks(unsigned long mpidr,
+ int start_afflvl,
+ int end_afflvl,
+ mpidr_aff_map_nodes mpidr_nodes);
+extern void psci_release_afflvl_locks(unsigned long mpidr,
+ int start_afflvl,
+ int end_afflvl,
+ mpidr_aff_map_nodes mpidr_nodes);
+
+/* Private exported functions from psci_setup.c */
+extern int psci_get_aff_map_nodes(unsigned long mpidr,
+ int start_afflvl,
+ int end_afflvl,
+ mpidr_aff_map_nodes mpidr_nodes);
+extern aff_map_node *psci_get_aff_map_node(unsigned long, int);
+
+/* Private exported functions from psci_affinity_on.c */
+extern int psci_afflvl_on(unsigned long,
+ unsigned long,
+ unsigned long,
+ int,
+ int);
+
+/* Private exported functions from psci_affinity_off.c */
+extern int psci_afflvl_off(unsigned long, int, int);
+
+/* Private exported functions from psci_affinity_suspend.c */
+extern void psci_set_suspend_afflvl(aff_map_node *node, int afflvl);
+extern int psci_get_suspend_afflvl(aff_map_node *node);
+extern int psci_afflvl_suspend(unsigned long,
+ unsigned long,
+ unsigned long,
+ unsigned int,
+ int,
+ int);
+extern unsigned int psci_afflvl_suspend_finish(unsigned long, int, int);
+
+/* Private exported functions from psci_main.c */
+extern 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);
+#endif /*__ASSEMBLY__*/
+
+#endif /* __PSCI_PRIVATE_H__ */
diff --git a/services/psci/psci_setup.c b/services/psci/psci_setup.c
new file mode 100644
index 0000000..8d7903c
--- /dev/null
+++ b/services/psci/psci_setup.c
@@ -0,0 +1,355 @@
+/*
+ * 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 <stdio.h>
+#include <string.h>
+#include <assert.h>
+#include <arch_helpers.h>
+#include <console.h>
+#include <platform.h>
+#include <psci_private.h>
+#include <context_mgmt.h>
+#include <runtime_svc.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 psci_ns_context[PLATFORM_CORE_COUNT];
+
+/*******************************************************************************
+ * 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 *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 mpidr_nodes)
+{
+ int rc = PSCI_E_INVALID_PARAMS, level;
+ aff_map_node *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;
+ 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);
+
+ /* Initialize exception stack in the context */
+ cm_init_exception_stack(mpidr, 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 *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;
+}
+
+/* Register PSCI as a run time service */
+DECLARE_RT_SVC(
+ psci,
+
+ OEN_STD_START,
+ OEN_STD_END,
+ SMC_TYPE_FAST,
+ psci_setup,
+ psci_smc_handler
+);