Merge branch 'linux-linaro-lsk-v4.4-android' of git://git.linaro.org/kernel/linux...

[firefly-linux-kernel-4.4.55.git] / drivers / clk / rockchip / clk-cpu.c

diff --git a/drivers/clk/rockchip/clk-cpu.c b/drivers/clk/rockchip/clk-cpu.c
index 330870a6d8bfa9163405398ff3de7e1eb0d8a864..cd2b1d87f4a4384f7a79c797df74624a0101a9ae 100644 (file)
--- a/drivers/clk/rockchip/clk-cpu.c
+++ b/drivers/clk/rockchip/clk-cpu.c
@@ -124,9 +124,18 @@ static int rockchip_cpuclk_pre_rate_change(struct rockchip_cpuclk *cpuclk,
                                           struct clk_notifier_data *ndata)
 {
        const struct rockchip_cpuclk_reg_data *reg_data = cpuclk->reg_data;
+       const struct rockchip_cpuclk_rate_table *rate;
        unsigned long alt_prate, alt_div;
        unsigned long flags;
 
+       /* check validity of the new rate */
+       rate = rockchip_get_cpuclk_settings(cpuclk, ndata->new_rate);
+       if (!rate) {
+               pr_err("%s: Invalid rate : %lu for cpuclk\n",
+                      __func__, ndata->new_rate);
+               return -EINVAL;
+       }
+
        alt_prate = clk_get_rate(cpuclk->alt_parent);
 
        spin_lock_irqsave(cpuclk->lock, flags);
@@ -158,12 +167,16 @@ static int rockchip_cpuclk_pre_rate_change(struct rockchip_cpuclk *cpuclk,
 
                writel(HIWORD_UPDATE(alt_div, reg_data->div_core_mask,
                                              reg_data->div_core_shift) |
-                      HIWORD_UPDATE(1, 1, reg_data->mux_core_shift),
+                      HIWORD_UPDATE(reg_data->mux_core_alt,
+                                    reg_data->mux_core_mask,
+                                    reg_data->mux_core_shift),
                       cpuclk->reg_base + reg_data->core_reg);
        } else {
                /* select alternate parent */
-               writel(HIWORD_UPDATE(1, 1, reg_data->mux_core_shift),
-                       cpuclk->reg_base + reg_data->core_reg);
+               writel(HIWORD_UPDATE(reg_data->mux_core_alt,
+                                    reg_data->mux_core_mask,
+                                    reg_data->mux_core_shift),
+                      cpuclk->reg_base + reg_data->core_reg);
        }
 
        spin_unlock_irqrestore(cpuclk->lock, flags);
@@ -198,7 +211,9 @@ static int rockchip_cpuclk_post_rate_change(struct rockchip_cpuclk *cpuclk,
 
        writel(HIWORD_UPDATE(0, reg_data->div_core_mask,
                                reg_data->div_core_shift) |
-              HIWORD_UPDATE(0, 1, reg_data->mux_core_shift),
+              HIWORD_UPDATE(reg_data->mux_core_main,
+                               reg_data->mux_core_mask,
+                               reg_data->mux_core_shift),
               cpuclk->reg_base + reg_data->core_reg);
 
        if (ndata->old_rate > ndata->new_rate)
@@ -242,8 +257,8 @@ struct clk *rockchip_clk_register_cpuclk(const char *name,
        struct clk *clk, *cclk;
        int ret;
 
-       if (num_parents != 2) {
-               pr_err("%s: needs two parent clocks\n", __func__);
+       if (num_parents < 2) {
+               pr_err("%s: needs at least two parent clocks\n", __func__);
                return ERR_PTR(-EINVAL);
        }
 
@@ -252,7 +267,7 @@ struct clk *rockchip_clk_register_cpuclk(const char *name,
                return ERR_PTR(-ENOMEM);
 
        init.name = name;
-       init.parent_names = &parent_names[0];
+       init.parent_names = &parent_names[reg_data->mux_core_main];
        init.num_parents = 1;
        init.ops = &rockchip_cpuclk_ops;
 
@@ -270,10 +285,10 @@ struct clk *rockchip_clk_register_cpuclk(const char *name,
        cpuclk->clk_nb.notifier_call = rockchip_cpuclk_notifier_cb;
        cpuclk->hw.init = &init;
 
-       cpuclk->alt_parent = __clk_lookup(parent_names[1]);
+       cpuclk->alt_parent = __clk_lookup(parent_names[reg_data->mux_core_alt]);
        if (!cpuclk->alt_parent) {
-               pr_err("%s: could not lookup alternate parent\n",
-                      __func__);
+               pr_err("%s: could not lookup alternate parent: (%d)\n",
+                      __func__, reg_data->mux_core_alt);
                ret = -EINVAL;
                goto free_cpuclk;
        }
@@ -285,10 +300,11 @@ struct clk *rockchip_clk_register_cpuclk(const char *name,
                goto free_cpuclk;
        }
 
-       clk = __clk_lookup(parent_names[0]);
+       clk = __clk_lookup(parent_names[reg_data->mux_core_main]);
        if (!clk) {
-               pr_err("%s: could not lookup parent clock %s\n",
-                      __func__, parent_names[0]);
+               pr_err("%s: could not lookup parent clock: (%d) %s\n",
+                      __func__, reg_data->mux_core_main,
+                      parent_names[reg_data->mux_core_main]);
                ret = -EINVAL;
                goto free_cpuclk;
        }
@@ -314,9 +330,9 @@ struct clk *rockchip_clk_register_cpuclk(const char *name,
        }
 
        cclk = clk_register(NULL, &cpuclk->hw);
-       if (IS_ERR(clk)) {
+       if (IS_ERR(cclk)) {
                pr_err("%s: could not register cpuclk %s\n", __func__,  name);
-               ret = PTR_ERR(clk);
+               ret = PTR_ERR(cclk);
                goto free_rate_table;
        }