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