Commit | Line | Data |
---|---|---|
f2e0a532 MR |
1 | /* |
2 | * Copyright (C) 2015 Maxime Ripard <maxime.ripard@free-electrons.com> | |
3 | * | |
4 | * This program is free software; you can redistribute it and/or modify | |
5 | * it under the terms of the GNU General Public License version 2 as | |
6 | * published by the Free Software Foundation. | |
7 | */ | |
8 | ||
9 | #include <linux/bitops.h> | |
10 | #include <linux/clk-provider.h> | |
11 | #include <linux/err.h> | |
12 | #include <linux/export.h> | |
13 | #include <linux/kernel.h> | |
14 | #include <linux/of.h> | |
15 | #include <linux/slab.h> | |
16 | ||
17 | #define to_clk_multiplier(_hw) container_of(_hw, struct clk_multiplier, hw) | |
18 | ||
19 | static unsigned long __get_mult(struct clk_multiplier *mult, | |
20 | unsigned long rate, | |
21 | unsigned long parent_rate) | |
22 | { | |
23 | if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST) | |
24 | return DIV_ROUND_CLOSEST(rate, parent_rate); | |
25 | ||
26 | return rate / parent_rate; | |
27 | } | |
28 | ||
29 | static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw, | |
30 | unsigned long parent_rate) | |
31 | { | |
32 | struct clk_multiplier *mult = to_clk_multiplier(hw); | |
33 | unsigned long val; | |
34 | ||
35 | val = clk_readl(mult->reg) >> mult->shift; | |
36 | val &= GENMASK(mult->width - 1, 0); | |
37 | ||
38 | if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS) | |
39 | val = 1; | |
40 | ||
41 | return parent_rate * val; | |
42 | } | |
43 | ||
44 | static bool __is_best_rate(unsigned long rate, unsigned long new, | |
45 | unsigned long best, unsigned long flags) | |
46 | { | |
47 | if (flags & CLK_MULTIPLIER_ROUND_CLOSEST) | |
48 | return abs(rate - new) < abs(rate - best); | |
49 | ||
50 | return new >= rate && new < best; | |
51 | } | |
52 | ||
53 | static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate, | |
54 | unsigned long *best_parent_rate, | |
55 | u8 width, unsigned long flags) | |
56 | { | |
57 | unsigned long orig_parent_rate = *best_parent_rate; | |
58 | unsigned long parent_rate, current_rate, best_rate = ~0; | |
59 | unsigned int i, bestmult = 0; | |
60 | ||
61 | if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) | |
62 | return rate / *best_parent_rate; | |
63 | ||
64 | for (i = 1; i < ((1 << width) - 1); i++) { | |
65 | if (rate == orig_parent_rate * i) { | |
66 | /* | |
67 | * This is the best case for us if we have a | |
68 | * perfect match without changing the parent | |
69 | * rate. | |
70 | */ | |
71 | *best_parent_rate = orig_parent_rate; | |
72 | return i; | |
73 | } | |
74 | ||
75 | parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw), | |
76 | rate / i); | |
77 | current_rate = parent_rate * i; | |
78 | ||
79 | if (__is_best_rate(rate, current_rate, best_rate, flags)) { | |
80 | bestmult = i; | |
81 | best_rate = current_rate; | |
82 | *best_parent_rate = parent_rate; | |
83 | } | |
84 | } | |
85 | ||
86 | return bestmult; | |
87 | } | |
88 | ||
89 | static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate, | |
90 | unsigned long *parent_rate) | |
91 | { | |
92 | struct clk_multiplier *mult = to_clk_multiplier(hw); | |
93 | unsigned long factor = __bestmult(hw, rate, parent_rate, | |
94 | mult->width, mult->flags); | |
95 | ||
96 | return *parent_rate * factor; | |
97 | } | |
98 | ||
99 | static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate, | |
100 | unsigned long parent_rate) | |
101 | { | |
102 | struct clk_multiplier *mult = to_clk_multiplier(hw); | |
103 | unsigned long factor = __get_mult(mult, rate, parent_rate); | |
104 | unsigned long flags = 0; | |
105 | unsigned long val; | |
106 | ||
107 | if (mult->lock) | |
108 | spin_lock_irqsave(mult->lock, flags); | |
109 | else | |
110 | __acquire(mult->lock); | |
111 | ||
112 | val = clk_readl(mult->reg); | |
113 | val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift); | |
114 | val |= factor << mult->shift; | |
115 | clk_writel(val, mult->reg); | |
116 | ||
117 | if (mult->lock) | |
118 | spin_unlock_irqrestore(mult->lock, flags); | |
119 | else | |
120 | __release(mult->lock); | |
121 | ||
122 | return 0; | |
123 | } | |
124 | ||
125 | const struct clk_ops clk_multiplier_ops = { | |
126 | .recalc_rate = clk_multiplier_recalc_rate, | |
127 | .round_rate = clk_multiplier_round_rate, | |
128 | .set_rate = clk_multiplier_set_rate, | |
129 | }; | |
130 | EXPORT_SYMBOL_GPL(clk_multiplier_ops); |