Loading...
1// SPDX-License-Identifier: GPL-2.0
2/* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com>
3 */
4#include <linux/spi/spi.h>
5#include "sja1105.h"
6
7/* The adjfine API clamps ppb between [-32,768,000, 32,768,000], and
8 * therefore scaled_ppm between [-2,147,483,648, 2,147,483,647].
9 * Set the maximum supported ppb to a round value smaller than the maximum.
10 *
11 * Percentually speaking, this is a +/- 0.032x adjustment of the
12 * free-running counter (0.968x to 1.032x).
13 */
14#define SJA1105_MAX_ADJ_PPB 32000000
15#define SJA1105_SIZE_PTP_CMD 4
16
17/* PTPSYNCTS has no interrupt or update mechanism, because the intended
18 * hardware use case is for the timestamp to be collected synchronously,
19 * immediately after the CAS_MASTER SJA1105 switch has performed a CASSYNC
20 * one-shot toggle (no return to level) on the PTP_CLK pin. When used as a
21 * generic extts source, the PTPSYNCTS register needs polling and a comparison
22 * with the old value. The polling interval is configured as the Nyquist rate
23 * of a signal with 50% duty cycle and 1Hz frequency, which is sadly all that
24 * this hardware can do (but may be enough for some setups). Anything of higher
25 * frequency than 1 Hz will be lost, since there is no timestamp FIFO.
26 */
27#define SJA1105_EXTTS_INTERVAL (HZ / 6)
28
29/* This range is actually +/- SJA1105_MAX_ADJ_PPB
30 * divided by 1000 (ppb -> ppm) and with a 16-bit
31 * "fractional" part (actually fixed point).
32 * |
33 * v
34 * Convert scaled_ppm from the +/- ((10^6) << 16) range
35 * into the +/- (1 << 31) range.
36 *
37 * This forgoes a "ppb" numeric representation (up to NSEC_PER_SEC)
38 * and defines the scaling factor between scaled_ppm and the actual
39 * frequency adjustments of the PHC.
40 *
41 * ptpclkrate = scaled_ppm * 2^31 / (10^6 * 2^16)
42 * simplifies to
43 * ptpclkrate = scaled_ppm * 2^9 / 5^6
44 */
45#define SJA1105_CC_MULT_NUM (1 << 9)
46#define SJA1105_CC_MULT_DEM 15625
47#define SJA1105_CC_MULT 0x80000000
48
49enum sja1105_ptp_clk_mode {
50 PTP_ADD_MODE = 1,
51 PTP_SET_MODE = 0,
52};
53
54#define extts_to_data(t) \
55 container_of((t), struct sja1105_ptp_data, extts_timer)
56#define ptp_caps_to_data(d) \
57 container_of((d), struct sja1105_ptp_data, caps)
58#define ptp_data_to_sja1105(d) \
59 container_of((d), struct sja1105_private, ptp_data)
60
61int sja1105_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr)
62{
63 struct sja1105_private *priv = ds->priv;
64 struct hwtstamp_config config;
65
66 if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
67 return -EFAULT;
68
69 switch (config.tx_type) {
70 case HWTSTAMP_TX_OFF:
71 priv->hwts_tx_en &= ~BIT(port);
72 break;
73 case HWTSTAMP_TX_ON:
74 priv->hwts_tx_en |= BIT(port);
75 break;
76 default:
77 return -ERANGE;
78 }
79
80 switch (config.rx_filter) {
81 case HWTSTAMP_FILTER_NONE:
82 priv->hwts_rx_en &= ~BIT(port);
83 break;
84 default:
85 priv->hwts_rx_en |= BIT(port);
86 break;
87 }
88
89 if (copy_to_user(ifr->ifr_data, &config, sizeof(config)))
90 return -EFAULT;
91 return 0;
92}
93
94int sja1105_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr)
95{
96 struct sja1105_private *priv = ds->priv;
97 struct hwtstamp_config config;
98
99 config.flags = 0;
100 if (priv->hwts_tx_en & BIT(port))
101 config.tx_type = HWTSTAMP_TX_ON;
102 else
103 config.tx_type = HWTSTAMP_TX_OFF;
104 if (priv->hwts_rx_en & BIT(port))
105 config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
106 else
107 config.rx_filter = HWTSTAMP_FILTER_NONE;
108
109 return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
110 -EFAULT : 0;
111}
112
113int sja1105_get_ts_info(struct dsa_switch *ds, int port,
114 struct kernel_ethtool_ts_info *info)
115{
116 struct sja1105_private *priv = ds->priv;
117 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
118
119 /* Called during cleanup */
120 if (!ptp_data->clock)
121 return -ENODEV;
122
123 info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
124 SOF_TIMESTAMPING_RX_HARDWARE |
125 SOF_TIMESTAMPING_RAW_HARDWARE;
126 info->tx_types = (1 << HWTSTAMP_TX_OFF) |
127 (1 << HWTSTAMP_TX_ON);
128 info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
129 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT);
130 info->phc_index = ptp_clock_index(ptp_data->clock);
131 return 0;
132}
133
134void sja1105et_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
135 enum packing_op op)
136{
137 const int size = SJA1105_SIZE_PTP_CMD;
138 /* No need to keep this as part of the structure */
139 u64 valid = 1;
140
141 sja1105_packing(buf, &valid, 31, 31, size, op);
142 sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
143 sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
144 sja1105_packing(buf, &cmd->startptpcp, 28, 28, size, op);
145 sja1105_packing(buf, &cmd->stopptpcp, 27, 27, size, op);
146 sja1105_packing(buf, &cmd->resptp, 2, 2, size, op);
147 sja1105_packing(buf, &cmd->corrclk4ts, 1, 1, size, op);
148 sja1105_packing(buf, &cmd->ptpclkadd, 0, 0, size, op);
149}
150
151void sja1105pqrs_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
152 enum packing_op op)
153{
154 const int size = SJA1105_SIZE_PTP_CMD;
155 /* No need to keep this as part of the structure */
156 u64 valid = 1;
157
158 sja1105_packing(buf, &valid, 31, 31, size, op);
159 sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
160 sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
161 sja1105_packing(buf, &cmd->startptpcp, 28, 28, size, op);
162 sja1105_packing(buf, &cmd->stopptpcp, 27, 27, size, op);
163 sja1105_packing(buf, &cmd->resptp, 3, 3, size, op);
164 sja1105_packing(buf, &cmd->corrclk4ts, 2, 2, size, op);
165 sja1105_packing(buf, &cmd->ptpclkadd, 0, 0, size, op);
166}
167
168int sja1105_ptp_commit(struct dsa_switch *ds, struct sja1105_ptp_cmd *cmd,
169 sja1105_spi_rw_mode_t rw)
170{
171 const struct sja1105_private *priv = ds->priv;
172 const struct sja1105_regs *regs = priv->info->regs;
173 u8 buf[SJA1105_SIZE_PTP_CMD] = {0};
174 int rc;
175
176 if (rw == SPI_WRITE)
177 priv->info->ptp_cmd_packing(buf, cmd, PACK);
178
179 rc = sja1105_xfer_buf(priv, rw, regs->ptp_control, buf,
180 SJA1105_SIZE_PTP_CMD);
181
182 if (rw == SPI_READ)
183 priv->info->ptp_cmd_packing(buf, cmd, UNPACK);
184
185 return rc;
186}
187
188/* The switch returns partial timestamps (24 bits for SJA1105 E/T, which wrap
189 * around in 0.135 seconds, and 32 bits for P/Q/R/S, wrapping around in 34.35
190 * seconds).
191 *
192 * This receives the RX or TX MAC timestamps, provided by hardware as
193 * the lower bits of the cycle counter, sampled at the time the timestamp was
194 * collected.
195 *
196 * To reconstruct into a full 64-bit-wide timestamp, the cycle counter is
197 * read and the high-order bits are filled in.
198 *
199 * Must be called within one wraparound period of the partial timestamp since
200 * it was generated by the MAC.
201 */
202static u64 sja1105_tstamp_reconstruct(struct dsa_switch *ds, u64 now,
203 u64 ts_partial)
204{
205 struct sja1105_private *priv = ds->priv;
206 u64 partial_tstamp_mask = CYCLECOUNTER_MASK(priv->info->ptp_ts_bits);
207 u64 ts_reconstructed;
208
209 ts_reconstructed = (now & ~partial_tstamp_mask) | ts_partial;
210
211 /* Check lower bits of current cycle counter against the timestamp.
212 * If the current cycle counter is lower than the partial timestamp,
213 * then wraparound surely occurred and must be accounted for.
214 */
215 if ((now & partial_tstamp_mask) <= ts_partial)
216 ts_reconstructed -= (partial_tstamp_mask + 1);
217
218 return ts_reconstructed;
219}
220
221/* Reads the SPI interface for an egress timestamp generated by the switch
222 * for frames sent using management routes.
223 *
224 * SJA1105 E/T layout of the 4-byte SPI payload:
225 *
226 * 31 23 15 7 0
227 * | | | | |
228 * +-----+-----+-----+ ^
229 * ^ |
230 * | |
231 * 24-bit timestamp Update bit
232 *
233 *
234 * SJA1105 P/Q/R/S layout of the 8-byte SPI payload:
235 *
236 * 31 23 15 7 0 63 55 47 39 32
237 * | | | | | | | | | |
238 * ^ +-----+-----+-----+-----+
239 * | ^
240 * | |
241 * Update bit 32-bit timestamp
242 *
243 * Notice that the update bit is in the same place.
244 * To have common code for E/T and P/Q/R/S for reading the timestamp,
245 * we need to juggle with the offset and the bit indices.
246 */
247static int sja1105_ptpegr_ts_poll(struct dsa_switch *ds, int port, u64 *ts)
248{
249 struct sja1105_private *priv = ds->priv;
250 const struct sja1105_regs *regs = priv->info->regs;
251 int tstamp_bit_start, tstamp_bit_end;
252 int timeout = 10;
253 u8 packed_buf[8];
254 u64 update;
255 int rc;
256
257 do {
258 rc = sja1105_xfer_buf(priv, SPI_READ, regs->ptpegr_ts[port],
259 packed_buf, priv->info->ptpegr_ts_bytes);
260 if (rc < 0)
261 return rc;
262
263 sja1105_unpack(packed_buf, &update, 0, 0,
264 priv->info->ptpegr_ts_bytes);
265 if (update)
266 break;
267
268 usleep_range(10, 50);
269 } while (--timeout);
270
271 if (!timeout)
272 return -ETIMEDOUT;
273
274 /* Point the end bit to the second 32-bit word on P/Q/R/S,
275 * no-op on E/T.
276 */
277 tstamp_bit_end = (priv->info->ptpegr_ts_bytes - 4) * 8;
278 /* Shift the 24-bit timestamp on E/T to be collected from 31:8.
279 * No-op on P/Q/R/S.
280 */
281 tstamp_bit_end += 32 - priv->info->ptp_ts_bits;
282 tstamp_bit_start = tstamp_bit_end + priv->info->ptp_ts_bits - 1;
283
284 *ts = 0;
285
286 sja1105_unpack(packed_buf, ts, tstamp_bit_start, tstamp_bit_end,
287 priv->info->ptpegr_ts_bytes);
288
289 return 0;
290}
291
292/* Caller must hold ptp_data->lock */
293static int sja1105_ptpclkval_read(struct sja1105_private *priv, u64 *ticks,
294 struct ptp_system_timestamp *ptp_sts)
295{
296 const struct sja1105_regs *regs = priv->info->regs;
297
298 return sja1105_xfer_u64(priv, SPI_READ, regs->ptpclkval, ticks,
299 ptp_sts);
300}
301
302/* Caller must hold ptp_data->lock */
303static int sja1105_ptpclkval_write(struct sja1105_private *priv, u64 ticks,
304 struct ptp_system_timestamp *ptp_sts)
305{
306 const struct sja1105_regs *regs = priv->info->regs;
307
308 return sja1105_xfer_u64(priv, SPI_WRITE, regs->ptpclkval, &ticks,
309 ptp_sts);
310}
311
312static void sja1105_extts_poll(struct sja1105_private *priv)
313{
314 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
315 const struct sja1105_regs *regs = priv->info->regs;
316 struct ptp_clock_event event;
317 u64 ptpsyncts = 0;
318 int rc;
319
320 rc = sja1105_xfer_u64(priv, SPI_READ, regs->ptpsyncts, &ptpsyncts,
321 NULL);
322 if (rc < 0)
323 dev_err_ratelimited(priv->ds->dev,
324 "Failed to read PTPSYNCTS: %d\n", rc);
325
326 if (ptpsyncts && ptp_data->ptpsyncts != ptpsyncts) {
327 event.index = 0;
328 event.type = PTP_CLOCK_EXTTS;
329 event.timestamp = ns_to_ktime(sja1105_ticks_to_ns(ptpsyncts));
330 ptp_clock_event(ptp_data->clock, &event);
331
332 ptp_data->ptpsyncts = ptpsyncts;
333 }
334}
335
336static long sja1105_rxtstamp_work(struct ptp_clock_info *ptp)
337{
338 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
339 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
340 struct dsa_switch *ds = priv->ds;
341 struct sk_buff *skb;
342
343 mutex_lock(&ptp_data->lock);
344
345 while ((skb = skb_dequeue(&ptp_data->skb_rxtstamp_queue)) != NULL) {
346 struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);
347 u64 ticks, ts;
348 int rc;
349
350 rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
351 if (rc < 0) {
352 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
353 kfree_skb(skb);
354 continue;
355 }
356
357 *shwt = (struct skb_shared_hwtstamps) {0};
358
359 ts = SJA1105_SKB_CB(skb)->tstamp;
360 ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
361
362 shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
363 netif_rx(skb);
364 }
365
366 if (ptp_data->extts_enabled)
367 sja1105_extts_poll(priv);
368
369 mutex_unlock(&ptp_data->lock);
370
371 /* Don't restart */
372 return -1;
373}
374
375bool sja1105_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
376{
377 struct sja1105_private *priv = ds->priv;
378 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
379
380 if (!(priv->hwts_rx_en & BIT(port)))
381 return false;
382
383 /* We need to read the full PTP clock to reconstruct the Rx
384 * timestamp. For that we need a sleepable context.
385 */
386 skb_queue_tail(&ptp_data->skb_rxtstamp_queue, skb);
387 ptp_schedule_worker(ptp_data->clock, 0);
388 return true;
389}
390
391bool sja1110_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
392{
393 struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);
394 u64 ts = SJA1105_SKB_CB(skb)->tstamp;
395
396 *shwt = (struct skb_shared_hwtstamps) {0};
397
398 shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
399
400 /* Don't defer */
401 return false;
402}
403
404/* Called from dsa_skb_defer_rx_timestamp */
405bool sja1105_port_rxtstamp(struct dsa_switch *ds, int port,
406 struct sk_buff *skb, unsigned int type)
407{
408 struct sja1105_private *priv = ds->priv;
409
410 return priv->info->rxtstamp(ds, port, skb);
411}
412
413void sja1110_process_meta_tstamp(struct dsa_switch *ds, int port, u8 ts_id,
414 enum sja1110_meta_tstamp dir, u64 tstamp)
415{
416 struct sja1105_private *priv = ds->priv;
417 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
418 struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
419 struct skb_shared_hwtstamps shwt = {0};
420
421 /* We don't care about RX timestamps on the CPU port */
422 if (dir == SJA1110_META_TSTAMP_RX)
423 return;
424
425 spin_lock(&ptp_data->skb_txtstamp_queue.lock);
426
427 skb_queue_walk_safe(&ptp_data->skb_txtstamp_queue, skb, skb_tmp) {
428 if (SJA1105_SKB_CB(skb)->ts_id != ts_id)
429 continue;
430
431 __skb_unlink(skb, &ptp_data->skb_txtstamp_queue);
432 skb_match = skb;
433
434 break;
435 }
436
437 spin_unlock(&ptp_data->skb_txtstamp_queue.lock);
438
439 if (WARN_ON(!skb_match))
440 return;
441
442 shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(tstamp));
443 skb_complete_tx_timestamp(skb_match, &shwt);
444}
445
446/* In addition to cloning the skb which is done by the common
447 * sja1105_port_txtstamp, we need to generate a timestamp ID and save the
448 * packet to the TX timestamping queue.
449 */
450void sja1110_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
451{
452 struct sk_buff *clone = SJA1105_SKB_CB(skb)->clone;
453 struct sja1105_private *priv = ds->priv;
454 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
455 u8 ts_id;
456
457 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
458
459 spin_lock(&priv->ts_id_lock);
460
461 ts_id = priv->ts_id;
462 /* Deal automatically with 8-bit wraparound */
463 priv->ts_id++;
464
465 SJA1105_SKB_CB(clone)->ts_id = ts_id;
466
467 spin_unlock(&priv->ts_id_lock);
468
469 skb_queue_tail(&ptp_data->skb_txtstamp_queue, clone);
470}
471
472/* Called from dsa_skb_tx_timestamp. This callback is just to clone
473 * the skb and have it available in SJA1105_SKB_CB in the .port_deferred_xmit
474 * callback, where we will timestamp it synchronously.
475 */
476void sja1105_port_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
477{
478 struct sja1105_private *priv = ds->priv;
479 struct sk_buff *clone;
480
481 if (!(priv->hwts_tx_en & BIT(port)))
482 return;
483
484 clone = skb_clone_sk(skb);
485 if (!clone)
486 return;
487
488 SJA1105_SKB_CB(skb)->clone = clone;
489
490 if (priv->info->txtstamp)
491 priv->info->txtstamp(ds, port, skb);
492}
493
494static int sja1105_ptp_reset(struct dsa_switch *ds)
495{
496 struct sja1105_private *priv = ds->priv;
497 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
498 struct sja1105_ptp_cmd cmd = ptp_data->cmd;
499 int rc;
500
501 mutex_lock(&ptp_data->lock);
502
503 cmd.resptp = 1;
504
505 dev_dbg(ds->dev, "Resetting PTP clock\n");
506 rc = sja1105_ptp_commit(ds, &cmd, SPI_WRITE);
507
508 sja1105_tas_clockstep(priv->ds);
509
510 mutex_unlock(&ptp_data->lock);
511
512 return rc;
513}
514
515/* Caller must hold ptp_data->lock */
516int __sja1105_ptp_gettimex(struct dsa_switch *ds, u64 *ns,
517 struct ptp_system_timestamp *ptp_sts)
518{
519 struct sja1105_private *priv = ds->priv;
520 u64 ticks;
521 int rc;
522
523 rc = sja1105_ptpclkval_read(priv, &ticks, ptp_sts);
524 if (rc < 0) {
525 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
526 return rc;
527 }
528
529 *ns = sja1105_ticks_to_ns(ticks);
530
531 return 0;
532}
533
534static int sja1105_ptp_gettimex(struct ptp_clock_info *ptp,
535 struct timespec64 *ts,
536 struct ptp_system_timestamp *ptp_sts)
537{
538 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
539 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
540 u64 now = 0;
541 int rc;
542
543 mutex_lock(&ptp_data->lock);
544
545 rc = __sja1105_ptp_gettimex(priv->ds, &now, ptp_sts);
546 *ts = ns_to_timespec64(now);
547
548 mutex_unlock(&ptp_data->lock);
549
550 return rc;
551}
552
553/* Caller must hold ptp_data->lock */
554static int sja1105_ptp_mode_set(struct sja1105_private *priv,
555 enum sja1105_ptp_clk_mode mode)
556{
557 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
558
559 if (ptp_data->cmd.ptpclkadd == mode)
560 return 0;
561
562 ptp_data->cmd.ptpclkadd = mode;
563
564 return sja1105_ptp_commit(priv->ds, &ptp_data->cmd, SPI_WRITE);
565}
566
567/* Write to PTPCLKVAL while PTPCLKADD is 0 */
568int __sja1105_ptp_settime(struct dsa_switch *ds, u64 ns,
569 struct ptp_system_timestamp *ptp_sts)
570{
571 struct sja1105_private *priv = ds->priv;
572 u64 ticks = ns_to_sja1105_ticks(ns);
573 int rc;
574
575 rc = sja1105_ptp_mode_set(priv, PTP_SET_MODE);
576 if (rc < 0) {
577 dev_err(priv->ds->dev, "Failed to put PTPCLK in set mode\n");
578 return rc;
579 }
580
581 rc = sja1105_ptpclkval_write(priv, ticks, ptp_sts);
582
583 sja1105_tas_clockstep(priv->ds);
584
585 return rc;
586}
587
588static int sja1105_ptp_settime(struct ptp_clock_info *ptp,
589 const struct timespec64 *ts)
590{
591 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
592 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
593 u64 ns = timespec64_to_ns(ts);
594 int rc;
595
596 mutex_lock(&ptp_data->lock);
597
598 rc = __sja1105_ptp_settime(priv->ds, ns, NULL);
599
600 mutex_unlock(&ptp_data->lock);
601
602 return rc;
603}
604
605static int sja1105_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
606{
607 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
608 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
609 const struct sja1105_regs *regs = priv->info->regs;
610 u32 clkrate32;
611 s64 clkrate;
612 int rc;
613
614 clkrate = (s64)scaled_ppm * SJA1105_CC_MULT_NUM;
615 clkrate = div_s64(clkrate, SJA1105_CC_MULT_DEM);
616
617 /* Take a +/- value and re-center it around 2^31. */
618 clkrate = SJA1105_CC_MULT + clkrate;
619 WARN_ON(abs(clkrate) >= GENMASK_ULL(31, 0));
620 clkrate32 = clkrate;
621
622 mutex_lock(&ptp_data->lock);
623
624 rc = sja1105_xfer_u32(priv, SPI_WRITE, regs->ptpclkrate, &clkrate32,
625 NULL);
626
627 sja1105_tas_adjfreq(priv->ds);
628
629 mutex_unlock(&ptp_data->lock);
630
631 return rc;
632}
633
634/* Write to PTPCLKVAL while PTPCLKADD is 1 */
635int __sja1105_ptp_adjtime(struct dsa_switch *ds, s64 delta)
636{
637 struct sja1105_private *priv = ds->priv;
638 s64 ticks = ns_to_sja1105_ticks(delta);
639 int rc;
640
641 rc = sja1105_ptp_mode_set(priv, PTP_ADD_MODE);
642 if (rc < 0) {
643 dev_err(priv->ds->dev, "Failed to put PTPCLK in add mode\n");
644 return rc;
645 }
646
647 rc = sja1105_ptpclkval_write(priv, ticks, NULL);
648
649 sja1105_tas_clockstep(priv->ds);
650
651 return rc;
652}
653
654static int sja1105_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
655{
656 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
657 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
658 int rc;
659
660 mutex_lock(&ptp_data->lock);
661
662 rc = __sja1105_ptp_adjtime(priv->ds, delta);
663
664 mutex_unlock(&ptp_data->lock);
665
666 return rc;
667}
668
669static void sja1105_ptp_extts_setup_timer(struct sja1105_ptp_data *ptp_data)
670{
671 unsigned long expires = ((jiffies / SJA1105_EXTTS_INTERVAL) + 1) *
672 SJA1105_EXTTS_INTERVAL;
673
674 mod_timer(&ptp_data->extts_timer, expires);
675}
676
677static void sja1105_ptp_extts_timer(struct timer_list *t)
678{
679 struct sja1105_ptp_data *ptp_data = extts_to_data(t);
680
681 ptp_schedule_worker(ptp_data->clock, 0);
682
683 sja1105_ptp_extts_setup_timer(ptp_data);
684}
685
686static int sja1105_change_ptp_clk_pin_func(struct sja1105_private *priv,
687 enum ptp_pin_function func)
688{
689 struct sja1105_avb_params_entry *avb;
690 enum ptp_pin_function old_func;
691
692 avb = priv->static_config.tables[BLK_IDX_AVB_PARAMS].entries;
693
694 if (priv->info->device_id == SJA1105E_DEVICE_ID ||
695 priv->info->device_id == SJA1105T_DEVICE_ID ||
696 avb->cas_master)
697 old_func = PTP_PF_PEROUT;
698 else
699 old_func = PTP_PF_EXTTS;
700
701 if (func == old_func)
702 return 0;
703
704 avb->cas_master = (func == PTP_PF_PEROUT);
705
706 return sja1105_dynamic_config_write(priv, BLK_IDX_AVB_PARAMS, 0, avb,
707 true);
708}
709
710/* The PTP_CLK pin may be configured to toggle with a 50% duty cycle and a
711 * frequency f:
712 *
713 * NSEC_PER_SEC
714 * f = ----------------------
715 * (PTPPINDUR * 8 ns) * 2
716 */
717static int sja1105_per_out_enable(struct sja1105_private *priv,
718 struct ptp_perout_request *perout,
719 bool on)
720{
721 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
722 const struct sja1105_regs *regs = priv->info->regs;
723 struct sja1105_ptp_cmd cmd = ptp_data->cmd;
724 int rc;
725
726 /* We only support one channel */
727 if (perout->index != 0)
728 return -EOPNOTSUPP;
729
730 /* Reject requests with unsupported flags */
731 if (perout->flags)
732 return -EOPNOTSUPP;
733
734 mutex_lock(&ptp_data->lock);
735
736 rc = sja1105_change_ptp_clk_pin_func(priv, PTP_PF_PEROUT);
737 if (rc)
738 goto out;
739
740 if (on) {
741 struct timespec64 pin_duration_ts = {
742 .tv_sec = perout->period.sec,
743 .tv_nsec = perout->period.nsec,
744 };
745 struct timespec64 pin_start_ts = {
746 .tv_sec = perout->start.sec,
747 .tv_nsec = perout->start.nsec,
748 };
749 u64 pin_duration = timespec64_to_ns(&pin_duration_ts);
750 u64 pin_start = timespec64_to_ns(&pin_start_ts);
751 u32 pin_duration32;
752 u64 now;
753
754 /* ptppindur: 32 bit register which holds the interval between
755 * 2 edges on PTP_CLK. So check for truncation which happens
756 * at periods larger than around 68.7 seconds.
757 */
758 pin_duration = ns_to_sja1105_ticks(pin_duration / 2);
759 if (pin_duration > U32_MAX) {
760 rc = -ERANGE;
761 goto out;
762 }
763 pin_duration32 = pin_duration;
764
765 /* ptppins: 64 bit register which needs to hold a PTP time
766 * larger than the current time, otherwise the startptpcp
767 * command won't do anything. So advance the current time
768 * by a number of periods in a way that won't alter the
769 * phase offset.
770 */
771 rc = __sja1105_ptp_gettimex(priv->ds, &now, NULL);
772 if (rc < 0)
773 goto out;
774
775 pin_start = future_base_time(pin_start, pin_duration,
776 now + 1ull * NSEC_PER_SEC);
777 pin_start = ns_to_sja1105_ticks(pin_start);
778
779 rc = sja1105_xfer_u64(priv, SPI_WRITE, regs->ptppinst,
780 &pin_start, NULL);
781 if (rc < 0)
782 goto out;
783
784 rc = sja1105_xfer_u32(priv, SPI_WRITE, regs->ptppindur,
785 &pin_duration32, NULL);
786 if (rc < 0)
787 goto out;
788 }
789
790 if (on)
791 cmd.startptpcp = true;
792 else
793 cmd.stopptpcp = true;
794
795 rc = sja1105_ptp_commit(priv->ds, &cmd, SPI_WRITE);
796
797out:
798 mutex_unlock(&ptp_data->lock);
799
800 return rc;
801}
802
803static int sja1105_extts_enable(struct sja1105_private *priv,
804 struct ptp_extts_request *extts,
805 bool on)
806{
807 int rc;
808
809 /* We only support one channel */
810 if (extts->index != 0)
811 return -EOPNOTSUPP;
812
813 /* Reject requests with unsupported flags */
814 if (extts->flags & ~(PTP_ENABLE_FEATURE |
815 PTP_RISING_EDGE |
816 PTP_FALLING_EDGE |
817 PTP_STRICT_FLAGS))
818 return -EOPNOTSUPP;
819
820 /* We can only enable time stamping on both edges, sadly. */
821 if ((extts->flags & PTP_STRICT_FLAGS) &&
822 (extts->flags & PTP_ENABLE_FEATURE) &&
823 (extts->flags & PTP_EXTTS_EDGES) != PTP_EXTTS_EDGES)
824 return -EOPNOTSUPP;
825
826 rc = sja1105_change_ptp_clk_pin_func(priv, PTP_PF_EXTTS);
827 if (rc)
828 return rc;
829
830 priv->ptp_data.extts_enabled = on;
831
832 if (on)
833 sja1105_ptp_extts_setup_timer(&priv->ptp_data);
834 else
835 del_timer_sync(&priv->ptp_data.extts_timer);
836
837 return 0;
838}
839
840static int sja1105_ptp_enable(struct ptp_clock_info *ptp,
841 struct ptp_clock_request *req, int on)
842{
843 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
844 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
845 int rc = -EOPNOTSUPP;
846
847 if (req->type == PTP_CLK_REQ_PEROUT)
848 rc = sja1105_per_out_enable(priv, &req->perout, on);
849 else if (req->type == PTP_CLK_REQ_EXTTS)
850 rc = sja1105_extts_enable(priv, &req->extts, on);
851
852 return rc;
853}
854
855static int sja1105_ptp_verify_pin(struct ptp_clock_info *ptp, unsigned int pin,
856 enum ptp_pin_function func, unsigned int chan)
857{
858 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
859 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
860
861 if (chan != 0 || pin != 0)
862 return -1;
863
864 switch (func) {
865 case PTP_PF_NONE:
866 case PTP_PF_PEROUT:
867 break;
868 case PTP_PF_EXTTS:
869 if (priv->info->device_id == SJA1105E_DEVICE_ID ||
870 priv->info->device_id == SJA1105T_DEVICE_ID)
871 return -1;
872 break;
873 default:
874 return -1;
875 }
876 return 0;
877}
878
879static struct ptp_pin_desc sja1105_ptp_pin = {
880 .name = "ptp_clk",
881 .index = 0,
882 .func = PTP_PF_NONE,
883};
884
885int sja1105_ptp_clock_register(struct dsa_switch *ds)
886{
887 struct sja1105_private *priv = ds->priv;
888 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
889
890 ptp_data->caps = (struct ptp_clock_info) {
891 .owner = THIS_MODULE,
892 .name = "SJA1105 PHC",
893 .adjfine = sja1105_ptp_adjfine,
894 .adjtime = sja1105_ptp_adjtime,
895 .gettimex64 = sja1105_ptp_gettimex,
896 .settime64 = sja1105_ptp_settime,
897 .enable = sja1105_ptp_enable,
898 .verify = sja1105_ptp_verify_pin,
899 .do_aux_work = sja1105_rxtstamp_work,
900 .max_adj = SJA1105_MAX_ADJ_PPB,
901 .pin_config = &sja1105_ptp_pin,
902 .n_pins = 1,
903 .n_ext_ts = 1,
904 .n_per_out = 1,
905 };
906
907 /* Only used on SJA1105 */
908 skb_queue_head_init(&ptp_data->skb_rxtstamp_queue);
909 /* Only used on SJA1110 */
910 skb_queue_head_init(&ptp_data->skb_txtstamp_queue);
911
912 ptp_data->clock = ptp_clock_register(&ptp_data->caps, ds->dev);
913 if (IS_ERR_OR_NULL(ptp_data->clock))
914 return PTR_ERR(ptp_data->clock);
915
916 ptp_data->cmd.corrclk4ts = true;
917 ptp_data->cmd.ptpclkadd = PTP_SET_MODE;
918
919 timer_setup(&ptp_data->extts_timer, sja1105_ptp_extts_timer, 0);
920
921 return sja1105_ptp_reset(ds);
922}
923
924void sja1105_ptp_clock_unregister(struct dsa_switch *ds)
925{
926 struct sja1105_private *priv = ds->priv;
927 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
928
929 if (IS_ERR_OR_NULL(ptp_data->clock))
930 return;
931
932 del_timer_sync(&ptp_data->extts_timer);
933 ptp_cancel_worker_sync(ptp_data->clock);
934 skb_queue_purge(&ptp_data->skb_txtstamp_queue);
935 skb_queue_purge(&ptp_data->skb_rxtstamp_queue);
936 ptp_clock_unregister(ptp_data->clock);
937 ptp_data->clock = NULL;
938}
939
940void sja1105_ptp_txtstamp_skb(struct dsa_switch *ds, int port,
941 struct sk_buff *skb)
942{
943 struct sja1105_private *priv = ds->priv;
944 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
945 struct skb_shared_hwtstamps shwt = {0};
946 u64 ticks, ts;
947 int rc;
948
949 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
950
951 mutex_lock(&ptp_data->lock);
952
953 rc = sja1105_ptpegr_ts_poll(ds, port, &ts);
954 if (rc < 0) {
955 dev_err(ds->dev, "timed out polling for tstamp\n");
956 kfree_skb(skb);
957 goto out;
958 }
959
960 rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
961 if (rc < 0) {
962 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
963 kfree_skb(skb);
964 goto out;
965 }
966
967 ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
968
969 shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
970 skb_complete_tx_timestamp(skb, &shwt);
971
972out:
973 mutex_unlock(&ptp_data->lock);
974}
1// SPDX-License-Identifier: GPL-2.0
2/* Copyright (c) 2019, Vladimir Oltean <olteanv@gmail.com>
3 */
4#include <linux/spi/spi.h>
5#include "sja1105.h"
6
7/* The adjfine API clamps ppb between [-32,768,000, 32,768,000], and
8 * therefore scaled_ppm between [-2,147,483,648, 2,147,483,647].
9 * Set the maximum supported ppb to a round value smaller than the maximum.
10 *
11 * Percentually speaking, this is a +/- 0.032x adjustment of the
12 * free-running counter (0.968x to 1.032x).
13 */
14#define SJA1105_MAX_ADJ_PPB 32000000
15#define SJA1105_SIZE_PTP_CMD 4
16
17/* PTPSYNCTS has no interrupt or update mechanism, because the intended
18 * hardware use case is for the timestamp to be collected synchronously,
19 * immediately after the CAS_MASTER SJA1105 switch has performed a CASSYNC
20 * one-shot toggle (no return to level) on the PTP_CLK pin. When used as a
21 * generic extts source, the PTPSYNCTS register needs polling and a comparison
22 * with the old value. The polling interval is configured as the Nyquist rate
23 * of a signal with 50% duty cycle and 1Hz frequency, which is sadly all that
24 * this hardware can do (but may be enough for some setups). Anything of higher
25 * frequency than 1 Hz will be lost, since there is no timestamp FIFO.
26 */
27#define SJA1105_EXTTS_INTERVAL (HZ / 6)
28
29/* This range is actually +/- SJA1105_MAX_ADJ_PPB
30 * divided by 1000 (ppb -> ppm) and with a 16-bit
31 * "fractional" part (actually fixed point).
32 * |
33 * v
34 * Convert scaled_ppm from the +/- ((10^6) << 16) range
35 * into the +/- (1 << 31) range.
36 *
37 * This forgoes a "ppb" numeric representation (up to NSEC_PER_SEC)
38 * and defines the scaling factor between scaled_ppm and the actual
39 * frequency adjustments of the PHC.
40 *
41 * ptpclkrate = scaled_ppm * 2^31 / (10^6 * 2^16)
42 * simplifies to
43 * ptpclkrate = scaled_ppm * 2^9 / 5^6
44 */
45#define SJA1105_CC_MULT_NUM (1 << 9)
46#define SJA1105_CC_MULT_DEM 15625
47#define SJA1105_CC_MULT 0x80000000
48
49enum sja1105_ptp_clk_mode {
50 PTP_ADD_MODE = 1,
51 PTP_SET_MODE = 0,
52};
53
54#define extts_to_data(t) \
55 container_of((t), struct sja1105_ptp_data, extts_timer)
56#define ptp_caps_to_data(d) \
57 container_of((d), struct sja1105_ptp_data, caps)
58#define ptp_data_to_sja1105(d) \
59 container_of((d), struct sja1105_private, ptp_data)
60
61/* Must be called only while the RX timestamping state of the tagger
62 * is turned off
63 */
64static int sja1105_change_rxtstamping(struct sja1105_private *priv,
65 bool on)
66{
67 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
68 struct sja1105_general_params_entry *general_params;
69 struct sja1105_table *table;
70
71 table = &priv->static_config.tables[BLK_IDX_GENERAL_PARAMS];
72 general_params = table->entries;
73 general_params->send_meta1 = on;
74 general_params->send_meta0 = on;
75
76 ptp_cancel_worker_sync(ptp_data->clock);
77 skb_queue_purge(&ptp_data->skb_txtstamp_queue);
78 skb_queue_purge(&ptp_data->skb_rxtstamp_queue);
79
80 return sja1105_static_config_reload(priv, SJA1105_RX_HWTSTAMPING);
81}
82
83int sja1105_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr)
84{
85 struct sja1105_tagger_data *tagger_data = sja1105_tagger_data(ds);
86 struct sja1105_private *priv = ds->priv;
87 struct hwtstamp_config config;
88 bool rx_on;
89 int rc;
90
91 if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
92 return -EFAULT;
93
94 switch (config.tx_type) {
95 case HWTSTAMP_TX_OFF:
96 priv->hwts_tx_en &= ~BIT(port);
97 break;
98 case HWTSTAMP_TX_ON:
99 priv->hwts_tx_en |= BIT(port);
100 break;
101 default:
102 return -ERANGE;
103 }
104
105 switch (config.rx_filter) {
106 case HWTSTAMP_FILTER_NONE:
107 rx_on = false;
108 break;
109 default:
110 rx_on = true;
111 break;
112 }
113
114 if (rx_on != tagger_data->rxtstamp_get_state(ds)) {
115 tagger_data->rxtstamp_set_state(ds, false);
116
117 rc = sja1105_change_rxtstamping(priv, rx_on);
118 if (rc < 0) {
119 dev_err(ds->dev,
120 "Failed to change RX timestamping: %d\n", rc);
121 return rc;
122 }
123 if (rx_on)
124 tagger_data->rxtstamp_set_state(ds, true);
125 }
126
127 if (copy_to_user(ifr->ifr_data, &config, sizeof(config)))
128 return -EFAULT;
129 return 0;
130}
131
132int sja1105_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr)
133{
134 struct sja1105_tagger_data *tagger_data = sja1105_tagger_data(ds);
135 struct sja1105_private *priv = ds->priv;
136 struct hwtstamp_config config;
137
138 config.flags = 0;
139 if (priv->hwts_tx_en & BIT(port))
140 config.tx_type = HWTSTAMP_TX_ON;
141 else
142 config.tx_type = HWTSTAMP_TX_OFF;
143 if (tagger_data->rxtstamp_get_state(ds))
144 config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
145 else
146 config.rx_filter = HWTSTAMP_FILTER_NONE;
147
148 return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
149 -EFAULT : 0;
150}
151
152int sja1105_get_ts_info(struct dsa_switch *ds, int port,
153 struct ethtool_ts_info *info)
154{
155 struct sja1105_private *priv = ds->priv;
156 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
157
158 /* Called during cleanup */
159 if (!ptp_data->clock)
160 return -ENODEV;
161
162 info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
163 SOF_TIMESTAMPING_RX_HARDWARE |
164 SOF_TIMESTAMPING_RAW_HARDWARE;
165 info->tx_types = (1 << HWTSTAMP_TX_OFF) |
166 (1 << HWTSTAMP_TX_ON);
167 info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
168 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT);
169 info->phc_index = ptp_clock_index(ptp_data->clock);
170 return 0;
171}
172
173void sja1105et_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
174 enum packing_op op)
175{
176 const int size = SJA1105_SIZE_PTP_CMD;
177 /* No need to keep this as part of the structure */
178 u64 valid = 1;
179
180 sja1105_packing(buf, &valid, 31, 31, size, op);
181 sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
182 sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
183 sja1105_packing(buf, &cmd->startptpcp, 28, 28, size, op);
184 sja1105_packing(buf, &cmd->stopptpcp, 27, 27, size, op);
185 sja1105_packing(buf, &cmd->resptp, 2, 2, size, op);
186 sja1105_packing(buf, &cmd->corrclk4ts, 1, 1, size, op);
187 sja1105_packing(buf, &cmd->ptpclkadd, 0, 0, size, op);
188}
189
190void sja1105pqrs_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
191 enum packing_op op)
192{
193 const int size = SJA1105_SIZE_PTP_CMD;
194 /* No need to keep this as part of the structure */
195 u64 valid = 1;
196
197 sja1105_packing(buf, &valid, 31, 31, size, op);
198 sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
199 sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
200 sja1105_packing(buf, &cmd->startptpcp, 28, 28, size, op);
201 sja1105_packing(buf, &cmd->stopptpcp, 27, 27, size, op);
202 sja1105_packing(buf, &cmd->resptp, 3, 3, size, op);
203 sja1105_packing(buf, &cmd->corrclk4ts, 2, 2, size, op);
204 sja1105_packing(buf, &cmd->ptpclkadd, 0, 0, size, op);
205}
206
207int sja1105_ptp_commit(struct dsa_switch *ds, struct sja1105_ptp_cmd *cmd,
208 sja1105_spi_rw_mode_t rw)
209{
210 const struct sja1105_private *priv = ds->priv;
211 const struct sja1105_regs *regs = priv->info->regs;
212 u8 buf[SJA1105_SIZE_PTP_CMD] = {0};
213 int rc;
214
215 if (rw == SPI_WRITE)
216 priv->info->ptp_cmd_packing(buf, cmd, PACK);
217
218 rc = sja1105_xfer_buf(priv, rw, regs->ptp_control, buf,
219 SJA1105_SIZE_PTP_CMD);
220
221 if (rw == SPI_READ)
222 priv->info->ptp_cmd_packing(buf, cmd, UNPACK);
223
224 return rc;
225}
226
227/* The switch returns partial timestamps (24 bits for SJA1105 E/T, which wrap
228 * around in 0.135 seconds, and 32 bits for P/Q/R/S, wrapping around in 34.35
229 * seconds).
230 *
231 * This receives the RX or TX MAC timestamps, provided by hardware as
232 * the lower bits of the cycle counter, sampled at the time the timestamp was
233 * collected.
234 *
235 * To reconstruct into a full 64-bit-wide timestamp, the cycle counter is
236 * read and the high-order bits are filled in.
237 *
238 * Must be called within one wraparound period of the partial timestamp since
239 * it was generated by the MAC.
240 */
241static u64 sja1105_tstamp_reconstruct(struct dsa_switch *ds, u64 now,
242 u64 ts_partial)
243{
244 struct sja1105_private *priv = ds->priv;
245 u64 partial_tstamp_mask = CYCLECOUNTER_MASK(priv->info->ptp_ts_bits);
246 u64 ts_reconstructed;
247
248 ts_reconstructed = (now & ~partial_tstamp_mask) | ts_partial;
249
250 /* Check lower bits of current cycle counter against the timestamp.
251 * If the current cycle counter is lower than the partial timestamp,
252 * then wraparound surely occurred and must be accounted for.
253 */
254 if ((now & partial_tstamp_mask) <= ts_partial)
255 ts_reconstructed -= (partial_tstamp_mask + 1);
256
257 return ts_reconstructed;
258}
259
260/* Reads the SPI interface for an egress timestamp generated by the switch
261 * for frames sent using management routes.
262 *
263 * SJA1105 E/T layout of the 4-byte SPI payload:
264 *
265 * 31 23 15 7 0
266 * | | | | |
267 * +-----+-----+-----+ ^
268 * ^ |
269 * | |
270 * 24-bit timestamp Update bit
271 *
272 *
273 * SJA1105 P/Q/R/S layout of the 8-byte SPI payload:
274 *
275 * 31 23 15 7 0 63 55 47 39 32
276 * | | | | | | | | | |
277 * ^ +-----+-----+-----+-----+
278 * | ^
279 * | |
280 * Update bit 32-bit timestamp
281 *
282 * Notice that the update bit is in the same place.
283 * To have common code for E/T and P/Q/R/S for reading the timestamp,
284 * we need to juggle with the offset and the bit indices.
285 */
286static int sja1105_ptpegr_ts_poll(struct dsa_switch *ds, int port, u64 *ts)
287{
288 struct sja1105_private *priv = ds->priv;
289 const struct sja1105_regs *regs = priv->info->regs;
290 int tstamp_bit_start, tstamp_bit_end;
291 int timeout = 10;
292 u8 packed_buf[8];
293 u64 update;
294 int rc;
295
296 do {
297 rc = sja1105_xfer_buf(priv, SPI_READ, regs->ptpegr_ts[port],
298 packed_buf, priv->info->ptpegr_ts_bytes);
299 if (rc < 0)
300 return rc;
301
302 sja1105_unpack(packed_buf, &update, 0, 0,
303 priv->info->ptpegr_ts_bytes);
304 if (update)
305 break;
306
307 usleep_range(10, 50);
308 } while (--timeout);
309
310 if (!timeout)
311 return -ETIMEDOUT;
312
313 /* Point the end bit to the second 32-bit word on P/Q/R/S,
314 * no-op on E/T.
315 */
316 tstamp_bit_end = (priv->info->ptpegr_ts_bytes - 4) * 8;
317 /* Shift the 24-bit timestamp on E/T to be collected from 31:8.
318 * No-op on P/Q/R/S.
319 */
320 tstamp_bit_end += 32 - priv->info->ptp_ts_bits;
321 tstamp_bit_start = tstamp_bit_end + priv->info->ptp_ts_bits - 1;
322
323 *ts = 0;
324
325 sja1105_unpack(packed_buf, ts, tstamp_bit_start, tstamp_bit_end,
326 priv->info->ptpegr_ts_bytes);
327
328 return 0;
329}
330
331/* Caller must hold ptp_data->lock */
332static int sja1105_ptpclkval_read(struct sja1105_private *priv, u64 *ticks,
333 struct ptp_system_timestamp *ptp_sts)
334{
335 const struct sja1105_regs *regs = priv->info->regs;
336
337 return sja1105_xfer_u64(priv, SPI_READ, regs->ptpclkval, ticks,
338 ptp_sts);
339}
340
341/* Caller must hold ptp_data->lock */
342static int sja1105_ptpclkval_write(struct sja1105_private *priv, u64 ticks,
343 struct ptp_system_timestamp *ptp_sts)
344{
345 const struct sja1105_regs *regs = priv->info->regs;
346
347 return sja1105_xfer_u64(priv, SPI_WRITE, regs->ptpclkval, &ticks,
348 ptp_sts);
349}
350
351static void sja1105_extts_poll(struct sja1105_private *priv)
352{
353 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
354 const struct sja1105_regs *regs = priv->info->regs;
355 struct ptp_clock_event event;
356 u64 ptpsyncts = 0;
357 int rc;
358
359 rc = sja1105_xfer_u64(priv, SPI_READ, regs->ptpsyncts, &ptpsyncts,
360 NULL);
361 if (rc < 0)
362 dev_err_ratelimited(priv->ds->dev,
363 "Failed to read PTPSYNCTS: %d\n", rc);
364
365 if (ptpsyncts && ptp_data->ptpsyncts != ptpsyncts) {
366 event.index = 0;
367 event.type = PTP_CLOCK_EXTTS;
368 event.timestamp = ns_to_ktime(sja1105_ticks_to_ns(ptpsyncts));
369 ptp_clock_event(ptp_data->clock, &event);
370
371 ptp_data->ptpsyncts = ptpsyncts;
372 }
373}
374
375static long sja1105_rxtstamp_work(struct ptp_clock_info *ptp)
376{
377 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
378 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
379 struct dsa_switch *ds = priv->ds;
380 struct sk_buff *skb;
381
382 mutex_lock(&ptp_data->lock);
383
384 while ((skb = skb_dequeue(&ptp_data->skb_rxtstamp_queue)) != NULL) {
385 struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);
386 u64 ticks, ts;
387 int rc;
388
389 rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
390 if (rc < 0) {
391 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
392 kfree_skb(skb);
393 continue;
394 }
395
396 *shwt = (struct skb_shared_hwtstamps) {0};
397
398 ts = SJA1105_SKB_CB(skb)->tstamp;
399 ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
400
401 shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
402 netif_rx(skb);
403 }
404
405 if (ptp_data->extts_enabled)
406 sja1105_extts_poll(priv);
407
408 mutex_unlock(&ptp_data->lock);
409
410 /* Don't restart */
411 return -1;
412}
413
414bool sja1105_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
415{
416 struct sja1105_tagger_data *tagger_data = sja1105_tagger_data(ds);
417 struct sja1105_private *priv = ds->priv;
418 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
419
420 if (!tagger_data->rxtstamp_get_state(ds))
421 return false;
422
423 /* We need to read the full PTP clock to reconstruct the Rx
424 * timestamp. For that we need a sleepable context.
425 */
426 skb_queue_tail(&ptp_data->skb_rxtstamp_queue, skb);
427 ptp_schedule_worker(ptp_data->clock, 0);
428 return true;
429}
430
431bool sja1110_rxtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
432{
433 struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);
434 u64 ts = SJA1105_SKB_CB(skb)->tstamp;
435
436 *shwt = (struct skb_shared_hwtstamps) {0};
437
438 shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
439
440 /* Don't defer */
441 return false;
442}
443
444/* Called from dsa_skb_defer_rx_timestamp */
445bool sja1105_port_rxtstamp(struct dsa_switch *ds, int port,
446 struct sk_buff *skb, unsigned int type)
447{
448 struct sja1105_private *priv = ds->priv;
449
450 return priv->info->rxtstamp(ds, port, skb);
451}
452
453void sja1110_process_meta_tstamp(struct dsa_switch *ds, int port, u8 ts_id,
454 enum sja1110_meta_tstamp dir, u64 tstamp)
455{
456 struct sja1105_private *priv = ds->priv;
457 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
458 struct sk_buff *skb, *skb_tmp, *skb_match = NULL;
459 struct skb_shared_hwtstamps shwt = {0};
460
461 /* We don't care about RX timestamps on the CPU port */
462 if (dir == SJA1110_META_TSTAMP_RX)
463 return;
464
465 spin_lock(&ptp_data->skb_txtstamp_queue.lock);
466
467 skb_queue_walk_safe(&ptp_data->skb_txtstamp_queue, skb, skb_tmp) {
468 if (SJA1105_SKB_CB(skb)->ts_id != ts_id)
469 continue;
470
471 __skb_unlink(skb, &ptp_data->skb_txtstamp_queue);
472 skb_match = skb;
473
474 break;
475 }
476
477 spin_unlock(&ptp_data->skb_txtstamp_queue.lock);
478
479 if (WARN_ON(!skb_match))
480 return;
481
482 shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(tstamp));
483 skb_complete_tx_timestamp(skb_match, &shwt);
484}
485
486/* In addition to cloning the skb which is done by the common
487 * sja1105_port_txtstamp, we need to generate a timestamp ID and save the
488 * packet to the TX timestamping queue.
489 */
490void sja1110_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
491{
492 struct sk_buff *clone = SJA1105_SKB_CB(skb)->clone;
493 struct sja1105_private *priv = ds->priv;
494 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
495 u8 ts_id;
496
497 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
498
499 spin_lock(&priv->ts_id_lock);
500
501 ts_id = priv->ts_id;
502 /* Deal automatically with 8-bit wraparound */
503 priv->ts_id++;
504
505 SJA1105_SKB_CB(clone)->ts_id = ts_id;
506
507 spin_unlock(&priv->ts_id_lock);
508
509 skb_queue_tail(&ptp_data->skb_txtstamp_queue, clone);
510}
511
512/* Called from dsa_skb_tx_timestamp. This callback is just to clone
513 * the skb and have it available in SJA1105_SKB_CB in the .port_deferred_xmit
514 * callback, where we will timestamp it synchronously.
515 */
516void sja1105_port_txtstamp(struct dsa_switch *ds, int port, struct sk_buff *skb)
517{
518 struct sja1105_private *priv = ds->priv;
519 struct sk_buff *clone;
520
521 if (!(priv->hwts_tx_en & BIT(port)))
522 return;
523
524 clone = skb_clone_sk(skb);
525 if (!clone)
526 return;
527
528 SJA1105_SKB_CB(skb)->clone = clone;
529
530 if (priv->info->txtstamp)
531 priv->info->txtstamp(ds, port, skb);
532}
533
534static int sja1105_ptp_reset(struct dsa_switch *ds)
535{
536 struct sja1105_private *priv = ds->priv;
537 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
538 struct sja1105_ptp_cmd cmd = ptp_data->cmd;
539 int rc;
540
541 mutex_lock(&ptp_data->lock);
542
543 cmd.resptp = 1;
544
545 dev_dbg(ds->dev, "Resetting PTP clock\n");
546 rc = sja1105_ptp_commit(ds, &cmd, SPI_WRITE);
547
548 sja1105_tas_clockstep(priv->ds);
549
550 mutex_unlock(&ptp_data->lock);
551
552 return rc;
553}
554
555/* Caller must hold ptp_data->lock */
556int __sja1105_ptp_gettimex(struct dsa_switch *ds, u64 *ns,
557 struct ptp_system_timestamp *ptp_sts)
558{
559 struct sja1105_private *priv = ds->priv;
560 u64 ticks;
561 int rc;
562
563 rc = sja1105_ptpclkval_read(priv, &ticks, ptp_sts);
564 if (rc < 0) {
565 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
566 return rc;
567 }
568
569 *ns = sja1105_ticks_to_ns(ticks);
570
571 return 0;
572}
573
574static int sja1105_ptp_gettimex(struct ptp_clock_info *ptp,
575 struct timespec64 *ts,
576 struct ptp_system_timestamp *ptp_sts)
577{
578 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
579 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
580 u64 now = 0;
581 int rc;
582
583 mutex_lock(&ptp_data->lock);
584
585 rc = __sja1105_ptp_gettimex(priv->ds, &now, ptp_sts);
586 *ts = ns_to_timespec64(now);
587
588 mutex_unlock(&ptp_data->lock);
589
590 return rc;
591}
592
593/* Caller must hold ptp_data->lock */
594static int sja1105_ptp_mode_set(struct sja1105_private *priv,
595 enum sja1105_ptp_clk_mode mode)
596{
597 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
598
599 if (ptp_data->cmd.ptpclkadd == mode)
600 return 0;
601
602 ptp_data->cmd.ptpclkadd = mode;
603
604 return sja1105_ptp_commit(priv->ds, &ptp_data->cmd, SPI_WRITE);
605}
606
607/* Write to PTPCLKVAL while PTPCLKADD is 0 */
608int __sja1105_ptp_settime(struct dsa_switch *ds, u64 ns,
609 struct ptp_system_timestamp *ptp_sts)
610{
611 struct sja1105_private *priv = ds->priv;
612 u64 ticks = ns_to_sja1105_ticks(ns);
613 int rc;
614
615 rc = sja1105_ptp_mode_set(priv, PTP_SET_MODE);
616 if (rc < 0) {
617 dev_err(priv->ds->dev, "Failed to put PTPCLK in set mode\n");
618 return rc;
619 }
620
621 rc = sja1105_ptpclkval_write(priv, ticks, ptp_sts);
622
623 sja1105_tas_clockstep(priv->ds);
624
625 return rc;
626}
627
628static int sja1105_ptp_settime(struct ptp_clock_info *ptp,
629 const struct timespec64 *ts)
630{
631 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
632 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
633 u64 ns = timespec64_to_ns(ts);
634 int rc;
635
636 mutex_lock(&ptp_data->lock);
637
638 rc = __sja1105_ptp_settime(priv->ds, ns, NULL);
639
640 mutex_unlock(&ptp_data->lock);
641
642 return rc;
643}
644
645static int sja1105_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
646{
647 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
648 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
649 const struct sja1105_regs *regs = priv->info->regs;
650 u32 clkrate32;
651 s64 clkrate;
652 int rc;
653
654 clkrate = (s64)scaled_ppm * SJA1105_CC_MULT_NUM;
655 clkrate = div_s64(clkrate, SJA1105_CC_MULT_DEM);
656
657 /* Take a +/- value and re-center it around 2^31. */
658 clkrate = SJA1105_CC_MULT + clkrate;
659 WARN_ON(abs(clkrate) >= GENMASK_ULL(31, 0));
660 clkrate32 = clkrate;
661
662 mutex_lock(&ptp_data->lock);
663
664 rc = sja1105_xfer_u32(priv, SPI_WRITE, regs->ptpclkrate, &clkrate32,
665 NULL);
666
667 sja1105_tas_adjfreq(priv->ds);
668
669 mutex_unlock(&ptp_data->lock);
670
671 return rc;
672}
673
674/* Write to PTPCLKVAL while PTPCLKADD is 1 */
675int __sja1105_ptp_adjtime(struct dsa_switch *ds, s64 delta)
676{
677 struct sja1105_private *priv = ds->priv;
678 s64 ticks = ns_to_sja1105_ticks(delta);
679 int rc;
680
681 rc = sja1105_ptp_mode_set(priv, PTP_ADD_MODE);
682 if (rc < 0) {
683 dev_err(priv->ds->dev, "Failed to put PTPCLK in add mode\n");
684 return rc;
685 }
686
687 rc = sja1105_ptpclkval_write(priv, ticks, NULL);
688
689 sja1105_tas_clockstep(priv->ds);
690
691 return rc;
692}
693
694static int sja1105_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
695{
696 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
697 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
698 int rc;
699
700 mutex_lock(&ptp_data->lock);
701
702 rc = __sja1105_ptp_adjtime(priv->ds, delta);
703
704 mutex_unlock(&ptp_data->lock);
705
706 return rc;
707}
708
709static void sja1105_ptp_extts_setup_timer(struct sja1105_ptp_data *ptp_data)
710{
711 unsigned long expires = ((jiffies / SJA1105_EXTTS_INTERVAL) + 1) *
712 SJA1105_EXTTS_INTERVAL;
713
714 mod_timer(&ptp_data->extts_timer, expires);
715}
716
717static void sja1105_ptp_extts_timer(struct timer_list *t)
718{
719 struct sja1105_ptp_data *ptp_data = extts_to_data(t);
720
721 ptp_schedule_worker(ptp_data->clock, 0);
722
723 sja1105_ptp_extts_setup_timer(ptp_data);
724}
725
726static int sja1105_change_ptp_clk_pin_func(struct sja1105_private *priv,
727 enum ptp_pin_function func)
728{
729 struct sja1105_avb_params_entry *avb;
730 enum ptp_pin_function old_func;
731
732 avb = priv->static_config.tables[BLK_IDX_AVB_PARAMS].entries;
733
734 if (priv->info->device_id == SJA1105E_DEVICE_ID ||
735 priv->info->device_id == SJA1105T_DEVICE_ID ||
736 avb->cas_master)
737 old_func = PTP_PF_PEROUT;
738 else
739 old_func = PTP_PF_EXTTS;
740
741 if (func == old_func)
742 return 0;
743
744 avb->cas_master = (func == PTP_PF_PEROUT);
745
746 return sja1105_dynamic_config_write(priv, BLK_IDX_AVB_PARAMS, 0, avb,
747 true);
748}
749
750/* The PTP_CLK pin may be configured to toggle with a 50% duty cycle and a
751 * frequency f:
752 *
753 * NSEC_PER_SEC
754 * f = ----------------------
755 * (PTPPINDUR * 8 ns) * 2
756 */
757static int sja1105_per_out_enable(struct sja1105_private *priv,
758 struct ptp_perout_request *perout,
759 bool on)
760{
761 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
762 const struct sja1105_regs *regs = priv->info->regs;
763 struct sja1105_ptp_cmd cmd = ptp_data->cmd;
764 int rc;
765
766 /* We only support one channel */
767 if (perout->index != 0)
768 return -EOPNOTSUPP;
769
770 /* Reject requests with unsupported flags */
771 if (perout->flags)
772 return -EOPNOTSUPP;
773
774 mutex_lock(&ptp_data->lock);
775
776 rc = sja1105_change_ptp_clk_pin_func(priv, PTP_PF_PEROUT);
777 if (rc)
778 goto out;
779
780 if (on) {
781 struct timespec64 pin_duration_ts = {
782 .tv_sec = perout->period.sec,
783 .tv_nsec = perout->period.nsec,
784 };
785 struct timespec64 pin_start_ts = {
786 .tv_sec = perout->start.sec,
787 .tv_nsec = perout->start.nsec,
788 };
789 u64 pin_duration = timespec64_to_ns(&pin_duration_ts);
790 u64 pin_start = timespec64_to_ns(&pin_start_ts);
791 u32 pin_duration32;
792 u64 now;
793
794 /* ptppindur: 32 bit register which holds the interval between
795 * 2 edges on PTP_CLK. So check for truncation which happens
796 * at periods larger than around 68.7 seconds.
797 */
798 pin_duration = ns_to_sja1105_ticks(pin_duration / 2);
799 if (pin_duration > U32_MAX) {
800 rc = -ERANGE;
801 goto out;
802 }
803 pin_duration32 = pin_duration;
804
805 /* ptppins: 64 bit register which needs to hold a PTP time
806 * larger than the current time, otherwise the startptpcp
807 * command won't do anything. So advance the current time
808 * by a number of periods in a way that won't alter the
809 * phase offset.
810 */
811 rc = __sja1105_ptp_gettimex(priv->ds, &now, NULL);
812 if (rc < 0)
813 goto out;
814
815 pin_start = future_base_time(pin_start, pin_duration,
816 now + 1ull * NSEC_PER_SEC);
817 pin_start = ns_to_sja1105_ticks(pin_start);
818
819 rc = sja1105_xfer_u64(priv, SPI_WRITE, regs->ptppinst,
820 &pin_start, NULL);
821 if (rc < 0)
822 goto out;
823
824 rc = sja1105_xfer_u32(priv, SPI_WRITE, regs->ptppindur,
825 &pin_duration32, NULL);
826 if (rc < 0)
827 goto out;
828 }
829
830 if (on)
831 cmd.startptpcp = true;
832 else
833 cmd.stopptpcp = true;
834
835 rc = sja1105_ptp_commit(priv->ds, &cmd, SPI_WRITE);
836
837out:
838 mutex_unlock(&ptp_data->lock);
839
840 return rc;
841}
842
843static int sja1105_extts_enable(struct sja1105_private *priv,
844 struct ptp_extts_request *extts,
845 bool on)
846{
847 int rc;
848
849 /* We only support one channel */
850 if (extts->index != 0)
851 return -EOPNOTSUPP;
852
853 /* Reject requests with unsupported flags */
854 if (extts->flags & ~(PTP_ENABLE_FEATURE |
855 PTP_RISING_EDGE |
856 PTP_FALLING_EDGE |
857 PTP_STRICT_FLAGS))
858 return -EOPNOTSUPP;
859
860 /* We can only enable time stamping on both edges, sadly. */
861 if ((extts->flags & PTP_STRICT_FLAGS) &&
862 (extts->flags & PTP_ENABLE_FEATURE) &&
863 (extts->flags & PTP_EXTTS_EDGES) != PTP_EXTTS_EDGES)
864 return -EOPNOTSUPP;
865
866 rc = sja1105_change_ptp_clk_pin_func(priv, PTP_PF_EXTTS);
867 if (rc)
868 return rc;
869
870 priv->ptp_data.extts_enabled = on;
871
872 if (on)
873 sja1105_ptp_extts_setup_timer(&priv->ptp_data);
874 else
875 del_timer_sync(&priv->ptp_data.extts_timer);
876
877 return 0;
878}
879
880static int sja1105_ptp_enable(struct ptp_clock_info *ptp,
881 struct ptp_clock_request *req, int on)
882{
883 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
884 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
885 int rc = -EOPNOTSUPP;
886
887 if (req->type == PTP_CLK_REQ_PEROUT)
888 rc = sja1105_per_out_enable(priv, &req->perout, on);
889 else if (req->type == PTP_CLK_REQ_EXTTS)
890 rc = sja1105_extts_enable(priv, &req->extts, on);
891
892 return rc;
893}
894
895static int sja1105_ptp_verify_pin(struct ptp_clock_info *ptp, unsigned int pin,
896 enum ptp_pin_function func, unsigned int chan)
897{
898 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
899 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
900
901 if (chan != 0 || pin != 0)
902 return -1;
903
904 switch (func) {
905 case PTP_PF_NONE:
906 case PTP_PF_PEROUT:
907 break;
908 case PTP_PF_EXTTS:
909 if (priv->info->device_id == SJA1105E_DEVICE_ID ||
910 priv->info->device_id == SJA1105T_DEVICE_ID)
911 return -1;
912 break;
913 default:
914 return -1;
915 }
916 return 0;
917}
918
919static struct ptp_pin_desc sja1105_ptp_pin = {
920 .name = "ptp_clk",
921 .index = 0,
922 .func = PTP_PF_NONE,
923};
924
925int sja1105_ptp_clock_register(struct dsa_switch *ds)
926{
927 struct sja1105_private *priv = ds->priv;
928 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
929
930 ptp_data->caps = (struct ptp_clock_info) {
931 .owner = THIS_MODULE,
932 .name = "SJA1105 PHC",
933 .adjfine = sja1105_ptp_adjfine,
934 .adjtime = sja1105_ptp_adjtime,
935 .gettimex64 = sja1105_ptp_gettimex,
936 .settime64 = sja1105_ptp_settime,
937 .enable = sja1105_ptp_enable,
938 .verify = sja1105_ptp_verify_pin,
939 .do_aux_work = sja1105_rxtstamp_work,
940 .max_adj = SJA1105_MAX_ADJ_PPB,
941 .pin_config = &sja1105_ptp_pin,
942 .n_pins = 1,
943 .n_ext_ts = 1,
944 .n_per_out = 1,
945 };
946
947 /* Only used on SJA1105 */
948 skb_queue_head_init(&ptp_data->skb_rxtstamp_queue);
949 /* Only used on SJA1110 */
950 skb_queue_head_init(&ptp_data->skb_txtstamp_queue);
951
952 ptp_data->clock = ptp_clock_register(&ptp_data->caps, ds->dev);
953 if (IS_ERR_OR_NULL(ptp_data->clock))
954 return PTR_ERR(ptp_data->clock);
955
956 ptp_data->cmd.corrclk4ts = true;
957 ptp_data->cmd.ptpclkadd = PTP_SET_MODE;
958
959 timer_setup(&ptp_data->extts_timer, sja1105_ptp_extts_timer, 0);
960
961 return sja1105_ptp_reset(ds);
962}
963
964void sja1105_ptp_clock_unregister(struct dsa_switch *ds)
965{
966 struct sja1105_private *priv = ds->priv;
967 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
968
969 if (IS_ERR_OR_NULL(ptp_data->clock))
970 return;
971
972 del_timer_sync(&ptp_data->extts_timer);
973 ptp_cancel_worker_sync(ptp_data->clock);
974 skb_queue_purge(&ptp_data->skb_txtstamp_queue);
975 skb_queue_purge(&ptp_data->skb_rxtstamp_queue);
976 ptp_clock_unregister(ptp_data->clock);
977 ptp_data->clock = NULL;
978}
979
980void sja1105_ptp_txtstamp_skb(struct dsa_switch *ds, int port,
981 struct sk_buff *skb)
982{
983 struct sja1105_private *priv = ds->priv;
984 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
985 struct skb_shared_hwtstamps shwt = {0};
986 u64 ticks, ts;
987 int rc;
988
989 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
990
991 mutex_lock(&ptp_data->lock);
992
993 rc = sja1105_ptpegr_ts_poll(ds, port, &ts);
994 if (rc < 0) {
995 dev_err(ds->dev, "timed out polling for tstamp\n");
996 kfree_skb(skb);
997 goto out;
998 }
999
1000 rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
1001 if (rc < 0) {
1002 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
1003 kfree_skb(skb);
1004 goto out;
1005 }
1006
1007 ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
1008
1009 shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
1010 skb_complete_tx_timestamp(skb, &shwt);
1011
1012out:
1013 mutex_unlock(&ptp_data->lock);
1014}