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 void ccu_nkmp_find_best(unsigned long parent, unsigned long rate,
  24                                struct _ccu_nkmp *nkmp)
  25 {
  26         unsigned long best_rate = 0;
  27         unsigned long best_n = 0, best_k = 0, best_m = 0, best_p = 0;
  28         unsigned long _n, _k, _m, _p;
  29
  30         for (_k = nkmp->min_k; _k <= nkmp->max_k; _k++) {
  31                 for (_n = nkmp->min_n; _n <= nkmp->max_n; _n++) {
  32                         for (_m = nkmp->min_m; _m <= nkmp->max_m; _m++) {
  33                                 for (_p = nkmp->min_p; _p <= nkmp->max_p; _p <<= 1) {
  34                                         unsigned long tmp_rate;
  35
  36                                         tmp_rate = parent * _n * _k / (_m * _p);
  37
  38                                         if (tmp_rate > rate)
  39                                                 continue;
  40
  41                                         if ((rate - tmp_rate) < (rate - best_rate)) {
  42                                                 best_rate = tmp_rate;
  43                                                 best_n = _n;
  44                                                 best_k = _k;
  45                                                 best_m = _m;
  46                                                 best_p = _p;
  47                                         }
  48                                 }
  49                         }
  50                 }
  51         }
  52
  53         nkmp->n = best_n;
  54         nkmp->k = best_k;
  55         nkmp->m = best_m;
  56         nkmp->p = best_p;
  57 }
  58
  59 static void ccu_nkmp_disable(struct clk_hw *hw)
  60 {
  61         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
  62
  63         return ccu_gate_helper_disable(&nkmp->common, nkmp->enable);
  64 }
  65
  66 static int ccu_nkmp_enable(struct clk_hw *hw)
  67 {
  68         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
  69
  70         return ccu_gate_helper_enable(&nkmp->common, nkmp->enable);
  71 }
  72
  73 static int ccu_nkmp_is_enabled(struct clk_hw *hw)
  74 {
  75         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
  76
  77         return ccu_gate_helper_is_enabled(&nkmp->common, nkmp->enable);
  78 }
  79
  80 static unsigned long ccu_nkmp_recalc_rate(struct clk_hw *hw,
  81                                         unsigned long parent_rate)
  82 {
  83         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
  84         unsigned long n, m, k, p;
  85         u32 reg;
  86
  87         reg = readl(nkmp->common.base + nkmp->common.reg);
  88
  89         n = reg >> nkmp->n.shift;
  90         n &= (1 << nkmp->n.width) - 1;
  91         n += nkmp->n.offset;
  92         if (!n)
  93                 n++;
  94
  95         k = reg >> nkmp->k.shift;
  96         k &= (1 << nkmp->k.width) - 1;
  97         k += nkmp->k.offset;
  98         if (!k)
  99                 k++;
 100
 101         m = reg >> nkmp->m.shift;
 102         m &= (1 << nkmp->m.width) - 1;
 103         m += nkmp->m.offset;
 104         if (!m)
 105                 m++;
 106
 107         p = reg >> nkmp->p.shift;
 108         p &= (1 << nkmp->p.width) - 1;
 109
 110         return (parent_rate * n * k >> p) / m;
 111 }
 112
 113 static long ccu_nkmp_round_rate(struct clk_hw *hw, unsigned long rate,
 114                               unsigned long *parent_rate)
 115 {
 116         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
 117         struct _ccu_nkmp _nkmp;
 118
 119         _nkmp.min_n = nkmp->n.min;
 120         _nkmp.max_n = nkmp->n.max ?: 1 << nkmp->n.width;
 121         _nkmp.min_k = nkmp->k.min;
 122         _nkmp.max_k = nkmp->k.max ?: 1 << nkmp->k.width;
 123         _nkmp.min_m = 1;
 124         _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
 125         _nkmp.min_p = 1;
 126         _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
 127
 128         ccu_nkmp_find_best(*parent_rate, rate, &_nkmp);
 129
 130         return *parent_rate * _nkmp.n * _nkmp.k / (_nkmp.m * _nkmp.p);
 131 }
 132
 133 static int ccu_nkmp_set_rate(struct clk_hw *hw, unsigned long rate,
 134                            unsigned long parent_rate)
 135 {
 136         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
 137         struct _ccu_nkmp _nkmp;
 138         unsigned long flags;
 139         u32 reg;
 140
 141         _nkmp.min_n = 1;
 142         _nkmp.max_n = nkmp->n.max ?: 1 << nkmp->n.width;
 143         _nkmp.min_k = 1;
 144         _nkmp.max_k = nkmp->k.max ?: 1 << nkmp->k.width;
 145         _nkmp.min_m = 1;
 146         _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
 147         _nkmp.min_p = 1;
 148         _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
 149
 150         ccu_nkmp_find_best(parent_rate, rate, &_nkmp);
 151
 152         spin_lock_irqsave(nkmp->common.lock, flags);
 153
 154         reg = readl(nkmp->common.base + nkmp->common.reg);
 155         reg &= ~GENMASK(nkmp->n.width + nkmp->n.shift - 1, nkmp->n.shift);
 156         reg &= ~GENMASK(nkmp->k.width + nkmp->k.shift - 1, nkmp->k.shift);
 157         reg &= ~GENMASK(nkmp->m.width + nkmp->m.shift - 1, nkmp->m.shift);
 158         reg &= ~GENMASK(nkmp->p.width + nkmp->p.shift - 1, nkmp->p.shift);
 159
 160         reg |= (_nkmp.n - nkmp->n.offset) << nkmp->n.shift;
 161         reg |= (_nkmp.k - nkmp->k.offset) << nkmp->k.shift;
 162         reg |= (_nkmp.m - nkmp->m.offset) << nkmp->m.shift;
 163         reg |= ilog2(_nkmp.p) << nkmp->p.shift;
 164
 165         writel(reg, nkmp->common.base + nkmp->common.reg);
 166
 167         spin_unlock_irqrestore(nkmp->common.lock, flags);
 168
 169         ccu_helper_wait_for_lock(&nkmp->common, nkmp->lock);
 170
 171         return 0;
 172 }
 173
 174 const struct clk_ops ccu_nkmp_ops = {
 175         .disable        = ccu_nkmp_disable,
 176         .enable         = ccu_nkmp_enable,
 177         .is_enabled     = ccu_nkmp_is_enabled,
 178
 179         .recalc_rate    = ccu_nkmp_recalc_rate,
 180         .round_rate     = ccu_nkmp_round_rate,
 181         .set_rate       = ccu_nkmp_set_rate,
 182 };