1// SPDX-License-Identifier: GPL-2.0-or-later
  2/*
  3 *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
  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>
 10#include <linux/mfd/syscon.h>
 11#include <linux/regmap.h>
 12
 13#include "pmc.h"
 14
 15#define PROG_ID_MAX		7
 16
 17#define PROG_STATUS_MASK(id)	(1 << ((id) + 8))
 18#define PROG_PRES(layout, pckr)	((pckr >> layout->pres_shift) & layout->pres_mask)
 19#define PROG_MAX_RM9200_CSS	3
 20
 21struct clk_programmable {
 22	struct clk_hw hw;
 23	struct regmap *regmap;
 24	u8 id;
 25	const struct clk_programmable_layout *layout;
 26};
 27
 28#define to_clk_programmable(hw) container_of(hw, struct clk_programmable, hw)
 29
 30static unsigned long clk_programmable_recalc_rate(struct clk_hw *hw,
 31						  unsigned long parent_rate)
 32{
 33	struct clk_programmable *prog = to_clk_programmable(hw);
 34	const struct clk_programmable_layout *layout = prog->layout;
 35	unsigned int pckr;
 36	unsigned long rate;
 37
 38	regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr);
 39
 40	if (layout->is_pres_direct)
 41		rate = parent_rate / (PROG_PRES(layout, pckr) + 1);
 42	else
 43		rate = parent_rate >> PROG_PRES(layout, pckr);
 44
 45	return rate;
 46}
 47
 48static int clk_programmable_determine_rate(struct clk_hw *hw,
 49					   struct clk_rate_request *req)
 50{
 51	struct clk_programmable *prog = to_clk_programmable(hw);
 52	const struct clk_programmable_layout *layout = prog->layout;
 53	struct clk_hw *parent;
 54	long best_rate = -EINVAL;
 55	unsigned long parent_rate;
 56	unsigned long tmp_rate = 0;
 57	int shift;
 58	int i;
 59
 60	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
 61		parent = clk_hw_get_parent_by_index(hw, i);
 62		if (!parent)
 63			continue;
 64
 65		parent_rate = clk_hw_get_rate(parent);
 66		if (layout->is_pres_direct) {
 67			for (shift = 0; shift <= layout->pres_mask; shift++) {
 68				tmp_rate = parent_rate / (shift + 1);
 69				if (tmp_rate <= req->rate)
 70					break;
 71			}
 72		} else {
 73			for (shift = 0; shift < layout->pres_mask; shift++) {
 74				tmp_rate = parent_rate >> shift;
 75				if (tmp_rate <= req->rate)
 76					break;
 77			}
 78		}
 79
 80		if (tmp_rate > req->rate)
 81			continue;
 82
 83		if (best_rate < 0 ||
 84		    (req->rate - tmp_rate) < (req->rate - best_rate)) {
 85			best_rate = tmp_rate;
 86			req->best_parent_rate = parent_rate;
 87			req->best_parent_hw = parent;
 88		}
 89
 90		if (!best_rate)
 91			break;
 92	}
 93
 94	if (best_rate < 0)
 95		return best_rate;
 96
 97	req->rate = best_rate;
 98	return 0;
 99}
100
101static int clk_programmable_set_parent(struct clk_hw *hw, u8 index)
102{
103	struct clk_programmable *prog = to_clk_programmable(hw);
104	const struct clk_programmable_layout *layout = prog->layout;
105	unsigned int mask = layout->css_mask;
106	unsigned int pckr = index;
107
108	if (layout->have_slck_mck)
109		mask |= AT91_PMC_CSSMCK_MCK;
110
111	if (index > layout->css_mask) {
112		if (index > PROG_MAX_RM9200_CSS && !layout->have_slck_mck)
113			return -EINVAL;
114
115		pckr |= AT91_PMC_CSSMCK_MCK;
116	}
117
118	regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id), mask, pckr);
119
120	return 0;
121}
122
123static u8 clk_programmable_get_parent(struct clk_hw *hw)
124{
125	struct clk_programmable *prog = to_clk_programmable(hw);
126	const struct clk_programmable_layout *layout = prog->layout;
127	unsigned int pckr;
128	u8 ret;
129
130	regmap_read(prog->regmap, AT91_PMC_PCKR(prog->id), &pckr);
131
132	ret = pckr & layout->css_mask;
133
134	if (layout->have_slck_mck && (pckr & AT91_PMC_CSSMCK_MCK) && !ret)
135		ret = PROG_MAX_RM9200_CSS + 1;
136
137	return ret;
138}
139
140static int clk_programmable_set_rate(struct clk_hw *hw, unsigned long rate,
141				     unsigned long parent_rate)
142{
143	struct clk_programmable *prog = to_clk_programmable(hw);
144	const struct clk_programmable_layout *layout = prog->layout;
145	unsigned long div = parent_rate / rate;
146	int shift = 0;
147
148	if (!div)
149		return -EINVAL;
150
151	if (layout->is_pres_direct) {
152		shift = div - 1;
153
154		if (shift > layout->pres_mask)
155			return -EINVAL;
156	} else {
157		shift = fls(div) - 1;
158
159		if (div != (1 << shift))
160			return -EINVAL;
161
162		if (shift >= layout->pres_mask)
163			return -EINVAL;
164	}
165
166	regmap_update_bits(prog->regmap, AT91_PMC_PCKR(prog->id),
167			   layout->pres_mask << layout->pres_shift,
168			   shift << layout->pres_shift);
169
170	return 0;
171}
172
173static const struct clk_ops programmable_ops = {
174	.recalc_rate = clk_programmable_recalc_rate,
175	.determine_rate = clk_programmable_determine_rate,
176	.get_parent = clk_programmable_get_parent,
177	.set_parent = clk_programmable_set_parent,
178	.set_rate = clk_programmable_set_rate,
179};
180
181struct clk_hw * __init
182at91_clk_register_programmable(struct regmap *regmap,
183			       const char *name, const char **parent_names,
184			       u8 num_parents, u8 id,
185			       const struct clk_programmable_layout *layout)
186{
187	struct clk_programmable *prog;
188	struct clk_hw *hw;
189	struct clk_init_data init;
190	int ret;
191
192	if (id > PROG_ID_MAX)
193		return ERR_PTR(-EINVAL);
194
195	prog = kzalloc(sizeof(*prog), GFP_KERNEL);
196	if (!prog)
197		return ERR_PTR(-ENOMEM);
198
199	init.name = name;
200	init.ops = &programmable_ops;
201	init.parent_names = parent_names;
202	init.num_parents = num_parents;
203	init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE;
204
205	prog->id = id;
206	prog->layout = layout;
207	prog->hw.init = &init;
208	prog->regmap = regmap;
209
210	hw = &prog->hw;
211	ret = clk_hw_register(NULL, &prog->hw);
212	if (ret) {
213		kfree(prog);
214		hw = ERR_PTR(ret);
215	} else {
216		pmc_register_pck(id);
217	}
218
219	return hw;
220}
221
222const struct clk_programmable_layout at91rm9200_programmable_layout = {
223	.pres_mask = 0x7,
224	.pres_shift = 2,
225	.css_mask = 0x3,
226	.have_slck_mck = 0,
227	.is_pres_direct = 0,
228};
229
230const struct clk_programmable_layout at91sam9g45_programmable_layout = {
231	.pres_mask = 0x7,
232	.pres_shift = 2,
233	.css_mask = 0x3,
234	.have_slck_mck = 1,
235	.is_pres_direct = 0,
236};
237
238const struct clk_programmable_layout at91sam9x5_programmable_layout = {
239	.pres_mask = 0x7,
240	.pres_shift = 4,
241	.css_mask = 0x7,
242	.have_slck_mck = 0,
243	.is_pres_direct = 0,
244};