Merge tag 'v4.20' into for-linus
[muen/linux.git] / drivers / clk / mmp / clk-frac.c
1 /*
2  * mmp factor clock operation source file
3  *
4  * Copyright (C) 2012 Marvell
5  * Chao Xie <xiechao.mail@gmail.com>
6  *
7  * This file is licensed under the terms of the GNU General Public
8  * License version 2. This program is licensed "as is" without any
9  * warranty of any kind, whether express or implied.
10  */
11
12 #include <linux/clk-provider.h>
13 #include <linux/slab.h>
14 #include <linux/io.h>
15 #include <linux/err.h>
16
17 #include "clk.h"
18 /*
19  * It is M/N clock
20  *
21  * Fout from synthesizer can be given from two equations:
22  * numerator/denominator = Fin / (Fout * factor)
23  */
24
25 #define to_clk_factor(hw) container_of(hw, struct mmp_clk_factor, hw)
26
27 static long clk_factor_round_rate(struct clk_hw *hw, unsigned long drate,
28                 unsigned long *prate)
29 {
30         struct mmp_clk_factor *factor = to_clk_factor(hw);
31         unsigned long rate = 0, prev_rate;
32         int i;
33
34         for (i = 0; i < factor->ftbl_cnt; i++) {
35                 prev_rate = rate;
36                 rate = (((*prate / 10000) * factor->ftbl[i].den) /
37                         (factor->ftbl[i].num * factor->masks->factor)) * 10000;
38                 if (rate > drate)
39                         break;
40         }
41         if ((i == 0) || (i == factor->ftbl_cnt)) {
42                 return rate;
43         } else {
44                 if ((drate - prev_rate) > (rate - drate))
45                         return rate;
46                 else
47                         return prev_rate;
48         }
49 }
50
51 static unsigned long clk_factor_recalc_rate(struct clk_hw *hw,
52                 unsigned long parent_rate)
53 {
54         struct mmp_clk_factor *factor = to_clk_factor(hw);
55         struct mmp_clk_factor_masks *masks = factor->masks;
56         unsigned int val, num, den;
57
58         val = readl_relaxed(factor->base);
59
60         /* calculate numerator */
61         num = (val >> masks->num_shift) & masks->num_mask;
62
63         /* calculate denominator */
64         den = (val >> masks->den_shift) & masks->den_mask;
65
66         if (!den)
67                 return 0;
68
69         return (((parent_rate / 10000)  * den) /
70                         (num * factor->masks->factor)) * 10000;
71 }
72
73 /* Configures new clock rate*/
74 static int clk_factor_set_rate(struct clk_hw *hw, unsigned long drate,
75                                 unsigned long prate)
76 {
77         struct mmp_clk_factor *factor = to_clk_factor(hw);
78         struct mmp_clk_factor_masks *masks = factor->masks;
79         int i;
80         unsigned long val;
81         unsigned long prev_rate, rate = 0;
82         unsigned long flags = 0;
83
84         for (i = 0; i < factor->ftbl_cnt; i++) {
85                 prev_rate = rate;
86                 rate = (((prate / 10000) * factor->ftbl[i].den) /
87                         (factor->ftbl[i].num * factor->masks->factor)) * 10000;
88                 if (rate > drate)
89                         break;
90         }
91         if (i > 0)
92                 i--;
93
94         if (factor->lock)
95                 spin_lock_irqsave(factor->lock, flags);
96
97         val = readl_relaxed(factor->base);
98
99         val &= ~(masks->num_mask << masks->num_shift);
100         val |= (factor->ftbl[i].num & masks->num_mask) << masks->num_shift;
101
102         val &= ~(masks->den_mask << masks->den_shift);
103         val |= (factor->ftbl[i].den & masks->den_mask) << masks->den_shift;
104
105         writel_relaxed(val, factor->base);
106
107         if (factor->lock)
108                 spin_unlock_irqrestore(factor->lock, flags);
109
110         return 0;
111 }
112
113 static void clk_factor_init(struct clk_hw *hw)
114 {
115         struct mmp_clk_factor *factor = to_clk_factor(hw);
116         struct mmp_clk_factor_masks *masks = factor->masks;
117         u32 val, num, den;
118         int i;
119         unsigned long flags = 0;
120
121         if (factor->lock)
122                 spin_lock_irqsave(factor->lock, flags);
123
124         val = readl(factor->base);
125
126         /* calculate numerator */
127         num = (val >> masks->num_shift) & masks->num_mask;
128
129         /* calculate denominator */
130         den = (val >> masks->den_shift) & masks->den_mask;
131
132         for (i = 0; i < factor->ftbl_cnt; i++)
133                 if (den == factor->ftbl[i].den && num == factor->ftbl[i].num)
134                         break;
135
136         if (i >= factor->ftbl_cnt) {
137                 val &= ~(masks->num_mask << masks->num_shift);
138                 val |= (factor->ftbl[0].num & masks->num_mask) <<
139                         masks->num_shift;
140
141                 val &= ~(masks->den_mask << masks->den_shift);
142                 val |= (factor->ftbl[0].den & masks->den_mask) <<
143                         masks->den_shift;
144
145                 writel(val, factor->base);
146         }
147
148         if (factor->lock)
149                 spin_unlock_irqrestore(factor->lock, flags);
150 }
151
152 static const struct clk_ops clk_factor_ops = {
153         .recalc_rate = clk_factor_recalc_rate,
154         .round_rate = clk_factor_round_rate,
155         .set_rate = clk_factor_set_rate,
156         .init = clk_factor_init,
157 };
158
159 struct clk *mmp_clk_register_factor(const char *name, const char *parent_name,
160                 unsigned long flags, void __iomem *base,
161                 struct mmp_clk_factor_masks *masks,
162                 struct mmp_clk_factor_tbl *ftbl,
163                 unsigned int ftbl_cnt, spinlock_t *lock)
164 {
165         struct mmp_clk_factor *factor;
166         struct clk_init_data init;
167         struct clk *clk;
168
169         if (!masks) {
170                 pr_err("%s: must pass a clk_factor_mask\n", __func__);
171                 return ERR_PTR(-EINVAL);
172         }
173
174         factor = kzalloc(sizeof(*factor), GFP_KERNEL);
175         if (!factor)
176                 return ERR_PTR(-ENOMEM);
177
178         /* struct clk_aux assignments */
179         factor->base = base;
180         factor->masks = masks;
181         factor->ftbl = ftbl;
182         factor->ftbl_cnt = ftbl_cnt;
183         factor->hw.init = &init;
184         factor->lock = lock;
185
186         init.name = name;
187         init.ops = &clk_factor_ops;
188         init.flags = flags;
189         init.parent_names = &parent_name;
190         init.num_parents = 1;
191
192         clk = clk_register(NULL, &factor->hw);
193         if (IS_ERR_OR_NULL(clk))
194                 kfree(factor);
195
196         return clk;
197 }