2 * Copyright (C) 2016 Maxime Ripard
3 * Maxime Ripard <maxime.ripard@free-electrons.com>
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.
11 #include <linux/clk.h>
12 #include <linux/clk-provider.h>
13 #include <linux/delay.h>
18 void ccu_mux_helper_adjust_parent_for_prediv(struct ccu_common *common,
19 struct ccu_mux_internal *cm,
21 unsigned long *parent_rate)
27 if (!((common->features & CCU_FEATURE_FIXED_PREDIV) ||
28 (common->features & CCU_FEATURE_VARIABLE_PREDIV)))
31 reg = readl(common->base + common->reg);
32 if (parent_index < 0) {
33 parent_index = reg >> cm->shift;
34 parent_index &= (1 << cm->width) - 1;
37 if (common->features & CCU_FEATURE_FIXED_PREDIV)
38 for (i = 0; i < cm->n_predivs; i++)
39 if (parent_index == cm->fixed_predivs[i].index)
40 prediv = cm->fixed_predivs[i].div;
42 if (common->features & CCU_FEATURE_VARIABLE_PREDIV)
43 if (parent_index == cm->variable_prediv.index) {
46 div = reg >> cm->variable_prediv.shift;
47 div &= (1 << cm->variable_prediv.width) - 1;
51 *parent_rate = *parent_rate / prediv;
54 int ccu_mux_helper_determine_rate(struct ccu_common *common,
55 struct ccu_mux_internal *cm,
56 struct clk_rate_request *req,
57 unsigned long (*round)(struct ccu_mux_internal *,
63 unsigned long best_parent_rate = 0, best_rate = 0;
64 struct clk_hw *best_parent, *hw = &common->hw;
67 for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
68 unsigned long tmp_rate, parent_rate;
69 struct clk_hw *parent;
71 parent = clk_hw_get_parent_by_index(hw, i);
75 parent_rate = clk_hw_get_rate(parent);
76 ccu_mux_helper_adjust_parent_for_prediv(common, cm, i,
79 tmp_rate = round(cm, clk_hw_get_rate(parent), req->rate, data);
80 if (tmp_rate == req->rate) {
82 best_parent_rate = parent_rate;
87 if ((req->rate - tmp_rate) < (req->rate - best_rate)) {
89 best_parent_rate = parent_rate;
98 req->best_parent_hw = best_parent;
99 req->best_parent_rate = best_parent_rate;
100 req->rate = best_rate;
104 u8 ccu_mux_helper_get_parent(struct ccu_common *common,
105 struct ccu_mux_internal *cm)
110 reg = readl(common->base + common->reg);
111 parent = reg >> cm->shift;
112 parent &= (1 << cm->width) - 1;
115 int num_parents = clk_hw_get_num_parents(&common->hw);
118 for (i = 0; i < num_parents; i++)
119 if (cm->table[i] == parent)
126 int ccu_mux_helper_set_parent(struct ccu_common *common,
127 struct ccu_mux_internal *cm,
134 index = cm->table[index];
136 spin_lock_irqsave(common->lock, flags);
138 reg = readl(common->base + common->reg);
139 reg &= ~GENMASK(cm->width + cm->shift - 1, cm->shift);
140 writel(reg | (index << cm->shift), common->base + common->reg);
142 spin_unlock_irqrestore(common->lock, flags);
147 static void ccu_mux_disable(struct clk_hw *hw)
149 struct ccu_mux *cm = hw_to_ccu_mux(hw);
151 return ccu_gate_helper_disable(&cm->common, cm->enable);
154 static int ccu_mux_enable(struct clk_hw *hw)
156 struct ccu_mux *cm = hw_to_ccu_mux(hw);
158 return ccu_gate_helper_enable(&cm->common, cm->enable);
161 static int ccu_mux_is_enabled(struct clk_hw *hw)
163 struct ccu_mux *cm = hw_to_ccu_mux(hw);
165 return ccu_gate_helper_is_enabled(&cm->common, cm->enable);
168 static u8 ccu_mux_get_parent(struct clk_hw *hw)
170 struct ccu_mux *cm = hw_to_ccu_mux(hw);
172 return ccu_mux_helper_get_parent(&cm->common, &cm->mux);
175 static int ccu_mux_set_parent(struct clk_hw *hw, u8 index)
177 struct ccu_mux *cm = hw_to_ccu_mux(hw);
179 return ccu_mux_helper_set_parent(&cm->common, &cm->mux, index);
182 static unsigned long ccu_mux_recalc_rate(struct clk_hw *hw,
183 unsigned long parent_rate)
185 struct ccu_mux *cm = hw_to_ccu_mux(hw);
187 ccu_mux_helper_adjust_parent_for_prediv(&cm->common, &cm->mux, -1,
193 const struct clk_ops ccu_mux_ops = {
194 .disable = ccu_mux_disable,
195 .enable = ccu_mux_enable,
196 .is_enabled = ccu_mux_is_enabled,
198 .get_parent = ccu_mux_get_parent,
199 .set_parent = ccu_mux_set_parent,
201 .determine_rate = __clk_mux_determine_rate,
202 .recalc_rate = ccu_mux_recalc_rate,
206 * This clock notifier is called when the frequency of the of the parent
207 * PLL clock is to be changed. The idea is to switch the parent to a
208 * stable clock, such as the main oscillator, while the PLL frequency
211 static int ccu_mux_notifier_cb(struct notifier_block *nb,
212 unsigned long event, void *data)
214 struct ccu_mux_nb *mux = to_ccu_mux_nb(nb);
217 if (event == PRE_RATE_CHANGE) {
218 mux->original_index = ccu_mux_helper_get_parent(mux->common,
220 ret = ccu_mux_helper_set_parent(mux->common, mux->cm,
222 } else if (event == POST_RATE_CHANGE) {
223 ret = ccu_mux_helper_set_parent(mux->common, mux->cm,
224 mux->original_index);
227 udelay(mux->delay_us);
229 return notifier_from_errno(ret);
232 int ccu_mux_notifier_register(struct clk *clk, struct ccu_mux_nb *mux_nb)
234 mux_nb->clk_nb.notifier_call = ccu_mux_notifier_cb;
236 return clk_notifier_register(clk, &mux_nb->clk_nb);