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 #include <linux/rational.h>
  13
  14 #include "ccu_gate.h"
  15 #include "ccu_nk.h"
  16
  17 static void ccu_nk_find_best(unsigned long parent, unsigned long rate,
  18                              unsigned int max_n, unsigned int max_k,
  19                              unsigned int *n, unsigned int *k)
  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 = 1; _k <= max_k; _k++) {
  26                 for (_n = 1; _n <= 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         *k = best_k;
  41         *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
  77         k = reg >> nk->k.shift;
  78         k &= (1 << nk->k.width) - 1;
  79
  80         rate = parent_rate * (n + 1) * (k + 1);
  81
  82         if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
  83                 rate /= nk->fixed_post_div;
  84
  85         return rate;
  86 }
  87
  88 static long ccu_nk_round_rate(struct clk_hw *hw, unsigned long rate,
  89                               unsigned long *parent_rate)
  90 {
  91         struct ccu_nk *nk = hw_to_ccu_nk(hw);
  92         unsigned int n, k;
  93
  94         if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
  95                 rate *= nk->fixed_post_div;
  96
  97         ccu_nk_find_best(*parent_rate, rate,
  98                          1 << nk->n.width, 1 << nk->k.width,
  99                          &n, &k);
 100
 101         rate = *parent_rate * n * k;
 102         if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
 103                 rate = rate / nk->fixed_post_div;
 104
 105         return rate;
 106 }
 107
 108 static int ccu_nk_set_rate(struct clk_hw *hw, unsigned long rate,
 109                            unsigned long parent_rate)
 110 {
 111         struct ccu_nk *nk = hw_to_ccu_nk(hw);
 112         unsigned long flags;
 113         unsigned int n, k;
 114         u32 reg;
 115
 116         if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
 117                 rate = rate * nk->fixed_post_div;
 118
 119         ccu_nk_find_best(parent_rate, rate,
 120                          1 << nk->n.width, 1 << nk->k.width,
 121                          &n, &k);
 122
 123         spin_lock_irqsave(nk->common.lock, flags);
 124
 125         reg = readl(nk->common.base + nk->common.reg);
 126         reg &= ~GENMASK(nk->n.width + nk->n.shift - 1, nk->n.shift);
 127         reg &= ~GENMASK(nk->k.width + nk->k.shift - 1, nk->k.shift);
 128
 129         writel(reg | ((k - 1) << nk->k.shift) | ((n - 1) << nk->n.shift),
 130                nk->common.base + nk->common.reg);
 131
 132         spin_unlock_irqrestore(nk->common.lock, flags);
 133
 134         ccu_helper_wait_for_lock(&nk->common, nk->lock);
 135
 136         return 0;
 137 }
 138
 139 const struct clk_ops ccu_nk_ops = {
 140         .disable        = ccu_nk_disable,
 141         .enable         = ccu_nk_enable,
 142         .is_enabled     = ccu_nk_is_enabled,
 143
 144         .recalc_rate    = ccu_nk_recalc_rate,
 145         .round_rate     = ccu_nk_round_rate,
 146         .set_rate       = ccu_nk_set_rate,
 147 };