Merge branch 'clk-rockchip' into clk-next

1 files changed, 123 insertions, 14 deletions

diff --git a/drivers/clk/rockchip/clk.c b/drivers/clk/rockchip/clk.c
index 443d6f07acad..d9a0b5d4d47f 100644
--- a/drivers/clk/rockchip/clk.c
+++ b/drivers/clk/rockchip/clk.c
@@ -102,22 +102,82 @@ static struct clk *rockchip_clk_register_branch(const char *name,
         return clk;
 }
 
+struct rockchip_clk_frac {
+        struct notifier_block                   clk_nb;
+        struct clk_fractional_divider           div;
+        struct clk_gate                         gate;
+
+        struct clk_mux                          mux;
+        const struct clk_ops                    *mux_ops;
+        int                                     mux_frac_idx;
+
+        bool                                    rate_change_remuxed;
+        int                                     rate_change_idx;
+};
+
+#define to_rockchip_clk_frac_nb(nb) \
+                        container_of(nb, struct rockchip_clk_frac, clk_nb)
+
+static int rockchip_clk_frac_notifier_cb(struct notifier_block *nb,
+                                         unsigned long event, void *data)
+{
+        struct clk_notifier_data *ndata = data;
+        struct rockchip_clk_frac *frac = to_rockchip_clk_frac_nb(nb);
+        struct clk_mux *frac_mux = &frac->mux;
+        int ret = 0;
+
+        pr_debug("%s: event %lu, old_rate %lu, new_rate: %lu\n",
+                 __func__, event, ndata->old_rate, ndata->new_rate);
+        if (event == PRE_RATE_CHANGE) {
+                frac->rate_change_idx = frac->mux_ops->get_parent(&frac_mux->hw);
+                if (frac->rate_change_idx != frac->mux_frac_idx) {
+                        frac->mux_ops->set_parent(&frac_mux->hw, frac->mux_frac_idx);
+                        frac->rate_change_remuxed = 1;
+                }
+        } else if (event == POST_RATE_CHANGE) {
+                /*
+                 * The POST_RATE_CHANGE notifier runs directly after the
+                 * divider clock is set in clk_change_rate, so we'll have
+                 * remuxed back to the original parent before clk_change_rate
+                 * reaches the mux itself.
+                 */
+                if (frac->rate_change_remuxed) {
+                        frac->mux_ops->set_parent(&frac_mux->hw, frac->rate_change_idx);
+                        frac->rate_change_remuxed = 0;
+                }
+        }
+
+        return notifier_from_errno(ret);
+}
+
 static struct clk *rockchip_clk_register_frac_branch(const char *name,
                 const char *const *parent_names, u8 num_parents,
                 void __iomem *base, int muxdiv_offset, u8 div_flags,
                 int gate_offset, u8 gate_shift, u8 gate_flags,
-                unsigned long flags, spinlock_t *lock)
+                unsigned long flags, struct rockchip_clk_branch *child,
+                spinlock_t *lock)
 {
+        struct rockchip_clk_frac *frac;
         struct clk *clk;
         struct clk_gate *gate = NULL;
         struct clk_fractional_divider *div = NULL;
         const struct clk_ops *div_ops = NULL, *gate_ops = NULL;
 
-        if (gate_offset >= 0) {
-                gate = kzalloc(sizeof(*gate), GFP_KERNEL);
-                if (!gate)
-                        return ERR_PTR(-ENOMEM);
+        if (muxdiv_offset < 0)
+                return ERR_PTR(-EINVAL);
 
+        if (child && child->branch_type != branch_mux) {
+                pr_err("%s: fractional child clock for %s can only be a mux\n",
+                       __func__, name);
+                return ERR_PTR(-EINVAL);
+        }
+
+        frac = kzalloc(sizeof(*frac), GFP_KERNEL);
+        if (!frac)
+                return ERR_PTR(-ENOMEM);
+
+        if (gate_offset >= 0) {
+                gate = &frac->gate;
                 gate->flags = gate_flags;
                 gate->reg = base + gate_offset;
                 gate->bit_idx = gate_shift;
@@ -125,13 +185,7 @@ static struct clk *rockchip_clk_register_frac_branch(const char *name,
                 gate_ops = &clk_gate_ops;
         }
 
-        if (muxdiv_offset < 0)
-                return ERR_PTR(-EINVAL);
-
-        div = kzalloc(sizeof(*div), GFP_KERNEL);
-        if (!div)
-                return ERR_PTR(-ENOMEM);
-
+        div = &frac->div;
         div->flags = div_flags;
         div->reg = base + muxdiv_offset;
         div->mshift = 16;
@@ -147,7 +201,61 @@ static struct clk *rockchip_clk_register_frac_branch(const char *name,
                                      NULL, NULL,
                                      &div->hw, div_ops,
                                      gate ? &gate->hw : NULL, gate_ops,
-                                     flags);
+                                     flags | CLK_SET_RATE_UNGATE);
+        if (IS_ERR(clk)) {
+                kfree(frac);
+                return clk;
+        }
+
+        if (child) {
+                struct clk_mux *frac_mux = &frac->mux;
+                struct clk_init_data init;
+                struct clk *mux_clk;
+                int i, ret;
+
+                frac->mux_frac_idx = -1;
+                for (i = 0; i < child->num_parents; i++) {
+                        if (!strcmp(name, child->parent_names[i])) {
+                                pr_debug("%s: found fractional parent in mux at pos %d\n",
+                                         __func__, i);
+                                frac->mux_frac_idx = i;
+                                break;
+                        }
+                }
+
+                frac->mux_ops = &clk_mux_ops;
+                frac->clk_nb.notifier_call = rockchip_clk_frac_notifier_cb;
+
+                frac_mux->reg = base + child->muxdiv_offset;
+                frac_mux->shift = child->mux_shift;
+                frac_mux->mask = BIT(child->mux_width) - 1;
+                frac_mux->flags = child->mux_flags;
+                frac_mux->lock = lock;
+                frac_mux->hw.init = &init;
+
+                init.name = child->name;
+                init.flags = child->flags | CLK_SET_RATE_PARENT;
+                init.ops = frac->mux_ops;
+                init.parent_names = child->parent_names;
+                init.num_parents = child->num_parents;
+
+                mux_clk = clk_register(NULL, &frac_mux->hw);
+                if (IS_ERR(mux_clk))
+                        return clk;
+
+                rockchip_clk_add_lookup(mux_clk, child->id);
+
+                /* notifier on the fraction divider to catch rate changes */
+                if (frac->mux_frac_idx >= 0) {
+                        ret = clk_notifier_register(clk, &frac->clk_nb);
+                        if (ret)
+                                pr_err("%s: failed to register clock notifier for %s\n",
+                                                __func__, name);
+                } else {
+                        pr_warn("%s: could not find %s as parent of %s, rate changes may not work\n",
+                                __func__, name, child->name);
+                }
+        }
 
         return clk;
 }
@@ -251,7 +359,8 @@ void __init rockchip_clk_register_branches(
                                 list->parent_names, list->num_parents,
                                 reg_base, list->muxdiv_offset, list->div_flags,
                                 list->gate_offset, list->gate_shift,
-                                list->gate_flags, flags, &clk_lock);
+                                list->gate_flags, flags, list->child,
+                                &clk_lock);
                         break;
                 case branch_gate:
                         flags |= CLK_SET_RATE_PARENT;