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