ccu_nkm.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_nkm.h"
 12
 13struct _ccu_nkm {
 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};
 18
 19static bool ccu_nkm_is_valid_rate(struct ccu_common *common, unsigned long parent,
 20				  unsigned long n, unsigned long m)
 21{
 22	struct ccu_nkm *nkm = container_of(common, struct ccu_nkm, common);
 23
 24	if (nkm->max_m_n_ratio && (m > nkm->max_m_n_ratio * n))
 25		return false;
 26
 27	if (nkm->min_parent_m_ratio && (parent < nkm->min_parent_m_ratio * m))
 28		return false;
 29
 30	return true;
 31}
 32
 33static unsigned long ccu_nkm_find_best_with_parent_adj(struct ccu_common *common,
 34						       struct clk_hw *parent_hw,
 35						       unsigned long *parent, unsigned long rate,
 36						       struct _ccu_nkm *nkm)
 37{
 38	unsigned long best_rate = 0, best_parent_rate = *parent;
 39	unsigned long best_n = 0, best_k = 0, best_m = 0;
 40	unsigned long _n, _k, _m;
 41
 42	for (_k = nkm->min_k; _k <= nkm->max_k; _k++) {
 43		for (_n = nkm->min_n; _n <= nkm->max_n; _n++) {
 44			for (_m = nkm->min_m; _m <= nkm->max_m; _m++) {
 45				unsigned long tmp_rate, tmp_parent;
 46
 47				tmp_parent = clk_hw_round_rate(parent_hw, rate * _m / (_n * _k));
 48
 49				if (!ccu_nkm_is_valid_rate(common, tmp_parent, _n, _m))
 50					continue;
 51
 52				tmp_rate = tmp_parent * _n * _k / _m;
 53
 54				if (ccu_is_better_rate(common, rate, tmp_rate, best_rate) ||
 55				    (tmp_parent == *parent && tmp_rate == best_rate)) {
 56					best_rate = tmp_rate;
 57					best_parent_rate = tmp_parent;
 58					best_n = _n;
 59					best_k = _k;
 60					best_m = _m;
 61				}
 62			}
 63		}
 64	}
 65
 66	nkm->n = best_n;
 67	nkm->k = best_k;
 68	nkm->m = best_m;
 69
 70	*parent = best_parent_rate;
 71
 72	return best_rate;
 73}
 74
 75static unsigned long ccu_nkm_find_best(unsigned long parent, unsigned long rate,
 76				       struct _ccu_nkm *nkm, struct ccu_common *common)
 77{
 78	unsigned long best_rate = 0;
 79	unsigned long best_n = 0, best_k = 0, best_m = 0;
 80	unsigned long _n, _k, _m;
 81
 82	for (_k = nkm->min_k; _k <= nkm->max_k; _k++) {
 83		for (_n = nkm->min_n; _n <= nkm->max_n; _n++) {
 84			for (_m = nkm->min_m; _m <= nkm->max_m; _m++) {
 85				if (!ccu_nkm_is_valid_rate(common, parent, _n, _m))
 86					continue;
 87
 88				unsigned long tmp_rate;
 89
 90				tmp_rate = parent * _n * _k / _m;
 91
 92				if (ccu_is_better_rate(common, rate, tmp_rate, best_rate)) {
 93					best_rate = tmp_rate;
 94					best_n = _n;
 95					best_k = _k;
 96					best_m = _m;
 97				}
 98			}
 99		}
100	}
101
102	nkm->n = best_n;
103	nkm->k = best_k;
104	nkm->m = best_m;
105
106	return best_rate;
107}
108
109static void ccu_nkm_disable(struct clk_hw *hw)
110{
111	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
112
113	return ccu_gate_helper_disable(&nkm->common, nkm->enable);
114}
115
116static int ccu_nkm_enable(struct clk_hw *hw)
117{
118	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
119
120	return ccu_gate_helper_enable(&nkm->common, nkm->enable);
121}
122
123static int ccu_nkm_is_enabled(struct clk_hw *hw)
124{
125	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
126
127	return ccu_gate_helper_is_enabled(&nkm->common, nkm->enable);
128}
129
130static unsigned long ccu_nkm_recalc_rate(struct clk_hw *hw,
131					unsigned long parent_rate)
132{
133	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
134	unsigned long n, m, k, rate;
135	u32 reg;
136
137	reg = readl(nkm->common.base + nkm->common.reg);
138
139	n = reg >> nkm->n.shift;
140	n &= (1 << nkm->n.width) - 1;
141	n += nkm->n.offset;
142	if (!n)
143		n++;
144
145	k = reg >> nkm->k.shift;
146	k &= (1 << nkm->k.width) - 1;
147	k += nkm->k.offset;
148	if (!k)
149		k++;
150
151	m = reg >> nkm->m.shift;
152	m &= (1 << nkm->m.width) - 1;
153	m += nkm->m.offset;
154	if (!m)
155		m++;
156
157	rate = parent_rate * n  * k / m;
158
159	if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
160		rate /= nkm->fixed_post_div;
161
162	return rate;
163}
164
165static unsigned long ccu_nkm_round_rate(struct ccu_mux_internal *mux,
166					struct clk_hw *parent_hw,
167					unsigned long *parent_rate,
168					unsigned long rate,
169					void *data)
170{
171	struct ccu_nkm *nkm = data;
172	struct _ccu_nkm _nkm;
173
174	_nkm.min_n = nkm->n.min ?: 1;
175	_nkm.max_n = nkm->n.max ?: 1 << nkm->n.width;
176	_nkm.min_k = nkm->k.min ?: 1;
177	_nkm.max_k = nkm->k.max ?: 1 << nkm->k.width;
178	_nkm.min_m = 1;
179	_nkm.max_m = nkm->m.max ?: 1 << nkm->m.width;
180
181	if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
182		rate *= nkm->fixed_post_div;
183
184	if (!clk_hw_can_set_rate_parent(&nkm->common.hw))
185		rate = ccu_nkm_find_best(*parent_rate, rate, &_nkm, &nkm->common);
186	else
187		rate = ccu_nkm_find_best_with_parent_adj(&nkm->common, parent_hw, parent_rate, rate,
188							 &_nkm);
189
190	if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
191		rate /= nkm->fixed_post_div;
192
193	return rate;
194}
195
196static int ccu_nkm_determine_rate(struct clk_hw *hw,
197				  struct clk_rate_request *req)
198{
199	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
200
201	return ccu_mux_helper_determine_rate(&nkm->common, &nkm->mux,
202					     req, ccu_nkm_round_rate, nkm);
203}
204
205static int ccu_nkm_set_rate(struct clk_hw *hw, unsigned long rate,
206			   unsigned long parent_rate)
207{
208	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
209	struct _ccu_nkm _nkm;
210	unsigned long flags;
211	u32 reg;
212
213	if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
214		rate *= nkm->fixed_post_div;
215
216	_nkm.min_n = nkm->n.min ?: 1;
217	_nkm.max_n = nkm->n.max ?: 1 << nkm->n.width;
218	_nkm.min_k = nkm->k.min ?: 1;
219	_nkm.max_k = nkm->k.max ?: 1 << nkm->k.width;
220	_nkm.min_m = 1;
221	_nkm.max_m = nkm->m.max ?: 1 << nkm->m.width;
222
223	ccu_nkm_find_best(parent_rate, rate, &_nkm, &nkm->common);
224
225	spin_lock_irqsave(nkm->common.lock, flags);
226
227	reg = readl(nkm->common.base + nkm->common.reg);
228	reg &= ~GENMASK(nkm->n.width + nkm->n.shift - 1, nkm->n.shift);
229	reg &= ~GENMASK(nkm->k.width + nkm->k.shift - 1, nkm->k.shift);
230	reg &= ~GENMASK(nkm->m.width + nkm->m.shift - 1, nkm->m.shift);
231
232	reg |= (_nkm.n - nkm->n.offset) << nkm->n.shift;
233	reg |= (_nkm.k - nkm->k.offset) << nkm->k.shift;
234	reg |= (_nkm.m - nkm->m.offset) << nkm->m.shift;
235	writel(reg, nkm->common.base + nkm->common.reg);
236
237	spin_unlock_irqrestore(nkm->common.lock, flags);
238
239	ccu_helper_wait_for_lock(&nkm->common, nkm->lock);
240
241	return 0;
242}
243
244static u8 ccu_nkm_get_parent(struct clk_hw *hw)
245{
246	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
247
248	return ccu_mux_helper_get_parent(&nkm->common, &nkm->mux);
249}
250
251static int ccu_nkm_set_parent(struct clk_hw *hw, u8 index)
252{
253	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
254
255	return ccu_mux_helper_set_parent(&nkm->common, &nkm->mux, index);
256}
257
258const struct clk_ops ccu_nkm_ops = {
259	.disable	= ccu_nkm_disable,
260	.enable		= ccu_nkm_enable,
261	.is_enabled	= ccu_nkm_is_enabled,
262
263	.get_parent	= ccu_nkm_get_parent,
264	.set_parent	= ccu_nkm_set_parent,
265
266	.determine_rate	= ccu_nkm_determine_rate,
267	.recalc_rate	= ccu_nkm_recalc_rate,
268	.set_rate	= ccu_nkm_set_rate,
269};
270EXPORT_SYMBOL_NS_GPL(ccu_nkm_ops, "SUNXI_CCU");