video: tegra: dc: organize clock setup by out type

Add an out_ops->setup_clk and move clock setup code to a function in each
output type.

Change-Id: I72bdce530fce2a68c5547ea2a6cee301bc9f2df1
Signed-off-by: Jon Mayo <jmayo@nvidia.com>
Reviewed-on: http://git-master/r/168138
Reviewed-by: Simone Willett <swillett@nvidia.com>
Tested-by: Simone Willett <swillett@nvidia.com>

1 files changed, 45 insertions, 0 deletions

diff --git a/drivers/video/tegra/dc/dsi.c b/drivers/video/tegra/dc/dsi.c
index 31ba5c302..c27a59f94 100644
--- a/drivers/video/tegra/dc/dsi.c
+++ b/drivers/video/tegra/dc/dsi.c
@@ -4068,6 +4068,50 @@ static void tegra_dc_dsi_enable(struct tegra_dc *dc)
         }
 }
 
+static long tegra_dc_dsi_setup_clk(struct tegra_dc *dc, struct clk *clk)
+{
+        unsigned long rate;
+        struct clk *parent_clk;
+        struct clk *base_clk;
+
+        if (clk == dc->clk) {
+                parent_clk = clk_get_sys(NULL,
+                                dc->out->parent_clk ? : "pll_d_out0");
+                base_clk = clk_get_parent(parent_clk);
+                tegra_clk_cfg_ex(base_clk,
+                                TEGRA_CLK_PLLD_DSI_OUT_ENB, 1);
+        } else {
+                if (dc->pdata->default_out->dsi->dsi_instance) {
+                        parent_clk = clk_get_sys(NULL,
+                                dc->out->parent_clk ? : "pll_d2_out0");
+                        base_clk = clk_get_parent(parent_clk);
+                        tegra_clk_cfg_ex(base_clk,
+                                        TEGRA_CLK_PLLD_CSI_OUT_ENB, 1);
+                } else {
+                        parent_clk = clk_get_sys(NULL,
+                                dc->out->parent_clk ? : "pll_d_out0");
+                        base_clk = clk_get_parent(parent_clk);
+                        tegra_clk_cfg_ex(base_clk,
+                                        TEGRA_CLK_PLLD_DSI_OUT_ENB, 1);
+                }
+        }
+
+        /* divide by 1000 to avoid overflow */
+        dc->mode.pclk /= 1000;
+        rate = (dc->mode.pclk * dc->shift_clk_div.mul * 2)
+                                / dc->shift_clk_div.div;
+        rate *= 1000;
+        dc->mode.pclk *= 1000;
+
+        if (rate != clk_get_rate(base_clk))
+                clk_set_rate(base_clk, rate);
+
+        if (clk_get_parent(clk) != parent_clk)
+                clk_set_parent(clk, parent_clk);
+
+        return tegra_dc_pclk_round_rate(dc, dc->mode.pclk);
+}
+
 struct tegra_dc_out_ops tegra_dc_dsi_ops = {
         .init = tegra_dc_dsi_init,
         .destroy = tegra_dc_dsi_destroy,
@@ -4079,4 +4123,5 @@ struct tegra_dc_out_ops tegra_dc_dsi_ops = {
         .suspend = tegra_dc_dsi_suspend,
         .resume = tegra_dc_dsi_resume,
 #endif
+        .setup_clk = tegra_dc_dsi_setup_clk,
 };