[mirror_ubuntu-hirsute-kernel.git] / drivers / clk / sunxi-ng / ccu_mult.c

/*
 * Copyright (C) 2016 Maxime Ripard
 * Maxime Ripard <maxime.ripard@free-electrons.com>
 *
 * This program is free software; you can redistribute it and/or
 * modify it under the terms of the GNU General Public License as
 * published by the Free Software Foundation; either version 2 of
 * the License, or (at your option) any later version.
 */

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

#include "ccu_gate.h"
#include "ccu_mult.h"

struct _ccu_mult {
	unsigned long	mult, min, max;
};

static void ccu_mult_find_best(unsigned long parent, unsigned long rate,
			       struct _ccu_mult *mult)
{
	int _mult;

	_mult = rate / parent;
	if (_mult < mult->min)
		_mult = mult->min;

	if (_mult > mult->max)
		_mult = mult->max;

	mult->mult = _mult;
}

static unsigned long ccu_mult_round_rate(struct ccu_mux_internal *mux,
					 struct clk_hw *parent,
					 unsigned long *parent_rate,
					 unsigned long rate,
					 void *data)
{
	struct ccu_mult *cm = data;
	struct _ccu_mult _cm;

	_cm.min = cm->mult.min;

	if (cm->mult.max)
		_cm.max = cm->mult.max;
	else
		_cm.max = (1 << cm->mult.width) + cm->mult.offset - 1;

	ccu_mult_find_best(*parent_rate, rate, &_cm);

	return *parent_rate * _cm.mult;
}

static void ccu_mult_disable(struct clk_hw *hw)
{
	struct ccu_mult *cm = hw_to_ccu_mult(hw);

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

static int ccu_mult_enable(struct clk_hw *hw)
{
	struct ccu_mult *cm = hw_to_ccu_mult(hw);

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

static int ccu_mult_is_enabled(struct clk_hw *hw)
{
	struct ccu_mult *cm = hw_to_ccu_mult(hw);

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

static unsigned long ccu_mult_recalc_rate(struct clk_hw *hw,
					unsigned long parent_rate)
{
	struct ccu_mult *cm = hw_to_ccu_mult(hw);
	unsigned long val;
	u32 reg;

	if (ccu_frac_helper_is_enabled(&cm->common, &cm->frac))
		return ccu_frac_helper_read_rate(&cm->common, &cm->frac);

	reg = readl(cm->common.base + cm->common.reg);
	val = reg >> cm->mult.shift;
	val &= (1 << cm->mult.width) - 1;

	parent_rate = ccu_mux_helper_apply_prediv(&cm->common, &cm->mux, -1,
						  parent_rate);

	return parent_rate * (val + cm->mult.offset);
}

static int ccu_mult_determine_rate(struct clk_hw *hw,
				struct clk_rate_request *req)
{
	struct ccu_mult *cm = hw_to_ccu_mult(hw);

	return ccu_mux_helper_determine_rate(&cm->common, &cm->mux,
					     req, ccu_mult_round_rate, cm);
}

static int ccu_mult_set_rate(struct clk_hw *hw, unsigned long rate,
			   unsigned long parent_rate)
{
	struct ccu_mult *cm = hw_to_ccu_mult(hw);
	struct _ccu_mult _cm;
	unsigned long flags;
	u32 reg;

	if (ccu_frac_helper_has_rate(&cm->common, &cm->frac, rate)) {
		ccu_frac_helper_enable(&cm->common, &cm->frac);

		return ccu_frac_helper_set_rate(&cm->common, &cm->frac,
						rate, cm->lock);
	} else {
		ccu_frac_helper_disable(&cm->common, &cm->frac);
	}

	parent_rate = ccu_mux_helper_apply_prediv(&cm->common, &cm->mux, -1,
						  parent_rate);

	_cm.min = cm->mult.min;

	if (cm->mult.max)
		_cm.max = cm->mult.max;
	else
		_cm.max = (1 << cm->mult.width) + cm->mult.offset - 1;

	ccu_mult_find_best(parent_rate, rate, &_cm);

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

	reg = readl(cm->common.base + cm->common.reg);
	reg &= ~GENMASK(cm->mult.width + cm->mult.shift - 1, cm->mult.shift);
	reg |= ((_cm.mult - cm->mult.offset) << cm->mult.shift);

	writel(reg, cm->common.base + cm->common.reg);

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

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

	return 0;
}

static u8 ccu_mult_get_parent(struct clk_hw *hw)
{
	struct ccu_mult *cm = hw_to_ccu_mult(hw);

	return ccu_mux_helper_get_parent(&cm->common, &cm->mux);
}

static int ccu_mult_set_parent(struct clk_hw *hw, u8 index)
{
	struct ccu_mult *cm = hw_to_ccu_mult(hw);

	return ccu_mux_helper_set_parent(&cm->common, &cm->mux, index);
}

const struct clk_ops ccu_mult_ops = {
	.disable	= ccu_mult_disable,
	.enable		= ccu_mult_enable,
	.is_enabled	= ccu_mult_is_enabled,

	.get_parent	= ccu_mult_get_parent,
	.set_parent	= ccu_mult_set_parent,

	.determine_rate	= ccu_mult_determine_rate,
	.recalc_rate	= ccu_mult_recalc_rate,
	.set_rate	= ccu_mult_set_rate,
};
Commit	Line	Data
aa152335 MR	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>
62e59c4e	12	#include <linux/io.h>
aa152335 MR	13
	14	#include "ccu_gate.h"
	15	#include "ccu_mult.h"
	16
b8302c72	17	struct _ccu_mult {
6e0d50da	18	unsigned long mult, min, max;
b8302c72 MR	19	};
b8302c72 MR	20
aa152335	21	static void ccu_mult_find_best(unsigned long parent, unsigned long rate,
b8302c72	22	struct _ccu_mult *mult)
aa152335	23	{
b8302c72 MR	24	int _mult;
	25
	26	_mult = rate / parent;
6e0d50da MR	27	if (_mult < mult->min)
	28	_mult = mult->min;
	29
b8302c72 MR	30	if (_mult > mult->max)
	31	_mult = mult->max;
	32
	33	mult->mult = _mult;
aa152335 MR	34	}
	35
	36	static unsigned long ccu_mult_round_rate(struct ccu_mux_internal *mux,
10a8d9b9 MR	37	struct clk_hw *parent,
	38	unsigned long *parent_rate,
	39	unsigned long rate,
	40	void *data)
aa152335 MR	41	{
aa152335 MR	42	struct ccu_mult *cm = data;
b8302c72	43	struct _ccu_mult _cm;
aa152335	44
c9520be3	45	_cm.min = cm->mult.min;
0c3c8e13 MR	46
	47	if (cm->mult.max)
	48	_cm.max = cm->mult.max;
	49	else
	50	_cm.max = (1 << cm->mult.width) + cm->mult.offset - 1;
	51
10a8d9b9	52	ccu_mult_find_best(*parent_rate, rate, &_cm);
aa152335	53
10a8d9b9	54	return parent_rate _cm.mult;
aa152335 MR	55	}
	56
	57	static void ccu_mult_disable(struct clk_hw *hw)
	58	{
	59	struct ccu_mult *cm = hw_to_ccu_mult(hw);
	60
	61	return ccu_gate_helper_disable(&cm->common, cm->enable);
	62	}
	63
	64	static int ccu_mult_enable(struct clk_hw *hw)
	65	{
	66	struct ccu_mult *cm = hw_to_ccu_mult(hw);
	67
	68	return ccu_gate_helper_enable(&cm->common, cm->enable);
	69	}
	70
	71	static int ccu_mult_is_enabled(struct clk_hw *hw)
	72	{
	73	struct ccu_mult *cm = hw_to_ccu_mult(hw);
	74
	75	return ccu_gate_helper_is_enabled(&cm->common, cm->enable);
	76	}
	77
	78	static unsigned long ccu_mult_recalc_rate(struct clk_hw *hw,
	79	unsigned long parent_rate)
	80	{
	81	struct ccu_mult *cm = hw_to_ccu_mult(hw);
	82	unsigned long val;
	83	u32 reg;
	84
d77e8135 MR	85	if (ccu_frac_helper_is_enabled(&cm->common, &cm->frac))
	86	return ccu_frac_helper_read_rate(&cm->common, &cm->frac);
	87
aa152335 MR	88	reg = readl(cm->common.base + cm->common.reg);
	89	val = reg >> cm->mult.shift;
	90	val &= (1 << cm->mult.width) - 1;
	91
d754b159 MR	92	parent_rate = ccu_mux_helper_apply_prediv(&cm->common, &cm->mux, -1,
d754b159 MR	93	parent_rate);
aa152335	94
e66f81bb	95	return parent_rate * (val + cm->mult.offset);
aa152335 MR	96	}
	97
	98	static int ccu_mult_determine_rate(struct clk_hw *hw,
	99	struct clk_rate_request *req)
	100	{
	101	struct ccu_mult *cm = hw_to_ccu_mult(hw);
	102
	103	return ccu_mux_helper_determine_rate(&cm->common, &cm->mux,
	104	req, ccu_mult_round_rate, cm);
	105	}
	106
	107	static int ccu_mult_set_rate(struct clk_hw *hw, unsigned long rate,
	108	unsigned long parent_rate)
	109	{
	110	struct ccu_mult *cm = hw_to_ccu_mult(hw);
b8302c72	111	struct _ccu_mult _cm;
aa152335	112	unsigned long flags;
aa152335 MR	113	u32 reg;
aa152335 MR	114
1e92ae65 JŠ	115	if (ccu_frac_helper_has_rate(&cm->common, &cm->frac, rate)) {
	116	ccu_frac_helper_enable(&cm->common, &cm->frac);
	117
1d42460a JŠ	118	return ccu_frac_helper_set_rate(&cm->common, &cm->frac,
1d42460a JŠ	119	rate, cm->lock);
1e92ae65	120	} else {
d77e8135	121	ccu_frac_helper_disable(&cm->common, &cm->frac);
1e92ae65	122	}
d77e8135	123
d754b159 MR	124	parent_rate = ccu_mux_helper_apply_prediv(&cm->common, &cm->mux, -1,
d754b159 MR	125	parent_rate);
aa152335	126
2beaa601	127	_cm.min = cm->mult.min;
0c3c8e13 MR	128
	129	if (cm->mult.max)
	130	_cm.max = cm->mult.max;
	131	else
	132	_cm.max = (1 << cm->mult.width) + cm->mult.offset - 1;
	133
b8302c72	134	ccu_mult_find_best(parent_rate, rate, &_cm);
aa152335 MR	135
	136	spin_lock_irqsave(cm->common.lock, flags);
	137
	138	reg = readl(cm->common.base + cm->common.reg);
	139	reg &= ~GENMASK(cm->mult.width + cm->mult.shift - 1, cm->mult.shift);
e66f81bb	140	reg \|= ((_cm.mult - cm->mult.offset) << cm->mult.shift);
aa152335	141
e66f81bb	142	writel(reg, cm->common.base + cm->common.reg);
aa152335 MR	143
	144	spin_unlock_irqrestore(cm->common.lock, flags);
	145
cf719012 CYT	146	ccu_helper_wait_for_lock(&cm->common, cm->lock);
cf719012 CYT	147
aa152335 MR	148	return 0;
	149	}
	150
	151	static u8 ccu_mult_get_parent(struct clk_hw *hw)
	152	{
	153	struct ccu_mult *cm = hw_to_ccu_mult(hw);
	154
	155	return ccu_mux_helper_get_parent(&cm->common, &cm->mux);
	156	}
	157
	158	static int ccu_mult_set_parent(struct clk_hw *hw, u8 index)
	159	{
	160	struct ccu_mult *cm = hw_to_ccu_mult(hw);
	161
	162	return ccu_mux_helper_set_parent(&cm->common, &cm->mux, index);
	163	}
	164
	165	const struct clk_ops ccu_mult_ops = {
	166	.disable = ccu_mult_disable,
	167	.enable = ccu_mult_enable,
	168	.is_enabled = ccu_mult_is_enabled,
	169
	170	.get_parent = ccu_mult_get_parent,
	171	.set_parent = ccu_mult_set_parent,
	172
	173	.determine_rate = ccu_mult_determine_rate,
	174	.recalc_rate = ccu_mult_recalc_rate,
	175	.set_rate = ccu_mult_set_rate,
	176	};