2244 lines · c
1// SPDX-License-Identifier: GPL-2.0-only2/****************************************************************************3 * Driver for Solarflare network controllers and boards4 * Copyright 2011-2013 Solarflare Communications Inc.5 */6 7/* Theory of operation:8 *9 * PTP support is assisted by firmware running on the MC, which provides10 * the hardware timestamping capabilities. Both transmitted and received11 * PTP event packets are queued onto internal queues for subsequent processing;12 * this is because the MC operations are relatively long and would block13 * block NAPI/interrupt operation.14 *15 * Receive event processing:16 * The event contains the packet's UUID and sequence number, together17 * with the hardware timestamp. The PTP receive packet queue is searched18 * for this UUID/sequence number and, if found, put on a pending queue.19 * Packets not matching are delivered without timestamps (MCDI events will20 * always arrive after the actual packet).21 * It is important for the operation of the PTP protocol that the ordering22 * of packets between the event and general port is maintained.23 *24 * Work queue processing:25 * If work waiting, synchronise host/hardware time26 *27 * Transmit: send packet through MC, which returns the transmission time28 * that is converted to an appropriate timestamp.29 *30 * Receive: the packet's reception time is converted to an appropriate31 * timestamp.32 */33#include <linux/ip.h>34#include <linux/udp.h>35#include <linux/time.h>36#include <linux/errno.h>37#include <linux/ktime.h>38#include <linux/module.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 "tx.h"47#include "nic.h" /* indirectly includes ptp.h */48#include "efx_channels.h"49 50/* Maximum number of events expected to make up a PTP event */51#define MAX_EVENT_FRAGS 352 53/* Maximum delay, ms, to begin synchronisation */54#define MAX_SYNCHRONISE_WAIT_MS 255 56/* How long, at most, to spend synchronising */57#define SYNCHRONISE_PERIOD_NS 25000058 59/* How often to update the shared memory time */60#define SYNCHRONISATION_GRANULARITY_NS 20061 62/* Minimum permitted length of a (corrected) synchronisation time */63#define DEFAULT_MIN_SYNCHRONISATION_NS 12064 65/* Maximum permitted length of a (corrected) synchronisation time */66#define MAX_SYNCHRONISATION_NS 100067 68/* How many (MC) receive events that can be queued */69#define MAX_RECEIVE_EVENTS 870 71/* Length of (modified) moving average. */72#define AVERAGE_LENGTH 1673 74/* How long an unmatched event or packet can be held */75#define PKT_EVENT_LIFETIME_MS 1076 77/* How long unused unicast filters can be held */78#define UCAST_FILTER_EXPIRY_JIFFIES msecs_to_jiffies(30000)79 80/* Offsets into PTP packet for identification. These offsets are from the81 * start of the IP header, not the MAC header. Note that neither PTP V1 nor82 * PTP V2 permit the use of IPV4 options.83 */84#define PTP_DPORT_OFFSET 2285 86#define PTP_V1_VERSION_LENGTH 287#define PTP_V1_VERSION_OFFSET 2888 89#define PTP_V1_SEQUENCE_LENGTH 290#define PTP_V1_SEQUENCE_OFFSET 5891 92/* The minimum length of a PTP V1 packet for offsets, etc. to be valid:93 * includes IP header.94 */95#define PTP_V1_MIN_LENGTH 6496 97#define PTP_V2_VERSION_LENGTH 198#define PTP_V2_VERSION_OFFSET 2999 100#define PTP_V2_SEQUENCE_LENGTH 2101#define PTP_V2_SEQUENCE_OFFSET 58102 103/* The minimum length of a PTP V2 packet for offsets, etc. to be valid:104 * includes IP header.105 */106#define PTP_V2_MIN_LENGTH 63107 108#define PTP_MIN_LENGTH 63109 110#define PTP_ADDR_IPV4 0xe0000181 /* 224.0.1.129 */111 112/* ff0e::181 */113static const struct in6_addr ptp_addr_ipv6 = { { {114 0xff, 0x0e, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0x01, 0x81 } } };115 116/* 01-1B-19-00-00-00 */117static const u8 ptp_addr_ether[ETH_ALEN] __aligned(2) = {118 0x01, 0x1b, 0x19, 0x00, 0x00, 0x00 };119 120#define PTP_EVENT_PORT 319121#define PTP_GENERAL_PORT 320122 123/* Annoyingly the format of the version numbers are different between124 * versions 1 and 2 so it isn't possible to simply look for 1 or 2.125 */126#define PTP_VERSION_V1 1127 128#define PTP_VERSION_V2 2129#define PTP_VERSION_V2_MASK 0x0f130 131enum ptp_packet_state {132 PTP_PACKET_STATE_UNMATCHED = 0,133 PTP_PACKET_STATE_MATCHED,134 PTP_PACKET_STATE_TIMED_OUT,135 PTP_PACKET_STATE_MATCH_UNWANTED136};137 138/* NIC synchronised with single word of time only comprising139 * partial seconds and full nanoseconds: 10^9 ~ 2^30 so 2 bits for seconds.140 */141#define MC_NANOSECOND_BITS 30142#define MC_NANOSECOND_MASK ((1 << MC_NANOSECOND_BITS) - 1)143#define MC_SECOND_MASK ((1 << (32 - MC_NANOSECOND_BITS)) - 1)144 145/* Maximum parts-per-billion adjustment that is acceptable */146#define MAX_PPB 1000000147 148/* Precalculate scale word to avoid long long division at runtime */149/* This is equivalent to 2^66 / 10^9. */150#define PPB_SCALE_WORD ((1LL << (57)) / 1953125LL)151 152/* How much to shift down after scaling to convert to FP40 */153#define PPB_SHIFT_FP40 26154/* ... and FP44. */155#define PPB_SHIFT_FP44 22156 157#define PTP_SYNC_ATTEMPTS 4158 159/**160 * struct efx_ptp_match - Matching structure, stored in sk_buff's cb area.161 * @expiry: Time after which the packet should be delivered irrespective of162 * event arrival.163 * @state: The state of the packet - whether it is ready for processing or164 * whether that is of no interest.165 */166struct efx_ptp_match {167 unsigned long expiry;168 enum ptp_packet_state state;169};170 171/**172 * struct efx_ptp_event_rx - A PTP receive event (from MC)173 * @link: list of events174 * @seq0: First part of (PTP) UUID175 * @seq1: Second part of (PTP) UUID and sequence number176 * @hwtimestamp: Event timestamp177 * @expiry: Time which the packet arrived178 */179struct efx_ptp_event_rx {180 struct list_head link;181 u32 seq0;182 u32 seq1;183 ktime_t hwtimestamp;184 unsigned long expiry;185};186 187/**188 * struct efx_ptp_timeset - Synchronisation between host and MC189 * @host_start: Host time immediately before hardware timestamp taken190 * @major: Hardware timestamp, major191 * @minor: Hardware timestamp, minor192 * @host_end: Host time immediately after hardware timestamp taken193 * @wait: Number of NIC clock ticks between hardware timestamp being read and194 * host end time being seen195 * @window: Difference of host_end and host_start196 * @valid: Whether this timeset is valid197 */198struct efx_ptp_timeset {199 u32 host_start;200 u32 major;201 u32 minor;202 u32 host_end;203 u32 wait;204 u32 window; /* Derived: end - start, allowing for wrap */205};206 207/**208 * struct efx_ptp_rxfilter - Filter for PTP packets209 * @list: Node of the list where the filter is added210 * @ether_type: Network protocol of the filter (ETHER_P_IP / ETHER_P_IPV6)211 * @loc_port: UDP port of the filter (PTP_EVENT_PORT / PTP_GENERAL_PORT)212 * @loc_host: IPv4/v6 address of the filter213 * @expiry: time when the filter expires, in jiffies214 * @handle: Handle ID for the MCDI filters table215 */216struct efx_ptp_rxfilter {217 struct list_head list;218 __be16 ether_type;219 __be16 loc_port;220 __be32 loc_host[4];221 unsigned long expiry;222 int handle;223};224 225/**226 * struct efx_ptp_data - Precision Time Protocol (PTP) state227 * @efx: The NIC context228 * @channel: The PTP channel (for Medford and Medford2)229 * @rxq: Receive SKB queue (awaiting timestamps)230 * @txq: Transmit SKB queue231 * @workwq: Work queue for processing pending PTP operations232 * @work: Work task233 * @cleanup_work: Work task for periodic cleanup234 * @reset_required: A serious error has occurred and the PTP task needs to be235 * reset (disable, enable).236 * @rxfilters_mcast: Receive filters for multicast PTP packets237 * @rxfilters_ucast: Receive filters for unicast PTP packets238 * @config: Current timestamp configuration239 * @enabled: PTP operation enabled240 * @mode: Mode in which PTP operating (PTP version)241 * @ns_to_nic_time: Function to convert from scalar nanoseconds to NIC time242 * @nic_to_kernel_time: Function to convert from NIC to kernel time243 * @nic_time: contains time details244 * @nic_time.minor_max: Wrap point for NIC minor times245 * @nic_time.sync_event_diff_min: Minimum acceptable difference between time246 * in packet prefix and last MCDI time sync event i.e. how much earlier than247 * the last sync event time a packet timestamp can be.248 * @nic_time.sync_event_diff_max: Maximum acceptable difference between time249 * in packet prefix and last MCDI time sync event i.e. how much later than250 * the last sync event time a packet timestamp can be.251 * @nic_time.sync_event_minor_shift: Shift required to make minor time from252 * field in MCDI time sync event.253 * @min_synchronisation_ns: Minimum acceptable corrected sync window254 * @capabilities: Capabilities flags from the NIC255 * @ts_corrections: contains corrections details256 * @ts_corrections.ptp_tx: Required driver correction of PTP packet transmit257 * timestamps258 * @ts_corrections.ptp_rx: Required driver correction of PTP packet receive259 * timestamps260 * @ts_corrections.pps_out: PPS output error (information only)261 * @ts_corrections.pps_in: Required driver correction of PPS input timestamps262 * @ts_corrections.general_tx: Required driver correction of general packet263 * transmit timestamps264 * @ts_corrections.general_rx: Required driver correction of general packet265 * receive timestamps266 * @evt_frags: Partly assembled PTP events267 * @evt_frag_idx: Current fragment number268 * @evt_code: Last event code269 * @start: Address at which MC indicates ready for synchronisation270 * @host_time_pps: Host time at last PPS271 * @adjfreq_ppb_shift: Shift required to convert scaled parts-per-billion272 * frequency adjustment into a fixed point fractional nanosecond format.273 * @current_adjfreq: Current ppb adjustment.274 * @phc_clock: Pointer to registered phc device (if primary function)275 * @phc_clock_info: Registration structure for phc device276 * @pps_work: pps work task for handling pps events277 * @pps_workwq: pps work queue278 * @nic_ts_enabled: Flag indicating if NIC generated TS events are handled279 * @txbuf: Buffer for use when transmitting (PTP) packets to MC (avoids280 * allocations in main data path).281 * @good_syncs: Number of successful synchronisations.282 * @fast_syncs: Number of synchronisations requiring short delay283 * @bad_syncs: Number of failed synchronisations.284 * @sync_timeouts: Number of synchronisation timeouts285 * @no_time_syncs: Number of synchronisations with no good times.286 * @invalid_sync_windows: Number of sync windows with bad durations.287 * @undersize_sync_windows: Number of corrected sync windows that are too small288 * @oversize_sync_windows: Number of corrected sync windows that are too large289 * @rx_no_timestamp: Number of packets received without a timestamp.290 * @timeset: Last set of synchronisation statistics.291 * @xmit_skb: Transmit SKB function.292 */293struct efx_ptp_data {294 struct efx_nic *efx;295 struct efx_channel *channel;296 struct sk_buff_head rxq;297 struct sk_buff_head txq;298 struct workqueue_struct *workwq;299 struct work_struct work;300 struct delayed_work cleanup_work;301 bool reset_required;302 struct list_head rxfilters_mcast;303 struct list_head rxfilters_ucast;304 struct kernel_hwtstamp_config config;305 bool enabled;306 unsigned int mode;307 void (*ns_to_nic_time)(s64 ns, u32 *nic_major, u32 *nic_minor);308 ktime_t (*nic_to_kernel_time)(u32 nic_major, u32 nic_minor,309 s32 correction);310 struct {311 u32 minor_max;312 u32 sync_event_diff_min;313 u32 sync_event_diff_max;314 unsigned int sync_event_minor_shift;315 } nic_time;316 unsigned int min_synchronisation_ns;317 unsigned int capabilities;318 struct {319 s32 ptp_tx;320 s32 ptp_rx;321 s32 pps_out;322 s32 pps_in;323 s32 general_tx;324 s32 general_rx;325 } ts_corrections;326 efx_qword_t evt_frags[MAX_EVENT_FRAGS];327 int evt_frag_idx;328 int evt_code;329 struct efx_buffer start;330 struct pps_event_time host_time_pps;331 unsigned int adjfreq_ppb_shift;332 s64 current_adjfreq;333 struct ptp_clock *phc_clock;334 struct ptp_clock_info phc_clock_info;335 struct work_struct pps_work;336 struct workqueue_struct *pps_workwq;337 bool nic_ts_enabled;338 efx_dword_t txbuf[MCDI_TX_BUF_LEN(MC_CMD_PTP_IN_TRANSMIT_LENMAX)];339 340 unsigned int good_syncs;341 unsigned int fast_syncs;342 unsigned int bad_syncs;343 unsigned int sync_timeouts;344 unsigned int no_time_syncs;345 unsigned int invalid_sync_windows;346 unsigned int undersize_sync_windows;347 unsigned int oversize_sync_windows;348 unsigned int rx_no_timestamp;349 struct efx_ptp_timeset350 timeset[MC_CMD_PTP_OUT_SYNCHRONIZE_TIMESET_MAXNUM];351 void (*xmit_skb)(struct efx_nic *efx, struct sk_buff *skb);352};353 354static int efx_phc_adjfine(struct ptp_clock_info *ptp, long scaled_ppm);355static int efx_phc_adjtime(struct ptp_clock_info *ptp, s64 delta);356static int efx_phc_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts);357static int efx_phc_settime(struct ptp_clock_info *ptp,358 const struct timespec64 *e_ts);359static int efx_phc_enable(struct ptp_clock_info *ptp,360 struct ptp_clock_request *request, int on);361static int efx_ptp_insert_unicast_filter(struct efx_nic *efx,362 struct sk_buff *skb);363 364bool efx_ptp_use_mac_tx_timestamps(struct efx_nic *efx)365{366 return efx_has_cap(efx, TX_MAC_TIMESTAMPING);367}368 369/* PTP 'extra' channel is still a traffic channel, but we only create TX queues370 * if PTP uses MAC TX timestamps, not if PTP uses the MC directly to transmit.371 */372static bool efx_ptp_want_txqs(struct efx_channel *channel)373{374 return efx_ptp_use_mac_tx_timestamps(channel->efx);375}376 377#define PTP_SW_STAT(ext_name, field_name) \378 { #ext_name, 0, offsetof(struct efx_ptp_data, field_name) }379#define PTP_MC_STAT(ext_name, mcdi_name) \380 { #ext_name, 32, MC_CMD_PTP_OUT_STATUS_STATS_ ## mcdi_name ## _OFST }381static const struct efx_hw_stat_desc efx_ptp_stat_desc[] = {382 PTP_SW_STAT(ptp_good_syncs, good_syncs),383 PTP_SW_STAT(ptp_fast_syncs, fast_syncs),384 PTP_SW_STAT(ptp_bad_syncs, bad_syncs),385 PTP_SW_STAT(ptp_sync_timeouts, sync_timeouts),386 PTP_SW_STAT(ptp_no_time_syncs, no_time_syncs),387 PTP_SW_STAT(ptp_invalid_sync_windows, invalid_sync_windows),388 PTP_SW_STAT(ptp_undersize_sync_windows, undersize_sync_windows),389 PTP_SW_STAT(ptp_oversize_sync_windows, oversize_sync_windows),390 PTP_SW_STAT(ptp_rx_no_timestamp, rx_no_timestamp),391 PTP_MC_STAT(ptp_tx_timestamp_packets, TX),392 PTP_MC_STAT(ptp_rx_timestamp_packets, RX),393 PTP_MC_STAT(ptp_timestamp_packets, TS),394 PTP_MC_STAT(ptp_filter_matches, FM),395 PTP_MC_STAT(ptp_non_filter_matches, NFM),396};397#define PTP_STAT_COUNT ARRAY_SIZE(efx_ptp_stat_desc)398static const unsigned long efx_ptp_stat_mask[] = {399 [0 ... BITS_TO_LONGS(PTP_STAT_COUNT) - 1] = ~0UL,400};401 402size_t efx_ptp_describe_stats(struct efx_nic *efx, u8 *strings)403{404 if (!efx->ptp_data)405 return 0;406 407 return efx_nic_describe_stats(efx_ptp_stat_desc, PTP_STAT_COUNT,408 efx_ptp_stat_mask, strings);409}410 411size_t efx_ptp_update_stats(struct efx_nic *efx, u64 *stats)412{413 MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_STATUS_LEN);414 MCDI_DECLARE_BUF(outbuf, MC_CMD_PTP_OUT_STATUS_LEN);415 size_t i;416 int rc;417 418 if (!efx->ptp_data)419 return 0;420 421 /* Copy software statistics */422 for (i = 0; i < PTP_STAT_COUNT; i++) {423 if (efx_ptp_stat_desc[i].dma_width)424 continue;425 stats[i] = *(unsigned int *)((char *)efx->ptp_data +426 efx_ptp_stat_desc[i].offset);427 }428 429 /* Fetch MC statistics. We *must* fill in all statistics or430 * risk leaking kernel memory to userland, so if the MCDI431 * request fails we pretend we got zeroes.432 */433 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_STATUS);434 MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);435 rc = efx_mcdi_rpc(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),436 outbuf, sizeof(outbuf), NULL);437 if (rc)438 memset(outbuf, 0, sizeof(outbuf));439 efx_nic_update_stats(efx_ptp_stat_desc, PTP_STAT_COUNT,440 efx_ptp_stat_mask,441 stats, _MCDI_PTR(outbuf, 0), false);442 443 return PTP_STAT_COUNT;444}445 446/* To convert from s27 format to ns we multiply then divide by a power of 2.447 * For the conversion from ns to s27, the operation is also converted to a448 * multiply and shift.449 */450#define S27_TO_NS_SHIFT (27)451#define NS_TO_S27_MULT (((1ULL << 63) + NSEC_PER_SEC / 2) / NSEC_PER_SEC)452#define NS_TO_S27_SHIFT (63 - S27_TO_NS_SHIFT)453#define S27_MINOR_MAX (1 << S27_TO_NS_SHIFT)454 455/* For Huntington platforms NIC time is in seconds and fractions of a second456 * where the minor register only uses 27 bits in units of 2^-27s.457 */458static void efx_ptp_ns_to_s27(s64 ns, u32 *nic_major, u32 *nic_minor)459{460 struct timespec64 ts = ns_to_timespec64(ns);461 u32 maj = (u32)ts.tv_sec;462 u32 min = (u32)(((u64)ts.tv_nsec * NS_TO_S27_MULT +463 (1ULL << (NS_TO_S27_SHIFT - 1))) >> NS_TO_S27_SHIFT);464 465 /* The conversion can result in the minor value exceeding the maximum.466 * In this case, round up to the next second.467 */468 if (min >= S27_MINOR_MAX) {469 min -= S27_MINOR_MAX;470 maj++;471 }472 473 *nic_major = maj;474 *nic_minor = min;475}476 477static inline ktime_t efx_ptp_s27_to_ktime(u32 nic_major, u32 nic_minor)478{479 u32 ns = (u32)(((u64)nic_minor * NSEC_PER_SEC +480 (1ULL << (S27_TO_NS_SHIFT - 1))) >> S27_TO_NS_SHIFT);481 return ktime_set(nic_major, ns);482}483 484static ktime_t efx_ptp_s27_to_ktime_correction(u32 nic_major, u32 nic_minor,485 s32 correction)486{487 /* Apply the correction and deal with carry */488 nic_minor += correction;489 if ((s32)nic_minor < 0) {490 nic_minor += S27_MINOR_MAX;491 nic_major--;492 } else if (nic_minor >= S27_MINOR_MAX) {493 nic_minor -= S27_MINOR_MAX;494 nic_major++;495 }496 497 return efx_ptp_s27_to_ktime(nic_major, nic_minor);498}499 500/* For Medford2 platforms the time is in seconds and quarter nanoseconds. */501static void efx_ptp_ns_to_s_qns(s64 ns, u32 *nic_major, u32 *nic_minor)502{503 struct timespec64 ts = ns_to_timespec64(ns);504 505 *nic_major = (u32)ts.tv_sec;506 *nic_minor = ts.tv_nsec * 4;507}508 509static ktime_t efx_ptp_s_qns_to_ktime_correction(u32 nic_major, u32 nic_minor,510 s32 correction)511{512 ktime_t kt;513 514 nic_minor = DIV_ROUND_CLOSEST(nic_minor, 4);515 correction = DIV_ROUND_CLOSEST(correction, 4);516 517 kt = ktime_set(nic_major, nic_minor);518 519 if (correction >= 0)520 kt = ktime_add_ns(kt, (u64)correction);521 else522 kt = ktime_sub_ns(kt, (u64)-correction);523 return kt;524}525 526struct efx_channel *efx_ptp_channel(struct efx_nic *efx)527{528 return efx->ptp_data ? efx->ptp_data->channel : NULL;529}530 531void efx_ptp_update_channel(struct efx_nic *efx, struct efx_channel *channel)532{533 if (efx->ptp_data)534 efx->ptp_data->channel = channel;535}536 537static u32 last_sync_timestamp_major(struct efx_nic *efx)538{539 struct efx_channel *channel = efx_ptp_channel(efx);540 u32 major = 0;541 542 if (channel)543 major = channel->sync_timestamp_major;544 return major;545}546 547/* The 8000 series and later can provide the time from the MAC, which is only548 * 48 bits long and provides meta-information in the top 2 bits.549 */550static ktime_t551efx_ptp_mac_nic_to_ktime_correction(struct efx_nic *efx,552 struct efx_ptp_data *ptp,553 u32 nic_major, u32 nic_minor,554 s32 correction)555{556 u32 sync_timestamp;557 ktime_t kt = { 0 };558 s16 delta;559 560 if (!(nic_major & 0x80000000)) {561 WARN_ON_ONCE(nic_major >> 16);562 563 /* Medford provides 48 bits of timestamp, so we must get the top564 * 16 bits from the timesync event state.565 *566 * We only have the lower 16 bits of the time now, but we do567 * have a full resolution timestamp at some point in past. As568 * long as the difference between the (real) now and the sync569 * is less than 2^15, then we can reconstruct the difference570 * between those two numbers using only the lower 16 bits of571 * each.572 *573 * Put another way574 *575 * a - b = ((a mod k) - b) mod k576 *577 * when -k/2 < (a-b) < k/2. In our case k is 2^16. We know578 * (a mod k) and b, so can calculate the delta, a - b.579 *580 */581 sync_timestamp = last_sync_timestamp_major(efx);582 583 /* Because delta is s16 this does an implicit mask down to584 * 16 bits which is what we need, assuming585 * MEDFORD_TX_SECS_EVENT_BITS is 16. delta is signed so that586 * we can deal with the (unlikely) case of sync timestamps587 * arriving from the future.588 */589 delta = nic_major - sync_timestamp;590 591 /* Recover the fully specified time now, by applying the offset592 * to the (fully specified) sync time.593 */594 nic_major = sync_timestamp + delta;595 596 kt = ptp->nic_to_kernel_time(nic_major, nic_minor,597 correction);598 }599 return kt;600}601 602ktime_t efx_ptp_nic_to_kernel_time(struct efx_tx_queue *tx_queue)603{604 struct efx_nic *efx = tx_queue->efx;605 struct efx_ptp_data *ptp = efx->ptp_data;606 ktime_t kt;607 608 if (efx_ptp_use_mac_tx_timestamps(efx))609 kt = efx_ptp_mac_nic_to_ktime_correction(efx, ptp,610 tx_queue->completed_timestamp_major,611 tx_queue->completed_timestamp_minor,612 ptp->ts_corrections.general_tx);613 else614 kt = ptp->nic_to_kernel_time(615 tx_queue->completed_timestamp_major,616 tx_queue->completed_timestamp_minor,617 ptp->ts_corrections.general_tx);618 return kt;619}620 621/* Get PTP attributes and set up time conversions */622static int efx_ptp_get_attributes(struct efx_nic *efx)623{624 MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_GET_ATTRIBUTES_LEN);625 MCDI_DECLARE_BUF(outbuf, MC_CMD_PTP_OUT_GET_ATTRIBUTES_LEN);626 struct efx_ptp_data *ptp = efx->ptp_data;627 int rc;628 u32 fmt;629 size_t out_len;630 631 /* Get the PTP attributes. If the NIC doesn't support the operation we632 * use the default format for compatibility with older NICs i.e.633 * seconds and nanoseconds.634 */635 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_GET_ATTRIBUTES);636 MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);637 rc = efx_mcdi_rpc_quiet(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),638 outbuf, sizeof(outbuf), &out_len);639 if (rc == 0) {640 fmt = MCDI_DWORD(outbuf, PTP_OUT_GET_ATTRIBUTES_TIME_FORMAT);641 } else if (rc == -EINVAL) {642 fmt = MC_CMD_PTP_OUT_GET_ATTRIBUTES_SECONDS_NANOSECONDS;643 } else if (rc == -EPERM) {644 pci_info(efx->pci_dev, "no PTP support\n");645 return rc;646 } else {647 efx_mcdi_display_error(efx, MC_CMD_PTP, sizeof(inbuf),648 outbuf, sizeof(outbuf), rc);649 return rc;650 }651 652 switch (fmt) {653 case MC_CMD_PTP_OUT_GET_ATTRIBUTES_SECONDS_27FRACTION:654 ptp->ns_to_nic_time = efx_ptp_ns_to_s27;655 ptp->nic_to_kernel_time = efx_ptp_s27_to_ktime_correction;656 ptp->nic_time.minor_max = 1 << 27;657 ptp->nic_time.sync_event_minor_shift = 19;658 break;659 case MC_CMD_PTP_OUT_GET_ATTRIBUTES_SECONDS_QTR_NANOSECONDS:660 ptp->ns_to_nic_time = efx_ptp_ns_to_s_qns;661 ptp->nic_to_kernel_time = efx_ptp_s_qns_to_ktime_correction;662 ptp->nic_time.minor_max = 4000000000UL;663 ptp->nic_time.sync_event_minor_shift = 24;664 break;665 default:666 return -ERANGE;667 }668 669 /* Precalculate acceptable difference between the minor time in the670 * packet prefix and the last MCDI time sync event. We expect the671 * packet prefix timestamp to be after of sync event by up to one672 * sync event interval (0.25s) but we allow it to exceed this by a673 * fuzz factor of (0.1s)674 */675 ptp->nic_time.sync_event_diff_min = ptp->nic_time.minor_max676 - (ptp->nic_time.minor_max / 10);677 ptp->nic_time.sync_event_diff_max = (ptp->nic_time.minor_max / 4)678 + (ptp->nic_time.minor_max / 10);679 680 /* MC_CMD_PTP_OP_GET_ATTRIBUTES has been extended twice from an older681 * operation MC_CMD_PTP_OP_GET_TIME_FORMAT. The function now may return682 * a value to use for the minimum acceptable corrected synchronization683 * window and may return further capabilities.684 * If we have the extra information store it. For older firmware that685 * does not implement the extended command use the default value.686 */687 if (rc == 0 &&688 out_len >= MC_CMD_PTP_OUT_GET_ATTRIBUTES_CAPABILITIES_OFST)689 ptp->min_synchronisation_ns =690 MCDI_DWORD(outbuf,691 PTP_OUT_GET_ATTRIBUTES_SYNC_WINDOW_MIN);692 else693 ptp->min_synchronisation_ns = DEFAULT_MIN_SYNCHRONISATION_NS;694 695 if (rc == 0 &&696 out_len >= MC_CMD_PTP_OUT_GET_ATTRIBUTES_LEN)697 ptp->capabilities = MCDI_DWORD(outbuf,698 PTP_OUT_GET_ATTRIBUTES_CAPABILITIES);699 else700 ptp->capabilities = 0;701 702 /* Set up the shift for conversion between frequency703 * adjustments in parts-per-billion and the fixed-point704 * fractional ns format that the adapter uses.705 */706 if (ptp->capabilities & (1 << MC_CMD_PTP_OUT_GET_ATTRIBUTES_FP44_FREQ_ADJ_LBN))707 ptp->adjfreq_ppb_shift = PPB_SHIFT_FP44;708 else709 ptp->adjfreq_ppb_shift = PPB_SHIFT_FP40;710 711 return 0;712}713 714/* Get PTP timestamp corrections */715static int efx_ptp_get_timestamp_corrections(struct efx_nic *efx)716{717 MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_GET_TIMESTAMP_CORRECTIONS_LEN);718 MCDI_DECLARE_BUF(outbuf, MC_CMD_PTP_OUT_GET_TIMESTAMP_CORRECTIONS_V2_LEN);719 int rc;720 size_t out_len;721 722 /* Get the timestamp corrections from the NIC. If this operation is723 * not supported (older NICs) then no correction is required.724 */725 MCDI_SET_DWORD(inbuf, PTP_IN_OP,726 MC_CMD_PTP_OP_GET_TIMESTAMP_CORRECTIONS);727 MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);728 729 rc = efx_mcdi_rpc_quiet(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),730 outbuf, sizeof(outbuf), &out_len);731 if (rc == 0) {732 efx->ptp_data->ts_corrections.ptp_tx = MCDI_DWORD(outbuf,733 PTP_OUT_GET_TIMESTAMP_CORRECTIONS_TRANSMIT);734 efx->ptp_data->ts_corrections.ptp_rx = MCDI_DWORD(outbuf,735 PTP_OUT_GET_TIMESTAMP_CORRECTIONS_RECEIVE);736 efx->ptp_data->ts_corrections.pps_out = MCDI_DWORD(outbuf,737 PTP_OUT_GET_TIMESTAMP_CORRECTIONS_PPS_OUT);738 efx->ptp_data->ts_corrections.pps_in = MCDI_DWORD(outbuf,739 PTP_OUT_GET_TIMESTAMP_CORRECTIONS_PPS_IN);740 741 if (out_len >= MC_CMD_PTP_OUT_GET_TIMESTAMP_CORRECTIONS_V2_LEN) {742 efx->ptp_data->ts_corrections.general_tx = MCDI_DWORD(743 outbuf,744 PTP_OUT_GET_TIMESTAMP_CORRECTIONS_V2_GENERAL_TX);745 efx->ptp_data->ts_corrections.general_rx = MCDI_DWORD(746 outbuf,747 PTP_OUT_GET_TIMESTAMP_CORRECTIONS_V2_GENERAL_RX);748 } else {749 efx->ptp_data->ts_corrections.general_tx =750 efx->ptp_data->ts_corrections.ptp_tx;751 efx->ptp_data->ts_corrections.general_rx =752 efx->ptp_data->ts_corrections.ptp_rx;753 }754 } else if (rc == -EINVAL) {755 efx->ptp_data->ts_corrections.ptp_tx = 0;756 efx->ptp_data->ts_corrections.ptp_rx = 0;757 efx->ptp_data->ts_corrections.pps_out = 0;758 efx->ptp_data->ts_corrections.pps_in = 0;759 efx->ptp_data->ts_corrections.general_tx = 0;760 efx->ptp_data->ts_corrections.general_rx = 0;761 } else {762 efx_mcdi_display_error(efx, MC_CMD_PTP, sizeof(inbuf), outbuf,763 sizeof(outbuf), rc);764 return rc;765 }766 767 return 0;768}769 770/* Enable MCDI PTP support. */771static int efx_ptp_enable(struct efx_nic *efx)772{773 MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_ENABLE_LEN);774 MCDI_DECLARE_BUF_ERR(outbuf);775 int rc;776 777 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_ENABLE);778 MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);779 MCDI_SET_DWORD(inbuf, PTP_IN_ENABLE_QUEUE,780 efx->ptp_data->channel ?781 efx->ptp_data->channel->channel : 0);782 MCDI_SET_DWORD(inbuf, PTP_IN_ENABLE_MODE, efx->ptp_data->mode);783 784 rc = efx_mcdi_rpc_quiet(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),785 outbuf, sizeof(outbuf), NULL);786 rc = (rc == -EALREADY) ? 0 : rc;787 if (rc)788 efx_mcdi_display_error(efx, MC_CMD_PTP,789 MC_CMD_PTP_IN_ENABLE_LEN,790 outbuf, sizeof(outbuf), rc);791 return rc;792}793 794/* Disable MCDI PTP support.795 *796 * Note that this function should never rely on the presence of ptp_data -797 * may be called before that exists.798 */799static int efx_ptp_disable(struct efx_nic *efx)800{801 MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_DISABLE_LEN);802 MCDI_DECLARE_BUF_ERR(outbuf);803 int rc;804 805 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_DISABLE);806 MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);807 rc = efx_mcdi_rpc_quiet(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),808 outbuf, sizeof(outbuf), NULL);809 rc = (rc == -EALREADY) ? 0 : rc;810 /* If we get ENOSYS, the NIC doesn't support PTP, and thus this function811 * should only have been called during probe.812 */813 if (rc == -ENOSYS || rc == -EPERM)814 pci_info(efx->pci_dev, "no PTP support\n");815 else if (rc)816 efx_mcdi_display_error(efx, MC_CMD_PTP,817 MC_CMD_PTP_IN_DISABLE_LEN,818 outbuf, sizeof(outbuf), rc);819 return rc;820}821 822static void efx_ptp_deliver_rx_queue(struct sk_buff_head *q)823{824 struct sk_buff *skb;825 826 while ((skb = skb_dequeue(q))) {827 local_bh_disable();828 netif_receive_skb(skb);829 local_bh_enable();830 }831}832 833static void efx_ptp_handle_no_channel(struct efx_nic *efx)834{835 netif_err(efx, drv, efx->net_dev,836 "ERROR: PTP requires MSI-X and 1 additional interrupt"837 "vector. PTP disabled\n");838}839 840/* Repeatedly send the host time to the MC which will capture the hardware841 * time.842 */843static void efx_ptp_send_times(struct efx_nic *efx,844 struct pps_event_time *last_time)845{846 struct pps_event_time now;847 struct timespec64 limit;848 struct efx_ptp_data *ptp = efx->ptp_data;849 int *mc_running = ptp->start.addr;850 851 pps_get_ts(&now);852 limit = now.ts_real;853 timespec64_add_ns(&limit, SYNCHRONISE_PERIOD_NS);854 855 /* Write host time for specified period or until MC is done */856 while ((timespec64_compare(&now.ts_real, &limit) < 0) &&857 READ_ONCE(*mc_running)) {858 struct timespec64 update_time;859 unsigned int host_time;860 861 /* Don't update continuously to avoid saturating the PCIe bus */862 update_time = now.ts_real;863 timespec64_add_ns(&update_time, SYNCHRONISATION_GRANULARITY_NS);864 do {865 pps_get_ts(&now);866 } while ((timespec64_compare(&now.ts_real, &update_time) < 0) &&867 READ_ONCE(*mc_running));868 869 /* Synchronise NIC with single word of time only */870 host_time = (now.ts_real.tv_sec << MC_NANOSECOND_BITS |871 now.ts_real.tv_nsec);872 /* Update host time in NIC memory */873 efx->type->ptp_write_host_time(efx, host_time);874 }875 *last_time = now;876}877 878/* Read a timeset from the MC's results and partial process. */879static void efx_ptp_read_timeset(MCDI_DECLARE_STRUCT_PTR(data),880 struct efx_ptp_timeset *timeset)881{882 unsigned start_ns, end_ns;883 884 timeset->host_start = MCDI_DWORD(data, PTP_OUT_SYNCHRONIZE_HOSTSTART);885 timeset->major = MCDI_DWORD(data, PTP_OUT_SYNCHRONIZE_MAJOR);886 timeset->minor = MCDI_DWORD(data, PTP_OUT_SYNCHRONIZE_MINOR);887 timeset->host_end = MCDI_DWORD(data, PTP_OUT_SYNCHRONIZE_HOSTEND);888 timeset->wait = MCDI_DWORD(data, PTP_OUT_SYNCHRONIZE_WAITNS);889 890 /* Ignore seconds */891 start_ns = timeset->host_start & MC_NANOSECOND_MASK;892 end_ns = timeset->host_end & MC_NANOSECOND_MASK;893 /* Allow for rollover */894 if (end_ns < start_ns)895 end_ns += NSEC_PER_SEC;896 /* Determine duration of operation */897 timeset->window = end_ns - start_ns;898}899 900/* Process times received from MC.901 *902 * Extract times from returned results, and establish the minimum value903 * seen. The minimum value represents the "best" possible time and events904 * too much greater than this are rejected - the machine is, perhaps, too905 * busy. A number of readings are taken so that, hopefully, at least one good906 * synchronisation will be seen in the results.907 */908static int909efx_ptp_process_times(struct efx_nic *efx, MCDI_DECLARE_STRUCT_PTR(synch_buf),910 size_t response_length,911 const struct pps_event_time *last_time)912{913 unsigned number_readings =914 MCDI_VAR_ARRAY_LEN(response_length,915 PTP_OUT_SYNCHRONIZE_TIMESET);916 unsigned i;917 unsigned ngood = 0;918 unsigned last_good = 0;919 struct efx_ptp_data *ptp = efx->ptp_data;920 u32 last_sec;921 u32 start_sec;922 struct timespec64 delta;923 ktime_t mc_time;924 925 if (number_readings == 0)926 return -EAGAIN;927 928 /* Read the set of results and find the last good host-MC929 * synchronization result. The MC times when it finishes reading the930 * host time so the corrected window time should be fairly constant931 * for a given platform. Increment stats for any results that appear932 * to be erroneous.933 */934 for (i = 0; i < number_readings; i++) {935 s32 window, corrected;936 struct timespec64 wait;937 938 efx_ptp_read_timeset(939 MCDI_ARRAY_STRUCT_PTR(synch_buf,940 PTP_OUT_SYNCHRONIZE_TIMESET, i),941 &ptp->timeset[i]);942 943 wait = ktime_to_timespec64(944 ptp->nic_to_kernel_time(0, ptp->timeset[i].wait, 0));945 window = ptp->timeset[i].window;946 corrected = window - wait.tv_nsec;947 948 /* We expect the uncorrected synchronization window to be at949 * least as large as the interval between host start and end950 * times. If it is smaller than this then this is mostly likely951 * to be a consequence of the host's time being adjusted.952 * Check that the corrected sync window is in a reasonable953 * range. If it is out of range it is likely to be because an954 * interrupt or other delay occurred between reading the system955 * time and writing it to MC memory.956 */957 if (window < SYNCHRONISATION_GRANULARITY_NS) {958 ++ptp->invalid_sync_windows;959 } else if (corrected >= MAX_SYNCHRONISATION_NS) {960 ++ptp->oversize_sync_windows;961 } else if (corrected < ptp->min_synchronisation_ns) {962 ++ptp->undersize_sync_windows;963 } else {964 ngood++;965 last_good = i;966 }967 }968 969 if (ngood == 0) {970 netif_warn(efx, drv, efx->net_dev,971 "PTP no suitable synchronisations\n");972 return -EAGAIN;973 }974 975 /* Calculate delay from last good sync (host time) to last_time.976 * It is possible that the seconds rolled over between taking977 * the start reading and the last value written by the host. The978 * timescales are such that a gap of more than one second is never979 * expected. delta is *not* normalised.980 */981 start_sec = ptp->timeset[last_good].host_start >> MC_NANOSECOND_BITS;982 last_sec = last_time->ts_real.tv_sec & MC_SECOND_MASK;983 if (start_sec != last_sec &&984 ((start_sec + 1) & MC_SECOND_MASK) != last_sec) {985 netif_warn(efx, hw, efx->net_dev,986 "PTP bad synchronisation seconds\n");987 return -EAGAIN;988 }989 delta.tv_sec = (last_sec - start_sec) & 1;990 delta.tv_nsec =991 last_time->ts_real.tv_nsec -992 (ptp->timeset[last_good].host_start & MC_NANOSECOND_MASK);993 994 /* Convert the NIC time at last good sync into kernel time.995 * No correction is required - this time is the output of a996 * firmware process.997 */998 mc_time = ptp->nic_to_kernel_time(ptp->timeset[last_good].major,999 ptp->timeset[last_good].minor, 0);1000 1001 /* Calculate delay from NIC top of second to last_time */1002 delta.tv_nsec += ktime_to_timespec64(mc_time).tv_nsec;1003 1004 /* Set PPS timestamp to match NIC top of second */1005 ptp->host_time_pps = *last_time;1006 pps_sub_ts(&ptp->host_time_pps, delta);1007 1008 return 0;1009}1010 1011/* Synchronize times between the host and the MC */1012static int efx_ptp_synchronize(struct efx_nic *efx, unsigned int num_readings)1013{1014 struct efx_ptp_data *ptp = efx->ptp_data;1015 MCDI_DECLARE_BUF(synch_buf, MC_CMD_PTP_OUT_SYNCHRONIZE_LENMAX);1016 size_t response_length;1017 int rc;1018 unsigned long timeout;1019 struct pps_event_time last_time = {};1020 unsigned int loops = 0;1021 int *start = ptp->start.addr;1022 1023 MCDI_SET_DWORD(synch_buf, PTP_IN_OP, MC_CMD_PTP_OP_SYNCHRONIZE);1024 MCDI_SET_DWORD(synch_buf, PTP_IN_PERIPH_ID, 0);1025 MCDI_SET_DWORD(synch_buf, PTP_IN_SYNCHRONIZE_NUMTIMESETS,1026 num_readings);1027 MCDI_SET_QWORD(synch_buf, PTP_IN_SYNCHRONIZE_START_ADDR,1028 ptp->start.dma_addr);1029 1030 /* Clear flag that signals MC ready */1031 WRITE_ONCE(*start, 0);1032 rc = efx_mcdi_rpc_start(efx, MC_CMD_PTP, synch_buf,1033 MC_CMD_PTP_IN_SYNCHRONIZE_LEN);1034 EFX_WARN_ON_ONCE_PARANOID(rc);1035 1036 /* Wait for start from MCDI (or timeout) */1037 timeout = jiffies + msecs_to_jiffies(MAX_SYNCHRONISE_WAIT_MS);1038 while (!READ_ONCE(*start) && (time_before(jiffies, timeout))) {1039 udelay(20); /* Usually start MCDI execution quickly */1040 loops++;1041 }1042 1043 if (loops <= 1)1044 ++ptp->fast_syncs;1045 if (!time_before(jiffies, timeout))1046 ++ptp->sync_timeouts;1047 1048 if (READ_ONCE(*start))1049 efx_ptp_send_times(efx, &last_time);1050 1051 /* Collect results */1052 rc = efx_mcdi_rpc_finish(efx, MC_CMD_PTP,1053 MC_CMD_PTP_IN_SYNCHRONIZE_LEN,1054 synch_buf, sizeof(synch_buf),1055 &response_length);1056 if (rc == 0) {1057 rc = efx_ptp_process_times(efx, synch_buf, response_length,1058 &last_time);1059 if (rc == 0)1060 ++ptp->good_syncs;1061 else1062 ++ptp->no_time_syncs;1063 }1064 1065 /* Increment the bad syncs counter if the synchronize fails, whatever1066 * the reason.1067 */1068 if (rc != 0)1069 ++ptp->bad_syncs;1070 1071 return rc;1072}1073 1074/* Transmit a PTP packet via the dedicated hardware timestamped queue. */1075static void efx_ptp_xmit_skb_queue(struct efx_nic *efx, struct sk_buff *skb)1076{1077 struct efx_ptp_data *ptp_data = efx->ptp_data;1078 u8 type = efx_tx_csum_type_skb(skb);1079 struct efx_tx_queue *tx_queue;1080 1081 tx_queue = efx_channel_get_tx_queue(ptp_data->channel, type);1082 if (tx_queue && tx_queue->timestamping) {1083 skb_get(skb);1084 1085 /* This code invokes normal driver TX code which is always1086 * protected from softirqs when called from generic TX code,1087 * which in turn disables preemption. Look at __dev_queue_xmit1088 * which uses rcu_read_lock_bh disabling preemption for RCU1089 * plus disabling softirqs. We do not need RCU reader1090 * protection here.1091 *1092 * Although it is theoretically safe for current PTP TX/RX code1093 * running without disabling softirqs, there are three good1094 * reasond for doing so:1095 *1096 * 1) The code invoked is mainly implemented for non-PTP1097 * packets and it is always executed with softirqs1098 * disabled.1099 * 2) This being a single PTP packet, better to not1100 * interrupt its processing by softirqs which can lead1101 * to high latencies.1102 * 3) netdev_xmit_more checks preemption is disabled and1103 * triggers a BUG_ON if not.1104 */1105 local_bh_disable();1106 efx_enqueue_skb(tx_queue, skb);1107 local_bh_enable();1108 1109 /* We need to add the filters after enqueuing the packet.1110 * Otherwise, there's high latency in sending back the1111 * timestamp, causing ptp4l timeouts1112 */1113 efx_ptp_insert_unicast_filter(efx, skb);1114 dev_consume_skb_any(skb);1115 } else {1116 WARN_ONCE(1, "PTP channel has no timestamped tx queue\n");1117 dev_kfree_skb_any(skb);1118 }1119}1120 1121/* Transmit a PTP packet, via the MCDI interface, to the wire. */1122static void efx_ptp_xmit_skb_mc(struct efx_nic *efx, struct sk_buff *skb)1123{1124 MCDI_DECLARE_BUF(txtime, MC_CMD_PTP_OUT_TRANSMIT_LEN);1125 struct efx_ptp_data *ptp_data = efx->ptp_data;1126 struct skb_shared_hwtstamps timestamps;1127 size_t len;1128 int rc;1129 1130 MCDI_SET_DWORD(ptp_data->txbuf, PTP_IN_OP, MC_CMD_PTP_OP_TRANSMIT);1131 MCDI_SET_DWORD(ptp_data->txbuf, PTP_IN_PERIPH_ID, 0);1132 MCDI_SET_DWORD(ptp_data->txbuf, PTP_IN_TRANSMIT_LENGTH, skb->len);1133 if (skb_shinfo(skb)->nr_frags != 0) {1134 rc = skb_linearize(skb);1135 if (rc != 0)1136 goto fail;1137 }1138 1139 if (skb->ip_summed == CHECKSUM_PARTIAL) {1140 rc = skb_checksum_help(skb);1141 if (rc != 0)1142 goto fail;1143 }1144 skb_copy_from_linear_data(skb,1145 MCDI_PTR(ptp_data->txbuf,1146 PTP_IN_TRANSMIT_PACKET),1147 skb->len);1148 rc = efx_mcdi_rpc(efx, MC_CMD_PTP,1149 ptp_data->txbuf, MC_CMD_PTP_IN_TRANSMIT_LEN(skb->len),1150 txtime, sizeof(txtime), &len);1151 if (rc != 0)1152 goto fail;1153 1154 memset(×tamps, 0, sizeof(timestamps));1155 timestamps.hwtstamp = ptp_data->nic_to_kernel_time(1156 MCDI_DWORD(txtime, PTP_OUT_TRANSMIT_MAJOR),1157 MCDI_DWORD(txtime, PTP_OUT_TRANSMIT_MINOR),1158 ptp_data->ts_corrections.ptp_tx);1159 1160 skb_tstamp_tx(skb, ×tamps);1161 1162 /* Add the filters after sending back the timestamp to avoid delaying it1163 * or ptp4l may timeout.1164 */1165 efx_ptp_insert_unicast_filter(efx, skb);1166 1167fail:1168 dev_kfree_skb_any(skb);1169 1170 return;1171}1172 1173/* Process any queued receive events and corresponding packets1174 *1175 * q is returned with all the packets that are ready for delivery.1176 */1177static void efx_ptp_process_events(struct efx_nic *efx, struct sk_buff_head *q)1178{1179 struct efx_ptp_data *ptp = efx->ptp_data;1180 struct sk_buff *skb;1181 1182 while ((skb = skb_dequeue(&ptp->rxq))) {1183 struct efx_ptp_match *match;1184 1185 match = (struct efx_ptp_match *)skb->cb;1186 if (match->state == PTP_PACKET_STATE_MATCH_UNWANTED) {1187 __skb_queue_tail(q, skb);1188 } else if (time_after(jiffies, match->expiry)) {1189 match->state = PTP_PACKET_STATE_TIMED_OUT;1190 ++ptp->rx_no_timestamp;1191 __skb_queue_tail(q, skb);1192 } else {1193 /* Replace unprocessed entry and stop */1194 skb_queue_head(&ptp->rxq, skb);1195 break;1196 }1197 }1198}1199 1200/* Complete processing of a received packet */1201static inline void efx_ptp_process_rx(struct efx_nic *efx, struct sk_buff *skb)1202{1203 local_bh_disable();1204 netif_receive_skb(skb);1205 local_bh_enable();1206}1207 1208static struct efx_ptp_rxfilter *1209efx_ptp_find_filter(struct list_head *filter_list, struct efx_filter_spec *spec)1210{1211 struct efx_ptp_rxfilter *rxfilter;1212 1213 list_for_each_entry(rxfilter, filter_list, list) {1214 if (rxfilter->ether_type == spec->ether_type &&1215 rxfilter->loc_port == spec->loc_port &&1216 !memcmp(rxfilter->loc_host, spec->loc_host, sizeof(spec->loc_host)))1217 return rxfilter;1218 }1219 1220 return NULL;1221}1222 1223static void efx_ptp_remove_one_filter(struct efx_nic *efx,1224 struct efx_ptp_rxfilter *rxfilter)1225{1226 efx_filter_remove_id_safe(efx, EFX_FILTER_PRI_REQUIRED,1227 rxfilter->handle);1228 list_del(&rxfilter->list);1229 kfree(rxfilter);1230}1231 1232static void efx_ptp_remove_filters(struct efx_nic *efx,1233 struct list_head *filter_list)1234{1235 struct efx_ptp_rxfilter *rxfilter, *tmp;1236 1237 list_for_each_entry_safe(rxfilter, tmp, filter_list, list)1238 efx_ptp_remove_one_filter(efx, rxfilter);1239}1240 1241static void efx_ptp_init_filter(struct efx_nic *efx,1242 struct efx_filter_spec *rxfilter)1243{1244 struct efx_channel *channel = efx->ptp_data->channel;1245 struct efx_rx_queue *queue = efx_channel_get_rx_queue(channel);1246 1247 efx_filter_init_rx(rxfilter, EFX_FILTER_PRI_REQUIRED, 0,1248 efx_rx_queue_index(queue));1249}1250 1251static int efx_ptp_insert_filter(struct efx_nic *efx,1252 struct list_head *filter_list,1253 struct efx_filter_spec *spec,1254 unsigned long expiry)1255{1256 struct efx_ptp_data *ptp = efx->ptp_data;1257 struct efx_ptp_rxfilter *rxfilter;1258 int rc;1259 1260 rxfilter = efx_ptp_find_filter(filter_list, spec);1261 if (rxfilter) {1262 rxfilter->expiry = expiry;1263 return 0;1264 }1265 1266 rxfilter = kzalloc(sizeof(*rxfilter), GFP_KERNEL);1267 if (!rxfilter)1268 return -ENOMEM;1269 1270 rc = efx_filter_insert_filter(efx, spec, true);1271 if (rc < 0)1272 goto fail;1273 1274 rxfilter->handle = rc;1275 rxfilter->ether_type = spec->ether_type;1276 rxfilter->loc_port = spec->loc_port;1277 memcpy(rxfilter->loc_host, spec->loc_host, sizeof(spec->loc_host));1278 rxfilter->expiry = expiry;1279 list_add(&rxfilter->list, filter_list);1280 1281 queue_delayed_work(ptp->workwq, &ptp->cleanup_work,1282 UCAST_FILTER_EXPIRY_JIFFIES + 1);1283 1284 return 0;1285 1286fail:1287 kfree(rxfilter);1288 return rc;1289}1290 1291static int efx_ptp_insert_ipv4_filter(struct efx_nic *efx,1292 struct list_head *filter_list,1293 __be32 addr, u16 port,1294 unsigned long expiry)1295{1296 struct efx_filter_spec spec;1297 1298 efx_ptp_init_filter(efx, &spec);1299 efx_filter_set_ipv4_local(&spec, IPPROTO_UDP, addr, htons(port));1300 return efx_ptp_insert_filter(efx, filter_list, &spec, expiry);1301}1302 1303static int efx_ptp_insert_ipv6_filter(struct efx_nic *efx,1304 struct list_head *filter_list,1305 const struct in6_addr *addr, u16 port,1306 unsigned long expiry)1307{1308 struct efx_filter_spec spec;1309 1310 efx_ptp_init_filter(efx, &spec);1311 efx_filter_set_ipv6_local(&spec, IPPROTO_UDP, addr, htons(port));1312 return efx_ptp_insert_filter(efx, filter_list, &spec, expiry);1313}1314 1315static int efx_ptp_insert_eth_multicast_filter(struct efx_nic *efx)1316{1317 struct efx_ptp_data *ptp = efx->ptp_data;1318 struct efx_filter_spec spec;1319 1320 efx_ptp_init_filter(efx, &spec);1321 efx_filter_set_eth_local(&spec, EFX_FILTER_VID_UNSPEC, ptp_addr_ether);1322 spec.match_flags |= EFX_FILTER_MATCH_ETHER_TYPE;1323 spec.ether_type = htons(ETH_P_1588);1324 return efx_ptp_insert_filter(efx, &ptp->rxfilters_mcast, &spec, 0);1325}1326 1327static int efx_ptp_insert_multicast_filters(struct efx_nic *efx)1328{1329 struct efx_ptp_data *ptp = efx->ptp_data;1330 int rc;1331 1332 if (!ptp->channel || !list_empty(&ptp->rxfilters_mcast))1333 return 0;1334 1335 /* Must filter on both event and general ports to ensure1336 * that there is no packet re-ordering.1337 */1338 rc = efx_ptp_insert_ipv4_filter(efx, &ptp->rxfilters_mcast,1339 htonl(PTP_ADDR_IPV4), PTP_EVENT_PORT,1340 0);1341 if (rc < 0)1342 goto fail;1343 1344 rc = efx_ptp_insert_ipv4_filter(efx, &ptp->rxfilters_mcast,1345 htonl(PTP_ADDR_IPV4), PTP_GENERAL_PORT,1346 0);1347 if (rc < 0)1348 goto fail;1349 1350 /* if the NIC supports hw timestamps by the MAC, we can support1351 * PTP over IPv6 and Ethernet1352 */1353 if (efx_ptp_use_mac_tx_timestamps(efx)) {1354 rc = efx_ptp_insert_ipv6_filter(efx, &ptp->rxfilters_mcast,1355 &ptp_addr_ipv6, PTP_EVENT_PORT, 0);1356 if (rc < 0)1357 goto fail;1358 1359 rc = efx_ptp_insert_ipv6_filter(efx, &ptp->rxfilters_mcast,1360 &ptp_addr_ipv6, PTP_GENERAL_PORT, 0);1361 if (rc < 0)1362 goto fail;1363 1364 rc = efx_ptp_insert_eth_multicast_filter(efx);1365 1366 /* Not all firmware variants support this filter */1367 if (rc < 0 && rc != -EPROTONOSUPPORT)1368 goto fail;1369 }1370 1371 return 0;1372 1373fail:1374 efx_ptp_remove_filters(efx, &ptp->rxfilters_mcast);1375 return rc;1376}1377 1378static bool efx_ptp_valid_unicast_event_pkt(struct sk_buff *skb)1379{1380 if (skb->protocol == htons(ETH_P_IP)) {1381 return ip_hdr(skb)->daddr != htonl(PTP_ADDR_IPV4) &&1382 ip_hdr(skb)->protocol == IPPROTO_UDP &&1383 udp_hdr(skb)->source == htons(PTP_EVENT_PORT);1384 } else if (skb->protocol == htons(ETH_P_IPV6)) {1385 return !ipv6_addr_equal(&ipv6_hdr(skb)->daddr, &ptp_addr_ipv6) &&1386 ipv6_hdr(skb)->nexthdr == IPPROTO_UDP &&1387 udp_hdr(skb)->source == htons(PTP_EVENT_PORT);1388 }1389 return false;1390}1391 1392static int efx_ptp_insert_unicast_filter(struct efx_nic *efx,1393 struct sk_buff *skb)1394{1395 struct efx_ptp_data *ptp = efx->ptp_data;1396 unsigned long expiry;1397 int rc;1398 1399 if (!efx_ptp_valid_unicast_event_pkt(skb))1400 return -EINVAL;1401 1402 expiry = jiffies + UCAST_FILTER_EXPIRY_JIFFIES;1403 1404 if (skb->protocol == htons(ETH_P_IP)) {1405 __be32 addr = ip_hdr(skb)->saddr;1406 1407 rc = efx_ptp_insert_ipv4_filter(efx, &ptp->rxfilters_ucast,1408 addr, PTP_EVENT_PORT, expiry);1409 if (rc < 0)1410 goto out;1411 1412 rc = efx_ptp_insert_ipv4_filter(efx, &ptp->rxfilters_ucast,1413 addr, PTP_GENERAL_PORT, expiry);1414 } else if (efx_ptp_use_mac_tx_timestamps(efx)) {1415 /* IPv6 PTP only supported by devices with MAC hw timestamp */1416 struct in6_addr *addr = &ipv6_hdr(skb)->saddr;1417 1418 rc = efx_ptp_insert_ipv6_filter(efx, &ptp->rxfilters_ucast,1419 addr, PTP_EVENT_PORT, expiry);1420 if (rc < 0)1421 goto out;1422 1423 rc = efx_ptp_insert_ipv6_filter(efx, &ptp->rxfilters_ucast,1424 addr, PTP_GENERAL_PORT, expiry);1425 } else {1426 return -EOPNOTSUPP;1427 }1428 1429out:1430 return rc;1431}1432 1433static int efx_ptp_start(struct efx_nic *efx)1434{1435 struct efx_ptp_data *ptp = efx->ptp_data;1436 int rc;1437 1438 ptp->reset_required = false;1439 1440 rc = efx_ptp_insert_multicast_filters(efx);1441 if (rc)1442 return rc;1443 1444 rc = efx_ptp_enable(efx);1445 if (rc != 0)1446 goto fail;1447 1448 ptp->evt_frag_idx = 0;1449 ptp->current_adjfreq = 0;1450 1451 return 0;1452 1453fail:1454 efx_ptp_remove_filters(efx, &ptp->rxfilters_mcast);1455 return rc;1456}1457 1458static int efx_ptp_stop(struct efx_nic *efx)1459{1460 struct efx_ptp_data *ptp = efx->ptp_data;1461 int rc;1462 1463 if (ptp == NULL)1464 return 0;1465 1466 rc = efx_ptp_disable(efx);1467 1468 efx_ptp_remove_filters(efx, &ptp->rxfilters_mcast);1469 efx_ptp_remove_filters(efx, &ptp->rxfilters_ucast);1470 1471 /* Make sure RX packets are really delivered */1472 efx_ptp_deliver_rx_queue(&efx->ptp_data->rxq);1473 skb_queue_purge(&efx->ptp_data->txq);1474 1475 return rc;1476}1477 1478static int efx_ptp_restart(struct efx_nic *efx)1479{1480 if (efx->ptp_data && efx->ptp_data->enabled)1481 return efx_ptp_start(efx);1482 return 0;1483}1484 1485static void efx_ptp_pps_worker(struct work_struct *work)1486{1487 struct efx_ptp_data *ptp =1488 container_of(work, struct efx_ptp_data, pps_work);1489 struct efx_nic *efx = ptp->efx;1490 struct ptp_clock_event ptp_evt;1491 1492 if (efx_ptp_synchronize(efx, PTP_SYNC_ATTEMPTS))1493 return;1494 1495 ptp_evt.type = PTP_CLOCK_PPSUSR;1496 ptp_evt.pps_times = ptp->host_time_pps;1497 ptp_clock_event(ptp->phc_clock, &ptp_evt);1498}1499 1500static void efx_ptp_worker(struct work_struct *work)1501{1502 struct efx_ptp_data *ptp_data =1503 container_of(work, struct efx_ptp_data, work);1504 struct efx_nic *efx = ptp_data->efx;1505 struct sk_buff *skb;1506 struct sk_buff_head tempq;1507 1508 if (ptp_data->reset_required) {1509 efx_ptp_stop(efx);1510 efx_ptp_start(efx);1511 return;1512 }1513 1514 __skb_queue_head_init(&tempq);1515 efx_ptp_process_events(efx, &tempq);1516 1517 while ((skb = skb_dequeue(&ptp_data->txq)))1518 ptp_data->xmit_skb(efx, skb);1519 1520 while ((skb = __skb_dequeue(&tempq)))1521 efx_ptp_process_rx(efx, skb);1522}1523 1524static void efx_ptp_cleanup_worker(struct work_struct *work)1525{1526 struct efx_ptp_data *ptp =1527 container_of(work, struct efx_ptp_data, cleanup_work.work);1528 struct efx_ptp_rxfilter *rxfilter, *tmp;1529 1530 list_for_each_entry_safe(rxfilter, tmp, &ptp->rxfilters_ucast, list) {1531 if (time_is_before_jiffies(rxfilter->expiry))1532 efx_ptp_remove_one_filter(ptp->efx, rxfilter);1533 }1534 1535 if (!list_empty(&ptp->rxfilters_ucast)) {1536 queue_delayed_work(ptp->workwq, &ptp->cleanup_work,1537 UCAST_FILTER_EXPIRY_JIFFIES + 1);1538 }1539}1540 1541static const struct ptp_clock_info efx_phc_clock_info = {1542 .owner = THIS_MODULE,1543 .name = "sfc",1544 .max_adj = MAX_PPB,1545 .n_alarm = 0,1546 .n_ext_ts = 0,1547 .n_per_out = 0,1548 .n_pins = 0,1549 .pps = 1,1550 .adjfine = efx_phc_adjfine,1551 .adjtime = efx_phc_adjtime,1552 .gettime64 = efx_phc_gettime,1553 .settime64 = efx_phc_settime,1554 .enable = efx_phc_enable,1555};1556 1557/* Initialise PTP state. */1558int efx_ptp_probe(struct efx_nic *efx, struct efx_channel *channel)1559{1560 struct efx_ptp_data *ptp;1561 int rc = 0;1562 1563 if (efx->ptp_data) {1564 efx->ptp_data->channel = channel;1565 return 0;1566 }1567 1568 ptp = kzalloc(sizeof(struct efx_ptp_data), GFP_KERNEL);1569 efx->ptp_data = ptp;1570 if (!efx->ptp_data)1571 return -ENOMEM;1572 1573 ptp->efx = efx;1574 ptp->channel = channel;1575 1576 rc = efx_nic_alloc_buffer(efx, &ptp->start, sizeof(int), GFP_KERNEL);1577 if (rc != 0)1578 goto fail1;1579 1580 skb_queue_head_init(&ptp->rxq);1581 skb_queue_head_init(&ptp->txq);1582 ptp->workwq = create_singlethread_workqueue("sfc_ptp");1583 if (!ptp->workwq) {1584 rc = -ENOMEM;1585 goto fail2;1586 }1587 1588 if (efx_ptp_use_mac_tx_timestamps(efx)) {1589 ptp->xmit_skb = efx_ptp_xmit_skb_queue;1590 /* Request sync events on this channel. */1591 channel->sync_events_state = SYNC_EVENTS_QUIESCENT;1592 } else {1593 ptp->xmit_skb = efx_ptp_xmit_skb_mc;1594 }1595 1596 INIT_WORK(&ptp->work, efx_ptp_worker);1597 INIT_DELAYED_WORK(&ptp->cleanup_work, efx_ptp_cleanup_worker);1598 ptp->config.flags = 0;1599 ptp->config.tx_type = HWTSTAMP_TX_OFF;1600 ptp->config.rx_filter = HWTSTAMP_FILTER_NONE;1601 INIT_LIST_HEAD(&ptp->rxfilters_mcast);1602 INIT_LIST_HEAD(&ptp->rxfilters_ucast);1603 1604 /* Get the NIC PTP attributes and set up time conversions */1605 rc = efx_ptp_get_attributes(efx);1606 if (rc < 0)1607 goto fail3;1608 1609 /* Get the timestamp corrections */1610 rc = efx_ptp_get_timestamp_corrections(efx);1611 if (rc < 0)1612 goto fail3;1613 1614 if (efx->mcdi->fn_flags &1615 (1 << MC_CMD_DRV_ATTACH_EXT_OUT_FLAG_PRIMARY)) {1616 ptp->phc_clock_info = efx_phc_clock_info;1617 ptp->phc_clock = ptp_clock_register(&ptp->phc_clock_info,1618 &efx->pci_dev->dev);1619 if (IS_ERR(ptp->phc_clock)) {1620 rc = PTR_ERR(ptp->phc_clock);1621 goto fail3;1622 } else if (ptp->phc_clock) {1623 INIT_WORK(&ptp->pps_work, efx_ptp_pps_worker);1624 ptp->pps_workwq = create_singlethread_workqueue("sfc_pps");1625 if (!ptp->pps_workwq) {1626 rc = -ENOMEM;1627 goto fail4;1628 }1629 }1630 }1631 ptp->nic_ts_enabled = false;1632 1633 return 0;1634fail4:1635 ptp_clock_unregister(efx->ptp_data->phc_clock);1636 1637fail3:1638 destroy_workqueue(efx->ptp_data->workwq);1639 1640fail2:1641 efx_nic_free_buffer(efx, &ptp->start);1642 1643fail1:1644 kfree(efx->ptp_data);1645 efx->ptp_data = NULL;1646 1647 return rc;1648}1649 1650/* Initialise PTP channel.1651 *1652 * Setting core_index to zero causes the queue to be initialised and doesn't1653 * overlap with 'rxq0' because ptp.c doesn't use skb_record_rx_queue.1654 */1655static int efx_ptp_probe_channel(struct efx_channel *channel)1656{1657 struct efx_nic *efx = channel->efx;1658 int rc;1659 1660 channel->irq_moderation_us = 0;1661 channel->rx_queue.core_index = 0;1662 1663 rc = efx_ptp_probe(efx, channel);1664 /* Failure to probe PTP is not fatal; this channel will just not be1665 * used for anything.1666 * In the case of EPERM, efx_ptp_probe will print its own message (in1667 * efx_ptp_get_attributes()), so we don't need to.1668 */1669 if (rc && rc != -EPERM)1670 netif_warn(efx, drv, efx->net_dev,1671 "Failed to probe PTP, rc=%d\n", rc);1672 return 0;1673}1674 1675void efx_ptp_remove(struct efx_nic *efx)1676{1677 if (!efx->ptp_data)1678 return;1679 1680 (void)efx_ptp_disable(efx);1681 1682 cancel_work_sync(&efx->ptp_data->work);1683 cancel_delayed_work_sync(&efx->ptp_data->cleanup_work);1684 if (efx->ptp_data->pps_workwq)1685 cancel_work_sync(&efx->ptp_data->pps_work);1686 1687 skb_queue_purge(&efx->ptp_data->rxq);1688 skb_queue_purge(&efx->ptp_data->txq);1689 1690 if (efx->ptp_data->phc_clock) {1691 destroy_workqueue(efx->ptp_data->pps_workwq);1692 ptp_clock_unregister(efx->ptp_data->phc_clock);1693 }1694 1695 destroy_workqueue(efx->ptp_data->workwq);1696 1697 efx_nic_free_buffer(efx, &efx->ptp_data->start);1698 kfree(efx->ptp_data);1699 efx->ptp_data = NULL;1700}1701 1702static void efx_ptp_remove_channel(struct efx_channel *channel)1703{1704 efx_ptp_remove(channel->efx);1705}1706 1707static void efx_ptp_get_channel_name(struct efx_channel *channel,1708 char *buf, size_t len)1709{1710 snprintf(buf, len, "%s-ptp", channel->efx->name);1711}1712 1713/* Determine whether this packet should be processed by the PTP module1714 * or transmitted conventionally.1715 */1716bool efx_ptp_is_ptp_tx(struct efx_nic *efx, struct sk_buff *skb)1717{1718 return efx->ptp_data &&1719 efx->ptp_data->enabled &&1720 skb->len >= PTP_MIN_LENGTH &&1721 skb->len <= MC_CMD_PTP_IN_TRANSMIT_PACKET_MAXNUM &&1722 likely(skb->protocol == htons(ETH_P_IP)) &&1723 skb_transport_header_was_set(skb) &&1724 skb_network_header_len(skb) >= sizeof(struct iphdr) &&1725 ip_hdr(skb)->protocol == IPPROTO_UDP &&1726 skb_headlen(skb) >=1727 skb_transport_offset(skb) + sizeof(struct udphdr) &&1728 udp_hdr(skb)->dest == htons(PTP_EVENT_PORT);1729}1730 1731/* Receive a PTP packet. Packets are queued until the arrival of1732 * the receive timestamp from the MC - this will probably occur after the1733 * packet arrival because of the processing in the MC.1734 */1735static bool efx_ptp_rx(struct efx_channel *channel, struct sk_buff *skb)1736{1737 struct efx_nic *efx = channel->efx;1738 struct efx_ptp_data *ptp = efx->ptp_data;1739 struct efx_ptp_match *match = (struct efx_ptp_match *)skb->cb;1740 unsigned int version;1741 u8 *data;1742 1743 match->expiry = jiffies + msecs_to_jiffies(PKT_EVENT_LIFETIME_MS);1744 1745 /* Correct version? */1746 if (ptp->mode == MC_CMD_PTP_MODE_V1) {1747 if (!pskb_may_pull(skb, PTP_V1_MIN_LENGTH)) {1748 return false;1749 }1750 data = skb->data;1751 version = ntohs(*(__be16 *)&data[PTP_V1_VERSION_OFFSET]);1752 if (version != PTP_VERSION_V1) {1753 return false;1754 }1755 } else {1756 if (!pskb_may_pull(skb, PTP_V2_MIN_LENGTH)) {1757 return false;1758 }1759 data = skb->data;1760 version = data[PTP_V2_VERSION_OFFSET];1761 if ((version & PTP_VERSION_V2_MASK) != PTP_VERSION_V2) {1762 return false;1763 }1764 }1765 1766 /* Does this packet require timestamping? */1767 if (ntohs(*(__be16 *)&data[PTP_DPORT_OFFSET]) == PTP_EVENT_PORT) {1768 match->state = PTP_PACKET_STATE_UNMATCHED;1769 1770 /* We expect the sequence number to be in the same position in1771 * the packet for PTP V1 and V21772 */1773 BUILD_BUG_ON(PTP_V1_SEQUENCE_OFFSET != PTP_V2_SEQUENCE_OFFSET);1774 BUILD_BUG_ON(PTP_V1_SEQUENCE_LENGTH != PTP_V2_SEQUENCE_LENGTH);1775 } else {1776 match->state = PTP_PACKET_STATE_MATCH_UNWANTED;1777 }1778 1779 skb_queue_tail(&ptp->rxq, skb);1780 queue_work(ptp->workwq, &ptp->work);1781 1782 return true;1783}1784 1785/* Transmit a PTP packet. This has to be transmitted by the MC1786 * itself, through an MCDI call. MCDI calls aren't permitted1787 * in the transmit path so defer the actual transmission to a suitable worker.1788 */1789int efx_ptp_tx(struct efx_nic *efx, struct sk_buff *skb)1790{1791 struct efx_ptp_data *ptp = efx->ptp_data;1792 1793 skb_queue_tail(&ptp->txq, skb);1794 1795 if ((udp_hdr(skb)->dest == htons(PTP_EVENT_PORT)) &&1796 (skb->len <= MC_CMD_PTP_IN_TRANSMIT_PACKET_MAXNUM))1797 efx_xmit_hwtstamp_pending(skb);1798 queue_work(ptp->workwq, &ptp->work);1799 1800 return NETDEV_TX_OK;1801}1802 1803int efx_ptp_get_mode(struct efx_nic *efx)1804{1805 return efx->ptp_data->mode;1806}1807 1808int efx_ptp_change_mode(struct efx_nic *efx, bool enable_wanted,1809 unsigned int new_mode)1810{1811 if ((enable_wanted != efx->ptp_data->enabled) ||1812 (enable_wanted && (efx->ptp_data->mode != new_mode))) {1813 int rc = 0;1814 1815 if (enable_wanted) {1816 /* Change of mode requires disable */1817 if (efx->ptp_data->enabled &&1818 (efx->ptp_data->mode != new_mode)) {1819 efx->ptp_data->enabled = false;1820 rc = efx_ptp_stop(efx);1821 if (rc != 0)1822 return rc;1823 }1824 1825 /* Set new operating mode and establish1826 * baseline synchronisation, which must1827 * succeed.1828 */1829 efx->ptp_data->mode = new_mode;1830 if (netif_running(efx->net_dev))1831 rc = efx_ptp_start(efx);1832 if (rc == 0) {1833 rc = efx_ptp_synchronize(efx,1834 PTP_SYNC_ATTEMPTS * 2);1835 if (rc != 0)1836 efx_ptp_stop(efx);1837 }1838 } else {1839 rc = efx_ptp_stop(efx);1840 }1841 1842 if (rc != 0)1843 return rc;1844 1845 efx->ptp_data->enabled = enable_wanted;1846 }1847 1848 return 0;1849}1850 1851static int efx_ptp_ts_init(struct efx_nic *efx, struct kernel_hwtstamp_config *init)1852{1853 int rc;1854 1855 if ((init->tx_type != HWTSTAMP_TX_OFF) &&1856 (init->tx_type != HWTSTAMP_TX_ON))1857 return -ERANGE;1858 1859 rc = efx->type->ptp_set_ts_config(efx, init);1860 if (rc)1861 return rc;1862 1863 efx->ptp_data->config = *init;1864 return 0;1865}1866 1867void efx_ptp_get_ts_info(struct efx_nic *efx, struct kernel_ethtool_ts_info *ts_info)1868{1869 struct efx_ptp_data *ptp = efx->ptp_data;1870 struct efx_nic *primary = efx->primary;1871 1872 ASSERT_RTNL();1873 1874 if (!ptp)1875 return;1876 1877 ts_info->so_timestamping |= (SOF_TIMESTAMPING_TX_HARDWARE |1878 SOF_TIMESTAMPING_RX_HARDWARE |1879 SOF_TIMESTAMPING_RAW_HARDWARE);1880 /* Check licensed features. If we don't have the license for TX1881 * timestamps, the NIC will not support them.1882 */1883 if (efx_ptp_use_mac_tx_timestamps(efx)) {1884 struct efx_ef10_nic_data *nic_data = efx->nic_data;1885 1886 if (!(nic_data->licensed_features &1887 (1 << LICENSED_V3_FEATURES_TX_TIMESTAMPS_LBN)))1888 ts_info->so_timestamping &=1889 ~SOF_TIMESTAMPING_TX_HARDWARE;1890 }1891 if (primary && primary->ptp_data && primary->ptp_data->phc_clock)1892 ts_info->phc_index =1893 ptp_clock_index(primary->ptp_data->phc_clock);1894 ts_info->tx_types = 1 << HWTSTAMP_TX_OFF | 1 << HWTSTAMP_TX_ON;1895 ts_info->rx_filters = ptp->efx->type->hwtstamp_filters;1896}1897 1898int efx_ptp_set_ts_config(struct efx_nic *efx,1899 struct kernel_hwtstamp_config *config,1900 struct netlink_ext_ack __always_unused *extack)1901{1902 /* Not a PTP enabled port */1903 if (!efx->ptp_data)1904 return -EOPNOTSUPP;1905 1906 return efx_ptp_ts_init(efx, config);1907}1908 1909int efx_ptp_get_ts_config(struct efx_nic *efx,1910 struct kernel_hwtstamp_config *config)1911{1912 /* Not a PTP enabled port */1913 if (!efx->ptp_data)1914 return -EOPNOTSUPP;1915 *config = efx->ptp_data->config;1916 return 0;1917}1918 1919static void ptp_event_failure(struct efx_nic *efx, int expected_frag_len)1920{1921 struct efx_ptp_data *ptp = efx->ptp_data;1922 1923 netif_err(efx, hw, efx->net_dev,1924 "PTP unexpected event length: got %d expected %d\n",1925 ptp->evt_frag_idx, expected_frag_len);1926 ptp->reset_required = true;1927 queue_work(ptp->workwq, &ptp->work);1928}1929 1930static void ptp_event_fault(struct efx_nic *efx, struct efx_ptp_data *ptp)1931{1932 int code = EFX_QWORD_FIELD(ptp->evt_frags[0], MCDI_EVENT_DATA);1933 if (ptp->evt_frag_idx != 1) {1934 ptp_event_failure(efx, 1);1935 return;1936 }1937 1938 netif_err(efx, hw, efx->net_dev, "PTP error %d\n", code);1939}1940 1941static void ptp_event_pps(struct efx_nic *efx, struct efx_ptp_data *ptp)1942{1943 if (ptp->nic_ts_enabled)1944 queue_work(ptp->pps_workwq, &ptp->pps_work);1945}1946 1947void efx_ptp_event(struct efx_nic *efx, efx_qword_t *ev)1948{1949 struct efx_ptp_data *ptp = efx->ptp_data;1950 int code = EFX_QWORD_FIELD(*ev, MCDI_EVENT_CODE);1951 1952 if (!ptp) {1953 if (!efx->ptp_warned) {1954 netif_warn(efx, drv, efx->net_dev,1955 "Received PTP event but PTP not set up\n");1956 efx->ptp_warned = true;1957 }1958 return;1959 }1960 1961 if (!ptp->enabled)1962 return;1963 1964 if (ptp->evt_frag_idx == 0) {1965 ptp->evt_code = code;1966 } else if (ptp->evt_code != code) {1967 netif_err(efx, hw, efx->net_dev,1968 "PTP out of sequence event %d\n", code);1969 ptp->evt_frag_idx = 0;1970 }1971 1972 ptp->evt_frags[ptp->evt_frag_idx++] = *ev;1973 if (!MCDI_EVENT_FIELD(*ev, CONT)) {1974 /* Process resulting event */1975 switch (code) {1976 case MCDI_EVENT_CODE_PTP_FAULT:1977 ptp_event_fault(efx, ptp);1978 break;1979 case MCDI_EVENT_CODE_PTP_PPS:1980 ptp_event_pps(efx, ptp);1981 break;1982 default:1983 netif_err(efx, hw, efx->net_dev,1984 "PTP unknown event %d\n", code);1985 break;1986 }1987 ptp->evt_frag_idx = 0;1988 } else if (MAX_EVENT_FRAGS == ptp->evt_frag_idx) {1989 netif_err(efx, hw, efx->net_dev,1990 "PTP too many event fragments\n");1991 ptp->evt_frag_idx = 0;1992 }1993}1994 1995void efx_time_sync_event(struct efx_channel *channel, efx_qword_t *ev)1996{1997 struct efx_nic *efx = channel->efx;1998 struct efx_ptp_data *ptp = efx->ptp_data;1999 2000 /* When extracting the sync timestamp minor value, we should discard2001 * the least significant two bits. These are not required in order2002 * to reconstruct full-range timestamps and they are optionally used2003 * to report status depending on the options supplied when subscribing2004 * for sync events.2005 */2006 channel->sync_timestamp_major = MCDI_EVENT_FIELD(*ev, PTP_TIME_MAJOR);2007 channel->sync_timestamp_minor =2008 (MCDI_EVENT_FIELD(*ev, PTP_TIME_MINOR_MS_8BITS) & 0xFC)2009 << ptp->nic_time.sync_event_minor_shift;2010 2011 /* if sync events have been disabled then we want to silently ignore2012 * this event, so throw away result.2013 */2014 (void) cmpxchg(&channel->sync_events_state, SYNC_EVENTS_REQUESTED,2015 SYNC_EVENTS_VALID);2016}2017 2018static inline u32 efx_rx_buf_timestamp_minor(struct efx_nic *efx, const u8 *eh)2019{2020#if defined(CONFIG_HAVE_EFFICIENT_UNALIGNED_ACCESS)2021 return __le32_to_cpup((const __le32 *)(eh + efx->rx_packet_ts_offset));2022#else2023 const u8 *data = eh + efx->rx_packet_ts_offset;2024 return (u32)data[0] |2025 (u32)data[1] << 8 |2026 (u32)data[2] << 16 |2027 (u32)data[3] << 24;2028#endif2029}2030 2031void __efx_rx_skb_attach_timestamp(struct efx_channel *channel,2032 struct sk_buff *skb)2033{2034 struct efx_nic *efx = channel->efx;2035 struct efx_ptp_data *ptp = efx->ptp_data;2036 u32 pkt_timestamp_major, pkt_timestamp_minor;2037 u32 diff, carry;2038 struct skb_shared_hwtstamps *timestamps;2039 2040 if (channel->sync_events_state != SYNC_EVENTS_VALID)2041 return;2042 2043 pkt_timestamp_minor = efx_rx_buf_timestamp_minor(efx, skb_mac_header(skb));2044 2045 /* get the difference between the packet and sync timestamps,2046 * modulo one second2047 */2048 diff = pkt_timestamp_minor - channel->sync_timestamp_minor;2049 if (pkt_timestamp_minor < channel->sync_timestamp_minor)2050 diff += ptp->nic_time.minor_max;2051 2052 /* do we roll over a second boundary and need to carry the one? */2053 carry = (channel->sync_timestamp_minor >= ptp->nic_time.minor_max - diff) ?2054 1 : 0;2055 2056 if (diff <= ptp->nic_time.sync_event_diff_max) {2057 /* packet is ahead of the sync event by a quarter of a second or2058 * less (allowing for fuzz)2059 */2060 pkt_timestamp_major = channel->sync_timestamp_major + carry;2061 } else if (diff >= ptp->nic_time.sync_event_diff_min) {2062 /* packet is behind the sync event but within the fuzz factor.2063 * This means the RX packet and sync event crossed as they were2064 * placed on the event queue, which can sometimes happen.2065 */2066 pkt_timestamp_major = channel->sync_timestamp_major - 1 + carry;2067 } else {2068 /* it's outside tolerance in both directions. this might be2069 * indicative of us missing sync events for some reason, so2070 * we'll call it an error rather than risk giving a bogus2071 * timestamp.2072 */2073 netif_vdbg(efx, drv, efx->net_dev,2074 "packet timestamp %x too far from sync event %x:%x\n",2075 pkt_timestamp_minor, channel->sync_timestamp_major,2076 channel->sync_timestamp_minor);2077 return;2078 }2079 2080 /* attach the timestamps to the skb */2081 timestamps = skb_hwtstamps(skb);2082 timestamps->hwtstamp =2083 ptp->nic_to_kernel_time(pkt_timestamp_major,2084 pkt_timestamp_minor,2085 ptp->ts_corrections.general_rx);2086}2087 2088static int efx_phc_adjfine(struct ptp_clock_info *ptp, long scaled_ppm)2089{2090 struct efx_ptp_data *ptp_data = container_of(ptp,2091 struct efx_ptp_data,2092 phc_clock_info);2093 s32 delta = scaled_ppm_to_ppb(scaled_ppm);2094 struct efx_nic *efx = ptp_data->efx;2095 MCDI_DECLARE_BUF(inadj, MC_CMD_PTP_IN_ADJUST_LEN);2096 s64 adjustment_ns;2097 int rc;2098 2099 if (delta > MAX_PPB)2100 delta = MAX_PPB;2101 else if (delta < -MAX_PPB)2102 delta = -MAX_PPB;2103 2104 /* Convert ppb to fixed point ns taking care to round correctly. */2105 adjustment_ns = ((s64)delta * PPB_SCALE_WORD +2106 (1 << (ptp_data->adjfreq_ppb_shift - 1))) >>2107 ptp_data->adjfreq_ppb_shift;2108 2109 MCDI_SET_DWORD(inadj, PTP_IN_OP, MC_CMD_PTP_OP_ADJUST);2110 MCDI_SET_DWORD(inadj, PTP_IN_PERIPH_ID, 0);2111 MCDI_SET_QWORD(inadj, PTP_IN_ADJUST_FREQ, adjustment_ns);2112 MCDI_SET_DWORD(inadj, PTP_IN_ADJUST_SECONDS, 0);2113 MCDI_SET_DWORD(inadj, PTP_IN_ADJUST_NANOSECONDS, 0);2114 rc = efx_mcdi_rpc(efx, MC_CMD_PTP, inadj, sizeof(inadj),2115 NULL, 0, NULL);2116 if (rc != 0)2117 return rc;2118 2119 ptp_data->current_adjfreq = adjustment_ns;2120 return 0;2121}2122 2123static int efx_phc_adjtime(struct ptp_clock_info *ptp, s64 delta)2124{2125 u32 nic_major, nic_minor;2126 struct efx_ptp_data *ptp_data = container_of(ptp,2127 struct efx_ptp_data,2128 phc_clock_info);2129 struct efx_nic *efx = ptp_data->efx;2130 MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_ADJUST_LEN);2131 2132 efx->ptp_data->ns_to_nic_time(delta, &nic_major, &nic_minor);2133 2134 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_ADJUST);2135 MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);2136 MCDI_SET_QWORD(inbuf, PTP_IN_ADJUST_FREQ, ptp_data->current_adjfreq);2137 MCDI_SET_DWORD(inbuf, PTP_IN_ADJUST_MAJOR, nic_major);2138 MCDI_SET_DWORD(inbuf, PTP_IN_ADJUST_MINOR, nic_minor);2139 return efx_mcdi_rpc(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),2140 NULL, 0, NULL);2141}2142 2143static int efx_phc_gettime(struct ptp_clock_info *ptp, struct timespec64 *ts)2144{2145 struct efx_ptp_data *ptp_data = container_of(ptp,2146 struct efx_ptp_data,2147 phc_clock_info);2148 struct efx_nic *efx = ptp_data->efx;2149 MCDI_DECLARE_BUF(inbuf, MC_CMD_PTP_IN_READ_NIC_TIME_LEN);2150 MCDI_DECLARE_BUF(outbuf, MC_CMD_PTP_OUT_READ_NIC_TIME_LEN);2151 int rc;2152 ktime_t kt;2153 2154 MCDI_SET_DWORD(inbuf, PTP_IN_OP, MC_CMD_PTP_OP_READ_NIC_TIME);2155 MCDI_SET_DWORD(inbuf, PTP_IN_PERIPH_ID, 0);2156 2157 rc = efx_mcdi_rpc(efx, MC_CMD_PTP, inbuf, sizeof(inbuf),2158 outbuf, sizeof(outbuf), NULL);2159 if (rc != 0)2160 return rc;2161 2162 kt = ptp_data->nic_to_kernel_time(2163 MCDI_DWORD(outbuf, PTP_OUT_READ_NIC_TIME_MAJOR),2164 MCDI_DWORD(outbuf, PTP_OUT_READ_NIC_TIME_MINOR), 0);2165 *ts = ktime_to_timespec64(kt);2166 return 0;2167}2168 2169static int efx_phc_settime(struct ptp_clock_info *ptp,2170 const struct timespec64 *e_ts)2171{2172 /* Get the current NIC time, efx_phc_gettime.2173 * Subtract from the desired time to get the offset2174 * call efx_phc_adjtime with the offset2175 */2176 int rc;2177 struct timespec64 time_now;2178 struct timespec64 delta;2179 2180 rc = efx_phc_gettime(ptp, &time_now);2181 if (rc != 0)2182 return rc;2183 2184 delta = timespec64_sub(*e_ts, time_now);2185 2186 rc = efx_phc_adjtime(ptp, timespec64_to_ns(&delta));2187 if (rc != 0)2188 return rc;2189 2190 return 0;2191}2192 2193static int efx_phc_enable(struct ptp_clock_info *ptp,2194 struct ptp_clock_request *request,2195 int enable)2196{2197 struct efx_ptp_data *ptp_data = container_of(ptp,2198 struct efx_ptp_data,2199 phc_clock_info);2200 if (request->type != PTP_CLK_REQ_PPS)2201 return -EOPNOTSUPP;2202 2203 ptp_data->nic_ts_enabled = !!enable;2204 return 0;2205}2206 2207static const struct efx_channel_type efx_ptp_channel_type = {2208 .handle_no_channel = efx_ptp_handle_no_channel,2209 .pre_probe = efx_ptp_probe_channel,2210 .post_remove = efx_ptp_remove_channel,2211 .get_name = efx_ptp_get_channel_name,2212 .copy = efx_copy_channel,2213 .receive_skb = efx_ptp_rx,2214 .want_txqs = efx_ptp_want_txqs,2215 .keep_eventq = false,2216};2217 2218void efx_ptp_defer_probe_with_channel(struct efx_nic *efx)2219{2220 /* Check whether PTP is implemented on this NIC. The DISABLE2221 * operation will succeed if and only if it is implemented.2222 */2223 if (efx_ptp_disable(efx) == 0)2224 efx->extra_channel_type[EFX_EXTRA_CHANNEL_PTP] =2225 &efx_ptp_channel_type;2226}2227 2228void efx_ptp_start_datapath(struct efx_nic *efx)2229{2230 if (efx_ptp_restart(efx))2231 netif_err(efx, drv, efx->net_dev, "Failed to restart PTP.\n");2232 /* re-enable timestamping if it was previously enabled */2233 if (efx->type->ptp_set_ts_sync_events)2234 efx->type->ptp_set_ts_sync_events(efx, true, true);2235}2236 2237void efx_ptp_stop_datapath(struct efx_nic *efx)2238{2239 /* temporarily disable timestamping */2240 if (efx->type->ptp_set_ts_sync_events)2241 efx->type->ptp_set_ts_sync_events(efx, false, true);2242 efx_ptp_stop(efx);2243}2244