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
7 #include "ccu.h"
8 #include "ccu_gate.h"
9 #include "ccu_nk.h"
10
11 struct _ccu_nk
12 {
13 unsigned long n, min_n, max_n;
14 unsigned long k, min_k, max_k;
15 };
16
ccu_nk_find_best(unsigned long parent,unsigned long rate,struct _ccu_nk * nk)17 static void ccu_nk_find_best(unsigned long parent, unsigned long rate,
18 struct _ccu_nk *nk)
19 {
20 unsigned long best_rate = 0;
21 unsigned int best_k = 0, best_n = 0;
22 unsigned int _k, _n;
23
24 for (_k = nk->min_k; _k <= nk->max_k; _k++)
25 {
26 for (_n = nk->min_n; _n <= nk->max_n; _n++)
27 {
28 unsigned long tmp_rate = parent * _n * _k;
29
30 if (tmp_rate > rate)
31 {
32 continue;
33 }
34
35 if ((rate - tmp_rate) < (rate - best_rate))
36 {
37 best_rate = tmp_rate;
38 best_k = _k;
39 best_n = _n;
40 }
41 }
42 }
43
44 nk->k = best_k;
45 nk->n = best_n;
46 }
47
ccu_nk_disable(struct clk_hw * hw)48 static void ccu_nk_disable(struct clk_hw *hw)
49 {
50 struct ccu_nk *nk = hw_to_ccu_nk(hw);
51
52 return ccu_gate_helper_disable(&nk->common, nk->enable);
53 }
54
ccu_nk_enable(struct clk_hw * hw)55 static int ccu_nk_enable(struct clk_hw *hw)
56 {
57 struct ccu_nk *nk = hw_to_ccu_nk(hw);
58
59 return ccu_gate_helper_enable(&nk->common, nk->enable);
60 }
61
ccu_nk_is_enabled(struct clk_hw * hw)62 static int ccu_nk_is_enabled(struct clk_hw *hw)
63 {
64 struct ccu_nk *nk = hw_to_ccu_nk(hw);
65
66 return ccu_gate_helper_is_enabled(&nk->common, nk->enable);
67 }
68
ccu_nk_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)69 static unsigned long ccu_nk_recalc_rate(struct clk_hw *hw,
70 unsigned long parent_rate)
71 {
72 struct ccu_nk *nk = hw_to_ccu_nk(hw);
73 unsigned long rate, n, k;
74 u32 reg;
75
76 reg = readl(nk->common.base + nk->common.reg);
77
78 n = reg >> nk->n.shift;
79 n &= (1 << nk->n.width) - 1;
80 n += nk->n.offset;
81 if (!n)
82 {
83 n++;
84 }
85
86 k = reg >> nk->k.shift;
87 k &= (1 << nk->k.width) - 1;
88 k += nk->k.offset;
89 if (!k)
90 {
91 k++;
92 }
93
94 rate = parent_rate * n * k;
95 if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
96 {
97 rate /= nk->fixed_post_div;
98 }
99
100 return rate;
101 }
102
ccu_nk_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate)103 static long ccu_nk_round_rate(struct clk_hw *hw, unsigned long rate,
104 unsigned long *parent_rate)
105 {
106 struct ccu_nk *nk = hw_to_ccu_nk(hw);
107 struct _ccu_nk _nk;
108
109 if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
110 {
111 rate *= nk->fixed_post_div;
112 }
113
114 _nk.min_n = nk->n.min ? : 1;
115 _nk.max_n = nk->n.max ? : 1 << nk->n.width;
116 _nk.min_k = nk->k.min ? : 1;
117 _nk.max_k = nk->k.max ? : 1 << nk->k.width;
118
119 ccu_nk_find_best(*parent_rate, rate, &_nk);
120 rate = *parent_rate * _nk.n * _nk.k;
121
122 if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
123 {
124 rate = rate / nk->fixed_post_div;
125 }
126
127 return rate;
128 }
129
ccu_nk_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)130 static int ccu_nk_set_rate(struct clk_hw *hw, unsigned long rate,
131 unsigned long parent_rate)
132 {
133 struct ccu_nk *nk = hw_to_ccu_nk(hw);
134 struct _ccu_nk _nk;
135 u32 reg;
136 u32 __cspr;
137
138 if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
139 {
140 rate = rate * nk->fixed_post_div;
141 }
142
143 _nk.min_n = nk->n.min ? : 1;
144 _nk.max_n = nk->n.max ? : 1 << nk->n.width;
145 _nk.min_k = nk->k.min ? : 1;
146 _nk.max_k = nk->k.max ? : 1 << nk->k.width;
147
148 ccu_nk_find_best(parent_rate, rate, &_nk);
149
150 __cspr = hal_spin_lock_irqsave(&nk->common.lock);
151
152 reg = readl(nk->common.base + nk->common.reg);
153 reg &= ~GENMASK(nk->n.width + nk->n.shift - 1, nk->n.shift);
154 reg &= ~GENMASK(nk->k.width + nk->k.shift - 1, nk->k.shift);
155
156 reg |= (_nk.k - nk->k.offset) << nk->k.shift;
157 reg |= (_nk.n - nk->n.offset) << nk->n.shift;
158 writel(reg, nk->common.base + nk->common.reg);
159
160 hal_spin_unlock_irqrestore(&nk->common.lock, __cspr);
161
162 ccu_helper_wait_for_lock(&nk->common, nk->lock);
163
164 return 0;
165 }
166
167 const struct clk_ops ccu_nk_ops =
168 {
169 .disable = ccu_nk_disable,
170 .enable = ccu_nk_enable,
171 .is_enabled = ccu_nk_is_enabled,
172
173 .recalc_rate = ccu_nk_recalc_rate,
174 .round_rate = ccu_nk_round_rate,
175 .set_rate = ccu_nk_set_rate,
176 };
177