1 | // SPDX-License-Identifier: GPL-2.0 |
2 | /* |
3 | * Copyright (C) 2022 Meta Platforms Inc. |
4 | * Copyright (C) 2022 Jonathan Lemon <jonathan.lemon@gmail.com> |
5 | */ |
6 | |
7 | #include <asm/unaligned.h> |
8 | #include <linux/mii.h> |
9 | #include <linux/phy.h> |
10 | #include <linux/ptp_classify.h> |
11 | #include <linux/ptp_clock_kernel.h> |
12 | #include <linux/net_tstamp.h> |
13 | #include <linux/netdevice.h> |
14 | #include <linux/workqueue.h> |
15 | |
16 | #include "bcm-phy-lib.h" |
17 | |
18 | /* IEEE 1588 Expansion registers */ |
19 | #define SLICE_CTRL 0x0810 |
20 | #define SLICE_TX_EN BIT(0) |
21 | #define SLICE_RX_EN BIT(8) |
22 | #define TX_EVENT_MODE 0x0811 |
23 | #define MODE_TX_UPDATE_CF BIT(0) |
24 | #define MODE_TX_REPLACE_TS_CF BIT(1) |
25 | #define MODE_TX_REPLACE_TS GENMASK(1, 0) |
26 | #define RX_EVENT_MODE 0x0819 |
27 | #define MODE_RX_UPDATE_CF BIT(0) |
28 | #define MODE_RX_INSERT_TS_48 BIT(1) |
29 | #define MODE_RX_INSERT_TS_64 GENMASK(1, 0) |
30 | |
31 | #define MODE_EVT_SHIFT_SYNC 0 |
32 | #define MODE_EVT_SHIFT_DELAY_REQ 2 |
33 | #define MODE_EVT_SHIFT_PDELAY_REQ 4 |
34 | #define MODE_EVT_SHIFT_PDELAY_RESP 6 |
35 | |
36 | #define MODE_SEL_SHIFT_PORT 0 |
37 | #define MODE_SEL_SHIFT_CPU 8 |
38 | |
39 | #define RX_MODE_SEL(sel, evt, act) \ |
40 | (((MODE_RX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel)) |
41 | |
42 | #define TX_MODE_SEL(sel, evt, act) \ |
43 | (((MODE_TX_##act) << (MODE_EVT_SHIFT_##evt)) << (MODE_SEL_SHIFT_##sel)) |
44 | |
45 | /* needs global TS capture first */ |
46 | #define TX_TS_CAPTURE 0x0821 |
47 | #define TX_TS_CAP_EN BIT(0) |
48 | #define RX_TS_CAPTURE 0x0822 |
49 | #define RX_TS_CAP_EN BIT(0) |
50 | |
51 | #define TIME_CODE_0 0x0854 |
52 | #define TIME_CODE_1 0x0855 |
53 | #define TIME_CODE_2 0x0856 |
54 | #define TIME_CODE_3 0x0857 |
55 | #define TIME_CODE_4 0x0858 |
56 | |
57 | #define DPLL_SELECT 0x085b |
58 | #define DPLL_HB_MODE2 BIT(6) |
59 | |
60 | #define SHADOW_CTRL 0x085c |
61 | #define SHADOW_LOAD 0x085d |
62 | #define TIME_CODE_LOAD BIT(10) |
63 | #define SYNC_OUT_LOAD BIT(9) |
64 | #define NCO_TIME_LOAD BIT(7) |
65 | #define FREQ_LOAD BIT(6) |
66 | #define INTR_MASK 0x085e |
67 | #define INTR_STATUS 0x085f |
68 | #define INTC_FSYNC BIT(0) |
69 | #define INTC_SOP BIT(1) |
70 | |
71 | #define NCO_FREQ_LSB 0x0873 |
72 | #define NCO_FREQ_MSB 0x0874 |
73 | |
74 | #define NCO_TIME_0 0x0875 |
75 | #define NCO_TIME_1 0x0876 |
76 | #define NCO_TIME_2_CTRL 0x0877 |
77 | #define FREQ_MDIO_SEL BIT(14) |
78 | |
79 | #define SYNC_OUT_0 0x0878 |
80 | #define SYNC_OUT_1 0x0879 |
81 | #define SYNC_OUT_2 0x087a |
82 | |
83 | #define SYNC_IN_DIVIDER 0x087b |
84 | |
85 | #define SYNOUT_TS_0 0x087c |
86 | #define SYNOUT_TS_1 0x087d |
87 | #define SYNOUT_TS_2 0x087e |
88 | |
89 | #define NSE_CTRL 0x087f |
90 | #define NSE_GMODE_EN GENMASK(15, 14) |
91 | #define NSE_CAPTURE_EN BIT(13) |
92 | #define NSE_INIT BIT(12) |
93 | #define NSE_CPU_FRAMESYNC BIT(5) |
94 | #define NSE_SYNC1_FRAMESYNC BIT(3) |
95 | #define NSE_FRAMESYNC_MASK GENMASK(5, 2) |
96 | #define NSE_PEROUT_EN BIT(1) |
97 | #define NSE_ONESHOT_EN BIT(0) |
98 | #define NSE_SYNC_OUT_MASK GENMASK(1, 0) |
99 | |
100 | #define TS_READ_CTRL 0x0885 |
101 | #define TS_READ_START BIT(0) |
102 | #define TS_READ_END BIT(1) |
103 | |
104 | #define HB_REG_0 0x0886 |
105 | #define HB_REG_1 0x0887 |
106 | #define HB_REG_2 0x0888 |
107 | #define HB_REG_3 0x08ec |
108 | #define HB_REG_4 0x08ed |
109 | #define HB_STAT_CTRL 0x088e |
110 | #define HB_READ_START BIT(10) |
111 | #define HB_READ_END BIT(11) |
112 | #define HB_READ_MASK GENMASK(11, 10) |
113 | |
114 | #define TS_REG_0 0x0889 |
115 | #define TS_REG_1 0x088a |
116 | #define TS_REG_2 0x088b |
117 | #define TS_REG_3 0x08c4 |
118 | |
119 | #define TS_INFO_0 0x088c |
120 | #define TS_INFO_1 0x088d |
121 | |
122 | #define TIMECODE_CTRL 0x08c3 |
123 | #define TX_TIMECODE_SEL GENMASK(7, 0) |
124 | #define RX_TIMECODE_SEL GENMASK(15, 8) |
125 | |
126 | #define TIME_SYNC 0x0ff5 |
127 | #define TIME_SYNC_EN BIT(0) |
128 | |
129 | struct bcm_ptp_private { |
130 | struct phy_device *phydev; |
131 | struct mii_timestamper mii_ts; |
132 | struct ptp_clock *ptp_clock; |
133 | struct ptp_clock_info ptp_info; |
134 | struct ptp_pin_desc pin; |
135 | struct mutex mutex; |
136 | struct sk_buff_head tx_queue; |
137 | int tx_type; |
138 | bool hwts_rx; |
139 | u16 nse_ctrl; |
140 | bool pin_active; |
141 | struct delayed_work pin_work; |
142 | }; |
143 | |
144 | struct bcm_ptp_skb_cb { |
145 | unsigned long timeout; |
146 | u16 seq_id; |
147 | u8 msgtype; |
148 | bool discard; |
149 | }; |
150 | |
151 | struct bcm_ptp_capture { |
152 | ktime_t hwtstamp; |
153 | u16 seq_id; |
154 | u8 msgtype; |
155 | bool tx_dir; |
156 | }; |
157 | |
158 | #define BCM_SKB_CB(skb) ((struct bcm_ptp_skb_cb *)(skb)->cb) |
159 | #define SKB_TS_TIMEOUT 10 /* jiffies */ |
160 | |
161 | #define BCM_MAX_PULSE_8NS ((1U << 9) - 1) |
162 | #define BCM_MAX_PERIOD_8NS ((1U << 30) - 1) |
163 | |
164 | #define BRCM_PHY_MODEL(phydev) \ |
165 | ((phydev)->drv->phy_id & (phydev)->drv->phy_id_mask) |
166 | |
167 | static struct bcm_ptp_private *mii2priv(struct mii_timestamper *mii_ts) |
168 | { |
169 | return container_of(mii_ts, struct bcm_ptp_private, mii_ts); |
170 | } |
171 | |
172 | static struct bcm_ptp_private *ptp2priv(struct ptp_clock_info *info) |
173 | { |
174 | return container_of(info, struct bcm_ptp_private, ptp_info); |
175 | } |
176 | |
177 | static void bcm_ptp_get_framesync_ts(struct phy_device *phydev, |
178 | struct timespec64 *ts) |
179 | { |
180 | u16 hb[4]; |
181 | |
182 | bcm_phy_write_exp(phydev, HB_STAT_CTRL, HB_READ_START); |
183 | |
184 | hb[0] = bcm_phy_read_exp(phydev, HB_REG_0); |
185 | hb[1] = bcm_phy_read_exp(phydev, HB_REG_1); |
186 | hb[2] = bcm_phy_read_exp(phydev, HB_REG_2); |
187 | hb[3] = bcm_phy_read_exp(phydev, HB_REG_3); |
188 | |
189 | bcm_phy_write_exp(phydev, HB_STAT_CTRL, HB_READ_END); |
190 | bcm_phy_write_exp(phydev, HB_STAT_CTRL, val: 0); |
191 | |
192 | ts->tv_sec = (hb[3] << 16) | hb[2]; |
193 | ts->tv_nsec = (hb[1] << 16) | hb[0]; |
194 | } |
195 | |
196 | static u16 bcm_ptp_framesync_disable(struct phy_device *phydev, u16 orig_ctrl) |
197 | { |
198 | u16 ctrl = orig_ctrl & ~(NSE_FRAMESYNC_MASK | NSE_CAPTURE_EN); |
199 | |
200 | bcm_phy_write_exp(phydev, NSE_CTRL, val: ctrl); |
201 | |
202 | return ctrl; |
203 | } |
204 | |
205 | static void bcm_ptp_framesync_restore(struct phy_device *phydev, u16 orig_ctrl) |
206 | { |
207 | if (orig_ctrl & NSE_FRAMESYNC_MASK) |
208 | bcm_phy_write_exp(phydev, NSE_CTRL, val: orig_ctrl); |
209 | } |
210 | |
211 | static void bcm_ptp_framesync(struct phy_device *phydev, u16 ctrl) |
212 | { |
213 | /* trigger framesync - must have 0->1 transition. */ |
214 | bcm_phy_write_exp(phydev, NSE_CTRL, val: ctrl | NSE_CPU_FRAMESYNC); |
215 | } |
216 | |
217 | static int bcm_ptp_framesync_ts(struct phy_device *phydev, |
218 | struct ptp_system_timestamp *sts, |
219 | struct timespec64 *ts, |
220 | u16 orig_ctrl) |
221 | { |
222 | u16 ctrl, reg; |
223 | int i; |
224 | |
225 | ctrl = bcm_ptp_framesync_disable(phydev, orig_ctrl); |
226 | |
227 | ptp_read_system_prets(sts); |
228 | |
229 | /* trigger framesync + capture */ |
230 | bcm_ptp_framesync(phydev, ctrl: ctrl | NSE_CAPTURE_EN); |
231 | |
232 | ptp_read_system_postts(sts); |
233 | |
234 | /* poll for FSYNC interrupt from TS capture */ |
235 | for (i = 0; i < 10; i++) { |
236 | reg = bcm_phy_read_exp(phydev, INTR_STATUS); |
237 | if (reg & INTC_FSYNC) { |
238 | bcm_ptp_get_framesync_ts(phydev, ts); |
239 | break; |
240 | } |
241 | } |
242 | |
243 | bcm_ptp_framesync_restore(phydev, orig_ctrl); |
244 | |
245 | return reg & INTC_FSYNC ? 0 : -ETIMEDOUT; |
246 | } |
247 | |
248 | static int bcm_ptp_gettimex(struct ptp_clock_info *info, |
249 | struct timespec64 *ts, |
250 | struct ptp_system_timestamp *sts) |
251 | { |
252 | struct bcm_ptp_private *priv = ptp2priv(info); |
253 | int err; |
254 | |
255 | mutex_lock(&priv->mutex); |
256 | err = bcm_ptp_framesync_ts(phydev: priv->phydev, sts, ts, orig_ctrl: priv->nse_ctrl); |
257 | mutex_unlock(lock: &priv->mutex); |
258 | |
259 | return err; |
260 | } |
261 | |
262 | static int bcm_ptp_settime_locked(struct bcm_ptp_private *priv, |
263 | const struct timespec64 *ts) |
264 | { |
265 | struct phy_device *phydev = priv->phydev; |
266 | u16 ctrl; |
267 | u64 ns; |
268 | |
269 | ctrl = bcm_ptp_framesync_disable(phydev, orig_ctrl: priv->nse_ctrl); |
270 | |
271 | /* set up time code */ |
272 | bcm_phy_write_exp(phydev, TIME_CODE_0, val: ts->tv_nsec); |
273 | bcm_phy_write_exp(phydev, TIME_CODE_1, val: ts->tv_nsec >> 16); |
274 | bcm_phy_write_exp(phydev, TIME_CODE_2, val: ts->tv_sec); |
275 | bcm_phy_write_exp(phydev, TIME_CODE_3, val: ts->tv_sec >> 16); |
276 | bcm_phy_write_exp(phydev, TIME_CODE_4, val: ts->tv_sec >> 32); |
277 | |
278 | /* set NCO counter to match */ |
279 | ns = timespec64_to_ns(ts); |
280 | bcm_phy_write_exp(phydev, NCO_TIME_0, val: ns >> 4); |
281 | bcm_phy_write_exp(phydev, NCO_TIME_1, val: ns >> 20); |
282 | bcm_phy_write_exp(phydev, NCO_TIME_2_CTRL, val: (ns >> 36) & 0xfff); |
283 | |
284 | /* set up load on next frame sync (auto-clears due to NSE_INIT) */ |
285 | bcm_phy_write_exp(phydev, SHADOW_LOAD, TIME_CODE_LOAD | NCO_TIME_LOAD); |
286 | |
287 | /* must have NSE_INIT in order to write time code */ |
288 | bcm_ptp_framesync(phydev, ctrl: ctrl | NSE_INIT); |
289 | |
290 | bcm_ptp_framesync_restore(phydev, orig_ctrl: priv->nse_ctrl); |
291 | |
292 | return 0; |
293 | } |
294 | |
295 | static int bcm_ptp_settime(struct ptp_clock_info *info, |
296 | const struct timespec64 *ts) |
297 | { |
298 | struct bcm_ptp_private *priv = ptp2priv(info); |
299 | int err; |
300 | |
301 | mutex_lock(&priv->mutex); |
302 | err = bcm_ptp_settime_locked(priv, ts); |
303 | mutex_unlock(lock: &priv->mutex); |
304 | |
305 | return err; |
306 | } |
307 | |
308 | static int bcm_ptp_adjtime_locked(struct bcm_ptp_private *priv, |
309 | s64 delta_ns) |
310 | { |
311 | struct timespec64 ts; |
312 | int err; |
313 | s64 ns; |
314 | |
315 | err = bcm_ptp_framesync_ts(phydev: priv->phydev, NULL, ts: &ts, orig_ctrl: priv->nse_ctrl); |
316 | if (!err) { |
317 | ns = timespec64_to_ns(ts: &ts) + delta_ns; |
318 | ts = ns_to_timespec64(nsec: ns); |
319 | err = bcm_ptp_settime_locked(priv, ts: &ts); |
320 | } |
321 | return err; |
322 | } |
323 | |
324 | static int bcm_ptp_adjtime(struct ptp_clock_info *info, s64 delta_ns) |
325 | { |
326 | struct bcm_ptp_private *priv = ptp2priv(info); |
327 | int err; |
328 | |
329 | mutex_lock(&priv->mutex); |
330 | err = bcm_ptp_adjtime_locked(priv, delta_ns); |
331 | mutex_unlock(lock: &priv->mutex); |
332 | |
333 | return err; |
334 | } |
335 | |
336 | /* A 125Mhz clock should adjust 8ns per pulse. |
337 | * The frequency adjustment base is 0x8000 0000, or 8*2^28. |
338 | * |
339 | * Frequency adjustment is |
340 | * adj = scaled_ppm * 8*2^28 / (10^6 * 2^16) |
341 | * which simplifies to: |
342 | * adj = scaled_ppm * 2^9 / 5^6 |
343 | */ |
344 | static int bcm_ptp_adjfine(struct ptp_clock_info *info, long scaled_ppm) |
345 | { |
346 | struct bcm_ptp_private *priv = ptp2priv(info); |
347 | int neg_adj = 0; |
348 | u32 diff, freq; |
349 | u16 ctrl; |
350 | u64 adj; |
351 | |
352 | if (scaled_ppm < 0) { |
353 | neg_adj = 1; |
354 | scaled_ppm = -scaled_ppm; |
355 | } |
356 | |
357 | adj = scaled_ppm << 9; |
358 | diff = div_u64(dividend: adj, divisor: 15625); |
359 | freq = (8 << 28) + (neg_adj ? -diff : diff); |
360 | |
361 | mutex_lock(&priv->mutex); |
362 | |
363 | ctrl = bcm_ptp_framesync_disable(phydev: priv->phydev, orig_ctrl: priv->nse_ctrl); |
364 | |
365 | bcm_phy_write_exp(phydev: priv->phydev, NCO_FREQ_LSB, val: freq); |
366 | bcm_phy_write_exp(phydev: priv->phydev, NCO_FREQ_MSB, val: freq >> 16); |
367 | |
368 | bcm_phy_write_exp(phydev: priv->phydev, NCO_TIME_2_CTRL, FREQ_MDIO_SEL); |
369 | |
370 | /* load on next framesync */ |
371 | bcm_phy_write_exp(phydev: priv->phydev, SHADOW_LOAD, FREQ_LOAD); |
372 | |
373 | bcm_ptp_framesync(phydev: priv->phydev, ctrl); |
374 | |
375 | /* clear load */ |
376 | bcm_phy_write_exp(phydev: priv->phydev, SHADOW_LOAD, val: 0); |
377 | |
378 | bcm_ptp_framesync_restore(phydev: priv->phydev, orig_ctrl: priv->nse_ctrl); |
379 | |
380 | mutex_unlock(lock: &priv->mutex); |
381 | |
382 | return 0; |
383 | } |
384 | |
385 | static bool bcm_ptp_rxtstamp(struct mii_timestamper *mii_ts, |
386 | struct sk_buff *skb, int type) |
387 | { |
388 | struct bcm_ptp_private *priv = mii2priv(mii_ts); |
389 | struct skb_shared_hwtstamps *hwts; |
390 | struct ptp_header *; |
391 | u32 sec, nsec; |
392 | u8 *data; |
393 | int off; |
394 | |
395 | if (!priv->hwts_rx) |
396 | return false; |
397 | |
398 | header = ptp_parse_header(skb, type); |
399 | if (!header) |
400 | return false; |
401 | |
402 | data = (u8 *)(header + 1); |
403 | sec = get_unaligned_be32(p: data); |
404 | nsec = get_unaligned_be32(p: data + 4); |
405 | |
406 | hwts = skb_hwtstamps(skb); |
407 | hwts->hwtstamp = ktime_set(secs: sec, nsecs: nsec); |
408 | |
409 | off = data - skb->data + 8; |
410 | if (off < skb->len) { |
411 | memmove(data, data + 8, skb->len - off); |
412 | __pskb_trim(skb, len: skb->len - 8); |
413 | } |
414 | |
415 | return false; |
416 | } |
417 | |
418 | static bool bcm_ptp_get_tstamp(struct bcm_ptp_private *priv, |
419 | struct bcm_ptp_capture *capts) |
420 | { |
421 | struct phy_device *phydev = priv->phydev; |
422 | u16 ts[4], reg; |
423 | u32 sec, nsec; |
424 | |
425 | mutex_lock(&priv->mutex); |
426 | |
427 | reg = bcm_phy_read_exp(phydev, INTR_STATUS); |
428 | if ((reg & INTC_SOP) == 0) { |
429 | mutex_unlock(lock: &priv->mutex); |
430 | return false; |
431 | } |
432 | |
433 | bcm_phy_write_exp(phydev, TS_READ_CTRL, TS_READ_START); |
434 | |
435 | ts[0] = bcm_phy_read_exp(phydev, TS_REG_0); |
436 | ts[1] = bcm_phy_read_exp(phydev, TS_REG_1); |
437 | ts[2] = bcm_phy_read_exp(phydev, TS_REG_2); |
438 | ts[3] = bcm_phy_read_exp(phydev, TS_REG_3); |
439 | |
440 | /* not in be32 format for some reason */ |
441 | capts->seq_id = bcm_phy_read_exp(phydev: priv->phydev, TS_INFO_0); |
442 | |
443 | reg = bcm_phy_read_exp(phydev, TS_INFO_1); |
444 | capts->msgtype = reg >> 12; |
445 | capts->tx_dir = !!(reg & BIT(11)); |
446 | |
447 | bcm_phy_write_exp(phydev, TS_READ_CTRL, TS_READ_END); |
448 | bcm_phy_write_exp(phydev, TS_READ_CTRL, val: 0); |
449 | |
450 | mutex_unlock(lock: &priv->mutex); |
451 | |
452 | sec = (ts[3] << 16) | ts[2]; |
453 | nsec = (ts[1] << 16) | ts[0]; |
454 | capts->hwtstamp = ktime_set(secs: sec, nsecs: nsec); |
455 | |
456 | return true; |
457 | } |
458 | |
459 | static void bcm_ptp_match_tstamp(struct bcm_ptp_private *priv, |
460 | struct bcm_ptp_capture *capts) |
461 | { |
462 | struct skb_shared_hwtstamps hwts; |
463 | struct sk_buff *skb, *ts_skb; |
464 | unsigned long flags; |
465 | bool first = false; |
466 | |
467 | ts_skb = NULL; |
468 | spin_lock_irqsave(&priv->tx_queue.lock, flags); |
469 | skb_queue_walk(&priv->tx_queue, skb) { |
470 | if (BCM_SKB_CB(skb)->seq_id == capts->seq_id && |
471 | BCM_SKB_CB(skb)->msgtype == capts->msgtype) { |
472 | first = skb_queue_is_first(list: &priv->tx_queue, skb); |
473 | __skb_unlink(skb, list: &priv->tx_queue); |
474 | ts_skb = skb; |
475 | break; |
476 | } |
477 | } |
478 | spin_unlock_irqrestore(lock: &priv->tx_queue.lock, flags); |
479 | |
480 | /* TX captures one-step packets, discard them if needed. */ |
481 | if (ts_skb) { |
482 | if (BCM_SKB_CB(ts_skb)->discard) { |
483 | kfree_skb(skb: ts_skb); |
484 | } else { |
485 | memset(&hwts, 0, sizeof(hwts)); |
486 | hwts.hwtstamp = capts->hwtstamp; |
487 | skb_complete_tx_timestamp(skb: ts_skb, hwtstamps: &hwts); |
488 | } |
489 | } |
490 | |
491 | /* not first match, try and expire entries */ |
492 | if (!first) { |
493 | while ((skb = skb_dequeue(list: &priv->tx_queue))) { |
494 | if (!time_after(jiffies, BCM_SKB_CB(skb)->timeout)) { |
495 | skb_queue_head(list: &priv->tx_queue, newsk: skb); |
496 | break; |
497 | } |
498 | kfree_skb(skb); |
499 | } |
500 | } |
501 | } |
502 | |
503 | static long bcm_ptp_do_aux_work(struct ptp_clock_info *info) |
504 | { |
505 | struct bcm_ptp_private *priv = ptp2priv(info); |
506 | struct bcm_ptp_capture capts; |
507 | bool reschedule = false; |
508 | |
509 | while (!skb_queue_empty_lockless(list: &priv->tx_queue)) { |
510 | if (!bcm_ptp_get_tstamp(priv, capts: &capts)) { |
511 | reschedule = true; |
512 | break; |
513 | } |
514 | bcm_ptp_match_tstamp(priv, capts: &capts); |
515 | } |
516 | |
517 | return reschedule ? 1 : -1; |
518 | } |
519 | |
520 | static int bcm_ptp_cancel_func(struct bcm_ptp_private *priv) |
521 | { |
522 | if (!priv->pin_active) |
523 | return 0; |
524 | |
525 | priv->pin_active = false; |
526 | |
527 | priv->nse_ctrl &= ~(NSE_SYNC_OUT_MASK | NSE_SYNC1_FRAMESYNC | |
528 | NSE_CAPTURE_EN); |
529 | bcm_phy_write_exp(phydev: priv->phydev, NSE_CTRL, val: priv->nse_ctrl); |
530 | |
531 | cancel_delayed_work_sync(dwork: &priv->pin_work); |
532 | |
533 | return 0; |
534 | } |
535 | |
536 | static void bcm_ptp_perout_work(struct work_struct *pin_work) |
537 | { |
538 | struct bcm_ptp_private *priv = |
539 | container_of(pin_work, struct bcm_ptp_private, pin_work.work); |
540 | struct phy_device *phydev = priv->phydev; |
541 | struct timespec64 ts; |
542 | u64 ns, next; |
543 | u16 ctrl; |
544 | |
545 | mutex_lock(&priv->mutex); |
546 | |
547 | /* no longer running */ |
548 | if (!priv->pin_active) { |
549 | mutex_unlock(lock: &priv->mutex); |
550 | return; |
551 | } |
552 | |
553 | bcm_ptp_framesync_ts(phydev, NULL, ts: &ts, orig_ctrl: priv->nse_ctrl); |
554 | |
555 | /* this is 1PPS only */ |
556 | next = NSEC_PER_SEC - ts.tv_nsec; |
557 | ts.tv_sec += next < NSEC_PER_MSEC ? 2 : 1; |
558 | ts.tv_nsec = 0; |
559 | |
560 | ns = timespec64_to_ns(ts: &ts); |
561 | |
562 | /* force 0->1 transition for ONESHOT */ |
563 | ctrl = bcm_ptp_framesync_disable(phydev, |
564 | orig_ctrl: priv->nse_ctrl & ~NSE_ONESHOT_EN); |
565 | |
566 | bcm_phy_write_exp(phydev, SYNOUT_TS_0, val: ns & 0xfff0); |
567 | bcm_phy_write_exp(phydev, SYNOUT_TS_1, val: ns >> 16); |
568 | bcm_phy_write_exp(phydev, SYNOUT_TS_2, val: ns >> 32); |
569 | |
570 | /* load values on next framesync */ |
571 | bcm_phy_write_exp(phydev, SHADOW_LOAD, SYNC_OUT_LOAD); |
572 | |
573 | bcm_ptp_framesync(phydev, ctrl: ctrl | NSE_ONESHOT_EN | NSE_INIT); |
574 | |
575 | priv->nse_ctrl |= NSE_ONESHOT_EN; |
576 | bcm_ptp_framesync_restore(phydev, orig_ctrl: priv->nse_ctrl); |
577 | |
578 | mutex_unlock(lock: &priv->mutex); |
579 | |
580 | next = next + NSEC_PER_MSEC; |
581 | schedule_delayed_work(dwork: &priv->pin_work, delay: nsecs_to_jiffies(n: next)); |
582 | } |
583 | |
584 | static int bcm_ptp_perout_locked(struct bcm_ptp_private *priv, |
585 | struct ptp_perout_request *req, int on) |
586 | { |
587 | struct phy_device *phydev = priv->phydev; |
588 | u64 period, pulse; |
589 | u16 val; |
590 | |
591 | if (!on) |
592 | return bcm_ptp_cancel_func(priv); |
593 | |
594 | /* 1PPS */ |
595 | if (req->period.sec != 1 || req->period.nsec != 0) |
596 | return -EINVAL; |
597 | |
598 | period = BCM_MAX_PERIOD_8NS; /* write nonzero value */ |
599 | |
600 | if (req->flags & PTP_PEROUT_PHASE) |
601 | return -EOPNOTSUPP; |
602 | |
603 | if (req->flags & PTP_PEROUT_DUTY_CYCLE) |
604 | pulse = ktime_to_ns(kt: ktime_set(secs: req->on.sec, nsecs: req->on.nsec)); |
605 | else |
606 | pulse = (u64)BCM_MAX_PULSE_8NS << 3; |
607 | |
608 | /* convert to 8ns units */ |
609 | pulse >>= 3; |
610 | |
611 | if (!pulse || pulse > period || pulse > BCM_MAX_PULSE_8NS) |
612 | return -EINVAL; |
613 | |
614 | bcm_phy_write_exp(phydev, SYNC_OUT_0, val: period); |
615 | |
616 | val = ((pulse & 0x3) << 14) | ((period >> 16) & 0x3fff); |
617 | bcm_phy_write_exp(phydev, SYNC_OUT_1, val); |
618 | |
619 | val = ((pulse >> 2) & 0x7f) | (pulse << 7); |
620 | bcm_phy_write_exp(phydev, SYNC_OUT_2, val); |
621 | |
622 | if (priv->pin_active) |
623 | cancel_delayed_work_sync(dwork: &priv->pin_work); |
624 | |
625 | priv->pin_active = true; |
626 | INIT_DELAYED_WORK(&priv->pin_work, bcm_ptp_perout_work); |
627 | schedule_delayed_work(dwork: &priv->pin_work, delay: 0); |
628 | |
629 | return 0; |
630 | } |
631 | |
632 | static void bcm_ptp_extts_work(struct work_struct *pin_work) |
633 | { |
634 | struct bcm_ptp_private *priv = |
635 | container_of(pin_work, struct bcm_ptp_private, pin_work.work); |
636 | struct phy_device *phydev = priv->phydev; |
637 | struct ptp_clock_event event; |
638 | struct timespec64 ts; |
639 | u16 reg; |
640 | |
641 | mutex_lock(&priv->mutex); |
642 | |
643 | /* no longer running */ |
644 | if (!priv->pin_active) { |
645 | mutex_unlock(lock: &priv->mutex); |
646 | return; |
647 | } |
648 | |
649 | reg = bcm_phy_read_exp(phydev, INTR_STATUS); |
650 | if ((reg & INTC_FSYNC) == 0) |
651 | goto out; |
652 | |
653 | bcm_ptp_get_framesync_ts(phydev, ts: &ts); |
654 | |
655 | event.index = 0; |
656 | event.type = PTP_CLOCK_EXTTS; |
657 | event.timestamp = timespec64_to_ns(ts: &ts); |
658 | ptp_clock_event(ptp: priv->ptp_clock, event: &event); |
659 | |
660 | out: |
661 | mutex_unlock(lock: &priv->mutex); |
662 | schedule_delayed_work(dwork: &priv->pin_work, HZ / 4); |
663 | } |
664 | |
665 | static int bcm_ptp_extts_locked(struct bcm_ptp_private *priv, int on) |
666 | { |
667 | struct phy_device *phydev = priv->phydev; |
668 | |
669 | if (!on) |
670 | return bcm_ptp_cancel_func(priv); |
671 | |
672 | if (priv->pin_active) |
673 | cancel_delayed_work_sync(dwork: &priv->pin_work); |
674 | |
675 | bcm_ptp_framesync_disable(phydev, orig_ctrl: priv->nse_ctrl); |
676 | |
677 | priv->nse_ctrl |= NSE_SYNC1_FRAMESYNC | NSE_CAPTURE_EN; |
678 | |
679 | bcm_ptp_framesync_restore(phydev, orig_ctrl: priv->nse_ctrl); |
680 | |
681 | priv->pin_active = true; |
682 | INIT_DELAYED_WORK(&priv->pin_work, bcm_ptp_extts_work); |
683 | schedule_delayed_work(dwork: &priv->pin_work, delay: 0); |
684 | |
685 | return 0; |
686 | } |
687 | |
688 | static int bcm_ptp_enable(struct ptp_clock_info *info, |
689 | struct ptp_clock_request *rq, int on) |
690 | { |
691 | struct bcm_ptp_private *priv = ptp2priv(info); |
692 | int err = -EBUSY; |
693 | |
694 | mutex_lock(&priv->mutex); |
695 | |
696 | switch (rq->type) { |
697 | case PTP_CLK_REQ_PEROUT: |
698 | if (priv->pin.func == PTP_PF_PEROUT) |
699 | err = bcm_ptp_perout_locked(priv, req: &rq->perout, on); |
700 | break; |
701 | case PTP_CLK_REQ_EXTTS: |
702 | if (priv->pin.func == PTP_PF_EXTTS) |
703 | err = bcm_ptp_extts_locked(priv, on); |
704 | break; |
705 | default: |
706 | err = -EOPNOTSUPP; |
707 | break; |
708 | } |
709 | |
710 | mutex_unlock(lock: &priv->mutex); |
711 | |
712 | return err; |
713 | } |
714 | |
715 | static int bcm_ptp_verify(struct ptp_clock_info *info, unsigned int pin, |
716 | enum ptp_pin_function func, unsigned int chan) |
717 | { |
718 | switch (func) { |
719 | case PTP_PF_NONE: |
720 | case PTP_PF_EXTTS: |
721 | case PTP_PF_PEROUT: |
722 | break; |
723 | default: |
724 | return -EOPNOTSUPP; |
725 | } |
726 | return 0; |
727 | } |
728 | |
729 | static const struct ptp_clock_info bcm_ptp_clock_info = { |
730 | .owner = THIS_MODULE, |
731 | .name = KBUILD_MODNAME, |
732 | .max_adj = 100000000, |
733 | .gettimex64 = bcm_ptp_gettimex, |
734 | .settime64 = bcm_ptp_settime, |
735 | .adjtime = bcm_ptp_adjtime, |
736 | .adjfine = bcm_ptp_adjfine, |
737 | .enable = bcm_ptp_enable, |
738 | .verify = bcm_ptp_verify, |
739 | .do_aux_work = bcm_ptp_do_aux_work, |
740 | .n_pins = 1, |
741 | .n_per_out = 1, |
742 | .n_ext_ts = 1, |
743 | }; |
744 | |
745 | static void bcm_ptp_txtstamp(struct mii_timestamper *mii_ts, |
746 | struct sk_buff *skb, int type) |
747 | { |
748 | struct bcm_ptp_private *priv = mii2priv(mii_ts); |
749 | struct ptp_header *hdr; |
750 | bool discard = false; |
751 | int msgtype; |
752 | |
753 | hdr = ptp_parse_header(skb, type); |
754 | if (!hdr) |
755 | goto out; |
756 | msgtype = ptp_get_msgtype(hdr, type); |
757 | |
758 | switch (priv->tx_type) { |
759 | case HWTSTAMP_TX_ONESTEP_P2P: |
760 | if (msgtype == PTP_MSGTYPE_PDELAY_RESP) |
761 | discard = true; |
762 | fallthrough; |
763 | case HWTSTAMP_TX_ONESTEP_SYNC: |
764 | if (msgtype == PTP_MSGTYPE_SYNC) |
765 | discard = true; |
766 | fallthrough; |
767 | case HWTSTAMP_TX_ON: |
768 | BCM_SKB_CB(skb)->timeout = jiffies + SKB_TS_TIMEOUT; |
769 | BCM_SKB_CB(skb)->seq_id = be16_to_cpu(hdr->sequence_id); |
770 | BCM_SKB_CB(skb)->msgtype = msgtype; |
771 | BCM_SKB_CB(skb)->discard = discard; |
772 | skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS; |
773 | skb_queue_tail(list: &priv->tx_queue, newsk: skb); |
774 | ptp_schedule_worker(ptp: priv->ptp_clock, delay: 0); |
775 | return; |
776 | default: |
777 | break; |
778 | } |
779 | |
780 | out: |
781 | kfree_skb(skb); |
782 | } |
783 | |
784 | static int bcm_ptp_hwtstamp(struct mii_timestamper *mii_ts, |
785 | struct kernel_hwtstamp_config *cfg, |
786 | struct netlink_ext_ack *extack) |
787 | { |
788 | struct bcm_ptp_private *priv = mii2priv(mii_ts); |
789 | u16 mode, ctrl; |
790 | |
791 | switch (cfg->rx_filter) { |
792 | case HWTSTAMP_FILTER_NONE: |
793 | priv->hwts_rx = false; |
794 | break; |
795 | case HWTSTAMP_FILTER_PTP_V2_L4_EVENT: |
796 | case HWTSTAMP_FILTER_PTP_V2_L4_SYNC: |
797 | case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ: |
798 | case HWTSTAMP_FILTER_PTP_V2_L2_EVENT: |
799 | case HWTSTAMP_FILTER_PTP_V2_L2_SYNC: |
800 | case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ: |
801 | case HWTSTAMP_FILTER_PTP_V2_EVENT: |
802 | case HWTSTAMP_FILTER_PTP_V2_SYNC: |
803 | case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ: |
804 | cfg->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT; |
805 | priv->hwts_rx = true; |
806 | break; |
807 | default: |
808 | return -ERANGE; |
809 | } |
810 | |
811 | priv->tx_type = cfg->tx_type; |
812 | |
813 | ctrl = priv->hwts_rx ? SLICE_RX_EN : 0; |
814 | ctrl |= priv->tx_type != HWTSTAMP_TX_OFF ? SLICE_TX_EN : 0; |
815 | |
816 | mode = TX_MODE_SEL(PORT, SYNC, REPLACE_TS) | |
817 | TX_MODE_SEL(PORT, DELAY_REQ, REPLACE_TS) | |
818 | TX_MODE_SEL(PORT, PDELAY_REQ, REPLACE_TS) | |
819 | TX_MODE_SEL(PORT, PDELAY_RESP, REPLACE_TS); |
820 | |
821 | bcm_phy_write_exp(phydev: priv->phydev, TX_EVENT_MODE, val: mode); |
822 | |
823 | mode = RX_MODE_SEL(PORT, SYNC, INSERT_TS_64) | |
824 | RX_MODE_SEL(PORT, DELAY_REQ, INSERT_TS_64) | |
825 | RX_MODE_SEL(PORT, PDELAY_REQ, INSERT_TS_64) | |
826 | RX_MODE_SEL(PORT, PDELAY_RESP, INSERT_TS_64); |
827 | |
828 | bcm_phy_write_exp(phydev: priv->phydev, RX_EVENT_MODE, val: mode); |
829 | |
830 | bcm_phy_write_exp(phydev: priv->phydev, SLICE_CTRL, val: ctrl); |
831 | |
832 | if (ctrl & SLICE_TX_EN) |
833 | bcm_phy_write_exp(phydev: priv->phydev, TX_TS_CAPTURE, TX_TS_CAP_EN); |
834 | else |
835 | ptp_cancel_worker_sync(ptp: priv->ptp_clock); |
836 | |
837 | /* purge existing data */ |
838 | skb_queue_purge(list: &priv->tx_queue); |
839 | |
840 | return 0; |
841 | } |
842 | |
843 | static int bcm_ptp_ts_info(struct mii_timestamper *mii_ts, |
844 | struct ethtool_ts_info *ts_info) |
845 | { |
846 | struct bcm_ptp_private *priv = mii2priv(mii_ts); |
847 | |
848 | ts_info->phc_index = ptp_clock_index(ptp: priv->ptp_clock); |
849 | ts_info->so_timestamping = |
850 | SOF_TIMESTAMPING_TX_HARDWARE | |
851 | SOF_TIMESTAMPING_RX_HARDWARE | |
852 | SOF_TIMESTAMPING_RAW_HARDWARE; |
853 | ts_info->tx_types = |
854 | BIT(HWTSTAMP_TX_ON) | |
855 | BIT(HWTSTAMP_TX_OFF) | |
856 | BIT(HWTSTAMP_TX_ONESTEP_SYNC) | |
857 | BIT(HWTSTAMP_TX_ONESTEP_P2P); |
858 | ts_info->rx_filters = |
859 | BIT(HWTSTAMP_FILTER_NONE) | |
860 | BIT(HWTSTAMP_FILTER_PTP_V2_EVENT); |
861 | |
862 | return 0; |
863 | } |
864 | |
865 | void bcm_ptp_stop(struct bcm_ptp_private *priv) |
866 | { |
867 | ptp_cancel_worker_sync(ptp: priv->ptp_clock); |
868 | bcm_ptp_cancel_func(priv); |
869 | } |
870 | EXPORT_SYMBOL_GPL(bcm_ptp_stop); |
871 | |
872 | void bcm_ptp_config_init(struct phy_device *phydev) |
873 | { |
874 | /* init network sync engine */ |
875 | bcm_phy_write_exp(phydev, NSE_CTRL, NSE_GMODE_EN | NSE_INIT); |
876 | |
877 | /* enable time sync (TX/RX SOP capture) */ |
878 | bcm_phy_write_exp(phydev, TIME_SYNC, TIME_SYNC_EN); |
879 | |
880 | /* use sec.nsec heartbeat capture */ |
881 | bcm_phy_write_exp(phydev, DPLL_SELECT, DPLL_HB_MODE2); |
882 | |
883 | /* use 64 bit timecode for TX */ |
884 | bcm_phy_write_exp(phydev, TIMECODE_CTRL, TX_TIMECODE_SEL); |
885 | |
886 | /* always allow FREQ_LOAD on framesync */ |
887 | bcm_phy_write_exp(phydev, SHADOW_CTRL, FREQ_LOAD); |
888 | |
889 | bcm_phy_write_exp(phydev, SYNC_IN_DIVIDER, val: 1); |
890 | } |
891 | EXPORT_SYMBOL_GPL(bcm_ptp_config_init); |
892 | |
893 | static void bcm_ptp_init(struct bcm_ptp_private *priv) |
894 | { |
895 | priv->nse_ctrl = NSE_GMODE_EN; |
896 | |
897 | mutex_init(&priv->mutex); |
898 | skb_queue_head_init(list: &priv->tx_queue); |
899 | |
900 | priv->mii_ts.rxtstamp = bcm_ptp_rxtstamp; |
901 | priv->mii_ts.txtstamp = bcm_ptp_txtstamp; |
902 | priv->mii_ts.hwtstamp = bcm_ptp_hwtstamp; |
903 | priv->mii_ts.ts_info = bcm_ptp_ts_info; |
904 | |
905 | priv->phydev->mii_ts = &priv->mii_ts; |
906 | } |
907 | |
908 | struct bcm_ptp_private *bcm_ptp_probe(struct phy_device *phydev) |
909 | { |
910 | struct bcm_ptp_private *priv; |
911 | struct ptp_clock *clock; |
912 | |
913 | switch (BRCM_PHY_MODEL(phydev)) { |
914 | case PHY_ID_BCM54210E: |
915 | break; |
916 | default: |
917 | return NULL; |
918 | } |
919 | |
920 | priv = devm_kzalloc(dev: &phydev->mdio.dev, size: sizeof(*priv), GFP_KERNEL); |
921 | if (!priv) |
922 | return ERR_PTR(error: -ENOMEM); |
923 | |
924 | priv->ptp_info = bcm_ptp_clock_info; |
925 | |
926 | snprintf(buf: priv->pin.name, size: sizeof(priv->pin.name), fmt: "SYNC_OUT" ); |
927 | priv->ptp_info.pin_config = &priv->pin; |
928 | |
929 | clock = ptp_clock_register(info: &priv->ptp_info, parent: &phydev->mdio.dev); |
930 | if (IS_ERR(ptr: clock)) |
931 | return ERR_CAST(ptr: clock); |
932 | priv->ptp_clock = clock; |
933 | |
934 | priv->phydev = phydev; |
935 | bcm_ptp_init(priv); |
936 | |
937 | return priv; |
938 | } |
939 | EXPORT_SYMBOL_GPL(bcm_ptp_probe); |
940 | |
941 | MODULE_LICENSE("GPL" ); |
942 | MODULE_DESCRIPTION("Broadcom PHY PTP driver" ); |
943 | |