1 // SPDX-License-Identifier: GPL-2.0
2 /*
3 * Copyright (c) 2019 Rockchip Electronics Co. Ltd.
4 *
5 * Base on code in drivers/clk/clk-fractional-divider.c.
6 * See clk-fractional-divider.c for further copyright information.
7 */
8
9 #include <linux/rational.h>
10
11 #include "clk-regmap.h"
12
13 #define to_clk_regmap_fractional_divider(_hw) \
14 container_of(_hw, struct clk_regmap_fractional_divider, hw)
15
16 static unsigned long
clk_regmap_fractional_divider_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)17 clk_regmap_fractional_divider_recalc_rate(struct clk_hw *hw,
18 unsigned long parent_rate)
19 {
20 struct clk_regmap_fractional_divider *fd =
21 to_clk_regmap_fractional_divider(hw);
22 unsigned long m, n;
23 u32 val;
24 u64 ret;
25
26 regmap_read(fd->regmap, fd->reg, &val);
27
28 m = (val & fd->mmask) >> fd->mshift;
29 n = (val & fd->nmask) >> fd->nshift;
30
31 if (!n || !m)
32 return parent_rate;
33
34 ret = (u64)parent_rate * m;
35 do_div(ret, n);
36
37 return ret;
38 }
39
clk_regmap_fractional_divider_approximation(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate,unsigned long * m,unsigned long * n)40 static void clk_regmap_fractional_divider_approximation(struct clk_hw *hw,
41 unsigned long rate, unsigned long *parent_rate,
42 unsigned long *m, unsigned long *n)
43 {
44 struct clk_regmap_fractional_divider *fd =
45 to_clk_regmap_fractional_divider(hw);
46 unsigned long p_rate, p_parent_rate;
47 struct clk_hw *p_parent;
48 unsigned long scale;
49
50 p_rate = clk_hw_get_rate(clk_hw_get_parent(hw));
51 if ((rate * 20 > p_rate) && (p_rate % rate != 0)) {
52 p_parent = clk_hw_get_parent(clk_hw_get_parent(hw));
53 p_parent_rate = clk_hw_get_rate(p_parent);
54 *parent_rate = p_parent_rate;
55 }
56
57 /*
58 * Get rate closer to *parent_rate to guarantee there is no overflow
59 * for m and n. In the result it will be the nearest rate left shifted
60 * by (scale - fd->nwidth) bits.
61 */
62 scale = fls_long(*parent_rate / rate - 1);
63 if (scale > fd->nwidth)
64 rate <<= scale - fd->nwidth;
65
66 rational_best_approximation(rate, *parent_rate,
67 GENMASK(fd->mwidth - 1, 0),
68 GENMASK(fd->nwidth - 1, 0),
69 m, n);
70 }
71
72 static long
clk_regmap_fractional_divider_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate)73 clk_regmap_fractional_divider_round_rate(struct clk_hw *hw, unsigned long rate,
74 unsigned long *parent_rate)
75 {
76 unsigned long m, n;
77 u64 ret;
78
79 if (!rate)
80 return *parent_rate;
81
82 if (rate >= *parent_rate)
83 return *parent_rate;
84
85 clk_regmap_fractional_divider_approximation(hw, rate, parent_rate,
86 &m, &n);
87
88 ret = (u64)*parent_rate * m;
89 do_div(ret, n);
90
91 return ret;
92 }
93
94 static int
clk_regmap_fractional_divider_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)95 clk_regmap_fractional_divider_set_rate(struct clk_hw *hw, unsigned long rate,
96 unsigned long parent_rate)
97 {
98 struct clk_regmap_fractional_divider *fd =
99 to_clk_regmap_fractional_divider(hw);
100 unsigned long m, n;
101 u32 val;
102
103 rational_best_approximation(rate, parent_rate,
104 GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
105 &m, &n);
106
107 dev_dbg(fd->dev, "%s: parent_rate=%ld, m=%ld, n=%ld, rate=%ld\n",
108 clk_hw_get_name(hw), parent_rate, m, n, rate);
109
110 regmap_read(fd->regmap, fd->reg, &val);
111 val &= ~(fd->mmask | fd->nmask);
112 val |= (m << fd->mshift) | (n << fd->nshift);
113
114 return regmap_write(fd->regmap, fd->reg, val);
115 }
116
117 const struct clk_ops clk_regmap_fractional_divider_ops = {
118 .recalc_rate = clk_regmap_fractional_divider_recalc_rate,
119 .round_rate = clk_regmap_fractional_divider_round_rate,
120 .set_rate = clk_regmap_fractional_divider_set_rate,
121 };
122 EXPORT_SYMBOL_GPL(clk_regmap_fractional_divider_ops);
123
124 struct clk *
devm_clk_regmap_register_fractional_divider(struct device * dev,const char * name,const char * parent_name,struct regmap * regmap,u32 reg,unsigned long flags)125 devm_clk_regmap_register_fractional_divider(struct device *dev,
126 const char *name,
127 const char *parent_name,
128 struct regmap *regmap,
129 u32 reg, unsigned long flags)
130 {
131 struct clk_regmap_fractional_divider *fd;
132 struct clk_init_data init;
133
134 fd = devm_kzalloc(dev, sizeof(*fd), GFP_KERNEL);
135 if (!fd)
136 return ERR_PTR(-ENOMEM);
137
138 init.name = name;
139 init.ops = &clk_regmap_fractional_divider_ops;
140 init.flags = flags;
141 init.parent_names = (parent_name ? &parent_name : NULL);
142 init.num_parents = (parent_name ? 1 : 0);
143
144 fd->dev = dev;
145 fd->regmap = regmap;
146 fd->reg = reg;
147 fd->mshift = 16;
148 fd->mwidth = 16;
149 fd->mmask = GENMASK(fd->mwidth - 1, 0) << fd->mshift;
150 fd->nshift = 0;
151 fd->nwidth = 16;
152 fd->nmask = GENMASK(fd->nwidth - 1, 0) << fd->nshift;
153 fd->hw.init = &init;
154
155 return devm_clk_register(dev, &fd->hw);
156 }
157 EXPORT_SYMBOL_GPL(devm_clk_regmap_register_fractional_divider);
158