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