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
  92         k = reg >> nkmp->k.shift;
  93         k &= (1 << nkmp->k.width) - 1;
  94
  95         m = reg >> nkmp->m.shift;
  96         m &= (1 << nkmp->m.width) - 1;
  97
  98         p = reg >> nkmp->p.shift;
  99         p &= (1 << nkmp->p.width) - 1;
 100
 101         return (parent_rate * (n + 1) * (k + 1) >> p) / (m + 1);
 102 }
 103
 104 static long ccu_nkmp_round_rate(struct clk_hw *hw, unsigned long rate,
 105                               unsigned long *parent_rate)
 106 {
 107         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
 108         struct _ccu_nkmp _nkmp;
 109
 110         _nkmp.min_n = nkmp->n.min;
 111         _nkmp.max_n = 1 << nkmp->n.width;
 112         _nkmp.min_k = nkmp->k.min;
 113         _nkmp.max_k = 1 << nkmp->k.width;
 114         _nkmp.min_m = 1;
 115         _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
 116         _nkmp.min_p = 1;
 117         _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
 118
 119         ccu_nkmp_find_best(*parent_rate, rate, &_nkmp);
 120
 121         return *parent_rate * _nkmp.n * _nkmp.k / (_nkmp.m * _nkmp.p);
 122 }
 123
 124 static int ccu_nkmp_set_rate(struct clk_hw *hw, unsigned long rate,
 125                            unsigned long parent_rate)
 126 {
 127         struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
 128         struct _ccu_nkmp _nkmp;
 129         unsigned long flags;
 130         u32 reg;
 131
 132         _nkmp.min_n = 1;
 133         _nkmp.max_n = 1 << nkmp->n.width;
 134         _nkmp.min_k = 1;
 135         _nkmp.max_k = 1 << nkmp->k.width;
 136         _nkmp.min_m = 1;
 137         _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
 138         _nkmp.min_p = 1;
 139         _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
 140
 141         ccu_nkmp_find_best(parent_rate, rate, &_nkmp);
 142
 143         spin_lock_irqsave(nkmp->common.lock, flags);
 144
 145         reg = readl(nkmp->common.base + nkmp->common.reg);
 146         reg &= ~GENMASK(nkmp->n.width + nkmp->n.shift - 1, nkmp->n.shift);
 147         reg &= ~GENMASK(nkmp->k.width + nkmp->k.shift - 1, nkmp->k.shift);
 148         reg &= ~GENMASK(nkmp->m.width + nkmp->m.shift - 1, nkmp->m.shift);
 149         reg &= ~GENMASK(nkmp->p.width + nkmp->p.shift - 1, nkmp->p.shift);
 150
 151         reg |= (_nkmp.n - 1) << nkmp->n.shift;
 152         reg |= (_nkmp.k - 1) << nkmp->k.shift;
 153         reg |= (_nkmp.m - 1) << nkmp->m.shift;
 154         reg |= ilog2(_nkmp.p) << nkmp->p.shift;
 155
 156         writel(reg, nkmp->common.base + nkmp->common.reg);
 157
 158         spin_unlock_irqrestore(nkmp->common.lock, flags);
 159
 160         ccu_helper_wait_for_lock(&nkmp->common, nkmp->lock);
 161
 162         return 0;
 163 }
 164
 165 const struct clk_ops ccu_nkmp_ops = {
 166         .disable        = ccu_nkmp_disable,
 167         .enable         = ccu_nkmp_enable,
 168         .is_enabled     = ccu_nkmp_is_enabled,
 169
 170         .recalc_rate    = ccu_nkmp_recalc_rate,
 171         .round_rate     = ccu_nkmp_round_rate,
 172         .set_rate       = ccu_nkmp_set_rate,
 173 };