drivers/clk/sunxi-ng/ccu_nkm.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_nkm.h"
  12
  13 struct _ccu_nkm {
  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 };
  18
  19 static unsigned long ccu_nkm_find_best_with_parent_adj(struct ccu_common *common,
  20                                                        struct clk_hw *parent_hw,
  21                                                        unsigned long *parent, unsigned long rate,
  22                                                        struct _ccu_nkm *nkm)
  23 {
  24         unsigned long best_rate = 0, best_parent_rate = *parent, tmp_parent = *parent;
  25         unsigned long best_n = 0, best_k = 0, best_m = 0;
  26         unsigned long _n, _k, _m;
  27
  28         for (_k = nkm->min_k; _k <= nkm->max_k; _k++) {
  29                 for (_n = nkm->min_n; _n <= nkm->max_n; _n++) {
  30                         for (_m = nkm->min_m; _m <= nkm->max_m; _m++) {
  31                                 unsigned long tmp_rate;
  32
  33                                 tmp_parent = clk_hw_round_rate(parent_hw, rate * _m / (_n * _k));
  34
  35                                 tmp_rate = tmp_parent * _n * _k / _m;
  36
  37                                 if (ccu_is_better_rate(common, rate, tmp_rate, best_rate) ||
  38                                     (tmp_parent == *parent && tmp_rate == best_rate)) {
  39                                         best_rate = tmp_rate;
  40                                         best_parent_rate = tmp_parent;
  41                                         best_n = _n;
  42                                         best_k = _k;
  43                                         best_m = _m;
  44                                 }
  45                         }
  46                 }
  47         }
  48
  49         nkm->n = best_n;
  50         nkm->k = best_k;
  51         nkm->m = best_m;
  52
  53         *parent = best_parent_rate;
  54
  55         return best_rate;
  56 }
  57
  58 static unsigned long ccu_nkm_find_best(unsigned long parent, unsigned long rate,
  59                                        struct _ccu_nkm *nkm, struct ccu_common *common)
  60 {
  61         unsigned long best_rate = 0;
  62         unsigned long best_n = 0, best_k = 0, best_m = 0;
  63         unsigned long _n, _k, _m;
  64
  65         for (_k = nkm->min_k; _k <= nkm->max_k; _k++) {
  66                 for (_n = nkm->min_n; _n <= nkm->max_n; _n++) {
  67                         for (_m = nkm->min_m; _m <= nkm->max_m; _m++) {
  68                                 unsigned long tmp_rate;
  69
  70                                 tmp_rate = parent * _n * _k / _m;
  71
  72                                 if (ccu_is_better_rate(common, rate, tmp_rate, best_rate)) {
  73                                         best_rate = tmp_rate;
  74                                         best_n = _n;
  75                                         best_k = _k;
  76                                         best_m = _m;
  77                                 }
  78                         }
  79                 }
  80         }
  81
  82         nkm->n = best_n;
  83         nkm->k = best_k;
  84         nkm->m = best_m;
  85
  86         return best_rate;
  87 }
  88
  89 static void ccu_nkm_disable(struct clk_hw *hw)
  90 {
  91         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
  92
  93         return ccu_gate_helper_disable(&nkm->common, nkm->enable);
  94 }
  95
  96 static int ccu_nkm_enable(struct clk_hw *hw)
  97 {
  98         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
  99
 100         return ccu_gate_helper_enable(&nkm->common, nkm->enable);
 101 }
 102
 103 static int ccu_nkm_is_enabled(struct clk_hw *hw)
 104 {
 105         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
 106
 107         return ccu_gate_helper_is_enabled(&nkm->common, nkm->enable);
 108 }
 109
 110 static unsigned long ccu_nkm_recalc_rate(struct clk_hw *hw,
 111                                         unsigned long parent_rate)
 112 {
 113         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
 114         unsigned long n, m, k, rate;
 115         u32 reg;
 116
 117         reg = readl(nkm->common.base + nkm->common.reg);
 118
 119         n = reg >> nkm->n.shift;
 120         n &= (1 << nkm->n.width) - 1;
 121         n += nkm->n.offset;
 122         if (!n)
 123                 n++;
 124
 125         k = reg >> nkm->k.shift;
 126         k &= (1 << nkm->k.width) - 1;
 127         k += nkm->k.offset;
 128         if (!k)
 129                 k++;
 130
 131         m = reg >> nkm->m.shift;
 132         m &= (1 << nkm->m.width) - 1;
 133         m += nkm->m.offset;
 134         if (!m)
 135                 m++;
 136
 137         rate = parent_rate * n  * k / m;
 138
 139         if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
 140                 rate /= nkm->fixed_post_div;
 141
 142         return rate;
 143 }
 144
 145 static unsigned long ccu_nkm_round_rate(struct ccu_mux_internal *mux,
 146                                         struct clk_hw *parent_hw,
 147                                         unsigned long *parent_rate,
 148                                         unsigned long rate,
 149                                         void *data)
 150 {
 151         struct ccu_nkm *nkm = data;
 152         struct _ccu_nkm _nkm;
 153
 154         _nkm.min_n = nkm->n.min ?: 1;
 155         _nkm.max_n = nkm->n.max ?: 1 << nkm->n.width;
 156         _nkm.min_k = nkm->k.min ?: 1;
 157         _nkm.max_k = nkm->k.max ?: 1 << nkm->k.width;
 158         _nkm.min_m = 1;
 159         _nkm.max_m = nkm->m.max ?: 1 << nkm->m.width;
 160
 161         if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
 162                 rate *= nkm->fixed_post_div;
 163
 164         if (!clk_hw_can_set_rate_parent(&nkm->common.hw))
 165                 rate = ccu_nkm_find_best(*parent_rate, rate, &_nkm, &nkm->common);
 166         else
 167                 rate = ccu_nkm_find_best_with_parent_adj(&nkm->common, parent_hw, parent_rate, rate,
 168                                                          &_nkm);
 169
 170         if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
 171                 rate /= nkm->fixed_post_div;
 172
 173         return rate;
 174 }
 175
 176 static int ccu_nkm_determine_rate(struct clk_hw *hw,
 177                                   struct clk_rate_request *req)
 178 {
 179         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
 180
 181         return ccu_mux_helper_determine_rate(&nkm->common, &nkm->mux,
 182                                              req, ccu_nkm_round_rate, nkm);
 183 }
 184
 185 static int ccu_nkm_set_rate(struct clk_hw *hw, unsigned long rate,
 186                            unsigned long parent_rate)
 187 {
 188         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
 189         struct _ccu_nkm _nkm;
 190         unsigned long flags;
 191         u32 reg;
 192
 193         if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
 194                 rate *= nkm->fixed_post_div;
 195
 196         _nkm.min_n = nkm->n.min ?: 1;
 197         _nkm.max_n = nkm->n.max ?: 1 << nkm->n.width;
 198         _nkm.min_k = nkm->k.min ?: 1;
 199         _nkm.max_k = nkm->k.max ?: 1 << nkm->k.width;
 200         _nkm.min_m = 1;
 201         _nkm.max_m = nkm->m.max ?: 1 << nkm->m.width;
 202
 203         ccu_nkm_find_best(parent_rate, rate, &_nkm, &nkm->common);
 204
 205         spin_lock_irqsave(nkm->common.lock, flags);
 206
 207         reg = readl(nkm->common.base + nkm->common.reg);
 208         reg &= ~GENMASK(nkm->n.width + nkm->n.shift - 1, nkm->n.shift);
 209         reg &= ~GENMASK(nkm->k.width + nkm->k.shift - 1, nkm->k.shift);
 210         reg &= ~GENMASK(nkm->m.width + nkm->m.shift - 1, nkm->m.shift);
 211
 212         reg |= (_nkm.n - nkm->n.offset) << nkm->n.shift;
 213         reg |= (_nkm.k - nkm->k.offset) << nkm->k.shift;
 214         reg |= (_nkm.m - nkm->m.offset) << nkm->m.shift;
 215         writel(reg, nkm->common.base + nkm->common.reg);
 216
 217         spin_unlock_irqrestore(nkm->common.lock, flags);
 218
 219         ccu_helper_wait_for_lock(&nkm->common, nkm->lock);
 220
 221         return 0;
 222 }
 223
 224 static u8 ccu_nkm_get_parent(struct clk_hw *hw)
 225 {
 226         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
 227
 228         return ccu_mux_helper_get_parent(&nkm->common, &nkm->mux);
 229 }
 230
 231 static int ccu_nkm_set_parent(struct clk_hw *hw, u8 index)
 232 {
 233         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
 234
 235         return ccu_mux_helper_set_parent(&nkm->common, &nkm->mux, index);
 236 }
 237
 238 const struct clk_ops ccu_nkm_ops = {
 239         .disable        = ccu_nkm_disable,
 240         .enable         = ccu_nkm_enable,
 241         .is_enabled     = ccu_nkm_is_enabled,
 242
 243         .get_parent     = ccu_nkm_get_parent,
 244         .set_parent     = ccu_nkm_set_parent,
 245
 246         .determine_rate = ccu_nkm_determine_rate,
 247         .recalc_rate    = ccu_nkm_recalc_rate,
 248         .set_rate       = ccu_nkm_set_rate,
 249 };
 250 EXPORT_SYMBOL_NS_GPL(ccu_nkm_ops, SUNXI_CCU);