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