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

diff --git a/include/bl31/runtime_svc.h b/include/bl31/runtime_svc.h
index 2d849863494607e4048a58133faef764eef5a949..f1124183501c2074b73cc6ba0d7da6ec196a9aec 100644 (file)
--- 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 de6fe4b8e86e77418c7af631ca8eeb6e98bac24b..59675810bc4c52d5181afa982aeee59d0ccd8e51 100644 (file)
--- 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 898a343df577dee120defa2bf271c4f4c9a4b2d4..a31643e4f343161fe1851fa7d854baf93a6a7614 100644 (file)
--- 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 af00551e8b21562dcac59b38eb3da54a67425054..0e10ac0502fede3fcad4688f53db72546f2a841e 100644 (file)
--- 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"
 
@@ -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 62477702cf50a36dd4ddaca7f6139fb7e6feaad4..548466569b6e7b77263e187f3896157ef6371af6 100644 (file)
--- 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 be504e819a85b651d2bf46d5a200b7bf5e6b9fbb..02a8786517791a252d8ccbbe2dae71444d265068 100644 (file)
--- 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;
 }