221 lines · c
1// SPDX-License-Identifier: GPL-2.0+2//3// OWL factor clock driver4//5// Copyright (c) 2014 Actions Semi Inc.6// Author: David Liu <liuwei@actions-semi.com>7//8// Copyright (c) 2018 Linaro Ltd.9// Author: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>10 11#include <linux/clk-provider.h>12#include <linux/regmap.h>13 14#include "owl-factor.h"15 16static unsigned int _get_table_maxval(const struct clk_factor_table *table)17{18 unsigned int maxval = 0;19 const struct clk_factor_table *clkt;20 21 for (clkt = table; clkt->div; clkt++)22 if (clkt->val > maxval)23 maxval = clkt->val;24 return maxval;25}26 27static int _get_table_div_mul(const struct clk_factor_table *table,28 unsigned int val, unsigned int *mul, unsigned int *div)29{30 const struct clk_factor_table *clkt;31 32 for (clkt = table; clkt->div; clkt++) {33 if (clkt->val == val) {34 *mul = clkt->mul;35 *div = clkt->div;36 return 1;37 }38 }39 40 return 0;41}42 43static unsigned int _get_table_val(const struct clk_factor_table *table,44 unsigned long rate, unsigned long parent_rate)45{46 const struct clk_factor_table *clkt;47 int val = -1;48 u64 calc_rate;49 50 for (clkt = table; clkt->div; clkt++) {51 calc_rate = parent_rate * clkt->mul;52 do_div(calc_rate, clkt->div);53 54 if ((unsigned long)calc_rate <= rate) {55 val = clkt->val;56 break;57 }58 }59 60 if (val == -1)61 val = _get_table_maxval(table);62 63 return val;64}65 66static int owl_clk_val_best(const struct owl_factor_hw *factor_hw,67 struct clk_hw *hw, unsigned long rate,68 unsigned long *best_parent_rate)69{70 const struct clk_factor_table *clkt = factor_hw->table;71 unsigned long parent_rate, try_parent_rate, best = 0, cur_rate;72 unsigned long parent_rate_saved = *best_parent_rate;73 int bestval = 0;74 75 if (!rate)76 rate = 1;77 78 if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {79 parent_rate = *best_parent_rate;80 bestval = _get_table_val(clkt, rate, parent_rate);81 return bestval;82 }83 84 for (clkt = factor_hw->table; clkt->div; clkt++) {85 try_parent_rate = rate * clkt->div / clkt->mul;86 87 if (try_parent_rate == parent_rate_saved) {88 pr_debug("%s: [%d %d %d] found try_parent_rate %ld\n",89 __func__, clkt->val, clkt->mul, clkt->div,90 try_parent_rate);91 /*92 * It's the most ideal case if the requested rate can be93 * divided from parent clock without any need to change94 * parent rate, so return the divider immediately.95 */96 *best_parent_rate = parent_rate_saved;97 return clkt->val;98 }99 100 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),101 try_parent_rate);102 cur_rate = DIV_ROUND_UP(parent_rate, clkt->div) * clkt->mul;103 if (cur_rate <= rate && cur_rate > best) {104 bestval = clkt->val;105 best = cur_rate;106 *best_parent_rate = parent_rate;107 }108 }109 110 if (!bestval) {111 bestval = _get_table_maxval(clkt);112 *best_parent_rate = clk_hw_round_rate(113 clk_hw_get_parent(hw), 1);114 }115 116 return bestval;117}118 119long owl_factor_helper_round_rate(struct owl_clk_common *common,120 const struct owl_factor_hw *factor_hw,121 unsigned long rate,122 unsigned long *parent_rate)123{124 const struct clk_factor_table *clkt = factor_hw->table;125 unsigned int val, mul = 0, div = 1;126 127 val = owl_clk_val_best(factor_hw, &common->hw, rate, parent_rate);128 _get_table_div_mul(clkt, val, &mul, &div);129 130 return *parent_rate * mul / div;131}132 133static long owl_factor_round_rate(struct clk_hw *hw, unsigned long rate,134 unsigned long *parent_rate)135{136 struct owl_factor *factor = hw_to_owl_factor(hw);137 struct owl_factor_hw *factor_hw = &factor->factor_hw;138 139 return owl_factor_helper_round_rate(&factor->common, factor_hw,140 rate, parent_rate);141}142 143unsigned long owl_factor_helper_recalc_rate(struct owl_clk_common *common,144 const struct owl_factor_hw *factor_hw,145 unsigned long parent_rate)146{147 const struct clk_factor_table *clkt = factor_hw->table;148 unsigned long long int rate;149 u32 reg, val, mul, div;150 151 div = 0;152 mul = 0;153 154 regmap_read(common->regmap, factor_hw->reg, ®);155 156 val = reg >> factor_hw->shift;157 val &= div_mask(factor_hw);158 159 _get_table_div_mul(clkt, val, &mul, &div);160 if (!div) {161 WARN(!(factor_hw->fct_flags & CLK_DIVIDER_ALLOW_ZERO),162 "%s: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n",163 __clk_get_name(common->hw.clk));164 return parent_rate;165 }166 167 rate = (unsigned long long int)parent_rate * mul;168 do_div(rate, div);169 170 return rate;171}172 173static unsigned long owl_factor_recalc_rate(struct clk_hw *hw,174 unsigned long parent_rate)175{176 struct owl_factor *factor = hw_to_owl_factor(hw);177 struct owl_factor_hw *factor_hw = &factor->factor_hw;178 struct owl_clk_common *common = &factor->common;179 180 return owl_factor_helper_recalc_rate(common, factor_hw, parent_rate);181}182 183int owl_factor_helper_set_rate(const struct owl_clk_common *common,184 const struct owl_factor_hw *factor_hw,185 unsigned long rate,186 unsigned long parent_rate)187{188 u32 val, reg;189 190 val = _get_table_val(factor_hw->table, rate, parent_rate);191 192 if (val > div_mask(factor_hw))193 val = div_mask(factor_hw);194 195 regmap_read(common->regmap, factor_hw->reg, ®);196 197 reg &= ~(div_mask(factor_hw) << factor_hw->shift);198 reg |= val << factor_hw->shift;199 200 regmap_write(common->regmap, factor_hw->reg, reg);201 202 return 0;203}204 205static int owl_factor_set_rate(struct clk_hw *hw, unsigned long rate,206 unsigned long parent_rate)207{208 struct owl_factor *factor = hw_to_owl_factor(hw);209 struct owl_factor_hw *factor_hw = &factor->factor_hw;210 struct owl_clk_common *common = &factor->common;211 212 return owl_factor_helper_set_rate(common, factor_hw,213 rate, parent_rate);214}215 216const struct clk_ops owl_factor_ops = {217 .round_rate = owl_factor_round_rate,218 .recalc_rate = owl_factor_recalc_rate,219 .set_rate = owl_factor_set_rate,220};221