[deliverable/linux.git] / drivers / clk / qcom / clk-rcg2.c

/*
 * Copyright (c) 2013, The Linux Foundation. All rights reserved.
 *
 * This software is licensed under the terms of the GNU General Public
 * License version 2, as published by the Free Software Foundation, and
 * may be copied, distributed, and modified under those terms.
 *
 * This program is distributed in the hope that it will be useful,
 * but WITHOUT ANY WARRANTY; without even the implied warranty of
 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
 * GNU General Public License for more details.
 */

#include <linux/kernel.h>
#include <linux/bitops.h>
#include <linux/err.h>
#include <linux/bug.h>
#include <linux/export.h>
#include <linux/clk-provider.h>
#include <linux/delay.h>
#include <linux/regmap.h>
#include <linux/math64.h>

#include <asm/div64.h>

#include "clk-rcg.h"

#define CMD_REG			0x0
#define CMD_UPDATE		BIT(0)
#define CMD_ROOT_EN		BIT(1)
#define CMD_DIRTY_CFG		BIT(4)
#define CMD_DIRTY_N		BIT(5)
#define CMD_DIRTY_M		BIT(6)
#define CMD_DIRTY_D		BIT(7)
#define CMD_ROOT_OFF		BIT(31)

#define CFG_REG			0x4
#define CFG_SRC_DIV_SHIFT	0
#define CFG_SRC_SEL_SHIFT	8
#define CFG_SRC_SEL_MASK	(0x7 << CFG_SRC_SEL_SHIFT)
#define CFG_MODE_SHIFT		12
#define CFG_MODE_MASK		(0x3 << CFG_MODE_SHIFT)
#define CFG_MODE_DUAL_EDGE	(0x2 << CFG_MODE_SHIFT)

#define M_REG			0x8
#define N_REG			0xc
#define D_REG			0x10

static int clk_rcg2_is_enabled(struct clk_hw *hw)
{
	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	u32 cmd;
	int ret;

	ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, &cmd);
	if (ret)
		return ret;

	return (cmd & CMD_ROOT_OFF) == 0;
}

static u8 clk_rcg2_get_parent(struct clk_hw *hw)
{
	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	int num_parents = __clk_get_num_parents(hw->clk);
	u32 cfg;
	int i, ret;

	ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
	if (ret)
		return ret;

	cfg &= CFG_SRC_SEL_MASK;
	cfg >>= CFG_SRC_SEL_SHIFT;

	for (i = 0; i < num_parents; i++)
		if (cfg == rcg->parent_map[i])
			return i;

	return -EINVAL;
}

static int update_config(struct clk_rcg2 *rcg)
{
	int count, ret;
	u32 cmd;
	struct clk_hw *hw = &rcg->clkr.hw;
	const char *name = __clk_get_name(hw->clk);

	ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG,
				 CMD_UPDATE, CMD_UPDATE);
	if (ret)
		return ret;

	/* Wait for update to take effect */
	for (count = 500; count > 0; count--) {
		ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, &cmd);
		if (ret)
			return ret;
		if (!(cmd & CMD_UPDATE))
			return 0;
		udelay(1);
	}

	WARN(1, "%s: rcg didn't update its configuration.", name);
	return 0;
}

static int clk_rcg2_set_parent(struct clk_hw *hw, u8 index)
{
	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	int ret;

	ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
				 CFG_SRC_SEL_MASK,
				 rcg->parent_map[index] << CFG_SRC_SEL_SHIFT);
	if (ret)
		return ret;

	return update_config(rcg);
}

/*
 * Calculate m/n:d rate
 *
 *          parent_rate     m
 *   rate = ----------- x  ---
 *            hid_div       n
 */
static unsigned long
calc_rate(unsigned long rate, u32 m, u32 n, u32 mode, u32 hid_div)
{
	if (hid_div) {
		rate *= 2;
		rate /= hid_div + 1;
	}

	if (mode) {
		u64 tmp = rate;
		tmp *= m;
		do_div(tmp, n);
		rate = tmp;
	}

	return rate;
}

static unsigned long
clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
{
	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	u32 cfg, hid_div, m = 0, n = 0, mode = 0, mask;

	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);

	if (rcg->mnd_width) {
		mask = BIT(rcg->mnd_width) - 1;
		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + M_REG, &m);
		m &= mask;
		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + N_REG, &n);
		n =  ~n;
		n &= mask;
		n += m;
		mode = cfg & CFG_MODE_MASK;
		mode >>= CFG_MODE_SHIFT;
	}

	mask = BIT(rcg->hid_width) - 1;
	hid_div = cfg >> CFG_SRC_DIV_SHIFT;
	hid_div &= mask;

	return calc_rate(parent_rate, m, n, mode, hid_div);
}

static const
struct freq_tbl *find_freq(const struct freq_tbl *f, unsigned long rate)
{
	if (!f)
		return NULL;

	for (; f->freq; f++)
		if (rate <= f->freq)
			return f;

	/* Default to our fastest rate */
	return f - 1;
}

static long _freq_tbl_determine_rate(struct clk_hw *hw,
		const struct freq_tbl *f, unsigned long rate,
		unsigned long *p_rate, struct clk **p)
{
	unsigned long clk_flags;

	f = find_freq(f, rate);
	if (!f)
		return -EINVAL;

	clk_flags = __clk_get_flags(hw->clk);
	*p = clk_get_parent_by_index(hw->clk, f->src);
	if (clk_flags & CLK_SET_RATE_PARENT) {
		if (f->pre_div) {
			rate /= 2;
			rate *= f->pre_div + 1;
		}

		if (f->n) {
			u64 tmp = rate;
			tmp = tmp * f->n;
			do_div(tmp, f->m);
			rate = tmp;
		}
	} else {
		rate =  __clk_get_rate(*p);
	}
	*p_rate = rate;

	return f->freq;
}

static long clk_rcg2_determine_rate(struct clk_hw *hw, unsigned long rate,
		unsigned long *p_rate, struct clk **p)
{
	struct clk_rcg2 *rcg = to_clk_rcg2(hw);

	return _freq_tbl_determine_rate(hw, rcg->freq_tbl, rate, p_rate, p);
}

static int clk_rcg2_configure(struct clk_rcg2 *rcg, const struct freq_tbl *f)
{
	u32 cfg, mask;
	int ret;

	if (rcg->mnd_width && f->n) {
		mask = BIT(rcg->mnd_width) - 1;
		ret = regmap_update_bits(rcg->clkr.regmap,
				rcg->cmd_rcgr + M_REG, mask, f->m);
		if (ret)
			return ret;

		ret = regmap_update_bits(rcg->clkr.regmap,
				rcg->cmd_rcgr + N_REG, mask, ~(f->n - f->m));
		if (ret)
			return ret;

		ret = regmap_update_bits(rcg->clkr.regmap,
				rcg->cmd_rcgr + D_REG, mask, ~f->n);
		if (ret)
			return ret;
	}

	mask = BIT(rcg->hid_width) - 1;
	mask |= CFG_SRC_SEL_MASK | CFG_MODE_MASK;
	cfg = f->pre_div << CFG_SRC_DIV_SHIFT;
	cfg |= rcg->parent_map[f->src] << CFG_SRC_SEL_SHIFT;
	if (rcg->mnd_width && f->n)
		cfg |= CFG_MODE_DUAL_EDGE;
	ret = regmap_update_bits(rcg->clkr.regmap,
			rcg->cmd_rcgr + CFG_REG, mask, cfg);
	if (ret)
		return ret;

	return update_config(rcg);
}

static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate)
{
	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	const struct freq_tbl *f;

	f = find_freq(rcg->freq_tbl, rate);
	if (!f)
		return -EINVAL;

	return clk_rcg2_configure(rcg, f);
}

static int clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate,
			    unsigned long parent_rate)
{
	return __clk_rcg2_set_rate(hw, rate);
}

static int clk_rcg2_set_rate_and_parent(struct clk_hw *hw,
		unsigned long rate, unsigned long parent_rate, u8 index)
{
	return __clk_rcg2_set_rate(hw, rate);
}

const struct clk_ops clk_rcg2_ops = {
	.is_enabled = clk_rcg2_is_enabled,
	.get_parent = clk_rcg2_get_parent,
	.set_parent = clk_rcg2_set_parent,
	.recalc_rate = clk_rcg2_recalc_rate,
	.determine_rate = clk_rcg2_determine_rate,
	.set_rate = clk_rcg2_set_rate,
	.set_rate_and_parent = clk_rcg2_set_rate_and_parent,
};
EXPORT_SYMBOL_GPL(clk_rcg2_ops);

struct frac_entry {
	int num;
	int den;
};

static const struct frac_entry frac_table_675m[] = {	/* link rate of 270M */
	{ 52, 295 },	/* 119 M */
	{ 11, 57 },	/* 130.25 M */
	{ 63, 307 },	/* 138.50 M */
	{ 11, 50 },	/* 148.50 M */
	{ 47, 206 },	/* 154 M */
	{ 31, 100 },	/* 205.25 M */
	{ 107, 269 },	/* 268.50 M */
	{ },
};

static struct frac_entry frac_table_810m[] = { /* Link rate of 162M */
	{ 31, 211 },	/* 119 M */
	{ 32, 199 },	/* 130.25 M */
	{ 63, 307 },	/* 138.50 M */
	{ 11, 60 },	/* 148.50 M */
	{ 50, 263 },	/* 154 M */
	{ 31, 120 },	/* 205.25 M */
	{ 119, 359 },	/* 268.50 M */
	{ },
};

static int clk_edp_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
			      unsigned long parent_rate)
{
	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	struct freq_tbl f = *rcg->freq_tbl;
	const struct frac_entry *frac;
	int delta = 100000;
	s64 src_rate = parent_rate;
	s64 request;
	u32 mask = BIT(rcg->hid_width) - 1;
	u32 hid_div;

	if (src_rate == 810000000)
		frac = frac_table_810m;
	else
		frac = frac_table_675m;

	for (; frac->num; frac++) {
		request = rate;
		request *= frac->den;
		request = div_s64(request, frac->num);
		if ((src_rate < (request - delta)) ||
		    (src_rate > (request + delta)))
			continue;

		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
				&hid_div);
		f.pre_div = hid_div;
		f.pre_div >>= CFG_SRC_DIV_SHIFT;
		f.pre_div &= mask;
		f.m = frac->num;
		f.n = frac->den;

		return clk_rcg2_configure(rcg, &f);
	}

	return -EINVAL;
}

static int clk_edp_pixel_set_rate_and_parent(struct clk_hw *hw,
		unsigned long rate, unsigned long parent_rate, u8 index)
{
	/* Parent index is set statically in frequency table */
	return clk_edp_pixel_set_rate(hw, rate, parent_rate);
}

static long clk_edp_pixel_determine_rate(struct clk_hw *hw, unsigned long rate,
				 unsigned long *p_rate, struct clk **p)
{
	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	const struct freq_tbl *f = rcg->freq_tbl;
	const struct frac_entry *frac;
	int delta = 100000;
	s64 src_rate = *p_rate;
	s64 request;
	u32 mask = BIT(rcg->hid_width) - 1;
	u32 hid_div;

	/* Force the correct parent */
	*p = clk_get_parent_by_index(hw->clk, f->src);

	if (src_rate == 810000000)
		frac = frac_table_810m;
	else
		frac = frac_table_675m;

	for (; frac->num; frac++) {
		request = rate;
		request *= frac->den;
		request = div_s64(request, frac->num);
		if ((src_rate < (request - delta)) ||
		    (src_rate > (request + delta)))
			continue;

		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
				&hid_div);
		hid_div >>= CFG_SRC_DIV_SHIFT;
		hid_div &= mask;

		return calc_rate(src_rate, frac->num, frac->den, !!frac->den,
				 hid_div);
	}

	return -EINVAL;
}

const struct clk_ops clk_edp_pixel_ops = {
	.is_enabled = clk_rcg2_is_enabled,
	.get_parent = clk_rcg2_get_parent,
	.set_parent = clk_rcg2_set_parent,
	.recalc_rate = clk_rcg2_recalc_rate,
	.set_rate = clk_edp_pixel_set_rate,
	.set_rate_and_parent = clk_edp_pixel_set_rate_and_parent,
	.determine_rate = clk_edp_pixel_determine_rate,
};
EXPORT_SYMBOL_GPL(clk_edp_pixel_ops);

static long clk_byte_determine_rate(struct clk_hw *hw, unsigned long rate,
			 unsigned long *p_rate, struct clk **p)
{
	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	const struct freq_tbl *f = rcg->freq_tbl;
	unsigned long parent_rate, div;
	u32 mask = BIT(rcg->hid_width) - 1;

	if (rate == 0)
		return -EINVAL;

	*p = clk_get_parent_by_index(hw->clk, f->src);
	*p_rate = parent_rate = __clk_round_rate(*p, rate);

	div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
	div = min_t(u32, div, mask);

	return calc_rate(parent_rate, 0, 0, 0, div);
}

static int clk_byte_set_rate(struct clk_hw *hw, unsigned long rate,
			 unsigned long parent_rate)
{
	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	struct freq_tbl f = *rcg->freq_tbl;
	unsigned long div;
	u32 mask = BIT(rcg->hid_width) - 1;

	div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
	div = min_t(u32, div, mask);

	f.pre_div = div;

	return clk_rcg2_configure(rcg, &f);
}

static int clk_byte_set_rate_and_parent(struct clk_hw *hw,
		unsigned long rate, unsigned long parent_rate, u8 index)
{
	/* Parent index is set statically in frequency table */
	return clk_byte_set_rate(hw, rate, parent_rate);
}

const struct clk_ops clk_byte_ops = {
	.is_enabled = clk_rcg2_is_enabled,
	.get_parent = clk_rcg2_get_parent,
	.set_parent = clk_rcg2_set_parent,
	.recalc_rate = clk_rcg2_recalc_rate,
	.set_rate = clk_byte_set_rate,
	.set_rate_and_parent = clk_byte_set_rate_and_parent,
	.determine_rate = clk_byte_determine_rate,
};
EXPORT_SYMBOL_GPL(clk_byte_ops);

static const struct frac_entry frac_table_pixel[] = {
	{ 3, 8 },
	{ 2, 9 },
	{ 4, 9 },
	{ 1, 1 },
	{ }
};

static long clk_pixel_determine_rate(struct clk_hw *hw, unsigned long rate,
				 unsigned long *p_rate, struct clk **p)
{
	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	unsigned long request, src_rate;
	int delta = 100000;
	const struct freq_tbl *f = rcg->freq_tbl;
	const struct frac_entry *frac = frac_table_pixel;
	struct clk *parent = *p = clk_get_parent_by_index(hw->clk, f->src);

	for (; frac->num; frac++) {
		request = (rate * frac->den) / frac->num;

		src_rate = __clk_round_rate(parent, request);
		if ((src_rate < (request - delta)) ||
			(src_rate > (request + delta)))
			continue;

		*p_rate = src_rate;
		return (src_rate * frac->num) / frac->den;
	}

	return -EINVAL;
}

static int clk_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
		unsigned long parent_rate)
{
	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	struct freq_tbl f = *rcg->freq_tbl;
	const struct frac_entry *frac = frac_table_pixel;
	unsigned long request, src_rate;
	int delta = 100000;
	u32 mask = BIT(rcg->hid_width) - 1;
	u32 hid_div;
	struct clk *parent = clk_get_parent_by_index(hw->clk, f.src);

	for (; frac->num; frac++) {
		request = (rate * frac->den) / frac->num;

		src_rate = __clk_round_rate(parent, request);
		if ((src_rate < (request - delta)) ||
			(src_rate > (request + delta)))
			continue;

		regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
				&hid_div);
		f.pre_div = hid_div;
		f.pre_div >>= CFG_SRC_DIV_SHIFT;
		f.pre_div &= mask;
		f.m = frac->num;
		f.n = frac->den;

		return clk_rcg2_configure(rcg, &f);
	}
	return -EINVAL;
}

static int clk_pixel_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
		unsigned long parent_rate, u8 index)
{
	/* Parent index is set statically in frequency table */
	return clk_pixel_set_rate(hw, rate, parent_rate);
}

const struct clk_ops clk_pixel_ops = {
	.is_enabled = clk_rcg2_is_enabled,
	.get_parent = clk_rcg2_get_parent,
	.set_parent = clk_rcg2_set_parent,
	.recalc_rate = clk_rcg2_recalc_rate,
	.set_rate = clk_pixel_set_rate,
	.set_rate_and_parent = clk_pixel_set_rate_and_parent,
	.determine_rate = clk_pixel_determine_rate,
};
EXPORT_SYMBOL_GPL(clk_pixel_ops);
Commit	Line	Data
bcd61c0f SB	1	/*
	2	* Copyright (c) 2013, The Linux Foundation. All rights reserved.
	3	*
	4	* This software is licensed under the terms of the GNU General Public
	5	* License version 2, as published by the Free Software Foundation, and
	6	* may be copied, distributed, and modified under those terms.
	7	*
	8	* This program is distributed in the hope that it will be useful,
	9	* but WITHOUT ANY WARRANTY; without even the implied warranty of
	10	* MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
	11	* GNU General Public License for more details.
	12	*/
	13
	14	#include <linux/kernel.h>
	15	#include <linux/bitops.h>
	16	#include <linux/err.h>
	17	#include <linux/bug.h>
	18	#include <linux/export.h>
	19	#include <linux/clk-provider.h>
	20	#include <linux/delay.h>
	21	#include <linux/regmap.h>
99cbd064	22	#include <linux/math64.h>
bcd61c0f SB	23
	24	#include <asm/div64.h>
	25
	26	#include "clk-rcg.h"
	27
	28	#define CMD_REG 0x0
	29	#define CMD_UPDATE BIT(0)
	30	#define CMD_ROOT_EN BIT(1)
	31	#define CMD_DIRTY_CFG BIT(4)
	32	#define CMD_DIRTY_N BIT(5)
	33	#define CMD_DIRTY_M BIT(6)
	34	#define CMD_DIRTY_D BIT(7)
	35	#define CMD_ROOT_OFF BIT(31)
	36
	37	#define CFG_REG 0x4
	38	#define CFG_SRC_DIV_SHIFT 0
	39	#define CFG_SRC_SEL_SHIFT 8
	40	#define CFG_SRC_SEL_MASK (0x7 << CFG_SRC_SEL_SHIFT)
	41	#define CFG_MODE_SHIFT 12
	42	#define CFG_MODE_MASK (0x3 << CFG_MODE_SHIFT)
	43	#define CFG_MODE_DUAL_EDGE (0x2 << CFG_MODE_SHIFT)
	44
	45	#define M_REG 0x8
	46	#define N_REG 0xc
	47	#define D_REG 0x10
	48
	49	static int clk_rcg2_is_enabled(struct clk_hw *hw)
	50	{
	51	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	52	u32 cmd;
	53	int ret;
	54
	55	ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, &cmd);
	56	if (ret)
	57	return ret;
	58
aa014149	59	return (cmd & CMD_ROOT_OFF) == 0;
bcd61c0f SB	60	}
	61
	62	static u8 clk_rcg2_get_parent(struct clk_hw *hw)
	63	{
	64	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	65	int num_parents = __clk_get_num_parents(hw->clk);
	66	u32 cfg;
	67	int i, ret;
	68
	69	ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
	70	if (ret)
	71	return ret;
	72
	73	cfg &= CFG_SRC_SEL_MASK;
	74	cfg >>= CFG_SRC_SEL_SHIFT;
	75
	76	for (i = 0; i < num_parents; i++)
	77	if (cfg == rcg->parent_map[i])
	78	return i;
	79
	80	return -EINVAL;
	81	}
	82
	83	static int update_config(struct clk_rcg2 *rcg)
	84	{
	85	int count, ret;
	86	u32 cmd;
	87	struct clk_hw *hw = &rcg->clkr.hw;
	88	const char *name = __clk_get_name(hw->clk);
	89
	90	ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG,
	91	CMD_UPDATE, CMD_UPDATE);
	92	if (ret)
	93	return ret;
	94
	95	/* Wait for update to take effect */
	96	for (count = 500; count > 0; count--) {
	97	ret = regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CMD_REG, &cmd);
	98	if (ret)
	99	return ret;
	100	if (!(cmd & CMD_UPDATE))
	101	return 0;
	102	udelay(1);
	103	}
	104
	105	WARN(1, "%s: rcg didn't update its configuration.", name);
	106	return 0;
	107	}
	108
	109	static int clk_rcg2_set_parent(struct clk_hw *hw, u8 index)
	110	{
	111	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	112	int ret;
	113
	114	ret = regmap_update_bits(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
	115	CFG_SRC_SEL_MASK,
	116	rcg->parent_map[index] << CFG_SRC_SEL_SHIFT);
	117	if (ret)
	118	return ret;
	119
	120	return update_config(rcg);
	121	}
	122
	123	/*
124	* Calculate m/n:d rate
125	*
126	* parent_rate m
127	* rate = ----------- x ---
128	* hid_div n
129	*/
130	static unsigned long
131	calc_rate(unsigned long rate, u32 m, u32 n, u32 mode, u32 hid_div)
132	{
133	if (hid_div) {
134	rate *= 2;
135	rate /= hid_div + 1;
136	}
137
138	if (mode) {
139	u64 tmp = rate;
140	tmp *= m;
141	do_div(tmp, n);
142	rate = tmp;
143	}
144
145	return rate;
146	}
147
148	static unsigned long
149	clk_rcg2_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
150	{
151	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
152	u32 cfg, hid_div, m = 0, n = 0, mode = 0, mask;
153
154	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &cfg);
155
156	if (rcg->mnd_width) {
157	mask = BIT(rcg->mnd_width) - 1;
158	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + M_REG, &m);
159	m &= mask;
160	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + N_REG, &n);
161	n = ~n;
162	n &= mask;
163	n += m;
164	mode = cfg & CFG_MODE_MASK;
165	mode >>= CFG_MODE_SHIFT;
166	}
167
168	mask = BIT(rcg->hid_width) - 1;
169	hid_div = cfg >> CFG_SRC_DIV_SHIFT;
170	hid_div &= mask;
171
172	return calc_rate(parent_rate, m, n, mode, hid_div);
173	}
174
175	static const
176	struct freq_tbl find_freq(const struct freq_tbl f, unsigned long rate)
177	{
178	if (!f)
179	return NULL;
180
181	for (; f->freq; f++)
182	if (rate <= f->freq)
183	return f;
184
9d011f3b SB	185	/* Default to our fastest rate */
9d011f3b SB	186	return f - 1;
bcd61c0f SB	187	}
	188
	189	static long _freq_tbl_determine_rate(struct clk_hw *hw,
	190	const struct freq_tbl *f, unsigned long rate,
	191	unsigned long p_rate, struct clk *p)
	192	{
	193	unsigned long clk_flags;
	194
	195	f = find_freq(f, rate);
	196	if (!f)
	197	return -EINVAL;
	198
	199	clk_flags = __clk_get_flags(hw->clk);
	200	*p = clk_get_parent_by_index(hw->clk, f->src);
	201	if (clk_flags & CLK_SET_RATE_PARENT) {
	202	if (f->pre_div) {
	203	rate /= 2;
	204	rate *= f->pre_div + 1;
	205	}
	206
	207	if (f->n) {
	208	u64 tmp = rate;
	209	tmp = tmp * f->n;
	210	do_div(tmp, f->m);
	211	rate = tmp;
	212	}
	213	} else {
	214	rate = __clk_get_rate(*p);
	215	}
	216	*p_rate = rate;
	217
	218	return f->freq;
	219	}
	220
	221	static long clk_rcg2_determine_rate(struct clk_hw *hw, unsigned long rate,
	222	unsigned long p_rate, struct clk *p)
	223	{
	224	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	225
	226	return _freq_tbl_determine_rate(hw, rcg->freq_tbl, rate, p_rate, p);
	227	}
	228
99cbd064	229	static int clk_rcg2_configure(struct clk_rcg2 rcg, const struct freq_tbl f)
bcd61c0f	230	{
bcd61c0f SB	231	u32 cfg, mask;
	232	int ret;
	233
bcd61c0f SB	234	if (rcg->mnd_width && f->n) {
bcd61c0f SB	235	mask = BIT(rcg->mnd_width) - 1;
99cbd064 SB	236	ret = regmap_update_bits(rcg->clkr.regmap,
99cbd064 SB	237	rcg->cmd_rcgr + M_REG, mask, f->m);
bcd61c0f SB	238	if (ret)
	239	return ret;
	240
99cbd064 SB	241	ret = regmap_update_bits(rcg->clkr.regmap,
99cbd064 SB	242	rcg->cmd_rcgr + N_REG, mask, ~(f->n - f->m));
bcd61c0f SB	243	if (ret)
	244	return ret;
	245
99cbd064 SB	246	ret = regmap_update_bits(rcg->clkr.regmap,
99cbd064 SB	247	rcg->cmd_rcgr + D_REG, mask, ~f->n);
bcd61c0f SB	248	if (ret)
	249	return ret;
	250	}
	251
	252	mask = BIT(rcg->hid_width) - 1;
	253	mask \|= CFG_SRC_SEL_MASK \| CFG_MODE_MASK;
	254	cfg = f->pre_div << CFG_SRC_DIV_SHIFT;
	255	cfg \|= rcg->parent_map[f->src] << CFG_SRC_SEL_SHIFT;
	256	if (rcg->mnd_width && f->n)
	257	cfg \|= CFG_MODE_DUAL_EDGE;
99cbd064 SB	258	ret = regmap_update_bits(rcg->clkr.regmap,
99cbd064 SB	259	rcg->cmd_rcgr + CFG_REG, mask, cfg);
bcd61c0f SB	260	if (ret)
	261	return ret;
	262
	263	return update_config(rcg);
	264	}
	265
99cbd064 SB	266	static int __clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate)
	267	{
	268	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	269	const struct freq_tbl *f;
	270
	271	f = find_freq(rcg->freq_tbl, rate);
	272	if (!f)
	273	return -EINVAL;
	274
	275	return clk_rcg2_configure(rcg, f);
	276	}
	277
bcd61c0f SB	278	static int clk_rcg2_set_rate(struct clk_hw *hw, unsigned long rate,
	279	unsigned long parent_rate)
	280	{
	281	return __clk_rcg2_set_rate(hw, rate);
	282	}
	283
	284	static int clk_rcg2_set_rate_and_parent(struct clk_hw *hw,
	285	unsigned long rate, unsigned long parent_rate, u8 index)
	286	{
	287	return __clk_rcg2_set_rate(hw, rate);
	288	}
	289
	290	const struct clk_ops clk_rcg2_ops = {
	291	.is_enabled = clk_rcg2_is_enabled,
	292	.get_parent = clk_rcg2_get_parent,
	293	.set_parent = clk_rcg2_set_parent,
	294	.recalc_rate = clk_rcg2_recalc_rate,
	295	.determine_rate = clk_rcg2_determine_rate,
	296	.set_rate = clk_rcg2_set_rate,
	297	.set_rate_and_parent = clk_rcg2_set_rate_and_parent,
	298	};
	299	EXPORT_SYMBOL_GPL(clk_rcg2_ops);
99cbd064 SB	300
	301	struct frac_entry {
	302	int num;
	303	int den;
	304	};
	305
	306	static const struct frac_entry frac_table_675m[] = { /* link rate of 270M */
	307	{ 52, 295 }, /* 119 M */
	308	{ 11, 57 }, /* 130.25 M */
	309	{ 63, 307 }, /* 138.50 M */
	310	{ 11, 50 }, /* 148.50 M */
	311	{ 47, 206 }, /* 154 M */
	312	{ 31, 100 }, /* 205.25 M */
	313	{ 107, 269 }, /* 268.50 M */
	314	{ },
	315	};
	316
	317	static struct frac_entry frac_table_810m[] = { /* Link rate of 162M */
	318	{ 31, 211 }, /* 119 M */
	319	{ 32, 199 }, /* 130.25 M */
	320	{ 63, 307 }, /* 138.50 M */
	321	{ 11, 60 }, /* 148.50 M */
	322	{ 50, 263 }, /* 154 M */
	323	{ 31, 120 }, /* 205.25 M */
	324	{ 119, 359 }, /* 268.50 M */
	325	{ },
	326	};
	327
	328	static int clk_edp_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
	329	unsigned long parent_rate)
	330	{
	331	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
	332	struct freq_tbl f = *rcg->freq_tbl;
	333	const struct frac_entry *frac;
	334	int delta = 100000;
	335	s64 src_rate = parent_rate;
	336	s64 request;
	337	u32 mask = BIT(rcg->hid_width) - 1;
	338	u32 hid_div;
	339
	340	if (src_rate == 810000000)
	341	frac = frac_table_810m;
	342	else
	343	frac = frac_table_675m;
	344
	345	for (; frac->num; frac++) {
	346	request = rate;
	347	request *= frac->den;
	348	request = div_s64(request, frac->num);
	349	if ((src_rate < (request - delta)) \|\|
	350	(src_rate > (request + delta)))
	351	continue;
	352
	353	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
	354	&hid_div);
	355	f.pre_div = hid_div;
	356	f.pre_div >>= CFG_SRC_DIV_SHIFT;
	357	f.pre_div &= mask;
	358	f.m = frac->num;
	359	f.n = frac->den;
	360
	361	return clk_rcg2_configure(rcg, &f);
	362	}
	363
364	return -EINVAL;
365	}
366
367	static int clk_edp_pixel_set_rate_and_parent(struct clk_hw *hw,
368	unsigned long rate, unsigned long parent_rate, u8 index)
369	{
370	/* Parent index is set statically in frequency table */
371	return clk_edp_pixel_set_rate(hw, rate, parent_rate);
372	}
373
374	static long clk_edp_pixel_determine_rate(struct clk_hw *hw, unsigned long rate,
375	unsigned long p_rate, struct clk *p)
376	{
377	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
378	const struct freq_tbl *f = rcg->freq_tbl;
379	const struct frac_entry *frac;
380	int delta = 100000;
381	s64 src_rate = *p_rate;
382	s64 request;
383	u32 mask = BIT(rcg->hid_width) - 1;
384	u32 hid_div;
385
386	/* Force the correct parent */
387	*p = clk_get_parent_by_index(hw->clk, f->src);
388
389	if (src_rate == 810000000)
390	frac = frac_table_810m;
391	else
392	frac = frac_table_675m;
393
394	for (; frac->num; frac++) {
395	request = rate;
396	request *= frac->den;
397	request = div_s64(request, frac->num);
398	if ((src_rate < (request - delta)) \|\|
399	(src_rate > (request + delta)))
400	continue;
401
402	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
403	&hid_div);
404	hid_div >>= CFG_SRC_DIV_SHIFT;
405	hid_div &= mask;
406
407	return calc_rate(src_rate, frac->num, frac->den, !!frac->den,
408	hid_div);
409	}
410
411	return -EINVAL;
412	}
413
414	const struct clk_ops clk_edp_pixel_ops = {
415	.is_enabled = clk_rcg2_is_enabled,
416	.get_parent = clk_rcg2_get_parent,
417	.set_parent = clk_rcg2_set_parent,
418	.recalc_rate = clk_rcg2_recalc_rate,
419	.set_rate = clk_edp_pixel_set_rate,
420	.set_rate_and_parent = clk_edp_pixel_set_rate_and_parent,
421	.determine_rate = clk_edp_pixel_determine_rate,
422	};
423	EXPORT_SYMBOL_GPL(clk_edp_pixel_ops);
424
425	static long clk_byte_determine_rate(struct clk_hw *hw, unsigned long rate,
426	unsigned long p_rate, struct clk *p)
427	{
428	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
429	const struct freq_tbl *f = rcg->freq_tbl;
430	unsigned long parent_rate, div;
431	u32 mask = BIT(rcg->hid_width) - 1;
432
433	if (rate == 0)
434	return -EINVAL;
435
436	*p = clk_get_parent_by_index(hw->clk, f->src);
437	p_rate = parent_rate = __clk_round_rate(p, rate);
438
439	div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
440	div = min_t(u32, div, mask);
441
442	return calc_rate(parent_rate, 0, 0, 0, div);
443	}
444
445	static int clk_byte_set_rate(struct clk_hw *hw, unsigned long rate,
446	unsigned long parent_rate)
447	{
448	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
449	struct freq_tbl f = *rcg->freq_tbl;
450	unsigned long div;
451	u32 mask = BIT(rcg->hid_width) - 1;
452
453	div = DIV_ROUND_UP((2 * parent_rate), rate) - 1;
454	div = min_t(u32, div, mask);
455
456	f.pre_div = div;
457
458	return clk_rcg2_configure(rcg, &f);
459	}
460
461	static int clk_byte_set_rate_and_parent(struct clk_hw *hw,
462	unsigned long rate, unsigned long parent_rate, u8 index)
463	{
464	/* Parent index is set statically in frequency table */
465	return clk_byte_set_rate(hw, rate, parent_rate);
466	}
467
468	const struct clk_ops clk_byte_ops = {
469	.is_enabled = clk_rcg2_is_enabled,
470	.get_parent = clk_rcg2_get_parent,
471	.set_parent = clk_rcg2_set_parent,
472	.recalc_rate = clk_rcg2_recalc_rate,
473	.set_rate = clk_byte_set_rate,
474	.set_rate_and_parent = clk_byte_set_rate_and_parent,
475	.determine_rate = clk_byte_determine_rate,
476	};
477	EXPORT_SYMBOL_GPL(clk_byte_ops);
478
479	static const struct frac_entry frac_table_pixel[] = {
480	{ 3, 8 },
481	{ 2, 9 },
482	{ 4, 9 },
483	{ 1, 1 },
484	{ }
485	};
486
487	static long clk_pixel_determine_rate(struct clk_hw *hw, unsigned long rate,
488	unsigned long p_rate, struct clk *p)
489	{
490	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
491	unsigned long request, src_rate;
492	int delta = 100000;
493	const struct freq_tbl *f = rcg->freq_tbl;
494	const struct frac_entry *frac = frac_table_pixel;
495	struct clk parent = p = clk_get_parent_by_index(hw->clk, f->src);
496
497	for (; frac->num; frac++) {
498	request = (rate * frac->den) / frac->num;
499
500	src_rate = __clk_round_rate(parent, request);
501	if ((src_rate < (request - delta)) \|\|
502	(src_rate > (request + delta)))
503	continue;
504
505	*p_rate = src_rate;
506	return (src_rate * frac->num) / frac->den;
507	}
508
509	return -EINVAL;
510	}
511
512	static int clk_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
513	unsigned long parent_rate)
514	{
515	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
516	struct freq_tbl f = *rcg->freq_tbl;
517	const struct frac_entry *frac = frac_table_pixel;
518	unsigned long request, src_rate;
519	int delta = 100000;
520	u32 mask = BIT(rcg->hid_width) - 1;
521	u32 hid_div;
522	struct clk *parent = clk_get_parent_by_index(hw->clk, f.src);
523
524	for (; frac->num; frac++) {
525	request = (rate * frac->den) / frac->num;
526
527	src_rate = __clk_round_rate(parent, request);
528	if ((src_rate < (request - delta)) \|\|
529	(src_rate > (request + delta)))
530	continue;
531
532	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG,
533	&hid_div);
534	f.pre_div = hid_div;
535	f.pre_div >>= CFG_SRC_DIV_SHIFT;
536	f.pre_div &= mask;
537	f.m = frac->num;
538	f.n = frac->den;
539
540	return clk_rcg2_configure(rcg, &f);
541	}
542	return -EINVAL;
543	}
544
545	static int clk_pixel_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
546	unsigned long parent_rate, u8 index)
547	{
548	/* Parent index is set statically in frequency table */
549	return clk_pixel_set_rate(hw, rate, parent_rate);
550	}
551
552	const struct clk_ops clk_pixel_ops = {
553	.is_enabled = clk_rcg2_is_enabled,
554	.get_parent = clk_rcg2_get_parent,
555	.set_parent = clk_rcg2_set_parent,
556	.recalc_rate = clk_rcg2_recalc_rate,
557	.set_rate = clk_pixel_set_rate,
558	.set_rate_and_parent = clk_pixel_set_rate_and_parent,
559	.determine_rate = clk_pixel_determine_rate,
560	};
561	EXPORT_SYMBOL_GPL(clk_pixel_ops);