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 <mark.rutland@arm.com>
Signed-off-by: Stefan Agner <stefan.agner@toradex.com>

diff --git a/arch/arm/mach-imx/mx7/psci-mx7.c b/arch/arm/mach-imx/mx7/psci-mx7.c
index 95f8cb4deface2b285b3a2b5cd4fddd10c6df06c..8b839d37a9cb887693ca5e373dcf454d88dfb809 100644 (file)
--- 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;
+}