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 
13 #include "ccu_gate.h"
14 #include "ccu_nkm.h"
15 
16 struct _ccu_nkm {
17 	unsigned long	n, min_n, max_n;
18 	unsigned long	k, min_k, max_k;
19 	unsigned long	m, min_m, max_m;
20 };
21 
ccu_nkm_find_best(unsigned long parent,unsigned long rate,struct _ccu_nkm * nkm)22 static void ccu_nkm_find_best(unsigned long parent, unsigned long rate,
23 			      struct _ccu_nkm *nkm)
24 {
25 	unsigned long best_rate = 0;
26 	unsigned long best_n = 0, best_k = 0, best_m = 0;
27 	unsigned long _n, _k, _m;
28 
29 	for (_k = nkm->min_k; _k <= nkm->max_k; _k++) {
30 		for (_n = nkm->min_n; _n <= nkm->max_n; _n++) {
31 			for (_m = nkm->min_m; _m <= nkm->max_m; _m++) {
32 				unsigned long tmp_rate;
33 
34 				tmp_rate = parent * _n * _k / _m;
35 
36 				if (tmp_rate > rate)
37 					continue;
38 				if ((rate - tmp_rate) < (rate - best_rate)) {
39 					best_rate = tmp_rate;
40 					best_n = _n;
41 					best_k = _k;
42 					best_m = _m;
43 				}
44 			}
45 		}
46 	}
47 
48 	nkm->n = best_n;
49 	nkm->k = best_k;
50 	nkm->m = best_m;
51 }
52 
ccu_nkm_disable(struct clk_hw * hw)53 static void ccu_nkm_disable(struct clk_hw *hw)
54 {
55 	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
56 
57 	return ccu_gate_helper_disable(&nkm->common, nkm->enable);
58 }
59 
ccu_nkm_enable(struct clk_hw * hw)60 static int ccu_nkm_enable(struct clk_hw *hw)
61 {
62 	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
63 
64 	return ccu_gate_helper_enable(&nkm->common, nkm->enable);
65 }
66 
ccu_nkm_is_enabled(struct clk_hw * hw)67 static int ccu_nkm_is_enabled(struct clk_hw *hw)
68 {
69 	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
70 
71 	return ccu_gate_helper_is_enabled(&nkm->common, nkm->enable);
72 }
73 
ccu_nkm_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)74 static unsigned long ccu_nkm_recalc_rate(struct clk_hw *hw,
75 					unsigned long parent_rate)
76 {
77 	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
78 	unsigned long n, m, k, rate;
79 	u32 reg;
80 
81 	reg = readl(nkm->common.base + nkm->common.reg);
82 
83 	n = reg >> nkm->n.shift;
84 	n &= (1 << nkm->n.width) - 1;
85 	n += nkm->n.offset;
86 	if (!n)
87 		n++;
88 
89 	k = reg >> nkm->k.shift;
90 	k &= (1 << nkm->k.width) - 1;
91 	k += nkm->k.offset;
92 	if (!k)
93 		k++;
94 
95 	m = reg >> nkm->m.shift;
96 	m &= (1 << nkm->m.width) - 1;
97 	m += nkm->m.offset;
98 	if (!m)
99 		m++;
100 
101 	rate = parent_rate * n  * k / m;
102 
103 	if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
104 		rate /= nkm->fixed_post_div;
105 
106 	return rate;
107 }
108 
ccu_nkm_round_rate(struct ccu_mux_internal * mux,struct clk_hw * hw,unsigned long * parent_rate,unsigned long rate,void * data)109 static unsigned long ccu_nkm_round_rate(struct ccu_mux_internal *mux,
110 					struct clk_hw *hw,
111 					unsigned long *parent_rate,
112 					unsigned long rate,
113 					void *data)
114 {
115 	struct ccu_nkm *nkm = data;
116 	struct _ccu_nkm _nkm;
117 
118 	_nkm.min_n = nkm->n.min ?: 1;
119 	_nkm.max_n = nkm->n.max ?: 1 << nkm->n.width;
120 	_nkm.min_k = nkm->k.min ?: 1;
121 	_nkm.max_k = nkm->k.max ?: 1 << nkm->k.width;
122 	_nkm.min_m = 1;
123 	_nkm.max_m = nkm->m.max ?: 1 << nkm->m.width;
124 
125 	if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
126 		rate *= nkm->fixed_post_div;
127 
128 	ccu_nkm_find_best(*parent_rate, rate, &_nkm);
129 
130 	rate = *parent_rate * _nkm.n * _nkm.k / _nkm.m;
131 
132 	if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
133 		rate /= nkm->fixed_post_div;
134 
135 	return rate;
136 }
137 
ccu_nkm_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)138 static int ccu_nkm_determine_rate(struct clk_hw *hw,
139 				  struct clk_rate_request *req)
140 {
141 	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
142 
143 	return ccu_mux_helper_determine_rate(&nkm->common, &nkm->mux,
144 					     req, ccu_nkm_round_rate, nkm);
145 }
146 
ccu_nkm_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)147 static int ccu_nkm_set_rate(struct clk_hw *hw, unsigned long rate,
148 			   unsigned long parent_rate)
149 {
150 	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
151 	struct _ccu_nkm _nkm;
152 	unsigned long flags;
153 	u32 reg;
154 
155 	if (nkm->common.features & CCU_FEATURE_FIXED_POSTDIV)
156 		rate *= nkm->fixed_post_div;
157 
158 	_nkm.min_n = nkm->n.min ?: 1;
159 	_nkm.max_n = nkm->n.max ?: 1 << nkm->n.width;
160 	_nkm.min_k = nkm->k.min ?: 1;
161 	_nkm.max_k = nkm->k.max ?: 1 << nkm->k.width;
162 	_nkm.min_m = 1;
163 	_nkm.max_m = nkm->m.max ?: 1 << nkm->m.width;
164 
165 	ccu_nkm_find_best(parent_rate, rate, &_nkm);
166 
167 	spin_lock_irqsave(nkm->common.lock, flags);
168 
169 	reg = readl(nkm->common.base + nkm->common.reg);
170 	reg &= ~GENMASK(nkm->n.width + nkm->n.shift - 1, nkm->n.shift);
171 	reg &= ~GENMASK(nkm->k.width + nkm->k.shift - 1, nkm->k.shift);
172 	reg &= ~GENMASK(nkm->m.width + nkm->m.shift - 1, nkm->m.shift);
173 
174 	reg |= (_nkm.n - nkm->n.offset) << nkm->n.shift;
175 	reg |= (_nkm.k - nkm->k.offset) << nkm->k.shift;
176 	reg |= (_nkm.m - nkm->m.offset) << nkm->m.shift;
177 	writel(reg, nkm->common.base + nkm->common.reg);
178 
179 	spin_unlock_irqrestore(nkm->common.lock, flags);
180 
181 	ccu_helper_wait_for_lock(&nkm->common, nkm->lock);
182 
183 	return 0;
184 }
185 
ccu_nkm_get_parent(struct clk_hw * hw)186 static u8 ccu_nkm_get_parent(struct clk_hw *hw)
187 {
188 	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
189 
190 	return ccu_mux_helper_get_parent(&nkm->common, &nkm->mux);
191 }
192 
ccu_nkm_set_parent(struct clk_hw * hw,u8 index)193 static int ccu_nkm_set_parent(struct clk_hw *hw, u8 index)
194 {
195 	struct ccu_nkm *nkm = hw_to_ccu_nkm(hw);
196 
197 	return ccu_mux_helper_set_parent(&nkm->common, &nkm->mux, index);
198 }
199 
200 const struct clk_ops ccu_nkm_ops = {
201 	.disable	= ccu_nkm_disable,
202 	.enable		= ccu_nkm_enable,
203 	.is_enabled	= ccu_nkm_is_enabled,
204 
205 	.get_parent	= ccu_nkm_get_parent,
206 	.set_parent	= ccu_nkm_set_parent,
207 
208 	.determine_rate	= ccu_nkm_determine_rate,
209 	.recalc_rate	= ccu_nkm_recalc_rate,
210 	.set_rate	= ccu_nkm_set_rate,
211 };
212