[mirror_ubuntu-zesty-kernel.git] / drivers / clk / mmp / clk-frac.c

/*
 * mmp factor clock operation source file
 *
 * Copyright (C) 2012 Marvell
 * Chao Xie <xiechao.mail@gmail.com>
 *
 * This file is licensed under the terms of the GNU General Public
 * License version 2. This program is licensed "as is" without any
 * warranty of any kind, whether express or implied.
 */

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

#include "clk.h"
/*
 * It is M/N clock
 *
 * Fout from synthesizer can be given from two equations:
 * numerator/denominator = Fin / (Fout * factor)
 */

#define to_clk_factor(hw) container_of(hw, struct clk_factor, hw)
struct clk_factor {
	struct clk_hw		hw;
	void __iomem		*base;
	struct clk_factor_masks	*masks;
	struct clk_factor_tbl	*ftbl;
	unsigned int		ftbl_cnt;
};

static long clk_factor_round_rate(struct clk_hw *hw, unsigned long drate,
		unsigned long *prate)
{
	struct clk_factor *factor = to_clk_factor(hw);
	unsigned long rate = 0, prev_rate;
	int i;

	for (i = 0; i < factor->ftbl_cnt; i++) {
		prev_rate = rate;
		rate = (((*prate / 10000) * factor->ftbl[i].den) /
			(factor->ftbl[i].num * factor->masks->factor)) * 10000;
		if (rate > drate)
			break;
	}
	if ((i == 0) || (i == factor->ftbl_cnt)) {
		return rate;
	} else {
		if ((drate - prev_rate) > (rate - drate))
			return rate;
		else
			return prev_rate;
	}
}

static unsigned long clk_factor_recalc_rate(struct clk_hw *hw,
		unsigned long parent_rate)
{
	struct clk_factor *factor = to_clk_factor(hw);
	struct clk_factor_masks *masks = factor->masks;
	unsigned int val, num, den;

	val = readl_relaxed(factor->base);

	/* calculate numerator */
	num = (val >> masks->num_shift) & masks->num_mask;

	/* calculate denominator */
	den = (val >> masks->den_shift) & masks->den_mask;

	if (!den)
		return 0;

	return (((parent_rate / 10000)  * den) /
			(num * factor->masks->factor)) * 10000;
}

/* Configures new clock rate*/
static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate,
				unsigned long prate)
{
	struct clk_factor *factor = to_clk_factor(hw);
	struct clk_factor_masks *masks = factor->masks;
	int i;
	unsigned long val;
	unsigned long prev_rate, rate = 0;

	for (i = 0; i < factor->ftbl_cnt; i++) {
		prev_rate = rate;
		rate = (((prate / 10000) * factor->ftbl[i].den) /
			(factor->ftbl[i].num * factor->masks->factor)) * 10000;
		if (rate > drate)
			break;
	}
	if (i > 0)
		i--;

	val = readl_relaxed(factor->base);

	val &= ~(masks->num_mask << masks->num_shift);
	val |= (factor->ftbl[i].num & masks->num_mask) << masks->num_shift;

	val &= ~(masks->den_mask << masks->den_shift);
	val |= (factor->ftbl[i].den & masks->den_mask) << masks->den_shift;

	writel_relaxed(val, factor->base);

	return 0;
}

static struct clk_ops clk_factor_ops = {
	.recalc_rate = clk_factor_recalc_rate,
	.round_rate = clk_factor_round_rate,
	.set_rate = clk_factor_set_rate,
};

struct clk *mmp_clk_register_factor(const char *name, const char *parent_name,
		unsigned long flags, void __iomem *base,
		struct clk_factor_masks *masks, struct clk_factor_tbl *ftbl,
		unsigned int ftbl_cnt)
{
	struct clk_factor *factor;
	struct clk_init_data init;
	struct clk *clk;

	if (!masks) {
		pr_err("%s: must pass a clk_factor_mask\n", __func__);
		return ERR_PTR(-EINVAL);
	}

	factor = kzalloc(sizeof(*factor), GFP_KERNEL);
	if (!factor) {
		pr_err("%s: could not allocate factor  clk\n", __func__);
		return ERR_PTR(-ENOMEM);
	}

	/* struct clk_aux assignments */
	factor->base = base;
	factor->masks = masks;
	factor->ftbl = ftbl;
	factor->ftbl_cnt = ftbl_cnt;
	factor->hw.init = &init;

	init.name = name;
	init.ops = &clk_factor_ops;
	init.flags = flags;
	init.parent_names = &parent_name;
	init.num_parents = 1;

	clk = clk_register(NULL, &factor->hw);
	if (IS_ERR_OR_NULL(clk))
		kfree(factor);

	return clk;
}
Commit	Line	Data
6b63f023 CX	1	/*
	2	* mmp factor clock operation source file
	3	*
	4	* Copyright (C) 2012 Marvell
	5	* Chao Xie <xiechao.mail@gmail.com>
	6	*
	7	* This file is licensed under the terms of the GNU General Public
	8	* License version 2. This program is licensed "as is" without any
	9	* warranty of any kind, whether express or implied.
	10	*/
	11
	12	#include <linux/clk-provider.h>
	13	#include <linux/slab.h>
	14	#include <linux/io.h>
	15	#include <linux/err.h>
	16
	17	#include "clk.h"
	18	/*
	19	* It is M/N clock
	20	*
	21	* Fout from synthesizer can be given from two equations:
	22	* numerator/denominator = Fin / (Fout * factor)
	23	*/
	24
	25	#define to_clk_factor(hw) container_of(hw, struct clk_factor, hw)
	26	struct clk_factor {
	27	struct clk_hw hw;
	28	void __iomem *base;
	29	struct clk_factor_masks *masks;
	30	struct clk_factor_tbl *ftbl;
	31	unsigned int ftbl_cnt;
	32	};
	33
	34	static long clk_factor_round_rate(struct clk_hw *hw, unsigned long drate,
	35	unsigned long *prate)
	36	{
	37	struct clk_factor *factor = to_clk_factor(hw);
	38	unsigned long rate = 0, prev_rate;
	39	int i;
	40
	41	for (i = 0; i < factor->ftbl_cnt; i++) {
	42	prev_rate = rate;
c45693a6 CX	43	rate = (((prate / 10000) factor->ftbl[i].den) /
c45693a6 CX	44	(factor->ftbl[i].num * factor->masks->factor)) * 10000;
6b63f023 CX	45	if (rate > drate)
	46	break;
	47	}
5d26c15d	48	if ((i == 0) \|\| (i == factor->ftbl_cnt)) {
6b63f023	49	return rate;
5d26c15d CX	50	} else {
	51	if ((drate - prev_rate) > (rate - drate))
	52	return rate;
	53	else
	54	return prev_rate;
	55	}
6b63f023 CX	56	}
	57
	58	static unsigned long clk_factor_recalc_rate(struct clk_hw *hw,
	59	unsigned long parent_rate)
	60	{
	61	struct clk_factor *factor = to_clk_factor(hw);
	62	struct clk_factor_masks *masks = factor->masks;
	63	unsigned int val, num, den;
	64
	65	val = readl_relaxed(factor->base);
	66
	67	/* calculate numerator */
	68	num = (val >> masks->num_shift) & masks->num_mask;
	69
	70	/* calculate denominator */
7433ab43	71	den = (val >> masks->den_shift) & masks->den_mask;
6b63f023 CX	72
	73	if (!den)
	74	return 0;
	75
	76	return (((parent_rate / 10000) * den) /
	77	(num * factor->masks->factor)) * 10000;
	78	}
	79
	80	/* Configures new clock rate*/
	81	static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate,
	82	unsigned long prate)
	83	{
	84	struct clk_factor *factor = to_clk_factor(hw);
	85	struct clk_factor_masks *masks = factor->masks;
	86	int i;
	87	unsigned long val;
	88	unsigned long prev_rate, rate = 0;
	89
	90	for (i = 0; i < factor->ftbl_cnt; i++) {
	91	prev_rate = rate;
c45693a6 CX	92	rate = (((prate / 10000) * factor->ftbl[i].den) /
c45693a6 CX	93	(factor->ftbl[i].num * factor->masks->factor)) * 10000;
6b63f023 CX	94	if (rate > drate)
	95	break;
	96	}
	97	if (i > 0)
	98	i--;
	99
	100	val = readl_relaxed(factor->base);
	101
	102	val &= ~(masks->num_mask << masks->num_shift);
	103	val \|= (factor->ftbl[i].num & masks->num_mask) << masks->num_shift;
	104
	105	val &= ~(masks->den_mask << masks->den_shift);
	106	val \|= (factor->ftbl[i].den & masks->den_mask) << masks->den_shift;
	107
	108	writel_relaxed(val, factor->base);
	109
	110	return 0;
	111	}
	112
	113	static struct clk_ops clk_factor_ops = {
	114	.recalc_rate = clk_factor_recalc_rate,
	115	.round_rate = clk_factor_round_rate,
	116	.set_rate = clk_factor_set_rate,
	117	};
	118
	119	struct clk mmp_clk_register_factor(const char name, const char *parent_name,
	120	unsigned long flags, void __iomem *base,
	121	struct clk_factor_masks masks, struct clk_factor_tbl ftbl,
	122	unsigned int ftbl_cnt)
	123	{
	124	struct clk_factor *factor;
	125	struct clk_init_data init;
	126	struct clk *clk;
	127
	128	if (!masks) {
	129	pr_err("%s: must pass a clk_factor_mask\n", __func__);
	130	return ERR_PTR(-EINVAL);
	131	}
	132
	133	factor = kzalloc(sizeof(*factor), GFP_KERNEL);
	134	if (!factor) {
	135	pr_err("%s: could not allocate factor clk\n", __func__);
	136	return ERR_PTR(-ENOMEM);
	137	}
	138
	139	/* struct clk_aux assignments */
	140	factor->base = base;
	141	factor->masks = masks;
	142	factor->ftbl = ftbl;
	143	factor->ftbl_cnt = ftbl_cnt;
	144	factor->hw.init = &init;
	145
	146	init.name = name;
	147	init.ops = &clk_factor_ops;
	148	init.flags = flags;
	149	init.parent_names = &parent_name;
	150	init.num_parents = 1;
	151
	152	clk = clk_register(NULL, &factor->hw);
	153	if (IS_ERR_OR_NULL(clk))
	154	kfree(factor);
	155
	156	return clk;
	157	}