Commit | Line | Data |
---|---|---|
df6561e6 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> | |
12 | #include <linux/rational.h> | |
13 | ||
14 | #include "ccu_gate.h" | |
15 | #include "ccu_nkm.h" | |
16 | ||
17 | struct _ccu_nkm { | |
18 | unsigned long n, max_n; | |
19 | unsigned long k, max_k; | |
20 | unsigned long m, max_m; | |
21 | }; | |
22 | ||
23 | static void ccu_nkm_find_best(unsigned long parent, unsigned long rate, | |
24 | struct _ccu_nkm *nkm) | |
25 | { | |
26 | unsigned long best_rate = 0; | |
27 | unsigned long best_n = 0, best_k = 0, best_m = 0; | |
28 | unsigned long _n, _k, _m; | |
29 | ||
30 | for (_k = 1; _k <= nkm->max_k; _k++) { | |
31 | unsigned long tmp_rate; | |
32 | ||
33 | rational_best_approximation(rate / _k, parent, | |
34 | nkm->max_n, nkm->max_m, &_n, &_m); | |
35 | ||
36 | tmp_rate = parent * _n * _k / _m; | |
37 | ||
38 | if (tmp_rate > rate) | |
39 | continue; | |
40 | ||
41 | if ((rate - tmp_rate) < (rate - best_rate)) { | |
42 | best_rate = tmp_rate; | |
43 | best_n = _n; | |
44 | best_k = _k; | |
45 | best_m = _m; | |
46 | } | |
47 | } | |
48 | ||
49 | nkm->n = best_n; | |
50 | nkm->k = best_k; | |
51 | nkm->m = best_m; | |
52 | } | |
53 | ||
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 | ||
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 | ||
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 | ||
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; | |
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 | ||
87 | k = reg >> nkm->k.shift; | |
88 | k &= (1 << nkm->k.width) - 1; | |
89 | ||
90 | m = reg >> nkm->m.shift; | |
91 | m &= (1 << nkm->m.width) - 1; | |
92 | ||
93 | return parent_rate * (n + 1) * (k + 1) / (m + 1); | |
94 | } | |
95 | ||
a3658359 CYT |
96 | static unsigned long ccu_nkm_round_rate(struct ccu_mux_internal *mux, |
97 | unsigned long parent_rate, | |
98 | unsigned long rate, | |
99 | void *data) | |
df6561e6 | 100 | { |
a3658359 | 101 | struct ccu_nkm *nkm = data; |
df6561e6 MR |
102 | struct _ccu_nkm _nkm; |
103 | ||
104 | _nkm.max_n = 1 << nkm->n.width; | |
105 | _nkm.max_k = 1 << nkm->k.width; | |
87ba9e59 | 106 | _nkm.max_m = nkm->m.max ?: 1 << nkm->m.width; |
df6561e6 | 107 | |
a3658359 | 108 | ccu_nkm_find_best(parent_rate, rate, &_nkm); |
df6561e6 | 109 | |
a3658359 CYT |
110 | return parent_rate * _nkm.n * _nkm.k / _nkm.m; |
111 | } | |
112 | ||
113 | static int ccu_nkm_determine_rate(struct clk_hw *hw, | |
114 | struct clk_rate_request *req) | |
115 | { | |
116 | struct ccu_nkm *nkm = hw_to_ccu_nkm(hw); | |
117 | ||
118 | return ccu_mux_helper_determine_rate(&nkm->common, &nkm->mux, | |
119 | req, ccu_nkm_round_rate, nkm); | |
df6561e6 MR |
120 | } |
121 | ||
122 | static int ccu_nkm_set_rate(struct clk_hw *hw, unsigned long rate, | |
123 | unsigned long parent_rate) | |
124 | { | |
125 | struct ccu_nkm *nkm = hw_to_ccu_nkm(hw); | |
126 | struct _ccu_nkm _nkm; | |
127 | unsigned long flags; | |
128 | u32 reg; | |
129 | ||
130 | _nkm.max_n = 1 << nkm->n.width; | |
131 | _nkm.max_k = 1 << nkm->k.width; | |
87ba9e59 | 132 | _nkm.max_m = nkm->m.max ?: 1 << nkm->m.width; |
df6561e6 MR |
133 | |
134 | ccu_nkm_find_best(parent_rate, rate, &_nkm); | |
135 | ||
136 | spin_lock_irqsave(nkm->common.lock, flags); | |
137 | ||
138 | reg = readl(nkm->common.base + nkm->common.reg); | |
139 | reg &= ~GENMASK(nkm->n.width + nkm->n.shift - 1, nkm->n.shift); | |
140 | reg &= ~GENMASK(nkm->k.width + nkm->k.shift - 1, nkm->k.shift); | |
141 | reg &= ~GENMASK(nkm->m.width + nkm->m.shift - 1, nkm->m.shift); | |
142 | ||
143 | reg |= (_nkm.n - 1) << nkm->n.shift; | |
144 | reg |= (_nkm.k - 1) << nkm->k.shift; | |
145 | reg |= (_nkm.m - 1) << nkm->m.shift; | |
146 | ||
147 | writel(reg, nkm->common.base + nkm->common.reg); | |
148 | ||
149 | spin_unlock_irqrestore(nkm->common.lock, flags); | |
150 | ||
151 | ccu_helper_wait_for_lock(&nkm->common, nkm->lock); | |
152 | ||
153 | return 0; | |
154 | } | |
155 | ||
a3658359 CYT |
156 | static u8 ccu_nkm_get_parent(struct clk_hw *hw) |
157 | { | |
158 | struct ccu_nkm *nkm = hw_to_ccu_nkm(hw); | |
159 | ||
160 | return ccu_mux_helper_get_parent(&nkm->common, &nkm->mux); | |
161 | } | |
162 | ||
163 | static int ccu_nkm_set_parent(struct clk_hw *hw, u8 index) | |
164 | { | |
165 | struct ccu_nkm *nkm = hw_to_ccu_nkm(hw); | |
166 | ||
167 | return ccu_mux_helper_set_parent(&nkm->common, &nkm->mux, index); | |
168 | } | |
169 | ||
df6561e6 MR |
170 | const struct clk_ops ccu_nkm_ops = { |
171 | .disable = ccu_nkm_disable, | |
172 | .enable = ccu_nkm_enable, | |
173 | .is_enabled = ccu_nkm_is_enabled, | |
174 | ||
a3658359 CYT |
175 | .get_parent = ccu_nkm_get_parent, |
176 | .set_parent = ccu_nkm_set_parent, | |
177 | ||
178 | .determine_rate = ccu_nkm_determine_rate, | |
df6561e6 | 179 | .recalc_rate = ccu_nkm_recalc_rate, |
df6561e6 MR |
180 | .set_rate = ccu_nkm_set_rate, |
181 | }; |