From d4aaca3647fe900a4e6fe87c1d9717c3e5292386 Mon Sep 17 00:00:00 2001 From: Sascha Hauer Date: Thu, 13 Mar 2014 10:04:46 +0100 Subject: [PATCH] clk: clk-divider: sync with kernel code This updates the clk-divider to Kernel code, but without power-of-two divider support which we do not need yet. This also adds table based divider support to the divider. Signed-off-by: Sascha Hauer --- drivers/clk/clk-divider.c | 201 +++++++++++++++++++++++++++++++------- include/linux/clk.h | 3 + 2 files changed, 169 insertions(+), 35 deletions(-) diff --git a/drivers/clk/clk-divider.c b/drivers/clk/clk-divider.c index bb8bcc126..9634bd3c8 100644 --- a/drivers/clk/clk-divider.c +++ b/drivers/clk/clk-divider.c @@ -20,61 +20,192 @@ #include #include -static unsigned int clk_divider_maxdiv(struct clk_divider *div) +#define div_mask(d) ((1 << ((d)->width)) - 1) + +static unsigned int _get_maxdiv(struct clk_divider *divider) { - if (div->flags & CLK_DIVIDER_ONE_BASED) - return (1 << div->width) - 1; - return 1 << div->width; + if (divider->flags & CLK_DIVIDER_ONE_BASED) + return div_mask(divider); + return div_mask(divider) + 1; } -static int clk_divider_set_rate(struct clk *clk, unsigned long rate, - unsigned long parent_rate) +static unsigned int _get_table_div(const struct clk_div_table *table, + unsigned int val) { - struct clk_divider *div = container_of(clk, struct clk_divider, clk); - unsigned int val, divval; - - if (rate > parent_rate) - rate = parent_rate; - if (!rate) - rate = 1; - - divval = DIV_ROUND_UP(parent_rate, rate); - if (divval > clk_divider_maxdiv(div)) - divval = clk_divider_maxdiv(div); - - if (!(div->flags & CLK_DIVIDER_ONE_BASED)) - divval--; - - val = readl(div->reg); - val &= ~(((1 << div->width) - 1) << div->shift); - val |= divval << div->shift; - writel(val, div->reg); + const struct clk_div_table *clkt; + for (clkt = table; clkt->div; clkt++) + if (clkt->val == val) + return clkt->div; return 0; } +static unsigned int _get_div(struct clk_divider *divider, unsigned int val) +{ + if (divider->flags & CLK_DIVIDER_ONE_BASED) + return val; + if (divider->table) + return _get_table_div(divider->table, val); + return val + 1; +} + +static unsigned int _get_table_val(const struct clk_div_table *table, + unsigned int div) +{ + const struct clk_div_table *clkt; + + for (clkt = table; clkt->div; clkt++) + if (clkt->div == div) + return clkt->val; + return 0; +} + +static unsigned int _get_val(struct clk_divider *divider, unsigned int div) +{ + if (divider->flags & CLK_DIVIDER_ONE_BASED) + return div; + if (divider->table) + return _get_table_val(divider->table, div); + return div - 1; +} + static unsigned long clk_divider_recalc_rate(struct clk *clk, unsigned long parent_rate) { - struct clk_divider *div = container_of(clk, struct clk_divider, clk); - unsigned int val; + struct clk_divider *divider = container_of(clk, struct clk_divider, clk); + unsigned int div, val; - val = readl(div->reg) >> div->shift; - val &= (1 << div->width) - 1; + val = readl(divider->reg) >> divider->shift; + val &= div_mask(divider); - if (div->flags & CLK_DIVIDER_ONE_BASED) { - if (!val) - val++; - } else { - val++; + div = _get_div(divider, val); + + return parent_rate / div; +} + +/* + * The reverse of DIV_ROUND_UP: The maximum number which + * divided by m is r + */ +#define MULT_ROUND_UP(r, m) ((r) * (m) + (m) - 1) + +static bool _is_valid_table_div(const struct clk_div_table *table, + unsigned int div) +{ + const struct clk_div_table *clkt; + + for (clkt = table; clkt->div; clkt++) + if (clkt->div == div) + return true; + return false; +} + +static bool _is_valid_div(struct clk_divider *divider, unsigned int div) +{ + if (divider->table) + return _is_valid_table_div(divider->table, div); + return true; +} + +static int clk_divider_bestdiv(struct clk *clk, unsigned long rate, + unsigned long *best_parent_rate) +{ + struct clk_divider *divider = container_of(clk, struct clk_divider, clk); + int i, bestdiv = 0; + unsigned long parent_rate, best = 0, now, maxdiv; + unsigned long parent_rate_saved = *best_parent_rate; + + if (!rate) + rate = 1; + + maxdiv = _get_maxdiv(divider); + + if (!(clk->flags & CLK_SET_RATE_PARENT)) { + parent_rate = *best_parent_rate; + bestdiv = DIV_ROUND_UP(parent_rate, rate); + bestdiv = bestdiv == 0 ? 1 : bestdiv; + bestdiv = bestdiv > maxdiv ? maxdiv : bestdiv; + return bestdiv; } - return parent_rate / val; + /* + * The maximum divider we can use without overflowing + * unsigned long in rate * i below + */ + maxdiv = min(ULONG_MAX / rate, maxdiv); + + for (i = 1; i <= maxdiv; i++) { + if (!_is_valid_div(divider, i)) + continue; + if (rate * i == parent_rate_saved) { + /* + * It's the most ideal case if the requested rate can be + * divided from parent clock without needing to change + * parent rate, so return the divider immediately. + */ + *best_parent_rate = parent_rate_saved; + return i; + } + parent_rate = clk_round_rate(clk_get_parent(clk), + MULT_ROUND_UP(rate, i)); + now = parent_rate / i; + if (now <= rate && now > best) { + bestdiv = i; + best = now; + *best_parent_rate = parent_rate; + } + } + + if (!bestdiv) { + bestdiv = _get_maxdiv(divider); + *best_parent_rate = clk_round_rate(clk_get_parent(clk), 1); + } + + return bestdiv; +} + +static long clk_divider_round_rate(struct clk *clk, unsigned long rate, + unsigned long *parent_rate) +{ + int div; + + div = clk_divider_bestdiv(clk, rate, parent_rate); + + return *parent_rate / div; +} + +static int clk_divider_set_rate(struct clk *clk, unsigned long rate, + unsigned long parent_rate) +{ + struct clk_divider *divider = container_of(clk, struct clk_divider, clk); + unsigned int div, value; + u32 val; + + if (clk->flags & CLK_SET_RATE_PARENT) { + unsigned long best_parent_rate = parent_rate; + div = clk_divider_bestdiv(clk, rate, &best_parent_rate); + clk_set_rate(clk_get_parent(clk), best_parent_rate); + } else { + div = parent_rate / rate; + } + + value = _get_val(divider, div); + + if (value > div_mask(divider)) + value = div_mask(divider); + + val = readl(divider->reg); + val &= ~(div_mask(divider) << divider->shift); + val |= value << divider->shift; + writel(val, divider->reg); + + return 0; } struct clk_ops clk_divider_ops = { .set_rate = clk_divider_set_rate, .recalc_rate = clk_divider_recalc_rate, + .round_rate = clk_divider_round_rate, }; struct clk *clk_divider(const char *name, const char *parent, diff --git a/include/linux/clk.h b/include/linux/clk.h index 2704b0f05..71b046607 100644 --- a/include/linux/clk.h +++ b/include/linux/clk.h @@ -240,6 +240,9 @@ struct clk_divider { const char *parent; #define CLK_DIVIDER_ONE_BASED (1 << 0) unsigned flags; + const struct clk_div_table *table; + int max_div_index; + int table_size; }; extern struct clk_ops clk_divider_ops;