From a89eb89b385908df511190cef7e360cb4c3f12f6 Mon Sep 17 00:00:00 2001 From: Stefan Agner Date: Sun, 24 Jun 2018 21:09:56 +0200 Subject: [PATCH] imx: mx7: psci: provide complete PSCI 1.0 implementation 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 Signed-off-by: Stefan Agner --- 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 #include #include +#include #include #include @@ -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; +} -- 2.39.5