From 8991eed7439cb565da505a2bf88e9ac87ad79c1c Mon Sep 17 00:00:00 2001 From: Soby Mathew Date: Thu, 23 Oct 2014 10:35:34 +0100 Subject: [PATCH 1/3] Rework the PSCI migrate APIs This patch reworks the PSCI MIGRATE, MIGRATE_INFO_TYPE and MIGRATE_INFO_UP_CPU support for Trusted Firmware. The implementation does the appropriate validation of parameters and invokes the appropriate hook exported by the SPD. The TSP is a MP Trusted OS. Hence the ability to actually migrate a Trusted OS has not been implemented. The corresponding function is not populated in the spd_pm_hooks structure for the TSPD. The `spd_pm_ops_t` has undergone changes with this patch. SPD PORTS MAY NEED TO BE UPDATED. Fixes ARM-software/tf-issues#249 Change-Id: Iabd87521bf7c530a5e4506b6d3bfd4f1bf87604f --- include/bl31/services/psci.h | 10 ++--- services/std_svc/psci/psci_common.c | 23 +++++++++++ services/std_svc/psci/psci_main.c | 60 +++++++++++++++++++++------- services/std_svc/psci/psci_private.h | 1 + 4 files changed, 75 insertions(+), 19 deletions(-) diff --git a/include/bl31/services/psci.h b/include/bl31/services/psci.h index 3804bf2c4f..de6fe4b8e8 100644 --- a/include/bl31/services/psci.h +++ b/include/bl31/services/psci.h @@ -191,8 +191,8 @@ typedef struct spd_pm_ops { 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 +202,9 @@ typedef struct spd_pm_ops { ******************************************************************************/ 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_common.c b/services/std_svc/psci/psci_common.c index d8c8618ff5..898a343df5 100644 --- a/services/std_svc/psci/psci_common.c +++ b/services/std_svc/psci/psci_common.c @@ -561,6 +561,29 @@ void psci_register_spd_pm_hook(const spd_pm_ops_t *pm) psci_spd_pm = pm; } +/******************************************************************************* + * 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 91d16f46a9..af00551e8b 100644 --- a/services/std_svc/psci/psci_main.c +++ b/services/std_svc/psci/psci_main.c @@ -219,25 +219,57 @@ int psci_affinity_info(unsigned long target_affinity, 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; -/* Unimplemented */ -unsigned int psci_migrate_info_type(void) -{ - return PSCI_TOS_NOT_PRESENT_MP; -} + 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; -unsigned long psci_migrate_info_up_cpu(void) -{ /* - * Return value of this currently unsupported call depends upon - * what psci_migrate_info_type() returns. + * Migrate should only be invoked on the CPU where + * the Secure OS is resident. */ - return PSCI_E_SUCCESS; + 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; +} + +int psci_migrate_info_type(void) +{ + unsigned long resident_cpu_mpidr; + + return psci_spd_migrate_info(&resident_cpu_mpidr); +} + +long psci_migrate_info_up_cpu(void) +{ + unsigned long resident_cpu_mpidr; + int rc; + + /* + * 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; } /******************************************************************************* diff --git a/services/std_svc/psci/psci_private.h b/services/std_svc/psci/psci_private.h index 4fc87217f9..62477702cf 100644 --- a/services/std_svc/psci/psci_private.h +++ b/services/std_svc/psci/psci_private.h @@ -120,6 +120,7 @@ void psci_set_max_phys_off_afflvl(uint32_t afflvl); 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, From 90e8258eec95bcad556426597489a34208232e39 Mon Sep 17 00:00:00 2001 From: Soby Mathew Date: Wed, 7 Jan 2015 11:10:22 +0000 Subject: [PATCH 2/3] Implement PSCI_FEATURES API This patch implements the PSCI_FEATURES function which is a mandatory API in the PSCI 1.0 specification. A capability variable is constructed during initialization by examining the plat_pm_ops and spd_pm_ops exported by the platform and the Secure Payload Dispatcher. This is used by the PSCI FEATURES function to determine which PSCI APIs are supported by the platform. Change-Id: I147ffc1bd5d90b469bd3cc4bbe0a20e95c247df7 --- include/bl31/runtime_svc.h | 8 ++++++ include/bl31/services/psci.h | 18 +++++++++++++- services/std_svc/psci/psci_common.c | 8 ++++++ services/std_svc/psci/psci_main.c | 37 ++++++++++++++++++++++++++++ services/std_svc/psci/psci_private.h | 21 ++++++++++++++++ services/std_svc/psci/psci_setup.c | 20 +++++++++++++++ 6 files changed, 111 insertions(+), 1 deletion(-) diff --git a/include/bl31/runtime_svc.h b/include/bl31/runtime_svc.h index 2d84986349..f112418350 100644 --- a/include/bl31/runtime_svc.h +++ b/include/bl31/runtime_svc.h @@ -176,6 +176,14 @@ typedef int32_t (*rt_svc_init_t)(void); #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 de6fe4b8e8..59675810bc 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 @@ -96,6 +100,18 @@ #define psci_get_pstate_afflvl(pstate) ((pstate >> PSTATE_AFF_LVL_SHIFT) & \ 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 ******************************************************************************/ diff --git a/services/std_svc/psci/psci_common.c b/services/std_svc/psci/psci_common.c index 898a343df5..a31643e4f3 100644 --- a/services/std_svc/psci/psci_common.c +++ b/services/std_svc/psci/psci_common.c @@ -558,7 +558,15 @@ void psci_afflvl_power_on_finish(int start_afflvl, ******************************************************************************/ 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); } /******************************************************************************* diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c index af00551e8b..0e10ac0502 100644 --- a/services/std_svc/psci/psci_main.c +++ b/services/std_svc/psci/psci_main.c @@ -32,6 +32,7 @@ #include #include #include +#include #include #include "psci_private.h" @@ -272,6 +273,39 @@ long psci_migrate_info_up_cpu(void) 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; +} + /******************************************************************************* * PSCI top level handler for servicing SMCs. ******************************************************************************/ @@ -327,6 +361,9 @@ uint64_t psci_smc_handler(uint32_t smc_fid, 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 62477702cf..548466569b 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 @@ typedef void (*afflvl_power_on_finisher_t)(aff_map_node_t *); ******************************************************************************/ 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 diff --git a/services/std_svc/psci/psci_setup.c b/services/std_svc/psci/psci_setup.c index be504e819a..02a8786517 100644 --- a/services/std_svc/psci/psci_setup.c +++ b/services/std_svc/psci/psci_setup.c @@ -57,6 +57,12 @@ static cpu_context_t psci_ns_context[PLATFORM_CORE_COUNT]; ******************************************************************************/ 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 @@ int32_t psci_setup(void) 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; } From b234b2c4a06169aa965b77dd40c17be454a9f609 Mon Sep 17 00:00:00 2001 From: Soby Mathew Date: Thu, 15 Jan 2015 11:49:49 +0000 Subject: [PATCH 3/3] Verify capabilities before handling PSCI calls This patch implements conditional checks in psci_smc_handler() to verify that the psci function invoked by the caller is supported by the platform or SPD implementation. The level of support is saved in the 'psci_caps' variable. This check allows the PSCI implementation to return an error early. As a result of the above verification, the checks performed within the psci handlers for the pm hooks are now removed and replaced with assertions. Change-Id: I9b5b646a01d8566dc28c4d77dd3aa54e9bf3981a --- services/std_svc/psci/psci_afflvl_off.c | 12 ++++++------ services/std_svc/psci/psci_afflvl_on.c | 18 +++++++----------- services/std_svc/psci/psci_afflvl_suspend.c | 18 +++++++----------- services/std_svc/psci/psci_main.c | 4 ++++ services/std_svc/psci/psci_system_off.c | 17 +++++------------ 5 files changed, 29 insertions(+), 40 deletions(-) diff --git a/services/std_svc/psci/psci_afflvl_off.c b/services/std_svc/psci/psci_afflvl_off.c index ceb51f83e9..7eb968899e 100644 --- a/services/std_svc/psci/psci_afflvl_off.c +++ b/services/std_svc/psci/psci_afflvl_off.c @@ -51,8 +51,6 @@ static void psci_afflvl0_off(aff_map_node_t *cpu_node) */ 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 @@ static void psci_afflvl1_off(aff_map_node_t *cluster_node) */ 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 @@ static void psci_afflvl2_off(aff_map_node_t *system_node) */ 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 @@ -162,6 +156,12 @@ int psci_afflvl_off(int start_afflvl, mpidr_aff_map_nodes_t mpidr_nodes; 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 diff --git a/services/std_svc/psci/psci_afflvl_on.c b/services/std_svc/psci/psci_afflvl_on.c index ad212b6521..0ee03cb5e1 100644 --- a/services/std_svc/psci/psci_afflvl_on.c +++ b/services/std_svc/psci/psci_afflvl_on.c @@ -75,8 +75,6 @@ static int psci_afflvl0_on(unsigned long target_cpu, /* 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 @@ static int psci_afflvl1_on(unsigned long target_cpu, /* 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 @@ static int psci_afflvl2_on(unsigned long target_cpu, /* 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 @@ -218,6 +212,13 @@ int psci_afflvl_on(unsigned long target_cpu, int rc; 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 @@ -313,7 +314,6 @@ static void psci_afflvl0_on_finish(aff_map_node_t *cpu_node) * 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 @@ static void psci_afflvl1_on_finish(aff_map_node_t *cluster_node) 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 @@ static void psci_afflvl2_on_finish(aff_map_node_t *system_node) /* 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 9ede65d015..dad0cefd86 100644 --- a/services/std_svc/psci/psci_afflvl_suspend.c +++ b/services/std_svc/psci/psci_afflvl_suspend.c @@ -119,8 +119,6 @@ static void psci_afflvl0_suspend(aff_map_node_t *cpu_node) */ 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 @@ static void psci_afflvl1_suspend(aff_map_node_t *cluster_node) */ 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 @@ static void psci_afflvl2_suspend(aff_map_node_t *system_node) * 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 @@ -261,6 +256,13 @@ void psci_afflvl_suspend(entry_point_info_t *ep, mpidr_aff_map_nodes_t mpidr_nodes; 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 @@ -370,8 +372,6 @@ static void psci_afflvl0_suspend_finish(aff_map_node_t *cpu_node) * 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 @@ static void psci_afflvl1_suspend_finish(aff_map_node_t *cluster_node) * 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 @@ static void psci_afflvl2_suspend_finish(aff_map_node_t *system_node) * 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_main.c b/services/std_svc/psci/psci_main.c index 0e10ac0502..d8a00097ba 100644 --- a/services/std_svc/psci/psci_main.c +++ b/services/std_svc/psci/psci_main.c @@ -321,6 +321,10 @@ uint64_t psci_smc_handler(uint32_t smc_fid, 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 */ diff --git a/services/std_svc/psci/psci_system_off.c b/services/std_svc/psci/psci_system_off.c index f2520b6dda..970d4bb501 100644 --- a/services/std_svc/psci/psci_system_off.c +++ b/services/std_svc/psci/psci_system_off.c @@ -30,20 +30,17 @@ #include #include +#include #include #include #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_off(void) 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();