xref: /linux/drivers/ptp/ptp_dte.c (revision 621cde16e49b3ecf7d59a8106a20aaebfb4a59a9)
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 
dte_write_nco(void __iomem * regs,s64 ns)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(0, (regs + DTE_NCO_LOW_TIME_REG));
66 	writel(sum2, (regs + DTE_NCO_TIME_REG));
67 	writel(sum3, (regs + DTE_NCO_OVERFLOW_REG));
68 }
69 
dte_read_nco(void __iomem * regs)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(regs + DTE_NCO_OVERFLOW_REG) & DTE_NCO_SUM3_MASK;
80 	sum2 = readl(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 
dte_write_nco_delta(struct ptp_dte * ptp_dte,s64 delta)87 static void dte_write_nco_delta(struct ptp_dte *ptp_dte, s64 delta)
88 {
89 	s64 ns;
90 
91 	ns = dte_read_nco(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(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 
dte_read_nco_with_ovf(struct ptp_dte * ptp_dte)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(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 
ptp_dte_adjfine(struct ptp_clock_info * ptp,long scaled_ppm)137 static int ptp_dte_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
138 {
139 	s32 ppb = scaled_ppm_to_ppb(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(nco_incr, ptp_dte->regs + DTE_NCO_INC_REG);
156 	spin_unlock_irqrestore(&ptp_dte->lock, flags);
157 
158 	return 0;
159 }
160 
ptp_dte_adjtime(struct ptp_clock_info * ptp,s64 delta)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(&ptp_dte->lock, flags);
169 
170 	return 0;
171 }
172 
ptp_dte_gettime(struct ptp_clock_info * ptp,struct timespec64 * ts)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(dte_read_nco_with_ovf(ptp_dte));
180 	spin_unlock_irqrestore(&ptp_dte->lock, flags);
181 
182 	return 0;
183 }
184 
ptp_dte_settime(struct ptp_clock_info * ptp,const struct timespec64 * ts)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(0, ptp_dte->regs + DTE_NCO_INC_REG);
195 
196 	dte_write_nco(ptp_dte->regs, 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, ptp_dte->regs + DTE_NCO_INC_REG);
204 
205 	spin_unlock_irqrestore(&ptp_dte->lock, flags);
206 
207 	return 0;
208 }
209 
ptp_dte_enable(struct ptp_clock_info * ptp,struct ptp_clock_request * rq,int on)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 
ptp_dte_probe(struct platform_device * pdev)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, sizeof(struct ptp_dte), GFP_KERNEL);
236 	if (!ptp_dte)
237 		return -ENOMEM;
238 
239 	ptp_dte->regs = devm_platform_ioremap_resource(pdev, 0);
240 	if (IS_ERR(ptp_dte->regs))
241 		return PTR_ERR(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(&ptp_dte->caps, &pdev->dev);
248 	if (IS_ERR(ptp_dte->ptp_clk)) {
249 		dev_err(dev,
250 			"%s: Failed to register ptp clock\n", __func__);
251 		return PTR_ERR(ptp_dte->ptp_clk);
252 	}
253 
254 	platform_set_drvdata(pdev, ptp_dte);
255 
256 	dev_info(dev, "ptp clk probe done\n");
257 
258 	return 0;
259 }
260 
ptp_dte_remove(struct platform_device * pdev)261 static void 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_dte->ptp_clk);
267 
268 	for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++)
269 		writel(0, ptp_dte->regs + (i * sizeof(u32)));
270 }
271 
272 #ifdef CONFIG_PM_SLEEP
ptp_dte_suspend(struct device * dev)273 static int ptp_dte_suspend(struct device *dev)
274 {
275 	struct ptp_dte *ptp_dte = dev_get_drvdata(dev);
276 	u8 i;
277 
278 	for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++) {
279 		ptp_dte->reg_val[i] =
280 			readl(ptp_dte->regs + (i * sizeof(u32)));
281 	}
282 
283 	/* disable the nco */
284 	writel(0, ptp_dte->regs + DTE_NCO_INC_REG);
285 
286 	return 0;
287 }
288 
ptp_dte_resume(struct device * dev)289 static int ptp_dte_resume(struct device *dev)
290 {
291 	struct ptp_dte *ptp_dte = dev_get_drvdata(dev);
292 	u8 i;
293 
294 	for (i = 0; i < DTE_NUM_REGS_TO_RESTORE; i++) {
295 		if ((i * sizeof(u32)) != DTE_NCO_OVERFLOW_REG)
296 			writel(ptp_dte->reg_val[i],
297 				(ptp_dte->regs + (i * sizeof(u32))));
298 		else
299 			writel(((ptp_dte->reg_val[i] &
300 				DTE_NCO_SUM3_MASK) << DTE_NCO_SUM3_WR_SHIFT),
301 				(ptp_dte->regs + (i * sizeof(u32))));
302 	}
303 
304 	return 0;
305 }
306 
307 static const struct dev_pm_ops ptp_dte_pm_ops = {
308 	.suspend = ptp_dte_suspend,
309 	.resume = ptp_dte_resume
310 };
311 
312 #define PTP_DTE_PM_OPS	(&ptp_dte_pm_ops)
313 #else
314 #define PTP_DTE_PM_OPS	NULL
315 #endif
316 
317 static const struct of_device_id ptp_dte_of_match[] = {
318 	{ .compatible = "brcm,ptp-dte", },
319 	{},
320 };
321 MODULE_DEVICE_TABLE(of, ptp_dte_of_match);
322 
323 static struct platform_driver ptp_dte_driver = {
324 	.driver = {
325 		.name = "ptp-dte",
326 		.pm = PTP_DTE_PM_OPS,
327 		.of_match_table = ptp_dte_of_match,
328 	},
329 	.probe    = ptp_dte_probe,
330 	.remove_new = ptp_dte_remove,
331 };
332 module_platform_driver(ptp_dte_driver);
333 
334 MODULE_AUTHOR("Broadcom");
335 MODULE_DESCRIPTION("Broadcom DTE PTP Clock driver");
336 MODULE_LICENSE("GPL v2");
337