[mirror_ubuntu-kernels.git] / drivers / clk / at91 / clk-usb.c

// SPDX-License-Identifier: GPL-2.0-or-later
/*
 *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
 */

#include <linux/clk-provider.h>
#include <linux/clkdev.h>
#include <linux/clk/at91_pmc.h>
#include <linux/of.h>
#include <linux/mfd/syscon.h>
#include <linux/regmap.h>

#include "pmc.h"

#define SAM9X5_USB_DIV_SHIFT	8
#define SAM9X5_USB_MAX_DIV	0xf

#define RM9200_USB_DIV_SHIFT	28
#define RM9200_USB_DIV_TAB_SIZE	4

#define SAM9X5_USBS_MASK	GENMASK(0, 0)
#define SAM9X60_USBS_MASK	GENMASK(1, 0)

struct at91sam9x5_clk_usb {
	struct clk_hw hw;
	struct regmap *regmap;
	u32 usbs_mask;
};

#define to_at91sam9x5_clk_usb(hw) \
	container_of(hw, struct at91sam9x5_clk_usb, hw)

struct at91rm9200_clk_usb {
	struct clk_hw hw;
	struct regmap *regmap;
	u32 divisors[4];
};

#define to_at91rm9200_clk_usb(hw) \
	container_of(hw, struct at91rm9200_clk_usb, hw)

static unsigned long at91sam9x5_clk_usb_recalc_rate(struct clk_hw *hw,
						    unsigned long parent_rate)
{
	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
	unsigned int usbr;
	u8 usbdiv;

	regmap_read(usb->regmap, AT91_PMC_USB, &usbr);
	usbdiv = (usbr & AT91_PMC_OHCIUSBDIV) >> SAM9X5_USB_DIV_SHIFT;

	return DIV_ROUND_CLOSEST(parent_rate, (usbdiv + 1));
}

static int at91sam9x5_clk_usb_determine_rate(struct clk_hw *hw,
					     struct clk_rate_request *req)
{
	struct clk_hw *parent;
	long best_rate = -EINVAL;
	unsigned long tmp_rate;
	int best_diff = -1;
	int tmp_diff;
	int i;

	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
		int div;

		parent = clk_hw_get_parent_by_index(hw, i);
		if (!parent)
			continue;

		for (div = 1; div < SAM9X5_USB_MAX_DIV + 2; div++) {
			unsigned long tmp_parent_rate;

			tmp_parent_rate = req->rate * div;
			tmp_parent_rate = clk_hw_round_rate(parent,
							   tmp_parent_rate);
			tmp_rate = DIV_ROUND_CLOSEST(tmp_parent_rate, div);
			if (tmp_rate < req->rate)
				tmp_diff = req->rate - tmp_rate;
			else
				tmp_diff = tmp_rate - req->rate;

			if (best_diff < 0 || best_diff > tmp_diff) {
				best_rate = tmp_rate;
				best_diff = tmp_diff;
				req->best_parent_rate = tmp_parent_rate;
				req->best_parent_hw = parent;
			}

			if (!best_diff || tmp_rate < req->rate)
				break;
		}

		if (!best_diff)
			break;
	}

	if (best_rate < 0)
		return best_rate;

	req->rate = best_rate;
	return 0;
}

static int at91sam9x5_clk_usb_set_parent(struct clk_hw *hw, u8 index)
{
	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);

	if (index > 1)
		return -EINVAL;

	regmap_update_bits(usb->regmap, AT91_PMC_USB, usb->usbs_mask, index);

	return 0;
}

static u8 at91sam9x5_clk_usb_get_parent(struct clk_hw *hw)
{
	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
	unsigned int usbr;

	regmap_read(usb->regmap, AT91_PMC_USB, &usbr);

	return usbr & usb->usbs_mask;
}

static int at91sam9x5_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate,
				       unsigned long parent_rate)
{
	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
	unsigned long div;

	if (!rate)
		return -EINVAL;

	div = DIV_ROUND_CLOSEST(parent_rate, rate);
	if (div > SAM9X5_USB_MAX_DIV + 1 || !div)
		return -EINVAL;

	regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_OHCIUSBDIV,
			   (div - 1) << SAM9X5_USB_DIV_SHIFT);

	return 0;
}

static const struct clk_ops at91sam9x5_usb_ops = {
	.recalc_rate = at91sam9x5_clk_usb_recalc_rate,
	.determine_rate = at91sam9x5_clk_usb_determine_rate,
	.get_parent = at91sam9x5_clk_usb_get_parent,
	.set_parent = at91sam9x5_clk_usb_set_parent,
	.set_rate = at91sam9x5_clk_usb_set_rate,
};

static int at91sam9n12_clk_usb_enable(struct clk_hw *hw)
{
	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);

	regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_USBS,
			   AT91_PMC_USBS);

	return 0;
}

static void at91sam9n12_clk_usb_disable(struct clk_hw *hw)
{
	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);

	regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_USBS, 0);
}

static int at91sam9n12_clk_usb_is_enabled(struct clk_hw *hw)
{
	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
	unsigned int usbr;

	regmap_read(usb->regmap, AT91_PMC_USB, &usbr);

	return usbr & AT91_PMC_USBS;
}

static const struct clk_ops at91sam9n12_usb_ops = {
	.enable = at91sam9n12_clk_usb_enable,
	.disable = at91sam9n12_clk_usb_disable,
	.is_enabled = at91sam9n12_clk_usb_is_enabled,
	.recalc_rate = at91sam9x5_clk_usb_recalc_rate,
	.determine_rate = at91sam9x5_clk_usb_determine_rate,
	.set_rate = at91sam9x5_clk_usb_set_rate,
};

static struct clk_hw * __init
_at91sam9x5_clk_register_usb(struct regmap *regmap, const char *name,
			     const char **parent_names, u8 num_parents,
			     u32 usbs_mask)
{
	struct at91sam9x5_clk_usb *usb;
	struct clk_hw *hw;
	struct clk_init_data init;
	int ret;

	usb = kzalloc(sizeof(*usb), GFP_KERNEL);
	if (!usb)
		return ERR_PTR(-ENOMEM);

	init.name = name;
	init.ops = &at91sam9x5_usb_ops;
	init.parent_names = parent_names;
	init.num_parents = num_parents;
	init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE |
		     CLK_SET_RATE_PARENT;

	usb->hw.init = &init;
	usb->regmap = regmap;
	usb->usbs_mask = SAM9X5_USBS_MASK;

	hw = &usb->hw;
	ret = clk_hw_register(NULL, &usb->hw);
	if (ret) {
		kfree(usb);
		hw = ERR_PTR(ret);
	}

	return hw;
}

struct clk_hw * __init
at91sam9x5_clk_register_usb(struct regmap *regmap, const char *name,
			    const char **parent_names, u8 num_parents)
{
	return _at91sam9x5_clk_register_usb(regmap, name, parent_names,
					    num_parents, SAM9X5_USBS_MASK);
}

struct clk_hw * __init
sam9x60_clk_register_usb(struct regmap *regmap, const char *name,
			 const char **parent_names, u8 num_parents)
{
	return _at91sam9x5_clk_register_usb(regmap, name, parent_names,
					    num_parents, SAM9X60_USBS_MASK);
}

struct clk_hw * __init
at91sam9n12_clk_register_usb(struct regmap *regmap, const char *name,
			     const char *parent_name)
{
	struct at91sam9x5_clk_usb *usb;
	struct clk_hw *hw;
	struct clk_init_data init;
	int ret;

	usb = kzalloc(sizeof(*usb), GFP_KERNEL);
	if (!usb)
		return ERR_PTR(-ENOMEM);

	init.name = name;
	init.ops = &at91sam9n12_usb_ops;
	init.parent_names = &parent_name;
	init.num_parents = 1;
	init.flags = CLK_SET_RATE_GATE | CLK_SET_RATE_PARENT;

	usb->hw.init = &init;
	usb->regmap = regmap;

	hw = &usb->hw;
	ret = clk_hw_register(NULL, &usb->hw);
	if (ret) {
		kfree(usb);
		hw = ERR_PTR(ret);
	}

	return hw;
}

static unsigned long at91rm9200_clk_usb_recalc_rate(struct clk_hw *hw,
						    unsigned long parent_rate)
{
	struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);
	unsigned int pllbr;
	u8 usbdiv;

	regmap_read(usb->regmap, AT91_CKGR_PLLBR, &pllbr);

	usbdiv = (pllbr & AT91_PMC_USBDIV) >> RM9200_USB_DIV_SHIFT;
	if (usb->divisors[usbdiv])
		return parent_rate / usb->divisors[usbdiv];

	return 0;
}

static long at91rm9200_clk_usb_round_rate(struct clk_hw *hw, unsigned long rate,
					  unsigned long *parent_rate)
{
	struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);
	struct clk_hw *parent = clk_hw_get_parent(hw);
	unsigned long bestrate = 0;
	int bestdiff = -1;
	unsigned long tmprate;
	int tmpdiff;
	int i = 0;

	for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) {
		unsigned long tmp_parent_rate;

		if (!usb->divisors[i])
			continue;

		tmp_parent_rate = rate * usb->divisors[i];
		tmp_parent_rate = clk_hw_round_rate(parent, tmp_parent_rate);
		tmprate = DIV_ROUND_CLOSEST(tmp_parent_rate, usb->divisors[i]);
		if (tmprate < rate)
			tmpdiff = rate - tmprate;
		else
			tmpdiff = tmprate - rate;

		if (bestdiff < 0 || bestdiff > tmpdiff) {
			bestrate = tmprate;
			bestdiff = tmpdiff;
			*parent_rate = tmp_parent_rate;
		}

		if (!bestdiff)
			break;
	}

	return bestrate;
}

static int at91rm9200_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate,
				       unsigned long parent_rate)
{
	int i;
	struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);
	unsigned long div;

	if (!rate)
		return -EINVAL;

	div = DIV_ROUND_CLOSEST(parent_rate, rate);

	for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) {
		if (usb->divisors[i] == div) {
			regmap_update_bits(usb->regmap, AT91_CKGR_PLLBR,
					   AT91_PMC_USBDIV,
					   i << RM9200_USB_DIV_SHIFT);

			return 0;
		}
	}

	return -EINVAL;
}

static const struct clk_ops at91rm9200_usb_ops = {
	.recalc_rate = at91rm9200_clk_usb_recalc_rate,
	.round_rate = at91rm9200_clk_usb_round_rate,
	.set_rate = at91rm9200_clk_usb_set_rate,
};

struct clk_hw * __init
at91rm9200_clk_register_usb(struct regmap *regmap, const char *name,
			    const char *parent_name, const u32 *divisors)
{
	struct at91rm9200_clk_usb *usb;
	struct clk_hw *hw;
	struct clk_init_data init;
	int ret;

	usb = kzalloc(sizeof(*usb), GFP_KERNEL);
	if (!usb)
		return ERR_PTR(-ENOMEM);

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

	usb->hw.init = &init;
	usb->regmap = regmap;
	memcpy(usb->divisors, divisors, sizeof(usb->divisors));

	hw = &usb->hw;
	ret = clk_hw_register(NULL, &usb->hw);
	if (ret) {
		kfree(usb);
		hw = ERR_PTR(ret);
	}

	return hw;
}
Commit	Line	Data
2874c5fd	1	// SPDX-License-Identifier: GPL-2.0-or-later
c84a61d8 BB	2	/*
c84a61d8 BB	3	* Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
c84a61d8 BB	4	*/
	5
	6	#include <linux/clk-provider.h>
	7	#include <linux/clkdev.h>
	8	#include <linux/clk/at91_pmc.h>
	9	#include <linux/of.h>
1bdf0232 BB	10	#include <linux/mfd/syscon.h>
1bdf0232 BB	11	#include <linux/regmap.h>
c84a61d8 BB	12
	13	#include "pmc.h"
	14
c84a61d8 BB	15	#define SAM9X5_USB_DIV_SHIFT 8
	16	#define SAM9X5_USB_MAX_DIV 0xf
	17
	18	#define RM9200_USB_DIV_SHIFT 28
	19	#define RM9200_USB_DIV_TAB_SIZE 4
	20
2423eeae AB	21	#define SAM9X5_USBS_MASK GENMASK(0, 0)
	22	#define SAM9X60_USBS_MASK GENMASK(1, 0)
	23
c84a61d8 BB	24	struct at91sam9x5_clk_usb {
c84a61d8 BB	25	struct clk_hw hw;
1bdf0232	26	struct regmap *regmap;
2423eeae	27	u32 usbs_mask;
c84a61d8 BB	28	};
	29
	30	#define to_at91sam9x5_clk_usb(hw) \
	31	container_of(hw, struct at91sam9x5_clk_usb, hw)
	32
	33	struct at91rm9200_clk_usb {
	34	struct clk_hw hw;
1bdf0232	35	struct regmap *regmap;
c84a61d8 BB	36	u32 divisors[4];
	37	};
	38
	39	#define to_at91rm9200_clk_usb(hw) \
	40	container_of(hw, struct at91rm9200_clk_usb, hw)
	41
	42	static unsigned long at91sam9x5_clk_usb_recalc_rate(struct clk_hw *hw,
	43	unsigned long parent_rate)
	44	{
c84a61d8	45	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
1bdf0232 BB	46	unsigned int usbr;
1bdf0232 BB	47	u8 usbdiv;
c84a61d8	48
1bdf0232 BB	49	regmap_read(usb->regmap, AT91_PMC_USB, &usbr);
1bdf0232 BB	50	usbdiv = (usbr & AT91_PMC_OHCIUSBDIV) >> SAM9X5_USB_DIV_SHIFT;
69daf75a BB	51
69daf75a BB	52	return DIV_ROUND_CLOSEST(parent_rate, (usbdiv + 1));
c84a61d8 BB	53	}
c84a61d8 BB	54
0817b62c BB	55	static int at91sam9x5_clk_usb_determine_rate(struct clk_hw *hw,
0817b62c BB	56	struct clk_rate_request *req)
c84a61d8	57	{
d0979335	58	struct clk_hw *parent;
45912431 BB	59	long best_rate = -EINVAL;
	60	unsigned long tmp_rate;
	61	int best_diff = -1;
	62	int tmp_diff;
	63	int i;
	64
497295af	65	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
45912431 BB	66	int div;
45912431 BB	67
d0979335	68	parent = clk_hw_get_parent_by_index(hw, i);
45912431 BB	69	if (!parent)
	70	continue;
	71
	72	for (div = 1; div < SAM9X5_USB_MAX_DIV + 2; div++) {
	73	unsigned long tmp_parent_rate;
	74
0817b62c	75	tmp_parent_rate = req->rate * div;
d0979335	76	tmp_parent_rate = clk_hw_round_rate(parent,
45912431 BB	77	tmp_parent_rate);
45912431 BB	78	tmp_rate = DIV_ROUND_CLOSEST(tmp_parent_rate, div);
0817b62c BB	79	if (tmp_rate < req->rate)
0817b62c BB	80	tmp_diff = req->rate - tmp_rate;
45912431	81	else
0817b62c	82	tmp_diff = tmp_rate - req->rate;
45912431 BB	83
	84	if (best_diff < 0 \|\| best_diff > tmp_diff) {
	85	best_rate = tmp_rate;
	86	best_diff = tmp_diff;
0817b62c	87	req->best_parent_rate = tmp_parent_rate;
d0979335	88	req->best_parent_hw = parent;
45912431 BB	89	}
45912431 BB	90
0817b62c	91	if (!best_diff \|\| tmp_rate < req->rate)
45912431 BB	92	break;
	93	}
	94
	95	if (!best_diff)
	96	break;
	97	}
	98
0817b62c BB	99	if (best_rate < 0)
	100	return best_rate;
	101
	102	req->rate = best_rate;
	103	return 0;
c84a61d8 BB	104	}
	105
	106	static int at91sam9x5_clk_usb_set_parent(struct clk_hw *hw, u8 index)
	107	{
c84a61d8	108	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
c84a61d8 BB	109
	110	if (index > 1)
	111	return -EINVAL;
1bdf0232	112
2423eeae	113	regmap_update_bits(usb->regmap, AT91_PMC_USB, usb->usbs_mask, index);
1bdf0232	114
c84a61d8 BB	115	return 0;
	116	}
	117
	118	static u8 at91sam9x5_clk_usb_get_parent(struct clk_hw *hw)
	119	{
	120	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
1bdf0232	121	unsigned int usbr;
c84a61d8	122
1bdf0232 BB	123	regmap_read(usb->regmap, AT91_PMC_USB, &usbr);
1bdf0232 BB	124
2423eeae	125	return usbr & usb->usbs_mask;
c84a61d8 BB	126	}
	127
	128	static int at91sam9x5_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate,
	129	unsigned long parent_rate)
	130	{
c84a61d8	131	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
69daf75a BB	132	unsigned long div;
	133
	134	if (!rate)
	135	return -EINVAL;
c84a61d8	136
69daf75a BB	137	div = DIV_ROUND_CLOSEST(parent_rate, rate);
69daf75a BB	138	if (div > SAM9X5_USB_MAX_DIV + 1 \|\| !div)
c84a61d8 BB	139	return -EINVAL;
c84a61d8 BB	140
1bdf0232 BB	141	regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_OHCIUSBDIV,
1bdf0232 BB	142	(div - 1) << SAM9X5_USB_DIV_SHIFT);
c84a61d8 BB	143
	144	return 0;
	145	}
	146
	147	static const struct clk_ops at91sam9x5_usb_ops = {
	148	.recalc_rate = at91sam9x5_clk_usb_recalc_rate,
45912431	149	.determine_rate = at91sam9x5_clk_usb_determine_rate,
c84a61d8 BB	150	.get_parent = at91sam9x5_clk_usb_get_parent,
	151	.set_parent = at91sam9x5_clk_usb_set_parent,
	152	.set_rate = at91sam9x5_clk_usb_set_rate,
	153	};
	154
	155	static int at91sam9n12_clk_usb_enable(struct clk_hw *hw)
	156	{
	157	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
c84a61d8	158
1bdf0232 BB	159	regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_USBS,
	160	AT91_PMC_USBS);
	161
c84a61d8 BB	162	return 0;
	163	}
	164
	165	static void at91sam9n12_clk_usb_disable(struct clk_hw *hw)
	166	{
	167	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
c84a61d8	168
1bdf0232	169	regmap_update_bits(usb->regmap, AT91_PMC_USB, AT91_PMC_USBS, 0);
c84a61d8 BB	170	}
	171
	172	static int at91sam9n12_clk_usb_is_enabled(struct clk_hw *hw)
	173	{
	174	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
1bdf0232	175	unsigned int usbr;
c84a61d8	176
1bdf0232 BB	177	regmap_read(usb->regmap, AT91_PMC_USB, &usbr);
	178
	179	return usbr & AT91_PMC_USBS;
c84a61d8 BB	180	}
	181
	182	static const struct clk_ops at91sam9n12_usb_ops = {
	183	.enable = at91sam9n12_clk_usb_enable,
	184	.disable = at91sam9n12_clk_usb_disable,
	185	.is_enabled = at91sam9n12_clk_usb_is_enabled,
	186	.recalc_rate = at91sam9x5_clk_usb_recalc_rate,
45912431	187	.determine_rate = at91sam9x5_clk_usb_determine_rate,
c84a61d8 BB	188	.set_rate = at91sam9x5_clk_usb_set_rate,
	189	};
	190
2423eeae AB	191	static struct clk_hw * __init
	192	_at91sam9x5_clk_register_usb(struct regmap regmap, const char name,
	193	const char **parent_names, u8 num_parents,
	194	u32 usbs_mask)
c84a61d8 BB	195	{
c84a61d8 BB	196	struct at91sam9x5_clk_usb *usb;
f5644f10	197	struct clk_hw *hw;
c84a61d8	198	struct clk_init_data init;
f5644f10	199	int ret;
c84a61d8 BB	200
	201	usb = kzalloc(sizeof(*usb), GFP_KERNEL);
	202	if (!usb)
	203	return ERR_PTR(-ENOMEM);
	204
	205	init.name = name;
	206	init.ops = &at91sam9x5_usb_ops;
	207	init.parent_names = parent_names;
	208	init.num_parents = num_parents;
45912431 BB	209	init.flags = CLK_SET_RATE_GATE \| CLK_SET_PARENT_GATE \|
45912431 BB	210	CLK_SET_RATE_PARENT;
c84a61d8 BB	211
c84a61d8 BB	212	usb->hw.init = &init;
1bdf0232	213	usb->regmap = regmap;
2423eeae	214	usb->usbs_mask = SAM9X5_USBS_MASK;
c84a61d8	215
f5644f10 SB	216	hw = &usb->hw;
	217	ret = clk_hw_register(NULL, &usb->hw);
	218	if (ret) {
c84a61d8	219	kfree(usb);
f5644f10 SB	220	hw = ERR_PTR(ret);
f5644f10 SB	221	}
c84a61d8	222
f5644f10	223	return hw;
c84a61d8 BB	224	}
c84a61d8 BB	225
2423eeae AB	226	struct clk_hw * __init
	227	at91sam9x5_clk_register_usb(struct regmap regmap, const char name,
	228	const char **parent_names, u8 num_parents)
	229	{
	230	return _at91sam9x5_clk_register_usb(regmap, name, parent_names,
	231	num_parents, SAM9X5_USBS_MASK);
	232	}
	233
	234	struct clk_hw * __init
	235	sam9x60_clk_register_usb(struct regmap regmap, const char name,
	236	const char **parent_names, u8 num_parents)
	237	{
	238	return _at91sam9x5_clk_register_usb(regmap, name, parent_names,
	239	num_parents, SAM9X60_USBS_MASK);
	240	}
	241
b2e39dc0	242	struct clk_hw * __init
1bdf0232	243	at91sam9n12_clk_register_usb(struct regmap regmap, const char name,
c84a61d8 BB	244	const char *parent_name)
	245	{
	246	struct at91sam9x5_clk_usb *usb;
f5644f10	247	struct clk_hw *hw;
c84a61d8	248	struct clk_init_data init;
f5644f10	249	int ret;
c84a61d8 BB	250
	251	usb = kzalloc(sizeof(*usb), GFP_KERNEL);
	252	if (!usb)
	253	return ERR_PTR(-ENOMEM);
	254
	255	init.name = name;
	256	init.ops = &at91sam9n12_usb_ops;
	257	init.parent_names = &parent_name;
	258	init.num_parents = 1;
45912431	259	init.flags = CLK_SET_RATE_GATE \| CLK_SET_RATE_PARENT;
c84a61d8 BB	260
c84a61d8 BB	261	usb->hw.init = &init;
1bdf0232	262	usb->regmap = regmap;
c84a61d8	263
f5644f10 SB	264	hw = &usb->hw;
	265	ret = clk_hw_register(NULL, &usb->hw);
	266	if (ret) {
c84a61d8	267	kfree(usb);
f5644f10 SB	268	hw = ERR_PTR(ret);
f5644f10 SB	269	}
c84a61d8	270
f5644f10	271	return hw;
c84a61d8 BB	272	}
	273
	274	static unsigned long at91rm9200_clk_usb_recalc_rate(struct clk_hw *hw,
	275	unsigned long parent_rate)
	276	{
	277	struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);
1bdf0232	278	unsigned int pllbr;
c84a61d8 BB	279	u8 usbdiv;
c84a61d8 BB	280
1bdf0232 BB	281	regmap_read(usb->regmap, AT91_CKGR_PLLBR, &pllbr);
	282
	283	usbdiv = (pllbr & AT91_PMC_USBDIV) >> RM9200_USB_DIV_SHIFT;
c84a61d8 BB	284	if (usb->divisors[usbdiv])
	285	return parent_rate / usb->divisors[usbdiv];
	286
	287	return 0;
	288	}
	289
	290	static long at91rm9200_clk_usb_round_rate(struct clk_hw *hw, unsigned long rate,
	291	unsigned long *parent_rate)
	292	{
	293	struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);
d0979335	294	struct clk_hw *parent = clk_hw_get_parent(hw);
c84a61d8 BB	295	unsigned long bestrate = 0;
	296	int bestdiff = -1;
	297	unsigned long tmprate;
	298	int tmpdiff;
	299	int i = 0;
	300
13a6073d BB	301	for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) {
	302	unsigned long tmp_parent_rate;
	303
c84a61d8 BB	304	if (!usb->divisors[i])
c84a61d8 BB	305	continue;
13a6073d BB	306
13a6073d BB	307	tmp_parent_rate = rate * usb->divisors[i];
d0979335	308	tmp_parent_rate = clk_hw_round_rate(parent, tmp_parent_rate);
ff553ea1	309	tmprate = DIV_ROUND_CLOSEST(tmp_parent_rate, usb->divisors[i]);
c84a61d8 BB	310	if (tmprate < rate)
	311	tmpdiff = rate - tmprate;
	312	else
	313	tmpdiff = tmprate - rate;
	314
	315	if (bestdiff < 0 \|\| bestdiff > tmpdiff) {
	316	bestrate = tmprate;
	317	bestdiff = tmpdiff;
13a6073d	318	*parent_rate = tmp_parent_rate;
c84a61d8 BB	319	}
	320
	321	if (!bestdiff)
	322	break;
	323	}
	324
	325	return bestrate;
	326	}
	327
	328	static int at91rm9200_clk_usb_set_rate(struct clk_hw *hw, unsigned long rate,
	329	unsigned long parent_rate)
	330	{
c84a61d8 BB	331	int i;
c84a61d8 BB	332	struct at91rm9200_clk_usb *usb = to_at91rm9200_clk_usb(hw);
16eeaec7	333	unsigned long div;
c84a61d8	334
ff553ea1	335	if (!rate)
c84a61d8	336	return -EINVAL;
16eeaec7	337
ff553ea1	338	div = DIV_ROUND_CLOSEST(parent_rate, rate);
16eeaec7	339
c84a61d8 BB	340	for (i = 0; i < RM9200_USB_DIV_TAB_SIZE; i++) {
c84a61d8 BB	341	if (usb->divisors[i] == div) {
1bdf0232 BB	342	regmap_update_bits(usb->regmap, AT91_CKGR_PLLBR,
	343	AT91_PMC_USBDIV,
	344	i << RM9200_USB_DIV_SHIFT);
	345
c84a61d8 BB	346	return 0;
	347	}
	348	}
	349
	350	return -EINVAL;
	351	}
	352
	353	static const struct clk_ops at91rm9200_usb_ops = {
	354	.recalc_rate = at91rm9200_clk_usb_recalc_rate,
	355	.round_rate = at91rm9200_clk_usb_round_rate,
	356	.set_rate = at91rm9200_clk_usb_set_rate,
	357	};
	358
b2e39dc0	359	struct clk_hw * __init
1bdf0232	360	at91rm9200_clk_register_usb(struct regmap regmap, const char name,
c84a61d8 BB	361	const char parent_name, const u32 divisors)
	362	{
	363	struct at91rm9200_clk_usb *usb;
f5644f10	364	struct clk_hw *hw;
c84a61d8	365	struct clk_init_data init;
f5644f10	366	int ret;
c84a61d8 BB	367
	368	usb = kzalloc(sizeof(*usb), GFP_KERNEL);
	369	if (!usb)
	370	return ERR_PTR(-ENOMEM);
	371
	372	init.name = name;
	373	init.ops = &at91rm9200_usb_ops;
	374	init.parent_names = &parent_name;
	375	init.num_parents = 1;
13a6073d	376	init.flags = CLK_SET_RATE_PARENT;
c84a61d8 BB	377
c84a61d8 BB	378	usb->hw.init = &init;
1bdf0232	379	usb->regmap = regmap;
c84a61d8 BB	380	memcpy(usb->divisors, divisors, sizeof(usb->divisors));
c84a61d8 BB	381
f5644f10 SB	382	hw = &usb->hw;
	383	ret = clk_hw_register(NULL, &usb->hw);
	384	if (ret) {
c84a61d8	385	kfree(usb);
f5644f10 SB	386	hw = ERR_PTR(ret);
f5644f10 SB	387	}
c84a61d8	388
f5644f10	389	return hw;
c84a61d8	390	}