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