ccu_nkmp.c

  1// SPDX-License-Identifier: GPL-2.0-or-later
  2/*
  3 * Copyright (C) 2016 Maxime Ripard
  4 * Maxime Ripard <maxime.ripard@free-electrons.com>
  5 */
  6
  7#include <linux/clk-provider.h>
  8#include <linux/io.h>
  9
 10#include "ccu_gate.h"
 11#include "ccu_nkmp.h"
 12
 13struct _ccu_nkmp {
 14	unsigned long	n, min_n, max_n;
 15	unsigned long	k, min_k, max_k;
 16	unsigned long	m, min_m, max_m;
 17	unsigned long	p, min_p, max_p;
 18};
 19
 20static unsigned long ccu_nkmp_calc_rate(unsigned long parent,
 21					unsigned long n, unsigned long k,
 22					unsigned long m, unsigned long p)
 23{
 24	u64 rate = parent;
 25
 26	rate *= n * k;
 27	do_div(rate, m * p);
 28
 29	return rate;
 30}
 31
 32static unsigned long ccu_nkmp_find_best(unsigned long parent, unsigned long rate,
 33					struct _ccu_nkmp *nkmp)
 34{
 35	unsigned long best_rate = 0;
 36	unsigned long best_n = 0, best_k = 0, best_m = 0, best_p = 0;
 37	unsigned long _n, _k, _m, _p;
 38
 39	for (_k = nkmp->min_k; _k <= nkmp->max_k; _k++) {
 40		for (_n = nkmp->min_n; _n <= nkmp->max_n; _n++) {
 41			for (_m = nkmp->min_m; _m <= nkmp->max_m; _m++) {
 42				for (_p = nkmp->min_p; _p <= nkmp->max_p; _p <<= 1) {
 43					unsigned long tmp_rate;
 44
 45					tmp_rate = ccu_nkmp_calc_rate(parent,
 46								      _n, _k,
 47								      _m, _p);
 48
 49					if (tmp_rate > rate)
 50						continue;
 51
 52					if ((rate - tmp_rate) < (rate - best_rate)) {
 53						best_rate = tmp_rate;
 54						best_n = _n;
 55						best_k = _k;
 56						best_m = _m;
 57						best_p = _p;
 58					}
 59				}
 60			}
 61		}
 62	}
 63
 64	nkmp->n = best_n;
 65	nkmp->k = best_k;
 66	nkmp->m = best_m;
 67	nkmp->p = best_p;
 68
 69	return best_rate;
 70}
 71
 72static void ccu_nkmp_disable(struct clk_hw *hw)
 73{
 74	struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
 75
 76	return ccu_gate_helper_disable(&nkmp->common, nkmp->enable);
 77}
 78
 79static int ccu_nkmp_enable(struct clk_hw *hw)
 80{
 81	struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
 82
 83	return ccu_gate_helper_enable(&nkmp->common, nkmp->enable);
 84}
 85
 86static int ccu_nkmp_is_enabled(struct clk_hw *hw)
 87{
 88	struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
 89
 90	return ccu_gate_helper_is_enabled(&nkmp->common, nkmp->enable);
 91}
 92
 93static unsigned long ccu_nkmp_recalc_rate(struct clk_hw *hw,
 94					unsigned long parent_rate)
 95{
 96	struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
 97	unsigned long n, m, k, p, rate;
 98	u32 reg;
 99
100	reg = readl(nkmp->common.base + nkmp->common.reg);
101
102	n = reg >> nkmp->n.shift;
103	n &= (1 << nkmp->n.width) - 1;
104	n += nkmp->n.offset;
105	if (!n)
106		n++;
107
108	k = reg >> nkmp->k.shift;
109	k &= (1 << nkmp->k.width) - 1;
110	k += nkmp->k.offset;
111	if (!k)
112		k++;
113
114	m = reg >> nkmp->m.shift;
115	m &= (1 << nkmp->m.width) - 1;
116	m += nkmp->m.offset;
117	if (!m)
118		m++;
119
120	p = reg >> nkmp->p.shift;
121	p &= (1 << nkmp->p.width) - 1;
122
123	rate = ccu_nkmp_calc_rate(parent_rate, n, k, m, 1 << p);
124	if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
125		rate /= nkmp->fixed_post_div;
126
127	return rate;
128}
129
130static int ccu_nkmp_determine_rate(struct clk_hw *hw,
131				   struct clk_rate_request *req)
132{
133	struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
134	struct _ccu_nkmp _nkmp;
135
136	if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
137		req->rate *= nkmp->fixed_post_div;
138
139	if (nkmp->max_rate && req->rate > nkmp->max_rate) {
140		req->rate = nkmp->max_rate;
141		if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
142			req->rate /= nkmp->fixed_post_div;
143		return 0;
144	}
145
146	_nkmp.min_n = nkmp->n.min ?: 1;
147	_nkmp.max_n = nkmp->n.max ?: 1 << nkmp->n.width;
148	_nkmp.min_k = nkmp->k.min ?: 1;
149	_nkmp.max_k = nkmp->k.max ?: 1 << nkmp->k.width;
150	_nkmp.min_m = 1;
151	_nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
152	_nkmp.min_p = 1;
153	_nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
154
155	req->rate = ccu_nkmp_find_best(req->best_parent_rate, req->rate,
156				       &_nkmp);
157
158	if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
159		req->rate = req->rate / nkmp->fixed_post_div;
160
161	return 0;
162}
163
164static int ccu_nkmp_set_rate(struct clk_hw *hw, unsigned long rate,
165			   unsigned long parent_rate)
166{
167	struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
168	u32 n_mask = 0, k_mask = 0, m_mask = 0, p_mask = 0;
169	struct _ccu_nkmp _nkmp;
170	unsigned long flags;
171	u32 reg;
172
173	if (nkmp->common.features & CCU_FEATURE_FIXED_POSTDIV)
174		rate = rate * nkmp->fixed_post_div;
175
176	_nkmp.min_n = nkmp->n.min ?: 1;
177	_nkmp.max_n = nkmp->n.max ?: 1 << nkmp->n.width;
178	_nkmp.min_k = nkmp->k.min ?: 1;
179	_nkmp.max_k = nkmp->k.max ?: 1 << nkmp->k.width;
180	_nkmp.min_m = 1;
181	_nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
182	_nkmp.min_p = 1;
183	_nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
184
185	ccu_nkmp_find_best(parent_rate, rate, &_nkmp);
186
187	/*
188	 * If width is 0, GENMASK() macro may not generate expected mask (0)
189	 * as it falls under undefined behaviour by C standard due to shifts
190	 * which are equal or greater than width of left operand. This can
191	 * be easily avoided by explicitly checking if width is 0.
192	 */
193	if (nkmp->n.width)
194		n_mask = GENMASK(nkmp->n.width + nkmp->n.shift - 1,
195				 nkmp->n.shift);
196	if (nkmp->k.width)
197		k_mask = GENMASK(nkmp->k.width + nkmp->k.shift - 1,
198				 nkmp->k.shift);
199	if (nkmp->m.width)
200		m_mask = GENMASK(nkmp->m.width + nkmp->m.shift - 1,
201				 nkmp->m.shift);
202	if (nkmp->p.width)
203		p_mask = GENMASK(nkmp->p.width + nkmp->p.shift - 1,
204				 nkmp->p.shift);
205
206	spin_lock_irqsave(nkmp->common.lock, flags);
207
208	reg = readl(nkmp->common.base + nkmp->common.reg);
209	reg &= ~(n_mask | k_mask | m_mask | p_mask);
210
211	reg |= ((_nkmp.n - nkmp->n.offset) << nkmp->n.shift) & n_mask;
212	reg |= ((_nkmp.k - nkmp->k.offset) << nkmp->k.shift) & k_mask;
213	reg |= ((_nkmp.m - nkmp->m.offset) << nkmp->m.shift) & m_mask;
214	reg |= (ilog2(_nkmp.p) << nkmp->p.shift) & p_mask;
215
216	writel(reg, nkmp->common.base + nkmp->common.reg);
217
218	spin_unlock_irqrestore(nkmp->common.lock, flags);
219
220	ccu_helper_wait_for_lock(&nkmp->common, nkmp->lock);
221
222	return 0;
223}
224
225const struct clk_ops ccu_nkmp_ops = {
226	.disable	= ccu_nkmp_disable,
227	.enable		= ccu_nkmp_enable,
228	.is_enabled	= ccu_nkmp_is_enabled,
229
230	.recalc_rate	= ccu_nkmp_recalc_rate,
231	.determine_rate = ccu_nkmp_determine_rate,
232	.set_rate	= ccu_nkmp_set_rate,
233};
234EXPORT_SYMBOL_NS_GPL(ccu_nkmp_ops, "SUNXI_CCU");