234 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * Copyright (C) 2016 Maxime Ripard4 * Maxime Ripard <maxime.ripard@free-electrons.com>5 */6 7#include <linux/clk-provider.h>8#include <linux/io.h>9 10#include "ccu_gate.h"11#include "ccu_nkmp.h"12 13struct _ccu_nkmp {14 unsigned long n, min_n, max_n;15 unsigned long k, min_k, max_k;16 unsigned long m, min_m, max_m;17 unsigned long p, min_p, max_p;18};19 20static unsigned long ccu_nkmp_calc_rate(unsigned long parent,21 unsigned long n, unsigned long k,22 unsigned long m, unsigned long p)23{24 u64 rate = parent;25 26 rate *= n * k;27 do_div(rate, m * p);28 29 return rate;30}31 32static unsigned long ccu_nkmp_find_best(unsigned long parent, unsigned long rate,33 struct _ccu_nkmp *nkmp)34{35 unsigned long best_rate = 0;36 unsigned long best_n = 0, best_k = 0, best_m = 0, best_p = 0;37 unsigned long _n, _k, _m, _p;38 39 for (_k = nkmp->min_k; _k <= nkmp->max_k; _k++) {40 for (_n = nkmp->min_n; _n <= nkmp->max_n; _n++) {41 for (_m = nkmp->min_m; _m <= nkmp->max_m; _m++) {42 for (_p = nkmp->min_p; _p <= nkmp->max_p; _p <<= 1) {43 unsigned long tmp_rate;44 45 tmp_rate = ccu_nkmp_calc_rate(parent,46 _n, _k,47 _m, _p);48 49 if (tmp_rate > rate)50 continue;51 52 if ((rate - tmp_rate) < (rate - best_rate)) {53 best_rate = tmp_rate;54 best_n = _n;55 best_k = _k;56 best_m = _m;57 best_p = _p;58 }59 }60 }61 }62 }63 64 nkmp->n = best_n;65 nkmp->k = best_k;66 nkmp->m = best_m;67 nkmp->p = best_p;68 69 return best_rate;70}71 72static void ccu_nkmp_disable(struct clk_hw *hw)73{74 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);75 76 return ccu_gate_helper_disable(&nkmp->common, nkmp->enable);77}78 79static int ccu_nkmp_enable(struct clk_hw *hw)80{81 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);82 83 return ccu_gate_helper_enable(&nkmp->common, nkmp->enable);84}85 86static int ccu_nkmp_is_enabled(struct clk_hw *hw)87{88 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);89 90 return ccu_gate_helper_is_enabled(&nkmp->common, nkmp->enable);91}92 93static unsigned long ccu_nkmp_recalc_rate(struct clk_hw *hw,94 unsigned long parent_rate)95{96 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);97 unsigned long n, m, k, p, rate;98 u32 reg;99 100 reg = readl(nkmp->common.base + nkmp->common.reg);101 102 n = reg >> nkmp->n.shift;103 n &= (1 << nkmp->n.width) - 1;104 n += nkmp->n.offset;105 if (!n)106 n++;107 108 k = reg >> nkmp->k.shift;109 k &= (1 << nkmp->k.width) - 1;110 k += nkmp->k.offset;111 if (!k)112 k++;113 114 m = reg >> nkmp->m.shift;115 m &= (1 << nkmp->m.width) - 1;116 m += nkmp->m.offset;117 if (!m)118 m++;119 120 p = reg >> nkmp->p.shift;121 p &= (1 << nkmp->p.width) - 1;122 123 rate = ccu_nkmp_calc_rate(parent_rate, n, k, m, 1 << p);124 if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)125 rate /= nkmp->fixed_post_div;126 127 return rate;128}129 130static long ccu_nkmp_round_rate(struct clk_hw *hw, unsigned long rate,131 unsigned long *parent_rate)132{133 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);134 struct _ccu_nkmp _nkmp;135 136 if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)137 rate *= nkmp->fixed_post_div;138 139 if (nkmp->max_rate && rate > nkmp->max_rate) {140 rate = nkmp->max_rate;141 if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)142 rate /= nkmp->fixed_post_div;143 return rate;144 }145 146 _nkmp.min_n = nkmp->n.min ?: 1;147 _nkmp.max_n = nkmp->n.max ?: 1 << nkmp->n.width;148 _nkmp.min_k = nkmp->k.min ?: 1;149 _nkmp.max_k = nkmp->k.max ?: 1 << nkmp->k.width;150 _nkmp.min_m = 1;151 _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;152 _nkmp.min_p = 1;153 _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);154 155 rate = ccu_nkmp_find_best(*parent_rate, rate, &_nkmp);156 157 if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)158 rate = rate / nkmp->fixed_post_div;159 160 return rate;161}162 163static int ccu_nkmp_set_rate(struct clk_hw *hw, unsigned long rate,164 unsigned long parent_rate)165{166 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);167 u32 n_mask = 0, k_mask = 0, m_mask = 0, p_mask = 0;168 struct _ccu_nkmp _nkmp;169 unsigned long flags;170 u32 reg;171 172 if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)173 rate = rate * nkmp->fixed_post_div;174 175 _nkmp.min_n = nkmp->n.min ?: 1;176 _nkmp.max_n = nkmp->n.max ?: 1 << nkmp->n.width;177 _nkmp.min_k = nkmp->k.min ?: 1;178 _nkmp.max_k = nkmp->k.max ?: 1 << nkmp->k.width;179 _nkmp.min_m = 1;180 _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;181 _nkmp.min_p = 1;182 _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);183 184 ccu_nkmp_find_best(parent_rate, rate, &_nkmp);185 186 /*187 * If width is 0, GENMASK() macro may not generate expected mask (0)188 * as it falls under undefined behaviour by C standard due to shifts189 * which are equal or greater than width of left operand. This can190 * be easily avoided by explicitly checking if width is 0.191 */192 if (nkmp->n.width)193 n_mask = GENMASK(nkmp->n.width + nkmp->n.shift - 1,194 nkmp->n.shift);195 if (nkmp->k.width)196 k_mask = GENMASK(nkmp->k.width + nkmp->k.shift - 1,197 nkmp->k.shift);198 if (nkmp->m.width)199 m_mask = GENMASK(nkmp->m.width + nkmp->m.shift - 1,200 nkmp->m.shift);201 if (nkmp->p.width)202 p_mask = GENMASK(nkmp->p.width + nkmp->p.shift - 1,203 nkmp->p.shift);204 205 spin_lock_irqsave(nkmp->common.lock, flags);206 207 reg = readl(nkmp->common.base + nkmp->common.reg);208 reg &= ~(n_mask | k_mask | m_mask | p_mask);209 210 reg |= ((_nkmp.n - nkmp->n.offset) << nkmp->n.shift) & n_mask;211 reg |= ((_nkmp.k - nkmp->k.offset) << nkmp->k.shift) & k_mask;212 reg |= ((_nkmp.m - nkmp->m.offset) << nkmp->m.shift) & m_mask;213 reg |= (ilog2(_nkmp.p) << nkmp->p.shift) & p_mask;214 215 writel(reg, nkmp->common.base + nkmp->common.reg);216 217 spin_unlock_irqrestore(nkmp->common.lock, flags);218 219 ccu_helper_wait_for_lock(&nkmp->common, nkmp->lock);220 221 return 0;222}223 224const struct clk_ops ccu_nkmp_ops = {225 .disable = ccu_nkmp_disable,226 .enable = ccu_nkmp_enable,227 .is_enabled = ccu_nkmp_is_enabled,228 229 .recalc_rate = ccu_nkmp_recalc_rate,230 .round_rate = ccu_nkmp_round_rate,231 .set_rate = ccu_nkmp_set_rate,232};233EXPORT_SYMBOL_NS_GPL(ccu_nkmp_ops, SUNXI_CCU);234