[PATCH] cpufreq: imx6q: Disable only available frequencies

[Christoph Niedermaier]

In the example in Documentation/power/opp.rst, an availability check
is present before disabling a specific frequency. If a frequency isn't
available, the warning of a failed disabling of a non-existent
frequency is misleading. Therefore, check the availability of the
frequency in a separate inline function before disabling it.

Signed-off-by: Christoph Niedermaier <cniedermaier at dh-electronics.com>
---
Cc: Viresh Kumar <viresh.kumar at linaro.org>
Cc: "Rafael J. Wysocki" <rafael at kernel.org>
Cc: Shawn Guo <shawnguo at kernel.org>
Cc: Marek Vasut <marex at denx.de>
Cc: Fabio Estevam <festevam at denx.de>
Cc: NXP Linux Team <linux-imx at nxp.com>
Cc: linux-pm at vger.kernel.org (open list:CPU FREQUENCY SCALING FRAMEWORK)
Cc: linux-kernel at vger.kernel.org (open list)
To: linux-arm-kernel at lists.infradead.org
---
 drivers/cpufreq/imx6q-cpufreq.c | 27 +++++++++++++++++++++------
 1 file changed, 21 insertions(+), 6 deletions(-)

diff --git a/drivers/cpufreq/imx6q-cpufreq.c b/drivers/cpufreq/imx6q-cpufreq.c
index 48e1772e98fd..4e2d2bc47aba 100644
--- a/drivers/cpufreq/imx6q-cpufreq.c
+++ b/drivers/cpufreq/imx6q-cpufreq.c
@@ -209,6 +209,21 @@ static struct cpufreq_driver imx6q_cpufreq_driver = {
 	.suspend = cpufreq_generic_suspend,
 };
 
+static inline int disable_freq_if_available(struct device *dev,
+					    unsigned long freq)
+{
+	struct dev_pm_opp *opp;
+	int ret = 0;
+
+	opp = dev_pm_opp_find_freq_exact(dev, freq, true);
+	if (!IS_ERR(opp)) {
+		dev_pm_opp_put(opp);
+		ret = dev_pm_opp_disable(dev, freq);
+	}
+
+	return ret;
+}
+
 #define OCOTP_CFG3			0x440
 #define OCOTP_CFG3_SPEED_SHIFT		16
 #define OCOTP_CFG3_SPEED_1P2GHZ		0x3
@@ -254,16 +269,16 @@ static int imx6q_opp_check_speed_grading(struct device *dev)
 	val &= 0x3;
 
 	if (val < OCOTP_CFG3_SPEED_996MHZ)
-		if (dev_pm_opp_disable(dev, 996000000))
+		if (disable_freq_if_available(dev, 996000000))
 			dev_warn(dev, "failed to disable 996MHz OPP\n");
 
 	if (of_machine_is_compatible("fsl,imx6q") ||
 	    of_machine_is_compatible("fsl,imx6qp")) {
 		if (val != OCOTP_CFG3_SPEED_852MHZ)
-			if (dev_pm_opp_disable(dev, 852000000))
+			if (disable_freq_if_available(dev, 852000000))
 				dev_warn(dev, "failed to disable 852MHz OPP\n");
 		if (val != OCOTP_CFG3_SPEED_1P2GHZ)
-			if (dev_pm_opp_disable(dev, 1200000000))
+			if (disable_freq_if_available(dev, 1200000000))
 				dev_warn(dev, "failed to disable 1.2GHz OPP\n");
 	}
 
@@ -318,17 +333,17 @@ static int imx6ul_opp_check_speed_grading(struct device *dev)
 
 	if (of_machine_is_compatible("fsl,imx6ul")) {
 		if (val != OCOTP_CFG3_6UL_SPEED_696MHZ)
-			if (dev_pm_opp_disable(dev, 696000000))
+			if (disable_freq_if_available(dev, 696000000))
 				dev_warn(dev, "failed to disable 696MHz OPP\n");
 	}
 
 	if (of_machine_is_compatible("fsl,imx6ull")) {
 		if (val != OCOTP_CFG3_6ULL_SPEED_792MHZ)
-			if (dev_pm_opp_disable(dev, 792000000))
+			if (disable_freq_if_available(dev, 792000000))
 				dev_warn(dev, "failed to disable 792MHz OPP\n");
 
 		if (val != OCOTP_CFG3_6ULL_SPEED_900MHZ)
-			if (dev_pm_opp_disable(dev, 900000000))
+			if (disable_freq_if_available(dev, 900000000))
 				dev_warn(dev, "failed to disable 900MHz OPP\n");
 	}
 
-- 
2.11.0