[linux-block.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;
	struct at91_clk_pms pms;
	u32 usbs_mask;
	u8 num_parents;
};

#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);
			if (!tmp_parent_rate)
				continue;

			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 >= usb->num_parents)
		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 int at91sam9x5_usb_save_context(struct clk_hw *hw)
{
	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
	struct clk_hw *parent_hw = clk_hw_get_parent(hw);

	usb->pms.parent = at91sam9x5_clk_usb_get_parent(hw);
	usb->pms.parent_rate = clk_hw_get_rate(parent_hw);
	usb->pms.rate = at91sam9x5_clk_usb_recalc_rate(hw, usb->pms.parent_rate);

	return 0;
}

static void at91sam9x5_usb_restore_context(struct clk_hw *hw)
{
	struct at91sam9x5_clk_usb *usb = to_at91sam9x5_clk_usb(hw);
	int ret;

	ret = at91sam9x5_clk_usb_set_parent(hw, usb->pms.parent);
	if (ret)
		return;

	at91sam9x5_clk_usb_set_rate(hw, usb->pms.rate, usb->pms.parent_rate);
}

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,
	.save_context = at91sam9x5_usb_save_context,
	.restore_context = at91sam9x5_usb_restore_context,
};

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 = usbs_mask;
	usb->num_parents = num_parents;

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