clk: tegra: dfll: Properly clean up on failure and removal

Upon failure to probe the DFLL, the OPP table will not be cleaned up
properly. Fix this and while at it make sure the OPP table will also be
cleared upon driver removal.

Signed-off-by: Thierry Reding <treding@nvidia.com>

diff --git a/drivers/clk/tegra/clk-dfll.h b/drivers/clk/tegra/clk-dfll.h
index d192982b1f96414980fdcb857b9851601a7fc9f5..ed2ad888268f39bd27d7ead71eb3875137f1d937 100644 (file)
--- a/drivers/clk/tegra/clk-dfll.h
+++ b/drivers/clk/tegra/clk-dfll.h
@@ -25,6 +25,7 @@
 /**
  * struct tegra_dfll_soc_data - SoC-specific hooks/integration for the DFLL driver
  * @dev: struct device * that holds the OPP table for the DFLL
+ * @max_freq: maximum frequency supported on this SoC
  * @cvb: CPU frequency table for this SoC
  * @init_clock_trimmers: callback to initialize clock trimmers
  * @set_clock_trimmers_high: callback to tune clock trimmers for high voltage
@@ -32,6 +33,7 @@
  */
 struct tegra_dfll_soc_data {
        struct device *dev;
+       unsigned long max_freq;
        const struct cvb_table *cvb;
 
        void (*init_clock_trimmers)(void);

diff --git a/drivers/clk/tegra/clk-tegra124-dfll-fcpu.c b/drivers/clk/tegra/clk-tegra124-dfll-fcpu.c
index c7ffd4fd2231030a77ec93a498bbd3a70edc412e..d052d9fa8230dcc3867861245fc399fc2fc87ec7 100644 (file)
--- a/drivers/clk/tegra/clk-tegra124-dfll-fcpu.c
+++ b/drivers/clk/tegra/clk-tegra124-dfll-fcpu.c
@@ -84,7 +84,7 @@ static const struct cvb_table tegra124_cpu_cvb_tables[] = {
 
 static int tegra124_dfll_fcpu_probe(struct platform_device *pdev)
 {
-       int process_id, speedo_id, speedo_value;
+       int process_id, speedo_id, speedo_value, err;
        struct tegra_dfll_soc_data *soc;
 
        process_id = tegra_sku_info.cpu_process_id;
@@ -107,18 +107,41 @@ static int tegra124_dfll_fcpu_probe(struct platform_device *pdev)
                return -ENODEV;
        }
 
+       soc->max_freq = cpu_max_freq_table[speedo_id];
+
        soc->cvb = tegra_cvb_add_opp_table(soc->dev, tegra124_cpu_cvb_tables,
                                           ARRAY_SIZE(tegra124_cpu_cvb_tables),
                                           process_id, speedo_id, speedo_value,
-                                          cpu_max_freq_table[speedo_id]);
+                                          soc->max_freq);
        if (IS_ERR(soc->cvb)) {
                dev_err(&pdev->dev, "couldn't add OPP table: %ld\n",
                        PTR_ERR(soc->cvb));
                return PTR_ERR(soc->cvb);
        }
 
+       err = tegra_dfll_register(pdev, soc);
+       if (err < 0) {
+               tegra_cvb_remove_opp_table(soc->dev, soc->cvb, soc->max_freq);
+               return err;
+       }
+
+       platform_set_drvdata(pdev, soc);
+
+       return 0;
+}
+
+static int tegra124_dfll_fcpu_remove(struct platform_device *pdev)
+{
+       struct tegra_dfll_soc_data *soc = platform_get_drvdata(pdev);
+       int err;
+
+       err = tegra_dfll_unregister(pdev);
+       if (err < 0)
+               dev_err(&pdev->dev, "failed to unregister DFLL: %d\n", err);
+
+       tegra_cvb_remove_opp_table(soc->dev, soc->cvb, soc->max_freq);
 
-       return tegra_dfll_register(pdev, soc);
+       return 0;
 }
 
 static const struct of_device_id tegra124_dfll_fcpu_of_match[] = {
@@ -134,7 +157,7 @@ static const struct dev_pm_ops tegra124_dfll_pm_ops = {
 
 static struct platform_driver tegra124_dfll_fcpu_driver = {
        .probe = tegra124_dfll_fcpu_probe,
-       .remove = tegra_dfll_unregister,
+       .remove = tegra124_dfll_fcpu_remove,
        .driver = {
                .name = "tegra124-dfll",
                .of_match_table = tegra124_dfll_fcpu_of_match,

diff --git a/drivers/clk/tegra/cvb.c b/drivers/clk/tegra/cvb.c
index 7a099b18c368b64c890bd2e84cb41ff63c9f6a78..624115e82ff92a6c982db5e65aa6e73c95732d5a 100644 (file)
--- a/drivers/clk/tegra/cvb.c
+++ b/drivers/clk/tegra/cvb.c
@@ -130,3 +130,19 @@ tegra_cvb_add_opp_table(struct device *dev, const struct cvb_table *tables,
 
        return ERR_PTR(-EINVAL);
 }
+
+void tegra_cvb_remove_opp_table(struct device *dev,
+                               const struct cvb_table *table,
+                               unsigned long max_freq)
+{
+       unsigned int i;
+
+       for (i = 0; i < MAX_DVFS_FREQS; i++) {
+               const struct cvb_table_freq_entry *entry = &table->entries[i];
+
+               if (!entry->freq || (entry->freq > max_freq))
+                       break;
+
+               dev_pm_opp_remove(dev, entry->freq);
+       }
+}

diff --git a/drivers/clk/tegra/cvb.h b/drivers/clk/tegra/cvb.h
index e6bf8581baddf1c4baebcbdbca4ea9d6dc35f1a4..c1f077993b2aacbea3a89f0c3557432e776a5115 100644 (file)
--- a/drivers/clk/tegra/cvb.h
+++ b/drivers/clk/tegra/cvb.h
@@ -61,5 +61,8 @@ const struct cvb_table *
 tegra_cvb_add_opp_table(struct device *dev, const struct cvb_table *cvb_tables,
                        size_t count, int process_id, int speedo_id,
                        int speedo_value, unsigned long max_freq);
+void tegra_cvb_remove_opp_table(struct device *dev,
+                               const struct cvb_table *table,
+                               unsigned long max_freq);
 
 #endif