1 // SPDX-License-Identifier: GPL-2.0-only
2 /*
3 * 1588 PTP support for Cadence GEM device.
4 *
5 * Copyright (C) 2017 Cadence Design Systems - https://www.cadence.com
6 *
7 * Authors: Rafal Ozieblo <rafalo@cadence.com>
8 * Bartosz Folta <bfolta@cadence.com>
9 */
10 #include <linux/kernel.h>
11 #include <linux/types.h>
12 #include <linux/clk.h>
13 #include <linux/device.h>
14 #include <linux/etherdevice.h>
15 #include <linux/platform_device.h>
16 #include <linux/time64.h>
17 #include <linux/ptp_classify.h>
18 #include <linux/if_ether.h>
19 #include <linux/if_vlan.h>
20 #include <linux/net_tstamp.h>
21 #include <linux/circ_buf.h>
22 #include <linux/spinlock.h>
23
24 #include "macb.h"
25
26 #define GEM_PTP_TIMER_NAME "gem-ptp-timer"
27
macb_ptp_desc(struct macb * bp,struct macb_dma_desc * desc)28 static struct macb_dma_desc_ptp *macb_ptp_desc(struct macb *bp,
29 struct macb_dma_desc *desc)
30 {
31 if (!macb_dma_ptp(bp))
32 return NULL;
33
34 if (macb_dma64(bp))
35 return (struct macb_dma_desc_ptp *)
36 ((u8 *)desc + sizeof(struct macb_dma_desc)
37 + sizeof(struct macb_dma_desc_64));
38 else
39 return (struct macb_dma_desc_ptp *)
40 ((u8 *)desc + sizeof(struct macb_dma_desc));
41 }
42
gem_tsu_get_time(struct ptp_clock_info * ptp,struct timespec64 * ts,struct ptp_system_timestamp * sts)43 static int gem_tsu_get_time(struct ptp_clock_info *ptp, struct timespec64 *ts,
44 struct ptp_system_timestamp *sts)
45 {
46 struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
47 unsigned long flags;
48 long first, second;
49 u32 secl, sech;
50
51 spin_lock_irqsave(&bp->tsu_clk_lock, flags);
52 ptp_read_system_prets(sts);
53 first = gem_readl(bp, TN);
54 ptp_read_system_postts(sts);
55 secl = gem_readl(bp, TSL);
56 sech = gem_readl(bp, TSH);
57 second = gem_readl(bp, TN);
58
59 /* test for nsec rollover */
60 if (first > second) {
61 /* if so, use later read & re-read seconds
62 * (assume all done within 1s)
63 */
64 ptp_read_system_prets(sts);
65 ts->tv_nsec = gem_readl(bp, TN);
66 ptp_read_system_postts(sts);
67 secl = gem_readl(bp, TSL);
68 sech = gem_readl(bp, TSH);
69 } else {
70 ts->tv_nsec = first;
71 }
72
73 spin_unlock_irqrestore(&bp->tsu_clk_lock, flags);
74 ts->tv_sec = (((u64)sech << GEM_TSL_SIZE) | secl)
75 & TSU_SEC_MAX_VAL;
76 return 0;
77 }
78
gem_tsu_set_time(struct ptp_clock_info * ptp,const struct timespec64 * ts)79 static int gem_tsu_set_time(struct ptp_clock_info *ptp,
80 const struct timespec64 *ts)
81 {
82 struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
83 unsigned long flags;
84 u32 ns, sech, secl;
85
86 secl = (u32)ts->tv_sec;
87 sech = (ts->tv_sec >> GEM_TSL_SIZE) & ((1 << GEM_TSH_SIZE) - 1);
88 ns = ts->tv_nsec;
89
90 spin_lock_irqsave(&bp->tsu_clk_lock, flags);
91
92 /* TSH doesn't latch the time and no atomicity! */
93 gem_writel(bp, TN, 0); /* clear to avoid overflow */
94 gem_writel(bp, TSH, sech);
95 /* write lower bits 2nd, for synchronized secs update */
96 gem_writel(bp, TSL, secl);
97 gem_writel(bp, TN, ns);
98
99 spin_unlock_irqrestore(&bp->tsu_clk_lock, flags);
100
101 return 0;
102 }
103
gem_tsu_incr_set(struct macb * bp,struct tsu_incr * incr_spec)104 static int gem_tsu_incr_set(struct macb *bp, struct tsu_incr *incr_spec)
105 {
106 unsigned long flags;
107
108 /* tsu_timer_incr register must be written after
109 * the tsu_timer_incr_sub_ns register and the write operation
110 * will cause the value written to the tsu_timer_incr_sub_ns register
111 * to take effect.
112 */
113 spin_lock_irqsave(&bp->tsu_clk_lock, flags);
114 /* RegBit[15:0] = Subns[23:8]; RegBit[31:24] = Subns[7:0] */
115 gem_writel(bp, TISUBN, GEM_BF(SUBNSINCRL, incr_spec->sub_ns) |
116 GEM_BF(SUBNSINCRH, (incr_spec->sub_ns >>
117 GEM_SUBNSINCRL_SIZE)));
118 gem_writel(bp, TI, GEM_BF(NSINCR, incr_spec->ns));
119 spin_unlock_irqrestore(&bp->tsu_clk_lock, flags);
120
121 return 0;
122 }
123
gem_ptp_adjfine(struct ptp_clock_info * ptp,long scaled_ppm)124 static int gem_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
125 {
126 struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
127 struct tsu_incr incr_spec;
128 bool neg_adj = false;
129 u32 word;
130 u64 adj;
131
132 if (scaled_ppm < 0) {
133 neg_adj = true;
134 scaled_ppm = -scaled_ppm;
135 }
136
137 /* Adjustment is relative to base frequency */
138 incr_spec.sub_ns = bp->tsu_incr.sub_ns;
139 incr_spec.ns = bp->tsu_incr.ns;
140
141 /* scaling: unused(8bit) | ns(8bit) | fractions(16bit) */
142 word = ((u64)incr_spec.ns << GEM_SUBNSINCR_SIZE) + incr_spec.sub_ns;
143 adj = (u64)scaled_ppm * word;
144 /* Divide with rounding, equivalent to floating dividing:
145 * (temp / USEC_PER_SEC) + 0.5
146 */
147 adj += (USEC_PER_SEC >> 1);
148 adj >>= PPM_FRACTION; /* remove fractions */
149 adj = div_u64(adj, USEC_PER_SEC);
150 adj = neg_adj ? (word - adj) : (word + adj);
151
152 incr_spec.ns = (adj >> GEM_SUBNSINCR_SIZE)
153 & ((1 << GEM_NSINCR_SIZE) - 1);
154 incr_spec.sub_ns = adj & ((1 << GEM_SUBNSINCR_SIZE) - 1);
155 gem_tsu_incr_set(bp, &incr_spec);
156 return 0;
157 }
158
gem_ptp_adjtime(struct ptp_clock_info * ptp,s64 delta)159 static int gem_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
160 {
161 struct macb *bp = container_of(ptp, struct macb, ptp_clock_info);
162 struct timespec64 now, then = ns_to_timespec64(delta);
163 u32 adj, sign = 0;
164
165 if (delta < 0) {
166 sign = 1;
167 delta = -delta;
168 }
169
170 if (delta > TSU_NSEC_MAX_VAL) {
171 gem_tsu_get_time(&bp->ptp_clock_info, &now, NULL);
172 now = timespec64_add(now, then);
173
174 gem_tsu_set_time(&bp->ptp_clock_info,
175 (const struct timespec64 *)&now);
176 } else {
177 adj = (sign << GEM_ADDSUB_OFFSET) | delta;
178
179 gem_writel(bp, TA, adj);
180 }
181
182 return 0;
183 }
184
gem_ptp_enable(struct ptp_clock_info * ptp,struct ptp_clock_request * rq,int on)185 static int gem_ptp_enable(struct ptp_clock_info *ptp,
186 struct ptp_clock_request *rq, int on)
187 {
188 return -EOPNOTSUPP;
189 }
190
191 static const struct ptp_clock_info gem_ptp_caps_template = {
192 .owner = THIS_MODULE,
193 .name = GEM_PTP_TIMER_NAME,
194 .max_adj = 0,
195 .n_alarm = 0,
196 .n_ext_ts = 0,
197 .n_per_out = 0,
198 .n_pins = 0,
199 .pps = 1,
200 .adjfine = gem_ptp_adjfine,
201 .adjtime = gem_ptp_adjtime,
202 .gettimex64 = gem_tsu_get_time,
203 .settime64 = gem_tsu_set_time,
204 .enable = gem_ptp_enable,
205 };
206
gem_ptp_init_timer(struct macb * bp)207 static void gem_ptp_init_timer(struct macb *bp)
208 {
209 u32 rem = 0;
210 u64 adj;
211
212 bp->tsu_incr.ns = div_u64_rem(NSEC_PER_SEC, bp->tsu_rate, &rem);
213 if (rem) {
214 adj = rem;
215 adj <<= GEM_SUBNSINCR_SIZE;
216 bp->tsu_incr.sub_ns = div_u64(adj, bp->tsu_rate);
217 } else {
218 bp->tsu_incr.sub_ns = 0;
219 }
220 }
221
gem_ptp_init_tsu(struct macb * bp)222 static void gem_ptp_init_tsu(struct macb *bp)
223 {
224 struct timespec64 ts;
225
226 /* 1. get current system time */
227 ts = ns_to_timespec64(ktime_to_ns(ktime_get_real()));
228
229 /* 2. set ptp timer */
230 gem_tsu_set_time(&bp->ptp_clock_info, &ts);
231
232 /* 3. set PTP timer increment value to BASE_INCREMENT */
233 gem_tsu_incr_set(bp, &bp->tsu_incr);
234
235 gem_writel(bp, TA, 0);
236 }
237
gem_ptp_clear_timer(struct macb * bp)238 static void gem_ptp_clear_timer(struct macb *bp)
239 {
240 bp->tsu_incr.sub_ns = 0;
241 bp->tsu_incr.ns = 0;
242
243 gem_writel(bp, TISUBN, GEM_BF(SUBNSINCR, 0));
244 gem_writel(bp, TI, GEM_BF(NSINCR, 0));
245 gem_writel(bp, TA, 0);
246 }
247
gem_hw_timestamp(struct macb * bp,u32 dma_desc_ts_1,u32 dma_desc_ts_2,struct timespec64 * ts)248 static int gem_hw_timestamp(struct macb *bp, u32 dma_desc_ts_1,
249 u32 dma_desc_ts_2, struct timespec64 *ts)
250 {
251 struct timespec64 tsu;
252
253 ts->tv_sec = (GEM_BFEXT(DMA_SECH, dma_desc_ts_2) << GEM_DMA_SECL_SIZE) |
254 GEM_BFEXT(DMA_SECL, dma_desc_ts_1);
255 ts->tv_nsec = GEM_BFEXT(DMA_NSEC, dma_desc_ts_1);
256
257 /* TSU overlapping workaround
258 * The timestamp only contains lower few bits of seconds,
259 * so add value from 1588 timer
260 */
261 gem_tsu_get_time(&bp->ptp_clock_info, &tsu, NULL);
262
263 ts->tv_sec |= ((~GEM_DMA_SEC_MASK) & tsu.tv_sec);
264
265 /* If the top bit is set in the timestamp,
266 * but not in 1588 timer, it has rolled over,
267 * so subtract max size
268 */
269 if ((ts->tv_sec & (GEM_DMA_SEC_TOP >> 1)) &&
270 !(tsu.tv_sec & (GEM_DMA_SEC_TOP >> 1)))
271 ts->tv_sec -= GEM_DMA_SEC_TOP;
272
273 return 0;
274 }
275
gem_ptp_rxstamp(struct macb * bp,struct sk_buff * skb,struct macb_dma_desc * desc)276 void gem_ptp_rxstamp(struct macb *bp, struct sk_buff *skb,
277 struct macb_dma_desc *desc)
278 {
279 struct skb_shared_hwtstamps *shhwtstamps = skb_hwtstamps(skb);
280 struct macb_dma_desc_ptp *desc_ptp;
281 struct timespec64 ts;
282
283 if (GEM_BFEXT(DMA_RXVALID, desc->addr)) {
284 desc_ptp = macb_ptp_desc(bp, desc);
285 /* Unlikely but check */
286 if (!desc_ptp) {
287 dev_warn_ratelimited(&bp->pdev->dev,
288 "Timestamp not supported in BD\n");
289 return;
290 }
291 gem_hw_timestamp(bp, desc_ptp->ts_1, desc_ptp->ts_2, &ts);
292 memset(shhwtstamps, 0, sizeof(struct skb_shared_hwtstamps));
293 shhwtstamps->hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
294 }
295 }
296
gem_ptp_txstamp(struct macb * bp,struct sk_buff * skb,struct macb_dma_desc * desc)297 void gem_ptp_txstamp(struct macb *bp, struct sk_buff *skb,
298 struct macb_dma_desc *desc)
299 {
300 struct skb_shared_hwtstamps shhwtstamps;
301 struct macb_dma_desc_ptp *desc_ptp;
302 struct timespec64 ts;
303
304 if (!GEM_BFEXT(DMA_TXVALID, desc->ctrl)) {
305 dev_warn_ratelimited(&bp->pdev->dev,
306 "Timestamp not set in TX BD as expected\n");
307 return;
308 }
309
310 desc_ptp = macb_ptp_desc(bp, desc);
311 /* Unlikely but check */
312 if (!desc_ptp) {
313 dev_warn_ratelimited(&bp->pdev->dev,
314 "Timestamp not supported in BD\n");
315 return;
316 }
317
318 /* ensure ts_1/ts_2 is loaded after ctrl (TX_USED check) */
319 dma_rmb();
320 gem_hw_timestamp(bp, desc_ptp->ts_1, desc_ptp->ts_2, &ts);
321
322 memset(&shhwtstamps, 0, sizeof(shhwtstamps));
323 shhwtstamps.hwtstamp = ktime_set(ts.tv_sec, ts.tv_nsec);
324 skb_tstamp_tx(skb, &shhwtstamps);
325 }
326
gem_ptp_init(struct net_device * dev)327 void gem_ptp_init(struct net_device *dev)
328 {
329 struct macb *bp = netdev_priv(dev);
330
331 bp->ptp_clock_info = gem_ptp_caps_template;
332
333 /* nominal frequency and maximum adjustment in ppb */
334 bp->tsu_rate = bp->ptp_info->get_tsu_rate(bp);
335 bp->ptp_clock_info.max_adj = bp->ptp_info->get_ptp_max_adj();
336 gem_ptp_init_timer(bp);
337 bp->ptp_clock = ptp_clock_register(&bp->ptp_clock_info, &dev->dev);
338 if (IS_ERR(bp->ptp_clock)) {
339 pr_err("ptp clock register failed: %ld\n",
340 PTR_ERR(bp->ptp_clock));
341 bp->ptp_clock = NULL;
342 return;
343 } else if (bp->ptp_clock == NULL) {
344 pr_err("ptp clock register failed\n");
345 return;
346 }
347
348 spin_lock_init(&bp->tsu_clk_lock);
349
350 gem_ptp_init_tsu(bp);
351
352 dev_info(&bp->pdev->dev, "%s ptp clock registered.\n",
353 GEM_PTP_TIMER_NAME);
354 }
355
gem_ptp_remove(struct net_device * ndev)356 void gem_ptp_remove(struct net_device *ndev)
357 {
358 struct macb *bp = netdev_priv(ndev);
359
360 if (bp->ptp_clock) {
361 ptp_clock_unregister(bp->ptp_clock);
362 bp->ptp_clock = NULL;
363 }
364
365 gem_ptp_clear_timer(bp);
366
367 dev_info(&bp->pdev->dev, "%s ptp clock unregistered.\n",
368 GEM_PTP_TIMER_NAME);
369 }
370
gem_ptp_set_ts_mode(struct macb * bp,enum macb_bd_control tx_bd_control,enum macb_bd_control rx_bd_control)371 static int gem_ptp_set_ts_mode(struct macb *bp,
372 enum macb_bd_control tx_bd_control,
373 enum macb_bd_control rx_bd_control)
374 {
375 gem_writel(bp, TXBDCTRL, GEM_BF(TXTSMODE, tx_bd_control));
376 gem_writel(bp, RXBDCTRL, GEM_BF(RXTSMODE, rx_bd_control));
377
378 return 0;
379 }
380
gem_get_hwtst(struct net_device * dev,struct kernel_hwtstamp_config * tstamp_config)381 int gem_get_hwtst(struct net_device *dev,
382 struct kernel_hwtstamp_config *tstamp_config)
383 {
384 struct macb *bp = netdev_priv(dev);
385
386 *tstamp_config = bp->tstamp_config;
387 if (!macb_dma_ptp(bp))
388 return -EOPNOTSUPP;
389
390 return 0;
391 }
392
gem_ptp_set_one_step_sync(struct macb * bp,u8 enable)393 static void gem_ptp_set_one_step_sync(struct macb *bp, u8 enable)
394 {
395 u32 reg_val;
396
397 reg_val = macb_readl(bp, NCR);
398
399 if (enable)
400 macb_writel(bp, NCR, reg_val | MACB_BIT(OSSMODE));
401 else
402 macb_writel(bp, NCR, reg_val & ~MACB_BIT(OSSMODE));
403 }
404
gem_set_hwtst(struct net_device * dev,struct kernel_hwtstamp_config * tstamp_config,struct netlink_ext_ack * extack)405 int gem_set_hwtst(struct net_device *dev,
406 struct kernel_hwtstamp_config *tstamp_config,
407 struct netlink_ext_ack *extack)
408 {
409 enum macb_bd_control tx_bd_control = TSTAMP_DISABLED;
410 enum macb_bd_control rx_bd_control = TSTAMP_DISABLED;
411 struct macb *bp = netdev_priv(dev);
412 u32 regval;
413
414 if (!macb_dma_ptp(bp))
415 return -EOPNOTSUPP;
416
417 switch (tstamp_config->tx_type) {
418 case HWTSTAMP_TX_OFF:
419 break;
420 case HWTSTAMP_TX_ONESTEP_SYNC:
421 gem_ptp_set_one_step_sync(bp, 1);
422 tx_bd_control = TSTAMP_ALL_FRAMES;
423 break;
424 case HWTSTAMP_TX_ON:
425 gem_ptp_set_one_step_sync(bp, 0);
426 tx_bd_control = TSTAMP_ALL_FRAMES;
427 break;
428 default:
429 return -ERANGE;
430 }
431
432 switch (tstamp_config->rx_filter) {
433 case HWTSTAMP_FILTER_NONE:
434 break;
435 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
436 break;
437 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
438 break;
439 case HWTSTAMP_FILTER_PTP_V2_EVENT:
440 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
441 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
442 case HWTSTAMP_FILTER_PTP_V2_SYNC:
443 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
444 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
445 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
446 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
447 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
448 rx_bd_control = TSTAMP_ALL_PTP_FRAMES;
449 tstamp_config->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
450 regval = macb_readl(bp, NCR);
451 macb_writel(bp, NCR, (regval | MACB_BIT(SRTSM)));
452 break;
453 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
454 case HWTSTAMP_FILTER_ALL:
455 rx_bd_control = TSTAMP_ALL_FRAMES;
456 tstamp_config->rx_filter = HWTSTAMP_FILTER_ALL;
457 break;
458 default:
459 tstamp_config->rx_filter = HWTSTAMP_FILTER_NONE;
460 return -ERANGE;
461 }
462
463 bp->tstamp_config = *tstamp_config;
464
465 if (gem_ptp_set_ts_mode(bp, tx_bd_control, rx_bd_control) != 0)
466 return -ERANGE;
467
468 return 0;
469 }
470
471