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 "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 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#define MAX_PPB_BITS 20
154
155
156
157
158
159#define PPB_EXTRA_BITS 2
160
161
162#define PPB_SCALE_WORD ((1LL << (PPB_EXTRA_BITS + MC_CMD_PTP_IN_ADJUST_BITS +\
163 MAX_PPB_BITS)) / 1000000000LL)
164
165#define PTP_SYNC_ATTEMPTS 4
166
167
168
169
170
171
172
173
174
175struct efx_ptp_match {
176 u32 words[DIV_ROUND_UP(PTP_V1_UUID_LENGTH, 4)];
177 unsigned long expiry;
178 enum ptp_packet_state state;
179};
180
181
182
183
184
185
186
187struct efx_ptp_event_rx {
188 struct list_head link;
189 u32 seq0;
190 u32 seq1;
191 ktime_t hwtimestamp;
192 unsigned long expiry;
193};
194
195
196
197
198
199
200
201
202
203
204
205
206struct efx_ptp_timeset {
207 u32 host_start;
208 u32 seconds;
209 u32 nanoseconds;
210 u32 host_end;
211 u32 waitns;
212 u32 window;
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
266struct efx_ptp_data {
267 struct efx_channel *channel;
268 struct sk_buff_head rxq;
269 struct sk_buff_head txq;
270 struct list_head evt_list;
271 struct list_head evt_free_list;
272 spinlock_t evt_lock;
273 struct efx_ptp_event_rx rx_evts[MAX_RECEIVE_EVENTS];
274 struct workqueue_struct *workwq;
275 struct work_struct work;
276 bool reset_required;
277 u32 rxfilter_event;
278 u32 rxfilter_general;
279 bool rxfilter_installed;
280 struct hwtstamp_config config;
281 bool enabled;
282 unsigned int mode;
283 efx_qword_t evt_frags[MAX_EVENT_FRAGS];
284 int evt_frag_idx;
285 int evt_code;
286 struct efx_buffer start;
287 struct pps_event_time host_time_pps;
288 unsigned last_sync_ns;
289 unsigned base_sync_ns;
290 bool base_sync_valid;
291 s64 current_adjfreq;
292 struct ptp_clock *phc_clock;
293 struct ptp_clock_info phc_clock_info;
294 struct work_struct pps_work;
295 struct workqueue_struct *pps_workwq;
296 bool nic_ts_enabled;
297 u8 txbuf[ALIGN(MC_CMD_PTP_IN_TRANSMIT_LEN(
298 MC_CMD_PTP_IN_TRANSMIT_PACKET_MAXNUM), 4)];
299 struct efx_ptp_timeset
300 timeset[MC_CMD_PTP_OUT_SYNCHRONIZE_TIMESET_MAXNUM];
301};
302
303static int efx_phc_adjfreq(struct ptp_clock_info *ptp, s32 delta);
304static int efx_phc_adjtime(struct ptp_clock_info *ptp, s64 delta);
305static int efx_phc_gettime(struct ptp_clock_info *ptp, struct timespec *ts);
306static int efx_phc_settime(struct ptp_clock_info *ptp,
307 const struct timespec *e_ts);
308static int efx_phc_enable(struct ptp_clock_info *ptp,
309 struct ptp_clock_request *request, int on);
310
311
312static int efx_ptp_enable(struct efx_nic *efx)
313{
314 u8 inbuf[MC_CMD_PTP_IN_ENABLE_LEN];
315
316 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_ENABLE);
317 MCDI_SET_DWORD(inbuf, PTP_IN_ENABLE_QUEUE,
318 efx->ptp_data->channel->channel);
319 MCDI_SET_DWORD(inbuf, PTP_IN_ENABLE_MODE, efx->ptp_data->mode);
320
321 return efx_mcdi_rpc(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
322 NULL, 0, NULL);
323}
324
325
326
327
328
329
330static int efx_ptp_disable(struct efx_nic *efx)
331{
332 u8 inbuf[MC_CMD_PTP_IN_DISABLE_LEN];
333
334 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_DISABLE);
335 return efx_mcdi_rpc(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
336 NULL, 0, NULL);
337}
338
339static void efx_ptp_deliver_rx_queue(struct sk_buff_head *q)
340{
341 struct sk_buff *skb;
342
343 while ((skb = skb_dequeue(q))) {
344 local_bh_disable();
345 netif_receive_skb(skb);
346 local_bh_enable();
347 }
348}
349
350static void efx_ptp_handle_no_channel(struct efx_nic *efx)
351{
352 netif_err(efx, drv, efx->net_dev,
353 "ERROR: PTP requires MSI-X and 1 additional interrupt"
354 "vector. PTP disabled\n");
355}
356
357
358
359
360static void efx_ptp_send_times(struct efx_nic *efx,
361 struct pps_event_time *last_time)
362{
363 struct pps_event_time now;
364 struct timespec limit;
365 struct efx_ptp_data *ptp = efx->ptp_data;
366 struct timespec start;
367 int *mc_running = ptp->start.addr;
368
369 pps_get_ts(&now);
370 start = now.ts_real;
371 limit = now.ts_real;
372 timespec_add_ns(&limit, SYNCHRONISE_PERIOD_NS);
373
374
375 while ((timespec_compare(&now.ts_real, &limit) < 0) &&
376 ACCESS_ONCE(*mc_running)) {
377 struct timespec update_time;
378 unsigned int host_time;
379
380
381 update_time = now.ts_real;
382 timespec_add_ns(&update_time, SYNCHRONISATION_GRANULARITY_NS);
383 do {
384 pps_get_ts(&now);
385 } while ((timespec_compare(&now.ts_real, &update_time) < 0) &&
386 ACCESS_ONCE(*mc_running));
387
388
389 host_time = (now.ts_real.tv_sec << MC_NANOSECOND_BITS |
390 now.ts_real.tv_nsec);
391
392 _efx_writed(efx, cpu_to_le32(host_time),
393 FR_CZ_MC_TREG_SMEM + MC_SMEM_P0_PTP_TIME_OFST);
394 }
395 *last_time = now;
396}
397
398
399static void efx_ptp_read_timeset(u8 *data, struct efx_ptp_timeset *timeset)
400{
401 unsigned start_ns, end_ns;
402
403 timeset->host_start = MCDI_DWORD(data, PTP_OUT_SYNCHRONIZE_HOSTSTART);
404 timeset->seconds = MCDI_DWORD(data, PTP_OUT_SYNCHRONIZE_SECONDS);
405 timeset->nanoseconds = MCDI_DWORD(data,
406 PTP_OUT_SYNCHRONIZE_NANOSECONDS);
407 timeset->host_end = MCDI_DWORD(data, PTP_OUT_SYNCHRONIZE_HOSTEND),
408 timeset->waitns = MCDI_DWORD(data, PTP_OUT_SYNCHRONIZE_WAITNS);
409
410
411 start_ns = timeset->host_start & MC_NANOSECOND_MASK;
412 end_ns = timeset->host_end & MC_NANOSECOND_MASK;
413
414 if (end_ns < start_ns)
415 end_ns += NSEC_PER_SEC;
416
417 timeset->window = end_ns - start_ns;
418}
419
420
421
422
423
424
425
426
427
428static int efx_ptp_process_times(struct efx_nic *efx, u8 *synch_buf,
429 size_t response_length,
430 const struct pps_event_time *last_time)
431{
432 unsigned number_readings = (response_length /
433 MC_CMD_PTP_OUT_SYNCHRONIZE_TIMESET_LEN);
434 unsigned i;
435 unsigned total;
436 unsigned ngood = 0;
437 unsigned last_good = 0;
438 struct efx_ptp_data *ptp = efx->ptp_data;
439 u32 last_sec;
440 u32 start_sec;
441 struct timespec delta;
442
443 if (number_readings == 0)
444 return -EAGAIN;
445
446
447
448
449 for (i = 0; i < number_readings; i++) {
450 efx_ptp_read_timeset(synch_buf, &ptp->timeset[i]);
451 synch_buf += MC_CMD_PTP_OUT_SYNCHRONIZE_TIMESET_LEN;
452 }
453
454
455
456
457
458 total = 0;
459 for (i = 0; i < number_readings; i++)
460 if (ptp->timeset[i].window > ptp->timeset[i].waitns) {
461 unsigned win;
462
463 win = ptp->timeset[i].window - ptp->timeset[i].waitns;
464 if (win >= MIN_SYNCHRONISATION_NS &&
465 win < MAX_SYNCHRONISATION_NS) {
466 total += ptp->timeset[i].window;
467 ngood++;
468 last_good = i;
469 }
470 }
471
472 if (ngood == 0) {
473 netif_warn(efx, drv, efx->net_dev,
474 "PTP no suitable synchronisations %dns\n",
475 ptp->base_sync_ns);
476 return -EAGAIN;
477 }
478
479
480 ptp->last_sync_ns = DIV_ROUND_UP(total, ngood);
481 if (!ptp->base_sync_valid || (ptp->last_sync_ns < ptp->base_sync_ns)) {
482 ptp->base_sync_valid = true;
483 ptp->base_sync_ns = ptp->last_sync_ns;
484 }
485
486
487 delta.tv_nsec =
488 ptp->timeset[last_good].nanoseconds +
489 last_time->ts_real.tv_nsec -
490 (ptp->timeset[last_good].host_start & MC_NANOSECOND_MASK);
491
492
493
494
495
496
497 start_sec = ptp->timeset[last_good].host_start >> MC_NANOSECOND_BITS;
498 last_sec = last_time->ts_real.tv_sec & MC_SECOND_MASK;
499 if (start_sec != last_sec) {
500 if (((start_sec + 1) & MC_SECOND_MASK) != last_sec) {
501 netif_warn(efx, hw, efx->net_dev,
502 "PTP bad synchronisation seconds\n");
503 return -EAGAIN;
504 } else {
505 delta.tv_sec = 1;
506 }
507 } else {
508 delta.tv_sec = 0;
509 }
510
511 ptp->host_time_pps = *last_time;
512 pps_sub_ts(&ptp->host_time_pps, delta);
513
514 return 0;
515}
516
517
518static int efx_ptp_synchronize(struct efx_nic *efx, unsigned int num_readings)
519{
520 struct efx_ptp_data *ptp = efx->ptp_data;
521 u8 synch_buf[MC_CMD_PTP_OUT_SYNCHRONIZE_LENMAX];
522 size_t response_length;
523 int rc;
524 unsigned long timeout;
525 struct pps_event_time last_time = {};
526 unsigned int loops = 0;
527 int *start = ptp->start.addr;
528
529 MCDI_SET_DWORD(synch_buf, PTP_IN_OP, MC_CMD_PTP_OP_SYNCHRONIZE);
530 MCDI_SET_DWORD(synch_buf, PTP_IN_SYNCHRONIZE_NUMTIMESETS,
531 num_readings);
532 MCDI_SET_DWORD(synch_buf, PTP_IN_SYNCHRONIZE_START_ADDR_LO,
533 (u32)ptp->start.dma_addr);
534 MCDI_SET_DWORD(synch_buf, PTP_IN_SYNCHRONIZE_START_ADDR_HI,
535 (u32)((u64)ptp->start.dma_addr >> 32));
536
537
538 ACCESS_ONCE(*start) = 0;
539 efx_mcdi_rpc_start(efx, MC_CMD_PTP, synch_buf,
540 MC_CMD_PTP_IN_SYNCHRONIZE_LEN);
541
542
543 timeout = jiffies + msecs_to_jiffies(MAX_SYNCHRONISE_WAIT_MS);
544 while (!ACCESS_ONCE(*start) && (time_before(jiffies, timeout))) {
545 udelay(20);
546 loops++;
547 }
548
549 if (ACCESS_ONCE(*start))
550 efx_ptp_send_times(efx, &last_time);
551
552
553 rc = efx_mcdi_rpc_finish(efx, MC_CMD_PTP,
554 MC_CMD_PTP_IN_SYNCHRONIZE_LEN,
555 synch_buf, sizeof(synch_buf),
556 &response_length);
557 if (rc == 0)
558 rc = efx_ptp_process_times(efx, synch_buf, response_length,
559 &last_time);
560
561 return rc;
562}
563
564
565static int efx_ptp_xmit_skb(struct efx_nic *efx, struct sk_buff *skb)
566{
567 u8 *txbuf = efx->ptp_data->txbuf;
568 struct skb_shared_hwtstamps timestamps;
569 int rc = -EIO;
570
571 size_t len = ALIGN(MC_CMD_PTP_IN_TRANSMIT_LEN(skb->len), 4);
572 u8 txtime[MC_CMD_PTP_OUT_TRANSMIT_LEN];
573
574 MCDI_SET_DWORD(txbuf, PTP_IN_OP, MC_CMD_PTP_OP_TRANSMIT);
575 MCDI_SET_DWORD(txbuf, PTP_IN_TRANSMIT_LENGTH, skb->len);
576 if (skb_shinfo(skb)->nr_frags != 0) {
577 rc = skb_linearize(skb);
578 if (rc != 0)
579 goto fail;
580 }
581
582 if (skb->ip_summed == CHECKSUM_PARTIAL) {
583 rc = skb_checksum_help(skb);
584 if (rc != 0)
585 goto fail;
586 }
587 skb_copy_from_linear_data(skb,
588 &txbuf[MC_CMD_PTP_IN_TRANSMIT_PACKET_OFST],
589 len);
590 rc = efx_mcdi_rpc(efx, MC_CMD_PTP, txbuf, len, txtime,
591 sizeof(txtime), &len);
592 if (rc != 0)
593 goto fail;
594
595 memset(×tamps, 0, sizeof(timestamps));
596 timestamps.hwtstamp = ktime_set(
597 MCDI_DWORD(txtime, PTP_OUT_TRANSMIT_SECONDS),
598 MCDI_DWORD(txtime, PTP_OUT_TRANSMIT_NANOSECONDS));
599
600 skb_tstamp_tx(skb, ×tamps);
601
602 rc = 0;
603
604fail:
605 dev_kfree_skb(skb);
606
607 return rc;
608}
609
610static void efx_ptp_drop_time_expired_events(struct efx_nic *efx)
611{
612 struct efx_ptp_data *ptp = efx->ptp_data;
613 struct list_head *cursor;
614 struct list_head *next;
615
616
617 spin_lock_bh(&ptp->evt_lock);
618 if (!list_empty(&ptp->evt_list)) {
619 list_for_each_safe(cursor, next, &ptp->evt_list) {
620 struct efx_ptp_event_rx *evt;
621
622 evt = list_entry(cursor, struct efx_ptp_event_rx,
623 link);
624 if (time_after(jiffies, evt->expiry)) {
625 list_move(&evt->link, &ptp->evt_free_list);
626 netif_warn(efx, hw, efx->net_dev,
627 "PTP rx event dropped\n");
628 }
629 }
630 }
631 spin_unlock_bh(&ptp->evt_lock);
632}
633
634static enum ptp_packet_state efx_ptp_match_rx(struct efx_nic *efx,
635 struct sk_buff *skb)
636{
637 struct efx_ptp_data *ptp = efx->ptp_data;
638 bool evts_waiting;
639 struct list_head *cursor;
640 struct list_head *next;
641 struct efx_ptp_match *match;
642 enum ptp_packet_state rc = PTP_PACKET_STATE_UNMATCHED;
643
644 spin_lock_bh(&ptp->evt_lock);
645 evts_waiting = !list_empty(&ptp->evt_list);
646 spin_unlock_bh(&ptp->evt_lock);
647
648 if (!evts_waiting)
649 return PTP_PACKET_STATE_UNMATCHED;
650
651 match = (struct efx_ptp_match *)skb->cb;
652
653 spin_lock_bh(&ptp->evt_lock);
654 list_for_each_safe(cursor, next, &ptp->evt_list) {
655 struct efx_ptp_event_rx *evt;
656
657 evt = list_entry(cursor, struct efx_ptp_event_rx, link);
658 if ((evt->seq0 == match->words[0]) &&
659 (evt->seq1 == match->words[1])) {
660 struct skb_shared_hwtstamps *timestamps;
661
662
663 timestamps = skb_hwtstamps(skb);
664 timestamps->hwtstamp = evt->hwtimestamp;
665
666 match->state = PTP_PACKET_STATE_MATCHED;
667 rc = PTP_PACKET_STATE_MATCHED;
668 list_move(&evt->link, &ptp->evt_free_list);
669 break;
670 }
671 }
672 spin_unlock_bh(&ptp->evt_lock);
673
674 return rc;
675}
676
677
678
679
680
681
682
683static bool efx_ptp_process_events(struct efx_nic *efx, struct sk_buff_head *q)
684{
685 struct efx_ptp_data *ptp = efx->ptp_data;
686 bool rc = false;
687 struct sk_buff *skb;
688
689 while ((skb = skb_dequeue(&ptp->rxq))) {
690 struct efx_ptp_match *match;
691
692 match = (struct efx_ptp_match *)skb->cb;
693 if (match->state == PTP_PACKET_STATE_MATCH_UNWANTED) {
694 __skb_queue_tail(q, skb);
695 } else if (efx_ptp_match_rx(efx, skb) ==
696 PTP_PACKET_STATE_MATCHED) {
697 rc = true;
698 __skb_queue_tail(q, skb);
699 } else if (time_after(jiffies, match->expiry)) {
700 match->state = PTP_PACKET_STATE_TIMED_OUT;
701 netif_warn(efx, rx_err, efx->net_dev,
702 "PTP packet - no timestamp seen\n");
703 __skb_queue_tail(q, skb);
704 } else {
705
706 skb_queue_head(&ptp->rxq, skb);
707 break;
708 }
709 }
710
711 return rc;
712}
713
714
715static inline void efx_ptp_process_rx(struct efx_nic *efx, struct sk_buff *skb)
716{
717 local_bh_disable();
718 netif_receive_skb(skb);
719 local_bh_enable();
720}
721
722static int efx_ptp_start(struct efx_nic *efx)
723{
724 struct efx_ptp_data *ptp = efx->ptp_data;
725 struct efx_filter_spec rxfilter;
726 int rc;
727
728 ptp->reset_required = false;
729
730
731
732
733 efx_filter_init_rx(&rxfilter, EFX_FILTER_PRI_REQUIRED, 0,
734 efx_rx_queue_index(
735 efx_channel_get_rx_queue(ptp->channel)));
736 rc = efx_filter_set_ipv4_local(&rxfilter, IPPROTO_UDP,
737 htonl(PTP_ADDRESS),
738 htons(PTP_EVENT_PORT));
739 if (rc != 0)
740 return rc;
741
742 rc = efx_filter_insert_filter(efx, &rxfilter, true);
743 if (rc < 0)
744 return rc;
745 ptp->rxfilter_event = rc;
746
747 efx_filter_init_rx(&rxfilter, EFX_FILTER_PRI_REQUIRED, 0,
748 efx_rx_queue_index(
749 efx_channel_get_rx_queue(ptp->channel)));
750 rc = efx_filter_set_ipv4_local(&rxfilter, IPPROTO_UDP,
751 htonl(PTP_ADDRESS),
752 htons(PTP_GENERAL_PORT));
753 if (rc != 0)
754 goto fail;
755
756 rc = efx_filter_insert_filter(efx, &rxfilter, true);
757 if (rc < 0)
758 goto fail;
759 ptp->rxfilter_general = rc;
760
761 rc = efx_ptp_enable(efx);
762 if (rc != 0)
763 goto fail2;
764
765 ptp->evt_frag_idx = 0;
766 ptp->current_adjfreq = 0;
767 ptp->rxfilter_installed = true;
768
769 return 0;
770
771fail2:
772 efx_filter_remove_id_safe(efx, EFX_FILTER_PRI_REQUIRED,
773 ptp->rxfilter_general);
774fail:
775 efx_filter_remove_id_safe(efx, EFX_FILTER_PRI_REQUIRED,
776 ptp->rxfilter_event);
777
778 return rc;
779}
780
781static int efx_ptp_stop(struct efx_nic *efx)
782{
783 struct efx_ptp_data *ptp = efx->ptp_data;
784 int rc = efx_ptp_disable(efx);
785 struct list_head *cursor;
786 struct list_head *next;
787
788 if (ptp->rxfilter_installed) {
789 efx_filter_remove_id_safe(efx, EFX_FILTER_PRI_REQUIRED,
790 ptp->rxfilter_general);
791 efx_filter_remove_id_safe(efx, EFX_FILTER_PRI_REQUIRED,
792 ptp->rxfilter_event);
793 ptp->rxfilter_installed = false;
794 }
795
796
797 efx_ptp_deliver_rx_queue(&efx->ptp_data->rxq);
798 skb_queue_purge(&efx->ptp_data->txq);
799
800
801 spin_lock_bh(&efx->ptp_data->evt_lock);
802 list_for_each_safe(cursor, next, &efx->ptp_data->evt_list) {
803 list_move(cursor, &efx->ptp_data->evt_free_list);
804 }
805 spin_unlock_bh(&efx->ptp_data->evt_lock);
806
807 return rc;
808}
809
810static void efx_ptp_pps_worker(struct work_struct *work)
811{
812 struct efx_ptp_data *ptp =
813 container_of(work, struct efx_ptp_data, pps_work);
814 struct efx_nic *efx = ptp->channel->efx;
815 struct ptp_clock_event ptp_evt;
816
817 if (efx_ptp_synchronize(efx, PTP_SYNC_ATTEMPTS))
818 return;
819
820 ptp_evt.type = PTP_CLOCK_PPSUSR;
821 ptp_evt.pps_times = ptp->host_time_pps;
822 ptp_clock_event(ptp->phc_clock, &ptp_evt);
823}
824
825
826
827static void efx_ptp_worker(struct work_struct *work)
828{
829 struct efx_ptp_data *ptp_data =
830 container_of(work, struct efx_ptp_data, work);
831 struct efx_nic *efx = ptp_data->channel->efx;
832 struct sk_buff *skb;
833 struct sk_buff_head tempq;
834
835 if (ptp_data->reset_required) {
836 efx_ptp_stop(efx);
837 efx_ptp_start(efx);
838 return;
839 }
840
841 efx_ptp_drop_time_expired_events(efx);
842
843 __skb_queue_head_init(&tempq);
844 if (efx_ptp_process_events(efx, &tempq) ||
845 !skb_queue_empty(&ptp_data->txq)) {
846
847 while ((skb = skb_dequeue(&ptp_data->txq)))
848 efx_ptp_xmit_skb(efx, skb);
849 }
850
851 while ((skb = __skb_dequeue(&tempq)))
852 efx_ptp_process_rx(efx, skb);
853}
854
855
856
857
858
859
860static int efx_ptp_probe_channel(struct efx_channel *channel)
861{
862 struct efx_nic *efx = channel->efx;
863 struct efx_ptp_data *ptp;
864 int rc = 0;
865 unsigned int pos;
866
867 channel->irq_moderation = 0;
868 channel->rx_queue.core_index = 0;
869
870 ptp = kzalloc(sizeof(struct efx_ptp_data), GFP_KERNEL);
871 efx->ptp_data = ptp;
872 if (!efx->ptp_data)
873 return -ENOMEM;
874
875 rc = efx_nic_alloc_buffer(efx, &ptp->start, sizeof(int));
876 if (rc != 0)
877 goto fail1;
878
879 ptp->channel = channel;
880 skb_queue_head_init(&ptp->rxq);
881 skb_queue_head_init(&ptp->txq);
882 ptp->workwq = create_singlethread_workqueue("sfc_ptp");
883 if (!ptp->workwq) {
884 rc = -ENOMEM;
885 goto fail2;
886 }
887
888 INIT_WORK(&ptp->work, efx_ptp_worker);
889 ptp->config.flags = 0;
890 ptp->config.tx_type = HWTSTAMP_TX_OFF;
891 ptp->config.rx_filter = HWTSTAMP_FILTER_NONE;
892 INIT_LIST_HEAD(&ptp->evt_list);
893 INIT_LIST_HEAD(&ptp->evt_free_list);
894 spin_lock_init(&ptp->evt_lock);
895 for (pos = 0; pos < MAX_RECEIVE_EVENTS; pos++)
896 list_add(&ptp->rx_evts[pos].link, &ptp->evt_free_list);
897
898 ptp->phc_clock_info.owner = THIS_MODULE;
899 snprintf(ptp->phc_clock_info.name,
900 sizeof(ptp->phc_clock_info.name),
901 "%pm", efx->net_dev->perm_addr);
902 ptp->phc_clock_info.max_adj = MAX_PPB;
903 ptp->phc_clock_info.n_alarm = 0;
904 ptp->phc_clock_info.n_ext_ts = 0;
905 ptp->phc_clock_info.n_per_out = 0;
906 ptp->phc_clock_info.pps = 1;
907 ptp->phc_clock_info.adjfreq = efx_phc_adjfreq;
908 ptp->phc_clock_info.adjtime = efx_phc_adjtime;
909 ptp->phc_clock_info.gettime = efx_phc_gettime;
910 ptp->phc_clock_info.settime = efx_phc_settime;
911 ptp->phc_clock_info.enable = efx_phc_enable;
912
913 ptp->phc_clock = ptp_clock_register(&ptp->phc_clock_info,
914 &efx->pci_dev->dev);
915 if (IS_ERR(ptp->phc_clock)) {
916 rc = PTR_ERR(ptp->phc_clock);
917 goto fail3;
918 }
919
920 INIT_WORK(&ptp->pps_work, efx_ptp_pps_worker);
921 ptp->pps_workwq = create_singlethread_workqueue("sfc_pps");
922 if (!ptp->pps_workwq) {
923 rc = -ENOMEM;
924 goto fail4;
925 }
926 ptp->nic_ts_enabled = false;
927
928 return 0;
929fail4:
930 ptp_clock_unregister(efx->ptp_data->phc_clock);
931
932fail3:
933 destroy_workqueue(efx->ptp_data->workwq);
934
935fail2:
936 efx_nic_free_buffer(efx, &ptp->start);
937
938fail1:
939 kfree(efx->ptp_data);
940 efx->ptp_data = NULL;
941
942 return rc;
943}
944
945static void efx_ptp_remove_channel(struct efx_channel *channel)
946{
947 struct efx_nic *efx = channel->efx;
948
949 if (!efx->ptp_data)
950 return;
951
952 (void)efx_ptp_disable(channel->efx);
953
954 cancel_work_sync(&efx->ptp_data->work);
955 cancel_work_sync(&efx->ptp_data->pps_work);
956
957 skb_queue_purge(&efx->ptp_data->rxq);
958 skb_queue_purge(&efx->ptp_data->txq);
959
960 ptp_clock_unregister(efx->ptp_data->phc_clock);
961
962 destroy_workqueue(efx->ptp_data->workwq);
963 destroy_workqueue(efx->ptp_data->pps_workwq);
964
965 efx_nic_free_buffer(efx, &efx->ptp_data->start);
966 kfree(efx->ptp_data);
967}
968
969static void efx_ptp_get_channel_name(struct efx_channel *channel,
970 char *buf, size_t len)
971{
972 snprintf(buf, len, "%s-ptp", channel->efx->name);
973}
974
975
976
977
978bool efx_ptp_is_ptp_tx(struct efx_nic *efx, struct sk_buff *skb)
979{
980 return efx->ptp_data &&
981 efx->ptp_data->enabled &&
982 skb->len >= PTP_MIN_LENGTH &&
983 skb->len <= MC_CMD_PTP_IN_TRANSMIT_PACKET_MAXNUM &&
984 likely(skb->protocol == htons(ETH_P_IP)) &&
985 ip_hdr(skb)->protocol == IPPROTO_UDP &&
986 udp_hdr(skb)->dest == htons(PTP_EVENT_PORT);
987}
988
989
990
991
992
993static bool efx_ptp_rx(struct efx_channel *channel, struct sk_buff *skb)
994{
995 struct efx_nic *efx = channel->efx;
996 struct efx_ptp_data *ptp = efx->ptp_data;
997 struct efx_ptp_match *match = (struct efx_ptp_match *)skb->cb;
998 u8 *match_data_012, *match_data_345;
999 unsigned int version;
1000
1001 match->expiry = jiffies + msecs_to_jiffies(PKT_EVENT_LIFETIME_MS);
1002
1003
1004 if (ptp->mode == MC_CMD_PTP_MODE_V1) {
1005 if (!pskb_may_pull(skb, PTP_V1_MIN_LENGTH)) {
1006 return false;
1007 }
1008 version = ntohs(*(__be16 *)&skb->data[PTP_V1_VERSION_OFFSET]);
1009 if (version != PTP_VERSION_V1) {
1010 return false;
1011 }
1012
1013
1014
1015
1016 match_data_012 = skb->data + PTP_V1_UUID_OFFSET;
1017 match_data_345 = skb->data + PTP_V1_UUID_OFFSET + 3;
1018 } else {
1019 if (!pskb_may_pull(skb, PTP_V2_MIN_LENGTH)) {
1020 return false;
1021 }
1022 version = skb->data[PTP_V2_VERSION_OFFSET];
1023 if ((version & PTP_VERSION_V2_MASK) != PTP_VERSION_V2) {
1024 return false;
1025 }
1026
1027
1028
1029
1030
1031
1032
1033
1034 match_data_345 = skb->data + PTP_V2_UUID_OFFSET + 5;
1035 if (ptp->mode == MC_CMD_PTP_MODE_V2) {
1036 match_data_012 = skb->data + PTP_V2_UUID_OFFSET + 2;
1037 } else {
1038 match_data_012 = skb->data + PTP_V2_UUID_OFFSET + 0;
1039 BUG_ON(ptp->mode != MC_CMD_PTP_MODE_V2_ENHANCED);
1040 }
1041 }
1042
1043
1044 if (ntohs(*(__be16 *)&skb->data[PTP_DPORT_OFFSET]) == PTP_EVENT_PORT) {
1045 struct skb_shared_hwtstamps *timestamps;
1046
1047 match->state = PTP_PACKET_STATE_UNMATCHED;
1048
1049
1050 timestamps = skb_hwtstamps(skb);
1051 memset(timestamps, 0, sizeof(*timestamps));
1052
1053
1054
1055
1056 BUILD_BUG_ON(PTP_V1_SEQUENCE_OFFSET != PTP_V2_SEQUENCE_OFFSET);
1057 BUILD_BUG_ON(PTP_V1_SEQUENCE_LENGTH != PTP_V2_SEQUENCE_LENGTH);
1058
1059
1060 match->words[0] = (match_data_012[0] |
1061 (match_data_012[1] << 8) |
1062 (match_data_012[2] << 16) |
1063 (match_data_345[0] << 24));
1064 match->words[1] = (match_data_345[1] |
1065 (match_data_345[2] << 8) |
1066 (skb->data[PTP_V1_SEQUENCE_OFFSET +
1067 PTP_V1_SEQUENCE_LENGTH - 1] <<
1068 16));
1069 } else {
1070 match->state = PTP_PACKET_STATE_MATCH_UNWANTED;
1071 }
1072
1073 skb_queue_tail(&ptp->rxq, skb);
1074 queue_work(ptp->workwq, &ptp->work);
1075
1076 return true;
1077}
1078
1079
1080
1081
1082
1083int efx_ptp_tx(struct efx_nic *efx, struct sk_buff *skb)
1084{
1085 struct efx_ptp_data *ptp = efx->ptp_data;
1086
1087 skb_queue_tail(&ptp->txq, skb);
1088
1089 if ((udp_hdr(skb)->dest == htons(PTP_EVENT_PORT)) &&
1090 (skb->len <= MC_CMD_PTP_IN_TRANSMIT_PACKET_MAXNUM))
1091 efx_xmit_hwtstamp_pending(skb);
1092 queue_work(ptp->workwq, &ptp->work);
1093
1094 return NETDEV_TX_OK;
1095}
1096
1097static int efx_ptp_change_mode(struct efx_nic *efx, bool enable_wanted,
1098 unsigned int new_mode)
1099{
1100 if ((enable_wanted != efx->ptp_data->enabled) ||
1101 (enable_wanted && (efx->ptp_data->mode != new_mode))) {
1102 int rc;
1103
1104 if (enable_wanted) {
1105
1106 if (efx->ptp_data->enabled &&
1107 (efx->ptp_data->mode != new_mode)) {
1108 efx->ptp_data->enabled = false;
1109 rc = efx_ptp_stop(efx);
1110 if (rc != 0)
1111 return rc;
1112 }
1113
1114
1115
1116
1117
1118 efx->ptp_data->mode = new_mode;
1119 rc = efx_ptp_start(efx);
1120 if (rc == 0) {
1121 rc = efx_ptp_synchronize(efx,
1122 PTP_SYNC_ATTEMPTS * 2);
1123 if (rc != 0)
1124 efx_ptp_stop(efx);
1125 }
1126 } else {
1127 rc = efx_ptp_stop(efx);
1128 }
1129
1130 if (rc != 0)
1131 return rc;
1132
1133 efx->ptp_data->enabled = enable_wanted;
1134 }
1135
1136 return 0;
1137}
1138
1139static int efx_ptp_ts_init(struct efx_nic *efx, struct hwtstamp_config *init)
1140{
1141 bool enable_wanted = false;
1142 unsigned int new_mode;
1143 int rc;
1144
1145 if (init->flags)
1146 return -EINVAL;
1147
1148 if ((init->tx_type != HWTSTAMP_TX_OFF) &&
1149 (init->tx_type != HWTSTAMP_TX_ON))
1150 return -ERANGE;
1151
1152 new_mode = efx->ptp_data->mode;
1153
1154 switch (init->rx_filter) {
1155 case HWTSTAMP_FILTER_NONE:
1156 break;
1157 case HWTSTAMP_FILTER_PTP_V1_L4_EVENT:
1158 case HWTSTAMP_FILTER_PTP_V1_L4_SYNC:
1159 case HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ:
1160 init->rx_filter = HWTSTAMP_FILTER_PTP_V1_L4_EVENT;
1161 new_mode = MC_CMD_PTP_MODE_V1;
1162 enable_wanted = true;
1163 break;
1164 case HWTSTAMP_FILTER_PTP_V2_L4_EVENT:
1165 case HWTSTAMP_FILTER_PTP_V2_L4_SYNC:
1166 case HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ:
1167
1168
1169
1170 init->rx_filter = HWTSTAMP_FILTER_PTP_V2_L4_EVENT;
1171 new_mode = MC_CMD_PTP_MODE_V2_ENHANCED;
1172 enable_wanted = true;
1173 break;
1174 case HWTSTAMP_FILTER_PTP_V2_EVENT:
1175 case HWTSTAMP_FILTER_PTP_V2_SYNC:
1176 case HWTSTAMP_FILTER_PTP_V2_DELAY_REQ:
1177 case HWTSTAMP_FILTER_PTP_V2_L2_EVENT:
1178 case HWTSTAMP_FILTER_PTP_V2_L2_SYNC:
1179 case HWTSTAMP_FILTER_PTP_V2_L2_DELAY_REQ:
1180
1181 return -ERANGE;
1182 break;
1183 default:
1184 return -ERANGE;
1185 }
1186
1187 if (init->tx_type != HWTSTAMP_TX_OFF)
1188 enable_wanted = true;
1189
1190
1191
1192
1193
1194
1195 rc = efx_ptp_change_mode(efx, enable_wanted, new_mode);
1196 if ((rc != 0) && (new_mode == MC_CMD_PTP_MODE_V2_ENHANCED))
1197 rc = efx_ptp_change_mode(efx, enable_wanted, MC_CMD_PTP_MODE_V2);
1198 if (rc != 0)
1199 return rc;
1200
1201 efx->ptp_data->config = *init;
1202
1203 return 0;
1204}
1205
1206void efx_ptp_get_ts_info(struct efx_nic *efx, struct ethtool_ts_info *ts_info)
1207{
1208 struct efx_ptp_data *ptp = efx->ptp_data;
1209
1210 if (!ptp)
1211 return;
1212
1213 ts_info->so_timestamping |= (SOF_TIMESTAMPING_TX_HARDWARE |
1214 SOF_TIMESTAMPING_RX_HARDWARE |
1215 SOF_TIMESTAMPING_RAW_HARDWARE);
1216 ts_info->phc_index = ptp_clock_index(ptp->phc_clock);
1217 ts_info->tx_types = 1 << HWTSTAMP_TX_OFF | 1 << HWTSTAMP_TX_ON;
1218 ts_info->rx_filters = (1 << HWTSTAMP_FILTER_NONE |
1219 1 << HWTSTAMP_FILTER_PTP_V1_L4_EVENT |
1220 1 << HWTSTAMP_FILTER_PTP_V1_L4_SYNC |
1221 1 << HWTSTAMP_FILTER_PTP_V1_L4_DELAY_REQ |
1222 1 << HWTSTAMP_FILTER_PTP_V2_L4_EVENT |
1223 1 << HWTSTAMP_FILTER_PTP_V2_L4_SYNC |
1224 1 << HWTSTAMP_FILTER_PTP_V2_L4_DELAY_REQ);
1225}
1226
1227int efx_ptp_ioctl(struct efx_nic *efx, struct ifreq *ifr, int cmd)
1228{
1229 struct hwtstamp_config config;
1230 int rc;
1231
1232
1233 if (!efx->ptp_data)
1234 return -EOPNOTSUPP;
1235
1236 if (copy_from_user(&config, ifr->ifr_data, sizeof(config)))
1237 return -EFAULT;
1238
1239 rc = efx_ptp_ts_init(efx, &config);
1240 if (rc != 0)
1241 return rc;
1242
1243 return copy_to_user(ifr->ifr_data, &config, sizeof(config))
1244 ? -EFAULT : 0;
1245}
1246
1247static void ptp_event_failure(struct efx_nic *efx, int expected_frag_len)
1248{
1249 struct efx_ptp_data *ptp = efx->ptp_data;
1250
1251 netif_err(efx, hw, efx->net_dev,
1252 "PTP unexpected event length: got %d expected %d\n",
1253 ptp->evt_frag_idx, expected_frag_len);
1254 ptp->reset_required = true;
1255 queue_work(ptp->workwq, &ptp->work);
1256}
1257
1258
1259
1260
1261
1262static void ptp_event_rx(struct efx_nic *efx, struct efx_ptp_data *ptp)
1263{
1264 struct efx_ptp_event_rx *evt = NULL;
1265
1266 if (ptp->evt_frag_idx != 3) {
1267 ptp_event_failure(efx, 3);
1268 return;
1269 }
1270
1271 spin_lock_bh(&ptp->evt_lock);
1272 if (!list_empty(&ptp->evt_free_list)) {
1273 evt = list_first_entry(&ptp->evt_free_list,
1274 struct efx_ptp_event_rx, link);
1275 list_del(&evt->link);
1276
1277 evt->seq0 = EFX_QWORD_FIELD(ptp->evt_frags[2], MCDI_EVENT_DATA);
1278 evt->seq1 = (EFX_QWORD_FIELD(ptp->evt_frags[2],
1279 MCDI_EVENT_SRC) |
1280 (EFX_QWORD_FIELD(ptp->evt_frags[1],
1281 MCDI_EVENT_SRC) << 8) |
1282 (EFX_QWORD_FIELD(ptp->evt_frags[0],
1283 MCDI_EVENT_SRC) << 16));
1284 evt->hwtimestamp = ktime_set(
1285 EFX_QWORD_FIELD(ptp->evt_frags[0], MCDI_EVENT_DATA),
1286 EFX_QWORD_FIELD(ptp->evt_frags[1], MCDI_EVENT_DATA));
1287 evt->expiry = jiffies + msecs_to_jiffies(PKT_EVENT_LIFETIME_MS);
1288 list_add_tail(&evt->link, &ptp->evt_list);
1289
1290 queue_work(ptp->workwq, &ptp->work);
1291 } else {
1292 netif_err(efx, rx_err, efx->net_dev, "No free PTP event");
1293 }
1294 spin_unlock_bh(&ptp->evt_lock);
1295}
1296
1297static void ptp_event_fault(struct efx_nic *efx, struct efx_ptp_data *ptp)
1298{
1299 int code = EFX_QWORD_FIELD(ptp->evt_frags[0], MCDI_EVENT_DATA);
1300 if (ptp->evt_frag_idx != 1) {
1301 ptp_event_failure(efx, 1);
1302 return;
1303 }
1304
1305 netif_err(efx, hw, efx->net_dev, "PTP error %d\n", code);
1306}
1307
1308static void ptp_event_pps(struct efx_nic *efx, struct efx_ptp_data *ptp)
1309{
1310 if (ptp->nic_ts_enabled)
1311 queue_work(ptp->pps_workwq, &ptp->pps_work);
1312}
1313
1314void efx_ptp_event(struct efx_nic *efx, efx_qword_t *ev)
1315{
1316 struct efx_ptp_data *ptp = efx->ptp_data;
1317 int code = EFX_QWORD_FIELD(*ev, MCDI_EVENT_CODE);
1318
1319 if (!ptp->enabled)
1320 return;
1321
1322 if (ptp->evt_frag_idx == 0) {
1323 ptp->evt_code = code;
1324 } else if (ptp->evt_code != code) {
1325 netif_err(efx, hw, efx->net_dev,
1326 "PTP out of sequence event %d\n", code);
1327 ptp->evt_frag_idx = 0;
1328 }
1329
1330 ptp->evt_frags[ptp->evt_frag_idx++] = *ev;
1331 if (!MCDI_EVENT_FIELD(*ev, CONT)) {
1332
1333 switch (code) {
1334 case MCDI_EVENT_CODE_PTP_RX:
1335 ptp_event_rx(efx, ptp);
1336 break;
1337 case MCDI_EVENT_CODE_PTP_FAULT:
1338 ptp_event_fault(efx, ptp);
1339 break;
1340 case MCDI_EVENT_CODE_PTP_PPS:
1341 ptp_event_pps(efx, ptp);
1342 break;
1343 default:
1344 netif_err(efx, hw, efx->net_dev,
1345 "PTP unknown event %d\n", code);
1346 break;
1347 }
1348 ptp->evt_frag_idx = 0;
1349 } else if (MAX_EVENT_FRAGS == ptp->evt_frag_idx) {
1350 netif_err(efx, hw, efx->net_dev,
1351 "PTP too many event fragments\n");
1352 ptp->evt_frag_idx = 0;
1353 }
1354}
1355
1356static int efx_phc_adjfreq(struct ptp_clock_info *ptp, s32 delta)
1357{
1358 struct efx_ptp_data *ptp_data = container_of(ptp,
1359 struct efx_ptp_data,
1360 phc_clock_info);
1361 struct efx_nic *efx = ptp_data->channel->efx;
1362 u8 inadj[MC_CMD_PTP_IN_ADJUST_LEN];
1363 s64 adjustment_ns;
1364 int rc;
1365
1366 if (delta > MAX_PPB)
1367 delta = MAX_PPB;
1368 else if (delta < -MAX_PPB)
1369 delta = -MAX_PPB;
1370
1371
1372 adjustment_ns = (((s64)delta * PPB_SCALE_WORD) >>
1373 (PPB_EXTRA_BITS + MAX_PPB_BITS));
1374
1375 MCDI_SET_DWORD(inadj, PTP_IN_OP, MC_CMD_PTP_OP_ADJUST);
1376 MCDI_SET_DWORD(inadj, PTP_IN_ADJUST_FREQ_LO, (u32)adjustment_ns);
1377 MCDI_SET_DWORD(inadj, PTP_IN_ADJUST_FREQ_HI,
1378 (u32)(adjustment_ns >> 32));
1379 MCDI_SET_DWORD(inadj, PTP_IN_ADJUST_SECONDS, 0);
1380 MCDI_SET_DWORD(inadj, PTP_IN_ADJUST_NANOSECONDS, 0);
1381 rc = efx_mcdi_rpc(efx, MC_CMD_PTP, inadj, sizeof(inadj),
1382 NULL, 0, NULL);
1383 if (rc != 0)
1384 return rc;
1385
1386 ptp_data->current_adjfreq = delta;
1387 return 0;
1388}
1389
1390static int efx_phc_adjtime(struct ptp_clock_info *ptp, s64 delta)
1391{
1392 struct efx_ptp_data *ptp_data = container_of(ptp,
1393 struct efx_ptp_data,
1394 phc_clock_info);
1395 struct efx_nic *efx = ptp_data->channel->efx;
1396 struct timespec delta_ts = ns_to_timespec(delta);
1397 u8 inbuf[MC_CMD_PTP_IN_ADJUST_LEN];
1398
1399 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_ADJUST);
1400 MCDI_SET_DWORD(inbuf, PTP_IN_ADJUST_FREQ_LO, 0);
1401 MCDI_SET_DWORD(inbuf, PTP_IN_ADJUST_FREQ_HI, 0);
1402 MCDI_SET_DWORD(inbuf, PTP_IN_ADJUST_SECONDS, (u32)delta_ts.tv_sec);
1403 MCDI_SET_DWORD(inbuf, PTP_IN_ADJUST_NANOSECONDS, (u32)delta_ts.tv_nsec);
1404 return efx_mcdi_rpc(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
1405 NULL, 0, NULL);
1406}
1407
1408static int efx_phc_gettime(struct ptp_clock_info *ptp, struct timespec *ts)
1409{
1410 struct efx_ptp_data *ptp_data = container_of(ptp,
1411 struct efx_ptp_data,
1412 phc_clock_info);
1413 struct efx_nic *efx = ptp_data->channel->efx;
1414 u8 inbuf[MC_CMD_PTP_IN_READ_NIC_TIME_LEN];
1415 u8 outbuf[MC_CMD_PTP_OUT_READ_NIC_TIME_LEN];
1416 int rc;
1417
1418 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_READ_NIC_TIME);
1419
1420 rc = efx_mcdi_rpc(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),
1421 outbuf, sizeof(outbuf), NULL);
1422 if (rc != 0)
1423 return rc;
1424
1425 ts->tv_sec = MCDI_DWORD(outbuf, PTP_OUT_READ_NIC_TIME_SECONDS);
1426 ts->tv_nsec = MCDI_DWORD(outbuf, PTP_OUT_READ_NIC_TIME_NANOSECONDS);
1427 return 0;
1428}
1429
1430static int efx_phc_settime(struct ptp_clock_info *ptp,
1431 const struct timespec *e_ts)
1432{
1433
1434
1435
1436
1437 int rc;
1438 struct timespec time_now;
1439 struct timespec delta;
1440
1441 rc = efx_phc_gettime(ptp, &time_now);
1442 if (rc != 0)
1443 return rc;
1444
1445 delta = timespec_sub(*e_ts, time_now);
1446
1447 rc = efx_phc_adjtime(ptp, timespec_to_ns(&delta));
1448 if (rc != 0)
1449 return rc;
1450
1451 return 0;
1452}
1453
1454static int efx_phc_enable(struct ptp_clock_info *ptp,
1455 struct ptp_clock_request *request,
1456 int enable)
1457{
1458 struct efx_ptp_data *ptp_data = container_of(ptp,
1459 struct efx_ptp_data,
1460 phc_clock_info);
1461 if (request->type != PTP_CLK_REQ_PPS)
1462 return -EOPNOTSUPP;
1463
1464 ptp_data->nic_ts_enabled = !!enable;
1465 return 0;
1466}
1467
1468static const struct efx_channel_type efx_ptp_channel_type = {
1469 .handle_no_channel = efx_ptp_handle_no_channel,
1470 .pre_probe = efx_ptp_probe_channel,
1471 .post_remove = efx_ptp_remove_channel,
1472 .get_name = efx_ptp_get_channel_name,
1473
1474 .receive_skb = efx_ptp_rx,
1475 .keep_eventq = false,
1476};
1477
1478void efx_ptp_probe(struct efx_nic *efx)
1479{
1480
1481
1482
1483 if (efx_ptp_disable(efx) == 0)
1484 efx->extra_channel_type[EFX_EXTRA_CHANNEL_PTP] =
1485 &efx_ptp_channel_type;
1486}
1487