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 <linux/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
mii2priv(struct mii_timestamper * mii_ts)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
ptp2priv(struct ptp_clock_info * info)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
bcm_ptp_get_framesync_ts(struct phy_device * phydev,struct timespec64 * ts)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, 0);
191
192 ts->tv_sec = (hb[3] << 16) | hb[2];
193 ts->tv_nsec = (hb[1] << 16) | hb[0];
194 }
195
bcm_ptp_framesync_disable(struct phy_device * phydev,u16 orig_ctrl)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, ctrl);
201
202 return ctrl;
203 }
204
bcm_ptp_framesync_restore(struct phy_device * phydev,u16 orig_ctrl)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, orig_ctrl);
209 }
210
bcm_ptp_framesync(struct phy_device * phydev,u16 ctrl)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, ctrl | NSE_CPU_FRAMESYNC);
215 }
216
bcm_ptp_framesync_ts(struct phy_device * phydev,struct ptp_system_timestamp * sts,struct timespec64 * ts,u16 orig_ctrl)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 | 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
bcm_ptp_gettimex(struct ptp_clock_info * info,struct timespec64 * ts,struct ptp_system_timestamp * sts)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(priv->phydev, sts, ts, priv->nse_ctrl);
257 mutex_unlock(&priv->mutex);
258
259 return err;
260 }
261
bcm_ptp_settime_locked(struct bcm_ptp_private * priv,const struct timespec64 * ts)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, priv->nse_ctrl);
270
271 /* set up time code */
272 bcm_phy_write_exp(phydev, TIME_CODE_0, ts->tv_nsec);
273 bcm_phy_write_exp(phydev, TIME_CODE_1, ts->tv_nsec >> 16);
274 bcm_phy_write_exp(phydev, TIME_CODE_2, ts->tv_sec);
275 bcm_phy_write_exp(phydev, TIME_CODE_3, ts->tv_sec >> 16);
276 bcm_phy_write_exp(phydev, TIME_CODE_4, 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, ns >> 4);
281 bcm_phy_write_exp(phydev, NCO_TIME_1, ns >> 20);
282 bcm_phy_write_exp(phydev, NCO_TIME_2_CTRL, (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 | NSE_INIT);
289
290 bcm_ptp_framesync_restore(phydev, priv->nse_ctrl);
291
292 return 0;
293 }
294
bcm_ptp_settime(struct ptp_clock_info * info,const struct timespec64 * ts)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(&priv->mutex);
304
305 return err;
306 }
307
bcm_ptp_adjtime_locked(struct bcm_ptp_private * priv,s64 delta_ns)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(priv->phydev, NULL, &ts, priv->nse_ctrl);
316 if (!err) {
317 ns = timespec64_to_ns(&ts) + delta_ns;
318 ts = ns_to_timespec64(ns);
319 err = bcm_ptp_settime_locked(priv, &ts);
320 }
321 return err;
322 }
323
bcm_ptp_adjtime(struct ptp_clock_info * info,s64 delta_ns)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(&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 */
bcm_ptp_adjfine(struct ptp_clock_info * info,long scaled_ppm)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(adj, 15625);
359 freq = (8 << 28) + (neg_adj ? -diff : diff);
360
361 mutex_lock(&priv->mutex);
362
363 ctrl = bcm_ptp_framesync_disable(priv->phydev, priv->nse_ctrl);
364
365 bcm_phy_write_exp(priv->phydev, NCO_FREQ_LSB, freq);
366 bcm_phy_write_exp(priv->phydev, NCO_FREQ_MSB, freq >> 16);
367
368 bcm_phy_write_exp(priv->phydev, NCO_TIME_2_CTRL, FREQ_MDIO_SEL);
369
370 /* load on next framesync */
371 bcm_phy_write_exp(priv->phydev, SHADOW_LOAD, FREQ_LOAD);
372
373 bcm_ptp_framesync(priv->phydev, ctrl);
374
375 /* clear load */
376 bcm_phy_write_exp(priv->phydev, SHADOW_LOAD, 0);
377
378 bcm_ptp_framesync_restore(priv->phydev, priv->nse_ctrl);
379
380 mutex_unlock(&priv->mutex);
381
382 return 0;
383 }
384
bcm_ptp_rxtstamp(struct mii_timestamper * mii_ts,struct sk_buff * skb,int type)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 *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(data);
404 nsec = get_unaligned_be32(data + 4);
405
406 hwts = skb_hwtstamps(skb);
407 hwts->hwtstamp = ktime_set(sec, 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, skb->len - 8);
413 }
414
415 return false;
416 }
417
bcm_ptp_get_tstamp(struct bcm_ptp_private * priv,struct bcm_ptp_capture * capts)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(&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(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, 0);
449
450 mutex_unlock(&priv->mutex);
451
452 sec = (ts[3] << 16) | ts[2];
453 nsec = (ts[1] << 16) | ts[0];
454 capts->hwtstamp = ktime_set(sec, nsec);
455
456 return true;
457 }
458
bcm_ptp_match_tstamp(struct bcm_ptp_private * priv,struct bcm_ptp_capture * capts)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(&priv->tx_queue, skb);
473 __skb_unlink(skb, &priv->tx_queue);
474 ts_skb = skb;
475 break;
476 }
477 }
478 spin_unlock_irqrestore(&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(ts_skb);
484 } else {
485 memset(&hwts, 0, sizeof(hwts));
486 hwts.hwtstamp = capts->hwtstamp;
487 skb_complete_tx_timestamp(ts_skb, &hwts);
488 }
489 }
490
491 /* not first match, try and expire entries */
492 if (!first) {
493 while ((skb = skb_dequeue(&priv->tx_queue))) {
494 if (!time_after(jiffies, BCM_SKB_CB(skb)->timeout)) {
495 skb_queue_head(&priv->tx_queue, skb);
496 break;
497 }
498 kfree_skb(skb);
499 }
500 }
501 }
502
bcm_ptp_do_aux_work(struct ptp_clock_info * info)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(&priv->tx_queue)) {
510 if (!bcm_ptp_get_tstamp(priv, &capts)) {
511 reschedule = true;
512 break;
513 }
514 bcm_ptp_match_tstamp(priv, &capts);
515 }
516
517 return reschedule ? 1 : -1;
518 }
519
bcm_ptp_cancel_func(struct bcm_ptp_private * priv)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(priv->phydev, NSE_CTRL, priv->nse_ctrl);
530
531 cancel_delayed_work_sync(&priv->pin_work);
532
533 return 0;
534 }
535
bcm_ptp_perout_work(struct work_struct * pin_work)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(&priv->mutex);
550 return;
551 }
552
553 bcm_ptp_framesync_ts(phydev, NULL, &ts, 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);
561
562 /* force 0->1 transition for ONESHOT */
563 ctrl = bcm_ptp_framesync_disable(phydev,
564 priv->nse_ctrl & ~NSE_ONESHOT_EN);
565
566 bcm_phy_write_exp(phydev, SYNOUT_TS_0, ns & 0xfff0);
567 bcm_phy_write_exp(phydev, SYNOUT_TS_1, ns >> 16);
568 bcm_phy_write_exp(phydev, SYNOUT_TS_2, 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 | NSE_ONESHOT_EN | NSE_INIT);
574
575 priv->nse_ctrl |= NSE_ONESHOT_EN;
576 bcm_ptp_framesync_restore(phydev, priv->nse_ctrl);
577
578 mutex_unlock(&priv->mutex);
579
580 next = next + NSEC_PER_MSEC;
581 schedule_delayed_work(&priv->pin_work, nsecs_to_jiffies(next));
582 }
583
bcm_ptp_perout_locked(struct bcm_ptp_private * priv,struct ptp_perout_request * req,int on)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 /* Reject unsupported flags */
601 if (req->flags & ~PTP_PEROUT_DUTY_CYCLE)
602 return -EOPNOTSUPP;
603
604 if (req->flags & PTP_PEROUT_DUTY_CYCLE)
605 pulse = ktime_to_ns(ktime_set(req->on.sec, req->on.nsec));
606 else
607 pulse = (u64)BCM_MAX_PULSE_8NS << 3;
608
609 /* convert to 8ns units */
610 pulse >>= 3;
611
612 if (!pulse || pulse > period || pulse > BCM_MAX_PULSE_8NS)
613 return -EINVAL;
614
615 bcm_phy_write_exp(phydev, SYNC_OUT_0, period);
616
617 val = ((pulse & 0x3) << 14) | ((period >> 16) & 0x3fff);
618 bcm_phy_write_exp(phydev, SYNC_OUT_1, val);
619
620 val = ((pulse >> 2) & 0x7f) | (pulse << 7);
621 bcm_phy_write_exp(phydev, SYNC_OUT_2, val);
622
623 if (priv->pin_active)
624 cancel_delayed_work_sync(&priv->pin_work);
625
626 priv->pin_active = true;
627 INIT_DELAYED_WORK(&priv->pin_work, bcm_ptp_perout_work);
628 schedule_delayed_work(&priv->pin_work, 0);
629
630 return 0;
631 }
632
bcm_ptp_extts_work(struct work_struct * pin_work)633 static void bcm_ptp_extts_work(struct work_struct *pin_work)
634 {
635 struct bcm_ptp_private *priv =
636 container_of(pin_work, struct bcm_ptp_private, pin_work.work);
637 struct phy_device *phydev = priv->phydev;
638 struct ptp_clock_event event;
639 struct timespec64 ts;
640 u16 reg;
641
642 mutex_lock(&priv->mutex);
643
644 /* no longer running */
645 if (!priv->pin_active) {
646 mutex_unlock(&priv->mutex);
647 return;
648 }
649
650 reg = bcm_phy_read_exp(phydev, INTR_STATUS);
651 if ((reg & INTC_FSYNC) == 0)
652 goto out;
653
654 bcm_ptp_get_framesync_ts(phydev, &ts);
655
656 event.index = 0;
657 event.type = PTP_CLOCK_EXTTS;
658 event.timestamp = timespec64_to_ns(&ts);
659 ptp_clock_event(priv->ptp_clock, &event);
660
661 out:
662 mutex_unlock(&priv->mutex);
663 schedule_delayed_work(&priv->pin_work, HZ / 4);
664 }
665
bcm_ptp_extts_locked(struct bcm_ptp_private * priv,int on)666 static int bcm_ptp_extts_locked(struct bcm_ptp_private *priv, int on)
667 {
668 struct phy_device *phydev = priv->phydev;
669
670 if (!on)
671 return bcm_ptp_cancel_func(priv);
672
673 if (priv->pin_active)
674 cancel_delayed_work_sync(&priv->pin_work);
675
676 bcm_ptp_framesync_disable(phydev, priv->nse_ctrl);
677
678 priv->nse_ctrl |= NSE_SYNC1_FRAMESYNC | NSE_CAPTURE_EN;
679
680 bcm_ptp_framesync_restore(phydev, priv->nse_ctrl);
681
682 priv->pin_active = true;
683 INIT_DELAYED_WORK(&priv->pin_work, bcm_ptp_extts_work);
684 schedule_delayed_work(&priv->pin_work, 0);
685
686 return 0;
687 }
688
bcm_ptp_enable(struct ptp_clock_info * info,struct ptp_clock_request * rq,int on)689 static int bcm_ptp_enable(struct ptp_clock_info *info,
690 struct ptp_clock_request *rq, int on)
691 {
692 struct bcm_ptp_private *priv = ptp2priv(info);
693 int err = -EBUSY;
694
695 mutex_lock(&priv->mutex);
696
697 switch (rq->type) {
698 case PTP_CLK_REQ_PEROUT:
699 if (priv->pin.func == PTP_PF_PEROUT)
700 err = bcm_ptp_perout_locked(priv, &rq->perout, on);
701 break;
702 case PTP_CLK_REQ_EXTTS:
703 if (priv->pin.func == PTP_PF_EXTTS)
704 err = bcm_ptp_extts_locked(priv, on);
705 break;
706 default:
707 err = -EOPNOTSUPP;
708 break;
709 }
710
711 mutex_unlock(&priv->mutex);
712
713 return err;
714 }
715
bcm_ptp_verify(struct ptp_clock_info * info,unsigned int pin,enum ptp_pin_function func,unsigned int chan)716 static int bcm_ptp_verify(struct ptp_clock_info *info, unsigned int pin,
717 enum ptp_pin_function func, unsigned int chan)
718 {
719 switch (func) {
720 case PTP_PF_NONE:
721 case PTP_PF_EXTTS:
722 case PTP_PF_PEROUT:
723 break;
724 default:
725 return -EOPNOTSUPP;
726 }
727 return 0;
728 }
729
730 static const struct ptp_clock_info bcm_ptp_clock_info = {
731 .owner = THIS_MODULE,
732 .name = KBUILD_MODNAME,
733 .max_adj = 100000000,
734 .gettimex64 = bcm_ptp_gettimex,
735 .settime64 = bcm_ptp_settime,
736 .adjtime = bcm_ptp_adjtime,
737 .adjfine = bcm_ptp_adjfine,
738 .enable = bcm_ptp_enable,
739 .verify = bcm_ptp_verify,
740 .do_aux_work = bcm_ptp_do_aux_work,
741 .n_pins = 1,
742 .n_per_out = 1,
743 .n_ext_ts = 1,
744 };
745
bcm_ptp_txtstamp(struct mii_timestamper * mii_ts,struct sk_buff * skb,int type)746 static void bcm_ptp_txtstamp(struct mii_timestamper *mii_ts,
747 struct sk_buff *skb, int type)
748 {
749 struct bcm_ptp_private *priv = mii2priv(mii_ts);
750 struct ptp_header *hdr;
751 bool discard = false;
752 int msgtype;
753
754 hdr = ptp_parse_header(skb, type);
755 if (!hdr)
756 goto out;
757 msgtype = ptp_get_msgtype(hdr, type);
758
759 switch (priv->tx_type) {
760 case HWTSTAMP_TX_ONESTEP_P2P:
761 if (msgtype == PTP_MSGTYPE_PDELAY_RESP)
762 discard = true;
763 fallthrough;
764 case HWTSTAMP_TX_ONESTEP_SYNC:
765 if (msgtype == PTP_MSGTYPE_SYNC)
766 discard = true;
767 fallthrough;
768 case HWTSTAMP_TX_ON:
769 BCM_SKB_CB(skb)->timeout = jiffies + SKB_TS_TIMEOUT;
770 BCM_SKB_CB(skb)->seq_id = be16_to_cpu(hdr->sequence_id);
771 BCM_SKB_CB(skb)->msgtype = msgtype;
772 BCM_SKB_CB(skb)->discard = discard;
773 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
774 skb_queue_tail(&priv->tx_queue, skb);
775 ptp_schedule_worker(priv->ptp_clock, 0);
776 return;
777 default:
778 break;
779 }
780
781 out:
782 kfree_skb(skb);
783 }
784
bcm_ptp_hwtstamp(struct mii_timestamper * mii_ts,struct kernel_hwtstamp_config * cfg,struct netlink_ext_ack * extack)785 static int bcm_ptp_hwtstamp(struct mii_timestamper *mii_ts,
786 struct kernel_hwtstamp_config *cfg,
787 struct netlink_ext_ack *extack)
788 {
789 struct bcm_ptp_private *priv = mii2priv(mii_ts);
790 u16 mode, ctrl;
791
792 switch (cfg->rx_filter) {
793 case HWTSTAMP_FILTER_NONE:
794 priv->hwts_rx = false;
795 break;
796 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
797 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
798 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
799 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
800 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
801 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
802 case HWTSTAMP_FILTER_PTP_V2_EVENT:
803 case HWTSTAMP_FILTER_PTP_V2_SYNC:
804 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
805 cfg->rx_filter = HWTSTAMP_FILTER_PTP_V2_EVENT;
806 priv->hwts_rx = true;
807 break;
808 default:
809 return -ERANGE;
810 }
811
812 priv->tx_type = cfg->tx_type;
813
814 ctrl = priv->hwts_rx ? SLICE_RX_EN : 0;
815 ctrl |= priv->tx_type != HWTSTAMP_TX_OFF ? SLICE_TX_EN : 0;
816
817 mode = TX_MODE_SEL(PORT, SYNC, REPLACE_TS) |
818 TX_MODE_SEL(PORT, DELAY_REQ, REPLACE_TS) |
819 TX_MODE_SEL(PORT, PDELAY_REQ, REPLACE_TS) |
820 TX_MODE_SEL(PORT, PDELAY_RESP, REPLACE_TS);
821
822 bcm_phy_write_exp(priv->phydev, TX_EVENT_MODE, mode);
823
824 mode = RX_MODE_SEL(PORT, SYNC, INSERT_TS_64) |
825 RX_MODE_SEL(PORT, DELAY_REQ, INSERT_TS_64) |
826 RX_MODE_SEL(PORT, PDELAY_REQ, INSERT_TS_64) |
827 RX_MODE_SEL(PORT, PDELAY_RESP, INSERT_TS_64);
828
829 bcm_phy_write_exp(priv->phydev, RX_EVENT_MODE, mode);
830
831 bcm_phy_write_exp(priv->phydev, SLICE_CTRL, ctrl);
832
833 if (ctrl & SLICE_TX_EN)
834 bcm_phy_write_exp(priv->phydev, TX_TS_CAPTURE, TX_TS_CAP_EN);
835 else
836 ptp_cancel_worker_sync(priv->ptp_clock);
837
838 /* purge existing data */
839 skb_queue_purge(&priv->tx_queue);
840
841 return 0;
842 }
843
bcm_ptp_ts_info(struct mii_timestamper * mii_ts,struct kernel_ethtool_ts_info * ts_info)844 static int bcm_ptp_ts_info(struct mii_timestamper *mii_ts,
845 struct kernel_ethtool_ts_info *ts_info)
846 {
847 struct bcm_ptp_private *priv = mii2priv(mii_ts);
848
849 ts_info->phc_index = ptp_clock_index(priv->ptp_clock);
850 ts_info->so_timestamping =
851 SOF_TIMESTAMPING_TX_HARDWARE |
852 SOF_TIMESTAMPING_RX_HARDWARE |
853 SOF_TIMESTAMPING_RAW_HARDWARE;
854 ts_info->tx_types =
855 BIT(HWTSTAMP_TX_ON) |
856 BIT(HWTSTAMP_TX_OFF) |
857 BIT(HWTSTAMP_TX_ONESTEP_SYNC) |
858 BIT(HWTSTAMP_TX_ONESTEP_P2P);
859 ts_info->rx_filters =
860 BIT(HWTSTAMP_FILTER_NONE) |
861 BIT(HWTSTAMP_FILTER_PTP_V2_EVENT);
862
863 return 0;
864 }
865
bcm_ptp_stop(struct bcm_ptp_private * priv)866 void bcm_ptp_stop(struct bcm_ptp_private *priv)
867 {
868 ptp_cancel_worker_sync(priv->ptp_clock);
869 bcm_ptp_cancel_func(priv);
870 }
871 EXPORT_SYMBOL_GPL(bcm_ptp_stop);
872
bcm_ptp_config_init(struct phy_device * phydev)873 void bcm_ptp_config_init(struct phy_device *phydev)
874 {
875 /* init network sync engine */
876 bcm_phy_write_exp(phydev, NSE_CTRL, NSE_GMODE_EN | NSE_INIT);
877
878 /* enable time sync (TX/RX SOP capture) */
879 bcm_phy_write_exp(phydev, TIME_SYNC, TIME_SYNC_EN);
880
881 /* use sec.nsec heartbeat capture */
882 bcm_phy_write_exp(phydev, DPLL_SELECT, DPLL_HB_MODE2);
883
884 /* use 64 bit timecode for TX */
885 bcm_phy_write_exp(phydev, TIMECODE_CTRL, TX_TIMECODE_SEL);
886
887 /* always allow FREQ_LOAD on framesync */
888 bcm_phy_write_exp(phydev, SHADOW_CTRL, FREQ_LOAD);
889
890 bcm_phy_write_exp(phydev, SYNC_IN_DIVIDER, 1);
891 }
892 EXPORT_SYMBOL_GPL(bcm_ptp_config_init);
893
bcm_ptp_init(struct bcm_ptp_private * priv)894 static void bcm_ptp_init(struct bcm_ptp_private *priv)
895 {
896 priv->nse_ctrl = NSE_GMODE_EN;
897
898 mutex_init(&priv->mutex);
899 skb_queue_head_init(&priv->tx_queue);
900
901 priv->mii_ts.rxtstamp = bcm_ptp_rxtstamp;
902 priv->mii_ts.txtstamp = bcm_ptp_txtstamp;
903 priv->mii_ts.hwtstamp = bcm_ptp_hwtstamp;
904 priv->mii_ts.ts_info = bcm_ptp_ts_info;
905
906 priv->phydev->mii_ts = &priv->mii_ts;
907 }
908
bcm_ptp_probe(struct phy_device * phydev)909 struct bcm_ptp_private *bcm_ptp_probe(struct phy_device *phydev)
910 {
911 struct bcm_ptp_private *priv;
912 struct ptp_clock *clock;
913
914 switch (BRCM_PHY_MODEL(phydev)) {
915 case PHY_ID_BCM54210E:
916 break;
917 default:
918 return NULL;
919 }
920
921 priv = devm_kzalloc(&phydev->mdio.dev, sizeof(*priv), GFP_KERNEL);
922 if (!priv)
923 return ERR_PTR(-ENOMEM);
924
925 priv->ptp_info = bcm_ptp_clock_info;
926
927 snprintf(priv->pin.name, sizeof(priv->pin.name), "SYNC_OUT");
928 priv->ptp_info.pin_config = &priv->pin;
929
930 clock = ptp_clock_register(&priv->ptp_info, &phydev->mdio.dev);
931 if (IS_ERR(clock))
932 return ERR_CAST(clock);
933 priv->ptp_clock = clock;
934
935 /* Timestamp selected by default to keep legacy API */
936 phydev->default_timestamp = true;
937
938 priv->phydev = phydev;
939 bcm_ptp_init(priv);
940
941 return priv;
942 }
943 EXPORT_SYMBOL_GPL(bcm_ptp_probe);
944
945 MODULE_LICENSE("GPL");
946 MODULE_DESCRIPTION("Broadcom PHY PTP driver");
947