static enum handoff mux_div_clk_handoff(struct clk *c) { struct mux_div_clk *md = to_mux_div_clk(c); unsigned long parent_rate; unsigned int numer; parent_rate = clk_get_rate(c->parent); if (!parent_rate) return HANDOFF_DISABLED_CLK; /* * div values are doubled for half dividers. * Adjust for that by picking a numer of 2. */ numer = md->data.is_half_divider ? 2 : 1; if (md->data.div) { c->rate = mult_frac(parent_rate, numer, md->data.div); } else { c->rate = 0; return HANDOFF_DISABLED_CLK; } if (!md->ops->is_enabled) return HANDOFF_DISABLED_CLK; if (md->ops->is_enabled(md)) return HANDOFF_ENABLED_CLK; return HANDOFF_DISABLED_CLK; }
static void mux_div_clk_disable(struct clk_hw *hw) { struct mux_div_clk *md = to_mux_div_clk(hw); if (md->ops->disable) return md->ops->disable(md); }
static int mux_div_clk_set_rate(struct clk_hw *hw, unsigned long rate, unsigned long parent_rate) { struct mux_div_clk *md = to_mux_div_clk(hw); return __mux_div_clk_set_rate_and_parent(hw, md->src_sel, parent_rate / rate); }
static int mux_div_clk_enable(struct clk_hw *hw) { struct mux_div_clk *md = to_mux_div_clk(hw); if (md->ops->enable) return md->ops->enable(md); return 0; }
static void mux_div_clk_disable(struct clk *c) { struct mux_div_clk *md = to_mux_div_clk(c); log_clk_call(c, FN_DISABLE, 300000000); if (md->ops->disable) return md->ops->disable(md); }
static int mux_div_clk_enable(struct clk *c) { struct mux_div_clk *md = to_mux_div_clk(c); log_clk_call(c, FN_ENABLE, c->rate); if (md->ops->enable) return md->ops->enable(md); return 0; }
static void __iomem *mux_div_clk_list_registers(struct clk *c, int n, struct clk_register_data **regs, u32 *size) { struct mux_div_clk *md = to_mux_div_clk(c); if (md->ops && md->ops->list_registers) return md->ops->list_registers(md, n , regs, size); return ERR_PTR(-EINVAL); }
static unsigned long mux_div_clk_recalc_rate(struct clk_hw *hw, unsigned long prate) { struct mux_div_clk *md = to_mux_div_clk(hw); u32 div, sel; md->ops->get_src_div(md, &sel, &div); return prate / div; }
static int __mux_div_clk_set_rate_and_parent(struct clk_hw *hw, u8 index, u32 div) { struct mux_div_clk *md = to_mux_div_clk(hw); int rc; rc = safe_parent_init_once(hw); if (rc) return rc; return __set_src_div(md, index, div); }
static enum handoff mux_div_clk_handoff(struct clk *c) { struct mux_div_clk *md = to_mux_div_clk(c); unsigned long parent_rate; parent_rate = clk_get_rate(c->parent); c->rate = parent_rate / md->data.div; if (!md->ops->is_enabled) return HANDOFF_DISABLED_CLK; if (md->ops->is_enabled(md)) return HANDOFF_ENABLED_CLK; return HANDOFF_DISABLED_CLK; }
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_div_clk_get_parent(struct clk *c) { struct mux_div_clk *md = to_mux_div_clk(c); u32 i, div, src_sel; md->ops->get_src_div(md, &src_sel, &div); md->data.div = div; md->src_sel = src_sel; for (i = 0; i < md->num_parents; i++) { if (md->parents[i].sel == src_sel) return md->parents[i].src; } return NULL; }
static void log_clk_call(struct clk *c, u32 fn, u32 rate) { struct mux_div_clk *md = to_mux_div_clk(c); struct clk_logger *log; unsigned long flags; spin_lock_irqsave(&log_lock, flags); log = &clk_log[buf_index]; log->timestamp = sched_clock(); log->fn = fn; log->cmd_reg = readl_relaxed(md->base + md->div_offset - 0x4); log->cfg_reg = readl_relaxed(md->base + md->div_offset); log->rate = rate; buf_index++; if (buf_index >= BUF_SIZE) buf_index = 0; spin_unlock_irqrestore(&log_lock, flags); }
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 __mux_div_round_rate(struct clk *c, unsigned long rate, struct clk **best_parent, int *best_div, unsigned long *best_prate) { struct mux_div_clk *md = to_mux_div_clk(c); unsigned int i; unsigned long rrate, best = 0, _best_div = 0, _best_prate = 0; struct clk *_best_parent = 0; for (i = 0; i < md->num_parents; i++) { int div; unsigned long prate; rrate = __div_round_rate(&md->data, rate, md->parents[i].src, &div, &prate); if (is_better_rate(rate, best, rrate)) { best = rrate; _best_div = div; _best_prate = prate; _best_parent = md->parents[i].src; } if (rate <= rrate && rrate <= rate + md->data.rate_margin) 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 int safe_parent_init_once(struct clk *c) { unsigned long rrate; u32 best_div; struct clk *best_parent; struct mux_div_clk *md = to_mux_div_clk(c); if (IS_ERR(md->safe_parent)) return -EINVAL; if (!md->safe_freq || md->safe_parent) return 0; rrate = __mux_div_round_rate(c, md->safe_freq, &best_parent, &best_div, NULL); if (rrate == md->safe_freq) { md->safe_div = best_div; md->safe_parent = best_parent; } else { md->safe_parent = ERR_PTR(-EINVAL); return -EINVAL; } return 0; }
static enum handoff mux_div_clk_handoff(struct clk *c) { struct mux_div_clk *md = to_mux_div_clk(c); unsigned long parent_rate; unsigned int numer; parent_rate = clk_get_rate(c->parent); if (!parent_rate) return HANDOFF_DISABLED_CLK; numer = md->data.is_half_divider ? 2 : 1; if (md->data.div) { c->rate = mult_frac(parent_rate, numer, md->data.div); } else { c->rate = 0; return HANDOFF_DISABLED_CLK; } if (!md->ops->is_enabled) return HANDOFF_DISABLED_CLK; if (md->ops->is_enabled(md)) return HANDOFF_ENABLED_CLK; return HANDOFF_DISABLED_CLK; }
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_parent(struct clk_hw *hw, u8 index) { struct mux_div_clk *md = to_mux_div_clk(hw); return __mux_div_clk_set_rate_and_parent(hw, md->parent_map[index], md->data.div); }
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; }