Implement attach_dev() callback of power domain drivers that initializes
the domain's performance state. GENPD core will apply the performance
state on the first runtime PM resume of the attached device.

Signed-off-by: Dmitry Osipenko <digetx@gmail.com>
---
 drivers/soc/tegra/pmc.c | 147 ++++++++++++++++++++++++++++++++++++++++
 1 file changed, 147 insertions(+)

diff --git a/drivers/soc/tegra/pmc.c b/drivers/soc/tegra/pmc.c
index f63dfb2ca3f9..ebafb818b08e 100644
--- a/drivers/soc/tegra/pmc.c
+++ b/drivers/soc/tegra/pmc.c

@@ -506,6 +506,151 @@ static void tegra_pmc_scratch_writel(struct tegra_pmc *pmc, u32 value,
                writel(value, pmc->scratch + offset);
 }

+static const char * const tegra_emc_compats[] = {
+       "nvidia,tegra20-emc",
+       "nvidia,tegra30-emc",
+       NULL,
+};
+
+/*
+ * This GENPD callback is used by both powergate and core domains.
+ *
+ * We retrieve clock rate of the attached device and initialize domain's
+ * performance state in accordance to the clock rate.
+ */
+static int tegra_pmc_pd_attach_dev(struct generic_pm_domain *genpd,
+                                  struct device *dev)
+{
+       struct generic_pm_domain_data *gpd_data = dev_gpd_data(dev);
+       struct opp_table *opp_table, *pd_opp_table;
+       struct generic_pm_domain *core_genpd;
+       struct dev_pm_opp *opp, *pd_opp;
+       unsigned long rate, state;
+       struct gpd_link *link;
+       struct clk *clk;
+       u32 hw_version;
+       int ret;
+
+       /*
+        * Tegra114+ SocS don't support OPP yet.  But if they will get OPP
+        * support, then we want to skip OPP for older kernels to preserve
+        * compatibility of newer DTBs with older kernels.
+        */
+       if (!pmc->soc->supports_core_domain)
+               return 0;
+
+       /*
+        * The EMC devices are a special case because we have a protection
+        * from non-EMC drivers getting clock handle before EMC driver is
+        * fully initialized.  The goal of the protection is to prevent
+        * devfreq driver from getting failures if it will try to change
+        * EMC clock rate until clock is fully initialized.  The EMC drivers
+        * will initialize the performance state by themselves.
+        */
+       if (of_device_compatible_match(dev->of_node, tegra_emc_compats))
+               return 0;
+
+       clk = clk_get(dev, NULL);
+       if (IS_ERR(clk)) {
+               dev_err(&genpd->dev, "failed to get clk of %s: %pe\n",
+                       dev_name(dev), clk);
+               return PTR_ERR(clk);
+       }
+
+       rate = clk_get_rate(clk);
+       if (!rate) {
+               dev_err(&genpd->dev, "failed to get clk rate of %s\n",
+                       dev_name(dev));
+               ret = -EINVAL;
+               goto put_clk;
+       }
+
+       if (of_machine_is_compatible("nvidia,tegra20"))
+               hw_version = BIT(tegra_sku_info.soc_process_id);
+       else
+               hw_version = BIT(tegra_sku_info.soc_speedo_id);
+
+       opp_table = dev_pm_opp_set_supported_hw(dev, &hw_version, 1);
+       if (IS_ERR(opp_table)) {
+               dev_err(&genpd->dev, "failed to set OPP supported HW for %s: %d\n",
+                       dev_name(dev), ret);
+               ret = PTR_ERR(opp_table);
+               goto put_clk;
+       }
+
+       ret = dev_pm_opp_of_add_table(dev);
+       if (ret) {
+               /* older DTBs that don't have OPPs will get -ENODEV here */
+               if (ret != -ENODEV)
+                       dev_err(&genpd->dev, "failed to get OPP table of %s: %d\n",
+                               dev_name(dev), ret);
+               else
+                       ret = 0;
+
+               goto put_supported_hw;
+       }
+
+       /* find suitable OPP for the rate */
+       opp = dev_pm_opp_find_freq_ceil(dev, &rate);
+
+       if (opp == ERR_PTR(-ERANGE))
+               opp = dev_pm_opp_find_freq_floor(dev, &rate);
+
+       if (IS_ERR(opp)) {
+               dev_err(&genpd->dev, "failed to find OPP for %luHz of %s: %pe\n",
+                       rate, dev_name(dev), opp);
+               ret = PTR_ERR(opp);
+               goto remove_dev_table;
+       }
+
+       if (!list_empty(&genpd->child_links)) {
+               link = list_first_entry(&genpd->child_links, struct gpd_link,
+                                       child_node);
+               core_genpd = link->parent;
+       } else {
+               core_genpd = genpd;
+       }