1 /*
2 * Copyright (C) 2016 Maxime Ripard
3 * Maxime Ripard <maxime.ripard@free-electrons.com>
4 *
5 * This program is free software; you can redistribute it and/or
6 * modify it under the terms of the GNU General Public License as
7 * published by the Free Software Foundation; either version 2 of
8 * the License, or (at your option) any later version.
9 */
10
11 #include <linux/clk-provider.h>
12 #include <linux/rational.h>
13
14 #include "ccu_gate.h"
15 #include "ccu_nkmp.h"
16
17 struct _ccu_nkmp {
18 unsigned long n, max_n;
19 unsigned long k, max_k;
20 unsigned long m, max_m;
21 unsigned long p, max_p;
22 };
23
ccu_nkmp_find_best(unsigned long parent,unsigned long rate,struct _ccu_nkmp * nkmp)24 static void ccu_nkmp_find_best(unsigned long parent, unsigned long rate,
25 struct _ccu_nkmp *nkmp)
26 {
27 unsigned long best_rate = 0;
28 unsigned long best_n = 0, best_k = 0, best_m = 0, best_p = 0;
29 unsigned long _n, _k, _m, _p;
30
31 for (_k = 1; _k <= nkmp->max_k; _k++) {
32 for (_p = 1; _p <= nkmp->max_p; _p <<= 1) {
33 unsigned long tmp_rate;
34
35 rational_best_approximation(rate / _k, parent / _p,
36 nkmp->max_n, nkmp->max_m,
37 &_n, &_m);
38
39 tmp_rate = parent * _n * _k / (_m * _p);
40
41 if (tmp_rate > rate)
42 continue;
43
44 if ((rate - tmp_rate) < (rate - best_rate)) {
45 best_rate = tmp_rate;
46 best_n = _n;
47 best_k = _k;
48 best_m = _m;
49 best_p = _p;
50 }
51 }
52 }
53
54 nkmp->n = best_n;
55 nkmp->k = best_k;
56 nkmp->m = best_m;
57 nkmp->p = best_p;
58 }
59
ccu_nkmp_disable(struct clk_hw * hw)60 static void ccu_nkmp_disable(struct clk_hw *hw)
61 {
62 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
63
64 return ccu_gate_helper_disable(&nkmp->common, nkmp->enable);
65 }
66
ccu_nkmp_enable(struct clk_hw * hw)67 static int ccu_nkmp_enable(struct clk_hw *hw)
68 {
69 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
70
71 return ccu_gate_helper_enable(&nkmp->common, nkmp->enable);
72 }
73
ccu_nkmp_is_enabled(struct clk_hw * hw)74 static int ccu_nkmp_is_enabled(struct clk_hw *hw)
75 {
76 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
77
78 return ccu_gate_helper_is_enabled(&nkmp->common, nkmp->enable);
79 }
80
ccu_nkmp_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)81 static unsigned long ccu_nkmp_recalc_rate(struct clk_hw *hw,
82 unsigned long parent_rate)
83 {
84 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
85 unsigned long n, m, k, p;
86 u32 reg;
87
88 reg = readl(nkmp->common.base + nkmp->common.reg);
89
90 n = reg >> nkmp->n.shift;
91 n &= (1 << nkmp->n.width) - 1;
92
93 k = reg >> nkmp->k.shift;
94 k &= (1 << nkmp->k.width) - 1;
95
96 m = reg >> nkmp->m.shift;
97 m &= (1 << nkmp->m.width) - 1;
98
99 p = reg >> nkmp->p.shift;
100 p &= (1 << nkmp->p.width) - 1;
101
102 return (parent_rate * (n + 1) * (k + 1) >> p) / (m + 1);
103 }
104
ccu_nkmp_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * parent_rate)105 static long ccu_nkmp_round_rate(struct clk_hw *hw, unsigned long rate,
106 unsigned long *parent_rate)
107 {
108 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
109 struct _ccu_nkmp _nkmp;
110
111 _nkmp.max_n = 1 << nkmp->n.width;
112 _nkmp.max_k = 1 << nkmp->k.width;
113 _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
114 _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
115
116 ccu_nkmp_find_best(*parent_rate, rate, &_nkmp);
117
118 return *parent_rate * _nkmp.n * _nkmp.k / (_nkmp.m * _nkmp.p);
119 }
120
ccu_nkmp_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)121 static int ccu_nkmp_set_rate(struct clk_hw *hw, unsigned long rate,
122 unsigned long parent_rate)
123 {
124 struct ccu_nkmp *nkmp = hw_to_ccu_nkmp(hw);
125 struct _ccu_nkmp _nkmp;
126 unsigned long flags;
127 u32 reg;
128
129 _nkmp.max_n = 1 << nkmp->n.width;
130 _nkmp.max_k = 1 << nkmp->k.width;
131 _nkmp.max_m = nkmp->m.max ?: 1 << nkmp->m.width;
132 _nkmp.max_p = nkmp->p.max ?: 1 << ((1 << nkmp->p.width) - 1);
133
134 ccu_nkmp_find_best(parent_rate, rate, &_nkmp);
135
136 spin_lock_irqsave(nkmp->common.lock, flags);
137
138 reg = readl(nkmp->common.base + nkmp->common.reg);
139 reg &= ~GENMASK(nkmp->n.width + nkmp->n.shift - 1, nkmp->n.shift);
140 reg &= ~GENMASK(nkmp->k.width + nkmp->k.shift - 1, nkmp->k.shift);
141 reg &= ~GENMASK(nkmp->m.width + nkmp->m.shift - 1, nkmp->m.shift);
142 reg &= ~GENMASK(nkmp->p.width + nkmp->p.shift - 1, nkmp->p.shift);
143
144 reg |= (_nkmp.n - 1) << nkmp->n.shift;
145 reg |= (_nkmp.k - 1) << nkmp->k.shift;
146 reg |= (_nkmp.m - 1) << nkmp->m.shift;
147 reg |= ilog2(_nkmp.p) << nkmp->p.shift;
148
149 writel(reg, nkmp->common.base + nkmp->common.reg);
150
151 spin_unlock_irqrestore(nkmp->common.lock, flags);
152
153 ccu_helper_wait_for_lock(&nkmp->common, nkmp->lock);
154
155 return 0;
156 }
157
158 const struct clk_ops ccu_nkmp_ops = {
159 .disable = ccu_nkmp_disable,
160 .enable = ccu_nkmp_enable,
161 .is_enabled = ccu_nkmp_is_enabled,
162
163 .recalc_rate = ccu_nkmp_recalc_rate,
164 .round_rate = ccu_nkmp_round_rate,
165 .set_rate = ccu_nkmp_set_rate,
166 };
167