clk/actions/owl-factor.c

0001 // SPDX-License-Identifier: GPL-2.0+
0002 //
0003 // OWL factor clock driver
0004 //
0005 // Copyright (c) 2014 Actions Semi Inc.
0006 // Author: David Liu <liuwei@actions-semi.com>
0007 //
0008 // Copyright (c) 2018 Linaro Ltd.
0009 // Author: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>
0010
0011 #include <linux/clk-provider.h>
0012 #include <linux/regmap.h>
0013
0014 #include "owl-factor.h"
0015
0016 static unsigned int _get_table_maxval(const struct clk_factor_table *table)
0017 {
0018     unsigned int maxval = 0;
0019     const struct clk_factor_table *clkt;
0020
0021     for (clkt = table; clkt->div; clkt++)
0022         if (clkt->val > maxval)
0023             maxval = clkt->val;
0024     return maxval;
0025 }
0026
0027 static int _get_table_div_mul(const struct clk_factor_table *table,
0028             unsigned int val, unsigned int *mul, unsigned int *div)
0029 {
0030     const struct clk_factor_table *clkt;
0031
0032     for (clkt = table; clkt->div; clkt++) {
0033         if (clkt->val == val) {
0034             *mul = clkt->mul;
0035             *div = clkt->div;
0036             return 1;
0037         }
0038     }
0039
0040     return 0;
0041 }
0042
0043 static unsigned int _get_table_val(const struct clk_factor_table *table,
0044             unsigned long rate, unsigned long parent_rate)
0045 {
0046     const struct clk_factor_table *clkt;
0047     int val = -1;
0048     u64 calc_rate;
0049
0050     for (clkt = table; clkt->div; clkt++) {
0051         calc_rate = parent_rate * clkt->mul;
0052         do_div(calc_rate, clkt->div);
0053
0054         if ((unsigned long)calc_rate <= rate) {
0055             val = clkt->val;
0056             break;
0057         }
0058     }
0059
0060     if (val == -1)
0061         val = _get_table_maxval(table);
0062
0063     return val;
0064 }
0065
0066 static int owl_clk_val_best(const struct owl_factor_hw *factor_hw,
0067             struct clk_hw *hw, unsigned long rate,
0068             unsigned long *best_parent_rate)
0069 {
0070     const struct clk_factor_table *clkt = factor_hw->table;
0071     unsigned long parent_rate, try_parent_rate, best = 0, cur_rate;
0072     unsigned long parent_rate_saved = *best_parent_rate;
0073     int bestval = 0;
0074
0075     if (!rate)
0076         rate = 1;
0077
0078     if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
0079         parent_rate = *best_parent_rate;
0080         bestval = _get_table_val(clkt, rate, parent_rate);
0081         return bestval;
0082     }
0083
0084     for (clkt = factor_hw->table; clkt->div; clkt++) {
0085         try_parent_rate = rate * clkt->div / clkt->mul;
0086
0087         if (try_parent_rate == parent_rate_saved) {
0088             pr_debug("%s: [%d %d %d] found try_parent_rate %ld\n",
0089                 __func__, clkt->val, clkt->mul, clkt->div,
0090                 try_parent_rate);
0091             /*
0092              * It's the most ideal case if the requested rate can be
0093              * divided from parent clock without any need to change
0094              * parent rate, so return the divider immediately.
0095              */
0096             *best_parent_rate = parent_rate_saved;
0097             return clkt->val;
0098         }
0099
0100         parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
0101                 try_parent_rate);
0102         cur_rate = DIV_ROUND_UP(parent_rate, clkt->div) * clkt->mul;
0103         if (cur_rate <= rate && cur_rate > best) {
0104             bestval = clkt->val;
0105             best = cur_rate;
0106             *best_parent_rate = parent_rate;
0107         }
0108     }
0109
0110     if (!bestval) {
0111         bestval = _get_table_maxval(clkt);
0112         *best_parent_rate = clk_hw_round_rate(
0113                 clk_hw_get_parent(hw), 1);
0114     }
0115
0116     return bestval;
0117 }
0118
0119 long owl_factor_helper_round_rate(struct owl_clk_common *common,
0120                 const struct owl_factor_hw *factor_hw,
0121                 unsigned long rate,
0122                 unsigned long *parent_rate)
0123 {
0124     const struct clk_factor_table *clkt = factor_hw->table;
0125     unsigned int val, mul = 0, div = 1;
0126
0127     val = owl_clk_val_best(factor_hw, &common->hw, rate, parent_rate);
0128     _get_table_div_mul(clkt, val, &mul, &div);
0129
0130     return *parent_rate * mul / div;
0131 }
0132
0133 static long owl_factor_round_rate(struct clk_hw *hw, unsigned long rate,
0134             unsigned long *parent_rate)
0135 {
0136     struct owl_factor *factor = hw_to_owl_factor(hw);
0137     struct owl_factor_hw *factor_hw = &factor->factor_hw;
0138
0139     return owl_factor_helper_round_rate(&factor->common, factor_hw,
0140                     rate, parent_rate);
0141 }
0142
0143 unsigned long owl_factor_helper_recalc_rate(struct owl_clk_common *common,
0144                      const struct owl_factor_hw *factor_hw,
0145                      unsigned long parent_rate)
0146 {
0147     const struct clk_factor_table *clkt = factor_hw->table;
0148     unsigned long long int rate;
0149     u32 reg, val, mul, div;
0150
0151     div = 0;
0152     mul = 0;
0153
0154     regmap_read(common->regmap, factor_hw->reg, &reg);
0155
0156     val = reg >> factor_hw->shift;
0157     val &= div_mask(factor_hw);
0158
0159     _get_table_div_mul(clkt, val, &mul, &div);
0160     if (!div) {
0161         WARN(!(factor_hw->fct_flags & CLK_DIVIDER_ALLOW_ZERO),
0162             "%s: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n",
0163             __clk_get_name(common->hw.clk));
0164         return parent_rate;
0165     }
0166
0167     rate = (unsigned long long int)parent_rate * mul;
0168     do_div(rate, div);
0169
0170     return rate;
0171 }
0172
0173 static unsigned long owl_factor_recalc_rate(struct clk_hw *hw,
0174             unsigned long parent_rate)
0175 {
0176     struct owl_factor *factor = hw_to_owl_factor(hw);
0177     struct owl_factor_hw *factor_hw = &factor->factor_hw;
0178     struct owl_clk_common *common = &factor->common;
0179
0180     return owl_factor_helper_recalc_rate(common, factor_hw, parent_rate);
0181 }
0182
0183 int owl_factor_helper_set_rate(const struct owl_clk_common *common,
0184                 const struct owl_factor_hw *factor_hw,
0185                 unsigned long rate,
0186                 unsigned long parent_rate)
0187 {
0188     u32 val, reg;
0189
0190     val = _get_table_val(factor_hw->table, rate, parent_rate);
0191
0192     if (val > div_mask(factor_hw))
0193         val = div_mask(factor_hw);
0194
0195     regmap_read(common->regmap, factor_hw->reg, &reg);
0196
0197     reg &= ~(div_mask(factor_hw) << factor_hw->shift);
0198     reg |= val << factor_hw->shift;
0199
0200     regmap_write(common->regmap, factor_hw->reg, reg);
0201
0202     return 0;
0203 }
0204
0205 static int owl_factor_set_rate(struct clk_hw *hw, unsigned long rate,
0206                    unsigned long parent_rate)
0207 {
0208     struct owl_factor *factor = hw_to_owl_factor(hw);
0209     struct owl_factor_hw *factor_hw = &factor->factor_hw;
0210     struct owl_clk_common *common = &factor->common;
0211
0212     return owl_factor_helper_set_rate(common, factor_hw,
0213                     rate, parent_rate);
0214 }
0215
0216 const struct clk_ops owl_factor_ops = {
0217     .round_rate = owl_factor_round_rate,
0218     .recalc_rate    = owl_factor_recalc_rate,
0219     .set_rate   = owl_factor_set_rate,
0220 };