4 files changed, 99 insertions, 1 deletions

diff --git a/arch/arm/include/asm/cpuidle.h b/arch/arm/include/asm/cpuidle.h
index 348dc817b9f3..0f8424924902 100644
--- a/arch/arm/include/asm/cpuidle.h
+++ b/arch/arm/include/asm/cpuidle.h
@@ -27,4 +27,25 @@ static inline int arm_cpuidle_simple_enter(struct cpuidle_device *dev,
  */
 #define ARM_CPUIDLE_WFI_STATE ARM_CPUIDLE_WFI_STATE_PWR(UINT_MAX)
 
+struct device_node;
+
+struct cpuidle_ops {
+        int (*suspend)(int cpu, unsigned long arg);
+        int (*init)(struct device_node *, int cpu);
+};
+
+struct of_cpuidle_method {
+        const char *method;
+        struct cpuidle_ops *ops;
+};
+
+#define CPUIDLE_METHOD_OF_DECLARE(name, _method, _ops)                  \
+        static const struct of_cpuidle_method __cpuidle_method_of_table_##name \
+        __used __section(__cpuidle_method_of_table)                     \
+        = { .method = _method, .ops = _ops }
+
+extern int arm_cpuidle_suspend(int index);
+
+extern int arm_cpuidle_init(int cpu);
+
 #endif
diff --git a/arch/arm/kernel/cpuidle.c b/arch/arm/kernel/cpuidle.c
index 45969f89f05c..2b0dae3cd058 100644
--- a/arch/arm/kernel/cpuidle.c
+++ b/arch/arm/kernel/cpuidle.c
@@ -10,8 +10,17 @@
  */
 
 #include <linux/cpuidle.h>
+#include <linux/of.h>
+#include <linux/of_device.h>
 #include <asm/cpuidle.h>
 
+extern struct of_cpuidle_method __cpuidle_method_of_table[];
+
+static const struct of_cpuidle_method __cpuidle_method_of_table_sentinel
+        __used __section(__cpuidle_method_of_table_end);
+
+static struct cpuidle_ops cpuidle_ops[NR_CPUS];
+
 int arm_cpuidle_simple_enter(struct cpuidle_device *dev,
                 struct cpuidle_driver *drv, int index)
 {
@@ -19,3 +28,66 @@ int arm_cpuidle_simple_enter(struct cpuidle_device *dev,
 
         return index;
 }
+
+int arm_cpuidle_suspend(int index)
+{
+        int ret = -EOPNOTSUPP;
+        int cpu = smp_processor_id();
+
+        if (cpuidle_ops[cpu].suspend)
+                ret = cpuidle_ops[cpu].suspend(cpu, index);
+
+        return ret;
+}
+
+static struct cpuidle_ops *__init arm_cpuidle_get_ops(const char *method)
+{
+        struct of_cpuidle_method *m = __cpuidle_method_of_table;
+
+        for (; m->method; m++)
+                if (!strcmp(m->method, method))
+                        return m->ops;
+
+        return NULL;
+}
+
+static int __init arm_cpuidle_read_ops(struct device_node *dn, int cpu)
+{
+        const char *enable_method;
+        struct cpuidle_ops *ops;
+
+        enable_method = of_get_property(dn, "enable-method", NULL);
+        if (!enable_method)
+                return -ENOENT;
+
+        ops = arm_cpuidle_get_ops(enable_method);
+        if (!ops) {
+                pr_warn("%s: unsupported enable-method property: %s\n",
+                        dn->full_name, enable_method);
+                return -EOPNOTSUPP;
+        }
+
+        cpuidle_ops[cpu] = *ops; /* structure copy */
+
+        pr_notice("cpuidle: enable-method property '%s'"
+                  " found operations\n", enable_method);
+
+        return 0;
+}
+
+int __init arm_cpuidle_init(int cpu)
+{
+        struct device_node *cpu_node = of_cpu_device_node_get(cpu);
+        int ret;
+
+        if (!cpu_node)
+                return -ENODEV;
+
+        ret = arm_cpuidle_read_ops(cpu_node, cpu);
+        if (!ret && cpuidle_ops[cpu].init)
+                ret = cpuidle_ops[cpu].init(cpu_node, cpu);
+
+        of_node_put(cpu_node);
+
+        return ret;
+}
diff --git a/arch/arm64/include/asm/cpuidle.h b/arch/arm64/include/asm/cpuidle.h
index c60643f14cda..460a38bb84b9 100644
--- a/arch/arm64/include/asm/cpuidle.h
+++ b/arch/arm64/include/asm/cpuidle.h
@@ -17,5 +17,8 @@ static inline int cpu_suspend(unsigned long arg)
         return -EOPNOTSUPP;
 }
 #endif
-
+static inline int arm_cpuidle_suspend(int index)
+{
+        return cpu_suspend(index);
+}
 #endif
diff --git a/include/asm-generic/vmlinux.lds.h b/include/asm-generic/vmlinux.lds.h
index ac78910d7416..91c09305106d 100644
--- a/include/asm-generic/vmlinux.lds.h
+++ b/include/asm-generic/vmlinux.lds.h
@@ -167,6 +167,7 @@
 #define IOMMU_OF_TABLES()       OF_TABLE(CONFIG_OF_IOMMU, iommu)
 #define RESERVEDMEM_OF_TABLES() OF_TABLE(CONFIG_OF_RESERVED_MEM, reservedmem)
 #define CPU_METHOD_OF_TABLES()  OF_TABLE(CONFIG_SMP, cpu_method)
+#define CPUIDLE_METHOD_OF_TABLES() OF_TABLE(CONFIG_CPU_IDLE, cpuidle_method)
 #define EARLYCON_OF_TABLES()    OF_TABLE(CONFIG_SERIAL_EARLYCON, earlycon)
 
 #define KERNEL_DTB()                                                    \
@@ -501,6 +502,7 @@
         CLKSRC_OF_TABLES()                                              \
         IOMMU_OF_TABLES()                                               \
         CPU_METHOD_OF_TABLES()                                          \
+        CPUIDLE_METHOD_OF_TABLES()                                      \
         KERNEL_DTB()                                                    \
         IRQCHIP_OF_MATCH_TABLE()                                        \
         EARLYCON_OF_TABLES()