static int mux_set_parent(struct clk *c, struct clk *p)
{
struct mux_clk *mux = to_mux_clk(c);
int sel = parent_to_src_sel(mux, p);
struct clk *old_parent;
int rc = 0;
unsigned long flags;
if (sel < 0)
return sel;
rc = __clk_pre_reparent(c, p, &flags);
if (rc)
goto out;
rc = mux->ops->set_mux_sel(mux, sel);
if (rc)
goto set_fail;
old_parent = c->parent;
c->parent = p;
__clk_post_reparent(c, old_parent, &flags);
return 0;
set_fail:
__clk_post_reparent(c, p, &flags);
out:
return rc;
}
static int rcg_clk_set_rate(struct clk *c, unsigned long rate)
{
struct clk_freq_tbl *cf, *nf;
struct rcg_clk *rcg = to_rcg_clk(c);
int rc;
unsigned long flags;
for (nf = rcg->freq_tbl; nf->freq_hz != FREQ_END
&& nf->freq_hz != rate; nf++)
;
if (nf->freq_hz == FREQ_END)
return -EINVAL;
cf = rcg->current_freq;
rc = __clk_pre_reparent(c, nf->src_clk, &flags);
if (rc)
return rc;
BUG_ON(!rcg->set_rate);
/* Perform clock-specific frequency switch operations. */
rcg->set_rate(rcg, nf);
rcg->current_freq = nf;
c->parent = nf->src_clk;
__clk_post_reparent(c, cf->src_clk, &flags);
return 0;
}
static int mux_set_parent(struct clk *c, struct clk *p)
{
struct mux_clk *mux = to_mux_clk(c);
int sel = parent_to_src_sel(mux->parents, mux->num_parents, p);
struct clk *old_parent;
int rc = 0, i;
unsigned long flags;
if (sel < 0 && mux->rec_set_par) {
for (i = 0; i < mux->num_parents; i++) {
rc = clk_set_parent(mux->parents[i].src, p);
if (!rc) {
sel = mux->parents[i].sel;
/*
* This is necessary to ensure prepare/enable
* counts get propagated correctly.
*/
p = mux->parents[i].src;
break;
}
}
}
if (sel < 0)
return sel;
rc = __clk_pre_reparent(c, p, &flags);
if (rc)
goto out;
rc = mux->ops->set_mux_sel(mux, sel);
if (rc)
goto set_fail;
old_parent = c->parent;
c->parent = p;
c->rate = clk_get_rate(p);
__clk_post_reparent(c, old_parent, &flags);
return 0;
set_fail:
__clk_post_reparent(c, p, &flags);
out:
return rc;
}
static int mux_set_parent(struct clk *c, struct clk *p)
{
struct mux_clk *mux = to_mux_clk(c);
int sel = mux_parent_to_src_sel(mux, p);
struct clk *old_parent;
int rc = 0, i;
unsigned long flags;
if (sel < 0 && mux->rec_parents) {
for (i = 0; i < mux->num_rec_parents; i++) {
rc = clk_set_parent(mux->rec_parents[i], p);
if (!rc) {
p = mux->rec_parents[i];
sel = mux_parent_to_src_sel(mux, p);
break;
}
}
}
if (sel < 0)
return sel;
rc = __clk_pre_reparent(c, p, &flags);
if (rc)
goto out;
rc = mux->ops->set_mux_sel(mux, sel);
if (rc)
goto set_fail;
old_parent = c->parent;
c->parent = p;
c->rate = clk_get_rate(p);
__clk_post_reparent(c, old_parent, &flags);
return 0;
set_fail:
__clk_post_reparent(c, p, &flags);
out:
return rc;
}
static int mux_div_clk_set_rate(struct clk *c, unsigned long rate)
{
struct mux_div_clk *md = to_mux_div_clk(c);
unsigned long flags, rrate;
unsigned long new_prate, old_prate;
struct clk *old_parent, *new_parent;
u32 new_div, old_div;
int rc;
#if defined(CONFIG_HTC_DEBUG_FOOTPRINT) && defined(CONFIG_MSM_CORTEX_A7)
set_acpuclk_footprint(0, ACPU_BEFORE_SAFE_PARENT_INIT);
#endif
log_clk_call(c, FN_SET_RATE, rate);
rc = safe_parent_init_once(c);
if (rc)
return rc;
rrate = __mux_div_round_rate(c, rate, &new_parent, &new_div,
&new_prate);
if (rrate != rate)
return -EINVAL;
old_parent = c->parent;
old_div = md->data.div;
old_prate = clk_get_rate(c->parent);
#if defined(CONFIG_HTC_DEBUG_FOOTPRINT) && defined(CONFIG_MSM_CORTEX_A7)
set_acpuclk_footprint(0, ACPU_BEFORE_SET_SAFE_RATE);
#endif
if (md->safe_freq)
rc = set_src_div(md, md->safe_parent, md->safe_div);
else if (new_parent == old_parent && new_div >= old_div) {
rc = set_src_div(md, old_parent, new_div);
}
if (rc) {
WARN(rc, "error switching to safe_parent freq=%ld\n", md->safe_freq);
return rc;
}
#if defined(CONFIG_HTC_DEBUG_FOOTPRINT) && defined(CONFIG_MSM_CORTEX_A7)
set_acpuclk_footprint(0, ACPU_BEFORE_SET_PARENT_RATE);
#endif
rc = clk_set_rate(new_parent, new_prate);
if (rc) {
pr_err("failed to set %s to %ld\n",
new_parent->dbg_name, new_prate);
goto err_set_rate;
}
#if defined(CONFIG_HTC_DEBUG_FOOTPRINT) && defined(CONFIG_MSM_CORTEX_A7)
set_acpuclk_footprint(0, ACPU_BEFORE_CLK_PREPARE);
#endif
rc = __clk_pre_reparent(c, new_parent, &flags);
if (rc)
goto err_pre_reparent;
#if defined(CONFIG_HTC_DEBUG_FOOTPRINT) && defined(CONFIG_MSM_CORTEX_A7)
set_acpuclk_footprint(0, ACPU_BEFORE_SET_RATE);
#endif
rc = __set_src_div(md, new_parent, new_div);
if (rc)
goto err_set_src_div;
#if defined(CONFIG_HTC_DEBUG_FOOTPRINT) && defined(CONFIG_MSM_CORTEX_A7)
set_acpuclk_cpu_freq_footprint(FT_CUR_RATE, 0, rrate);
set_acpuclk_footprint(0, ACPU_BEFORE_CLK_UNPREPARE);
#endif
c->parent = new_parent;
__clk_post_reparent(c, old_parent, &flags);
#if defined(CONFIG_HTC_DEBUG_FOOTPRINT) && defined(CONFIG_MSM_CORTEX_A7)
set_acpuclk_footprint(0, ACPU_BEFORE_RETURN);
#endif
return 0;
err_set_src_div:
WARN(rc, "disabling %s\n", new_parent->dbg_name);
#if defined(CONFIG_HTC_DEBUG_FOOTPRINT) && defined(CONFIG_MSM_CORTEX_A7)
set_acpuclk_footprint(0, ACPU_BEFORE_ERR_CLK_UNPREPARE);
#endif
__clk_post_reparent(c, new_parent, &flags);
err_pre_reparent:
#if defined(CONFIG_HTC_DEBUG_FOOTPRINT) && defined(CONFIG_MSM_CORTEX_A7)
set_acpuclk_footprint(0, ACPU_BEFORE_ERR_SET_PARENT_RATE);
#endif
WARN(rc, "%s: error changing parent (%s) rate to %ld\n",
c->dbg_name, old_parent->dbg_name, old_prate);
rc = clk_set_rate(old_parent, old_prate);
err_set_rate:
#if defined(CONFIG_HTC_DEBUG_FOOTPRINT) && defined(CONFIG_MSM_CORTEX_A7)
set_acpuclk_footprint(0, ACPU_BEFORE_ERR_SET_RATE);
#endif
WARN(rc, "%s: error changing back to original div (%d) and parent (%s)\n",
c->dbg_name, old_div, old_parent->dbg_name);
rc = set_src_div(md, old_parent, old_div);
#if defined(CONFIG_HTC_DEBUG_FOOTPRINT) && defined(CONFIG_MSM_CORTEX_A7)
set_acpuclk_footprint(0, ACPU_BEFORE_ERR_RETURN);
#endif
return rc;
}
static int mux_div_clk_set_rate(struct clk *c, unsigned long rate)
{
struct mux_div_clk *md = to_mux_div_clk(c);
unsigned long flags, rrate;
unsigned long new_prate, old_prate;
struct clk *old_parent, *new_parent;
u32 new_div, old_div;
int rc;
rc = safe_parent_init_once(c);
if (rc)
return rc;
rrate = __mux_div_round_rate(c, rate, &new_parent, &new_div,
&new_prate);
if (rrate != rate)
return -EINVAL;
old_parent = c->parent;
old_div = md->data.div;
old_prate = clk_get_rate(c->parent);
/* Refer to the description of safe_freq in clock-generic.h */
if (md->safe_freq)
rc = set_src_div(md, md->safe_parent, md->safe_div);
else if (new_parent == old_parent && new_div >= old_div) {
/*
* If both the parent_rate and divider changes, there may be an
* intermediate frequency generated. Ensure this intermediate
* frequency is less than both the new rate and previous rate.
*/
rc = set_src_div(md, old_parent, new_div);
}
if (rc)
return rc;
rc = clk_set_rate(new_parent, new_prate);
if (rc) {
pr_err("failed to set %s to %ld\n",
new_parent->dbg_name, new_prate);
goto err_set_rate;
}
rc = __clk_pre_reparent(c, new_parent, &flags);
if (rc)
goto err_pre_reparent;
/* Set divider and mux src atomically */
rc = __set_src_div(md, new_parent, new_div);
if (rc)
goto err_set_src_div;
c->parent = new_parent;
__clk_post_reparent(c, old_parent, &flags);
return 0;
err_set_src_div:
/* Not switching to new_parent, so disable it */
__clk_post_reparent(c, new_parent, &flags);
err_pre_reparent:
rc = clk_set_rate(old_parent, old_prate);
WARN(rc, "%s: error changing parent (%s) rate to %ld\n",
c->dbg_name, old_parent->dbg_name, old_prate);
err_set_rate:
rc = set_src_div(md, old_parent, old_div);
WARN(rc, "%s: error changing back to original div (%d) and parent (%s)\n",
c->dbg_name, old_div, old_parent->dbg_name);
return rc;
}
static int mux_div_clk_set_rate(struct clk *c, unsigned long rate)
{
struct mux_div_clk *md = to_mux_div_clk(c);
unsigned long flags, rrate;
unsigned long new_prate, old_prate;
struct clk *old_parent, *new_parent;
u32 new_div, old_div;
int rc;
rc = safe_parent_init_once(c);
if (rc)
return rc;
rrate = __mux_div_round_rate(c, rate, &new_parent, &new_div,
&new_prate);
if (rrate != rate)
return -EINVAL;
old_parent = c->parent;
old_div = md->data.div;
old_prate = clk_get_rate(c->parent);
if (md->safe_freq)
rc = set_src_div(md, md->safe_parent, md->safe_div);
else if (new_parent == old_parent && new_div >= old_div) {
rc = set_src_div(md, old_parent, new_div);
}
if (rc)
return rc;
rc = clk_set_rate(new_parent, new_prate);
if (rc) {
pr_err("failed to set %s to %ld\n",
clk_name(new_parent), new_prate);
goto err_set_rate;
}
rc = __clk_pre_reparent(c, new_parent, &flags);
if (rc)
goto err_pre_reparent;
rc = __set_src_div(md, new_parent, new_div);
if (rc)
goto err_set_src_div;
c->parent = new_parent;
__clk_post_reparent(c, old_parent, &flags);
return 0;
err_set_src_div:
__clk_post_reparent(c, new_parent, &flags);
err_pre_reparent:
rc = clk_set_rate(old_parent, old_prate);
WARN(rc, "%s: error changing parent (%s) rate to %ld\n",
clk_name(c), clk_name(old_parent), old_prate);
err_set_rate:
rc = set_src_div(md, old_parent, old_div);
WARN(rc, "%s: error changing back to original div (%d) and parent (%s)\n",
clk_name(c), old_div, clk_name(old_parent));
return rc;
}
