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>
15#include <linux/of_address.h>
16#include <linux/io.h>
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
25struct at91sam9x5_clk_smd {
26	struct clk_hw hw;
27	struct at91_pmc *pmc;
28};
29
30#define to_at91sam9x5_clk_smd(hw) \
31	container_of(hw, struct at91sam9x5_clk_smd, hw)
32
33static unsigned long at91sam9x5_clk_smd_recalc_rate(struct clk_hw *hw,
34						    unsigned long parent_rate)
35{
36	u32 tmp;
37	u8 smddiv;
38	struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
39	struct at91_pmc *pmc = smd->pmc;
40
41	tmp = pmc_read(pmc, AT91_PMC_SMD);
42	smddiv = (tmp & AT91_PMC_SMD_DIV) >> SMD_DIV_SHIFT;
43	return parent_rate / (smddiv + 1);
44}
45
46static 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
68static int at91sam9x5_clk_smd_set_parent(struct clk_hw *hw, u8 index)
69{
70	u32 tmp;
71	struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
72	struct at91_pmc *pmc = smd->pmc;
73
74	if (index > 1)
75		return -EINVAL;
76	tmp = pmc_read(pmc, AT91_PMC_SMD) & ~AT91_PMC_SMDS;
77	if (index)
78		tmp |= AT91_PMC_SMDS;
79	pmc_write(pmc, AT91_PMC_SMD, tmp);
80	return 0;
81}
82
83static u8 at91sam9x5_clk_smd_get_parent(struct clk_hw *hw)
84{
85	struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
86	struct at91_pmc *pmc = smd->pmc;
87
88	return pmc_read(pmc, AT91_PMC_SMD) & AT91_PMC_SMDS;
89}
90
91static int at91sam9x5_clk_smd_set_rate(struct clk_hw *hw, unsigned long rate,
92				       unsigned long parent_rate)
93{
94	u32 tmp;
95	struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw);
96	struct at91_pmc *pmc = smd->pmc;
97	unsigned long div = parent_rate / rate;
98
99	if (parent_rate % rate || div < 1 || div > (SMD_MAX_DIV + 1))
100		return -EINVAL;
101	tmp = pmc_read(pmc, AT91_PMC_SMD) & ~AT91_PMC_SMD_DIV;
102	tmp |= (div - 1) << SMD_DIV_SHIFT;
103	pmc_write(pmc, AT91_PMC_SMD, tmp);
104
105	return 0;
106}
107
108static const struct clk_ops at91sam9x5_smd_ops = {
109	.recalc_rate = at91sam9x5_clk_smd_recalc_rate,
110	.round_rate = at91sam9x5_clk_smd_round_rate,
111	.get_parent = at91sam9x5_clk_smd_get_parent,
112	.set_parent = at91sam9x5_clk_smd_set_parent,
113	.set_rate = at91sam9x5_clk_smd_set_rate,
114};
115
116static struct clk * __init
117at91sam9x5_clk_register_smd(struct at91_pmc *pmc, const char *name,
118			    const char **parent_names, u8 num_parents)
119{
120	struct at91sam9x5_clk_smd *smd;
121	struct clk *clk = NULL;
122	struct clk_init_data init;
123
124	smd = kzalloc(sizeof(*smd), GFP_KERNEL);
125	if (!smd)
126		return ERR_PTR(-ENOMEM);
127
128	init.name = name;
129	init.ops = &at91sam9x5_smd_ops;
130	init.parent_names = parent_names;
131	init.num_parents = num_parents;
132	init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
133
134	smd->hw.init = &init;
135	smd->pmc = pmc;
136
137	clk = clk_register(NULL, &smd->hw);
138	if (IS_ERR(clk))
139		kfree(smd);
140
141	return clk;
142}
143
144void __init of_at91sam9x5_clk_smd_setup(struct device_node *np,
145					struct at91_pmc *pmc)
146{
147	struct clk *clk;
148	int i;
149	int num_parents;
150	const char *parent_names[SMD_SOURCE_MAX];
151	const char *name = np->name;
152
153	num_parents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
154	if (num_parents <= 0 || num_parents > SMD_SOURCE_MAX)
155		return;
156
157	for (i = 0; i < num_parents; i++) {
158		parent_names[i] = of_clk_get_parent_name(np, i);
159		if (!parent_names[i])
160			return;
161	}
162
163	of_property_read_string(np, "clock-output-names", &name);
164
165	clk = at91sam9x5_clk_register_smd(pmc, name, parent_names,
166					  num_parents);
167	if (IS_ERR(clk))
168		return;
169
170	of_clk_add_provider(np, of_clk_src_simple_get, clk);
171}
172