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