diff options
Diffstat (limited to 'drivers/clk/bcm/clk-iproc-pll.c')
-rw-r--r-- | drivers/clk/bcm/clk-iproc-pll.c | 97 |
1 files changed, 82 insertions, 15 deletions
diff --git a/drivers/clk/bcm/clk-iproc-pll.c b/drivers/clk/bcm/clk-iproc-pll.c index 375d8dd80d45..9514ecf319b0 100644 --- a/drivers/clk/bcm/clk-iproc-pll.c +++ b/drivers/clk/bcm/clk-iproc-pll.c @@ -95,6 +95,39 @@ struct iproc_pll { #define to_iproc_clk(hw) container_of(hw, struct iproc_clk, hw) +static int pll_calc_param(unsigned long target_rate, + unsigned long parent_rate, + struct iproc_pll_vco_param *vco_out) +{ + u64 ndiv_int, ndiv_frac, residual; + + ndiv_int = target_rate / parent_rate; + + if (!ndiv_int || (ndiv_int > 255)) + return -EINVAL; + + residual = target_rate - (ndiv_int * parent_rate); + residual <<= 20; + + /* + * Add half of the divisor so the result will be rounded to closest + * instead of rounded down. + */ + residual += (parent_rate / 2); + ndiv_frac = div64_u64((u64)residual, (u64)parent_rate); + + vco_out->ndiv_int = ndiv_int; + vco_out->ndiv_frac = ndiv_frac; + vco_out->pdiv = 1; + + vco_out->rate = vco_out->ndiv_int * parent_rate; + residual = (u64)vco_out->ndiv_frac * (u64)parent_rate; + residual >>= 20; + vco_out->rate += residual; + + return 0; +} + /* * Based on the target frequency, find a match from the VCO frequency parameter * table and return its index @@ -252,11 +285,10 @@ static void __pll_bring_out_reset(struct iproc_pll *pll, unsigned int kp, iproc_pll_write(pll, pll->control_base, reset->offset, val); } -static int pll_set_rate(struct iproc_clk *clk, unsigned int rate_index, +static int pll_set_rate(struct iproc_clk *clk, struct iproc_pll_vco_param *vco, unsigned long parent_rate) { struct iproc_pll *pll = clk->pll; - const struct iproc_pll_vco_param *vco = &pll->vco_param[rate_index]; const struct iproc_pll_ctrl *ctrl = pll->ctrl; int ka = 0, ki, kp, ret; unsigned long rate = vco->rate; @@ -431,25 +463,50 @@ static unsigned long iproc_pll_recalc_rate(struct clk_hw *hw, return clk->rate; } -static long iproc_pll_round_rate(struct clk_hw *hw, unsigned long rate, - unsigned long *parent_rate) +static int iproc_pll_determine_rate(struct clk_hw *hw, + struct clk_rate_request *req) { - unsigned i; + unsigned int i; struct iproc_clk *clk = to_iproc_clk(hw); struct iproc_pll *pll = clk->pll; + const struct iproc_pll_ctrl *ctrl = pll->ctrl; + unsigned long diff, best_diff; + unsigned int best_idx = 0; + int ret; - if (rate == 0 || *parent_rate == 0 || !pll->vco_param) + if (req->rate == 0 || req->best_parent_rate == 0) return -EINVAL; + if (ctrl->flags & IPROC_CLK_PLL_CALC_PARAM) { + struct iproc_pll_vco_param vco_param; + + ret = pll_calc_param(req->rate, req->best_parent_rate, + &vco_param); + if (ret) + return ret; + + req->rate = vco_param.rate; + return 0; + } + + if (!pll->vco_param) + return -EINVAL; + + best_diff = ULONG_MAX; for (i = 0; i < pll->num_vco_entries; i++) { - if (rate <= pll->vco_param[i].rate) + diff = abs(req->rate - pll->vco_param[i].rate); + if (diff <= best_diff) { + best_diff = diff; + best_idx = i; + } + /* break now if perfect match */ + if (diff == 0) break; } - if (i == pll->num_vco_entries) - i--; + req->rate = pll->vco_param[best_idx].rate; - return pll->vco_param[i].rate; + return 0; } static int iproc_pll_set_rate(struct clk_hw *hw, unsigned long rate, @@ -457,13 +514,23 @@ static int iproc_pll_set_rate(struct clk_hw *hw, unsigned long rate, { struct iproc_clk *clk = to_iproc_clk(hw); struct iproc_pll *pll = clk->pll; + const struct iproc_pll_ctrl *ctrl = pll->ctrl; + struct iproc_pll_vco_param vco_param; int rate_index, ret; - rate_index = pll_get_rate_index(pll, rate); - if (rate_index < 0) - return rate_index; + if (ctrl->flags & IPROC_CLK_PLL_CALC_PARAM) { + ret = pll_calc_param(rate, parent_rate, &vco_param); + if (ret) + return ret; + } else { + rate_index = pll_get_rate_index(pll, rate); + if (rate_index < 0) + return rate_index; + + vco_param = pll->vco_param[rate_index]; + } - ret = pll_set_rate(clk, rate_index, parent_rate); + ret = pll_set_rate(clk, &vco_param, parent_rate); return ret; } @@ -471,7 +538,7 @@ static const struct clk_ops iproc_pll_ops = { .enable = iproc_pll_enable, .disable = iproc_pll_disable, .recalc_rate = iproc_pll_recalc_rate, - .round_rate = iproc_pll_round_rate, + .determine_rate = iproc_pll_determine_rate, .set_rate = iproc_pll_set_rate, }; |