GNU Linux-libre 4.19.314-gnu1
[releases.git] / drivers / clk / sunxi-ng / ccu_nkm.c
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
13 #include "ccu_gate.h"
14 #include "ccu_nkm.h"
15
16 struct _ccu_nkm {
17         unsigned long   n, min_n, max_n;
18         unsigned long   k, min_k, max_k;
19         unsigned long   m, min_m, max_m;
20 };
21
22 static void ccu_nkm_find_best(unsigned long parent, unsigned long rate,
23                               struct _ccu_nkm *nkm)
24 {
25         unsigned long best_rate = 0;
26         unsigned long best_n = 0, best_k = 0, best_m = 0;
27         unsigned long _n, _k, _m;
28
29         for (_k = nkm->min_k; _k <= nkm->max_k; _k++) {
30                 for (_n = nkm->min_n; _n <= nkm->max_n; _n++) {
31                         for (_m = nkm->min_m; _m <= nkm->max_m; _m++) {
32                                 unsigned long tmp_rate;
33
34                                 tmp_rate = parent * _n * _k / _m;
35
36                                 if (tmp_rate > rate)
37                                         continue;
38                                 if ((rate - tmp_rate) < (rate - best_rate)) {
39                                         best_rate = tmp_rate;
40                                         best_n = _n;
41                                         best_k = _k;
42                                         best_m = _m;
43                                 }
44                         }
45                 }
46         }
47
48         nkm->n = best_n;
49         nkm->k = best_k;
50         nkm->m = best_m;
51 }
52
53 static void ccu_nkm_disable(struct clk_hw *hw)
54 {
55         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
56
57         return ccu_gate_helper_disable(&nkm->common, nkm->enable);
58 }
59
60 static int ccu_nkm_enable(struct clk_hw *hw)
61 {
62         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
63
64         return ccu_gate_helper_enable(&nkm->common, nkm->enable);
65 }
66
67 static int ccu_nkm_is_enabled(struct clk_hw *hw)
68 {
69         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
70
71         return ccu_gate_helper_is_enabled(&nkm->common, nkm->enable);
72 }
73
74 static unsigned long ccu_nkm_recalc_rate(struct clk_hw *hw,
75                                         unsigned long parent_rate)
76 {
77         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
78         unsigned long n, m, k, rate;
79         u32 reg;
80
81         reg = readl(nkm->common.base + nkm->common.reg);
82
83         n = reg >> nkm->n.shift;
84         n &= (1 << nkm->n.width) - 1;
85         n += nkm->n.offset;
86         if (!n)
87                 n++;
88
89         k = reg >> nkm->k.shift;
90         k &= (1 << nkm->k.width) - 1;
91         k += nkm->k.offset;
92         if (!k)
93                 k++;
94
95         m = reg >> nkm->m.shift;
96         m &= (1 << nkm->m.width) - 1;
97         m += nkm->m.offset;
98         if (!m)
99                 m++;
100
101         rate = parent_rate * n  * k / m;
102
103         if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
104                 rate /= nkm->fixed_post_div;
105
106         return rate;
107 }
108
109 static unsigned long ccu_nkm_round_rate(struct ccu_mux_internal *mux,
110                                         struct clk_hw *hw,
111                                         unsigned long *parent_rate,
112                                         unsigned long rate,
113                                         void *data)
114 {
115         struct ccu_nkm *nkm = data;
116         struct _ccu_nkm _nkm;
117
118         _nkm.min_n = nkm->n.min ?: 1;
119         _nkm.max_n = nkm->n.max ?: 1 << nkm->n.width;
120         _nkm.min_k = nkm->k.min ?: 1;
121         _nkm.max_k = nkm->k.max ?: 1 << nkm->k.width;
122         _nkm.min_m = 1;
123         _nkm.max_m = nkm->m.max ?: 1 << nkm->m.width;
124
125         if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
126                 rate *= nkm->fixed_post_div;
127
128         ccu_nkm_find_best(*parent_rate, rate, &_nkm);
129
130         rate = *parent_rate * _nkm.n * _nkm.k / _nkm.m;
131
132         if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
133                 rate /= nkm->fixed_post_div;
134
135         return rate;
136 }
137
138 static int ccu_nkm_determine_rate(struct clk_hw *hw,
139                                   struct clk_rate_request *req)
140 {
141         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
142
143         return ccu_mux_helper_determine_rate(&nkm->common, &nkm->mux,
144                                              req, ccu_nkm_round_rate, nkm);
145 }
146
147 static int ccu_nkm_set_rate(struct clk_hw *hw, unsigned long rate,
148                            unsigned long parent_rate)
149 {
150         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
151         struct _ccu_nkm _nkm;
152         unsigned long flags;
153         u32 reg;
154
155         if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
156                 rate *= nkm->fixed_post_div;
157
158         _nkm.min_n = nkm->n.min ?: 1;
159         _nkm.max_n = nkm->n.max ?: 1 << nkm->n.width;
160         _nkm.min_k = nkm->k.min ?: 1;
161         _nkm.max_k = nkm->k.max ?: 1 << nkm->k.width;
162         _nkm.min_m = 1;
163         _nkm.max_m = nkm->m.max ?: 1 << nkm->m.width;
164
165         ccu_nkm_find_best(parent_rate, rate, &_nkm);
166
167         spin_lock_irqsave(nkm->common.lock, flags);
168
169         reg = readl(nkm->common.base + nkm->common.reg);
170         reg &= ~GENMASK(nkm->n.width + nkm->n.shift - 1, nkm->n.shift);
171         reg &= ~GENMASK(nkm->k.width + nkm->k.shift - 1, nkm->k.shift);
172         reg &= ~GENMASK(nkm->m.width + nkm->m.shift - 1, nkm->m.shift);
173
174         reg |= (_nkm.n - nkm->n.offset) << nkm->n.shift;
175         reg |= (_nkm.k - nkm->k.offset) << nkm->k.shift;
176         reg |= (_nkm.m - nkm->m.offset) << nkm->m.shift;
177         writel(reg, nkm->common.base + nkm->common.reg);
178
179         spin_unlock_irqrestore(nkm->common.lock, flags);
180
181         ccu_helper_wait_for_lock(&nkm->common, nkm->lock);
182
183         return 0;
184 }
185
186 static u8 ccu_nkm_get_parent(struct clk_hw *hw)
187 {
188         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
189
190         return ccu_mux_helper_get_parent(&nkm->common, &nkm->mux);
191 }
192
193 static int ccu_nkm_set_parent(struct clk_hw *hw, u8 index)
194 {
195         struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
196
197         return ccu_mux_helper_set_parent(&nkm->common, &nkm->mux, index);
198 }
199
200 const struct clk_ops ccu_nkm_ops = {
201         .disable        = ccu_nkm_disable,
202         .enable         = ccu_nkm_enable,
203         .is_enabled     = ccu_nkm_is_enabled,
204
205         .get_parent     = ccu_nkm_get_parent,
206         .set_parent     = ccu_nkm_set_parent,
207
208         .determine_rate = ccu_nkm_determine_rate,
209         .recalc_rate    = ccu_nkm_recalc_rate,
210         .set_rate       = ccu_nkm_set_rate,
211 };