?  search:
summary | log | graphiclog1 | graphiclog2 | commit | commitdiff | tree | refs | edit | fork
blame | history | raw | HEAD
treewide: remove redundant IS_ERR() before error code check
[linux/fpc-iii.git] / drivers / net / dsa / sja1105 / sja1105_ptp.c
bloba836fc38c4a4db6a416c58c7326b81ec09aff740
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 /* This range is actually +/- SJA1105_MAX_ADJ_PPB
18 * divided by 1000 (ppb -> ppm) and with a 16-bit
19 * "fractional" part (actually fixed point).
20 * |
21 * v
22 * Convert scaled_ppm from the +/- ((10^6) << 16) range
23 * into the +/- (1 << 31) range.
24 *
25 * This forgoes a "ppb" numeric representation (up to NSEC_PER_SEC)
26 * and defines the scaling factor between scaled_ppm and the actual
27 * frequency adjustments of the PHC.
28 *
29 * ptpclkrate = scaled_ppm * 2^31 / (10^6 * 2^16)
30 * simplifies to
31 * ptpclkrate = scaled_ppm * 2^9 / 5^6
32 */
33 #define SJA1105_CC_MULT_NUM (1 << 9)
34 #define SJA1105_CC_MULT_DEM 15625
35 #define SJA1105_CC_MULT 0x80000000
36
37 enum sja1105_ptp_clk_mode {
38 PTP_ADD_MODE = 1,
39 PTP_SET_MODE = 0,
40 };
41
42 #define ptp_caps_to_data(d) \
43 container_of((d), struct sja1105_ptp_data, caps)
44 #define ptp_data_to_sja1105(d) \
45 container_of((d), struct sja1105_private, ptp_data)
46
47 static int sja1105_init_avb_params(struct sja1105_private *priv,
48 bool on)
49 {
50 struct sja1105_avb_params_entry *avb;
51 struct sja1105_table *table;
52
53 table = &priv->static_config.tables[BLK_IDX_AVB_PARAMS];
54
55 /* Discard previous AVB Parameters Table */
56 if (table->entry_count) {
57 kfree(table->entries);
58 table->entry_count = 0;
59 }
60
61 /* Configure the reception of meta frames only if requested */
62 if (!on)
63 return 0;
64
65 table->entries = kcalloc(SJA1105_MAX_AVB_PARAMS_COUNT,
66 table->ops->unpacked_entry_size, GFP_KERNEL);
67 if (!table->entries)
68 return -ENOMEM;
69
70 table->entry_count = SJA1105_MAX_AVB_PARAMS_COUNT;
71
72 avb = table->entries;
73
74 avb->destmeta = SJA1105_META_DMAC;
75 avb->srcmeta = SJA1105_META_SMAC;
76
77 return 0;
78 }
79
80 /* Must be called only with priv->tagger_data.state bit
81 * SJA1105_HWTS_RX_EN cleared
82 */
83 static int sja1105_change_rxtstamping(struct sja1105_private *priv,
84 bool on)
85 {
86 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
87 struct sja1105_general_params_entry *general_params;
88 struct sja1105_table *table;
89 int rc;
90
91 table = &priv->static_config.tables[BLK_IDX_GENERAL_PARAMS];
92 general_params = table->entries;
93 general_params->send_meta1 = on;
94 general_params->send_meta0 = on;
95
96 rc = sja1105_init_avb_params(priv, on);
97 if (rc < 0)
98 return rc;
99
100 /* Initialize the meta state machine to a known state */
101 if (priv->tagger_data.stampable_skb) {
102 kfree_skb(priv->tagger_data.stampable_skb);
103 priv->tagger_data.stampable_skb = NULL;
104 }
105 ptp_cancel_worker_sync(ptp_data->clock);
106 skb_queue_purge(&ptp_data->skb_rxtstamp_queue);
107
108 return sja1105_static_config_reload(priv, SJA1105_RX_HWTSTAMPING);
109 }
110
111 int sja1105_hwtstamp_set(struct dsa_switch *ds, int port, struct ifreq *ifr)
112 {
113 struct sja1105_private *priv = ds->priv;
114 struct hwtstamp_config config;
115 bool rx_on;
116 int rc;
117
118 if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
119 return -EFAULT;
120
121 switch (config.tx_type) {
122 case HWTSTAMP_TX_OFF:
123 priv->ports[port].hwts_tx_en = false;
124 break;
125 case HWTSTAMP_TX_ON:
126 priv->ports[port].hwts_tx_en = true;
127 break;
128 default:
129 return -ERANGE;
130 }
131
132 switch (config.rx_filter) {
133 case HWTSTAMP_FILTER_NONE:
134 rx_on = false;
135 break;
136 default:
137 rx_on = true;
138 break;
139 }
140
141 if (rx_on != test_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state)) {
142 clear_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state);
143
144 rc = sja1105_change_rxtstamping(priv, rx_on);
145 if (rc < 0) {
146 dev_err(ds->dev,
147 "Failed to change RX timestamping: %d\n", rc);
148 return rc;
149 }
150 if (rx_on)
151 set_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state);
152 }
153
154 if (copy_to_user(ifr->ifr_data, &config, sizeof(config)))
155 return -EFAULT;
156 return 0;
157 }
158
159 int sja1105_hwtstamp_get(struct dsa_switch *ds, int port, struct ifreq *ifr)
160 {
161 struct sja1105_private *priv = ds->priv;
162 struct hwtstamp_config config;
163
164 config.flags = 0;
165 if (priv->ports[port].hwts_tx_en)
166 config.tx_type = HWTSTAMP_TX_ON;
167 else
168 config.tx_type = HWTSTAMP_TX_OFF;
169 if (test_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state))
170 config.rx_filter = HWTSTAMP_FILTER_PTP_V2_L2_EVENT;
171 else
172 config.rx_filter = HWTSTAMP_FILTER_NONE;
173
174 return copy_to_user(ifr->ifr_data, &config, sizeof(config)) ?
175 -EFAULT : 0;
176 }
177
178 int sja1105_get_ts_info(struct dsa_switch *ds, int port,
179 struct ethtool_ts_info *info)
180 {
181 struct sja1105_private *priv = ds->priv;
182 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
183
184 /* Called during cleanup */
185 if (!ptp_data->clock)
186 return -ENODEV;
187
188 info->so_timestamping = SOF_TIMESTAMPING_TX_HARDWARE |
189 SOF_TIMESTAMPING_RX_HARDWARE |
190 SOF_TIMESTAMPING_RAW_HARDWARE;
191 info->tx_types = (1 << HWTSTAMP_TX_OFF) |
192 (1 << HWTSTAMP_TX_ON);
193 info->rx_filters = (1 << HWTSTAMP_FILTER_NONE) |
194 (1 << HWTSTAMP_FILTER_PTP_V2_L2_EVENT);
195 info->phc_index = ptp_clock_index(ptp_data->clock);
196 return 0;
197 }
198
199 void sja1105et_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
200 enum packing_op op)
201 {
202 const int size = SJA1105_SIZE_PTP_CMD;
203 /* No need to keep this as part of the structure */
204 u64 valid = 1;
205
206 sja1105_packing(buf, &valid, 31, 31, size, op);
207 sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
208 sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
209 sja1105_packing(buf, &cmd->resptp, 2, 2, size, op);
210 sja1105_packing(buf, &cmd->corrclk4ts, 1, 1, size, op);
211 sja1105_packing(buf, &cmd->ptpclkadd, 0, 0, size, op);
212 }
213
214 void sja1105pqrs_ptp_cmd_packing(u8 *buf, struct sja1105_ptp_cmd *cmd,
215 enum packing_op op)
216 {
217 const int size = SJA1105_SIZE_PTP_CMD;
218 /* No need to keep this as part of the structure */
219 u64 valid = 1;
220
221 sja1105_packing(buf, &valid, 31, 31, size, op);
222 sja1105_packing(buf, &cmd->ptpstrtsch, 30, 30, size, op);
223 sja1105_packing(buf, &cmd->ptpstopsch, 29, 29, size, op);
224 sja1105_packing(buf, &cmd->resptp, 3, 3, size, op);
225 sja1105_packing(buf, &cmd->corrclk4ts, 2, 2, size, op);
226 sja1105_packing(buf, &cmd->ptpclkadd, 0, 0, size, op);
227 }
228
229 int sja1105_ptp_commit(struct dsa_switch *ds, struct sja1105_ptp_cmd *cmd,
230 sja1105_spi_rw_mode_t rw)
231 {
232 const struct sja1105_private *priv = ds->priv;
233 const struct sja1105_regs *regs = priv->info->regs;
234 u8 buf[SJA1105_SIZE_PTP_CMD] = {0};
235 int rc;
236
237 if (rw == SPI_WRITE)
238 priv->info->ptp_cmd_packing(buf, cmd, PACK);
239
240 rc = sja1105_xfer_buf(priv, rw, regs->ptp_control, buf,
241 SJA1105_SIZE_PTP_CMD);
242
243 if (rw == SPI_READ)
244 priv->info->ptp_cmd_packing(buf, cmd, UNPACK);
245
246 return rc;
247 }
248
249 /* The switch returns partial timestamps (24 bits for SJA1105 E/T, which wrap
250 * around in 0.135 seconds, and 32 bits for P/Q/R/S, wrapping around in 34.35
251 * seconds).
252 *
253 * This receives the RX or TX MAC timestamps, provided by hardware as
254 * the lower bits of the cycle counter, sampled at the time the timestamp was
255 * collected.
256 *
257 * To reconstruct into a full 64-bit-wide timestamp, the cycle counter is
258 * read and the high-order bits are filled in.
259 *
260 * Must be called within one wraparound period of the partial timestamp since
261 * it was generated by the MAC.
262 */
263 static u64 sja1105_tstamp_reconstruct(struct dsa_switch *ds, u64 now,
264 u64 ts_partial)
265 {
266 struct sja1105_private *priv = ds->priv;
267 u64 partial_tstamp_mask = CYCLECOUNTER_MASK(priv->info->ptp_ts_bits);
268 u64 ts_reconstructed;
269
270 ts_reconstructed = (now & ~partial_tstamp_mask) | ts_partial;
271
272 /* Check lower bits of current cycle counter against the timestamp.
273 * If the current cycle counter is lower than the partial timestamp,
274 * then wraparound surely occurred and must be accounted for.
275 */
276 if ((now & partial_tstamp_mask) <= ts_partial)
277 ts_reconstructed -= (partial_tstamp_mask + 1);
278
279 return ts_reconstructed;
280 }
281
282 /* Reads the SPI interface for an egress timestamp generated by the switch
283 * for frames sent using management routes.
284 *
285 * SJA1105 E/T layout of the 4-byte SPI payload:
286 *
287 * 31 23 15 7 0
288 * | | | | |
289 * +-----+-----+-----+ ^
290 * ^ |
291 * | |
292 * 24-bit timestamp Update bit
293 *
294 *
295 * SJA1105 P/Q/R/S layout of the 8-byte SPI payload:
296 *
297 * 31 23 15 7 0 63 55 47 39 32
298 * | | | | | | | | | |
299 * ^ +-----+-----+-----+-----+
300 * | ^
301 * | |
302 * Update bit 32-bit timestamp
303 *
304 * Notice that the update bit is in the same place.
305 * To have common code for E/T and P/Q/R/S for reading the timestamp,
306 * we need to juggle with the offset and the bit indices.
307 */
308 static int sja1105_ptpegr_ts_poll(struct dsa_switch *ds, int port, u64 *ts)
309 {
310 struct sja1105_private *priv = ds->priv;
311 const struct sja1105_regs *regs = priv->info->regs;
312 int tstamp_bit_start, tstamp_bit_end;
313 int timeout = 10;
314 u8 packed_buf[8];
315 u64 update;
316 int rc;
317
318 do {
319 rc = sja1105_xfer_buf(priv, SPI_READ, regs->ptpegr_ts[port],
320 packed_buf, priv->info->ptpegr_ts_bytes);
321 if (rc < 0)
322 return rc;
323
324 sja1105_unpack(packed_buf, &update, 0, 0,
325 priv->info->ptpegr_ts_bytes);
326 if (update)
327 break;
328
329 usleep_range(10, 50);
330 } while (--timeout);
331
332 if (!timeout)
333 return -ETIMEDOUT;
334
335 /* Point the end bit to the second 32-bit word on P/Q/R/S,
336 * no-op on E/T.
337 */
338 tstamp_bit_end = (priv->info->ptpegr_ts_bytes - 4) * 8;
339 /* Shift the 24-bit timestamp on E/T to be collected from 31:8.
340 * No-op on P/Q/R/S.
341 */
342 tstamp_bit_end += 32 - priv->info->ptp_ts_bits;
343 tstamp_bit_start = tstamp_bit_end + priv->info->ptp_ts_bits - 1;
344
345 *ts = 0;
346
347 sja1105_unpack(packed_buf, ts, tstamp_bit_start, tstamp_bit_end,
348 priv->info->ptpegr_ts_bytes);
349
350 return 0;
351 }
352
353 /* Caller must hold ptp_data->lock */
354 static int sja1105_ptpclkval_read(struct sja1105_private *priv, u64 *ticks,
355 struct ptp_system_timestamp *ptp_sts)
356 {
357 const struct sja1105_regs *regs = priv->info->regs;
358
359 return sja1105_xfer_u64(priv, SPI_READ, regs->ptpclkval, ticks,
360 ptp_sts);
361 }
362
363 /* Caller must hold ptp_data->lock */
364 static int sja1105_ptpclkval_write(struct sja1105_private *priv, u64 ticks,
365 struct ptp_system_timestamp *ptp_sts)
366 {
367 const struct sja1105_regs *regs = priv->info->regs;
368
369 return sja1105_xfer_u64(priv, SPI_WRITE, regs->ptpclkval, &ticks,
370 ptp_sts);
371 }
372
373 static long sja1105_rxtstamp_work(struct ptp_clock_info *ptp)
374 {
375 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
376 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
377 struct dsa_switch *ds = priv->ds;
378 struct sk_buff *skb;
379
380 mutex_lock(&ptp_data->lock);
381
382 while ((skb = skb_dequeue(&ptp_data->skb_rxtstamp_queue)) != NULL) {
383 struct skb_shared_hwtstamps *shwt = skb_hwtstamps(skb);
384 u64 ticks, ts;
385 int rc;
386
387 rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
388 if (rc < 0) {
389 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
390 kfree_skb(skb);
391 continue;
392 }
393
394 *shwt = (struct skb_shared_hwtstamps) {0};
395
396 ts = SJA1105_SKB_CB(skb)->meta_tstamp;
397 ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
398
399 shwt->hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
400 netif_rx_ni(skb);
401 }
402
403 mutex_unlock(&ptp_data->lock);
404
405 /* Don't restart */
406 return -1;
407 }
408
409 /* Called from dsa_skb_defer_rx_timestamp */
410 bool sja1105_port_rxtstamp(struct dsa_switch *ds, int port,
411 struct sk_buff *skb, unsigned int type)
412 {
413 struct sja1105_private *priv = ds->priv;
414 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
415
416 if (!test_bit(SJA1105_HWTS_RX_EN, &priv->tagger_data.state))
417 return false;
418
419 /* We need to read the full PTP clock to reconstruct the Rx
420 * timestamp. For that we need a sleepable context.
421 */
422 skb_queue_tail(&ptp_data->skb_rxtstamp_queue, skb);
423 ptp_schedule_worker(ptp_data->clock, 0);
424 return true;
425 }
426
427 /* Called from dsa_skb_tx_timestamp. This callback is just to make DSA clone
428 * the skb and have it available in DSA_SKB_CB in the .port_deferred_xmit
429 * callback, where we will timestamp it synchronously.
430 */
431 bool sja1105_port_txtstamp(struct dsa_switch *ds, int port,
432 struct sk_buff *skb, unsigned int type)
433 {
434 struct sja1105_private *priv = ds->priv;
435 struct sja1105_port *sp = &priv->ports[port];
436
437 if (!sp->hwts_tx_en)
438 return false;
439
440 return true;
441 }
442
443 static int sja1105_ptp_reset(struct dsa_switch *ds)
444 {
445 struct sja1105_private *priv = ds->priv;
446 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
447 struct sja1105_ptp_cmd cmd = ptp_data->cmd;
448 int rc;
449
450 mutex_lock(&ptp_data->lock);
451
452 cmd.resptp = 1;
453
454 dev_dbg(ds->dev, "Resetting PTP clock\n");
455 rc = sja1105_ptp_commit(ds, &cmd, SPI_WRITE);
456
457 sja1105_tas_clockstep(priv->ds);
458
459 mutex_unlock(&ptp_data->lock);
460
461 return rc;
462 }
463
464 /* Caller must hold ptp_data->lock */
465 int __sja1105_ptp_gettimex(struct dsa_switch *ds, u64 *ns,
466 struct ptp_system_timestamp *ptp_sts)
467 {
468 struct sja1105_private *priv = ds->priv;
469 u64 ticks;
470 int rc;
471
472 rc = sja1105_ptpclkval_read(priv, &ticks, ptp_sts);
473 if (rc < 0) {
474 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
475 return rc;
476 }
477
478 *ns = sja1105_ticks_to_ns(ticks);
479
480 return 0;
481 }
482
483 static int sja1105_ptp_gettimex(struct ptp_clock_info *ptp,
484 struct timespec64 *ts,
485 struct ptp_system_timestamp *ptp_sts)
486 {
487 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
488 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
489 u64 now = 0;
490 int rc;
491
492 mutex_lock(&ptp_data->lock);
493
494 rc = __sja1105_ptp_gettimex(priv->ds, &now, ptp_sts);
495 *ts = ns_to_timespec64(now);
496
497 mutex_unlock(&ptp_data->lock);
498
499 return rc;
500 }
501
502 /* Caller must hold ptp_data->lock */
503 static int sja1105_ptp_mode_set(struct sja1105_private *priv,
504 enum sja1105_ptp_clk_mode mode)
505 {
506 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
507
508 if (ptp_data->cmd.ptpclkadd == mode)
509 return 0;
510
511 ptp_data->cmd.ptpclkadd = mode;
512
513 return sja1105_ptp_commit(priv->ds, &ptp_data->cmd, SPI_WRITE);
514 }
515
516 /* Write to PTPCLKVAL while PTPCLKADD is 0 */
517 int __sja1105_ptp_settime(struct dsa_switch *ds, u64 ns,
518 struct ptp_system_timestamp *ptp_sts)
519 {
520 struct sja1105_private *priv = ds->priv;
521 u64 ticks = ns_to_sja1105_ticks(ns);
522 int rc;
523
524 rc = sja1105_ptp_mode_set(priv, PTP_SET_MODE);
525 if (rc < 0) {
526 dev_err(priv->ds->dev, "Failed to put PTPCLK in set mode\n");
527 return rc;
528 }
529
530 rc = sja1105_ptpclkval_write(priv, ticks, ptp_sts);
531
532 sja1105_tas_clockstep(priv->ds);
533
534 return rc;
535 }
536
537 static int sja1105_ptp_settime(struct ptp_clock_info *ptp,
538 const struct timespec64 *ts)
539 {
540 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
541 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
542 u64 ns = timespec64_to_ns(ts);
543 int rc;
544
545 mutex_lock(&ptp_data->lock);
546
547 rc = __sja1105_ptp_settime(priv->ds, ns, NULL);
548
549 mutex_unlock(&ptp_data->lock);
550
551 return rc;
552 }
553
554 static int sja1105_ptp_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)
555 {
556 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
557 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
558 const struct sja1105_regs *regs = priv->info->regs;
559 u32 clkrate32;
560 s64 clkrate;
561 int rc;
562
563 clkrate = (s64)scaled_ppm * SJA1105_CC_MULT_NUM;
564 clkrate = div_s64(clkrate, SJA1105_CC_MULT_DEM);
565
566 /* Take a +/- value and re-center it around 2^31. */
567 clkrate = SJA1105_CC_MULT + clkrate;
568 WARN_ON(abs(clkrate) >= GENMASK_ULL(31, 0));
569 clkrate32 = clkrate;
570
571 mutex_lock(&ptp_data->lock);
572
573 rc = sja1105_xfer_u32(priv, SPI_WRITE, regs->ptpclkrate, &clkrate32,
574 NULL);
575
576 sja1105_tas_adjfreq(priv->ds);
577
578 mutex_unlock(&ptp_data->lock);
579
580 return rc;
581 }
582
583 /* Write to PTPCLKVAL while PTPCLKADD is 1 */
584 int __sja1105_ptp_adjtime(struct dsa_switch *ds, s64 delta)
585 {
586 struct sja1105_private *priv = ds->priv;
587 s64 ticks = ns_to_sja1105_ticks(delta);
588 int rc;
589
590 rc = sja1105_ptp_mode_set(priv, PTP_ADD_MODE);
591 if (rc < 0) {
592 dev_err(priv->ds->dev, "Failed to put PTPCLK in add mode\n");
593 return rc;
594 }
595
596 rc = sja1105_ptpclkval_write(priv, ticks, NULL);
597
598 sja1105_tas_clockstep(priv->ds);
599
600 return rc;
601 }
602
603 static int sja1105_ptp_adjtime(struct ptp_clock_info *ptp, s64 delta)
604 {
605 struct sja1105_ptp_data *ptp_data = ptp_caps_to_data(ptp);
606 struct sja1105_private *priv = ptp_data_to_sja1105(ptp_data);
607 int rc;
608
609 mutex_lock(&ptp_data->lock);
610
611 rc = __sja1105_ptp_adjtime(priv->ds, delta);
612
613 mutex_unlock(&ptp_data->lock);
614
615 return rc;
616 }
617
618 int sja1105_ptp_clock_register(struct dsa_switch *ds)
619 {
620 struct sja1105_private *priv = ds->priv;
621 struct sja1105_tagger_data *tagger_data = &priv->tagger_data;
622 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
623
624 ptp_data->caps = (struct ptp_clock_info) {
625 .owner = THIS_MODULE,
626 .name = "SJA1105 PHC",
627 .adjfine = sja1105_ptp_adjfine,
628 .adjtime = sja1105_ptp_adjtime,
629 .gettimex64 = sja1105_ptp_gettimex,
630 .settime64 = sja1105_ptp_settime,
631 .do_aux_work = sja1105_rxtstamp_work,
632 .max_adj = SJA1105_MAX_ADJ_PPB,
633 };
634
635 skb_queue_head_init(&ptp_data->skb_rxtstamp_queue);
636 spin_lock_init(&tagger_data->meta_lock);
637
638 ptp_data->clock = ptp_clock_register(&ptp_data->caps, ds->dev);
639 if (IS_ERR_OR_NULL(ptp_data->clock))
640 return PTR_ERR(ptp_data->clock);
641
642 ptp_data->cmd.corrclk4ts = true;
643 ptp_data->cmd.ptpclkadd = PTP_SET_MODE;
644
645 return sja1105_ptp_reset(ds);
646 }
647
648 void sja1105_ptp_clock_unregister(struct dsa_switch *ds)
649 {
650 struct sja1105_private *priv = ds->priv;
651 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
652
653 if (IS_ERR_OR_NULL(ptp_data->clock))
654 return;
655
656 ptp_cancel_worker_sync(ptp_data->clock);
657 skb_queue_purge(&ptp_data->skb_rxtstamp_queue);
658 ptp_clock_unregister(ptp_data->clock);
659 ptp_data->clock = NULL;
660 }
661
662 void sja1105_ptp_txtstamp_skb(struct dsa_switch *ds, int port,
663 struct sk_buff *skb)
664 {
665 struct sja1105_private *priv = ds->priv;
666 struct sja1105_ptp_data *ptp_data = &priv->ptp_data;
667 struct skb_shared_hwtstamps shwt = {0};
668 u64 ticks, ts;
669 int rc;
670
671 skb_shinfo(skb)->tx_flags |= SKBTX_IN_PROGRESS;
672
673 mutex_lock(&ptp_data->lock);
674
675 rc = sja1105_ptpclkval_read(priv, &ticks, NULL);
676 if (rc < 0) {
677 dev_err(ds->dev, "Failed to read PTP clock: %d\n", rc);
678 kfree_skb(skb);
679 goto out;
680 }
681
682 rc = sja1105_ptpegr_ts_poll(ds, port, &ts);
683 if (rc < 0) {
684 dev_err(ds->dev, "timed out polling for tstamp\n");
685 kfree_skb(skb);
686 goto out;
687 }
688
689 ts = sja1105_tstamp_reconstruct(ds, ticks, ts);
690
691 shwt.hwtstamp = ns_to_ktime(sja1105_ticks_to_ns(ts));
692 skb_complete_tx_timestamp(skb, &shwt);
693
694 out:
695 mutex_unlock(&ptp_data->lock);
696 }
Linux with added FPC-III support