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