1 | // SPDX-License-Identifier: GPL-2.0-only |
2 | // Copyright 2017 Broadcom |
3 | |
4 | #include <linux/err.h> |
5 | #include <linux/io.h> |
6 | #include <linux/module.h> |
7 | #include <linux/mod_devicetable.h> |
8 | #include <linux/platform_device.h> |
9 | #include <linux/ptp_clock_kernel.h> |
10 | #include <linux/types.h> |
11 | |
12 | #define DTE_NCO_LOW_TIME_REG 0x00 |
13 | #define DTE_NCO_TIME_REG 0x04 |
14 | #define DTE_NCO_OVERFLOW_REG 0x08 |
15 | #define DTE_NCO_INC_REG 0x0c |
16 | |
17 | #define DTE_NCO_SUM2_MASK 0xffffffff |
18 | #define DTE_NCO_SUM2_SHIFT 4ULL |
19 | |
20 | #define DTE_NCO_SUM3_MASK 0xff |
21 | #define DTE_NCO_SUM3_SHIFT 36ULL |
22 | #define DTE_NCO_SUM3_WR_SHIFT 8 |
23 | |
24 | #define DTE_NCO_TS_WRAP_MASK 0xfff |
25 | #define DTE_NCO_TS_WRAP_LSHIFT 32 |
26 | |
27 | #define DTE_NCO_INC_DEFAULT 0x80000000 |
28 | #define DTE_NUM_REGS_TO_RESTORE 4 |
29 | |
30 | /* Full wrap around is 44bits in ns (~4.887 hrs) */ |
31 | #define DTE_WRAP_AROUND_NSEC_SHIFT 44 |
32 | |
33 | /* 44 bits NCO */ |
34 | #define DTE_NCO_MAX_NS 0xFFFFFFFFFFFLL |
35 | |
36 | /* 125MHz with 3.29 reg cfg */ |
37 | #define DTE_PPB_ADJ(ppb) (u32)(div64_u64((((u64)abs(ppb) * BIT(28)) +\ |
38 | 62500000ULL), 125000000ULL)) |
39 | |
40 | /* ptp dte priv structure */ |
41 | struct ptp_dte { |
42 | void __iomem *regs; |
43 | struct ptp_clock *ptp_clk; |
44 | struct ptp_clock_info caps; |
45 | struct device *dev; |
46 | u32 ts_ovf_last; |
47 | u32 ts_wrap_cnt; |
48 | spinlock_t lock; |
49 | u32 reg_val[DTE_NUM_REGS_TO_RESTORE]; |
50 | }; |
51 | |
52 | static void dte_write_nco(void __iomem *regs, s64 ns) |
53 | { |
54 | u32 sum2, sum3; |
55 | |
56 | sum2 = (u32)((ns >> DTE_NCO_SUM2_SHIFT) & DTE_NCO_SUM2_MASK); |
57 | /* compensate for ignoring sum1 */ |
58 | if (sum2 != DTE_NCO_SUM2_MASK) |
59 | sum2++; |
60 | |
61 | /* to write sum3, bits [15:8] needs to be written */ |
62 | sum3 = (u32)(((ns >> DTE_NCO_SUM3_SHIFT) & DTE_NCO_SUM3_MASK) << |
63 | DTE_NCO_SUM3_WR_SHIFT); |
64 | |
65 | writel(val: 0, addr: (regs + DTE_NCO_LOW_TIME_REG)); |
66 | writel(val: sum2, addr: (regs + DTE_NCO_TIME_REG)); |
67 | writel(val: sum3, addr: (regs + DTE_NCO_OVERFLOW_REG)); |
68 | } |
69 | |
70 | static s64 dte_read_nco(void __iomem *regs) |
71 | { |
72 | u32 sum2, sum3; |
73 | s64 ns; |
74 | |
75 | /* |
76 | * ignoring sum1 (4 bits) gives a 16ns resolution, which |
77 | * works due to the async register read. |
78 | */ |
79 | sum3 = readl(addr: regs + DTE_NCO_OVERFLOW_REG) & DTE_NCO_SUM3_MASK; |
80 | sum2 = readl(addr: regs + DTE_NCO_TIME_REG); |
81 | ns = ((s64)sum3 << DTE_NCO_SUM3_SHIFT) | |
82 | ((s64)sum2 << DTE_NCO_SUM2_SHIFT); |
83 | |
84 | return ns; |
85 | } |
86 | |
87 | static void dte_write_nco_delta(struct ptp_dte *ptp_dte, s64 delta) |
88 | { |
89 | s64 ns; |
90 | |
91 | ns = dte_read_nco(regs: ptp_dte->regs); |
92 | |
93 | /* handle wraparound conditions */ |
94 | if ((delta < 0) && (abs(delta) > ns)) { |
95 | if (ptp_dte->ts_wrap_cnt) { |
96 | ns += DTE_NCO_MAX_NS + delta; |
97 | ptp_dte->ts_wrap_cnt--; |
98 | } else { |
99 | ns = 0; |
100 | } |
101 | } else { |
102 | ns += delta; |
103 | if (ns > DTE_NCO_MAX_NS) { |
104 | ptp_dte->ts_wrap_cnt++; |
105 | ns -= DTE_NCO_MAX_NS; |
106 | } |
107 | } |
108 | |
109 | dte_write_nco(regs: ptp_dte->regs, ns); |
110 | |
111 | ptp_dte->ts_ovf_last = (ns >> DTE_NCO_TS_WRAP_LSHIFT) & |
112 | DTE_NCO_TS_WRAP_MASK; |
113 | } |
114 | |
115 | static s64 dte_read_nco_with_ovf(struct ptp_dte *ptp_dte) |
116 | { |
117 | u32 ts_ovf; |
118 | s64 ns = 0; |
119 | |
120 | ns = dte_read_nco(regs: ptp_dte->regs); |
121 | |
122 | /*Timestamp overflow: 8 LSB bits of sum3, 4 MSB bits of sum2 */ |
123 | ts_ovf = (ns >> DTE_NCO_TS_WRAP_LSHIFT) & DTE_NCO_TS_WRAP_MASK; |
124 | |
125 | /* Check for wrap around */ |
126 | if (ts_ovf < ptp_dte->ts_ovf_last) |
127 | ptp_dte->ts_wrap_cnt++; |
128 | |
129 | ptp_dte->ts_ovf_last = ts_ovf; |
130 | |
131 | /* adjust for wraparounds */ |
132 | ns += (s64)(BIT_ULL(DTE_WRAP_AROUND_NSEC_SHIFT) * ptp_dte->ts_wrap_cnt); |
133 | |
134 | return ns; |
135 | } |
136 | |
137 | static int ptp_dte_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) |
138 | { |
139 | s32 ppb = scaled_ppm_to_ppb(ppm: scaled_ppm); |
140 | u32 nco_incr; |
141 | unsigned long flags; |
142 | struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps); |
143 | |
144 | if (abs(ppb) > ptp_dte->caps.max_adj) { |
145 | dev_err(ptp_dte->dev, "ppb adj too big\n" ); |
146 | return -EINVAL; |
147 | } |
148 | |
149 | if (ppb < 0) |
150 | nco_incr = DTE_NCO_INC_DEFAULT - DTE_PPB_ADJ(ppb); |
151 | else |
152 | nco_incr = DTE_NCO_INC_DEFAULT + DTE_PPB_ADJ(ppb); |
153 | |
154 | spin_lock_irqsave(&ptp_dte->lock, flags); |
155 | writel(val: nco_incr, addr: ptp_dte->regs + DTE_NCO_INC_REG); |
156 | spin_unlock_irqrestore(lock: &ptp_dte->lock, flags); |
157 | |
158 | return 0; |
159 | } |
160 | |
161 | static int ptp_dte_adjtime(struct ptp_clock_info *ptp, s64 delta) |
162 | { |
163 | unsigned long flags; |
164 | struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps); |
165 | |
166 | spin_lock_irqsave(&ptp_dte->lock, flags); |
167 | dte_write_nco_delta(ptp_dte, delta); |
168 | spin_unlock_irqrestore(lock: &ptp_dte->lock, flags); |
169 | |
170 | return 0; |
171 | } |
172 | |
173 | static int ptp_dte_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts) |
174 | { |
175 | unsigned long flags; |
176 | struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps); |
177 | |
178 | spin_lock_irqsave(&ptp_dte->lock, flags); |
179 | *ts = ns_to_timespec64(nsec: dte_read_nco_with_ovf(ptp_dte)); |
180 | spin_unlock_irqrestore(lock: &ptp_dte->lock, flags); |
181 | |
182 | return 0; |
183 | } |
184 | |
185 | static int ptp_dte_settime(struct ptp_clock_info *ptp, |
186 | const struct timespec64 *ts) |
187 | { |
188 | unsigned long flags; |
189 | struct ptp_dte *ptp_dte = container_of(ptp, struct ptp_dte, caps); |
190 | |
191 | spin_lock_irqsave(&ptp_dte->lock, flags); |
192 | |
193 | /* Disable nco increment */ |
194 | writel(val: 0, addr: ptp_dte->regs + DTE_NCO_INC_REG); |
195 | |
196 | dte_write_nco(regs: ptp_dte->regs, ns: timespec64_to_ns(ts)); |
197 | |
198 | /* reset overflow and wrap counter */ |
199 | ptp_dte->ts_ovf_last = 0; |
200 | ptp_dte->ts_wrap_cnt = 0; |
201 | |
202 | /* Enable nco increment */ |
203 | writel(DTE_NCO_INC_DEFAULT, addr: ptp_dte->regs + DTE_NCO_INC_REG); |
204 | |
205 | spin_unlock_irqrestore(lock: &ptp_dte->lock, flags); |
206 | |
207 | return 0; |
208 | } |
209 | |
210 | static int ptp_dte_enable(struct ptp_clock_info *ptp, |
211 | struct ptp_clock_request *rq, int on) |
212 | { |
213 | return -EOPNOTSUPP; |
214 | } |
215 | |
216 | static const struct ptp_clock_info ptp_dte_caps = { |
217 | .owner = THIS_MODULE, |
218 | .name = "DTE PTP timer" , |
219 | .max_adj = 50000000, |
220 | .n_ext_ts = 0, |
221 | .n_pins = 0, |
222 | .pps = 0, |
223 | .adjfine = ptp_dte_adjfine, |
224 | .adjtime = ptp_dte_adjtime, |
225 | .gettime64 = ptp_dte_gettime, |
226 | .settime64 = ptp_dte_settime, |
227 | .enable = ptp_dte_enable, |
228 | }; |
229 | |
230 | static int ptp_dte_probe(struct platform_device *pdev) |
231 | { |
232 | struct ptp_dte *ptp_dte; |
233 | struct device *dev = &pdev->dev; |
234 | |
235 | ptp_dte = devm_kzalloc(dev, size: sizeof(struct ptp_dte), GFP_KERNEL); |
236 | if (!ptp_dte) |
237 | return -ENOMEM; |
238 | |
239 | ptp_dte->regs = devm_platform_ioremap_resource(pdev, index: 0); |
240 | if (IS_ERR(ptr: ptp_dte->regs)) |
241 | return PTR_ERR(ptr: ptp_dte->regs); |
242 | |
243 | spin_lock_init(&ptp_dte->lock); |
244 | |
245 | ptp_dte->dev = dev; |
246 | ptp_dte->caps = ptp_dte_caps; |
247 | ptp_dte->ptp_clk = ptp_clock_register(info: &ptp_dte->caps, parent: &pdev->dev); |
248 | if (IS_ERR(ptr: ptp_dte->ptp_clk)) { |
249 | dev_err(dev, |
250 | "%s: Failed to register ptp clock\n" , __func__); |
251 | return PTR_ERR(ptr: ptp_dte->ptp_clk); |
252 | } |
253 | |
254 | platform_set_drvdata(pdev, data: ptp_dte); |
255 | |
256 | dev_info(dev, "ptp clk probe done\n" ); |
257 | |
258 | return 0; |
259 | } |
260 | |
261 | static int ptp_dte_remove(struct platform_device *pdev) |
262 | { |
263 | struct ptp_dte *ptp_dte = platform_get_drvdata(pdev); |
264 | u8 i; |
265 | |
266 | ptp_clock_unregister(ptp: ptp_dte->ptp_clk); |
267 | |
268 | for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++) |
269 | writel(val: 0, addr: ptp_dte->regs + (i * sizeof(u32))); |
270 | |
271 | return 0; |
272 | } |
273 | |
274 | #ifdef CONFIG_PM_SLEEP |
275 | static int ptp_dte_suspend(struct device *dev) |
276 | { |
277 | struct ptp_dte *ptp_dte = dev_get_drvdata(dev); |
278 | u8 i; |
279 | |
280 | for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++) { |
281 | ptp_dte->reg_val[i] = |
282 | readl(addr: ptp_dte->regs + (i * sizeof(u32))); |
283 | } |
284 | |
285 | /* disable the nco */ |
286 | writel(val: 0, addr: ptp_dte->regs + DTE_NCO_INC_REG); |
287 | |
288 | return 0; |
289 | } |
290 | |
291 | static int ptp_dte_resume(struct device *dev) |
292 | { |
293 | struct ptp_dte *ptp_dte = dev_get_drvdata(dev); |
294 | u8 i; |
295 | |
296 | for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++) { |
297 | if ((i * sizeof(u32)) != DTE_NCO_OVERFLOW_REG) |
298 | writel(val: ptp_dte->reg_val[i], |
299 | addr: (ptp_dte->regs + (i * sizeof(u32)))); |
300 | else |
301 | writel(val: ((ptp_dte->reg_val[i] & |
302 | DTE_NCO_SUM3_MASK) << DTE_NCO_SUM3_WR_SHIFT), |
303 | addr: (ptp_dte->regs + (i * sizeof(u32)))); |
304 | } |
305 | |
306 | return 0; |
307 | } |
308 | |
309 | static const struct dev_pm_ops ptp_dte_pm_ops = { |
310 | .suspend = ptp_dte_suspend, |
311 | .resume = ptp_dte_resume |
312 | }; |
313 | |
314 | #define PTP_DTE_PM_OPS (&ptp_dte_pm_ops) |
315 | #else |
316 | #define PTP_DTE_PM_OPS NULL |
317 | #endif |
318 | |
319 | static const struct of_device_id ptp_dte_of_match[] = { |
320 | { .compatible = "brcm,ptp-dte" , }, |
321 | {}, |
322 | }; |
323 | MODULE_DEVICE_TABLE(of, ptp_dte_of_match); |
324 | |
325 | static struct platform_driver ptp_dte_driver = { |
326 | .driver = { |
327 | .name = "ptp-dte" , |
328 | .pm = PTP_DTE_PM_OPS, |
329 | .of_match_table = ptp_dte_of_match, |
330 | }, |
331 | .probe = ptp_dte_probe, |
332 | .remove = ptp_dte_remove, |
333 | }; |
334 | module_platform_driver(ptp_dte_driver); |
335 | |
336 | MODULE_AUTHOR("Broadcom" ); |
337 | MODULE_DESCRIPTION("Broadcom DTE PTP Clock driver" ); |
338 | MODULE_LICENSE("GPL v2" ); |
339 | |