diff --git a/drivers/net/ethernet/engleder/tsnep_hw.h b/drivers/net/ethernet/engleder/tsnep_hw.h index 71cc8577d640..916ceac3ada2 100644 --- a/drivers/net/ethernet/engleder/tsnep_hw.h +++ b/drivers/net/ethernet/engleder/tsnep_hw.h @@ -43,6 +43,10 @@ #define ECM_RESET_CHANNEL 0x00000100 #define ECM_RESET_TXRX 0x00010000 +/* counter */ +#define ECM_COUNTER_LOW 0x0028 +#define ECM_COUNTER_HIGH 0x002C + /* control and status */ #define ECM_STATUS 0x0080 #define ECM_LINK_MODE_OFF 0x01000000 @@ -190,7 +194,8 @@ struct tsnep_tx_desc { /* tsnep TX descriptor writeback */ struct tsnep_tx_desc_wb { __le32 properties; - __le32 reserved1[3]; + __le32 reserved1; + __le64 counter; __le64 timestamp; __le32 dma_delay; __le32 reserved2; @@ -221,7 +226,7 @@ struct tsnep_rx_desc_wb { /* tsnep RX inline meta */ struct tsnep_rx_inline { - __le64 reserved; + __le64 counter; __le64 timestamp; }; diff --git a/drivers/net/ethernet/engleder/tsnep_main.c b/drivers/net/ethernet/engleder/tsnep_main.c index 49c93aa38862..cb069a0af7b9 100644 --- a/drivers/net/ethernet/engleder/tsnep_main.c +++ b/drivers/net/ethernet/engleder/tsnep_main.c @@ -470,8 +470,15 @@ static bool tsnep_tx_poll(struct tsnep_tx *tx, int napi_budget) (__le32_to_cpu(entry->desc_wb->properties) & TSNEP_DESC_EXTENDED_WRITEBACK_FLAG)) { struct skb_shared_hwtstamps hwtstamps; - u64 timestamp = - __le64_to_cpu(entry->desc_wb->timestamp); + u64 timestamp; + + if (skb_shinfo(entry->skb)->tx_flags & + SKBTX_HW_TSTAMP_USE_CYCLES) + timestamp = + __le64_to_cpu(entry->desc_wb->counter); + else + timestamp = + __le64_to_cpu(entry->desc_wb->timestamp); memset(&hwtstamps, 0, sizeof(hwtstamps)); hwtstamps.hwtstamp = ns_to_ktime(timestamp); @@ -704,11 +711,11 @@ static int tsnep_rx_poll(struct tsnep_rx *rx, struct napi_struct *napi, skb_hwtstamps(skb); struct tsnep_rx_inline *rx_inline = (struct tsnep_rx_inline *)skb->data; - u64 timestamp = - __le64_to_cpu(rx_inline->timestamp); + skb_shinfo(skb)->tx_flags |= + SKBTX_HW_TSTAMP_NETDEV; memset(hwtstamps, 0, sizeof(*hwtstamps)); - hwtstamps->hwtstamp = ns_to_ktime(timestamp); + hwtstamps->netdev_data = rx_inline; } skb_pull(skb, TSNEP_RX_INLINE_METADATA_SIZE); skb->protocol = eth_type_trans(skb, @@ -1010,6 +1017,21 @@ static int tsnep_netdev_set_mac_address(struct net_device *netdev, void *addr) return 0; } +static ktime_t tsnep_netdev_get_tstamp(struct net_device *netdev, + const struct skb_shared_hwtstamps *hwtstamps, + bool cycles) +{ + struct tsnep_rx_inline *rx_inline = hwtstamps->netdev_data; + u64 timestamp; + + if (cycles) + timestamp = __le64_to_cpu(rx_inline->counter); + else + timestamp = __le64_to_cpu(rx_inline->timestamp); + + return ns_to_ktime(timestamp); +} + static const struct net_device_ops tsnep_netdev_ops = { .ndo_open = tsnep_netdev_open, .ndo_stop = tsnep_netdev_close, @@ -1019,6 +1041,7 @@ static const struct net_device_ops tsnep_netdev_ops = { .ndo_get_stats64 = tsnep_netdev_get_stats64, .ndo_set_mac_address = tsnep_netdev_set_mac_address, + .ndo_get_tstamp = tsnep_netdev_get_tstamp, .ndo_setup_tc = tsnep_tc_setup, }; diff --git a/drivers/net/ethernet/engleder/tsnep_ptp.c b/drivers/net/ethernet/engleder/tsnep_ptp.c index eaad453d487e..54fbf0126815 100644 --- a/drivers/net/ethernet/engleder/tsnep_ptp.c +++ b/drivers/net/ethernet/engleder/tsnep_ptp.c @@ -175,6 +175,33 @@ static int tsnep_ptp_settime64(struct ptp_clock_info *ptp, return 0; } +static int tsnep_ptp_getcyclesx64(struct ptp_clock_info *ptp, + struct timespec64 *ts, + struct ptp_system_timestamp *sts) +{ + struct tsnep_adapter *adapter = container_of(ptp, struct tsnep_adapter, + ptp_clock_info); + u32 high_before; + u32 low; + u32 high; + u64 counter; + + /* read high dword twice to detect overrun */ + high = ioread32(adapter->addr + ECM_COUNTER_HIGH); + do { + ptp_read_system_prets(sts); + low = ioread32(adapter->addr + ECM_COUNTER_LOW); + ptp_read_system_postts(sts); + high_before = high; + high = ioread32(adapter->addr + ECM_COUNTER_HIGH); + } while (high != high_before); + counter = (((u64)high) << 32) | ((u64)low); + + *ts = ns_to_timespec64(counter); + + return 0; +} + int tsnep_ptp_init(struct tsnep_adapter *adapter) { int retval = 0; @@ -192,6 +219,7 @@ int tsnep_ptp_init(struct tsnep_adapter *adapter) adapter->ptp_clock_info.adjtime = tsnep_ptp_adjtime; adapter->ptp_clock_info.gettimex64 = tsnep_ptp_gettimex64; adapter->ptp_clock_info.settime64 = tsnep_ptp_settime64; + adapter->ptp_clock_info.getcyclesx64 = tsnep_ptp_getcyclesx64; spin_lock_init(&adapter->ptp_lock); diff --git a/drivers/ptp/ptp_clock.c b/drivers/ptp/ptp_clock.c index b6f2cfd15dd2..688cde320bb0 100644 --- a/drivers/ptp/ptp_clock.c +++ b/drivers/ptp/ptp_clock.c @@ -77,8 +77,8 @@ static int ptp_clock_settime(struct posix_clock *pc, const struct timespec64 *tp { struct ptp_clock *ptp = container_of(pc, struct ptp_clock, clock); - if (ptp_vclock_in_use(ptp)) { - pr_err("ptp: virtual clock in use\n"); + if (ptp_clock_freerun(ptp)) { + pr_err("ptp: physical clock is free running\n"); return -EBUSY; } @@ -103,8 +103,8 @@ static int ptp_clock_adjtime(struct posix_clock *pc, struct __kernel_timex *tx) struct ptp_clock_info *ops; int err = -EOPNOTSUPP; - if (ptp_vclock_in_use(ptp)) { - pr_err("ptp: virtual clock in use\n"); + if (ptp_clock_freerun(ptp)) { + pr_err("ptp: physical clock is free running\n"); return -EBUSY; } @@ -178,6 +178,14 @@ static void ptp_clock_release(struct device *dev) kfree(ptp); } +static int ptp_getcycles64(struct ptp_clock_info *info, struct timespec64 *ts) +{ + if (info->getcyclesx64) + return info->getcyclesx64(info, ts, NULL); + else + return info->gettime64(info, ts); +} + static void ptp_aux_kworker(struct kthread_work *work) { struct ptp_clock *ptp = container_of(work, struct ptp_clock, @@ -225,6 +233,21 @@ struct ptp_clock *ptp_clock_register(struct ptp_clock_info *info, mutex_init(&ptp->n_vclocks_mux); init_waitqueue_head(&ptp->tsev_wq); + if (ptp->info->getcycles64 || ptp->info->getcyclesx64) { + ptp->has_cycles = true; + if (!ptp->info->getcycles64 && ptp->info->getcyclesx64) + ptp->info->getcycles64 = ptp_getcycles64; + } else { + /* Free running cycle counter not supported, use time. */ + ptp->info->getcycles64 = ptp_getcycles64; + + if (ptp->info->gettimex64) + ptp->info->getcyclesx64 = ptp->info->gettimex64; + + if (ptp->info->getcrosststamp) + ptp->info->getcrosscycles = ptp->info->getcrosststamp; + } + if (ptp->info->do_aux_work) { kthread_init_delayed_work(&ptp->aux_work, ptp_aux_kworker); ptp->kworker = kthread_create_worker(0, "ptp%d", ptp->index); diff --git a/drivers/ptp/ptp_private.h b/drivers/ptp/ptp_private.h index dba6be477067..77918a2c6701 100644 --- a/drivers/ptp/ptp_private.h +++ b/drivers/ptp/ptp_private.h @@ -52,6 +52,7 @@ struct ptp_clock { int *vclock_index; struct mutex n_vclocks_mux; /* protect concurrent n_vclocks access */ bool is_virtual_clock; + bool has_cycles; }; #define info_to_vclock(d) container_of((d), struct ptp_vclock, info) @@ -62,6 +63,7 @@ struct ptp_vclock { struct ptp_clock *pclock; struct ptp_clock_info info; struct ptp_clock *clock; + struct hlist_node vclock_hash_node; struct cyclecounter cc; struct timecounter tc; spinlock_t lock; /* protects tc/cc */ @@ -96,6 +98,15 @@ static inline bool ptp_vclock_in_use(struct ptp_clock *ptp) return in_use; } +/* Check if ptp clock shall be free running */ +static inline bool ptp_clock_freerun(struct ptp_clock *ptp) +{ + if (ptp->has_cycles) + return false; + + return ptp_vclock_in_use(ptp); +} + extern struct class *ptp_class; /* diff --git a/drivers/ptp/ptp_sysfs.c b/drivers/ptp/ptp_sysfs.c index 9233bfedeb17..f30b0a439470 100644 --- a/drivers/ptp/ptp_sysfs.c +++ b/drivers/ptp/ptp_sysfs.c @@ -231,10 +231,13 @@ static ssize_t n_vclocks_store(struct device *dev, *(ptp->vclock_index + ptp->n_vclocks - i) = -1; } - if (num == 0) - dev_info(dev, "only physical clock in use now\n"); - else - dev_info(dev, "guarantee physical clock free running\n"); + /* Need to inform about changed physical clock behavior */ + if (!ptp->has_cycles) { + if (num == 0) + dev_info(dev, "only physical clock in use now\n"); + else + dev_info(dev, "guarantee physical clock free running\n"); + } ptp->n_vclocks = num; mutex_unlock(&ptp->n_vclocks_mux); diff --git a/drivers/ptp/ptp_vclock.c b/drivers/ptp/ptp_vclock.c index cb179a3ea508..1c0ed4805c0a 100644 --- a/drivers/ptp/ptp_vclock.c +++ b/drivers/ptp/ptp_vclock.c @@ -5,6 +5,7 @@ * Copyright 2021 NXP */ #include +#include #include "ptp_private.h" #define PTP_VCLOCK_CC_SHIFT 31 @@ -13,6 +14,32 @@ #define PTP_VCLOCK_FADJ_DENOMINATOR 15625ULL #define PTP_VCLOCK_REFRESH_INTERVAL (HZ * 2) +/* protects vclock_hash addition/deletion */ +static DEFINE_SPINLOCK(vclock_hash_lock); + +static DEFINE_READ_MOSTLY_HASHTABLE(vclock_hash, 8); + +static void ptp_vclock_hash_add(struct ptp_vclock *vclock) +{ + spin_lock(&vclock_hash_lock); + + hlist_add_head_rcu(&vclock->vclock_hash_node, + &vclock_hash[vclock->clock->index % HASH_SIZE(vclock_hash)]); + + spin_unlock(&vclock_hash_lock); +} + +static void ptp_vclock_hash_del(struct ptp_vclock *vclock) +{ + spin_lock(&vclock_hash_lock); + + hlist_del_init_rcu(&vclock->vclock_hash_node); + + spin_unlock(&vclock_hash_lock); + + synchronize_rcu(); +} + static int ptp_vclock_adjfine(struct ptp_clock_info *ptp, long scaled_ppm) { struct ptp_vclock *vclock = info_to_vclock(ptp); @@ -68,7 +95,7 @@ static int ptp_vclock_gettimex(struct ptp_clock_info *ptp, int err; u64 ns; - err = pptp->info->gettimex64(pptp->info, &pts, sts); + err = pptp->info->getcyclesx64(pptp->info, &pts, sts); if (err) return err; @@ -104,7 +131,7 @@ static int ptp_vclock_getcrosststamp(struct ptp_clock_info *ptp, int err; u64 ns; - err = pptp->info->getcrosststamp(pptp->info, xtstamp); + err = pptp->info->getcrosscycles(pptp->info, xtstamp); if (err) return err; @@ -143,10 +170,7 @@ static u64 ptp_vclock_read(const struct cyclecounter *cc) struct ptp_clock *ptp = vclock->pclock; struct timespec64 ts = {}; - if (ptp->info->gettimex64) - ptp->info->gettimex64(ptp->info, &ts, NULL); - else - ptp->info->gettime64(ptp->info, &ts); + ptp->info->getcycles64(ptp->info, &ts); return timespec64_to_ns(&ts); } @@ -168,17 +192,19 @@ struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock) vclock->pclock = pclock; vclock->info = ptp_vclock_info; - if (pclock->info->gettimex64) + if (pclock->info->getcyclesx64) vclock->info.gettimex64 = ptp_vclock_gettimex; else vclock->info.gettime64 = ptp_vclock_gettime; - if (pclock->info->getcrosststamp) + if (pclock->info->getcrosscycles) vclock->info.getcrosststamp = ptp_vclock_getcrosststamp; vclock->cc = ptp_vclock_cc; snprintf(vclock->info.name, PTP_CLOCK_NAME_LEN, "ptp%d_virt", pclock->index); + INIT_HLIST_NODE(&vclock->vclock_hash_node); + spin_lock_init(&vclock->lock); vclock->clock = ptp_clock_register(&vclock->info, &pclock->dev); @@ -190,11 +216,15 @@ struct ptp_vclock *ptp_vclock_register(struct ptp_clock *pclock) timecounter_init(&vclock->tc, &vclock->cc, 0); ptp_schedule_worker(vclock->clock, PTP_VCLOCK_REFRESH_INTERVAL); + ptp_vclock_hash_add(vclock); + return vclock; } void ptp_vclock_unregister(struct ptp_vclock *vclock) { + ptp_vclock_hash_del(vclock); + ptp_clock_unregister(vclock->clock); kfree(vclock); } @@ -235,37 +265,31 @@ int ptp_get_vclocks_index(int pclock_index, int **vclock_index) } EXPORT_SYMBOL(ptp_get_vclocks_index); -ktime_t ptp_convert_timestamp(const struct skb_shared_hwtstamps *hwtstamps, - int vclock_index) +ktime_t ptp_convert_timestamp(const ktime_t *hwtstamp, int vclock_index) { - char name[PTP_CLOCK_NAME_LEN] = ""; + unsigned int hash = vclock_index % HASH_SIZE(vclock_hash); struct ptp_vclock *vclock; - struct ptp_clock *ptp; unsigned long flags; - struct device *dev; u64 ns; + u64 vclock_ns = 0; - snprintf(name, PTP_CLOCK_NAME_LEN, "ptp%d", vclock_index); - dev = class_find_device_by_name(ptp_class, name); - if (!dev) - return 0; + ns = ktime_to_ns(*hwtstamp); - ptp = dev_get_drvdata(dev); - if (!ptp->is_virtual_clock) { - put_device(dev); - return 0; + rcu_read_lock(); + + hlist_for_each_entry_rcu(vclock, &vclock_hash[hash], vclock_hash_node) { + if (vclock->clock->index != vclock_index) + continue; + + spin_lock_irqsave(&vclock->lock, flags); + vclock_ns = timecounter_cyc2time(&vclock->tc, ns); + spin_unlock_irqrestore(&vclock->lock, flags); + break; } - vclock = info_to_vclock(ptp->info); + rcu_read_unlock(); - ns = ktime_to_ns(hwtstamps->hwtstamp); - - spin_lock_irqsave(&vclock->lock, flags); - ns = timecounter_cyc2time(&vclock->tc, ns); - spin_unlock_irqrestore(&vclock->lock, flags); - - put_device(dev); - return ns_to_ktime(ns); + return ns_to_ktime(vclock_ns); } EXPORT_SYMBOL(ptp_convert_timestamp); #endif diff --git a/include/linux/netdevice.h b/include/linux/netdevice.h index 74c97a34921d..51fe143091cf 100644 --- a/include/linux/netdevice.h +++ b/include/linux/netdevice.h @@ -1356,6 +1356,12 @@ struct netdev_net_notifier { * The caller must be under RCU read context. * int (*ndo_fill_forward_path)(struct net_device_path_ctx *ctx, struct net_device_path *path); * Get the forwarding path to reach the real device from the HW destination address + * ktime_t (*ndo_get_tstamp)(struct net_device *dev, + * const struct skb_shared_hwtstamps *hwtstamps, + * bool cycles); + * Get hardware timestamp based on normal/adjustable time or free running + * cycle counter. This function is required if physical clock supports a + * free running cycle counter. */ struct net_device_ops { int (*ndo_init)(struct net_device *dev); @@ -1578,6 +1584,9 @@ struct net_device_ops { struct net_device * (*ndo_get_peer_dev)(struct net_device *dev); int (*ndo_fill_forward_path)(struct net_device_path_ctx *ctx, struct net_device_path *path); + ktime_t (*ndo_get_tstamp)(struct net_device *dev, + const struct skb_shared_hwtstamps *hwtstamps, + bool cycles); }; /** @@ -4761,6 +4770,18 @@ static inline void netdev_rx_csum_fault(struct net_device *dev, void net_enable_timestamp(void); void net_disable_timestamp(void); +static inline ktime_t netdev_get_tstamp(struct net_device *dev, + const struct skb_shared_hwtstamps *hwtstamps, + bool cycles) +{ + const struct net_device_ops *ops = dev->netdev_ops; + + if (ops->ndo_get_tstamp) + return ops->ndo_get_tstamp(dev, hwtstamps, cycles); + + return hwtstamps->hwtstamp; +} + static inline netdev_tx_t __netdev_start_xmit(const struct net_device_ops *ops, struct sk_buff *skb, struct net_device *dev, bool more) diff --git a/include/linux/ptp_clock_kernel.h b/include/linux/ptp_clock_kernel.h index e8cc8b6bbf50..92b44161408e 100644 --- a/include/linux/ptp_clock_kernel.h +++ b/include/linux/ptp_clock_kernel.h @@ -108,6 +108,32 @@ struct ptp_system_timestamp { * @settime64: Set the current time on the hardware clock. * parameter ts: Time value to set. * + * @getcycles64: Reads the current free running cycle counter from the hardware + * clock. + * If @getcycles64 and @getcyclesx64 are not supported, then + * @gettime64 or @gettimex64 will be used as default + * implementation. + * parameter ts: Holds the result. + * + * @getcyclesx64: Reads the current free running cycle counter from the + * hardware clock and optionally also the system clock. + * If @getcycles64 and @getcyclesx64 are not supported, then + * @gettimex64 will be used as default implementation if + * available. + * parameter ts: Holds the PHC timestamp. + * parameter sts: If not NULL, it holds a pair of timestamps + * from the system clock. The first reading is made right before + * reading the lowest bits of the PHC timestamp and the second + * reading immediately follows that. + * + * @getcrosscycles: Reads the current free running cycle counter from the + * hardware clock and system clock simultaneously. + * If @getcycles64 and @getcyclesx64 are not supported, then + * @getcrosststamp will be used as default implementation if + * available. + * parameter cts: Contains timestamp (device,system) pair, + * where system time is realtime and monotonic. + * * @enable: Request driver to enable or disable an ancillary feature. * parameter request: Desired resource to enable or disable. * parameter on: Caller passes one to enable or zero to disable. @@ -155,6 +181,11 @@ struct ptp_clock_info { int (*getcrosststamp)(struct ptp_clock_info *ptp, struct system_device_crosststamp *cts); int (*settime64)(struct ptp_clock_info *p, const struct timespec64 *ts); + int (*getcycles64)(struct ptp_clock_info *ptp, struct timespec64 *ts); + int (*getcyclesx64)(struct ptp_clock_info *ptp, struct timespec64 *ts, + struct ptp_system_timestamp *sts); + int (*getcrosscycles)(struct ptp_clock_info *ptp, + struct system_device_crosststamp *cts); int (*enable)(struct ptp_clock_info *ptp, struct ptp_clock_request *request, int on); int (*verify)(struct ptp_clock_info *ptp, unsigned int pin, @@ -353,17 +384,16 @@ int ptp_get_vclocks_index(int pclock_index, int **vclock_index); /** * ptp_convert_timestamp() - convert timestamp to a ptp vclock time * - * @hwtstamps: skb_shared_hwtstamps structure pointer + * @hwtstamp: timestamp * @vclock_index: phc index of ptp vclock. * * Returns converted timestamp, or 0 on error. */ -ktime_t ptp_convert_timestamp(const struct skb_shared_hwtstamps *hwtstamps, - int vclock_index); +ktime_t ptp_convert_timestamp(const ktime_t *hwtstamp, int vclock_index); #else static inline int ptp_get_vclocks_index(int pclock_index, int **vclock_index) { return 0; } -static inline ktime_t ptp_convert_timestamp(const struct skb_shared_hwtstamps *hwtstamps, +static inline ktime_t ptp_convert_timestamp(const ktime_t *hwtstamp, int vclock_index) { return 0; } diff --git a/include/linux/skbuff.h b/include/linux/skbuff.h index d58669d6cb91..97de40bcfef6 100644 --- a/include/linux/skbuff.h +++ b/include/linux/skbuff.h @@ -588,8 +588,10 @@ static inline bool skb_frag_must_loop(struct page *p) /** * struct skb_shared_hwtstamps - hardware time stamps - * @hwtstamp: hardware time stamp transformed into duration - * since arbitrary point in time + * @hwtstamp: hardware time stamp transformed into duration + * since arbitrary point in time + * @netdev_data: address/cookie of network device driver used as + * reference to actual hardware time stamp * * Software time stamps generated by ktime_get_real() are stored in * skb->tstamp. @@ -601,7 +603,10 @@ static inline bool skb_frag_must_loop(struct page *p) * &skb_shared_info. Use skb_hwtstamps() to get a pointer. */ struct skb_shared_hwtstamps { - ktime_t hwtstamp; + union { + ktime_t hwtstamp; + void *netdev_data; + }; }; /* Definitions for tx_flags in struct skb_shared_info */ @@ -615,16 +620,24 @@ enum { /* device driver is going to provide hardware time stamp */ SKBTX_IN_PROGRESS = 1 << 2, + /* generate hardware time stamp based on cycles if supported */ + SKBTX_HW_TSTAMP_USE_CYCLES = 1 << 3, + /* generate wifi status information (where possible) */ SKBTX_WIFI_STATUS = 1 << 4, + /* determine hardware time stamp based on time or cycles */ + SKBTX_HW_TSTAMP_NETDEV = 1 << 5, + /* generate software time stamp when entering packet scheduling */ SKBTX_SCHED_TSTAMP = 1 << 6, }; #define SKBTX_ANY_SW_TSTAMP (SKBTX_SW_TSTAMP | \ SKBTX_SCHED_TSTAMP) -#define SKBTX_ANY_TSTAMP (SKBTX_HW_TSTAMP | SKBTX_ANY_SW_TSTAMP) +#define SKBTX_ANY_TSTAMP (SKBTX_HW_TSTAMP | \ + SKBTX_HW_TSTAMP_USE_CYCLES | \ + SKBTX_ANY_SW_TSTAMP) /* Definitions for flags in struct skb_shared_info */ enum { diff --git a/net/socket.c b/net/socket.c index f0c39c874665..6ee634c01eca 100644 --- a/net/socket.c +++ b/net/socket.c @@ -683,9 +683,18 @@ void __sock_tx_timestamp(__u16 tsflags, __u8 *tx_flags) { u8 flags = *tx_flags; - if (tsflags & SOF_TIMESTAMPING_TX_HARDWARE) + if (tsflags & SOF_TIMESTAMPING_TX_HARDWARE) { flags |= SKBTX_HW_TSTAMP; + /* PTP hardware clocks can provide a free running cycle counter + * as a time base for virtual clocks. Tell driver to use the + * free running cycle counter for timestamp if socket is bound + * to virtual clock. + */ + if (tsflags & SOF_TIMESTAMPING_BIND_PHC) + flags |= SKBTX_HW_TSTAMP_USE_CYCLES; + } + if (tsflags & SOF_TIMESTAMPING_TX_SOFTWARE) flags |= SKBTX_SW_TSTAMP; @@ -796,7 +805,28 @@ static bool skb_is_swtx_tstamp(const struct sk_buff *skb, int false_tstamp) return skb->tstamp && !false_tstamp && skb_is_err_queue(skb); } -static void put_ts_pktinfo(struct msghdr *msg, struct sk_buff *skb) +static ktime_t get_timestamp(struct sock *sk, struct sk_buff *skb, int *if_index) +{ + bool cycles = sk->sk_tsflags & SOF_TIMESTAMPING_BIND_PHC; + struct skb_shared_hwtstamps *shhwtstamps = skb_hwtstamps(skb); + struct net_device *orig_dev; + ktime_t hwtstamp; + + rcu_read_lock(); + orig_dev = dev_get_by_napi_id(skb_napi_id(skb)); + if (orig_dev) { + *if_index = orig_dev->ifindex; + hwtstamp = netdev_get_tstamp(orig_dev, shhwtstamps, cycles); + } else { + hwtstamp = shhwtstamps->hwtstamp; + } + rcu_read_unlock(); + + return hwtstamp; +} + +static void put_ts_pktinfo(struct msghdr *msg, struct sk_buff *skb, + int if_index) { struct scm_ts_pktinfo ts_pktinfo; struct net_device *orig_dev; @@ -806,11 +836,14 @@ static void put_ts_pktinfo(struct msghdr *msg, struct sk_buff *skb) memset(&ts_pktinfo, 0, sizeof(ts_pktinfo)); - rcu_read_lock(); - orig_dev = dev_get_by_napi_id(skb_napi_id(skb)); - if (orig_dev) - ts_pktinfo.if_index = orig_dev->ifindex; - rcu_read_unlock(); + if (!if_index) { + rcu_read_lock(); + orig_dev = dev_get_by_napi_id(skb_napi_id(skb)); + if (orig_dev) + if_index = orig_dev->ifindex; + rcu_read_unlock(); + } + ts_pktinfo.if_index = if_index; ts_pktinfo.pkt_length = skb->len - skb_mac_offset(skb); put_cmsg(msg, SOL_SOCKET, SCM_TIMESTAMPING_PKTINFO, @@ -830,6 +863,7 @@ void __sock_recv_timestamp(struct msghdr *msg, struct sock *sk, int empty = 1, false_tstamp = 0; struct skb_shared_hwtstamps *shhwtstamps = skb_hwtstamps(skb); + int if_index; ktime_t hwtstamp; /* Race occurred between timestamp enabling and packet @@ -878,18 +912,22 @@ void __sock_recv_timestamp(struct msghdr *msg, struct sock *sk, if (shhwtstamps && (sk->sk_tsflags & SOF_TIMESTAMPING_RAW_HARDWARE) && !skb_is_swtx_tstamp(skb, false_tstamp)) { - if (sk->sk_tsflags & SOF_TIMESTAMPING_BIND_PHC) - hwtstamp = ptp_convert_timestamp(shhwtstamps, - sk->sk_bind_phc); + if_index = 0; + if (skb_shinfo(skb)->tx_flags & SKBTX_HW_TSTAMP_NETDEV) + hwtstamp = get_timestamp(sk, skb, &if_index); else hwtstamp = shhwtstamps->hwtstamp; + if (sk->sk_tsflags & SOF_TIMESTAMPING_BIND_PHC) + hwtstamp = ptp_convert_timestamp(&hwtstamp, + sk->sk_bind_phc); + if (ktime_to_timespec64_cond(hwtstamp, tss.ts + 2)) { empty = 0; if ((sk->sk_tsflags & SOF_TIMESTAMPING_OPT_PKTINFO) && !skb_is_err_queue(skb)) - put_ts_pktinfo(msg, skb); + put_ts_pktinfo(msg, skb, if_index); } } if (!empty) {