owl-factor.c

  1// SPDX-License-Identifier: GPL-2.0+
  2//
  3// OWL factor clock driver
  4//
  5// Copyright (c) 2014 Actions Semi Inc.
  6// Author: David Liu <liuwei@actions-semi.com>
  7//
  8// Copyright (c) 2018 Linaro Ltd.
  9// Author: Manivannan Sadhasivam <manivannan.sadhasivam@linaro.org>
 10
 11#include <linux/clk-provider.h>
 12#include <linux/regmap.h>
 13
 14#include "owl-factor.h"
 15
 16static unsigned int _get_table_maxval(const struct clk_factor_table *table)
 17{
 18	unsigned int maxval = 0;
 19	const struct clk_factor_table *clkt;
 20
 21	for (clkt = table; clkt->div; clkt++)
 22		if (clkt->val > maxval)
 23			maxval = clkt->val;
 24	return maxval;
 25}
 26
 27static int _get_table_div_mul(const struct clk_factor_table *table,
 28			unsigned int val, unsigned int *mul, unsigned int *div)
 29{
 30	const struct clk_factor_table *clkt;
 31
 32	for (clkt = table; clkt->div; clkt++) {
 33		if (clkt->val == val) {
 34			*mul = clkt->mul;
 35			*div = clkt->div;
 36			return 1;
 37		}
 38	}
 39
 40	return 0;
 41}
 42
 43static unsigned int _get_table_val(const struct clk_factor_table *table,
 44			unsigned long rate, unsigned long parent_rate)
 45{
 46	const struct clk_factor_table *clkt;
 47	int val = -1;
 48	u64 calc_rate;
 49
 50	for (clkt = table; clkt->div; clkt++) {
 51		calc_rate = parent_rate * clkt->mul;
 52		do_div(calc_rate, clkt->div);
 53
 54		if ((unsigned long)calc_rate <= rate) {
 55			val = clkt->val;
 56			break;
 57		}
 58	}
 59
 60	if (val == -1)
 61		val = _get_table_maxval(table);
 62
 63	return val;
 64}
 65
 66static int owl_clk_val_best(const struct owl_factor_hw *factor_hw,
 67			struct clk_hw *hw, unsigned long rate,
 68			unsigned long *best_parent_rate)
 69{
 70	const struct clk_factor_table *clkt = factor_hw->table;
 71	unsigned long parent_rate, try_parent_rate, best = 0, cur_rate;
 72	unsigned long parent_rate_saved = *best_parent_rate;
 73	int bestval = 0;
 74
 75	if (!rate)
 76		rate = 1;
 77
 78	if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
 79		parent_rate = *best_parent_rate;
 80		bestval = _get_table_val(clkt, rate, parent_rate);
 81		return bestval;
 82	}
 83
 84	for (clkt = factor_hw->table; clkt->div; clkt++) {
 85		try_parent_rate = rate * clkt->div / clkt->mul;
 86
 87		if (try_parent_rate == parent_rate_saved) {
 88			pr_debug("%s: [%d %d %d] found try_parent_rate %ld\n",
 89				__func__, clkt->val, clkt->mul, clkt->div,
 90				try_parent_rate);
 91			/*
 92			 * It's the most ideal case if the requested rate can be
 93			 * divided from parent clock without any need to change
 94			 * parent rate, so return the divider immediately.
 95			 */
 96			*best_parent_rate = parent_rate_saved;
 97			return clkt->val;
 98		}
 99
100		parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
101				try_parent_rate);
102		cur_rate = DIV_ROUND_UP(parent_rate, clkt->div) * clkt->mul;
103		if (cur_rate <= rate && cur_rate > best) {
104			bestval = clkt->val;
105			best = cur_rate;
106			*best_parent_rate = parent_rate;
107		}
108	}
109
110	if (!bestval) {
111		bestval = _get_table_maxval(clkt);
112		*best_parent_rate = clk_hw_round_rate(
113				clk_hw_get_parent(hw), 1);
114	}
115
116	return bestval;
117}
118
119long owl_factor_helper_round_rate(struct owl_clk_common *common,
120				const struct owl_factor_hw *factor_hw,
121				unsigned long rate,
122				unsigned long *parent_rate)
123{
124	const struct clk_factor_table *clkt = factor_hw->table;
125	unsigned int val, mul = 0, div = 1;
126
127	val = owl_clk_val_best(factor_hw, &common->hw, rate, parent_rate);
128	_get_table_div_mul(clkt, val, &mul, &div);
129
130	return *parent_rate * mul / div;
131}
132
133static int owl_factor_determine_rate(struct clk_hw *hw,
134				     struct clk_rate_request *req)
135{
136	struct owl_factor *factor = hw_to_owl_factor(hw);
137	struct owl_factor_hw *factor_hw = &factor->factor_hw;
138
139	req->rate = owl_factor_helper_round_rate(&factor->common, factor_hw,
140						 req->rate, &req->best_parent_rate);
141
142	return 0;
143}
144
145unsigned long owl_factor_helper_recalc_rate(struct owl_clk_common *common,
146					 const struct owl_factor_hw *factor_hw,
147					 unsigned long parent_rate)
148{
149	const struct clk_factor_table *clkt = factor_hw->table;
150	unsigned long long int rate;
151	u32 reg, val, mul, div;
152
153	div = 0;
154	mul = 0;
155
156	regmap_read(common->regmap, factor_hw->reg, &reg);
157
158	val = reg >> factor_hw->shift;
159	val &= div_mask(factor_hw);
160
161	_get_table_div_mul(clkt, val, &mul, &div);
162	if (!div) {
163		WARN(!(factor_hw->fct_flags & CLK_DIVIDER_ALLOW_ZERO),
164			"%s: Zero divisor and CLK_DIVIDER_ALLOW_ZERO not set\n",
165			__clk_get_name(common->hw.clk));
166		return parent_rate;
167	}
168
169	rate = (unsigned long long int)parent_rate * mul;
170	do_div(rate, div);
171
172	return rate;
173}
174
175static unsigned long owl_factor_recalc_rate(struct clk_hw *hw,
176			unsigned long parent_rate)
177{
178	struct owl_factor *factor = hw_to_owl_factor(hw);
179	struct owl_factor_hw *factor_hw = &factor->factor_hw;
180	struct owl_clk_common *common = &factor->common;
181
182	return owl_factor_helper_recalc_rate(common, factor_hw, parent_rate);
183}
184
185int owl_factor_helper_set_rate(const struct owl_clk_common *common,
186				const struct owl_factor_hw *factor_hw,
187				unsigned long rate,
188				unsigned long parent_rate)
189{
190	u32 val, reg;
191
192	val = _get_table_val(factor_hw->table, rate, parent_rate);
193
194	if (val > div_mask(factor_hw))
195		val = div_mask(factor_hw);
196
197	regmap_read(common->regmap, factor_hw->reg, &reg);
198
199	reg &= ~(div_mask(factor_hw) << factor_hw->shift);
200	reg |= val << factor_hw->shift;
201
202	regmap_write(common->regmap, factor_hw->reg, reg);
203
204	return 0;
205}
206
207static int owl_factor_set_rate(struct clk_hw *hw, unsigned long rate,
208			       unsigned long parent_rate)
209{
210	struct owl_factor *factor = hw_to_owl_factor(hw);
211	struct owl_factor_hw *factor_hw = &factor->factor_hw;
212	struct owl_clk_common *common = &factor->common;
213
214	return owl_factor_helper_set_rate(common, factor_hw,
215					rate, parent_rate);
216}
217
218const struct clk_ops owl_factor_ops = {
219	.determine_rate = owl_factor_determine_rate,
220	.recalc_rate	= owl_factor_recalc_rate,
221	.set_rate	= owl_factor_set_rate,
222};