[PATCH V8 10/14] thermal: tegra: handle clocks in one function

[Wei Ni]

Handle clock enable/disable codes in one funcion
soctherm_clk_enable(), so that the codes are more clear.

Signed-off-by: Wei Ni <w...@nvidia.com>
---
 drivers/thermal/tegra/soctherm.c | 51 ++++++++++++++++++++++++++++------------
 1 file changed, 36 insertions(+), 15 deletions(-)

diff --git a/drivers/thermal/tegra/soctherm.c b/drivers/thermal/tegra/soctherm.c
index 67e4a094b430..f60106884ac4 100644
--- a/drivers/thermal/tegra/soctherm.c
+++ b/drivers/thermal/tegra/soctherm.c
@@ -428,6 +428,39 @@ static void soctherm_debug_init(struct platform_device 
*pdev)
 static inline void soctherm_debug_init(struct platform_device *pdev) {}
 #endif
 
+static int soctherm_clk_enable(struct platform_device *pdev, bool enable)
+{
+       struct tegra_soctherm *tegra = platform_get_drvdata(pdev);
+       int err;
+
+       if (tegra->clock_soctherm == NULL || tegra->clock_tsensor == NULL)
+               return -EINVAL;
+
+       reset_control_assert(tegra->reset);
+
+       if (enable) {
+               err = clk_prepare_enable(tegra->clock_soctherm);
+               if (err) {
+                       reset_control_deassert(tegra->reset);
+                       return err;
+               }
+
+               err = clk_prepare_enable(tegra->clock_tsensor);
+               if (err) {
+                       clk_disable_unprepare(tegra->clock_soctherm);
+                       reset_control_deassert(tegra->reset);
+                       return err;
+               }
+       } else {
+               clk_disable_unprepare(tegra->clock_tsensor);
+               clk_disable_unprepare(tegra->clock_soctherm);
+       }
+
+       reset_control_deassert(tegra->reset);
+
+       return 0;
+}
+
 static const struct of_device_id tegra_soctherm_of_match[] = {
 #ifdef CONFIG_ARCH_TEGRA_124_SOC
        {
@@ -496,20 +529,10 @@ static int tegra_soctherm_probe(struct platform_device 
*pdev)
                return PTR_ERR(tegra->clock_soctherm);
        }
 
-       reset_control_assert(tegra->reset);
-
-       err = clk_prepare_enable(tegra->clock_soctherm);
+       err = soctherm_clk_enable(pdev, true);
        if (err)
                return err;
 
-       err = clk_prepare_enable(tegra->clock_tsensor);
-       if (err) {
-               clk_disable_unprepare(tegra->clock_soctherm);
-               return err;
-       }
-
-       reset_control_deassert(tegra->reset);
-
        /* Initialize raw sensors */
 
        tegra->calib = devm_kzalloc(&pdev->dev,
@@ -579,8 +602,7 @@ static int tegra_soctherm_probe(struct platform_device 
*pdev)
        return 0;
 
 disable_clocks:
-       clk_disable_unprepare(tegra->clock_tsensor);
-       clk_disable_unprepare(tegra->clock_soctherm);
+       soctherm_clk_enable(pdev, false);
 
        return err;
 }
@@ -591,8 +613,7 @@ static int tegra_soctherm_remove(struct platform_device 
*pdev)
 
        debugfs_remove_recursive(tegra->debugfs_dir);
 
-       clk_disable_unprepare(tegra->clock_tsensor);
-       clk_disable_unprepare(tegra->clock_soctherm);
+       soctherm_clk_enable(pdev, false);
 
        return 0;
 }
-- 
1.9.1