1 files changed, 41 insertions, 32 deletions

diff --git a/arch/arm/mach-imx/cpuidle-imx6q.c b/arch/arm/mach-imx/cpuidle-imx6q.c
index 23ddfb693b2d..2b0d3160f993 100644
--- a/arch/arm/mach-imx/cpuidle-imx6q.c
+++ b/arch/arm/mach-imx/cpuidle-imx6q.c
@@ -1,43 +1,38 @@
+// SPDX-License-Identifier: GPL-2.0-only
 /*
  * Copyright (C) 2012 Freescale Semiconductor, Inc.
- *
- * 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.
  */
 
+#include <linux/context_tracking.h>
 #include <linux/cpuidle.h>
 #include <linux/module.h>
 #include <asm/cpuidle.h>
-#include <asm/proc-fns.h>
+
+#include <soc/imx/cpuidle.h>
 
 #include "common.h"
 #include "cpuidle.h"
+#include "hardware.h"
 
-static atomic_t master = ATOMIC_INIT(0);
-static DEFINE_SPINLOCK(master_lock);
+static int num_idle_cpus = 0;
+static DEFINE_RAW_SPINLOCK(cpuidle_lock);
 
-static int imx6q_enter_wait(struct cpuidle_device *dev,
-			    struct cpuidle_driver *drv, int index)
+static __cpuidle int imx6q_enter_wait(struct cpuidle_device *dev,
+				      struct cpuidle_driver *drv, int index)
 {
-	if (atomic_inc_return(&master) == num_online_cpus()) {
-		/*
-		 * With this lock, we prevent other cpu to exit and enter
-		 * this function again and become the master.
-		 */
-		if (!spin_trylock(&master_lock))
-			goto idle;
-		imx6q_set_lpm(WAIT_UNCLOCKED);
-		cpu_do_idle();
-		imx6q_set_lpm(WAIT_CLOCKED);
-		spin_unlock(&master_lock);
-		goto done;
-	}
+	raw_spin_lock(&cpuidle_lock);
+	if (++num_idle_cpus == num_online_cpus())
+		imx6_set_lpm(WAIT_UNCLOCKED);
+	raw_spin_unlock(&cpuidle_lock);
 
-idle:
+	ct_cpuidle_enter();
 	cpu_do_idle();
-done:
-	atomic_dec(&master);
+	ct_cpuidle_exit();
+
+	raw_spin_lock(&cpuidle_lock);
+	if (num_idle_cpus-- == num_online_cpus())
+		imx6_set_lpm(WAIT_CLOCKED);
+	raw_spin_unlock(&cpuidle_lock);
 
 	return index;
 }
@@ -52,8 +47,7 @@ static struct cpuidle_driver imx6q_cpuidle_driver = {
 		{
 			.exit_latency = 50,
 			.target_residency = 75,
-			.flags = CPUIDLE_FLAG_TIME_VALID |
-			         CPUIDLE_FLAG_TIMER_STOP,
+			.flags = CPUIDLE_FLAG_TIMER_STOP | CPUIDLE_FLAG_RCU_IDLE,
 			.enter = imx6q_enter_wait,
 			.name = "WAIT",
 			.desc = "Clock off",
@@ -63,13 +57,28 @@ static struct cpuidle_driver imx6q_cpuidle_driver = {
 	.safe_state_index = 0,
 };
 
-int __init imx6q_cpuidle_init(void)
+/*
+ * i.MX6 Q/DL has an erratum (ERR006687) that prevents the FEC from waking the
+ * CPUs when they are in wait(unclocked) state. As the hardware workaround isn't
+ * applicable to all boards, disable the deeper idle state when the workaround
+ * isn't present and the FEC is in use.
+ */
+void imx6q_cpuidle_fec_irqs_used(void)
 {
-	/* Need to enable SCU standby for entering WAIT modes */
-	imx_scu_standby_enable();
+	cpuidle_driver_state_disabled(&imx6q_cpuidle_driver, 1, true);
+}
+EXPORT_SYMBOL_GPL(imx6q_cpuidle_fec_irqs_used);
 
-	/* Set chicken bit to get a reliable WAIT mode support */
-	imx6q_set_chicken_bit();
+void imx6q_cpuidle_fec_irqs_unused(void)
+{
+	cpuidle_driver_state_disabled(&imx6q_cpuidle_driver, 1, false);
+}
+EXPORT_SYMBOL_GPL(imx6q_cpuidle_fec_irqs_unused);
+
+int __init imx6q_cpuidle_init(void)
+{
+	/* Set INT_MEM_CLK_LPM bit to get a reliable WAIT mode support */
+	imx6_set_int_mem_clk_lpm(true);
 
 	return cpuidle_register(&imx6q_cpuidle_driver, NULL);
 }