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