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
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
|
/*
* ZynqMP clock driver
*
* Copyright (C) 2016 Xilinx, Inc.
*
* SPDX-License-Identifier: GPL-2.0+
*/
#include <common.h>
#include <linux/bitops.h>
#include <clk-uclass.h>
#include <dm/device.h>
#include <clk.h>
#define ZYNQMP_GEM0_REF_CTRL 0xFF5E0050
#define ZYNQMP_IOPLL_CTRL 0xFF5E0020
#define ZYNQMP_RPLL_CTRL 0xFF5E0030
#define ZYNQMP_DPLL_CTRL 0xFD1A002C
#define ZYNQMP_SIP_SVC_MMIO_WRITE 0xC2000013
#define ZYNQMP_SIP_SVC_MMIO_WRITE 0xC2000013
#define ZYNQMP_SIP_SVC_MMIO_WRITE 0xC2000013
#define ZYNQMP_SIP_SVC_MMIO_READ 0xC2000014
#define ZYNQMP_DIV_MAX_VAL 0x3F
#define ZYNQMP_DIV1_SHFT 8
#define ZYNQMP_DIV1_SHFT 8
#define ZYNQMP_DIV2_SHFT 16
#define ZYNQMP_DIV_MASK 0x3F
#define ZYNQMP_PLL_CTRL_FBDIV_MASK 0x7F
#define ZYNQMP_PLL_CTRL_FBDIV_SHFT 8
#define ZYNQMP_GEM_REF_CTRL_SRC_MASK 0x7
#define ZYNQMP_GEM0_CLK_ID 45
#define ZYNQMP_GEM1_CLK_ID 46
#define ZYNQMP_GEM2_CLK_ID 47
#define ZYNQMP_GEM3_CLK_ID 48
static unsigned long pss_ref_clk;
static int zynqmp_calculate_divisors(unsigned long req_rate,
unsigned long parent_rate,
u32 *div1, u32 *div2)
{
u32 req_div = 1;
u32 i;
/*
* calculate two divisors to get
* required rate and each divisor
* should be less than 63
*/
req_div = DIV_ROUND_UP(parent_rate, req_rate);
for (i = 1; i <= req_div; i++) {
if ((req_div % i) == 0) {
*div1 = req_div / i;
*div2 = i;
if ((*div1 < ZYNQMP_DIV_MAX_VAL) &&
(*div2 < ZYNQMP_DIV_MAX_VAL))
return 0;
}
}
return -1;
}
static int zynqmp_get_periph_id(unsigned long id)
{
int periph_id;
switch (id) {
case ZYNQMP_GEM0_CLK_ID:
periph_id = 0;
break;
case ZYNQMP_GEM1_CLK_ID:
periph_id = 1;
break;
case ZYNQMP_GEM2_CLK_ID:
periph_id = 2;
break;
case ZYNQMP_GEM3_CLK_ID:
periph_id = 3;
break;
default:
printf("%s, Invalid clock id:%ld\n", __func__, id);
return -EINVAL;
}
return periph_id;
}
static int zynqmp_set_clk(unsigned long id, u32 div1, u32 div2)
{
struct pt_regs regs;
ulong reg;
u32 mask, value;
id = zynqmp_get_periph_id(id);
if (id < 0)
return -EINVAL;
reg = (ulong)((u32 *)ZYNQMP_GEM0_REF_CTRL + id);
mask = (ZYNQMP_DIV_MASK << ZYNQMP_DIV1_SHFT) |
(ZYNQMP_DIV_MASK << ZYNQMP_DIV2_SHFT);
value = (div1 << ZYNQMP_DIV1_SHFT) | (div2 << ZYNQMP_DIV2_SHFT);
debug("%s: reg:0x%lx, mask:0x%x, value:0x%x\n", __func__, reg, mask,
value);
regs.regs[0] = ZYNQMP_SIP_SVC_MMIO_WRITE;
regs.regs[1] = ((u64)mask << 32) | reg;
regs.regs[2] = value;
regs.regs[3] = 0;
smc_call(®s);
return regs.regs[0];
}
static unsigned long zynqmp_clk_get_rate(struct clk *clk)
{
struct pt_regs regs;
ulong reg;
unsigned long value;
int id;
id = zynqmp_get_periph_id(clk->id);
if (id < 0)
return -EINVAL;
reg = (ulong)((u32 *)ZYNQMP_GEM0_REF_CTRL + id);
regs.regs[0] = ZYNQMP_SIP_SVC_MMIO_READ;
regs.regs[1] = reg;
regs.regs[2] = 0;
regs.regs[3] = 0;
smc_call(®s);
value = upper_32_bits(regs.regs[0]);
value &= ZYNQMP_GEM_REF_CTRL_SRC_MASK;
switch (value) {
case 0:
regs.regs[1] = ZYNQMP_IOPLL_CTRL;
break;
case 2:
regs.regs[1] = ZYNQMP_RPLL_CTRL;
break;
case 3:
regs.regs[1] = ZYNQMP_DPLL_CTRL;
break;
default:
return -EINVAL;
}
regs.regs[0] = ZYNQMP_SIP_SVC_MMIO_READ;
regs.regs[2] = 0;
regs.regs[3] = 0;
smc_call(®s);
value = upper_32_bits(regs.regs[0]) &
(ZYNQMP_PLL_CTRL_FBDIV_MASK <<
ZYNQMP_PLL_CTRL_FBDIV_SHFT);
value >>= ZYNQMP_PLL_CTRL_FBDIV_SHFT;
value *= pss_ref_clk;
return value;
}
static ulong zynqmp_clk_set_rate(struct clk *clk, unsigned long clk_rate)
{
int ret;
u32 div1 = 0;
u32 div2 = 0;
unsigned long input_clk;
input_clk = zynqmp_clk_get_rate(clk);
if (IS_ERR_VALUE(input_clk)) {
dev_err(dev, "failed to get input_clk\n");
return -EINVAL;
}
debug("%s: i/p CLK %ld, clk_rate:0x%ld\n", __func__, input_clk,
clk_rate);
ret = zynqmp_calculate_divisors(clk_rate, input_clk, &div1, &div2);
if (ret) {
dev_err(dev, "failed to proper divisors\n");
return -EINVAL;
}
debug("%s: Div1:%d, Div2:%d\n", __func__, div1, div2);
ret = zynqmp_set_clk(clk->id, div1, div2);
if (ret) {
dev_err(dev, "failed to set gem clk\n");
return -EINVAL;
}
return 0;
}
static int zynqmp_clk_probe(struct udevice *dev)
{
struct clk clk;
int ret;
debug("%s\n", __func__);
ret = clk_get_by_name(dev, "pss_ref_clk", &clk);
if (ret < 0) {
dev_err(dev, "failed to get pss_ref_clk\n");
return ret;
}
pss_ref_clk = clk_get_rate(&clk);
if (IS_ERR_VALUE(pss_ref_clk)) {
dev_err(dev, "failed to get rate pss_ref_clk\n");
return -EINVAL;
}
return 0;
}
static struct clk_ops zynqmp_clk_ops = {
.set_rate = zynqmp_clk_set_rate,
.get_rate = zynqmp_clk_get_rate,
};
static const struct udevice_id zynqmp_clk_ids[] = {
{ .compatible = "xlnx,zynqmp-clkc" },
{ }
};
U_BOOT_DRIVER(zynqmp_clk) = {
.name = "zynqmp-clk",
.id = UCLASS_CLK,
.of_match = zynqmp_clk_ids,
.probe = zynqmp_clk_probe,
.ops = &zynqmp_clk_ops,
};
|
