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> | |
12 | #include <linux/rational.h> | |
13 | ||
14 | #include "ccu_gate.h" | |
15 | #include "ccu_nk.h" | |
16 | ||
17 | void ccu_nk_find_best(unsigned long parent, unsigned long rate, | |
18 | unsigned int max_n, unsigned int max_k, | |
19 | unsigned int *n, unsigned int *k) | |
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 = 1; _k <= max_k; _k++) { | |
26 | for (_n = 1; _n <= 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 | *k = best_k; | |
41 | *n = best_n; | |
42 | } | |
43 | ||
44 | static void ccu_nk_disable(struct clk_hw *hw) | |
45 | { | |
46 | struct ccu_nk *nk = hw_to_ccu_nk(hw); | |
47 | ||
48 | return ccu_gate_helper_disable(&nk->common, nk->enable); | |
49 | } | |
50 | ||
51 | static int ccu_nk_enable(struct clk_hw *hw) | |
52 | { | |
53 | struct ccu_nk *nk = hw_to_ccu_nk(hw); | |
54 | ||
55 | return ccu_gate_helper_enable(&nk->common, nk->enable); | |
56 | } | |
57 | ||
58 | static int ccu_nk_is_enabled(struct clk_hw *hw) | |
59 | { | |
60 | struct ccu_nk *nk = hw_to_ccu_nk(hw); | |
61 | ||
62 | return ccu_gate_helper_is_enabled(&nk->common, nk->enable); | |
63 | } | |
64 | ||
65 | static unsigned long ccu_nk_recalc_rate(struct clk_hw *hw, | |
66 | unsigned long parent_rate) | |
67 | { | |
68 | struct ccu_nk *nk = hw_to_ccu_nk(hw); | |
69 | unsigned long rate, n, k; | |
70 | u32 reg; | |
71 | ||
72 | reg = readl(nk->common.base + nk->common.reg); | |
73 | ||
74 | n = reg >> nk->n.shift; | |
75 | n &= (1 << nk->n.width) - 1; | |
76 | ||
77 | k = reg >> nk->k.shift; | |
78 | k &= (1 << nk->k.width) - 1; | |
79 | ||
80 | rate = parent_rate * (n + 1) * (k + 1); | |
81 | ||
82 | if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV) | |
83 | rate /= nk->fixed_post_div; | |
84 | ||
85 | return rate; | |
86 | } | |
87 | ||
88 | static long ccu_nk_round_rate(struct clk_hw *hw, unsigned long rate, | |
89 | unsigned long *parent_rate) | |
90 | { | |
91 | struct ccu_nk *nk = hw_to_ccu_nk(hw); | |
92 | unsigned int n, k; | |
93 | ||
94 | if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV) | |
95 | rate *= nk->fixed_post_div; | |
96 | ||
97 | ccu_nk_find_best(*parent_rate, rate, | |
98 | 1 << nk->n.width, 1 << nk->k.width, | |
99 | &n, &k); | |
100 | ||
101 | rate = *parent_rate * n * k; | |
102 | if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV) | |
103 | rate = rate / nk->fixed_post_div; | |
104 | ||
105 | return rate; | |
106 | } | |
107 | ||
108 | static int ccu_nk_set_rate(struct clk_hw *hw, unsigned long rate, | |
109 | unsigned long parent_rate) | |
110 | { | |
111 | struct ccu_nk *nk = hw_to_ccu_nk(hw); | |
112 | unsigned long flags; | |
113 | unsigned int n, k; | |
114 | u32 reg; | |
115 | ||
116 | if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV) | |
117 | rate = rate * nk->fixed_post_div; | |
118 | ||
119 | ccu_nk_find_best(parent_rate, rate, | |
120 | 1 << nk->n.width, 1 << nk->k.width, | |
121 | &n, &k); | |
122 | ||
123 | spin_lock_irqsave(nk->common.lock, flags); | |
124 | ||
125 | reg = readl(nk->common.base + nk->common.reg); | |
126 | reg &= ~GENMASK(nk->n.width + nk->n.shift - 1, nk->n.shift); | |
127 | reg &= ~GENMASK(nk->k.width + nk->k.shift - 1, nk->k.shift); | |
128 | ||
129 | writel(reg | ((k - 1) << nk->k.shift) | ((n - 1) << nk->n.shift), | |
130 | nk->common.base + nk->common.reg); | |
131 | ||
132 | spin_unlock_irqrestore(nk->common.lock, flags); | |
133 | ||
134 | ccu_helper_wait_for_lock(&nk->common, nk->lock); | |
135 | ||
136 | return 0; | |
137 | } | |
138 | ||
139 | const struct clk_ops ccu_nk_ops = { | |
140 | .disable = ccu_nk_disable, | |
141 | .enable = ccu_nk_enable, | |
142 | .is_enabled = ccu_nk_is_enabled, | |
143 | ||
144 | .recalc_rate = ccu_nk_recalc_rate, | |
145 | .round_rate = ccu_nk_round_rate, | |
146 | .set_rate = ccu_nk_set_rate, | |
147 | }; |