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.h>
12 #include <linux/clk-provider.h>
13 #include <linux/delay.h>
14 
15 #include "ccu_gate.h"
16 #include "ccu_mux.h"
17 
ccu_mux_get_prediv(struct ccu_common * common,struct ccu_mux_internal * cm,int parent_index)18 static u16 ccu_mux_get_prediv(struct ccu_common *common,
19 			      struct ccu_mux_internal *cm,
20 			      int parent_index)
21 {
22 	u16 prediv = 1;
23 	u32 reg;
24 
25 	if (!((common->features & CCU_FEATURE_FIXED_PREDIV) ||
26 	      (common->features & CCU_FEATURE_VARIABLE_PREDIV) ||
27 	      (common->features & CCU_FEATURE_ALL_PREDIV)))
28 		return 1;
29 
30 	if (common->features & CCU_FEATURE_ALL_PREDIV)
31 		return common->prediv;
32 
33 	reg = readl(common->base + common->reg);
34 	if (parent_index < 0) {
35 		parent_index = reg >> cm->shift;
36 		parent_index &= (1 << cm->width) - 1;
37 	}
38 
39 	if (common->features & CCU_FEATURE_FIXED_PREDIV) {
40 		int i;
41 
42 		for (i = 0; i < cm->n_predivs; i++)
43 			if (parent_index == cm->fixed_predivs[i].index)
44 				prediv = cm->fixed_predivs[i].div;
45 	}
46 
47 	if (common->features & CCU_FEATURE_VARIABLE_PREDIV) {
48 		int i;
49 
50 		for (i = 0; i < cm->n_var_predivs; i++)
51 			if (parent_index == cm->var_predivs[i].index) {
52 				u8 div;
53 
54 				div = reg >> cm->var_predivs[i].shift;
55 				div &= (1 << cm->var_predivs[i].width) - 1;
56 				prediv = div + 1;
57 			}
58 	}
59 
60 	return prediv;
61 }
62 
ccu_mux_helper_apply_prediv(struct ccu_common * common,struct ccu_mux_internal * cm,int parent_index,unsigned long parent_rate)63 unsigned long ccu_mux_helper_apply_prediv(struct ccu_common *common,
64 					  struct ccu_mux_internal *cm,
65 					  int parent_index,
66 					  unsigned long parent_rate)
67 {
68 	return parent_rate / ccu_mux_get_prediv(common, cm, parent_index);
69 }
70 
ccu_mux_helper_unapply_prediv(struct ccu_common * common,struct ccu_mux_internal * cm,int parent_index,unsigned long parent_rate)71 static unsigned long ccu_mux_helper_unapply_prediv(struct ccu_common *common,
72 					    struct ccu_mux_internal *cm,
73 					    int parent_index,
74 					    unsigned long parent_rate)
75 {
76 	return parent_rate * ccu_mux_get_prediv(common, cm, parent_index);
77 }
78 
ccu_mux_helper_determine_rate(struct ccu_common * common,struct ccu_mux_internal * cm,struct clk_rate_request * req,unsigned long (* round)(struct ccu_mux_internal *,struct clk_hw *,unsigned long *,unsigned long,void *),void * data)79 int ccu_mux_helper_determine_rate(struct ccu_common *common,
80 				  struct ccu_mux_internal *cm,
81 				  struct clk_rate_request *req,
82 				  unsigned long (*round)(struct ccu_mux_internal *,
83 							 struct clk_hw *,
84 							 unsigned long *,
85 							 unsigned long,
86 							 void *),
87 				  void *data)
88 {
89 	unsigned long best_parent_rate = 0, best_rate = 0;
90 	struct clk_hw *best_parent, *hw = &common->hw;
91 	unsigned int i;
92 
93 	if (clk_hw_get_flags(hw) & CLK_SET_RATE_NO_REPARENT) {
94 		unsigned long adj_parent_rate;
95 
96 		best_parent = clk_hw_get_parent(hw);
97 		best_parent_rate = clk_hw_get_rate(best_parent);
98 		adj_parent_rate = ccu_mux_helper_apply_prediv(common, cm, -1,
99 							      best_parent_rate);
100 
101 		best_rate = round(cm, best_parent, &adj_parent_rate,
102 				  req->rate, data);
103 
104 		/*
105 		 * adj_parent_rate might have been modified by our clock.
106 		 * Unapply the pre-divider if there's one, and give
107 		 * the actual frequency the parent needs to run at.
108 		 */
109 		best_parent_rate = ccu_mux_helper_unapply_prediv(common, cm, -1,
110 								 adj_parent_rate);
111 
112 		goto out;
113 	}
114 
115 	for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
116 		unsigned long tmp_rate, parent_rate;
117 		struct clk_hw *parent;
118 
119 		parent = clk_hw_get_parent_by_index(hw, i);
120 		if (!parent)
121 			continue;
122 
123 		parent_rate = ccu_mux_helper_apply_prediv(common, cm, i,
124 							  clk_hw_get_rate(parent));
125 
126 		tmp_rate = round(cm, parent, &parent_rate, req->rate, data);
127 
128 		/*
129 		 * parent_rate might have been modified by our clock.
130 		 * Unapply the pre-divider if there's one, and give
131 		 * the actual frequency the parent needs to run at.
132 		 */
133 		parent_rate = ccu_mux_helper_unapply_prediv(common, cm, i,
134 							    parent_rate);
135 		if (tmp_rate == req->rate) {
136 			best_parent = parent;
137 			best_parent_rate = parent_rate;
138 			best_rate = tmp_rate;
139 			goto out;
140 		}
141 
142 		if ((req->rate - tmp_rate) < (req->rate - best_rate)) {
143 			best_rate = tmp_rate;
144 			best_parent_rate = parent_rate;
145 			best_parent = parent;
146 		}
147 	}
148 
149 	if (best_rate == 0)
150 		return -EINVAL;
151 
152 out:
153 	req->best_parent_hw = best_parent;
154 	req->best_parent_rate = best_parent_rate;
155 	req->rate = best_rate;
156 	return 0;
157 }
158 
ccu_mux_helper_get_parent(struct ccu_common * common,struct ccu_mux_internal * cm)159 u8 ccu_mux_helper_get_parent(struct ccu_common *common,
160 			     struct ccu_mux_internal *cm)
161 {
162 	u32 reg;
163 	u8 parent;
164 
165 	reg = readl(common->base + common->reg);
166 	parent = reg >> cm->shift;
167 	parent &= (1 << cm->width) - 1;
168 
169 	if (cm->table) {
170 		int num_parents = clk_hw_get_num_parents(&common->hw);
171 		int i;
172 
173 		for (i = 0; i < num_parents; i++)
174 			if (cm->table[i] == parent)
175 				return i;
176 	}
177 
178 	return parent;
179 }
180 
ccu_mux_helper_set_parent(struct ccu_common * common,struct ccu_mux_internal * cm,u8 index)181 int ccu_mux_helper_set_parent(struct ccu_common *common,
182 			      struct ccu_mux_internal *cm,
183 			      u8 index)
184 {
185 	unsigned long flags;
186 	u32 reg;
187 
188 	if (cm->table)
189 		index = cm->table[index];
190 
191 	spin_lock_irqsave(common->lock, flags);
192 
193 	reg = readl(common->base + common->reg);
194 	reg &= ~GENMASK(cm->width + cm->shift - 1, cm->shift);
195 	writel(reg | (index << cm->shift), common->base + common->reg);
196 
197 	spin_unlock_irqrestore(common->lock, flags);
198 
199 	return 0;
200 }
201 
ccu_mux_disable(struct clk_hw * hw)202 static void ccu_mux_disable(struct clk_hw *hw)
203 {
204 	struct ccu_mux *cm = hw_to_ccu_mux(hw);
205 
206 	return ccu_gate_helper_disable(&cm->common, cm->enable);
207 }
208 
ccu_mux_enable(struct clk_hw * hw)209 static int ccu_mux_enable(struct clk_hw *hw)
210 {
211 	struct ccu_mux *cm = hw_to_ccu_mux(hw);
212 
213 	return ccu_gate_helper_enable(&cm->common, cm->enable);
214 }
215 
ccu_mux_is_enabled(struct clk_hw * hw)216 static int ccu_mux_is_enabled(struct clk_hw *hw)
217 {
218 	struct ccu_mux *cm = hw_to_ccu_mux(hw);
219 
220 	return ccu_gate_helper_is_enabled(&cm->common, cm->enable);
221 }
222 
ccu_mux_get_parent(struct clk_hw * hw)223 static u8 ccu_mux_get_parent(struct clk_hw *hw)
224 {
225 	struct ccu_mux *cm = hw_to_ccu_mux(hw);
226 
227 	return ccu_mux_helper_get_parent(&cm->common, &cm->mux);
228 }
229 
ccu_mux_set_parent(struct clk_hw * hw,u8 index)230 static int ccu_mux_set_parent(struct clk_hw *hw, u8 index)
231 {
232 	struct ccu_mux *cm = hw_to_ccu_mux(hw);
233 
234 	return ccu_mux_helper_set_parent(&cm->common, &cm->mux, index);
235 }
236 
ccu_mux_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)237 static unsigned long ccu_mux_recalc_rate(struct clk_hw *hw,
238 					 unsigned long parent_rate)
239 {
240 	struct ccu_mux *cm = hw_to_ccu_mux(hw);
241 
242 	return ccu_mux_helper_apply_prediv(&cm->common, &cm->mux, -1,
243 					   parent_rate);
244 }
245 
246 const struct clk_ops ccu_mux_ops = {
247 	.disable	= ccu_mux_disable,
248 	.enable		= ccu_mux_enable,
249 	.is_enabled	= ccu_mux_is_enabled,
250 
251 	.get_parent	= ccu_mux_get_parent,
252 	.set_parent	= ccu_mux_set_parent,
253 
254 	.determine_rate	= __clk_mux_determine_rate,
255 	.recalc_rate	= ccu_mux_recalc_rate,
256 };
257 
258 /*
259  * This clock notifier is called when the frequency of the of the parent
260  * PLL clock is to be changed. The idea is to switch the parent to a
261  * stable clock, such as the main oscillator, while the PLL frequency
262  * stabilizes.
263  */
ccu_mux_notifier_cb(struct notifier_block * nb,unsigned long event,void * data)264 static int ccu_mux_notifier_cb(struct notifier_block *nb,
265 			       unsigned long event, void *data)
266 {
267 	struct ccu_mux_nb *mux = to_ccu_mux_nb(nb);
268 	int ret = 0;
269 
270 	if (event == PRE_RATE_CHANGE) {
271 		mux->original_index = ccu_mux_helper_get_parent(mux->common,
272 								mux->cm);
273 		ret = ccu_mux_helper_set_parent(mux->common, mux->cm,
274 						mux->bypass_index);
275 	} else if (event == POST_RATE_CHANGE) {
276 		ret = ccu_mux_helper_set_parent(mux->common, mux->cm,
277 						mux->original_index);
278 	}
279 
280 	udelay(mux->delay_us);
281 
282 	return notifier_from_errno(ret);
283 }
284 
ccu_mux_notifier_register(struct clk * clk,struct ccu_mux_nb * mux_nb)285 int ccu_mux_notifier_register(struct clk *clk, struct ccu_mux_nb *mux_nb)
286 {
287 	mux_nb->clk_nb.notifier_call = ccu_mux_notifier_cb;
288 
289 	return clk_notifier_register(clk, &mux_nb->clk_nb);
290 }
291