clk: armada-37xx-periph: Support changing clock parent and rate

Add support for changing clock rate and parent clock for Armada 37xx
peripheral clocks.

Only clocks which can be disabled (.can_gate is true) can have parent
or rate changed.

This is needed so that Turris Mox can change SPI clock in device tree.

Signed-off-by: Marek Behun <marek.behun@nic.cz>
Signed-off-by: Stefan Roese <sr@denx.de>

diff --git a/drivers/clk/mvebu/armada-37xx-periph.c b/drivers/clk/mvebu/armada-37xx-periph.c
index 902a6cc..b1a3596 100644 (file)
--- a/drivers/clk/mvebu/armada-37xx-periph.c
+++ b/drivers/clk/mvebu/armada-37xx-periph.c
@@ -224,11 +224,21 @@ static const struct clk_periph clks_sb[] = {
        { },
 };
 
-static inline int get_mux(struct a37xx_periphclk *priv, int shift)
+static int get_mux(struct a37xx_periphclk *priv, int shift)
 {
        return (readl(priv->reg + TBG_SEL) >> shift) & 3;
 }
 
+static void set_mux(struct a37xx_periphclk *priv, int shift, int val)
+{
+       u32 reg;
+
+       reg = readl(priv->reg + TBG_SEL);
+       reg &= ~(3 << shift);
+       reg |= (val & 3) << shift;
+       writel(reg, priv->reg + TBG_SEL);
+}
+
 static ulong periph_clk_get_rate(struct a37xx_periphclk *priv, int id);
 
 static ulong get_parent_rate(struct a37xx_periphclk *priv, int id)
@@ -277,6 +287,17 @@ static ulong get_div(struct a37xx_periphclk *priv,
        return 0;
 }
 
+static void set_div_val(struct a37xx_periphclk *priv,
+                       const struct clk_periph *clk, int idx, int val)
+{
+       u32 reg;
+
+       reg = readl(priv->reg + clk->div_reg_off[idx]);
+       reg &= ~(clk->div_mask[idx] << clk->div_shift[idx]);
+       reg |= (val & clk->div_mask[idx]) << clk->div_shift[idx];
+       writel(reg, priv->reg + clk->div_reg_off[idx]);
+}
+
 static ulong periph_clk_get_rate(struct a37xx_periphclk *priv, int id)
 {
        const struct clk_periph *clk = &priv->clks[id];
@@ -337,6 +358,111 @@ static int armada_37xx_periph_clk_disable(struct clk *clk)
        return periph_clk_enable(clk, 0);
 }
 
+#define diff(a, b) abs((long)(a) - (long)(b))
+
+static ulong find_best_div(const struct clk_div_table *t0,
+                          const struct clk_div_table *t1, ulong parent_rate,
+                          ulong req_rate, int *v0, int *v1)
+{
+       const struct clk_div_table *i, *j;
+       ulong rate, best_rate = 0;
+
+       for (i = t0; i && i->div; ++i) {
+               for (j = t1; j && j->div; ++j) {
+                       rate = DIV_ROUND_UP(parent_rate, i->div * j->div);
+
+                       if (!best_rate ||
+                           diff(rate, req_rate) < diff(best_rate, req_rate)) {
+                               best_rate = rate;
+                               *v0 = i->val;
+                               *v1 = j->val;
+                       }
+               }
+       }
+
+       return best_rate;
+}
+
+static ulong armada_37xx_periph_clk_set_rate(struct clk *clk, ulong req_rate)
+{
+       struct a37xx_periphclk *priv = dev_get_priv(clk->dev);
+       const struct clk_periph *periph_clk = &priv->clks[clk->id];
+       ulong rate, old_rate, parent_rate;
+       int div_val0 = 0, div_val1 = 0;
+       const struct clk_div_table *t1;
+       static const struct clk_div_table empty_table[2] = {
+               { 1, 0 },
+               { 0, 0 }
+       };
+
+       if (clk->id > priv->count)
+               return -EINVAL;
+
+       old_rate = periph_clk_get_rate(priv, clk->id);
+       if (old_rate == -EINVAL)
+               return -EINVAL;
+
+       if (old_rate == req_rate)
+               return old_rate;
+
+       if (!periph_clk->can_gate || !periph_clk->dividers)
+               return -ENOTSUPP;
+
+       parent_rate = get_parent_rate(priv, clk->id);
+       if (parent_rate == -EINVAL)
+               return -EINVAL;
+
+       t1 = empty_table;
+       if (periph_clk->dividers > 1)
+               t1 = periph_clk->div_table[1];
+
+       rate = find_best_div(periph_clk->div_table[0], t1, parent_rate,
+                            req_rate, &div_val0, &div_val1);
+
+       periph_clk_enable(clk, 0);
+
+       set_div_val(priv, periph_clk, 0, div_val0);
+       if (periph_clk->dividers > 1)
+               set_div_val(priv, periph_clk, 1, div_val1);
+
+       periph_clk_enable(clk, 1);
+
+       return rate;
+}
+
+static int armada_37xx_periph_clk_set_parent(struct clk *clk,
+                                            struct clk *parent)
+{
+       struct a37xx_periphclk *priv = dev_get_priv(clk->dev);
+       const struct clk_periph *periph_clk = &priv->clks[clk->id];
+       struct clk check_parent;
+       int ret;
+
+       /* We also check if parent is our TBG clock */
+       if (clk->id > priv->count || parent->id >= MAX_TBG_PARENTS)
+               return -EINVAL;
+
+       if (!periph_clk->can_mux || !periph_clk->can_gate)
+               return -ENOTSUPP;
+
+       ret = clk_get_by_index(clk->dev, 0, &check_parent);
+       if (ret < 0)
+               return ret;
+
+       if (parent->dev != check_parent.dev)
+               ret = -EINVAL;
+
+       clk_free(&check_parent);
+       if (ret < 0)
+               return ret;
+
+       periph_clk_enable(clk, 0);
+       set_mux(priv, periph_clk->mux_shift, parent->id);
+       periph_clk_enable(clk, 1);
+
+       return 0;
+}
+
 #if defined(CONFIG_CMD_CLK) && defined(CONFIG_CLK_ARMADA_3720)
 static int armada_37xx_periph_clk_dump(struct udevice *dev)
 {
@@ -473,6 +599,8 @@ static int armada_37xx_periph_clk_probe(struct udevice *dev)
 
 static const struct clk_ops armada_37xx_periph_clk_ops = {
        .get_rate = armada_37xx_periph_clk_get_rate,
+       .set_rate = armada_37xx_periph_clk_set_rate,
+       .set_parent = armada_37xx_periph_clk_set_parent,
        .enable = armada_37xx_periph_clk_enable,
        .disable = armada_37xx_periph_clk_disable,
 };