1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | /* |
3 | * mmp factor clock operation source file |
4 | * |
5 | * Copyright (C) 2012 Marvell |
6 | * Chao Xie <xiechao.mail@gmail.com> |
7 | */ |
8 | |
9 | #include <linux/clk-provider.h> |
10 | #include <linux/slab.h> |
11 | #include <linux/io.h> |
12 | #include <linux/err.h> |
13 | |
14 | #include "clk.h" |
15 | /* |
16 | * It is M/N clock |
17 | * |
18 | * Fout from synthesizer can be given from two equations: |
19 | * numerator/denominator = Fin / (Fout * factor) |
20 | */ |
21 | |
22 | #define to_clk_factor(hw) container_of(hw, struct mmp_clk_factor, hw) |
23 | |
24 | static long clk_factor_round_rate(struct clk_hw *hw, unsigned long drate, |
25 | unsigned long *prate) |
26 | { |
27 | struct mmp_clk_factor *factor = to_clk_factor(hw); |
28 | u64 rate = 0, prev_rate; |
29 | int i; |
30 | |
31 | for (i = 0; i < factor->ftbl_cnt; i++) { |
32 | prev_rate = rate; |
33 | rate = *prate; |
34 | rate *= factor->ftbl[i].den; |
35 | do_div(rate, factor->ftbl[i].num * factor->masks->factor); |
36 | |
37 | if (rate > drate) |
38 | break; |
39 | } |
40 | if ((i == 0) || (i == factor->ftbl_cnt)) { |
41 | return rate; |
42 | } else { |
43 | if ((drate - prev_rate) > (rate - drate)) |
44 | return rate; |
45 | else |
46 | return prev_rate; |
47 | } |
48 | } |
49 | |
50 | static unsigned long clk_factor_recalc_rate(struct clk_hw *hw, |
51 | unsigned long parent_rate) |
52 | { |
53 | struct mmp_clk_factor *factor = to_clk_factor(hw); |
54 | struct mmp_clk_factor_masks *masks = factor->masks; |
55 | unsigned int val, num, den; |
56 | u64 rate; |
57 | |
58 | val = readl_relaxed(factor->base); |
59 | |
60 | /* calculate numerator */ |
61 | num = (val >> masks->num_shift) & masks->num_mask; |
62 | |
63 | /* calculate denominator */ |
64 | den = (val >> masks->den_shift) & masks->den_mask; |
65 | |
66 | if (!den) |
67 | return 0; |
68 | |
69 | rate = parent_rate; |
70 | rate *= den; |
71 | do_div(rate, num * factor->masks->factor); |
72 | |
73 | return rate; |
74 | } |
75 | |
76 | /* Configures new clock rate*/ |
77 | static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate, |
78 | unsigned long prate) |
79 | { |
80 | struct mmp_clk_factor *factor = to_clk_factor(hw); |
81 | struct mmp_clk_factor_masks *masks = factor->masks; |
82 | int i; |
83 | unsigned long val; |
84 | unsigned long flags = 0; |
85 | u64 rate = 0; |
86 | |
87 | for (i = 0; i < factor->ftbl_cnt; i++) { |
88 | rate = prate; |
89 | rate *= factor->ftbl[i].den; |
90 | do_div(rate, factor->ftbl[i].num * factor->masks->factor); |
91 | |
92 | if (rate > drate) |
93 | break; |
94 | } |
95 | if (i > 0) |
96 | i--; |
97 | |
98 | if (factor->lock) |
99 | spin_lock_irqsave(factor->lock, flags); |
100 | |
101 | val = readl_relaxed(factor->base); |
102 | |
103 | val &= ~(masks->num_mask << masks->num_shift); |
104 | val |= (factor->ftbl[i].num & masks->num_mask) << masks->num_shift; |
105 | |
106 | val &= ~(masks->den_mask << masks->den_shift); |
107 | val |= (factor->ftbl[i].den & masks->den_mask) << masks->den_shift; |
108 | |
109 | writel_relaxed(val, factor->base); |
110 | |
111 | if (factor->lock) |
112 | spin_unlock_irqrestore(lock: factor->lock, flags); |
113 | |
114 | return 0; |
115 | } |
116 | |
117 | static int clk_factor_init(struct clk_hw *hw) |
118 | { |
119 | struct mmp_clk_factor *factor = to_clk_factor(hw); |
120 | struct mmp_clk_factor_masks *masks = factor->masks; |
121 | u32 val, num, den; |
122 | int i; |
123 | unsigned long flags = 0; |
124 | |
125 | if (factor->lock) |
126 | spin_lock_irqsave(factor->lock, flags); |
127 | |
128 | val = readl(addr: factor->base); |
129 | |
130 | /* calculate numerator */ |
131 | num = (val >> masks->num_shift) & masks->num_mask; |
132 | |
133 | /* calculate denominator */ |
134 | den = (val >> masks->den_shift) & masks->den_mask; |
135 | |
136 | for (i = 0; i < factor->ftbl_cnt; i++) |
137 | if (den == factor->ftbl[i].den && num == factor->ftbl[i].num) |
138 | break; |
139 | |
140 | if (i >= factor->ftbl_cnt) { |
141 | val &= ~(masks->num_mask << masks->num_shift); |
142 | val |= (factor->ftbl[0].num & masks->num_mask) << |
143 | masks->num_shift; |
144 | |
145 | val &= ~(masks->den_mask << masks->den_shift); |
146 | val |= (factor->ftbl[0].den & masks->den_mask) << |
147 | masks->den_shift; |
148 | } |
149 | |
150 | if (!(val & masks->enable_mask) || i >= factor->ftbl_cnt) { |
151 | val |= masks->enable_mask; |
152 | writel(val, addr: factor->base); |
153 | } |
154 | |
155 | if (factor->lock) |
156 | spin_unlock_irqrestore(lock: factor->lock, flags); |
157 | |
158 | return 0; |
159 | } |
160 | |
161 | static const struct clk_ops clk_factor_ops = { |
162 | .recalc_rate = clk_factor_recalc_rate, |
163 | .round_rate = clk_factor_round_rate, |
164 | .set_rate = clk_factor_set_rate, |
165 | .init = clk_factor_init, |
166 | }; |
167 | |
168 | struct clk *mmp_clk_register_factor(const char *name, const char *parent_name, |
169 | unsigned long flags, void __iomem *base, |
170 | struct mmp_clk_factor_masks *masks, |
171 | struct mmp_clk_factor_tbl *ftbl, |
172 | unsigned int ftbl_cnt, spinlock_t *lock) |
173 | { |
174 | struct mmp_clk_factor *factor; |
175 | struct clk_init_data init; |
176 | struct clk *clk; |
177 | |
178 | if (!masks) { |
179 | pr_err("%s: must pass a clk_factor_mask\n" , __func__); |
180 | return ERR_PTR(error: -EINVAL); |
181 | } |
182 | |
183 | factor = kzalloc(size: sizeof(*factor), GFP_KERNEL); |
184 | if (!factor) |
185 | return ERR_PTR(error: -ENOMEM); |
186 | |
187 | /* struct clk_aux assignments */ |
188 | factor->base = base; |
189 | factor->masks = masks; |
190 | factor->ftbl = ftbl; |
191 | factor->ftbl_cnt = ftbl_cnt; |
192 | factor->hw.init = &init; |
193 | factor->lock = lock; |
194 | |
195 | init.name = name; |
196 | init.ops = &clk_factor_ops; |
197 | init.flags = flags; |
198 | init.parent_names = &parent_name; |
199 | init.num_parents = 1; |
200 | |
201 | clk = clk_register(NULL, hw: &factor->hw); |
202 | if (IS_ERR_OR_NULL(ptr: clk)) |
203 | kfree(objp: factor); |
204 | |
205 | return clk; |
206 | } |
207 | |