1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33#include <linux/ip.h>
34#include <linux/udp.h>
35#include <linux/time.h>
36#include <linux/ktime.h>
37#include <linux/module.h>
38#include <linux/pps_kernel.h>
39#include <linux/ptp_clock_kernel.h>
40#include "net_driver.h"
41#include "efx.h"
42#include "mcdi.h"
43#include "mcdi_pcol.h"
44#include "io.h"
45#include "farch_regs.h"
46#include "tx.h"
47#include "nic.h"
48
49
50#define MAX_EVENT_FRAGS 3
51
52
53#define MAX_SYNCHRONISE_WAIT_MS 2
54
55
56#define SYNCHRONISE_PERIOD_NS 250000
57
58
59#define SYNCHRONISATION_GRANULARITY_NS 200
60
61
62#define DEFAULT_MIN_SYNCHRONISATION_NS 120
63
64
65#define MAX_SYNCHRONISATION_NS 1000
66
67
68#define MAX_RECEIVE_EVENTS 8
69
70
71#define AVERAGE_LENGTH 16
72
73
74#define PKT_EVENT_LIFETIME_MS 10
75
76
77
78
79
80#define PTP_DPORT_OFFSET 22
81
82#define PTP_V1_VERSION_LENGTH 2
83#define PTP_V1_VERSION_OFFSET 28
84
85#define PTP_V1_UUID_LENGTH 6
86#define PTP_V1_UUID_OFFSET 50
87
88#define PTP_V1_SEQUENCE_LENGTH 2
89#define PTP_V1_SEQUENCE_OFFSET 58
90
91
92
93
94#define PTP_V1_MIN_LENGTH 64
95
96#define PTP_V2_VERSION_LENGTH 1
97#define PTP_V2_VERSION_OFFSET 29
98
99#define PTP_V2_UUID_LENGTH 8
100#define PTP_V2_UUID_OFFSET 48
101
102
103
104
105
106
107#define PTP_V2_MC_UUID_LENGTH 6
108#define PTP_V2_MC_UUID_OFFSET 50
109
110#define PTP_V2_SEQUENCE_LENGTH 2
111#define PTP_V2_SEQUENCE_OFFSET 58
112
113
114
115
116#define PTP_V2_MIN_LENGTH 63
117
118#define PTP_MIN_LENGTH 63
119
120#define PTP_ADDRESS 0xe0000181
121#define PTP_EVENT_PORT 319
122#define PTP_GENERAL_PORT 320
123
124
125
126
127#define PTP_VERSION_V1 1
128
129#define PTP_VERSION_V2 2
130#define PTP_VERSION_V2_MASK 0x0f
131
132enum ptp_packet_state {
133 PTP_PACKET_STATE_UNMATCHED = 0,
134 PTP_PACKET_STATE_MATCHED,
135 PTP_PACKET_STATE_TIMED_OUT,
136 PTP_PACKET_STATE_MATCH_UNWANTED
137};
138
139
140
141
142#define MC_NANOSECOND_BITS 30
143#define MC_NANOSECOND_MASK ((1 << MC_NANOSECOND_BITS) - 1)
144#define MC_SECOND_MASK ((1 << (32 - MC_NANOSECOND_BITS)) - 1)
145
146
147#define MAX_PPB 1000000
148
149
150
151#define PPB_SCALE_WORD ((1LL << (57)) / 1953125LL)
152
153
154#define PPB_SHIFT_FP40 26
155
156#define PPB_SHIFT_FP44 22
157
158#define PTP_SYNC_ATTEMPTS 4
159
160
161
162
163
164
165
166
167
168struct efx_ptp_match {
169 u32 words[DIV_ROUND_UP(PTP_V1_UUID_LENGTH, 4)];
170 unsigned long expiry;
171 enum ptp_packet_state state;
172};
173
174
175
176
177
178
179
180
181
182struct efx_ptp_event_rx {
183 struct list_head link;
184 u32 seq0;
185 u32 seq1;
186 ktime_t hwtimestamp;
187 unsigned long expiry;
188};
189
190
191
192
193
194
195
196
197
198
199
200
201struct efx_ptp_timeset {
202 u32 host_start;
203 u32 major;
204 u32 minor;
205 u32 host_end;
206 u32 wait;
207 u32 window;
208};
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284struct efx_ptp_data {
285 struct efx_nic *efx;
286 struct efx_channel *channel;
287 bool rx_ts_inline;
288 struct sk_buff_head rxq;
289 struct sk_buff_head txq;
290 struct list_head evt_list;
291 struct list_head evt_free_list;
292 spinlock_t evt_lock;
293 struct efx_ptp_event_rx rx_evts[MAX_RECEIVE_EVENTS];
294 struct workqueue_struct *workwq;
295 struct work_struct work;
296 bool reset_required;
297 u32 rxfilter_event;
298 u32 rxfilter_general;
299 bool rxfilter_installed;
300 struct hwtstamp_config config;
301 bool enabled;
302 unsigned int mode;
303 void (*ns_to_nic_time)(s64 ns, u32 *nic_major, u32 *nic_minor);
304 ktime_t (*nic_to_kernel_time)(u32 nic_major, u32 nic_minor,
305 s32 correction);
306 struct {
307 u32 minor_max;
308 u32 sync_event_diff_min;
309 u32 sync_event_diff_max;
310 unsigned int sync_event_minor_shift;
311 } nic_time;
312 unsigned int min_synchronisation_ns;
313 unsigned int capabilities;
314 struct {
315 s32 ptp_tx;
316 s32 ptp_rx;
317 s32 pps_out;
318 s32 pps_in;
319 s32 general_tx;
320 s32 general_rx;
321 } ts_corrections;
322 efx_qword_t evt_frags[MAX_EVENT_FRAGS];
323 int evt_frag_idx;
324 int evt_code;
325 struct efx_buffer start;
326 struct pps_event_time host_time_pps;
327 unsigned int adjfreq_ppb_shift;
328 s64 current_adjfreq;
329 struct ptp_clock *phc_clock;
330 struct ptp_clock_info phc_clock_info;
331 struct work_struct pps_work;
332 struct workqueue_struct *pps_workwq;
333 bool nic_ts_enabled;
334 efx_dword_t txbuf[MCDI_TX_BUF_LEN(MC_CMD_PTP_IN_TRANSMIT_LENMAX)];
335
336 unsigned int good_syncs;
337 unsigned int fast_syncs;
338 unsigned int bad_syncs;
339 unsigned int sync_timeouts;
340 unsigned int no_time_syncs;
341 unsigned int invalid_sync_windows;
342 unsigned int undersize_sync_windows;
343 unsigned int oversize_sync_windows;
344 unsigned int rx_no_timestamp;
345 struct efx_ptp_timeset
346 timeset[MC_CMD_PTP_OUT_SYNCHRONIZE_TIMESET_MAXNUM];
347 void (*xmit_skb)(struct efx_nic *efx, struct sk_buff *skb);
348};
349
350static int efx_phc_adjfreq(struct ptp_clock_info *ptp, s32 delta);
351static int efx_phc_adjtime(struct ptp_clock_info *ptp, s64 delta);
352static int efx_phc_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts);
353static int efx_phc_settime(struct ptp_clock_info *ptp,
354 const struct timespec64 *e_ts);
355static int efx_phc_enable(struct ptp_clock_info *ptp,
356 struct ptp_clock_request *request, int on);
357
358bool efx_ptp_use_mac_tx_timestamps(struct efx_nic *efx)
359{
360 return efx_has_cap(efx, TX_MAC_TIMESTAMPING);
361}
362
363
364
365
366static bool efx_ptp_want_txqs(struct efx_channel *channel)
367{
368 return efx_ptp_use_mac_tx_timestamps(channel->efx);
369}
370
371#define PTP_SW_STAT(ext_name, field_name) \
372 { #ext_name, 0, offsetof(struct efx_ptp_data, field_name) }
373#define PTP_MC_STAT(ext_name, mcdi_name) \
374 { #ext_name, 32, MC_CMD_PTP_OUT_STATUS_STATS_ ## mcdi_name ## _OFST }
375static const struct efx_hw_stat_desc efx_ptp_stat_desc[] = {
376 PTP_SW_STAT(ptp_good_syncs, good_syncs),
377 PTP_SW_STAT(ptp_fast_syncs, fast_syncs),
378 PTP_SW_STAT(ptp_bad_syncs, bad_syncs),
379 PTP_SW_STAT(ptp_sync_timeouts, sync_timeouts),
380 PTP_SW_STAT(ptp_no_time_syncs, no_time_syncs),
381 PTP_SW_STAT(ptp_invalid_sync_windows, invalid_sync_windows),
382 PTP_SW_STAT(ptp_undersize_sync_windows, undersize_sync_windows),
383 PTP_SW_STAT(ptp_oversize_sync_windows, oversize_sync_windows),
384 PTP_SW_STAT(ptp_rx_no_timestamp, rx_no_timestamp),
385 PTP_MC_STAT(ptp_tx_timestamp_packets, TX),
386 PTP_MC_STAT(ptp_rx_timestamp_packets, RX),
387 PTP_MC_STAT(ptp_timestamp_packets, TS),
388 PTP_MC_STAT(ptp_filter_matches, FM),
389 PTP_MC_STAT(ptp_non_filter_matches, NFM),
390};
391#define PTP_STAT_COUNT ARRAY_SIZE(efx_ptp_stat_desc)
392static const unsigned long efx_ptp_stat_mask[] = {
393 [0 ... BITS_TO_LONGS(PTP_STAT_COUNT) - 1] = ~0UL,
394};
395
396size_t efx_ptp_describe_stats(struct efx_nic *efx, u8 *strings)
397{
398 if (!efx->ptp_data)
399 return 0;
400
401 return efx_nic_describe_stats(efx_ptp_stat_desc, PTP_STAT_COUNT,
402 efx_ptp_stat_mask, strings);
403}
404
405size_t efx_ptp_update_stats(struct efx_nic *efx, u64 *stats)
406{
407 MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_STATUS_LEN);
408 MCDI_DECLARE_BUF(outbuf, MC_CMD_PTP_OUT_STATUS_LEN);
409 size_t i;
410 int rc;
411
412 if (!efx->ptp_data)
413 return 0;
414
415
416 for (i = 0; i < PTP_STAT_COUNT; i++) {
417 if (efx_ptp_stat_desc[i].dma_width)
418 continue;
419 stats[i] = *(unsigned int *)((char *)efx->ptp_data +
420 efx_ptp_stat_desc[i].offset);
421 }
422
423
424
425
426
427 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_STATUS);
428 MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);
429 rc = efx_mcdi_rpc(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
430 outbuf, sizeof(outbuf), NULL);
431 if (rc)
432 memset(outbuf, 0, sizeof(outbuf));
433 efx_nic_update_stats(efx_ptp_stat_desc, PTP_STAT_COUNT,
434 efx_ptp_stat_mask,
435 stats, _MCDI_PTR(outbuf, 0), false);
436
437 return PTP_STAT_COUNT;
438}
439
440
441static void efx_ptp_ns_to_s_ns(s64 ns, u32 *nic_major, u32 *nic_minor)
442{
443 struct timespec64 ts = ns_to_timespec64(ns);
444 *nic_major = (u32)ts.tv_sec;
445 *nic_minor = ts.tv_nsec;
446}
447
448static ktime_t efx_ptp_s_ns_to_ktime_correction(u32 nic_major, u32 nic_minor,
449 s32 correction)
450{
451 ktime_t kt = ktime_set(nic_major, nic_minor);
452 if (correction >= 0)
453 kt = ktime_add_ns(kt, (u64)correction);
454 else
455 kt = ktime_sub_ns(kt, (u64)-correction);
456 return kt;
457}
458
459
460
461
462
463#define S27_TO_NS_SHIFT (27)
464#define NS_TO_S27_MULT (((1ULL << 63) + NSEC_PER_SEC / 2) / NSEC_PER_SEC)
465#define NS_TO_S27_SHIFT (63 - S27_TO_NS_SHIFT)
466#define S27_MINOR_MAX (1 << S27_TO_NS_SHIFT)
467
468
469
470
471static void efx_ptp_ns_to_s27(s64 ns, u32 *nic_major, u32 *nic_minor)
472{
473 struct timespec64 ts = ns_to_timespec64(ns);
474 u32 maj = (u32)ts.tv_sec;
475 u32 min = (u32)(((u64)ts.tv_nsec * NS_TO_S27_MULT +
476 (1ULL << (NS_TO_S27_SHIFT - 1))) >> NS_TO_S27_SHIFT);
477
478
479
480
481 if (min >= S27_MINOR_MAX) {
482 min -= S27_MINOR_MAX;
483 maj++;
484 }
485
486 *nic_major = maj;
487 *nic_minor = min;
488}
489
490static inline ktime_t efx_ptp_s27_to_ktime(u32 nic_major, u32 nic_minor)
491{
492 u32 ns = (u32)(((u64)nic_minor * NSEC_PER_SEC +
493 (1ULL << (S27_TO_NS_SHIFT - 1))) >> S27_TO_NS_SHIFT);
494 return ktime_set(nic_major, ns);
495}
496
497static ktime_t efx_ptp_s27_to_ktime_correction(u32 nic_major, u32 nic_minor,
498 s32 correction)
499{
500
501 nic_minor += correction;
502 if ((s32)nic_minor < 0) {
503 nic_minor += S27_MINOR_MAX;
504 nic_major--;
505 } else if (nic_minor >= S27_MINOR_MAX) {
506 nic_minor -= S27_MINOR_MAX;
507 nic_major++;
508 }
509
510 return efx_ptp_s27_to_ktime(nic_major, nic_minor);
511}
512
513
514static void efx_ptp_ns_to_s_qns(s64 ns, u32 *nic_major, u32 *nic_minor)
515{
516 struct timespec64 ts = ns_to_timespec64(ns);
517
518 *nic_major = (u32)ts.tv_sec;
519 *nic_minor = ts.tv_nsec * 4;
520}
521
522static ktime_t efx_ptp_s_qns_to_ktime_correction(u32 nic_major, u32 nic_minor,
523 s32 correction)
524{
525 ktime_t kt;
526
527 nic_minor = DIV_ROUND_CLOSEST(nic_minor, 4);
528 correction = DIV_ROUND_CLOSEST(correction, 4);
529
530 kt = ktime_set(nic_major, nic_minor);
531
532 if (correction >= 0)
533 kt = ktime_add_ns(kt, (u64)correction);
534 else
535 kt = ktime_sub_ns(kt, (u64)-correction);
536 return kt;
537}
538
539struct efx_channel *efx_ptp_channel(struct efx_nic *efx)
540{
541 return efx->ptp_data ? efx->ptp_data->channel : NULL;
542}
543
544static u32 last_sync_timestamp_major(struct efx_nic *efx)
545{
546 struct efx_channel *channel = efx_ptp_channel(efx);
547 u32 major = 0;
548
549 if (channel)
550 major = channel->sync_timestamp_major;
551 return major;
552}
553
554
555
556
557static ktime_t
558efx_ptp_mac_nic_to_ktime_correction(struct efx_nic *efx,
559 struct efx_ptp_data *ptp,
560 u32 nic_major, u32 nic_minor,
561 s32 correction)
562{
563 u32 sync_timestamp;
564 ktime_t kt = { 0 };
565 s16 delta;
566
567 if (!(nic_major & 0x80000000)) {
568 WARN_ON_ONCE(nic_major >> 16);
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588 sync_timestamp = last_sync_timestamp_major(efx);
589
590
591
592
593
594
595
596 delta = nic_major - sync_timestamp;
597
598
599
600
601 nic_major = sync_timestamp + delta;
602
603 kt = ptp->nic_to_kernel_time(nic_major, nic_minor,
604 correction);
605 }
606 return kt;
607}
608
609ktime_t efx_ptp_nic_to_kernel_time(struct efx_tx_queue *tx_queue)
610{
611 struct efx_nic *efx = tx_queue->efx;
612 struct efx_ptp_data *ptp = efx->ptp_data;
613 ktime_t kt;
614
615 if (efx_ptp_use_mac_tx_timestamps(efx))
616 kt = efx_ptp_mac_nic_to_ktime_correction(efx, ptp,
617 tx_queue->completed_timestamp_major,
618 tx_queue->completed_timestamp_minor,
619 ptp->ts_corrections.general_tx);
620 else
621 kt = ptp->nic_to_kernel_time(
622 tx_queue->completed_timestamp_major,
623 tx_queue->completed_timestamp_minor,
624 ptp->ts_corrections.general_tx);
625 return kt;
626}
627
628
629static int efx_ptp_get_attributes(struct efx_nic *efx)
630{
631 MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_GET_ATTRIBUTES_LEN);
632 MCDI_DECLARE_BUF(outbuf, MC_CMD_PTP_OUT_GET_ATTRIBUTES_LEN);
633 struct efx_ptp_data *ptp = efx->ptp_data;
634 int rc;
635 u32 fmt;
636 size_t out_len;
637
638
639
640
641
642 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_GET_ATTRIBUTES);
643 MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);
644 rc = efx_mcdi_rpc_quiet(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
645 outbuf, sizeof(outbuf), &out_len);
646 if (rc == 0) {
647 fmt = MCDI_DWORD(outbuf, PTP_OUT_GET_ATTRIBUTES_TIME_FORMAT);
648 } else if (rc == -EINVAL) {
649 fmt = MC_CMD_PTP_OUT_GET_ATTRIBUTES_SECONDS_NANOSECONDS;
650 } else if (rc == -EPERM) {
651 netif_info(efx, probe, efx->net_dev, "no PTP support\n");
652 return rc;
653 } else {
654 efx_mcdi_display_error(efx, MC_CMD_PTP, sizeof(inbuf),
655 outbuf, sizeof(outbuf), rc);
656 return rc;
657 }
658
659 switch (fmt) {
660 case MC_CMD_PTP_OUT_GET_ATTRIBUTES_SECONDS_27FRACTION:
661 ptp->ns_to_nic_time = efx_ptp_ns_to_s27;
662 ptp->nic_to_kernel_time = efx_ptp_s27_to_ktime_correction;
663 ptp->nic_time.minor_max = 1 << 27;
664 ptp->nic_time.sync_event_minor_shift = 19;
665 break;
666 case MC_CMD_PTP_OUT_GET_ATTRIBUTES_SECONDS_NANOSECONDS:
667 ptp->ns_to_nic_time = efx_ptp_ns_to_s_ns;
668 ptp->nic_to_kernel_time = efx_ptp_s_ns_to_ktime_correction;
669 ptp->nic_time.minor_max = 1000000000;
670 ptp->nic_time.sync_event_minor_shift = 22;
671 break;
672 case MC_CMD_PTP_OUT_GET_ATTRIBUTES_SECONDS_QTR_NANOSECONDS:
673 ptp->ns_to_nic_time = efx_ptp_ns_to_s_qns;
674 ptp->nic_to_kernel_time = efx_ptp_s_qns_to_ktime_correction;
675 ptp->nic_time.minor_max = 4000000000UL;
676 ptp->nic_time.sync_event_minor_shift = 24;
677 break;
678 default:
679 return -ERANGE;
680 }
681
682
683
684
685
686
687
688 ptp->nic_time.sync_event_diff_min = ptp->nic_time.minor_max
689 - (ptp->nic_time.minor_max / 10);
690 ptp->nic_time.sync_event_diff_max = (ptp->nic_time.minor_max / 4)
691 + (ptp->nic_time.minor_max / 10);
692
693
694
695
696
697
698
699
700 if (rc == 0 &&
701 out_len >= MC_CMD_PTP_OUT_GET_ATTRIBUTES_CAPABILITIES_OFST)
702 ptp->min_synchronisation_ns =
703 MCDI_DWORD(outbuf,
704 PTP_OUT_GET_ATTRIBUTES_SYNC_WINDOW_MIN);
705 else
706 ptp->min_synchronisation_ns = DEFAULT_MIN_SYNCHRONISATION_NS;
707
708 if (rc == 0 &&
709 out_len >= MC_CMD_PTP_OUT_GET_ATTRIBUTES_LEN)
710 ptp->capabilities = MCDI_DWORD(outbuf,
711 PTP_OUT_GET_ATTRIBUTES_CAPABILITIES);
712 else
713 ptp->capabilities = 0;
714
715
716
717
718
719 if (ptp->capabilities & (1 << MC_CMD_PTP_OUT_GET_ATTRIBUTES_FP44_FREQ_ADJ_LBN))
720 ptp->adjfreq_ppb_shift = PPB_SHIFT_FP44;
721 else
722 ptp->adjfreq_ppb_shift = PPB_SHIFT_FP40;
723
724 return 0;
725}
726
727
728static int efx_ptp_get_timestamp_corrections(struct efx_nic *efx)
729{
730 MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_GET_TIMESTAMP_CORRECTIONS_LEN);
731 MCDI_DECLARE_BUF(outbuf, MC_CMD_PTP_OUT_GET_TIMESTAMP_CORRECTIONS_V2_LEN);
732 int rc;
733 size_t out_len;
734
735
736
737
738 MCDI_SET_DWORD(inbuf, PTP_IN_OP,
739 MC_CMD_PTP_OP_GET_TIMESTAMP_CORRECTIONS);
740 MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);
741
742 rc = efx_mcdi_rpc_quiet(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
743 outbuf, sizeof(outbuf), &out_len);
744 if (rc == 0) {
745 efx->ptp_data->ts_corrections.ptp_tx = MCDI_DWORD(outbuf,
746 PTP_OUT_GET_TIMESTAMP_CORRECTIONS_TRANSMIT);
747 efx->ptp_data->ts_corrections.ptp_rx = MCDI_DWORD(outbuf,
748 PTP_OUT_GET_TIMESTAMP_CORRECTIONS_RECEIVE);
749 efx->ptp_data->ts_corrections.pps_out = MCDI_DWORD(outbuf,
750 PTP_OUT_GET_TIMESTAMP_CORRECTIONS_PPS_OUT);
751 efx->ptp_data->ts_corrections.pps_in = MCDI_DWORD(outbuf,
752 PTP_OUT_GET_TIMESTAMP_CORRECTIONS_PPS_IN);
753
754 if (out_len >= MC_CMD_PTP_OUT_GET_TIMESTAMP_CORRECTIONS_V2_LEN) {
755 efx->ptp_data->ts_corrections.general_tx = MCDI_DWORD(
756 outbuf,
757 PTP_OUT_GET_TIMESTAMP_CORRECTIONS_V2_GENERAL_TX);
758 efx->ptp_data->ts_corrections.general_rx = MCDI_DWORD(
759 outbuf,
760 PTP_OUT_GET_TIMESTAMP_CORRECTIONS_V2_GENERAL_RX);
761 } else {
762 efx->ptp_data->ts_corrections.general_tx =
763 efx->ptp_data->ts_corrections.ptp_tx;
764 efx->ptp_data->ts_corrections.general_rx =
765 efx->ptp_data->ts_corrections.ptp_rx;
766 }
767 } else if (rc == -EINVAL) {
768 efx->ptp_data->ts_corrections.ptp_tx = 0;
769 efx->ptp_data->ts_corrections.ptp_rx = 0;
770 efx->ptp_data->ts_corrections.pps_out = 0;
771 efx->ptp_data->ts_corrections.pps_in = 0;
772 efx->ptp_data->ts_corrections.general_tx = 0;
773 efx->ptp_data->ts_corrections.general_rx = 0;
774 } else {
775 efx_mcdi_display_error(efx, MC_CMD_PTP, sizeof(inbuf), outbuf,
776 sizeof(outbuf), rc);
777 return rc;
778 }
779
780 return 0;
781}
782
783
784static int efx_ptp_enable(struct efx_nic *efx)
785{
786 MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_ENABLE_LEN);
787 MCDI_DECLARE_BUF_ERR(outbuf);
788 int rc;
789
790 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_ENABLE);
791 MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);
792 MCDI_SET_DWORD(inbuf, PTP_IN_ENABLE_QUEUE,
793 efx->ptp_data->channel ?
794 efx->ptp_data->channel->channel : 0);
795 MCDI_SET_DWORD(inbuf, PTP_IN_ENABLE_MODE, efx->ptp_data->mode);
796
797 rc = efx_mcdi_rpc_quiet(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
798 outbuf, sizeof(outbuf), NULL);
799 rc = (rc == -EALREADY) ? 0 : rc;
800 if (rc)
801 efx_mcdi_display_error(efx, MC_CMD_PTP,
802 MC_CMD_PTP_IN_ENABLE_LEN,
803 outbuf, sizeof(outbuf), rc);
804 return rc;
805}
806
807
808
809
810
811
812static int efx_ptp_disable(struct efx_nic *efx)
813{
814 MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_DISABLE_LEN);
815 MCDI_DECLARE_BUF_ERR(outbuf);
816 int rc;
817
818 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_DISABLE);
819 MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);
820 rc = efx_mcdi_rpc_quiet(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
821 outbuf, sizeof(outbuf), NULL);
822 rc = (rc == -EALREADY) ? 0 : rc;
823
824
825
826 if (rc == -ENOSYS || rc == -EPERM)
827 netif_info(efx, probe, efx->net_dev, "no PTP support\n");
828 else if (rc)
829 efx_mcdi_display_error(efx, MC_CMD_PTP,
830 MC_CMD_PTP_IN_DISABLE_LEN,
831 outbuf, sizeof(outbuf), rc);
832 return rc;
833}
834
835static void efx_ptp_deliver_rx_queue(struct sk_buff_head *q)
836{
837 struct sk_buff *skb;
838
839 while ((skb = skb_dequeue(q))) {
840 local_bh_disable();
841 netif_receive_skb(skb);
842 local_bh_enable();
843 }
844}
845
846static void efx_ptp_handle_no_channel(struct efx_nic *efx)
847{
848 netif_err(efx, drv, efx->net_dev,
849 "ERROR: PTP requires MSI-X and 1 additional interrupt"
850 "vector. PTP disabled\n");
851}
852
853
854
855
856static void efx_ptp_send_times(struct efx_nic *efx,
857 struct pps_event_time *last_time)
858{
859 struct pps_event_time now;
860 struct timespec64 limit;
861 struct efx_ptp_data *ptp = efx->ptp_data;
862 int *mc_running = ptp->start.addr;
863
864 pps_get_ts(&now);
865 limit = now.ts_real;
866 timespec64_add_ns(&limit, SYNCHRONISE_PERIOD_NS);
867
868
869 while ((timespec64_compare(&now.ts_real, &limit) < 0) &&
870 READ_ONCE(*mc_running)) {
871 struct timespec64 update_time;
872 unsigned int host_time;
873
874
875 update_time = now.ts_real;
876 timespec64_add_ns(&update_time, SYNCHRONISATION_GRANULARITY_NS);
877 do {
878 pps_get_ts(&now);
879 } while ((timespec64_compare(&now.ts_real, &update_time) < 0) &&
880 READ_ONCE(*mc_running));
881
882
883 host_time = (now.ts_real.tv_sec << MC_NANOSECOND_BITS |
884 now.ts_real.tv_nsec);
885
886 efx->type->ptp_write_host_time(efx, host_time);
887 }
888 *last_time = now;
889}
890
891
892static void efx_ptp_read_timeset(MCDI_DECLARE_STRUCT_PTR(data),
893 struct efx_ptp_timeset *timeset)
894{
895 unsigned start_ns, end_ns;
896
897 timeset->host_start = MCDI_DWORD(data, PTP_OUT_SYNCHRONIZE_HOSTSTART);
898 timeset->major = MCDI_DWORD(data, PTP_OUT_SYNCHRONIZE_MAJOR);
899 timeset->minor = MCDI_DWORD(data, PTP_OUT_SYNCHRONIZE_MINOR);
900 timeset->host_end = MCDI_DWORD(data, PTP_OUT_SYNCHRONIZE_HOSTEND),
901 timeset->wait = MCDI_DWORD(data, PTP_OUT_SYNCHRONIZE_WAITNS);
902
903
904 start_ns = timeset->host_start & MC_NANOSECOND_MASK;
905 end_ns = timeset->host_end & MC_NANOSECOND_MASK;
906
907 if (end_ns < start_ns)
908 end_ns += NSEC_PER_SEC;
909
910 timeset->window = end_ns - start_ns;
911}
912
913
914
915
916
917
918
919
920
921static int
922efx_ptp_process_times(struct efx_nic *efx, MCDI_DECLARE_STRUCT_PTR(synch_buf),
923 size_t response_length,
924 const struct pps_event_time *last_time)
925{
926 unsigned number_readings =
927 MCDI_VAR_ARRAY_LEN(response_length,
928 PTP_OUT_SYNCHRONIZE_TIMESET);
929 unsigned i;
930 unsigned ngood = 0;
931 unsigned last_good = 0;
932 struct efx_ptp_data *ptp = efx->ptp_data;
933 u32 last_sec;
934 u32 start_sec;
935 struct timespec64 delta;
936 ktime_t mc_time;
937
938 if (number_readings == 0)
939 return -EAGAIN;
940
941
942
943
944
945
946
947 for (i = 0; i < number_readings; i++) {
948 s32 window, corrected;
949 struct timespec64 wait;
950
951 efx_ptp_read_timeset(
952 MCDI_ARRAY_STRUCT_PTR(synch_buf,
953 PTP_OUT_SYNCHRONIZE_TIMESET, i),
954 &ptp->timeset[i]);
955
956 wait = ktime_to_timespec64(
957 ptp->nic_to_kernel_time(0, ptp->timeset[i].wait, 0));
958 window = ptp->timeset[i].window;
959 corrected = window - wait.tv_nsec;
960
961
962
963
964
965
966
967
968
969
970 if (window < SYNCHRONISATION_GRANULARITY_NS) {
971 ++ptp->invalid_sync_windows;
972 } else if (corrected >= MAX_SYNCHRONISATION_NS) {
973 ++ptp->oversize_sync_windows;
974 } else if (corrected < ptp->min_synchronisation_ns) {
975 ++ptp->undersize_sync_windows;
976 } else {
977 ngood++;
978 last_good = i;
979 }
980 }
981
982 if (ngood == 0) {
983 netif_warn(efx, drv, efx->net_dev,
984 "PTP no suitable synchronisations\n");
985 return -EAGAIN;
986 }
987
988
989
990
991
992
993
994 start_sec = ptp->timeset[last_good].host_start >> MC_NANOSECOND_BITS;
995 last_sec = last_time->ts_real.tv_sec & MC_SECOND_MASK;
996 if (start_sec != last_sec &&
997 ((start_sec + 1) & MC_SECOND_MASK) != last_sec) {
998 netif_warn(efx, hw, efx->net_dev,
999 "PTP bad synchronisation seconds\n");
1000 return -EAGAIN;
1001 }
1002 delta.tv_sec = (last_sec - start_sec) & 1;
1003 delta.tv_nsec =
1004 last_time->ts_real.tv_nsec -
1005 (ptp->timeset[last_good].host_start & MC_NANOSECOND_MASK);
1006
1007
1008
1009
1010
1011 mc_time = ptp->nic_to_kernel_time(ptp->timeset[last_good].major,
1012 ptp->timeset[last_good].minor, 0);
1013
1014
1015 delta.tv_nsec += ktime_to_timespec64(mc_time).tv_nsec;
1016
1017
1018 ptp->host_time_pps = *last_time;
1019 pps_sub_ts(&ptp->host_time_pps, delta);
1020
1021 return 0;
1022}
1023
1024
1025static int efx_ptp_synchronize(struct efx_nic *efx, unsigned int num_readings)
1026{
1027 struct efx_ptp_data *ptp = efx->ptp_data;
1028 MCDI_DECLARE_BUF(synch_buf, MC_CMD_PTP_OUT_SYNCHRONIZE_LENMAX);
1029 size_t response_length;
1030 int rc;
1031 unsigned long timeout;
1032 struct pps_event_time last_time = {};
1033 unsigned int loops = 0;
1034 int *start = ptp->start.addr;
1035
1036 MCDI_SET_DWORD(synch_buf, PTP_IN_OP, MC_CMD_PTP_OP_SYNCHRONIZE);
1037 MCDI_SET_DWORD(synch_buf, PTP_IN_PERIPH_ID, 0);
1038 MCDI_SET_DWORD(synch_buf, PTP_IN_SYNCHRONIZE_NUMTIMESETS,
1039 num_readings);
1040 MCDI_SET_QWORD(synch_buf, PTP_IN_SYNCHRONIZE_START_ADDR,
1041 ptp->start.dma_addr);
1042
1043
1044 WRITE_ONCE(*start, 0);
1045 rc = efx_mcdi_rpc_start(efx, MC_CMD_PTP, synch_buf,
1046 MC_CMD_PTP_IN_SYNCHRONIZE_LEN);
1047 EFX_WARN_ON_ONCE_PARANOID(rc);
1048
1049
1050 timeout = jiffies + msecs_to_jiffies(MAX_SYNCHRONISE_WAIT_MS);
1051 while (!READ_ONCE(*start) && (time_before(jiffies, timeout))) {
1052 udelay(20);
1053 loops++;
1054 }
1055
1056 if (loops <= 1)
1057 ++ptp->fast_syncs;
1058 if (!time_before(jiffies, timeout))
1059 ++ptp->sync_timeouts;
1060
1061 if (READ_ONCE(*start))
1062 efx_ptp_send_times(efx, &last_time);
1063
1064
1065 rc = efx_mcdi_rpc_finish(efx, MC_CMD_PTP,
1066 MC_CMD_PTP_IN_SYNCHRONIZE_LEN,
1067 synch_buf, sizeof(synch_buf),
1068 &response_length);
1069 if (rc == 0) {
1070 rc = efx_ptp_process_times(efx, synch_buf, response_length,
1071 &last_time);
1072 if (rc == 0)
1073 ++ptp->good_syncs;
1074 else
1075 ++ptp->no_time_syncs;
1076 }
1077
1078
1079
1080
1081 if (rc != 0)
1082 ++ptp->bad_syncs;
1083
1084 return rc;
1085}
1086
1087
1088static void efx_ptp_xmit_skb_queue(struct efx_nic *efx, struct sk_buff *skb)
1089{
1090 struct efx_ptp_data *ptp_data = efx->ptp_data;
1091 u8 type = efx_tx_csum_type_skb(skb);
1092 struct efx_tx_queue *tx_queue;
1093
1094 tx_queue = efx_channel_get_tx_queue(ptp_data->channel, type);
1095 if (tx_queue && tx_queue->timestamping) {
1096 efx_enqueue_skb(tx_queue, skb);
1097 } else {
1098 WARN_ONCE(1, "PTP channel has no timestamped tx queue\n");
1099 dev_kfree_skb_any(skb);
1100 }
1101}
1102
1103
1104static void efx_ptp_xmit_skb_mc(struct efx_nic *efx, struct sk_buff *skb)
1105{
1106 struct efx_ptp_data *ptp_data = efx->ptp_data;
1107 struct skb_shared_hwtstamps timestamps;
1108 int rc = -EIO;
1109 MCDI_DECLARE_BUF(txtime, MC_CMD_PTP_OUT_TRANSMIT_LEN);
1110 size_t len;
1111
1112 MCDI_SET_DWORD(ptp_data->txbuf, PTP_IN_OP, MC_CMD_PTP_OP_TRANSMIT);
1113 MCDI_SET_DWORD(ptp_data->txbuf, PTP_IN_PERIPH_ID, 0);
1114 MCDI_SET_DWORD(ptp_data->txbuf, PTP_IN_TRANSMIT_LENGTH, skb->len);
1115 if (skb_shinfo(skb)->nr_frags != 0) {
1116 rc = skb_linearize(skb);
1117 if (rc != 0)
1118 goto fail;
1119 }
1120
1121 if (skb->ip_summed == CHECKSUM_PARTIAL) {
1122 rc = skb_checksum_help(skb);
1123 if (rc != 0)
1124 goto fail;
1125 }
1126 skb_copy_from_linear_data(skb,
1127 MCDI_PTR(ptp_data->txbuf,
1128 PTP_IN_TRANSMIT_PACKET),
1129 skb->len);
1130 rc = efx_mcdi_rpc(efx, MC_CMD_PTP,
1131 ptp_data->txbuf, MC_CMD_PTP_IN_TRANSMIT_LEN(skb->len),
1132 txtime, sizeof(txtime), &len);
1133 if (rc != 0)
1134 goto fail;
1135
1136 memset(×tamps, 0, sizeof(timestamps));
1137 timestamps.hwtstamp = ptp_data->nic_to_kernel_time(
1138 MCDI_DWORD(txtime, PTP_OUT_TRANSMIT_MAJOR),
1139 MCDI_DWORD(txtime, PTP_OUT_TRANSMIT_MINOR),
1140 ptp_data->ts_corrections.ptp_tx);
1141
1142 skb_tstamp_tx(skb, ×tamps);
1143
1144 rc = 0;
1145
1146fail:
1147 dev_kfree_skb_any(skb);
1148
1149 return;
1150}
1151
1152static void efx_ptp_drop_time_expired_events(struct efx_nic *efx)
1153{
1154 struct efx_ptp_data *ptp = efx->ptp_data;
1155 struct list_head *cursor;
1156 struct list_head *next;
1157
1158 if (ptp->rx_ts_inline)
1159 return;
1160
1161
1162 spin_lock_bh(&ptp->evt_lock);
1163 list_for_each_safe(cursor, next, &ptp->evt_list) {
1164 struct efx_ptp_event_rx *evt;
1165
1166 evt = list_entry(cursor, struct efx_ptp_event_rx,
1167 link);
1168 if (time_after(jiffies, evt->expiry)) {
1169 list_move(&evt->link, &ptp->evt_free_list);
1170 netif_warn(efx, hw, efx->net_dev,
1171 "PTP rx event dropped\n");
1172 }
1173 }
1174 spin_unlock_bh(&ptp->evt_lock);
1175}
1176
1177static enum ptp_packet_state efx_ptp_match_rx(struct efx_nic *efx,
1178 struct sk_buff *skb)
1179{
1180 struct efx_ptp_data *ptp = efx->ptp_data;
1181 bool evts_waiting;
1182 struct list_head *cursor;
1183 struct list_head *next;
1184 struct efx_ptp_match *match;
1185 enum ptp_packet_state rc = PTP_PACKET_STATE_UNMATCHED;
1186
1187 WARN_ON_ONCE(ptp->rx_ts_inline);
1188
1189 spin_lock_bh(&ptp->evt_lock);
1190 evts_waiting = !list_empty(&ptp->evt_list);
1191 spin_unlock_bh(&ptp->evt_lock);
1192
1193 if (!evts_waiting)
1194 return PTP_PACKET_STATE_UNMATCHED;
1195
1196 match = (struct efx_ptp_match *)skb->cb;
1197
1198 spin_lock_bh(&ptp->evt_lock);
1199 list_for_each_safe(cursor, next, &ptp->evt_list) {
1200 struct efx_ptp_event_rx *evt;
1201
1202 evt = list_entry(cursor, struct efx_ptp_event_rx, link);
1203 if ((evt->seq0 == match->words[0]) &&
1204 (evt->seq1 == match->words[1])) {
1205 struct skb_shared_hwtstamps *timestamps;
1206
1207
1208 timestamps = skb_hwtstamps(skb);
1209 timestamps->hwtstamp = evt->hwtimestamp;
1210
1211 match->state = PTP_PACKET_STATE_MATCHED;
1212 rc = PTP_PACKET_STATE_MATCHED;
1213 list_move(&evt->link, &ptp->evt_free_list);
1214 break;
1215 }
1216 }
1217 spin_unlock_bh(&ptp->evt_lock);
1218
1219 return rc;
1220}
1221
1222
1223
1224
1225
1226static void efx_ptp_process_events(struct efx_nic *efx, struct sk_buff_head *q)
1227{
1228 struct efx_ptp_data *ptp = efx->ptp_data;
1229 struct sk_buff *skb;
1230
1231 while ((skb = skb_dequeue(&ptp->rxq))) {
1232 struct efx_ptp_match *match;
1233
1234 match = (struct efx_ptp_match *)skb->cb;
1235 if (match->state == PTP_PACKET_STATE_MATCH_UNWANTED) {
1236 __skb_queue_tail(q, skb);
1237 } else if (efx_ptp_match_rx(efx, skb) ==
1238 PTP_PACKET_STATE_MATCHED) {
1239 __skb_queue_tail(q, skb);
1240 } else if (time_after(jiffies, match->expiry)) {
1241 match->state = PTP_PACKET_STATE_TIMED_OUT;
1242 ++ptp->rx_no_timestamp;
1243 __skb_queue_tail(q, skb);
1244 } else {
1245
1246 skb_queue_head(&ptp->rxq, skb);
1247 break;
1248 }
1249 }
1250}
1251
1252
1253static inline void efx_ptp_process_rx(struct efx_nic *efx, struct sk_buff *skb)
1254{
1255 local_bh_disable();
1256 netif_receive_skb(skb);
1257 local_bh_enable();
1258}
1259
1260static void efx_ptp_remove_multicast_filters(struct efx_nic *efx)
1261{
1262 struct efx_ptp_data *ptp = efx->ptp_data;
1263
1264 if (ptp->rxfilter_installed) {
1265 efx_filter_remove_id_safe(efx, EFX_FILTER_PRI_REQUIRED,
1266 ptp->rxfilter_general);
1267 efx_filter_remove_id_safe(efx, EFX_FILTER_PRI_REQUIRED,
1268 ptp->rxfilter_event);
1269 ptp->rxfilter_installed = false;
1270 }
1271}
1272
1273static int efx_ptp_insert_multicast_filters(struct efx_nic *efx)
1274{
1275 struct efx_ptp_data *ptp = efx->ptp_data;
1276 struct efx_filter_spec rxfilter;
1277 int rc;
1278
1279 if (!ptp->channel || ptp->rxfilter_installed)
1280 return 0;
1281
1282
1283
1284
1285 efx_filter_init_rx(&rxfilter, EFX_FILTER_PRI_REQUIRED, 0,
1286 efx_rx_queue_index(
1287 efx_channel_get_rx_queue(ptp->channel)));
1288 rc = efx_filter_set_ipv4_local(&rxfilter, IPPROTO_UDP,
1289 htonl(PTP_ADDRESS),
1290 htons(PTP_EVENT_PORT));
1291 if (rc != 0)
1292 return rc;
1293
1294 rc = efx_filter_insert_filter(efx, &rxfilter, true);
1295 if (rc < 0)
1296 return rc;
1297 ptp->rxfilter_event = rc;
1298
1299 efx_filter_init_rx(&rxfilter, EFX_FILTER_PRI_REQUIRED, 0,
1300 efx_rx_queue_index(
1301 efx_channel_get_rx_queue(ptp->channel)));
1302 rc = efx_filter_set_ipv4_local(&rxfilter, IPPROTO_UDP,
1303 htonl(PTP_ADDRESS),
1304 htons(PTP_GENERAL_PORT));
1305 if (rc != 0)
1306 goto fail;
1307
1308 rc = efx_filter_insert_filter(efx, &rxfilter, true);
1309 if (rc < 0)
1310 goto fail;
1311 ptp->rxfilter_general = rc;
1312
1313 ptp->rxfilter_installed = true;
1314 return 0;
1315
1316fail:
1317 efx_filter_remove_id_safe(efx, EFX_FILTER_PRI_REQUIRED,
1318 ptp->rxfilter_event);
1319 return rc;
1320}
1321
1322static int efx_ptp_start(struct efx_nic *efx)
1323{
1324 struct efx_ptp_data *ptp = efx->ptp_data;
1325 int rc;
1326
1327 ptp->reset_required = false;
1328
1329 rc = efx_ptp_insert_multicast_filters(efx);
1330 if (rc)
1331 return rc;
1332
1333 rc = efx_ptp_enable(efx);
1334 if (rc != 0)
1335 goto fail;
1336
1337 ptp->evt_frag_idx = 0;
1338 ptp->current_adjfreq = 0;
1339
1340 return 0;
1341
1342fail:
1343 efx_ptp_remove_multicast_filters(efx);
1344 return rc;
1345}
1346
1347static int efx_ptp_stop(struct efx_nic *efx)
1348{
1349 struct efx_ptp_data *ptp = efx->ptp_data;
1350 struct list_head *cursor;
1351 struct list_head *next;
1352 int rc;
1353
1354 if (ptp == NULL)
1355 return 0;
1356
1357 rc = efx_ptp_disable(efx);
1358
1359 efx_ptp_remove_multicast_filters(efx);
1360
1361
1362 efx_ptp_deliver_rx_queue(&efx->ptp_data->rxq);
1363 skb_queue_purge(&efx->ptp_data->txq);
1364
1365
1366 spin_lock_bh(&efx->ptp_data->evt_lock);
1367 list_for_each_safe(cursor, next, &efx->ptp_data->evt_list) {
1368 list_move(cursor, &efx->ptp_data->evt_free_list);
1369 }
1370 spin_unlock_bh(&efx->ptp_data->evt_lock);
1371
1372 return rc;
1373}
1374
1375static int efx_ptp_restart(struct efx_nic *efx)
1376{
1377 if (efx->ptp_data && efx->ptp_data->enabled)
1378 return efx_ptp_start(efx);
1379 return 0;
1380}
1381
1382static void efx_ptp_pps_worker(struct work_struct *work)
1383{
1384 struct efx_ptp_data *ptp =
1385 container_of(work, struct efx_ptp_data, pps_work);
1386 struct efx_nic *efx = ptp->efx;
1387 struct ptp_clock_event ptp_evt;
1388
1389 if (efx_ptp_synchronize(efx, PTP_SYNC_ATTEMPTS))
1390 return;
1391
1392 ptp_evt.type = PTP_CLOCK_PPSUSR;
1393 ptp_evt.pps_times = ptp->host_time_pps;
1394 ptp_clock_event(ptp->phc_clock, &ptp_evt);
1395}
1396
1397static void efx_ptp_worker(struct work_struct *work)
1398{
1399 struct efx_ptp_data *ptp_data =
1400 container_of(work, struct efx_ptp_data, work);
1401 struct efx_nic *efx = ptp_data->efx;
1402 struct sk_buff *skb;
1403 struct sk_buff_head tempq;
1404
1405 if (ptp_data->reset_required) {
1406 efx_ptp_stop(efx);
1407 efx_ptp_start(efx);
1408 return;
1409 }
1410
1411 efx_ptp_drop_time_expired_events(efx);
1412
1413 __skb_queue_head_init(&tempq);
1414 efx_ptp_process_events(efx, &tempq);
1415
1416 while ((skb = skb_dequeue(&ptp_data->txq)))
1417 ptp_data->xmit_skb(efx, skb);
1418
1419 while ((skb = __skb_dequeue(&tempq)))
1420 efx_ptp_process_rx(efx, skb);
1421}
1422
1423static const struct ptp_clock_info efx_phc_clock_info = {
1424 .owner = THIS_MODULE,
1425 .name = "sfc",
1426 .max_adj = MAX_PPB,
1427 .n_alarm = 0,
1428 .n_ext_ts = 0,
1429 .n_per_out = 0,
1430 .n_pins = 0,
1431 .pps = 1,
1432 .adjfreq = efx_phc_adjfreq,
1433 .adjtime = efx_phc_adjtime,
1434 .gettime64 = efx_phc_gettime,
1435 .settime64 = efx_phc_settime,
1436 .enable = efx_phc_enable,
1437};
1438
1439
1440int efx_ptp_probe(struct efx_nic *efx, struct efx_channel *channel)
1441{
1442 struct efx_ptp_data *ptp;
1443 int rc = 0;
1444 unsigned int pos;
1445
1446 ptp = kzalloc(sizeof(struct efx_ptp_data), GFP_KERNEL);
1447 efx->ptp_data = ptp;
1448 if (!efx->ptp_data)
1449 return -ENOMEM;
1450
1451 ptp->efx = efx;
1452 ptp->channel = channel;
1453 ptp->rx_ts_inline = efx_nic_rev(efx) >= EFX_REV_HUNT_A0;
1454
1455 rc = efx_nic_alloc_buffer(efx, &ptp->start, sizeof(int), GFP_KERNEL);
1456 if (rc != 0)
1457 goto fail1;
1458
1459 skb_queue_head_init(&ptp->rxq);
1460 skb_queue_head_init(&ptp->txq);
1461 ptp->workwq = create_singlethread_workqueue("sfc_ptp");
1462 if (!ptp->workwq) {
1463 rc = -ENOMEM;
1464 goto fail2;
1465 }
1466
1467 if (efx_ptp_use_mac_tx_timestamps(efx)) {
1468 ptp->xmit_skb = efx_ptp_xmit_skb_queue;
1469
1470 channel->sync_events_state = SYNC_EVENTS_QUIESCENT;
1471 } else {
1472 ptp->xmit_skb = efx_ptp_xmit_skb_mc;
1473 }
1474
1475 INIT_WORK(&ptp->work, efx_ptp_worker);
1476 ptp->config.flags = 0;
1477 ptp->config.tx_type = HWTSTAMP_TX_OFF;
1478 ptp->config.rx_filter = HWTSTAMP_FILTER_NONE;
1479 INIT_LIST_HEAD(&ptp->evt_list);
1480 INIT_LIST_HEAD(&ptp->evt_free_list);
1481 spin_lock_init(&ptp->evt_lock);
1482 for (pos = 0; pos < MAX_RECEIVE_EVENTS; pos++)
1483 list_add(&ptp->rx_evts[pos].link, &ptp->evt_free_list);
1484
1485
1486 rc = efx_ptp_get_attributes(efx);
1487 if (rc < 0)
1488 goto fail3;
1489
1490
1491 rc = efx_ptp_get_timestamp_corrections(efx);
1492 if (rc < 0)
1493 goto fail3;
1494
1495 if (efx->mcdi->fn_flags &
1496 (1 << MC_CMD_DRV_ATTACH_EXT_OUT_FLAG_PRIMARY)) {
1497 ptp->phc_clock_info = efx_phc_clock_info;
1498 ptp->phc_clock = ptp_clock_register(&ptp->phc_clock_info,
1499 &efx->pci_dev->dev);
1500 if (IS_ERR(ptp->phc_clock)) {
1501 rc = PTR_ERR(ptp->phc_clock);
1502 goto fail3;
1503 } else if (ptp->phc_clock) {
1504 INIT_WORK(&ptp->pps_work, efx_ptp_pps_worker);
1505 ptp->pps_workwq = create_singlethread_workqueue("sfc_pps");
1506 if (!ptp->pps_workwq) {
1507 rc = -ENOMEM;
1508 goto fail4;
1509 }
1510 }
1511 }
1512 ptp->nic_ts_enabled = false;
1513
1514 return 0;
1515fail4:
1516 ptp_clock_unregister(efx->ptp_data->phc_clock);
1517
1518fail3:
1519 destroy_workqueue(efx->ptp_data->workwq);
1520
1521fail2:
1522 efx_nic_free_buffer(efx, &ptp->start);
1523
1524fail1:
1525 kfree(efx->ptp_data);
1526 efx->ptp_data = NULL;
1527
1528 return rc;
1529}
1530
1531
1532
1533
1534
1535
1536static int efx_ptp_probe_channel(struct efx_channel *channel)
1537{
1538 struct efx_nic *efx = channel->efx;
1539 int rc;
1540
1541 channel->irq_moderation_us = 0;
1542 channel->rx_queue.core_index = 0;
1543
1544 rc = efx_ptp_probe(efx, channel);
1545
1546
1547
1548
1549
1550 if (rc && rc != -EPERM)
1551 netif_warn(efx, drv, efx->net_dev,
1552 "Failed to probe PTP, rc=%d\n", rc);
1553 return 0;
1554}
1555
1556void efx_ptp_remove(struct efx_nic *efx)
1557{
1558 if (!efx->ptp_data)
1559 return;
1560
1561 (void)efx_ptp_disable(efx);
1562
1563 cancel_work_sync(&efx->ptp_data->work);
1564 if (efx->ptp_data->pps_workwq)
1565 cancel_work_sync(&efx->ptp_data->pps_work);
1566
1567 skb_queue_purge(&efx->ptp_data->rxq);
1568 skb_queue_purge(&efx->ptp_data->txq);
1569
1570 if (efx->ptp_data->phc_clock) {
1571 destroy_workqueue(efx->ptp_data->pps_workwq);
1572 ptp_clock_unregister(efx->ptp_data->phc_clock);
1573 }
1574
1575 destroy_workqueue(efx->ptp_data->workwq);
1576
1577 efx_nic_free_buffer(efx, &efx->ptp_data->start);
1578 kfree(efx->ptp_data);
1579 efx->ptp_data = NULL;
1580}
1581
1582static void efx_ptp_remove_channel(struct efx_channel *channel)
1583{
1584 efx_ptp_remove(channel->efx);
1585}
1586
1587static void efx_ptp_get_channel_name(struct efx_channel *channel,
1588 char *buf, size_t len)
1589{
1590 snprintf(buf, len, "%s-ptp", channel->efx->name);
1591}
1592
1593
1594
1595
1596bool efx_ptp_is_ptp_tx(struct efx_nic *efx, struct sk_buff *skb)
1597{
1598 return efx->ptp_data &&
1599 efx->ptp_data->enabled &&
1600 skb->len >= PTP_MIN_LENGTH &&
1601 skb->len <= MC_CMD_PTP_IN_TRANSMIT_PACKET_MAXNUM &&
1602 likely(skb->protocol == htons(ETH_P_IP)) &&
1603 skb_transport_header_was_set(skb) &&
1604 skb_network_header_len(skb) >= sizeof(struct iphdr) &&
1605 ip_hdr(skb)->protocol == IPPROTO_UDP &&
1606 skb_headlen(skb) >=
1607 skb_transport_offset(skb) + sizeof(struct udphdr) &&
1608 udp_hdr(skb)->dest == htons(PTP_EVENT_PORT);
1609}
1610
1611
1612
1613
1614
1615static bool efx_ptp_rx(struct efx_channel *channel, struct sk_buff *skb)
1616{
1617 struct efx_nic *efx = channel->efx;
1618 struct efx_ptp_data *ptp = efx->ptp_data;
1619 struct efx_ptp_match *match = (struct efx_ptp_match *)skb->cb;
1620 u8 *match_data_012, *match_data_345;
1621 unsigned int version;
1622 u8 *data;
1623
1624 match->expiry = jiffies + msecs_to_jiffies(PKT_EVENT_LIFETIME_MS);
1625
1626
1627 if (ptp->mode == MC_CMD_PTP_MODE_V1) {
1628 if (!pskb_may_pull(skb, PTP_V1_MIN_LENGTH)) {
1629 return false;
1630 }
1631 data = skb->data;
1632 version = ntohs(*(__be16 *)&data[PTP_V1_VERSION_OFFSET]);
1633 if (version != PTP_VERSION_V1) {
1634 return false;
1635 }
1636
1637
1638
1639
1640 match_data_012 = data + PTP_V1_UUID_OFFSET;
1641 match_data_345 = data + PTP_V1_UUID_OFFSET + 3;
1642 } else {
1643 if (!pskb_may_pull(skb, PTP_V2_MIN_LENGTH)) {
1644 return false;
1645 }
1646 data = skb->data;
1647 version = data[PTP_V2_VERSION_OFFSET];
1648 if ((version & PTP_VERSION_V2_MASK) != PTP_VERSION_V2) {
1649 return false;
1650 }
1651
1652
1653
1654
1655
1656
1657
1658
1659 match_data_345 = data + PTP_V2_UUID_OFFSET + 5;
1660 if (ptp->mode == MC_CMD_PTP_MODE_V2) {
1661 match_data_012 = data + PTP_V2_UUID_OFFSET + 2;
1662 } else {
1663 match_data_012 = data + PTP_V2_UUID_OFFSET + 0;
1664 BUG_ON(ptp->mode != MC_CMD_PTP_MODE_V2_ENHANCED);
1665 }
1666 }
1667
1668
1669 if (ntohs(*(__be16 *)&data[PTP_DPORT_OFFSET]) == PTP_EVENT_PORT) {
1670 match->state = PTP_PACKET_STATE_UNMATCHED;
1671
1672
1673
1674
1675 BUILD_BUG_ON(PTP_V1_SEQUENCE_OFFSET != PTP_V2_SEQUENCE_OFFSET);
1676 BUILD_BUG_ON(PTP_V1_SEQUENCE_LENGTH != PTP_V2_SEQUENCE_LENGTH);
1677
1678
1679 match->words[0] = (match_data_012[0] |
1680 (match_data_012[1] << 8) |
1681 (match_data_012[2] << 16) |
1682 (match_data_345[0] << 24));
1683 match->words[1] = (match_data_345[1] |
1684 (match_data_345[2] << 8) |
1685 (data[PTP_V1_SEQUENCE_OFFSET +
1686 PTP_V1_SEQUENCE_LENGTH - 1] <<
1687 16));
1688 } else {
1689 match->state = PTP_PACKET_STATE_MATCH_UNWANTED;
1690 }
1691
1692 skb_queue_tail(&ptp->rxq, skb);
1693 queue_work(ptp->workwq, &ptp->work);
1694
1695 return true;
1696}
1697
1698
1699
1700
1701
1702int efx_ptp_tx(struct efx_nic *efx, struct sk_buff *skb)
1703{
1704 struct efx_ptp_data *ptp = efx->ptp_data;
1705
1706 skb_queue_tail(&ptp->txq, skb);
1707
1708 if ((udp_hdr(skb)->dest == htons(PTP_EVENT_PORT)) &&
1709 (skb->len <= MC_CMD_PTP_IN_TRANSMIT_PACKET_MAXNUM))
1710 efx_xmit_hwtstamp_pending(skb);
1711 queue_work(ptp->workwq, &ptp->work);
1712
1713 return NETDEV_TX_OK;
1714}
1715
1716int efx_ptp_get_mode(struct efx_nic *efx)
1717{
1718 return efx->ptp_data->mode;
1719}
1720
1721int efx_ptp_change_mode(struct efx_nic *efx, bool enable_wanted,
1722 unsigned int new_mode)
1723{
1724 if ((enable_wanted != efx->ptp_data->enabled) ||
1725 (enable_wanted && (efx->ptp_data->mode != new_mode))) {
1726 int rc = 0;
1727
1728 if (enable_wanted) {
1729
1730 if (efx->ptp_data->enabled &&
1731 (efx->ptp_data->mode != new_mode)) {
1732 efx->ptp_data->enabled = false;
1733 rc = efx_ptp_stop(efx);
1734 if (rc != 0)
1735 return rc;
1736 }
1737
1738
1739
1740
1741
1742 efx->ptp_data->mode = new_mode;
1743 if (netif_running(efx->net_dev))
1744 rc = efx_ptp_start(efx);
1745 if (rc == 0) {
1746 rc = efx_ptp_synchronize(efx,
1747 PTP_SYNC_ATTEMPTS * 2);
1748 if (rc != 0)
1749 efx_ptp_stop(efx);
1750 }
1751 } else {
1752 rc = efx_ptp_stop(efx);
1753 }
1754
1755 if (rc != 0)
1756 return rc;
1757
1758 efx->ptp_data->enabled = enable_wanted;
1759 }
1760
1761 return 0;
1762}
1763
1764static int efx_ptp_ts_init(struct efx_nic *efx, struct hwtstamp_config *init)
1765{
1766 int rc;
1767
1768 if (init->flags)
1769 return -EINVAL;
1770
1771 if ((init->tx_type != HWTSTAMP_TX_OFF) &&
1772 (init->tx_type != HWTSTAMP_TX_ON))
1773 return -ERANGE;
1774
1775 rc = efx->type->ptp_set_ts_config(efx, init);
1776 if (rc)
1777 return rc;
1778
1779 efx->ptp_data->config = *init;
1780 return 0;
1781}
1782
1783void efx_ptp_get_ts_info(struct efx_nic *efx, struct ethtool_ts_info *ts_info)
1784{
1785 struct efx_ptp_data *ptp = efx->ptp_data;
1786 struct efx_nic *primary = efx->primary;
1787
1788 ASSERT_RTNL();
1789
1790 if (!ptp)
1791 return;
1792
1793 ts_info->so_timestamping |= (SOF_TIMESTAMPING_TX_HARDWARE |
1794 SOF_TIMESTAMPING_RX_HARDWARE |
1795 SOF_TIMESTAMPING_RAW_HARDWARE);
1796
1797
1798
1799 if (efx_ptp_use_mac_tx_timestamps(efx)) {
1800 struct efx_ef10_nic_data *nic_data = efx->nic_data;
1801
1802 if (!(nic_data->licensed_features &
1803 (1 << LICENSED_V3_FEATURES_TX_TIMESTAMPS_LBN)))
1804 ts_info->so_timestamping &=
1805 ~SOF_TIMESTAMPING_TX_HARDWARE;
1806 }
1807 if (primary && primary->ptp_data && primary->ptp_data->phc_clock)
1808 ts_info->phc_index =
1809 ptp_clock_index(primary->ptp_data->phc_clock);
1810 ts_info->tx_types = 1 << HWTSTAMP_TX_OFF | 1 << HWTSTAMP_TX_ON;
1811 ts_info->rx_filters = ptp->efx->type->hwtstamp_filters;
1812}
1813
1814int efx_ptp_set_ts_config(struct efx_nic *efx, struct ifreq *ifr)
1815{
1816 struct hwtstamp_config config;
1817 int rc;
1818
1819
1820 if (!efx->ptp_data)
1821 return -EOPNOTSUPP;
1822
1823 if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
1824 return -EFAULT;
1825
1826 rc = efx_ptp_ts_init(efx, &config);
1827 if (rc != 0)
1828 return rc;
1829
1830 return copy_to_user(ifr->ifr_data, &config, sizeof(config))
1831 ? -EFAULT : 0;
1832}
1833
1834int efx_ptp_get_ts_config(struct efx_nic *efx, struct ifreq *ifr)
1835{
1836 if (!efx->ptp_data)
1837 return -EOPNOTSUPP;
1838
1839 return copy_to_user(ifr->ifr_data, &efx->ptp_data->config,
1840 sizeof(efx->ptp_data->config)) ? -EFAULT : 0;
1841}
1842
1843static void ptp_event_failure(struct efx_nic *efx, int expected_frag_len)
1844{
1845 struct efx_ptp_data *ptp = efx->ptp_data;
1846
1847 netif_err(efx, hw, efx->net_dev,
1848 "PTP unexpected event length: got %d expected %d\n",
1849 ptp->evt_frag_idx, expected_frag_len);
1850 ptp->reset_required = true;
1851 queue_work(ptp->workwq, &ptp->work);
1852}
1853
1854
1855
1856
1857
1858static void ptp_event_rx(struct efx_nic *efx, struct efx_ptp_data *ptp)
1859{
1860 struct efx_ptp_event_rx *evt = NULL;
1861
1862 if (WARN_ON_ONCE(ptp->rx_ts_inline))
1863 return;
1864
1865 if (ptp->evt_frag_idx != 3) {
1866 ptp_event_failure(efx, 3);
1867 return;
1868 }
1869
1870 spin_lock_bh(&ptp->evt_lock);
1871 if (!list_empty(&ptp->evt_free_list)) {
1872 evt = list_first_entry(&ptp->evt_free_list,
1873 struct efx_ptp_event_rx, link);
1874 list_del(&evt->link);
1875
1876 evt->seq0 = EFX_QWORD_FIELD(ptp->evt_frags[2], MCDI_EVENT_DATA);
1877 evt->seq1 = (EFX_QWORD_FIELD(ptp->evt_frags[2],
1878 MCDI_EVENT_SRC) |
1879 (EFX_QWORD_FIELD(ptp->evt_frags[1],
1880 MCDI_EVENT_SRC) << 8) |
1881 (EFX_QWORD_FIELD(ptp->evt_frags[0],
1882 MCDI_EVENT_SRC) << 16));
1883 evt->hwtimestamp = efx->ptp_data->nic_to_kernel_time(
1884 EFX_QWORD_FIELD(ptp->evt_frags[0], MCDI_EVENT_DATA),
1885 EFX_QWORD_FIELD(ptp->evt_frags[1], MCDI_EVENT_DATA),
1886 ptp->ts_corrections.ptp_rx);
1887 evt->expiry = jiffies + msecs_to_jiffies(PKT_EVENT_LIFETIME_MS);
1888 list_add_tail(&evt->link, &ptp->evt_list);
1889
1890 queue_work(ptp->workwq, &ptp->work);
1891 } else if (net_ratelimit()) {
1892
1893 netif_err(efx, rx_err, efx->net_dev, "PTP event queue overflow\n");
1894 }
1895 spin_unlock_bh(&ptp->evt_lock);
1896}
1897
1898static void ptp_event_fault(struct efx_nic *efx, struct efx_ptp_data *ptp)
1899{
1900 int code = EFX_QWORD_FIELD(ptp->evt_frags[0], MCDI_EVENT_DATA);
1901 if (ptp->evt_frag_idx != 1) {
1902 ptp_event_failure(efx, 1);
1903 return;
1904 }
1905
1906 netif_err(efx, hw, efx->net_dev, "PTP error %d\n", code);
1907}
1908
1909static void ptp_event_pps(struct efx_nic *efx, struct efx_ptp_data *ptp)
1910{
1911 if (ptp->nic_ts_enabled)
1912 queue_work(ptp->pps_workwq, &ptp->pps_work);
1913}
1914
1915void efx_ptp_event(struct efx_nic *efx, efx_qword_t *ev)
1916{
1917 struct efx_ptp_data *ptp = efx->ptp_data;
1918 int code = EFX_QWORD_FIELD(*ev, MCDI_EVENT_CODE);
1919
1920 if (!ptp) {
1921 if (!efx->ptp_warned) {
1922 netif_warn(efx, drv, efx->net_dev,
1923 "Received PTP event but PTP not set up\n");
1924 efx->ptp_warned = true;
1925 }
1926 return;
1927 }
1928
1929 if (!ptp->enabled)
1930 return;
1931
1932 if (ptp->evt_frag_idx == 0) {
1933 ptp->evt_code = code;
1934 } else if (ptp->evt_code != code) {
1935 netif_err(efx, hw, efx->net_dev,
1936 "PTP out of sequence event %d\n", code);
1937 ptp->evt_frag_idx = 0;
1938 }
1939
1940 ptp->evt_frags[ptp->evt_frag_idx++] = *ev;
1941 if (!MCDI_EVENT_FIELD(*ev, CONT)) {
1942
1943 switch (code) {
1944 case MCDI_EVENT_CODE_PTP_RX:
1945 ptp_event_rx(efx, ptp);
1946 break;
1947 case MCDI_EVENT_CODE_PTP_FAULT:
1948 ptp_event_fault(efx, ptp);
1949 break;
1950 case MCDI_EVENT_CODE_PTP_PPS:
1951 ptp_event_pps(efx, ptp);
1952 break;
1953 default:
1954 netif_err(efx, hw, efx->net_dev,
1955 "PTP unknown event %d\n", code);
1956 break;
1957 }
1958 ptp->evt_frag_idx = 0;
1959 } else if (MAX_EVENT_FRAGS == ptp->evt_frag_idx) {
1960 netif_err(efx, hw, efx->net_dev,
1961 "PTP too many event fragments\n");
1962 ptp->evt_frag_idx = 0;
1963 }
1964}
1965
1966void efx_time_sync_event(struct efx_channel *channel, efx_qword_t *ev)
1967{
1968 struct efx_nic *efx = channel->efx;
1969 struct efx_ptp_data *ptp = efx->ptp_data;
1970
1971
1972
1973
1974
1975
1976
1977 channel->sync_timestamp_major = MCDI_EVENT_FIELD(*ev, PTP_TIME_MAJOR);
1978 channel->sync_timestamp_minor =
1979 (MCDI_EVENT_FIELD(*ev, PTP_TIME_MINOR_MS_8BITS) & 0xFC)
1980 << ptp->nic_time.sync_event_minor_shift;
1981
1982
1983
1984
1985 (void) cmpxchg(&channel->sync_events_state, SYNC_EVENTS_REQUESTED,
1986 SYNC_EVENTS_VALID);
1987}
1988
1989static inline u32 efx_rx_buf_timestamp_minor(struct efx_nic *efx, const u8 *eh)
1990{
1991#if defined(CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS)
1992 return __le32_to_cpup((const __le32 *)(eh + efx->rx_packet_ts_offset));
1993#else
1994 const u8 *data = eh + efx->rx_packet_ts_offset;
1995 return (u32)data[0] |
1996 (u32)data[1] << 8 |
1997 (u32)data[2] << 16 |
1998 (u32)data[3] << 24;
1999#endif
2000}
2001
2002void __efx_rx_skb_attach_timestamp(struct efx_channel *channel,
2003 struct sk_buff *skb)
2004{
2005 struct efx_nic *efx = channel->efx;
2006 struct efx_ptp_data *ptp = efx->ptp_data;
2007 u32 pkt_timestamp_major, pkt_timestamp_minor;
2008 u32 diff, carry;
2009 struct skb_shared_hwtstamps *timestamps;
2010
2011 if (channel->sync_events_state != SYNC_EVENTS_VALID)
2012 return;
2013
2014 pkt_timestamp_minor = efx_rx_buf_timestamp_minor(efx, skb_mac_header(skb));
2015
2016
2017
2018
2019 diff = pkt_timestamp_minor - channel->sync_timestamp_minor;
2020 if (pkt_timestamp_minor < channel->sync_timestamp_minor)
2021 diff += ptp->nic_time.minor_max;
2022
2023
2024 carry = (channel->sync_timestamp_minor >= ptp->nic_time.minor_max - diff) ?
2025 1 : 0;
2026
2027 if (diff <= ptp->nic_time.sync_event_diff_max) {
2028
2029
2030
2031 pkt_timestamp_major = channel->sync_timestamp_major + carry;
2032 } else if (diff >= ptp->nic_time.sync_event_diff_min) {
2033
2034
2035
2036
2037 pkt_timestamp_major = channel->sync_timestamp_major - 1 + carry;
2038 } else {
2039
2040
2041
2042
2043
2044 netif_vdbg(efx, drv, efx->net_dev,
2045 "packet timestamp %x too far from sync event %x:%x\n",
2046 pkt_timestamp_minor, channel->sync_timestamp_major,
2047 channel->sync_timestamp_minor);
2048 return;
2049 }
2050
2051
2052 timestamps = skb_hwtstamps(skb);
2053 timestamps->hwtstamp =
2054 ptp->nic_to_kernel_time(pkt_timestamp_major,
2055 pkt_timestamp_minor,
2056 ptp->ts_corrections.general_rx);
2057}
2058
2059static int efx_phc_adjfreq(struct ptp_clock_info *ptp, s32 delta)
2060{
2061 struct efx_ptp_data *ptp_data = container_of(ptp,
2062 struct efx_ptp_data,
2063 phc_clock_info);
2064 struct efx_nic *efx = ptp_data->efx;
2065 MCDI_DECLARE_BUF(inadj, MC_CMD_PTP_IN_ADJUST_LEN);
2066 s64 adjustment_ns;
2067 int rc;
2068
2069 if (delta > MAX_PPB)
2070 delta = MAX_PPB;
2071 else if (delta < -MAX_PPB)
2072 delta = -MAX_PPB;
2073
2074
2075 adjustment_ns = ((s64)delta * PPB_SCALE_WORD +
2076 (1 << (ptp_data->adjfreq_ppb_shift - 1))) >>
2077 ptp_data->adjfreq_ppb_shift;
2078
2079 MCDI_SET_DWORD(inadj, PTP_IN_OP, MC_CMD_PTP_OP_ADJUST);
2080 MCDI_SET_DWORD(inadj, PTP_IN_PERIPH_ID, 0);
2081 MCDI_SET_QWORD(inadj, PTP_IN_ADJUST_FREQ, adjustment_ns);
2082 MCDI_SET_DWORD(inadj, PTP_IN_ADJUST_SECONDS, 0);
2083 MCDI_SET_DWORD(inadj, PTP_IN_ADJUST_NANOSECONDS, 0);
2084 rc = efx_mcdi_rpc(efx, MC_CMD_PTP, inadj, sizeof(inadj),
2085 NULL, 0, NULL);
2086 if (rc != 0)
2087 return rc;
2088
2089 ptp_data->current_adjfreq = adjustment_ns;
2090 return 0;
2091}
2092
2093static int efx_phc_adjtime(struct ptp_clock_info *ptp, s64 delta)
2094{
2095 u32 nic_major, nic_minor;
2096 struct efx_ptp_data *ptp_data = container_of(ptp,
2097 struct efx_ptp_data,
2098 phc_clock_info);
2099 struct efx_nic *efx = ptp_data->efx;
2100 MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_ADJUST_LEN);
2101
2102 efx->ptp_data->ns_to_nic_time(delta, &nic_major, &nic_minor);
2103
2104 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_ADJUST);
2105 MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);
2106 MCDI_SET_QWORD(inbuf, PTP_IN_ADJUST_FREQ, ptp_data->current_adjfreq);
2107 MCDI_SET_DWORD(inbuf, PTP_IN_ADJUST_MAJOR, nic_major);
2108 MCDI_SET_DWORD(inbuf, PTP_IN_ADJUST_MINOR, nic_minor);
2109 return efx_mcdi_rpc(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
2110 NULL, 0, NULL);
2111}
2112
2113static int efx_phc_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)
2114{
2115 struct efx_ptp_data *ptp_data = container_of(ptp,
2116 struct efx_ptp_data,
2117 phc_clock_info);
2118 struct efx_nic *efx = ptp_data->efx;
2119 MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_READ_NIC_TIME_LEN);
2120 MCDI_DECLARE_BUF(outbuf, MC_CMD_PTP_OUT_READ_NIC_TIME_LEN);
2121 int rc;
2122 ktime_t kt;
2123
2124 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_READ_NIC_TIME);
2125 MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);
2126
2127 rc = efx_mcdi_rpc(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
2128 outbuf, sizeof(outbuf), NULL);
2129 if (rc != 0)
2130 return rc;
2131
2132 kt = ptp_data->nic_to_kernel_time(
2133 MCDI_DWORD(outbuf, PTP_OUT_READ_NIC_TIME_MAJOR),
2134 MCDI_DWORD(outbuf, PTP_OUT_READ_NIC_TIME_MINOR), 0);
2135 *ts = ktime_to_timespec64(kt);
2136 return 0;
2137}
2138
2139static int efx_phc_settime(struct ptp_clock_info *ptp,
2140 const struct timespec64 *e_ts)
2141{
2142
2143
2144
2145
2146 int rc;
2147 struct timespec64 time_now;
2148 struct timespec64 delta;
2149
2150 rc = efx_phc_gettime(ptp, &time_now);
2151 if (rc != 0)
2152 return rc;
2153
2154 delta = timespec64_sub(*e_ts, time_now);
2155
2156 rc = efx_phc_adjtime(ptp, timespec64_to_ns(&delta));
2157 if (rc != 0)
2158 return rc;
2159
2160 return 0;
2161}
2162
2163static int efx_phc_enable(struct ptp_clock_info *ptp,
2164 struct ptp_clock_request *request,
2165 int enable)
2166{
2167 struct efx_ptp_data *ptp_data = container_of(ptp,
2168 struct efx_ptp_data,
2169 phc_clock_info);
2170 if (request->type != PTP_CLK_REQ_PPS)
2171 return -EOPNOTSUPP;
2172
2173 ptp_data->nic_ts_enabled = !!enable;
2174 return 0;
2175}
2176
2177static const struct efx_channel_type efx_ptp_channel_type = {
2178 .handle_no_channel = efx_ptp_handle_no_channel,
2179 .pre_probe = efx_ptp_probe_channel,
2180 .post_remove = efx_ptp_remove_channel,
2181 .get_name = efx_ptp_get_channel_name,
2182
2183 .receive_skb = efx_ptp_rx,
2184 .want_txqs = efx_ptp_want_txqs,
2185 .keep_eventq = false,
2186};
2187
2188void efx_ptp_defer_probe_with_channel(struct efx_nic *efx)
2189{
2190
2191
2192
2193 if (efx_ptp_disable(efx) == 0)
2194 efx->extra_channel_type[EFX_EXTRA_CHANNEL_PTP] =
2195 &efx_ptp_channel_type;
2196}
2197
2198void efx_ptp_start_datapath(struct efx_nic *efx)
2199{
2200 if (efx_ptp_restart(efx))
2201 netif_err(efx, drv, efx->net_dev, "Failed to restart PTP.\n");
2202
2203 if (efx->type->ptp_set_ts_sync_events)
2204 efx->type->ptp_set_ts_sync_events(efx, true, true);
2205}
2206
2207void efx_ptp_stop_datapath(struct efx_nic *efx)
2208{
2209
2210 if (efx->type->ptp_set_ts_sync_events)
2211 efx->type->ptp_set_ts_sync_events(efx, false, true);
2212 efx_ptp_stop(efx);
2213}
2214