Merge pull request #244 from danh-arm/sm/psci_mig_feat
Rework PSCI migrate APIs and implement the FEATURES API v2
diff --git a/include/bl31/runtime_svc.h b/include/bl31/runtime_svc.h
index 2d84986..f112418 100644
--- a/include/bl31/runtime_svc.h
+++ b/include/bl31/runtime_svc.h
@@ -176,6 +176,14 @@
#define SMC_SET_EL3(_h, _e, _v) \
write_ctx_reg(get_el3state_ctx(_h), (_e), (_v));
+/* The macro below is used to identify a Standard Service SMC call */
+#define is_std_svc_call(_fid) ((((_fid) >> FUNCID_OEN_SHIFT) & \
+ FUNCID_OEN_MASK) == OEN_STD_START)
+
+/* The macro below is used to identify a valid Fast SMC call */
+#define is_valid_fast_smc(_fid) ((!(((_fid) >> 16) & 0xff)) && \
+ (GET_SMC_TYPE(_fid) == SMC_TYPE_FAST))
+
/*
* Prototype for runtime service SMC handler function. x0 (SMC Function ID) to
* x4 are as passed by the caller. Rest of the arguments to SMC and the context
diff --git a/include/bl31/services/psci.h b/include/bl31/services/psci.h
index 3804bf2..5967581 100644
--- a/include/bl31/services/psci.h
+++ b/include/bl31/services/psci.h
@@ -61,11 +61,15 @@
#define PSCI_MIG_INFO_UP_CPU_AARCH64 0xc4000007
#define PSCI_SYSTEM_OFF 0x84000008
#define PSCI_SYSTEM_RESET 0x84000009
+#define PSCI_FEATURES 0x8400000A
+
+/* Macro to help build the psci capabilities bitfield */
+#define define_psci_cap(x) (1 << (x & 0x1f))
/*
* Number of PSCI calls (above) implemented
*/
-#define PSCI_NUM_CALLS 15
+#define PSCI_NUM_CALLS 16
/*******************************************************************************
* PSCI Migrate and friends
@@ -97,6 +101,18 @@
PSTATE_AFF_LVL_MASK)
/*******************************************************************************
+ * PSCI CPU_FEATURES feature flag specific defines
+ ******************************************************************************/
+/* Features flags for CPU SUSPEND power state parameter format. Bits [1:1] */
+#define FF_PSTATE_SHIFT 1
+#define FF_PSTATE_ORIG 0
+#define FF_PSTATE_EXTENDED 1
+
+/* Features flags for CPU SUSPEND OS Initiated mode support. Bits [0:0] */
+#define FF_MODE_SUPPORT_SHIFT 0
+#define FF_SUPPORTS_OS_INIT_MODE 1
+
+/*******************************************************************************
* PSCI version
******************************************************************************/
#define PSCI_MAJOR_VER (0 << 16)
@@ -191,8 +207,8 @@
void (*svc_suspend)(uint64_t __unused);
void (*svc_on_finish)(uint64_t __unused);
void (*svc_suspend_finish)(uint64_t suspend_level);
- void (*svc_migrate)(uint64_t __unused1, uint64_t __unused2);
- int32_t (*svc_migrate_info)(uint64_t *__unused);
+ int32_t (*svc_migrate)(uint64_t from_cpu, uint64_t to_cpu);
+ int32_t (*svc_migrate_info)(uint64_t *resident_cpu);
void (*svc_system_off)(void);
void (*svc_system_reset)(void);
} spd_pm_ops_t;
@@ -202,9 +218,9 @@
******************************************************************************/
unsigned int psci_version(void);
int psci_affinity_info(unsigned long, unsigned int);
-int psci_migrate(unsigned int);
-unsigned int psci_migrate_info_type(void);
-unsigned long psci_migrate_info_up_cpu(void);
+int psci_migrate(unsigned long);
+int psci_migrate_info_type(void);
+long psci_migrate_info_up_cpu(void);
int psci_cpu_on(unsigned long,
unsigned long,
unsigned long);
diff --git a/services/std_svc/psci/psci_afflvl_off.c b/services/std_svc/psci/psci_afflvl_off.c
index ceb51f8..7eb9688 100644
--- a/services/std_svc/psci/psci_afflvl_off.c
+++ b/services/std_svc/psci/psci_afflvl_off.c
@@ -51,8 +51,6 @@
*/
psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL0);
- assert(psci_plat_pm_ops->affinst_off);
-
/*
* Plat. management: Perform platform specific actions to turn this
* cpu off e.g. exit cpu coherency, program the power controller etc.
@@ -72,8 +70,6 @@
*/
psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL1);
- assert(psci_plat_pm_ops->affinst_off);
-
/*
* Plat. Management. Allow the platform to do its cluster
* specific bookeeping e.g. turn off interconnect coherency,
@@ -99,8 +95,6 @@
*/
psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL2);
- assert(psci_plat_pm_ops->affinst_off);
-
/*
* Plat. Management : Allow the platform to do its bookeeping
* at this affinity level
@@ -163,6 +157,12 @@
unsigned int max_phys_off_afflvl;
/*
+ * This function must only be called on platforms where the
+ * CPU_OFF platform hooks have been implemented.
+ */
+ assert(psci_plat_pm_ops->affinst_off);
+
+ /*
* 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
diff --git a/services/std_svc/psci/psci_afflvl_on.c b/services/std_svc/psci/psci_afflvl_on.c
index ad212b6..0ee03cb 100644
--- a/services/std_svc/psci/psci_afflvl_on.c
+++ b/services/std_svc/psci/psci_afflvl_on.c
@@ -75,8 +75,6 @@
/* Set the secure world (EL3) re-entry point after BL1 */
psci_entrypoint = (unsigned long) psci_aff_on_finish_entry;
- assert(psci_plat_pm_ops->affinst_on);
-
/*
* Plat. management: Give the platform the current state
* of the target cpu to allow it to perform the necessary
@@ -107,8 +105,6 @@
/* State management: Is not required while turning a cluster on */
- assert(psci_plat_pm_ops->affinst_on);
-
/*
* Plat. management: Give the platform the current state
* of the target cpu to allow it to perform the necessary
@@ -141,8 +137,6 @@
/* State management: Is not required while turning a system on */
- assert(psci_plat_pm_ops->affinst_on);
-
/*
* Plat. management: Give the platform the current state
* of the target cpu to allow it to perform the necessary
@@ -219,6 +213,13 @@
mpidr_aff_map_nodes_t target_cpu_nodes;
/*
+ * This function must only be called on platforms where the
+ * CPU_ON platform hooks have been implemented.
+ */
+ assert(psci_plat_pm_ops->affinst_on &&
+ psci_plat_pm_ops->affinst_on_finish);
+
+ /*
* 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
@@ -313,7 +314,6 @@
* register. The actual state of this cpu has already been
* changed.
*/
- assert(psci_plat_pm_ops->affinst_on_finish);
/* Get the physical state of this cpu */
plat_state = get_phys_state(state);
@@ -357,8 +357,6 @@
assert(cluster_node->level == MPIDR_AFFLVL1);
- assert(psci_plat_pm_ops->affinst_on_finish);
-
/*
* Plat. management: Perform the platform specific actions
* as per the old state of the cluster e.g. enabling
@@ -380,8 +378,6 @@
/* Cannot go beyond this affinity level */
assert(system_node->level == MPIDR_AFFLVL2);
- assert(psci_plat_pm_ops->affinst_on_finish);
-
/*
* Currently, there are no architectural actions to perform
* at the system level.
diff --git a/services/std_svc/psci/psci_afflvl_suspend.c b/services/std_svc/psci/psci_afflvl_suspend.c
index 9ede65d..dad0cef 100644
--- a/services/std_svc/psci/psci_afflvl_suspend.c
+++ b/services/std_svc/psci/psci_afflvl_suspend.c
@@ -119,8 +119,6 @@
*/
psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL0);
- assert(psci_plat_pm_ops->affinst_suspend);
-
/*
* Plat. management: Allow the platform to perform the
* necessary actions to turn off this cpu e.g. set the
@@ -146,8 +144,6 @@
*/
psci_do_pwrdown_cache_maintenance(MPIDR_AFFLVL1);
- assert(psci_plat_pm_ops->affinst_suspend);
-
/*
* Plat. Management. Allow the platform to do its cluster specific
* bookeeping e.g. turn off interconnect coherency, program the power
@@ -188,7 +184,6 @@
* Plat. Management : Allow the platform to do its bookeeping
* at this affinity level
*/
- assert(psci_plat_pm_ops->affinst_suspend);
/*
* Sending the psci entrypoint is currently redundant
@@ -262,6 +257,13 @@
unsigned int max_phys_off_afflvl;
/*
+ * This function must only be called on platforms where the
+ * CPU_SUSPEND platform hooks have been implemented.
+ */
+ assert(psci_plat_pm_ops->affinst_suspend &&
+ psci_plat_pm_ops->affinst_suspend_finish);
+
+ /*
* 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
@@ -370,8 +372,6 @@
* situation.
*/
- assert(psci_plat_pm_ops->affinst_suspend_finish);
-
/* Get the physical state of this cpu */
plat_state = get_phys_state(state);
psci_plat_pm_ops->affinst_suspend_finish(cpu_node->level,
@@ -428,8 +428,6 @@
* situation.
*/
- assert(psci_plat_pm_ops->affinst_suspend_finish);
-
/* Get the physical state of this cpu */
plat_state = psci_get_phys_state(cluster_node);
psci_plat_pm_ops->affinst_suspend_finish(cluster_node->level,
@@ -458,8 +456,6 @@
* situation.
*/
- assert(psci_plat_pm_ops->affinst_suspend_finish);
-
/* Get the physical state of the system */
plat_state = psci_get_phys_state(system_node);
psci_plat_pm_ops->affinst_suspend_finish(system_node->level,
diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c
index d8c8618..a31643e 100644
--- a/services/std_svc/psci/psci_common.c
+++ b/services/std_svc/psci/psci_common.c
@@ -558,9 +558,40 @@
******************************************************************************/
void psci_register_spd_pm_hook(const spd_pm_ops_t *pm)
{
+ assert(pm);
psci_spd_pm = pm;
+
+ if (pm->svc_migrate)
+ psci_caps |= define_psci_cap(PSCI_MIG_AARCH64);
+
+ if (pm->svc_migrate_info)
+ psci_caps |= define_psci_cap(PSCI_MIG_INFO_UP_CPU_AARCH64)
+ | define_psci_cap(PSCI_MIG_INFO_TYPE);
+}
+
+/*******************************************************************************
+ * This function invokes the migrate info hook in the spd_pm_ops. It performs
+ * the necessary return value validation. If the Secure Payload is UP and
+ * migrate capable, it returns the mpidr of the CPU on which the Secure payload
+ * is resident through the mpidr parameter. Else the value of the parameter on
+ * return is undefined.
+ ******************************************************************************/
+int psci_spd_migrate_info(uint64_t *mpidr)
+{
+ int rc;
+
+ if (!psci_spd_pm || !psci_spd_pm->svc_migrate_info)
+ return PSCI_E_NOT_SUPPORTED;
+
+ rc = psci_spd_pm->svc_migrate_info(mpidr);
+
+ assert(rc == PSCI_TOS_UP_MIG_CAP || rc == PSCI_TOS_NOT_UP_MIG_CAP \
+ || rc == PSCI_TOS_NOT_PRESENT_MP || rc == PSCI_E_NOT_SUPPORTED);
+
+ return rc;
}
+
/*******************************************************************************
* This function prints the state of all affinity instances present in the
* system
diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c
index 91d16f4..d8a0009 100644
--- a/services/std_svc/psci/psci_main.c
+++ b/services/std_svc/psci/psci_main.c
@@ -32,6 +32,7 @@
#include <arch_helpers.h>
#include <assert.h>
#include <runtime_svc.h>
+#include <std_svc.h>
#include <debug.h>
#include "psci_private.h"
@@ -219,24 +220,89 @@
return rc;
}
-/* Unimplemented */
-int psci_migrate(unsigned int target_cpu)
+int psci_migrate(unsigned long target_cpu)
{
- return PSCI_E_NOT_SUPPORTED;
+ int rc;
+ unsigned long resident_cpu_mpidr;
+
+ rc = psci_spd_migrate_info(&resident_cpu_mpidr);
+ if (rc != PSCI_TOS_UP_MIG_CAP)
+ return (rc == PSCI_TOS_NOT_UP_MIG_CAP) ?
+ PSCI_E_DENIED : PSCI_E_NOT_SUPPORTED;
+
+ /*
+ * Migrate should only be invoked on the CPU where
+ * the Secure OS is resident.
+ */
+ if (resident_cpu_mpidr != read_mpidr_el1())
+ return PSCI_E_NOT_PRESENT;
+
+ /* Check the validity of the specified target cpu */
+ rc = psci_validate_mpidr(target_cpu, MPIDR_AFFLVL0);
+ if (rc != PSCI_E_SUCCESS)
+ return PSCI_E_INVALID_PARAMS;
+
+ assert(psci_spd_pm && psci_spd_pm->svc_migrate);
+
+ rc = psci_spd_pm->svc_migrate(read_mpidr_el1(), target_cpu);
+ assert(rc == PSCI_E_SUCCESS || rc == PSCI_E_INTERN_FAIL);
+
+ return rc;
}
-/* Unimplemented */
-unsigned int psci_migrate_info_type(void)
+int psci_migrate_info_type(void)
{
- return PSCI_TOS_NOT_PRESENT_MP;
+ unsigned long resident_cpu_mpidr;
+
+ return psci_spd_migrate_info(&resident_cpu_mpidr);
}
-unsigned long psci_migrate_info_up_cpu(void)
+long psci_migrate_info_up_cpu(void)
{
+ unsigned long resident_cpu_mpidr;
+ int rc;
+
/*
- * Return value of this currently unsupported call depends upon
- * what psci_migrate_info_type() returns.
+ * Return value of this depends upon what
+ * psci_spd_migrate_info() returns.
*/
+ rc = psci_spd_migrate_info(&resident_cpu_mpidr);
+ if (rc != PSCI_TOS_NOT_UP_MIG_CAP && rc != PSCI_TOS_UP_MIG_CAP)
+ return PSCI_E_INVALID_PARAMS;
+
+ return resident_cpu_mpidr;
+}
+
+int psci_features(unsigned int psci_fid)
+{
+ uint32_t local_caps = psci_caps;
+
+ /* Check if it is a 64 bit function */
+ if (((psci_fid >> FUNCID_CC_SHIFT) & FUNCID_CC_MASK) == SMC_64)
+ local_caps &= PSCI_CAP_64BIT_MASK;
+
+ /* Check for invalid fid */
+ if (!(is_std_svc_call(psci_fid) && is_valid_fast_smc(psci_fid)
+ && is_psci_fid(psci_fid)))
+ return PSCI_E_NOT_SUPPORTED;
+
+
+ /* Check if the psci fid is supported or not */
+ if (!(local_caps & define_psci_cap(psci_fid)))
+ return PSCI_E_NOT_SUPPORTED;
+
+ /* Format the feature flags */
+ if (psci_fid == PSCI_CPU_SUSPEND_AARCH32 ||
+ psci_fid == PSCI_CPU_SUSPEND_AARCH64) {
+ /*
+ * The trusted firmware uses the original power state format
+ * and does not support OS Initiated Mode.
+ */
+ return (FF_PSTATE_ORIG << FF_PSTATE_SHIFT) |
+ ((!FF_SUPPORTS_OS_INIT_MODE) << FF_MODE_SUPPORT_SHIFT);
+ }
+
+ /* Return 0 for all other fid's */
return PSCI_E_SUCCESS;
}
@@ -255,6 +321,10 @@
if (is_caller_secure(flags))
SMC_RET1(handle, SMC_UNK);
+ /* Check the fid against the capabilities */
+ if (!(psci_caps & define_psci_cap(smc_fid)))
+ SMC_RET1(handle, SMC_UNK);
+
if (((smc_fid >> FUNCID_CC_SHIFT) & FUNCID_CC_MASK) == SMC_32) {
/* 32-bit PSCI function, clear top parameter bits */
@@ -295,6 +365,9 @@
psci_system_reset();
/* We should never return from psci_system_reset() */
+ case PSCI_FEATURES:
+ SMC_RET1(handle, psci_features(x1));
+
default:
break;
}
diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h
index 4fc8721..5484665 100644
--- a/services/std_svc/psci/psci_private.h
+++ b/services/std_svc/psci/psci_private.h
@@ -52,6 +52,26 @@
CPU_DATA_PSCI_LOCK_OFFSET)
#endif
+/*
+ * The PSCI capability which are provided by the generic code but does not
+ * depend on the platform or spd capabilities.
+ */
+#define PSCI_GENERIC_CAP \
+ (define_psci_cap(PSCI_VERSION) | \
+ define_psci_cap(PSCI_AFFINITY_INFO_AARCH64) | \
+ define_psci_cap(PSCI_FEATURES))
+
+/*
+ * The PSCI capabilities mask for 64 bit functions.
+ */
+#define PSCI_CAP_64BIT_MASK \
+ (define_psci_cap(PSCI_CPU_SUSPEND_AARCH64) | \
+ define_psci_cap(PSCI_CPU_ON_AARCH64) | \
+ define_psci_cap(PSCI_AFFINITY_INFO_AARCH64) | \
+ define_psci_cap(PSCI_MIG_AARCH64) | \
+ define_psci_cap(PSCI_MIG_INFO_UP_CPU_AARCH64))
+
+
/*******************************************************************************
* 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.
@@ -82,6 +102,7 @@
******************************************************************************/
extern const plat_pm_ops_t *psci_plat_pm_ops;
extern aff_map_node_t psci_aff_map[PSCI_NUM_AFFS];
+extern uint32_t psci_caps;
/*******************************************************************************
* SPD's power management hooks registered with PSCI
@@ -120,6 +141,7 @@
uint32_t psci_find_max_phys_off_afflvl(uint32_t start_afflvl,
uint32_t end_afflvl,
aff_map_node_t *mpidr_nodes[]);
+int psci_spd_migrate_info(uint64_t *mpidr);
/* Private exported functions from psci_setup.c */
int psci_get_aff_map_nodes(unsigned long mpidr,
diff --git a/services/std_svc/psci/psci_setup.c b/services/std_svc/psci/psci_setup.c
index be504e8..02a8786 100644
--- a/services/std_svc/psci/psci_setup.c
+++ b/services/std_svc/psci/psci_setup.c
@@ -57,6 +57,12 @@
******************************************************************************/
static aff_limits_node_t psci_aff_limits[MPIDR_MAX_AFFLVL + 1];
+/******************************************************************************
+ * Define the psci capability variable.
+ *****************************************************************************/
+uint32_t psci_caps;
+
+
/*******************************************************************************
* 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
@@ -372,5 +378,19 @@
platform_setup_pm(&psci_plat_pm_ops);
assert(psci_plat_pm_ops);
+ /* Initialize the psci capability */
+ psci_caps = PSCI_GENERIC_CAP;
+
+ if (psci_plat_pm_ops->affinst_off)
+ psci_caps |= define_psci_cap(PSCI_CPU_OFF);
+ if (psci_plat_pm_ops->affinst_on && psci_plat_pm_ops->affinst_on_finish)
+ psci_caps |= define_psci_cap(PSCI_CPU_ON_AARCH64);
+ if (psci_plat_pm_ops->affinst_suspend && psci_plat_pm_ops->affinst_suspend_finish)
+ psci_caps |= define_psci_cap(PSCI_CPU_SUSPEND_AARCH64);
+ if (psci_plat_pm_ops->system_off)
+ psci_caps |= define_psci_cap(PSCI_SYSTEM_OFF);
+ if (psci_plat_pm_ops->system_reset)
+ psci_caps |= define_psci_cap(PSCI_SYSTEM_RESET);
+
return 0;
}
diff --git a/services/std_svc/psci/psci_system_off.c b/services/std_svc/psci/psci_system_off.c
index f2520b6..970d4bb 100644
--- a/services/std_svc/psci/psci_system_off.c
+++ b/services/std_svc/psci/psci_system_off.c
@@ -30,20 +30,17 @@
#include <stddef.h>
#include <arch_helpers.h>
+#include <assert.h>
#include <debug.h>
#include <platform.h>
#include "psci_private.h"
void psci_system_off(void)
{
- /* Check platform support */
- if (!psci_plat_pm_ops->system_off) {
- ERROR("Platform has not exported a PSCI System Off hook.\n");
- panic();
- }
-
psci_print_affinity_map();
+ assert(psci_plat_pm_ops->system_off);
+
/* Notify the Secure Payload Dispatcher */
if (psci_spd_pm && psci_spd_pm->svc_system_off) {
psci_spd_pm->svc_system_off();
@@ -57,14 +54,10 @@
void psci_system_reset(void)
{
- /* Check platform support */
- if (!psci_plat_pm_ops->system_reset) {
- ERROR("Platform has not exported a PSCI System Reset hook.\n");
- panic();
- }
-
psci_print_affinity_map();
+ assert(psci_plat_pm_ops->system_reset);
+
/* Notify the Secure Payload Dispatcher */
if (psci_spd_pm && psci_spd_pm->svc_system_reset) {
psci_spd_pm->svc_system_reset();