clk-frac-divider.c
2.28 KB
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
/*
* Copyright (C) 2016 Freescale Semiconductor, Inc.
*
* Based on driver/clk/clk-fractional-divider.c
*
* The code contained herein is licensed under the GNU General Public
* License. You may obtain a copy of the GNU General Public License
* Version 2 or later at the following locations:
*
* http://www.opensource.org/licenses/gpl-license.html
* http://www.gnu.org/copyleft/gpl.html
*/
#include <linux/bitops.h>
#include <linux/clk-provider.h>
#include <linux/err.h>
#include <linux/io.h>
#include <linux/slab.h>
#include <linux/rational.h>
#include "clk.h"
#define to_clk_frac_divider(_hw) container_of(_hw, struct clk_frac_divider, hw)
static unsigned long clk_frac_divider_recalc_rate(struct clk_hw *hw,
unsigned long parent_rate)
{
struct clk_frac_divider *fd = to_clk_frac_divider(hw);
u32 val, m, n;
u64 ret;
val = readl_relaxed(fd->reg);
m = (val & fd->mmask) >> fd->mshift;
n = (val & fd->nmask) >> fd->nshift;
ret = (u64)parent_rate * (m + 1);
do_div(ret, n + 1);
return ret;
}
static long clk_frac_divider_round_rate(struct clk_hw *hw, unsigned long rate,
unsigned long *parent_rate)
{
struct clk_frac_divider *fd = to_clk_frac_divider(hw);
unsigned long scale;
unsigned long m, n;
u64 ret;
if (!rate || rate >= *parent_rate)
return *parent_rate;
scale = fls_long(*parent_rate / rate - 1);
if (scale > 4)
rate <<= scale - fd->nwidth;
rational_best_approximation(rate, *parent_rate,
GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
&m, &n);
ret = (u64)*parent_rate * m;
do_div(ret, n);
return ret;
}
static int clk_frac_divider_set_rate(struct clk_hw *hw, unsigned long rate,
unsigned long parent_rate)
{
struct clk_frac_divider *fd = to_clk_frac_divider(hw);
unsigned long m, n;
u32 val;
rational_best_approximation(rate, parent_rate,
GENMASK(fd->mwidth - 1, 0), GENMASK(fd->nwidth - 1, 0),
&m, &n);
m = m - 1;
n = n - 1;
if (m && !n)
return -EINVAL;
val = readl_relaxed(fd->reg);
val &= ~(fd->mmask | fd->nmask);
val |= (m << fd->mshift) | (n << fd->nshift);
writel_relaxed(val, fd->reg);
return 0;
}
const struct clk_ops clk_frac_divider_ops = {
.recalc_rate = clk_frac_divider_recalc_rate,
.round_rate = clk_frac_divider_round_rate,
.set_rate = clk_frac_divider_set_rate,
};
EXPORT_SYMBOL_GPL(clk_frac_divider_ops);