diff --git a/drivers/clk/rockchip/rk618/Makefile b/drivers/clk/rockchip/rk618/Makefile index 01e59d6f63b8..55e7964bd190 100644 --- a/drivers/clk/rockchip/rk618/Makefile +++ b/drivers/clk/rockchip/rk618/Makefile @@ -6,6 +6,7 @@ obj-$(CONFIG_MFD_RK618) += clk-regmap-mux.o \ clk-regmap-divider.o \ clk-regmap-gate.o \ + clk-regmap-fractional-divider.o \ clk-regmap-composite.o \ clk-regmap-pll.o \ clk-rk618.o diff --git a/drivers/clk/rockchip/rk618/clk-regmap-fractional-divider.c b/drivers/clk/rockchip/rk618/clk-regmap-fractional-divider.c new file mode 100644 index 000000000000..3d5f1d2691e3 --- /dev/null +++ b/drivers/clk/rockchip/rk618/clk-regmap-fractional-divider.c @@ -0,0 +1,157 @@ +// SPDX-License-Identifier: GPL-2.0 +/* + * Copyright (c) 2019 Rockchip Electronics Co. Ltd. + * + * Base on code in drivers/clk/clk-fractional-divider.c. + * See clk-fractional-divider.c for further copyright information. + */ + +#include + +#include "clk-regmap.h" + +#define to_clk_regmap_fractional_divider(_hw) \ + container_of(_hw, struct clk_regmap_fractional_divider, hw) + +static unsigned long +clk_regmap_fractional_divider_recalc_rate(struct clk_hw *hw, + unsigned long parent_rate) +{ + struct clk_regmap_fractional_divider *fd = + to_clk_regmap_fractional_divider(hw); + unsigned long m, n; + u32 val; + u64 ret; + + regmap_read(fd->regmap, fd->reg, &val); + + m = (val & fd->mmask) >> fd->mshift; + n = (val & fd->nmask) >> fd->nshift; + + if (!n || !m) + return parent_rate; + + ret = (u64)parent_rate * m; + do_div(ret, n); + + return ret; +} + +static void clk_regmap_fractional_divider_approximation(struct clk_hw *hw, + unsigned long rate, unsigned long *parent_rate, + unsigned long *m, unsigned long *n) +{ + struct clk_regmap_fractional_divider *fd = + to_clk_regmap_fractional_divider(hw); + unsigned long p_rate, p_parent_rate; + struct clk_hw *p_parent; + unsigned long scale; + + p_rate = clk_hw_get_rate(clk_hw_get_parent(hw)); + if ((rate * 20 > p_rate) && (p_rate % rate != 0)) { + p_parent = clk_hw_get_parent(clk_hw_get_parent(hw)); + p_parent_rate = clk_hw_get_rate(p_parent); + *parent_rate = p_parent_rate; + } + + /* + * Get rate closer to *parent_rate to guarantee there is no overflow + * for m and n. In the result it will be the nearest rate left shifted + * by (scale - fd->nwidth) bits. + */ + scale = fls_long(*parent_rate / rate - 1); + if (scale > fd->nwidth) + rate <<= scale - fd->nwidth; + + rational_best_approximation(rate, *parent_rate, + GENMASK(fd->mwidth - 1, 0), + GENMASK(fd->nwidth - 1, 0), + m, n); +} + +static long +clk_regmap_fractional_divider_round_rate(struct clk_hw *hw, unsigned long rate, + unsigned long *parent_rate) +{ + unsigned long m, n; + u64 ret; + + if (!rate) + return *parent_rate; + + if (rate >= *parent_rate) + return *parent_rate; + + clk_regmap_fractional_divider_approximation(hw, rate, parent_rate, + &m, &n); + + ret = (u64)*parent_rate * m; + do_div(ret, n); + + return ret; +} + +static int +clk_regmap_fractional_divider_set_rate(struct clk_hw *hw, unsigned long rate, + unsigned long parent_rate) +{ + struct clk_regmap_fractional_divider *fd = + to_clk_regmap_fractional_divider(hw); + unsigned long m, n; + u32 val; + + rational_best_approximation(rate, parent_rate, + GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0), + &m, &n); + + dev_dbg(fd->dev, "%s: parent_rate=%ld, m=%ld, n=%ld, rate=%ld\n", + clk_hw_get_name(hw), parent_rate, m, n, rate); + + regmap_read(fd->regmap, fd->reg, &val); + val &= ~(fd->mmask | fd->nmask); + val |= (m << fd->mshift) | (n << fd->nshift); + + return regmap_write(fd->regmap, fd->reg, val); +} + +const struct clk_ops clk_regmap_fractional_divider_ops = { + .recalc_rate = clk_regmap_fractional_divider_recalc_rate, + .round_rate = clk_regmap_fractional_divider_round_rate, + .set_rate = clk_regmap_fractional_divider_set_rate, +}; +EXPORT_SYMBOL_GPL(clk_regmap_fractional_divider_ops); + +struct clk * +devm_clk_regmap_register_fractional_divider(struct device *dev, + const char *name, + const char *parent_name, + struct regmap *regmap, + u32 reg, unsigned long flags) +{ + struct clk_regmap_fractional_divider *fd; + struct clk_init_data init; + + fd = devm_kzalloc(dev, sizeof(*fd), GFP_KERNEL); + if (!fd) + return ERR_PTR(-ENOMEM); + + init.name = name; + init.ops = &clk_regmap_fractional_divider_ops; + init.flags = flags; + init.parent_names = (parent_name ? &parent_name : NULL); + init.num_parents = (parent_name ? 1 : 0); + + fd->dev = dev; + fd->regmap = regmap; + fd->reg = reg; + fd->mshift = 16; + fd->mwidth = 16; + fd->mmask = GENMASK(fd->mwidth - 1, 0) << fd->mshift; + fd->nshift = 0; + fd->nwidth = 16; + fd->nmask = GENMASK(fd->nwidth - 1, 0) << fd->nshift; + fd->hw.init = &init; + + return devm_clk_register(dev, &fd->hw); +} +EXPORT_SYMBOL_GPL(devm_clk_regmap_register_fractional_divider); diff --git a/drivers/clk/rockchip/rk618/clk-regmap.h b/drivers/clk/rockchip/rk618/clk-regmap.h index 9d3b1fbfbf53..2ccc3d46cddc 100644 --- a/drivers/clk/rockchip/rk618/clk-regmap.h +++ b/drivers/clk/rockchip/rk618/clk-regmap.h @@ -26,6 +26,19 @@ #define UPDATE(x, h, l) (((x) << (l)) & GENMASK((h), (l))) #define HIWORD_UPDATE(v, h, l) (((v) << (l)) | (GENMASK((h), (l)) << 16)) +struct clk_regmap_fractional_divider { + struct clk_hw hw; + struct device *dev; + struct regmap *regmap; + u32 reg; + u8 mshift; + u8 mwidth; + u32 mmask; + u8 nshift; + u8 nwidth; + u32 nmask; +}; + struct clk_regmap_divider { struct clk_hw hw; struct device *dev; @@ -55,6 +68,7 @@ struct clk_regmap_mux { extern const struct clk_ops clk_regmap_mux_ops; extern const struct clk_ops clk_regmap_divider_ops; extern const struct clk_ops clk_regmap_gate_ops; +extern const struct clk_ops clk_regmap_fractional_divider_ops; struct clk * devm_clk_regmap_register_pll(struct device *dev, const char *name, @@ -80,6 +94,13 @@ devm_clk_regmap_register_gate(struct device *dev, const char *name, struct regmap *regmap, u32 reg, u8 shift, unsigned long flags); +struct clk * +devm_clk_regmap_register_fractional_divider(struct device *dev, + const char *name, + const char *parent_name, + struct regmap *regmap, + u32 reg, unsigned long flags); + struct clk * devm_clk_regmap_register_composite(struct device *dev, const char *name, const char *const *parent_names,