--- zzzz-none-000/linux-3.10.107/drivers/clk/sunxi/clk-factors.c 2017-06-27 09:49:32.000000000 +0000 +++ scorpion-7490-727/linux-3.10.107/drivers/clk/sunxi/clk-factors.c 2021-02-04 17:41:59.000000000 +0000 @@ -9,18 +9,18 @@ */ #include +#include +#include +#include #include +#include #include -#include -#include #include -#include - #include "clk-factors.h" /* - * DOC: basic adjustable factor-based clock that cannot gate + * DOC: basic adjustable factor-based clock * * Traits of this clock: * prepare - clk_prepare only ensures that parents are prepared @@ -30,17 +30,11 @@ * parent - fixed parent. No clk_set_parent support */ -struct clk_factors { - struct clk_hw hw; - void __iomem *reg; - struct clk_factors_config *config; - void (*get_factors) (u32 *rate, u32 parent, u8 *n, u8 *k, u8 *m, u8 *p); - spinlock_t *lock; -}; - #define to_clk_factors(_hw) container_of(_hw, struct clk_factors, hw) -#define SETMASK(len, pos) (((-1U) >> (31-len)) << (pos)) +#define FACTORS_MAX_PARENTS 5 + +#define SETMASK(len, pos) (((1U << (len)) - 1) << (pos)) #define CLRMASK(len, pos) (~(SETMASK(len, pos))) #define FACTOR_GET(bit, len, reg) (((reg) & SETMASK(len, bit)) >> (bit)) @@ -85,10 +79,48 @@ return rate; } +static int clk_factors_determine_rate(struct clk_hw *hw, + struct clk_rate_request *req) +{ + struct clk_hw *parent, *best_parent = NULL; + int i, num_parents; + unsigned long parent_rate, best = 0, child_rate, best_child_rate = 0; + + /* find the parent that can help provide the fastest rate <= rate */ + num_parents = clk_hw_get_num_parents(hw); + for (i = 0; i < num_parents; i++) { + parent = clk_hw_get_parent_by_index(hw, i); + if (!parent) + continue; + if (clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT) + parent_rate = clk_hw_round_rate(parent, req->rate); + else + parent_rate = clk_hw_get_rate(parent); + + child_rate = clk_factors_round_rate(hw, req->rate, + &parent_rate); + + if (child_rate <= req->rate && child_rate > best_child_rate) { + best_parent = parent; + best = parent_rate; + best_child_rate = child_rate; + } + } + + if (!best_parent) + return -EINVAL; + + req->best_parent_hw = best_parent; + req->best_parent_rate = best; + req->rate = best_child_rate; + + return 0; +} + static int clk_factors_set_rate(struct clk_hw *hw, unsigned long rate, unsigned long parent_rate) { - u8 n, k, m, p; + u8 n = 0, k = 0, m = 0, p = 0; u32 reg; struct clk_factors *factors = to_clk_factors(hw); struct clk_factors_config *config = factors->config; @@ -121,60 +153,91 @@ } static const struct clk_ops clk_factors_ops = { + .determine_rate = clk_factors_determine_rate, .recalc_rate = clk_factors_recalc_rate, .round_rate = clk_factors_round_rate, .set_rate = clk_factors_set_rate, }; -/** - * clk_register_factors - register a factors clock with - * the clock framework - * @dev: device registering this clock - * @name: name of this clock - * @parent_name: name of clock's parent - * @flags: framework-specific flags - * @reg: register address to adjust factors - * @config: shift and width of factors n, k, m and p - * @get_factors: function to calculate the factors for a given frequency - * @lock: shared register lock for this clock - */ -struct clk *clk_register_factors(struct device *dev, const char *name, - const char *parent_name, - unsigned long flags, void __iomem *reg, - struct clk_factors_config *config, - void (*get_factors)(u32 *rate, u32 parent, - u8 *n, u8 *k, u8 *m, u8 *p), - spinlock_t *lock) +struct clk *sunxi_factors_register(struct device_node *node, + const struct factors_data *data, + spinlock_t *lock, + void __iomem *reg) { - struct clk_factors *factors; struct clk *clk; - struct clk_init_data init; + struct clk_factors *factors; + struct clk_gate *gate = NULL; + struct clk_mux *mux = NULL; + struct clk_hw *gate_hw = NULL; + struct clk_hw *mux_hw = NULL; + const char *clk_name = node->name; + const char *parents[FACTORS_MAX_PARENTS]; + int i = 0; + + /* if we have a mux, we will have >1 parents */ + i = of_clk_parent_fill(node, parents, FACTORS_MAX_PARENTS); + + /* + * some factor clocks, such as pll5 and pll6, may have multiple + * outputs, and have their name designated in factors_data + */ + if (data->name) + clk_name = data->name; + else + of_property_read_string(node, "clock-output-names", &clk_name); - /* allocate the factors */ factors = kzalloc(sizeof(struct clk_factors), GFP_KERNEL); - if (!factors) { - pr_err("%s: could not allocate factors clk\n", __func__); - return ERR_PTR(-ENOMEM); - } - - init.name = name; - init.ops = &clk_factors_ops; - init.flags = flags; - init.parent_names = (parent_name ? &parent_name : NULL); - init.num_parents = (parent_name ? 1 : 0); + if (!factors) + return NULL; - /* struct clk_factors assignments */ + /* set up factors properties */ factors->reg = reg; - factors->config = config; + factors->config = data->table; + factors->get_factors = data->getter; factors->lock = lock; - factors->hw.init = &init; - factors->get_factors = get_factors; - /* register the clock */ - clk = clk_register(dev, &factors->hw); + /* Add a gate if this factor clock can be gated */ + if (data->enable) { + gate = kzalloc(sizeof(struct clk_gate), GFP_KERNEL); + if (!gate) { + kfree(factors); + return NULL; + } + + /* set up gate properties */ + gate->reg = reg; + gate->bit_idx = data->enable; + gate->lock = factors->lock; + gate_hw = &gate->hw; + } + + /* Add a mux if this factor clock can be muxed */ + if (data->mux) { + mux = kzalloc(sizeof(struct clk_mux), GFP_KERNEL); + if (!mux) { + kfree(factors); + kfree(gate); + return NULL; + } + + /* set up gate properties */ + mux->reg = reg; + mux->shift = data->mux; + mux->mask = data->muxmask; + mux->lock = factors->lock; + mux_hw = &mux->hw; + } - if (IS_ERR(clk)) - kfree(factors); + clk = clk_register_composite(NULL, clk_name, + parents, i, + mux_hw, &clk_mux_ops, + &factors->hw, &clk_factors_ops, + gate_hw, &clk_gate_ops, 0); + + if (!IS_ERR(clk)) { + of_clk_add_provider(node, of_clk_src_simple_get, clk); + clk_register_clkdev(clk, clk_name, NULL); + } return clk; }