Commit | Line | Data |
---|---|---|
2ab836db 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 | ||
13 | #include "ccu_gate.h" | |
14 | #include "ccu_mp.h" | |
15 | ||
16 | static void ccu_mp_find_best(unsigned long parent, unsigned long rate, | |
17 | unsigned int max_m, unsigned int max_p, | |
18 | unsigned int *m, unsigned int *p) | |
19 | { | |
20 | unsigned long best_rate = 0; | |
21 | unsigned int best_m = 0, best_p = 0; | |
22 | unsigned int _m, _p; | |
23 | ||
87ba9e59 | 24 | for (_p = 1; _p <= max_p; _p <<= 1) { |
2ab836db | 25 | for (_m = 1; _m <= max_m; _m++) { |
87ba9e59 | 26 | unsigned long tmp_rate = parent / _p / _m; |
2ab836db MR |
27 | |
28 | if (tmp_rate > rate) | |
29 | continue; | |
30 | ||
31 | if ((rate - tmp_rate) < (rate - best_rate)) { | |
32 | best_rate = tmp_rate; | |
33 | best_m = _m; | |
34 | best_p = _p; | |
35 | } | |
36 | } | |
37 | } | |
38 | ||
39 | *m = best_m; | |
40 | *p = best_p; | |
41 | } | |
42 | ||
43 | static unsigned long ccu_mp_round_rate(struct ccu_mux_internal *mux, | |
44 | unsigned long parent_rate, | |
45 | unsigned long rate, | |
46 | void *data) | |
47 | { | |
48 | struct ccu_mp *cmp = data; | |
87ba9e59 | 49 | unsigned int max_m, max_p; |
2ab836db MR |
50 | unsigned int m, p; |
51 | ||
87ba9e59 MR |
52 | max_m = cmp->m.max ?: 1 << cmp->m.width; |
53 | max_p = cmp->p.max ?: 1 << ((1 << cmp->p.width) - 1); | |
2ab836db | 54 | |
87ba9e59 MR |
55 | ccu_mp_find_best(parent_rate, rate, max_m, max_p, &m, &p); |
56 | ||
57 | return parent_rate / p / m; | |
2ab836db MR |
58 | } |
59 | ||
60 | static void ccu_mp_disable(struct clk_hw *hw) | |
61 | { | |
62 | struct ccu_mp *cmp = hw_to_ccu_mp(hw); | |
63 | ||
64 | return ccu_gate_helper_disable(&cmp->common, cmp->enable); | |
65 | } | |
66 | ||
67 | static int ccu_mp_enable(struct clk_hw *hw) | |
68 | { | |
69 | struct ccu_mp *cmp = hw_to_ccu_mp(hw); | |
70 | ||
71 | return ccu_gate_helper_enable(&cmp->common, cmp->enable); | |
72 | } | |
73 | ||
74 | static int ccu_mp_is_enabled(struct clk_hw *hw) | |
75 | { | |
76 | struct ccu_mp *cmp = hw_to_ccu_mp(hw); | |
77 | ||
78 | return ccu_gate_helper_is_enabled(&cmp->common, cmp->enable); | |
79 | } | |
80 | ||
81 | static unsigned long ccu_mp_recalc_rate(struct clk_hw *hw, | |
82 | unsigned long parent_rate) | |
83 | { | |
84 | struct ccu_mp *cmp = hw_to_ccu_mp(hw); | |
85 | unsigned int m, p; | |
86 | u32 reg; | |
87 | ||
88 | reg = readl(cmp->common.base + cmp->common.reg); | |
89 | ||
90 | m = reg >> cmp->m.shift; | |
91 | m &= (1 << cmp->m.width) - 1; | |
92 | ||
93 | p = reg >> cmp->p.shift; | |
94 | p &= (1 << cmp->p.width) - 1; | |
95 | ||
96 | return (parent_rate >> p) / (m + 1); | |
97 | } | |
98 | ||
99 | static int ccu_mp_determine_rate(struct clk_hw *hw, | |
100 | struct clk_rate_request *req) | |
101 | { | |
102 | struct ccu_mp *cmp = hw_to_ccu_mp(hw); | |
103 | ||
104 | return ccu_mux_helper_determine_rate(&cmp->common, &cmp->mux, | |
105 | req, ccu_mp_round_rate, cmp); | |
106 | } | |
107 | ||
108 | static int ccu_mp_set_rate(struct clk_hw *hw, unsigned long rate, | |
109 | unsigned long parent_rate) | |
110 | { | |
111 | struct ccu_mp *cmp = hw_to_ccu_mp(hw); | |
112 | unsigned long flags; | |
87ba9e59 | 113 | unsigned int max_m, max_p; |
2ab836db MR |
114 | unsigned int m, p; |
115 | u32 reg; | |
116 | ||
87ba9e59 MR |
117 | max_m = cmp->m.max ?: 1 << cmp->m.width; |
118 | max_p = cmp->p.max ?: 1 << ((1 << cmp->p.width) - 1); | |
2ab836db | 119 | |
87ba9e59 | 120 | ccu_mp_find_best(parent_rate, rate, max_m, max_p, &m, &p); |
2ab836db MR |
121 | |
122 | spin_lock_irqsave(cmp->common.lock, flags); | |
123 | ||
124 | reg = readl(cmp->common.base + cmp->common.reg); | |
125 | reg &= ~GENMASK(cmp->m.width + cmp->m.shift - 1, cmp->m.shift); | |
126 | reg &= ~GENMASK(cmp->p.width + cmp->p.shift - 1, cmp->p.shift); | |
127 | ||
87ba9e59 | 128 | writel(reg | (ilog2(p) << cmp->p.shift) | ((m - 1) << cmp->m.shift), |
2ab836db MR |
129 | cmp->common.base + cmp->common.reg); |
130 | ||
131 | spin_unlock_irqrestore(cmp->common.lock, flags); | |
132 | ||
133 | return 0; | |
134 | } | |
135 | ||
136 | static u8 ccu_mp_get_parent(struct clk_hw *hw) | |
137 | { | |
138 | struct ccu_mp *cmp = hw_to_ccu_mp(hw); | |
139 | ||
140 | return ccu_mux_helper_get_parent(&cmp->common, &cmp->mux); | |
141 | } | |
142 | ||
143 | static int ccu_mp_set_parent(struct clk_hw *hw, u8 index) | |
144 | { | |
145 | struct ccu_mp *cmp = hw_to_ccu_mp(hw); | |
146 | ||
147 | return ccu_mux_helper_set_parent(&cmp->common, &cmp->mux, index); | |
148 | } | |
149 | ||
150 | const struct clk_ops ccu_mp_ops = { | |
151 | .disable = ccu_mp_disable, | |
152 | .enable = ccu_mp_enable, | |
153 | .is_enabled = ccu_mp_is_enabled, | |
154 | ||
155 | .get_parent = ccu_mp_get_parent, | |
156 | .set_parent = ccu_mp_set_parent, | |
157 | ||
158 | .determine_rate = ccu_mp_determine_rate, | |
159 | .recalc_rate = ccu_mp_recalc_rate, | |
160 | .set_rate = ccu_mp_set_rate, | |
161 | }; |