From b4b7ff4b08e691656c9d77c758fc355833128ac0 Mon Sep 17 00:00:00 2001 From: André Fabian Silva Delgado Date: Wed, 20 Jan 2016 14:01:31 -0300 Subject: Linux-libre 4.4-gnu --- drivers/firmware/psci.c | 108 +++++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 98 insertions(+), 10 deletions(-) (limited to 'drivers/firmware/psci.c') diff --git a/drivers/firmware/psci.c b/drivers/firmware/psci.c index 42700f09a..d24f35d74 100644 --- a/drivers/firmware/psci.c +++ b/drivers/firmware/psci.c @@ -20,23 +20,25 @@ #include #include #include +#include #include #include #include #include +#include /* * While a 64-bit OS can make calls with SMC32 calling conventions, for some - * calls it is necessary to use SMC64 to pass or return 64-bit values. For such - * calls PSCI_0_2_FN_NATIVE(x) will choose the appropriate (native-width) - * function ID. + * calls it is necessary to use SMC64 to pass or return 64-bit values. + * For such calls PSCI_FN_NATIVE(version, name) will choose the appropriate + * (native-width) function ID. */ #ifdef CONFIG_64BIT -#define PSCI_0_2_FN_NATIVE(name) PSCI_0_2_FN64_##name +#define PSCI_FN_NATIVE(version, name) PSCI_##version##_FN64_##name #else -#define PSCI_0_2_FN_NATIVE(name) PSCI_0_2_FN_##name +#define PSCI_FN_NATIVE(version, name) PSCI_##version##_FN_##name #endif /* @@ -70,6 +72,41 @@ enum psci_function { static u32 psci_function_id[PSCI_FN_MAX]; +#define PSCI_0_2_POWER_STATE_MASK \ + (PSCI_0_2_POWER_STATE_ID_MASK | \ + PSCI_0_2_POWER_STATE_TYPE_MASK | \ + PSCI_0_2_POWER_STATE_AFFL_MASK) + +#define PSCI_1_0_EXT_POWER_STATE_MASK \ + (PSCI_1_0_EXT_POWER_STATE_ID_MASK | \ + PSCI_1_0_EXT_POWER_STATE_TYPE_MASK) + +static u32 psci_cpu_suspend_feature; + +static inline bool psci_has_ext_power_state(void) +{ + return psci_cpu_suspend_feature & + PSCI_1_0_FEATURES_CPU_SUSPEND_PF_MASK; +} + +bool psci_power_state_loses_context(u32 state) +{ + const u32 mask = psci_has_ext_power_state() ? + PSCI_1_0_EXT_POWER_STATE_TYPE_MASK : + PSCI_0_2_POWER_STATE_TYPE_MASK; + + return state & mask; +} + +bool psci_power_state_is_valid(u32 state) +{ + const u32 valid_mask = psci_has_ext_power_state() ? + PSCI_1_0_EXT_POWER_STATE_MASK : + PSCI_0_2_POWER_STATE_MASK; + + return !(state & ~valid_mask); +} + static int psci_to_linux_errno(int errno) { switch (errno) { @@ -78,6 +115,7 @@ static int psci_to_linux_errno(int errno) case PSCI_RET_NOT_SUPPORTED: return -EOPNOTSUPP; case PSCI_RET_INVALID_PARAMS: + case PSCI_RET_INVALID_ADDRESS: return -EINVAL; case PSCI_RET_DENIED: return -EPERM; @@ -134,7 +172,7 @@ static int psci_migrate(unsigned long cpuid) static int psci_affinity_info(unsigned long target_affinity, unsigned long lowest_affinity_level) { - return invoke_psci_fn(PSCI_0_2_FN_NATIVE(AFFINITY_INFO), + return invoke_psci_fn(PSCI_FN_NATIVE(0_2, AFFINITY_INFO), target_affinity, lowest_affinity_level, 0); } @@ -145,7 +183,7 @@ static int psci_migrate_info_type(void) static unsigned long psci_migrate_info_up_cpu(void) { - return invoke_psci_fn(PSCI_0_2_FN_NATIVE(MIGRATE_INFO_UP_CPU), + return invoke_psci_fn(PSCI_FN_NATIVE(0_2, MIGRATE_INFO_UP_CPU), 0, 0, 0); } @@ -181,6 +219,49 @@ static void psci_sys_poweroff(void) invoke_psci_fn(PSCI_0_2_FN_SYSTEM_OFF, 0, 0, 0); } +static int __init psci_features(u32 psci_func_id) +{ + return invoke_psci_fn(PSCI_1_0_FN_PSCI_FEATURES, + psci_func_id, 0, 0); +} + +static int psci_system_suspend(unsigned long unused) +{ + return invoke_psci_fn(PSCI_FN_NATIVE(1_0, SYSTEM_SUSPEND), + virt_to_phys(cpu_resume), 0, 0); +} + +static int psci_system_suspend_enter(suspend_state_t state) +{ + return cpu_suspend(0, psci_system_suspend); +} + +static const struct platform_suspend_ops psci_suspend_ops = { + .valid = suspend_valid_only_mem, + .enter = psci_system_suspend_enter, +}; + +static void __init psci_init_system_suspend(void) +{ + int ret; + + if (!IS_ENABLED(CONFIG_SUSPEND)) + return; + + ret = psci_features(PSCI_FN_NATIVE(1_0, SYSTEM_SUSPEND)); + + if (ret != PSCI_RET_NOT_SUPPORTED) + suspend_set_ops(&psci_suspend_ops); +} + +static void __init psci_init_cpu_suspend(void) +{ + int feature = psci_features(psci_function_id[PSCI_FN_CPU_SUSPEND]); + + if (feature != PSCI_RET_NOT_SUPPORTED) + psci_cpu_suspend_feature = feature; +} + /* * Detect the presence of a resident Trusted OS which may cause CPU_OFF to * return DENIED (which would be fatal). @@ -224,16 +305,17 @@ static void __init psci_init_migrate(void) static void __init psci_0_2_set_functions(void) { pr_info("Using standard PSCI v0.2 function IDs\n"); - psci_function_id[PSCI_FN_CPU_SUSPEND] = PSCI_0_2_FN_NATIVE(CPU_SUSPEND); + psci_function_id[PSCI_FN_CPU_SUSPEND] = + PSCI_FN_NATIVE(0_2, CPU_SUSPEND); psci_ops.cpu_suspend = psci_cpu_suspend; psci_function_id[PSCI_FN_CPU_OFF] = PSCI_0_2_FN_CPU_OFF; psci_ops.cpu_off = psci_cpu_off; - psci_function_id[PSCI_FN_CPU_ON] = PSCI_0_2_FN_NATIVE(CPU_ON); + psci_function_id[PSCI_FN_CPU_ON] = PSCI_FN_NATIVE(0_2, CPU_ON); psci_ops.cpu_on = psci_cpu_on; - psci_function_id[PSCI_FN_MIGRATE] = PSCI_0_2_FN_NATIVE(MIGRATE); + psci_function_id[PSCI_FN_MIGRATE] = PSCI_FN_NATIVE(0_2, MIGRATE); psci_ops.migrate = psci_migrate; psci_ops.affinity_info = psci_affinity_info; @@ -265,6 +347,11 @@ static int __init psci_probe(void) psci_init_migrate(); + if (PSCI_VERSION_MAJOR(ver) >= 1) { + psci_init_cpu_suspend(); + psci_init_system_suspend(); + } + return 0; } @@ -340,6 +427,7 @@ out_put_node: static const struct of_device_id const psci_of_match[] __initconst = { { .compatible = "arm,psci", .data = psci_0_1_init}, { .compatible = "arm,psci-0.2", .data = psci_0_2_init}, + { .compatible = "arm,psci-1.0", .data = psci_0_2_init}, {}, }; -- cgit v1.2.3-54-g00ecf