blob: 4470ffc8cf0d3f2cd68e0587eb2f873e87dac008 [file] [log] [blame]
Maxime Ripardadbfb002016-06-29 21:05:30 +02001/*
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_nk.h"
16
17void ccu_nk_find_best(unsigned long parent, unsigned long rate,
18 unsigned int max_n, unsigned int max_k,
19 unsigned int *n, unsigned int *k)
20{
21 unsigned long best_rate = 0;
22 unsigned int best_k = 0, best_n = 0;
23 unsigned int _k, _n;
24
25 for (_k = 1; _k <= max_k; _k++) {
26 for (_n = 1; _n <= max_n; _n++) {
27 unsigned long tmp_rate = parent * _n * _k;
28
29 if (tmp_rate > rate)
30 continue;
31
32 if ((rate - tmp_rate) < (rate - best_rate)) {
33 best_rate = tmp_rate;
34 best_k = _k;
35 best_n = _n;
36 }
37 }
38 }
39
40 *k = best_k;
41 *n = best_n;
42}
43
44static void ccu_nk_disable(struct clk_hw *hw)
45{
46 struct ccu_nk *nk = hw_to_ccu_nk(hw);
47
48 return ccu_gate_helper_disable(&nk->common, nk->enable);
49}
50
51static int ccu_nk_enable(struct clk_hw *hw)
52{
53 struct ccu_nk *nk = hw_to_ccu_nk(hw);
54
55 return ccu_gate_helper_enable(&nk->common, nk->enable);
56}
57
58static int ccu_nk_is_enabled(struct clk_hw *hw)
59{
60 struct ccu_nk *nk = hw_to_ccu_nk(hw);
61
62 return ccu_gate_helper_is_enabled(&nk->common, nk->enable);
63}
64
65static unsigned long ccu_nk_recalc_rate(struct clk_hw *hw,
66 unsigned long parent_rate)
67{
68 struct ccu_nk *nk = hw_to_ccu_nk(hw);
69 unsigned long rate, n, k;
70 u32 reg;
71
72 reg = readl(nk->common.base + nk->common.reg);
73
74 n = reg >> nk->n.shift;
75 n &= (1 << nk->n.width) - 1;
76
77 k = reg >> nk->k.shift;
78 k &= (1 << nk->k.width) - 1;
79
80 rate = parent_rate * (n + 1) * (k + 1);
81
82 if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
83 rate /= nk->fixed_post_div;
84
85 return rate;
86}
87
88static long ccu_nk_round_rate(struct clk_hw *hw, unsigned long rate,
89 unsigned long *parent_rate)
90{
91 struct ccu_nk *nk = hw_to_ccu_nk(hw);
92 unsigned int n, k;
93
94 if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
95 rate *= nk->fixed_post_div;
96
97 ccu_nk_find_best(*parent_rate, rate,
98 1 << nk->n.width, 1 << nk->k.width,
99 &n, &k);
100
101 rate = *parent_rate * n * k;
102 if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
103 rate = rate / nk->fixed_post_div;
104
105 return rate;
106}
107
108static int ccu_nk_set_rate(struct clk_hw *hw, unsigned long rate,
109 unsigned long parent_rate)
110{
111 struct ccu_nk *nk = hw_to_ccu_nk(hw);
112 unsigned long flags;
113 unsigned int n, k;
114 u32 reg;
115
116 if (nk->common.features & CCU_FEATURE_FIXED_POSTDIV)
117 rate = rate * nk->fixed_post_div;
118
119 ccu_nk_find_best(parent_rate, rate,
120 1 << nk->n.width, 1 << nk->k.width,
121 &n, &k);
122
123 spin_lock_irqsave(nk->common.lock, flags);
124
125 reg = readl(nk->common.base + nk->common.reg);
126 reg &= ~GENMASK(nk->n.width + nk->n.shift - 1, nk->n.shift);
127 reg &= ~GENMASK(nk->k.width + nk->k.shift - 1, nk->k.shift);
128
129 writel(reg | ((k - 1) << nk->k.shift) | ((n - 1) << nk->n.shift),
130 nk->common.base + nk->common.reg);
131
132 spin_unlock_irqrestore(nk->common.lock, flags);
133
134 ccu_helper_wait_for_lock(&nk->common, nk->lock);
135
136 return 0;
137}
138
139const struct clk_ops ccu_nk_ops = {
140 .disable = ccu_nk_disable,
141 .enable = ccu_nk_enable,
142 .is_enabled = ccu_nk_is_enabled,
143
144 .recalc_rate = ccu_nk_recalc_rate,
145 .round_rate = ccu_nk_round_rate,
146 .set_rate = ccu_nk_set_rate,
147};