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

/*
 *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.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/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

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

#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, AT91_PMC_USBS,
			   index ? AT91_PMC_USBS : 0);

	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 & AT91_PMC_USBS;
}

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,
};

struct clk_hw * __init
at91sam9x5_clk_register_usb(struct regmap *regmap, const char *name,
			    const char **parent_names, u8 num_parents)
{
	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;

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

	return hw;
}

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