1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3 * Copyright (C) 2016 Maxime Ripard
4 * Maxime Ripard <maxime.ripard@free-electrons.com>
5 */
6 #include "ccu.h"
7 #include "ccu_gate.h"
8 #include "ccu_nkm.h"
9
10 struct _ccu_nkm
11 {
12 unsigned long n, min_n, max_n;
13 unsigned long k, min_k, max_k;
14 unsigned long m, min_m, max_m;
15 };
16
ccu_nkm_find_best(unsigned long parent,unsigned long rate,struct _ccu_nkm * nkm)17 static void ccu_nkm_find_best(unsigned long parent, unsigned long rate,
18 struct _ccu_nkm *nkm)
19 {
20 unsigned long best_rate = 0;
21 unsigned long best_n = 0, best_k = 0, best_m = 0;
22 unsigned long _n, _k, _m;
23
24 for (_k = nkm->min_k; _k <= nkm->max_k; _k++)
25 {
26 for (_n = nkm->min_n; _n <= nkm->max_n; _n++)
27 {
28 for (_m = nkm->min_m; _m <= nkm->max_m; _m++)
29 {
30 unsigned long tmp_rate;
31
32 tmp_rate = parent * _n * _k / _m;
33
34 if (tmp_rate > rate)
35 {
36 continue;
37 }
38 if ((rate - tmp_rate) < (rate - best_rate))
39 {
40 best_rate = tmp_rate;
41 best_n = _n;
42 best_k = _k;
43 best_m = _m;
44 }
45 }
46 }
47 }
48
49 nkm->n = best_n;
50 nkm->k = best_k;
51 nkm->m = best_m;
52 }
53
ccu_nkm_disable(struct clk_hw * hw)54 static void ccu_nkm_disable(struct clk_hw *hw)
55 {
56 struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
57
58 return ccu_gate_helper_disable(&nkm->common, nkm->enable);
59 }
60
ccu_nkm_enable(struct clk_hw * hw)61 static int ccu_nkm_enable(struct clk_hw *hw)
62 {
63 struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
64
65 return ccu_gate_helper_enable(&nkm->common, nkm->enable);
66 }
67
ccu_nkm_is_enabled(struct clk_hw * hw)68 static int ccu_nkm_is_enabled(struct clk_hw *hw)
69 {
70 struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
71
72 return ccu_gate_helper_is_enabled(&nkm->common, nkm->enable);
73 }
74
ccu_nkm_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)75 static unsigned long ccu_nkm_recalc_rate(struct clk_hw *hw,
76 unsigned long parent_rate)
77 {
78 struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
79 unsigned long n, m, k, rate;
80 u32 reg;
81
82 reg = readl(nkm->common.base + nkm->common.reg);
83
84 n = reg >> nkm->n.shift;
85 n &= (1 << nkm->n.width) - 1;
86 n += nkm->n.offset;
87 if (!n)
88 {
89 n++;
90 }
91
92 k = reg >> nkm->k.shift;
93 k &= (1 << nkm->k.width) - 1;
94 k += nkm->k.offset;
95 if (!k)
96 {
97 k++;
98 }
99
100 m = reg >> nkm->m.shift;
101 m &= (1 << nkm->m.width) - 1;
102 m += nkm->m.offset;
103 if (!m)
104 {
105 m++;
106 }
107
108 rate = parent_rate * n * k / m;
109
110 if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
111 {
112 rate /= nkm->fixed_post_div;
113 }
114
115 return rate;
116 }
117
ccu_nkm_round_rate(struct ccu_mux_internal * mux,struct clk_hw * hw,unsigned long * parent_rate,unsigned long rate,void * data)118 static unsigned long ccu_nkm_round_rate(struct ccu_mux_internal *mux,
119 struct clk_hw *hw,
120 unsigned long *parent_rate,
121 unsigned long rate,
122 void *data)
123 {
124 struct ccu_nkm *nkm = data;
125 struct _ccu_nkm _nkm;
126
127 _nkm.min_n = nkm->n.min ? : 1;
128 _nkm.max_n = nkm->n.max ? : 1 << nkm->n.width;
129 _nkm.min_k = nkm->k.min ? : 1;
130 _nkm.max_k = nkm->k.max ? : 1 << nkm->k.width;
131 _nkm.min_m = 1;
132 _nkm.max_m = nkm->m.max ? : 1 << nkm->m.width;
133
134 if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
135 {
136 rate *= nkm->fixed_post_div;
137 }
138
139 ccu_nkm_find_best(*parent_rate, rate, &_nkm);
140
141 rate = *parent_rate * _nkm.n * _nkm.k / _nkm.m;
142
143 if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
144 {
145 rate /= nkm->fixed_post_div;
146 }
147
148 return rate;
149 }
150
ccu_nkm_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)151 static int ccu_nkm_determine_rate(struct clk_hw *hw,
152 struct clk_rate_request *req)
153 {
154 struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
155
156 return ccu_mux_helper_determine_rate(&nkm->common, &nkm->mux,
157 req, ccu_nkm_round_rate, nkm);
158 }
159
ccu_nkm_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)160 static int ccu_nkm_set_rate(struct clk_hw *hw, unsigned long rate,
161 unsigned long parent_rate)
162 {
163 struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
164 struct _ccu_nkm _nkm;
165 u32 reg;
166 u32 __cspr;
167
168 if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
169 {
170 rate *= nkm->fixed_post_div;
171 }
172
173 _nkm.min_n = nkm->n.min ? : 1;
174 _nkm.max_n = nkm->n.max ? : 1 << nkm->n.width;
175 _nkm.min_k = nkm->k.min ? : 1;
176 _nkm.max_k = nkm->k.max ? : 1 << nkm->k.width;
177 _nkm.min_m = 1;
178 _nkm.max_m = nkm->m.max ? : 1 << nkm->m.width;
179
180 ccu_nkm_find_best(parent_rate, rate, &_nkm);
181
182 __cspr = hal_spin_lock_irqsave(&nkm->common.lock);
183
184 reg = readl(nkm->common.base + nkm->common.reg);
185 reg &= ~GENMASK(nkm->n.width + nkm->n.shift - 1, nkm->n.shift);
186 reg &= ~GENMASK(nkm->k.width + nkm->k.shift - 1, nkm->k.shift);
187 reg &= ~GENMASK(nkm->m.width + nkm->m.shift - 1, nkm->m.shift);
188
189 reg |= (_nkm.n - nkm->n.offset) << nkm->n.shift;
190 reg |= (_nkm.k - nkm->k.offset) << nkm->k.shift;
191 reg |= (_nkm.m - nkm->m.offset) << nkm->m.shift;
192 writel(reg, nkm->common.base + nkm->common.reg);
193
194 hal_spin_unlock_irqrestore(&nkm->common.lock, __cspr);
195
196 ccu_helper_wait_for_lock(&nkm->common, nkm->lock);
197
198 return 0;
199 }
200
ccu_nkm_get_parent(struct clk_hw * hw)201 static u8 ccu_nkm_get_parent(struct clk_hw *hw)
202 {
203 struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
204
205 return ccu_mux_helper_get_parent(&nkm->common, &nkm->mux);
206 }
207
ccu_nkm_set_parent(struct clk_hw * hw,u8 index)208 static int ccu_nkm_set_parent(struct clk_hw *hw, u8 index)
209 {
210 struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
211
212 return ccu_mux_helper_set_parent(&nkm->common, &nkm->mux, index);
213 }
214
215 const struct clk_ops ccu_nkm_ops =
216 {
217 .disable = ccu_nkm_disable,
218 .enable = ccu_nkm_enable,
219 .is_enabled = ccu_nkm_is_enabled,
220
221 .get_parent = ccu_nkm_get_parent,
222 .set_parent = ccu_nkm_set_parent,
223
224 .determine_rate = ccu_nkm_determine_rate,
225 .recalc_rate = ccu_nkm_recalc_rate,
226 .set_rate = ccu_nkm_set_rate,
227 };
228