2 * Copyright (C) 2016 Maxime Ripard
3 * Maxime Ripard <maxime.ripard@free-electrons.com>
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.
11 #include <linux/clk-provider.h>
12 #include <linux/rational.h>
18 unsigned long n, max_n;
19 unsigned long k, max_k;
20 unsigned long m, max_m;
21 unsigned long p, max_p;
24 static void ccu_nkmp_find_best(unsigned long parent, unsigned long rate,
25 struct _ccu_nkmp *nkmp)
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;
31 for (_k = 1; _k <= nkmp->max_k; _k++) {
32 for (_p = 1; _p <= nkmp->max_p; _p <<= 1) {
33 unsigned long tmp_rate;
35 rational_best_approximation(rate / _k, parent / _p,
36 nkmp->max_n, nkmp->max_m,
39 tmp_rate = parent * _n * _k / (_m * _p);
44 if ((rate - tmp_rate) < (rate - best_rate)) {
60 static void ccu_nkmp_disable(struct clk_hw *hw)
62 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
64 return ccu_gate_helper_disable(&nkmp->common, nkmp->enable);
67 static int ccu_nkmp_enable(struct clk_hw *hw)
69 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
71 return ccu_gate_helper_enable(&nkmp->common, nkmp->enable);
74 static int ccu_nkmp_is_enabled(struct clk_hw *hw)
76 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
78 return ccu_gate_helper_is_enabled(&nkmp->common, nkmp->enable);
81 static unsigned long ccu_nkmp_recalc_rate(struct clk_hw *hw,
82 unsigned long parent_rate)
84 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
85 unsigned long n, m, k, p;
88 reg = readl(nkmp->common.base + nkmp->common.reg);
90 n = reg >> nkmp->n.shift;
91 n &= (1 << nkmp->n.width) - 1;
93 k = reg >> nkmp->k.shift;
94 k &= (1 << nkmp->k.width) - 1;
96 m = reg >> nkmp->m.shift;
97 m &= (1 << nkmp->m.width) - 1;
99 p = reg >> nkmp->p.shift;
100 p &= (1 << nkmp->p.width) - 1;
102 return (parent_rate * (n + 1) * (k + 1) >> p) / (m + 1);
105 static long ccu_nkmp_round_rate(struct clk_hw *hw, unsigned long rate,
106 unsigned long *parent_rate)
108 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
109 struct _ccu_nkmp _nkmp;
111 _nkmp.max_n = 1 << nkmp->n.width;
112 _nkmp.max_k = 1 << nkmp->k.width;
113 _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
114 _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
116 ccu_nkmp_find_best(*parent_rate, rate, &_nkmp);
118 return *parent_rate * _nkmp.n * _nkmp.k / (_nkmp.m * _nkmp.p);
121 static int ccu_nkmp_set_rate(struct clk_hw *hw, unsigned long rate,
122 unsigned long parent_rate)
124 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
125 struct _ccu_nkmp _nkmp;
129 _nkmp.max_n = 1 << nkmp->n.width;
130 _nkmp.max_k = 1 << nkmp->k.width;
131 _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
132 _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
134 ccu_nkmp_find_best(parent_rate, rate, &_nkmp);
136 spin_lock_irqsave(nkmp->common.lock, flags);
138 reg = readl(nkmp->common.base + nkmp->common.reg);
139 reg &= ~GENMASK(nkmp->n.width + nkmp->n.shift - 1, nkmp->n.shift);
140 reg &= ~GENMASK(nkmp->k.width + nkmp->k.shift - 1, nkmp->k.shift);
141 reg &= ~GENMASK(nkmp->m.width + nkmp->m.shift - 1, nkmp->m.shift);
142 reg &= ~GENMASK(nkmp->p.width + nkmp->p.shift - 1, nkmp->p.shift);
144 reg |= (_nkmp.n - 1) << nkmp->n.shift;
145 reg |= (_nkmp.k - 1) << nkmp->k.shift;
146 reg |= (_nkmp.m - 1) << nkmp->m.shift;
147 reg |= ilog2(_nkmp.p) << nkmp->p.shift;
149 writel(reg, nkmp->common.base + nkmp->common.reg);
151 spin_unlock_irqrestore(nkmp->common.lock, flags);
153 ccu_helper_wait_for_lock(&nkmp->common, nkmp->lock);
158 const struct clk_ops ccu_nkmp_ops = {
159 .disable = ccu_nkmp_disable,
160 .enable = ccu_nkmp_enable,
161 .is_enabled = ccu_nkmp_is_enabled,
163 .recalc_rate = ccu_nkmp_recalc_rate,
164 .round_rate = ccu_nkmp_round_rate,
165 .set_rate = ccu_nkmp_set_rate,