[linux-block.git] / drivers / clk / sunxi-ng / ccu_nk.c

// SPDX-License-Identifier: GPL-2.0-or-later
/*
 * Copyright (C) 2016 Maxime Ripard
 * Maxime Ripard <maxime.ripard@free-electrons.com>
 */

#include <linux/clk-provider.h>
#include <linux/io.h>

#include "ccu_gate.h"
#include "ccu_nk.h"

struct _ccu_nk {
	unsigned long	n, min_n, max_n;
	unsigned long	k, min_k, max_k;
};

static unsigned long ccu_nk_find_best(unsigned long parent, unsigned long rate,
				      struct _ccu_nk *nk)
{
	unsigned long best_rate = 0;
	unsigned int best_k = 0, best_n = 0;
	unsigned int _k, _n;

	for (_k = nk->min_k; _k <= nk->max_k; _k++) {
		for (_n = nk->min_n; _n <= nk->max_n; _n++) {
			unsigned long tmp_rate = parent * _n * _k;

			if (tmp_rate > rate)
				continue;

			if ((rate - tmp_rate) < (rate - best_rate)) {
				best_rate = tmp_rate;
				best_k = _k;
				best_n = _n;
			}
		}
	}

	nk->k = best_k;
	nk->n = best_n;

	return best_rate;
}

static void ccu_nk_disable(struct clk_hw *hw)
{
	struct ccu_nk *nk = hw_to_ccu_nk(hw);

	return ccu_gate_helper_disable(&nk->common, nk->enable);
}

static int ccu_nk_enable(struct clk_hw *hw)
{
	struct ccu_nk *nk = hw_to_ccu_nk(hw);

	return ccu_gate_helper_enable(&nk->common, nk->enable);
}

static int ccu_nk_is_enabled(struct clk_hw *hw)
{
	struct ccu_nk *nk = hw_to_ccu_nk(hw);

	return ccu_gate_helper_is_enabled(&nk->common, nk->enable);
}

static unsigned long ccu_nk_recalc_rate(struct clk_hw *hw,
					unsigned long parent_rate)
{
	struct ccu_nk *nk = hw_to_ccu_nk(hw);
	unsigned long rate, n, k;
	u32 reg;

	reg = readl(nk->common.base + nk->common.reg);

	n = reg >> nk->n.shift;
	n &= (1 << nk->n.width) - 1;
	n += nk->n.offset;
	if (!n)
		n++;

	k = reg >> nk->k.shift;
	k &= (1 << nk->k.width) - 1;
	k += nk->k.offset;
	if (!k)
		k++;

	rate = parent_rate * n * k;
	if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
		rate /= nk->fixed_post_div;

	return rate;
}

static long ccu_nk_round_rate(struct clk_hw *hw, unsigned long rate,
			      unsigned long *parent_rate)
{
	struct ccu_nk *nk = hw_to_ccu_nk(hw);
	struct _ccu_nk _nk;

	if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
		rate *= nk->fixed_post_div;

	_nk.min_n = nk->n.min ?: 1;
	_nk.max_n = nk->n.max ?: 1 << nk->n.width;
	_nk.min_k = nk->k.min ?: 1;
	_nk.max_k = nk->k.max ?: 1 << nk->k.width;

	rate = ccu_nk_find_best(*parent_rate, rate, &_nk);

	if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
		rate = rate / nk->fixed_post_div;

	return rate;
}

static int ccu_nk_set_rate(struct clk_hw *hw, unsigned long rate,
			   unsigned long parent_rate)
{
	struct ccu_nk *nk = hw_to_ccu_nk(hw);
	unsigned long flags;
	struct _ccu_nk _nk;
	u32 reg;

	if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
		rate = rate * nk->fixed_post_div;

	_nk.min_n = nk->n.min ?: 1;
	_nk.max_n = nk->n.max ?: 1 << nk->n.width;
	_nk.min_k = nk->k.min ?: 1;
	_nk.max_k = nk->k.max ?: 1 << nk->k.width;

	ccu_nk_find_best(parent_rate, rate, &_nk);

	spin_lock_irqsave(nk->common.lock, flags);

	reg = readl(nk->common.base + nk->common.reg);
	reg &= ~GENMASK(nk->n.width + nk->n.shift - 1, nk->n.shift);
	reg &= ~GENMASK(nk->k.width + nk->k.shift - 1, nk->k.shift);

	reg |= (_nk.k - nk->k.offset) << nk->k.shift;
	reg |= (_nk.n - nk->n.offset) << nk->n.shift;
	writel(reg, nk->common.base + nk->common.reg);

	spin_unlock_irqrestore(nk->common.lock, flags);

	ccu_helper_wait_for_lock(&nk->common, nk->lock);

	return 0;
}

const struct clk_ops ccu_nk_ops = {
	.disable	= ccu_nk_disable,
	.enable		= ccu_nk_enable,
	.is_enabled	= ccu_nk_is_enabled,

	.recalc_rate	= ccu_nk_recalc_rate,
	.round_rate	= ccu_nk_round_rate,
	.set_rate	= ccu_nk_set_rate,
};
EXPORT_SYMBOL_NS_GPL(ccu_nk_ops, SUNXI_CCU);
Commit	Line	Data
2874c5fd	1	// SPDX-License-Identifier: GPL-2.0-or-later
adbfb005 MR	2	/*
	3	* Copyright (C) 2016 Maxime Ripard
	4	* Maxime Ripard <maxime.ripard@free-electrons.com>
adbfb005 MR	5	*/
	6
	7	#include <linux/clk-provider.h>
62e59c4e	8	#include <linux/io.h>
adbfb005 MR	9
	10	#include "ccu_gate.h"
	11	#include "ccu_nk.h"
	12
b8302c72	13	struct _ccu_nk {
6e0d50da MR	14	unsigned long n, min_n, max_n;
6e0d50da MR	15	unsigned long k, min_k, max_k;
b8302c72 MR	16	};
b8302c72 MR	17
657f477a SH	18	static unsigned long ccu_nk_find_best(unsigned long parent, unsigned long rate,
657f477a SH	19	struct _ccu_nk *nk)
adbfb005 MR	20	{
	21	unsigned long best_rate = 0;
	22	unsigned int best_k = 0, best_n = 0;
	23	unsigned int _k, _n;
	24
6e0d50da MR	25	for (_k = nk->min_k; _k <= nk->max_k; _k++) {
6e0d50da MR	26	for (_n = nk->min_n; _n <= nk->max_n; _n++) {
adbfb005 MR	27	unsigned long tmp_rate = parent * _n * _k;
	28
	29	if (tmp_rate > rate)
	30	continue;
	31
	32	if ((rate - tmp_rate) < (rate - best_rate)) {
	33	best_rate = tmp_rate;
	34	best_k = _k;
	35	best_n = _n;
	36	}
	37	}
	38	}
	39
b8302c72 MR	40	nk->k = best_k;
b8302c72 MR	41	nk->n = best_n;
657f477a SH	42
657f477a SH	43	return best_rate;
adbfb005 MR	44	}
	45
	46	static void ccu_nk_disable(struct clk_hw *hw)
	47	{
	48	struct ccu_nk *nk = hw_to_ccu_nk(hw);
	49
	50	return ccu_gate_helper_disable(&nk->common, nk->enable);
	51	}
	52
	53	static int ccu_nk_enable(struct clk_hw *hw)
	54	{
	55	struct ccu_nk *nk = hw_to_ccu_nk(hw);
	56
	57	return ccu_gate_helper_enable(&nk->common, nk->enable);
	58	}
	59
	60	static int ccu_nk_is_enabled(struct clk_hw *hw)
	61	{
	62	struct ccu_nk *nk = hw_to_ccu_nk(hw);
	63
	64	return ccu_gate_helper_is_enabled(&nk->common, nk->enable);
	65	}
	66
	67	static unsigned long ccu_nk_recalc_rate(struct clk_hw *hw,
	68	unsigned long parent_rate)
	69	{
	70	struct ccu_nk *nk = hw_to_ccu_nk(hw);
	71	unsigned long rate, n, k;
	72	u32 reg;
	73
	74	reg = readl(nk->common.base + nk->common.reg);
	75
	76	n = reg >> nk->n.shift;
	77	n &= (1 << nk->n.width) - 1;
e66f81bb MR	78	n += nk->n.offset;
	79	if (!n)
	80	n++;
adbfb005 MR	81
	82	k = reg >> nk->k.shift;
	83	k &= (1 << nk->k.width) - 1;
e66f81bb MR	84	k += nk->k.offset;
	85	if (!k)
	86	k++;
adbfb005	87
e66f81bb	88	rate = parent_rate * n * k;
adbfb005 MR	89	if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
	90	rate /= nk->fixed_post_div;
	91
	92	return rate;
	93	}
	94
	95	static long ccu_nk_round_rate(struct clk_hw *hw, unsigned long rate,
	96	unsigned long *parent_rate)
	97	{
	98	struct ccu_nk *nk = hw_to_ccu_nk(hw);
b8302c72	99	struct _ccu_nk _nk;
adbfb005 MR	100
	101	if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
	102	rate *= nk->fixed_post_div;
	103
4162c5ce	104	_nk.min_n = nk->n.min ?: 1;
0c3c8e13	105	_nk.max_n = nk->n.max ?: 1 << nk->n.width;
4162c5ce	106	_nk.min_k = nk->k.min ?: 1;
0c3c8e13	107	_nk.max_k = nk->k.max ?: 1 << nk->k.width;
b8302c72	108
657f477a	109	rate = ccu_nk_find_best(*parent_rate, rate, &_nk);
adbfb005	110
adbfb005 MR	111	if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
	112	rate = rate / nk->fixed_post_div;
	113
	114	return rate;
	115	}
	116
	117	static int ccu_nk_set_rate(struct clk_hw *hw, unsigned long rate,
	118	unsigned long parent_rate)
	119	{
	120	struct ccu_nk *nk = hw_to_ccu_nk(hw);
	121	unsigned long flags;
b8302c72	122	struct _ccu_nk _nk;
adbfb005 MR	123	u32 reg;
	124
	125	if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
	126	rate = rate * nk->fixed_post_div;
	127
4162c5ce	128	_nk.min_n = nk->n.min ?: 1;
0c3c8e13	129	_nk.max_n = nk->n.max ?: 1 << nk->n.width;
4162c5ce	130	_nk.min_k = nk->k.min ?: 1;
0c3c8e13	131	_nk.max_k = nk->k.max ?: 1 << nk->k.width;
b8302c72 MR	132
b8302c72 MR	133	ccu_nk_find_best(parent_rate, rate, &_nk);
adbfb005 MR	134
	135	spin_lock_irqsave(nk->common.lock, flags);
	136
	137	reg = readl(nk->common.base + nk->common.reg);
	138	reg &= ~GENMASK(nk->n.width + nk->n.shift - 1, nk->n.shift);
	139	reg &= ~GENMASK(nk->k.width + nk->k.shift - 1, nk->k.shift);
	140
e66f81bb MR	141	reg \|= (_nk.k - nk->k.offset) << nk->k.shift;
	142	reg \|= (_nk.n - nk->n.offset) << nk->n.shift;
	143	writel(reg, nk->common.base + nk->common.reg);
adbfb005 MR	144
	145	spin_unlock_irqrestore(nk->common.lock, flags);
	146
	147	ccu_helper_wait_for_lock(&nk->common, nk->lock);
	148
	149	return 0;
	150	}
	151
	152	const struct clk_ops ccu_nk_ops = {
	153	.disable = ccu_nk_disable,
	154	.enable = ccu_nk_enable,
	155	.is_enabled = ccu_nk_is_enabled,
	156
	157	.recalc_rate = ccu_nk_recalc_rate,
	158	.round_rate = ccu_nk_round_rate,
	159	.set_rate = ccu_nk_set_rate,
	160	};
551b62b1	161	EXPORT_SYMBOL_NS_GPL(ccu_nk_ops, SUNXI_CCU);