From: Marek Behún Date: Fri, 17 Aug 2018 10:58:52 +0000 (+0200) Subject: clk: armada-37xx-periph: Support changing clock parent and rate X-Git-Url: http://git.dujemihanovic.xyz/?a=commitdiff_plain;h=de75fb09a18e0c9bc2a503a6512a4e433856a30a;p=u-boot.git 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 Signed-off-by: Stefan Roese --- diff --git a/drivers/clk/mvebu/armada-37xx-periph.c b/drivers/clk/mvebu/armada-37xx-periph.c index 902a6cc9ef..b1a35968e1 100644 --- 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, };