net/core/timestamping.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  * PTP 1588 clock support - support for timestamping in PHY devices
   4  *
   5  * Copyright (C) 2010 OMICRON electronics GmbH
   6  */
   7 #include <linux/errqueue.h>
   8 #include <linux/phy.h>
   9 #include <linux/ptp_classify.h>
  10 #include <linux/skbuff.h>
  11 #include <linux/export.h>
  12 #include <linux/ptp_clock_kernel.h>
  13
  14 static unsigned int classify(const struct sk_buff *skb)
  15 {
  16         if (likely(skb->dev && skb->dev->phydev &&
  17                    skb->dev->phydev->mii_ts))
  18                 return ptp_classify_raw(skb);
  19         else
  20                 return PTP_CLASS_NONE;
  21 }
  22
  23 void skb_clone_tx_timestamp(struct sk_buff *skb)
  24 {
  25         struct hwtstamp_provider *hwprov;
  26         struct mii_timestamper *mii_ts;
  27         struct phy_device *phydev;
  28         struct sk_buff *clone;
  29         unsigned int type;
  30
  31         if (!skb->sk || !skb->dev)
  32                 return;
  33
  34         rcu_read_lock();
  35         hwprov = rcu_dereference(skb->dev->hwprov);
  36         if (hwprov) {
  37                 if (hwprov->source != HWTSTAMP_SOURCE_PHYLIB ||
  38                     !hwprov->phydev) {
  39                         rcu_read_unlock();
  40                         return;
  41                 }
  42
  43                 phydev = hwprov->phydev;
  44         } else {
  45                 phydev = skb->dev->phydev;
  46                 if (!phy_is_default_hwtstamp(phydev)) {
  47                         rcu_read_unlock();
  48                         return;
  49                 }
  50         }
  51         rcu_read_unlock();
  52
  53         type = classify(skb);
  54         if (type == PTP_CLASS_NONE)
  55                 return;
  56
  57         mii_ts = phydev->mii_ts;
  58         if (likely(mii_ts->txtstamp)) {
  59                 clone = skb_clone_sk(skb);
  60                 if (!clone)
  61                         return;
  62                 mii_ts->txtstamp(mii_ts, clone, type);
  63         }
  64 }
  65 EXPORT_SYMBOL_GPL(skb_clone_tx_timestamp);
  66
  67 bool skb_defer_rx_timestamp(struct sk_buff *skb)
  68 {
  69         struct hwtstamp_provider *hwprov;
  70         struct mii_timestamper *mii_ts;
  71         struct phy_device *phydev;
  72         unsigned int type;
  73
  74         if (!skb->dev)
  75                 return false;
  76
  77         rcu_read_lock();
  78         hwprov = rcu_dereference(skb->dev->hwprov);
  79         if (hwprov) {
  80                 if (hwprov->source != HWTSTAMP_SOURCE_PHYLIB ||
  81                     !hwprov->phydev) {
  82                         rcu_read_unlock();
  83                         return false;
  84                 }
  85
  86                 phydev = hwprov->phydev;
  87         } else {
  88                 phydev = skb->dev->phydev;
  89                 if (!phy_is_default_hwtstamp(phydev)) {
  90                         rcu_read_unlock();
  91                         return false;
  92                 }
  93         }
  94         rcu_read_unlock();
  95
  96         if (skb_headroom(skb) < ETH_HLEN)
  97                 return false;
  98
  99         __skb_push(skb, ETH_HLEN);
 100
 101         type = ptp_classify_raw(skb);
 102
 103         __skb_pull(skb, ETH_HLEN);
 104
 105         if (type == PTP_CLASS_NONE)
 106                 return false;
 107
 108         mii_ts = phydev->mii_ts;
 109         if (likely(mii_ts->rxtstamp))
 110                 return mii_ts->rxtstamp(mii_ts, skb, type);
 111
 112         return false;
 113 }
 114 EXPORT_SYMBOL_GPL(skb_defer_rx_timestamp);