drivers/clk/sunxi-ng/ccu_nk.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_nk.h"
  12
  13 struct _ccu_nk {
  14         unsigned long   n, min_n, max_n;
  15         unsigned long   k, min_k, max_k;
  16 };
  17
  18 static void ccu_nk_find_best(unsigned long parent, unsigned long rate,
  19                              struct _ccu_nk *nk)
  20 {
  21         unsigned long best_rate = 0;
  22         unsigned int best_k = 0, best_n = 0;
  23         unsigned int _k, _n;
  24
  25         for (_k = nk->min_k; _k <= nk->max_k; _k++) {
  26                 for (_n = nk->min_n; _n <= nk->max_n; _n++) {
  27                         unsigned long tmp_rate = parent * _n * _k;
  28
  29                         if (tmp_rate > rate)
  30                                 continue;
  31
  32                         if ((rate - tmp_rate) < (rate - best_rate)) {
  33                                 best_rate = tmp_rate;
  34                                 best_k = _k;
  35                                 best_n = _n;
  36                         }
  37                 }
  38         }
  39
  40         nk->k = best_k;
  41         nk->n = best_n;
  42 }
  43
  44 static void ccu_nk_disable(struct clk_hw *hw)
  45 {
  46         struct ccu_nk *nk = hw_to_ccu_nk(hw);
  47
  48         return ccu_gate_helper_disable(&nk->common, nk->enable);
  49 }
  50
  51 static int ccu_nk_enable(struct clk_hw *hw)
  52 {
  53         struct ccu_nk *nk = hw_to_ccu_nk(hw);
  54
  55         return ccu_gate_helper_enable(&nk->common, nk->enable);
  56 }
  57
  58 static int ccu_nk_is_enabled(struct clk_hw *hw)
  59 {
  60         struct ccu_nk *nk = hw_to_ccu_nk(hw);
  61
  62         return ccu_gate_helper_is_enabled(&nk->common, nk->enable);
  63 }
  64
  65 static unsigned long ccu_nk_recalc_rate(struct clk_hw *hw,
  66                                         unsigned long parent_rate)
  67 {
  68         struct ccu_nk *nk = hw_to_ccu_nk(hw);
  69         unsigned long rate, n, k;
  70         u32 reg;
  71
  72         reg = readl(nk->common.base + nk->common.reg);
  73
  74         n = reg >> nk->n.shift;
  75         n &= (1 << nk->n.width) - 1;
  76         n += nk->n.offset;
  77         if (!n)
  78                 n++;
  79
  80         k = reg >> nk->k.shift;
  81         k &= (1 << nk->k.width) - 1;
  82         k += nk->k.offset;
  83         if (!k)
  84                 k++;
  85
  86         rate = parent_rate * n * k;
  87         if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
  88                 rate /= nk->fixed_post_div;
  89
  90         return rate;
  91 }
  92
  93 static long ccu_nk_round_rate(struct clk_hw *hw, unsigned long rate,
  94                               unsigned long *parent_rate)
  95 {
  96         struct ccu_nk *nk = hw_to_ccu_nk(hw);
  97         struct _ccu_nk _nk;
  98
  99         if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
 100                 rate *= nk->fixed_post_div;
 101
 102         _nk.min_n = nk->n.min ?: 1;
 103         _nk.max_n = nk->n.max ?: 1 << nk->n.width;
 104         _nk.min_k = nk->k.min ?: 1;
 105         _nk.max_k = nk->k.max ?: 1 << nk->k.width;
 106
 107         ccu_nk_find_best(*parent_rate, rate, &_nk);
 108         rate = *parent_rate * _nk.n * _nk.k;
 109
 110         if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
 111                 rate = rate / nk->fixed_post_div;
 112
 113         return rate;
 114 }
 115
 116 static int ccu_nk_set_rate(struct clk_hw *hw, unsigned long rate,
 117                            unsigned long parent_rate)
 118 {
 119         struct ccu_nk *nk = hw_to_ccu_nk(hw);
 120         unsigned long flags;
 121         struct _ccu_nk _nk;
 122         u32 reg;
 123
 124         if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
 125                 rate = rate * nk->fixed_post_div;
 126
 127         _nk.min_n = nk->n.min ?: 1;
 128         _nk.max_n = nk->n.max ?: 1 << nk->n.width;
 129         _nk.min_k = nk->k.min ?: 1;
 130         _nk.max_k = nk->k.max ?: 1 << nk->k.width;
 131
 132         ccu_nk_find_best(parent_rate, rate, &_nk);
 133
 134         spin_lock_irqsave(nk->common.lock, flags);
 135
 136         reg = readl(nk->common.base + nk->common.reg);
 137         reg &= ~GENMASK(nk->n.width + nk->n.shift - 1, nk->n.shift);
 138         reg &= ~GENMASK(nk->k.width + nk->k.shift - 1, nk->k.shift);
 139
 140         reg |= (_nk.k - nk->k.offset) << nk->k.shift;
 141         reg |= (_nk.n - nk->n.offset) << nk->n.shift;
 142         writel(reg, nk->common.base + nk->common.reg);
 143
 144         spin_unlock_irqrestore(nk->common.lock, flags);
 145
 146         ccu_helper_wait_for_lock(&nk->common, nk->lock);
 147
 148         return 0;
 149 }
 150
 151 const struct clk_ops ccu_nk_ops = {
 152         .disable        = ccu_nk_disable,
 153         .enable         = ccu_nk_enable,
 154         .is_enabled     = ccu_nk_is_enabled,
 155
 156         .recalc_rate    = ccu_nk_recalc_rate,
 157         .round_rate     = ccu_nk_round_rate,
 158         .set_rate       = ccu_nk_set_rate,
 159 };