Loading...
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/io.h>
11#include <linux/kernel.h>
12#include <linux/of.h>
13#include <linux/slab.h>
14
15static inline u32 clk_mult_readl(struct clk_multiplier *mult)
16{
17 if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
18 return ioread32be(mult->reg);
19
20 return readl(mult->reg);
21}
22
23static inline void clk_mult_writel(struct clk_multiplier *mult, u32 val)
24{
25 if (mult->flags & CLK_MULTIPLIER_BIG_ENDIAN)
26 iowrite32be(val, mult->reg);
27 else
28 writel(val, mult->reg);
29}
30
31static unsigned long __get_mult(struct clk_multiplier *mult,
32 unsigned long rate,
33 unsigned long parent_rate)
34{
35 if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST)
36 return DIV_ROUND_CLOSEST(rate, parent_rate);
37
38 return rate / parent_rate;
39}
40
41static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw,
42 unsigned long parent_rate)
43{
44 struct clk_multiplier *mult = to_clk_multiplier(hw);
45 unsigned long val;
46
47 val = clk_mult_readl(mult) >> mult->shift;
48 val &= GENMASK(mult->width - 1, 0);
49
50 if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)
51 val = 1;
52
53 return parent_rate * val;
54}
55
56static bool __is_best_rate(unsigned long rate, unsigned long new,
57 unsigned long best, unsigned long flags)
58{
59 if (flags & CLK_MULTIPLIER_ROUND_CLOSEST)
60 return abs(rate - new) < abs(rate - best);
61
62 return new >= rate && new < best;
63}
64
65static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,
66 unsigned long *best_parent_rate,
67 u8 width, unsigned long flags)
68{
69 struct clk_multiplier *mult = to_clk_multiplier(hw);
70 unsigned long orig_parent_rate = *best_parent_rate;
71 unsigned long parent_rate, current_rate, best_rate = ~0;
72 unsigned int i, bestmult = 0;
73 unsigned int maxmult = (1 << width) - 1;
74
75 if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT)) {
76 bestmult = rate / orig_parent_rate;
77
78 /* Make sure we don't end up with a 0 multiplier */
79 if ((bestmult == 0) &&
80 !(mult->flags & CLK_MULTIPLIER_ZERO_BYPASS))
81 bestmult = 1;
82
83 /* Make sure we don't overflow the multiplier */
84 if (bestmult > maxmult)
85 bestmult = maxmult;
86
87 return bestmult;
88 }
89
90 for (i = 1; i < maxmult; i++) {
91 if (rate == orig_parent_rate * i) {
92 /*
93 * This is the best case for us if we have a
94 * perfect match without changing the parent
95 * rate.
96 */
97 *best_parent_rate = orig_parent_rate;
98 return i;
99 }
100
101 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
102 rate / i);
103 current_rate = parent_rate * i;
104
105 if (__is_best_rate(rate, current_rate, best_rate, flags)) {
106 bestmult = i;
107 best_rate = current_rate;
108 *best_parent_rate = parent_rate;
109 }
110 }
111
112 return bestmult;
113}
114
115static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
116 unsigned long *parent_rate)
117{
118 struct clk_multiplier *mult = to_clk_multiplier(hw);
119 unsigned long factor = __bestmult(hw, rate, parent_rate,
120 mult->width, mult->flags);
121
122 return *parent_rate * factor;
123}
124
125static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
126 unsigned long parent_rate)
127{
128 struct clk_multiplier *mult = to_clk_multiplier(hw);
129 unsigned long factor = __get_mult(mult, rate, parent_rate);
130 unsigned long flags = 0;
131 unsigned long val;
132
133 if (mult->lock)
134 spin_lock_irqsave(mult->lock, flags);
135 else
136 __acquire(mult->lock);
137
138 val = clk_mult_readl(mult);
139 val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
140 val |= factor << mult->shift;
141 clk_mult_writel(mult, val);
142
143 if (mult->lock)
144 spin_unlock_irqrestore(mult->lock, flags);
145 else
146 __release(mult->lock);
147
148 return 0;
149}
150
151const struct clk_ops clk_multiplier_ops = {
152 .recalc_rate = clk_multiplier_recalc_rate,
153 .round_rate = clk_multiplier_round_rate,
154 .set_rate = clk_multiplier_set_rate,
155};
156EXPORT_SYMBOL_GPL(clk_multiplier_ops);
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
17static unsigned long __get_mult(struct clk_multiplier *mult,
18 unsigned long rate,
19 unsigned long parent_rate)
20{
21 if (mult->flags & CLK_MULTIPLIER_ROUND_CLOSEST)
22 return DIV_ROUND_CLOSEST(rate, parent_rate);
23
24 return rate / parent_rate;
25}
26
27static unsigned long clk_multiplier_recalc_rate(struct clk_hw *hw,
28 unsigned long parent_rate)
29{
30 struct clk_multiplier *mult = to_clk_multiplier(hw);
31 unsigned long val;
32
33 val = clk_readl(mult->reg) >> mult->shift;
34 val &= GENMASK(mult->width - 1, 0);
35
36 if (!val && mult->flags & CLK_MULTIPLIER_ZERO_BYPASS)
37 val = 1;
38
39 return parent_rate * val;
40}
41
42static bool __is_best_rate(unsigned long rate, unsigned long new,
43 unsigned long best, unsigned long flags)
44{
45 if (flags & CLK_MULTIPLIER_ROUND_CLOSEST)
46 return abs(rate - new) < abs(rate - best);
47
48 return new >= rate && new < best;
49}
50
51static unsigned long __bestmult(struct clk_hw *hw, unsigned long rate,
52 unsigned long *best_parent_rate,
53 u8 width, unsigned long flags)
54{
55 unsigned long orig_parent_rate = *best_parent_rate;
56 unsigned long parent_rate, current_rate, best_rate = ~0;
57 unsigned int i, bestmult = 0;
58
59 if (!(clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT))
60 return rate / *best_parent_rate;
61
62 for (i = 1; i < ((1 << width) - 1); i++) {
63 if (rate == orig_parent_rate * i) {
64 /*
65 * This is the best case for us if we have a
66 * perfect match without changing the parent
67 * rate.
68 */
69 *best_parent_rate = orig_parent_rate;
70 return i;
71 }
72
73 parent_rate = clk_hw_round_rate(clk_hw_get_parent(hw),
74 rate / i);
75 current_rate = parent_rate * i;
76
77 if (__is_best_rate(rate, current_rate, best_rate, flags)) {
78 bestmult = i;
79 best_rate = current_rate;
80 *best_parent_rate = parent_rate;
81 }
82 }
83
84 return bestmult;
85}
86
87static long clk_multiplier_round_rate(struct clk_hw *hw, unsigned long rate,
88 unsigned long *parent_rate)
89{
90 struct clk_multiplier *mult = to_clk_multiplier(hw);
91 unsigned long factor = __bestmult(hw, rate, parent_rate,
92 mult->width, mult->flags);
93
94 return *parent_rate * factor;
95}
96
97static int clk_multiplier_set_rate(struct clk_hw *hw, unsigned long rate,
98 unsigned long parent_rate)
99{
100 struct clk_multiplier *mult = to_clk_multiplier(hw);
101 unsigned long factor = __get_mult(mult, rate, parent_rate);
102 unsigned long flags = 0;
103 unsigned long val;
104
105 if (mult->lock)
106 spin_lock_irqsave(mult->lock, flags);
107 else
108 __acquire(mult->lock);
109
110 val = clk_readl(mult->reg);
111 val &= ~GENMASK(mult->width + mult->shift - 1, mult->shift);
112 val |= factor << mult->shift;
113 clk_writel(val, mult->reg);
114
115 if (mult->lock)
116 spin_unlock_irqrestore(mult->lock, flags);
117 else
118 __release(mult->lock);
119
120 return 0;
121}
122
123const struct clk_ops clk_multiplier_ops = {
124 .recalc_rate = clk_multiplier_recalc_rate,
125 .round_rate = clk_multiplier_round_rate,
126 .set_rate = clk_multiplier_set_rate,
127};
128EXPORT_SYMBOL_GPL(clk_multiplier_ops);