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 void 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
  70 static void ccu_nkmp_disable(struct clk_hw *hw)
  71 {
  72         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
  73
  74         return ccu_gate_helper_disable(&nkmp->common, nkmp->enable);
  75 }
  76
  77 static int ccu_nkmp_enable(struct clk_hw *hw)
  78 {
  79         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
  80
  81         return ccu_gate_helper_enable(&nkmp->common, nkmp->enable);
  82 }
  83
  84 static int ccu_nkmp_is_enabled(struct clk_hw *hw)
  85 {
  86         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
  87
  88         return ccu_gate_helper_is_enabled(&nkmp->common, nkmp->enable);
  89 }
  90
  91 static unsigned long ccu_nkmp_recalc_rate(struct clk_hw *hw,
  92                                         unsigned long parent_rate)
  93 {
  94         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
  95         unsigned long n, m, k, p, rate;
  96         u32 reg;
  97
  98         reg = readl(nkmp->common.base + nkmp->common.reg);
  99
 100         n = reg >> nkmp->n.shift;
 101         n &= (1 << nkmp->n.width) - 1;
 102         n += nkmp->n.offset;
 103         if (!n)
 104                 n++;
 105
 106         k = reg >> nkmp->k.shift;
 107         k &= (1 << nkmp->k.width) - 1;
 108         k += nkmp->k.offset;
 109         if (!k)
 110                 k++;
 111
 112         m = reg >> nkmp->m.shift;
 113         m &= (1 << nkmp->m.width) - 1;
 114         m += nkmp->m.offset;
 115         if (!m)
 116                 m++;
 117
 118         p = reg >> nkmp->p.shift;
 119         p &= (1 << nkmp->p.width) - 1;
 120
 121         rate = ccu_nkmp_calc_rate(parent_rate, n, k, m, 1 << p);
 122         if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
 123                 rate /= nkmp->fixed_post_div;
 124
 125         return rate;
 126 }
 127
 128 static long ccu_nkmp_round_rate(struct clk_hw *hw, unsigned long rate,
 129                               unsigned long *parent_rate)
 130 {
 131         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
 132         struct _ccu_nkmp _nkmp;
 133
 134         if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
 135                 rate *= nkmp->fixed_post_div;
 136
 137         if (nkmp->max_rate && rate > nkmp->max_rate) {
 138                 rate = nkmp->max_rate;
 139                 if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
 140                         rate /= nkmp->fixed_post_div;
 141                 return rate;
 142         }
 143
 144         _nkmp.min_n = nkmp->n.min ?: 1;
 145         _nkmp.max_n = nkmp->n.max ?: 1 << nkmp->n.width;
 146         _nkmp.min_k = nkmp->k.min ?: 1;
 147         _nkmp.max_k = nkmp->k.max ?: 1 << nkmp->k.width;
 148         _nkmp.min_m = 1;
 149         _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
 150         _nkmp.min_p = 1;
 151         _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
 152
 153         ccu_nkmp_find_best(*parent_rate, rate, &_nkmp);
 154
 155         rate = ccu_nkmp_calc_rate(*parent_rate, _nkmp.n, _nkmp.k,
 156                                   _nkmp.m, _nkmp.p);
 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 };