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 14 #include "owl-factor.h" 15 16 static 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 27 static 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 43 static 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 66 static 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 be 93 * divided from parent clock without any need to change 94 * 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 119 long 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 133 static 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 143 unsigned 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 173 static 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 183 int 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 205 static 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 216 const 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