1// SPDX-License-Identifier: GPL-2.0
  2/*
  3 * Copyright (c) 2017, Linaro Limited
  4 * Author: Georgi Djakov <georgi.djakov@linaro.org>
  5 */
  6
  7#include <linux/bitops.h>
  8#include <linux/delay.h>
  9#include <linux/kernel.h>
 10#include <linux/regmap.h>
 11
 12#include "clk-regmap-mux-div.h"
 13
 14#define CMD_RCGR			0x0
 15#define CMD_RCGR_UPDATE			BIT(0)
 16#define CMD_RCGR_DIRTY_CFG		BIT(4)
 17#define CMD_RCGR_ROOT_OFF		BIT(31)
 18#define CFG_RCGR			0x4
 19
 20#define to_clk_regmap_mux_div(_hw) \
 21	container_of(to_clk_regmap(_hw), struct clk_regmap_mux_div, clkr)
 22
 23int mux_div_set_src_div(struct clk_regmap_mux_div *md, u32 src, u32 div)
 24{
 25	int ret, count;
 26	u32 val, mask;
 27	const char *name = clk_hw_get_name(&md->clkr.hw);
 28
 29	val = (div << md->hid_shift) | (src << md->src_shift);
 30	mask = ((BIT(md->hid_width) - 1) << md->hid_shift) |
 31	       ((BIT(md->src_width) - 1) << md->src_shift);
 32
 33	ret = regmap_update_bits(md->clkr.regmap, CFG_RCGR + md->reg_offset,
 34				 mask, val);
 35	if (ret)
 36		return ret;
 37
 38	ret = regmap_update_bits(md->clkr.regmap, CMD_RCGR + md->reg_offset,
 39				 CMD_RCGR_UPDATE, CMD_RCGR_UPDATE);
 40	if (ret)
 41		return ret;
 42
 43	/* Wait for update to take effect */
 44	for (count = 500; count > 0; count--) {
 45		ret = regmap_read(md->clkr.regmap, CMD_RCGR + md->reg_offset,
 46				  &val);
 47		if (ret)
 48			return ret;
 49		if (!(val & CMD_RCGR_UPDATE))
 50			return 0;
 51		udelay(1);
 52	}
 53
 54	pr_err("%s: RCG did not update its configuration", name);
 55	return -EBUSY;
 56}
 57EXPORT_SYMBOL_GPL(mux_div_set_src_div);
 58
 59static void mux_div_get_src_div(struct clk_regmap_mux_div *md, u32 *src,
 60				u32 *div)
 61{
 62	u32 val, d, s;
 63	const char *name = clk_hw_get_name(&md->clkr.hw);
 64
 65	regmap_read(md->clkr.regmap, CMD_RCGR + md->reg_offset, &val);
 66
 67	if (val & CMD_RCGR_DIRTY_CFG) {
 68		pr_err("%s: RCG configuration is pending\n", name);
 69		return;
 70	}
 71
 72	regmap_read(md->clkr.regmap, CFG_RCGR + md->reg_offset, &val);
 73	s = (val >> md->src_shift);
 74	s &= BIT(md->src_width) - 1;
 75	*src = s;
 76
 77	d = (val >> md->hid_shift);
 78	d &= BIT(md->hid_width) - 1;
 79	*div = d;
 80}
 81
 82static inline bool is_better_rate(unsigned long req, unsigned long best,
 83				  unsigned long new)
 84{
 85	return (req <= new && new < best) || (best < req && best < new);
 86}
 87
 88static int mux_div_determine_rate(struct clk_hw *hw,
 89				  struct clk_rate_request *req)
 90{
 91	struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
 92	unsigned int i, div, max_div;
 93	unsigned long actual_rate, best_rate = 0;
 94	unsigned long req_rate = req->rate;
 95
 96	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
 97		struct clk_hw *parent = clk_hw_get_parent_by_index(hw, i);
 98		unsigned long parent_rate = clk_hw_get_rate(parent);
 99
100		max_div = BIT(md->hid_width) - 1;
101		for (div = 1; div < max_div; div++) {
102			parent_rate = mult_frac(req_rate, div, 2);
103			parent_rate = clk_hw_round_rate(parent, parent_rate);
104			actual_rate = mult_frac(parent_rate, 2, div);
105
106			if (is_better_rate(req_rate, best_rate, actual_rate)) {
107				best_rate = actual_rate;
108				req->rate = best_rate;
109				req->best_parent_rate = parent_rate;
110				req->best_parent_hw = parent;
111			}
112
113			if (actual_rate < req_rate || best_rate <= req_rate)
114				break;
115		}
116	}
117
118	if (!best_rate)
119		return -EINVAL;
120
121	return 0;
122}
123
124static int __mux_div_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
125					 unsigned long prate, u32 src)
126{
127	struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
128	int ret;
129	u32 div, max_div, best_src = 0, best_div = 0;
130	unsigned int i;
131	unsigned long actual_rate, best_rate = 0;
132
133	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
134		struct clk_hw *parent = clk_hw_get_parent_by_index(hw, i);
135		unsigned long parent_rate = clk_hw_get_rate(parent);
136
137		max_div = BIT(md->hid_width) - 1;
138		for (div = 1; div < max_div; div++) {
139			parent_rate = mult_frac(rate, div, 2);
140			parent_rate = clk_hw_round_rate(parent, parent_rate);
141			actual_rate = mult_frac(parent_rate, 2, div);
142
143			if (is_better_rate(rate, best_rate, actual_rate)) {
144				best_rate = actual_rate;
145				best_src = md->parent_map[i];
146				best_div = div - 1;
147			}
148
149			if (actual_rate < rate || best_rate <= rate)
150				break;
151		}
152	}
153
154	ret = mux_div_set_src_div(md, best_src, best_div);
155	if (!ret) {
156		md->div = best_div;
157		md->src = best_src;
158	}
159
160	return ret;
161}
162
163static u8 mux_div_get_parent(struct clk_hw *hw)
164{
165	struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
166	const char *name = clk_hw_get_name(hw);
167	u32 i, div, src = 0;
168
169	mux_div_get_src_div(md, &src, &div);
170
171	for (i = 0; i < clk_hw_get_num_parents(hw); i++)
172		if (src == md->parent_map[i])
173			return i;
174
175	pr_err("%s: Can't find parent with src %d\n", name, src);
176	return 0;
177}
178
179static int mux_div_set_parent(struct clk_hw *hw, u8 index)
180{
181	struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
182
183	return mux_div_set_src_div(md, md->parent_map[index], md->div);
184}
185
186static int mux_div_set_rate(struct clk_hw *hw,
187			    unsigned long rate, unsigned long prate)
188{
189	struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
190
191	return __mux_div_set_rate_and_parent(hw, rate, prate, md->src);
192}
193
194static int mux_div_set_rate_and_parent(struct clk_hw *hw,  unsigned long rate,
195				       unsigned long prate, u8 index)
196{
197	struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
198
199	return __mux_div_set_rate_and_parent(hw, rate, prate,
200					     md->parent_map[index]);
201}
202
203static unsigned long mux_div_recalc_rate(struct clk_hw *hw, unsigned long prate)
204{
205	struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw);
206	u32 div, src;
207	int i, num_parents = clk_hw_get_num_parents(hw);
208	const char *name = clk_hw_get_name(hw);
209
210	mux_div_get_src_div(md, &src, &div);
211	for (i = 0; i < num_parents; i++)
212		if (src == md->parent_map[i]) {
213			struct clk_hw *p = clk_hw_get_parent_by_index(hw, i);
214			unsigned long parent_rate = clk_hw_get_rate(p);
215
216			return mult_frac(parent_rate, 2, div + 1);
217		}
218
219	pr_err("%s: Can't find parent %d\n", name, src);
220	return 0;
221}
222
223const struct clk_ops clk_regmap_mux_div_ops = {
224	.get_parent = mux_div_get_parent,
225	.set_parent = mux_div_set_parent,
226	.set_rate = mux_div_set_rate,
227	.set_rate_and_parent = mux_div_set_rate_and_parent,
228	.determine_rate = mux_div_determine_rate,
229	.recalc_rate = mux_div_recalc_rate,
230};
231EXPORT_SYMBOL_GPL(clk_regmap_mux_div_ops);