1 | // SPDX-License-Identifier: GPL-2.0-or-later |
2 | /* |
3 | * Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com> |
4 | */ |
5 | |
6 | #include <linux/clk-provider.h> |
7 | #include <linux/clkdev.h> |
8 | #include <linux/clk/at91_pmc.h> |
9 | #include <linux/of.h> |
10 | #include <linux/mfd/syscon.h> |
11 | #include <linux/regmap.h> |
12 | |
13 | #include "pmc.h" |
14 | |
15 | #define SMD_DIV_SHIFT 8 |
16 | #define SMD_MAX_DIV 0xf |
17 | |
18 | struct at91sam9x5_clk_smd { |
19 | struct clk_hw hw; |
20 | struct regmap *regmap; |
21 | }; |
22 | |
23 | #define to_at91sam9x5_clk_smd(hw) \ |
24 | container_of(hw, struct at91sam9x5_clk_smd, hw) |
25 | |
26 | static unsigned long at91sam9x5_clk_smd_recalc_rate(struct clk_hw *hw, |
27 | unsigned long parent_rate) |
28 | { |
29 | struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw); |
30 | unsigned int smdr; |
31 | u8 smddiv; |
32 | |
33 | regmap_read(map: smd->regmap, AT91_PMC_SMD, val: &smdr); |
34 | smddiv = (smdr & AT91_PMC_SMD_DIV) >> SMD_DIV_SHIFT; |
35 | |
36 | return parent_rate / (smddiv + 1); |
37 | } |
38 | |
39 | static int at91sam9x5_clk_smd_determine_rate(struct clk_hw *hw, |
40 | struct clk_rate_request *req) |
41 | { |
42 | unsigned long div; |
43 | unsigned long bestrate; |
44 | unsigned long tmp; |
45 | |
46 | if (req->rate >= req->best_parent_rate) { |
47 | req->rate = req->best_parent_rate; |
48 | return 0; |
49 | } |
50 | |
51 | div = req->best_parent_rate / req->rate; |
52 | if (div > SMD_MAX_DIV) { |
53 | req->rate = req->best_parent_rate / (SMD_MAX_DIV + 1); |
54 | return 0; |
55 | } |
56 | |
57 | bestrate = req->best_parent_rate / div; |
58 | tmp = req->best_parent_rate / (div + 1); |
59 | if (bestrate - req->rate > req->rate - tmp) |
60 | bestrate = tmp; |
61 | |
62 | req->rate = bestrate; |
63 | return 0; |
64 | } |
65 | |
66 | static int at91sam9x5_clk_smd_set_parent(struct clk_hw *hw, u8 index) |
67 | { |
68 | struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw); |
69 | |
70 | if (index > 1) |
71 | return -EINVAL; |
72 | |
73 | regmap_update_bits(map: smd->regmap, AT91_PMC_SMD, AT91_PMC_SMDS, |
74 | val: index ? AT91_PMC_SMDS : 0); |
75 | |
76 | return 0; |
77 | } |
78 | |
79 | static u8 at91sam9x5_clk_smd_get_parent(struct clk_hw *hw) |
80 | { |
81 | struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw); |
82 | unsigned int smdr; |
83 | |
84 | regmap_read(map: smd->regmap, AT91_PMC_SMD, val: &smdr); |
85 | |
86 | return smdr & AT91_PMC_SMDS; |
87 | } |
88 | |
89 | static int at91sam9x5_clk_smd_set_rate(struct clk_hw *hw, unsigned long rate, |
90 | unsigned long parent_rate) |
91 | { |
92 | struct at91sam9x5_clk_smd *smd = to_at91sam9x5_clk_smd(hw); |
93 | unsigned long div = parent_rate / rate; |
94 | |
95 | if (parent_rate % rate || div < 1 || div > (SMD_MAX_DIV + 1)) |
96 | return -EINVAL; |
97 | |
98 | regmap_update_bits(map: smd->regmap, AT91_PMC_SMD, AT91_PMC_SMD_DIV, |
99 | val: (div - 1) << SMD_DIV_SHIFT); |
100 | |
101 | return 0; |
102 | } |
103 | |
104 | static const struct clk_ops at91sam9x5_smd_ops = { |
105 | .recalc_rate = at91sam9x5_clk_smd_recalc_rate, |
106 | .determine_rate = at91sam9x5_clk_smd_determine_rate, |
107 | .get_parent = at91sam9x5_clk_smd_get_parent, |
108 | .set_parent = at91sam9x5_clk_smd_set_parent, |
109 | .set_rate = at91sam9x5_clk_smd_set_rate, |
110 | }; |
111 | |
112 | struct clk_hw * __init |
113 | at91sam9x5_clk_register_smd(struct regmap *regmap, const char *name, |
114 | const char **parent_names, u8 num_parents) |
115 | { |
116 | struct at91sam9x5_clk_smd *smd; |
117 | struct clk_hw *hw; |
118 | struct clk_init_data init; |
119 | int ret; |
120 | |
121 | smd = kzalloc(size: sizeof(*smd), GFP_KERNEL); |
122 | if (!smd) |
123 | return ERR_PTR(error: -ENOMEM); |
124 | |
125 | init.name = name; |
126 | init.ops = &at91sam9x5_smd_ops; |
127 | init.parent_names = parent_names; |
128 | init.num_parents = num_parents; |
129 | init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE; |
130 | |
131 | smd->hw.init = &init; |
132 | smd->regmap = regmap; |
133 | |
134 | hw = &smd->hw; |
135 | ret = clk_hw_register(NULL, hw: &smd->hw); |
136 | if (ret) { |
137 | kfree(objp: smd); |
138 | hw = ERR_PTR(error: ret); |
139 | } |
140 | |
141 | return hw; |
142 | } |
143 | |