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 67 static int owl_clk_val_best(const struct owl_factor_hw *factor_hw, 68 struct clk_hw *hw, unsigned long rate, 69 unsigned long *best_parent_rate) 70 { 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 128 val = owl_clk_val_best(factor_hw, &common->hw, rate, parent_rate); 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 }; 222