Merge pull request #244 from danh-arm/sm/psci_mig_feat

Rework PSCI migrate APIs and implement the FEATURES API v2

1 files changed, 82 insertions, 9 deletions

diff --git a/services/std_svc/psci/psci_main.c b/services/std_svc/psci/psci_main.c
index 91d16f46..d8a00097 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 @@ 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;
+
+	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 @@ 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 */
 
@@ -295,6 +365,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;
 		}