]>
Commit | Line | Data |
---|---|---|
aa152335 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_mult.h" | |
15 | ||
b8302c72 MR |
16 | struct _ccu_mult { |
17 | unsigned long mult, max; | |
18 | }; | |
19 | ||
aa152335 | 20 | static void ccu_mult_find_best(unsigned long parent, unsigned long rate, |
b8302c72 | 21 | struct _ccu_mult *mult) |
aa152335 | 22 | { |
b8302c72 MR |
23 | int _mult; |
24 | ||
25 | _mult = rate / parent; | |
26 | if (_mult > mult->max) | |
27 | _mult = mult->max; | |
28 | ||
29 | mult->mult = _mult; | |
aa152335 MR |
30 | } |
31 | ||
32 | static unsigned long ccu_mult_round_rate(struct ccu_mux_internal *mux, | |
33 | unsigned long parent_rate, | |
34 | unsigned long rate, | |
35 | void *data) | |
36 | { | |
37 | struct ccu_mult *cm = data; | |
b8302c72 | 38 | struct _ccu_mult _cm; |
aa152335 | 39 | |
b8302c72 MR |
40 | _cm.max = 1 << cm->mult.width; |
41 | ccu_mult_find_best(parent_rate, rate, &_cm); | |
aa152335 | 42 | |
b8302c72 | 43 | return parent_rate * _cm.mult; |
aa152335 MR |
44 | } |
45 | ||
46 | static void ccu_mult_disable(struct clk_hw *hw) | |
47 | { | |
48 | struct ccu_mult *cm = hw_to_ccu_mult(hw); | |
49 | ||
50 | return ccu_gate_helper_disable(&cm->common, cm->enable); | |
51 | } | |
52 | ||
53 | static int ccu_mult_enable(struct clk_hw *hw) | |
54 | { | |
55 | struct ccu_mult *cm = hw_to_ccu_mult(hw); | |
56 | ||
57 | return ccu_gate_helper_enable(&cm->common, cm->enable); | |
58 | } | |
59 | ||
60 | static int ccu_mult_is_enabled(struct clk_hw *hw) | |
61 | { | |
62 | struct ccu_mult *cm = hw_to_ccu_mult(hw); | |
63 | ||
64 | return ccu_gate_helper_is_enabled(&cm->common, cm->enable); | |
65 | } | |
66 | ||
67 | static unsigned long ccu_mult_recalc_rate(struct clk_hw *hw, | |
68 | unsigned long parent_rate) | |
69 | { | |
70 | struct ccu_mult *cm = hw_to_ccu_mult(hw); | |
71 | unsigned long val; | |
72 | u32 reg; | |
73 | ||
74 | reg = readl(cm->common.base + cm->common.reg); | |
75 | val = reg >> cm->mult.shift; | |
76 | val &= (1 << cm->mult.width) - 1; | |
77 | ||
78 | ccu_mux_helper_adjust_parent_for_prediv(&cm->common, &cm->mux, -1, | |
79 | &parent_rate); | |
80 | ||
81 | return parent_rate * (val + 1); | |
82 | } | |
83 | ||
84 | static int ccu_mult_determine_rate(struct clk_hw *hw, | |
85 | struct clk_rate_request *req) | |
86 | { | |
87 | struct ccu_mult *cm = hw_to_ccu_mult(hw); | |
88 | ||
89 | return ccu_mux_helper_determine_rate(&cm->common, &cm->mux, | |
90 | req, ccu_mult_round_rate, cm); | |
91 | } | |
92 | ||
93 | static int ccu_mult_set_rate(struct clk_hw *hw, unsigned long rate, | |
94 | unsigned long parent_rate) | |
95 | { | |
96 | struct ccu_mult *cm = hw_to_ccu_mult(hw); | |
b8302c72 | 97 | struct _ccu_mult _cm; |
aa152335 | 98 | unsigned long flags; |
aa152335 MR |
99 | u32 reg; |
100 | ||
101 | ccu_mux_helper_adjust_parent_for_prediv(&cm->common, &cm->mux, -1, | |
102 | &parent_rate); | |
103 | ||
b8302c72 MR |
104 | _cm.max = 1 << cm->mult.width; |
105 | ccu_mult_find_best(parent_rate, rate, &_cm); | |
aa152335 MR |
106 | |
107 | spin_lock_irqsave(cm->common.lock, flags); | |
108 | ||
109 | reg = readl(cm->common.base + cm->common.reg); | |
110 | reg &= ~GENMASK(cm->mult.width + cm->mult.shift - 1, cm->mult.shift); | |
111 | ||
b8302c72 | 112 | writel(reg | ((_cm.mult - 1) << cm->mult.shift), |
aa152335 MR |
113 | cm->common.base + cm->common.reg); |
114 | ||
115 | spin_unlock_irqrestore(cm->common.lock, flags); | |
116 | ||
117 | return 0; | |
118 | } | |
119 | ||
120 | static u8 ccu_mult_get_parent(struct clk_hw *hw) | |
121 | { | |
122 | struct ccu_mult *cm = hw_to_ccu_mult(hw); | |
123 | ||
124 | return ccu_mux_helper_get_parent(&cm->common, &cm->mux); | |
125 | } | |
126 | ||
127 | static int ccu_mult_set_parent(struct clk_hw *hw, u8 index) | |
128 | { | |
129 | struct ccu_mult *cm = hw_to_ccu_mult(hw); | |
130 | ||
131 | return ccu_mux_helper_set_parent(&cm->common, &cm->mux, index); | |
132 | } | |
133 | ||
134 | const struct clk_ops ccu_mult_ops = { | |
135 | .disable = ccu_mult_disable, | |
136 | .enable = ccu_mult_enable, | |
137 | .is_enabled = ccu_mult_is_enabled, | |
138 | ||
139 | .get_parent = ccu_mult_get_parent, | |
140 | .set_parent = ccu_mult_set_parent, | |
141 | ||
142 | .determine_rate = ccu_mult_determine_rate, | |
143 | .recalc_rate = ccu_mult_recalc_rate, | |
144 | .set_rate = ccu_mult_set_rate, | |
145 | }; |