firmware: psci: remove fiq enable after cpu_suspend

[firefly-linux-kernel-4.4.55.git] / drivers / firmware / psci.c

diff --git a/drivers/firmware/psci.c b/drivers/firmware/psci.c
index d24f35d74b27079afeae5c08d601c3bcd899dee6..28395eb9e05eef5efd4c5301e582676fdf78cddf 100644 (file)
--- a/drivers/firmware/psci.c
+++ b/drivers/firmware/psci.c
@@ -13,6 +13,8 @@
 
 #define pr_fmt(fmt) "psci: " fmt
 
+#include <linux/arm-smccc.h>
+#include <linux/cpuidle.h>
 #include <linux/errno.h>
 #include <linux/linkage.h>
 #include <linux/of.h>
@@ -20,10 +22,13 @@
 #include <linux/printk.h>
 #include <linux/psci.h>
 #include <linux/reboot.h>
+#include <linux/slab.h>
 #include <linux/suspend.h>
+#include <linux/regulator/machine.h>
 
 #include <uapi/linux/psci.h>
 
+#include <asm/cpuidle.h>
 #include <asm/cputype.h>
 #include <asm/system_misc.h>
 #include <asm/smp_plat.h>
@@ -58,8 +63,6 @@ struct psci_operations psci_ops;
 
 typedef unsigned long (psci_fn)(unsigned long, unsigned long,
                                unsigned long, unsigned long);
-asmlinkage psci_fn __invoke_psci_fn_hvc;
-asmlinkage psci_fn __invoke_psci_fn_smc;
 static psci_fn *invoke_psci_fn;
 
 enum psci_function {
@@ -107,6 +110,26 @@ bool psci_power_state_is_valid(u32 state)
        return !(state & ~valid_mask);
 }
 
+static unsigned long __invoke_psci_fn_hvc(unsigned long function_id,
+                       unsigned long arg0, unsigned long arg1,
+                       unsigned long arg2)
+{
+       struct arm_smccc_res res;
+
+       arm_smccc_hvc(function_id, arg0, arg1, arg2, 0, 0, 0, 0, &res);
+       return res.a0;
+}
+
+static unsigned long __invoke_psci_fn_smc(unsigned long function_id,
+                       unsigned long arg0, unsigned long arg1,
+                       unsigned long arg2)
+{
+       struct arm_smccc_res res;
+
+       arm_smccc_smc(function_id, arg0, arg1, arg2, 0, 0, 0, 0, &res);
+       return res.a0;
+}
+
 static int psci_to_linux_errno(int errno)
 {
        switch (errno) {
@@ -225,6 +248,123 @@ static int __init psci_features(u32 psci_func_id)
                              psci_func_id, 0, 0);
 }
 
+#ifdef CONFIG_CPU_IDLE
+static DEFINE_PER_CPU_READ_MOSTLY(u32 *, psci_power_state);
+
+static int psci_dt_cpu_init_idle(struct device_node *cpu_node, int cpu)
+{
+       int i, ret, count = 0;
+       u32 *psci_states;
+       struct device_node *state_node;
+
+       /*
+        * If the PSCI cpu_suspend function hook has not been initialized
+        * idle states must not be enabled, so bail out
+        */
+       if (!psci_ops.cpu_suspend)
+               return -EOPNOTSUPP;
+
+       /* Count idle states */
+       while ((state_node = of_parse_phandle(cpu_node, "cpu-idle-states",
+                                             count))) {
+               count++;
+               of_node_put(state_node);
+       }
+
+       if (!count)
+               return -ENODEV;
+
+       psci_states = kcalloc(count, sizeof(*psci_states), GFP_KERNEL);
+       if (!psci_states)
+               return -ENOMEM;
+
+       for (i = 0; i < count; i++) {
+               u32 state;
+
+               state_node = of_parse_phandle(cpu_node, "cpu-idle-states", i);
+
+               ret = of_property_read_u32(state_node,
+                                          "arm,psci-suspend-param",
+                                          &state);
+               if (ret) {
+                       pr_warn(" * %s missing arm,psci-suspend-param property\n",
+                               state_node->full_name);
+                       of_node_put(state_node);
+                       goto free_mem;
+               }
+
+               of_node_put(state_node);
+               pr_debug("psci-power-state %#x index %d\n", state, i);
+               if (!psci_power_state_is_valid(state)) {
+                       pr_warn("Invalid PSCI power state %#x\n", state);
+                       ret = -EINVAL;
+                       goto free_mem;
+               }
+               psci_states[i] = state;
+       }
+       /* Idle states parsed correctly, initialize per-cpu pointer */
+       per_cpu(psci_power_state, cpu) = psci_states;
+       return 0;
+
+free_mem:
+       kfree(psci_states);
+       return ret;
+}
+
+int psci_cpu_init_idle(unsigned int cpu)
+{
+       struct device_node *cpu_node;
+       int ret;
+
+       cpu_node = of_get_cpu_node(cpu, NULL);
+       if (!cpu_node)
+               return -ENODEV;
+
+       ret = psci_dt_cpu_init_idle(cpu_node, cpu);
+
+       of_node_put(cpu_node);
+
+       return ret;
+}
+
+static int psci_suspend_finisher(unsigned long index)
+{
+       u32 *state = __this_cpu_read(psci_power_state);
+
+       return psci_ops.cpu_suspend(state[index - 1],
+                                   virt_to_phys(cpu_resume));
+}
+
+int psci_cpu_suspend_enter(unsigned long index)
+{
+       int ret;
+       u32 *state = __this_cpu_read(psci_power_state);
+       /*
+        * idle state index 0 corresponds to wfi, should never be called
+        * from the cpu_suspend operations
+        */
+       if (WARN_ON_ONCE(!index))
+               return -EINVAL;
+
+       if (!psci_power_state_loses_context(state[index - 1]))
+               ret = psci_ops.cpu_suspend(state[index - 1], 0);
+       else
+               ret = cpu_suspend(index, psci_suspend_finisher);
+
+       return ret;
+}
+
+/* ARM specific CPU idle operations */
+#ifdef CONFIG_ARM
+static struct cpuidle_ops psci_cpuidle_ops __initdata = {
+       .suspend = psci_cpu_suspend_enter,
+       .init = psci_dt_cpu_init_idle,
+};
+
+CPUIDLE_METHOD_OF_DECLARE(psci, "arm,psci", &psci_cpuidle_ops);
+#endif
+#endif
+
 static int psci_system_suspend(unsigned long unused)
 {
        return invoke_psci_fn(PSCI_FN_NATIVE(1_0, SYSTEM_SUSPEND),
@@ -236,9 +376,25 @@ static int psci_system_suspend_enter(suspend_state_t state)
        return cpu_suspend(0, psci_system_suspend);
 }
 
+static int psci_system_suspend_prepare(void)
+{
+       return regulator_suspend_prepare(PM_SUSPEND_MEM);
+}
+
+static void psci_system_suspend_finish(void)
+{
+       int ret;
+
+       ret = regulator_suspend_finish();
+       if (ret < 0)
+               pr_warn("regulator_suspend_finish failed (%d)\n", ret);
+}
+
 static const struct platform_suspend_ops psci_suspend_ops = {
        .valid          = suspend_valid_only_mem,
        .enter          = psci_system_suspend_enter,
+       .prepare        = psci_system_suspend_prepare,
+       .finish         = psci_system_suspend_finish,
 };
 
 static void __init psci_init_system_suspend(void)