19c92ab61SThomas Gleixner // SPDX-License-Identifier: GPL-2.0-only
2bcd61c0fSStephen Boyd /*
3bcd61c0fSStephen Boyd * Copyright (c) 2013, The Linux Foundation. All rights reserved.
4bcd61c0fSStephen Boyd */
5bcd61c0fSStephen Boyd
6bcd61c0fSStephen Boyd #include <linux/kernel.h>
7bcd61c0fSStephen Boyd #include <linux/bitops.h>
8bcd61c0fSStephen Boyd #include <linux/err.h>
9bcd61c0fSStephen Boyd #include <linux/export.h>
10bcd61c0fSStephen Boyd #include <linux/clk-provider.h>
11bcd61c0fSStephen Boyd #include <linux/regmap.h>
12bcd61c0fSStephen Boyd
13bcd61c0fSStephen Boyd #include <asm/div64.h>
14bcd61c0fSStephen Boyd
15bcd61c0fSStephen Boyd #include "clk-rcg.h"
1650c6a503SStephen Boyd #include "common.h"
17bcd61c0fSStephen Boyd
ns_to_src(struct src_sel * s,u32 ns)18bcd61c0fSStephen Boyd static u32 ns_to_src(struct src_sel *s, u32 ns)
19bcd61c0fSStephen Boyd {
20bcd61c0fSStephen Boyd ns >>= s->src_sel_shift;
21bcd61c0fSStephen Boyd ns &= SRC_SEL_MASK;
22bcd61c0fSStephen Boyd return ns;
23bcd61c0fSStephen Boyd }
24bcd61c0fSStephen Boyd
src_to_ns(struct src_sel * s,u8 src,u32 ns)25bcd61c0fSStephen Boyd static u32 src_to_ns(struct src_sel *s, u8 src, u32 ns)
26bcd61c0fSStephen Boyd {
27bcd61c0fSStephen Boyd u32 mask;
28bcd61c0fSStephen Boyd
29bcd61c0fSStephen Boyd mask = SRC_SEL_MASK;
30bcd61c0fSStephen Boyd mask <<= s->src_sel_shift;
31bcd61c0fSStephen Boyd ns &= ~mask;
32bcd61c0fSStephen Boyd
33bcd61c0fSStephen Boyd ns |= src << s->src_sel_shift;
34bcd61c0fSStephen Boyd return ns;
35bcd61c0fSStephen Boyd }
36bcd61c0fSStephen Boyd
clk_rcg_get_parent(struct clk_hw * hw)37bcd61c0fSStephen Boyd static u8 clk_rcg_get_parent(struct clk_hw *hw)
38bcd61c0fSStephen Boyd {
39bcd61c0fSStephen Boyd struct clk_rcg *rcg = to_clk_rcg(hw);
40497295afSStephen Boyd int num_parents = clk_hw_get_num_parents(hw);
41bcd61c0fSStephen Boyd u32 ns;
427f218978SGeorgi Djakov int i, ret;
43bcd61c0fSStephen Boyd
447f218978SGeorgi Djakov ret = regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
457f218978SGeorgi Djakov if (ret)
467f218978SGeorgi Djakov goto err;
47bcd61c0fSStephen Boyd ns = ns_to_src(&rcg->s, ns);
48bcd61c0fSStephen Boyd for (i = 0; i < num_parents; i++)
49293d2e97SGeorgi Djakov if (ns == rcg->s.parent_map[i].cfg)
50bcd61c0fSStephen Boyd return i;
51bcd61c0fSStephen Boyd
527f218978SGeorgi Djakov err:
537f218978SGeorgi Djakov pr_debug("%s: Clock %s has invalid parent, using default.\n",
54ac269395SStephen Boyd __func__, clk_hw_get_name(hw));
557f218978SGeorgi Djakov return 0;
56bcd61c0fSStephen Boyd }
57bcd61c0fSStephen Boyd
reg_to_bank(struct clk_dyn_rcg * rcg,u32 bank)58bcd61c0fSStephen Boyd static int reg_to_bank(struct clk_dyn_rcg *rcg, u32 bank)
59bcd61c0fSStephen Boyd {
60bcd61c0fSStephen Boyd bank &= BIT(rcg->mux_sel_bit);
61bcd61c0fSStephen Boyd return !!bank;
62bcd61c0fSStephen Boyd }
63bcd61c0fSStephen Boyd
clk_dyn_rcg_get_parent(struct clk_hw * hw)64bcd61c0fSStephen Boyd static u8 clk_dyn_rcg_get_parent(struct clk_hw *hw)
65bcd61c0fSStephen Boyd {
66bcd61c0fSStephen Boyd struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
67497295afSStephen Boyd int num_parents = clk_hw_get_num_parents(hw);
68229fd4a5SStephen Boyd u32 ns, reg;
69bcd61c0fSStephen Boyd int bank;
707f218978SGeorgi Djakov int i, ret;
71bcd61c0fSStephen Boyd struct src_sel *s;
72bcd61c0fSStephen Boyd
737f218978SGeorgi Djakov ret = regmap_read(rcg->clkr.regmap, rcg->bank_reg, ®);
747f218978SGeorgi Djakov if (ret)
757f218978SGeorgi Djakov goto err;
76229fd4a5SStephen Boyd bank = reg_to_bank(rcg, reg);
77bcd61c0fSStephen Boyd s = &rcg->s[bank];
78bcd61c0fSStephen Boyd
797f218978SGeorgi Djakov ret = regmap_read(rcg->clkr.regmap, rcg->ns_reg[bank], &ns);
807f218978SGeorgi Djakov if (ret)
817f218978SGeorgi Djakov goto err;
82bcd61c0fSStephen Boyd ns = ns_to_src(s, ns);
83bcd61c0fSStephen Boyd
84bcd61c0fSStephen Boyd for (i = 0; i < num_parents; i++)
85293d2e97SGeorgi Djakov if (ns == s->parent_map[i].cfg)
86bcd61c0fSStephen Boyd return i;
87bcd61c0fSStephen Boyd
887f218978SGeorgi Djakov err:
897f218978SGeorgi Djakov pr_debug("%s: Clock %s has invalid parent, using default.\n",
90ac269395SStephen Boyd __func__, clk_hw_get_name(hw));
917f218978SGeorgi Djakov return 0;
92bcd61c0fSStephen Boyd }
93bcd61c0fSStephen Boyd
clk_rcg_set_parent(struct clk_hw * hw,u8 index)94bcd61c0fSStephen Boyd static int clk_rcg_set_parent(struct clk_hw *hw, u8 index)
95bcd61c0fSStephen Boyd {
96bcd61c0fSStephen Boyd struct clk_rcg *rcg = to_clk_rcg(hw);
97bcd61c0fSStephen Boyd u32 ns;
98bcd61c0fSStephen Boyd
99bcd61c0fSStephen Boyd regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
100293d2e97SGeorgi Djakov ns = src_to_ns(&rcg->s, rcg->s.parent_map[index].cfg, ns);
101bcd61c0fSStephen Boyd regmap_write(rcg->clkr.regmap, rcg->ns_reg, ns);
102bcd61c0fSStephen Boyd
103bcd61c0fSStephen Boyd return 0;
104bcd61c0fSStephen Boyd }
105bcd61c0fSStephen Boyd
md_to_m(struct mn * mn,u32 md)106bcd61c0fSStephen Boyd static u32 md_to_m(struct mn *mn, u32 md)
107bcd61c0fSStephen Boyd {
108bcd61c0fSStephen Boyd md >>= mn->m_val_shift;
109bcd61c0fSStephen Boyd md &= BIT(mn->width) - 1;
110bcd61c0fSStephen Boyd return md;
111bcd61c0fSStephen Boyd }
112bcd61c0fSStephen Boyd
ns_to_pre_div(struct pre_div * p,u32 ns)113bcd61c0fSStephen Boyd static u32 ns_to_pre_div(struct pre_div *p, u32 ns)
114bcd61c0fSStephen Boyd {
115bcd61c0fSStephen Boyd ns >>= p->pre_div_shift;
116bcd61c0fSStephen Boyd ns &= BIT(p->pre_div_width) - 1;
117bcd61c0fSStephen Boyd return ns;
118bcd61c0fSStephen Boyd }
119bcd61c0fSStephen Boyd
pre_div_to_ns(struct pre_div * p,u8 pre_div,u32 ns)120bcd61c0fSStephen Boyd static u32 pre_div_to_ns(struct pre_div *p, u8 pre_div, u32 ns)
121bcd61c0fSStephen Boyd {
122bcd61c0fSStephen Boyd u32 mask;
123bcd61c0fSStephen Boyd
124bcd61c0fSStephen Boyd mask = BIT(p->pre_div_width) - 1;
125bcd61c0fSStephen Boyd mask <<= p->pre_div_shift;
126bcd61c0fSStephen Boyd ns &= ~mask;
127bcd61c0fSStephen Boyd
128bcd61c0fSStephen Boyd ns |= pre_div << p->pre_div_shift;
129bcd61c0fSStephen Boyd return ns;
130bcd61c0fSStephen Boyd }
131bcd61c0fSStephen Boyd
mn_to_md(struct mn * mn,u32 m,u32 n,u32 md)132bcd61c0fSStephen Boyd static u32 mn_to_md(struct mn *mn, u32 m, u32 n, u32 md)
133bcd61c0fSStephen Boyd {
134bcd61c0fSStephen Boyd u32 mask, mask_w;
135bcd61c0fSStephen Boyd
136bcd61c0fSStephen Boyd mask_w = BIT(mn->width) - 1;
137bcd61c0fSStephen Boyd mask = (mask_w << mn->m_val_shift) | mask_w;
138bcd61c0fSStephen Boyd md &= ~mask;
139bcd61c0fSStephen Boyd
140bcd61c0fSStephen Boyd if (n) {
141bcd61c0fSStephen Boyd m <<= mn->m_val_shift;
142bcd61c0fSStephen Boyd md |= m;
143bcd61c0fSStephen Boyd md |= ~n & mask_w;
144bcd61c0fSStephen Boyd }
145bcd61c0fSStephen Boyd
146bcd61c0fSStephen Boyd return md;
147bcd61c0fSStephen Boyd }
148bcd61c0fSStephen Boyd
ns_m_to_n(struct mn * mn,u32 ns,u32 m)149bcd61c0fSStephen Boyd static u32 ns_m_to_n(struct mn *mn, u32 ns, u32 m)
150bcd61c0fSStephen Boyd {
151bcd61c0fSStephen Boyd ns = ~ns >> mn->n_val_shift;
152bcd61c0fSStephen Boyd ns &= BIT(mn->width) - 1;
153bcd61c0fSStephen Boyd return ns + m;
154bcd61c0fSStephen Boyd }
155bcd61c0fSStephen Boyd
reg_to_mnctr_mode(struct mn * mn,u32 val)156bcd61c0fSStephen Boyd static u32 reg_to_mnctr_mode(struct mn *mn, u32 val)
157bcd61c0fSStephen Boyd {
158bcd61c0fSStephen Boyd val >>= mn->mnctr_mode_shift;
159bcd61c0fSStephen Boyd val &= MNCTR_MODE_MASK;
160bcd61c0fSStephen Boyd return val;
161bcd61c0fSStephen Boyd }
162bcd61c0fSStephen Boyd
mn_to_ns(struct mn * mn,u32 m,u32 n,u32 ns)163bcd61c0fSStephen Boyd static u32 mn_to_ns(struct mn *mn, u32 m, u32 n, u32 ns)
164bcd61c0fSStephen Boyd {
165bcd61c0fSStephen Boyd u32 mask;
166bcd61c0fSStephen Boyd
167bcd61c0fSStephen Boyd mask = BIT(mn->width) - 1;
168bcd61c0fSStephen Boyd mask <<= mn->n_val_shift;
169bcd61c0fSStephen Boyd ns &= ~mask;
170bcd61c0fSStephen Boyd
171bcd61c0fSStephen Boyd if (n) {
172bcd61c0fSStephen Boyd n = n - m;
173bcd61c0fSStephen Boyd n = ~n;
174bcd61c0fSStephen Boyd n &= BIT(mn->width) - 1;
175bcd61c0fSStephen Boyd n <<= mn->n_val_shift;
176bcd61c0fSStephen Boyd ns |= n;
177bcd61c0fSStephen Boyd }
178bcd61c0fSStephen Boyd
179bcd61c0fSStephen Boyd return ns;
180bcd61c0fSStephen Boyd }
181bcd61c0fSStephen Boyd
mn_to_reg(struct mn * mn,u32 m,u32 n,u32 val)182bcd61c0fSStephen Boyd static u32 mn_to_reg(struct mn *mn, u32 m, u32 n, u32 val)
183bcd61c0fSStephen Boyd {
184bcd61c0fSStephen Boyd u32 mask;
185bcd61c0fSStephen Boyd
186bcd61c0fSStephen Boyd mask = MNCTR_MODE_MASK << mn->mnctr_mode_shift;
187bcd61c0fSStephen Boyd mask |= BIT(mn->mnctr_en_bit);
188bcd61c0fSStephen Boyd val &= ~mask;
189bcd61c0fSStephen Boyd
190bcd61c0fSStephen Boyd if (n) {
191bcd61c0fSStephen Boyd val |= BIT(mn->mnctr_en_bit);
192bcd61c0fSStephen Boyd val |= MNCTR_MODE_DUAL << mn->mnctr_mode_shift;
193bcd61c0fSStephen Boyd }
194bcd61c0fSStephen Boyd
195bcd61c0fSStephen Boyd return val;
196bcd61c0fSStephen Boyd }
197bcd61c0fSStephen Boyd
configure_bank(struct clk_dyn_rcg * rcg,const struct freq_tbl * f)198fae507afSGeorgi Djakov static int configure_bank(struct clk_dyn_rcg *rcg, const struct freq_tbl *f)
199bcd61c0fSStephen Boyd {
200229fd4a5SStephen Boyd u32 ns, md, reg;
201293d2e97SGeorgi Djakov int bank, new_bank, ret, index;
202bcd61c0fSStephen Boyd struct mn *mn;
203bcd61c0fSStephen Boyd struct pre_div *p;
204bcd61c0fSStephen Boyd struct src_sel *s;
205bcd61c0fSStephen Boyd bool enabled;
206229fd4a5SStephen Boyd u32 md_reg, ns_reg;
207bcd61c0fSStephen Boyd bool banked_mn = !!rcg->mn[1].width;
208229fd4a5SStephen Boyd bool banked_p = !!rcg->p[1].pre_div_width;
209bcd61c0fSStephen Boyd struct clk_hw *hw = &rcg->clkr.hw;
210bcd61c0fSStephen Boyd
211bcd61c0fSStephen Boyd enabled = __clk_is_enabled(hw->clk);
212bcd61c0fSStephen Boyd
213fae507afSGeorgi Djakov ret = regmap_read(rcg->clkr.regmap, rcg->bank_reg, ®);
214fae507afSGeorgi Djakov if (ret)
215fae507afSGeorgi Djakov return ret;
216229fd4a5SStephen Boyd bank = reg_to_bank(rcg, reg);
217bcd61c0fSStephen Boyd new_bank = enabled ? !bank : bank;
218bcd61c0fSStephen Boyd
219229fd4a5SStephen Boyd ns_reg = rcg->ns_reg[new_bank];
220fae507afSGeorgi Djakov ret = regmap_read(rcg->clkr.regmap, ns_reg, &ns);
221fae507afSGeorgi Djakov if (ret)
222fae507afSGeorgi Djakov return ret;
223229fd4a5SStephen Boyd
224bcd61c0fSStephen Boyd if (banked_mn) {
225bcd61c0fSStephen Boyd mn = &rcg->mn[new_bank];
226bcd61c0fSStephen Boyd md_reg = rcg->md_reg[new_bank];
227bcd61c0fSStephen Boyd
228bcd61c0fSStephen Boyd ns |= BIT(mn->mnctr_reset_bit);
229fae507afSGeorgi Djakov ret = regmap_write(rcg->clkr.regmap, ns_reg, ns);
230fae507afSGeorgi Djakov if (ret)
231fae507afSGeorgi Djakov return ret;
232bcd61c0fSStephen Boyd
233fae507afSGeorgi Djakov ret = regmap_read(rcg->clkr.regmap, md_reg, &md);
234fae507afSGeorgi Djakov if (ret)
235fae507afSGeorgi Djakov return ret;
236bcd61c0fSStephen Boyd md = mn_to_md(mn, f->m, f->n, md);
237fae507afSGeorgi Djakov ret = regmap_write(rcg->clkr.regmap, md_reg, md);
238fae507afSGeorgi Djakov if (ret)
239fae507afSGeorgi Djakov return ret;
240bcd61c0fSStephen Boyd ns = mn_to_ns(mn, f->m, f->n, ns);
241fae507afSGeorgi Djakov ret = regmap_write(rcg->clkr.regmap, ns_reg, ns);
242fae507afSGeorgi Djakov if (ret)
243fae507afSGeorgi Djakov return ret;
244bcd61c0fSStephen Boyd
245229fd4a5SStephen Boyd /* Two NS registers means mode control is in NS register */
246229fd4a5SStephen Boyd if (rcg->ns_reg[0] != rcg->ns_reg[1]) {
247229fd4a5SStephen Boyd ns = mn_to_reg(mn, f->m, f->n, ns);
248fae507afSGeorgi Djakov ret = regmap_write(rcg->clkr.regmap, ns_reg, ns);
249fae507afSGeorgi Djakov if (ret)
250fae507afSGeorgi Djakov return ret;
251229fd4a5SStephen Boyd } else {
252229fd4a5SStephen Boyd reg = mn_to_reg(mn, f->m, f->n, reg);
253fae507afSGeorgi Djakov ret = regmap_write(rcg->clkr.regmap, rcg->bank_reg,
254fae507afSGeorgi Djakov reg);
255fae507afSGeorgi Djakov if (ret)
256fae507afSGeorgi Djakov return ret;
257229fd4a5SStephen Boyd }
258bcd61c0fSStephen Boyd
259bcd61c0fSStephen Boyd ns &= ~BIT(mn->mnctr_reset_bit);
260fae507afSGeorgi Djakov ret = regmap_write(rcg->clkr.regmap, ns_reg, ns);
261fae507afSGeorgi Djakov if (ret)
262fae507afSGeorgi Djakov return ret;
263229fd4a5SStephen Boyd }
264229fd4a5SStephen Boyd
265229fd4a5SStephen Boyd if (banked_p) {
266bcd61c0fSStephen Boyd p = &rcg->p[new_bank];
267bcd61c0fSStephen Boyd ns = pre_div_to_ns(p, f->pre_div - 1, ns);
268bcd61c0fSStephen Boyd }
269bcd61c0fSStephen Boyd
270bcd61c0fSStephen Boyd s = &rcg->s[new_bank];
271293d2e97SGeorgi Djakov index = qcom_find_src_index(hw, s->parent_map, f->src);
272293d2e97SGeorgi Djakov if (index < 0)
273293d2e97SGeorgi Djakov return index;
274293d2e97SGeorgi Djakov ns = src_to_ns(s, s->parent_map[index].cfg, ns);
275fae507afSGeorgi Djakov ret = regmap_write(rcg->clkr.regmap, ns_reg, ns);
276fae507afSGeorgi Djakov if (ret)
277fae507afSGeorgi Djakov return ret;
278bcd61c0fSStephen Boyd
279bcd61c0fSStephen Boyd if (enabled) {
280fae507afSGeorgi Djakov ret = regmap_read(rcg->clkr.regmap, rcg->bank_reg, ®);
281fae507afSGeorgi Djakov if (ret)
282fae507afSGeorgi Djakov return ret;
283229fd4a5SStephen Boyd reg ^= BIT(rcg->mux_sel_bit);
284fae507afSGeorgi Djakov ret = regmap_write(rcg->clkr.regmap, rcg->bank_reg, reg);
285fae507afSGeorgi Djakov if (ret)
286fae507afSGeorgi Djakov return ret;
287bcd61c0fSStephen Boyd }
288fae507afSGeorgi Djakov return 0;
289bcd61c0fSStephen Boyd }
290bcd61c0fSStephen Boyd
clk_dyn_rcg_set_parent(struct clk_hw * hw,u8 index)291bcd61c0fSStephen Boyd static int clk_dyn_rcg_set_parent(struct clk_hw *hw, u8 index)
292bcd61c0fSStephen Boyd {
293bcd61c0fSStephen Boyd struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
294229fd4a5SStephen Boyd u32 ns, md, reg;
295bcd61c0fSStephen Boyd int bank;
296bcd61c0fSStephen Boyd struct freq_tbl f = { 0 };
297bcd61c0fSStephen Boyd bool banked_mn = !!rcg->mn[1].width;
298229fd4a5SStephen Boyd bool banked_p = !!rcg->p[1].pre_div_width;
299bcd61c0fSStephen Boyd
300229fd4a5SStephen Boyd regmap_read(rcg->clkr.regmap, rcg->bank_reg, ®);
301bcd61c0fSStephen Boyd bank = reg_to_bank(rcg, reg);
302bcd61c0fSStephen Boyd
303229fd4a5SStephen Boyd regmap_read(rcg->clkr.regmap, rcg->ns_reg[bank], &ns);
304229fd4a5SStephen Boyd
305bcd61c0fSStephen Boyd if (banked_mn) {
306bcd61c0fSStephen Boyd regmap_read(rcg->clkr.regmap, rcg->md_reg[bank], &md);
307bcd61c0fSStephen Boyd f.m = md_to_m(&rcg->mn[bank], md);
308bcd61c0fSStephen Boyd f.n = ns_m_to_n(&rcg->mn[bank], ns, f.m);
309bcd61c0fSStephen Boyd }
310bcd61c0fSStephen Boyd
311229fd4a5SStephen Boyd if (banked_p)
312229fd4a5SStephen Boyd f.pre_div = ns_to_pre_div(&rcg->p[bank], ns) + 1;
313229fd4a5SStephen Boyd
3142f272e7bSGeorgi Djakov f.src = qcom_find_src_index(hw, rcg->s[bank].parent_map, index);
315fae507afSGeorgi Djakov return configure_bank(rcg, &f);
316bcd61c0fSStephen Boyd }
317bcd61c0fSStephen Boyd
318bcd61c0fSStephen Boyd /*
319bcd61c0fSStephen Boyd * Calculate m/n:d rate
320bcd61c0fSStephen Boyd *
321bcd61c0fSStephen Boyd * parent_rate m
322bcd61c0fSStephen Boyd * rate = ----------- x ---
323bcd61c0fSStephen Boyd * pre_div n
324bcd61c0fSStephen Boyd */
325bcd61c0fSStephen Boyd static unsigned long
calc_rate(unsigned long rate,u32 m,u32 n,u32 mode,u32 pre_div)326bcd61c0fSStephen Boyd calc_rate(unsigned long rate, u32 m, u32 n, u32 mode, u32 pre_div)
327bcd61c0fSStephen Boyd {
328bcd61c0fSStephen Boyd if (pre_div)
329bcd61c0fSStephen Boyd rate /= pre_div + 1;
330bcd61c0fSStephen Boyd
331bcd61c0fSStephen Boyd if (mode) {
332bcd61c0fSStephen Boyd u64 tmp = rate;
333bcd61c0fSStephen Boyd tmp *= m;
334bcd61c0fSStephen Boyd do_div(tmp, n);
335bcd61c0fSStephen Boyd rate = tmp;
336bcd61c0fSStephen Boyd }
337bcd61c0fSStephen Boyd
338bcd61c0fSStephen Boyd return rate;
339bcd61c0fSStephen Boyd }
340bcd61c0fSStephen Boyd
341bcd61c0fSStephen Boyd static unsigned long
clk_rcg_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)342bcd61c0fSStephen Boyd clk_rcg_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
343bcd61c0fSStephen Boyd {
344bcd61c0fSStephen Boyd struct clk_rcg *rcg = to_clk_rcg(hw);
345bcd61c0fSStephen Boyd u32 pre_div, m = 0, n = 0, ns, md, mode = 0;
346bcd61c0fSStephen Boyd struct mn *mn = &rcg->mn;
347bcd61c0fSStephen Boyd
348bcd61c0fSStephen Boyd regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
349bcd61c0fSStephen Boyd pre_div = ns_to_pre_div(&rcg->p, ns);
350bcd61c0fSStephen Boyd
351bcd61c0fSStephen Boyd if (rcg->mn.width) {
352bcd61c0fSStephen Boyd regmap_read(rcg->clkr.regmap, rcg->md_reg, &md);
353bcd61c0fSStephen Boyd m = md_to_m(mn, md);
354bcd61c0fSStephen Boyd n = ns_m_to_n(mn, ns, m);
355bcd61c0fSStephen Boyd /* MN counter mode is in hw.enable_reg sometimes */
356bcd61c0fSStephen Boyd if (rcg->clkr.enable_reg != rcg->ns_reg)
357bcd61c0fSStephen Boyd regmap_read(rcg->clkr.regmap, rcg->clkr.enable_reg, &mode);
358bcd61c0fSStephen Boyd else
359bcd61c0fSStephen Boyd mode = ns;
360bcd61c0fSStephen Boyd mode = reg_to_mnctr_mode(mn, mode);
361bcd61c0fSStephen Boyd }
362bcd61c0fSStephen Boyd
363bcd61c0fSStephen Boyd return calc_rate(parent_rate, m, n, mode, pre_div);
364bcd61c0fSStephen Boyd }
365bcd61c0fSStephen Boyd
366bcd61c0fSStephen Boyd static unsigned long
clk_dyn_rcg_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)367bcd61c0fSStephen Boyd clk_dyn_rcg_recalc_rate(struct clk_hw *hw, unsigned long parent_rate)
368bcd61c0fSStephen Boyd {
369bcd61c0fSStephen Boyd struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
370bcd61c0fSStephen Boyd u32 m, n, pre_div, ns, md, mode, reg;
371bcd61c0fSStephen Boyd int bank;
372bcd61c0fSStephen Boyd struct mn *mn;
373229fd4a5SStephen Boyd bool banked_p = !!rcg->p[1].pre_div_width;
374bcd61c0fSStephen Boyd bool banked_mn = !!rcg->mn[1].width;
375bcd61c0fSStephen Boyd
376229fd4a5SStephen Boyd regmap_read(rcg->clkr.regmap, rcg->bank_reg, ®);
377bcd61c0fSStephen Boyd bank = reg_to_bank(rcg, reg);
378bcd61c0fSStephen Boyd
379229fd4a5SStephen Boyd regmap_read(rcg->clkr.regmap, rcg->ns_reg[bank], &ns);
380229fd4a5SStephen Boyd m = n = pre_div = mode = 0;
381229fd4a5SStephen Boyd
382bcd61c0fSStephen Boyd if (banked_mn) {
383bcd61c0fSStephen Boyd mn = &rcg->mn[bank];
384bcd61c0fSStephen Boyd regmap_read(rcg->clkr.regmap, rcg->md_reg[bank], &md);
385bcd61c0fSStephen Boyd m = md_to_m(mn, md);
386bcd61c0fSStephen Boyd n = ns_m_to_n(mn, ns, m);
387229fd4a5SStephen Boyd /* Two NS registers means mode control is in NS register */
388229fd4a5SStephen Boyd if (rcg->ns_reg[0] != rcg->ns_reg[1])
389229fd4a5SStephen Boyd reg = ns;
390bcd61c0fSStephen Boyd mode = reg_to_mnctr_mode(mn, reg);
391bcd61c0fSStephen Boyd }
392229fd4a5SStephen Boyd
393229fd4a5SStephen Boyd if (banked_p)
394229fd4a5SStephen Boyd pre_div = ns_to_pre_div(&rcg->p[bank], ns);
395229fd4a5SStephen Boyd
396229fd4a5SStephen Boyd return calc_rate(parent_rate, m, n, mode, pre_div);
397bcd61c0fSStephen Boyd }
398bcd61c0fSStephen Boyd
_freq_tbl_determine_rate(struct clk_hw * hw,const struct freq_tbl * f,struct clk_rate_request * req,const struct parent_map * parent_map)3990817b62cSBoris Brezillon static int _freq_tbl_determine_rate(struct clk_hw *hw, const struct freq_tbl *f,
4000817b62cSBoris Brezillon struct clk_rate_request *req,
4012f272e7bSGeorgi Djakov const struct parent_map *parent_map)
402bcd61c0fSStephen Boyd {
4030817b62cSBoris Brezillon unsigned long clk_flags, rate = req->rate;
404ac269395SStephen Boyd struct clk_hw *p;
4052f272e7bSGeorgi Djakov int index;
406bcd61c0fSStephen Boyd
40750c6a503SStephen Boyd f = qcom_find_freq(f, rate);
408bcd61c0fSStephen Boyd if (!f)
409bcd61c0fSStephen Boyd return -EINVAL;
410bcd61c0fSStephen Boyd
4112f272e7bSGeorgi Djakov index = qcom_find_src_index(hw, parent_map, f->src);
4122f272e7bSGeorgi Djakov if (index < 0)
4132f272e7bSGeorgi Djakov return index;
4142f272e7bSGeorgi Djakov
41598d8a60eSStephen Boyd clk_flags = clk_hw_get_flags(hw);
416ac269395SStephen Boyd p = clk_hw_get_parent_by_index(hw, index);
417bcd61c0fSStephen Boyd if (clk_flags & CLK_SET_RATE_PARENT) {
418bcd61c0fSStephen Boyd rate = rate * f->pre_div;
419bcd61c0fSStephen Boyd if (f->n) {
420bcd61c0fSStephen Boyd u64 tmp = rate;
421bcd61c0fSStephen Boyd tmp = tmp * f->n;
422bcd61c0fSStephen Boyd do_div(tmp, f->m);
423bcd61c0fSStephen Boyd rate = tmp;
424bcd61c0fSStephen Boyd }
425bcd61c0fSStephen Boyd } else {
426ac269395SStephen Boyd rate = clk_hw_get_rate(p);
427bcd61c0fSStephen Boyd }
428ac269395SStephen Boyd req->best_parent_hw = p;
4290817b62cSBoris Brezillon req->best_parent_rate = rate;
4300817b62cSBoris Brezillon req->rate = f->freq;
431bcd61c0fSStephen Boyd
4320817b62cSBoris Brezillon return 0;
433bcd61c0fSStephen Boyd }
434bcd61c0fSStephen Boyd
clk_rcg_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)4350817b62cSBoris Brezillon static int clk_rcg_determine_rate(struct clk_hw *hw,
4360817b62cSBoris Brezillon struct clk_rate_request *req)
437bcd61c0fSStephen Boyd {
438bcd61c0fSStephen Boyd struct clk_rcg *rcg = to_clk_rcg(hw);
439bcd61c0fSStephen Boyd
4400817b62cSBoris Brezillon return _freq_tbl_determine_rate(hw, rcg->freq_tbl, req,
4410817b62cSBoris Brezillon rcg->s.parent_map);
442bcd61c0fSStephen Boyd }
443bcd61c0fSStephen Boyd
clk_dyn_rcg_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)4440817b62cSBoris Brezillon static int clk_dyn_rcg_determine_rate(struct clk_hw *hw,
4450817b62cSBoris Brezillon struct clk_rate_request *req)
446bcd61c0fSStephen Boyd {
447bcd61c0fSStephen Boyd struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
4482f272e7bSGeorgi Djakov u32 reg;
4492f272e7bSGeorgi Djakov int bank;
4502f272e7bSGeorgi Djakov struct src_sel *s;
4512f272e7bSGeorgi Djakov
4522f272e7bSGeorgi Djakov regmap_read(rcg->clkr.regmap, rcg->bank_reg, ®);
4532f272e7bSGeorgi Djakov bank = reg_to_bank(rcg, reg);
4542f272e7bSGeorgi Djakov s = &rcg->s[bank];
455bcd61c0fSStephen Boyd
4560817b62cSBoris Brezillon return _freq_tbl_determine_rate(hw, rcg->freq_tbl, req, s->parent_map);
457bcd61c0fSStephen Boyd }
458bcd61c0fSStephen Boyd
clk_rcg_bypass_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)4590817b62cSBoris Brezillon static int clk_rcg_bypass_determine_rate(struct clk_hw *hw,
4600817b62cSBoris Brezillon struct clk_rate_request *req)
461bcd61c0fSStephen Boyd {
462bcd61c0fSStephen Boyd struct clk_rcg *rcg = to_clk_rcg(hw);
463404c1ff6SStephen Boyd const struct freq_tbl *f = rcg->freq_tbl;
464ac269395SStephen Boyd struct clk_hw *p;
4652f272e7bSGeorgi Djakov int index = qcom_find_src_index(hw, rcg->s.parent_map, f->src);
466404c1ff6SStephen Boyd
467ac269395SStephen Boyd req->best_parent_hw = p = clk_hw_get_parent_by_index(hw, index);
468ac269395SStephen Boyd req->best_parent_rate = clk_hw_round_rate(p, req->rate);
4690817b62cSBoris Brezillon req->rate = req->best_parent_rate;
470404c1ff6SStephen Boyd
4710817b62cSBoris Brezillon return 0;
472404c1ff6SStephen Boyd }
473404c1ff6SStephen Boyd
__clk_rcg_set_rate(struct clk_rcg * rcg,const struct freq_tbl * f)474404c1ff6SStephen Boyd static int __clk_rcg_set_rate(struct clk_rcg *rcg, const struct freq_tbl *f)
475404c1ff6SStephen Boyd {
476bcd61c0fSStephen Boyd u32 ns, md, ctl;
477bcd61c0fSStephen Boyd struct mn *mn = &rcg->mn;
478bcd61c0fSStephen Boyd u32 mask = 0;
479bcd61c0fSStephen Boyd unsigned int reset_reg;
480bcd61c0fSStephen Boyd
481bcd61c0fSStephen Boyd if (rcg->mn.reset_in_cc)
482bcd61c0fSStephen Boyd reset_reg = rcg->clkr.enable_reg;
483bcd61c0fSStephen Boyd else
484bcd61c0fSStephen Boyd reset_reg = rcg->ns_reg;
485bcd61c0fSStephen Boyd
486bcd61c0fSStephen Boyd if (rcg->mn.width) {
487bcd61c0fSStephen Boyd mask = BIT(mn->mnctr_reset_bit);
488bcd61c0fSStephen Boyd regmap_update_bits(rcg->clkr.regmap, reset_reg, mask, mask);
489bcd61c0fSStephen Boyd
490bcd61c0fSStephen Boyd regmap_read(rcg->clkr.regmap, rcg->md_reg, &md);
491bcd61c0fSStephen Boyd md = mn_to_md(mn, f->m, f->n, md);
492bcd61c0fSStephen Boyd regmap_write(rcg->clkr.regmap, rcg->md_reg, md);
493bcd61c0fSStephen Boyd
494bcd61c0fSStephen Boyd regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
495bcd61c0fSStephen Boyd /* MN counter mode is in hw.enable_reg sometimes */
496bcd61c0fSStephen Boyd if (rcg->clkr.enable_reg != rcg->ns_reg) {
497bcd61c0fSStephen Boyd regmap_read(rcg->clkr.regmap, rcg->clkr.enable_reg, &ctl);
498bcd61c0fSStephen Boyd ctl = mn_to_reg(mn, f->m, f->n, ctl);
499bcd61c0fSStephen Boyd regmap_write(rcg->clkr.regmap, rcg->clkr.enable_reg, ctl);
500bcd61c0fSStephen Boyd } else {
501bcd61c0fSStephen Boyd ns = mn_to_reg(mn, f->m, f->n, ns);
502bcd61c0fSStephen Boyd }
503bcd61c0fSStephen Boyd ns = mn_to_ns(mn, f->m, f->n, ns);
504bcd61c0fSStephen Boyd } else {
505bcd61c0fSStephen Boyd regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
506bcd61c0fSStephen Boyd }
507bcd61c0fSStephen Boyd
508bcd61c0fSStephen Boyd ns = pre_div_to_ns(&rcg->p, f->pre_div - 1, ns);
509bcd61c0fSStephen Boyd regmap_write(rcg->clkr.regmap, rcg->ns_reg, ns);
510bcd61c0fSStephen Boyd
511bcd61c0fSStephen Boyd regmap_update_bits(rcg->clkr.regmap, reset_reg, mask, 0);
512bcd61c0fSStephen Boyd
513bcd61c0fSStephen Boyd return 0;
514bcd61c0fSStephen Boyd }
515bcd61c0fSStephen Boyd
clk_rcg_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)516404c1ff6SStephen Boyd static int clk_rcg_set_rate(struct clk_hw *hw, unsigned long rate,
517404c1ff6SStephen Boyd unsigned long parent_rate)
518404c1ff6SStephen Boyd {
519404c1ff6SStephen Boyd struct clk_rcg *rcg = to_clk_rcg(hw);
520404c1ff6SStephen Boyd const struct freq_tbl *f;
521404c1ff6SStephen Boyd
52250c6a503SStephen Boyd f = qcom_find_freq(rcg->freq_tbl, rate);
523404c1ff6SStephen Boyd if (!f)
524404c1ff6SStephen Boyd return -EINVAL;
525404c1ff6SStephen Boyd
526404c1ff6SStephen Boyd return __clk_rcg_set_rate(rcg, f);
527404c1ff6SStephen Boyd }
528404c1ff6SStephen Boyd
clk_rcg_set_floor_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)529*33958ad3SAnsuel Smith static int clk_rcg_set_floor_rate(struct clk_hw *hw, unsigned long rate,
530*33958ad3SAnsuel Smith unsigned long parent_rate)
531*33958ad3SAnsuel Smith {
532*33958ad3SAnsuel Smith struct clk_rcg *rcg = to_clk_rcg(hw);
533*33958ad3SAnsuel Smith const struct freq_tbl *f;
534*33958ad3SAnsuel Smith
535*33958ad3SAnsuel Smith f = qcom_find_freq_floor(rcg->freq_tbl, rate);
536*33958ad3SAnsuel Smith if (!f)
537*33958ad3SAnsuel Smith return -EINVAL;
538*33958ad3SAnsuel Smith
539*33958ad3SAnsuel Smith return __clk_rcg_set_rate(rcg, f);
540*33958ad3SAnsuel Smith }
541*33958ad3SAnsuel Smith
clk_rcg_bypass_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)542404c1ff6SStephen Boyd static int clk_rcg_bypass_set_rate(struct clk_hw *hw, unsigned long rate,
543404c1ff6SStephen Boyd unsigned long parent_rate)
544404c1ff6SStephen Boyd {
545404c1ff6SStephen Boyd struct clk_rcg *rcg = to_clk_rcg(hw);
546404c1ff6SStephen Boyd
547404c1ff6SStephen Boyd return __clk_rcg_set_rate(rcg, rcg->freq_tbl);
548404c1ff6SStephen Boyd }
549404c1ff6SStephen Boyd
clk_rcg_bypass2_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)550d8aa2beeSArchit Taneja static int clk_rcg_bypass2_determine_rate(struct clk_hw *hw,
551d8aa2beeSArchit Taneja struct clk_rate_request *req)
552d8aa2beeSArchit Taneja {
553d8aa2beeSArchit Taneja struct clk_hw *p;
554d8aa2beeSArchit Taneja
555d8aa2beeSArchit Taneja p = req->best_parent_hw;
556d8aa2beeSArchit Taneja req->best_parent_rate = clk_hw_round_rate(p, req->rate);
557d8aa2beeSArchit Taneja req->rate = req->best_parent_rate;
558d8aa2beeSArchit Taneja
559d8aa2beeSArchit Taneja return 0;
560d8aa2beeSArchit Taneja }
561d8aa2beeSArchit Taneja
clk_rcg_bypass2_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)562d8aa2beeSArchit Taneja static int clk_rcg_bypass2_set_rate(struct clk_hw *hw, unsigned long rate,
563d8aa2beeSArchit Taneja unsigned long parent_rate)
564d8aa2beeSArchit Taneja {
565d8aa2beeSArchit Taneja struct clk_rcg *rcg = to_clk_rcg(hw);
566d8aa2beeSArchit Taneja struct freq_tbl f = { 0 };
567d8aa2beeSArchit Taneja u32 ns, src;
568d8aa2beeSArchit Taneja int i, ret, num_parents = clk_hw_get_num_parents(hw);
569d8aa2beeSArchit Taneja
570d8aa2beeSArchit Taneja ret = regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
571d8aa2beeSArchit Taneja if (ret)
572d8aa2beeSArchit Taneja return ret;
573d8aa2beeSArchit Taneja
574d8aa2beeSArchit Taneja src = ns_to_src(&rcg->s, ns);
575d8aa2beeSArchit Taneja f.pre_div = ns_to_pre_div(&rcg->p, ns) + 1;
576d8aa2beeSArchit Taneja
577d8aa2beeSArchit Taneja for (i = 0; i < num_parents; i++) {
578d8aa2beeSArchit Taneja if (src == rcg->s.parent_map[i].cfg) {
579d8aa2beeSArchit Taneja f.src = rcg->s.parent_map[i].src;
580d8aa2beeSArchit Taneja return __clk_rcg_set_rate(rcg, &f);
581d8aa2beeSArchit Taneja }
582d8aa2beeSArchit Taneja }
583d8aa2beeSArchit Taneja
584d8aa2beeSArchit Taneja return -EINVAL;
585d8aa2beeSArchit Taneja }
586d8aa2beeSArchit Taneja
clk_rcg_bypass2_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)587d8aa2beeSArchit Taneja static int clk_rcg_bypass2_set_rate_and_parent(struct clk_hw *hw,
588d8aa2beeSArchit Taneja unsigned long rate, unsigned long parent_rate, u8 index)
589d8aa2beeSArchit Taneja {
590d8aa2beeSArchit Taneja /* Read the hardware to determine parent during set_rate */
591d8aa2beeSArchit Taneja return clk_rcg_bypass2_set_rate(hw, rate, parent_rate);
592d8aa2beeSArchit Taneja }
593d8aa2beeSArchit Taneja
594d8aa2beeSArchit Taneja struct frac_entry {
595d8aa2beeSArchit Taneja int num;
596d8aa2beeSArchit Taneja int den;
597d8aa2beeSArchit Taneja };
598d8aa2beeSArchit Taneja
599d8aa2beeSArchit Taneja static const struct frac_entry pixel_table[] = {
600d8aa2beeSArchit Taneja { 1, 2 },
601d8aa2beeSArchit Taneja { 1, 3 },
602d8aa2beeSArchit Taneja { 3, 16 },
603d8aa2beeSArchit Taneja { }
604d8aa2beeSArchit Taneja };
605d8aa2beeSArchit Taneja
clk_rcg_pixel_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)606d8aa2beeSArchit Taneja static int clk_rcg_pixel_determine_rate(struct clk_hw *hw,
607d8aa2beeSArchit Taneja struct clk_rate_request *req)
608d8aa2beeSArchit Taneja {
609d8aa2beeSArchit Taneja int delta = 100000;
610d8aa2beeSArchit Taneja const struct frac_entry *frac = pixel_table;
611d8aa2beeSArchit Taneja unsigned long request, src_rate;
612d8aa2beeSArchit Taneja
613d8aa2beeSArchit Taneja for (; frac->num; frac++) {
614d8aa2beeSArchit Taneja request = (req->rate * frac->den) / frac->num;
615d8aa2beeSArchit Taneja
616d8aa2beeSArchit Taneja src_rate = clk_hw_round_rate(req->best_parent_hw, request);
617d8aa2beeSArchit Taneja
618d8aa2beeSArchit Taneja if ((src_rate < (request - delta)) ||
619d8aa2beeSArchit Taneja (src_rate > (request + delta)))
620d8aa2beeSArchit Taneja continue;
621d8aa2beeSArchit Taneja
622d8aa2beeSArchit Taneja req->best_parent_rate = src_rate;
623d8aa2beeSArchit Taneja req->rate = (src_rate * frac->num) / frac->den;
624d8aa2beeSArchit Taneja return 0;
625d8aa2beeSArchit Taneja }
626d8aa2beeSArchit Taneja
627d8aa2beeSArchit Taneja return -EINVAL;
628d8aa2beeSArchit Taneja }
629d8aa2beeSArchit Taneja
clk_rcg_pixel_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)630d8aa2beeSArchit Taneja static int clk_rcg_pixel_set_rate(struct clk_hw *hw, unsigned long rate,
631d8aa2beeSArchit Taneja unsigned long parent_rate)
632d8aa2beeSArchit Taneja {
633d8aa2beeSArchit Taneja struct clk_rcg *rcg = to_clk_rcg(hw);
634d8aa2beeSArchit Taneja int delta = 100000;
635d8aa2beeSArchit Taneja const struct frac_entry *frac = pixel_table;
636d8aa2beeSArchit Taneja unsigned long request;
637d8aa2beeSArchit Taneja struct freq_tbl f = { 0 };
638d8aa2beeSArchit Taneja u32 ns, src;
639d8aa2beeSArchit Taneja int i, ret, num_parents = clk_hw_get_num_parents(hw);
640d8aa2beeSArchit Taneja
641d8aa2beeSArchit Taneja ret = regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
642d8aa2beeSArchit Taneja if (ret)
643d8aa2beeSArchit Taneja return ret;
644d8aa2beeSArchit Taneja
645d8aa2beeSArchit Taneja src = ns_to_src(&rcg->s, ns);
646d8aa2beeSArchit Taneja
647d8aa2beeSArchit Taneja for (i = 0; i < num_parents; i++) {
648d8aa2beeSArchit Taneja if (src == rcg->s.parent_map[i].cfg) {
649d8aa2beeSArchit Taneja f.src = rcg->s.parent_map[i].src;
650d8aa2beeSArchit Taneja break;
651d8aa2beeSArchit Taneja }
652d8aa2beeSArchit Taneja }
653d8aa2beeSArchit Taneja
654811a498eSArchit Taneja /* bypass the pre divider */
655811a498eSArchit Taneja f.pre_div = 1;
656811a498eSArchit Taneja
657d8aa2beeSArchit Taneja /* let us find appropriate m/n values for this */
658d8aa2beeSArchit Taneja for (; frac->num; frac++) {
659d8aa2beeSArchit Taneja request = (rate * frac->den) / frac->num;
660d8aa2beeSArchit Taneja
661d8aa2beeSArchit Taneja if ((parent_rate < (request - delta)) ||
662d8aa2beeSArchit Taneja (parent_rate > (request + delta)))
663d8aa2beeSArchit Taneja continue;
664d8aa2beeSArchit Taneja
665d8aa2beeSArchit Taneja f.m = frac->num;
666d8aa2beeSArchit Taneja f.n = frac->den;
667d8aa2beeSArchit Taneja
668d8aa2beeSArchit Taneja return __clk_rcg_set_rate(rcg, &f);
669d8aa2beeSArchit Taneja }
670d8aa2beeSArchit Taneja
671d8aa2beeSArchit Taneja return -EINVAL;
672d8aa2beeSArchit Taneja }
673d8aa2beeSArchit Taneja
clk_rcg_pixel_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)674d8aa2beeSArchit Taneja static int clk_rcg_pixel_set_rate_and_parent(struct clk_hw *hw,
675d8aa2beeSArchit Taneja unsigned long rate, unsigned long parent_rate, u8 index)
676d8aa2beeSArchit Taneja {
677d8aa2beeSArchit Taneja return clk_rcg_pixel_set_rate(hw, rate, parent_rate);
678d8aa2beeSArchit Taneja }
679d8aa2beeSArchit Taneja
clk_rcg_esc_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)680d8aa2beeSArchit Taneja static int clk_rcg_esc_determine_rate(struct clk_hw *hw,
681d8aa2beeSArchit Taneja struct clk_rate_request *req)
682d8aa2beeSArchit Taneja {
683d8aa2beeSArchit Taneja struct clk_rcg *rcg = to_clk_rcg(hw);
684d8aa2beeSArchit Taneja int pre_div_max = BIT(rcg->p.pre_div_width);
685d8aa2beeSArchit Taneja int div;
686d8aa2beeSArchit Taneja unsigned long src_rate;
687d8aa2beeSArchit Taneja
688d8aa2beeSArchit Taneja if (req->rate == 0)
689d8aa2beeSArchit Taneja return -EINVAL;
690d8aa2beeSArchit Taneja
691d8aa2beeSArchit Taneja src_rate = clk_hw_get_rate(req->best_parent_hw);
692d8aa2beeSArchit Taneja
693d8aa2beeSArchit Taneja div = src_rate / req->rate;
694d8aa2beeSArchit Taneja
695d8aa2beeSArchit Taneja if (div >= 1 && div <= pre_div_max) {
696d8aa2beeSArchit Taneja req->best_parent_rate = src_rate;
697d8aa2beeSArchit Taneja req->rate = src_rate / div;
698d8aa2beeSArchit Taneja return 0;
699d8aa2beeSArchit Taneja }
700d8aa2beeSArchit Taneja
701d8aa2beeSArchit Taneja return -EINVAL;
702d8aa2beeSArchit Taneja }
703d8aa2beeSArchit Taneja
clk_rcg_esc_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)704d8aa2beeSArchit Taneja static int clk_rcg_esc_set_rate(struct clk_hw *hw, unsigned long rate,
705d8aa2beeSArchit Taneja unsigned long parent_rate)
706d8aa2beeSArchit Taneja {
707d8aa2beeSArchit Taneja struct clk_rcg *rcg = to_clk_rcg(hw);
708d8aa2beeSArchit Taneja struct freq_tbl f = { 0 };
709d8aa2beeSArchit Taneja int pre_div_max = BIT(rcg->p.pre_div_width);
710d8aa2beeSArchit Taneja int div;
711d8aa2beeSArchit Taneja u32 ns;
712d8aa2beeSArchit Taneja int i, ret, num_parents = clk_hw_get_num_parents(hw);
713d8aa2beeSArchit Taneja
714d8aa2beeSArchit Taneja if (rate == 0)
715d8aa2beeSArchit Taneja return -EINVAL;
716d8aa2beeSArchit Taneja
717d8aa2beeSArchit Taneja ret = regmap_read(rcg->clkr.regmap, rcg->ns_reg, &ns);
718d8aa2beeSArchit Taneja if (ret)
719d8aa2beeSArchit Taneja return ret;
720d8aa2beeSArchit Taneja
721d8aa2beeSArchit Taneja ns = ns_to_src(&rcg->s, ns);
722d8aa2beeSArchit Taneja
723d8aa2beeSArchit Taneja for (i = 0; i < num_parents; i++) {
724d8aa2beeSArchit Taneja if (ns == rcg->s.parent_map[i].cfg) {
725d8aa2beeSArchit Taneja f.src = rcg->s.parent_map[i].src;
726d8aa2beeSArchit Taneja break;
727d8aa2beeSArchit Taneja }
728d8aa2beeSArchit Taneja }
729d8aa2beeSArchit Taneja
730d8aa2beeSArchit Taneja div = parent_rate / rate;
731d8aa2beeSArchit Taneja
732d8aa2beeSArchit Taneja if (div >= 1 && div <= pre_div_max) {
733d8aa2beeSArchit Taneja f.pre_div = div;
734d8aa2beeSArchit Taneja return __clk_rcg_set_rate(rcg, &f);
735d8aa2beeSArchit Taneja }
736d8aa2beeSArchit Taneja
737d8aa2beeSArchit Taneja return -EINVAL;
738d8aa2beeSArchit Taneja }
739d8aa2beeSArchit Taneja
clk_rcg_esc_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)740d8aa2beeSArchit Taneja static int clk_rcg_esc_set_rate_and_parent(struct clk_hw *hw,
741d8aa2beeSArchit Taneja unsigned long rate, unsigned long parent_rate, u8 index)
742d8aa2beeSArchit Taneja {
743d8aa2beeSArchit Taneja return clk_rcg_esc_set_rate(hw, rate, parent_rate);
744d8aa2beeSArchit Taneja }
745d8aa2beeSArchit Taneja
7469d3745d4SStephen Boyd /*
7479d3745d4SStephen Boyd * This type of clock has a glitch-free mux that switches between the output of
7489d3745d4SStephen Boyd * the M/N counter and an always on clock source (XO). When clk_set_rate() is
7499d3745d4SStephen Boyd * called we need to make sure that we don't switch to the M/N counter if it
7509d3745d4SStephen Boyd * isn't clocking because the mux will get stuck and the clock will stop
7519d3745d4SStephen Boyd * outputting a clock. This can happen if the framework isn't aware that this
7529d3745d4SStephen Boyd * clock is on and so clk_set_rate() doesn't turn on the new parent. To fix
7539d3745d4SStephen Boyd * this we switch the mux in the enable/disable ops and reprogram the M/N
7549d3745d4SStephen Boyd * counter in the set_rate op. We also make sure to switch away from the M/N
7559d3745d4SStephen Boyd * counter in set_rate if software thinks the clock is off.
7569d3745d4SStephen Boyd */
clk_rcg_lcc_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)7579d3745d4SStephen Boyd static int clk_rcg_lcc_set_rate(struct clk_hw *hw, unsigned long rate,
7589d3745d4SStephen Boyd unsigned long parent_rate)
7599d3745d4SStephen Boyd {
7609d3745d4SStephen Boyd struct clk_rcg *rcg = to_clk_rcg(hw);
7619d3745d4SStephen Boyd const struct freq_tbl *f;
7629d3745d4SStephen Boyd int ret;
7639d3745d4SStephen Boyd u32 gfm = BIT(10);
7649d3745d4SStephen Boyd
7659d3745d4SStephen Boyd f = qcom_find_freq(rcg->freq_tbl, rate);
7669d3745d4SStephen Boyd if (!f)
7679d3745d4SStephen Boyd return -EINVAL;
7689d3745d4SStephen Boyd
7699d3745d4SStephen Boyd /* Switch to XO to avoid glitches */
7709d3745d4SStephen Boyd regmap_update_bits(rcg->clkr.regmap, rcg->ns_reg, gfm, 0);
7719d3745d4SStephen Boyd ret = __clk_rcg_set_rate(rcg, f);
7729d3745d4SStephen Boyd /* Switch back to M/N if it's clocking */
7739d3745d4SStephen Boyd if (__clk_is_enabled(hw->clk))
7749d3745d4SStephen Boyd regmap_update_bits(rcg->clkr.regmap, rcg->ns_reg, gfm, gfm);
7759d3745d4SStephen Boyd
7769d3745d4SStephen Boyd return ret;
7779d3745d4SStephen Boyd }
7789d3745d4SStephen Boyd
clk_rcg_lcc_enable(struct clk_hw * hw)7799d3745d4SStephen Boyd static int clk_rcg_lcc_enable(struct clk_hw *hw)
7809d3745d4SStephen Boyd {
7819d3745d4SStephen Boyd struct clk_rcg *rcg = to_clk_rcg(hw);
7829d3745d4SStephen Boyd u32 gfm = BIT(10);
7839d3745d4SStephen Boyd
7849d3745d4SStephen Boyd /* Use M/N */
7859d3745d4SStephen Boyd return regmap_update_bits(rcg->clkr.regmap, rcg->ns_reg, gfm, gfm);
7869d3745d4SStephen Boyd }
7879d3745d4SStephen Boyd
clk_rcg_lcc_disable(struct clk_hw * hw)7889d3745d4SStephen Boyd static void clk_rcg_lcc_disable(struct clk_hw *hw)
7899d3745d4SStephen Boyd {
7909d3745d4SStephen Boyd struct clk_rcg *rcg = to_clk_rcg(hw);
7919d3745d4SStephen Boyd u32 gfm = BIT(10);
7929d3745d4SStephen Boyd
7939d3745d4SStephen Boyd /* Use XO */
7949d3745d4SStephen Boyd regmap_update_bits(rcg->clkr.regmap, rcg->ns_reg, gfm, 0);
7959d3745d4SStephen Boyd }
7969d3745d4SStephen Boyd
__clk_dyn_rcg_set_rate(struct clk_hw * hw,unsigned long rate)797bcd61c0fSStephen Boyd static int __clk_dyn_rcg_set_rate(struct clk_hw *hw, unsigned long rate)
798bcd61c0fSStephen Boyd {
799bcd61c0fSStephen Boyd struct clk_dyn_rcg *rcg = to_clk_dyn_rcg(hw);
800bcd61c0fSStephen Boyd const struct freq_tbl *f;
801bcd61c0fSStephen Boyd
80250c6a503SStephen Boyd f = qcom_find_freq(rcg->freq_tbl, rate);
803bcd61c0fSStephen Boyd if (!f)
804bcd61c0fSStephen Boyd return -EINVAL;
805bcd61c0fSStephen Boyd
806fae507afSGeorgi Djakov return configure_bank(rcg, f);
807bcd61c0fSStephen Boyd }
808bcd61c0fSStephen Boyd
clk_dyn_rcg_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)809bcd61c0fSStephen Boyd static int clk_dyn_rcg_set_rate(struct clk_hw *hw, unsigned long rate,
810bcd61c0fSStephen Boyd unsigned long parent_rate)
811bcd61c0fSStephen Boyd {
812bcd61c0fSStephen Boyd return __clk_dyn_rcg_set_rate(hw, rate);
813bcd61c0fSStephen Boyd }
814bcd61c0fSStephen Boyd
clk_dyn_rcg_set_rate_and_parent(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate,u8 index)815bcd61c0fSStephen Boyd static int clk_dyn_rcg_set_rate_and_parent(struct clk_hw *hw,
816bcd61c0fSStephen Boyd unsigned long rate, unsigned long parent_rate, u8 index)
817bcd61c0fSStephen Boyd {
818bcd61c0fSStephen Boyd return __clk_dyn_rcg_set_rate(hw, rate);
819bcd61c0fSStephen Boyd }
820bcd61c0fSStephen Boyd
821bcd61c0fSStephen Boyd const struct clk_ops clk_rcg_ops = {
822bcd61c0fSStephen Boyd .enable = clk_enable_regmap,
823bcd61c0fSStephen Boyd .disable = clk_disable_regmap,
824bcd61c0fSStephen Boyd .get_parent = clk_rcg_get_parent,
825bcd61c0fSStephen Boyd .set_parent = clk_rcg_set_parent,
826bcd61c0fSStephen Boyd .recalc_rate = clk_rcg_recalc_rate,
827bcd61c0fSStephen Boyd .determine_rate = clk_rcg_determine_rate,
828bcd61c0fSStephen Boyd .set_rate = clk_rcg_set_rate,
829bcd61c0fSStephen Boyd };
830bcd61c0fSStephen Boyd EXPORT_SYMBOL_GPL(clk_rcg_ops);
831bcd61c0fSStephen Boyd
832*33958ad3SAnsuel Smith const struct clk_ops clk_rcg_floor_ops = {
833*33958ad3SAnsuel Smith .enable = clk_enable_regmap,
834*33958ad3SAnsuel Smith .disable = clk_disable_regmap,
835*33958ad3SAnsuel Smith .get_parent = clk_rcg_get_parent,
836*33958ad3SAnsuel Smith .set_parent = clk_rcg_set_parent,
837*33958ad3SAnsuel Smith .recalc_rate = clk_rcg_recalc_rate,
838*33958ad3SAnsuel Smith .determine_rate = clk_rcg_determine_rate,
839*33958ad3SAnsuel Smith .set_rate = clk_rcg_set_floor_rate,
840*33958ad3SAnsuel Smith };
841*33958ad3SAnsuel Smith EXPORT_SYMBOL_GPL(clk_rcg_floor_ops);
842*33958ad3SAnsuel Smith
843404c1ff6SStephen Boyd const struct clk_ops clk_rcg_bypass_ops = {
844404c1ff6SStephen Boyd .enable = clk_enable_regmap,
845404c1ff6SStephen Boyd .disable = clk_disable_regmap,
846404c1ff6SStephen Boyd .get_parent = clk_rcg_get_parent,
847404c1ff6SStephen Boyd .set_parent = clk_rcg_set_parent,
848404c1ff6SStephen Boyd .recalc_rate = clk_rcg_recalc_rate,
849404c1ff6SStephen Boyd .determine_rate = clk_rcg_bypass_determine_rate,
850404c1ff6SStephen Boyd .set_rate = clk_rcg_bypass_set_rate,
851404c1ff6SStephen Boyd };
852404c1ff6SStephen Boyd EXPORT_SYMBOL_GPL(clk_rcg_bypass_ops);
853404c1ff6SStephen Boyd
854d8aa2beeSArchit Taneja const struct clk_ops clk_rcg_bypass2_ops = {
855d8aa2beeSArchit Taneja .enable = clk_enable_regmap,
856d8aa2beeSArchit Taneja .disable = clk_disable_regmap,
857d8aa2beeSArchit Taneja .get_parent = clk_rcg_get_parent,
858d8aa2beeSArchit Taneja .set_parent = clk_rcg_set_parent,
859d8aa2beeSArchit Taneja .recalc_rate = clk_rcg_recalc_rate,
860d8aa2beeSArchit Taneja .determine_rate = clk_rcg_bypass2_determine_rate,
861d8aa2beeSArchit Taneja .set_rate = clk_rcg_bypass2_set_rate,
862d8aa2beeSArchit Taneja .set_rate_and_parent = clk_rcg_bypass2_set_rate_and_parent,
863d8aa2beeSArchit Taneja };
864d8aa2beeSArchit Taneja EXPORT_SYMBOL_GPL(clk_rcg_bypass2_ops);
865d8aa2beeSArchit Taneja
866d8aa2beeSArchit Taneja const struct clk_ops clk_rcg_pixel_ops = {
867d8aa2beeSArchit Taneja .enable = clk_enable_regmap,
868d8aa2beeSArchit Taneja .disable = clk_disable_regmap,
869d8aa2beeSArchit Taneja .get_parent = clk_rcg_get_parent,
870d8aa2beeSArchit Taneja .set_parent = clk_rcg_set_parent,
871d8aa2beeSArchit Taneja .recalc_rate = clk_rcg_recalc_rate,
872d8aa2beeSArchit Taneja .determine_rate = clk_rcg_pixel_determine_rate,
873d8aa2beeSArchit Taneja .set_rate = clk_rcg_pixel_set_rate,
874d8aa2beeSArchit Taneja .set_rate_and_parent = clk_rcg_pixel_set_rate_and_parent,
875d8aa2beeSArchit Taneja };
876d8aa2beeSArchit Taneja EXPORT_SYMBOL_GPL(clk_rcg_pixel_ops);
877d8aa2beeSArchit Taneja
878d8aa2beeSArchit Taneja const struct clk_ops clk_rcg_esc_ops = {
879d8aa2beeSArchit Taneja .enable = clk_enable_regmap,
880d8aa2beeSArchit Taneja .disable = clk_disable_regmap,
881d8aa2beeSArchit Taneja .get_parent = clk_rcg_get_parent,
882d8aa2beeSArchit Taneja .set_parent = clk_rcg_set_parent,
883d8aa2beeSArchit Taneja .recalc_rate = clk_rcg_recalc_rate,
884d8aa2beeSArchit Taneja .determine_rate = clk_rcg_esc_determine_rate,
885d8aa2beeSArchit Taneja .set_rate = clk_rcg_esc_set_rate,
886d8aa2beeSArchit Taneja .set_rate_and_parent = clk_rcg_esc_set_rate_and_parent,
887d8aa2beeSArchit Taneja };
888d8aa2beeSArchit Taneja EXPORT_SYMBOL_GPL(clk_rcg_esc_ops);
889d8aa2beeSArchit Taneja
8909d3745d4SStephen Boyd const struct clk_ops clk_rcg_lcc_ops = {
8919d3745d4SStephen Boyd .enable = clk_rcg_lcc_enable,
8929d3745d4SStephen Boyd .disable = clk_rcg_lcc_disable,
8939d3745d4SStephen Boyd .get_parent = clk_rcg_get_parent,
8949d3745d4SStephen Boyd .set_parent = clk_rcg_set_parent,
8959d3745d4SStephen Boyd .recalc_rate = clk_rcg_recalc_rate,
8969d3745d4SStephen Boyd .determine_rate = clk_rcg_determine_rate,
8979d3745d4SStephen Boyd .set_rate = clk_rcg_lcc_set_rate,
8989d3745d4SStephen Boyd };
8999d3745d4SStephen Boyd EXPORT_SYMBOL_GPL(clk_rcg_lcc_ops);
9009d3745d4SStephen Boyd
901bcd61c0fSStephen Boyd const struct clk_ops clk_dyn_rcg_ops = {
902bcd61c0fSStephen Boyd .enable = clk_enable_regmap,
903bcd61c0fSStephen Boyd .is_enabled = clk_is_enabled_regmap,
904bcd61c0fSStephen Boyd .disable = clk_disable_regmap,
905bcd61c0fSStephen Boyd .get_parent = clk_dyn_rcg_get_parent,
906bcd61c0fSStephen Boyd .set_parent = clk_dyn_rcg_set_parent,
907bcd61c0fSStephen Boyd .recalc_rate = clk_dyn_rcg_recalc_rate,
908bcd61c0fSStephen Boyd .determine_rate = clk_dyn_rcg_determine_rate,
909bcd61c0fSStephen Boyd .set_rate = clk_dyn_rcg_set_rate,
910bcd61c0fSStephen Boyd .set_rate_and_parent = clk_dyn_rcg_set_rate_and_parent,
911bcd61c0fSStephen Boyd };
912bcd61c0fSStephen Boyd EXPORT_SYMBOL_GPL(clk_dyn_rcg_ops);
913