drivers/clk/sunxi-ng/ccu_nkmp.c

   1 /*
   2  * Copyright (C) 2016 Maxime Ripard
   3  * Maxime Ripard <maxime.ripard@free-electrons.com>
   4  *
   5  * This program is free software; you can redistribute it and/or
   6  * modify it under the terms of the GNU General Public License as
   7  * published by the Free Software Foundation; either version 2 of
   8  * the License, or (at your option) any later version.
   9  */
  10
  11 #include <linux/clk-provider.h>
  12
  13 #include "ccu_gate.h"
  14 #include "ccu_nkmp.h"
  15
  16 struct _ccu_nkmp {
  17         unsigned long   n, min_n, max_n;
  18         unsigned long   k, min_k, max_k;
  19         unsigned long   m, min_m, max_m;
  20         unsigned long   p, min_p, max_p;
  21 };
  22
  23 static unsigned long ccu_nkmp_calc_rate(unsigned long parent,
  24                                         unsigned long n, unsigned long k,
  25                                         unsigned long m, unsigned long p)
  26 {
  27         u64 rate = parent;
  28
  29         rate *= n * k;
  30         do_div(rate, m * p);
  31
  32         return rate;
  33 }
  34
  35 static void ccu_nkmp_find_best(unsigned long parent, unsigned long rate,
  36                                struct _ccu_nkmp *nkmp)
  37 {
  38         unsigned long best_rate = 0;
  39         unsigned long best_n = 0, best_k = 0, best_m = 0, best_p = 0;
  40         unsigned long _n, _k, _m, _p;
  41
  42         for (_k = nkmp->min_k; _k <= nkmp->max_k; _k++) {
  43                 for (_n = nkmp->min_n; _n <= nkmp->max_n; _n++) {
  44                         for (_m = nkmp->min_m; _m <= nkmp->max_m; _m++) {
  45                                 for (_p = nkmp->min_p; _p <= nkmp->max_p; _p <<= 1) {
  46                                         unsigned long tmp_rate;
  47
  48                                         tmp_rate = ccu_nkmp_calc_rate(parent,
  49                                                                       _n, _k,
  50                                                                       _m, _p);
  51
  52                                         if (tmp_rate > rate)
  53                                                 continue;
  54
  55                                         if ((rate - tmp_rate) < (rate - best_rate)) {
  56                                                 best_rate = tmp_rate;
  57                                                 best_n = _n;
  58                                                 best_k = _k;
  59                                                 best_m = _m;
  60                                                 best_p = _p;
  61                                         }
  62                                 }
  63                         }
  64                 }
  65         }
  66
  67         nkmp->n = best_n;
  68         nkmp->k = best_k;
  69         nkmp->m = best_m;
  70         nkmp->p = best_p;
  71 }
  72
  73 static void ccu_nkmp_disable(struct clk_hw *hw)
  74 {
  75         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
  76
  77         return ccu_gate_helper_disable(&nkmp->common, nkmp->enable);
  78 }
  79
  80 static int ccu_nkmp_enable(struct clk_hw *hw)
  81 {
  82         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
  83
  84         return ccu_gate_helper_enable(&nkmp->common, nkmp->enable);
  85 }
  86
  87 static int ccu_nkmp_is_enabled(struct clk_hw *hw)
  88 {
  89         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
  90
  91         return ccu_gate_helper_is_enabled(&nkmp->common, nkmp->enable);
  92 }
  93
  94 static unsigned long ccu_nkmp_recalc_rate(struct clk_hw *hw,
  95                                         unsigned long parent_rate)
  96 {
  97         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
  98         unsigned long n, m, k, p, rate;
  99         u32 reg;
 100
 101         reg = readl(nkmp->common.base + nkmp->common.reg);
 102
 103         n = reg >> nkmp->n.shift;
 104         n &= (1 << nkmp->n.width) - 1;
 105         n += nkmp->n.offset;
 106         if (!n)
 107                 n++;
 108
 109         k = reg >> nkmp->k.shift;
 110         k &= (1 << nkmp->k.width) - 1;
 111         k += nkmp->k.offset;
 112         if (!k)
 113                 k++;
 114
 115         m = reg >> nkmp->m.shift;
 116         m &= (1 << nkmp->m.width) - 1;
 117         m += nkmp->m.offset;
 118         if (!m)
 119                 m++;
 120
 121         p = reg >> nkmp->p.shift;
 122         p &= (1 << nkmp->p.width) - 1;
 123
 124         rate = ccu_nkmp_calc_rate(parent_rate, n, k, m, 1 << p);
 125         if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
 126                 rate /= nkmp->fixed_post_div;
 127
 128         return rate;
 129 }
 130
 131 static long ccu_nkmp_round_rate(struct clk_hw *hw, unsigned long rate,
 132                               unsigned long *parent_rate)
 133 {
 134         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
 135         struct _ccu_nkmp _nkmp;
 136
 137         if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
 138                 rate *= nkmp->fixed_post_div;
 139
 140         _nkmp.min_n = nkmp->n.min ?: 1;
 141         _nkmp.max_n = nkmp->n.max ?: 1 << nkmp->n.width;
 142         _nkmp.min_k = nkmp->k.min ?: 1;
 143         _nkmp.max_k = nkmp->k.max ?: 1 << nkmp->k.width;
 144         _nkmp.min_m = 1;
 145         _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
 146         _nkmp.min_p = 1;
 147         _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
 148
 149         ccu_nkmp_find_best(*parent_rate, rate, &_nkmp);
 150
 151         rate = ccu_nkmp_calc_rate(*parent_rate, _nkmp.n, _nkmp.k,
 152                                   _nkmp.m, _nkmp.p);
 153         if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
 154                 rate = rate / nkmp->fixed_post_div;
 155
 156         return rate;
 157 }
 158
 159 static int ccu_nkmp_set_rate(struct clk_hw *hw, unsigned long rate,
 160                            unsigned long parent_rate)
 161 {
 162         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
 163         u32 n_mask, k_mask, m_mask, p_mask;
 164         struct _ccu_nkmp _nkmp;
 165         unsigned long flags;
 166         u32 reg;
 167
 168         if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
 169                 rate = rate * nkmp->fixed_post_div;
 170
 171         _nkmp.min_n = nkmp->n.min ?: 1;
 172         _nkmp.max_n = nkmp->n.max ?: 1 << nkmp->n.width;
 173         _nkmp.min_k = nkmp->k.min ?: 1;
 174         _nkmp.max_k = nkmp->k.max ?: 1 << nkmp->k.width;
 175         _nkmp.min_m = 1;
 176         _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
 177         _nkmp.min_p = 1;
 178         _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
 179
 180         ccu_nkmp_find_best(parent_rate, rate, &_nkmp);
 181
 182         n_mask = GENMASK(nkmp->n.width + nkmp->n.shift - 1, nkmp->n.shift);
 183         k_mask = GENMASK(nkmp->k.width + nkmp->k.shift - 1, nkmp->k.shift);
 184         m_mask = GENMASK(nkmp->m.width + nkmp->m.shift - 1, nkmp->m.shift);
 185         p_mask = GENMASK(nkmp->p.width + nkmp->p.shift - 1, nkmp->p.shift);
 186
 187         spin_lock_irqsave(nkmp->common.lock, flags);
 188
 189         reg = readl(nkmp->common.base + nkmp->common.reg);
 190         reg &= ~(n_mask | k_mask | m_mask | p_mask);
 191
 192         reg |= ((_nkmp.n - nkmp->n.offset) << nkmp->n.shift) & n_mask;
 193         reg |= ((_nkmp.k - nkmp->k.offset) << nkmp->k.shift) & k_mask;
 194         reg |= ((_nkmp.m - nkmp->m.offset) << nkmp->m.shift) & m_mask;
 195         reg |= (ilog2(_nkmp.p) << nkmp->p.shift) & p_mask;
 196
 197         writel(reg, nkmp->common.base + nkmp->common.reg);
 198
 199         spin_unlock_irqrestore(nkmp->common.lock, flags);
 200
 201         ccu_helper_wait_for_lock(&nkmp->common, nkmp->lock);
 202
 203         return 0;
 204 }
 205
 206 const struct clk_ops ccu_nkmp_ops = {
 207         .disable        = ccu_nkmp_disable,
 208         .enable         = ccu_nkmp_enable,
 209         .is_enabled     = ccu_nkmp_is_enabled,
 210
 211         .recalc_rate    = ccu_nkmp_recalc_rate,
 212         .round_rate     = ccu_nkmp_round_rate,
 213         .set_rate       = ccu_nkmp_set_rate,
 214 };