]>
Commit | Line | Data |
---|---|---|
6174a1e2 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> | |
6174a1e2 MR |
12 | |
13 | #include "ccu_frac.h" | |
14 | #include "ccu_gate.h" | |
15 | #include "ccu_nm.h" | |
16 | ||
ee28648c | 17 | struct _ccu_nm { |
6e0d50da MR |
18 | unsigned long n, min_n, max_n; |
19 | unsigned long m, min_m, max_m; | |
ee28648c MR |
20 | }; |
21 | ||
22 | static void ccu_nm_find_best(unsigned long parent, unsigned long rate, | |
23 | struct _ccu_nm *nm) | |
24 | { | |
25 | unsigned long best_rate = 0; | |
26 | unsigned long best_n = 0, best_m = 0; | |
27 | unsigned long _n, _m; | |
28 | ||
6e0d50da MR |
29 | for (_n = nm->min_n; _n <= nm->max_n; _n++) { |
30 | for (_m = nm->min_m; _m <= nm->max_m; _m++) { | |
ee28648c MR |
31 | unsigned long tmp_rate = parent * _n / _m; |
32 | ||
33 | if (tmp_rate > rate) | |
34 | continue; | |
35 | ||
36 | if ((rate - tmp_rate) < (rate - best_rate)) { | |
37 | best_rate = tmp_rate; | |
38 | best_n = _n; | |
39 | best_m = _m; | |
40 | } | |
41 | } | |
42 | } | |
43 | ||
44 | nm->n = best_n; | |
45 | nm->m = best_m; | |
46 | } | |
47 | ||
6174a1e2 MR |
48 | static void ccu_nm_disable(struct clk_hw *hw) |
49 | { | |
50 | struct ccu_nm *nm = hw_to_ccu_nm(hw); | |
51 | ||
52 | return ccu_gate_helper_disable(&nm->common, nm->enable); | |
53 | } | |
54 | ||
55 | static int ccu_nm_enable(struct clk_hw *hw) | |
56 | { | |
57 | struct ccu_nm *nm = hw_to_ccu_nm(hw); | |
58 | ||
59 | return ccu_gate_helper_enable(&nm->common, nm->enable); | |
60 | } | |
61 | ||
62 | static int ccu_nm_is_enabled(struct clk_hw *hw) | |
63 | { | |
64 | struct ccu_nm *nm = hw_to_ccu_nm(hw); | |
65 | ||
66 | return ccu_gate_helper_is_enabled(&nm->common, nm->enable); | |
67 | } | |
68 | ||
69 | static unsigned long ccu_nm_recalc_rate(struct clk_hw *hw, | |
70 | unsigned long parent_rate) | |
71 | { | |
72 | struct ccu_nm *nm = hw_to_ccu_nm(hw); | |
73 | unsigned long n, m; | |
74 | u32 reg; | |
75 | ||
76 | if (ccu_frac_helper_is_enabled(&nm->common, &nm->frac)) | |
77 | return ccu_frac_helper_read_rate(&nm->common, &nm->frac); | |
78 | ||
79 | reg = readl(nm->common.base + nm->common.reg); | |
80 | ||
81 | n = reg >> nm->n.shift; | |
82 | n &= (1 << nm->n.width) - 1; | |
e66f81bb MR |
83 | n += nm->n.offset; |
84 | if (!n) | |
85 | n++; | |
6174a1e2 MR |
86 | |
87 | m = reg >> nm->m.shift; | |
88 | m &= (1 << nm->m.width) - 1; | |
e66f81bb MR |
89 | m += nm->m.offset; |
90 | if (!m) | |
91 | m++; | |
6174a1e2 | 92 | |
e66f81bb | 93 | return parent_rate * n / m; |
6174a1e2 MR |
94 | } |
95 | ||
96 | static long ccu_nm_round_rate(struct clk_hw *hw, unsigned long rate, | |
97 | unsigned long *parent_rate) | |
98 | { | |
99 | struct ccu_nm *nm = hw_to_ccu_nm(hw); | |
ee28648c | 100 | struct _ccu_nm _nm; |
6174a1e2 | 101 | |
2beaa601 | 102 | _nm.min_n = nm->n.min; |
0c3c8e13 | 103 | _nm.max_n = nm->n.max ?: 1 << nm->n.width; |
6e0d50da | 104 | _nm.min_m = 1; |
ee28648c | 105 | _nm.max_m = nm->m.max ?: 1 << nm->m.width; |
87ba9e59 | 106 | |
ee28648c | 107 | ccu_nm_find_best(*parent_rate, rate, &_nm); |
6174a1e2 | 108 | |
ee28648c | 109 | return *parent_rate * _nm.n / _nm.m; |
6174a1e2 MR |
110 | } |
111 | ||
112 | static int ccu_nm_set_rate(struct clk_hw *hw, unsigned long rate, | |
113 | unsigned long parent_rate) | |
114 | { | |
115 | struct ccu_nm *nm = hw_to_ccu_nm(hw); | |
ee28648c | 116 | struct _ccu_nm _nm; |
6174a1e2 | 117 | unsigned long flags; |
6174a1e2 MR |
118 | u32 reg; |
119 | ||
120 | if (ccu_frac_helper_has_rate(&nm->common, &nm->frac, rate)) | |
121 | return ccu_frac_helper_set_rate(&nm->common, &nm->frac, rate); | |
122 | else | |
123 | ccu_frac_helper_disable(&nm->common, &nm->frac); | |
124 | ||
6e0d50da | 125 | _nm.min_n = 1; |
0c3c8e13 | 126 | _nm.max_n = nm->n.max ?: 1 << nm->n.width; |
6e0d50da | 127 | _nm.min_m = 1; |
ee28648c | 128 | _nm.max_m = nm->m.max ?: 1 << nm->m.width; |
87ba9e59 | 129 | |
ee28648c | 130 | ccu_nm_find_best(parent_rate, rate, &_nm); |
6174a1e2 MR |
131 | |
132 | spin_lock_irqsave(nm->common.lock, flags); | |
133 | ||
134 | reg = readl(nm->common.base + nm->common.reg); | |
135 | reg &= ~GENMASK(nm->n.width + nm->n.shift - 1, nm->n.shift); | |
136 | reg &= ~GENMASK(nm->m.width + nm->m.shift - 1, nm->m.shift); | |
137 | ||
e66f81bb MR |
138 | reg |= (_nm.n - nm->n.offset) << nm->n.shift; |
139 | reg |= (_nm.m - nm->m.offset) << nm->m.shift; | |
140 | writel(reg, nm->common.base + nm->common.reg); | |
6174a1e2 MR |
141 | |
142 | spin_unlock_irqrestore(nm->common.lock, flags); | |
143 | ||
144 | ccu_helper_wait_for_lock(&nm->common, nm->lock); | |
145 | ||
146 | return 0; | |
147 | } | |
148 | ||
149 | const struct clk_ops ccu_nm_ops = { | |
150 | .disable = ccu_nm_disable, | |
151 | .enable = ccu_nm_enable, | |
152 | .is_enabled = ccu_nm_is_enabled, | |
153 | ||
154 | .recalc_rate = ccu_nm_recalc_rate, | |
155 | .round_rate = ccu_nm_round_rate, | |
156 | .set_rate = ccu_nm_set_rate, | |
157 | }; |