diff --git a/drivers/clk/tegra/clk-dfll.h b/drivers/clk/tegra/clk-dfll.h index d192982b1f96..ed2ad888268f 100644 --- 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 c7ffd4fd2231..d052d9fa8230 100644 --- 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; + } - return tegra_dfll_register(pdev, soc); + 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 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 7a099b18c368..624115e82ff9 100644 --- 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 e6bf8581badd..c1f077993b2a 100644 --- 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