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_gate.h" 14 #include "ccu_nkmp.h" 15 16 struct _ccu_nkmp { 17 unsigned long n, min_n, max_n; 18 unsigned long k, min_k, max_k; 19 unsigned long m, min_m, max_m; 20 unsigned long p, min_p, max_p; 21 }; 22 23 static unsigned long ccu_nkmp_calc_rate(unsigned long parent, 24 unsigned long n, unsigned long k, 25 unsigned long m, unsigned long p) 26 { 27 u64 rate = parent; 28 29 rate *= n * k; 30 do_div(rate, m * p); 31 32 return rate; 33 } 34 35 static void ccu_nkmp_find_best(unsigned long parent, unsigned long rate, 36 struct _ccu_nkmp *nkmp) 37 { 38 unsigned long best_rate = 0; 39 unsigned long best_n = 0, best_k = 0, best_m = 0, best_p = 0; 40 unsigned long _n, _k, _m, _p; 41 42 for (_k = nkmp->min_k; _k <= nkmp->max_k; _k++) { 43 for (_n = nkmp->min_n; _n <= nkmp->max_n; _n++) { 44 for (_m = nkmp->min_m; _m <= nkmp->max_m; _m++) { 45 for (_p = nkmp->min_p; _p <= nkmp->max_p; _p <<= 1) { 46 unsigned long tmp_rate; 47 48 tmp_rate = ccu_nkmp_calc_rate(parent, 49 _n, _k, 50 _m, _p); 51 52 if (tmp_rate > rate) 53 continue; 54 55 if ((rate - tmp_rate) < (rate - best_rate)) { 56 best_rate = tmp_rate; 57 best_n = _n; 58 best_k = _k; 59 best_m = _m; 60 best_p = _p; 61 } 62 } 63 } 64 } 65 } 66 67 nkmp->n = best_n; 68 nkmp->k = best_k; 69 nkmp->m = best_m; 70 nkmp->p = best_p; 71 } 72 73 static void ccu_nkmp_disable(struct clk_hw *hw) 74 { 75 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw); 76 77 return ccu_gate_helper_disable(&nkmp->common, nkmp->enable); 78 } 79 80 static int ccu_nkmp_enable(struct clk_hw *hw) 81 { 82 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw); 83 84 return ccu_gate_helper_enable(&nkmp->common, nkmp->enable); 85 } 86 87 static int ccu_nkmp_is_enabled(struct clk_hw *hw) 88 { 89 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw); 90 91 return ccu_gate_helper_is_enabled(&nkmp->common, nkmp->enable); 92 } 93 94 static unsigned long ccu_nkmp_recalc_rate(struct clk_hw *hw, 95 unsigned long parent_rate) 96 { 97 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw); 98 unsigned long n, m, k, p, rate; 99 u32 reg; 100 101 reg = readl(nkmp->common.base + nkmp->common.reg); 102 103 n = reg >> nkmp->n.shift; 104 n &= (1 << nkmp->n.width) - 1; 105 n += nkmp->n.offset; 106 if (!n) 107 n++; 108 109 k = reg >> nkmp->k.shift; 110 k &= (1 << nkmp->k.width) - 1; 111 k += nkmp->k.offset; 112 if (!k) 113 k++; 114 115 m = reg >> nkmp->m.shift; 116 m &= (1 << nkmp->m.width) - 1; 117 m += nkmp->m.offset; 118 if (!m) 119 m++; 120 121 p = reg >> nkmp->p.shift; 122 p &= (1 << nkmp->p.width) - 1; 123 124 rate = ccu_nkmp_calc_rate(parent_rate, n, k, m, 1 << p); 125 if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV) 126 rate /= nkmp->fixed_post_div; 127 128 return rate; 129 } 130 131 static long ccu_nkmp_round_rate(struct clk_hw *hw, unsigned long rate, 132 unsigned long *parent_rate) 133 { 134 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw); 135 struct _ccu_nkmp _nkmp; 136 137 if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV) 138 rate *= nkmp->fixed_post_div; 139 140 if (nkmp->max_rate && rate > nkmp->max_rate) { 141 rate = nkmp->max_rate; 142 if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV) 143 rate /= nkmp->fixed_post_div; 144 return rate; 145 } 146 147 _nkmp.min_n = nkmp->n.min ?: 1; 148 _nkmp.max_n = nkmp->n.max ?: 1 << nkmp->n.width; 149 _nkmp.min_k = nkmp->k.min ?: 1; 150 _nkmp.max_k = nkmp->k.max ?: 1 << nkmp->k.width; 151 _nkmp.min_m = 1; 152 _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width; 153 _nkmp.min_p = 1; 154 _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1); 155 156 ccu_nkmp_find_best(*parent_rate, rate, &_nkmp); 157 158 rate = ccu_nkmp_calc_rate(*parent_rate, _nkmp.n, _nkmp.k, 159 _nkmp.m, _nkmp.p); 160 if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV) 161 rate = rate / nkmp->fixed_post_div; 162 163 return rate; 164 } 165 166 static int ccu_nkmp_set_rate(struct clk_hw *hw, unsigned long rate, 167 unsigned long parent_rate) 168 { 169 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw); 170 u32 n_mask, k_mask, m_mask, p_mask; 171 struct _ccu_nkmp _nkmp; 172 unsigned long flags; 173 u32 reg; 174 175 if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV) 176 rate = rate * nkmp->fixed_post_div; 177 178 _nkmp.min_n = nkmp->n.min ?: 1; 179 _nkmp.max_n = nkmp->n.max ?: 1 << nkmp->n.width; 180 _nkmp.min_k = nkmp->k.min ?: 1; 181 _nkmp.max_k = nkmp->k.max ?: 1 << nkmp->k.width; 182 _nkmp.min_m = 1; 183 _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width; 184 _nkmp.min_p = 1; 185 _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1); 186 187 ccu_nkmp_find_best(parent_rate, rate, &_nkmp); 188 189 n_mask = GENMASK(nkmp->n.width + nkmp->n.shift - 1, nkmp->n.shift); 190 k_mask = GENMASK(nkmp->k.width + nkmp->k.shift - 1, nkmp->k.shift); 191 m_mask = GENMASK(nkmp->m.width + nkmp->m.shift - 1, nkmp->m.shift); 192 p_mask = GENMASK(nkmp->p.width + nkmp->p.shift - 1, nkmp->p.shift); 193 194 spin_lock_irqsave(nkmp->common.lock, flags); 195 196 reg = readl(nkmp->common.base + nkmp->common.reg); 197 reg &= ~(n_mask | k_mask | m_mask | p_mask); 198 199 reg |= ((_nkmp.n - nkmp->n.offset) << nkmp->n.shift) & n_mask; 200 reg |= ((_nkmp.k - nkmp->k.offset) << nkmp->k.shift) & k_mask; 201 reg |= ((_nkmp.m - nkmp->m.offset) << nkmp->m.shift) & m_mask; 202 reg |= (ilog2(_nkmp.p) << nkmp->p.shift) & p_mask; 203 204 writel(reg, nkmp->common.base + nkmp->common.reg); 205 206 spin_unlock_irqrestore(nkmp->common.lock, flags); 207 208 ccu_helper_wait_for_lock(&nkmp->common, nkmp->lock); 209 210 return 0; 211 } 212 213 const struct clk_ops ccu_nkmp_ops = { 214 .disable = ccu_nkmp_disable, 215 .enable = ccu_nkmp_enable, 216 .is_enabled = ccu_nkmp_is_enabled, 217 218 .recalc_rate = ccu_nkmp_recalc_rate, 219 .round_rate = ccu_nkmp_round_rate, 220 .set_rate = ccu_nkmp_set_rate, 221 }; 222