From: Stefan Agner stefan.agner@toradex.com
PSCI 1.0 require PSCI_VERSION, PSCI_FEATURES, AFFINITY_INFO and CPU_SUSPEND to be implemented. Commit 0ec3d98f7692 ("mx7_common: use psci 1.0 instead of 0.1") marked the i.MX 7 implementation to be PSCI 1.0 compliant but failed to implement those functions. Especially the missing PSCI version callback was noticeable when booting Linux:
[ 0.000000] psci: probing for conduit method from DT. [ 0.000000] psci: PSCIv65535.65535 detected in firmware. [ 0.000000] psci: Using standard PSCI v0.2 function IDs [ 0.000000] psci: MIGRATE_INFO_TYPE not supported. [ 0.000000] psci: SMC Calling Convention v1.0
This patch provides a minimal implementation thereof. With this patch applied Linux detects PSCI 1.0:
[ 0.000000] psci: probing for conduit method from DT. [ 0.000000] psci: PSCIv1.0 detected in firmware. [ 0.000000] psci: Using standard PSCI v0.2 function IDs [ 0.000000] psci: MIGRATE_INFO_TYPE not supported. [ 0.000000] psci: SMC Calling Convention v1.0
Fixes: 0ec3d98f7692 ("mx7_common: use psci 1.0 instead of 0.1") Suggested-by: Mark Rutland mark.rutland@arm.com Signed-off-by: Stefan Agner stefan.agner@toradex.com ---
arch/arm/mach-imx/mx7/psci-mx7.c | 96 +++++++++++++++++++++++++++++++- 1 file changed, 93 insertions(+), 3 deletions(-)
diff --git a/arch/arm/mach-imx/mx7/psci-mx7.c b/arch/arm/mach-imx/mx7/psci-mx7.c index 95f8cb4def..8b839d37a9 100644 --- a/arch/arm/mach-imx/mx7/psci-mx7.c +++ b/arch/arm/mach-imx/mx7/psci-mx7.c @@ -8,6 +8,7 @@ #include <asm/psci.h> #include <asm/secure.h> #include <asm/arch/imx-regs.h> +#include <linux/bitops.h> #include <common.h> #include <fsl_wdog.h>
@@ -34,6 +35,24 @@ #define CCM_ROOT_WDOG 0xbb80 #define CCM_CCGR_WDOG1 0x49c0
+#define MPIDR_AFF0 GENMASK(7, 0) + +#define IMX7D_PSCI_NR_CPUS 2 +#if IMX7D_PSCI_NR_CPUS > CONFIG_ARMV7_PSCI_NR_CPUS +#error "invalid value for CONFIG_ARMV7_PSCI_NR_CPUS" +#endif + +u8 psci_state[IMX7D_PSCI_NR_CPUS] __secure_data = { + PSCI_AFFINITY_LEVEL_ON, + PSCI_AFFINITY_LEVEL_OFF}; + +static inline void psci_set_state(int cpu, u8 state) +{ + psci_state[cpu] = state; + dsb(); + isb(); +} + static inline void imx_gpcv2_set_m_core_pgc(bool enable, u32 offset) { writel(enable, GPC_IPS_BASE_ADDR + offset); @@ -67,25 +86,51 @@ __secure void imx_enable_cpu_ca7(int cpu, bool enable) writel(val, SRC_BASE_ADDR + SRC_A7RCR1); }
+__secure void psci_arch_cpu_entry(void) +{ + u32 cpu = psci_get_cpu_id(); + + psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON); +} + __secure s32 psci_cpu_on(u32 __always_unused function_id, u32 mpidr, u32 ep, u32 context_id) { - u32 cpu = (mpidr & 0x1); + u32 cpu = mpidr & MPIDR_AFF0; + + if (mpidr & ~MPIDR_AFF0) + return ARM_PSCI_RET_INVAL; + + if (cpu >= IMX7D_PSCI_NR_CPUS) + return ARM_PSCI_RET_INVAL; + + if (psci_state[cpu] == PSCI_AFFINITY_LEVEL_ON) + return ARM_PSCI_RET_ALREADY_ON; + + if (psci_state[cpu] == PSCI_AFFINITY_LEVEL_ON_PENDING) + return ARM_PSCI_RET_ON_PENDING;
psci_save(cpu, ep, context_id);
writel((u32)psci_cpu_entry, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D); + + psci_set_state(cpu, PSCI_AFFINITY_LEVEL_ON_PENDING); + imx_gpcv2_set_core1_power(true); imx_enable_cpu_ca7(cpu, true); - return 0; + + return ARM_PSCI_RET_SUCCESS; }
__secure s32 psci_cpu_off(void) { int cpu;
- psci_cpu_off_common(); cpu = psci_get_cpu_id(); + + psci_cpu_off_common(); + psci_set_state(cpu, PSCI_AFFINITY_LEVEL_OFF); + imx_enable_cpu_ca7(cpu, false); imx_gpcv2_set_core1_power(false); writel(0, SRC_BASE_ADDR + cpu * 8 + SRC_GPR1_MX7D + 4); @@ -121,3 +166,48 @@ __secure void psci_system_off(void) while (1) wfi(); } + +__secure u32 psci_version(void) +{ + return ARM_PSCI_VER_1_0; +} + +__secure s32 psci_cpu_suspend(u32 __always_unused function_id, u32 power_state, + u32 entry_point_address, + u32 context_id) +{ + return ARM_PSCI_RET_INVAL; +} + +__secure s32 psci_affinity_info(u32 __always_unused function_id, + u32 target_affinity, + u32 lowest_affinity_level) +{ + u32 cpu = target_affinity & MPIDR_AFF0; + + if (lowest_affinity_level > 0) + return ARM_PSCI_RET_INVAL; + + if (target_affinity & ~MPIDR_AFF0) + return ARM_PSCI_RET_INVAL; + + if (cpu >= IMX7D_PSCI_NR_CPUS) + return ARM_PSCI_RET_INVAL; + + return psci_state[cpu]; +} + +__secure s32 psci_features(u32 __always_unused function_id, u32 psci_fid) +{ + switch (psci_fid) { + case ARM_PSCI_0_2_FN_PSCI_VERSION: + case ARM_PSCI_0_2_FN_CPU_OFF: + case ARM_PSCI_0_2_FN_CPU_ON: + case ARM_PSCI_0_2_FN_AFFINITY_INFO: + case ARM_PSCI_0_2_FN_SYSTEM_OFF: + case ARM_PSCI_0_2_FN_SYSTEM_RESET: + case ARM_PSCI_1_0_FN_PSCI_FEATURES: + return 0x0; + } + return ARM_PSCI_RET_NI; +}