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