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         n += nk->n.offset;
  80         if (!n)
  81                 n++;
  82
  83         k = reg >> nk->k.shift;
  84         k &= (1 << nk->k.width) - 1;
  85         k += nk->k.offset;
  86         if (!k)
  87                 k++;
  88
  89         rate = parent_rate * n * k;
  90         if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
  91                 rate /= nk->fixed_post_div;
  92
  93         return rate;
  94 }
  95
  96 static long ccu_nk_round_rate(struct clk_hw *hw, unsigned long rate,
  97                               unsigned long *parent_rate)
  98 {
  99         struct ccu_nk *nk = hw_to_ccu_nk(hw);
 100         struct _ccu_nk _nk;
 101
 102         if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
 103                 rate *= nk->fixed_post_div;
 104
 105         _nk.min_n = nk->n.min ?: 1;
 106         _nk.max_n = nk->n.max ?: 1 << nk->n.width;
 107         _nk.min_k = nk->k.min ?: 1;
 108         _nk.max_k = nk->k.max ?: 1 << nk->k.width;
 109
 110         ccu_nk_find_best(*parent_rate, rate, &_nk);
 111         rate = *parent_rate * _nk.n * _nk.k;
 112
 113         if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
 114                 rate = rate / nk->fixed_post_div;
 115
 116         return rate;
 117 }
 118
 119 static int ccu_nk_set_rate(struct clk_hw *hw, unsigned long rate,
 120                            unsigned long parent_rate)
 121 {
 122         struct ccu_nk *nk = hw_to_ccu_nk(hw);
 123         unsigned long flags;
 124         struct _ccu_nk _nk;
 125         u32 reg;
 126
 127         if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
 128                 rate = rate * nk->fixed_post_div;
 129
 130         _nk.min_n = nk->n.min ?: 1;
 131         _nk.max_n = nk->n.max ?: 1 << nk->n.width;
 132         _nk.min_k = nk->k.min ?: 1;
 133         _nk.max_k = nk->k.max ?: 1 << nk->k.width;
 134
 135         ccu_nk_find_best(parent_rate, rate, &_nk);
 136
 137         spin_lock_irqsave(nk->common.lock, flags);
 138
 139         reg = readl(nk->common.base + nk->common.reg);
 140         reg &= ~GENMASK(nk->n.width + nk->n.shift - 1, nk->n.shift);
 141         reg &= ~GENMASK(nk->k.width + nk->k.shift - 1, nk->k.shift);
 142
 143         reg |= (_nk.k - nk->k.offset) << nk->k.shift;
 144         reg |= (_nk.n - nk->n.offset) << nk->n.shift;
 145         writel(reg, nk->common.base + nk->common.reg);
 146
 147         spin_unlock_irqrestore(nk->common.lock, flags);
 148
 149         ccu_helper_wait_for_lock(&nk->common, nk->lock);
 150
 151         return 0;
 152 }
 153
 154 const struct clk_ops ccu_nk_ops = {
 155         .disable        = ccu_nk_disable,
 156         .enable         = ccu_nk_enable,
 157         .is_enabled     = ccu_nk_is_enabled,
 158
 159         .recalc_rate    = ccu_nk_recalc_rate,
 160         .round_rate     = ccu_nk_round_rate,
 161         .set_rate       = ccu_nk_set_rate,
 162 };