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