diff --git a/drivers/clk/qcom/Makefile b/drivers/clk/qcom/Makefile index 4c18181c047c..d3e88f40bdfd 100644 --- a/drivers/clk/qcom/Makefile +++ b/drivers/clk/qcom/Makefile @@ -9,6 +9,7 @@ clk-qcom-y += clk-rcg2.o clk-qcom-y += clk-branch.o clk-qcom-y += clk-regmap-divider.o clk-qcom-y += clk-regmap-mux.o +clk-qcom-y += clk-regmap-mux-div.o clk-qcom-$(CONFIG_KRAIT_CLOCKS) += clk-krait.o clk-qcom-y += clk-hfpll.o clk-qcom-y += reset.o clk-voter.o diff --git a/drivers/clk/qcom/clk-regmap-mux-div.c b/drivers/clk/qcom/clk-regmap-mux-div.c new file mode 100644 index 000000000000..9593aefb0bf6 --- /dev/null +++ b/drivers/clk/qcom/clk-regmap-mux-div.c @@ -0,0 +1,255 @@ +/* + * Copyright (c) 2015, Linaro Limited + * Copyright (c) 2014, The Linux Foundation. All rights reserved. + * + * 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 +#include +#include +#include +#include + +#include "clk-regmap-mux-div.h" + +#define CMD_RCGR 0x0 +#define CMD_RCGR_UPDATE BIT(0) +#define CMD_RCGR_DIRTY_CFG BIT(4) +#define CMD_RCGR_ROOT_OFF BIT(31) +#define CFG_RCGR 0x4 + +#define to_clk_regmap_mux_div(_hw) \ + container_of(to_clk_regmap(_hw), struct clk_regmap_mux_div, clkr) + +int __mux_div_set_src_div(struct clk_regmap_mux_div *md, u32 src, u32 div) +{ + int ret, count; + u32 val, mask; + const char *name = clk_hw_get_name(&md->clkr.hw); + + val = (div << md->hid_shift) | (src << md->src_shift); + mask = ((BIT(md->hid_width) - 1) << md->hid_shift) | + ((BIT(md->src_width) - 1) << md->src_shift); + + ret = regmap_update_bits(md->clkr.regmap, CFG_RCGR + md->reg_offset, + mask, val); + if (ret) + return ret; + + ret = regmap_update_bits(md->clkr.regmap, CMD_RCGR + md->reg_offset, + CMD_RCGR_UPDATE, CMD_RCGR_UPDATE); + if (ret) + return ret; + + /* Wait for update to take effect */ + for (count = 500; count > 0; count--) { + ret = regmap_read(md->clkr.regmap, CMD_RCGR + md->reg_offset, + &val); + if (ret) + return ret; + if (!(val & CMD_RCGR_UPDATE)) + return 0; + udelay(1); + } + + pr_err("%s: RCG did not update its configuration", name); + return -EBUSY; +} + +static void __mux_div_get_src_div(struct clk_regmap_mux_div *md, u32 *src, + u32 *div) +{ + u32 val, __div, __src; + const char *name = clk_hw_get_name(&md->clkr.hw); + + regmap_read(md->clkr.regmap, CMD_RCGR + md->reg_offset, &val); + + if (val & CMD_RCGR_DIRTY_CFG) { + pr_err("%s: RCG configuration is pending\n", name); + return; + } + + regmap_read(md->clkr.regmap, CFG_RCGR + md->reg_offset, &val); + __src = (val >> md->src_shift); + __src &= BIT(md->src_width) - 1; + *src = __src; + + __div = (val >> md->hid_shift); + __div &= BIT(md->hid_width) - 1; + *div = __div; +} + +static int mux_div_enable(struct clk_hw *hw) +{ + struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); + + return __mux_div_set_src_div(md, md->src, md->div); +} + +static inline bool is_better_rate(unsigned long req, unsigned long best, + unsigned long new) +{ + return (req <= new && new < best) || (best < req && best < new); +} + +static int mux_div_determine_rate(struct clk_hw *hw, + struct clk_rate_request *req) +{ + struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); + unsigned int i, div, max_div; + unsigned long actual_rate, best_rate = 0; + unsigned long req_rate = req->rate; + + for (i = 0; i < clk_hw_get_num_parents(hw); i++) { + struct clk_hw *parent = clk_hw_get_parent_by_index(hw, i); + unsigned long parent_rate = clk_hw_get_rate(parent); + + max_div = BIT(md->hid_width) - 1; + for (div = 1; div < max_div; div++) { + parent_rate = mult_frac(req_rate, div, 2); + parent_rate = clk_hw_round_rate(parent, parent_rate); + actual_rate = mult_frac(parent_rate, 2, div); + + if (is_better_rate(req_rate, best_rate, actual_rate)) { + best_rate = actual_rate; + req->rate = best_rate; + req->best_parent_rate = parent_rate; + req->best_parent_hw = parent; + } + + if (actual_rate < req_rate || best_rate <= req_rate) + break; + } + } + + if (!best_rate) + return -EINVAL; + + return 0; +} + +static int __mux_div_set_rate_and_parent(struct clk_hw *hw, unsigned long rate, + unsigned long prate, u32 src) +{ + struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); + int ret; + u32 div, max_div, best_src = 0, best_div = 0; + unsigned int i; + unsigned long actual_rate, best_rate = 0; + + for (i = 0; i < clk_hw_get_num_parents(hw); i++) { + struct clk_hw *parent = clk_hw_get_parent_by_index(hw, i); + unsigned long parent_rate = clk_hw_get_rate(parent); + + max_div = BIT(md->hid_width) - 1; + for (div = 1; div < max_div; div++) { + parent_rate = mult_frac(rate, div, 2); + parent_rate = clk_hw_round_rate(parent, parent_rate); + actual_rate = mult_frac(parent_rate, 2, div); + + if (is_better_rate(rate, best_rate, actual_rate)) { + best_rate = actual_rate; + best_src = md->parent_map[i].cfg; + best_div = div - 1; + } + + if (actual_rate < rate || best_rate <= rate) + break; + } + } + + ret = __mux_div_set_src_div(md, best_src, best_div); + if (!ret) { + md->div = best_div; + md->src = best_src; + } + + return ret; +} + +static u8 mux_div_get_parent(struct clk_hw *hw) +{ + struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); + const char *name = clk_hw_get_name(hw); + u32 i, div, src = 0; + + __mux_div_get_src_div(md, &src, &div); + + for (i = 0; i < clk_hw_get_num_parents(hw); i++) + if (src == md->parent_map[i].cfg) + return i; + + pr_err("%s: Can't find parent with src %d\n", name, src); + return 0; +} + +static int mux_div_set_parent(struct clk_hw *hw, u8 index) +{ + struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); + + return __mux_div_set_src_div(md, md->parent_map[index].cfg, md->div); +} + +static int mux_div_set_rate(struct clk_hw *hw, + unsigned long rate, unsigned long prate) +{ + struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); + + return __mux_div_set_rate_and_parent(hw, rate, prate, md->src); +} + +static int mux_div_set_rate_and_parent(struct clk_hw *hw, unsigned long rate, + unsigned long prate, u8 index) +{ + struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); + + return __mux_div_set_rate_and_parent(hw, rate, prate, + md->parent_map[index].cfg); +} + +static unsigned long mux_div_recalc_rate(struct clk_hw *hw, unsigned long prate) +{ + struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); + u32 div, src; + int i, num_parents = clk_hw_get_num_parents(hw); + const char *name = clk_hw_get_name(hw); + + __mux_div_get_src_div(md, &src, &div); + for (i = 0; i < num_parents; i++) + if (src == md->parent_map[i].cfg) { + struct clk_hw *p = clk_hw_get_parent_by_index(hw, i); + unsigned long parent_rate = clk_hw_get_rate(p); + + return mult_frac(parent_rate, 2, div + 1); + } + + pr_err("%s: Can't find parent %d\n", name, src); + return 0; +} + +static void mux_div_disable(struct clk_hw *hw) +{ + struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); + + __mux_div_set_src_div(md, md->safe_src, md->safe_div); +} + +const struct clk_ops clk_regmap_mux_div_ops = { + .enable = mux_div_enable, + .disable = mux_div_disable, + .get_parent = mux_div_get_parent, + .set_parent = mux_div_set_parent, + .set_rate = mux_div_set_rate, + .set_rate_and_parent = mux_div_set_rate_and_parent, + .determine_rate = mux_div_determine_rate, + .recalc_rate = mux_div_recalc_rate, +}; +EXPORT_SYMBOL_GPL(clk_regmap_mux_div_ops); diff --git a/drivers/clk/qcom/clk-regmap-mux-div.h b/drivers/clk/qcom/clk-regmap-mux-div.h new file mode 100644 index 000000000000..6fac5c54a824 --- /dev/null +++ b/drivers/clk/qcom/clk-regmap-mux-div.h @@ -0,0 +1,65 @@ +/* + * Copyright (c) 2015, Linaro Limited + * Copyright (c) 2014, The Linux Foundation. All rights reserved. + * + * 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. + */ + +#ifndef __QCOM_CLK_REGMAP_MUX_DIV_H__ +#define __QCOM_CLK_REGMAP_MUX_DIV_H__ + +#include +#include "clk-rcg.h" +#include "clk-regmap.h" + +/** + * struct mux_div_clk - combined mux/divider clock + * @reg_offset: offset of the mux/divider register + * @hid_width: number of bits in half integer divider + * @hid_shift: lowest bit of hid value field + * @src_width: number of bits in source select + * @src_shift: lowest bit of source select field + * @div: the divider raw configuration value + * @src: the mux index which will be used if the clock is enabled + * @safe_src: the safe source mux value we switch to, while the main PLL is + * reconfigured + * @safe_div: the safe divider value that we set, while the main PLL is + * reconfigured + * @safe_freq: When switching rates from A to B, the mux div clock will + * instead switch from A -> safe_freq -> B. This allows the + * mux_div clock to change rates while enabled, even if this + * behavior is not supported by the parent clocks. + * If changing the rate of parent A also causes the rate of + * parent B to change, then safe_freq must be defined. + * safe_freq is expected to have a source clock which is always + * on and runs at only one rate. + * @parent_map: pointer to parent_map struct + * @clkr: handle between common and hardware-specific interfaces + */ + +struct clk_regmap_mux_div { + u32 reg_offset; + u32 hid_width; + u32 hid_shift; + u32 src_width; + u32 src_shift; + u32 div; + u32 src; + u32 safe_src; + u32 safe_div; + unsigned long safe_freq; + const struct parent_map *parent_map; + struct clk_regmap clkr; +}; + +extern const struct clk_ops clk_regmap_mux_div_ops; +int __mux_div_set_src_div(struct clk_regmap_mux_div *md, u32 src, u32 div); + +#endif