The new Rockchip socs have optional half divider, so we use "branch_half_divider" + "COMPOSITE_NOMUX_HALFDIV \ DIV_HALF" to hook that special divider clock-type into our clock-tree.
Signed-off-by: Elaine Zhang <zhangq...@rock-chips.com> --- drivers/clk/rockchip/Makefile | 1 + drivers/clk/rockchip/clk-half-divider.c | 235 ++++++++++++++++++++++++++++++++ drivers/clk/rockchip/clk.c | 10 ++ drivers/clk/rockchip/clk.h | 45 ++++++ 4 files changed, 291 insertions(+) create mode 100644 drivers/clk/rockchip/clk-half-divider.c diff --git a/drivers/clk/rockchip/Makefile b/drivers/clk/rockchip/Makefile index 59b8d320960a..023f83ad3429 100644 --- a/drivers/clk/rockchip/Makefile +++ b/drivers/clk/rockchip/Makefile @@ -7,6 +7,7 @@ obj-y += clk-rockchip.o obj-y += clk.o obj-y += clk-pll.o obj-y += clk-cpu.o +obj-y += clk-half-divider.o obj-y += clk-inverter.o obj-y += clk-mmc-phase.o obj-y += clk-muxgrf.o diff --git a/drivers/clk/rockchip/clk-half-divider.c b/drivers/clk/rockchip/clk-half-divider.c new file mode 100644 index 000000000000..23830de254ec --- /dev/null +++ b/drivers/clk/rockchip/clk-half-divider.c @@ -0,0 +1,235 @@ +/* + * + * This software is licensed under the terms of the GNU General Public + * License version 2, as published by the Free Software Foundation, and + * may be copied, distributed, and modified under those terms. + * + * This program is distributed in the hope that it will be useful, + * but WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + * GNU General Public License for more details. + */ + +#include <linux/slab.h> +#include <linux/bitops.h> +#include <linux/regmap.h> +#include <linux/clk.h> +#include <linux/clk-provider.h> +#include "clk.h" + +#define div_mask(width) ((1 << (width)) - 1) + +static bool _is_best_half_div(unsigned long rate, unsigned long now, + unsigned long best, unsigned long flags) +{ + if (flags & CLK_DIVIDER_ROUND_CLOSEST) + return abs(rate - now) < abs(rate - best); + + return now <= rate && now > best; +} + +static unsigned long clk_half_divider_recalc_rate(struct clk_hw *hw, + unsigned long parent_rate) +{ + struct clk_divider *divider = to_clk_divider(hw); + unsigned int val; + + val = clk_readl(divider->reg) >> divider->shift; + val &= div_mask(divider->width); + val = val * 2 + 3; + + return DIV_ROUND_UP_ULL((u64)(parent_rate * 2), val); +} + +static int clk_half_divider_bestdiv(struct clk_hw *hw, unsigned long rate, + unsigned long *best_parent_rate, u8 width, + unsigned long flags) +{ + 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 = div_mask(width); + + if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) { + parent_rate = *best_parent_rate; + bestdiv = DIV_ROUND_UP_ULL((u64)(parent_rate * 2), rate); + if (bestdiv < 3) + bestdiv = 0; + else + bestdiv = (bestdiv - 3) / 2; + bestdiv = bestdiv > maxdiv ? maxdiv : bestdiv; + return bestdiv; + } + + /* + * The maximum divider we can use without overflowing + * unsigned long in rate * i below + */ + maxdiv = min(ULONG_MAX / rate, maxdiv); + + for (i = 0; i <= maxdiv; i++) { + if ((rate * (i * 2 + 3)) == (parent_rate_saved * 2)) { + /* + * 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_hw_round_rate(clk_hw_get_parent(hw), + (rate * (i * 2 + 3)) / 2); + now = DIV_ROUND_UP_ULL((u64)(parent_rate * 2), (i * 2 + 3)); + if (_is_best_half_div(rate, now, best, flags)) { + bestdiv = i; + best = now; + *best_parent_rate = parent_rate; + } + } + + if (!bestdiv) { + bestdiv = div_mask(width); + *best_parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw), 1); + } + + return bestdiv; +} + +static long clk_half_divider_round_rate(struct clk_hw *hw, unsigned long rate, + unsigned long *prate) +{ + struct clk_divider *divider = to_clk_divider(hw); + int div; + + div = clk_half_divider_bestdiv(hw, rate, prate, + divider->width, + divider->flags); + + return DIV_ROUND_UP_ULL((u64)(*prate * 2), div * 2 + 3); +} + +static int clk_half_divider_set_rate(struct clk_hw *hw, unsigned long rate, + unsigned long parent_rate) +{ + struct clk_divider *divider = to_clk_divider(hw); + unsigned int value; + unsigned long flags = 0; + u32 val; + + value = DIV_ROUND_UP_ULL((u64)(2 * parent_rate), rate); + value = (value - 3) / 2; + value = min_t(unsigned int, value, div_mask(divider->width)); + + if (divider->lock) + spin_lock_irqsave(divider->lock, flags); + else + __acquire(divider->lock); + + if (divider->flags & CLK_DIVIDER_HIWORD_MASK) { + val = div_mask(divider->width) << (divider->shift + 16); + } else { + val = clk_readl(divider->reg); + val &= ~(div_mask(divider->width) << divider->shift); + } + val |= value << divider->shift; + clk_writel(val, divider->reg); + + if (divider->lock) + spin_unlock_irqrestore(divider->lock, flags); + else + __release(divider->lock); + + return 0; +} + +const struct clk_ops clk_half_divider_ops = { + .recalc_rate = clk_half_divider_recalc_rate, + .round_rate = clk_half_divider_round_rate, + .set_rate = clk_half_divider_set_rate, +}; +EXPORT_SYMBOL_GPL(clk_half_divider_ops); + +/** + * Register a clock branch. + * Most clock branches have a form like + * + * src1 --|--\ + * |M |--[GATE]-[DIV]- + * src2 --|--/ + * + * sometimes without one of those components. + */ +struct clk *rockchip_clk_register_halfdiv(const char *name, + const char *const *parent_names, + u8 num_parents, void __iomem *base, + int muxdiv_offset, u8 mux_shift, + u8 mux_width, u8 mux_flags, + u8 div_shift, u8 div_width, + u8 div_flags, int gate_offset, + u8 gate_shift, u8 gate_flags, + unsigned long flags, + spinlock_t *lock) +{ + struct clk *clk; + struct clk_mux *mux = NULL; + struct clk_gate *gate = NULL; + struct clk_divider *div = NULL; + const struct clk_ops *mux_ops = NULL, *div_ops = NULL, + *gate_ops = NULL; + + if (num_parents > 1) { + mux = kzalloc(sizeof(*mux), GFP_KERNEL); + if (!mux) + return ERR_PTR(-ENOMEM); + + mux->reg = base + muxdiv_offset; + mux->shift = mux_shift; + mux->mask = BIT(mux_width) - 1; + mux->flags = mux_flags; + mux->lock = lock; + mux_ops = (mux_flags & CLK_MUX_READ_ONLY) ? &clk_mux_ro_ops + : &clk_mux_ops; + } + + if (gate_offset >= 0) { + gate = kzalloc(sizeof(*gate), GFP_KERNEL); + if (!gate) + goto err_gate; + + gate->flags = gate_flags; + gate->reg = base + gate_offset; + gate->bit_idx = gate_shift; + gate->lock = lock; + gate_ops = &clk_gate_ops; + } + + if (div_width > 0) { + div = kzalloc(sizeof(*div), GFP_KERNEL); + if (!div) + goto err_div; + + div->flags = div_flags; + div->reg = base + muxdiv_offset; + div->shift = div_shift; + div->width = div_width; + div->lock = lock; + div_ops = &clk_half_divider_ops; + } + + clk = clk_register_composite(NULL, name, parent_names, num_parents, + mux ? &mux->hw : NULL, mux_ops, + div ? &div->hw : NULL, div_ops, + gate ? &gate->hw : NULL, gate_ops, + flags); + + return clk; +err_div: + kfree(gate); +err_gate: + kfree(mux); + return ERR_PTR(-ENOMEM); +} diff --git a/drivers/clk/rockchip/clk.c b/drivers/clk/rockchip/clk.c index 3cd8ad59e0b7..cd1474c05c91 100644 --- a/drivers/clk/rockchip/clk.c +++ b/drivers/clk/rockchip/clk.c @@ -498,6 +498,16 @@ void __init rockchip_clk_register_branches( list->gate_flags, flags, list->child, &ctx->lock); break; + case branch_half_divider: + clk = rockchip_clk_register_halfdiv(list->name, + list->parent_names, list->num_parents, + ctx->reg_base, list->muxdiv_offset, + list->mux_shift, list->mux_width, + list->mux_flags, list->div_shift, + list->div_width, list->div_flags, + list->gate_offset, list->gate_shift, + list->gate_flags, flags, &ctx->lock); + break; case branch_gate: flags |= CLK_SET_RATE_PARENT; diff --git a/drivers/clk/rockchip/clk.h b/drivers/clk/rockchip/clk.h index ef601dded32c..7a3660c2bbb5 100644 --- a/drivers/clk/rockchip/clk.h +++ b/drivers/clk/rockchip/clk.h @@ -354,6 +354,7 @@ enum rockchip_clk_branch_type { branch_inverter, branch_factor, branch_ddrclk, + branch_half_divider, }; struct rockchip_clk_branch { @@ -684,6 +685,39 @@ struct rockchip_clk_branch { .gate_flags = gf, \ } +#define COMPOSITE_NOMUX_HALFDIV(_id, cname, pname, f, mo, ds, dw, df, \ + go, gs, gf) \ + { \ + .id = _id, \ + .branch_type = branch_half_divider, \ + .name = cname, \ + .parent_names = (const char *[]){ pname }, \ + .num_parents = 1, \ + .flags = f, \ + .muxdiv_offset = mo, \ + .div_shift = ds, \ + .div_width = dw, \ + .div_flags = df, \ + .gate_offset = go, \ + .gate_shift = gs, \ + .gate_flags = gf, \ + } + +#define DIV_HALF(_id, cname, pname, f, o, s, w, df) \ + { \ + .id = _id, \ + .branch_type = branch_half_divider, \ + .name = cname, \ + .parent_names = (const char *[]){ pname }, \ + .num_parents = 1, \ + .flags = f, \ + .muxdiv_offset = o, \ + .div_shift = s, \ + .div_width = w, \ + .div_flags = df, \ + .gate_offset = -1, \ + } + struct rockchip_clk_provider *rockchip_clk_init(struct device_node *np, void __iomem *base, unsigned long nr_clks); void rockchip_clk_of_add_provider(struct device_node *np, @@ -708,6 +742,17 @@ void rockchip_register_restart_notifier(struct rockchip_clk_provider *ctx, #define ROCKCHIP_SOFTRST_HIWORD_MASK BIT(0) +struct clk *rockchip_clk_register_halfdiv(const char *name, + const char *const *parent_names, + u8 num_parents, void __iomem *base, + int muxdiv_offset, u8 mux_shift, + u8 mux_width, u8 mux_flags, + u8 div_shift, u8 div_width, + u8 div_flags, int gate_offset, + u8 gate_shift, u8 gate_flags, + unsigned long flags, + spinlock_t *lock); + #ifdef CONFIG_RESET_CONTROLLER void rockchip_register_softrst(struct device_node *np, unsigned int num_regs, -- 1.9.1