]>
Commit | Line | Data |
---|---|---|
6174a1e2 MR |
1 | /* |
2 | * Copyright (C) 2016 Maxime Ripard | |
3 | * Maxime Ripard <maxime.ripard@free-electrons.com> | |
4 | * | |
5 | * This program is free software; you can redistribute it and/or | |
6 | * modify it under the terms of the GNU General Public License as | |
7 | * published by the Free Software Foundation; either version 2 of | |
8 | * the License, or (at your option) any later version. | |
9 | */ | |
10 | ||
11 | #include <linux/clk-provider.h> | |
6174a1e2 MR |
12 | |
13 | #include "ccu_frac.h" | |
14 | #include "ccu_gate.h" | |
15 | #include "ccu_nm.h" | |
16 | ||
ee28648c | 17 | struct _ccu_nm { |
6e0d50da MR |
18 | unsigned long n, min_n, max_n; |
19 | unsigned long m, min_m, max_m; | |
ee28648c MR |
20 | }; |
21 | ||
22 | static void ccu_nm_find_best(unsigned long parent, unsigned long rate, | |
23 | struct _ccu_nm *nm) | |
24 | { | |
25 | unsigned long best_rate = 0; | |
26 | unsigned long best_n = 0, best_m = 0; | |
27 | unsigned long _n, _m; | |
28 | ||
6e0d50da MR |
29 | for (_n = nm->min_n; _n <= nm->max_n; _n++) { |
30 | for (_m = nm->min_m; _m <= nm->max_m; _m++) { | |
ee28648c MR |
31 | unsigned long tmp_rate = parent * _n / _m; |
32 | ||
33 | if (tmp_rate > rate) | |
34 | continue; | |
35 | ||
36 | if ((rate - tmp_rate) < (rate - best_rate)) { | |
37 | best_rate = tmp_rate; | |
38 | best_n = _n; | |
39 | best_m = _m; | |
40 | } | |
41 | } | |
42 | } | |
43 | ||
44 | nm->n = best_n; | |
45 | nm->m = best_m; | |
46 | } | |
47 | ||
6174a1e2 MR |
48 | static void ccu_nm_disable(struct clk_hw *hw) |
49 | { | |
50 | struct ccu_nm *nm = hw_to_ccu_nm(hw); | |
51 | ||
52 | return ccu_gate_helper_disable(&nm->common, nm->enable); | |
53 | } | |
54 | ||
55 | static int ccu_nm_enable(struct clk_hw *hw) | |
56 | { | |
57 | struct ccu_nm *nm = hw_to_ccu_nm(hw); | |
58 | ||
59 | return ccu_gate_helper_enable(&nm->common, nm->enable); | |
60 | } | |
61 | ||
62 | static int ccu_nm_is_enabled(struct clk_hw *hw) | |
63 | { | |
64 | struct ccu_nm *nm = hw_to_ccu_nm(hw); | |
65 | ||
66 | return ccu_gate_helper_is_enabled(&nm->common, nm->enable); | |
67 | } | |
68 | ||
69 | static unsigned long ccu_nm_recalc_rate(struct clk_hw *hw, | |
70 | unsigned long parent_rate) | |
71 | { | |
72 | struct ccu_nm *nm = hw_to_ccu_nm(hw); | |
73 | unsigned long n, m; | |
74 | u32 reg; | |
75 | ||
76 | if (ccu_frac_helper_is_enabled(&nm->common, &nm->frac)) | |
77 | return ccu_frac_helper_read_rate(&nm->common, &nm->frac); | |
78 | ||
79 | reg = readl(nm->common.base + nm->common.reg); | |
80 | ||
81 | n = reg >> nm->n.shift; | |
82 | n &= (1 << nm->n.width) - 1; | |
e66f81bb MR |
83 | n += nm->n.offset; |
84 | if (!n) | |
85 | n++; | |
6174a1e2 MR |
86 | |
87 | m = reg >> nm->m.shift; | |
88 | m &= (1 << nm->m.width) - 1; | |
e66f81bb MR |
89 | m += nm->m.offset; |
90 | if (!m) | |
91 | m++; | |
6174a1e2 | 92 | |
392ba5fa CYT |
93 | if (ccu_sdm_helper_is_enabled(&nm->common, &nm->sdm)) { |
94 | unsigned long rate = | |
95 | ccu_sdm_helper_read_rate(&nm->common, &nm->sdm, | |
96 | m, n); | |
97 | if (rate) | |
98 | return rate; | |
99 | } | |
100 | ||
e66f81bb | 101 | return parent_rate * n / m; |
6174a1e2 MR |
102 | } |
103 | ||
104 | static long ccu_nm_round_rate(struct clk_hw *hw, unsigned long rate, | |
105 | unsigned long *parent_rate) | |
106 | { | |
107 | struct ccu_nm *nm = hw_to_ccu_nm(hw); | |
ee28648c | 108 | struct _ccu_nm _nm; |
6174a1e2 | 109 | |
4cdbc40d CYT |
110 | if (ccu_frac_helper_has_rate(&nm->common, &nm->frac, rate)) |
111 | return rate; | |
112 | ||
392ba5fa CYT |
113 | if (ccu_sdm_helper_has_rate(&nm->common, &nm->sdm, rate)) |
114 | return rate; | |
115 | ||
4162c5ce | 116 | _nm.min_n = nm->n.min ?: 1; |
0c3c8e13 | 117 | _nm.max_n = nm->n.max ?: 1 << nm->n.width; |
6e0d50da | 118 | _nm.min_m = 1; |
ee28648c | 119 | _nm.max_m = nm->m.max ?: 1 << nm->m.width; |
87ba9e59 | 120 | |
ee28648c | 121 | ccu_nm_find_best(*parent_rate, rate, &_nm); |
6174a1e2 | 122 | |
ee28648c | 123 | return *parent_rate * _nm.n / _nm.m; |
6174a1e2 MR |
124 | } |
125 | ||
126 | static int ccu_nm_set_rate(struct clk_hw *hw, unsigned long rate, | |
127 | unsigned long parent_rate) | |
128 | { | |
129 | struct ccu_nm *nm = hw_to_ccu_nm(hw); | |
ee28648c | 130 | struct _ccu_nm _nm; |
6174a1e2 | 131 | unsigned long flags; |
6174a1e2 MR |
132 | u32 reg; |
133 | ||
b64dfec0 JŠ |
134 | if (ccu_frac_helper_has_rate(&nm->common, &nm->frac, rate)) { |
135 | spin_lock_irqsave(nm->common.lock, flags); | |
136 | ||
137 | /* most SoCs require M to be 0 if fractional mode is used */ | |
138 | reg = readl(nm->common.base + nm->common.reg); | |
139 | reg &= ~GENMASK(nm->m.width + nm->m.shift - 1, nm->m.shift); | |
140 | writel(reg, nm->common.base + nm->common.reg); | |
141 | ||
142 | spin_unlock_irqrestore(nm->common.lock, flags); | |
143 | ||
144 | ccu_frac_helper_enable(&nm->common, &nm->frac); | |
145 | ||
1d42460a JŠ |
146 | return ccu_frac_helper_set_rate(&nm->common, &nm->frac, |
147 | rate, nm->lock); | |
b64dfec0 | 148 | } else { |
6174a1e2 | 149 | ccu_frac_helper_disable(&nm->common, &nm->frac); |
b64dfec0 | 150 | } |
6174a1e2 | 151 | |
95ad8ed9 | 152 | _nm.min_n = nm->n.min ?: 1; |
0c3c8e13 | 153 | _nm.max_n = nm->n.max ?: 1 << nm->n.width; |
6e0d50da | 154 | _nm.min_m = 1; |
ee28648c | 155 | _nm.max_m = nm->m.max ?: 1 << nm->m.width; |
87ba9e59 | 156 | |
392ba5fa CYT |
157 | if (ccu_sdm_helper_has_rate(&nm->common, &nm->sdm, rate)) { |
158 | ccu_sdm_helper_enable(&nm->common, &nm->sdm, rate); | |
159 | ||
160 | /* Sigma delta modulation requires specific N and M factors */ | |
161 | ccu_sdm_helper_get_factors(&nm->common, &nm->sdm, rate, | |
162 | &_nm.m, &_nm.n); | |
163 | } else { | |
164 | ccu_sdm_helper_disable(&nm->common, &nm->sdm); | |
165 | ccu_nm_find_best(parent_rate, rate, &_nm); | |
166 | } | |
6174a1e2 MR |
167 | |
168 | spin_lock_irqsave(nm->common.lock, flags); | |
169 | ||
170 | reg = readl(nm->common.base + nm->common.reg); | |
171 | reg &= ~GENMASK(nm->n.width + nm->n.shift - 1, nm->n.shift); | |
172 | reg &= ~GENMASK(nm->m.width + nm->m.shift - 1, nm->m.shift); | |
173 | ||
e66f81bb MR |
174 | reg |= (_nm.n - nm->n.offset) << nm->n.shift; |
175 | reg |= (_nm.m - nm->m.offset) << nm->m.shift; | |
176 | writel(reg, nm->common.base + nm->common.reg); | |
6174a1e2 MR |
177 | |
178 | spin_unlock_irqrestore(nm->common.lock, flags); | |
179 | ||
180 | ccu_helper_wait_for_lock(&nm->common, nm->lock); | |
181 | ||
182 | return 0; | |
183 | } | |
184 | ||
185 | const struct clk_ops ccu_nm_ops = { | |
186 | .disable = ccu_nm_disable, | |
187 | .enable = ccu_nm_enable, | |
188 | .is_enabled = ccu_nm_is_enabled, | |
189 | ||
190 | .recalc_rate = ccu_nm_recalc_rate, | |
191 | .round_rate = ccu_nm_round_rate, | |
192 | .set_rate = ccu_nm_set_rate, | |
193 | }; |