| 1 | |
| 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);
|