1 /* 2 * Copyright (C) 2016 Maxime Ripard 3 * Maxime Ripard <maxime.ripard@free-electrons.com> 4 * 5 * This program is free software; you can redistribute it and/or 6 * modify it under the terms of the GNU General Public License as 7 * published by the Free Software Foundation; either version 2 of 8 * the License, or (at your option) any later version. 9 */ 10 11 #include <linux/clk-provider.h> 12 13 #include "ccu_frac.h" 14 #include "ccu_gate.h" 15 #include "ccu_nm.h" 16 17 struct _ccu_nm { 18 unsigned long n, min_n, max_n; 19 unsigned long m, min_m, max_m; 20 }; 21 22 static unsigned long ccu_nm_calc_rate(unsigned long parent, 23 unsigned long n, unsigned long m) 24 { 25 u64 rate = parent; 26 27 rate *= n; 28 do_div(rate, m); 29 30 return rate; 31 } 32 33 static void ccu_nm_find_best(unsigned long parent, unsigned long rate, 34 struct _ccu_nm *nm) 35 { 36 unsigned long best_rate = 0; 37 unsigned long best_n = 0, best_m = 0; 38 unsigned long _n, _m; 39 40 for (_n = nm->min_n; _n <= nm->max_n; _n++) { 41 for (_m = nm->min_m; _m <= nm->max_m; _m++) { 42 unsigned long tmp_rate = ccu_nm_calc_rate(parent, 43 _n, _m); 44 45 if (tmp_rate > rate) 46 continue; 47 48 if ((rate - tmp_rate) < (rate - best_rate)) { 49 best_rate = tmp_rate; 50 best_n = _n; 51 best_m = _m; 52 } 53 } 54 } 55 56 nm->n = best_n; 57 nm->m = best_m; 58 } 59 60 static void ccu_nm_disable(struct clk_hw *hw) 61 { 62 struct ccu_nm *nm = hw_to_ccu_nm(hw); 63 64 return ccu_gate_helper_disable(&nm->common, nm->enable); 65 } 66 67 static int ccu_nm_enable(struct clk_hw *hw) 68 { 69 struct ccu_nm *nm = hw_to_ccu_nm(hw); 70 71 return ccu_gate_helper_enable(&nm->common, nm->enable); 72 } 73 74 static int ccu_nm_is_enabled(struct clk_hw *hw) 75 { 76 struct ccu_nm *nm = hw_to_ccu_nm(hw); 77 78 return ccu_gate_helper_is_enabled(&nm->common, nm->enable); 79 } 80 81 static unsigned long ccu_nm_recalc_rate(struct clk_hw *hw, 82 unsigned long parent_rate) 83 { 84 struct ccu_nm *nm = hw_to_ccu_nm(hw); 85 unsigned long rate; 86 unsigned long n, m; 87 u32 reg; 88 89 if (ccu_frac_helper_is_enabled(&nm->common, &nm->frac)) { 90 rate = ccu_frac_helper_read_rate(&nm->common, &nm->frac); 91 92 if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 93 rate /= nm->fixed_post_div; 94 95 return rate; 96 } 97 98 reg = readl(nm->common.base + nm->common.reg); 99 100 n = reg >> nm->n.shift; 101 n &= (1 << nm->n.width) - 1; 102 n += nm->n.offset; 103 if (!n) 104 n++; 105 106 m = reg >> nm->m.shift; 107 m &= (1 << nm->m.width) - 1; 108 m += nm->m.offset; 109 if (!m) 110 m++; 111 112 if (ccu_sdm_helper_is_enabled(&nm->common, &nm->sdm)) 113 rate = ccu_sdm_helper_read_rate(&nm->common, &nm->sdm, m, n); 114 else 115 rate = ccu_nm_calc_rate(parent_rate, n, m); 116 117 if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 118 rate /= nm->fixed_post_div; 119 120 return rate; 121 } 122 123 static long ccu_nm_round_rate(struct clk_hw *hw, unsigned long rate, 124 unsigned long *parent_rate) 125 { 126 struct ccu_nm *nm = hw_to_ccu_nm(hw); 127 struct _ccu_nm _nm; 128 129 if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 130 rate *= nm->fixed_post_div; 131 132 if (rate < nm->min_rate) { 133 rate = nm->min_rate; 134 if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 135 rate /= nm->fixed_post_div; 136 return rate; 137 } 138 139 if (nm->max_rate && rate > nm->max_rate) { 140 rate = nm->max_rate; 141 if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 142 rate /= nm->fixed_post_div; 143 return rate; 144 } 145 146 if (ccu_frac_helper_has_rate(&nm->common, &nm->frac, rate)) { 147 if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 148 rate /= nm->fixed_post_div; 149 return rate; 150 } 151 152 if (ccu_sdm_helper_has_rate(&nm->common, &nm->sdm, rate)) { 153 if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 154 rate /= nm->fixed_post_div; 155 return rate; 156 } 157 158 _nm.min_n = nm->n.min ?: 1; 159 _nm.max_n = nm->n.max ?: 1 << nm->n.width; 160 _nm.min_m = 1; 161 _nm.max_m = nm->m.max ?: 1 << nm->m.width; 162 163 ccu_nm_find_best(*parent_rate, rate, &_nm); 164 rate = ccu_nm_calc_rate(*parent_rate, _nm.n, _nm.m); 165 166 if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 167 rate /= nm->fixed_post_div; 168 169 return rate; 170 } 171 172 static int ccu_nm_set_rate(struct clk_hw *hw, unsigned long rate, 173 unsigned long parent_rate) 174 { 175 struct ccu_nm *nm = hw_to_ccu_nm(hw); 176 struct _ccu_nm _nm; 177 unsigned long flags; 178 u32 reg; 179 180 /* Adjust target rate according to post-dividers */ 181 if (nm->common.features & CCU_FEATURE_FIXED_POSTDIV) 182 rate = rate * nm->fixed_post_div; 183 184 if (ccu_frac_helper_has_rate(&nm->common, &nm->frac, rate)) { 185 spin_lock_irqsave(nm->common.lock, flags); 186 187 /* most SoCs require M to be 0 if fractional mode is used */ 188 reg = readl(nm->common.base + nm->common.reg); 189 reg &= ~GENMASK(nm->m.width + nm->m.shift - 1, nm->m.shift); 190 writel(reg, nm->common.base + nm->common.reg); 191 192 spin_unlock_irqrestore(nm->common.lock, flags); 193 194 ccu_frac_helper_enable(&nm->common, &nm->frac); 195 196 return ccu_frac_helper_set_rate(&nm->common, &nm->frac, 197 rate, nm->lock); 198 } else { 199 ccu_frac_helper_disable(&nm->common, &nm->frac); 200 } 201 202 _nm.min_n = nm->n.min ?: 1; 203 _nm.max_n = nm->n.max ?: 1 << nm->n.width; 204 _nm.min_m = 1; 205 _nm.max_m = nm->m.max ?: 1 << nm->m.width; 206 207 if (ccu_sdm_helper_has_rate(&nm->common, &nm->sdm, rate)) { 208 ccu_sdm_helper_enable(&nm->common, &nm->sdm, rate); 209 210 /* Sigma delta modulation requires specific N and M factors */ 211 ccu_sdm_helper_get_factors(&nm->common, &nm->sdm, rate, 212 &_nm.m, &_nm.n); 213 } else { 214 ccu_sdm_helper_disable(&nm->common, &nm->sdm); 215 ccu_nm_find_best(parent_rate, rate, &_nm); 216 } 217 218 spin_lock_irqsave(nm->common.lock, flags); 219 220 reg = readl(nm->common.base + nm->common.reg); 221 reg &= ~GENMASK(nm->n.width + nm->n.shift - 1, nm->n.shift); 222 reg &= ~GENMASK(nm->m.width + nm->m.shift - 1, nm->m.shift); 223 224 reg |= (_nm.n - nm->n.offset) << nm->n.shift; 225 reg |= (_nm.m - nm->m.offset) << nm->m.shift; 226 writel(reg, nm->common.base + nm->common.reg); 227 228 spin_unlock_irqrestore(nm->common.lock, flags); 229 230 ccu_helper_wait_for_lock(&nm->common, nm->lock); 231 232 return 0; 233 } 234 235 const struct clk_ops ccu_nm_ops = { 236 .disable = ccu_nm_disable, 237 .enable = ccu_nm_enable, 238 .is_enabled = ccu_nm_is_enabled, 239 240 .recalc_rate = ccu_nm_recalc_rate, 241 .round_rate = ccu_nm_round_rate, 242 .set_rate = ccu_nm_set_rate, 243 }; 244