clk/sunxi-ng/ccu_nkmp.c

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003  * Copyright (C) 2016 Maxime Ripard
0004  * Maxime Ripard <maxime.ripard@free-electrons.com>
0005  */
0006
0007 #include <linux/clk-provider.h>
0008 #include <linux/io.h>
0009
0010 #include "ccu_gate.h"
0011 #include "ccu_nkmp.h"
0012
0013 struct _ccu_nkmp {
0014     unsigned long   n, min_n, max_n;
0015     unsigned long   k, min_k, max_k;
0016     unsigned long   m, min_m, max_m;
0017     unsigned long   p, min_p, max_p;
0018 };
0019
0020 static unsigned long ccu_nkmp_calc_rate(unsigned long parent,
0021                     unsigned long n, unsigned long k,
0022                     unsigned long m, unsigned long p)
0023 {
0024     u64 rate = parent;
0025
0026     rate *= n * k;
0027     do_div(rate, m * p);
0028
0029     return rate;
0030 }
0031
0032 static void ccu_nkmp_find_best(unsigned long parent, unsigned long rate,
0033                    struct _ccu_nkmp *nkmp)
0034 {
0035     unsigned long best_rate = 0;
0036     unsigned long best_n = 0, best_k = 0, best_m = 0, best_p = 0;
0037     unsigned long _n, _k, _m, _p;
0038
0039     for (_k = nkmp->min_k; _k <= nkmp->max_k; _k++) {
0040         for (_n = nkmp->min_n; _n <= nkmp->max_n; _n++) {
0041             for (_m = nkmp->min_m; _m <= nkmp->max_m; _m++) {
0042                 for (_p = nkmp->min_p; _p <= nkmp->max_p; _p <<= 1) {
0043                     unsigned long tmp_rate;
0044
0045                     tmp_rate = ccu_nkmp_calc_rate(parent,
0046                                       _n, _k,
0047                                       _m, _p);
0048
0049                     if (tmp_rate > rate)
0050                         continue;
0051
0052                     if ((rate - tmp_rate) < (rate - best_rate)) {
0053                         best_rate = tmp_rate;
0054                         best_n = _n;
0055                         best_k = _k;
0056                         best_m = _m;
0057                         best_p = _p;
0058                     }
0059                 }
0060             }
0061         }
0062     }
0063
0064     nkmp->n = best_n;
0065     nkmp->k = best_k;
0066     nkmp->m = best_m;
0067     nkmp->p = best_p;
0068 }
0069
0070 static void ccu_nkmp_disable(struct clk_hw *hw)
0071 {
0072     struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
0073
0074     return ccu_gate_helper_disable(&nkmp->common, nkmp->enable);
0075 }
0076
0077 static int ccu_nkmp_enable(struct clk_hw *hw)
0078 {
0079     struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
0080
0081     return ccu_gate_helper_enable(&nkmp->common, nkmp->enable);
0082 }
0083
0084 static int ccu_nkmp_is_enabled(struct clk_hw *hw)
0085 {
0086     struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
0087
0088     return ccu_gate_helper_is_enabled(&nkmp->common, nkmp->enable);
0089 }
0090
0091 static unsigned long ccu_nkmp_recalc_rate(struct clk_hw *hw,
0092                     unsigned long parent_rate)
0093 {
0094     struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
0095     unsigned long n, m, k, p, rate;
0096     u32 reg;
0097
0098     reg = readl(nkmp->common.base + nkmp->common.reg);
0099
0100     n = reg >> nkmp->n.shift;
0101     n &= (1 << nkmp->n.width) - 1;
0102     n += nkmp->n.offset;
0103     if (!n)
0104         n++;
0105
0106     k = reg >> nkmp->k.shift;
0107     k &= (1 << nkmp->k.width) - 1;
0108     k += nkmp->k.offset;
0109     if (!k)
0110         k++;
0111
0112     m = reg >> nkmp->m.shift;
0113     m &= (1 << nkmp->m.width) - 1;
0114     m += nkmp->m.offset;
0115     if (!m)
0116         m++;
0117
0118     p = reg >> nkmp->p.shift;
0119     p &= (1 << nkmp->p.width) - 1;
0120
0121     rate = ccu_nkmp_calc_rate(parent_rate, n, k, m, 1 << p);
0122     if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
0123         rate /= nkmp->fixed_post_div;
0124
0125     return rate;
0126 }
0127
0128 static long ccu_nkmp_round_rate(struct clk_hw *hw, unsigned long rate,
0129                   unsigned long *parent_rate)
0130 {
0131     struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
0132     struct _ccu_nkmp _nkmp;
0133
0134     if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
0135         rate *= nkmp->fixed_post_div;
0136
0137     if (nkmp->max_rate && rate > nkmp->max_rate) {
0138         rate = nkmp->max_rate;
0139         if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
0140             rate /= nkmp->fixed_post_div;
0141         return rate;
0142     }
0143
0144     _nkmp.min_n = nkmp->n.min ?: 1;
0145     _nkmp.max_n = nkmp->n.max ?: 1 << nkmp->n.width;
0146     _nkmp.min_k = nkmp->k.min ?: 1;
0147     _nkmp.max_k = nkmp->k.max ?: 1 << nkmp->k.width;
0148     _nkmp.min_m = 1;
0149     _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
0150     _nkmp.min_p = 1;
0151     _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
0152
0153     ccu_nkmp_find_best(*parent_rate, rate, &_nkmp);
0154
0155     rate = ccu_nkmp_calc_rate(*parent_rate, _nkmp.n, _nkmp.k,
0156                   _nkmp.m, _nkmp.p);
0157     if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
0158         rate = rate / nkmp->fixed_post_div;
0159
0160     return rate;
0161 }
0162
0163 static int ccu_nkmp_set_rate(struct clk_hw *hw, unsigned long rate,
0164                unsigned long parent_rate)
0165 {
0166     struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
0167     u32 n_mask = 0, k_mask = 0, m_mask = 0, p_mask = 0;
0168     struct _ccu_nkmp _nkmp;
0169     unsigned long flags;
0170     u32 reg;
0171
0172     if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
0173         rate = rate * nkmp->fixed_post_div;
0174
0175     _nkmp.min_n = nkmp->n.min ?: 1;
0176     _nkmp.max_n = nkmp->n.max ?: 1 << nkmp->n.width;
0177     _nkmp.min_k = nkmp->k.min ?: 1;
0178     _nkmp.max_k = nkmp->k.max ?: 1 << nkmp->k.width;
0179     _nkmp.min_m = 1;
0180     _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
0181     _nkmp.min_p = 1;
0182     _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
0183
0184     ccu_nkmp_find_best(parent_rate, rate, &_nkmp);
0185
0186     /*
0187      * If width is 0, GENMASK() macro may not generate expected mask (0)
0188      * as it falls under undefined behaviour by C standard due to shifts
0189      * which are equal or greater than width of left operand. This can
0190      * be easily avoided by explicitly checking if width is 0.
0191      */
0192     if (nkmp->n.width)
0193         n_mask = GENMASK(nkmp->n.width + nkmp->n.shift - 1,
0194                  nkmp->n.shift);
0195     if (nkmp->k.width)
0196         k_mask = GENMASK(nkmp->k.width + nkmp->k.shift - 1,
0197                  nkmp->k.shift);
0198     if (nkmp->m.width)
0199         m_mask = GENMASK(nkmp->m.width + nkmp->m.shift - 1,
0200                  nkmp->m.shift);
0201     if (nkmp->p.width)
0202         p_mask = GENMASK(nkmp->p.width + nkmp->p.shift - 1,
0203                  nkmp->p.shift);
0204
0205     spin_lock_irqsave(nkmp->common.lock, flags);
0206
0207     reg = readl(nkmp->common.base + nkmp->common.reg);
0208     reg &= ~(n_mask | k_mask | m_mask | p_mask);
0209
0210     reg |= ((_nkmp.n - nkmp->n.offset) << nkmp->n.shift) & n_mask;
0211     reg |= ((_nkmp.k - nkmp->k.offset) << nkmp->k.shift) & k_mask;
0212     reg |= ((_nkmp.m - nkmp->m.offset) << nkmp->m.shift) & m_mask;
0213     reg |= (ilog2(_nkmp.p) << nkmp->p.shift) & p_mask;
0214
0215     writel(reg, nkmp->common.base + nkmp->common.reg);
0216
0217     spin_unlock_irqrestore(nkmp->common.lock, flags);
0218
0219     ccu_helper_wait_for_lock(&nkmp->common, nkmp->lock);
0220
0221     return 0;
0222 }
0223
0224 const struct clk_ops ccu_nkmp_ops = {
0225     .disable    = ccu_nkmp_disable,
0226     .enable     = ccu_nkmp_enable,
0227     .is_enabled = ccu_nkmp_is_enabled,
0228
0229     .recalc_rate    = ccu_nkmp_recalc_rate,
0230     .round_rate = ccu_nkmp_round_rate,
0231     .set_rate   = ccu_nkmp_set_rate,
0232 };
0233 EXPORT_SYMBOL_NS_GPL(ccu_nkmp_ops, SUNXI_CCU);