1 files changed, 76 insertions, 72 deletions

diff --git a/drivers/cpuidle/cpuidle-arm.c b/drivers/cpuidle/cpuidle-arm.c
index 7080c384ad5d..e044fefdb816 100644
--- a/drivers/cpuidle/cpuidle-arm.c
+++ b/drivers/cpuidle/cpuidle-arm.c
@@ -1,16 +1,14 @@
+// SPDX-License-Identifier: GPL-2.0-only
 /*
  * ARM/ARM64 generic CPU idle driver.
  *
  * Copyright (C) 2014 ARM Ltd.
  * Author: Lorenzo Pieralisi <lorenzo.pieralisi@arm.com>
- *
- * This program is free software; you can redistribute it and/or modify
- * it under the terms of the GNU General Public License version 2 as
- * published by the Free Software Foundation.
  */
 
 #define pr_fmt(fmt) "CPUidle arm: " fmt
 
+#include <linux/cpu_cooling.h>
 #include <linux/cpuidle.h>
 #include <linux/cpumask.h>
 #include <linux/cpu_pm.h>
@@ -18,7 +16,6 @@
 #include <linux/module.h>
 #include <linux/of.h>
 #include <linux/slab.h>
-#include <linux/topology.h>
 
 #include <asm/cpuidle.h>
 
@@ -34,8 +31,8 @@
  * Called from the CPUidle framework to program the device to the
  * specified target state selected by the governor.
  */
-static int arm_enter_idle_state(struct cpuidle_device *dev,
-				struct cpuidle_driver *drv, int idx)
+static __cpuidle int arm_enter_idle_state(struct cpuidle_device *dev,
+					  struct cpuidle_driver *drv, int idx)
 {
 	/*
 	 * Pass idle state index to arm_cpuidle_suspend which in turn
@@ -72,90 +69,97 @@ static const struct of_device_id arm_idle_state_match[] __initconst = {
 };
 
 /*
- * arm_idle_init
+ * arm_idle_init_cpu
  *
  * Registers the arm specific cpuidle driver with the cpuidle
  * framework. It relies on core code to parse the idle states
  * and initialize them using driver data structures accordingly.
  */
-static int __init arm_idle_init(void)
+static int __init arm_idle_init_cpu(int cpu)
 {
-	int cpu, ret;
+	int ret;
 	struct cpuidle_driver *drv;
-	struct cpuidle_device *dev;
 
-	for_each_possible_cpu(cpu) {
+	drv = kmemdup(&arm_idle_driver, sizeof(*drv), GFP_KERNEL);
+	if (!drv)
+		return -ENOMEM;
 
-		drv = kmemdup(&arm_idle_driver, sizeof(*drv), GFP_KERNEL);
-		if (!drv) {
-			ret = -ENOMEM;
-			goto out_fail;
-		}
-
-		drv->cpumask = (struct cpumask *)cpumask_of(cpu);
-
-		/*
-		 * Initialize idle states data, starting at index 1.  This
-		 * driver is DT only, if no DT idle states are detected (ret
-		 * == 0) let the driver initialization fail accordingly since
-		 * there is no reason to initialize the idle driver if only
-		 * wfi is supported.
-		 */
-		ret = dt_init_idle_driver(drv, arm_idle_state_match, 1);
-		if (ret <= 0) {
-			ret = ret ? : -ENODEV;
-			goto out_fail;
-		}
+	drv->cpumask = (struct cpumask *)cpumask_of(cpu);
 
-		ret = cpuidle_register_driver(drv);
-		if (ret) {
-			pr_err("Failed to register cpuidle driver\n");
-			goto out_fail;
-		}
-
-		/*
-		 * Call arch CPU operations in order to initialize
-		 * idle states suspend back-end specific data
-		 */
-		ret = arm_cpuidle_init(cpu);
-
-		/*
-		 * Skip the cpuidle device initialization if the reported
-		 * failure is a HW misconfiguration/breakage (-ENXIO).
-		 */
-		if (ret == -ENXIO)
-			continue;
-
-		if (ret) {
+	/*
+	 * Initialize idle states data, starting at index 1.  This
+	 * driver is DT only, if no DT idle states are detected (ret
+	 * == 0) let the driver initialization fail accordingly since
+	 * there is no reason to initialize the idle driver if only
+	 * wfi is supported.
+	 */
+	ret = dt_init_idle_driver(drv, arm_idle_state_match, 1);
+	if (ret <= 0) {
+		ret = ret ? : -ENODEV;
+		goto out_kfree_drv;
+	}
+
+	/*
+	 * Call arch CPU operations in order to initialize
+	 * idle states suspend back-end specific data
+	 */
+	ret = arm_cpuidle_init(cpu);
+
+	/*
+	 * Allow the initialization to continue for other CPUs, if the
+	 * reported failure is a HW misconfiguration/breakage (-ENXIO).
+	 *
+	 * Some platforms do not support idle operations
+	 * (arm_cpuidle_init() returning -EOPNOTSUPP), we should
+	 * not flag this case as an error, it is a valid
+	 * configuration.
+	 */
+	if (ret) {
+		if (ret != -EOPNOTSUPP)
 			pr_err("CPU %d failed to init idle CPU ops\n", cpu);
-			goto out_fail;
-		}
+		ret = ret == -ENXIO ? 0 : ret;
+		goto out_kfree_drv;
+	}
 
-		dev = kzalloc(sizeof(*dev), GFP_KERNEL);
-		if (!dev) {
-			pr_err("Failed to allocate cpuidle device\n");
-			ret = -ENOMEM;
-			goto out_fail;
-		}
-		dev->cpu = cpu;
-
-		ret = cpuidle_register_device(dev);
-		if (ret) {
-			pr_err("Failed to register cpuidle device for CPU %d\n",
-			       cpu);
-			kfree(dev);
+	ret = cpuidle_register(drv, NULL);
+	if (ret)
+		goto out_kfree_drv;
+
+	cpuidle_cooling_register(drv);
+
+	return 0;
+
+out_kfree_drv:
+	kfree(drv);
+	return ret;
+}
+
+/*
+ * arm_idle_init - Initializes arm cpuidle driver
+ *
+ * Initializes arm cpuidle driver for all present CPUs, if any
+ * CPU fails to register cpuidle driver then rollback to cancel
+ * all CPUs registration.
+ */
+static int __init arm_idle_init(void)
+{
+	int cpu, ret;
+	struct cpuidle_driver *drv;
+	struct cpuidle_device *dev;
+
+	for_each_present_cpu(cpu) {
+		ret = arm_idle_init_cpu(cpu);
+		if (ret)
 			goto out_fail;
-		}
 	}
 
 	return 0;
+
 out_fail:
 	while (--cpu >= 0) {
 		dev = per_cpu(cpuidle_devices, cpu);
-		cpuidle_unregister_device(dev);
-		kfree(dev);
-		drv = cpuidle_get_driver();
-		cpuidle_unregister_driver(drv);
+		drv = cpuidle_get_cpu_driver(dev);
+		cpuidle_unregister(drv);
 		kfree(drv);
 	}