drivers/clk/sunxi-ng/ccu_nkmp.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * Copyright (C) 2016 Maxime Ripard
   4  * 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
  13 struct _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
  20 static 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
  32 static 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
  72 static 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
  79 static 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
  86 static 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
  93 static 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
 130 static 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
 163 static 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 shifts
 189          * which are equal or greater than width of left operand. This can
 190          * 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
 224 const 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 };
 233 EXPORT_SYMBOL_NS_GPL(ccu_nkmp_ops, SUNXI_CCU);