int qcom_find_src_index(struct clk_hw *hw, const struct parent_map *map, u8 src)
{
int i, num_parents = __clk_get_num_parents(hw->clk);
for (i = 0; i < num_parents; i++)
if (src == map[i].src)
return i;
return -ENOENT;
}
static int pix_rdi_set_parent(struct clk_hw *hw, u8 index)
{
int i;
int ret = 0;
u32 val;
struct clk_pix_rdi *rdi = to_clk_pix_rdi(hw);
struct clk *clk = hw->clk;
int num_parents = __clk_get_num_parents(hw->clk);
/*
* These clocks select three inputs via two muxes. One mux selects
* between csi0 and csi1 and the second mux selects between that mux's
* output and csi2. The source and destination selections for each
* mux must be clocking for the switch to succeed so just turn on
* all three sources because it's easier than figuring out what source
* needs to be on at what time.
*/
for (i = 0; i < num_parents; i++) {
ret = clk_prepare_enable(clk_get_parent_by_index(clk, i));
if (ret)
goto err;
}
if (index == 2)
val = rdi->s2_mask;
else
val = 0;
regmap_update_bits(rdi->clkr.regmap, rdi->s2_reg, rdi->s2_mask, val);
/*
* Wait at least 6 cycles of slowest clock
* for the glitch-free MUX to fully switch sources.
*/
udelay(1);
if (index == 1)
val = rdi->s_mask;
else
val = 0;
regmap_update_bits(rdi->clkr.regmap, rdi->s_reg, rdi->s_mask, val);
/*
* Wait at least 6 cycles of slowest clock
* for the glitch-free MUX to fully switch sources.
*/
udelay(1);
err:
for (i--; i >= 0; i--)
clk_disable_unprepare(clk_get_parent_by_index(clk, i));
return ret;
}
static u8 mux_div_clk_get_parent(struct clk_hw *hw)
{
struct mux_div_clk *md = to_mux_div_clk(hw);
int num_parents = __clk_get_num_parents(hw->clk);
u32 i, div, sel;
md->ops->get_src_div(md, &sel, &div);
md->src_sel = sel;
for (i = 0; i < num_parents; i++)
if (sel == md->parent_map[i])
return i;
WARN(1, "Can't find parent\n");
return -EINVAL;
}
static struct clk *mux_get_safe_parent(struct clk_hw *hw)
{
int i;
struct mux_clk *mux = to_mux_clk(hw);
int num_parents = __clk_get_num_parents(hw->clk);
if (!mux->has_safe_parent)
return NULL;
i = mux->safe_sel;
if (mux->parent_map)
for (i = 0; i < num_parents; i++)
if (mux->safe_sel == mux->parent_map[i])
break;
return clk_get_parent_by_index(hw->clk, i);
}
static u8 mux_get_parent(struct clk_hw *hw)
{
struct mux_clk *mux = to_mux_clk(hw);
int num_parents = __clk_get_num_parents(hw->clk);
int i;
u8 sel;
sel = mux->ops->get_mux_sel(mux);
if (mux->parent_map) {
for (i = 0; i < num_parents; i++)
if (sel == mux->parent_map[i])
return i;
WARN(1, "Can't find parent\n");
return -EINVAL;
}
return sel;
}
static u8 clk_rcg2_get_parent(struct clk_hw *hw)
{
struct clk_rcg2 *rcg = to_clk_rcg2(hw);
int num_parents = __clk_get_num_parents(hw->clk);
u32 cfg;
int i, ret;
ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
if (ret)
return ret;
cfg &= CFG_SRC_SEL_MASK;
cfg >>= CFG_SRC_SEL_SHIFT;
for (i = 0; i < num_parents; i++)
if (cfg == rcg->parent_map[i])
return i;
return -EINVAL;
}
static long __mux_div_round_rate(struct clk_hw *hw, unsigned long rate,
struct clk **best_parent, int *best_div, unsigned long *best_prate)
{
struct mux_div_clk *md = to_mux_div_clk(hw);
unsigned int i;
unsigned long rrate, best = 0, _best_div = 0, _best_prate = 0;
struct clk *_best_parent = 0;
int num_parents = __clk_get_num_parents(hw->clk);
bool set_parent = __clk_get_flags(hw->clk) & CLK_SET_RATE_PARENT;
for (i = 0; i < num_parents; i++) {
int div;
unsigned long prate;
struct clk *p = clk_get_parent_by_index(hw->clk, i);
rrate = __div_round_rate(&md->data, rate, p, &div, &prate,
set_parent);
if (is_better_rate(rate, best, rrate)) {
best = rrate;
_best_div = div;
_best_prate = prate;
_best_parent = p;
}
if (rate <= rrate)
break;
}
if (best_div)
*best_div = _best_div;
if (best_prate)
*best_prate = _best_prate;
if (best_parent)
*best_parent = _best_parent;
if (best)
return best;
return -EINVAL;
}
static long clk_composite_determine_rate(struct clk_hw *hw, unsigned long rate,
unsigned long min_rate,
unsigned long max_rate,
unsigned long *best_parent_rate,
struct clk_hw **best_parent_p)
{
struct clk_composite *composite = to_clk_composite(hw);
const struct clk_ops *rate_ops = composite->rate_ops;
const struct clk_ops *mux_ops = composite->mux_ops;
struct clk_hw *rate_hw = composite->rate_hw;
struct clk_hw *mux_hw = composite->mux_hw;
struct clk *parent;
unsigned long parent_rate;
long tmp_rate, best_rate = 0;
unsigned long rate_diff;
unsigned long best_rate_diff = ULONG_MAX;
int i;
if (rate_hw && rate_ops && rate_ops->determine_rate) {
__clk_hw_set_clk(rate_hw, hw);
return rate_ops->determine_rate(rate_hw, rate, min_rate,
max_rate,
best_parent_rate,
best_parent_p);
} else if (rate_hw && rate_ops && rate_ops->round_rate &&
mux_hw && mux_ops && mux_ops->set_parent) {
*best_parent_p = NULL;
if (__clk_get_flags(hw->clk) & CLK_SET_RATE_NO_REPARENT) {
parent = clk_get_parent(mux_hw->clk);
*best_parent_p = __clk_get_hw(parent);
*best_parent_rate = __clk_get_rate(parent);
return rate_ops->round_rate(rate_hw, rate,
best_parent_rate);
}
for (i = 0; i < __clk_get_num_parents(mux_hw->clk); i++) {
parent = clk_get_parent_by_index(mux_hw->clk, i);
if (!parent)
continue;
parent_rate = __clk_get_rate(parent);
tmp_rate = rate_ops->round_rate(rate_hw, rate,
&parent_rate);
if (tmp_rate < 0)
continue;
rate_diff = abs(rate - tmp_rate);
if (!rate_diff || !*best_parent_p
|| best_rate_diff > rate_diff) {
*best_parent_p = __clk_get_hw(parent);
*best_parent_rate = parent_rate;
best_rate_diff = rate_diff;
best_rate = tmp_rate;
}
if (!rate_diff)
return rate;
}
return best_rate;
} else if (mux_hw && mux_ops && mux_ops->determine_rate) {
__clk_hw_set_clk(mux_hw, hw);
return mux_ops->determine_rate(mux_hw, rate, min_rate,
max_rate, best_parent_rate,
best_parent_p);
} else {
pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n");
return 0;
}
}
