[linux-2.6-block.git] / drivers / clk / at91 / clk-smd.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 SMD_SOURCE_MAX		2

#define SMD_DIV_SHIFT		8
#define SMD_MAX_DIV		0xf

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

#define to_at91sam9x5_clk_smd(hw) \
	container_of(hw, struct at91sam9x5_clk_smd, hw)

static unsigned long at91sam9x5_clk_smd_recalc_rate(struct clk_hw *hw,
						    unsigned long parent_rate)
{
	struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
	unsigned int smdr;
	u8 smddiv;

	regmap_read(smd->regmap, AT91_PMC_SMD, &smdr);
	smddiv = (smdr & AT91_PMC_SMD_DIV) >> SMD_DIV_SHIFT;

	return parent_rate / (smddiv + 1);
}

static long at91sam9x5_clk_smd_round_rate(struct clk_hw *hw, unsigned long rate,
					  unsigned long *parent_rate)
{
	unsigned long div;
	unsigned long bestrate;
	unsigned long tmp;

	if (rate >= *parent_rate)
		return *parent_rate;

	div = *parent_rate / rate;
	if (div > SMD_MAX_DIV)
		return *parent_rate / (SMD_MAX_DIV + 1);

	bestrate = *parent_rate / div;
	tmp = *parent_rate / (div + 1);
	if (bestrate - rate > rate - tmp)
		bestrate = tmp;

	return bestrate;
}

static int at91sam9x5_clk_smd_set_parent(struct clk_hw *hw, u8 index)
{
	struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);

	if (index > 1)
		return -EINVAL;

	regmap_update_bits(smd->regmap, AT91_PMC_SMD, AT91_PMC_SMDS,
			   index ? AT91_PMC_SMDS : 0);

	return 0;
}

static u8 at91sam9x5_clk_smd_get_parent(struct clk_hw *hw)
{
	struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
	unsigned int smdr;

	regmap_read(smd->regmap, AT91_PMC_SMD, &smdr);

	return smdr & AT91_PMC_SMDS;
}

static int at91sam9x5_clk_smd_set_rate(struct clk_hw *hw, unsigned long rate,
				       unsigned long parent_rate)
{
	struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
	unsigned long div = parent_rate / rate;

	if (parent_rate % rate || div < 1 || div > (SMD_MAX_DIV + 1))
		return -EINVAL;

	regmap_update_bits(smd->regmap, AT91_PMC_SMD, AT91_PMC_SMD_DIV,
			   (div - 1) << SMD_DIV_SHIFT);

	return 0;
}

static const struct clk_ops at91sam9x5_smd_ops = {
	.recalc_rate = at91sam9x5_clk_smd_recalc_rate,
	.round_rate = at91sam9x5_clk_smd_round_rate,
	.get_parent = at91sam9x5_clk_smd_get_parent,
	.set_parent = at91sam9x5_clk_smd_set_parent,
	.set_rate = at91sam9x5_clk_smd_set_rate,
};

static struct clk * __init
at91sam9x5_clk_register_smd(struct regmap *regmap, const char *name,
			    const char **parent_names, u8 num_parents)
{
	struct at91sam9x5_clk_smd *smd;
	struct clk *clk = NULL;
	struct clk_init_data init;

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

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

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

	clk = clk_register(NULL, &smd->hw);
	if (IS_ERR(clk))
		kfree(smd);

	return clk;
}

static void __init of_at91sam9x5_clk_smd_setup(struct device_node *np)
{
	struct clk *clk;
	unsigned int num_parents;
	const char *parent_names[SMD_SOURCE_MAX];
	const char *name = np->name;
	struct regmap *regmap;

	num_parents = of_clk_get_parent_count(np);
	if (num_parents == 0 || num_parents > SMD_SOURCE_MAX)
		return;

	of_clk_parent_fill(np, parent_names, num_parents);

	of_property_read_string(np, "clock-output-names", &name);

	regmap = syscon_node_to_regmap(of_get_parent(np));
	if (IS_ERR(regmap))
		return;

	clk = at91sam9x5_clk_register_smd(regmap, name, parent_names,
					  num_parents);
	if (IS_ERR(clk))
		return;

	of_clk_add_provider(np, of_clk_src_simple_get, clk);
}
CLK_OF_DECLARE(at91sam9x5_clk_smd, "atmel,at91sam9x5-clk-smd",
	       of_at91sam9x5_clk_smd_setup);
Commit	Line	Data
a9c0688f 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>
a9c0688f BB	17
	18	#include "pmc.h"
	19
	20	#define SMD_SOURCE_MAX 2
	21
	22	#define SMD_DIV_SHIFT 8
	23	#define SMD_MAX_DIV 0xf
	24
	25	struct at91sam9x5_clk_smd {
	26	struct clk_hw hw;
1bdf0232	27	struct regmap *regmap;
a9c0688f BB	28	};
	29
	30	#define to_at91sam9x5_clk_smd(hw) \
	31	container_of(hw, struct at91sam9x5_clk_smd, hw)
	32
	33	static unsigned long at91sam9x5_clk_smd_recalc_rate(struct clk_hw *hw,
	34	unsigned long parent_rate)
	35	{
a9c0688f	36	struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
1bdf0232 BB	37	unsigned int smdr;
	38	u8 smddiv;
	39
	40	regmap_read(smd->regmap, AT91_PMC_SMD, &smdr);
	41	smddiv = (smdr & AT91_PMC_SMD_DIV) >> SMD_DIV_SHIFT;
a9c0688f	42
a9c0688f BB	43	return parent_rate / (smddiv + 1);
	44	}
	45
	46	static long at91sam9x5_clk_smd_round_rate(struct clk_hw *hw, unsigned long rate,
	47	unsigned long *parent_rate)
	48	{
	49	unsigned long div;
	50	unsigned long bestrate;
	51	unsigned long tmp;
	52
	53	if (rate >= *parent_rate)
	54	return *parent_rate;
	55
	56	div = *parent_rate / rate;
	57	if (div > SMD_MAX_DIV)
	58	return *parent_rate / (SMD_MAX_DIV + 1);
	59
	60	bestrate = *parent_rate / div;
	61	tmp = *parent_rate / (div + 1);
	62	if (bestrate - rate > rate - tmp)
	63	bestrate = tmp;
	64
	65	return bestrate;
	66	}
	67
	68	static int at91sam9x5_clk_smd_set_parent(struct clk_hw *hw, u8 index)
	69	{
a9c0688f	70	struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
a9c0688f BB	71
	72	if (index > 1)
	73	return -EINVAL;
1bdf0232 BB	74
	75	regmap_update_bits(smd->regmap, AT91_PMC_SMD, AT91_PMC_SMDS,
	76	index ? AT91_PMC_SMDS : 0);
	77
a9c0688f BB	78	return 0;
	79	}
	80
	81	static u8 at91sam9x5_clk_smd_get_parent(struct clk_hw *hw)
	82	{
	83	struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
1bdf0232	84	unsigned int smdr;
a9c0688f	85
1bdf0232 BB	86	regmap_read(smd->regmap, AT91_PMC_SMD, &smdr);
	87
	88	return smdr & AT91_PMC_SMDS;
a9c0688f BB	89	}
	90
	91	static int at91sam9x5_clk_smd_set_rate(struct clk_hw *hw, unsigned long rate,
	92	unsigned long parent_rate)
	93	{
a9c0688f	94	struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
a9c0688f BB	95	unsigned long div = parent_rate / rate;
	96
	97	if (parent_rate % rate \|\| div < 1 \|\| div > (SMD_MAX_DIV + 1))
	98	return -EINVAL;
1bdf0232 BB	99
	100	regmap_update_bits(smd->regmap, AT91_PMC_SMD, AT91_PMC_SMD_DIV,
	101	(div - 1) << SMD_DIV_SHIFT);
a9c0688f BB	102
	103	return 0;
	104	}
	105
	106	static const struct clk_ops at91sam9x5_smd_ops = {
	107	.recalc_rate = at91sam9x5_clk_smd_recalc_rate,
	108	.round_rate = at91sam9x5_clk_smd_round_rate,
	109	.get_parent = at91sam9x5_clk_smd_get_parent,
	110	.set_parent = at91sam9x5_clk_smd_set_parent,
	111	.set_rate = at91sam9x5_clk_smd_set_rate,
	112	};
	113
	114	static struct clk * __init
1bdf0232	115	at91sam9x5_clk_register_smd(struct regmap regmap, const char name,
a9c0688f BB	116	const char **parent_names, u8 num_parents)
	117	{
	118	struct at91sam9x5_clk_smd *smd;
	119	struct clk *clk = NULL;
	120	struct clk_init_data init;
	121
	122	smd = kzalloc(sizeof(*smd), GFP_KERNEL);
	123	if (!smd)
	124	return ERR_PTR(-ENOMEM);
	125
	126	init.name = name;
	127	init.ops = &at91sam9x5_smd_ops;
	128	init.parent_names = parent_names;
	129	init.num_parents = num_parents;
	130	init.flags = CLK_SET_RATE_GATE \| CLK_SET_PARENT_GATE;
	131
	132	smd->hw.init = &init;
1bdf0232	133	smd->regmap = regmap;
a9c0688f BB	134
	135	clk = clk_register(NULL, &smd->hw);
	136	if (IS_ERR(clk))
	137	kfree(smd);
	138
	139	return clk;
	140	}
	141
1bdf0232	142	static void __init of_at91sam9x5_clk_smd_setup(struct device_node *np)
a9c0688f BB	143	{
a9c0688f BB	144	struct clk *clk;
8c1b1e54	145	unsigned int num_parents;
a9c0688f BB	146	const char *parent_names[SMD_SOURCE_MAX];
a9c0688f BB	147	const char *name = np->name;
1bdf0232	148	struct regmap *regmap;
a9c0688f	149
51a43be9	150	num_parents = of_clk_get_parent_count(np);
8c1b1e54	151	if (num_parents == 0 \|\| num_parents > SMD_SOURCE_MAX)
a9c0688f BB	152	return;
a9c0688f BB	153
f0557fbe	154	of_clk_parent_fill(np, parent_names, num_parents);
a9c0688f BB	155
	156	of_property_read_string(np, "clock-output-names", &name);
	157
1bdf0232 BB	158	regmap = syscon_node_to_regmap(of_get_parent(np));
	159	if (IS_ERR(regmap))
	160	return;
	161
	162	clk = at91sam9x5_clk_register_smd(regmap, name, parent_names,
a9c0688f BB	163	num_parents);
	164	if (IS_ERR(clk))
	165	return;
	166
	167	of_clk_add_provider(np, of_clk_src_simple_get, clk);
	168	}
1bdf0232 BB	169	CLK_OF_DECLARE(at91sam9x5_clk_smd, "atmel,at91sam9x5-clk-smd",
1bdf0232 BB	170	of_at91sam9x5_clk_smd_setup);