[mirror_ubuntu-bionic-kernel.git] / drivers / clk / mmp / clk-mix.c

/*
 * mmp mix(div and mux) clock operation source file
 *
 * Copyright (C) 2014 Marvell
 * Chao Xie <chao.xie@marvell.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"

/*
 * The mix clock is a clock combined mux and div type clock.
 * Because the div field and mux field need to be set at same
 * time, we can not divide it into 2 types of clock
 */

#define to_clk_mix(hw)	container_of(hw, struct mmp_clk_mix, hw)

static unsigned int _get_maxdiv(struct mmp_clk_mix *mix)
{
	unsigned int div_mask = (1 << mix->reg_info.width_div) - 1;
	unsigned int maxdiv = 0;
	struct clk_div_table *clkt;

	if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
		return div_mask;
	if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
		return 1 << div_mask;
	if (mix->div_table) {
		for (clkt = mix->div_table; clkt->div; clkt++)
			if (clkt->div > maxdiv)
				maxdiv = clkt->div;
		return maxdiv;
	}
	return div_mask + 1;
}

static unsigned int _get_div(struct mmp_clk_mix *mix, unsigned int val)
{
	struct clk_div_table *clkt;

	if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
		return val;
	if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
		return 1 << val;
	if (mix->div_table) {
		for (clkt = mix->div_table; clkt->div; clkt++)
			if (clkt->val == val)
				return clkt->div;
		if (clkt->div == 0)
			return 0;
	}
	return val + 1;
}

static unsigned int _get_mux(struct mmp_clk_mix *mix, unsigned int val)
{
	int num_parents = clk_hw_get_num_parents(&mix->hw);
	int i;

	if (mix->mux_flags & CLK_MUX_INDEX_BIT)
		return ffs(val) - 1;
	if (mix->mux_flags & CLK_MUX_INDEX_ONE)
		return val - 1;
	if (mix->mux_table) {
		for (i = 0; i < num_parents; i++)
			if (mix->mux_table[i] == val)
				return i;
		if (i == num_parents)
			return 0;
	}

	return val;
}
static unsigned int _get_div_val(struct mmp_clk_mix *mix, unsigned int div)
{
	struct clk_div_table *clkt;

	if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
		return div;
	if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
		return __ffs(div);
	if (mix->div_table) {
		for (clkt = mix->div_table; clkt->div; clkt++)
			if (clkt->div == div)
				return clkt->val;
		if (clkt->div == 0)
			return 0;
	}

	return div - 1;
}

static unsigned int _get_mux_val(struct mmp_clk_mix *mix, unsigned int mux)
{
	if (mix->mux_table)
		return mix->mux_table[mux];

	return mux;
}

static void _filter_clk_table(struct mmp_clk_mix *mix,
				struct mmp_clk_mix_clk_table *table,
				unsigned int table_size)
{
	int i;
	struct mmp_clk_mix_clk_table *item;
	struct clk_hw *parent, *hw;
	unsigned long parent_rate;

	hw = &mix->hw;

	for (i = 0; i < table_size; i++) {
		item = &table[i];
		parent = clk_hw_get_parent_by_index(hw, item->parent_index);
		parent_rate = clk_hw_get_rate(parent);
		if (parent_rate % item->rate) {
			item->valid = 0;
		} else {
			item->divisor = parent_rate / item->rate;
			item->valid = 1;
		}
	}
}

static int _set_rate(struct mmp_clk_mix *mix, u32 mux_val, u32 div_val,
			unsigned int change_mux, unsigned int change_div)
{
	struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
	u8 width, shift;
	u32 mux_div, fc_req;
	int ret, timeout = 50;
	unsigned long flags = 0;

	if (!change_mux && !change_div)
		return -EINVAL;

	if (mix->lock)
		spin_lock_irqsave(mix->lock, flags);

	if (mix->type == MMP_CLK_MIX_TYPE_V1
		|| mix->type == MMP_CLK_MIX_TYPE_V2)
		mux_div = readl(ri->reg_clk_ctrl);
	else
		mux_div = readl(ri->reg_clk_sel);

	if (change_div) {
		width = ri->width_div;
		shift = ri->shift_div;
		mux_div &= ~MMP_CLK_BITS_MASK(width, shift);
		mux_div |= MMP_CLK_BITS_SET_VAL(div_val, width, shift);
	}

	if (change_mux) {
		width = ri->width_mux;
		shift = ri->shift_mux;
		mux_div &= ~MMP_CLK_BITS_MASK(width, shift);
		mux_div |= MMP_CLK_BITS_SET_VAL(mux_val, width, shift);
	}

	if (mix->type == MMP_CLK_MIX_TYPE_V1) {
		writel(mux_div, ri->reg_clk_ctrl);
	} else if (mix->type == MMP_CLK_MIX_TYPE_V2) {
		mux_div |= (1 << ri->bit_fc);
		writel(mux_div, ri->reg_clk_ctrl);

		do {
			fc_req = readl(ri->reg_clk_ctrl);
			timeout--;
			if (!(fc_req & (1 << ri->bit_fc)))
				break;
		} while (timeout);

		if (timeout == 0) {
			pr_err("%s:%s cannot do frequency change\n",
				__func__, clk_hw_get_name(&mix->hw));
			ret = -EBUSY;
			goto error;
		}
	} else {
		fc_req = readl(ri->reg_clk_ctrl);
		fc_req |= 1 << ri->bit_fc;
		writel(fc_req, ri->reg_clk_ctrl);
		writel(mux_div, ri->reg_clk_sel);
		fc_req &= ~(1 << ri->bit_fc);
	}

	ret = 0;
error:
	if (mix->lock)
		spin_unlock_irqrestore(mix->lock, flags);

	return ret;
}

static int mmp_clk_mix_determine_rate(struct clk_hw *hw,
				      struct clk_rate_request *req)
{
	struct mmp_clk_mix *mix = to_clk_mix(hw);
	struct mmp_clk_mix_clk_table *item;
	struct clk_hw *parent, *parent_best;
	unsigned long parent_rate, mix_rate, mix_rate_best, parent_rate_best;
	unsigned long gap, gap_best;
	u32 div_val_max;
	unsigned int div;
	int i, j;


	mix_rate_best = 0;
	parent_rate_best = 0;
	gap_best = ULONG_MAX;
	parent_best = NULL;

	if (mix->table) {
		for (i = 0; i < mix->table_size; i++) {
			item = &mix->table[i];
			if (item->valid == 0)
				continue;
			parent = clk_hw_get_parent_by_index(hw,
							item->parent_index);
			parent_rate = clk_hw_get_rate(parent);
			mix_rate = parent_rate / item->divisor;
			gap = abs(mix_rate - req->rate);
			if (parent_best == NULL || gap < gap_best) {
				parent_best = parent;
				parent_rate_best = parent_rate;
				mix_rate_best = mix_rate;
				gap_best = gap;
				if (gap_best == 0)
					goto found;
			}
		}
	} else {
		for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
			parent = clk_hw_get_parent_by_index(hw, i);
			parent_rate = clk_hw_get_rate(parent);
			div_val_max = _get_maxdiv(mix);
			for (j = 0; j < div_val_max; j++) {
				div = _get_div(mix, j);
				mix_rate = parent_rate / div;
				gap = abs(mix_rate - req->rate);
				if (parent_best == NULL || gap < gap_best) {
					parent_best = parent;
					parent_rate_best = parent_rate;
					mix_rate_best = mix_rate;
					gap_best = gap;
					if (gap_best == 0)
						goto found;
				}
			}
		}
	}

found:
	if (!parent_best)
		return -EINVAL;

	req->best_parent_rate = parent_rate_best;
	req->best_parent_hw = parent_best;
	req->rate = mix_rate_best;

	return 0;
}

static int mmp_clk_mix_set_rate_and_parent(struct clk_hw *hw,
						unsigned long rate,
						unsigned long parent_rate,
						u8 index)
{
	struct mmp_clk_mix *mix = to_clk_mix(hw);
	unsigned int div;
	u32 div_val, mux_val;

	div = parent_rate / rate;
	div_val = _get_div_val(mix, div);
	mux_val = _get_mux_val(mix, index);

	return _set_rate(mix, mux_val, div_val, 1, 1);
}

static u8 mmp_clk_mix_get_parent(struct clk_hw *hw)
{
	struct mmp_clk_mix *mix = to_clk_mix(hw);
	struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
	unsigned long flags = 0;
	u32 mux_div = 0;
	u8 width, shift;
	u32 mux_val;

	if (mix->lock)
		spin_lock_irqsave(mix->lock, flags);

	if (mix->type == MMP_CLK_MIX_TYPE_V1
		|| mix->type == MMP_CLK_MIX_TYPE_V2)
		mux_div = readl(ri->reg_clk_ctrl);
	else
		mux_div = readl(ri->reg_clk_sel);

	if (mix->lock)
		spin_unlock_irqrestore(mix->lock, flags);

	width = mix->reg_info.width_mux;
	shift = mix->reg_info.shift_mux;

	mux_val = MMP_CLK_BITS_GET_VAL(mux_div, width, shift);

	return _get_mux(mix, mux_val);
}

static unsigned long mmp_clk_mix_recalc_rate(struct clk_hw *hw,
					unsigned long parent_rate)
{
	struct mmp_clk_mix *mix = to_clk_mix(hw);
	struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
	unsigned long flags = 0;
	u32 mux_div = 0;
	u8 width, shift;
	unsigned int div;

	if (mix->lock)
		spin_lock_irqsave(mix->lock, flags);

	if (mix->type == MMP_CLK_MIX_TYPE_V1
		|| mix->type == MMP_CLK_MIX_TYPE_V2)
		mux_div = readl(ri->reg_clk_ctrl);
	else
		mux_div = readl(ri->reg_clk_sel);

	if (mix->lock)
		spin_unlock_irqrestore(mix->lock, flags);

	width = mix->reg_info.width_div;
	shift = mix->reg_info.shift_div;

	div = _get_div(mix, MMP_CLK_BITS_GET_VAL(mux_div, width, shift));

	return parent_rate / div;
}

static int mmp_clk_set_parent(struct clk_hw *hw, u8 index)
{
	struct mmp_clk_mix *mix = to_clk_mix(hw);
	struct mmp_clk_mix_clk_table *item;
	int i;
	u32 div_val, mux_val;

	if (mix->table) {
		for (i = 0; i < mix->table_size; i++) {
			item = &mix->table[i];
			if (item->valid == 0)
				continue;
			if (item->parent_index == index)
				break;
		}
		if (i < mix->table_size) {
			div_val = _get_div_val(mix, item->divisor);
			mux_val = _get_mux_val(mix, item->parent_index);
		} else
			return -EINVAL;
	} else {
		mux_val = _get_mux_val(mix, index);
		div_val = 0;
	}

	return _set_rate(mix, mux_val, div_val, 1, div_val ? 1 : 0);
}

static int mmp_clk_set_rate(struct clk_hw *hw, unsigned long rate,
				unsigned long best_parent_rate)
{
	struct mmp_clk_mix *mix = to_clk_mix(hw);
	struct mmp_clk_mix_clk_table *item;
	unsigned long parent_rate;
	unsigned int best_divisor;
	struct clk_hw *parent;
	int i;

	best_divisor = best_parent_rate / rate;

	if (mix->table) {
		for (i = 0; i < mix->table_size; i++) {
			item = &mix->table[i];
			if (item->valid == 0)
				continue;
			parent = clk_hw_get_parent_by_index(hw,
							item->parent_index);
			parent_rate = clk_hw_get_rate(parent);
			if (parent_rate == best_parent_rate
				&& item->divisor == best_divisor)
				break;
		}
		if (i < mix->table_size)
			return _set_rate(mix,
					_get_mux_val(mix, item->parent_index),
					_get_div_val(mix, item->divisor),
					1, 1);
		else
			return -EINVAL;
	} else {
		for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
			parent = clk_hw_get_parent_by_index(hw, i);
			parent_rate = clk_hw_get_rate(parent);
			if (parent_rate == best_parent_rate)
				break;
		}
		if (i < clk_hw_get_num_parents(hw))
			return _set_rate(mix, _get_mux_val(mix, i),
					_get_div_val(mix, best_divisor), 1, 1);
		else
			return -EINVAL;
	}
}

static void mmp_clk_mix_init(struct clk_hw *hw)
{
	struct mmp_clk_mix *mix = to_clk_mix(hw);

	if (mix->table)
		_filter_clk_table(mix, mix->table, mix->table_size);
}

const struct clk_ops mmp_clk_mix_ops = {
	.determine_rate = mmp_clk_mix_determine_rate,
	.set_rate_and_parent = mmp_clk_mix_set_rate_and_parent,
	.set_rate = mmp_clk_set_rate,
	.set_parent = mmp_clk_set_parent,
	.get_parent = mmp_clk_mix_get_parent,
	.recalc_rate = mmp_clk_mix_recalc_rate,
	.init = mmp_clk_mix_init,
};

struct clk *mmp_clk_register_mix(struct device *dev,
					const char *name,
					const char **parent_names,
					u8 num_parents,
					unsigned long flags,
					struct mmp_clk_mix_config *config,
					spinlock_t *lock)
{
	struct mmp_clk_mix *mix;
	struct clk *clk;
	struct clk_init_data init;
	size_t table_bytes;

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

	init.name = name;
	init.flags = flags | CLK_GET_RATE_NOCACHE;
	init.parent_names = parent_names;
	init.num_parents = num_parents;
	init.ops = &mmp_clk_mix_ops;

	memcpy(&mix->reg_info, &config->reg_info, sizeof(config->reg_info));
	if (config->table) {
		table_bytes = sizeof(*config->table) * config->table_size;
		mix->table = kmemdup(config->table, table_bytes, GFP_KERNEL);
		if (!mix->table) {
			pr_err("%s:%s: could not allocate mmp mix table\n",
				__func__, name);
			kfree(mix);
			return ERR_PTR(-ENOMEM);
		}
		mix->table_size = config->table_size;
	}

	if (config->mux_table) {
		table_bytes = sizeof(u32) * num_parents;
		mix->mux_table = kmemdup(config->mux_table, table_bytes,
					 GFP_KERNEL);
		if (!mix->mux_table) {
			pr_err("%s:%s: could not allocate mmp mix mux-table\n",
				__func__, name);
			kfree(mix->table);
			kfree(mix);
			return ERR_PTR(-ENOMEM);
		}
	}

	mix->div_flags = config->div_flags;
	mix->mux_flags = config->mux_flags;
	mix->lock = lock;
	mix->hw.init = &init;

	if (config->reg_info.bit_fc >= 32)
		mix->type = MMP_CLK_MIX_TYPE_V1;
	else if (config->reg_info.reg_clk_sel)
		mix->type = MMP_CLK_MIX_TYPE_V3;
	else
		mix->type = MMP_CLK_MIX_TYPE_V2;
	clk = clk_register(dev, &mix->hw);

	if (IS_ERR(clk)) {
		kfree(mix->mux_table);
		kfree(mix->table);
		kfree(mix);
	}

	return clk;
}
Commit	Line	Data
ee81f4ee CX	1	/*
	2	* mmp mix(div and mux) clock operation source file
	3	*
	4	* Copyright (C) 2014 Marvell
	5	* Chao Xie <chao.xie@marvell.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	/*
	20	* The mix clock is a clock combined mux and div type clock.
	21	* Because the div field and mux field need to be set at same
	22	* time, we can not divide it into 2 types of clock
	23	*/
	24
	25	#define to_clk_mix(hw) container_of(hw, struct mmp_clk_mix, hw)
	26
	27	static unsigned int _get_maxdiv(struct mmp_clk_mix *mix)
	28	{
	29	unsigned int div_mask = (1 << mix->reg_info.width_div) - 1;
	30	unsigned int maxdiv = 0;
	31	struct clk_div_table *clkt;
	32
	33	if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
	34	return div_mask;
	35	if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
	36	return 1 << div_mask;
	37	if (mix->div_table) {
	38	for (clkt = mix->div_table; clkt->div; clkt++)
	39	if (clkt->div > maxdiv)
	40	maxdiv = clkt->div;
	41	return maxdiv;
	42	}
	43	return div_mask + 1;
	44	}
	45
	46	static unsigned int _get_div(struct mmp_clk_mix *mix, unsigned int val)
	47	{
	48	struct clk_div_table *clkt;
	49
	50	if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
	51	return val;
	52	if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
	53	return 1 << val;
	54	if (mix->div_table) {
	55	for (clkt = mix->div_table; clkt->div; clkt++)
	56	if (clkt->val == val)
	57	return clkt->div;
	58	if (clkt->div == 0)
	59	return 0;
	60	}
	61	return val + 1;
	62	}
	63
	64	static unsigned int _get_mux(struct mmp_clk_mix *mix, unsigned int val)
65	{
497295af	66	int num_parents = clk_hw_get_num_parents(&mix->hw);
ee81f4ee CX	67	int i;
	68
	69	if (mix->mux_flags & CLK_MUX_INDEX_BIT)
	70	return ffs(val) - 1;
	71	if (mix->mux_flags & CLK_MUX_INDEX_ONE)
	72	return val - 1;
	73	if (mix->mux_table) {
	74	for (i = 0; i < num_parents; i++)
	75	if (mix->mux_table[i] == val)
	76	return i;
	77	if (i == num_parents)
	78	return 0;
	79	}
	80
	81	return val;
	82	}
	83	static unsigned int _get_div_val(struct mmp_clk_mix *mix, unsigned int div)
	84	{
	85	struct clk_div_table *clkt;
	86
	87	if (mix->div_flags & CLK_DIVIDER_ONE_BASED)
	88	return div;
	89	if (mix->div_flags & CLK_DIVIDER_POWER_OF_TWO)
	90	return __ffs(div);
	91	if (mix->div_table) {
	92	for (clkt = mix->div_table; clkt->div; clkt++)
	93	if (clkt->div == div)
	94	return clkt->val;
	95	if (clkt->div == 0)
	96	return 0;
	97	}
	98
	99	return div - 1;
	100	}
	101
	102	static unsigned int _get_mux_val(struct mmp_clk_mix *mix, unsigned int mux)
	103	{
	104	if (mix->mux_table)
	105	return mix->mux_table[mux];
	106
	107	return mux;
	108	}
	109
	110	static void _filter_clk_table(struct mmp_clk_mix *mix,
	111	struct mmp_clk_mix_clk_table *table,
	112	unsigned int table_size)
	113	{
	114	int i;
	115	struct mmp_clk_mix_clk_table *item;
aef28cb6	116	struct clk_hw parent, hw;
ee81f4ee CX	117	unsigned long parent_rate;
ee81f4ee CX	118
aef28cb6	119	hw = &mix->hw;
ee81f4ee CX	120
	121	for (i = 0; i < table_size; i++) {
	122	item = &table[i];
aef28cb6 SB	123	parent = clk_hw_get_parent_by_index(hw, item->parent_index);
aef28cb6 SB	124	parent_rate = clk_hw_get_rate(parent);
ee81f4ee CX	125	if (parent_rate % item->rate) {
	126	item->valid = 0;
	127	} else {
	128	item->divisor = parent_rate / item->rate;
	129	item->valid = 1;
	130	}
	131	}
	132	}
	133
	134	static int _set_rate(struct mmp_clk_mix *mix, u32 mux_val, u32 div_val,
	135	unsigned int change_mux, unsigned int change_div)
	136	{
	137	struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
	138	u8 width, shift;
	139	u32 mux_div, fc_req;
	140	int ret, timeout = 50;
	141	unsigned long flags = 0;
	142
	143	if (!change_mux && !change_div)
	144	return -EINVAL;
	145
	146	if (mix->lock)
	147	spin_lock_irqsave(mix->lock, flags);
	148
	149	if (mix->type == MMP_CLK_MIX_TYPE_V1
	150	\|\| mix->type == MMP_CLK_MIX_TYPE_V2)
	151	mux_div = readl(ri->reg_clk_ctrl);
	152	else
	153	mux_div = readl(ri->reg_clk_sel);
	154
	155	if (change_div) {
	156	width = ri->width_div;
	157	shift = ri->shift_div;
	158	mux_div &= ~MMP_CLK_BITS_MASK(width, shift);
	159	mux_div \|= MMP_CLK_BITS_SET_VAL(div_val, width, shift);
	160	}
	161
	162	if (change_mux) {
	163	width = ri->width_mux;
	164	shift = ri->shift_mux;
	165	mux_div &= ~MMP_CLK_BITS_MASK(width, shift);
	166	mux_div \|= MMP_CLK_BITS_SET_VAL(mux_val, width, shift);
	167	}
	168
	169	if (mix->type == MMP_CLK_MIX_TYPE_V1) {
	170	writel(mux_div, ri->reg_clk_ctrl);
	171	} else if (mix->type == MMP_CLK_MIX_TYPE_V2) {
	172	mux_div \|= (1 << ri->bit_fc);
	173	writel(mux_div, ri->reg_clk_ctrl);
	174
	175	do {
	176	fc_req = readl(ri->reg_clk_ctrl);
	177	timeout--;
	178	if (!(fc_req & (1 << ri->bit_fc)))
	179	break;
	180	} while (timeout);
	181
	182	if (timeout == 0) {
	183	pr_err("%s:%s cannot do frequency change\n",
aef28cb6	184	__func__, clk_hw_get_name(&mix->hw));
ee81f4ee CX	185	ret = -EBUSY;
	186	goto error;
	187	}
	188	} else {
	189	fc_req = readl(ri->reg_clk_ctrl);
	190	fc_req \|= 1 << ri->bit_fc;
	191	writel(fc_req, ri->reg_clk_ctrl);
	192	writel(mux_div, ri->reg_clk_sel);
	193	fc_req &= ~(1 << ri->bit_fc);
	194	}
	195
	196	ret = 0;
	197	error:
	198	if (mix->lock)
	199	spin_unlock_irqrestore(mix->lock, flags);
	200
	201	return ret;
	202	}
	203
0817b62c BB	204	static int mmp_clk_mix_determine_rate(struct clk_hw *hw,
0817b62c BB	205	struct clk_rate_request *req)
ee81f4ee CX	206	{
	207	struct mmp_clk_mix *mix = to_clk_mix(hw);
	208	struct mmp_clk_mix_clk_table *item;
aef28cb6	209	struct clk_hw parent, parent_best;
ee81f4ee CX	210	unsigned long parent_rate, mix_rate, mix_rate_best, parent_rate_best;
	211	unsigned long gap, gap_best;
	212	u32 div_val_max;
	213	unsigned int div;
	214	int i, j;
	215
ee81f4ee	216
ee81f4ee CX	217	mix_rate_best = 0;
ee81f4ee CX	218	parent_rate_best = 0;
57d866e6	219	gap_best = ULONG_MAX;
ee81f4ee CX	220	parent_best = NULL;
	221
	222	if (mix->table) {
	223	for (i = 0; i < mix->table_size; i++) {
	224	item = &mix->table[i];
	225	if (item->valid == 0)
	226	continue;
aef28cb6	227	parent = clk_hw_get_parent_by_index(hw,
ee81f4ee	228	item->parent_index);
aef28cb6	229	parent_rate = clk_hw_get_rate(parent);
ee81f4ee	230	mix_rate = parent_rate / item->divisor;
0817b62c	231	gap = abs(mix_rate - req->rate);
ee81f4ee CX	232	if (parent_best == NULL \|\| gap < gap_best) {
	233	parent_best = parent;
	234	parent_rate_best = parent_rate;
	235	mix_rate_best = mix_rate;
	236	gap_best = gap;
	237	if (gap_best == 0)
	238	goto found;
	239	}
	240	}
	241	} else {
497295af	242	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
aef28cb6 SB	243	parent = clk_hw_get_parent_by_index(hw, i);
aef28cb6 SB	244	parent_rate = clk_hw_get_rate(parent);
ee81f4ee CX	245	div_val_max = _get_maxdiv(mix);
	246	for (j = 0; j < div_val_max; j++) {
	247	div = _get_div(mix, j);
	248	mix_rate = parent_rate / div;
0817b62c	249	gap = abs(mix_rate - req->rate);
ee81f4ee CX	250	if (parent_best == NULL \|\| gap < gap_best) {
	251	parent_best = parent;
	252	parent_rate_best = parent_rate;
	253	mix_rate_best = mix_rate;
	254	gap_best = gap;
	255	if (gap_best == 0)
	256	goto found;
	257	}
	258	}
	259	}
	260	}
	261
	262	found:
57d866e6 BB	263	if (!parent_best)
	264	return -EINVAL;
	265
0817b62c	266	req->best_parent_rate = parent_rate_best;
aef28cb6	267	req->best_parent_hw = parent_best;
0817b62c	268	req->rate = mix_rate_best;
ee81f4ee	269
0817b62c	270	return 0;
ee81f4ee CX	271	}
	272
	273	static int mmp_clk_mix_set_rate_and_parent(struct clk_hw *hw,
	274	unsigned long rate,
	275	unsigned long parent_rate,
	276	u8 index)
	277	{
	278	struct mmp_clk_mix *mix = to_clk_mix(hw);
	279	unsigned int div;
	280	u32 div_val, mux_val;
	281
	282	div = parent_rate / rate;
	283	div_val = _get_div_val(mix, div);
	284	mux_val = _get_mux_val(mix, index);
	285
	286	return _set_rate(mix, mux_val, div_val, 1, 1);
	287	}
	288
	289	static u8 mmp_clk_mix_get_parent(struct clk_hw *hw)
	290	{
	291	struct mmp_clk_mix *mix = to_clk_mix(hw);
	292	struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
	293	unsigned long flags = 0;
	294	u32 mux_div = 0;
	295	u8 width, shift;
	296	u32 mux_val;
	297
	298	if (mix->lock)
	299	spin_lock_irqsave(mix->lock, flags);
	300
	301	if (mix->type == MMP_CLK_MIX_TYPE_V1
	302	\|\| mix->type == MMP_CLK_MIX_TYPE_V2)
	303	mux_div = readl(ri->reg_clk_ctrl);
	304	else
	305	mux_div = readl(ri->reg_clk_sel);
	306
	307	if (mix->lock)
	308	spin_unlock_irqrestore(mix->lock, flags);
	309
	310	width = mix->reg_info.width_mux;
	311	shift = mix->reg_info.shift_mux;
	312
	313	mux_val = MMP_CLK_BITS_GET_VAL(mux_div, width, shift);
	314
	315	return _get_mux(mix, mux_val);
	316	}
	317
	318	static unsigned long mmp_clk_mix_recalc_rate(struct clk_hw *hw,
	319	unsigned long parent_rate)
	320	{
	321	struct mmp_clk_mix *mix = to_clk_mix(hw);
	322	struct mmp_clk_mix_reg_info *ri = &mix->reg_info;
	323	unsigned long flags = 0;
	324	u32 mux_div = 0;
	325	u8 width, shift;
	326	unsigned int div;
	327
	328	if (mix->lock)
	329	spin_lock_irqsave(mix->lock, flags);
	330
	331	if (mix->type == MMP_CLK_MIX_TYPE_V1
	332	\|\| mix->type == MMP_CLK_MIX_TYPE_V2)
	333	mux_div = readl(ri->reg_clk_ctrl);
	334	else
335	mux_div = readl(ri->reg_clk_sel);
336
337	if (mix->lock)
338	spin_unlock_irqrestore(mix->lock, flags);
339
340	width = mix->reg_info.width_div;
341	shift = mix->reg_info.shift_div;
342
343	div = _get_div(mix, MMP_CLK_BITS_GET_VAL(mux_div, width, shift));
344
345	return parent_rate / div;
346	}
347
348	static int mmp_clk_set_parent(struct clk_hw *hw, u8 index)
349	{
350	struct mmp_clk_mix *mix = to_clk_mix(hw);
351	struct mmp_clk_mix_clk_table *item;
352	int i;
353	u32 div_val, mux_val;
354
355	if (mix->table) {
356	for (i = 0; i < mix->table_size; i++) {
357	item = &mix->table[i];
358	if (item->valid == 0)
359	continue;
360	if (item->parent_index == index)
361	break;
362	}
363	if (i < mix->table_size) {
364	div_val = _get_div_val(mix, item->divisor);
365	mux_val = _get_mux_val(mix, item->parent_index);
366	} else
367	return -EINVAL;
368	} else {
369	mux_val = _get_mux_val(mix, index);
370	div_val = 0;
371	}
372
373	return _set_rate(mix, mux_val, div_val, 1, div_val ? 1 : 0);
374	}
375
376	static int mmp_clk_set_rate(struct clk_hw *hw, unsigned long rate,
377	unsigned long best_parent_rate)
378	{
379	struct mmp_clk_mix *mix = to_clk_mix(hw);
380	struct mmp_clk_mix_clk_table *item;
381	unsigned long parent_rate;
382	unsigned int best_divisor;
aef28cb6	383	struct clk_hw *parent;
ee81f4ee CX	384	int i;
	385
	386	best_divisor = best_parent_rate / rate;
	387
ee81f4ee CX	388	if (mix->table) {
	389	for (i = 0; i < mix->table_size; i++) {
	390	item = &mix->table[i];
	391	if (item->valid == 0)
	392	continue;
aef28cb6	393	parent = clk_hw_get_parent_by_index(hw,
ee81f4ee	394	item->parent_index);
aef28cb6	395	parent_rate = clk_hw_get_rate(parent);
ee81f4ee CX	396	if (parent_rate == best_parent_rate
	397	&& item->divisor == best_divisor)
	398	break;
	399	}
	400	if (i < mix->table_size)
	401	return _set_rate(mix,
	402	_get_mux_val(mix, item->parent_index),
	403	_get_div_val(mix, item->divisor),
	404	1, 1);
	405	else
	406	return -EINVAL;
	407	} else {
497295af	408	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
aef28cb6 SB	409	parent = clk_hw_get_parent_by_index(hw, i);
aef28cb6 SB	410	parent_rate = clk_hw_get_rate(parent);
ee81f4ee CX	411	if (parent_rate == best_parent_rate)
	412	break;
	413	}
497295af	414	if (i < clk_hw_get_num_parents(hw))
ee81f4ee CX	415	return _set_rate(mix, _get_mux_val(mix, i),
	416	_get_div_val(mix, best_divisor), 1, 1);
	417	else
	418	return -EINVAL;
	419	}
	420	}
	421
	422	static void mmp_clk_mix_init(struct clk_hw *hw)
	423	{
	424	struct mmp_clk_mix *mix = to_clk_mix(hw);
	425
	426	if (mix->table)
	427	_filter_clk_table(mix, mix->table, mix->table_size);
	428	}
	429
	430	const struct clk_ops mmp_clk_mix_ops = {
	431	.determine_rate = mmp_clk_mix_determine_rate,
	432	.set_rate_and_parent = mmp_clk_mix_set_rate_and_parent,
	433	.set_rate = mmp_clk_set_rate,
	434	.set_parent = mmp_clk_set_parent,
	435	.get_parent = mmp_clk_mix_get_parent,
	436	.recalc_rate = mmp_clk_mix_recalc_rate,
	437	.init = mmp_clk_mix_init,
	438	};
	439
	440	struct clk mmp_clk_register_mix(struct device dev,
	441	const char *name,
	442	const char **parent_names,
	443	u8 num_parents,
	444	unsigned long flags,
	445	struct mmp_clk_mix_config *config,
	446	spinlock_t *lock)
	447	{
	448	struct mmp_clk_mix *mix;
	449	struct clk *clk;
	450	struct clk_init_data init;
	451	size_t table_bytes;
	452
	453	mix = kzalloc(sizeof(*mix), GFP_KERNEL);
	454	if (!mix) {
	455	pr_err("%s:%s: could not allocate mmp mix clk\n",
	456	__func__, name);
	457	return ERR_PTR(-ENOMEM);
	458	}
	459
	460	init.name = name;
	461	init.flags = flags \| CLK_GET_RATE_NOCACHE;
	462	init.parent_names = parent_names;
	463	init.num_parents = num_parents;
	464	init.ops = &mmp_clk_mix_ops;
	465
	466	memcpy(&mix->reg_info, &config->reg_info, sizeof(config->reg_info));
	467	if (config->table) {
	468	table_bytes = sizeof(config->table) config->table_size;
e8f35aab	469	mix->table = kmemdup(config->table, table_bytes, GFP_KERNEL);
ee81f4ee CX	470	if (!mix->table) {
	471	pr_err("%s:%s: could not allocate mmp mix table\n",
	472	__func__, name);
	473	kfree(mix);
	474	return ERR_PTR(-ENOMEM);
	475	}
ee81f4ee CX	476	mix->table_size = config->table_size;
	477	}
	478
	479	if (config->mux_table) {
	480	table_bytes = sizeof(u32) * num_parents;
e8f35aab AH	481	mix->mux_table = kmemdup(config->mux_table, table_bytes,
e8f35aab AH	482	GFP_KERNEL);
ee81f4ee CX	483	if (!mix->mux_table) {
	484	pr_err("%s:%s: could not allocate mmp mix mux-table\n",
	485	__func__, name);
	486	kfree(mix->table);
	487	kfree(mix);
	488	return ERR_PTR(-ENOMEM);
	489	}
ee81f4ee CX	490	}
	491
	492	mix->div_flags = config->div_flags;
	493	mix->mux_flags = config->mux_flags;
	494	mix->lock = lock;
	495	mix->hw.init = &init;
	496
	497	if (config->reg_info.bit_fc >= 32)
	498	mix->type = MMP_CLK_MIX_TYPE_V1;
	499	else if (config->reg_info.reg_clk_sel)
	500	mix->type = MMP_CLK_MIX_TYPE_V3;
	501	else
	502	mix->type = MMP_CLK_MIX_TYPE_V2;
	503	clk = clk_register(dev, &mix->hw);
	504
	505	if (IS_ERR(clk)) {
	506	kfree(mix->mux_table);
	507	kfree(mix->table);
	508	kfree(mix);
	509	}
	510
	511	return clk;
	512	}