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